[論文レビュー] Associating Uncertainty to Extended Poses for on Lie Group IMU Preintegration with Rotating Earth

Martin Brossard, Axel Barrau+2 more|arXiv (Cornell University)|Jul 28, 2020

Robotics and Sensor-Based Localization参考文献 54被引用数 77

ひとこと要約

本論文は、回転する地球を考慮した IMU 事前積分のための SE2(3) における拡張姿勢へ不確実性を付与する理論を展開し、厳密な事前積分公式を導出し、シミュレーションと車両上の LiDAR-IMU 融合によって検証する。

ABSTRACT

The recently introduced matrix group SE2(3) provides a 5x5 matrix representation for the orientation, velocity and position of an object in the 3-D space, a triplet we call "extended pose". In this paper we build on this group to develop a theory to associate uncertainty with extended poses represented by 5x5 matrices. Our approach is particularly suited to describe how uncertainty propagates when the extended pose represents the state of an Inertial Measurement Unit (IMU). In particular it allows revisiting the theory of IMU preintegration on manifold and reaching a further theoretic level in this field. Exact preintegration formulas that account for rotating Earth, that is, centrifugal force and Coriolis force, are derived as a byproduct, and the factors are shown to be more accurate. The approach is validated through extensive simulations and applied to sensor-fusion where a loosely-coupled fixed-lag smoother fuses IMU and LiDAR on one hour long experiments using our experimental car. It shows how handling rotating Earth may be beneficial for long-term navigation within incremental smoothing algorithms.

研究の動機と目的

• SE2(3) を用いて拡張姿勢（姿勢・速度・位置）へ不確実性表現を拡張する。
• 地球の回転効果を含む SE2(3) 群上の IMU 事前積分公式を導出する。
• IMU運動学と離散的事前積分ステップを通じた厳密な不確実性伝播を提供する。
• シミュレーションと実車の1時間実験でフレームワークを検証し、付随するコードと補足資料を公開する。

提案手法

• 拡張姿勢を SE2(3) の 5x5 行列構造の要素として表現する。
• 不確実性を集中ガウス分布で記述する: T = T̂ exp(xi) with xi ~ N(0, Σ).
• BCH ベースのツールと Ad_T 演算子を用いて IMU 運動学を通じて平均と共分散を伝搬する。
• T_{i+1} = Γ_i Φ(T_i) Υ_i の厳密な離散化を導出し、a_i = Ad_{Υ_i^{-1}} F ξ を用いた高次補正とともに不確実性を伝搬させる。
• 回転する地球を組み込み、遠心力とコリオリ力を事前積分フレームワークに合わせて考慮する。
• 二次・四次項（S4th）共分散補正を提供し、バイアス処理について論じる。

実験結果

リサーチクエスチョン

• RQ1 IMUアプリケーションのために SE2(3) で表される拡張姿勢 (R, v, p) に不確実性を一貫して関係づけるにはどうすればよいか？
• RQ2 IMU運動学モデルや Lie群上の IMU事前積分を通じて拡張姿勢の不確実性を伝搬させるにはどうするのか？
• RQ3 長期ナビゲーションに特に有効な、正確な IMU 事前積分に回転する地球の効果を含める利点は何か？
• RQ4 提案された SE2(3) フレームワークは、長いシーケンスにわたるセンサ融合シナリオ（例：IMU-LiDAR）で正確なファクター共分散を生み出せるか？

主な発見

• 拡張姿勢上の不確実性は、SE2(3)上のIMU運動学を介して正確に伝搬でき、平均は T̂_{i+1} によって更新され、共分散は Σ_{i+1} = Ad_{Υ_i^{-1}} F Σ_i (Ad_{Υ_i^{-1}} F)^T で得られる。
• ノイズを含むIMU測定を取り入れると、第四階 BCH 項は共分散補正 S_4th をもたらし、二次近似と比較して精度を改善する。
• ノイズ無しモデルを通じて SE2(3) 上の集中ガウスを保持し、平均の進化は小さな摂動の下で合理的のままである。
• 姿勢更新の厳密な離散化（離散化誤差なし）が達成され、実装と数値実験には実用的なオイラー型ステップが用いられる。
• 実車の loosely-coupled iSAM2 フレームワークで one-hour IMU-LiDAR シーケンスを用いた実験が method の有効性を示し、長期の増分平滑化に利点を示す。
• Python スクリプトと GTSAM のフォークにより、主要な方程式とプロットの再現が可能になる。