ROS2 自作 Turtlebot3 による 草刈りロボット開発。#12 床が高低差がある場所での走行。
環境:
実機ロボット:foxbot_core3 for ros2
OrangePi 5 and Armbian 24.04
OAK-D Lite depth Camera
ROS2 Jazzy Navigation2
Launch: OAK-D Lite Depth Camera and Rtabmap_ros による、Active SLAM
rtabmap_oak-d_rgb_depth.launch.py
C++ 自動走行プログラム:multi_goals4_nav2.launch.py
BNO086 DMP でなんとか、走行できるようになったので、
Rtabmap_ros の 2D で、床が高低差がある場所でロボットを走行させて、2D Mapping が精度よく取れるかためしてみた。
おんちゃんの部屋は、畳で、床の高さが一定でない。以前は、それでも、そのままRtabmap_ros の 2D Mapping を行っていたが、
なんとなく、高低差が気になりだした。
ずっと以前も試したが、BNO086 DMP で安定して IMU が取れだしたので、再度、IMU の Pitch を使って、床の高さを使ってみようと思う。
2D Rtabmap_ros でも、ロボットの tf-basefoot_print の pose.pose.position.z に、高低値を入れてみると、一応加味して、動作するみたい。
ここで、出てくるのは、あくまでも 2D Mapがでてくる。
3D Map は必要ないので、これで十分じゃ。
ただし、どこまで正確に、pose.pose.position.z を出せるかがもんだいじゃが。
注) RTK-GNSS(GPS) を使う場合、RTK-GNSS(GPS) を使って、予め Map 作成すれば良いので、Rtabmap_ros 自体出番がないのだが。
pose.pose.position.z の算出は、IMU からでてきた q(quatnion) を回転行列に変換して、
これに、単位 ロボットの前後(x軸)の移動距離のみをかけ合わせて、その結果の z 成分が、単位移動高さになる。
これを単純に、累積させていけばよいのだが、多分、ここで、誤差がでてくる。
細かい誤差には目をつぶって、取り敢えず、使ってみる。
2D Rtabmap_ros でも、Rviz2 tf を見ていると、 ロボットの z軸の補正も、まわりの情報から補正してくれる。
ロボットが、z=0 の面から、上に行ったり、下に行ったりするのが、なにか感動する。
SLAM での、現在位置の算出がよくなる気がするのは、おんちゃんだけか?
2. 手順。
1) ロボットを、水平な場所で起動して、IMU DMP9 は、Z 軸のみZero キャリブレーションして、X, Y は、生の値を使うようにして、
$ ros2 topic echo /debug_right で確認する。この時の X, Y の符号を逆にした値を、ロボット自体の傾きの補正値とする。
これを、前向きと後ろ向きの2回測定して、それの平均を使う。
この測定値を使って、foxbot_core3_r2.ino の IMUの補正を行う。
下記が、IMUの補正値を設定した所。
foxbot_core3_r2.ino
foxbot3::euler_to_quat<double>((0.029593099677307663-0.017842030929861176)/2.0, (0.03781347133215712 + 0.02501328678268437)/2.0,0.0,q_Zero);
2) Rtabmap_ros の Active SLAM の走行時には、IMU DMP9 のquatnion を、上記ロボット自体の傾きの補正値で補正して、実際の地面、床の傾きとして使う。
3) 上記補正された、quatnion を回転行列に変換して、ロボットのMotor による、前進距離から、ロボットの地面、床面での移動距離、鉛直の移動距離を出す。
4) Rtabmap_ros で、Depth Camera を使うので、このDepth カメラの 取り付けPitchを、正確に、tf-base_link -> oak に渡す。
rtabmap_oak-d_rgb_depth.launch.py
$ ros2 launch rtabmap_ros_my rtabmap_oak-d_rgb_depth.launch.py PC2:=true
を使うと、Rtabmap_ros の 前方の映像が確認できるので、前方の床、地面が実際より、上がったり、下がったりしていないように調整する。
前方の床が、実際より上がっていると、ロボットがその方向に移動したとき、ロボットがRviz2 上で、地面に埋もれてしまう。
3. 実際の、自分の部屋で試す。
おんちゃんの部屋は、畳で少しは、高低差はあるとは思うが、ほとんど測定には出てこなかった。もちろん誤差として、ロボットの鉛直位置がずれることもなかった。
まあ、しかし、 Rtabmap_ros の Depth Camera による、Active SLAM は、スムーズにできた気がする。
以前は、1) ロボット自体の傾きを補正値 として使っていなかったので、ここが具合が悪かったと思う。
それが、今回の収穫じゃ。by nishi 2025.6.23
実機のテストをするにつれて、ロボットが前進するたびに、Z軸が高くなって行く気がする。
なので、ロボットのIMUの Y(Pitch) を、0.2[degree] 下に向ける。
foxbot_core3_r2.ino の IMU のY(pitch) 補正。
$ python3
>>> import math
>>> math.radians(0.2)
0.003490658503988659
foxbot3::euler_to_quat<double>((0.029593099677307663-0.017842030929861176)/2.0, (0.03781347133215712 + 0.02501328678268437)/2.0 + 0.003490658503988659 ,0.0,q_Zero);
結局、0.4[degree] 下向かせれば、よくなった。
これで、Rtabmap_ros の Active SLAM が、かなり良くなった。
スタート位置から、毎回、半径 7[cm] ほどのところまで、帰ってこれるようになった。これは、すごいことじゃ。
やはり、ロボットの標高も正確にしたほうが良い。
しかし、実機走行で、これほどシビアに調整しないと行けないのは、実用的ではないきがする。
実際、屋外で、Rtabmap_ros で、Map を作成する機会はないのかもしれない。
余談だが、もっと良い Depth Camera がほしいものじゃ。
OAK-D Lite の Depth Camera だと、本当に性能が悪い。latency(遅延) が最悪じゃ。タイムラグありあり。
4. 参照。
1) 施設園芸用移動ロボットのための意味情報付き 3 次元地図の生成
息抜きに、フィギュア でも買って、机の隅に並べてみては、どうぞね!!
新作フィギュア最大２５％オフ！ホビーストック
(広告)