ジャイロセンサでオイラー角の姿勢推定


# ジャイロセンサで3次元の姿勢推定

ジャイロセンサから角速度のデータを得られる。
角速度を積分することで角度 (姿勢) を導出できる。実際には積算 (足し算) で求める。

本記事では、ジャイロセンサからオイラー角での姿勢推定を行うための内容を記載する。

# 前提条件

## オイラー角の系
 
オイラー角は**『Z-Y-X系』**として解析する。
オイラー角の表現は**『 $\boldsymbol{(\theta_x, \theta_y, \theta_z )}$ 』**の順で記載する.

例として、Z-Y-X系の$(\gamma , \beta, \alpha )$ が与えられた場合を考える。

1. 初期の座標系を x-y-z とする
1. z 軸周りで α 回転する
1. 回転後のy' 軸周りで β 回転する
1. 回転後のx'' 軸周りで γ 回転する

> **注意**
オイラー角の軸順が変わると、以降の数式も変わる。
パラメータの記載順が変わっても、数式は変わる。

> 自身の扱う系に合わせ、自身で計算することをオススメする。
ここの確認を怠ると、全てが台無しになる。超重要！！

## 回転行列

各軸の回転に対応する回転行列を以下に示す。

```math
\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{\omega}_s$ とする。添え字は$sensor$ の頭文字。
オイラー角Z-Y-X系の角速度を $\boldsymbol{\omega}_E$ とする。添え字は $Euler$ の頭文字。

それぞれ3軸の値を持つため、下記のように表現する。

```math
\boldsymbol{\omega}_s = (\omega_{sx},\omega_{sy},\omega_{sz}) \\
\boldsymbol{\omega}_E = (\omega_{Ex},\omega_{Ey},\omega_{Ez}) \\
```

## 座標系

後述する計算過程で、それぞれの回転操作を独立して考えるため、回転途中の座標系を定義する。

1. 初期の座標系を $\boldsymbol{\Sigma}_E$ とする。
1. $\boldsymbol{\Sigma}_E$ の z 軸周りで α 回転する。この座標系を $\boldsymbol{\Sigma}'_E$ とする。
1. $\boldsymbol{\Sigma}'_E$ の y 軸周りで β 回転する。この座標系を $\boldsymbol{\Sigma}''_E$ とする。
1. $\boldsymbol{\Sigma}''_E$ の x 軸周りで γ 回転する。この座標系を $\boldsymbol{\Sigma}_s$ とし、センサ座標系である。

以下に、それぞれの座標系の関係式を示す。

```math
\begin{align}
\boldsymbol{\Sigma}_s & = \boldsymbol{R}_x^{-1}(\gamma)\boldsymbol{R}_y^{-1}(\beta)\boldsymbol{R}_z^{-1}(\alpha)\boldsymbol{\Sigma}_E \\
\boldsymbol{\Sigma}_s & = \boldsymbol{R}_x^{-1}(\gamma)\boldsymbol{R}_y^{-1}(\beta)\boldsymbol{\Sigma}'_E \\
\boldsymbol{\Sigma}_s & = \boldsymbol{R}_x^{-1}(\gamma)\boldsymbol{\Sigma}''_E \\
\end{align} \\
```

## オイラー角の角速度

具体的な計算に入る前に、オイラー角Z-Y-X系の角速度 $\boldsymbol{\omega}_E$ について定義する。
$\boldsymbol{\omega}_E = (\omega _{Ex},\omega _{Ey},\omega _{Ez})$ としたとき、各成分は下記の意味を持つ。

- $\omega _{Ez}$：座標系 $\boldsymbol{\Sigma}_E$ または $\boldsymbol{\Sigma}'_E$ における z軸の角速度
- $\omega _{Ey}$：座標系 $\boldsymbol{\Sigma}'_E$ または $\boldsymbol{\Sigma}''_E$ における y軸の角速度
- $\omega _{Ex}$：座標系 $\boldsymbol{\Sigma}''_E$ または $\boldsymbol{\Sigma}s$ における x軸の角速度

各成分、2つの座標系に対して定義されている。
これは回転操作において**回転軸は不動**であり、**どちらの座標系で考えても角速度は同値**なためである。

> 注意
オイラー角の角速度 $\boldsymbol{\omega}_E = (\omega _{Ex},\omega _{Ey},\omega _{Ez})$ は、初期座標系 $\boldsymbol{\Sigma}_E$ のx軸、y軸、z軸に対する角速度ではない。
回転操作ごとの座標系で考える必要がある。
しばらく誤解してたの俺だけ？

# 角速度の座標変換

オイラー角Z-Y-X系$(\gamma , \beta, \alpha)$ が与えられたとき、姿勢を表す回転行列は下記のようになる。

```math
\begin{align}
\boldsymbol{R} _{zyx} (\gamma, \beta, \alpha) 
& = \boldsymbol{R} _z (\alpha) \boldsymbol{R} _y (\beta) \boldsymbol{R} _x (\gamma )  \\
\\
& = 
\begin{pmatrix}
\cos \alpha& -\sin \alpha& 0  \\
\sin \alpha& \cos \alpha& 0  \\
0 & 0 & 1 
\end{pmatrix}
\begin{pmatrix}
\cos \beta& 0 & \sin \beta\\
0 & 1 & 0 \\
-\sin \beta& 0 & \cos \beta
\end{pmatrix}
\begin{pmatrix}
1 & 0 & 0 \\
0 & \cos \gamma& -\sin \gamma\\
0 & \sin \gamma& \cos \gamma
\end{pmatrix}
\end{align}　\\
```

通常、センサ座標系に存在する適当なベクトル $\boldsymbol{v}_s$ を初期座標系で $\boldsymbol{v}_E$ として表す場合、下記のように計算される。

```math
\boldsymbol{v}_E = \boldsymbol{R} _{zyx} \boldsymbol{v}_s \qquad \boldsymbol{v}_s = \boldsymbol{R} _{zyx}^{-1} \boldsymbol{v}_E \\
```

しかし、角速度を考える場合ではこの等式は成り立たない。

```math
\boldsymbol{\omega}_E \neq \boldsymbol{R} _{zyx} \boldsymbol{\omega}_s \qquad \boldsymbol{\omega}_s \neq \boldsymbol{R} _{zyx}^{-1} \boldsymbol{\omega}_E \\
```

さて、ローカル座標系で観測される角速度 $\boldsymbol{\omega}_w$ をワールド座標系の角速度 $\boldsymbol{\omega}_w$ として表すには、ひと工夫しなければならない。
ひと工夫とは、**各軸ごとに対応する回転操作を加える**ことである。

オイラー角Z-Y-X系の回転順序は **z軸 ⇒ y軸 ⇒ x軸**であり、角速度は逆順の **x成分 ⇒ y成分 ⇒ z成分** で考えてみる。

***

- **角速度 x成分**

ここでのポイントを下記に記載する。

1. $\omega _{Ex}$：座標系 $\boldsymbol{\Sigma}''_E$ または $\boldsymbol{\Sigma}s$ における x軸の角速度

ここでは、$\omega _{Ex}$ を $\boldsymbol{\Sigma}s$ における x軸の角速度として考える。
つまり、オイラー角の角速度のx成分は下記の状態を満たす。

```math
\omega _{Ex}(\boldsymbol{\Sigma}_s) = \omega _{Ex} \\
```

上式が意味することは、**座標系 $\boldsymbol{\Sigma}_s$ から見た $\omega _{Ex}$ は、元の $\omega _{Ex}$ と同値で**あることを示している。

***

- **角速度 y成分**

ここでのポイントを下記に記載する。いずれも前項で示したものである。

1. $\omega _{Ey}$：座標系 $\boldsymbol{\Sigma}'_E$ または $\boldsymbol{\Sigma}''_E$ における y軸の角速度
1. $\boldsymbol{\Sigma}_s = \boldsymbol{R}_x^{-1}(\gamma)\boldsymbol{\Sigma}''_E$ を満たす


ここでは、$\omega _{Ey}$ を $\boldsymbol{\Sigma}''_E$ における y軸の角速度として考える。
また 2. より、$\boldsymbol{\Sigma}''_E$ を用いて $\boldsymbol{\Sigma}_s$ を表すと $\boldsymbol{\Sigma}_s = \boldsymbol{R}_x^{-1}(\gamma)\boldsymbol{\Sigma}''_E$ となる。

よって、オイラー角の角速度のy成分は下記の状態を満たす。
これは、**座標系 $\boldsymbol{\Sigma}_s$ から見た $\omega _{Ey}$  は、元の $\omega _{Ey}$ を x軸で $\gamma$ 回転させた値**であることを示している。

```math
\omega _{Ey}(\boldsymbol{\Sigma}_s) = \boldsymbol{R}_x^{-1}(\gamma)\omega _{Ey} \\
```

***

- **角速度 z成分**

ここでのポイントを下記に記載する。いずれも前項で示したものである。

1. $\omega _{Ez}$：座標系 $\boldsymbol{\Sigma}_E$ または $\boldsymbol{\Sigma}'_E$ における z軸の角速度
1. $\boldsymbol{\Sigma}_s = \boldsymbol{R}_x^{-1}(\gamma)\boldsymbol{R}_y^{-1}(\beta)\boldsymbol{\Sigma}'_E$ を満たす

ここでは、$\omega _{Ez}$ を $\boldsymbol{\Sigma}'_E$ における z軸の角速度として考える。
また 2. より、$\boldsymbol{\Sigma}'_E$ を用いて $\boldsymbol{\Sigma}_s$ を表すと $\boldsymbol{\Sigma}_s = \boldsymbol{R}_x^{-1}(\gamma)\boldsymbol{R}_y^{-1}(\beta)\boldsymbol{\Sigma}'_E$ となる。

よって、オイラー角の角速度のz成分は下記の状態を満たす。
これは、**座標系 $\boldsymbol{\Sigma}_s$ から見た $\omega _{Ez}$  は、元の $\omega _{Ez}$ を y軸で$\beta$ 回転し、x軸で $\gamma$ 回転させた値**であることを示している。

```math
\omega _{Ez}(\boldsymbol{\Sigma}_s) = \boldsymbol{R}_x^{-1}(\gamma)\boldsymbol{R}_y^{-1}(\beta)\omega _{Ez} \\
```

***

もっと感覚的に記載すると、下記の様になる。

- オイラー角Z-Y-X系の回転順序は z軸 ⇒ y軸 ⇒ x軸。
- $\omega _{Ex}$ は、もう回転しきっているため、センサ座標系で見ても同値である。
- $\omega _{Ey}$ は、追加でx軸回転しないとセンサ座標系と一致しないから、x軸の回転行列を掛ける必要がある。
- $\omega _{Ez}$ は、追加でy軸回転とx軸回転しないとセンサ座標系と一致しないから、y軸とx軸の回転行列を掛ける必要がある。

***

以上で、オイラー角の角速度3成分をセンサ座標系 $\boldsymbol{\Sigma}_s$ で表現できた。
この3成分を足し合わせることで、$\boldsymbol{\Sigma}_s$ から見たオイラー角の角速度 $\boldsymbol{\omega}_E$ を数式として扱うことができる。

```math
\begin{align}
\boldsymbol{\omega} _{E}(\boldsymbol{\Sigma}_s) & = \omega _{Ex}(\boldsymbol{\Sigma}_s) + \omega _{Ey}(\boldsymbol{\Sigma}_s) + \omega _{Ez}(\boldsymbol{\Sigma}_s) \\
\\ & = \omega _{Ex} + \boldsymbol{R}_x^{-1}(\gamma)\omega _{Ey} + \boldsymbol{R}_x^{-1}(\gamma)\boldsymbol{R}_y^{-1}(\beta)\omega _{Ez}
\\
\end{align} \\
```

そして、これはジャイロセンサの観測データと同値である。
ついでに3成分もベクトル化と行列計算もしておく。

```math
\boldsymbol{\omega} _s = \boldsymbol{\omega} _{Ex} + \boldsymbol{R}_x^{-1}(\gamma)\boldsymbol{\omega} _{Ey} + \boldsymbol{R}_x^{-1}(\gamma)\boldsymbol{R}_y^{-1}(\beta)\boldsymbol{\omega} _{Ez} \\
```

```math
\scriptsize
\begin{align}
\begin{pmatrix}
\omega_{sx} \\ \omega_{sy} \\ \omega_{sz}
\end{pmatrix}
& = \begin{pmatrix}
\omega_{Ex} \\ 0 \\ 0
\end{pmatrix}
+ \begin{pmatrix}
1 & 0 & 0 \\
0 & \cos \gamma & \sin \gamma \\
0 & -\sin \gamma & \cos \gamma
\end{pmatrix}
\begin{pmatrix}
0 \\ \omega_{Ey} \\ 0
\end{pmatrix}
+ \begin{pmatrix}
1 & 0 & 0 \\
0 & \cos \gamma & \sin \gamma \\
0 & -\sin \gamma & \cos \gamma
\end{pmatrix}
\begin{pmatrix}
\cos \beta & 0 & -\sin \beta \\
0 & 1 & 0 \\
\sin \beta & 0 & \cos \beta
\end{pmatrix}
\begin{pmatrix}
0 \\ 0 \\ \omega_{Ex}
\end{pmatrix} \\
& = \begin{pmatrix}
\omega_{Ex} \\ 0 \\ 0
\end{pmatrix}
+ \begin{pmatrix}
0 \\ \omega_{Ey} \cos \gamma \\ -\omega_{Ey} \sin \gamma
\end{pmatrix}
+ \begin{pmatrix}
-\omega_{Ex} \sin \beta \\ \omega_{Ex} \cos \beta \sin \gamma \\ \omega_{Ex} \cos \beta \cos \gamma
\end{pmatrix} \\
& = \begin{pmatrix}
1 & 0 & -\sin \beta \\ 
0 & \cos \gamma & \cos \beta \sin \gamma \\
0 & -\sin \gamma & \cos \beta \cos \gamma
\end{pmatrix}
\begin{pmatrix}
\omega_{Ex} \\ \omega_{Ey} \\ \omega_{Ez}
\end{pmatrix}
\end{align} \\
```

ということで、やっと **観測値 $\boldsymbol{\omega}_s$ と Z-Y-X系角速度$\boldsymbol{\omega}_E$ の関係式を導出**することが出来た。

```math
\begin{pmatrix}
\omega_{sx} \\ \omega_{sy} \\ \omega_{sz}
\end{pmatrix}
= \begin{pmatrix}
1 & 0 & -\sin \beta \\ 
0 & \cos \gamma & \cos \beta \sin \gamma \\
0 & -\sin \gamma & \cos \beta \cos \gamma
\end{pmatrix}
\begin{pmatrix}
\omega_{Ex} \\ \omega_{Ey} \\ \omega_{Ez}
\end{pmatrix} \\
```

```math
\begin{pmatrix}
\omega_{Ex} \\ \omega_{Ey} \\ \omega_{Ez}
\end{pmatrix}
= \begin{pmatrix}
1 & 0 & -\sin \beta \\ 
0 & \cos \gamma & \cos \beta \sin \gamma \\
0 & -\sin \gamma & \cos \beta \cos \gamma
\end{pmatrix}^{-1}
\begin{pmatrix}
\omega_{sx} \\ \omega_{sy} \\ \omega_{sz}
\end{pmatrix} \\
```

あとは、この逆行列を求める必要がある。
3×3 の逆行列なら手計算で求めることができる。
実際に手計算してみると、行列式などもキレイな形で計算可能。

```math
\begin{pmatrix}
1 & 0 & -\sin \beta \\ 
0 & \cos \gamma & \cos \beta \sin \gamma \\
0 & -\sin \gamma & \cos \beta \cos \gamma
\end{pmatrix}^{-1}
= \begin{pmatrix}
1 & \tan \beta \sin \gamma & \tan \beta \cos \gamma \\ 
0 & \cos \gamma & -\sin \gamma \\
0 & \frac{\sin \gamma}{\cos \beta} & \frac{\cos \gamma}{\cos \beta}
\end{pmatrix} \\
```

上記の計算により、下記の式が導出される。
これで、**ジャイロセンサの観測値 $\boldsymbol{\omega}_s$ から Z-Y-X系角速度 $\boldsymbol{\omega}_E$ を計算できる**ようになった。

```math
\begin{pmatrix}
\omega_{Ex} \\ \omega_{Ey} \\ \omega_{Ez}
\end{pmatrix}
= \begin{pmatrix}
1 & \tan \beta \sin \gamma & \tan \beta \cos \gamma \\ 
0 & \cos \gamma & -\sin \gamma \\
0 & \frac{\sin \gamma}{\cos \beta} & \frac{\cos \gamma}{\cos \beta}
\end{pmatrix}
\begin{pmatrix}
\omega_{sx} \\ \omega_{sy} \\ \omega_{sz}
\end{pmatrix} \\
```

# 姿勢計算

Z-Y-X系角速度 $\boldsymbol{\omega}_E$ が求まったら、現在の角度に角速度×時間の値を積算していくだけである。

```math
\begin{pmatrix}
\gamma_{t+1} \\ \beta_{t+1} \\ \alpha_{t+1}
\end{pmatrix}
= \begin{pmatrix}
\gamma_t \\ \beta_t \\ \alpha_t
\end{pmatrix}
+ \Delta T \begin{pmatrix}
\omega_{Ex} \\ \omega_{Ey} \\ \omega_{Ez}
\end{pmatrix}\\
```

※ 通常のオイラー角は回転操作の順番がネックとなる。
　 しかし、角速度の積算の場合は気にせず足し合わせればよい。
　 角度変化が微小な場合、回転行列の積が可換となるためである。

# まとめ

何度も記載しているように、本記事で導出したものは**オイラー角『Z-Y-X系』**の角速度である。
異なる系を扱うなら、その系に対応する角速度を同様の流れで計算する必要がある。

計算さえできてしまえば、プログラムへの実装は簡単。
観測データに導出した行列を掛けて既存の角度に足し合わせるだけ。


# 参考ページ
https://jaxa.repo.nii.ac.jp/?action=pages_view_main&active_action=repository_view_main_item_detail&item_id=44219&item_no=1&page_id=13&block_id=21
http://hooktail.sub.jp/mechanics/infinitesimalRot1/