SparkFun ICM-20948 Arduino で、 DMP Fusion 9軸 がやっとOKになった。
今まで、SparkFun ICM-20948 Arduino で、 DMP Fusion 9軸 がうまく動作しなかったが、
再度チャレンジしてやっと動き出した。
ただし、結論を先に言うと、
ICM-20948 DMP 6 、DMP 9 は、使い込んでみると、信頼性が低いのが判った。
使っている途中で、quaternion の Z値が取れなくなる。
これだと、実際のロボットでは使えない。
代替として、DMP を諦めて、ACC、Gyro 、Magnet + MadgwickAHRS があるが、こちらは安定はしているが、精度がいまいち。 by nishi 2025.4.1
どうしようもない。今は、BNO086 の評価待ち。あるいは、ICM-20948 も石に拠っては、問題ない場合もありそうなので、幾つか購入して、良いものを使うか?
SparkFun ICM-20948 用の DMP img ファイルが良くないのか?
最終的には、これを改変しなければ解決しないのかも?
下記 Dev kit board を使って自分でいじらないといかんのかも?
eMD_Software_Guide_ICM20948.pdf
yomuevans/DK-20948
開発環境
Board: ESP32
Platform I/O IDE
IMU: SparkFun ICM-20948
Arduino Library: SparkFun_ICM-20948_ArduinoLibrary
Sample Program: Example6_DMP_Quat9_Orientation
Interface: SPI
1. Sample & 修正箇所。
Example6_DMP_Quat9_Orientation.ino の抜粋。
上記修正で、多分動くと思う。
あとは、Sampling Rate を、適切に調整すれば、OK だと思う。
DMP is running at 55Hz
とコメントがあるが、おんちゃんは、とりあえず、34[Hz] 辺りで試しています。
実際の動作確認は、ROS2 を使って、Rviz2 で、TF を表示させた状態で、
IMUを回転させて、動きが正常かチェックしないと、判らないぞね。
これで、やっと、自作 Turtlebot3 の IMU で、DMP 9軸 フュージョンが使えるか?
やれやれ。 by nishi 2024.2.19
2. 参照。
ROS2 で試すなら。
humble 版で、
micro-ROS/micro_ros_arduino を使って、PC で、/tf topic を拾う。
サンプルは、同じく、humble版で下記にあるので、実際に、IMU.h のところに、
Example6_DMP_Quat9_Orientation.ino を組み込む。
micro_ros_arduino/tree/humble/examples/micro-ros_tf_publisher
注) IMU.cpp、IMU.h は、無いので、使わずに、Example6_DMP_Quat9_Orientation.ino の内容を組み込む。
または、Example6_DMP_Quat9_Orientation.ino を、IMU.cpp として組み込む。
3. その後。
ROS2 Turtlebot3 自作 に、組み込んで、試しています。
tosa-no-onchan/foxbot_core3
問題なさそう。こちらのほうが、地球の地磁気が使えるので、安心感があるぞね。
注) Foxbot_coer3_r2 ロボットカーの電池での使用時は、に何故か、 ICM-20948 の接続に失敗することが、結構ある。
電池の電圧低下かと思っていたが、どうやら、 Pin Header とケーブルの接触不良が原因だったみたいじゃ。by nishi 2025.3.12
注2) 新しい ICM-20948 を購入して、DMP 9 で Rtabmap_ros2 Active SLAM and Mapping を動かしていたら、
突然、DMP 9 の データが途切れてしまうことが、生じているのが判った。
ロボットは、回転しているのに、Rviz2 のロボットの向きは止まったままになる。
古い、 ICM-20948 で、同じことを試したが、やはり、頻度の違いで、同じみたい。
こちらの、ソフトの問題か。かなり、頻繁にいじっている所為か。
どうやら、DMP9 、DMP6 で、ずっと使っていると、 ICM-20948 のデータは、上がってくるが、quaternion の Z値が正しく反映されなくなるのが原因みたい。
これでは、使い物にならない。
ICM-20948 の SparckFun の Arduino Library の DMP9、DMP6 は、ROS2 ロボットには使えないみたい。by nishi 2025.3.22
代替ライブラリーをためしてみるか?
Arduino library for the ICM-20948 motion tracking sensor -- with DMP support
こちらも、同じみたい。
ICM-20948 DMP は使えないみたい。
あとは、ACC、Gyro、 Magnet + MadgwickAHRS を使うしかない。残念じゃ。
ACC、Gyro、Magnet + MadgwickAHRS の方だと、問題は出ない。しかし、やはりドリフトがでるから、使えない。どうしたものか? by nishi 2025.3.26
上記点だが、Acc、Gyro の Low-Pass Filter を調整したら、ドリフトがなくなった。
Acc.
myDLPcfg.a = acc_d11bw5_n17bw; // acc_d11bw5_n17bw
Gyro
myDLPcfg.g = gyr_d11bw6_n17bw8; // gyr_d11bw6_n17bw8
上記フィルターで、OK でした。
したがって、下記、記述は、不要ぞね。 by nishi 2025.3.29
6軸 + MadgwickAHRS だとドリフトが少なくて、幸い Z軸だけのようだが、
このドリフトの単位量を計測して、それを使って定期的に補正する事にした。
静止時の、Roll、Pitch、Yaw のドリフト値(オイラー値)を目視チェックして、その逆値を使って、Madgwick の q0,q1,q2,q3 を quaternion の 積で、逐一補正すれば、できた。
Yaw は、だいぶ抑えられるようになった。もう少し自動で値を求める方法があれば良いと思う!!
ICM-20948 の石ごとに補正値を求めないとかん。
今は、Roll、Pitch のドリフトが気になりだした。
あとは、TKJElectronics/KalmanFilter を試してみるべきか?
4. SparkFun_ICM-20948_ArduinoLibrary のサンプル、Example10_DMP_FastMultipleSensors.ino の中に、ICM_20948_Status_e ICM_20948::initializeDMP(void) を、Over Write して、
DMP の設定を変える方法がある。
上記を参考に、試しに、DMP の DLPF(Digital Low-Pass Filter) だけを設定してみた。
オリジナルは、設定していないみたい。
でも、やはり、現象は変わらないみたい。
5. 参照。
ジャイロのドリフト補正と比較（カルマン、相補フィルター）
自作ロボットで、rtabmap_ros を使って、自分の部屋の Map を作成してみるが、
いずれにしても、6軸 + MadgwickAHRS は、 DMP 9、6 程には精度がでない気がする。
かと言って、ICM-20948 DMP だと、石によってばらつきがあって、データが途切れる。実際の運用には疑問が残る。
いっそのこと他の IMU を試すべきか?
Qwiic - BNO086搭載 VR IMUモジュール の
Example_01_RotationVector.ino だと、Quaternion がかんたんに取れそう。しかもドリフトが抑えられているとのこと。
難点は、ちょっと値段が高い!!