Ground-Fusion：一种对Corner-case具有鲁棒性的低成本地面SLAM系统

文章：Ground-Fusion: A Low-cost Ground SLAM System Robust to Corner Cases

作者：Jie Yin, Ang Li, Wei Xi, Wenxian Yu, and Danping Zou

编辑：点云PCL

代码与数据：https://github.com/SJTUViSYS/Ground-Fusion

摘要

本文介绍了Ground-Fusion，这是一种用于地面车辆的低成本传感器融合同时定位和地图构建（SLAM）系统。该系统具有高效的初始化、有效的传感器异常检测和处理、实时的稠密彩色建图，以及在不同环境中具有鲁棒性的定位能力。我们在因子图中紧密集成了RGB-D图像、惯性测量、轮式里程计和GNSS信号，以实现在室内和室外的准确可靠的定位。为了确保成功初始化，我们提出了一个高效的策略，包括三种不同的方法：静止、视觉和动态，以应对各种情况。此外，我们开发了一些机制来检测传感器异常和退化，并灵活处理以保持系统精度。在公共和自采集的数据集上的实验结果表明，Ground-Fusion在Corner-case情况下优于现有的低成本SLAM系统。

主要贡献

本文着重研究了两个方面：鲁棒的系统初始化和Corner-case情况处理，为了确保成功初始化，我们提出了一个有效的策略，包括三种不同的方法：静止、视觉和动态，旨在处理各种情况。此外讨论了可能在Corner-case情况下发生的传感器故障，并相应地处理它们。并进行了大量实验来评估我们的方法。结果显示了我们方法在不同场景中的鲁棒性。本文的主要贡献如下：

• 实现了一种低成本的SLAM系统，紧密耦合了GNSS-RGBD-IMU-Wheel传感器，通过充分利用每个传感器，在各种情况下都能可靠地在室内和室外工作，实现了鲁棒的初始化。
• 提出了有效的策略来检测和处理可能在传感器融合系统中出现的传感器故障，包括视觉失败、轮速异常和GNSS退化，从而极大地增强了鲁棒性。
• 提供了一个用于挑战性Corner-case情况的新的SLAM数据集，作为新的基准。

内容概述

本文的Ground-Fusion系统将RGB图像、深度信息、惯性信息和轮式里程计测量紧密集成在优化框架中。所有传感器测量都在滑动窗口中维护，以实现实时性能。该系统包括自适应初始化、具有Corner-case情况处理的多传感器状态估计器以及稠密建图模块，如图1。

图1. 该系统采用基于机器人运动状态的自适应初始化策略，潜在的传感器故障将被检测并相应地处理。支持实时稠密色彩建图，以便于导航任务。

自适应初始化

自适应初始化模块通过GLRT（ (Generalized Likelihood Ratio Test ）方法来检测足够的运动激励，根据GLRT值将运动状态划分为静止、缓慢运动和剧烈运动三种情况，并提出了三种不同的局部里程计初始化策略：动态方法、视觉方法和静止方法。

静止：在静止情况下，系统采用了三种方法来验证和处理自适应初始化：动态方法，视觉方法和静止方法。系统根据广义似然比检验（GLRT）方法来确定是否存在足够的运动激励，从而将运动状态分为静止、缓慢运动和剧烈运动三种情况。在静止情况下，系统使用轮式和视觉观测来进一步确定车辆是否静止。如果系统满足至少两个静止准则，例如加速度、轮式预积分和视觉视差等，那么车辆被认为是静止的。在确认静止情况下，系统将第一个相机帧建立为本地世界坐标系，并将其z轴与重力方向对齐。然后，所有其他姿态都与第一个姿态对齐，同时速度设置为零。静止检测和零速更新（ZUPT）不仅适用于初始化阶段，还用于整个优化过程中。

缓慢运动：在缓慢运动情况下，系统可以通过解决PnP（透视-多点）问题来计算两帧之间的相机姿态。由于RGBD相机可以直接测量深度信息，因此可以在没有尺度参数的情况下计算IMU姿态。同时，结合IMU预积分项，可以通过最小化一个最小二乘函数来校准陀螺仪偏差。该过程首先获得重力矢量的初始值，然后进行进一步细化。

剧烈运动：在剧烈运动情况下，视觉特征可能不稳定，因此不适合使用视觉SfM进行姿态估计。相比之下，轮式里程计测量使速度和尺度可观察，因此可以通过轮式积分计算姿态。在剧烈运动情况下，系统不使用视觉SfM进行姿态估计，而是采用轮式辅助初始化方法。系统利用轮式里程计姿态替换相机姿态以求解陀螺仪偏差，从而建立一个一致的参考坐标系。相比之下，该方法消除了多余的SfM组件，充分利用了轮式里程计进行更高效的初始化过程。值得注意的是，一旦成功初始化，系统仍然可以将视觉因子集成到紧密耦合的优化过程中。

具备处理特殊情况的多传感器状态估计器模块

该模块旨在提高系统对特殊情况的鲁棒性。该模块分为三个部分：轮速异常、视觉异常和GNSS异常。

轮速异常： 该部分处理轮速里程计的异常情况。通过对轮速里程计测量值进行线性拟合，使用IMU的角速度替代原始的轮速里程计测量值，以消除轮速里程计的误差。同时，通过对当前帧和倒数第二帧之间的IMU和轮速里程计预积分之差的检测，识别轮速的异常情况。在检测到异常情况时，不将当前轮速里程计的观测值纳入后续的优化过程中。

视觉异常： 此部分解决视觉跟踪中可能遇到的异常情况。通过KLT稀疏光流算法对特征点进行跟踪，并采用特征筛选和深度验证策略处理视觉异常。在特征筛选方面，通过光流回溯方法和基于轮速的移动一致性检查（MCC）方法，从当前帧中预先排除潜在的错误跟踪特征。在深度验证方面，将深度测量与特征点关联，并使用三角测量方法计算特征点的深度，并对深度超出有效范围的像素进行处理，确保深度信息的准确性。

GNSS异常： 该部分处理GNSS系统可能遇到的异常情况。主要包括卫星数量不足、无卫星信号和低速移动等情况。通过过滤掉可疑的卫星，限制可靠卫星数量以及在低速移动情况下不考虑GNSS因素，来应对这些异常情况。

实验

基准测试

定位性能： 这里使用Openloris-Scene数据集进行定位性能测试，该数据集由携带RGBD相机、IMU和轮速里程计的地面机器人收集。在Openloris-Scene数据集的三个场景（Office、Home和Corridor）上对Ground-Fusion系统进行了测试，并与最新的SLAM系统进行了比较。表I显示了Ground-Fusion在这些场景中的良好表现。

初始化性能：在Ground-Challenge数据集中进行了初始化测试，该数据集包含一些在复杂场景中的序列。为评估系统初始化的效率，测量了每个系统完成初始化过程所需的时间，定义为系统接收到第一个观测到的时间戳与第一个输出姿态之间的时间差。在初始化质量方面，我们评估了每个系统在每个序列的最初10秒内的绝对轨迹误差（ATE）RMSE，并选择了Ground-Challenge数据集中一些具有挑战性的序列进行测试。这些序列包括具有光照变化的Office3、在黑暗房间中录制的Darkroom2、在没有纹理的墙前捕获的Wall2等。此外，Motionblur3在剧烈运动时存在严重的相机运动模糊，而Occlusion4涉及严重的相机遮挡。我们在这些具有挑战性的序列上对我们的方法进行了评估。表II的结果显示，我们的方法在初始化的质量和效率方面表现优异。

视觉挑战：这里进一步引入了两个新的序列进行评估：序列Hall1是在一个人员活动频繁的大厅录制的；序列Rotation3涉及机器人进行旋转而几乎不进行平移，这可能会影响视觉前端的三角测量过程。在上述序列上评估了最先进的SLAM系统以及我们的方法。评估的算法包括VINS-Mono、VINS-RGBD、VIW-Fusion 3 和 TartanVO （基于学习的VO）。

此外，我们实现了一种基于ESKF的IMU-轮速融合测距作为没有视觉输入的基线。结果显示在表III中，并且一些示例序列的轨迹可见于图2。总的来说，Ground-Fusion在所有测试序列中都取得了最佳的定位结果。

图2，部分样本序列上的估计轨迹和地面实况（GT）轨迹在x-y平面上可视化

为了说明视觉挑战，我们在图3中绘制了每种方法的相对姿态误差（RPE）以及随时间变化的有效视觉特征点数量。

图3，绘制了每种方法的相对姿态误差（m）和一些视觉挑战序列上有效特征点的数量随时间的变化

轮速里程计挑战：在复杂地形和打滑路面的情况下，其他SLAM系统可能受到影响，而我们的方法通过有效地融合IMU和轮速里程计数据，在这些挑战性序列中取得了最佳性能。我们的方法还能够有效地检测和处理轮速异常情况，如在Anomaly序列中，成功消除了错误的轮速里程计读数，从而提高了定位的准确性。表III显示我们的方法在所有这些序列中都取得了最佳的性能，这里进一步进行了消融测试来验证将IMU角速度替代轮速角速度的效果，选择了两个具有急转弯的序列，包括corridor1和loop2，表IV中的结果表明，IMU-里程计测量对于更好的定位精度有所贡献。

如图4（a）所示，在20秒到40秒之间明显出现了轮速异常。本文的方法在这里熟练地消除了错误的轮速里程计读数。图4（b）显示只有本文的方法与地面真值轨迹匹配得很好。

图4. (a) Anomaly序列中轮速异常分析和 (b) 不同方法的轨迹

户外实验

作者构建了一个地面机器人进行数据收集，所有传感器都经过了良好的同步和校准，记录了一些不同场景下的序列，并在本文中选择了三个最具挑战性的序列：

• 在序列 Lowspeed 中，地面车辆以较低的速度行驶并多次停下；
• Sequence Tree 在茂密的树木覆盖下行驶，导致GNSS卫星的遮挡；
• 在序列 Switch 中，车辆从室外转移到室内，然后再次返回室外。

GNSS挑战：将我们的方法与基线方法在GNSS挑战下进行评估，其定位结果显示在表V中。总体而言，Ground-Fusion 在所有这些情况下都表现优异。

在 Lowspeed 中，当机器人静止时，GVINS 由于多普勒噪声而定位失败，而 VINS-GW 也会出现漂移，而 Ground-Fusion 则通过删除不可靠的GNSS测量值稳健地工作。如图5(a)所示，在 Switch 中，由于松散耦合的GNSS信号随着机器人接近室内区域而显著恶化，VINSGW 出现严重漂移，而 GVINS 和我们的方法则不受影响，因为它们进行了紧密耦合的集成。在 Sequence Tree 中，由于不稳定的视觉特征，GVINS 出现故障，而我们的方法由于紧密耦合的轮速和深度测量而保持稳健。

图5. (a) 轮速异常序列上SLAM系统的ATE RMSE（米） (b) 实线表示每种方法的值，虚线表示它们对应的阈值。灰色阴影表示至少满足两个静止条件的区域

零速度更新（ZUPT）：我们在图5(b)中使用Lowspeed序列的GT距离可视化了三种静止检测方法。图表显示，单一传感器可能会误分类运动状态。例如，在大约110秒时，轮速方法未能检测到静止状态。相比之下，我们的方案结合了三种传感器，可靠地检测到静止状态。定量上，在ZUPT后，Lowspeed中的ATE RMSE减少了0.05m。

总结

本文提出了一种紧密耦合的RGBD-轮速-IMU-GNSS SLAM系统，以实现地面车辆的可靠定位。该系统通过三种策略实现了稳健的初始化。我们还设计了有效的异常检测和处理方法来解决Corner-case情况，实验结果表明了我们系统的优越性。