地磁気センサで3次元の姿勢推定
地磁気センサから方位のデータを得られる。
方位ベクトルの変位から、3次元空間での姿勢を計算できる。
本記事では、地磁気センサからオイラー角での姿勢推定を行うための内容を記載する。
ちょっとした一言
地磁気センサの役割
地磁気センサでの姿勢計算は、多くの場合は加速度センサやジャイロセンサの補助として用いられる。
本記事の内容も、加速度センサでの姿勢計算を補う形で地磁気センサを用いている。
姿勢計算の流れは下記の通りだが、そのうち 2. について記載する。
- 加速度センサで3軸のうち2軸の角度を計算する。(通常はx軸とy軸が多い)
- 地磁気センサで残り1軸の角度を計算する。(通常はz軸が多い)
加速度センサでの姿勢計算は下記の記事に記載している。
加速度センサと同様の計算を行えば、地磁気センサでもオイラー角の3軸のうち2軸の角度を計算できる。
が、地磁気センサの役割は、加速度センサでは計算できない軸の角度を求めることである。
今回の記事もそれに則って記載する。
観測値の補正
なお、地磁気センサの観測値は周辺環境の状況により真値とのズレが生じる。
姿勢計算に用いる値は、観測値からバイアス値 (ズレ) を補正したものを使用すること。
バイアス値の計算は、観測データから球体フィッティングを行うことで求める。
フィッティングの内容に関しては、下記の記事に記載している。
前提条件
座標系
基準となる静止した座標系を $\boldsymbol{\Sigma}_w$ とする。添え字は$world$ の頭文字。
地磁気センサの姿勢を基準とする座標系を $\boldsymbol{\Sigma}_s$ とする。添え字は$sensor$ の頭文字。
ワールド座標系の地磁気
ワールド座標系 $\boldsymbol{\Sigma}_w$ での地磁気を $\boldsymbol{m}_w = (m _{wx},m _{wy},m _{wz})$ とする。
センサ座標系 $\boldsymbol{\Sigma}_s$ で観測される地磁気を $\boldsymbol{m}_s = (m _{sx},m _{sy},m _{sz})$ とする。
このとき、北方向を y軸とし、$\boldsymbol{m}_w = (0, M, 0)$ として計算を進めていく。
オイラー角の系
オイラー角の系は、加速度センサの系と同一にする必要がある。絶対！！
今回の考えるオイラー角の系を、**『Z-Y-X系』として計算する。
オイラー角の表現は『 $\boldsymbol{(\theta_x, \theta_y, \theta_z )}$ 』**の順で記載する.
例として、Z-Y-X系の$(\gamma , \beta, \alpha )$ が与えられた場合を考える。
- 初期の座標系を x-y-z とする
- z 軸周りで α 回転する
- 回転後のy' 軸周りで β 回転する
- 回転後のx'' 軸周りで γ 回転する
回転行列
各軸の回転に対応する回転行列を以下に示す。
\boldsymbol{R} _x (\theta) =
\begin{pmatrix}
1 & 0 & 0 \\
0 & \cos \theta & -\sin \theta \\
0 & \sin \theta & \cos \theta
\end{pmatrix}
\\
\boldsymbol{R} _y (\theta) =
\begin{pmatrix}
\cos \theta & 0 & \sin \theta \\
0 & 1 & 0 \\
-\sin \theta & 0 & \cos \theta
\end{pmatrix}
\\
\boldsymbol{R} _z (\theta) =
\begin{pmatrix}
\cos \theta & -\sin \theta & 0 \\
\sin \theta & \cos \theta & 0 \\
0 & 0 & 1
\end{pmatrix}
上記の行列を転置させたものを回転行列として用いている解説サイトがあるが、それは『座標回転行列』である。
間違っているわけではないが、回転方向が逆になるため注意が必要。
地磁気データから姿勢計算
前述した設定を元に、地磁気センサの観測値 $\boldsymbol{m}_s$ からオイラー角を計算する。
やることリスト
- 加速度センサで x軸と y軸の角度 $\theta_{x}$ と $\theta _{y}$ を計算する。(済)
- 地磁気センサで残りの z軸の角度 $\theta_{z}$ を計算する。(いまココ)
$\boldsymbol{m}_w$ と $\boldsymbol{m}_s$ の関係は、回転行列を用いて下記のように表される。
\begin{align}
\boldsymbol{m}_w & = \boldsymbol{R}_{zyx}\boldsymbol{m}_s \\
& = \boldsymbol{R}_z(\theta_z)\boldsymbol{R}_y(\theta_y)\boldsymbol{R}_x(\theta_x)\boldsymbol{m}_s
\end{align} \\
上式を少し変形させ、下記の形に書き換える。
\boldsymbol{R}_z(\theta_z)^{-1}\boldsymbol{m}_w = \boldsymbol{R}_y(\theta_y)\boldsymbol{R}_x(\theta_x)\boldsymbol{m}_s \\
これで、計算済みの $\theta_{x},,\theta _{y}$ と、これから計算する $\theta _{z}$ を分離できた。
この数式に、それぞれの行列やベクトルを代入し計算すると、下記のようになる。
記載上は、$\sin \theta$ ⇒ $S\theta$、$\cos \theta$ ⇒ $C\theta$ とする。
\begin{align}
\begin{pmatrix}
C\theta_z & S\theta_z & 0 \\
-S\theta_z & C\theta_z & 0 \\
0 & 0 & 1
\end{pmatrix}
\begin{pmatrix}
0 \\ M \\ 0 \\
\end{pmatrix}
& = \begin{pmatrix}
C\theta_y & 0 & S\theta_y \\
0 & 1 & 0 \\
-S\theta_y & 0 & C\theta_y
\end{pmatrix}
\begin{pmatrix}
1 & 0 & 0 \\
0 & C\theta_x & -S\theta_x \\
0 & S\theta_x & C\theta_x
\end{pmatrix}
\begin{pmatrix}
m_{sx} \\ m_{sy} \\ m_{sz} \\
\end{pmatrix} \\
\\
\begin{pmatrix}
MS\theta_z \\ MC\theta_z \\ 0 \\
\end{pmatrix}
& = \begin{pmatrix}
C\theta_y & S\theta_y S\theta_x & S\theta_y C\theta_x \\
0 & C\theta_x & -S\theta_x \\
-S\theta_y & C\theta_y S\theta_x & C\theta_y C\theta_x
\end{pmatrix}
\begin{pmatrix}
m_{sx} \\ m_{sy} \\ m_{sz} \\
\end{pmatrix} \\
\\
\begin{pmatrix}
MS\theta_z \\ MC\theta_z \\ 0 \\
\end{pmatrix}
& = \begin{pmatrix}
m_{sx}C\theta_y + m_{sy}S\theta_y S\theta_x + m_{sz}S\theta_y C\theta_x \\
m_{sy}C\theta_x - m_{sz}S\theta_x \\
-m_{sx}S\theta_y + m_{sy}C\theta_y S\theta_x + m_{sz}C\theta_y C\theta_x
\end{pmatrix} \\
\end{align} \\
上記の計算で、ベクトルの等式が導出された。
これらのベクトルの第1要素と第2要素を用いると $\theta_{z}$ を求めることができる。
\begin{align}
\tan \theta_z & = \frac{MS\theta_z}{MC\theta_z} = \frac{m_{sx}C\theta_y + m_{sy}S\theta_y S\theta_x + m_{sz}S\theta_y C\theta_x}{m_{sy}C\theta_x - m_{sz}S\theta_x} \\
\\
\boldsymbol{\theta_z} & = \boldsymbol{\tan^{-1} \biggl( \frac{m_{sx}C\theta_y + m_{sy}S\theta_y S\theta_x + m_{sz}S\theta_y C\theta_x}{m_{sy}C\theta_x - m_{sz}S\theta_x} \biggl)} \\
\\
\end{align} \\
ということで、Z-Y-X系オイラー角の$\boldsymbol{\theta _z}$ を計算する式が導出できた。
計算には、地磁気センサの観測値 $\boldsymbol{m}_s = (m _{sx},m _{sy},m _{sz})$ と加速度センサから求めた $\boldsymbol{\theta_x, \theta_y}$ が必要である。
まとめ
- 地磁気センサの観測値からオイラー角を導出した。
- 求める角度は、加速度センサで計算していない軸の角度。
- オイラー角の系は、加速度センサの系と同一にする。
- 計算には、地磁気センサの観測値と加速度センサから計算した2軸の角度が必要。
今回の計算例では、**『北方向を y軸』とし、オイラー角の系は『Z-Y-X系』**として計算した結果、『$\boldsymbol{\theta_z}$』を求める式を導出した。
\boldsymbol{\theta_z} = \boldsymbol{\tan^{-1} \biggl( \frac{m_{sx}C\theta_y + m_{sy}S\theta_y S\theta_x + m_{sz}S\theta_y C\theta_x}{m_{sy}C\theta_x - m_{sz}S\theta_x} \biggl)} \\