自律移動ロボットにとって「自分が今どこにいるか」を知ることは、地図作成にも障害物回避にも先立つ最も基礎的な機能である。newbotでは、3D LiDAR(Livox Mid-360)によるLiDAR-Inertial Odometry(FAST-LIO)を主な自己位置推定源とし、LiDAR非搭載時はIMUを含むセンサーフュージョン(EKF)にフォールバックする設計にしている。

ROS 2ROS 2

画像: Robot Operating Systemロゴ、Wikimedia Commons(CC BY-SA 4.0)

なぜオドメトリの権威を1つに絞るのか

自己位置(/odom)とロボットの姿勢を表すTF(座標変換)は、動作モードごとにちょうど1つのノードだけが出力するよう設計している。

複数のノードが同じ情報を独立に出す状態(二重ソース)は、値が食い違った瞬間にどちらを信じるべきか判断できなくなり、地図作成やナビゲーションの挙動を不定にする。この曖昧さを設計段階で排除するのが狙いである。上位のmap→odom変換は常にslam_toolboxが担うため、TFツリーは map → (slam_toolbox) → odom → (FAST-LIOまたはEKF) → base_link → (URDF) → 各センサーframe という一本道になる。

FAST-LIO2という選択

3D LiDAR-Inertial Odometryのアルゴリズムには複数の選択肢があるが、Livox Mid-360が6軸IMU(角速度+加速度のみ、地磁気センサーなし)を搭載していることから、6軸IMUに適合し依存関係が軽いFAST-LIO2を採用した。9軸IMU+GTSAMを前提とするLIO-SAM等は、この機体には不適合と判断している。

導入にあたっては、標準実装がtwist(速度)を出力せずpose(姿勢)のみを出す仕様だったため、速度成分を計算して充填するパッチを当て、完全なオドメトリメッセージとして扱えるようにした。また、FAST-LIOは推定対象をbody_frame(≒IMU本体)としているが、これをbase_linkとして近似的に扱っている。センサーを車体中心の直上に搭載しているため、平地走行における水平方向の誤差は実質ゼロになる。

実測したドリフト精度

出発点へ戻る周回走行を行い、始点と終点のズレをドリフトとして実測した。

指標 FAST-LIO(LiDAR) 車輪オドメトリ
水平方向の周回誤差 経路長の約1.0〜1.3% 経路長の約6.9〜27.4%

その場旋回主体でスリップの影響を受けやすい、LiDARにとって不利な走行条件下でもこの精度が出ており、車輪オドメトリ単体との差は数値上明確である。一方で、垂直方向(高さ)には約0.2mのドリフトが残っており未解決の課題として残している。屋内の狭い範囲・旋回主体の走行では垂直方向の特徴が乏しく、Z軸が拘束されにくいことが要因と見ている。2次元の経路計画では水平面へ投影するため実用上の影響は小さいと考えているが、EKFの二次元拘束モードで後段からZ/roll/pitchを0に押し込む対策を検討中である。

机上点検で見つけたEKFの設定ミス

実機投入前の机上点検で、EKF設定に潜んでいた不具合を1件見つけている。使用しているLiDARドライバ(livox_ros_driver2)は、IMUメッセージのorientation(絶対姿勢)フィールドを一切設定せず、角速度と加速度のみを配信する仕様になっている。ところが当初のekf.yamlは、このorientationのyaw成分を融合対象としてtrueに設定していた。存在しないはずの値(実際にはゼロ埋めされた値)を絶対姿勢として信じ込む設定だったため、EKFのyaw推定が実質的に固定されて壊れる状態だった。角速度(vyaw)のみを融合する設定に修正し、あわせて重力除去フラグも走行環境に合わせて見直している。この修正はコードを読むだけで気づけるものだったが、実機を動かして初めて症状として顕在化するタイプの不具合であり、机上点検の価値を裏付ける一件になった。

冗長性の欠如という設計上の弱点

現在のTFツリーでは、odom→base_linkを出せるのはFAST-LIOだけであり、LiDARが機能を失えば自己位置は即座に失われる。実際の走行検証では、1日の走行中にLiDARが複数回(最大5回)ドロップする事象が発生している。

LiDAR脱落の原因調査

脱落のたびに再起動で復旧させるだけでなく、原因の切り分けも進めている。LiDARとの通信に使う有線Ethernetのエラーカウンタを確認したところ、約280万パケット中エラーはわずか2件で、通信品質そのものに問題は見られなかった。一方、脱落から復帰までの所要時間は5〜16秒で、これはLiDAR本体が再起動するのに要する時間とほぼ一致する。この2つの事実を合わせると、通信経路ではなくLiDAR本体の電源側で瞬断が起きている可能性が高いと見ている。決定的なのは、電源電圧を監視する分圧回路が現状まだ配線されておらず、電圧降下が実際に起きているかどうかを直接確認する手段がないことである。自動復旧の仕組みを入れたことで走行自体は継続できるようになったが、これは症状への対処であって原因の根治ではなく、電源系統の実測が残された課題として記録されている。

さらに根が深いのは復帰の仕方である。LiDARの脱落を検知して自動復旧する仕組み(watchdogがFAST-LIOのプロセスを強制終了し、launchのrespawn機能で作り直す)は実装済みだが、FAST-LIOは再起動すると必ず原点から自己位置を再初期化する。するとodomフレームの原点が飛び、map→odom変換を出しているslam_toolboxはその飛びを関知しないため、地図との対応関係が壊れる。つまり現状の自動復旧は「オドメトリの配信自体は再開する」ところまでしか回復させておらず、Nav2は新しい(実際には誤っている)自己位置を信じて動作を継続してしまう。走行が止まらないように見えても、安全な状態への復帰とは言えない。

この問題への対処として、既存の地図に対してスキャンマッチングによる再局所化を復旧シーケンスに組み込む案(slam_toolboxのlocalizationモード、あるいは初期姿勢を与えた上でFAST-LIOを再開する仕組み)を検討している。加えて、自己位置の状態を「正しい/壊れている」の二値ではなく、「LiDAR脱落直後で信頼度が低下している」といった中間状態として表現できるようにし、Nav2側が減速・新規ゴール拒否・その場待機といった保守的な振る舞いに切り替えられるようにする方向性も検討中である。

車輪エンコーダの分解精度の限界

LiDARが使えない場面のフォールバックとして車輪オドメトリを使う以上、そのロータリーエンコーダの品質そのものにも実測で向き合う必要があった。現在のファームウェアはA相の立ち上がりエッジのみをカウントし(1逓倍)、方向はB相の状態から判定する方式を採っている。この方式には低速・高速それぞれで固有の弱点があることを実測で確認している。

この誤差は、直進走行中の見かけの回転(左右輪の距離差から角速度を算出する車輪オドメトリの原理上、片輪が過剰カウントすると直進を旋回と誤認する)として現れる。実測では、1.2mの直進走行でFAST-LIOが+6.5°の姿勢変化を示したのに対し、車輪オドメトリの計算上は+80°もの回転が起きたことになっていた。現状ではEKFの車輪オドメトリ融合設定(odom0)がyawとvyawの両方を取り込む構成になっているが、これはLiDAR非搭載モード専用であり、通常のFAST-LIO構成では有効化されていないため実害は出ていない。ただし、将来的にLiDAR脱落時のフォールバックとしてこの経路を積極的に使う設計にする場合は、A相をCHANGE割り込みにしてB相とXORする4逓倍化(あるいはESP32のハードウェアPCNTペリフェラルの活用)による分解能・耐ノイズ性の向上が前提条件になる。

参考リンク

#FAST-LIO #ROS2