GR-Fusion：强鲁棒低漂移的多传感器融合系统（IROS2021）

来源：GR-Fusion Multi-Sensor Fusion SLAM for Ground Robots with High Robustness and Low Drift IROS 2021单位：中科院沈阳自动化研究所

作者：Maple|来源微信公众号：3D视觉工坊

针对问题：针对地面机器人的多传感器融合SLAM系统

提出方法：系统融合紧耦合IMU、轮速计、视觉、LiDAR和GNSS多源观测信息，并结合地面机器人平面运动约束，构建了融合多模态观测信息的因子图优化模型，针对各种传感器退化场景提出了有效的运动退化检测方法，使得系统能够在复杂环境中仍保持较强的鲁棒性和较低的漂移。

达到效果：所提出系统在室内数据集、地下数据集、开放式草坪数据集以及校园场景数据集中进行了广泛测试，与现有方案对比达到了最先进的效果。

数据集开源地址https://drive.google.com/drive/folders/110Hko3z PcDmY0_bnZdXxJXJKe6wr3t10?usp=sharing

Abstract

本文提出了一个紧耦合LiDAR、相机、IMU、轮速计和GNSS信息的SLAM系统。系统通过将LiDAR点云和地面投影到图像中恢复视觉特征深度，选择跟踪的高质量视觉特征和LiDAR特征，并紧耦合IMU和轮速计的预积分值来优化机器人的状态增量。我们使用估计的相对位姿来重新度量局部滑窗中特征之间的关联距离，并去除动态物体和异常值。在建图节点中，我们使用精炼的特征，并紧耦合GNSS测量值、增量因子和局部平面约束，通过将LiDAR特征与全局地图对齐，进一步优化机器人的全局状态。此外，该方法可以检测传感器的退化情况并自动重新配置优化过程。系统基于一个六轮地面机器人在室内和室外环境中进行了广泛的实验，证明了所提出的GR-Fusion在精度和鲁棒性方面优于最先进的SLAM方法。

Introduction主要贡献：

本文提出了一个可以融合来自LiDAR、相机、IMU、轮速计、GNSS和局部平面约束的多模态观测的框架，用于机器人状态估计；
系统将LiDAR点云和地面被投影到图像中，以提取视觉特征的深度。该方法可以检测每个传感器的退化情况，并重新配置优化过程，以应对各种具有挑战性的情况；
所构建系统在真实的地面机器人上进行了广泛的实验，表明GR-Fusion具有高鲁棒性和低漂移性。

The proposed GR-Fusion1.系统概述

所提方法的流程图如上图所示，系统包含四个节点。每个传感器的测量量分别为：轮速计100HZ，IMU 100HZ，相机30HZ，LiDAR 10HZ，GNSS 5HZ。测量预处理节点首先进行数据缓存和同步。然后根据时间戳对轮速计和IMU的测量进行预积分，以消除LiDAR点云的运动失真。根据我们之前研究中提出的地面测量模型，提取地面点，并拟合地平面参数。然后将计算出的局部平面和LiDAR点云投影到图像中，计算出视觉特征的深度。里程计节点通过紧耦合多模态的局部约束因素来估计机器人在滑动窗口中的运动增量。建图节点将LiDAR特征与全局地图对齐，并将本地约束和GNSS约束紧耦合起来，以优化机器人的全局状态。闭环节点接收来自GR-mapping的位姿和特征，并进行闭环检测和优化以消除漂移。

2.Measurement pre-processing 测量数据预处理(1)轮速计IMU预积分

我们使用了[10]中提出的流形上的轮速计增量模型，它可以融合轮速计和IMU的测量值，计算机器人在复杂地形（如斜坡、楼梯和草坪）上的三维位置增量，如下图所示。接下来，轮速计和IMU的测量结果根据LiDAR和图像的时间戳进行预积分，以计算帧之间的位姿增量。在正常情况下，位置增量由轮速计提供，旋转增量由IMU提供。如果检测到机器人的车轮在打滑，则由IMU提供全部增量。

（2）LiDAR和相机观测预处理

首先，根据帧间的运动增量，去除点云的运动失真。为了去除噪声和不稳定的点，我们按照[13]提出的方法进行点云分割。根据粗糙度提取了角和平面特征。由于机器人总是在地面上运行，我们提出使用局部平面约束来优化机器人的位姿。我们使用之前研究中提出的地面测量模型来提取地面点，之后我们拟合地面平面参数来计算当前可见的地面区域。对于来自双目相机的图像，进行角点检测。通过设置两个相邻特征之间的最小像素间隔，实现了特征的均匀分布。这些特征使用KLT光流算法进行追踪，同时我们还使用KLT跟踪器来匹配左右图像之间的特征。在大规模环境中，由于基线较小，使用视觉特征的三角测量法得到的深度有很大的误差。因此，我们通过[20]介绍的方法，将LiDAR点云投射到图像中，以提取视觉特征的更鲁棒的深度。地面点被投影到图像中，以分割地面上的视觉特征。相机投影模型被用来计算地面特征的深度。实验发现，位于地面的视觉特征移动速度快，跟踪时间短，往往无法通过三角测量法获得准确的深度。因此，利用LiDAR拟合的地面参数可以直接计算出地面特征的深度。此外，它还隐式地提供了地面视觉特征的共面约束。

3.GR-odometry里程计节点被设计为选择少量但高质量的特征，以快速估计机器人在局部滑窗内的运动增量。同时，优化过程根据传感器的退化情况进行了重新配置。此外，在当前LiDAR帧被发送到建图线程之前，利用里程计的结果对当前帧的特征进行细化调整，位于动态目标上的特征和不稳定的特征点将被剔除。

（1）状态向量本文所提出的方法以LiDAR帧为优化节点，选择最近的图像作为视觉约束。状态向量表示如下：

我们使用IMU时间戳作为基准，将LiDAR和相机的测量时间对齐IMU的时间戳。然后与IMU的观测一起进行优化，以估计传感器之间的时间偏移。

（2）因子图优化模型

我们在里程计部分维护一个滑窗，如上图所示。有n+1个节点，优化窗口包含三个最新的连续节点。其他节点根据移动距离被选为关键帧，并将其保留在滑动窗口中，为优化窗口提供约束。滑动窗口中的LiDAR特征被用来维护本地点云地图，最新的帧被投影到图像中。对于每个视觉特征，相邻的点云块被提取出来并进行深度估计。对于深度已经从地面参数获得的特征点，其深度值保持不变。此外，当从新的图像中提取特征时，在点云覆盖的区域，视觉特征间距被设定为其他区域的一半。这种策略能够从点云覆盖的图像区域中提取更多的特征，而且深度可以直接估算，更加鲁棒。一个特征点被追踪的次数越多，它的质量就越高。因此，我们将跟踪次数超过阈值的点加入到优化中。接下来，根据跟踪结果构建了以下重投影误差约束：

我们将从LiDAR点云中提取的角点和平面特征与当前的局部地图相匹配，并使用点到线和点到面的距离来构建LiDAR约束因子。此外，轮速计和IMU的测量结果通过预积分构建一个增量约束因子。

（3）局部平面约束我们认为，当机器人在地面上移动时，局部平面在两个连续的帧之间是不变的，如果平坦的地面被拟合，那么在局部平面的Z轴上的位移接近于零。为了确保我们的公式的鲁棒性，我们考虑了法向量、地面粗糙度和地面连续性等因素来决定是否添加地面约束。地面的法向量和机器人的Z轴之间的角度必须小于设定的阈值;分割的地面点与拟合的地面之间的距离的平方之和必须小于设定的阈值;最远点和最近点地面之间的距离必须小于设定的阈值。

（4）传感器退化检测在弱纹理环境或黑暗环境中，相机无法提取鲁棒的特征进行跟踪。当场景中存在大量的动态目标时，也会出现较大的跟踪误差。首先，我们统计了当前帧中每个特征的跟踪时间，如果小于设定的阈值，就降低视觉特征的权重。如果所有特征的跟踪时间之和小于设定的阈值，则被认为是不稳定的，这时，视觉约束将不被用于后续的优化过程。LiDAR在高度重复的场景中会退化，例如走廊或开放的户外环境。因此，我们区分了最新一帧点云的深度和优化窗口外的第一帧点云的深度，并计算出平均深度差。接下来，我们得到了所有点深度差的直方图统计。如果深度差大于阈值点的数量很少，则LiDAR被认为是退化的。这时，LiDAR约束将不会被添加到优化中，优化滑窗的大小也被调整。该系统退化为GR-SLAM，并将相机、轮速计和IMU紧耦合在一起进行状态估计。然而，局部的点云仍然被维护，建图节点仍然运行以维护全局地图。由于IMU是一个内置的传感器，不受外部环境的影响，在短时间内的估计结果是相对准确的。机器人车轮的滑动不可避免地增加了轮速计的预积分值。因此，我们计算了轮速计的预积分值与IMU的预积分值比率。如果它大于设定的阈值，则认为车轮在打滑，轮速计的约束就不会被添加到优化中。此外，实验结果显示，机器人在转弯时很容易打滑，而轮速计在直线运动时的精度更高。因此，我们也用机器人的转动来调整轮速计的优化权重。

（5）局部因子图优化通过BA优化所有因素的残差，并获得机器人状态的最大后验估计，如下所示。

其中ro(.)代表局部地面约束。值得注意的是，这里优化的是图4所示优化窗口中节点的状态增量。只有当节点滑出优化窗口后，才会被送至建图节点进行全局优化。另外，我们使用GR-odometry的结果来去除动态物体和不稳定的特征。首先，当前的LiDAR特征被投影到局部滑窗的起始帧，以计算匹配距离。对于静态环境，投影的LiDAR点是重合的，或者匹配距离很小。对于移动物体，投影点的匹配距离较大。因此，根据匹配距离对LiDAR特征进行排序，匹配距离超过设定阈值的点被判断为动态物体而被剔除。最大剔除率被设定为10%。这一策略利用了动态物体的运动特征，并使用滑动窗口的时间跨度，这样可以突出动态物体的投影误差。最后，提炼出的特征点将被送到GR-Mapping节点。

4.GR-建图和闭环检测

（1）GNSS 权重尽管我们可以融合多模态的测量来进行机器人的状态估计，但长期运行仍会产生累积误差。GNSS可以提供绝对位置。然而，实验结果显示，当GNSS受到建筑物或树木的阻挡时，GNSS的测量会发生跳跃，导致大的误差甚至失败。因此，我们根据卫星使用数量、精度衰减因子、局部增量方差评估GNSS的质量。搜索的卫星越多，GNSS的测量就越准确。然而，这也与卫星的空间几何分布有关。因此，我们也使用GNSS输出的DOP值来评估测量误差。此外，我们还计算了局部GNSS增量和里程计增量的方差来评价GNSS的测量质量。尽管里程计测量法会产生累积误差，但其局部精度相对较高。因此，里程计可以用来确定GNSS测量是否有跳跃，以及本地测量的质量是否下降，以避免大的误差。

（2）全局因子图优化我们在全局优化中加入的制约因素有里程计增量因子、局部平面约束因子、来自IMU的角度预测因子、GNSS因子、地图因子。闭环因子使用里程计测量的估计结果作为初始值，我们将LiDAR特征与全局地图对齐，同时耦合其他局部约束和全局约束，以优化机器人的全局状态。闭环因子来自于闭环检测节点，我们使用了[21]提出的点云描述符进行位置识别。它还可以保存和重用地图，并快速重定位到当前地图，值得注意的是，我们的方法支持只包含轮速计、IMU和GNSS的低成本配置，并对地面机器人进行有效的状态估计。

Experiments我们在不同的环境中收集了多个数据集。这些数据集分别被称为室内、地下、露天草坪和校园。

1.室内数据集

这个数据集包含了室内走廊环境，以及楼梯。轨迹的起点和终点是重合的，我们用离线批量优化和闭环优化的结果作为真值。结果显示在图6中。机器人在图6(b)中圆圈标记的地方上下楼。由于LiDAR视角的急剧变化，LIO_SAM和LOAM_IMU都产生了很大的误差。LIOM[14]与IMU紧耦合，误差很小。GR_Fusion将摄像头、IMU、轮速计和LiDAR紧耦合在前端，快速估计机器人的状态增量，可以应对观察角度急剧变化的情况。图6(b)底部的曲线是算法在Z轴上的位移。由于GR_Fusion在优化时考虑了局部平面的约束，所以它的精度更高。

所有方法的绝对平移误差见表二。LOAM_IMU是优化的LOAM。我们增加了IMU的局部姿态增量和全局重力约束，以提高其估计的准确性。

2.地下数据集

在这个数据集中，机器人从校园出发，经过地下车库，然后返回地面校园。地下车库的通道有一个旋转通道和一个直线通道，如图7(a)所示。这些通道的环境是高度重复的，造成LiDAR的退化。图7(b)中显示了每种算法的结果。可以看出，LIOM、LIO_SAM和LOAM_IMU由于LiDAR的退化，在地下通道中都有明显的错误。GR_Fusion仍然可以依靠视觉、IMU和轮速计增量模型进行连续状态估计。当GR_Fusion检测到LiDAR的退化时，它将减少LiDAR的优化权重。而在现有状态的基础上，局部滑动窗口的大小将逐渐增加，并包含更多的数据进行优化。在保证位姿不跳跃的同时，保证了状态估计的准确性。

3.开放式草坪数据集

在这个测试中，我们评估了所提方法在户外大规模开放环境中的表现。在开放草坪数据集中，机器人穿越了一个非常开放的草坪，在远处只能看到一些稀疏的树木，如图8所示。在穿越草坪时，从LiDAR中提取的大部分特征都分布在地面上，使运动估计发生退化。从图8(b)中可以看出，LIOM和LOAM_IMU由于LiDAR的退化而产生了明显的误差。因为LIO_SAM融合了GNSS测量，所以估计的全局姿态可以保证一定的精度。但是当我们去掉GNSS测量时，LIO_SAM估计的状态会出现跳变，由此产生的误差比LOAM_IMU更大。这是因为LIO_SAM在估计时使用的特征比LOAM_IMU少。此外，我们发现在室外环境中，从LiDAR中提取的特征相对稀疏，所以与室内环境相比，所评估的其他算法的性能有所下降。GR_Fusion结合了LiDAR、相机、IMU和轮速计的测量结果，以确保无论在室内还是室外环境下都有相同的精度。而通过融合GNSS测量，GR_Fusion可以输出全局无漂移的估算结果。

4.校园数据集

在这个测试中，我们评估了GR_Fusion在不同配置下的性能。GR_Fusion_Local使用LiDAR、照相机、IMU和轮速计作为传感器。GR_Fusion_GNSS进一步融合了GNSS测量。GR_Fusion_Low_Cost只使用低成本的传感器配置，包括GNSS、IMU和轮速计。GR_Fusion_Loop有闭环优化。应该注意的是，在以前的实验中没有使用闭环优化。结果显示在图9中。GR_Fusion_Loop的精度最高，可以消除累积误差。GR_Fusion_Local的精度比GR_Fusion_GNSS的精度高。这是因为在轨迹的某一段中，GNSS的测量质量降低了，这导致了全局优化的误差。然而，GR_Fusion_GNSS在长时间运行时具有更高的精度。GR_Fusion_LowCost只使用IMU和轮速计增量模型进行局部估计，并使用GNSS进行全局状态优化。它的精度很容易受到GNSS测量的影响，但它仍然可以提供可接受的结果，对低成本机器人非常有用。

5.动态物体剔除

我们还测试了环境中出现动态物体时GR_Fusion的建图性能。图10显示了车辆经过时的建图结果。可以看出，其他算法在地图中包括了所有移动车辆的点云。GR_Fusion可以有效地消除动态物体，建立一个更合理的静态环境地图。

此外，我们还对算法的实时性能进行了统计，如下表所示。可以看出，虽然GR_Fusion融合了更多的传感器，但通过对信息的仔细处理和选择，以及工程实施中的有效策略，它可以提供高效和准确的估计结果。

Conclusions And Future work

我们提出了一种多模态的传感器融合方法，可以鲁棒且准确地估计机器人的状态。通过对各传感器数据的精心选择和细化，在提高精度的同时，还可以消除动态目标和不稳定的特征。它可以实时检测传感器的退化情况，并可以灵活地配置为多种工作模式。目前，视觉信息仅在前端用于估计机器人的局部状态。在未来，我们将结合视觉和LiDAR来创建具有全局描述符的地标，以进一步优化机器人的全局状态。

备注：作者也是我们「3D视觉从入门到精通」特邀嘉宾：一个超干货的3D视觉学习社区本文仅做学术分享，如有侵权，请联系删文。

3D视觉精品课程推荐：

1.面向自动驾驶领域的多传感器数据融合技术

2.面向自动驾驶领域的3D点云目标检测全栈学习路线！(单模态+多模态/数据+代码)

3.彻底搞透视觉三维重建：原理剖析、代码讲解、及优化改进

4.国内首个面向工业级实战的点云处理课程

5.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解

6.彻底搞懂视觉-惯性SLAM：基于VINS-Fusion正式开课啦

7.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化

8.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer+LOAM +LIO-SAM)

9.从零搭建一套结构光3D重建系统[理论+源码+实践]

10.单目深度估计方法：算法梳理与代码实现 11.自动驾驶中的深度学习模型部署实战 12.相机模型与标定(单目+双目+鱼眼）