IMUセンサーと四元数（クォーターニオン）

　慣性センサーなど制御系で個人的に使用する式だけ抜き出したたんなる覚書です。

　まずクォーターニオン $q$ を次のようにします。

$$\begin{align} q = q_\omega + q_xi + q_yj + q_zk = \begin{bmatrix} q_\omega\\ q_x\\ q_y\\ q_z\\ \end{bmatrix}\\\\ i^2=j^2=k^2=ijk=-1 \end{align}$$

　表現が$q=\begin{bmatrix}q_x&q_y&q_z&q_w\end{bmatrix}^T$となっていたり、$q=\begin{bmatrix}q_1&q_2&q_3&q_4\end{bmatrix}^T$とか$q=\begin{bmatrix}q_0&q_1&q_2&q_3\end{bmatrix}^T$となっていたりいろいろあり紛らわしいので明記しておきます。

クォーターニオンの積

　クォーターニオン同士の積です。

$$\begin{align} q_1q_2 &= \left( \omega_1 + x_1i + y_1j + z_1k \right)\left( \omega_2 + x_2i + y_2j + z_2k \right)\\ &= \left(\omega_1\omega_2 – x_1x_2 -y_1y_2-z_1z_2\right)\\ &+ \left( \omega_1 x_2 +\omega_2 x_1 + y_1z_2 – z_1y_2\right)i\\ &+ \left( \omega_1 y_2 +\omega_2 y_1 + z_1x_2 – x_1z_2\right)j\\ &+ \left( \omega_1 z_2 +\omega_2 z_1 + x_1y_2 – y_1x_2\right)k\\ \end{align}$$

▲展開して各項でまとめるとこのようになります。

　ベクトル表記すると▼

$$\begin{align} q_1q_2 &= \left( \omega_1\omega_2 – \vec{v_1}\cdot\vec{v_2}\quad,\quad \omega_1\vec{v_2}+\omega_2\vec{v_1} + \vec{v_1}\times\vec{v_2}\right)\\\\ &内積：\vec{v_1}\cdot\vec{v_2}= x_1 x_2 + y_1 y_2 + z_1 z_2 \\ &外積：\left(y_1z_2-z_1y_2 , z_1x_2-x_1z_2 , x_1y_2-y_1x_2 \right) \end{align}$$

　内積と外積で実部、虚部に分けてすっきり表現できます。

オイラー角からクォーターニオン

　任意の軸（単位ベクトル）$\vec{n}= \left(n_x, n_y, n_z\right)$で$\theta$回転させるときのクォーターニオンは

$$\begin{align} q = \begin{bmatrix} \cos\frac{\theta}{2}\\ n_x\sin\frac{\theta}{2}\\ n_y\sin\frac{\theta}{2}\\ n_z\sin\frac{\theta}{2}\\ \end{bmatrix} \end{align}$$

で表現されます。

　xyzの各軸廻りはそれぞれ

$$\begin{align} q_x = \begin{bmatrix} \cos\frac{\theta_x}{2}\\ n_x\sin\frac{\theta_x}{2}\\ 0\\ 0\\ \end{bmatrix} \quad q_y = \begin{bmatrix} \cos\frac{\theta_y}{2}\\ 0\\ n_y\sin\frac{\theta_y}{2}\\ 0\\ \end{bmatrix} \quad q_z = \begin{bmatrix} \cos\frac{\theta_z}{2}\\ 0\\ 0\\ n_z\sin\frac{\theta_z}{2}\\ \end{bmatrix} \end{align}$$

で表現され、クォーターニオンの積より、オイラー角（3軸回転xyz順）からクォーターニオンへの変換は、

$$\begin{align} q_xq_yq_z = \begin{bmatrix} -\sin\frac{\theta_x}{2}\sin\frac{\theta_y}{2}\sin\frac{\theta_z}{2} +\cos\frac{\theta_x}{2}\cos\frac{\theta_y}{2}\cos\frac{\theta_z}{2}\\ \cos\frac{\theta_x}{2}\sin\frac{\theta_y}{2}\sin\frac{\theta_z}{2} +\sin\frac{\theta_x}{2}\cos\frac{\theta_y}{2}\cos\frac{\theta_z}{2}\\ -\sin\frac{\theta_x}{2}\cos\frac{\theta_y}{2}\sin\frac{\theta_z}{2} +\cos\frac{\theta_x}{2}\sin\frac{\theta_y}{2}\cos\frac{\theta_z}{2}\\ \cos\frac{\theta_x}{2}\cos\frac{\theta_y}{2}\sin\frac{\theta_z}{2} +\sin\frac{\theta_x}{2}\sin\frac{\theta_y}{2}\cos\frac{\theta_z}{2}\\ \end{bmatrix} \end{align}$$

　このように展開されます。

クォーターニオンから回転行列

　クォーターニオンから回転行列への変換です。ベクトル$\vec{r}=\left(r_x,r_y,r_z\right)$をクォーターニオン$q$で回転させると

$$\begin{align} \vec{r^{\prime}} &= q\vec{r}q^*\\ &=\left(q_\omega+q_xi+q_yj+q_zk\right)\left(r_xi+r_yj+r_zk\right)\left(q_\omega-q_xi-q_yj-q_zk\right) \end{align}$$

で表現されます。で頑張って展開して、対応する$i,j,k$成分で行列式の形でまとめ直すと

$$\begin{align} \begin{bmatrix} r^\prime_x\\ r^\prime_y\\ r^\prime_z\\ \end{bmatrix}= \begin{bmatrix} q_\omega^2+q_x^2-q_y^2-q_z^2 & 2\left(q_xq_y-q_{\omega}q_z\right) & 2\left(q_{\omega}q_y+q_xq_z\right)\\ 2\left(q_{\omega}q_z+q_xq_y\right) & q_\omega^2-q_x^2+q_y^2-q_z^2 & 2\left(q_yq_z-q_{\omega}q_x\right)\\ 2\left(q_xq_z-q_{\omega}q_y\right) & 2\left(q_yq_z+q_{\omega}q_x\right) & q_\omega^2-q_x^2-q_y^2+q_z^2 \end{bmatrix} \begin{bmatrix} r_x\\ r_y\\ r_z\\ \end{bmatrix} \end{align}$$

　となります。

　よく使いそうな式の単なる個人的な覚書です。