6軸IMU~拡張カルマンフィルタ

概要

6軸IMU(慣性センサ)3軸加速度センサ + 3軸ジャイロセンサ。3軸加速度センサにてセンサローカル座標系(センサ座標系と称す)の3軸における重力加速度gの分量が出力され、観測方程式によりワールド座標系から見たピッチ・ロール角が計算できる。3軸ジャイロセンサからセンサ座標系の3軸まわりの角速度が出力され、状態方程式によりワールド座標系から見たピッチ・ロール角・ヨー角が計算できる。また、誤差(ノイズ)が存在し、そしてドリフトが蓄積していくので、補正の必要がある。補正には、拡張カルマンフィルタ状態空間モデルが適用される。6軸IMU慣性センサまたは9軸IMU慣性センサは、ロボット、ドローンなどの位置推定、姿勢推定に幅広く利用されている。

3軸加速度センサ

3軸加速度センサのローカル座標のX軸・Y軸・Z軸とも回転する場合

3軸加速度計のX軸・Y軸・Z軸とも回転する
3軸加速度計のX軸・Y軸・Z軸とも回転する

回転行列

センサ座標系⇒ワールド座標系の回転行列R(ℇ)は、オイラー角がXYZ軸まわりの回転順として、
$$R(ℇ) = R_z(ψ)R_y(θ)R_x(ϕ)$$
$$\scriptsize R(ℇ) = \begin{bmatrix}cosψ&-sinψ&0\\sinψ&cosψ&0\\0&0&1\end{bmatrix}\begin{bmatrix}cosθ&0&sinθ\\0&1&0\\-sinθ&0&cosθ\end{bmatrix}\begin{bmatrix}1&0&0\\0&cosϕ&-sinϕ\\0&sinϕ&cosϕ\end{bmatrix}$$

観測方程式

$$a_{world} = \begin{bmatrix}0&0&-g\end{bmatrix}^T \\ a_{sensor} = \begin{bmatrix}a_x&a_y&a_z\end{bmatrix}^T$$
ただし、gは重力加速度、\(a_x、a_y、a_z\)は重力加速度がセンサ座標系xyz軸における分量
$$a_{world} = R(ℇ)a_{sensor} \\ a_{sensor} = R(ℇ)^{-1} a_{world}$$
$$\scriptsize\scriptsize\begin{bmatrix}a_x\\a_y\\a_z\end{bmatrix}=\begin{bmatrix}csψcsθ&snψcsθ&-snθ\\csψsnθsnϕ-snψcsϕ&snψsnθsnϕ+csψcsϕ&csθsnϕ\\csψsnθsnϕ+snψsnϕ&snψsnθcsϕ-csψsnϕ&csθcsϕ\end{bmatrix}\begin{bmatrix}0\\0\\-g\end{bmatrix}$$
ただし、cs = cos 、sn = sin
よって、3軸加速度センサの観測方程式は以下の式で表される。また、\(a_x、a_y、a_z\)は、ヨー角ψと関係ないことに気付き、逆にヨー角ψは、3軸加速度センサで推定できないことが分かる。
$$\begin{bmatrix}a_x\\a_y\\a_z\end{bmatrix} = \begin{bmatrix}gsinθ\\-gcosθsinϕ\\-gcosθcosϕ\end{bmatrix}$$

傾斜角度

よって、重力加速度gの分量により3軸加速度計センサの傾斜角度が以下の式で表される。
$$\begin{bmatrix}ϕ\\θ\end{bmatrix} = \begin{bmatrix}tan^{-1} \frac{a_y}{a_z}\\-tan ^{-1}\frac{a_x}{\sqrt{a_y^2 + a_z^2}}\end{bmatrix}$$
ただし、ϕ、θは、それぞれワールド座標系から見たロール・ピッチ角と呼ばれる。ジンバルロック現象を避けたいので、ϕ ≠ ±90°。
なお、ワールド座標系のヨー角は、3軸加速度センサだけでは推定できないゆえに、3軸ジャイロセンサまたはコンパスセンサが必要になる。しかし、センサ融合手法として活用された拡張カルマンフィルタは、複数のセンサとも推定できる角度しか求めないので、ヨー角は、以下の拡張カルマンフィルタの状態方程式から除外される。ヨー角の求め方について、角速度\(ω_z\)の時間積分して単独に求められる。または、ヨー角が別途観測可能なコンパスセンサを加え、これで9軸IMU = 6軸IMU + 3軸コンパスセンサで拡張カルマンフィルタが適用するようになる。

3軸ジャイロセンサ

coordinate
coordinate

状態方程式

センサ座標系のジャイロセンサ角速度の\(ω_x、ω_y、ω_z\)(*)のまわり軸がセンサ座標系のxyz軸と合致しているが、オイラー角(の微分または角速度)(**)のまわり軸が必ずしもワールド座標系のXYZ軸と合致してないので、(*)と(**)の相互関係を求めるには、オイラー角の微分がワールド座標系のXYZ軸まわりへ変換する。
$$\scriptsize R(ℇ)\begin{bmatrix}ω_x\\ω_y\\ω_z\end{bmatrix} = \begin{bmatrix}0\\0\\\dot{ψ}\end{bmatrix}+R_z(ψ)\begin{bmatrix}0\\\dot{θ}\\0\end{bmatrix} + R_z(ψ)R_y(θ)\begin{bmatrix}\dot{ϕ}\\0\\0\end{bmatrix}$$
よって、回転行列を利用して、センサ座標系の角速度から、ワールド座標系のオイラー角の微分が求められる。
$$\scriptsize \begin{bmatrix}\dot{ϕ}\\\dot{θ}\\\dot{ψ}\end{bmatrix} = \begin{bmatrix}1&sinϕtanθ&cosϕtanθ\\0&cosϕ&-sinϕ\\0&−sinϕsecθ&cosϕsecθ\end{bmatrix}\begin{bmatrix}ω_x\\ω_y\\ω_z\end{bmatrix}$$
ただし、ϕ、θ、ψはそれぞれワールド座標系から見たロール・ピッチ・ヨー角、ωx、ωy、ωzはセンサ座標系でのx、y、z軸周りの角速度を表す。ワールド座標系からセンサ座標系への回転はヨー・ピッチ・ロールの順とする。

回転角度での状態方程式の表現

ジャイロセンサの角速度からワールド座標系から見たロール・ピッチ角の計算は以下の式にて表される。
$$\begin{bmatrix}ϕ\\θ\end{bmatrix} = \begin{bmatrix}ϕ+\dot{ϕ}Δt\\θ+\dot{θ}Δt\end{bmatrix}$$
ただし、ヨー角の積分、加算はここで省略する。

誤差の補正

加速度センサの精度が悪く、とくに運動中の場合、但し加速度センサの誤差が蓄積しない。ジャイロセンサの精度が良く、但しドリフト現象が起きる。ドリフトにより、時間とともに誤差がじわじわと蓄積していく。補正手法として、加速度センサにジャイロセンサの出力値を組み合わせて、静的重み係数(経験値)を付ける相補フィルタの他、時系列分析から異なるセンサの出力値に信頼性に関わる動的重み係数を付ける、いわゆるカルマンフィルタ(Kalman Filter)の再帰的アルゴリズム(手法)がある。フィルタとは誤差(ノイズ)をフィルタリングする意味合いがある。性能からみれば、論理的にカルマンフィルターが良いといわれる。

*センサ座標系で3軸加速度センサから傾斜角度と3軸ジャイロセンサから回転角度を求めて、静的重み付け(経験値)で加算するセンサ融合の手法がある。出典4.を参照する。

カルマンフィルタ

wikipedia: カルマンフィルタ (Kalman Filter、KFと略す) は、誤差のある観測値を用いて、ある動的システムの状態を推定あるいは制御するための、無限インパルス応答フィルタの一種である。この解釈では理解できたというわけではなく、以下カルマンフィルター、拡張カルマンフィルタで状態を推定してみる。

状態空間モデル

状態方程式、観測方程式は以下のとおり。
$$\begin{eqnarray*}&& x_t = F_{t-1}(x_{t-1}^{true}+ p_{t-1} ) + w_{t-1}\\&& y_t = H_t(x_t^{true}) + v_t\end{eqnarray*}$$
ただし、F(・)、H(・)は実装に際して、3軸加速度センサ、3軸ジャイロセンサに掲載した状態方程式、観測方程式のとおりで利用する。システム誤差、観測誤差の分布は期待値が0の正規分布(ガウス分布)にする。
$$\begin{eqnarray*}&& w_t ~ N(0,Wt)\\&& v_t ~ N(0,Vt)\end{eqnarray*}$$

パラメータ初期化

$$\begin{eqnarray*}&& \hat{x}_0 = E(x_0)\\&& \hat{P_0} = E((x_0-\hat{x_0})(x_0-\hat{x_0})^T)\end{eqnarray*}$$

状態値、共分散行列予測ステップ

$$\begin{eqnarray*}&& \bar{x_t} = F_{t-1}(\hat{x}_{t-1})\\&& \bar{P_t} = F_{t-1}\hat{P}_{t-1}F_{t-1}^T + W_{t-1}\end{eqnarray*}$$
ただし、\(\bar{x_t}\)は予測値、\(\bar{P_t}\)は事前共分散行列、\(W_{t-1}=w_{t-1}w^T_{t-1}\)

カルマンゲイン、状態値、共分散行列更新ステップ

$$\begin{eqnarray*}&& K_t = \bar{P_t}H_t^T(H_t\bar{P_t}H_t^T + V_t)^{-1}\\
&& \hat{x_t} = \bar{x_t} + K_t(y_t – H_t\bar{x_t})\\&&\hat{P_t} = (I-K_tH_t)\bar{P_t} \end{eqnarray*}$$
ただし、\(K_t\)はカルマンゲイン、\(\hat{x_t}\)は状態推定値、\(\hat{P_t}\)は事後共分散行列、\(V_{t}=v_{t}v^T_{t}\)

拡張カルマンフィルタ

拡張カルマンフィルタ(Extended Kalman Filter、EKFと略す)は、非線形フィルタリングである。前述した状態方程式、観測方程式より、以下の状態空間モデルのf(⋅)またはH(⋅)が非線形関数であり、6軸IMU(慣性センサ)の角度計算に拡張カルマンフィルタが適用される。テイラー展開より、2次微分以降の項目を省略して、非線形であるf(⋅)の1次微分を線形化とし、カルマンフィルタのアルゴリズムが適用可能となる。しかし、f(⋅)の微分ではf(⋅)の一部しか表現できず、この線形化処理(1次微分)に誤差を大きく招く場合がある。ここではH(⋅)は線形関数であるため、f(⋅)のみを線形化する。
$$\begin{eqnarray*}&& \hat{F}_t =\frac{\partial f_t(x)}{\partial x}|_{x=\hat{x}_{t-1}}\end{eqnarray*}$$

状態空間モデル

$$\begin{eqnarray*}&& x_t = f_{t-1}(x_{t-1}^{true}+p_{t-1}) + w_{t-1}\\&& y_t = H_t(x_t^{true}) + v_t\end{eqnarray*}$$

パラメータ初期化

$$\begin{eqnarray*}&& \hat{x}_0 = E(x_0)\\&& \hat{P_0} = E((x_0-\hat{x_0})(x_0-\hat{x_0})^T)\end{eqnarray*}$$

状態値、共分散行列予測ステップ

$$\begin{eqnarray*}&& \bar{x_t} = f_{t-1}(\hat{x}_{t-1})\\&& \bar{P_t} = \hat{F}_{t-1}\hat{P}_{t-1}\hat{F}_{t-1}^T + W_{t-1}\end{eqnarray*}$$
ただし、\(\bar{x_t}\)は予測値、\(\bar{P_t}\)は事前共分散行列、\(W_{t-1}=w_{t-1}w^T_{t-1}\)

カルマンゲイン、状態値、共分散行列更新ステップ

$$\begin{eqnarray*}&& K_t = \bar{P_t}H_t^T(H_t\bar{P_t}H_t^T + V_t)^{-1}\\
&& \hat{x_t} = \bar{x_t} + K_t(y_t – H_t\bar{x_t})\\&&\hat{P_t} = (I-K_tH_t)\bar{P_t} \end{eqnarray*}$$
ただし、\(K_t\)はカルマンゲイン、\(\hat{x_t}\)は状態推定値、\(\hat{P_t}\)は事後共分散行列、\(V_{t}=v_{t}v^T_{t}\)

拡張カルマンフィルタを6軸IMUへの適用

状態空間モデルは以下のようになる。
$$\scriptsize x_t=f_{t-1}(x_{t-1}^{true}+p_{t-1})+w_{t-1}=\begin{bmatrix}ϕ+\dot{ϕ}Δt\\θ+\dot{θ}Δt\end{bmatrix}+w_{t-1}$$
$$y_t=H_t(x_t^{true})+v_t=\begin{bmatrix}ϕ\\θ\end{bmatrix}+v_t$$

予測値は以下のように求められる。
$$\bar{x_t}=\begin{bmatrix}ϕ+\dot{ϕ}Δt\\θ+\dot{θ}Δt\end{bmatrix}\\\scriptsize =\begin{bmatrix}ϕ+Δt(ω_x+ω_y\cdot sinϕ\cdot tanθ+ω_z\cdot cosϕ\cdot tanθ)\\θ+Δt(ω_y\cdot cosϕ-ω_z\cdot sinϕ)\end{bmatrix}$$
ただし、\(ω_x、ω_y、ω_z\)はジャイロセンサからの観測値。
状態観測値は以下のように求められる。
$$y_t=\begin{bmatrix}ϕ\\θ\end{bmatrix}=\begin{bmatrix}tan^{-1} \frac{a_y}{a_z}\\-tan^{-1}\frac{a_x}{\sqrt{a_y^2 + a_z^2}}\end{bmatrix}$$
ただし、\(a_x、a_y、a_z\)は加速度センサからの観測値。

1次微分線形化(ヤコビアン行列)は以下のように求められる。
$$\hat{F}_t =\frac{\partial f_t(x)}{\partial x}|_{x=\hat{x}_{t-1}}\\\scriptsize=\begin{bmatrix}1+Δt(ω_y\cdot cosϕ\cdot tanθ-ω_z\cdot sinϕ\cdot tanθ)&Δt(\frac{ω_y\cdot sinϕ}{cos^{2}θ} + \frac{ω_z\cdot cosϕ}{cos^{2}θ})\\-Δt(ω_y\cdot sinϕ+ω_z\cdot cosϕ)&1\end{bmatrix}$$

拡張カルマンフィルタによる姿勢推定動作の可視化

拡張カルマンフィルタを実装したPythonスクリプトはGitHubで公開した。
imu_ekf
実行した結果は以下のようになる。画像をクリックするとYouTubeへジャンプする。

imu_ekf
imu_ekf

拡張カルマンフィルタの解釈

拡張カルマンフィルタでは、状態推定値を予測結果Xt(実装例ではジャイロセンサーデータ)と観測データYt(実装例では加速度センサデータ)の線形結合で合成し,その誤差分散を最小にする推定法だと分かる。これはシステム誤差、観測誤差が数学期待が0の正規分布との前提条件から由来した推定法である。しかし、拡張カルマンフィルタをかけることで、状態推定値は予測結果と観測データの間にあるのは、真値からかなり乖離してしまう場合にあるのか。について、懸念している。また、ジャイロセンサーデータと、加速度センサデータとも観測値Ytにする方法がある。それぞれのパーフォーマンスの検証は、比較的精確な実験環境(比較用の高精度ジャイロセンサ、加速度センサ、エンコーダ、モータ)がないと、実は容易ではない。というよりも、シミュレーションをかけてカルマンフィルタのアルゴリズムを検証するのが、確実に可能である。

【追記】UKF(Unscented Kalman Filter)は、非線形状態、測量方程式(F(・)、H(・))がテイラー展開より線形化ではなく、重み付き線形回帰より線形化する手法。UKFの精度は拡張カルマンフィルタより、高まるという。また、線形問題に対しては、カルマンフィルタと効果が同様。UKF、EKF、KFが、状態値、観測値ともガウス分布(正規分布)に従うまたは近似する場合限り、論理的に適用可能とされる。

ロボットSLAM~拡張カルマンフィルタ

上述の拡張カルマンフィルタがIMUセンサへの応用のみならず、同じ理由で、時系列の移動ロボットSLAMのGmapping法にも、エンコーダ情報(オドメトリ情報またはodom情報)とレーザセンサのLidarスキャンマッチング情報をセンサ融合またはセンサフュージョンへ応用される。凹凸な環境でない限り、Lidarスキャンマッチング情報のみを使用したHector法より精度が良いだろう。凹凸な環境でエンコーダ情報が正確な位置情報に大いに誤差を招くことがあるから、センサ融合にマイナス効果が出ると考えられる。また、高精度、高スキャンレートのLidar+IMUを使用したHector法の精度が高まる。

出典

1.wikipedia: カルマンフィルタ、オイラー角
2.Analog Device AN-1057、アプリケーション・ノート、加速度センサーによる傾きの検出、著者:Christopher J. Fisher
3.マルチボディダイナミクスの基礎―3次元運動方程式の立て方、著者:田島 洋
4.センサ融合を加速度センサやジャイロスコープに適用、著者:Bonnie Baker
5.基礎からわかる時系列分析 Rで実践するカルマンフィルタ・MCMC・粒子フィルタ、著者:萩原淳一郎 等

関連記事

SLAM~拡張カルマンフィルタ
研究開発用 台車型ロボット キット

ロボット・ドローン部品お探しなら、ロボット翔をご利用下さい