センサフュージョンがうまくいかない理由 — 時刻ずれ・座標系・外部較正

センサを増やしたのに位置推定が悪くなるとき、最初に疑うのはフィルタの難しさだけではない。同じ物理量でも、時刻・座標・単位が異なれば、そのまま比較できない。まず入力単体を確認し、その後で融合器へ渡す境界を点検する。

切り分けの順番を固定する

図1 · 横にスクロールして図を読む

同じ短いログを使い、車輪だけ、IMUだけ、両方という順に確認する。毎回走行条件まで変えると、改善が設定変更によるものか床面や速度の違いなのか判断しにくい。基礎はセンサフュージョン入門で確認できる。

時刻ずれは移動量のずれになる

等速で1 m/sの車体を考える。観測が50 msずれると、同じ瞬間として比較した位置には 1\times0.05=0.05 mの差が生まれる。角速度90度/sなら、同じ50 msで姿勢差は4.5度となる。これは定速度を仮定した計算例で、許容誤差の推奨値ではない。

メッセージのタイムスタンプがセンサの取得時刻か、ドライバの受信時刻かを調べる。通信遅延が一定でも、取得時刻を正しく使えば対応できる場合がある。逆にホスト間の時計が異なると、単にキューを増やしても直らない。ログ再生では全ノードのシミュレーション時刻設定をそろえる。

座標の向きと原点を確認する

ROSの車体座標は通常x前方・y左方・z上方、カメラの光学座標はz前方・x右方・y下方である。単位と軸の定義はREP-103で確認する。frame名を変更するだけでは数値を回転したことにならない。

センサ座標の点を車体へ変換する式は p_b=R_{bs}p_s+t_{bs}。回転だけでなく並進も必要で、逆変換の並進は単に -t ではなく -R^Tt になる。較正値の向きを「sensorからbody」なのか「bodyからsensor」なのか記録する。

取付位置の違いは運動の違いでもある

回転する剛体では、センサ位置の速度は v_s=v_b+\omega\times r となる。原点から0.2 m離れた位置で角速度1 rad/s、距離ベクトルが回転軸に直交する例なら、速度差の大きさは0.2 m/s。車体がその場旋回していても、離れたセンサが静止しているわけではない。

平行移動だけでなく旋回を含めて確認する。ただし本格的な外部較正には十分な運動励起が必要で、直進だけのデータから全自由度を求められるとは限らない。座標の役割はREP-105とも照合する。

残差と共分散を最後に読む

残差は観測と予測の差である。共分散を小さくすると入力を強く信じるが、間違った軸や時刻を正しくする効果はない。同じエンコーダから作った位置と速度を独立した観測として強く信じれば、重複した情報を過大評価する可能性もある。robot_localizationの公式実装で、使用する入力項目とフレーム処理を確認する。

原因を一つずつ確かめる

設定変更は一回に一つとし、同じログで差を確認してから新しい走行で再確認する。静止で値が妥当でも、動いているときの同期・取付誤差は残り得る。SLAM評価へ進む前に、時刻の定義、単位、TFの配信元、較正値の方向を一枚の記録にそろえよう。