加速度センサで3次元の姿勢推定
加速度センサから加速度のデータを得られる。
加速度ベクトルの変位から、3次元空間での姿勢を計算できる。
本記事では、加速度センサからオイラー角での姿勢推定を行うための内容を記載する。
ちょっとした一言
加速度センサからの姿勢計算は、特に難しくはない。
しかし、加速度データだけではオイラー角の3軸のうち2つの角度しか求められない。
後述する今回の設定の場合、z軸の回転角度は求まらない。
さらに、適用できるオイラー角の系もある程度限られる。
適用可能な系についても簡単に記載しておく。
一応、いろんなサイトで紹介されている解析と同等な設定にした。
前提条件
座標系
基準となる静止した座標系を $\boldsymbol{\Sigma}_w$ とする。添え字は$world$ の頭文字。
加速度センサの姿勢を基準とする座標系を $\boldsymbol{\Sigma}_s$ とする。添え字は$sensor$ の頭文字。
ワールド座標系の加速度
ワールド座標系 $\boldsymbol{\Sigma}_w$ での加速度を $\boldsymbol{a}_w = (a _{wx},a _{wy},a _{wz})$ とする。
センサ座標系 $\boldsymbol{\Sigma}_s$ で観測される加速度を $\boldsymbol{a}_s = (a _{sx},a _{sy},a _{sz})$ とする。
このとき、z軸の反対方向を重力方向にするとして、$\boldsymbol{a}_w = (0, 0, -g)\ $として計算を進めていく。
この場合、z軸のオイラー角は計算できない。
※ $\boldsymbol{a}_w = (-g, 0, 0)\ $ や $\boldsymbol{a}_w = (0, -g, 0)\ $ としても構わないが、適用できるオイラー角の系が変わってくる。
注意
オイラー角の系は12通りあるが、計算に用いる系は z軸から始まる系じゃないと計算できない。(多分。)
正確には、ここで設定した $\boldsymbol{\Sigma}_w$ での加速度方向と同じ軸から始まる系を選択する必要がある。
オイラー角の系
今回の考えるオイラー角の系を、『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{a}_s$ からオイラー角を計算する。
$\boldsymbol{a}_w$ と $\boldsymbol{a}_s$ の関係は、回転行列を用いて下記のように表される。
\boldsymbol{a}_s = \boldsymbol{R}_{zyx}^{-1}\boldsymbol{a}_w \\
ここで、 $\boldsymbol{R}_{zyx}$ はオイラー角を回転行列で表現したものであり、$\boldsymbol{R} _{zyx} = \boldsymbol{R}_z(\theta_z)\boldsymbol{R}_y(\theta_y)\boldsymbol{R}_x(\theta_x)$ である。
で、それぞれの行列やベクトルを代入し計算すると、下記のようになる。
記載上は、$\sin \theta$ ⇒ $S\theta$、$\cos \theta$ ⇒ $C\theta$ とする。
\begin{align}
\begin{pmatrix}
a_{sx} \\ a_{sy} \\ a_{sz} \\
\end{pmatrix}
& = \begin{pmatrix}
C\theta_{y}C\theta_{z} & S\theta_{x}S\theta_{y}C\theta_{z} - C\theta_{x}S\theta_{z} & C\theta_{x}S\theta_{y}C\theta_{z} + S\theta_{x}S\theta_{z} \\
C\theta_{y}S\theta_{z} & S\theta_{x}S\theta_{y}S\theta_{z} + C\theta_{x}C\theta_{z} & C\theta_{x}S\theta_{y}S\theta_{z} -S\theta_{x}C\theta_{z} \\
-S\theta_{y} & S\theta_{x}C\theta _{y} & C\theta_{x}C\theta_{y}
\end{pmatrix}^{-1}
\begin{pmatrix}
0 \\ 0 \\ -g \\
\end{pmatrix} \\
\\ & = \begin{pmatrix}
C\theta_{y}C\theta_{z} & C\theta_{y}S\theta_{z} & -S\theta_{y} \\
S\theta_{x}S\theta_{y}C\theta_{z} - C\theta_{x}S\theta_{z} & S\theta_{x}S\theta_{y}S\theta_{z} + C\theta_{x}C\theta_{z} & S\theta_{x}C\theta _{y} \\
C\theta_{x}S\theta_{y}C\theta_{z} + S\theta_{x}S\theta_{z} & C\theta_{x}S\theta_{y}S\theta_{z} -S\theta_{x}C\theta_{z} & C\theta_{x}C\theta_{y}
\end{pmatrix}
\begin{pmatrix}
0 \\ 0 \\ -g \\
\end{pmatrix} \\
\\ & = \begin{pmatrix}
gS\theta_{y} \\ -gS\theta_{x}C\theta _{y} \\ -gC\theta_{x}C\theta_{y} \\
\end{pmatrix} \\
\end{align} \\
上記の様に、簡単な三角関数の式になる。
この式から、$\theta_{x}$ と $\theta _{y}$ を計算する。
\begin{align}
\frac{-a_{sy}}{-a_{sz}} & = \frac{gS\theta_{x}C\theta _{y}}{gC\theta_{x}C\theta_{y}} = \frac{S\theta_{x}}{C\theta_{x}} = \tan\theta_{x} \\
\\ \boldsymbol{\theta_{x}} & = \boldsymbol{\tan ^{-1}\biggl(\frac{-a_{sy}}{-a_{sz}}\biggl)} \\
\\ \\
\frac{a_{sx}}{\sqrt{a_{sy}^2+a_{sz}^2}} & = \frac{gS\theta_{y}}{\sqrt{(-gS\theta_{x}C\theta _{y})^2+(-gC\theta_{x}C\theta_{y})^2}} = \frac{S\theta_{y}}{C\theta_{y}} = \tan\theta_{y} \\
\\ \boldsymbol{\theta_{y}} & = \boldsymbol{\tan ^{-1}\biggl(\frac{a_{sx}}{\sqrt{a_{sy}^2+a_{sz}^2}}\biggl)} \\
\end{align} \\
ということで、加速度の観測値 $\boldsymbol{a}_s = (a _{sx},a _{sy},a _{sz})\ $ から Z-Y-X系オイラー角の $\boldsymbol{\theta_x, \theta_y}$ を計算する式が導出できた。
観測で得られた値を上式に代入するだけで、3次元の姿勢を求めることができる。
※ $\theta_{x}$ の計算で加速度aにマイナスを付けたままなのは、180度反転するのを防ぐため。
まとめ
- 加速度の観測値からオイラー角を導出した。
- オイラー角は3軸のうち2軸の角度しか導出できない。
- 導出できない軸は、重力方向と一致する軸。
- オイラー角の系は、重力方向と一致する軸から始まる系を選択しなければならない。
今回の計算例では、『重力方向を z軸』とし、オイラー角の系は『Z-Y-X系』として計算した結果、『$\boldsymbol{\theta_x, \theta_y}$』を求める式を導出した。
\boldsymbol{\theta_{x}} = \boldsymbol{\tan ^{-1}\biggl(\frac{-a_{sy}}{-a_{sz}}\biggl)}
\qquad
\boldsymbol{\theta_{y}} = \boldsymbol{\tan ^{-1}\biggl(\frac{a_{sx}}{\sqrt{a_{sy}^2+a_{sz}^2}}\biggl)} \\