IMU＆ホイールオドメトリ【EKF】

移動ロボットの移動量推定（＝オドメトリ）では、6軸IMUとホイールエンコーダがよく用いられます。そして、これらは、拡張カルマンフィルタ（EKF）で統合されるのが一般的です。しかし、とても一般的であるにも関わらず、このオドメトリについて、数式をまとめている文献が見つかりませんでした。

そのため、頑張って本記事でまとめてみることにしました。

表記ルール

この記事では以下のルールで表記を行います。

• 細字の小文字（\(a\)）：スカラー
• 太字の小文字（\(\boldsymbol{a}\)）：ベクトル
• 太字の大文字（\(\boldsymbol{A}\)）：行列
• 下付き文字（\(a_k\)）：時間ステップ
• \(S_a, C_a, T_a\)：\(\sin{a}, \cos{a}, \tan{a}\)

モデリング

まずは、オドメトリを状態方程式と観測方程式で表します。

状態ベクトル\(\boldsymbol{x}\)

\[ \begin{equation} \boldsymbol{x} = \begin{pmatrix} X \cr Y \cr Z \cr \varPhi \cr \varTheta \cr \varPsi \end{pmatrix} \end{equation} \]

• \(X\)：グローバル座標系でのロボットのX座標
• \(Y\)：グローバル座標系でのロボットのY座標
• \(Z\)：グローバル座標系でのロボットのZ座標
• \(\varPhi\)：グローバル座標系でのロボットのロール角
• \(\varTheta\)：グローバル座標系でのロボットのピッチ角
• \(\varPsi\)：グローバル座標系でのロボットのヨー角

入力ベクトル\(\boldsymbol{u}\)

\[ \begin{equation} \boldsymbol{u} = \begin{pmatrix} \boldsymbol{v} \cr \boldsymbol{\omega} \end{pmatrix} \Delta t = \begin{pmatrix} \dot{x} \cr \dot{y} \cr \dot{z} \cr \dot{\phi} \cr \dot{\theta} \cr \dot{\psi} \end{pmatrix} \Delta t = \begin{pmatrix} \Delta x \cr \Delta y \cr \Delta z \cr \Delta \phi \cr \Delta \theta \cr \Delta \psi \end{pmatrix} \end{equation} \]

• \(\boldsymbol{v}\)：ホイールエンコーダで計測するローカル座標系での速度
• \(\boldsymbol{\omega}\)：IMUで計測するローカル座標系での角速度
• \(\Delta t\)：時間差分
• \(x\)：ローカル座標系でのx座標
• \(y\)：ローカル座標系でのy座標
• \(z\)：ローカル座標系でのz座標
• \(\phi\)：ローカル座標系でのロール角
• \(\theta\)：ローカル座標系でのピッチ角
• \(\psi\)：ローカル座標系でのヨー角

状態方程式\(f\)

\[ \begin{equation} \begin{split} \boldsymbol{x}_k &= f_{(\boldsymbol{x}_{k-1}, \boldsymbol{u_{k-1}})} + \boldsymbol{w}_{\textbf{pre}_{k-1}} \\&= \boldsymbol{x}_{k-1} + \begin{pmatrix} \boldsymbol{R}^\textbf{xyz}_{(\boldsymbol{x}_{k-1})} \boldsymbol{v_{k-1}} \cr \boldsymbol{R}^\textbf{rpy}_{(\boldsymbol{x}_{k-1})} \boldsymbol{\omega_{k-1}} \end{pmatrix} + \boldsymbol{w}_{\textbf{pre}_{k-1}} \\&= \begin{pmatrix} X_{k-1} \cr Y_{k-1} \cr Z_{k-1} \cr \varPhi_{k-1} \cr \varTheta_{k-1} \cr \varPsi_{k-1} \end{pmatrix} + \begin{pmatrix} \begin{pmatrix} C_{\varTheta_{k-1}}C_{\varPsi_{k-1}} & S_{\varPhi_{k-1}}S_{\varTheta_{k-1}}C_{\varPsi_{k-1}} - C_{\varPhi_{k-1}}S_{\varPsi_{k-1}} & C_{\varPhi_{k-1}}S_{\varTheta_{k-1}}C_{\varPsi_{k-1}} + S_{\varPhi_{k-1}}S_{\varPsi_{k-1}} \cr C_{\varTheta_{k-1}}S_{\varPsi_{k-1}} & S_{\varPhi_{k-1}}S_{\varTheta_{k-1}}S_{\varPsi_{k-1}} + C_{\varPhi_{k-1}}C_{\varPsi_{k-1}} & C_{\varPhi_{k-1}}S_{\varTheta_{k-1}}S_{\varPsi_{k-1}} - S_{\varPhi_{k-1}}C_{\varPsi_{k-1}} \cr -S_{\varTheta_{k-1}} & S_{\varPhi_{k-1}}C_{\varTheta_{k-1}} & C_{\varPhi_{k-1}}C_{\varTheta_{k-1}} \end{pmatrix} \begin{pmatrix} \Delta x_{k-1} \cr \Delta y_{k-1} \cr \Delta z_{k-1} \end{pmatrix} \cr \begin{pmatrix} 1 & S_{\varPhi_{k-1}} T_{\varTheta_{k-1}} & C_{\varPhi_{k-1}} T_{\varTheta_{k-1}} \cr 0 & C_{\varPhi_{k-1}} & -S_{\varPhi_{k-1}} \cr 0 & S_{\varPhi_{k-1}} / C_{\varTheta_{k-1}} & C_{\varPhi_{k-1}} / C_{\varTheta_{k-1}} \end{pmatrix} \begin{pmatrix} \Delta \phi_{k-1} \cr \Delta \theta_{k-1} \cr \Delta \psi_{k-1} \end{pmatrix} \end{pmatrix} + \boldsymbol{w}_{\textbf{pre}_{k-1}} \\&= \begin{pmatrix} X_{k-1} + C_{\varTheta_{k-1}}C_{\varPsi_{k-1}} \Delta x_{k-1} + (S_{\varPhi_{k-1}}S_{\varTheta_{k-1}}C_{\varPsi_{k-1}} - C_{\varPhi_{k-1}}S_{\varPsi_{k-1}}) \Delta y_{k-1} + (C_{\varPhi_{k-1}}S_{\varTheta_{k-1}}C_{\varPsi_{k-1}} + S_{\varPhi_{k-1}}S_{\varPsi_{k-1}}) \Delta z_{k-1} \cr Y_{k-1} + C_{\varTheta_{k-1}}S_{\varPsi_{k-1}} \Delta x_{k-1} + (S_{\varPhi_{k-1}}S_{\varTheta_{k-1}}S_{\varPsi_{k-1}} + C_{\varPhi_{k-1}}C_{\varPsi_{k-1}}) \Delta y_{k-1} + (C_{\varPhi_{k-1}}S_{\varTheta_{k-1}}S_{\varPsi_{k-1}} - S_{\varPhi_{k-1}}C_{\varPsi_{k-1}}) \Delta z_{k-1} \cr Z_{k-1} - S_{\varTheta_{k-1}} \Delta x_{k-1} + S_{\varPhi_{k-1}}C_{\varTheta_{k-1}} \Delta y_{k-1} + C_{\varPhi_{k-1}}C_{\varTheta_{k-1}} \Delta z_{k-1} \cr \varPhi_{k-1} + \Delta \phi_{k-1} + S_{\phi_{k-1}} T_{\theta_{k-1}} \Delta \theta_{k-1} + C_{\phi_{k-1}} T_{\theta_{k-1}} \Delta \psi_{k-1} \cr \varTheta_{k-1} + C_{\phi_{k-1}} \Delta \theta_{k-1} - S_{\phi_{k-1}} \Delta \psi_{k-1} \cr \varPsi_{k-1} + S_{\phi_{k-1}} / C_{\theta_{k-1}} \Delta \theta_{k-1} + C_{\phi_{k-1}} / C_{\theta_{k-1}} \Delta \psi_{k-1} \end{pmatrix} + \boldsymbol{w}_{\textbf{pre}_{k-1}} \end{split} \end{equation} \]

• \(\boldsymbol{R}^\textbf{xyz}\)：ローカル座標系からグローバル座標系への座標の回転行列
• \(\boldsymbol{R}^\textbf{rpy}\)：ローカル座標系からグローバル座標系への姿勢角の回転行列
• \(\boldsymbol{w}_\textbf{pre}\)：平均0、共分散\(\boldsymbol{Q}\)の正規分布に従うノイズ

\[ \begin{equation} \boldsymbol{w}_{\textbf{pre}_{k-1}} \sim N(0, \boldsymbol{Q}_{k-1}) \end{equation} \]

観測ベクトル\(\boldsymbol{z}\)

\[ \begin{equation} \boldsymbol{z} = \begin{pmatrix} \varPhi_\mathrm{imu} \cr \varTheta_\mathrm{imu} \end{pmatrix} = \begin{pmatrix} \tan^{-1}{\frac{-\ddot{y}}{-\ddot{z}}} \cr \tan^{-1}{\frac{\ddot{x}}{\sqrt{\ddot{y}^2 + \ddot{z}^2}}} \end{pmatrix} \end{equation} \]

• \(\varPhi_\mathrm{imu}\)：IMUで計測する加速度から算出されるロール
• \(\varTheta_\mathrm{imu}\)：IMUで計測する加速度から算出されるピッチ

観測方程式\(h\)

\[ \begin{equation} \begin{split} \boldsymbol{y}_k &= h_{(\boldsymbol{x}_k)} + \boldsymbol{w}_{\textbf{obs}_k} \\&= \boldsymbol{I}_{2 \times 6} \boldsymbol{x}_k + \boldsymbol{w}_{\textbf{obs}_k} \\&= \begin{pmatrix} 0 & 0 & 0 & 1 & 0 & 0 \cr 0 & 0 & 0 & 0 & 1 & 0 \end{pmatrix} \begin{pmatrix} X_k \cr Y_k \cr Z_k \cr \varPhi_k \cr \varTheta_k \cr \varPsi_k \end{pmatrix} + \boldsymbol{w}_{\textbf{obs}_k} \\&= \begin{pmatrix} \varPhi_k \cr \varTheta_k \end{pmatrix} + \boldsymbol{w}_{\textbf{obs}_k} \end{split} \end{equation} \]

• \(\boldsymbol{w}_\textbf{pre}\)：平均0、共分散\(\boldsymbol{R}\)の正規分布に従うノイズ

\[ \begin{equation} \boldsymbol{w}_{\textbf{obs}_k} \sim N(0, \boldsymbol{R}_k) \end{equation} \]

EKF（拡張カルマンフィルタ）

上記のモデルにEKFを適用していきます。

予測推定と観測更新を繰り返すことで、状態ベクトル\(\boldsymbol{x}\)と共分散行列\(\boldsymbol{P}\)を更新します。

予測推定

予測推定で行う計算を示します。

\[ \begin{equation} \bar{\boldsymbol{x}_{k}} = f_{(\boldsymbol{x}_{k-1}, \boldsymbol{u}_{k-1})} \end{equation} \] \[ \begin{equation} \bar{\boldsymbol{P}_{k}} = {\boldsymbol{J_f}}_{k-1} \boldsymbol{P}_{k-1} {\boldsymbol{J_f}}_{k-1}^{\mathrm{T}} + \boldsymbol{Q}_{k-1} \end{equation} \]

ここで、\(\boldsymbol{J_f}\)は状態方程式のヤコビアンです。

\[ \begin{equation} \begin{split} {\boldsymbol{J_f}}_{k-1} &= \left. \frac{\partial f}{\partial \boldsymbol{x}} \right|_{\boldsymbol{x}_{k-1}, \boldsymbol{u}_{k-1}} \\&= \begin{pmatrix} 1 & 0 & 0 & (C_{\varPhi_{k-1}}S_{\varTheta_{k-1}}C_{\varPsi_{k-1}} + S_{\varPhi_{k-1}}S_{\varPsi_{k-1}}) \Delta y_{k-1} + (-S_{\varPhi_{k-1}}S_{\varTheta_{k-1}}C_{\varPsi_{k-1}} + C_{\varPhi_{k-1}}S_{\varPsi_{k-1}}) \Delta z_{k-1} & -S_{\varTheta_{k-1}}C_{\varPsi_{k-1}} \Delta x_{k-1} + S_{\varPhi_{k-1}}C_{\varTheta_{k-1}}C_{\varPsi_{k-1}} \Delta y_{k-1} + C_{\varPhi_{k-1}}C_{\varTheta_{k-1}}C_{\varPsi_{k-1}} \Delta z_{k-1} & -C_{\varTheta_{k-1}}S_{\varPsi_{k-1}} \Delta x_{k-1} + (-S_{\varPhi_{k-1}}S_{\varTheta_{k-1}}S_{\varPsi_{k-1}} - C_{\varPhi_{k-1}}C_{\varPsi_{k-1}}) \Delta y_{k-1} + (-C_{\varPhi_{k-1}}S_{\varTheta_{k-1}}S_{\varPsi_{k-1}} + S_{\varPhi_{k-1}}C_{\varPsi_{k-1}}) \Delta z_{k-1} \cr 0 & 1 & 0 & (C_{\varPhi_{k-1}}S_{\varTheta_{k-1}}S_{\varPsi_{k-1}} - S_{\varPhi_{k-1}}C_{\varPsi_{k-1}}) \Delta y_{k-1} + (-S_{\varPhi_{k-1}}S_{\varTheta_{k-1}}S_{\varPsi_{k-1}} - C_{\varPhi_{k-1}}C_{\varPsi_{k-1}}) \Delta z_{k-1} & -S_{\varTheta_{k-1}}S_{\varPsi_{k-1}} \Delta x_{k-1} + S_{\varPhi_{k-1}}C_{\varTheta_{k-1}}S_{\varPsi_{k-1}} \Delta y_{k-1} + C_{\varPhi_{k-1}}C_{\varTheta_{k-1}}S_{\varPsi_{k-1}} \Delta z_{k-1} & C_{\varTheta_{k-1}}C_{\varPsi_{k-1}} \Delta x_{k-1} + (S_{\varPhi_{k-1}}S_{\varTheta_{k-1}}C_{\varPsi_{k-1}} - C_{\varPhi_{k-1}}S_{\varPsi_{k-1}}) \Delta y_{k-1} + (C_{\varPhi_{k-1}}S_{\varTheta_{k-1}}C_{\varPsi_{k-1}} + S_{\varPhi_{k-1}}S_{\varPsi_{k-1}}) \Delta z_{k-1} \cr 0 & 0 & 1 & C_{\varPhi_{k-1}}C_{\varTheta_{k-1}} \Delta y_{k-1} - S_{\varPhi_{k-1}}C_{\varTheta_{k-1}} \Delta z_{k-1} & - C_{\varTheta_{k-1}} \Delta x_{k-1} - S_{\varPhi_{k-1}}S_{\varTheta_{k-1}} \Delta y_{k-1} - C_{\varPhi_{k-1}}S_{\varTheta_{k-1}} \Delta z_{k-1} & 0 \cr 0 & 0 & 0 & 1 + C_{\varPhi_{k-1}} T_{\varTheta_{k-1}} \Delta \theta_{k-1} - S_{\varPhi_{k-1}} T_{\varTheta_{k-1}} \Delta \psi_{k-1} & S_{\varPhi_{k-1}} / C^2_{\varTheta_{k-1}} \Delta \theta_{k-1} + C_{\varPhi_{k-1}} / C^2_{\varTheta_{k-1}} \Delta \psi_{k-1} & 0 \cr 0 & 0 & 0 & - S_{\varPhi_{k-1}} \Delta \theta_{k-1} - C_{\varPhi_{k-1}} \Delta \psi_{k-1} & 1 & 0 \cr 0 & 0 & 0 & C_{\varPhi_{k-1}} / C_{\varTheta_{k-1}} \Delta \theta_{k-1} - S_{\varPhi_{k-1}} / C_{\varTheta_{k-1}} \Delta \psi_{k-1} & S_{\varPhi_{k-1}} S_{\varTheta_{k-1}} / C^2_{\varTheta_{k-1}} \Delta \theta_{k-1} + C_{\varPhi_{k-1}} S_{\varTheta_{k-1}} / C^2_{\varTheta_{k-1}} \Delta \psi_{k-1} & 1 \end{pmatrix} \end{split} \end{equation} \]

観測更新

観測更新で行う計算を示します。

\[ \begin{equation} \hat{\boldsymbol{x}_k} = \boldsymbol{x}_k + \boldsymbol{K}_k (\boldsymbol{z}_k - h_{(\boldsymbol{x}_k)}) \end{equation} \] \[ \begin{equation} \hat{\boldsymbol{P}_k} = (\boldsymbol{I} - \boldsymbol{K}_k {\boldsymbol{J_h}}_k)\boldsymbol{P}_k \end{equation} \]

ここで、\(\boldsymbol{K}\)はカルマンゲイン、\(\boldsymbol{J_h}\)は観測方程式のヤコビアンです。

\[ \begin{equation} \boldsymbol{K}_k = \boldsymbol{P}_k {\boldsymbol{J_h}}_k^{\mathrm{T}} ({\boldsymbol{J_h}}_k \boldsymbol{P}_k {\boldsymbol{J_h}}_k^{\mathrm{T}} + \boldsymbol{R}_k)^{-1} \end{equation} \] \[ \begin{equation} {\boldsymbol{J_h}}_k = \left. \frac{\partial h}{\partial \boldsymbol{x}} \right|_{\boldsymbol{x}_k} = \begin{pmatrix} 0 & 0 & 0 & 1 & 0 & 0 \cr 0 & 0 & 0 & 0 & 1 & 0 \end{pmatrix} \end{equation} \]

さいごに

6軸IMUとホイールエンコーダを用いたEKFベースのオドメトリについて、数式をまとめてみました。参考になれば幸いです。

以上です。