More than 3 years have passed since last update.

角速度から回転行列を求める - 理論編

Last updated at 2021-06-15Posted at 2021-06-13

はじめに

Inertial Measurement Unit (IMU)で取得したセンサデータをセンサ座標系から基準座標系に変換するとき、回転行列が必要になります。回転行列の一番簡単な計算方法は角速度から求める方法だと思います。私はその方法がわかっているようでわかっていなかったので、きちんとまとめてみました。

回転行列をただ使いたいだけの方は、Kalman filter、Madgwick filter、各種プログラム言語のライブラリ、スマートフォンのAPIから得られる回転ベクトル（Android、iOS）などを使用することをお勧めします。

結論

時刻tに観測した3次元角速度$\boldsymbol{w}(t) \in \mathbb{R}^{3}$を$\boldsymbol{n} \in so(3)$へ変換します
$$\boldsymbol{n} = \frac{\boldsymbol{w}(t)dt}{||\boldsymbol{w}(t)dt||} \tag{1}$$

回転量$\theta$を求めます。
$$\theta = ||\boldsymbol{w}(t)dt|| \tag{2}$$

$\boldsymbol{n}$を交代行列$\boldsymbol{N}$へ変換します
$$\boldsymbol{N}=[\boldsymbol{n}]_{\times} \tag{3}$$

$\boldsymbol{N}$を$\boldsymbol{\Omega} \in SO(3)$へ変換します
$$\boldsymbol{\Omega} = \boldsymbol{I} + (sin\theta)\boldsymbol{N} + (1-cos\theta)\boldsymbol{N}^{2} \tag{4}$$

$\boldsymbol{\Omega}(t)$で$\boldsymbol{R}(t) \in SO(3)$を更新します
$$\boldsymbol{R}(t)=\boldsymbol{R}(t-1)\boldsymbol{\Omega}(t) \tag{5}$$
$$\boldsymbol{R}(0)=\boldsymbol{I}$$

以上です。

補足

より詳しい情報が知りたい方向けに補足します。

式(1)の補足

$\boldsymbol{n}$は回転軸の方向を示す単位ベクトルであり、$so(3)$はそういった単位ベクトルの集合を示しています（正確な定義ではないかもしれません）。

式(2)の補足

$\theta$は$\boldsymbol{n}$軸上での回転量を示しています。

式(3)の補足

この操作の目的は、３次元ベクトルである$\boldsymbol{n}$を３ｘ３の交代行列である$\boldsymbol{N}$に変換することです。ベクトルの外積は交代行列とベクトルの積で表現できるという性質があります。この性質を用いて、$\boldsymbol{n}$の交代行列$[\boldsymbol{n}]_{\times}$を導出していきます。

はじめに$\boldsymbol{n}$と任意の３次元ベクトル$\boldsymbol{v} \in \mathbb{R}^{3}$を以下のように定義します。

\boldsymbol{n} = 
\begin{pmatrix}
n_x \\
n_y \\
n_z
\end{pmatrix}\\
\boldsymbol{v} = 
\begin{pmatrix}
v_x \\
v_y \\
v_z
\end{pmatrix}

また、交代行列の性質より以下の式が成り立ちます。

$$[\boldsymbol{n}]_{\times} \boldsymbol{v} = \boldsymbol{n} \times \boldsymbol{v}$$

右辺を展開し、左辺と比較します。

\begin{pmatrix}
(\boldsymbol{n} \times \boldsymbol{v})_{x} \\
(\boldsymbol{n} \times \boldsymbol{v})_{y} \\
(\boldsymbol{n} \times \boldsymbol{v})_{z}
\end{pmatrix}
= 
\begin{pmatrix}
n_{y}v_{z}-n_{z}v_{y} \\
n_{z}v_{x}-n_{x}v_{z} \\
n_{x}v_{y}-n_{y}v_{x}
\end{pmatrix}
=
\begin{pmatrix}
0 & -n_{z} & n_{y} \\
n_{z} & 0 & -n_{x} \\
-n_{y} & n_{x} & 0
\end{pmatrix}
\begin{pmatrix}
v_x \\
v_y \\
v_z
\end{pmatrix}
= 
[\boldsymbol{n}]_{\times}
\begin{pmatrix}
v_x \\
v_y \\
v_z
\end{pmatrix}\\

$\boldsymbol{n}$を用いて$[\boldsymbol{n}]_{\times}$が表現できました。

[\boldsymbol{n}]_{\times}
= 
\begin{pmatrix}
0 & -n_{z} & n_{y} \\
n_{z} & 0 & -n_{x} \\
-n_{y} & n_{x} & 0
\end{pmatrix}

式(4)の補足

ここでは、式(2)で求めた回転量$\theta$と式(3)で求めた交代行列$\boldsymbol{N}$とロドリゲスの回転公式を用いて、回転行列$\boldsymbol{\Omega}$を求めます。SO(3)は３次の特殊直行群を示します。$\boldsymbol{I}$は３ｘ３の単位行列です。式(4)は行列指数関数を用いて、以下のように示されている場合もありますが意味は同じです。
$$\boldsymbol{\Omega}=\exp(\boldsymbol{N})$$

式(4)では無限小回転の場合の議論も可能です。
$\theta$が微小であるとき。$\sin\theta\approx\theta$、$\cos\theta\approx1$です。つまり式(4)は以下の式で近似できます。
$$\boldsymbol{\Omega}=\boldsymbol{I} + \theta\boldsymbol{N}$$

ここで、連続した２つの無限小回転$\boldsymbol{\Omega}_1$と$\boldsymbol{\Omega}_2$を考えます。そして、$\boldsymbol{\Omega}_1$と$\boldsymbol{\Omega}_2$積を展開します。
$$\boldsymbol{\Omega}_1 \boldsymbol{\Omega}_2 = (\boldsymbol{I} + \theta_1\boldsymbol{N}_1)(\boldsymbol{I} + \theta_2\boldsymbol{N}_2)=\boldsymbol{I}+\theta_1\boldsymbol{N}_1+\theta_2\boldsymbol{N}_2+\theta_1\theta_2\boldsymbol{N}_1\boldsymbol{N}_2$$

微小量$\theta_1$と$\theta_2$の積の項を無視すると、$\boldsymbol{\Omega}_1$と$\boldsymbol{\Omega}_2$の積は可換であり、無限小回転の性質を満たしていることがわかります。
$$\boldsymbol{\Omega}_1 \boldsymbol{\Omega}_2=\boldsymbol{I}+\theta_1\boldsymbol{N}_1+\theta_2\boldsymbol{N}_2= \boldsymbol{\Omega}_2 \boldsymbol{\Omega}_1 $$

式(5)の補足

ここでは、時刻$t$におけるセンサー座標系を基準座標系（初期座標系）に変換する回転行列$\boldsymbol{R}(t)$を求めます。回転行列$\boldsymbol{R}(t-1)$に回転行列$\boldsymbol{\Omega}(t)$をかけることで$\boldsymbol{R}(t)$に更新できます。$\boldsymbol{\Omega}(t)$は時刻$t$のセンサー座標系を時刻$t-1$のセンサ座標系に変換する回転行列です。

回転行列の使用例

例えば、デバイス座標系（DCS）で時刻$t$に観測した加速度$\boldsymbol{a}^{DCS}(t)$を基準座標系（RCS）に変換するときは以下の式で実現できます。
$$\boldsymbol{a}^{RCS}(t) = \boldsymbol{R}(t-1)\boldsymbol{a}^{DCS}(t)$$

続編

角速度から回転行列を求める - 実装編

参考

You get articles that match your needs
You can efficiently read back useful information
You can use dark theme

What you can do with signing up