Turtlebot3 の自作 #5

おんちゃんは、SparkFun 9DoF IMU (ICM-20948) Breakout Hookup Guide をみて、
今回は、Arduinoの Library があるので、それをそのまま使うことにしました。
上記ライブラリーを、git clone で ~/Arduino/lib-nishi3/ 下へダウンロードして、
examples/Arduino/Example1_Basics/Example1_Basics.ino を、Platformio IDE で、適当なプロジェクトを作って、ESP32 で試してみました。
簡単に、確認できました。

1. Foxbot_core3 への組み込み。
早速、Foxbot_core3 へ組み込んでみます。
IMU機能の組み込みには、Turtlboot3(Arduino OpenCRのサンプル) の中の、turtlebot3_sensor.cpp、turtlebot3_sensor.h と
同じく IMU (ライブラリ) を使います。
turtlebot3_sensor.cpp、turtlebot3_sensor.h は、 Platformio IDE の foxbot_core3 プロジェクトの src フォルダーへコピーします。
IMU(フォルダー毎) は、~/Arduino/lib-nishi3/ 下へコピーします。

1.1 turtlebot3_sensor.cpp、turtlebot3_sensor.h は、IMU 部分以外は、コメントですべて除外します。

1.2 IMU ライブラリーは、
新たに、ICM20948.cpp、ICM20948.h を、ICM20648.cpp 、ICM20648.h をコピーして、作成します。
ただし、ICM20948.cpp、ICM20948.h は、先に ダウンロードした、SparkFun_ICM-20948_ArduinoLibrary を使って、ICM-20948 から、SPI でセンサーデータを受信するように改造します。
注) その後、SparkFun の Library を解析して、 ICM20948.cpp、ICM20948.h に Magnetometer(AK09916) のデータ受信処理を組み込みました。
これで、Sparkfun のICM20948 用ライブラリーが不要になりました。

2. 動作確認。
なんとか、上記プログラムの改造ができたので、早速、自作機で試してみます。
自作機で試すには、Turtlebot3 の自作 #4 の "5. Turtlebot3 として、SLAMを試す。" の操作をすれば出来ます。

結論としては、どうなのか? よく判りません。
Rviz 画面で、 IMU を有効にすれば、天井に向かうおきな矢印が出てきました。
実は、中華製の MPU-6500 6軸 (MPU-9250の偽物)でも試して、同じような症状になったので、バグったのかと思いましたが、
IMU の加速度センサーには、重力 1G が検出されるとネットに記載があったので、それが、Rviz 画面上ででているんだなーと、納得しました。

キー操作で、自作機 を動かしてみると、それにつれて、天井に向かう IMU の大きな 矢印がゆらゆらと、動くのがわかります。
精度を詰めるのは、今後の課題かな!!

なにぶん、MPU-9250 で試せないので、正解値が判りません。
Gazebo 上で、Turtlebot3 を動かして、その時の /IMU の Topicと比べるしかないのか!!

Gazebo 上で、確認してみましたが、どうやら、天井に向かう大きな矢印は、出ていなくて、
前進させると、その一瞬だけ、小さな矢印がでてきます。やはり、加速値だけが表示されるみたいです。
これは、見直しが必要ぞね!!!

どうやら、これは、加速度には、動加速度、静加速度とがあって、
今回は、静加速度を使っていて、これは、重力(1G)加速度を計測しているとの事。
この静的加速度の事を、加速度のDC成分。動的加速度の事を、加速度のAC成分と言うらしい。
この静的加速度から、重力抜き加速度を求めるのは、加速度の low-pass フィルターを使うとの事らしい。
ちょどサンプルが、SparkFun のライブライリーに添付の Examples の中に、Example2_Advanced.ino があるので、
これを参考に試して見ますが、-3DB 低下のフィルターで、どれも完全には取り除けないきがします。

どうやら、この情報は誤りみたいです。
加速度のDC成分の除去は、low pass filter では、出来ないみたいです。
low pass filter は、ちいさな揺れによるゴミを取り除くのが目的みたいです。
加速度のDC成分の除去は、どうすればできるのか? または、出来ないのか?

さて、次は、ブレッドボードを、プリント基盤に載せ替えて、Jetson Nano 2GB とSerial 通信をできるようにします。
そのあと、Jetson Nano 2GB に ROS をインストールして、Jetson Nano に USB Wi-Fi を セットして、
いま、 ESP32-WiFi 経由で行っていることを、Jetson Nano Wi-Fi 経由にします。

最後に、 Intel Depth カメラを Jetson Nano に繋げて、
Rtabmap_ros で、3D Slam を行うのが目標ぞね!!
この時点で、今回組み込んだ、IMU の精度の問題が出てくるのか!!

ここまで来て、やっと草刈りロボットの開発がスタートする事になるぞね!!

後書き。
ESP32 と ICM-20948 は、SPI 接続なので、ICM-20948の外部 I2C デバイスの AK09916 とどうやって通信するのか判らなかったので、Turtlebot3 の IMU ライブラリーのMPU9250.cpp 、 .h を解析してみました。

ESP32 と ICM-20948 が I2C 接続であれば、Pass Throught Mode (INT_PIN_CFG:0Fh - BYPASS_EN(02x = 1)) でできるので疑問はなかったのですが、
SPI の時は、もう一つのモード、 I2C Master Mode (INT_PIN_CFG:0Fh - BYPASS_EN(02x = 0)) で、I2C_SLVx_ADDR 、I2C_SLVx_REG、I2C_SLVx_CTRL、I2C_SLVx_DO を操作して、
(これは、 ESP32 から SPI通信で、ICM-20948 の該当レジスターを書き換えて行います。)
ICM-20948 を I2C Master として、AK09916 と I2C 通信を行わせるとの事です。
後、その I2C でのデータ受信は、Single Action と、 Continuous 通信があるとの事です。
Magetometer のデータの受信は、後者のContinuous 通信 (Continuous measurement modex) で行えます。
これで、やっと疑問が晴れました。

ここまで判れば、後は、そのコードを書き加えるだけですが、
最初は、MagID (WIA2:01H) = 0x09 の取得です。
此処がまたよくわからん。
結局、Sparkafun のライブラリーの該当箇所を見つけて、同じ処理を行ってやっと、AK09916 と通信ができるようになりました。
流れとしては、
i) i2cMasterPassthrough(false) にする。
INT_PIN_CFG - BYPASS_EN = 0

ii) i2cMasterEnable(true) にする。
a. I2C_MST_CTRL = 0x17
- I2C_MST_P_NSR = ON
- I2C_MST_CLK = 7
b. USER_CTRL - I2C_MST_EN = 1

iii) これで、やっと resetMag() を行えます。
上記、I2C_SLV4_ADDR 等を使って、ICM-20948 に、AK09916 へ、I2C コマンドの書き込み (以後、AK09916write()) を行い。
AK09916_CNTL3 へ、0x01 を送信する。
delay(1);
iv) ここでやっと、MagID の受信。
AK09916read() で AK09916_WIA2(:01H) を受信。
ちなみに、AK09916 の I2C アドレスは、 0x0C です。

この後、Magetometer のデータのContinuous 通信の設定ですが、
またうまく行かないので、 MPU9250.cpp の方のコードに立ち返ってやっと出来ました。
流れとしては、
i) AK09916write() で、 AK09916_CNTL2 = AK09916_POWER_DOWN(0x00) 送って、
AK09916 を Power down
delay(1);
これからは、I2C_SLV0_ADDR 等 を使う準備です。
ii) I2C_SLV0_ADDR = 0x8C
0x0C は、 AK09916 の I2C アドレス
0x80 は、 I2C の Read設定。
delay(1);
iii) I2C_SLV0_REG = AK09916_ST1(0x10)
AK09916 の ST1 (0x10) - Status のレジスターアドレスをセット。
delay(1);
iv) I2C_SLV0_CTRL = 0x89
delay(1);
v) AK09916write() で、
AK09916_CNTL2(31H) レジスターへ、AK09916_CONTINUOUS_MEASUREMENT4(0x08) を送る。
delay(1);
vi) I2C_SLV4_CTRL = 0x09
delay(1);
vii) I2C_MST_DELAY_CTRL = 0x81
delay(100);

どうやら、AK09916 のデータは、8 バイト+1 = 9 バイト(なぜ!) 取り込む?
Magetometer のデータは、 6 バイトなのに? まだ、バグか?
しかし、Sparkafun のライブラリー をつかった場合と同じ値が取れているので、これでOKか?
こっほん!!!

3. 後書き。
Stereo、Depth Camera を、Intel Realsense D435i を使うのであれば、ここでの IMU は、
必要ない気がします。 by おんちゃん 2021.9.10