FAST-LIVO2

Zheng 2024 · 論文

一行要約 — FAST-LIVO2は、異種であるLiDARと画像測定値の間の次元不一致を解決する逐次更新を伴う誤差状態反復カルマンフィルタによってIMU、LiDAR、カメラを融合し、オンボード計算資源上での直接LVIオドメトリの基準を打ち立てた。

問題

LiDARスキャンは1回の更新で数千の低次元幾何残差を提供するのに対し、カメラフレームはまったく異なる構造の測光残差を提供する——両方を1つのカルマンステップで同時に更新するのは扱いにくく、FAST-LIVOの簡略化（アフィンワープのためのパッチごとの一定深度仮定、露出処理なし、LiDAR点が欠損した際のフォールバックなし）は精度とロバスト性を犠牲にしていた。FAST-LIVO2は、リアルタイムかつ完全オンボードのロボット用途に十分な厳密性と効率性を持つよう融合を再設計する。

手法とアーキテクチャ

19次元多様体 M=SO(3)×R16\mathcal{M} = SO(3)\times\mathbb{R}^{16} 上の状態：x=[GRIT GpIT GvIT bgT baT GgT τ]T\mathbf{x} = [{^G}\mathbf{R}_I^T\ {^G}\mathbf{p}_I^T\ {^G}\mathbf{v}_I^T\ \mathbf{b}_g^T\ \mathbf{b}_a^T\ {^G}\mathbf{g}^T\ \tau]^T で、τ\tau は逆カメラ露出時間（最初のフレームに対する相対値、可観測性のため τ0=1\tau_0 = 1 に固定）であり、ランダムウォークとしてモデル化される。スキャンの再結合により、LiDARストリームはカメラのサンプリング時刻でスキャンに再分割され、両センサが同じ10 Hzで更新される。

p(x∣yl,yc)∝p(yc∣x) p(yl∣x) p(x)⏟∝ p(x∣yl),p(\mathbf{x} \mid \mathbf{y}_l, \mathbf{y}_c) \propto p(\mathbf{y}_c \mid \mathbf{x})\, \underbrace{p(\mathbf{y}_l \mid \mathbf{x})\, p(\mathbf{x})}_{\propto\, p(\mathbf{x} \mid \mathbf{y}_l)},

と因数分解し、フィルタはまずIMUで伝播された事前分布に対してLiDAR更新を反復し、続いてLiDAR収束後の状態と共分散に対して視覚更新を行う——2つの雑音が独立であれば理論的には結合更新と等価だが、各モジュールは自身の反復と構造を保持する。各ステップは標準的な反復カルマンゲイン K=((Hκ)TR−1Hκ+P^−1)−1(Hκ)TR−1\mathbf{K} = ((\mathbf{H}^\kappa)^T\mathbf{R}^{-1}\mathbf{H}^\kappa + \widehat{\mathbf{P}}^{-1})^{-1}(\mathbf{H}^\kappa)^T\mathbf{R}^{-1} を用いる。

0=τk Ik(ui+Δu)−τr Ir(ui′+Air Δu),\mathbf{0} = \tau_k\,\mathbf{I}_k(\mathbf{u}_i + \Delta\mathbf{u}) - \tau_r\,\mathbf{I}_r(\mathbf{u}'_i + \mathbf{A}^r_i\,\Delta\mathbf{u}),

ここで Air\mathbf{A}^r_i は、FAST-LIVOの一定深度仮定の代わりにLiDAR平面事前情報（法線は任意でマルチパッチ測光最適化により精緻化される）から計算されたアフィンワープである。逆合成的な形式化により、ポーズ増分は参照側に配置されるためヤコビアンは一度だけ計算され、アライメントは3段のピラミッドレベルにわたって粗から密へ実行される。τk\tau_k が残差に現れるため、露出も同じ更新の中で推定される。

実験結果

SLAMにおける意義

FAST-LIVO2は、最強のオープンソース直接LVIオドメトリとして広く認識されている——HKU MARS系列（FAST-LIO2 → FAST-LIVO → FAST-LIVO2）の集大成である。その逐次更新のトリックは、次元と構造が大きく異なる測定値を持つセンサを融合するための一般的に有用なパターンであり、実証されたUAV展開は、エッジコンピューティング予算下で直接三重融合が実用可能であることを示している。今日、ロボット向けの現代的なLVIシステムを選ぶなら、これが打倒すべきデフォルトの候補である。

ハンズオン

関連ノート