9軸センサーICM-42688+MMC5983 6軸&9軸回転ベクトル&3軸オイラー角 MAX1000Hz同時出力 ROS/ROS2対応 USB接続

はじめに

令和3年度発売の旧型機種のhayate_imu v2は多くの企業、学校法人のユーザー様にご利用いただいたことに、厚く御礼申し上げます。ありがとうございます。旧機種はv2.4までとリリースさせていただいておりますが、いまユーザー様のお手元にある旧バージョン製品のファームバージョンアップは、ユーザー様のもとで実施可能なので、詳細については、別途順次ご案内申し上げます。昨今の半導体ショックにより供給不足、価格高騰などの影響を受ける中、後継機種の開発を続けてきた結果、令和5年4月下旬より、9軸IMU/AHRS haya_imu v3.2の発売をお知らせさせていただきます。

製品紹介

Cortex-M4 (クロック周波数120MHz)、新型6軸IMUのICM-42688(ICM-42688-Pまたは、ICM-42688-V)、高精度3軸AMR方式地磁気センサMMC5983MAの実装により、通常出力モード、デモストレーションモード、キャリブレーションモード(初期バイアス測定)、6軸フュージョン回転ベクトルクォータニオン、9軸フュージョン回転ベクトルクォータニオン、3軸オイラー角の同時出力は最大1000Hzまで可能となります。ROS/ROS2とも対応しており、ドライバーはGithubよりダウンロードして製品とセットでご利用いただけます。

主な仕様

・型番 haya_imu v3.x
・内蔵チップ Microchip Cortex-M4(120MHz)、ICM-42688-VまたはICM-42688-P、MMC5983MA実装
・外部接続 USB2.0+ Type-C、USB+5V給電
・最大出力レート
  - 6軸/9軸フュージョン回転ベクトル四元数 1000Hz
  - 3軸オイラー角  1000Hz
  - 3軸加速度(アクセル)データ  1000Hz
  - 3軸角速度(ジャイロ)データ  1000Hz
  - IMU内部温度データ      1000Hz
  - 3軸地磁気(コンパス)データ  500Hz

・測定レンジ
  - 加速度(アクセル)センサ  ±8g
  - 角速度(ジャイロ)センサ  ±2000dps
  - 地磁気(コンパス)センサ  ±800µT

・バイアス測定補正 初期バイアス測定、動作時即時測定、内蔵補正機能あり
・消費電力 150mW以下(環境温度21℃ 実測値)
・寸法 38.0mm × 39.0mm × 4.8mm(突起物含む)
・取付穴 M3x4、隣り合う穴の中心間距離32.0mm

主な特長

・サービスモード 通常出力モード、デモンストレーションモード、キャリブレーションモード
・結果出力 6軸フュージョン回転クォータニオン、9軸フュージョン回転クォータニオン、3軸オイラー角1KHzまで同時出力、結果出力レートに関わらずIMU/地磁気センサのデータサンプリング周波数、フュージョン周波数は常に1000Hz/500Hzに設定済み
・初期バイアス測定 使用環境変化あった際に利用可能なキャリブレーションモードで最短数分程度で初期バイアス測定完了、MCUフラッシュに自動的に保存して、動作時に読み込んで即時バイアス測定&補正あり
・地磁気センサ温度補償 地磁気センサは、計測時間1msにわたるセットリセット計測(温度補償機能)使用済み
・磁気外乱による干渉 受けにくいことが当社実験(磁束密度約2G)にて確認済み
・ROS/ROS2対応 本体にはROS/ROS2ライブラリを実装せず、対向装置にドライバーインストールにより実現

詳細情報

【製品名称】haya_imu v3.x
【開発会社】ROBOT翔(株式会社翔雲)
【発売時期】令和5年4月下旬頃
【商品情報】9軸センサー6軸&9軸回転ベクトル 3軸オイラー角 MAX1000Hz同時出力 ROS/ROS2対応 USB接続 | ROBOT翔

参考情報

エンコーダ付きDCモータPID制御の実験-haya_imu応用例

3+

ロボット・ドローン部品お探しなら
ROBOT翔・電子部品ストア

9軸IMUセンサ ICM-20948をロボットに組み込もう

はじめに

TDK Invensense製9軸IMUのICM-20948は、MPU-9250の後継機種で、MPU-9250のVDDは2.4V~3.6V、VDDIOは1.71V~VDDに対して、ICM-20948のVDDは1.71V~3.6V、VDDIOは1.71V~1.95Vに低めに設定して、省電力となった。また、デジタルモーションプロセッサDMP(ICM-20948内蔵FPGA)によるデータフュージョン(FPGAによるFusion)の特長が継承して、さらにRAM容量が拡張して、6軸フュージョンのみならず9軸フュージョンまで増強して、較正機能もあると、以下参考文献を読むと詳細まで分かる。

環境

・Ubuntu 18.04
・ROS Melodic
・MCU: Cortex M0+
・IMU: ICM-20948

DMP3の出力確認

以下のように、出力レート50Hz、加速度Ax Ay Az、角速度Gx Gy Gz、磁場Mx My Mz、4元数Qw Qx Qy Qzの順に出力させる。

imu-icm20948-output
imu-icm20948-output

4時間+にわたる連続動作して出力を確かめる。確認環境は完全に静止な状態でもないので、ドリフトは納得いく範囲内にとどまっている。rvizで確かめてもドリフトが肉眼では見えないほど。ドリフトにおいては、MPU-9250から大いに改善されたと見られる。

imu-icm20948-output-4hours
imu-icm20948-output-4hours

出力確認動画は以下イメージをクリックすると、youtubeへジャンプする。

icm20948_imu_ros
icm20948_imu_ros

最後に

MPUシリーズと比べて、ユーザの事前校正いらず、長時間(実験は4時間程度まで)においても、ドリフトとくにヨウ角(Yaw、方向角)のドリフトは目立たないほどになった。また、1.71Vの低電圧でも動作可能なのでスマートデバイスや、ロボットの長時間電池駆動が可能になる。なお、出力レートはMax 225Hzと確認できた。ICM-20948 DMP3(IMU内蔵FPGA)から出力した、Accel/Gyro/Mag計9軸データ出力にFusion Quaternionの4元数データがそのまま利用可能で、遅延もソフト・カルマンフィルタなどより少なく他機種IMUより優れる(低遅延、6軸/9軸フュージョンデータ出力レート225Hz)ため、ロボットの精度向上に利用可能。1.8V VDDIO対応、DMP3の出力に手間かかった末、地磁気センサ出力は75Hzまでと少し残念だが、総じて優秀としか思わないICM-20948をロボット装置に組み込もうと決めた。

商品化モジュール

ICM-20948とCoretex M0+を組み込んだ回路を設計して、ROSに対応したロボット専用センサモジュールとして商品化して、2021年1月~、リリースと予定している。この商品は皆さんの学術研究にお役に立てるようと願う。主な仕様は以下のとおり。
・構成 CortexM0+ & TDK Invensense ICM-20948(9軸)実装
・接続 USB Type-Cコネクタ実装
・出力 6軸/9軸融合4元数はFPGA on chipから低遅延で出力、別途ソフトでフュージョン必要なし、出力レート225Hz、同時に加速度(アクセル)3軸データ225Hz、角速度(ジャイロ)3軸データ225Hz、地磁気(コンパス)3軸データ75Hzまで出力可能
・ROSパッケージ、Githubへ公開、ROS Kinetic以降対応、ROS TopicへSubscribeすることでデータが受け取り可能
・rviz実演、実演ビデオあり

【2021年3月いま現在】1回目制作分(評価版)、大学など研究機関へ無料配布中(アンケート調査あり)、WEBでの募集を含めて順次終了。

【2021年4~5月予定】2回目制作分(商用版)、販売の予定。

詳細情報

9軸IMUセンサ 6軸/9軸フュージョン 低遅延 USB出力 補正済み ROS対応

取扱店舗

9軸IMUセンサ 6軸/9軸フュージョン 低遅延 USB出力 ROS対応 | ROBOT翔

参考文献

Migrating from MPU-9250 to ICM-20948-InvenSense

関連記事

9軸IMUセンサ MPU-9250をロボットに組み込もう
6軸IMUセンサ MPU-6050をロボットに組み込もう

2+

ロボット・ドローン部品お探しなら
ROBOT翔・電子部品ストア

9軸IMUセンサ MPU-9250をロボットに組み込もう

はじめに

9軸IMUのMPU-9250はTDK InvenSense社製I2Cインターフェースの3軸ジャイロセンサ+3軸加速度センサ+3軸コンパスセンサIC、内蔵DMP(Digital Motion Processor)機能を使うことで、補正済みデータとしての4元数Quaternionまたはオイラー角、ロールRoll・ピッチPitch・ヨウYaw角の出力が選べる。また、MPUシリーズはすでに新規設計非推奨になっているため、後継機種はICMシリーズで、MPU-9250の後継機種はICM-20948となって、MPU-9250に比べて、事前校正不要で、ドリフトの低減、省電力などにおいてパフォーマンスが改善された。本文は、6軸MPU-6050に続いて、9軸MPU-9250 DMPから4元数Quaternionを読み込んで可視化するまでの手順を以下のとおりに示して、ROSドライバをGithubへ公開する。安価のため、MPU-9250サンプルの入手ルートはAliexpressにした。

mpu6050-mpu9250
mpu6050(6軸)-mpu9250(9軸)

I2Cインターフェースは、vcc、gnd、scl、sdaの4pinインターフェース

環境

・ubuntu 18.04 Tinker board(or Raspiberry Pi, PC)
・ROS melodic
・DFRobot Romeo mini v1.1(or arduino uno互換)
・MPU-9250/6500

準備①

・ros-melodic-rosserial-arduino、ros-melodic-rosserial、rviz_imu_pluginを入れる

$sudo apt-get update
$sudo apt-get install ros-melodic-rosserial-arduino
$sudo apt-get install ros-melodic-rosserial
$cd ~catkin_ws/src/
$git clone -b melodic https://github.com/ccny-ros-pkg/imu_tools
$cd ..
$catkin_make_isolated

・mpu9250_imu_rosを入れる

$cd ~/catkin_ws/src/
$git clone https://github.com/soarbear/mpu9250_imu_ros.git
$cd ~/catkin_ws/
$catkin_make_isolated

準備②

・firmware/MPU9250_DMP/MPU9250_DMP.inoをArduino IDEでArduinoに書き込む。

imu/dataの可視化

・実に使われるポートtty????を確認する。
・rvizが自動起動して、画面にあるセンサの動きを観察する。

$sudo ls -l /dev/ttyACM*
$sudo chmod 777 /dev/ttyACM0
$roslaunch mpu9250_imu_driver mpu9250_imu.launch

・以下スクリーンショットをクリックすると、youtubeへ遷移する。

mpu6050_imu_ros
mpu9250_imu_ros

センサ融合について

MPU-9250内蔵DMPおよび、センサ融合またはデータ同化Fusionに定番アルゴリズムであるKalman Filterの他、Complementary Filter、Madgwick Filterがある。振動やシステム誤差によって測定値に大きな影響あり、フィルタリングが必須とは言える。

ソースコード

mpu9250_imu_rosソースコード(Github)

後継機種

ICM-20948はMPU-9250の後継機種、その製品化情報は 9軸IMU/AHRS 6軸&9軸回転ベクトル&3軸オイラー角 MAX1000Hz同時出力 ROS/ROS2対応 USB接続9軸IMU 6軸/9軸フュージョン ICM-20948 Cortex-M0+内蔵 ROS対応

参考文献

1-Jeff Rowberg氏: I2C driver
2-ROS Repository: ROS imu_tools

関連記事

9軸IMU/AHRS 6軸&9軸回転ベクトル&3軸オイラー角 MAX1000Hz同時出力 ROS/ROS2対応 USB接続
9軸IMUセンサ 6軸/9軸フュージョン 低遅延 USB出力 補正済み ROS対応
9軸IMU ICM-20948をロボットに組み込もう
6軸IMU MPU-6050をロボットに組み込もう

3+

ロボット・ドローン部品お探しなら
ROBOT翔・電子部品ストア

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

概要

6軸IMU(慣性センサ)3軸加速度センサ + 3軸ジャイロセンサに対して拡張カルマンフィルタをかけてセンサ融合\((Sensor Fusion)\)またはデータ同化\((Data Assimilation)\)を行う。\(3\)軸加速度センサにてセンサローカル座標系(機体座標系と称す)の\(3\)軸における重力加速度gの分量が出力され、観測方程式によりオイラー角の\(θ,ϕ\)が計算できる。\(3\)軸ジャイロセンサから機体座標系の\(3\)軸まわりの角速度が出力され、状態方程式によりオイラー角の\(ψ,θ,ϕ\)が計算できる。また、ランダム誤差(ノイズ)が存在し、そしてドリフトが蓄積していくので、拡張カルマンフィルタの状態空間モデルでの補正を行う。\(6\)軸\(IMU\)慣性センサまたは\(9\)軸\(IMU\)慣性センサは、ロボット、ドローンなどの位置推定、姿勢推定などに大いに活用されている。本文の固定座標系は地面から見た、Z軸はUP方向の右手の法則に従う座標系とする。機体座標系は移動体にあったIMUセンサの座標系で、最初に機体座標系は固定座標系と重なるとする。固定座標系の\(Z, Y, X\)軸回りの固定角はヨー、ピッチ、ロールとする。本文の姿勢の表現は\(ZY’X^{\prime \prime}\)軸回り順のオイラー角\(ψ, θ, ϕ\)とする。

3軸加速度センサ

オイラー角

\(ZY’X^{\prime \prime}\)軸回り順として、つまり\(Z\)軸を第\(1\)回転軸、回転角度はオイラー角\(ψ\)、\(Y’\)軸を第\(2\)回転軸、回転角度はオイラー角\(θ\)、\(X^{\prime \prime}\)軸を第\(3\)回転軸、回転角度はオイラー角\(ϕ\)、即ち\(R_z(ψ)→R_y(θ)→R_x(ϕ)\)と記す場合、機体座標系とオイラー角の対応関係は以下の図に示す。オイラー角は必ず軸まわりの回転順と絡むので、変数は\(4\)つと考えても良い。本文では、オイラー角の回転方向と回転軸の関係は右手の法則に従う。

固定座標系・機体座標系・オイラー角(右手の法則)
固定座標系・機体座標系・オイラー角(右手の法則)

回転行列

固定座標系ベクトル\(p\)は物体座標系とともに、\(Z→Y’→X^{\prime \prime}\)軸まわり順として、それぞれオイラー角の\(ψ, θ, ϕ\)の回転移動を行って物体座標系ベクトル\(p’\)へ変換された場合、固定座標系ベクトル\(p\)、機体座標系ベクトル\(p’\)、三次元空間回転行列\(R_{zyx}\)の関係は、以下の式で表現できる。
$$ p=R_{zyx}p’ \\ R_{zyx} = R_z(ψ)R_y(θ)R_x(ϕ) \\ R_z(ψ) = \begin{bmatrix} cosψ & -sinψ & 0 \\ sinψ & cosψ & 0 \\ 0 & 0 & 1 \end{bmatrix} $$ $$ R_y(θ) = \begin{bmatrix} cosθ & 0 & sinθ \\ 0 & 1 &0 \\ -sinθ & 0 & cosθ \end{bmatrix} $$ $$ R_x(ϕ) = \begin{bmatrix}1 & 0 & 0\\ 0 & cosϕ & -sinϕ \\ 0 & sinϕ & cosϕ \end{bmatrix} $$ 本文の機体座標系(オイラー角)回転行列\(R\)の計算は右乗として、また固定座標系(固定角)回転行列\(R’\)の計算は左乗に注意する。\(R_{xyz}=R_{zyx}^{\prime},R_{zyx}=R_{xyz}^{\prime} \)の関係がある。

観測方程式

$$ p’ = \begin{bmatrix} a_x & a_y & a_z \end{bmatrix}^T \\ p = \begin{bmatrix} 0 & 0 &-g \end{bmatrix}^T $$ ただし、\(g\)は重力加速度\( (\simeq 9.81m/s^2 ) \)、\(a_x, a_y, a_z\)は重力加速度が機体座標系\(X^{\prime \prime}Y^{\prime \prime}Z^{\prime \prime}\)軸における成分。
$$ p’ = R^{-1}_{zyx}A = R_{x}^{-1}R_{y}^{-1} R_{z}^{-1} p $$ $$ \small R^{-1}_{zyx} = \begin{bmatrix} CθCψ & CθSψ & -Sθ \\ SϕSθCψ-CϕSψ & SϕSθSψ+CϕCψ & SϕSθ \\ CϕSθCψ+SϕSψ & CϕSθSψ-SϕCψ & CϕCθ \end{bmatrix} $$ ただし、\(C = cos, S = sin \)とする。
よって、\(3\)軸加速度センサの観測方程式は以下の式で表す。
$$ \begin{bmatrix}a_x\\a_y\\a_z\end{bmatrix} = \begin{bmatrix} gsinθ \\ -gsinϕcosθ \\ -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軸ジャイロセンサ

Gyrosensor_ωx_ωy_ωz
Gyrosensor_ωx_ωy_ωz

状態方程式

ジャイロセンサから測定した機体座標系の\(X^{\prime \prime}Y^{\prime \prime}Z^{\prime \prime}\)軸回りの角速度成分は、\(ZY’X^{\prime \prime}\)軸回りのオイラー角の時間微分\(\dot{ϕ}, \dot{θ}, \dot{ψ} \)との対応関係は以下の式で表す。
$$\scriptsize \begin{bmatrix}ω_x\\ω_y\\ω_z\end{bmatrix} = \begin{bmatrix} \dot{ϕ} \\ 0 \\ 0 \end{bmatrix} +R_x^{-1}(ϕ) \begin{bmatrix}0 \\ \dot{ θ}\\ 0 \end{bmatrix} + [R_y(θ)R_x(ϕ)]^{-1}\begin{bmatrix}0 \\ 0 \\ \dot{ψ}\end{bmatrix}$$ よって、回転行列を利用して、機体座標系の角速度から、\(ZY’X^{\prime \prime}\)軸回りのオイラー角の時間微分は求められる。$$\small\begin{bmatrix}\dot{ϕ}\\\dot{θ}\\\dot{ψ}\end{bmatrix} = \begin{bmatrix}1 & \frac{sinϕ sinθ}{cosθ}& \frac{cosϕ sinθ}{cosθ}\\0&cosϕ&-sinϕ\\0 & \frac{sinϕ}{cosθ} & \frac{cosϕ}{cosθ}\end{bmatrix}\begin{bmatrix}ω_x\\ω_y\\ω_z\end{bmatrix} $$ ただし、\( θ ≠ ±90° \)ジンバルロック回避、 \(\dot{ϕ}, \dot{θ}, \dot{ψ} \)はそれぞれオイラー角の時間微分、\(ω_x, ω_y, ω_z\)は機体座標系の\(X^{\prime \prime}Y^{\prime \prime}Z^{\prime \prime}\)軸回りの角速度成分となる。

オイラー角で状態方程式の表現

ジャイロセンサの角速度からオイラー角の計算式は以下となる。$$\begin{bmatrix}ϕ\\θ\end{bmatrix} = \begin{bmatrix}ϕ+\dot{ϕ}Δt\\θ+\dot{θ}Δt\end{bmatrix}$$ ただし、\(ψ\)角の積分計算はここで省略する。

誤差の補正

カルマンフィルタおよび本文の及んだ誤差はランダム誤差といい、バイアス誤差はすでにキャリブレーションなどの手法で取り除いたと想定する。加速度センサの精度が悪く、とくに運動中の場合、ただし加速度センサの誤差が蓄積しない。ジャイロセンサの精度が良く、ただしドリフト現象が起きる。ドリフトにより時間とともに誤差がじわじわと蓄積していく。補正手法として、加速度センサにジャイロセンサの出力値を組み合わせて、静的重み係数(経験値)を付ける相補フィルタの他、時系列分析から異なるセンサの出力値に信頼性に関わる動的重み係数を付ける、いわゆるカルマンフィルタ\((Kalman Filter)\)の再帰的アルゴリズム(手法)がある。フィルタとはランダム誤差(ノイズ)をフィルタリングする意味合いがある。性能からみればカルマンフィルタのほうは優れるのでよく使われる。

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

カルマンフィルタ

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

状態空間モデル

一般、汎用的な状態方程式、観測方程式は以下のとおり。
$$\begin{eqnarray*}&& x_t = F_{t-1}(x_{t-1}^{true}+ p_{t-1} ) + q_{t-1}\\&& y_t = H_t(x_t^{true}) + r_t\end{eqnarray*}$$ ただし、\(F(⋅),H(⋅)\)は実装に際して、\(3\)軸加速度センサ、\(3\)軸ジャイロセンサに掲載した状態方程式、観測方程式のとおりで利用する。システム誤差、観測誤差\((q_t, r_t)\)は期待値が\(0\)、分散が\((Q_t, R_t)\)の独立ガウス分布にする。
$$\begin{eqnarray*}&& q_t ~ N(0,Qt)\\&& r_t ~ N(0,Rt)\\&& E[q_t r_t]=0 \\&& p_{t-1}=x_{t-1}-x_{t-1}^{true}\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*}$$

状態値、共分散行列予測ステップ(t>0)

$$\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 + Q_{t-1}\end{eqnarray*}$$ ただし、\(\bar{x_t}\)は予測値、\(\bar{P_t}\)は事前共分散行列、\(Q_{t-1}=E[q_{t-1}q_{t-1}^T]\)

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

$$\begin{eqnarray*}&& K_t = \bar{P_t}H_t^T(H_t\bar{P_t}H_t^T + R_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}\)は事後共分散行列、\(R_{t}=E[r_{t}r_{t}^T]\)

フローチャート

Kalman_Filter_Flowchart_2
Kalman_Filter_Flowchart_2

導出に関わる参考情報として、本サイト記事 カルマンフィルタの導出 に載ってある。

拡張カルマンフィルタ

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

状態空間モデル

$$\begin{eqnarray*}&& x_t = f_{t-1}(x_{t-1}^{true}+p_{t-1}) + q_{t-1}\\&& y_t = H_t(x_t^{true}) + r_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*}$$

状態値、共分散行列予測ステップ(t>0)

$$\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 + Q_{t-1}\end{eqnarray*}$$ ただし、\(\bar{x_t}\)は予測値、\(\bar{P_t}\)は事前共分散行列、\(Q_{t-1}=E[q_{t-1}q_{t-1}^T]\)

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

$$\begin{eqnarray*}&& K_t = \bar{P_t}H_t^T(H_t\bar{P_t}H_t^T + R_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}\)は事後共分散行列、\(R_t=E[r_{t}r_{t}^T]\)

フローチャート

Extended_Kalman_Filter_Flowchart_1
Extended_Kalman_Filter_Flowchart_1

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

状態空間モデルは以下のようになる。
$$\scriptsize x_t=f_{t-1}(x_{t-1}^{true}+p_{t-1})+q_{t-1}=\begin{bmatrix}ϕ+\dot{ϕ}Δt\\θ+\dot{θ}Δt\end{bmatrix}+q_{t-1}$$
$$y_t=H_t(x_t^{true})+r_t=\begin{bmatrix}ϕ\\θ\end{bmatrix}+r_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} ϕ \\ θ \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-1}(x)}{\partial x}|_{x=\hat{x}_{t-1}}\\=\begin{bmatrix}\frac{\partial (ϕ+\dot{ϕ}Δt)}{\partial ϕ}&\frac{\partial (ϕ+\dot{ϕ}Δt)}{\partial θ}\\\frac{\partial (θ+\dot{θ}Δt)}{\partial ϕ}&\frac{\partial (θ+\dot{θ}Δt)}{\partial θ}\end{bmatrix}\\\scriptsize=\begin{bmatrix}1+Δt(ω_y\cdot cϕ\cdot tθ-ω_z\cdot sϕ\cdot tθ)&Δt(\frac{ω_y\cdot sϕ}{c^{2}θ} + \frac{ω_z\cdot cϕ}{c^{2}θ})\\-Δt(ω_y\cdot sϕ+ω_z\cdot cϕ)&1\end{bmatrix}$$ ただし、\(c = cos, s = sin, t = tan\)

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

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

imu_ekf
imu_ekf

カルマンフィルタの解釈

カルマンフィルタの原理は以下のイメージに示す。

kalman_filter_principle_n
kalman_filter_principle_n

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

他のフィルタについて

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

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

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

参考文献

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

関連記事

粒子フィルタ
SLAM~Unscentedカルマンフィルタ
9軸IMU 6軸/9軸フュージョン 低遅延 USB出力 補正済み ROS対応
YDLIDAR G4=16m 薄型 ROS対応SLAM LIDAR
点検向け自律移動ロボットRED(薄型・小型)
研究開発用 台車型ロボット キット

2+

ロボット・ドローン部品お探しなら
ROBOT翔・電子部品ストア