自作 Turtlebot3 自律走行に向けたプログラム。#2

おんちゃん (2021年10月11日 18:09)

自作 Turtlebot3 自律走行に向けたプログラム。 #2
--- 自作 Turtlebot3 Rtabmap_ros で、Odometry と IMU のフュージョンを行う。 ---

前回、自作 Turtlebot3 自律走行に向けたプログラム で、Odometry と IMU のフュージョンが必要だと痛感したので、
今回は、それを試してみます。

Odometry と IMU のフュージョンに関しては、下記を参考にしています。
ROS講座61 位置情報の統合

1. 環境
1)リモートPC
Ubuntu Mate 18.04
ROS: Melodic
IP:192.168.1.170

2)TurtleBot3 SBC
Jetson Nano 2G
Wi-Fi Lan
IP:192.168.1.37
ROS:Melodic
USB Camera: Logi Cool C270 x 2
IMU:ICM-20948 / Switch Science SparkFun のこちら

2. launch ファイル
ベースの rtabmap_ros のサンプルは、rtabmap_ros/Tutorials/StereoOutdoorMapping です。
まだ、テスト中ですが、下記になります。
catkin_ws/src/rtabmap_ros_my/launch/rtabmap-nishi_stereo_outdoor2_bx_navi.launch
update 2021.10.16 by nishi


注1) foxbot_core3.ino の odom を odom_fox に変更しました。
foxbot_core_connfig.h

注1) robot_localization/ekf_localization_node
に渡すパラメータは、全て下記、mode1.yaml で指定します。

3. mode1.yaml ファイル
catkin_ws/src/rtabmap_ros_my/launch/config/ekf_localization/mode1.yaml

注) 当初、publish_tf: false で試していましたが、今は、publish_tf: true
でOKとなりました。
但し、ICM20948 のセンサーデータは、6軸に制限しています。
9軸にすると、未だ不具合があります。

4. 実行手順
取り敢えず、PC上で試してみます。
1) リモートPC
$ roscore

2) SBC(Jetson Nano 2G)
$ sudo chmod 666 /dev/ttyTHS1
$ roslaunch turtlebot3_bringup turtlebot3_robot.launch

$ roslaunch uvc_camera stereo_node.launch fps:=16

3) リモートPC
$ roslaunch rtabmap_ros_my rtabmap-nishi_stereo_outdoor2_bx_navi.launch SBC:=true fusion:=true

$ roslaunch rtabmap_ros_my rtabmap-nishi_stereo_outdoor2_bx_navi.launch fusion:=true

4) リモートPC
あとは、自動走行の Python プログラを起動すれば、OK ですが。
$ rosrun ros_start multi_goals1.py

今回は、ここからが、苦労の始まりでした。

5. 不具合対策。
1) まず、出てきたのが、下記ワーニング


色々試して、やっとわかったのが、
foxbot_core3(turtlebot3_core) からの、 /odom のパブリッシュ間隔が間にあっていない。という事らしい。
$ rostopicc hz /odom で出てくる時間が遅いのが原因のようです。
下記は、改善した場合の値です。

考え方は、通信の遅れをや、ボトルネックを見つけて、改善する事。です。

ESP32 -- (Serial 115200) -- JetSon Nano 2G -- (WiFi) -- Wi-Fi Router -- (LAN Cable 10M or 100M ?) -- PC

上記、全てにボトルネックとなる箇所が、潜んでいます。
今回、おんちゃんは、手始めに、 ESP32 と Serial の改善から始めました。

1) ESP32(foxbot_core3.ino) の /odom の Publish 間隔を短くします。
いまは、 turtlebot3_core.ino にならって、 33 [ms]
foxbot_core_config.h

これでも改善しませんでした。

2) Serial の barud rate を、 115200 -> 230400 にする。

foxbot_core3.ino


catkin_ws/src/turtlebot3_bringup/launch/turtlebot3_core.launch


上記で、Could not get transform from odom to base_footprint after は、解消されました。

3) ただし、今度は、 Stereo Camera のフレームがロックアウトされて、かなり途切れてぃます。
こんどは、これを解消しないと、いけないぞね。

Wi-Fi Router -- (LAN Cable 10M or 100M ?) -- PC
LAN Cable = 1G に変更したら、改善しました。

改善後のセッティングです。
ESP32 -- (Serial 230400) -- JetSon Nano 2G -- (WiFi) -- Wi-Fi Router -- (LAN Cable 1G) -- PC

6. これで、やっと、
$ rosrun ros_start multi_goals1.py
が、実行できるようになりました。但し、rtabmap の [WARN] が幾つか出ています。
RGBD/OptimizeMaxError=3.3
にしてみます。

7. あとは、
robot_localization/ekf_localization_node
publish_tf=true
で実行できるようにしないといけないぞね。
publish_tf=false だと、ロボットの回転に誤差が出る。fusion を使ってない時と大差無い。

foxbot_core3.ino(turtlebot3_core.ino) の imu の値について、判った事。
orientation(クオータリオン) の値は、9軸センサーの場合、常に、絶対的なロボットの向きを示す。みたいです。
電源を入れた後、IMUをリセットさせた後も変わりなく、方位磁石をもとにした、ロボットの向きを出力する。
注) つまり、屋外、屋内にかかわらず、ロボットの向きに関しては、一定した情報を得られる。

なので、ekf_localization_node には、ロボットの起動時の向きを与えてやらないといけない。
そうしないと、publish_tf=true にしたとき、起動した時点でbase_footprint と base_link が一致しない。
または、turtlebot3_sensor.cpp で、下駄をはかす。
Turtlebot3Sensor::getIMU(void)
で、ロボットの起動時の向きに一致するように、値を合わせる。クオータリオン は、これが、
できるのだろうか? 出来ないのかも?


あと、publish_tf=true にすると、
下記ワーニングが、出ます。これを、解決しなといけないぞね。!!

8. ekf_localization_node のみ動かす。
ekf_localization_node のみで動かして、問題解決を図っちょります。なにが、いけないのか?

fusion_nishi.launch


結論。
[ WARN] ... Transform from base_footprint to base_link was unavailable for the time requested.
表示されても問題ない。表示されるのが正常です。
表示されないと、うまく動きません。
publish_tf: true の場合、TF の tree が、
それまでは、 odom -> base_footprint -> base_link
でしたが、
odom -> base_footprint
oodom -> base_link
となって、base_footprint とbase_link が親子ではなくなって、並列になる。
どちらも　odom の子になるので、 base_footprint とbase_link の親子リンクが不要になる。

注) この件に関して、自作 Turtlebot3 自律走行に向けたプログラム。#13 で、解決方法を改めて記述しています。そちらをご覧ください。by nishi 2022.9.4

9. 話は外れますが、ESP32 が古くて、具合が悪かったようで、IMUデータが正しく取れていなかった様です。
ESP32 を新調して、ICM20948 を使って試してみました。

ICM20948 の MadgwickAHRS へ渡すデータを、9軸から6軸に制限して試すと、現状、OK となりました。
9軸の場合だと、ロボットを回転させると、/stereo_camera/fusion/odom の回転量が現実の半分程の値になります。
この点は、これからの解決になります。が、

注1) この点は、後でわかりました。
どうやら、IMU の Magnet Sensor が駆動用のモーター磁石の影響を受けていたようです。
IMU を上板に移動させれば、OK となりました。但し、9軸情報を、MadgwickAHRS.cpp の update() へ渡すと、
回転時の値が多く算出されます。何故か!! まだ確認しないといけないです。
SparkFun_ICM-20948_ArduinoLibrary の dmp を使っても同じ様に、実際の回転より、計算値が大きくなります。
現在位置の緯度、海抜等でなにか補正がひつようなのか? どうやらキャリブレーションの話みたい。 by nishi 2021.11.8

注2) ICM-20948 で、MadgwickAHRS.cpp 9軸フュージョンは、精度自体が悪そうです。
地磁気センサー自体がそれ程精度が良いわけではなさそうです。
ROSで、センサーの向き(クオータリオン) を、TF 出力して、Rviz 上で確認していますが、
センサーを、90度ずつ回して、Rviz で確認していますが、結構ずれます。
これでは、正確な位置は望めません。
地磁気の真北と地球の真北がずれている事自体が影響しているのか。やはり補正の問題か。
6軸フュージョン だと、きっちり 90度ずつ回ります。

ICM-20948 9軸フュージョン で、比較的優秀なのが、
DPEng_ICM20948_AK09916 | ahrs_fusion_usb/ahrs_fusion_usb.ino ですが、
注) ソースコードにバグがあるので、少し手直しがひつよです。
これとて、90度ずつ回転させると、ある方向だけ、ずれます。
やはり、緯度、経度で補正が必要なのではと思います。
行き着くところは、GPS になるのか?

話を、6軸 に戻して、
現状では、2 [M] 行って帰って来た時、ロボットの向きが、少しずれて止まります。
base_footprint が真正面になっていて、base_link が少し真正面よりずれています。
ロボットの向きは、base_link と一致していて、move_base が、base_link を基準に停止してくれれば、完璧ですが、
move_base が、base_footprint を基準に停止しているような気がします。

turtlebot3_navigation/launch/move_base.launch へのパラメータが間違っているのか?

カテゴリ:

検索

このブログ記事について

このページは、おんちゃんが2021年10月11日 18:09に書いたブログ記事です。

ひとつ前のブログ記事は「自作 Turtlebot3 自律走行に向けたプログラム。」です。

次のブログ記事は「自作 Turtlebot3 自律走行に向けたプログラム。#3」です。

最近のコンテンツはインデックスページで見られます。過去に書かれたものはアーカイブのページで見られます。

カテゴリ

月別 アーカイブ

ウェブページ

サイトナビ