提高极端环境下SLAM精度的研究：LP-ICP方法

文章：LP-ICP: General Localizability-Aware Point Cloud Registration for Robust Localization in Extreme Unstructured Environments

作者：Haosong Yue, Qingyuan Xu, Fei Chen, Jia Pan, and Weihai Chen

编辑：点云PCL

欢迎各位加入知识星球，获取PDF论文，欢迎转发朋友圈。文章仅做学术分享，如有侵权联系删文。

公众号致力于点云处理，SLAM，三维视觉，高精地图等领域相关内容的干货分享，欢迎各位加入，有兴趣的可联系dianyunpcl@163.com。文章未申请原创，未经过本人允许请勿转载

摘要

迭代最近点（ICP）算法是基于LiDAR的SLAM算法的关键组成部分。然而，在缺乏特征和几何结构的非结构化环境中，其性能可能受到负面影响，导致定位和地图构建的准确性低且鲁棒性差。众所周知，由于缺乏几何约束，退化会导致在条件较差的方向上出现6自由度姿态估计误差。因此，需要一种更广泛、更细致的退化检测与处理方法。本文提出了一种新的点云配准框架——LP-ICP，该框架结合了ICP算法中的点到线和点到平面距离度量，并进行了局部化感知检测与处理。LP-ICP包括一个局部化感知检测模块和一个优化模块。局部化感知检测模块通过利用扫描与地图之间边缘点（低局部平滑度）与线段、以及平面点（高局部平滑度）与平面之间的对应关系进行局部化感知分析。单个对应约束的局部化感知贡献可以应用于更广泛的范围。优化模块根据局部化感知类别向优化方程中添加额外的软约束和硬约束。这使得姿态能够在条件较差的方向上受到约束，更新要么朝着约束值趋近，要么保持初始估计不变，从而提高了准确性并减少了波动。通过在仿真数据集和真实世界数据集上的广泛实验评估，验证了该方法的准确性优于或与现有最先进方法相当。本文的数据集和代码也将开源

https://github.com/xuqingyuan2000/LP-ICP。

图 1. 典型的退化环境：来自PLAM 数据集的类行星环境和来自 CERBERUSDARPA 地下挑战数据集的地下隧道环境

主要贡献

本文提出了一种新的局部化感知点云配准框架—LP-ICP，旨在解决LiDAR退化问题。该框架设计用于提高极端非结构化环境中SLAM系统的定位和地图构建准确性。所提出的框架如图2所示。具体来说LP-ICP是一个结合了点到线和点到平面距离度量的ICP点云配准框架，并具有局部化感知检测与处理功能。单个对应约束的局部化感知贡献的计算公式可以应用于其他变种的ICP算法，或者更广泛地应用于通过高斯-牛顿法优化的姿态估计问题，如通过最小化基于视觉的方法中的重投影误差来估计姿态。因此，它有潜力扩展到多传感器融合框架。LP-ICP由两个模块组成：局部感知检测模块（LocDetect-Module）和带有软硬约束的优化模块（Optimization-Module）。

在扫描到地图的配准过程中，LocDetect-Module利用边缘点与线、平面点与平面之间的对应关系，分析在优化的特征空间主方向上的贡献强度。然后，我们过滤掉噪声，挑选出具有高局部化感知贡献的部分。接着，通过三值阈值判断评估每个6-DoF位姿方向的局部化感知情况，并将其分类为：完全可局部化、部分可局部化和不可局部化。

优化模块首先通过利用几何约束和具有高局部化感知贡献的对应关系，计算额外的约束。然后根据LocDetect-Module中的局部化感知类别，向优化方程中添加软硬约束。这确保在不可局部化的方向上，姿态不进行更新，保持初始估计不变。在部分可局部化方向上，姿态会在额外约束下进行更新，趋向于约束值，而不是严格等于约束值。这减少了大幅波动和漂移的风险，避免了可能导致定位失败的情况。对于完全可局部化的方向，位姿更新不受影响。目标是有效利用更多几何约束信息，同时确保姿态更新相对稳定。此外，为了测试该算法在典型极端非结构化环境中的性能，我们收集了一个模拟的SLAM数据集，其中包含来自视觉真实的月球类环境中的LiDAR、IMU和摄像头数据。该数据集包含10个序列，总轨迹长度为10公里，可用于测试SLAM框架的各个组件以及在行星探索中的不同任务。

所提出的框架已在多个实验中进行了测试，涵盖了具有挑战性的仿真数据集和真实数据集。实验结果表明LP-ICP在极端非结构化环境中表现良好，能够提高机器人在退化环境中的定位能力和地图构建准确性。与现有最先进的方法相比，所提出的框架在所有实验中都取得了更高或相当的准确性。本文的贡献如下：

• 提出了一种新的ICP算法框架，具有局部化感知检测和处理功能，利用点到线和点到平面的局部化感知信息，应用于检测和处理极端非结构化环境中的退化问题。
• 提出了一种通用的计算单个对应约束局部化感知贡献的方法，适用于通过高斯-牛顿法优化的姿态估计问题。
• 开发了一个结合软硬约束的ICP优化模块，利用局部化感知信息来处理退化问题。
• 收集了一个多传感器模拟的SLAM数据集，来自视觉真实的月球类环境，供典型极端退化环境中的SLAM测试使用。
• 与现有最先进的方法进行比较，经过多项实验验证了所提出框架的有效性。

主要内容

基于优化的点云配准

在点云配准的过程中，通常会使用基于优化的算法来求解一个状态估计问题。具体而言，这个问题的目标是通过最小化某个代价函数，求出一个最优的位姿变换。位姿变换是指将源点云（当前的扫描数据）与目标点云（地图数据）之间的关系通过旋转和平移的变换进行对齐，从而估计出机器人的姿态（位置和方向）。在优化过程中，代价函数的每一项表示源点云和目标点云之间某个匹配点对的“残差”，即它们之间的差距。通过对所有匹配点的残差进行求和，优化算法的目标就是找到一组旋转和位移，使得所有点之间的差距最小。具体来说，这种优化过程不仅考虑点与点之间的差距，还结合了点到线和点到平面之间的距离度量，通常采用“点到线”和“点到平面”的配准方法。

点到线和点到平面的方法是通过对不同类型的几何特征点进行配准来提高优化精度。在实际应用中，边缘点通常会用点到线的代价函数来进行优化，而平面点则使用点到平面的代价函数。将这两种方法结合在一起，可以同时优化边缘点和平面点，从而提高点云配准的精度和鲁棒性。

在具体的点云配准过程中，算法需要通过优化旋转矩阵和位移向量来实现源点云和目标点云的对齐。通过这种优化，算法可以调整源点云的位置和方向，使其与目标点云尽可能对齐，从而获得最优的姿态估计。通常，解决这个问题的方法是使用高斯-牛顿法等迭代优化技术。该方法通过多次迭代来逐步更新位姿估计，并在每次迭代中根据当前的残差情况调整变换，直到找到一个最优的结果。通过这种优化过程，ICP（迭代最近点）算法能够有效地对齐源点云和目标点云，并最终得出机器人在环境中的准确定位。

系统概述

图 2. 所提出的点云配准框架概述。使用初始估计 x0 将来自 LiDAR 框架的当前扫描转换为地图框架，并与现有地图一起进入 ICP 循环。接下来，将点到线和点到平面距离度量结合到 ICP 框架的 LP-ICP 应用所提出的可定位性检测和优化模块来估计最佳姿势。

LP-ICP框架被分为两个主要模块：局部化感知检测模块（LocDetect-Module）和带有软硬约束的优化模块（Optimization-Module）。这些模块协同工作，共同提高在极端非结构化环境中进行点云配准的精度和鲁棒性。

局部化感知检测模块（LocDetect-Module）：这个模块的作用是评估每个点云对应关系在不同方向上的局部化感知贡献。它首先通过一些公式来计算每个匹配点（如点到线或点到平面对应关系）的局部化感知贡献。具体来说，边缘点和平面点的局部化感知会分别计算在旋转和位移两个方向上的贡献。然后，这些贡献值会进行汇总，以评估每个方向的局部化感知情况。最后，通过与预设的阈值比较，系统会将局部化感知分类为三种类型：不可局部化（即在该方向上无法获取有效的信息）、部分可局部化（即在该方向上有部分有效信息）、完全可局部化（即在该方向上有充分的有效信息）。这种分类方法使得系统能够细致地区分不同的局部化感知贡献。

优化模块（Optimization-Module）：该模块的任务是根据局部化感知检测模块的结果，通过优化过程来估计出最优的位姿（位置和方向）。优化过程结合了软约束和硬约束：

• 硬约束：在局部化感知检测模块标记为“不可局部化”的方向上，优化过程中会使用硬约束，确保位姿在这些方向上不会被更新，从而避免不必要的错误或不准确的更新。
• 软约束：在“部分可局部化”的方向上，位姿会根据软约束进行更新，使得位姿在这些方向上朝着约束的目标值趋近，但不会严格要求完全匹配，从而避免过于严格的更新带来的误差过大。
• 在“完全可局部化”的方向上，位姿的更新不受限制，正常进行。

这种结合软硬约束的优化方法，有助于在退化的环境中保持稳定的定位，同时减少姿态估计中的波动和漂移，降低定位失败的风险。LP-ICP被集成到一个现有的点云配准框架中，具体来说是集成到了LVI-SAM（LiDAR视觉惯性同步定位与地图构建）系统中。LVI-SAM本身结合了LiDAR-惯性里程计（LIO）和视觉-惯性里程计（VIO）两个子模块，其中LIO子模块负责核心的位姿估计，VIO子模块则为LIO提供初步的估计。在LVI-SAM中，使用欧拉角表示姿态中的三自由度旋转。

实验

实现细节

本文提出的框架LP-ICP集成到LVI-SAM的点云配准框架中。LVI-SAM结合了LIO（LiDAR-IMU Odometry）和VIO（Visual-IMU Odometry）子模块，其中LIO子模块是主要组件，VIO子模块提供初始估计。LIO模块使用边缘点到线和面点到面的对应关系进行扫描到地图的配准。所有实验均在配备Intel i7-12700H处理器的笔记本电脑上进行。

算法比较

将当前的最先进方法集成到LVI-SAM的点云配准框架中进行实验和比较。在LVI-SAM中，使用Zhang et al. 的退化检测方法。为了确保Zhang et al. 在所有实验中有效执行退化检测，我们根据经验将其阈值设置为50，并进行了不同阈值设置下的实验。X-ICP的源代码尚未公开，我们基于文献重现了该方法并将其集成进LVI-SAM框架。在所有实验中，为了提高时间效率，三种方法的方向可定位性类别仅在点云配准的第一次迭代中进行检测，但在每次优化迭代中都会使用这些类别和附加约束。本文中，三种集成到LVI-SAM框架中的方法分别称为：LVI-SAM + Ours、LVI-SAM + Zhang et al. 和LVI-SAM + Xn-ICP 。此外还测试了其他先进的SLAM系统的性能，以与我们的方法准确度进行比较。

图 8. 比较方法和 LP-ICP 对 ANYmal 1 序列得出的结果。

类似行星环境中的仿真实验

类似行星的环境是典型的退化场景，给现有的SLAM算法带来重大挑战。由于目前直接在月球或火星等非结构化环境中进行实地实验并收集数据是困难且昂贵的，因此我们在一个视觉上逼真的类月球模拟环境中收集了一个模拟SLAM数据集——PLAM数据集。该数据集包含来自LiDAR、相机和IMU的传感器数据。

图 5. 显示了 PLAM 数据集的模拟环境和玉兔号月球车的月球彩色图像。

评估了LVI-SAM + Ours、LVI-SAM + Zhang et al. 和LVI-SAM + Xn-ICP 在PLAM数据集上的定位性能，并进行了比较分析。同时，我们还测试和评估了一些先进的SLAM系统（如ORB-SLAM3、VINS-MONO和LIO-SAM）的性能。我们将每个算法的估计轨迹与真实轨迹进行比较，并使用EVO工具对前50个姿态进行对齐，计算绝对轨迹误差（ATE）。结果以均方根误差（RMSE）形式展示。还提供了一个额外的度量，表示算法估计的姿态与真实轨迹总长度的比值。该值是估计轨迹所关联的真实轨迹长度除以序列的总真实轨迹长度。在实验中，当轨迹发生较大的漂移或波动，导致算法无法继续时，我们停止了实验。

CERBERUS DARPA地下挑战数据集的实地实验

CERBERUS DARPA地下挑战数据集在肯塔基州的Louisville Mega Cavern中收集。该数据集由配备相机、IMU和LiDAR的四足机器人ANYmal在DARPA Subterranean（SubT）挑战的最终事件中收集。地下隧道环境具有恶劣的照明条件、类似长走廊的洞穴环境、自相似区域和由崎岖地形引起的运动振动等特征，给现有的SLAM方法带来了挑战。在CERBERUS数据集上测试并评估了LVI-SAM + Ours、LVI-SAM + Xn-ICP 和LVI-SAM + Zhang et al. 的定位和建图性能。此外，还测试了几种最先进的SLAM系统（如VINS-MONO、LIO-SAM和R3LIVE ）。我们同样使用EVO工具对结果进行了评估，计算估计轨迹与真实轨迹的对齐度和绝对轨迹误差（ATE）。

图 6. 显示了 LVI-SAM + Ours 和最新方法数据集上估计的轨迹以及真实轨迹。

运行时评估

表IV显示了LP-ICP在CERBERUS DARPA地下挑战数据集ANYmal 1序列上的运行时结果。我们测试了LVI-SAM（不含退化检测）、LVI-SAM + Ours和LVI-SAM + Zhang et al. [20]三种方法的扫描到地图配准模块的平均时间消耗。结果表明三种方法的时间消耗相似。值得注意的是，未进行退化检测的方法消耗的时间最多，可能是因为在处理恶化方向时优化更困难，导致更多的迭代次数。这表明LP-ICP方法能够在机器人系统上实时运行。

总结

为了提高LiDAR基础SLAM算法在极端非结构化环境中的准确性，退化检测和处理至关重要。本文提出了一种新的ICP算法框架LP-ICP，它结合了点到线和点到面的代价函数，并引入了可定位性检测与处理。LP-ICP通过计算几何对应关系的可定位性贡献来检测退化，并添加附加约束以提高姿态估计的准确性。通过在类行星模拟数据集和实际地下隧道数据集上的实验评估，验证了该方法的有效性，结果表明其准确度高于或与最先进方法相当。