Rodriguesの回転公式

TECH徒然 (ITや数学のトピック) TECH徒然

はじめに

前回は3Dの回転を四元数 で表しましたが,もちろん回転は回転行列でも表せます.今回は回転軸と回転角から回転行列を計算する「Rodriguesの回転公式」を紹介します.

記号は下記を使います.

  • \(R_{u,\theta}\): 単位ベクトル \(u \in \mathbb{R}^3\) を軸とする回転角 \(\theta\) [rad]の回転行列.
  • \(x_{u,\theta} = R_{u,\theta} \cdot x\): 任意の \(x \in \mathbb{R}^3\) の回転後のベクトル.
  • \(v_\times\): 任意の \(v=[v_x, v_y, v_z]^T \in \mathbb{R}^3\) の外積作用の行列 (i.e., \(v_\times \cdot x \equiv v \times x\),下記).\[v_\times ~:=~ \left[ \begin{array}{ccc} 0 & -v_z & v_y \\ v_z & 0 & -v_x \\ -v_y & v_x & 0 \end{array} \right]\]

Rodriguesの回転公式

\(x \in \mathbb{R}^3\) を回転した \(x_{u,\theta}\) は,元の \(x\) の \(u\) 方向成分を変えず,\(u\) と直交する成分を法平面内で \(\theta\) 回転したものです.これは次のように射影や外積で作れます.図解は…略.

  1. \(x_\parallel ~:=~ (xのu方向成分) ~=~ \langle x,u \rangle \cdot u ~=~ uu^T x\)
  2. \(x_\perp ~:=~ (xのu直交成分) ~=~ x – x_\parallel ~=~ (I – uu^T) \cdot x\)
  3. \(x_{\perp\perp} ~:=~ (x_\perpをu軸まわり90度回転) ~=~ u \times x_\perp ~=~ u \times x ~=~ u_\times \cdot x\)
  4. \(x_{\perp\theta} ~:=~ (x_\perpをu軸まわり\theta回転) ~=~ \cos\theta \cdot x_\perp + \sin\theta \cdot x_{\perp\perp}\)
  5. \(x_{u,\theta} ~:=~ (xをu軸まわり\theta回転) ~=~ x_{\perp\theta} + x_\parallel\)

以上をまとめると,次の形の「Rodriguesの回転公式」を得ます. \[ \begin{eqnarray} x_{u,\theta} &=& \cos\theta \cdot x_\perp + \sin\theta \cdot x_{\perp\perp} + x_\parallel \\ &=& \cos\theta \cdot (I – uu^T) \cdot x + \sin\theta \cdot u \times x + uu^T \cdot x \end{eqnarray} \] あるいは等価ですが次の形をよく見かけます.回転行列 \(R_{u,\theta}\) も併記します. \[ \begin{eqnarray} x_{u,\theta} &=& \cos\theta \cdot x + \sin\theta \cdot u \times x + (1 – \cos\theta) \cdot uu^T \cdot x \\ R_{u,\theta} &=& \cos\theta \cdot I + \sin\theta \cdot u_\times + (1 – \cos\theta) \cdot uu^T \end{eqnarray} \]

別解

線形代数による省エネ方式.\(u\) で始まる任意の正規直交右手系を \(u, v, w \in \mathbb{R}^3\) とします. この基底で\(u\)軸まわり\(\theta\)回転を表現する行列 \(R_{u,\theta}’ ~:=~ [u, v, w]^T \cdot R_{u,\theta} \cdot [u, v, w]\) は次式です. \[ \begin{eqnarray} R_{u,\theta}’ &=& \left[ \begin{array}{c|cc} 1 & & \\ \hline & \cos\theta & -\sin\theta \\ & \sin\theta & \cos\theta \end{array} \right] \\ &=& \cos\theta \cdot I + \sin\theta \cdot \left[ \begin{array}{c|cc} 0 & & \\ \hline & 0 & -1 \\ & 1 & 0 \end{array} \right] + (1 – \cos\theta) \cdot \left[ \begin{array}{c|cc} 1 & & \\ \hline & 0 & \\ & & 0 \end{array} \right] \end{eqnarray} \] 2行目の幾何学的な意味は明瞭で,第2項の行列は\(v w\)面内成分の90度回転(= \(u\)の外積作用),第3項の行列は\(u\)方向への射影です. したがって標準の基底での表現 \(R_{u,\theta}\) は計算するまでもなく: \[ \begin{eqnarray} R_{u,\theta} &=& \cos\theta \cdot I + \sin\theta \cdot u_\times + (1 – \cos\theta) \cdot uu^T \end{eqnarray} \]

QuaternionとRodriguesの換算

回転行列 \(R = R_{u,\theta}\) と単位四元数 \(q = \cos\frac{\theta}{2} + \sin\frac{\theta}{2} \cdot u\) は同じ回転を表しています(前回参照). それらを相互換算する公式を求めます.
以下,\(q\) の実部を \(q_r ~(= \cos\frac{\theta}{2})\),虚部相当の3Dベクトルを \(q_i ~(= \sin\frac{\theta}{2} \cdot u)\) と書きます.

まず \(q\) から \(R\) への換算は簡単,次式で一発OK. \[ \begin{eqnarray} R &=& \cos\theta \cdot I + \sin\theta \cdot u_\times + (1 – \cos\theta) \cdot uu^T \\ &=& (2 \cos^2\frac{\theta}{2} – 1) \cdot I + 2 \cos\frac{\theta}{2} \sin\frac{\theta}{2} \cdot u_\times + 2 \sin^2\frac{\theta}{2} \cdot uu^T \\ &=& (2 {q_r}^2 – 1) \cdot I + 2 q_r \cdot (q_i)_\times + 2 q_i \cdot {q_i}^T \end{eqnarray} \]

次に \(R\) から \(q\) への換算. 上式を眺めると,実部 \(q_r\) は対角部分に,虚部 \(q_i\) は反対称部分 (外積の項) に情報がありそうです.実際それぞれ, \[\begin{eqnarray} \mathrm{tr} (R) &=& (2 {q_r}^2 – 1) \cdot 3 + 2 q_r \cdot 0 + 2~ \| q_i \|^2 ~=~ 4 {q_r}^2 – 1 \\ R – R^T &=& 2 \cdot 2 q_r \cdot (q_i)_\times \end{eqnarray}\] ということで,次式でOK (下付きの “3D” は \(v \mapsto v_\times\) の逆,i.e., \((v_\times)_{\mathrm{3D}} \equiv v\)). \[ \begin{eqnarray} q_r &=& \pm \frac{1}{2}~\sqrt{1 + \mathrm{tr} (R)} \\ q_i &=& \frac{1}{4 q_r} ~(R – R^T)_{\mathrm{3D}} \end{eqnarray} \] ただし \(q_r \approx 0\) (\(\theta \approx 180°\)) の場合は桁落ち・ゼロ除算の対策が必要です.下記2例ご参考.

  • まず \(R\) の回転軸 \(u\) を求め \(q_i := \pm \frac{\sqrt{3 – \mathrm{tr} (R)}}{2} ~ u\).次に \(R – R^T\) の式で \(q_r\) を決定.
  • 適当な \(q^{\prime}~(\ne 1)\),対応する \(R^{\prime}\),誤差 \(R^{\prime\prime} := R {R^{\prime}}^T\),対応する \(q^{\prime\prime}\) を求め, \(q := q^{\prime\prime} q^{\prime}\).

Accutheraでは加速器・医療システム・機械学習・放射線シミュレーションなどの専門技術を軸に開発やコンサルティングを承っております。お困りの案件がございましたら、こちら からお気軽にお問合せください。

タイトルとURLをコピーしました