自作 Turtlebot3 自律走行に向けたプログラム。#7
--- Turtlebot3 SBC(Jetson Nano 2G) で、rtabmap_ros で、色々試す。 ---
自作 Turtlebot3 自律走行に向けたプログラム。#5 の続きです。
14. たまに、IMU データの音信不通がありますが、それでも大夫安定して動く様になったので、 rtabmap_ros の使い込みをしています。
その中で、判った事を書いています。
注) IMU データの音信不通 の原因がわかりました。/odom_fox の出力を、camera_info に同期して出す
様に組み込んだ処理にバグがあるようです。
いまは、この機能を外して使用します。by nishi 2022.5.18
1)
RGBD/OptimizeMaxError=3.0(default) - 3.3
この値によって、tf-odom の補正の量が左右されるみたいです。
デフォルトの 3.0 より大きめの 3.3 にしています。動かしていて、ワーニングが出たので少しだけ大きくしました。
試しに、2.1 など、小さくして試しましたが、そうすると、tf-odom の補正がされなくなりす。
Rviz で見ていると、tf-odom が補正されると、黄色の線が表示されるようです。
tf-odom が補正されないと、2M 行って帰ってきた時、出発点から、随分ずれて止まります。
2)
Stereo/MinDisparity=5
Stereo/MaxDisparity=2000
Stereo/MinDisparity を大きくすると、遠くの物のマッチングがされなくなります。
そうすると、ナビゲーションで、ロボットを直進させようとすると、
自分の現在位置がつかめなくなるようで、右往左往して、そこで止まってしまいます。
move_base の costmap_common_params_waffle.yaml の
obstacle_range: 1.0
raytrace_range: 3.5
のどちらかより近い所が判別出来なくて支障をきたすのでは?
Stereo/MaxDisparity を小さくすると、近くの物のマッチングがされなくなります。
Stereo Cameraの最小焦点距離 以上に指定しても意味がないのですが、気休めに
2000 にしています。
3)
Grid/MaxGroundAngle=35
ステレオカメラ(PointCloud2)の上下の画角の上の画角みたいです。
デフォルト=45 ですが、デフォルトだと、 Rviz の 3D-map で壁が余りに高くなるので制限しています。
が、あまり小さくすると、対象物に接近した時に、画面視野が狭くなるので、マッチング性能が落ちて、自分の位置が判定できなくなるようです。
Stereo Cameraの最小焦点距離や近接物のキャリブレーションの性能でも影響がするのでしょうが?
15. rtabmap_ros で、3D Map にしようとすると、tf-basefoot_print の transform.translation.z に、ロボットの位置の高さが必要みたい。
Vis/EstimationType=0 # 0=3D->3D, 1=3D->2D (PnP)
IMU の 加速度データから、計算出来ないか試してみました。
手始めに、ブレッドボードで、IMU を前後に動かして、その時の、x軸の加速度から、
x軸の速度を出して、
x軸の移動距離が算出できるか試してみました。
これがとんでもなく、出来ません。
IMU 加速度データ自体、常に、1G の影響を受けているので、
IMUが鉛直で無ければ、常に、加速度データの x,y,z に1G が分配されて入ってきます。
これだと、ジャイロのデータを使って、IMUの傾きから 1Gの影響を取り去った、加速度データの x,y,z を計算しないと、
ロボットの加速どから、速度を出して、移動距離を計算するのは出来ないみたいです。
あと、IMU の Sampling は、最低 200[Hz] 、あるいはもっと大きくないと、出来ないみたいです。
さらから考えるのは、大変なので、ドローンでも、IMU を使って同じ処理をしているのではと、思って、そちらを少し参考にしてみます。
「インターフェース 2020.3 飛行・走行・航行 ドローン&ロボ制御」にドローンのプログラムの解説が出ているので、
そちらを参考にしてみます。
STSW-FCU001
ソースは、こちらです。
STMicroelectronics-CentralLabs /
ST_Drone_FCU_F401
ジャイロ、加速度の Sampling rate = 800[Hz] の様です。
後、「機体の速度と位置の算出」が、 同上誌 P77 にありました。
PN = [PxN PyN PzN]t = ∫CBvBdt ----- (17) 基準座系での位置
vB = ∫ 加速度 * dt ----- (16) 機体座標系での速度
注) 速度は、前の速度に、今回の速度(加速度*dt) を加算していかないと算出できません。
加速度=[acc_x acc_y acc_z]t ------ IMU の加速度センサー値
dt: IMU のアクセス間隔。1/800[Hz]=0.00125[sec]
CB は、同誌 P76 重力の計算 の項にあります。
CB =
|q0q0+q1q1-q2q2-q3q3 2(q1q2 - q3q0) 2(q3q1+q2q0|
|2(q1q2 + q3q0) q0q0-q1q1+q2q2-q3q3 2(q2q3-q1q0)|
|2(q3q1-q2q0) 2(q2q3+q1q0) q0q0-q1q1-q2q2+q3q3| ------ (6)
CB は、クォータニオンから、回転行列への変換式でした。
注) CBの捉え方が間違っていました。by nishi 2022.4.16
試しに、ブレッドボード上で、
IMU の加速度、ジャイロデータをSampling Rate=400[Hz] で取り込んで、 MadgwickAHRS.cpp で、 6軸フュージョンで試してみます。
但し、vB は演算上ずっと加算して行くので、どこかのタイミングでリセットしないと、誤差の蓄積のオンパレードとなります。
やはり、Sampling Rate=800[Hz] で試す。
結論。
現状のおんちゃんのスキルでは、無理のよう。
Sampling Rate=800[Hz] にしても、ICM-20948 の 加速度センサーの誤差:20[%] で、IMU を前に 5[cm]程動かして、止めたとしても、
その後の、x軸の速度が、0 に戻らない。
加速度センサー値の内積を計算して、起動時の内積と比較しても、停止しているのか、
定速運動しているのか区別が出来ないので、
定期的に、速度のごみをクリアーできません。
ここで、考え直して、ロボットの前進、後退の移動距離の値は、タイヤの回転から求める数値がより正確ではないのか?
だとしたら、IMU 6軸フュージョンから得たクゥオータニオンからオイラー角を求めて、Y軸(Pitch)と、ロボットのモータ回転からの、
前後移動距離から上下の移動距離を計算すれば、起動時からのロボットの高低差が求まるのでは?
又は、上の vB=[dx 0 0]t
dx:タイヤ回転から求めた、ロボットの前後の移動距離
を使って、PN = [PxN PyN PzN]t = ∫CBvBdt を計算して、
PzN のみを、使って、ロボットの高低値とすればOKか?
結局、オイラー角の Pitch角とロボットの前後移動距離の積分で、簡単に出来ました。
変換は、下記関数で出来ます。
問題は、精度だけです。
Rviz で見ると、ロボットの高さ位置が変わるのが確認できます。
おんちゃんの畳の部屋では、へこんだ場所があるので、3D の Mapping には最適です。
いずれにしても、誤差がつきまとうので、最終的には、拡張カルマンフィルターで補正しないと使い物にならないみたいです。
おんちゃんは、下記ページを見て一から勉強です。
Python で学ぶ ベイズフィルタとカルマンフィルタ
この解説書は、中で使われている記号がちゃん説明されていて本当に良いです。
他の解説書(書籍)では、知らない記号が突然出てきて、それから先に進めない事が多々ありました。
おんちゃんは、文系の出なので、大学の理系で当然習う事を前提に書かれても、そこで止まってしまいます。
16. 再挑戦。
『インターフェース 2021.9 数学とプログラミング』P58 以降にも
オイラー角、クォータニオンの記述があります。
オイラー角で言えば、
ロボットの現在の姿勢 Roll-Pitch-Yow を基準に、IMUのジャイロから、向きの変化をオイラー角で計算して求める。
Roll-Pitch-Yow オイラー角を回転行列に変換して、IMU のジャイロの、z,y,x 値をその中に割り当てれば、
回転後の、Roll-Pitch-Yow 値がでます。
その3方向に、IMU の加速度・時間・時間から、移動距離を掛け算すれば、ロボットの移動方向、距離が出ます。
後は、ロボットの座標系をMapの基準座標系に変換すれば良い。
但し、オイラー角では、計算がマイコンでは重いので、クォータニオンで計算したほうが良い。
上記、P64 式(16) に、クォータニオンから、回転行列への変換式(前述の CB)があるので、
CB に、右側から、[acc_x acc_y acc_z]T を掛ければ、ロボット座標から、基準座標への変換後の 加速度 X-Y-Z のベクトルが、そのまま、出てくるとの事です。
X = (q0q0+q1q1-q2q2-q3q3)*ac_x + 2(q1q2+q0q3)*ac_y + 2(q1q3-q0q2)*ac_z
Y = 2(q1q2-q0q3)*ac_x + (q0q0-q1q1+q2q2-q3q3)*ac_y + 2(q2q3-q0q1)*ac_z
Z = 2(q1q3+q0q2)*ac_x + 2(q2q3+q0q1)*ac_y + (q0q0-q1q1-q2q2+q3q3)*ac_z
注) 一時、誤りがありました。 by nishi 2022.4.22
後は、これにそれぞれ、*時間・時間 の積分を取れば、移動後の座標が出てくると、思う?
前述のオイラー角への変換+モータ駆動 方式は、止めて、今回は、IMUだけで算出する。
ICM20948 DMP6軸フージョンと、Acc があれば、計算できそうです。
此処で、今まで経験した事を列挙すると。
ブレッドボードに付けたIMU を、机の上で、ちょっと動かして止める動作を測定すると、
sampling 700[Hz] 以上ないと、測定出来ない。今回は、800[Hz] にします。
ICM-20948 の加速度センサーの最大分解能 = ±4 [G] ( 8192.0 [LSB/g] )にします。
測定値、acc_x, acc_y, acc_z に、下記定数を掛けると、加速度[m/s^2] が算出できます。
(float)acc_x,y,z / 8192.0 * 9.80665 [m/s^2]
また、IMUの加速度が曲者で、移動初めは、+値で、等速運動になると=0、減速になると -値、停止すると=0 となる。
わかっていても、これを頭に入れとかないと、計算が変になる。
加速後、等速運動=0 になっても、IMUは、同じ速度で、移動している。なので、速度(加速度*dt) は、前の速度に、常に加算していかないといけない。
そうすると、減速後の停止時の=0 が、0 で無くて誤差が検出されると、永遠に、IMUは動いている事になる。
此処が、本当に厄介だ。
下の式で、vB が 0 とならない。
vB = ∫ 加速度 * dt
ここで、ICM20948 のスペック表を元にした、誤差のフィルターリングが必要か?
acc の誤差=0.5[%] なので、acc=0 ±5[%]は、0 とする。
又は、スピードの一定値は、Cut Off して、停止とみなす。など
i) IMU 加速度センサーの 1Gキャンセル方法。
IMU 加速度センサーには、常に1G が、IMU の傾きに応じて、xyz軸に分配されて、計測されています。
その、1G のキャンセル方法ですが、
1G の値を、上記 CB の Z軸(行)の、X、Y、Z列にそれぞれ掛ければ、分配値がでました。
1G は、 8192( 4G 最大分解能 ) を、使いました。
Gx=2(q1q3+q0q2)*8192 、Gy=2(q2q3+q0q1)*8192 、Gz=(q0q0-q1q1-q2q2+q3q3)*8192
これを、それぞれの、acc_x, y, z から引けばキャンセルができました。
但し、完全には、0 にはならずに、誤差がでるので、或る一定値以内は、0 とみなします。( ±28 以内は、 0 にしました。)
ii) 懸案の、速度[vB] = 0 の判定方法について。
仮定として、宇宙空間でなければ、一定速度で移動している間に、加速度値 = 0 が検出されることはまず無い。と仮定します。
であれば、加速度値=0 で、速度が同じであれば、停止していると見なす。
この処理の組み込みで、なんとか、速度0 を判定できました。by nishi 2022.4.21
これを、さらに拡大解釈して、加速度 3軸とも、0 であれば、速度=0 とする。
iii) 取り敢えず、前後、左右に移動させるテストをします。
この時、IMU を傾けても、速度=0 のままである事を確認します。
注) ii) 速度[vB] = 0 の判定の組み込みで、なんとか、ここは、OKとなりました。しかし、未だ、不要な動きをします。 by nishi 2022.4.22
但し、IMUには、誤差がつきまとうので、最終的には、フィルター(拡張カルマンフィルター等) を通さないといけないぞね。
一通り、確認ができたら、github に公開するぞね!!
ROS IMU Test
SOC: ESP32
IMU: ICM20948
Platform I/O IDE
Arduino
1. tosa-no-onchan/ReadIMU
ESP32 へ書き込みます。
2. tosa-no-onchan/lib-nishi3
IMU ライブラリー
ReadIMU ビルド時に必要です。
2. tosa-no-onchan/rtabmap_ros_my
ROS launch ファイル
実行方法
$ roscore
$ roslaunch rtabmap_ros_my madgwick.launch
$ rviz
iv. どうも誤差が大きいので、IMU へのアクセス Rate を実測してみました。
acc access rate = 114[hz] (実測 2022.4.27)
800[hz] には、とうてい及ばないです。これが原因のようです!!
DMP 6軸フージョン & Acc の同時アクセスなので、これが原因か?
Acc & Gyro であれば、800[Hz] は出るのだろうか?
v. DMP 6軸フージョン を止めて、Acc & Gyro & MadgwickAHRS にして、かつ、 SparkFun_ICM-20948_ArduinoLibrary を止めて、
自分で組み込んだ ICM-20948 アクセスルーチンにしてみました。
こちらで、700 Hz (実測 686.5 Hz) で試してみました。
Acc-x,y,z の 1G キャンセル後の値を、 Arduino IDE の Serial Plotter で可視化して、ブレッドボードを動かしてみますが、どうも、揺り戻しの方が大きな振幅をしめします。
Rviz の TF base_foorprint もその信号のように、揺り戻しの方に多く動くのが確認できます。
これって、IMU の方の問題なのか?
注) この構成で、100 Hz(実測 99.5 Hz) でも、おなじ様に動きます。これでも良いのかも。
結構強めに、ブレッドボードを動かすと、なんとか、それにつれて動きます。
ICM-20948 の、感度が悪いのか? 試しに、 Acc の Low Pass filter を OFF にして試してみましたが、同じです。
注) もしかしたら、IMU をブレッドボードにしっかり固定していないのが、原因かもしれません。
いまは、セロテープで、ブレッドボードに貼り付けているだけなので、その所為かも?
最新版を、github にアップロードしました。 by nishi 2022.5.2
vi. Sparkfun の LSM9DS1 を今試していますが、Acc and Gyro Z軸のノイズが、めちゃくちゃ入ります。試しに、
指で LSM9DS1 をブレッドボードに押し当てると、嘘のように無くなります。 セロハンテープで貼り付けるだけでは、問題の様です。
LSM9DS1 にスペーサを付けて、それを両面テープで、ブレッドボードに固定しました。それだけで、Acc and Gyro Z軸のノイズ が取れました。
あまり細かい調整はまだですが、早速、LSM9DS1 で試してみました。
Rviz で、tf を表示して、odom と base_footprint だけ有効にして、ブレッドボードを動かして見ましたが、
こちらのほうが、ICM-20948 よりも、感度が良くて、リニアに反応するみたいです。
それにしても、LSM9DS1 は、Acc and Gyro Z軸のセンサーがめちゃくちゃ感度が高い気がします。
ブレッドボードを持ち上げると、tf base_footprint が、よからぬところ飛んで行きます。
IMUを傾けた時の、1G のキャンセルがうまく行っていない様です。
acc_x 軸の値が異様に長く検出されるみたいです。これは、致命的です。
どうやら、gyro_y の極性が逆のようです。ちなみに、acc_y も極性が逆です。
これで、LSM9DS1 でやっと動くようになりました。by nishi 2022.5.8
ICM-20948 よりも、感度が良くて、リニアに反応するみたいですが、やはり、acc_x、acc_y の揺り戻しの方が、
より多く検出されます。まだまだ、実用には、耐えられません。
注1) LSM9DS1 は、Acc、Gyro とも、突発的に大きなノイズが入ります。また、全体的にノイズが多い(Low Pass フィルターの所為かも)気がします。
ICM-20948 の方がより安定しているようです。
Arduino IDE のシリアルプロターだと、此処らへんが視覚的に確認できるので、IMU の性能の良し悪しが判定できます。
ICM-20948 では、スペーサを付けて、しっかり固定して、試してみましたが、こちらは、ほとんど変わりません。
ICM-20948 も、LSM9DS1 もどちらも、 acc_x、acc_y の揺り戻しの方が多く検出される傾向は、同じようです。
LSM9DS1 の 生の acc_x、acc_y をプロットしてみましたが、やはり揺り戻しの方がより多く表示されます。
揺り戻し自体は、加速した後の、定速運動になった状態から、減速、停止のプロセスを表しているので、問題は無いのですが、
ただ、揺り戻しのデータ量が、加速から定速運動になる間のデータ量より明らかに多いのが問題です。
これだと、IMUブレッドボードをX軸の +方向に10[cm]程動かして、止めているのに、Rviz の tf base_footprint が、ちょっと前進した後、後戻りします。
ただ、少し強めに動かすと、それなりに動くようです。もしかしたら、ドローン向きのIMU なのかも知れません。
おんちゃんの目的である、自作Turtlebot3 の、Rtabmap-ros の 3D Mapping での IMU使用による、高さ測定には、使えないようです。
LSM9DS1 だと、 Acc 950[Hz] で、受信割り込みでのデータ取り込み方法がある様なので、こちらの方を使うと改善するのかも?
早速、LSM9DS1_Interrupts.ino をいじって試してみましたが、やはり特性は、Acc の揺り戻しが大きい様です。
Acc rate : 942[Hz]
後、LSM9DS1 は、 FIFO というのが使えるみたいです。これを使うと、違うのか?
注2) LSM9DS1 は、今朝から、Gyro が入りません。調子悪いです。by nishi 2022.5.9
注3) IMU を動かしてテストする時は、ブレッドボードを手に持って、浮かしてやると、きれいな波形が観測できるようです。
または、ガラス、金属の上で動かさいないといけない様です。
そうすると、動かした時の波形が、絵に書いたようにきれいに出てきます。
ここで、今のアルゴリズムでは、CUT OFF の処理が入っているので、0 に近づいた時に、波形が切れまてしまいます。
それでも、やはり、揺り戻しの方がどうしても、大きく観測されるのは、変わりません。 by nishi 2022.5.10
注4) もう一度、ICM-20948 で、Acc の生データを、Arduino IDE Serial Plotter で見てみました。
加速時と減速・停止時の波形が、それぞれ逆向きに、同じ大きさで出ている様です。たまに、若干、戻りの方が大きい時がありますが、
特に、Y軸に関しては、逆向きの、同じ波形の様に思います。
その後、1G キャンセル後の波形も確認してみましたが、明らかに、戻りデータが大きくなっているようです。
特に、Y軸に関しては、違いが大きい様です。どうやら、問題は、この 1Gキャンセルの処理にあるようです。
今回の、目玉の処理に問題が有るのは、大変ぞね。
MadgwickAHRS.cpp のパラメータをいじってみました。
//#define betaDef 0.1f
#define betaDef 0.01f
これで、大夫よくなりました。が、十分ではない!!
beta(betaDef) の意味が判るページがありました。MadgwickフィルタとINS/GPS複合航法の比較
値が小さくなるに従って、ジャイロによる慣性航法に対する、加速度、マグネットデータの利用割合が小さくなるみたいぞね。
なら、beta=0.0 も試してみるべきか?
また、ジャイロのデータが重要と言うことか?
Madgwick には、幾つかバージョンが有るようだが!!
最新版は、xioTechnologies/Fusion みたいぞね。
これを、ESP32 で試してみる!!
Madgwickフィルターを使った1Gキャンセル処理で、いまは、揺り戻しが生じている。これを改善しないと、まだまだ使える代物にならない。
逆に言えば、Madgwickフィルターの精度が改善すれば、使える物になりそうぞね。!!
同じ代物を、TouTube で良く見るが、その人達は、此処の問題を、どうしているのだろうか? 聞いてみたいものだ。
github にアップロードしたぞね。by nishi 2022.5.11
まだまだ、動作は、完成には程遠いですが、ESP32+IMU を宙に浮かして動かして、 Rviz 上で、tf base_footprint が、3D で動くのが確認できます。
ESP32 は、Wi-Fi で、PC上の、ROSと通信ができるので、ESP32を電池駆動にして、完成すれば、楽しいかも!!
どちらも、スイッチサイエンスで購入しましたが、あまり、精度が良くないのかも知れません。いわゆる、
昔、秋葉原で良く流通した、市販に流す、B級品なのかも。
この点に関しては、訂正しないといかんぞね。 by nishi 2022.5.10
特別悪くもないが、特別良くも無い。So So 品
この、IMU の記事は、別途ページを用意したので、そちらに記述します。
ROS IMU による移動距離と向き測定
17. 15.の手法で、改めて考えてみます。by nishi 2022.5.16
ホイールでの、移動距離を、Wd=[x 0 0]T とします。
このロボットは、前進、後退と、方向転換しかしないので、x=ホイールでの前後移動距離、y=0、z=0 になります。
∫CBWd で、単純に3D空間での、基本座標での位置が算出できます。
これで、再度試してみます。
注) モーター回転による、距離計算のサンプリング間隔と、IMU の サンプリング間隔が余りに違うので、その影響がどうなるのか?
注2) この方法だと、坂道を通った時の距離も、ちゃんとでてきそうです。
注2) この計算式だと、左右に斜めに傾いた時にロボットが前後に移動した時に、y軸の値がでてきそうです。
空中だとそれでOKだが、地上だとタイヤと地面の摩擦で、そうはならないのではないか?なので、y軸は無視するか?
CB の x行(0行)に、x を掛けて算出した、y成分(1列)の値を 正負逆にした値を、y' とすると、どうなるか?
Wd=[x y' 0]T
試した結果、Wd=[x y 0]T x=ホイール回転値、y=ホイール回転値 、z=0 にして、
∫CBWd の z のみ、/odom_fox と、 tf base_footprint の、z に使いました。
Single USB Stereo Camera を試す。 向けに作成した、rtabmap_ros with Single Stereo Camera で試してみました。
これで、ロボットが、床にめり込むことも少し改善できましたが、まだ少しめり込んで行きます。
精度に問題ありか!!。
rtabmap_ros の 3D-mapping には、高さ情報が必須かと思ったけれど、今の所、精度に難があるので、顕著な違いは、
確認できないぞね。by nishi 2022.5.19
2M行って、元のところへ帰ってくる、Python プログラムで、いつも、2M行って振り返った所か、そこから、1M 帰って来た所で、
いつも行きあぐんで止まってしまうので、記憶した遠くの目印が見つからないような気がしました。なので、
Stero/MinDisparity=5 -> 0.5
にしてみました。これで、やっと途中でとまらずに帰ってくるようになりました。
これの効果か判りませんが、tf basefoot_print の高さ情報にも反応している様です。
そういう意味では、効果は出ているのかも知れません。
あと、今まで、ずっと テスト途中で、IMU 情報のクオータニオン が途切れてしまう現象に悩まされたいましたが、原因がやっとわかりました。
どうやら、IMU のヘッダーピンに挿した、GND のメスケーブルが、ヘンダーピンの根本までしっかり入っていなかったようです。
いつも、Spark Fun の DMP 6軸Fusion を使った時に限って、初期化エラーが頻発していました。GND ラインの接触不良で、電力を多く使うときに限って、この症状がでていた模様です。
GND のメスケーブルをしっかり挿してから、初期化エラーは出なくなりました。
あと、Madgwick.cpp を止めて、Spark Fun の DMP 6軸Fusion で、300[Hz] にしてみました。これで、高さ情報の効果が確認できるようになったのかも。
おんちゃんの、部屋の畳が途中へこんでいるので、それに応じて、Turtlebot3 が Rviz 上で床に潜ったり、出てきたりします。
これが、正解なのかもしれません。by nishi 2022.5.20
Vis/EstimationType=1 // 1: 3D->2D
Odom/FilteringStrategy=1 // 1:Kalman filtering
で、3回、それなりにスムーズに完走できました。
これは、効果があるのでは、と思います。本物かも?
その後、完走出来ないこともあったので、念の為、turtlebot3_teleop_key.launch で動かしながら、rostopic echo /odom_fox | more で、pose:pose:position.z を確認してみました。
どうも、前進するたびに、z値 が小さくなっているようです。foxbot_core3 の車体が少し前傾している所為のように思えます。
Spark Fun DMP 6軸フュージョンをキャリブレーションして、前傾値を補正したほうが良いようです。
残念、上記方法では、車体の傾きの補正はうまく出来ませんでした。仕方がないので、IMUの取り付けスペーサーの両面テープで調整しました。
再度、turtlebot3_teleop_key.launch で確認してみましたが、うまく行きません。やはり精度不足の様です。
それでも、ロボットが床に沈み込んだり上がったりしているのは、pose:pose:position.z とは関係なく、rtabamap_ros が、
回りの目印情報を元に、高さを補正している様です。
この記事は、長くなったので、続きを別ページに記載します。