クォータニオンの基礎

JPLとHamiltonの使い分け

回転を扱うときの表記でHamiltonとJPL(Jet Propulsion Laboratory)という2つの表記があることを知った。まず初めに両者の使い分けについて示す。

参考

https://hal.archives-ouvertes.fr/hal-01122406v3/document

https://upcommons.upc.edu/bitstream/handle/2117/104997/1773-Quaternion-kinematics-for-the-error-state-Kalman-filter.pdf

定義

Hamilton JPL
\vec{q}の表現順序 ( q_{w}, q_{x}, q_{y}, q_{z}) ( q_{x}, q_{y}, q_{z}, q_{w})
i \times j k -k
系 右手系 左手系
Active/Passive Passive Passive
座標系が左→右にいくにつれて GlobalからLocalの座標系へ LocalからGlobalの座標系へ

回転を表現する時に、座標系は固定で位置ベクトルを回転させるのがActive、位置ベクトルは固定で座標系を反対方向に回転させるのがPassive。

クォータニオンの表現

クォータニアンどうしに対して和差積の演算が定義できる。

Hamilton記法

\begin{align} (w, x, y, z) = (w, \vec{v})= w+xi + yj+zk \\\ w^2 + x^2 + y^2 + z^2 = 1 \\\ I = (1, \vec{0}) \end{align}

Iは単位クォータニオン（全く回転させない）を表す。

\begin{align} i^{2} = j^{2} = k^{2} &=& -1\\\ i \cdot j = -j \cdot i &=& k\\\ j \cdot k = -k \cdot j &=& i\\\ k \cdot i = -i \cdot k &=& j \end{align}

JPL記法

\begin{align} (x, y, z, w) = (\vec{v}, w)= w+xi + yj+zk \\\ w^2 + x^2 + y^2 + z^2 = 1 \\\ I = (\vec{0}, 1) \end{align}

Iは単位クォータニオン（全く回転させない）を表す。

\begin{align} i^{2} = j^{2} = k^{2} &=& -1\\\ i \cdot j = -j \cdot i &=& -k\\\ j \cdot k = -k \cdot j &=& -i\\\ k \cdot i = -i \cdot k &=& -j \end{align}

定理

Hamilton記法

\begin{equation} (w_{1}, \vec{v}_{1}) \cdot (w_{2}, \vec{v}_{2}) = (w_{1}w_{2}-\vec{v}_{1}\cdot \vec{v}_{2},w_{1}\vec{v}_{2}+w_{2}\vec{v}_{1}+\vec{v}_{1}\times \vec{v}_{2}) \end{equation}

q = (w, \vec{v})の逆元は、|q| = \sqrt{w^{2}+\vec{v}^{2}}として q^{-1} = \frac{1}{|q|^{2}}(w, - \vec{v})

q=(\cos\frac{\theta}{2}, \sin\frac{\theta}{2}\vec{n})と表現し直した時、このクォータニオンは\vec{n}まわりの右手系での\theta回転を表す。

JPL記法

\begin{equation} (w_{1}, \vec{v}_{1}) \cdot (w_{2}, \vec{v}_{2}) = ? \end{equation}

q = (\vec{v}, w)の逆元は、|q| = \sqrt{w^{2}+\vec{v}^{2}}として q^{-1} = \frac{1}{|q|^{2}}(- \vec{v}, w)

q=( \sin\frac{\theta}{2}\vec{n}, \cos\frac{\theta}{2})と表現し直した時、このクォータニオンは\vec{n}まわりの左手系での\theta回転、すなわち右手系での -\theta回転を表す。

Hamilton表記とJPL表記の関係性

\begin{eqnarray} {}_{A}^{B}q(J) = {}_{B}^{A}q(J)^{*} = {}_{A}^{B}q(H)^{*} \\\ {}_{A}^{B}R(J) = {}_{B}^{A}R(J)^\mathsf{T} = {}_{A}^{B}R(H)^\mathsf{T} \end{eqnarray}

Hamiltonは右手系、JPLは左手系を使っていて、左手系での \theta回転は右手系での -\theta回転に相当するため、A系からB系への同じ変換であっても両者は一致せず逆元の関係になる。

回転の実施

位置ベクトルを回転するか座標系を回転するかでクォータニオンの演算が異なることに注意。

座標系を固定した上での位置ベクトルの回転

位置\vec{r}を表すクォータニオンを\vec{p} = (0, \vec{r})と表記した時に、\vec{r}を\vec{n}まわりに\theta回転した後のクォータニオンは、 q=(\cos\frac{\theta}{2}, \sin\frac{\theta}{2}\vec{n})を使って以下のように表せる。

\begin{equation} \vec{p}^{'} = \vec{q}\ \vec{p}\ \vec{q}^{-1} \end{equation}

q=(w, \vec{v})として実際に計算すると

\begin{equation} \vec{p}^{'} = (0,\ \ (w^{2}-\vec{v}^{2})\vec{v}\ +\ 2(\vec{r}\cdot \vec{v})\vec{v}\ +\ 2w\vec{v}\times\vec{r}) \end{equation}

第0成分が0のままになっていることが確認できる。 q=(\cos\frac{\theta}{2}, \sin\frac{\theta}{2}\vec{n})を使って表すと

\begin{equation} \vec{p}^{'} = (0, \cos\theta \vec{r} + \sin\theta (\vec{n}\times\vec{r})+(1-\cos \theta)(\vec{n}\cdot\vec{r})\vec{n}) \end{equation}

となり、この第1~3成分 \vec{r}^{'} \equiv \cos\theta \vec{r} + \sin\theta (\vec{n}\times\vec{r})+(1-\cos \theta)(\vec{n}\cdot\vec{r})\vec{n}は \vec{r}を \vec{n}まわりに \theta回転させた後のベクトルを表すので、クォータニオンの演算により回転が実施されたことがわかった。注意すべきなのは、ここでの回転は「姿勢角の回転」ではなく「原点周りの位置ベクトルの回転」を表すということである。 f:id:salpik:20201103172134j:plain

位置ベクトルを固定した上での座標系の回転

「座標系を固定した上での位置ベクトルの回転」と逆の操作となり

\begin{equation} \vec{p}^{'} = \vec{q}^{-1}\ \vec{p}\ \vec{q} \end{equation}

表現の自由度

回転角\thetaを2\pi + \thetaとしても同じ操作であるが、

\begin{equation} (\cos(\frac{\theta + 2\pi}{2}), \sin(\frac{\theta + 2\pi}{2})\vec{n}) = (-\cos \frac{\theta}{2}, -\sin \frac{\theta}{2}\vec{n}) \end{equation}

の変形からわかるようにクォータニオンとしては異なる。つまり qと -qで同じ回転を表すことになり回転表現の自由度があるので、クォータニオンどうしを比較したい場合は定義に気を付ける必要がある（最初の0でない成分を正にするなど）

回転行列との対応

Hamilton記法

クォータニオンに相当する回転演算を回転行列の形で書くことを考える。

\begin{eqnarray} q_{1} q_{2} &=& (w_{1}, x_{1}, y_{1}, z_{1})\ (w_{2}, x_{2}, y_{2}, z_{2}) \\\ &=& (w_{1} + x_{1} \vec{i} + y_{1} \vec{j} + z_{1} \vec{k}) (w_{2} + x_{2} \vec{i} + y_{2} \vec{j} + z_{2} \vec{k}) \\\ &=& (w_{1}w_{2} - x_{1}x_{2} - y_{1} y_{2} - z_{1}z_{2}) + (w_{1}x_{2} + x_{1}w_{2} + y_{1}z_{2} - z_{1}y_{2})\vec{i} \\\ && (w_{1}y_{2} + y_{1}w_{2} - x_{1}z_{2} + z_{1}x_{2})\vec{j} + (w_{1}z_{2} + z_{1}w_{2} + x_{1}y_{2} - y_{1}x_{2})\vec{k} \end{eqnarray}


\begin{equation} q_{1} q_{2} = \left( \begin{array}{cccc} w_{1} & -x_{1} & -y_{1} & -z_{1} \\ x_{1} & w_{1} & -z_{1} & y_{1} \\ y_{1} & z_{1} & w_{1} & -x_{1} \\ z_{1} & -y_{1} & x_{1} & w_{1} \end{array} \right) \left( \begin{array}{c} w_{2} \\ x_{2} \\ y_{2} \\ z_{2} \end{array} \right) = \left( \begin{array}{cccc} w_{2} & -x_{2} & -y_{2} & -z_{2} \\ x_{2} & w_{2} & z_{2} & -y_{2} \\ y_{2} & -z_{2} & w_{2} & x_{2} \\ z_{2} & y_{2} & -x_{2} & w_{2} \end{array} \right) \left( \begin{array}{c} w_{1} \\ x_{1} \\ y_{1} \\ z_{1} \end{array} \right) \end{equation}


クォータニアンを左あるいは右から演算することはそれぞれ上記行列をかけることに対応する。それゆえ

\begin{align} (0, X^{'}, Y^{'}, Z^{'}) &=& q(0, X, Y, Z)q^{-1} \\\ &=& \left( \begin{array}{cccc} w & -x & -y & -z \\ x & w & -z & y \\ y & z & w & -x \\ z & -y & x & w \end{array} \right) \left( \begin{array}{cccc} w & x & y & z \\ x & -w & -z & y \\ y & z & -w & -x \\ z & -y & x & -w \end{array} \right) \left( \begin{array}{c} 0 \\ X \\ Y \\ Z \end{array} \right) \\\ &=& \left( \begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & x^{2}+w^{2}-z^{2}-y^{2} & 2(xy-wz) & 2(xz+wy) \\ 0 & 2(xy+wz) & y^{2}-z^{2}+w^{2}-x^{2} & 2(yz-xw) \\ 0 & 2(xz-yw) & 2(yz+wx) & z^{2}-y^{2}-x^{2}+w^{2} \end{array} \right) \left( \begin{array}{c} 0 \\ X \\ Y \\ Z \end{array} \right) \end{align}

まとめると、 q=(w, x, y, z)に対応する3×3の回転行列は

\begin{equation} R \equiv \left( \begin{array}{ccc} x^{2}+w^{2}-z^{2}-y^{2} & 2(xy-wz) & 2(xz+wy) \\ 2(xy+wz) & y^{2}-z^{2}+w^{2}-x^{2} & 2(yz-xw) \\ 2(xz-yw) & 2(yz+wx) & z^{2}-y^{2}-x^{2}+w^{2} \end{array} \right) \end{equation}

と表される。 RR^{T}が単位行列になることも確認できる。

JPL記法

\begin{eqnarray} q_{1} q_{2} &=& (x_{1}, y_{1}, z_{1}, w_{1})\ (x_{2}, y_{2}, z_{2}, w_{2}) \\\ &=& (w_{1} + x_{1} \vec{i} + y_{1} \vec{j} + z_{1} \vec{k}) (w_{2} + x_{2} \vec{i} + y_{2} \vec{j} + z_{2} \vec{k}) \\\ &=& (w_{1}w_{2} - x_{1}x_{2} - y_{1} y_{2} - z_{1}z_{2}) + (w_{1}x_{2} + x_{1}w_{2} - y_{1}z_{2} + z_{1}y_{2})\vec{i} \\\ && (w_{1}y_{2} + y_{1}w_{2} + x_{1}z_{2} - z_{1}x_{2})\vec{j} + (w_{1}z_{2} + z_{1}w_{2} - x_{1}y_{2} + y_{1}x_{2})\vec{k} \end{eqnarray}


\begin{equation} q_{1} q_{2} = \left( \begin{array}{cccc} w_{1} & z_{1} & -y_{1} & x_{1} \\ -z_{1} & w_{1} & x_{1} & y_{1} \\ y_{1} & -x_{1} & w_{1} & z_{1} \\ -x_{1} & -y_{1} & -z_{1} & w_{1} \end{array} \right) \left( \begin{array}{c} w_{2} \\ x_{2} \\ y_{2} \\ z_{2} \end{array} \right) = \left( \begin{array}{cccc} w_{2} & -z_{2} & y_{2} & x_{2} \\ z_{2} & w_{2} & -x_{2} & y_{2} \\ -y_{2} & x_{2} & w_{2} & z_{2} \\ -x_{2} & -y_{2} & -z_{2} & w_{2} \end{array} \right) \left( \begin{array}{c} w_{1} \\ x_{1} \\ y_{1} \\ z_{1} \end{array} \right) \end{equation}


特に w_{1}\sim0、 x_{1}, y_{1}, z_{1} \ll 1のとき

\begin{eqnarray} q_{1} q_{2} &\sim& \left( \begin{array}{cccc} 0 & z_{1} & -y_{1} & x_{1} \\ -z_{1} & 0 & x_{1} & y_{1} \\ y_{1} & -x_{1} & 0 & z_{1} \\ -x_{1} & -y_{1} & -z_{1} & 0 \end{array} \right) \left( \begin{array}{c} w_{2} \\ x_{2} \\ y_{2} \\ z_{2} \end{array} \right) \sim \left( \begin{array}{cccc} 0 & -z_{2} & y_{2} & x_{2} \\ z_{2} & 0 & -x_{2} & y_{2} \\ -y_{2} & x_{2} & 0 & z_{2} \\ -x_{2} & -y_{2} & -z_{2} & 0 \end{array} \right) \left( \begin{array}{c} w_{1} \\ x_{1} \\ y_{1} \\ z_{1} \end{array} \right) \\\ &=& \left( \begin{array}{cc} - \lfloor \vec{r}_{1} \rfloor & \vec{r}_{1} \\ -\vec{r}_{1}^\mathsf{T} & 0 \end{array} \right) \left( \begin{array}{c} w_{2} \\ x_{2} \\ y_{2} \\ z_{2} \end{array} \right) \sim \left( \begin{array}{cc} \lfloor \vec{r}_{2} \rfloor & \vec{r}_{2} \\ -\vec{r}_{2}^\mathsf{T} & 0 \end{array} \right) \left( \begin{array}{c} w_{1} \\ x_{1} \\ y_{1} \\ z_{1} \end{array} \right) \end{eqnarray}

ここで \lfloor \vec{r} \rfloor はskew symmetric matrixと呼ばれる。

クォータニオンの合成

位置ベクトルを回転させるか座標系を回転させるかで合成の順序が異なるので注意。

座標系を固定した上での位置ベクトルの回転の場合

点 pに対して q_{1}の回転あとに q_{2}の回転を実施した場合

\begin{equation} q_{2}(q_{1}pq_{1}^{-1})q_{2}^{-1} = (q_{2}q_{1})p(q_{2}q_{1})^{-1} \end{equation}

となるので q_{2} q_{1}の回転を1度実施したことと等価。

位置ベクトルを固定した上での座標系の回転

q_{1}の回転あとに q_{2}の回転を実施した場合

\begin{equation} q_{2}^{-1}(q_{1}^{-1}pq_{1})q_{2} = (q_{1}q_{2})^{-1}p(q_{1}q_{2}) \end{equation}

となるので q_{1} q_{2}の回転を1度実施したことと等価。

座標変換の合成

SE(3)である剛体座標変換として座標系 Aから座標系 Bへの変換を表す便利な表記として2種類がある。
1: 行列

\begin{equation} T_{A}^{B} \equiv \left( \begin{array}{cc} R_{A}^{B} & t_{A}^{B} \\ \vec{0}^{T} & 1 \end{array} \right) \end{equation}

という4×4の行列で表す。ここで R_{A}^{B}は3次元空間における回転を表しSO(3)群である。 t_{A}^{B}は並進を表す3次元ベクトルであり、座標系 Aの原点が座標系 Bでどこの座標となるかを表す。座標変換を合成すると

\begin{equation} T_{A}^{C} = T_{B}^{C} T_{A}^{B} = \left( \begin{array}{cc} R_{B}^{C} R_{A}^{B} & R_{B}^{C} t_{A}^{B} + t_{B}^{C} \\ \vec{0}^{T} & 1 \end{array} \right) \end{equation}

となる。

2: 並進ベクトルとクォータニオンのペア
並進ベクトルとクォータニオンのペア (\vec{p}, q)で表す方法がある。 T_{A}^{B} = (\vec{p}_{1}, q_{1})、 T_{B}^{C} = (\vec{p}_{2}, q_{2})の場合、座標変換を合成すると

\begin{equation} T_{A}^{C} = (q_{2}q_{1}, (\vec{p}_{2}, 0) + q_{2}(\vec{p}_{1}, 0)q_{2}^{-1}) \end{equation}

となる。

クォータニオン の微分

\begin{eqnarray} \dot{q} &=& \frac{1}{\Delta t} \left( q(t+\Delta t) - q(t) \right) \\\ &=& \frac{1}{\Delta t} \left( q(\Delta t) q(t) - q(t) \right) \ \ \ \ \ (q(t)だけ回転させた後にq(\Delta t)回転させると考える) \\\ &=& \frac{1}{\Delta t} \left[ (n_{x}\sin(\frac{\omega \Delta t}{2}), n_{y}\sin(\frac{\omega \Delta t}{2}), n_{z}\sin(\frac{\omega \Delta t}{2}), \cos (\frac{\omega \Delta t}{2}))q(t) -q(t)\right] \\\ &\sim& \frac{1}{\Delta t} \left[ (n_{x}\frac{\omega \Delta t}{2}, n_{y}\frac{\omega \Delta t}{2}, n_{z}\frac{\omega \Delta t}{2}, 1) q(t) -q(t)\right] \\\ &=& \frac{1}{\Delta t} \left[ (\frac{\omega_{x} \Delta t}{2}, \frac{\omega_{y} \Delta t}{2}, \frac{\omega_{z} \Delta t}{2}, 1) - (0, 0, 0, 1)\right] q(t) \\\ &=& \frac{1}{\Delta t} (\vec{\omega}\Delta t, 0) q(t) \\\ &=& \frac{1}{2\Delta t} \left[ \left( \begin{array}{cccc} 1 & -\frac{1}{2}\omega_{z}\Delta t & -\frac{1}{2}\omega_{y}\Delta t & \frac{1}{2}\omega_{x}\Delta t \\ -\frac{1}{2}\omega_{z}\Delta t & 1 & +\frac{1}{2}\omega_{x}\Delta t & \frac{1}{2}\omega_{y}\Delta t \\ \frac{1}{2}\omega_{y}\Delta t & -\frac{1}{2}\omega_{x}\Delta t & 1 & \frac{1}{2}\omega_{z}\Delta t \\ -\frac{1}{2}\omega_{x}\Delta t & -\frac{1}{2}\omega_{y}\Delta t & -\frac{1}{2}\omega_{z}\Delta t & 1 \end{array} \right) - I_{4}\right] q(t) \\\ &=& \frac{1}{2} \left( \begin{array}{cccc} 0 & \omega_{z} & -\omega_{y} & \omega_{x} \\ -\omega_{z} & 0 & \omega_{x} & \omega_{y} \\ \omega_{y} & -\omega_{x} & 0 & \omega_{z} \\ -\omega_{x} & -\omega_{y} & -\omega_{z} & 0 \end{array} \right) q(t) \\\ &\equiv& \frac{1}{2} \left( \begin{array}{cc} -\lfloor \vec{\omega} \rfloor & \vec{\omega} \\ -\vec{\omega} & 0 \end{array} \right) q(t) \\\ &\equiv& \frac{1}{2} \Omega(\vec{w}) q(t) \end{eqnarray}

Local pertubationの展開

A系からB系への変換に対して、Local座標での摂動がある場合の展開を考える。Hamilton記法では

\begin{eqnarray} {}_{A}^{B}R(H) &=& {}_{A}^{B}\hat{R}(H)\tilde{R} \ \ \ \ \ (local摂動なのでHamilton記法では\hat{R}の右から微笑回転行列をかける) \\\ &=& {}_{A}^{B}\hat{R}(H)\exp(\vec{\theta}(H)) \\\ &=& {}_{A}^{B}\hat{R}(H)(I + \lfloor \vec{\theta} \rfloor) \end{eqnarray}

一方JPL記法で書くと

\begin{eqnarray} {}_{A}^{B}R(J) &=& \tilde{R{}_{A}^{B}\hat{R}(J)} \ \ \ \ \ (local摂動なのでJPL記法では\hat{R}の左から微笑回転行列をかける) \\\ &=& \exp(\vec{\theta}(J)) {}_{A}^{B}\hat{R}(J) \\\ &=& \exp(-\vec{\theta}(H)) {}_{A}^{B}\hat{R}(J) \\\ &=& (I - \lfloor \vec{\theta} \rfloor){}_{A}^{B}\hat{R}(J) \end{eqnarray}

JPL記法の展開に対して転置をとると

\begin{eqnarray} {}_{A}^{B}R(J)^\mathsf{T} &=& {}_{A}^{B}R(H) \\\ \left( (I - \lfloor \vec{\theta} \rfloor){}_{A}^{B}\hat{R}(J) \right)^\mathsf{T} &=& {}_{A}^{B}\hat{R}(J)^\mathsf{T} (I - \lfloor \vec{\theta} \rfloor )^\mathsf{T} \\\ &=& {}_{A}^{B}\hat{R}(H)(I+\lfloor \vec{\theta} \rfloor) \end{eqnarray}

となり

\begin{equation} {}_{A}^{B}R(H) = {}_{A}^{B}\hat{R}(H)(I+\lfloor \vec{\theta} \rfloor) \end{equation}

すなわちHamilton記法と一致することが示せた。

球面線形補間

q_{1}を始点、 q_{2}の終点とした回転処理を行う場合で、その間を連続的に q(t) (0 \le t \le 1)補間したクォータニオン q(t)を求めたい場合

\begin{equation} q(t) = (q_{2} q_{1}^{-1})^{t} q_{1} \end{equation}

ここで、余弦定理を使うと q_{2}と q_{1}^{-1}のなす角度、すなわち q_{2}と q_{1}のなす角度 \phiは

\begin{equation} \cos\phi = q_{1} \cdot q_{2} \equiv w_{1}w_{2} + \vec{v}_{1}\cdot \vec{v}_{2} \end{equation}

と表される（余弦定理によりもとまる角度がクォータニオン4次元空間におけるなす角であるという考え方に基づく）。したがって、

\begin{equation} (q_{2} q_{1}^{-1}) = (\cos \phi, \sin \phi\vec{n}) \end{equation}

と表せる。球面線形補間は適当な係数 a(t)、 b(t)を使った線形結合として

\begin{equation} (q_{2} q_{1}^{-1})^{t} = a(t) (1, \vec{0}) + b(t) (\cos \phi, \sin \phi \vec{n}) \end{equation}

と表現される。こちらを参照すると

\begin{equation} (q_{2} q_{1}^{-1})^{t} = \frac{\sin t \phi}{\sin \phi} (1, \vec{0}) + \frac{\sin (1-t)\phi}{\sin \phi} (\cos \phi, \sin \phi \vec{n}) \end{equation}

と表されることがわかるので、最終的には

\begin{equation} q(t) = \frac{\sin t \phi}{\sin \phi} q_{1} + \frac{\sin (1-t)\phi}{\sin \phi} q_{2} \ \ \ \ \ (0 \le t \le 1) \end{equation}

はまったこと

回転を考える時に矢印で考えてはいけない。図のように人間で考えないといけない。後ろ向きに向かせる回転であっても違った表現ができることに注意

f:id:salpik:20210226231916p:plain
人間の回転

参考

CGのためのクォータニオン - Qiita

クォータニオンと回転 - エフアンダーバー

https://www.mss.co.jp/technology/report/pdf/18-07.pdf