LIO-SAM

Shan 2020 · 論文

一行要約 — LIO-SAMは、LiDAR-慣性オドメトリをファクタグラフによる平滑化として再定式化し、IMUプリインテグレーション、スキャンマッチング、GPS、ループ閉じ込みをすべて1つのMAP推定問題に取り込みつつ、キーフレームをローカルなスライディングウィンドウマップのみとマッチングすることでリアルタイム性を維持した。

問題

LOAMはデータをグローバルなボクセルマップに保存するため、ループ閉じ込み検出やGPSのような絶対測定値の取り込みが難しく、そのIMUはスキャンのスキュー除去と運動事前分布の提供にのみ使われる（疎結合）ため、ボクセルマップが密になるにつれ最適化が悪化する。LIOMのような密結合の代替手法はすべての測定値を結合処理するが、実時間の約0.6倍でしか動作しない。LIO-SAMはこの両方の問題を同時に解決する。ファクタグラフは異種の相対・絶対測定値をファクタとして受け入れ、ローカルスケールでのスキャンマッチング（グローバルスケールではなく）が計算量を有界に保つ。

手法とアーキテクチャ

ロボットの状態は $\mathbf{x} = [\,\mathbf{R}^{\top}, \mathbf{p}^{\top}, \mathbf{v}^{\top}, \mathbf{b}^{\top}\,]^{\top}$（姿勢、位置、速度、IMUバイアス）である。ポーズの変化が閾値を超えると新しい状態ノードが追加され、グラフは4種類のファクタのもとでiSAM2によりインクリメンタルに最適化される。

• IMUプリインテグレーションファクタ：時刻 $i$ と $j$ の間で、生のIMUレートは相対運動制約に積分される。

  $$\Delta\mathbf{v}_{ij} = \mathbf{R}_i^{\top}(\mathbf{v}_j - \mathbf{v}_i - \mathbf{g}\Delta t_{ij}), \quad \Delta\mathbf{p}_{ij} = \mathbf{R}_i^{\top}\left(\mathbf{p}_j - \mathbf{p}_i - \mathbf{v}_i\Delta t_{ij} - \tfrac{1}{2}\mathbf{g}\Delta t_{ij}^2\right), \quad \Delta\mathbf{R}_{ij} = \mathbf{R}_i^{\top}\mathbf{R}_j.$$

  プリインテグレーションは二重の役割を果たす。点群のスキュー除去とスキャンマッチングの初期化を行い、最適化されたLiDARオドメトリがグラフ内でIMUバイアスを推定する。

• LiDARオドメトリファクタ：LOAM方式のエッジ・平面特徴（局所的な粗さによる）が各スキャンから抽出される。ポーズが1 m または10°以上変化するとキーフレームが選ばれ、その間のフレームは捨てられる。新しいキーフレームは、グローバルマップではなく、直近の $n = 25$個のサブキーフレームから統合されたボクセルマップ（エッジマップは0.2 m、平面マップは0.4 mでダウンサンプリング）に対してレジストレーションされる。点対線・点対平面距離、例えばエッジ特徴に対して

  $$\mathbf{d}_{e_k} = \frac{\left|(\mathbf{p}^{e}_{i+1,k}-\mathbf{p}^{e}_{i,u}) \times (\mathbf{p}^{e}_{i+1,k}-\mathbf{p}^{e}_{i,v})\right|}{\left|\mathbf{p}^{e}_{i,u}-\mathbf{p}^{e}_{i,v}\right|},$$

  はガウス-ニュートン法で $\mathbf{T}_{i+1}$ について最小化され、$\min_{\mathbf{T}_{i+1}} \{ \sum_k \mathbf{d}_{e_k} + \sum_k \mathbf{d}_{p_k} \}$、得られた相対変換 $\Delta\mathbf{T}_{i,i+1} = \mathbf{T}_i^{\top}\mathbf{T}_{i+1}$ が連続する状態を結ぶファクタとなる。

• GPSファクタ：絶対位置はローカルなカルテシアン座標に変換され——LiDAR-慣性ドリフトはゆっくりと増大するため——推定位置共分散がGPS共分散を超えたときのみ、タイムスタンプ整合のための線形補間とともに追加される。

• ループ閉じ込みファクタ：単純だが効果的なユークリッド探索により、新しい状態から15 m以内の過去の状態を見つける。新しいキーフレームは、その候補周辺の $2m+1$個のサブキーフレーム（$m = 12$）に対してスキャンマッチングされる。ループ閉じ込みは特に高度ドリフトの補正に有効であり、著者らのテストではGPSの標高誤差が100 mに達することもあった。

実験結果

VLP-16、MicroStrain 3DM-GX5-25 IMU、Reach M GPSを用い、i7-10710U CPU（GPUなし）上で、5つの自己収集データセット（Rotation、Walking、Campus、Park、Amsterdam）を、ハンドヘルド機器、Clearpath Jackal UGV、Duffy 21電動ボートという3つのプラットフォームで評価した。

• 端点並進誤差（m）：Campus（1437 m）：LOAM 192.43、LIO-odom（GPS／ループなし）9.44、LIO-GPS 6.87、LIO-SAM 0.12。Park（2898 m）：LOAM 121.74、LIOM 34.60、LIO-GPS 2.93、LIO-SAM 0.04。Amsterdam（19 065 m、3時間の運河クルーズ）：LIO-GPS（1.21）とLIO-SAM（0.17）のみが意味のある結果を出した。
• GPS地上真値に対するRMSE（Park）：LIO-SAM 0.96 m対LOAM 47.31 m、LIOM 28.96 m、LIO-odom 23.96 m。
• ロバスト性：Rotationテスト（静止状態で最大133.7 °/s）では、LIOMが初期化に失敗する一方、LIO-SAMは $SO(3)$ で正確にレジストレーションする；Walkingデータセットは213.9 °/sに達する。
• 実行時間：スキャンあたりのマッピング時間、例えばWalking：LIO-SAM 58.4 ms対LOAM 253.6 msおよびLIOM 339.8 ms；ストレステストでは実時間の最大13倍の再生速度でも正常動作を示した。LIOMは実時間の約0.6倍でしか動作しなかった。

SLAMにおける意義

LIO-SAMは、VINS-MonoとOKVISがカメラに対して行ったことをLiDARに対して行った。すなわち、グラフ最適化による密結合の慣性融合をデフォルトのアーキテクチャにした。測定源をファクタとして明確に分離したことで拡張が容易になり——LVI-SAMはその上に視覚-慣性サブシステム全体を追加している——今もFAST-LIO2のようなフィルタベースシステムが比較される標準的なファクタグラフのベースラインである。ループ閉じ込み、GPS融合、そして平滑化バックエンドを標準で備えたいときに使うべき手法である。

関連ノート

• LOAM — 特徴抽出とスキャンマッチングの基盤
• LVI-SAM — LiDAR-視覚-慣性融合への直接的な拡張
• FAST-LIO2 — 競合する直接方式、フィルタベースの手法
• IMU preintegration — 鍵となる慣性の仕組み
• Factor graph — バックエンドの形式論