本文经3D视觉之心公众号授权转载，转载请联系出处。
解决垂直方向漂移
定位在自动驾驶背景下至关重要。它是安全高效导航的基础模块，使车辆能够准确地确定其在环境中的位置。对于不平整地形的导航，车辆在穿越崎岖地形时必然会出现波动。刚性安装在地面车辆上的传感器特别容易受到这些运动的影响，使得越野定位成为一个极具挑战性的任务。
SLAM技术能够同时对传感器的自身位姿进行定位并提供环境地图。这种方法为在未知且不平整的环境中的定位提供了有效的解决方案。
本文介绍的重点在于利用基于 LiDAR 的 SLAM 方法进行不平整地形导航。LiDAR 方法通常利用连续的激光扫描进行点云的局部与全局配准，从而估计传感器的精确自运动。这些方法在不平整的室外场景中表现出对环境条件的不敏感性、远距离感知能力以及低测量噪声的优势。
现成的基于 LiDAR 的 SLAM 通常包含两个模块：前端的 LiDAR 里程计和后端的建图优化。前端通过帧间配准提供初始位姿估计，而后端则通过全局对齐和优化方法精细调整位姿估计并重建周围环境。这种方法使框架能够实现从粗到细的定位，适用于诸如城市平坦道路等常规场景。然而，在不平整的地形中，应用于地面车辆的基于 LiDAR 的 SLAM 方法会出现不可忽略的定位漂移，从而导致失真或倾斜的地图。其主要原因在于垂直方向的位姿分量因车辆在地形表面运动而发生显著变化。这些变化直接导致帧间匹配时的共识集合减少，使得前端无法提供正确的初始位姿估计。尽管已有大量研究在此方面取得了显著改进，这一课题依然充满挑战并需要进一步优化的解决方案。
项目链接：https://github.com/sdwyc/ROLO
针对这一问题，提出了ROLO-SLAM【1】（旋转优化激光雷达专用 SLAM）：一种旨在减少垂直方向位姿漂移并精确估计地面车辆位姿的 LiDAR 专用 SLAM 框架。我们基于在崎岖地形下位姿估计漂移的观察，将前端划分为三个独立模块。在前端，开发了一种前向位置预测，用于粗略的平移估计，以实现旋转与平移的解耦。随后，通过体素化匹配和旋转配准来独立估计两次连续扫描之间的精确旋转。基于连续时间的平移估计方法进一步优化扫描的平移精度。最终，将该方法整合到一个高效的 SLAM 框架中，结合扫描到子地图的对齐和全局因子图作为后端。
主要贡献：
- 提出了一种前向位置预测方法，实现了旋转估计与平移估计之间的柔性解耦，使得能够独立估计旋转和平移。
- 在前端提出了一种双阶段旋转和平移估计范式，利用球面对齐和连续时间优化提供了后端优化所需的精确初始位姿。
- 建立了一个紧凑的 LiDAR SLAM 框架，集成了扫描到子地图的对齐和全局因子图优化，从而支持地面车辆在不平整地形中的定位。
问题定义
大多数基于 LiDAR 的 SLAM 方法在地面车辆经过不平整地形时会产生明显的垂直方向位姿漂移。导致这一问题的主要原因包括以下两个方面：
- 不平整地形表面：车辆的姿态，尤其是滚转角（roll）和俯仰角（pitch），随着地形表面的起伏而发生变化。在这种情况下，LiDAR 传感器在垂直方向上会产生较大的角位移。然而，由于 LiDAR 在垂直方向上的分辨率有限，这些位姿误差会逐渐累积。
本研究的目标是针对不平整地形中的车辆定位问题，如何利用单一 LiDAR 传感器提高车辆定位精度。为此，我们重构了整个前端并将其划分为多个模块以优化位姿估计。随后，我们引入了扫描到子地图的对齐和因子图，以在地图内优化车辆的位姿。
具体方法
ROLO-SLAM 系统流程
ROLO-SLAM 的体系架构如图 3 所示。该框架由两个主要组件组成：前端 LiDAR 里程计模块和后端建图模块。
首先，利用后端的里程计数据对 LiDAR 扫描数据进行矫正，以消除运动畸变。在前端，基于光滑度指标提取边缘和平面特征。随后，开发了一种前向位置预测，用于快速对 LiDAR 平移进行初步估计，从而实现旋转和平移的松散解耦。在此基础上，通过开发的体素化方法确定点云的对应关系。旋转和平移分别独立估计，其中旋转通过球面对齐模型注册，平移通过连续时间优化获得。
此外，后端通过关键帧的聚合来构建子地图，并通过扫描到子地图的对齐进行优化。最后，利用因子图进一步优化 LiDAR 的全局位姿和点云地图。
前向位置预测
在前端，我们通过前向位置预测实现了连续扫描之间旋转和平移估计的解耦。这是通过消除扫描之间的平移差异来实现的。
传统的配准方法通常将旋转和平移估计结合起来，使得各自的独立挑战被掩盖，从而可能导致车辆姿态和位置估计的潜在不准确。通过引入前向位置预测，我们对连续 LiDAR 扫描之间的平移进行粗略估计，从而在前端实现了旋转和平移估计的解耦。这为扫描位置建立了一致的基准，预计能够提高车辆旋转估计的准确性。
体素化匹配与旋转配准
上述过程详细描述于 算法 1 的第 8-12 行。
基于体素化的匹配
旋转配准
通过匹配过程，我们得到了源点和目标体素之间的对应关系C。接下来，通过将源点云中的点与目标体素中的高斯分布均值Pk对齐，计算扫描之间的旋转。旋转配准模型如 图 5 所示。
最终，该优化问题可通过高斯-牛顿（GN）或 Levenberg-Marquardt（LM）算法迭代求解。
基于连续时间的平移优化
在前向位置预测之后，车辆的平移分量tk已经得到了初步估计。然而，由于崎岖地形对位姿的影响，直接使用初步预测的平移可能会导致估计误差累积。因此，我们提出了一种基于连续时间的平移优化方法，以进一步提高位姿估计的准确性。
平移模型
连续时间优化
后端建图与回环检测
为了减少累积误差并进一步优化位姿估计，我们在 ROLO-SLAM 的后端设计了一个建图与回环检测模块。该模块结合局部子地图构建和全局因子图优化，确保车辆的定位在长期运行中保持精确和一致。
局部子地图构建
在后端，我们基于关键帧聚合构建局部子地图。关键帧是经过挑选的 LiDAR 扫描帧，能够充分代表车辆在环境中的运动状态。在生成局部子地图时，关键帧间的重叠率需要达到预设阈值，以确保每个子地图能够包含足够的环境特征。
局部子地图的构建能够显著减少处理点云的计算量，同时为全局对齐和回环检测提供可靠的局部环境描述。
回环检测与因子图优化
回环检测
回环检测的目的是识别车辆在某一时刻重新访问了先前经过的地点。这一过程通过当前扫描与全局地图的匹配来实现。当检测到回环时，我们将生成一个回环约束以减少累积误差。
回环检测的步骤包括：
- 从全局地图中提取候选关键帧；
- 通过快速点云描述子（如 FPFH 或 ISS 特征）进行粗匹配；
- 利用点对点的精确 ICP 算法进行精细配准，计算当前扫描与历史关键帧之间的相对位姿。
因子图优化通常通过非线性最小二乘方法（如 Ceres 或 GTSAM）求解，以同时校正所有关键帧的全局位姿。
全局地图生成
全局地图不仅能够准确描述环境，还可以用于导航、路径规划以及后续的定位任务。
实验效果
总结一下
ROLO-SLAM是一种新颖的 LiDAR 专用 SLAM 框架，针对复杂地形下地面车辆的位姿漂移问题进行了优化。我们的框架通过前端和后端的协同设计，能够有效减少垂直方向的漂移，提高定位和建图精度。在实验中，ROLO-SLAM 在多种场景下展示了卓越的性能，尤其是在地形崎岖环境中的精度和鲁棒性。未来，我们计划将多传感器融合引入系统中，以进一步提升在极端场景下的鲁棒性和实时性。此外，开发更加高效的算法以进一步优化计算性能也是未来的研究方向。
