[论文解读] 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

一句话总结

该论文提出了一种不变EKF-SLAM算法，通过在SE(3)李群上使用右不变误差公式，解决了标准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那样依赖线性化点选择策略。

提出的方法

• 该方法使用右不变误差变量，定义为SE(3)李群中真实状态与估计状态之间的群差，确保几何一致性。
• 状态表示为 $ X = (R, x, p^1, \dots, p^K) \in SO(3) \times \mathbb{R}^{3+3K} $，通过映射 $ \Psi(X) $ 将其转换为矩阵李群 $ G = SE_{K+1}(3) $。
• 通过群误差 $ \eta = \chi \hat{\chi}^{-1} $ 的矩阵对数运算推导误差向量 $ \xi $，并一阶线性化，得到李代数中的状态误差。
• 基于线性化误差重新表述卡尔曼滤波方程，雅可比矩阵和协方差矩阵在当前估计定义的局部坐标系中表达。
• 系统模型通过伴随作用 $ 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在不可观测方向上保持了正确的信息矩阵衰减，避免了估计中的过度自信。
• 该方法在无需复杂线性化点优化的情况下实现了一致估计，与观测约束EKF（OC-EKF）形成对比。
• 该算法在计算复杂度上与标准EKF-SLAM保持一致，同时显著提升了估计一致性，表现为姿态和位置估计的过度自信程度降低。
• 所提方法对初始状态不确定性具有鲁棒性，并在多次运行中保持稳定性能，展示了其在真实世界SLAM应用中的实际可行性。