前言

最近看到一篇ICRA2024上的新文章，是关于多传感器融合SLAM的，好像使用了最近几年文章中较火的轮式里程计。感觉这篇文章成果不错，代码和数据集都是开源的，今天仔细读并且翻译一下，理解创新点、感悟研究方向、指导自己的研究。这篇文章通篇略读，主要做了工作做了一个紧耦合的RGBD - Wheel - IMUGNSS SLAM系统，然后加了两个创新点工作，一个是初始化，一个是传感器退化检测。

一、问题背景

室内外定位可靠性不足；基于激光雷达的SLAM成本高，基于VIO的引入不可观测自由度，并且有累计漂移误差。现有SLAM系统在具有挑战性场景中鲁棒性有待提高。总结以前的研究成果，GNSS - RGBD - IMU - Wheel融合SLAM可以充分利用各传感器的互补优势，这篇文章关注系统初始化问题和coner case addressing问题（传感器病态检测）。

多传感器融合SLAM系统初始化方面，以前的各种SLAM系统已经有了很多关于系统初始化的成果。多传感器SLAM系统，特别是那些紧耦合的系统，由于其对系统鲁棒性和精度的深刻影响，严重依赖于高质量的初始化。文章介绍了VINS-Mono等以前的初始化成果并指出这些系统的初始化对于严重的传感器故障仍然不够鲁棒。基于这些见解，引出该文章的一种自适应初始化策略，该策略包含三种不同的方法，适用于不同的场景。

传感器退化方面，按照传感器种类分为视觉退化、轮式里程计退化、GNSS退化、IMU退化。视觉退化分为特征不足问题，通常是由于缺乏纹理或光照不足引起的；由于显著的遮挡或侵略性运动而没有有效的特征点；具有大量移动对象的动态环境。轮式里程计退化：角速度不准确和车轮异常。典型的GNSS挑战有3种：低速运动、少于4颗卫星和无GNSS信号。IMU退化：在该研究中，认为滑动窗口内的短期IMU观测值是可靠的，不存在故障。根据不同传感器退化特点文章做了分析，提出了不同的解决方案。

二、主要架构

地面融合系统在优化框架内将RGB图像、深度信息、惯性信息和轮式里程计测量紧密地集成在一起。所有的感知测量都保持在一个滑动窗口中，以保证实时性。该系统由自适应初始化、带退化处理的多传感器状态估计器和稠密映射模块组成。

三、理论创新

1、自适应初始化

系统根据GLRT (广义似然比检验) [ 25 ]方法判断是否存在足够的运动激励，其形式为：

GLRT值将运动状态大致分为以下3类，其中β和γ的阈值通过实验方法确定：

（1）Stationary

我们引入了车轮和视觉观测，进一步确保系统是否是静态的。我们使用轮子中值积分法来预测位姿：

假设连续图像ck和ck + 1之间存在n个里程计帧，则它们之间车轮预积分位姿的范数可以表示为：

此外，我们可以从最新的帧中提取特征点，并将其与滑动窗口中的图像进行匹配。然后，平均视觉视差可以表示为：

在初始化阶段N，如果至少有两个平稳准则{ G < β，W < η，V < θ }满足(实验方法确定的所有阈值)，则认为车辆是静态的。否则，我们将车辆视为运动，并使用下段中的方法进行初始化。在确认静止的情况下，我们建立第一个相机框架作为局部世界框架，并将z轴与重力方向对齐。随后，滑动窗口内的所有其他位姿与第一个位姿对齐，而速度设置为零。在优化过程中将系统状态p，v，q设置为常数块。平稳检测和ZUPT不仅适用于初始化阶段，而且应用于整个优化过程。

（2）Slow Motion

在慢速运动情况下，通过求解一个PnP ( Perspective-n-Point )问题，可以计算出两帧之间的相机位姿p，q。由于RGBD相机可以直接测量深度信息，因此可以在没有尺度参数的情况下计算IMU位姿：

结合上述状态和IMU预积分项γ，我们可以通过最小化跟随最小二乘函数来校准陀螺仪偏差：

考虑两个连续的IMU帧bk和bk + 1，我们有如下方程

结合这个和该小节第一个方程，我们可以求解XI的初值。最后，对上一步线性初始化步骤得到的重力向量进一步细化。

（3）Aggressive Motion

采用轮式辅助初始化方法。为了建立一个一致的参考框架，我们使用第一个轮子框架定义世界框架，并将其z轴与轮子框架的z轴对齐。与文献[ 13 ]中使用轮式里程计进行尺度精化的方法相比，我们的方法消除了冗余的SfM分量，充分利用了轮式里程计进行更有效的初始化过程。值得注意的是，虽然这种初始化方法不依赖于视觉信息，但一旦初始化成功，在系统识别有效特征点时，视觉因素仍然可以融入紧耦合优化过程。在使用上述三种方法中的任何一种成功地进行局部初始化之后，我们执行了三步的全局初始化，这是从[ 8 ]中改编的。

2、带退化处理的多传感器状态估计器

我们将状态估计建模为一个最大化后验概率( MAP )问题。我们沿用[ 5 ]的因子图框架，保持滑动窗口，进一步扩展到GNSS - RGBD - IMU - Wheel融合系统。残差和Jacobi的计算可以参考之前的文献[ 8 ] [ 4 ] [ 5 ]。接下来，我们主要介绍了我们的系统如何处理传感器测量数据，以使其对角落情况更加鲁棒

（1）车轮异常

轮式里程计的误差主要来源于不准确的角速度估计和突然的底盘异常，如车轮打滑和碰撞。由于IMU的角速度测量比轮式里程计更可靠，且具有更高的帧频，因此我们采用线性拟合的方法将原有的轮式里程计测量替换为IMU角速度

为了检测车轮异常，我们比较了当前帧和第二个最新帧之间IMU和轮式里程计测量值的预积分。如果它们产生的姿态范数的差异超过阈值ε = 0.015，我们将其视为车轮异常。在这种情况下，我们避免将当前的轮式里程计观测值纳入到后续的优化过程中。

（2）视觉异常

我们的系统采用KLT稀疏光流算法[ 26 ]来跟踪由[ 12 ]改编的特征点。三个视觉挑战包括初始化过程中的无效特征问题，定位和动态环境中的特征不足问题。第一个问题已经在Sec III ( a )中解决，而第二个问题可以通过紧耦合的轮式里程计和IMU数据来缓解。为了解决动态物体，我们进一步引入了两种策略：特征过滤和深度验证

对于特征过滤，我们首先采用流回溯的方法，通过反转两帧的顺序进行光流回溯。只有在两次迭代过程中成功跟踪并显示出低于指定阈值的相邻距离的特征点被保留用于进一步处理。此外，我们还介绍了一种车轮辅助的移动一致性检查( MCC )方法。我们的系统利用了车轮预积分位姿和之前优化的位姿。对于在滑动窗口内的第i幅图像中首次观测到，随后在其他m幅图像中观测到的特征，我们定义特征观测值的平均重投影残差rk为：

对于深度验证，我们首先将从深度相机获取的深度测量值与代表特征点的每个像素相关联。在深度测量超出深度相机有效范围的情况下，像素暂时留空。随后，我们在RGB图像上使用三角测量法计算特征点的深度，从而填充所有的像素深度。此外，对于那些由深度相机测量的深度和通过三角剖分计算的深度之间的视差低于预定义的阈值的特征点，我们记录它们的索引，并在优化阶段将它们的深度值固定为恒定值。

（3）GNSS异常

3种GNSS挑战场景包括卫星过少、无卫星信号和低速运动。在前两种情况下，[ 8 ]已经证明了在GNSS - Visual - Inertial紧耦合框架的帮助下，有限的可靠卫星仍然可以有效地改善全局状态估计，并且当没有观测到GNSS信号时，GVIO系统将退化为VIO系统。在这项工作中，我们的系统首先过滤了伪距和多普勒不确定性过大的不可靠卫星，跟踪次数不足的卫星和低仰角的卫星。在GNSS接收机速度低于阈值vths = 0.3m / s (多普勒频移的噪声水平)的低速场景中，我们在优化中不涉及GNSS因素，以防止GNSS噪声。

四、实验方案

1、定位表现

Openloris-Scene [ 24 ]是一个由地面机器人结合RGBD相机、IMU和轮式里程计采集的SLAM数据集。在Openloris - Scene [ 24 ]的3个场景下，即Office ( 7 seqs )、Home ( 5 seqs )和Louad ( 3 seqs )，对Ground - Fusion与前沿SLAM系统进行了测试。表1显示，地面融合在这些场景中表现良好。

2、初始化表现

在地面挑战数据集[ 11 ]上进行初始化测试，在角点情况下使用复杂序列2。为了评估系统初始化的效率，我们测量每个系统完成初始化过程所需的时间，定义为系统接收到的第1次观测的时间戳与第1次输出位姿的时间戳之差。

3、退化表现

（1）视觉挑战

为了说明视觉挑战，我们在图3中绘制了每种方法的相对位姿误差( RPE )和有效视觉特征点的数量随时间的变化。结果表明，特征点不足会极大地降低视觉前端的性能。例如，在Sequence O f ice3中的25秒，特征点数量由于丢失而突然下降为零，导致VINSMono [ 12 ]和VINS - RGBD [ 4 ]的RPE显著增加。类似地，VINS - Mono在纯旋转(旋转3 )过程中挣扎着通过三角测量来估计深度，产生了明显的漂移。在这样的场景下，由于采用了紧耦合的轮式里程计，我们的系统仍然表现良好。在没有观测到有效特征点的Occlusion4中，包括VIW - Fusion在内的大多数系统初始化失败。相比之下，我们的系统使用轮式辅助动态方法初始化，并且在定位精度方面优于轮式IMU融合ESKF基线

（2）轮式里程计挑战

序列廊道1是一条长廊道中的曲折路线。表III表明，我们的方法在所有这些序列中都取得了最好的性能。我们进一步进行了烧蚀试验，验证了IMU角速度作为车轮角速度替代物的效果。我们选取了两条急转弯的序列，包括走廊1和环路2。表4中的结果表明，IMU -里程计测量有助于获得更好的定位精度。

此外，我们在具有轮异常的序列上测试了我们的方法。在Anomaly序列中，机器人本体随着其下方的地毯被拉开而移动，而机器人轮子不移动。相反，在静态序列中，机器人是悬浮的，因此即使在车轮移动时，机器人本体也不会移动。

（3）GNSS挑战

我们在大规模室外环境中进一步评估了我们的方法，具体如下：我们建立了一个地面机器人用于数据收集，所有的传感器具有良好的同步性和校准。我们记录了各种场景下的一些序列4，并在本文中选择了三个最具挑战性的序列：在序列Lowspeed中，地面车辆低速移动并做了数次停车；序列树在茂密的树木覆盖下，导致GNSS卫星被遮挡；在序列切换中，车辆从室外过渡到室内，然后再次返回室外。

我们在GNSS挑战基线方法的情况下评估了我们的方法，其定位结果如表5所示。

（4）零速校正

我们在图5 ( b )中用GT距离在低速序列上对三种平稳检测方法进行了可视化。从图中可以看出，单个传感器可能会对运动状态进行误分类。例如，车轮方法在大约110秒时无法检测到静止状态。相比之下，我们的方案结合了三个传感器，实现了对静止状态的可靠检测。定量上，ZUPT后，低速时的ATE RMSE降低了0.05 m。

五、创新总结

为了确保成功的初始化，提出了一种有效的策略，包括三种不同的方法：静态，视觉和动态，它们是为了处理各种情况而设计的。此外还讨论了拐角情况下可能出现的传感器故障[ 11 ]，并进行了相应的处理。进行了大量的实验来评估我们的方法。结果表明了方法在不同场景下的鲁棒性。

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

这篇文章脉络比较清晰，一个系统框架，两个主要改进创新点，并且代码开源，研究思路值得我去借鉴学习。什么是创新？针对问题提出解决方案。