オイラー角からクォータニオンへの変換

# 3次元姿勢表現の相互変換

最近流行りのクォータニオン。
オイラー角からクォータニオンへの変換方法が見当たらなかったので、記載しておく。

回転行列、オイラー角、クォータニオンの相互変換は、下記の記事に詳しく記載されている。
本記事は、オイラー角 ⇒ クォータニオン の計算過程を補足するものと考える。

https://qiita.com/aa_debdeb/items/3d02e28fb9ebfa357eaf

# クォータニオンの積
クォータニオン自体は下記のように表現される。

```math
\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}$ は、それぞれの積に対して下記の性質を持つ。

```math
\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'$ の積を考える。

```math
\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}'$ の積は下記のようになる。

```math
\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} \\
```

まとめると、 **クォータニオンの積は、簡単なベクトル計算で表すことができる**

```math
\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$ を回転角度として用いる。

```math
\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}$は下記のようになる。

```math
\boldsymbol{n}_x = (1,0,0)\qquad\boldsymbol{n}_y = (0,1,0)\qquad\boldsymbol{n}_z = (0,0,1)  \\
```
```math
\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)$ とする。

1. 初期の座標系を x-y-z とする
1. x 軸周りで α 回転する
1. 回転後のy' 軸周りで β 回転する
1. 回転後のz'' 軸周りで γ 回転する

なお、数式の記載上は $\sin\theta = S\theta , \cos\theta = C\theta$ とする。

```math
\boldsymbol{q}_{xyz} = \boldsymbol{q}_x\boldsymbol{q}_y\boldsymbol{q}_z 
```

```math
\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} \\
```

```math
\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)$ をクォータニオンに変換**した。

```math
\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/