articles:quaternion [RoboLab.Suehiro]

この文書は読取専用です。文書のソースを閲覧することは可能ですが、変更はできません。もし変更したい場合は管理者に連絡してください。
====== 四元数 ======
四元数は複素数を拡張したもので3つの虚数単位$i, j , k$を用いて
$$
q = q_w + q_x i + q_y j + q_z k
$$
と表される数である．
ここで，$q_w,q_x, q_y, q_z$は実数であり,
$$
i^2 = j^2 = k^2 = ijk = -1
$$
となっている．$i,j,k$相互の積は，
$$
ij = k,\; jk = i, \; ki = j, \; ji = -k, \; kj = -i, \; ik=-j
$$
となり虚数単位同士の交換則が成り立たっていないことに注意する必要がある．

===== 四元数によるベクトルの回転の表現 =====

ベクトル$ \boldsymbol{a} = \left( \begin{array}{c} a_x \\ a_y \\ a_z \end{array} \right)$を\\
回転軸$ \boldsymbol{n} = \left( \begin{array}{c} n_x \\ n_y \\ n_z \end{array} \right)$の周りに
回転角$\theta$で回転させることを考える．

$\boldsymbol{n}$は正規化されている．
また$ - \pi \le \theta \le \pi $である．

これを四元数で以下のように表現する．

まずベクトル$\boldsymbol{a}$に対応する四元数を
$$
a = a_x i + a_y j + a_z k
$$
と表わす．
そのとき回転は以下の四元数$q$とその共役四元数$\overline{q}$の2つの四元数で表現される．
$$
q = \mathrm{cos}\,{\theta \over 2} + (n_x i + n_y j + n_z k)\mathrm{sin}\,{\theta \over 2} \tag{1} 
$$
$$
\overline{q} = \mathrm{cos}\,{\theta \over 2} - (n_x i + n_y j + n_z k)\mathrm{sin}\,{\theta \over 2}
$$
$\boldsymbol{a}$を回転した
ベクトル$\boldsymbol{b}=\left( \begin{array}{c} b_x \\ b_y \\ b_z \end{array} \right) $ は，
$$
b = qa\overline{q} = b_x i + b_y j + b_z k
$$
として得られる((この節はちょっと天下り的．すみません．))  ．

===== 回転を表す四元数と直交行列 =====

四元数で回転を表すとき，その絶対値（大きさ）は$1$である．すなわち，
$$
q = q_w + q_x i + q_y j + q_z k
$$
としたとき，
$$
|q|=\sqrt{q \overline{q}} = \sqrt{ q_w^2 + q_x^2 + q_y^2 + q_z^2 } = 1
$$
また，式(1)で$ -\pi/2 \le \theta/2 \le \pi/2$なので$0 \le q_w \le 1$.

この四元数から直交行列を導くのは座標軸ベクトルがどのように変換されるかを計算すれば簡単に求められる
(([[articles:matrix#直交行列と直交座標系|直交行列と直交座標系]]での説明のように直行行列は座標系の基底を並べたものになっている))．
ベクトル$\boldsymbol{x}$に対応する四元数は，
$$
x = i
$$
これを四元数で回転させると
$$
\begin{eqnarray}
x' = q\,i\,\overline{q} 
&=& (q_w + q_x i + q_y j + q_z k)(q_w i - q_x ii - q_y ij - q_z ik) \\
&=& (q_w + q_x i + q_y j + q_z k)(q_w i + q_x - q_y k + q_z j) \\

&=& (q_w^2 i + q_w q_x - q_w q_y k + q_w q_z j) \\
 &+& (- q_x q_w + q_x^2 i + q_x q_y j + q_x q_z k) \\
 &+& (- q_y q_w k + q_y q_x j - q_y^2 i - q_y q_z) \\
 &+& (q_z q_w j + q_z q_x k + q_z q_y - q_z^2 i) \\

&=& (q_w q_x - q_x q_w - q_y q_z + q_z q_y) \\
 &+& (q_w^2 + q_x^2 - q_y^2  - q_z^2)i \\
 &+& (q_w q_z+q_x q_y + q_y q_x + q_z q_w)j \\
 &+& (- q_w q_y + q_x q_z - q_y q_w + q_z q_x)k \\

&=& 0
 + (q_w^2 + q_x^2 - q_y^2  - q_z^2)i
 + 2(q_y q_x + q_w q_z)j
 + 2(q_z q_x - q_w q_y)k
\end{eqnarray}
$$
よって回転後のベクトル$\boldsymbol{x}'$は，
$$
\boldsymbol{x}' = \left( \begin{array}{c}
q_w^2 + q_x^2 - q_y^2  - q_z^2 \\
2(q_y q_x + q_w q_z) \\
2(q_z q_x - q_w q_y)
\end{array} \right)

$$
同様に((お約束どおりx->yのシフト))y,z軸について求めると
$$
\boldsymbol{y}' = \left( \begin{array}{c}
2(q_x q_y - q_w q_z) \\
q_w^2 + q_y^2 - q_z^2  - q_x^2 \\
2(q_z q_y + q_w q_x) 

\end{array} \right)

$$
$$
\boldsymbol{z}' = \left( \begin{array}{c}

2(q_x q_z + q_w q_y) \\
2(q_y q_z - q_w q_x) \\
q_w^2 + q_z^2 - q_x^2  - q_y^2 
\end{array} \right)

$$
直交行列の形にまとめると
$$
\left( \begin{array}{cc}
q_w^2 + q_x^2 - q_y^2  - q_z^2 & 2(q_x q_y - q_w q_z) & 2(q_x q_z + q_w q_y) \\
2(q_y q_x + q_w q_z) & q_w^2 + q_y^2 - q_z^2  - q_x^2 & 2(q_y q_z - q_w q_x) \\
2(q_z q_x - q_w q_y) & 2(q_z q_y + q_w q_x) & q_w^2 + q_z^2 - q_x^2  - q_y^2 
\end{array} \right)　\tag{2}
$$
===== 直交行列から四元数へ =====
これは基本的には[[articles:rotation#直交行列から回転軸，角度表現を求める|直交行列の回転軸，回転角度を求める]]のと同じである．

$$
A 
= \left( \begin{array}{cc}
a_{xx} & a_{yx} & a_{zx} \\
a_{xy} & a_{yy} & a_{zy} \\
a_{xz} & a_{yz} & a_{zz} 
\end{array} \right)
\tag{3}
$$
式(2)と係数を比較して，
$$
a_{xx} + a_{yy} + a_{zz} = 3 q_w^2 - (q_x^2 + q_y^2  + q_z^2 )
$$
ここで
$$
q_x^2 + q_y^2  + q_z^2 = \mathrm{sin}^2{\theta \over 2} = 1 - \mathrm{cos}^2{\theta \over 2} = 1 - q_w^2 \tag{4}
$$
よって，
$$
a_{xx} + a_{yy} + a_{zz} = 4 q_w^2 - 1
$$
$q_w=\mathrm{cos}{\theta \over 2} \ge 0$を考慮して，
$$
q_w = {1 \over 2} \sqrt{a_{xx} + a_{yy} + a_{zz} + 1}   \tag{5}
$$
また
$$
a_{yz} - a_{zy} = 2(q_z q_y + q_w q_x) - 2(q_y q_z - q_w q_x) = 4 q_w q_x
$$
よって$ q_w \ne 0 $の場合，
$$
q_x = {{a_{yz} - a_{zy}} \over {4 q_w}} \tag{6}
$$
しかし，これでは$q_w$が小さいときに誤差が大きくなるので以下のように解くことにする．
$$
a_{xx} - a_{yy} - a_{zz} = - q_w^2 + 3q_x^2 - q_y^2  - q_z^2 
$$
ここで，式(4)を使い$q_w$を消すと$q_y$，$q_z$も消えて，
$$
a_{xx} - a_{yy} - a_{zz} = 4q_x^2 - 1
$$
すなわち
$$
q_x^2 = {1 \over 4} (a_{xx} - a_{yy} - a_{zz} + 1)
$$
この平方根を求めれば良いのだが，正負の選択が問題になる．これは式(6)を利用することで決定できる．

したがって，
$$
q_x = \mathrm{sign}(a_{yz} - a_{zy}){1 \over 2} \sqrt{a_{xx} - a_{yy} - a_{zz} + 1} \tag{7}
$$
同様に，
$$
q_y = \mathrm{sign}(a_{zx} - a_{xz}){1 \over 2} \sqrt{- a_{xx} + a_{yy} - a_{zz} + 1} 
$$
$$
q_z = \mathrm{sign}(a_{xy} - a_{yx}){1 \over 2} \sqrt{- a_{xx} - a_{yy} + a_{zz} + 1} 
$$

この計算方法はゼロ割が発生しないので安定して計算ができるが，
実際のプログラムでは計算誤差の影響で平方根の中が極めて小さいときに負になることがあるという点に注意を払う必要がある．
===== 四元数による回転の連鎖と座標系の姿勢表現 =====

たとえば[[articles:rotation#オイラー角による表現|オイラー角]]$ \alpha, \beta, \gamma $の四元数表現を考える．x,y,z軸周りの回転は以下のようになる．
$$
q_\alpha(\alpha) = \mathrm{cos}{\alpha \over 2} + i\,\mathrm{sin}{\alpha \over 2}
$$
$$
q_\beta(\beta) = \mathrm{cos}{\beta \over 2} + j\,\mathrm{sin}{\beta \over 2}
$$
$$
q_\gamma(\gamma) = \mathrm{cos}{\gamma \over 2} + k\,\mathrm{sin}{\gamma \over 2} 
$$
これを合成した回転はこれらの積で表現される．
$$
q_{\alpha\beta\gamma}(\alpha, \beta, \gamma) = q_\alpha(\alpha)q_\beta(\beta) q_\gamma(\gamma)
$$
これでベクトル$\boldsymbol{v}$（四元数表現は$v=i\,v_x + j\,v_y + k\,v_z$）を回転させると
$$
v' 
= q_{\alpha\beta\gamma}(\alpha, \beta, \gamma)\, v\, \overline{q_{\alpha\beta\gamma}}(\alpha, \beta, \gamma)
= q_\alpha(\alpha)q_\beta(\beta) q_\gamma(\gamma)\, v\, \overline{q_\gamma}(\gamma)\overline{q_\beta}(\beta) \overline{q_\alpha}(\alpha)
$$
となる．

また回転を表す四元数は絶対値が1なので，
$$
q\,\overline{q}=1
$$
すなわち$q$の逆元（逆変換）が$\overline{q}$となる．

[[articles:matrix|直交行列]]で述べたように座標系の姿勢の表現と
座標系やベクトルの回転変換は基本的に同じものである．
四元数で回転変換を表現できるということは，すななわち四元数は座標系の姿勢の表現になっている．
四元数を座標系の姿勢の表現としてみると，四元数の虚部の係数はベクトルの成分表示になっているので
どの座標系での四元数表現を意識する必要がある．

たとえば座標系$\Sigma_0$から見た座標系$\Sigma_1$の姿勢を$^0q_1$と表現し，
座標系$\Sigma_1$から見た座標系$\Sigma_2$の姿勢を$^1q_2$と表現すると，
座標系$\Sigma_0$から見た座標系$\Sigma_2$の姿勢$^0q_2$は以下のように計算される．
$$
^0q_2 = {^0q_1} \, ^1q_2
$$
という具合である．
===== 四元数による表現と直交行列との比較 =====


四元数による表現を直交行列やオイラー角などの表現と比較すると，

利点は
  * 任意の回転軸を陽に扱える，一軸周りの回転を素直に表せること　\\ これらは直交行列ではRodrigues変換をする必要がある
  * パラメタが4つで冗長パラメタが少ないので正規化しやすい　\\ 直交行列はパラメタが9で直交性を維持するのが難しい
  * ジンバルロックを起こさない（オイラー角と比較して）
  * 回転や座標系の連鎖（積の計算）が軽い（積16和12. 直交行列だと積27和18）

欠点は
  * 座標系の表現と捉えた場合，座標軸がどうなっているか計算しないと分からない
  * 人間にとって分かりにくい（オイラー角と比較して）
  * ベクトルの回転の計算が重い（積28和20((ベクトルの実部の計算を除いている．普通に四元数の積でやると積32,和24))．直交行列だと積9和6）

四元数，直交行列，それぞれ使いどころによると思うが，
原点位置を考慮した座標系の表現や位置ベクトルの変換を考えると
私にはあまり四元数のメリットが見えない．

四元数の一番のメリットは計算誤差の蓄積を防ぐ正規化が容易なことかもしれない．

回転行列の正規直交化は少しならず工夫が必要である．
  *  x => y => z などの順に正規直交化を行う．ただし軸の平等性が崩れる．
  *  ロドリゲス，四元数へ変換した後，正規化を行い，回転行列に戻す．ただし直交行列以外の変換には正当性がない．
  *  特異値分解を行い，対角成分をすべて1((鏡像変換になるのを避ける必要あり))にして戻す．おそらく一番良さそうだが手間がかかる((参考文献：３次元回転，金谷健一．4.5節 回転行列の最適補正))．
いずれにしても少々手間がかかるので都度やるのは面倒である．

===== プログラムと練習問題 =====

[[articles:geo_basic#QUATERNION|四元数のプログラム]]

[[articles:geo_manual#四元数|練習問題]]