ROS2 自作 Turtlebot3 による 草刈りロボット開発。#12 床が高低差がある場所での走行。
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) Rtabmap_ros の Active SLAM の走行時には、IMU DMP9 のquatnion を、上記ロボット自体の傾きの補正値で補正して、実際の地面、床の傾きとして使う。
3) 上記補正された、quatnion を回転行列に変換して、ロボットのMotor による、前進距離から、ロボットの地面、床面での移動距離、鉛直の移動距離を出す。
3. 実際の、自分の部屋で試す。
おんちゃんの部屋は、畳で少しは、高低差はあるとは思うが、ほとんど測定には出てこなかった。もちろん誤差として、ロボットの鉛直位置がずれることもなかった。
まあ、しかし、 Rtabmap_ros の Depth Camera による、Active SLAM は、スムーズにできた気がする。
以前は、1) ロボット自体の傾きを補正値 として使っていなかったので、ここが具合が悪かったと思う。
それが、今回の収穫じゃ。by nishi 2025.6.23