[論文レビュー] An EKF-SLAM algorithm with consistency properties

Axel Barrau, Silvère Bonnabel|arXiv (Cornell University)|Oct 21, 2015

Robotics and Sensor-Based Localization参考文献 39被引用数 105

ひとこと要約

本稿では、Lie群SE(3)上での右不変誤差定式化を用いることで、標準的EKF-SLAMの不一致を解消する不変EKF-SLAMアルゴリズムを提案する。これにより、グローバルフレームの原点および姿勢における非観測性が正しく反映され、共分散行列が正しい形で不観測方向を反映する。本手法は、不観測方向における適切な情報行列の減衰を維持し、理論的に証明され、モンテカルロシミュレーションによって一貫性のある性能が確認された。

ABSTRACT

In this paper we address the inconsistency of the EKF-based SLAM algorithm that stems from non-observability of the origin and orientation of the global reference frame. We prove on the non-linear two-dimensional problem with point landmarks observed that this type of inconsistency is remedied using the Invariant EKF, a recently introduced variant ot the EKF meant to account for the symmetries of the state space. Extensive Monte-Carlo runs illustrate the theoretical results.

研究の動機と目的

• グローバルフレームの原点および姿勢の非観測性に起因するEKF-SLAMの有名な不一致を解消すること。
• 標準的EKF-SLAMが、状態空間における対称性を適切に扱わずに、不観測方向において誤って不確実性を減少させることを示すこと。
• 右不変誤差変数を用いる幾何学的構造を尊重するEKF-SLAMの変種、すなわち革新EKF-SLAM（IEKF-SLAM）を提案すること。
• IEKF-SLAMが、不観測部分空間の正しい次元を自動的に維持することを証明すること。
• OC-EKFと同様に線形化点選択戦略を必要としない、広範なモンテカルロシミュレーションを通じて、提案手法が一貫した推定を達成することを示すこと。

提案手法

• 真の状態と推定状態との間の群差分として定義される右不変誤差変数を用い、幾何学的整合性を確保する。
• 状態は $ X = (R, x, p^1, \dots, p^K) \in SO(3) \times \mathbb{R}^{3+3K} $ として表現され、行列群 $ G = SE_{K+1}(3) $ への写像 $ \Psi(X) $ によって表現される。
• 群誤差 $ \eta = \chi \hat{\chi}^{-1} $ の行列対数を用いて誤差ベクトル $ \xi $ を導出する。1次近似により、リー代数上の状態誤差に線形化する。
• 線形化誤差を用いてカルマンフィルタ方程式を再定式化し、ジャコビアンおよび共分散行列を現在の推定値によって定義される局所フレームで表現する。
• システムモデルは、随伴作用 $ G_n = \mathrm{Ad}_{\hat{\chi}_{n|n-1}} $ を用いて発展させ、出力マップ $ H_n $ は回転されたフレームにおける観測関数のヤコビアンから導出する。
• 更新ステップでは、標準的カルマンゲインを用いるが、補正は指数写像を用いて群要素 $ \hat{\chi}_{n|n} = \exp(K_n z_n) \Psi(\hat{X}_{n|n-1}) $ を更新し、その後逆写像により状態推定値を回復する。

実験結果

リサーチクエスチョン

• RQ1 グローバルフレームの原点および姿勢の非観測性に起因するEKF-SLAMの不一致は、幾何学的に整合性のある誤差定式化によって解消可能か？
• RQ2 EKFフレームワークにおいて右不変誤差変数を用いることで、不観測部分空間の正しい次元が自動的に維持されるか？
• RQ3 IEKF-SLAMは、標準的EKF-SLAMおよびOC-EKFと比較して、共分散の一貫性および推定精度においてどのように異なるか？
• RQ4 線形化点選択に依存せずに、不観測方向における情報行列の減衰に関する理論的保証を導出可能か？
• RQ5 IEKF-SLAMアルゴリズムは、現実的なSLAM条件下でのモンテカルロシミュレーションにおいて、どのように実験的性能を示すか？

主な発見

• IEKF-SLAMアルゴリズムは、不観測方向（グローバルトランスレーションおよび回転）における人工的な共分散低減を効果的に防止し、EKF-SLAMの不一致の根本的要因を解消した。
• 理論的解析により、右不変誤差を用いる場合、すべての状態において不観測部分空間が同一のベクトルで一貫して張られることを証明した。これにより、適切な可観測性構造が保証された。
• モンテカルロシミュレーションにより、IEKF-SLAMが不観測方向における正しい情報行列の減衰を維持していることが確認され、推定値に対する過信を回避した。
• OC-EKFと同様に、複雑な線形化点最適化を必要とせず、一貫した推定が達成された。
• 標準的EKF-SLAMと同等の計算複雑性を維持しながら、方向および位置推定値における過信の低減により、著しく一貫性の高い推定を実現した。
• 本手法は初期状態の不確実性に対してもロバストであり、複数回の実行において安定した性能を示し、実世界のSLAM応用における実用的妥当性を示した。