3次元姿勢表現の相互変換
最近流行りのクォータニオン。
オイラー角からクォータニオンへの変換方法が見当たらなかったので、記載しておく。
回転行列、オイラー角、クォータニオンの相互変換は、下記の記事に詳しく記載されている。
本記事は、オイラー角 ⇒ クォータニオン の計算過程を補足するものと考える。
クォータニオンの積
クォータニオン自体は下記のように表現される。
\begin{align}
\boldsymbol{q}
& = q_0+q_1\boldsymbol{i}+q_2\boldsymbol{j}+q_3\boldsymbol{k} \\
& = q_0+\boldsymbol{q}_i
\end{align} \\
ここで虚数部を $\boldsymbol{q}_i = q_1\boldsymbol{i}+q_2\boldsymbol{j}+q_3\boldsymbol{k}$ とする。
※ 注意
クォータニオンの表現を $\boldsymbol{q} = q_1\boldsymbol{i}+q_2\boldsymbol{j}+q_3\boldsymbol{k}+q_4$ とすることもある。
後述する列ベクトル表記では記載順が変わるため、よく確認すること。
上式の虚数 $\boldsymbol{i},\boldsymbol{j},\boldsymbol{k}$ は、それぞれの積に対して下記の性質を持つ。
\boldsymbol{i}^2 = \boldsymbol{j}^2 = \boldsymbol{k}^2 = \boldsymbol{ijk} = -1 \\
\boldsymbol{ij} = -\boldsymbol{ji} = \boldsymbol{k} \\
\boldsymbol{jk} = -\boldsymbol{kj} = \boldsymbol{i} \\
\boldsymbol{ki} = -\boldsymbol{ik} = \boldsymbol{j} \\
事前準備として、2つの虚数部 $\boldsymbol{q}_i$ と $\boldsymbol{q}_i'$ の積を考える。
\begin{align}
\boldsymbol{q}_i\boldsymbol{q}_i'
& =
(q_1\boldsymbol{i}+q_2\boldsymbol{j}+q_3\boldsymbol{k})(q_1'\boldsymbol{i}+q_2'\boldsymbol{j}+q_3'\boldsymbol{k}) \\
\\
& = q_1q_1'\boldsymbol{i}^2+q_1q_2'\boldsymbol{i}\boldsymbol{j}+q_1q_3'\boldsymbol{i}\boldsymbol{k} \\
& + q_2q_1'\boldsymbol{j}\boldsymbol{i}+q_2q_2'\boldsymbol{j}^2+q_2q_3'\boldsymbol{j}\boldsymbol{k} \\
& + q_3q_1'\boldsymbol{k}\boldsymbol{i}+q_3q_2'\boldsymbol{k}\boldsymbol{j}+q_3q_3'\boldsymbol{k}^2 \\
\\
& = -(q_1q_1'+q_2q_2'+q_3q_3') \\
& + (q_2q_3'-q_3q_2')\boldsymbol{i} \\
& + (q_3q_1'-q_1q_3')\boldsymbol{j} \\
& + (q_1q_2'-q_2q_1')\boldsymbol{k} \\
\\
& = -\boldsymbol{q}_i\cdot\boldsymbol{q}_i'+\boldsymbol{q}_i\times\boldsymbol{q}_i'
\end{align}
上記の計算より、クォータニオン虚数部の積は、内積と外積を用いて表すことができる。
続いて、本題のクォータニオンの積を考える。
2つのクォータニオン $\boldsymbol{q}$ と $\boldsymbol{q}'$ の積は下記のようになる。
\begin{align}
\boldsymbol{q} \boldsymbol{q}'
& = (q_0+\boldsymbol{q}_i)(q_0'+\boldsymbol{q}_i') \\
& = q_0q_0'+q_0\boldsymbol{q}_i'+q_0'\boldsymbol{q}_i+\boldsymbol{q}_i\boldsymbol{q}_i' \\
& = q_0q_0'+q_0\boldsymbol{q}_i'+q_0'\boldsymbol{q}_i-\boldsymbol{q}_i\cdot\boldsymbol{q}_i'+\boldsymbol{q}_i\times\boldsymbol{q}_i'
\end{align} \\
まとめると、 クォータニオンの積は、簡単なベクトル計算で表すことができる
\boldsymbol{q} \boldsymbol{q}' = q_0q_0'+q_0\boldsymbol{q}_i'+q_0'\boldsymbol{q}_i-\boldsymbol{q}_i\cdot\boldsymbol{q}_i'+\boldsymbol{q}_i\times\boldsymbol{q}_i'
姿勢クォータニオン
クォータニオンの中でも、今回は3次元姿勢を表現するものを扱う。
3次元姿勢を表すクォータニオンは、下記のように表現される。
ここで $\boldsymbol{n}=(n_x,n_y,n_z)$ を回転軸、$\theta$ を回転角度として用いる。
\boldsymbol{q} =
\begin{pmatrix}
q_0 \\ q_1 \\ q_2 \\ q_3 \\
\end{pmatrix}
=
\begin{pmatrix}
\cos \frac{\theta}{2} \\
n_x \sin \frac{\theta}{2} \\
n_y \sin \frac{\theta}{2} \\
n_z \sin \frac{\theta}{2} \\
\end{pmatrix}
=
\cos \frac{\theta}{2} + \boldsymbol{n} \sin \frac{\theta}{2}
さらに、回転軸$\boldsymbol{n}$ が x軸、y軸、z軸である場合、$\boldsymbol{n}$、$\boldsymbol{q}$は下記のようになる。
\boldsymbol{n}_x = (1,0,0)\qquad\boldsymbol{n}_y = (0,1,0)\qquad\boldsymbol{n}_z = (0,0,1) \\
\boldsymbol{q}_x =
\begin{pmatrix}
\cos \frac{\theta}{2} \\ \sin \frac{\theta}{2} \\ 0 \\ 0 \\
\end{pmatrix}
\qquad
\boldsymbol{q}_y =
\begin{pmatrix}
\cos \frac{\theta}{2} \\ 0 \\ \sin \frac{\theta}{2} \\ 0 \\
\end{pmatrix}
\qquad
\boldsymbol{q}_z =
\begin{pmatrix}
\cos \frac{\theta}{2} \\ 0 \\ 0 \\ \sin \frac{\theta}{2} \\
\end{pmatrix} \\
オイラー角からの変換
前項までで、『クォータニオンの積』と『各軸に対応するクォータニオン』を得た。
オイラー角をクォータニオンに変換するには、オイラー角の軸順と同順にクォータニオンの積を取れば導出できる。
例として**『オイラー角XYZ系』**に対応するクォータニオンを計算する。
この時の回転角度を $(\theta_x,\theta_y,\theta_z) = (\alpha,\beta,\gamma)$ とする。
- 初期の座標系を x-y-z とする
- x 軸周りで α 回転する
- 回転後のy' 軸周りで β 回転する
- 回転後のz'' 軸周りで γ 回転する
なお、数式の記載上は $\sin\theta = S\theta , \cos\theta = C\theta$ とする。
\boldsymbol{q}_{xyz} = \boldsymbol{q}_x\boldsymbol{q}_y\boldsymbol{q}_z
\small
\begin{align}
\boldsymbol{q}_x\boldsymbol{q}_y
& = \begin{pmatrix}
C\frac{\alpha}{2} \\ S\frac{\alpha}{2} \\ 0 \\ 0 \\
\end{pmatrix}
\begin{pmatrix}
C\frac{\beta}{2} \\ 0 \\ S\frac{\beta}{2} \\ 0 \\
\end{pmatrix} \\
& =
C\frac{\alpha}{2}C\frac{\beta}{2}
+C\frac{\alpha}{2}
\begin{pmatrix}
0 \\ S\frac{\beta}{2} \\ 0 \\
\end{pmatrix}
+C\frac{\beta}{2}
\begin{pmatrix}
S\frac{\alpha}{2} \\ 0 \\ 0 \\
\end{pmatrix}
- \begin{pmatrix}
S\frac{\alpha}{2} \\ 0 \\ 0 \\
\end{pmatrix}
\cdot
\begin{pmatrix}
0 \\ S\frac{\beta}{2} \\ 0 \\
\end{pmatrix}
+ \begin{pmatrix}
S\frac{\alpha}{2} \\ 0 \\ 0 \\
\end{pmatrix}
\times
\begin{pmatrix}
0 \\ S\frac{\beta}{2} \\ 0 \\
\end{pmatrix} \\
& =
C\frac{\alpha}{2}C\frac{\beta}{2}
+\begin{pmatrix}
0 \\ C\frac{\alpha}{2}S\frac{\beta}{2} \\ 0 \\
\end{pmatrix}
+\begin{pmatrix}
S\frac{\alpha}{2}C\frac{\beta}{2} \\ 0 \\ 0 \\
\end{pmatrix}
- 0
+ \begin{pmatrix}
0 \\ 0 \\ S\frac{\alpha}{2}S\frac{\beta}{2} \\
\end{pmatrix} \\
& = \begin{pmatrix}
C\frac{\alpha}{2}C\frac{\beta}{2} \\ S\frac{\alpha}{2}C\frac{\beta}{2} \\ C\frac{\alpha}{2}S\frac{\beta}{2} \\ S\frac{\alpha}{2}S\frac{\beta}{2} \\
\end{pmatrix}
\end{align} \\
\scriptsize
\begin{align}
\boldsymbol{q}_x\boldsymbol{q}_y\boldsymbol{q}_z
& = \begin{pmatrix}
C\frac{\alpha}{2}C\frac{\beta}{2} \\ S\frac{\alpha}{2}C\frac{\beta}{2} \\ C\frac{\alpha}{2}S\frac{\beta}{2} \\ S\frac{\alpha}{2}S\frac{\beta}{2} \\
\end{pmatrix}
\begin{pmatrix}
C\frac{\gamma}{2} \\ 0 \\ 0 \\ S\frac{\gamma}{2} \\
\end{pmatrix} \\
& = C\frac{\alpha}{2}C\frac{\beta}{2}C\frac{\gamma}{2}
+ C\frac{\alpha}{2}C\frac{\beta}{2}
\begin{pmatrix}
0 \\ 0 \\ S\frac{\gamma}{2} \\
\end{pmatrix}
+ C\frac{\gamma}{2}
\begin{pmatrix}
S\frac{\alpha}{2}C\frac{\beta}{2} \\ C\frac{\alpha}{2}S\frac{\beta}{2} \\ S\frac{\alpha}{2}S\frac{\beta}{2} \\
\end{pmatrix}
- \begin{pmatrix}
S\frac{\alpha}{2}C\frac{\beta}{2} \\ C\frac{\alpha}{2}S\frac{\beta}{2} \\ S\frac{\alpha}{2}S\frac{\beta}{2} \\
\end{pmatrix}
\cdot
\begin{pmatrix}
0 \\ 0 \\ S\frac{\gamma}{2} \\
\end{pmatrix}
+ \begin{pmatrix}
S\frac{\alpha}{2}C\frac{\beta}{2} \\ C\frac{\alpha}{2}S\frac{\beta}{2} \\ S\frac{\alpha}{2}S\frac{\beta}{2} \\
\end{pmatrix}
\times
\begin{pmatrix}
0 \\ 0 \\ S\frac{\gamma}{2} \\
\end{pmatrix} \\
& = C\frac{\alpha}{2}C\frac{\beta}{2}C\frac{\gamma}{2}
+ \begin{pmatrix}
0 \\ 0 \\ C\frac{\alpha}{2}C\frac{\beta}{2}S\frac{\gamma}{2} \\
\end{pmatrix}
+ \begin{pmatrix}
S\frac{\alpha}{2}C\frac{\beta}{2}C\frac{\gamma}{2} \\ C\frac{\alpha}{2}S\frac{\beta}{2}C\frac{\gamma}{2} \\ S\frac{\alpha}{2}S\frac{\beta}{2}C\frac{\gamma}{2} \\
\end{pmatrix}
- S\frac{\alpha}{2}S\frac{\beta}{2}S\frac{\gamma}{2}
+ \begin{pmatrix}
C\frac{\alpha}{2}S\frac{\beta}{2}S\frac{\gamma}{2} \\ -S\frac{\alpha}{2}C\frac{\beta}{2}S\frac{\gamma}{2} \\ 0 \\
\end{pmatrix}\\
& = \begin{pmatrix}
C\frac{\alpha}{2}C\frac{\beta}{2}C\frac{\gamma}{2} - S\frac{\alpha}{2}S\frac{\beta}{2}S\frac{\gamma}{2} \\
S\frac{\alpha}{2}C\frac{\beta}{2}C\frac{\gamma}{2} + C\frac{\alpha}{2}S\frac{\beta}{2}S\frac{\gamma}{2} \\
C\frac{\alpha}{2}S\frac{\beta}{2}C\frac{\gamma}{2} - S\frac{\alpha}{2}C\frac{\beta}{2}S\frac{\gamma}{2} \\
C\frac{\alpha}{2}C\frac{\beta}{2}S\frac{\gamma}{2} + S\frac{\alpha}{2}S\frac{\beta}{2}C\frac{\gamma}{2} \\
\end{pmatrix} \\
\\
\end{align} \\
上記の計算過程により、オイラー角XYZ系 $(\alpha,\beta,\gamma)$ をクォータニオンに変換した。
\boldsymbol{q}_{xyz} = \begin{pmatrix}
C\frac{\alpha}{2}C\frac{\beta}{2}C\frac{\gamma}{2} - S\frac{\alpha}{2}S\frac{\beta}{2}S\frac{\gamma}{2} \\
S\frac{\alpha}{2}C\frac{\beta}{2}C\frac{\gamma}{2} + C\frac{\alpha}{2}S\frac{\beta}{2}S\frac{\gamma}{2} \\
C\frac{\alpha}{2}S\frac{\beta}{2}C\frac{\gamma}{2} - S\frac{\alpha}{2}C\frac{\beta}{2}S\frac{\gamma}{2} \\
C\frac{\alpha}{2}C\frac{\beta}{2}S\frac{\gamma}{2} + S\frac{\alpha}{2}S\frac{\beta}{2}C\frac{\gamma}{2} \\
\end{pmatrix} \\
オイラー角は軸順により12通り存在するが、自身が使用する系に合わせ同様の計算を行うことで導出できる。
参考ページ
https://www.mss.co.jp/technology/report/pdf/18-07.pdf
https://uchu-engineer.com/structure-dynamics/dcm-euler-quaternion/