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 がかんたんに取れそう。しかもドリフトが抑えられているとのこと。
難点は、ちょっと値段が高い!!
6. 別の、Madgwick Filter があったので試してみた。
DonovanZhu / 9DoF_MARG_Madgwick_Filter
これは、ドリフトしないので、使えそう。
ブルブルしているので、少し、beta = 1.5 を下げてやれば、使えそうじゃ。
あと、g の単位を、 DPS から RadPS に変えないといかんみたい。
// Convert gyroscope degrees/sec to radians/sec
gx *= 0.0174533f;
gy *= 0.0174533f;
gz *= 0.0174533f;
大元の最新版も、今まで使っていたものより、だいぶ良さそう。
Open source IMU and AHRS algorithms
6軸版だと、動作も安定しているが、やっぱり、ほんの少しだけドリフトするみたい。
9軸版だと、ドリフトしないが、ブルブルが出てくる。
また、9軸版だと、Magnet のキャリブレーションの仕方で、動きがかなり違うみたい。
7. Mahony を使ったサンプルがあるので、これを参考に、キャリブレーションの方法から見直ししたほうが良いかも?
jremington / ICM_20948-AHRS
Open source IMU and AHRS algorithms の中の、MahonyAHRS を試してみた。
MahonyAHRS の 9軸で、うまく行った。6軸だと、z軸が、すこしドリフトする。
Magenet のキャリブレーションも不要だった。ICM-20948 から取り込んだ mx , my, mz の単位を、
scaled data [uT] -> [T] にして、
注) scaled data [uT] でも、OK みたい。
ただし、 my , mz は、極性を逆転させないといけないみたい。
これは、すごい!!
はやくMahonyに、辿りつければよかった!!
今、ROS2 の Rviz2 で動作確認をしているが、Z軸にIMU を回転させると、少しオーバーシュートして、逆戻りするのが気になる。
もしかしたら、リニアリティーが問題かも知れない。
ここが、改善されなければ、ROS2 の実機ロボットでは使えないかも。
もしかしたら、Magneto のキャリブレーションが問題なのかも?
float twoKp = 2.0f * 0.5f;
をかなり大きくするみたい。しかしあまり大きくすると、ぶるぶる震えてくる。
Calculation of Magnetometer Heading (relative to magnetic north / 0 - 360°) #33 に、IMU Magetomerte
から、方角を出す方法があったので、
float z = (atan2(IMU.getMagY_uT(), IMU.getMagX_uT()) * 180) / PI;
単純に、これを使って、x=y=0.0 のオイラー角をクオータニオンに変換して、Ros2 TF で出してみた。
ぶるぶる震えてはいるが、一応、IMUの向きは、捉えているし、ドリフトは無い。
ただし、IMU を回転させると、リニアでは無い。ここでキャリブレーションが必要になることが確認できる。
Mahony に Mageneto を与えるときも、同じだろう。
自分の IMUの今の環境にあったキャリブレーションが必要じゃ。
ここで、気になるのが、ICM-20948 の DMP 9 の場合はどうなんだろう?
Magneto をキャリブレーションしているのだろうか?
また、ICM-20948 の DMP 6 は、Magneto を使っていないと思うが、ドリフトはしない。
8. SparkFun_ICM-20948_ArduinoLibrary の DMP sensor options の中に、
INV_ICM20948_SENSOR_GEOMAGNETIC_FIELD (32-bit calibrated compass)
の指定がある。
DMPを使えば、Magneto のキャリブレーションデータが取れるみたいだ。
サンプルの、Example9_DMP_MultipleSensors.ino を参考に、採ってみた。
一応、取り込めるみたいじゃ。
試しに、DMP 経由で、Acc, Gyro and Magneto を同時に採って、MahonyAHRS で使ってみた。
これは、うまく行った。ブルブルもなく、ICM-20948 の回転にもリニアに反応し、なにより、ドリフトがない。
唯一の気がかりが、DMP を使っていることじゃ。また、DMP 9 、 DMP 6 のときのように、データが途切れないかが、心配じゃ。
やっぱり、DMP のデータが止まってしまうので、これもNG か。
思うに、データが、途切れなければ、ICM-20948 DMP 9 が、性能的には一番安定していると思う。
また、石によっての個体差がある気がする。
長時間、DMP 9 でランニングテストをして、Rateが安定している石を使うようにしたほうが良いのかもしれない。
ROS2 で、IMU のTF Pose を RVIZ2 で表示して、ドリフトのチェックと、IMU の Rate を、別途 ROS2 Topic に出力して、
ICM-20948 の DMP 9 の切断がないのを、ただひたすら確認する。
Rate: 28[Hz] で安定している。
もしかしたら、SPI、I2C の接続ケーブルの 6P、4Pの 端子の隙間かも知れない。ここは、ケーブルを新品に替えて試す。
IMUを、下敷きの上にテープで留めて、始終細かに動かしていると、28[Hz] -> 10[Hz] に落ちて、TF の向きがぐるっと変わった。
ROS2 Rtabmap_ros で、Active SLAM で、永く動かしていると、ロボットが暴走するときの現象に一致する。
ちなみに、今は、SPI を止めて、I2C で使っています。
理由は、いろんな石を試すのに、I2C が結局はかんたんだから。by nishi 2025.4.23
BNO086 を試してみる事にする。
9. 参照2.
1) ロボティクスにおける地磁気センサの基礎知識
2) 地磁気センサでオイラー角の姿勢推定
3) 球面フィッティングの導出と実装
J-ROCKET-BOY/SS-Fitting
4) IMUと地磁気センサーを組み合わせて、ストラップダウン方式の慣性ナビゲーション・システムを構築