中華製の BNO086 に懲りたので、正規版の BNO086 を購入して、DMPを試すことにした。
Mouser、DigiKey だと、6,000円台で手に入るので、正規版を購入して試すことにした。
Mouser で購入した、正規版が届いたので、早速、テストとしたぞね。
突然のノイズも出なくて、OKだった。
やはり、正規版は、いいぞね。
自作 Turtlebot3(foxbot_core3_r2) で、早速走行テストじゃ。
rtabmap_ros_my/launch/rtabmap_oak-d_rgb_depth.launch.py
で、foxbot_cor3_r2 を、C++ プログラムで、部屋の中を自動走行させて、
Rtabmap_ros で、Map 作成させてみた。
久しぶりに完走できました。by nishi 2025.6.20
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-20948 と BNO086 のコンビネーションで、Mahony の 9軸Fushion を試してみる。
MotionCal で、ICM-20948 の Magnetoをキャリブレーションするのは至難の業だと思うので、 ICM-20948 Acc 、Gyro と
BNO086 MagCali(キャリブレーションされた Magneto) を、Mahony または、 Madgwick に通して、9軸Fushion できないか試してみる。
ICM-20948 Acc 、Gyro : 400 - 500[Hz]
BNO086 MagCali: 100[Hz]
でサンプリングして、Mahony または、 Madgwick の 9軸Fushion をしてみる。
MagCali 100[Hz] なので、MagCali が取れていないときは、mx=my=mz=0.0 として、自動的に 6軸Fushion となるようにする。
Madgwick も、Acc、Gyro を 500[Hz] 辺りで与えると、ブルブルも気にならなくなる。
この組み合わせだと、ドリフトがでなくなるのでは? ただし、向きの精度は、最終的には、BNO086 MagCali の精度と同じにるのでは?
ポンコツの ICM-20948 DMP を諦めて、BNO086 を試して、結局ICM-20948 Madgwick 6軸 Fusion になった。
注) ICM-20948 DMP がポンコツと言うわけでは無いみたい、SparkFun のライブラリーに入っている、icm20948_img.dmp3a.h が 1.1.0 で、古いのが問題みたい。
最新の、1.1.1 版を使えば、OK とおもったが、やはりだめだった。やはりポンコツか!!
結局は、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の長期テストでは、問題ないみたい。
あとは、草刈りロボットの実機で使ってみて、評価しないといかんぞね!!
SparkFun ICM-20948 Arduino で、 DMP Fusion 9軸 がやっとOKになった。
今まで、SparkFun ICM-20948 Arduino で、 DMP Fusion 9軸 がうまく動作しなかったが、
再度チャレンジしてやっと動き出した。
ただし、
ICM-20948 DMP 9 は、実機で使い込んでいると、途中で、IMU のデータが上がってこなくなる現象に悩まされた。
ロボットは、回転しているのに、TF Pose がそのまま動かない。回復した時は、かなりずれている。
DMP 6 でも同じ症状がでる。
これだと、実際のロボットでは使えない。
eMD-SmartMotion-ICM20948-1.1.1.zip の中の、icm20948_img.dmp3a.h を試しに使ってみた。SparkFun のライブラリー版は 1.1.0 で古いみたい。
これで、DMP 6 を試すと症状が出なくなったみたい。
もう少し使い込んでみないと、わからんが。by nishi 2025.6.18
eMD-SmartMotion-ICM20948-1.1.1.zip のダウンロード方法は、11.参照2. 7) に記載した。
やっぱり、改善されないみたい。残念。
開発環境
Board: ESP32
Platform I/O IDE
IMU: SparkFun ICM-20948
Arduino Library: SparkFun_ICM-20948_ArduinoLibrary
Sample Program: Example6_DMP_Quat9_Orientation
Interface: SPI
ROS IMU による移動距離(距離測定) と 向き測定。
IMU 6軸フュージョンを使って、距離と向きを測定します。
このページは、自作 Turtlebot3 自立走行に向けたプログラム。#7 の、16. 再挑戦。 以降を、実際のやり方に向けて、改めて記述したものです。
今回のプログラムの考え方を書いているの、一度ご覧ください。
IMU(ICM-20948) 6軸センサー(加速度、ジャイロ)値を、ESP32で取り込んで、6軸フュージョン(MadgwickAHRS) で、
クオータニオンを算出して、それを元に、加速度センサーの読み取り値から、1G の影響をキャンセルした正味のAccを算出した、加速度と時間から速度、移動距離を出して、
ロボットの座標系から、ROS の基準座標系に変換した位置と姿勢データを、ESP32 Wi-Fi で、リモートPC 上のROS サーバーへ送って、
リモートPC上の Rviz を用いて、3D での姿勢と移動距離を見える化します。
良くYouTube で見かける、IMU を手に持って、空中で動かして、それに連れて、PCのモニターの中の物が3Dで動く、やつです。
注) 但し、あまり完成度は、高くありません。ぜひ、ご自分で完成度を上げっていてください。by nishi 2022.5.13
注2) 基準座標系: ロボットの開始時点の座標。ROS の TF で言えば、odom になります。
開発環境
1. 開発 PC
Ubuntu Mate 18.04
Visual Studio Code + Platform I/O IDE
ESP32 Arduino Framework
Arduino IDE ( こちらは、Tools -> Serial Plottter を使う為 )
2.実行環境
1) PC / ROS Server
Ubuntu Mate 18.04
WiFi 環境が必要です。
2) SOC
ESP32
Wi-Fi Ros Serial通信
TTL-Serial : デバッグ用に使います。テストが終われば、不要です。
3) IMU
Spark Fun ICM-20948
SPI 接続
3. ハード構成
ESP32端子 SPI & TTL-Serial
IO1(TxD0) ---- Rx
IO3(RxD0) ---- Tx
IO23(MOSI) ---- MOSI(DA/SDA) and with 1K Pull Up
IO19(MISO) ---- MISO(ADO) and with 1K Pull Up
IO18(SCLK) ---- SCLK(CL/SCL) and with 1K Pull Up
IO5(CS) ---- CS(SS) and with 1K Pull Up
IO17 ---- LED
自作 Turtlebot3 自律走行に向けたプログラム。#7
--- Turtlebot3 SBC(Jetson Nano 2G) で、rtabmap_ros で、色々試す。 ---
自作 Turtlebot3 自律走行に向けたプログラム。#5 の続きです。
14. たまに、IMU データの音信不通がありますが、それでも大夫安定して動く様になったので、 rtabmap_ros の使い込みをしています。
その中で、判った事を書いています。
注) IMU データの音信不通 の原因がわかりました。/odom_fox の出力を、camera_info に同期して出す
様に組み込んだ処理にバグがあるようです。
いまは、この機能を外して使用します。by nishi 2022.5.18
1)
RGBD/OptimizeMaxError=3.0(default) - 3.3
この値によって、tf-odom の補正の量が左右されるみたいです。
デフォルトの 3.0 より大きめの 3.3 にしています。動かしていて、ワーニングが出たので少しだけ大きくしました。
試しに、2.1 など、小さくして試しましたが、そうすると、tf-odom の補正がされなくなりす。
Rviz で見ていると、tf-odom が補正されると、黄色の線が表示されるようです。
tf-odom が補正されないと、2M 行って帰ってきた時、出発点から、随分ずれて止まります。