IMUの姿勢計算に便利なmadgwickフィルタ
ROSにはIMUのセンサフュージョン処理をするimu_toolsというパッケージがあります．この中にあるmadgwickフィルタというのが軽量で高精度でマイコンで使うのにとても良いのでメモ．
imu_filter_madgwick - ROS Wiki
大元？はこちら．
Open source IMU and AHRS algorithms – x-io Technologies
フィルタの内容については下記ブログでほどよくまとめられています．
フィルタのソースコードは上記大元のサイトや，imu_toolsのレポジトリから見ることができます．
imu_tools/imu_filter_madgwick at indigo · ccny-ros-pkg/imu_tools · GitHub
ここのimu_filter.hとimu_filter.cppがフィルタの本体です．下記データを入力するとIMUの姿勢をクォータニオンで算出してくれます．
・3軸の角速度
・3軸の加速度
・サンプリングタイム
・座標系の構成(nwuとかneuとか）
このフィルタの良いところは何より計算がかなり軽量なのに精度がいいこと．唯一重そうな平方根の計算も数値計算で高速化してあるので速いです．計算が軽いのでマイコン上で余裕で回せてありがたい．FPUのないSTM32F1のマイコン(クロック168MHz)で2000Hzで回しても余裕です(といいつつ，他のフィルタを知らないので比較的なことはわかってません)．
僕の使い方は以下のようなもの．
- このフィルタのソースコードから必要な部分を取り出してマイコンに移植
- ロボットに搭載したマイコンでIMUから角速度と加速度データを取得
- マイコン内でフィルタ処理をして姿勢を計算
- 計算した姿勢データを制御PCに送信
というものです．imu_toolsを使いたいからといって生データをPCに送ってフィルタをかけると通信量を食うしサンプリング周期を上げられなかったりするので，マイコン上で計算できるならそうした方がいいです．
また，基本のドリフト対策として，最初に静止状態で角速度のゼロ点をサンプリングしておいて，その値を差し引いた角速度データをフィルタに入力しています．これをすると生データを入れるよりもかなりドリフトが軽減されます．静止することがあるロボットの場合は静止したタイミングでこのゼロ点を更新していくのは有効です．
このソースコード内ではクォータニオンはq0, q1, q2, q3と表記されています．これはwxyzの表記だとqw=q0, qx=q1, qy=q2, qz=q3という対応です．
僕が使用しているIMUはお馴染みのInvenSenseのMPU9250やMPU6000です．これらはIMU内部にフィルタ処理をしてくれるDMPなるものを書き込んで使えるそうで，これが使えるならそれで十分かも知れません．mbedやarduinoのライブラリだと簡単に使えるようで，それらから移植できればいいのでしょうが，僕は手っ取り早さとゲイン調整とかのわかりやすさでmadgwickフィルタを使っています．
そういえば最近マイクロマウス界では新たなIMUが使われ始めたという噂だけ聞いて型番を聞きそびれていましたが，調べるとICM-20602ですかね? 精度が上がってるみたいですね．こういうのはなるべく最新を追い続けておきたいです．
