RF-LIO: 面向高动态场景的紧耦合LiDAR惯导融合里程计
单位：西安交通大学
针对问题：
实际场景中动态因素的引入造成基于静态假设的LIO严重位姿漂移
提出方法：
提出基于自适应的多分辨率Range Image的动态点移除算法，并使用紧耦合的激光雷达惯导里程计，首先去除移动物体，然后将激光雷达扫描与子图相匹配，构建基于优先移除的面向高动态场景的LIO。
达到效果：
在不同动态程度的实际场景数据集中测试了系统的定位精度，RF-LIO的绝对轨迹精度相较于LOAM和LIO-SAM显著提升。所提出基于Range Image的动态点剔除算法可剔除场景中96%以上的动态点。
Abstract
SLAM被认为是智能车辆和移动机器人的一项基本能力。然而，目前大多数的激光雷达SLAM方法都是基于静态环境的假设。因此，在一个有多个移动物体的动态环境中的定位实际上是不可靠的。本文在LIO-SAM的基础上提出了一个动态SLAM框架RF-LIO，它增加了自适应的多分辨率Range Image，并使用紧耦合的激光雷达惯导里程计，首先去除移动物体，然后将激光雷达扫描与子图相匹配。因此，即使在高动态环境中，它也能获得准确的位姿。提出的RF-LIO在我们收集的数据集和开放的Urbanloco数据集上进行了评估。高动态环境下的实验结果表明，与LOAM和LIO-SAM相比，所提出的RF-LIO的绝对轨迹精度可以分别提高90%和70%。RF-LIO是高动态环境中最先进的SLAM系统之一。
Introduction
本文提出了一个新的移除优先的紧耦合激光雷达惯导里程计框架，即RF-LIO，以解决高动态环境下的SLAM问题。移除优先是指所提出的RF-LIO首先移除没有准确位姿的移动物体，然后采用扫描匹配。当新的扫描到达时，RF-LIO不会立即进行扫描匹配以获得准确的姿态，因为它很容易受到动态环境的影响。相反，我们使用紧耦合的IMU里程计来获得一个粗略的初始状态估计。然后，RF-LIO可以通过使用自适应分辨率Range Image来初步去除环境中的移动点。在初步去除移动点后，RF-LIO使用扫描匹配来获得相对更准确的位姿。通过这些迭代去除和扫描匹配的步骤，RF-LIO可以在高动态环境中最终获得准确的位姿。我们使用移动物体的移除率和绝对轨迹精度来评估RF-LIO在自我收集的数据集和开放的Urbanloco数据集上的表现。实验表明，与LOAM和LIO-SAM相比，RF-LIO的平均移动物体去除率为96.1%，RF-LIO的绝对轨迹精度可以分别提高90%和70%。
RF-LIO
1.系统框架
图2显示了RF-LIO的整体框架，它由三个主要模块组成。IMU预积分、特征提取和建图。首先，IMU预积分模块用于推断系统的运动并生成IMU观测。然后，特征提取模块对点云的运动失真进行补偿。边缘和平面特征是通过评估点的粗糙度来提取的。
建图模块是我们提出的方法的关键模块。为了在没有准确位姿的情况下首先实现移动物体的移除，有几个关键步骤。(i) 初始姿态是由IMU测距得到的。然后用IMU预积分和扫描匹配之间的误差来确定初始分辨率（即每个像素对应多少个角度的FOV）。(ii) RF-LIO使用这个初始分辨率，分别从当前激光雷达扫描和相应的子图中构建测距图像。(iii) 通过比较它们的可见度，删除子图的主要移动点。(iv) RF-LIO将激光雷达扫描与子图相匹配，并判断扫描匹配是否收敛。如果是收敛的，在图形优化后，最终的精细分辨率被用来去除当前关键帧中剩余的移动点。否则，将产生一个新的分辨率，并重复步骤（二）、（三）和（四）。
2.IMU预积分和初始位姿
我们首先将世界坐标系表示为W，将与机器人body坐标系重合的IMU坐标系表示为B，系统的状态可以表示为：
当IMU测量值到来时，通过IMU预积分得到从上一个关键帧k获得当前关键帧k 1的初始姿态：
3.IMU预积分误差和初始分辨率
在使用IMU进行航迹推演时，不可避免地会出现与真值之间的偏差，这使得查询的扫描点与对应的地图点存在模糊性。为了解决这个问题，Palazzolo和Stachniss提出了一种基于窗口的方法（即不是像素对像素，而是像素对窗口的比较）。Removert中提出了一种更方便的方法，它使用具有不同分辨率的多个Range Image。然而，Removert使用固定的分辨率，因为它是基于准确的定位信息。但是RF-LIO需要在扫描匹配之前去除动态点（即没有准确的位姿）。因此，我们使用IMU预积分和扫描匹配之间的位姿误差来动态地生成初始分辨率。当进行扫描匹配时，IMU预积分的平移和方向误差可以按以下方式计算出来：
通过上式，我们可以得到前一个关键帧k的位姿误差。然而，在扫描匹配之前，用上述方法无法得到当前关键帧k 1的IMU预积分误差。为了得到当前关键帧k 1的误差，我们使用非线性系统的误差传递关系：
我们只对δX的δθ和δp感兴趣，因此我们的方法可以写成如下形式：
通过预测IMU里程计的定位误差，我们可以得到Range Image的初始分辨率，我们使用以下经验公式将平移和方向误差转换为分辨率。
其中，α是一个介于0和1之间的值，用于平衡转换误差和方向误差对分辨率的贡献。
为了平衡实时性能和去除率，并避免错误的预测分辨率造成的影响，我们设定了一个最小的分辨率r0，并适当扩大了预测的分辨率r，最终的初始分辨率rf定义如下：
4.Range Image 构建以及动态点剔除
我们首先将定义为当前的关键帧扫描，将定义为相应的子图，即围绕用滑动窗口法创建的点云图。此外，为了平衡移动点的去除率和实时性能，我们使用完整的查询扫描来与特征子图进行比较。这是因为具有多个关键帧的特征子图具有与完整查询扫描相似的密度，并且比完整子图的点数少。然后，我们将点云分为两个互斥的子集：动态点集，和静态点集。形式上，上述问题表示为：
这里D和S的交集为空集。
其中γ是相对于点的距离的灵敏度。我们建议将γ设置为图4(c)所示的最终分辨率的最大值，这样可以有效避免因预测分辨率错误而将静态点误认为动态点。
5.雷达里程计和新分辨率构建
当扫描匹配是收敛的，我们可以得到两个关键帧k和k 1之间的相对变换。所以激光雷达的里程计可以写成
然而，在一个有多个移动物体的高动态环境中，激光雷达的里程计会发生漂移。如图2所示，我们已经在III-C和III-D部分的步骤中初步消除了移动点，但它仍然不能保证扫描匹配的可靠性。因此，我们需要判断其收敛性。我们描述并实现了一种简单但有效的方法，它是由基于欧式距离的边缘点Fek 1判断的。它可以自然地嵌入到我们的扫描匹配方法中，而无需额外的计算。此外，与使用所有的扫描点相比，边缘点的稀疏性可以确保该方法足够稳健。我们还注意到，我们的框架可以与其他方法兼容，如[22]和[23]，但这些方法需要其他时间来计算。我们的实验表明，我们的方法是足够有效的。它被总结在Alg. 1.
EXPERIMENTS
1.实验设置
我们通过一系列的实验来评估我们的RF-LIO，并将其与LOAM[1]和LIO-SAM[2]进行比较。我们注意到RF-LIO和LIO-SAM都使用相同的特征提取和闭环检测方法，并且都使用GTSAM[24]来优化因子图。用LIO-SAM进行的消融实验显示了RFLIO中移除优先的效果。在所有的实验中，RF-LIO使用了表I中所示的相同参数。所有的方法都是用C 实现的，并在一台装有英特尔i7-10700k CPU的计算机上执行，使用Ubuntu Linux的机器人操作系统（ROS）。为了验证，我们使用了我们录制的数据集和开放的UrbanLoco数据集[7]，其中UrbanLoco数据集包含大量的移动物体。这些数据集的细节显示在表二中。对于有少量移动物体的数据集，我们将其定义为低动态数据集。对于有大量移动物体的数据集，我们将其定义为高动态数据集。而中等动态数据集则介于低动态数据集和高动态数据集之间。所有的方法都只使用激光雷达和IMU不使用GPS，GPS数据只作为真值。
2.初始分辨率设置
设置正确的Range Image的初始分辨率可以有效地去除移动的点，避免去除固定点的错误。因此，本实验旨在证明第二节IC中提出的方法的正确性。从图4(b)我们可以看出，预测的方向误差与真实的误差有很好的一致性。由于平移是加速度的双积分，预测的平移误差不如预测的方向误差准确。图4(a)显示，预测的平移误差与真实平移误差之间的偏差总是在可接受的范围内。如图4(c)所示，预测分辨率与真实分辨率基本一致，而最终的分辨率总是大于真实分辨率。这使得我们的方法可以有效减少识别移动点的错误。
3.动态物体移除测试
在这个测试中，移动点的移除率被用来评估RF-LIO。为此，我们收集了三个在不同环境中有多个移动物体的数据集。图5(a)是有许多移动车辆的郊区数据集的快照。RF-LIO与LIO-SAM有相同的特征提取方法。因此，我们用LIO SAM作为评价标准来评估RF-LIO的移动点去除率。从LIO-SAM和RF-LIO得到的地图分别显示在图5(b)和5(c)。如图所示，LIO-SAM在点云图中渲染了大量的鬼影。与之相比，RF-LIO可以得到一个更干净的地图。我们分别计算LIO-SAM和RF-LIO地图中残留的移动点的数量，然后计算去除率。结果如表三所示。我们可以看到，与LIO-SAM相比，RF-LIO的平均去除率达到了96.1%。我们注意到，RF-LIO并没有完全去除所有的移动点。因为有些移动点离地面太近（距离<0.5，小于我们设定的阈值），还有一些是由平行于地面的激光雷达光束产生的，这使得我们很难在子图中找到相应的远点来去除它们。
4.不同动态程度数据集实验结果
在这个实验中，我们使用了三个自己收集的数据集，其中只有少数移动物体。由于LOAM和LIO SAM是在静态环境下设计的，我们与它们进行比较，以显示RF-LIO在一般情况下的性能。
城市数据集包括各种各样的城市地形：住宅区、立交桥、建筑区等。图6显示了RF-LIO的细节和最终的点云地图。为了直观的显示，RF-LIO的地图被叠加在卫星图像上。校园数据集是从西安交通大学校园内收集的，有多个行人。如图5(a)所示，郊区的数据集包含各种移动的车辆。为了说明在扫描匹配前去除移动点的好处，我们将RF-LIO分为三种不同的类型。当先去除移动点，后进行扫描匹配时，我们的方法被称为RF-LIO（第一种）。同样地，当移动点在扫描匹配之后被移除时，我们的方法被称为RF-LIO（After）。当首先移除移动点，然后进行扫描匹配，最后再次移除移动点时，我们的方法被称为RF-LIO（FA）。所有方法的绝对轨迹误差如表四所示。LOAM在所有三个数据集上的表现都不理想，因此这里我们只与LIO-SAM进行比较。从结果可以看出，RF-LIO(After)和LIO-SAM有相似的性能，而RFLIO(First)和RF-LIO(FA)，使用移除优先，有明显的改善。与LIO-SAM相比，RF-LIO（FA）分别提高了36.7%、3%和18.3%的绝对轨迹精度。
5.高动态场景实验结果
这个实验旨在证明我们的方法在一个具有挑战性的高动态环境中的优越性。我们选择开放的UrbanLoco数据集作为测试数据集，该数据集是在高度城市化的场景中收集的，有许多移动的物体。一些测试环境的截图如图7所示。在这个实验中，当特征点落在移动物体上时，图8(b)所示的LOAM图在多个位置出现了分歧。LIO-SAM在这个测试中的表现优于LOAM，其地图如图8(c)所示。与LIO-SAM相比，RF-LIO的漂移较小，由于去除移动物体，闭环检测性能更好。所有方法的绝对轨迹误差如下表所示：
在高动态数据集中，RF-LIO对SLAM性能的提高有更突出的作用。与LIO-SAM相比，RF-LIO（后）分别提高了61.6%和65.1%的绝对轨迹精度。实验结果表明，我们的移动物体去除方法可以有效地去除动态特征点，提高SLAM性能。在这两个数据集上，RFLIO（First）比RF-LIO（After）分别提高了34.0%和3.6%。这些结果与低度和中度动态数据集上的结果一致，但更加显著。所以我们可以得出结论，移除优先是另一种有效的方法来提高动态环境下的SLAM性能。由于移除优先删除了大部分动态点，RF-LIO（First）和RF-LIO（FA）的轨迹误差没有明显差异。总之，RF-LIO（FA）在所有方法中取得了最好的结果。
在实际应用中，实时性能是评估SLAM系统的另一个关键指标。我们测试了RF-LIO（后）、RF-LIO（先）和RF-LIO（FA）在所有五个数据集上的运行时间。结果显示在表六中。我们看到，RF-LIO（FA）的运行时间在低动态环境中小于100毫秒，而在高动态环境中，它也小于121毫秒。此外，我们注意到RF-LIO（First）的运行时间明显小于RF-LIO（After），甚至RF-LIO（FA）的运行时间也小于RF-LIO（After）。实验结果表明，移除优先需要额外的时间来去除移动物体，但干净的点云可以减少扫描匹配的时间。
虽然移除-后也会移除移动的点，但此时扫描匹配已经完成。因此，其效果不如移除优先好。
CONCLUSIONS
我们提出RF-LIO，以在高动态环境中进行实时和鲁棒的状态估计和建图。RFLIO采用移动物体去除优先算法与紧耦合的LIO相结合，解决了在高动态环境下先去除动态点或先扫描匹配的问题。所提出的自适应Range Image动态点去除算法不依赖于任何先前的训练数据，也不受移动物体的类别和数量的限制。因此，RF-LIO可以稳健地应用于各种场景。
然而，RF-LIO仍有一些正在进行的工作。在一个非常开放的环境中，如果周围环境中没有相应的远点，基于可见度的范围图像方法就不能去除移动点。另一个问题是，当移动物体完全阻挡了我们的传感器的FOV时，该方法不适合。
备注：作者也是我们「3D视觉从入门到精通」特邀嘉宾：一个超干货的3D视觉学习社区
本文仅做学术分享，如有侵权，请联系删文。
