[论文解读] Robust tightly coupled pose estimation based on monocular vision, inertia, and wheel speed

Gang Peng, Zezao Lu+3 more|arXiv (Cornell University)|Mar 3, 2020

Robotics and Sensor-Based Localization参考文献 19被引用 4

一句话总结

本文提出了一种将单目视觉、惯性测量（IMU）和轮速数据紧密耦合的多传感器融合SLAM算法，适用于地面机器人。通过非线性优化融合传感器数据，并引入轮速里程计预积分技术，该方法在视觉丢失情况下仍能实现高精度（812米行程累计误差2.2米）和鲁棒性，优于单目视觉-惯性SLAM和传统轮速里程计方法。

ABSTRACT

The visual SLAM method is widely used for self-localization and mapping in complex environments. Visual-inertia SLAM, which combines a camera with IMU, can significantly improve the robustness and enable scale weak-visibility, whereas monocular visual SLAM is scale-invisible. For ground mobile robots, the introduction of a wheel speed sensor can solve the scale weak-visible problem and improve the robustness under abnormal conditions. In this thesis, a multi-sensor fusion SLAM algorithm using monocular vision, inertia, and wheel speed measurements is proposed. The sensor measurements are combined in a tightly coupled manner, and a nonlinear optimization method is used to maximize the posterior probability to solve the optimal state estimation. Loop detection and back-end optimization are added to help reduce or even eliminate the cumulative error of the estimated poses, thus ensuring global consistency of the trajectory and map. The wheel odometer pre-integration algorithm, which combines the chassis speed and IMU angular speed, can avoid repeated integration caused by linearization point changes during iterative optimization; state initialization based on the wheel odometer and IMU enables a quick and reliable calculation of the initial state values required by the state estimator in both stationary and moving states. Comparative experiments were carried out in room-scale scenes, building scale scenes, and visual loss scenarios. The results showed that the proposed algorithm has high accuracy, 2.2 m of cumulative error after moving 812 m (0.28%, loopback optimization disabled), strong robustness, and effective localization capability even in the event of sensor loss such as visual loss. The accuracy and robustness of the proposed method are superior to those of monocular visual inertia SLAM and traditional wheel odometers.

研究动机与目标

• 解决单目视觉SLAM在地面机器人中尺度模糊性和鲁棒性不足的问题。
• 在复杂或低能见度环境中提升轨迹一致性并减少累计误差。
• 利用轮速里程计和IMU实现在静止和运动状态下的可靠状态初始化。
• 在传感器故障（尤其是视觉丢失）期间增强系统鲁棒性。
• 高效集成轮速和IMU数据，避免在优化过程中重复积分。

提出的方法

• 采用紧密耦合的优化框架，融合单目相机、IMU和轮速测量，以最大化后验概率用于状态估计。
• 轮速里程计预积分将车身速度与IMU角速率结合，避免在迭代优化过程中重复积分。
• 状态初始化利用轮速里程计和IMU数据，在静态和动态状态下均提供精确的初始估计。
• 集成回环检测与后端优化，以校正累计漂移并确保全局轨迹一致性。
• 使用非线性优化联合估计位姿、速度和偏差状态，同时最小化所有传感器测量的误差。
• 通过预积分轮速和IMU数据，避免线性化点重新积分，提升数值稳定性。

实验结果

研究问题

• RQ1 单目视觉、IMU和轮速的融合是否能显著提升地面机器人SLAM中的位姿估计精度和鲁棒性？
• RQ2 所提出的轮速里程计预积分技术如何在迭代优化过程中减少数值误差？
• RQ3 在视觉丢失或能见度差的情况下，该系统在多大程度上仍能保持精度和一致性？
• RQ4 所提出的初始化方法是否能在静止和运动场景下可靠估计初始状态？
• RQ5 与单目视觉-惯性SLAM和传统轮速里程计相比，三传感器紧密耦合融合在累计误差方面表现如何？

主要发现

• 所提方法在812米行驶距离内实现2.2米的累计误差，回环关闭禁用时误差率为0.28%。
• 即使在长时间视觉丢失情况下，系统仍保持高精度和鲁棒性，展现出有效的传感器融合抗干扰能力。
• 轮速里程计预积分技术成功避免因线性化点变化导致的重复积分，提升了优化稳定性。
• 基于轮速里程计和IMU的状态初始化方法可在静态和动态条件下实现快速可靠的收敛。
• 在室内外尺度环境中，该方法在精度和鲁棒性方面均优于单目视觉-惯性SLAM和传统轮速里程计。
• 回环优化有效减少了累计漂移，确保了长序列中全局轨迹的一致性。