6軸センサの状態方程式

目次

 はじめに

6軸センサは3軸の加速度センサ(x, y, z)と3軸のジャイロセンサ(roll, pitch, yaw)を複合させたセンサの名称で，ロボットなどの姿勢を推定するためによく利用されるセンサです．

このセンサによって3軸の姿勢(roll, pitch, yaw)を推定するために，拡張カルマンフィルタ(Extended Kalman filter; EKF)が使用されることも多くあります．
低周波数域での特性に優れた加速度センサと，高週数域での特性に優れたジャイロセンサをEKFを用いたセンサフュージョン(Sensor fusion)により組み合わせることで，広い周波数域での高精度な姿勢推定を可能にします．

EKFではシステムの状態方程式が必要になるのですが，一部導出に躓いたのでまとめておこうと思います．

状態方程式はロール・ピッチ・ヨーのオイラー角(Euler angle)で表現されるものを想定しています．
オイラーパラメータ(Euler parameter; またはQuaternion)により表現する場合には導出は比較的簡単なようです．

よく知られている6軸センサの状態方程式 / 観測方程式

6軸センサの状態方程式 / 観測方程式としてよく知られている式です．
式の変形によりここに到達することが今回の目標です．

状態方程式

ロール・ピッチ・ヨーのオイラー角を利用した6軸センサは以下のような状態方程式で表せることが知られています．

\begin{align}
\left[ \begin{array}{c}
\dot{\phi} \\
\dot{\theta} \\
\dot{\psi}
\end{array} \right] = \left[ \begin{array}{ccc}
1 & \sin\phi \tan\theta & \cos\phi\tan\theta \\
0 & \cos\phi & -\sin\phi \\
0 & \sin\phi \sec\theta & \cos\phi \sec\theta
\end{array} \right] \left[ \begin{array}{c}
\omega_{x} \\
\omega_{y} \\
\omega_{z}
\end{array} \right]
\end{align}

ただし，，，はそれぞれグローバルな座標系から見たロール・ピッチ・ヨー角，，，は6軸センサを基準とした座標系でのx，y，z軸周りの角速度を表します．

観測方程式

観測方程式は以下の式で表されます．

\begin{align}
\left[ \begin{array}{c}
a_{x} \\
a_{y} \\
a_{z}
\end{array} \right] = \left[ \begin{array}{c}
g \sin\theta \\
-g \sin\phi \cos\theta \\
-g \cos\phi \cos\theta
\end{array} \right]
\end{align}

状態方程式 / 観測方程式の導出

観測方程式は比較的導出が容易なため，観測方程式⇒状態方程式の順に導出していきます．

以下の文献を大いに参考にしました．

マルチボディダイナミクスの基礎―3次元運動方程式の立て方

回転行列

3軸を同時に扱うため，あらかじめ回転行列を定義しておきます．
ロール・ピッチ・ヨー方向の回転行列をそれぞれ，，とします．

ロール・ピッチ・ヨーのオイラー角を利用する場合には，グローバルな座標系とセンサの座標系は以下の関係で表すことができます．

\begin{align}
r_{Global} = C_{z}(\psi) C_{y}(\theta) C_{x}(\phi) r_{Sensor}
\end{align}

それぞれの回転行列の中身については以下となります．

ロール方向

\begin{align}
C_{x}(\phi) = \left[ \begin{array}{ccc}
1 & 0 & 0 \\
0 & \cos\phi & -\sin\phi \\
0 & \sin\phi & \cos\phi
\end{array} \right]
\end{align}

ピッチ方向

\begin{align}
C_{y}(\theta) = \left[ \begin{array}{ccc}
\cos\theta & 0 & \sin\theta \\
0 & 1 & 0 \\
-\sin\theta & 0 & \cos\theta
\end{array} \right]
\end{align}

ピッチ方向

\begin{align}
C_{z}(\psi) = \left[ \begin{array}{ccc}
\cos\psi & -\sin\psi & 0 \\
\sin\psi & \cos\psi & 0 \\
0 & 0 & 1
\end{array} \right]
\end{align}

観測方程式 

まずはじめに，状態をと定義します．
このロール・ピッチ・ヨー角による状態はグローバルな座標系で定義されます．

また，6軸センサでは，観測値は加速度センサの出力を使用します．
この加速度はセンサの座標系で計測されるため，はセンサ座標系で定義されます．

センサの出力と対になるものとして，グローバル座標系での加速度を使用します．
加速度センサから出力される加速度の方向と，地球が発生する重力加速度の方向から，以下の関係が導かれます．

\begin{align}
a_{Global} = C_{z}(\psi) C_{y}(\theta) C_{x}(\phi) a_{Sensor}
\end{align}

実際には出力はとして計測されるため，回転行列の逆行列を取って最終的に以下の式が得られます．

\begin{align}
a_{Sensor} = [ C_{z}(\psi) C_{y}(\theta) C_{x}(\phi) ]^{-1} a_{Global}
\end{align}

回転行列は直交行列(Orthogonal matrix)の性質を持つため逆行列と転置が等しく，次のように書くこともできます．

\begin{align}
a_{Sensor} = [ C_{z}(\psi) C_{y}(\theta) C_{x}(\phi) ]^{T} a_{Global}
\end{align}

これを展開すると以下のようになり，元の式と一致することが確認できます．

\begin{align}
a_{Sensor} &= \left[ \begin{array}{ccc}
\cos\psi \cos\theta & \sin\psi \cos\theta & -\sin\theta \\
\cos\psi \sin\theta \sin\phi - \sin\psi \cos\phi & \sin\psi \sin\theta \sin\phi + \cos\psi \cos\phi & \cos\theta \sin\phi \\
\cos\psi \sin\theta \cos\phi + \sin\psi \sin\phi & \sin\psi \sin\theta \cos\phi - \cos\psi \sin\phi & \cos\theta \cos\phi
\end{array} \right] \left[ \begin{array}{c}
0 \\
0 \\
g
\end{array} \right] \\
&= \left[ \begin{array}{c}
-g \sin\theta \\
g \cos\theta \sin\phi \\
g \cos\theta \cos\phi
\end{array} \right]
\end{align}

ただし，船舶業界ではz軸を下側に取ることが多いため，符号は逆転しています．

 状態方程式

 状態方程式の導出で陥ってしまった誤りは， ロール・ピッチ・ヨー方向の角速度からセンサ座標系での角速度へは回転行列で変換することができると思いこんでいたことでした．
回転行列を利用した以下の式は成り立たないのです．

\begin{align}
\dot{x}_{Global} = [ C_{z}(\psi) C_{y}(\theta) C_{x}(\phi) ] \omega_{Sensor}
\end{align}

ただし，，です．

”グローバル座標系での回転からセンサ座標系での回転への変換”と聞くと下の図のようなイメージを持つかと思うのですが，実はこれが誤りの元でした．

回転行列による角速度の座標変換

実は以下のような角速度の定義であれば回転行列による変換は可能です．

この角速度の定義であれば，以下の式が成り立ちます．

\begin{align}
\Omega_{OS} = [ C_{z}(\psi) C_{y}(\theta) C_{x}(\phi) ] \Omega_{OS}'
\end{align}

ただし，，であり，OやSは”どの座標系から見た角速度か”を表す添字です．
具体的には，Oがグローバル座標，Sがセンサ座標系です．

はグローバル座標で見たときのセンサの角速度，はセンサ座標系で見たときのセンサの角速度を表します．

”あらゆる方向の回転は，適当な1軸周りの回転に置き換えられる”という考え方があります．
まさしくこれがオイラーパラメータの考え方なのですが，上式の座標変換ではその”ある1軸”の方向を座標変換しているイメージになります．

この2つの式を見比べてみると，右辺は文字を置き換えただけなので等しいことがわかります．
実は左辺が違っているのです．

後者の左辺ではグローバル座標系Oにおいて，ある1つの軸を基準とした回転を3つの成分，，に分けて記述しています．

一方で前者の左辺はオイラー角であり，回転に順番があります．

ここが両者の差の原因となっています．

オイラー角の書き直し

改めてオイラー角を見直してみます．

グローバル座標系Oからセンサ座標系Sへの回転はヨー・ピッチ・ロールの順なので，下図のような回転を行うことになります．

このときのOからSまでの回転行列を，回転中の座標系S1，S2を利用して次のように書き直します．

\begin{align}
C_{OS}(\phi, \theta, \psi) = C_{zOS_{1}}(\psi) C_{yS_{1}S_{2}}(\theta) C_{xS_{2}S}(\phi)
\end{align}

ロール・ピッチ・ヨーのグローバル座標への変換

上述の通り，オイラー角にはヨー回転→ピッチ回転→ロール回転という順番があります．

これをグローバルな角速度に変換してみます．

簡単のため，それぞれの回転方向に分けて考えます．

これによって，それぞれの回転を"ある1軸での回転"に置き換えることができます．

つまり，角速度の座標変換式が使えるようになります．

まず，ヨー回転はそっくりそのままグローバル座標系でのz軸回転なので，以下のようになります．

\begin{align}
\left[ \begin{array}{c}
\Omega_{OS{x}} \\
\Omega_{OSy} \\
\Omega_{OSz}
\end{array} \right]_{yaw}  = \left[ \begin{array}{c}
0 \\
0 \\
\dot{\psi}
\end{array} \right] = \left[ \begin{array}{c}
0 \\
0 \\
1
\end{array} \right] \dot{\psi}
\end{align}

次に，ピッチ回転はヨー方向に回転したあとのy軸回転なので，以下のように表すことができます．

\begin{align}
\left[ \begin{array}{c}
\Omega_{OS{x}} \\
\Omega_{OSy} \\
\Omega_{OSz}
\end{array} \right]_{pitch} = C_{zOS_{1}}(\psi) \left[ \begin{array}{c}
0 \\
1 \\
0
\end{array} \right] \dot{\theta}
\end{align}

最後に，ロール回転は，ヨー回転とピッチ回転をしたあとのx軸回転なので，ピッチ回転と同様に以下のようになります．

\begin{align}
\left[ \begin{array}{c}
\Omega_{OS{x}} \\
\Omega_{OSy} \\
\Omega_{OSz}
\end{array} \right]_{roll} = C_{zOS_{1}}(\psi) C_{yS_{1}S_{2}}(\theta) \left[ \begin{array}{c}
1 \\
0 \\
0
\end{array} \right] \dot{\phi}
\end{align}

それぞれの回転での角速度を足し合わせると，以下の関係を得ることができます．

\begin{align}
\left[ \begin{array}{c}
\Omega_{OS{x}} \\
\Omega_{OSy} \\
\Omega_{OSz}
\end{array} \right] &= \left[ \begin{array}{c}
\Omega_{OS{x}} \\
\Omega_{OSy} \\
\Omega_{OSz}
\end{array} \right]_{yaw} + \left[ \begin{array}{c}
\Omega_{OS{x}} \\
\Omega_{OSy} \\
\Omega_{OSz}
\end{array} \right]_{pitch} + \left[ \begin{array}{c}
\Omega_{OS{x}} \\
\Omega_{OSy} \\
\Omega_{OSz}
\end{array} \right]_{roll} \\
&= \left[ \begin{array}{c}
0 \\
0 \\
1
\end{array} \right] \dot{\psi} + C_{zOS_{1}}(\psi) \left[ \begin{array}{c}
0 \\
1 \\
0
\end{array} \right] \dot{\theta} + C_{zOS_{1}}(\psi) C_{yS_{1}S_{2}}(\theta) \left[ \begin{array}{c}
1 \\
0 \\
0
\end{array} \right] \dot{\phi}
\end{align}

ここで，，，とし，ロール・ピッチ・ヨーの角速度をまとめると，次の簡単な式を得ることができます．

\begin{align}
\Omega_{OS} = \left[ \begin{array}{ccc}
C_{zOS_{1}}(\psi) C_{yS_{1}S_{2}}(\theta) D_{x} & C_{zOS_{1}}(\psi) D_{y} & D_{z}
\end{array} \right] \left[ \begin{array}{c}
\dot{\phi} \\
\dot{\theta} \\
\dot{\psi} 
\end{array} \right]
\end{align}

ロール・ピッチ・ヨー角速度の座標変換

さて，ロール・ピッチ・ヨー角速度の座標変換では，これまでに得られた次の2つの式を利用します．

\begin{align}
\Omega_{OS} = C_{zOS_{1}}(\psi) C_{yS_{1}S_{2}}(\theta) C_{xS_{2}S}(\phi) \Omega_{OS}'
\end{align}

\begin{align}
\Omega_{OS} = \left[ \begin{array}{ccc}
C_{zOS_{1}}(\psi) C_{yS_{1}S_{2}}(\theta) D_{x} & C_{zOS_{1}}(\psi) D_{y} & D_{z}
\end{array} \right] \left[ \begin{array}{c}
\dot{\phi} \\
\dot{\theta} \\
\dot{\psi} 
\end{array} \right]
\end{align}

を代入すると，

\begin{align}
\left[ \begin{array}{ccc}
C_{zOS_{1}}(\psi) C_{yS_{1}S_{2}}(\theta) D_{x} & C_{zOS_{1}}(\psi) D_{y} & D_{z}
\end{array} \right] \left[ \begin{array}{c}
\dot{\phi} \\
\dot{\theta} \\
\dot{\psi} 
\end{array} \right] = C_{zOS_{1}}(\psi) C_{yS_{1}S_{2}}(\theta) C_{xS_{2}S}(\phi) \Omega_{OS}'
\end{align}

したがって角速度は，

\begin{align}
\left[ \begin{array}{c}
\dot{\phi} \\
\dot{\theta} \\
\dot{\psi} 
\end{array} \right] = \left[ \begin{array}{ccc}
C_{zOS_{1}}(\psi) C_{yS_{1}S_{2}}(\theta) D_{x} & C_{zOS_{1}}(\psi) D_{y} & D_{z}
\end{array} \right]^{-1} C_{zOS_{1}}(\psi) C_{yS_{1}S_{2}}(\theta) C_{xS_{2}S}(\phi) \Omega_{OS}'
\end{align}

実際に中身を計算すると，最終的に以下の式が得られます．

\begin{align}
\left[ \begin{array}{c}
\dot{\phi} \\
\dot{\theta} \\
\dot{\psi} 
\end{array} \right] = \left[ \begin{array}{ccc}
1 & \displaystyle \frac{ \sin\phi \sin\theta }{ \cos\theta } & \displaystyle \frac{ \cos\phi \sin\theta }{ \cos\theta } \\
0 & \cos\phi & -\sin\phi \\
0 & \displaystyle \frac{ \sin\phi }{ \cos\theta } & \displaystyle \frac{ \cos\phi }{ \cos\theta }
\end{array} \right] \left[ \begin{array}{c}
\omega_{x} \\
\omega_{y} \\
\omega_{z}
\end{array} \right]
\end{align}

これは最初に示した式と一致していることがわかるかと思います．

おわりに

6軸センサの状態方程式の導出で躓いたので書き起こしましたが，重要なところは"オイラー角は順序を持っている"ということと，"順序ごとに回転を分解して考える必要がある"（今回はロール・ピッチ・ヨー）というところな気がします．

マルチボディダイナミクスの理論に基づくと，同一の枠組みで多様なシステムを導出できるようになるのでとても便利ですね．