ポンコツの ICM-20948 DMP を諦めて、BNO086 を試して、結局ICM-20948 Madgwick 6軸 Fusion になった。
結局は、ICM-20948 Madgwick 6軸 Fusion で、なんとか使えるのが実現できたの話。
最後まで、目を通しとうせ。by nishi 2025.5.25
SparkFun ICM-20948 Arduino で、 DMP Fusion 9軸 がやっとOKになった。 の続きです。
1. 中華製の安い BNO086 があったので、購入して、DMP 9 を試してみる事にした。
当初、SparkFun の Arduino Library で試していたが、これがまるっきり使えない。
やはり、正規版の石でないとだめなのかと思ったが、
Adafruit の Arduino Library があったので、こちらを試してみた。
adafruit / Adafruit_BNO08x
こちらだと、安定して動くみたい。
しかも、こちらだと、RST、INT ピン接続が必要ないみたい。
もしかしたら、Chip のfirmware のバージョンの新旧の違いがあるのかも知れない。
中華製の Chip の firmware のバージョンが 旧で、それが、Adafruit の Arduino Library と一致したのかも?
注) ESP32 の場合、Adafruit の I2C Arduino Library が相性が悪いことが判明、逆にSparkFun ICM-20948 Arduino のほうが相性が良い。
SparkFun ICM-20948 Arduino で、 DMP Fusion 9軸 がやっとOKになった。 で実施した、Rviz2の長期テストでは、問題ないみたい。
あとは、草刈りロボットの実機で使ってみて、評価しないといかんぞね!!
下記ライブラリーも、同時に必要みたい。
1) adafruit / Adafruit_BusIO
2) adafruit / Adafruit_Sensor
2. Adafruit DMP 9 を 自作 Turtlebot3(foxbot_core3_r2) で試してみた。
時々、ノイズみたいなのが入って、tf-base_footprint が、一瞬狂うみたい。
それがなければ良いのだが、現状ではあまり使えそうにない。
残念じゃ。
中華のコピー版だからか? 正規版だとまた、違うのだろうか?
ちょっと、改善できないか試す。
// interval_us 20000 -> 50[Hz]
rc = myIMU.enableReport(SH2_ROTATION_VECTOR,20000);
interval_us=20000 -> 50[Hz]
を指定して、再度ためしてみる。
実測 out put rates: 48 - 28 [Hz] の範囲で変化する。
以前は、指定無しで、 14 - 10 [Hz] だったので、改善するのかも?
結果は、同じ。時々tf-base_footprint が、とんでもない方を向く。
2.1 Adafruit で、Acc + Gyro + Mahony では、ドリフトが結構少ないみたい。
ICM-20948 DMP 6 or 9 ほどではないが、結構頑張っているみたい。
ただし、これは、acc calibrated , gyro calibrated データだからみたい。
uncalibrated だと、やはりドリフトするみたい。
しかし、これも gyro の一瞬の大きなノイズで使えないと思う。
2.2 SparkFun のライブラリーで再度、DMP を試してみる。
bool BNO08x::enableRotationVector(uint16_t timeBetweenReports)
timeBetweenReports: interval timer[ms] みたいなので、
rc = myIMU.enableRotationVector(20);
50[Hz] で使ってみた。
こちらは、突然のノイズは入らないみたい、だが、実機で試さないとわからん。
実際の、out put rate は、 48[Hz] - 17[Hz] で、変化するみたい。
昨日は、timeBetweenReports をデフォルト指定で、試して少しドリフトしたが、
20 を指定したら、ドリフトしていないみたい。
後で、自作 Turtlebot3(foxbot_core3_r2) で試してみる。
結果は、こちらも同じ。時々tf-base_footprint が、とんでもない方を向く。
SparkFun で、Acc + Gyro + Mahony は、ノイズが多くてまともに使えない。
とにかく、Acc 、Gyro とノイズが多い。
特に、問題なのは、Gyro の Z軸に対して、時々、 30 - 40[degree per sec] のノイズが入る。
これが原因で、DMP での tf -> basefoot-print の向きが、一瞬ほかを向く。
I2C だと、頻繁に、
requestFrom(): i2cRead returned Error 263
が出るし、Acc か、Gyro で時々大きなノイスが入ってすべて台無しになる。
ESP32 の場合の、Adafruit の I2C Arduino Library 使用について、
【IMU】BNO085は、BNO055のファーム改良品＜Adafruit版BNO085ゲット＞ に、
BNO08xのI2C実装は、いくつかの状況でI2Cプロトコルに違反します。そのため、特定のチップファミリーとうまく動作しません。
Espressif ESP32、ESP32-S3、NXP i.MX RT1011ではうまく動作せず、I2Cマルチプレクサでもうまく動作しません。SAMD51、RP2040、STM32F4、nRF52840での動作はより確実です。
とある。AdaFruit ライブラリーだと、 ESP32 だと、SPI 接続 がおすすめみたいだが、
そもそも、この中華版は、SPI では繋がらない。
ESP32 の場合の I2C接続は、SparkFun の方を使う事にしたら解決した。
だが、そうすると、 Acc 、Gyro Data が単体でうまく取れない。
あと、問題は、 Acc または Gyro で、時々大きなノイズが入ることだ。
これがそもそもの問題で、中華製 BNO086 に問題があるのだろう。
中華製 BNO086 は安いが、おすすめではない!!
3. ICM-20948 の後継機種として、ICM-42688 があるみたい。
こちらは、6軸センサーだが、精度とRate が、良くなっているみたい。
それであれば、Mahony で使えば、良いような気がする。
1) ICM-20948 と ICM-42688 の比較がある。
9軸センサーICM-42688 + MMC5983MA vs. ICM-20948
2)
ICM-42688 6軸センサモジュール(３軸加速度＋３軸ジャイロ)
3)
ICM-42688 and ICM-42670 PDF こちらにスペックの比較がある。
For low power
applications, ICM-42670-P, and for higher performance applications, ICM-42688-P is recommended,.. との事みたい。
device: a) ICM-20948 b) ICM-42670-P c) ICM-42688-P
ACCEL Noise (μg/√Hz): a) 230 b) 100 c) xy:65 x:70
GYRO Noise (dps/√Hz): a) 0.015 b) 0.007 c) 0.0028
ICM-42670-P は、ICM-20948 の半減か?
ICM-42688-P は、ICM-20948 の ACC: 1/3 と GYRO: 1/5 か。
精度がほしければ、ICM-42688-P のほうが良いみたい。ICM-20948 + Mahony で、ドリフトでさんざんやられたので、ICM-42688-P が良いのかも。
半減の ICM-42670-P でも可能か?
Magnetometer が必要であれば、 IST-8306 Magnetometer、MMC5983 と組み合わせれば良いみたいじゃ。
Accel と Gyro のデータの取り込みに関しては、ICM-20948 系は、格段に安心じゃ。
BNOxxx 系は、使い物にならん、散々じゃ。
Mahony、Madgwick だと、9軸でも、Magneto Data が無いときは、0 を渡せば、Accel と Gyro だけで動作する。
ならば、ICM-42688-P の Accel と、Gyro を基本に使って、Magneto は、ICM-20948 DMP の キャリブレーションされた値が取れた時だけ使えば、OKか。
その前に、ぜひ、ICM-42688-P の Accel + Gyro + Mahony 6軸フュージョンで試してみたい!!
4. ICM-20948 Acc + Gyro Madgwick 6軸フュージョンを改めてためす。
今回は、Gyro の感度を、2000[dps] -> 500[dps] に落とす。
Acc 4G 500[Hz]
Gyro dps500 500[Hz]
Madgwick beta = 0.1f
で、再度ためしてみたら、z軸のドリフトがかなり少ないみたい。ドリフトは、皆無ではないが。
自作 Turtlebot3(foxbot_core3_r2) で試してみたが、15分ほどの走行であれば、使えそうじゃが。下に書いた、Madgwick の フュージョン6 の z軸の補正機能を使う事にした。
次の IMU に移行するまでは、これを使うことにする。
これなら、ICM-42688-P の Accel + Gyro + Madgwick 6軸フュージョン でも、十分使えそうな気がする。
4.1 ドリフト補正の導入。
ドリフト補正としては、ロボットが停止している間は、Madgwick を止めても良いかも知れない。
これは、やはり、どうだろう。ノイズは、動いている時の信号に比して、十分小さいので、ありかもしれないが?。
プログラムが、複雑な気がするので、選択から外す。
Gyro のノイズが、ドリフトの原因の気がする。特に z軸 gz の静止時のノイズが問題みたい。
もちろんIMUの移動時も、ノイズが影響しているのだろうが、ここは、とにかく、IMUの静止時のドリフトをなんとかしたい。
gyro の起動時のキャリブレーションを行って、このキャリブレーションで、gyro の補正(バイアス補正)をしたあと、
gz のノイズを削ることにした。
おんちゃんの場合、+- 10 以内の場合は、0 にしてみた。
if(abs(gyroADC[2]) <= 10)
gyroADC[2]=0;
注1) gyroADC[2]: キャリブレーションで補正(バイアス補正)したあとの、gyro z軸の値
補正値の決め方は、Arduino-IDE のSeial Monitor、Serial Plotter で確認すれば良い。
注2) バイアス補正は、今までint の整数だったが、float にして、浮動少数点にして、細かくする。
注3) gz のみ補正する?。gx、gy も、補正しても良いみたい。3軸すべて補正したほうが、平面での回転時のMadgwick のブレは少ないかも。
注4) Madgwick は、起動してから、pose が落ち着くまでえらい時間が掛かるのが難点。
注5) Gyro バイアス z (float) の値を、Madgwick の z軸のドリフトに合わせて、自動調節させても良いかも知れない。
これで、結構効果があるみたいじゃ。
Madgwick では、70[分] で、ドリフトなし。ただし、IMUを平面上で回転させる時、Pitch が少しだけ上下する。
これは、Rtabmap_ros では、悪さをしそう。
Mahony では、30[分] で、本当に少しだけ、0.5[度] ほどドリフトするかも。しかし、pitchの上下は無い。
それならと、Dual Filters にして、Madgwick で、z を、 Mahony で、y , x を分担させて、逐次、Madgwick の z で、Manony の z を書き換えるようにした。
pitch の上下のブレは、だいぶ収まった。
あとは、実機で試してみる。
いかん、これは、180[度] 回転したところで、TF-basefoot_print が狂った。IMU 単体でバグを削ってから、再度チャンレンジする。
やはり、これは、NG みたいだ。IMU を回転させると、傾くみたいじゃ。
rtabmap_ros での Active Slam でマップ作成には、IMUの性能が大きく影響する。誤差の少ないIMU をつかう場合と、そうでない場合とでは、顕著に差が出る。
Madgwick 単体で、GYRO_NOISE_CUT_OFF=10 で、rtabmap_ros_my/launch/rtabmap_oak-d_rgb_depth.launch.py を使って、
部屋をActive SLAM でマップを作成させてみが、なかなか良い具合じゃった。
ただ、ロボットが回転するときに、IMUのPose の Pitch が上下していると思うので、これを取れば完了だろうが、
これは、ICM-42688 Madgwick 6軸Fusion に託すことにする。
これは、 githubu に公開したぞね。by nishi 2025.5.25
foxbot_core3 for ros2
IMU/ICM20948.cpp
実機ロボットで、Rtabmap_ros で、Mapping に使っていると、やはり、DMP には敵わないきがした。
なんとか、DMP で実現できる方法を考えたほうがよさそう。
5. 参照。
1) Turtlebot3 IMU オリジナル。
opencr_arduino/IMU
2) カルマンフィルターの適用。
９軸センサーMPU9250へのカルマンフィルタ適用
3)
ジャイロのドリフト補正と比較（カルマン、相補フィルター）