LoginSignup
7
7

More than 1 year has passed since last update.

@take4eng

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

Last updated at Posted at 2021-07-01

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)$ とする。

  1. 初期の座標系を x-y-z とする
  2. x 軸周りで α 回転する
  3. 回転後のy' 軸周りで β 回転する
  4. 回転後の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/

7
7
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
7
7