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
4. ソフト環境
1) tosa-no-onchan/ReadIMU をダウンロード。
IMU.cpp から取り込んだ、向きと位置情報を、ROS メッセージとして、出力します。
Platform I/O IDE のプロジェクトへ組み込んでください。
2) Arduino Libraliy のダウンロード。
/tosa-no-onchan/lib-nishi3
今回の主役。IMU.cpp で、IMU からデータを取り込んで、現在の位置と向きを算出します。
$ cd ~/Arduino
$ git clone https://github.com/tosa-no-onchan/lib-nishi3
3)
ここで、Platform I/O IDE で、ReadIMU/src/main.cpp をビルドして、ESP32 へ書き込みます。
その前に、下記を修正。
注) 192.168.1.170 は、自分のPC の IP に変えて下さい。
4) madgwick.launch のダウンロード
$ cd catkin_ws/src/
$ git clone https://github.com/tosa-no-onchan/rtabmap_ros_my
$cd ..
$ catkin_make
5) madgwick.launch で、Turtlebot3 の turtlebot3_remote.launch を使っているので、Turtlebot3 を ~/catkin_ws/src へダウンロードして、ビルドします。
$ cd catkin_ws/src/
$ git clone Turtlebot3 all package ( git URL)
$ cd ..
$ catkin_make
6) ~/.bashrc を修正
もしかしたら、不要かも?
export TURTLEBOT3_MODEL=waffle
5. 実行
$ roscore
$ roslaunch rtabmap_ros_my/madgwick.launch
$ rviz
Global Options -> Fixed Frame: odom
Add TF
TF
Frames: odom check
Frames: base_footprint check
これで、テストできます。
6. 調整
IMU に合わせて、調整が必要です。
~/Arduino/lib-nishi3/IMU/IMU.cpp を調整します。
調整1.
起動直後は、関係ない表示がでてくるので、暫くは、Arduino IDE の Serial Monitor で表示データが安定するまで確認します。
その後、表示データが安定したら、Serial Plotter に切り替えます。
ここでは、IMU の加速度センサーの基本性能を評価します。ここが駄目なら、別の IMU を使っとうせ!!
調整2. and 調整3.
此処が、自動設定できれば、良いのだが。
調整4.
此処も、自動設定できれば、良いのだが。
調整5.
半波できれいに終わらずに、逆方向に波が少しでも出てしまうと、逆戻りする動作になります。
ここで、いかに、きれいに半波だけを出すようにプログラムを作れるかが、決めてぞね!!
また、使っている 6軸フュージョンのフィルターの性能の限界が現れてきます。
おんちゃんは、MadgwickAHRS.cpp の限界を感じちょります。
調整6.
IMUを動かした後、停止さた時、暴走する(速度が 0 にならない)ようであれば、調整します。
以上ぞね。
7. 最後に
まだまだ、動作は、完成には程遠いですが、ESP32+IMU を宙に浮かして動かして、 Rviz 上で、tf base_footprint が、
3D で動くのが確認できます。
調整が終わった、PC との TTL-Serial を外して、Wi-Fi 通信のみにすれば、自由に空中を動かせます。
Madgwickフィルターを使った1Gキャンセル処理で、いまは、揺り戻しが生じている。
これを改善しないと、まだまだ使える代物にならない。
逆に言えば、Madgwickフィルターの精度が改善すれば、使える物になりそうぞね。!!
誰か、Madgwickフィルターの精度が改善 方法が解ったら、教えて欲しいぞね。
以上。
今回、自分で１から作ったけれども、市販で、Qwiic - BNO080搭載 VR IMUモジュール があるので、これを使ったほうが早かったかも。
8. その後。
Madgwick には、幾つかバージョンが有るようだが!!
最新版が、xioTechnologies/Fusion にあったので、これを、ESP32 で試してみます!!
1) download and build
$ cd ~/Arduino/lib-nishi3
$ git clone https://github.com/xioTechnologies/Fusion.git
~/Arduino/lib-nishi3/Fusion/Examples/Simple/main.c
~/Arduino/lib-nishi3/Fusion/Examples/Advanced/main.c
を参考に IMU.cpp へ組み込んでみました。
注1) ~/Arduino/lib-nishi3/Fusion/Python/Python-C-API/imufusion.c は、imufusion.c-not-build にファイル名を変更しました。
注2) FusionAhrsSetSettings で、セッティングをするみたいぞね。
.gain = 0.004f で、試してみました。揺り戻しは改善されたような気がしますが、
IMU を静止しているのに、acc_x、acc_y が増えて行ってしまいます。此処を改善しないといかんぞね。by nishi 2022.5.12
.rejectionTimeout = 0 で、改善するみたいだが、そうすると、揺り戻しばかりになって、まともに動きません。
この指定は、acc と gyro = 0.0 のときデータアクセスエラーと判定するので、その時のエラーとするまでの待ち時間のようです。
.rejectionTimeout = 5 * SAMPLE_RATE にすると、揺り戻し がきれいに取れますが、上述のように、静止時の acc_x が徐々に増えて行きます。
これは、とんでもない事がわかりました。
FusionAhrs.c では、acc値、gyro値 = 0.0 だとデータアクセスエラーと判定するのであれば、MadgwickAHRS.cpp も同じ事が言えるのでは?
今まで、MadgwickAHRS.cpp の gryo の入力データに、 CUT OFF した、0.0 を使っていました。早速、これをなくして、
従来の MadgwickAHRS.cpp 版を試してみると、動きがかなり改善されました。今まで、IMU を斜めに動かしても Rviz でそれが確認出来ませんでしたが、
それができる様になりました。
注3) ICM-20948 で、Acc と Gyro の単位設定を間違っていたようです。これを直したら、MadgwickAHRS.cpp で、もう少し良く動くようになりました。
こっほ!! by nishi 2022.5.13
注4) IMU 停止時の暴走を防ぐロジックを少し修正しました。平面上では、結構良くなりました。が、空中では、まだまだか!!
注5) 1Gキャンセル用のクォータニオンを、一つ前:今回 = 3:7 の按分にしてみました。
空中でも少しだけ改善したかも。
tosa-no-onchan/lib-nishi3 の "update 2022.5.16" 版を、使うとうせ!! by nishi 2022.5.16
後は、FusionAhrs.c でも、うまく動く様になればOKだが? こちらは、難しそう。
IMUを回しても、quartanion がちゃんと入ってこないみたい。こちらは、もう断念します。
この後は、6軸フュージョンを、MadgwickAHRS に変えて Complementary filter にすると、どうなるのか試してみます。
こちらで試してみました。
1回の移動動作で、半波ではなく、両波が、いつも観測されます。従来の処理だと、揺り戻しになって、動きません。
でも余りに、波形がきれいなので、試しに、更に*時間で積分してみました。
これで、1回の移動動作で、きれいに半波になって、Rviz 上の base_footprint も逆戻りせずに動きます。
vB = ∫ 加速度 * dt
V = ∫ vB * dt
D = ∫ V * dt
なんと、言う事じゃ!! 但し、距離の単位はどうなるの?
また、z 軸は、x,y に比べて、大きく動きます。
Complementary filter は、acc の誤り制御のフィードバックみたいだから、うまくないのかも。
これはどう言事かと言えば、ステレオアンプの負帰還(FNB)の様なもの、出力側で計算した、帰還信号を、入力側に直ぐフィードバックしても、
入力には、帰還信号の元になった信号は通り過ぎていて、もう次の信号が来ていて、それに、前の信号に基づく負帰還をかけても、手遅れだと言うこと。
Complementary filter も、今回の処理に使えそうにありません。
但し、クォータニオンの姿勢情報には、問題ありません。
つまり、1Gの分配を決める情報にはならないと言うことです。
同じ事をトライしている記述がありました。
IMUと地磁気センサーを組み合わせて、ストラップダウン方式の慣性ナビゲーション・システムを構築
但し、説明だけで、実際のプログラムは公開されていないので、残念!!
この人も加速度センサーでの速度0 の算出方法に理論を述べていますが、最終的には、実験値から、トライアンドエラーで良い塩梅になるアルゴリズムを決めているだけ見たい。
但し、
vi = vi_1 + ai_1t + (ai - ai_1)/2
注) vi_1、ai_1 は、1つ前の値の意味
は、試す価値はあるかも?
しかし、 + (ai - ai_1)/2 は、意味あるのだろうか?
IMU による、移動距離の測定は、このくらいで、保留にします。
本来の、自作 Tutlebot3(foxbot_core3) での、Rtabmap_ros の 3Dの高さ処理に戻ります。
自作 Turtlebot3 自立走行に向けたプログラム。#7