姿勢行列と角軸ベクトルの相互変換（等価角軸変換）

はじめに

こちらの記事で，剛体の3次元姿勢表現について書きました．

その要点をまとめます．

• 剛体の3次元配位は6個の数字で決まる．そのうち3個は並進（位置），残り3個は回転（姿勢）を与える．
• 物体の姿勢は，物体とともに運動する直交座標系（物体座標系）の3本の基底ベクトルの組（姿勢行列）でも表せる．
• 姿勢行列は合計9個パラメータを持つが，そこから正規直交性を表す等式制約条件の数6を引いた数3は，姿勢を表す数字の数3と整合する．
• 姿勢を表す3個の数字の選び方はいろいろある．

元の記事で話題にしたのはオイラー角で，これは姿勢を表す3個の数字の例として便利ではあるのですが，ジンバルロックが起こる（正則でない姿勢が存在する）という難点があります．また，各成分の値変化の大きさに対する姿勢の変化の大きさが一様でない（2番目の成分の値が1変わったときに姿勢がどのくらい変わるかは，1番目の値に依存する）という性質もあって，どちらかと言うとこちらの方が計算上は嫌がられます．
これらの問題を解消する別の「姿勢を表す3つの値」の一つである角軸ベクトルが，本記事の主役の一人です．

「姿勢を表せる」とは「姿勢行列と相互変換可能である」と言い換えられます．姿勢行列が，本記事のもう一人の主役です．

角軸ベクトルから姿勢行列への変換を説明する記事はわりと多くある一方，その逆変換はあまり説明されていないようで，本記事はそこを埋める趣旨のものです．

3次元姿勢表現にまつわる数学はとても奥深いです．本記事はその奥深さに迫る趣旨のものではありませんので，ご了承下さい．¹

オイラーの回転定理とロドリゲスの公式

剛体の回転については，オイラーによる次の重要な定理が知られています．

「いかなる回転においても，その結果として位置を変えないような直線を，球の極を通るように少なくとも一本見出すことができる．」
Leonhard Euler, Formulae generales pro translatione quacunque corporum rigidorum, Novi Commentarii academiae scientiarum Petropolitanae, Volume 20, pp. 189-207.

平易な言葉で言い換えると，「任意のある姿勢から別の任意のある姿勢へは，ある直線まわりの高々1回の回転だけで至れる」ということです．オイラー自身による証明は煩雑な計算を伴うものでしたが，後にロドリゲスがこれと等価な計算を直接的に示しました．

Olinde Rodrigues, Des lois géométriques qui régissent les déplacements d'un système solide dans l'espace, et de la variation des coordonnées provenant de ces déplacements considérés indépendamment des causes qui peuvent les produire, Journal de Mathématiques Pures et Appliquées, 1re série, tome 5, pp. 380-440, 1840.

現代的なベクトル表記を用いてロドリゲスの結論を記述すれば，「任意の回転を表す（i.e. 鏡像変換を含まない）$3\times 3$正規直交行列$\boldsymbol{R}$は，ある3次元単位ベクトル$\boldsymbol{n}=[n_{x}~n_{y}~n_{z}]^{\mathrm{T}}$とあるスカラー$\theta$を用いて

\boldsymbol{R}=\boldsymbol{1}+\sin\theta[\boldsymbol{n}\times]+(1-\cos\theta)[\boldsymbol{n}\times]^{2}

と表せる」ということになります． ²³ 「任意の回転を表す（鏡像を含まない）」とは，$\boldsymbol{R}$の張る座標系が常に右手系である，と仮定するのと同義です．
$\boldsymbol{1}$は$3\times 3$単位行列です．
$[\boldsymbol{n}\times]$は次のように定義しました．

[\boldsymbol{n}\times]=\begin{bmatrix}
0 & -n_{z} & n_{y} \\
n_{z} & 0 & -n_{x} \\
-n_{y} & n_{x} & 0
\end{bmatrix}

任意の3次元ベクトル$\boldsymbol{v}$に対し，$[\boldsymbol{n}\times]\boldsymbol{v}\equiv\boldsymbol{n}\times\boldsymbol{v}$が恒等的に成り立つので，これは外積行列と呼ばれることもあります．
また，$[\boldsymbol{n}\times]^{2}\equiv\boldsymbol{n}\boldsymbol{n}^{\mathrm{T}}-\boldsymbol{1}$も恒等的に成り立ちますので，上の式は

\boldsymbol{R}=\cos\theta\boldsymbol{1}+\sin\theta[\boldsymbol{n}\times]+(1-\cos\theta)\boldsymbol{n}\boldsymbol{n}^{\mathrm{T}}

と書き換えることも出来ます．

$\boldsymbol{n}$と$\theta$の組$(\theta,\boldsymbol{n})$は角軸表現と呼ばれ，合計4個のパラメータを持ちますが，$\boldsymbol{n}$は単位ベクトルですので等式制約条件$\|\boldsymbol{n}\|=1$が課されます．そこで，$\theta$と$\boldsymbol{n}$の積に相当する$\boldsymbol{\xi}=\theta\boldsymbol{n}$を，本記事では角軸ベクトルと呼ぶことにします． ⁴ $\boldsymbol{\xi}$は任意の長さをとることができますので，パラメータ数は3個になります．

ちなみに，上記のロドリゲスの論文は，いわゆるオイラー・ロドリゲス・パラメータ ⁵ を提案した論文としても有名です．これは，角軸表現から導出されるものですが，ハミルトンが1844年に提案した四元数（クォータニオン）のノルムが1であるもの（単位クォータニオン）を用いた姿勢表現と実質的に同じものです． ⁶

単位クォータニオンに関する情報は角軸ベクトルに比べて大変充実しているので，本記事では解説しませんが，姿勢表現に用いる単位クォータニオンを単にクォータニオンとしか呼んでいない文献が散見されます．これは誤りですので注意しましょう．

等価角軸変換

角軸表現から姿勢行列への変換

「はじめに」に書いた通り，ある3つの数の組が姿勢を表せるということは，姿勢行列とその数の組が相互変換可能であるということです．

まずは角軸表現$(\theta,\boldsymbol{n})$から姿勢行列への変換を求めましょう．リー代数に基づく精緻な議論ではなく，直感的に理解しやすい幾何学的方法で導出します． ⁷

上図のように，原点を通り$\boldsymbol{n}$を方向ベクトルとする軸まわりに，空間中のある点Aを角度$\theta$回転させて点A'とすることを考えます．回転方向は，$\boldsymbol{n}$の指す方を向いて右まわり（時計回り）を正とします（いわゆる右ねじの法則です）．点A，A'の位置ベクトルをそれぞれ$\boldsymbol{a}$，$\boldsymbol{a}^{\prime}$とおきます．点AからA'まで回転する軌跡は，点Aを通り$\boldsymbol{n}$に直交する平面上の円弧となります．

$\boldsymbol{n}\times\boldsymbol{a}$は，（$\boldsymbol{n}$に直交するので）明らかにこの平面上のベクトルとなります．さらにこれと$\boldsymbol{n}$との外積$\boldsymbol{n}\times(\boldsymbol{n}\times\boldsymbol{a})$も$\boldsymbol{n}$に直交し，かつ$\boldsymbol{n}\times\boldsymbol{a}$にも直交します．したがって，これらの二つのベクトル$\boldsymbol{n}\times\boldsymbol{a}$および$\boldsymbol{n}\times(\boldsymbol{n}\times\boldsymbol{a})$によってこの平面は張られます．

ここで，円弧の半径を$r$，$\boldsymbol{n}$と$\boldsymbol{a}$のなす角を$\phi$とおけば，次が成り立ちます．

\displaylines{
\|\boldsymbol{n}\times\boldsymbol{a}\|=\|\boldsymbol{n}\|\|\boldsymbol{a}\|\sin\phi=r \\
\|\boldsymbol{n}\times(\boldsymbol{n}\times\boldsymbol{a})\|
=\|\boldsymbol{n}\|\|\boldsymbol{n}\times\boldsymbol{a}\|\sin\frac{\pi}{2}
=\|\boldsymbol{n}\times\boldsymbol{a}\|=r
}

つまり、この二つのベクトルの長さはどちらも円弧の半径に一致します．
下図のように回転軸を真上から見たとき，弧AA'を含む平面と軸との交点に$\boldsymbol{n}\times(\boldsymbol{n}\times\boldsymbol{a})$を継ぎ足せば点Aに至りますし，同じ点に$\boldsymbol{n}\times\boldsymbol{a}$を継ぎ足せば，点Aから円弧に沿って90°回した点に至ることが分かります．

これを踏まえて，上図中の点Aから点Bを経由して点A'に至ることを考えると，$\boldsymbol{a}^{\prime}$は次のように求まります．

\displaylines{
\boldsymbol{a}^{\prime}
&=&\boldsymbol{a}+\sin\theta\boldsymbol{n}\times\boldsymbol{a}+(1-\cos\theta)\boldsymbol{n}\times(\boldsymbol{n}\times\boldsymbol{a})
\\
&=&\left\{\boldsymbol{1}+\sin\theta[\boldsymbol{n}\times]+(1-\cos\theta)[\boldsymbol{n}\times]^{2}\right\}\boldsymbol{a}
}

すなわち，点Aを点A'に変換する行列$\boldsymbol{R}$は次式のように表せます．

\boldsymbol{R}=\boldsymbol{1}+\sin\theta[\boldsymbol{n}\times]+(1-\cos\theta)[\boldsymbol{n}\times]^{2}

前節に記したロドリゲスの公式に一致しました．

姿勢行列から角軸ベクトルへの変換

さて，ここからが本記事の主題です．任意の姿勢行列$\boldsymbol{R}$から角軸ベクトルへの変換を求めましょう．予め，$\boldsymbol{R}$を次のように成分表示しておきます．

\boldsymbol{R}=\begin{bmatrix}
r_{11} & r_{12} & r_{13} \\
r_{21} & r_{22} & r_{23} \\
r_{31} & r_{32} & r_{33}
\end{bmatrix}

ロドリゲスの公式を成分に書き下すと，次のようになります．

\boldsymbol{R}
=\begin{bmatrix}
 1-(1-\cos\theta)(n_{y}^{2}+n_{z}^{2}) &
 -\sin\theta n_{z}+(1-\cos\theta)n_{x}n_{y} &
  \sin\theta n_{y}+(1-\cos\theta)n_{z}n_{x} \\
  \sin\theta n_{z}+(1-\cos\theta)n_{x}n_{y} &
 1-(1-\cos\theta)(n_{z}^{2}+n_{x}^{2}) &
 -\sin\theta n_{x}+(1-\cos\theta)n_{y}n_{z} \\
 -\sin\theta n_{y}+(1-\cos\theta)n_{z}n_{x} &
  \sin\theta n_{x}+(1-\cos\theta)n_{y}n_{z} &
 1-(1-\cos\theta)(n_{x}^{2}+n_{y}^{2})
\end{bmatrix}

これらを見比べることで，次式を得ます．

\displaylines{
\mathrm{tr}{\boldsymbol{R}}(=r_{11}+r_{12}+r_{13})
=3-2(1-\cos\theta)(n_{z}^{2}+n_{y}^{2}+n_{z}^{2})
=1+2\cos\theta
\\
\begin{cases}
r_{32}-r_{23}=2\sin\theta n_{x} \\
r_{13}-r_{31}=2\sin\theta n_{y} \\
r_{21}-r_{12}=2\sin\theta n_{z}
\end{cases}
\qquad\Leftrightarrow\qquad
\boldsymbol{l}=2\sin\theta\boldsymbol{n}
}

ただし，$\boldsymbol{l}=\left[r_{32}-r_{23}~r_{13}-r_{31}~r_{21}-r_{12}\right]^{\mathrm{T}}$とおき，$\|\boldsymbol{n}\|^{2}=n_{z}^{2}+n_{y}^{2}+n_{z}^{2}=1$を用いました．よって，$\theta$は次のように求まります．

\theta=\mathrm{atan2}\left(\|\boldsymbol{l}\|,\mathrm{tr}\boldsymbol{R}-1\right) + 2N\pi

再度，$\|\boldsymbol{n}\|=1$を利用しました．$N$は任意の整数です．

このように，姿勢行列から角軸表現への変換は多価関数となるので注意が必要です．以降は$N=0$である，つまり$-\pi<\theta\leq\pi$であることを仮定し，常に

\theta=\mathrm{atan2}\left(\|\boldsymbol{l}\|,\mathrm{tr}\boldsymbol{R}-1\right)

と選ぶことにします．

この計算は，$\|\boldsymbol{l}\|$と$\mathrm{tr}\boldsymbol{R}-1$が同時に$0$になる状況が存在しないことを暗に仮定しています（もしそのような状況が存在したら，$\theta$は不定になってしまいます）．このことを補題として証明しておきましょう．

まず，回転によるベクトルの変換はノルムを変化させませんので，$\boldsymbol{R}$の固有値は全て絶対値が$1$になります．$\|\boldsymbol{l}\|=0$すなわち$\boldsymbol{l}=\boldsymbol{0}$となるのは，$r_{32}=r_{23}$かつ$r_{13}=r_{31}$かつ$r_{21}=r_{12}$，すなわち$\boldsymbol{R}$が対称行列となる場合です．対称行列の固有値は必ず実数になりますから，このような$\boldsymbol{R}$の固有値は$1$か$-1$のどちらかです．$\boldsymbol{R}$の張る座標系が右手系ならば，$\boldsymbol{R}$の行列式は$\det\boldsymbol{R}=1$となります．行列式は全ての固有値の積でもあるので，結局，$\boldsymbol{R}$の固有値の組み合わせは$(1,1,1)$か$(1,-1-1)$のどちらかとなります．
3つの固有値が$(1,1,1)$となる$\boldsymbol{R}$は，単位行列$\boldsymbol{1}$以外にありません．このとき，$\mathrm{tr}\boldsymbol{R}-1=1+1+1-1=2\neq 0$が保証されます．3つの固有値が$(1,-1,-1)$となるときは，行列の固有値とトレースの関係からやはり$\mathrm{tr}\boldsymbol{R}=1+(-1)+(-1)=-1\neq 0$が保証されます．よって，$\|\boldsymbol{l}\|$と$\mathrm{tr}\boldsymbol{R}-1$が同時に$0$になることはありません．（証明終）

さて，$\boldsymbol{l}\neq\boldsymbol{0}$であるならば，それを正規化することで$\boldsymbol{n}$が得られます．

\boldsymbol{n}=\frac{\boldsymbol{l}}{\|\boldsymbol{l}\|}

一方，$\boldsymbol{l}=\boldsymbol{0}$である場合，$\theta=\mathrm{atan2}(0,2)=0$または$\theta=\mathrm{atan2}(0,-1)=\pi$となります．

$\theta=0$であるならば上の証明にあるように$\boldsymbol{R}=\boldsymbol{1}$，つまり姿勢は変化していないことになるので，$\boldsymbol{n}$は不定となります．これは困ることではなく，$\boldsymbol{n}$が何であっても構わない，ということです．

$\theta=\pi$であるならば，ロドリゲスの公式より$\boldsymbol{R}$は次のような対称行列になっているはずです．

\boldsymbol{R}
=\begin{bmatrix}
 2n_{x}^{2}-1 &
 2n_{x}n_{y} &
 2n_{z}n_{x} \\
 2n_{x}n_{y} &
 2n_{y}^{2}-1 &
 2n_{y}n_{z} \\
 2n_{z}n_{x} &
 2n_{y}n_{z} &
 2n_{z}^{2}-1
\end{bmatrix}

便宜上，$n_{x}=n_{1}$，$n_{y}=n_{2}$，$n_{z}=n_{3}$と書き換えることにすると，次のようにも書けます．

\begin{cases}
 n_{i}^{2}=\frac{\displaystyle r_{ii}+1}{\displaystyle 2} & (\forall i\in\{1,2,3\}) \\
 n_{i}n_{j}=\frac{\displaystyle r_{ij}}{\displaystyle 2} & (\forall\in\{1,2,3\}, \forall j\in\{1,2,3\}, i\neq j)
\end{cases}

この式からは$n_{i}$の符号が決まりません．しかし$n_{i}$の符号が反転すると$n_{j}$の符号も反転するので，結局$\boldsymbol{n}$自体が反転することになります．回転方向が真逆になるということですが，今，回転角は$\theta=\pi$ですので，右回りでも左回りでも最終的な姿勢は同じになります．つまり，どちらの符号を選んでも構わない，ということです．
$n_{i}$（$i=1,2,3$）が全て同時に$0$にならないことは自明なので，零除算を避けるために，

1. まず$n_{i}=\sqrt{\frac{\displaystyle r_{ii}+1}{\displaystyle 2}}$（$i=1,2,3$）を求める
2. これらの最大値を与える$i$を求める
3. $i=1$ならば$(j,k)=(2,3)$，$i=2$ならば$(j,k)=(3,1)$，$i=3$ならば$(j,k)=(1,2)$とする
4. $n_{j}=\frac{\displaystyle r_{ij}}{\displaystyle 2n_{i}}$，$n_{k}=\frac{\displaystyle r_{ik}}{\displaystyle 2n_{i}}$とする

という手順で$\boldsymbol{n}$を求めれば良いです．

以上で，$\boldsymbol{R}$から$(\theta,\boldsymbol{n})$への変換ができるようになりました．$(\theta,\boldsymbol{n})$から$\boldsymbol{\xi}=\theta\boldsymbol{n}$への変換は直接的ですので，$\boldsymbol{R}$から$\boldsymbol{\xi}$への変換も同時に得られたことになります．
最後に$\boldsymbol{\xi}$から$(\theta,\boldsymbol{n})$への変換ですが，

\displaylines{
\theta=\pm\|\boldsymbol{\xi}\| \\
\boldsymbol{n}=\begin{cases}
\boldsymbol{\xi}/\theta & (\theta\neq 0\mbox{のとき}) \\
\mbox{任意の単位ベクトル} & (\theta=0\mbox{のとき})
\end{cases}
}

となります．またも$\theta$の符号が決まりませんが，$\theta$が反転すると$\boldsymbol{n}$も反転し，$(\theta,\boldsymbol{n})$と$(-\theta,-\boldsymbol{n})$は同じ姿勢になるので，これもどちらを選んでも構いません．
$\theta=0$のときは$\boldsymbol{n}$は任意の単位ベクトルとなりますが，このときは$\boldsymbol{\xi}$を$(\theta,\boldsymbol{n})$に変換するまでもなく$\boldsymbol{R}=\boldsymbol{1}$と決まります．

ジンバルロックを起こさず，ノルムがそのまま姿勢変化の大きさを表している点で，姿勢表現として角軸ベクトルは好ましいものです．一方で，ベクトルの加減算と姿勢変化が直接の対応を持たないので，回転操作をする場合には毎度別の表現（姿勢行列など）に変換してやる必要があります．もう一つ，姿勢行列からの逆変換において$\theta$の値域を$-\pi<\theta\leq\pi$に限定したり，符号選択を$+$に決めたりしたことで，ある角軸ベクトルを姿勢行列にしたのちにまた角軸ベクトルに変換しても，元とは異なるベクトルになってしまう可能性があります．これらは実用上，注意すべきことです．

Zeoでの実装

Zeoでは，$\boldsymbol{\xi}$から$\boldsymbol{R}$への変換をzMat3DFromAA()，$\boldsymbol{R}$から$\boldsymbol{\xi}$への変換をzMat3DToAA()としてそれぞれ次のように実装しています．

/* create a 3D attitude matrix from an angle-axis vector. */
zMat3D *zMat3DFromAA(zMat3D *m, const zVec3D *aa)
{
  double l, c, s, r1, r2, r3;
  zVec3D n, nl;

  if( _zVec3DIsTiny( aa ) ) return zMat3DIdent( m );
  l = _zVec3DNorm( aa );
  _zSinCos( l, &s, &c );
  zVec3DMul( aa, 1.0/l, &n );
  zVec3DMul( &n, 1-c, &nl );
  _zVec3DTripleProdToMat3D( &n, &nl, m );
  r1 = s * n.c.x;
  r2 = s * n.c.y;
  r3 = s * n.c.z;
  m->e[0][0] += 1.0;
  m->e[1][0] -= r3;
  m->e[2][0] += r2;
  m->e[0][1] += r3;
  m->e[1][1] += 1.0;
  m->e[2][1] -= r1;
  m->e[0][2] -= r2;
  m->e[1][2] += r1;
  m->e[2][2] += 1.0;
  return m;
}

/* convert a 3D attitude matrix to an angle-axis vector. */
zVec3D *zMat3DToAA(const zMat3D *m, zVec3D *aa)
{
  double l, a;
  int i0, i1, i2;

  _zVec3DCreate( aa, m->c.yz-m->c.zy, m->c.zx-m->c.xz, m->c.xy-m->c.yx );
  l = _zVec3DNorm( aa );
  a = atan2( l, m->c.xx+m->c.yy+m->c.zz-1 );
  if( zIsTiny( a )  ) return zVec3DZero( aa ); /* identity case */
  if( !zIsTiny( l ) ) return zVec3DMulDRC( aa, a/l ); /* regular case */
  /* singular case (a=pi) */
  i0 = m->c.xx > m->c.yy ? ( m->c.xx > m->c.zz ? 0 : 2 ) : ( m->c.yy > m->c.zz ? 1 : 2 );
  i1 = ( i0 + 1 ) % 3;
  i2 = ( i0 + 2 ) % 3;
  aa->e[i0] = sqrt( 0.5 * m->e[i0][i0] + 0.5 );
  aa->e[i1] = 0.5 * m->e[i0][i1] / aa->e[i0];
  aa->e[i2] = 0.5 * m->e[i0][i2] / aa->e[i0];
  return zVec3DMulDRC( aa, a );
}

ほか，角軸ベクトルの合成や角軸ベクトルによる点の直接回転，オイラー・ロドリゲス・パラメータとの相互変換などを用意しています．

1. たとえばこちらの記事は，その奥深い数理の入口をとても分かり易く説明されています． ↩

2. このような表現を最初にしたのは，1901年のウィルソンの著書と言われています． ↩

3. 内山・中村はその著書の中で，この式をロドリゲスの公式と呼ぶのは後の学者の誤った引用によるもの，としています．筆者の考えではこれは言い過ぎで，この式と数学的に等価な計算は確かにオイラーが示しましたが，このような形に整理したのはロドリゲスで間違いありません． ↩

4. 軸角ベクトル，回転ベクトルと呼ばれることもあるようです． ↩

5. 単にオイラー・パラメータと呼ぶ人も多いですが，オイラーはこのようなパラメータの存在までは示していませんでした． ↩

6. このことは1845年にケイリーが指摘し，クラインが1884年の著書で体系化しました． ↩

7. こんな分かりやすい動画をYouTubeに上げている方もいます． ↩