ICM-42688 Madgwick、Mahony 6軸 Fusion を試す。
ポンコツの ICM-20948 DMP を諦めて、BNO086 を試して、結局ICM-20948 Madgwick 6軸 Fusion になった。 の続きです。
ICM-42688 Acc + Gyro Mahony と Madgwick 6軸フュージョンを試す。
ICM-20948 より格段にノイズが低い、ICM-42688 を使って、Mahony と Madgwick 6軸フュージョンで、
実用に耐えうるノンドリフトの性能が実現できるか確認する。
目標は、ROS2 Rtabmap_ros の Depth camera による、Active SLAM で、毎回キレイに Mapが作成できるか確認する。
開発環境
Platform I/O IDE
ESP32 Arduino
ICM-42688 Acc 、Gyro : 500[Hz]
I2C
実機での確認
foxbot_core3 for ros2 に乗っけて、動作確認をする。
Arduino のライブラリーは、下記を使う。
finani / ICM42688
残念ながら、tdk-invn-oss のオリジナルでは無いみたい。
ただし、ICM42670P のオリジナル、motion.arduino.ICM42670P より、使い良いかも知れない。
SparkFun の ICM-20948 Arduino ライブラリー版に似ているので、Acc + Gyro 同時取得が、簡単のようだ。
ありがとう、finani
1. ソースダウンロード
$ cd ~/Auduino/lib-foxbot_core3_r2
$ git clone https://github.com/finani/ICM42688.git
ICM42688/src/ICM42688.cpp 、 ICM42688.h をそれぞれ、
ICM_42688.cpp 、 ICM_42688.h に名称を替えて、下記に修正。
注) 自分のソースが、ICM42688.cpp 、 ICM42688.h を使っているので!!
ICM_42688.cpp
ICM_42688.h
2. 使用する、ICM-42688 と結線。
ICM-42688 は、
中華版に懲りたので、今回は、正規の石が入っているであろう、
ICM-42688 6軸センサモジュール(３軸加速度＋３軸ジャイロ) にした。
しかしこれは、基本ブレッドボード向けじゃ。ロボットに搭載するには、このままでは不向き。
一回り大きな基盤に乗せて結線をするか、やはり、中華版を購入したほうがよさそう。
結線方法は、
ICM-42688説明書
を参考に結線した。
外付けの Pull UP は、必須みたい。
3. 使った感想。
ICM-20948 よりは、断然ノイズが少ないし、Out Put Rate も大きい。
普通に、Madgwick で、6軸 Fusion を試してみた。
ドリフトは、かなり少ないが、やはり有る。
Mahony で、
Acc 4G 500[hz]
Gyro 2000[dps] 500[hz]
myICM.setFilters(true, false); // gyroFilters = true
で、結構ドリフトしないみたい。皆無ではないが、
これなら、実機で試してみないといかん。
これ以上は、
Gyro Z の Noise Cut-Off を行うか、別のIMUの キャリブレーションされた Magnet を使って、9軸フュージョンをにするかどちらかか?
しかし、IMUを手で回転させてみると、Rviz2 の TF-Pose が、しっかり感がある。