ROS2 自作 Turtlebot3 による 草刈りロボット開発。#11 ロボットの走行方向がずれる。
SBC: Orange pi 5 Armbian Ubuntu 24.04
ROS2: Jazzy
Jazzy 版にして、rtabmap_ros_my/foxbot_nav2_oak-d_depth_gps.launch.py で、自作 Turtlebot3 を動かしたら、最近どうも、ロボットの向きがずれる気がする。
Rviz2 の画面上は、ロボットは、目的地にちゃんと到達しているが、実機のロボットは、結構ずれた位置に行ってしまう。
1. EKF の設定が変わったのかと、いろいりためしていたが、
どうやら、原因は、 IMU ボードの取り付け向きが、ロボットの進行方向にから少しずれているみたいじゃ。
今まで、あまり気にしていなかったが、もしかしたら、DMP 9 の場合の、IMU のキャリブレーションを取っ払ってしまっていたかも知れない。
もとい、これは、単純に、IMU ボードの向きを正すしかない。
あと、ICM-20948 の接続ケーブルの接触が、時々不良になるみたい。そのせいで、急に Device ID が取れなくて、初期化処理でずっこける。
以前は、電源電圧が低下している所為と思っていたが、単純に、接触不良みたいじゃ。
やはり、原因は、別のところみたい。
robot_localization / ekf_node の jazzy 版が、どうやら問題だったみたい。
なんとなく、 IMU のフージョンで、ロボットの向きが狂っているみたい。
yaw_offset: 0.0
use_odometry_yaw: true
は、 navsat の方のパラメータみたいだが、なぜか効果があるみたい。
おんちゃんの勘違いか?
imu0_pose_rejection_threshold: 0.95 で、 imu フュージョンを抑えられるのか?
参考。
use_odometry_yaw: true #919
IMU のフージョンで、ロボットの向きを決めるのを禁止して、odometry の pose のみを使う。
なぜなら、おんちゃんは、 foxbot_core3_r2 の方で、 odometry の pose に、 ICM-20948 9軸 DMP の quaternion 値を出しているから。
大体、IMU の値を、ホスト側で処理するのは無理ではないのか?
やはり、IMU の値は、デバイス上で処理すべきだと思う。
少しは、改善したが、まだロボットの向きが、実機と、Rviz2 の画面と一致しない。
ICM-20948 9軸 DMP の限界だろうか?
ekf_filter_node から、エラーが出ているみたい。
上記、エラーに対応したら、良くなった。
実ロボットの位置と向きが、Rviz2 上のロボットの位置と向きとが一致した。
xxx_differential: true は、センサーが2個ある場合に、 true にするみたい。
2. もっと根本的な問題があった。
ICM-20948 DMP9 の起動時の計測値自体が、少しずれているみたい。
以前から、 実機 foxbot-core3-r2 のスタート時のロボットのむきが、Rviz2 でみると、なぜか前方から少しずれている。
どうやら、DMP9 の起動時に少しキャリブレーションをしてから、それでも少しずれているのが判った。
理想は、q0(1,0,0,0) がほしいが、そうはならない。なので、起動時の 実測の q1 の共役 q1' を補正値として使うことにした。
P = q1' x P0 のクォータニオンの回転式をつかったらうまく行った。
P : 補正後のロボットの向き。
P0 : ICM-20948 DMP9 の取り込み値の クォータニオン。
q1' : スタート時の実測の q1 の 共役。
クォータニオン回転の C++ コードは、下記を拝借しました。ありがとう!!
四元数で３次元回転 （ソースコード付き）
注) クォータニオン の回転でよく見かける式
p' = qpq*
だとうまくいかない。上記だと、元に戻ってしまうみたい。
C++ ソースは、下記に公開しています。
tosa-no-onchan / foxbot_core3
3. そのご、さらに根本的な問題が判った。
ICM-20948 の SparckFun の Arduino Library の DMP9、DMP6 は、しばらく使っていると、ICM-20948 のデータは、上がってくるが、
quaternion の Z値が正しく上がって来なくなるのが判った。
なんじゃこりゃ!! これでは、実機では、とうてい使えない。by nishi 2025.3.22
DMP は諦めて、ACC、Gyro、 Magnet + MadgwickAHRS を使うことにした。
詳しくは、SparkFun ICM-20948 Arduino で、 DMP Fusion 9軸 がやっとOKになった。 に記載した。