ROLO-SLAM: 地面车辆在不平地形中基于旋转优化的纯激光雷达同步定位与建图

文章：ROLO-SLAM: Rotation-Optimized LiDAR-Only SLAM in Uneven Terrain with Ground Vehicle

作者：Yinchuan Wang , Bin Ren , Xiang Zhang , Pengyu Wang, Chaoqun Wang, Rui Song , Yibin Li1 , Max Q.-H. Meng

编辑：点云PCL

开源地址：https://github.com/sdwyc/ROLO

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

从技术角度观察行业发展，努力跟上时代的步伐。公众号致力于点云处理，SLAM，三维视觉，具身智能，自动驾驶等领域相关内容的干货分享，欢迎各位加入，有兴趣的可联系dianyunpcl@163.com。文章未申请原创，未经过本人允许请勿转载，有意转载联系微信920177957。

摘要

本文介绍了一种名为ROLO-SLAM的LiDAR-only SLAM方法，旨在提高地面车辆在不平坦地形中的定位精度。该方法通过预测前方位置粗略消除连续扫描之间的位置差异，并采用并行能力的空间体素化进行匹配。同时，利用几何对齐引入运动约束，优化LiDAR的平移估计。最后，提取关键帧构建子图，并利用当前扫描与子图的对齐进行精确姿态估计。实验结果表明，ROLO-SLAM在地面车辆的姿态估计方面表现出色，优于现有的LiDAR SLAM框架。

图1：顶部图像显示了一辆真实车辆在越野场景中行驶。底部图像展示了ROLO-SLAM输出的点云地图和轨迹。

主要贡献

在自动驾驶场景中，车辆在崎岖地形行驶时，定位的准确性面临巨大挑战。基于激光雷达的 SLAM 方法虽应用广泛，但现有方法在面对不平坦地面时，会出现明显的位姿估计漂移，尤其是垂直方向的误差，这会导致全局地图严重扭曲。这主要是因为车辆在颠簸过程中，激光雷达传感器的垂直位移和角度变化难以准确捕捉，同时传统的点云配准方法在处理这种复杂情况时容易出现错误匹配。针对这些问题，本文提出了 ROLO-SLAM：一种旋转优化的 LiDAR 专用 SLAM 框架，旨在减少垂直方向的姿势漂移，并在不平坦的地形中更精确地估计地面车辆的姿势。

图2：在不平坦地形上出现对应问题的简单示例。

该系统将前端分为三个独立的模块，这是基于对崎岖地形情况下姿势估计的垂直漂移的观察。在前端，开发的前向位置预测用于粗略平移估计，以将旋转和平移分离。随后，利用体素化匹配和旋转配准来独立估计两次连续扫描之间的精确旋转。利用基于连续时间的平移估计来获得更精确的扫描平移。然后，我们将我们的方法集成到一个高效的 SLAM 框架中，以扫描到子图对齐和全局因子图作为后端。总体而言主要贡献在于以下方面：

• 利用前向位置预测来实现旋转估计和平移估计之间的软解耦，这使我们能够独立估计旋转和平移。
• 在前端提出了一种使用球面对齐和连续时间优化进行旋转和平移估计的双相范式。它旨在为后端优化提供精确的初始姿势。
• 通过集成扫描到子图对齐和全局因子图优化建立了一个紧凑的 LiDAR SLAM 框架，这有助于在不平坦的地形中定位地面车辆。

进行了广泛的实验以验证所提出方法的有效性，结果表明，与最先进的 SLAM 框架相比，我们的方法具有最佳的统一性能。图 1 显示了越野场景中的示例结果和真实世界快照。此外该方法的源代码已开源：

https://github.com/sdwyc/ROLO。

ROLO - SLAM 方法原理

系统架构概述

ROLO - SLAM 主要由前端激光雷达里程计模块和后端建图模块组成。首先，利用后端的里程计数据对激光雷达扫描数据进行运动畸变校正。前端通过提取几何特征，并采用前向位置预测、体素化匹配和旋转配准等方法，分别对激光雷达的平移和旋转进行估计。后端则通过构建子图和因子图优化，进一步提高定位精度和地图质量。

图3：ROLO-SLAM系统，包括前端LiDAR里程计模块和后端建图模块。

关键技术

1

前向位置预测

在前端处理中，为了分离旋转和平移估计，采用前向位置预测来粗略消除连续扫描的位置差异。根据相邻扫描时刻的车辆速度和时间间隔，预估车辆的位置变化，从而为后续的旋转和平移独立估计奠定基础。例如，在车辆行驶过程中，假设在短时间内速度相对稳定，通过记录上一时刻和当前时刻的速度以及时间戳，就可以大致估算出车辆的平移量，而对于旋转角度，由于其受地面起伏影响较大且变化复杂，暂不进行类似的简单预估。

图4：车辆行驶在不平坦地形中的快照。这里任意两辆车的姿态在相同时间间隔内被记录。

2

体素化匹配与旋转配准

由于直接识别扫描间的点对应关系较为困难，ROLO-SLAM 采用高斯体素图进行处理。首先在目标点云的坐标系中构建体素图，将每个点分配到相应的体素中，并通过高斯分布近似体素内点的空间特征。然后，基于前向位置预测的平移信息，将连续扫描的传感器中心对齐，寻找源点与目标体素的对应关系。通过最小化源点与目标体素高斯分布均值位置的旋转角度差异，利用优化算法（如高斯 - 牛顿法或 Levenberg–Marquardt 法）计算旋转矩阵，实现旋转配准。

图5：旋转对齐模型。绿色点是源点（Ps中的点），蓝色椭圆球是mk中的高斯分布。紫色箭头表示可能的旋转方向。

图6：旋转对齐的示例演示

图7：通过使用SVD正则化对表示3D高斯分布的空间流形进行变换。

3

基于连续时间的平移优化

在获得旋转矩阵和初步平移估计后，为进一步优化平移，设计了包含连续时间平移约束的目标函数。该约束基于车辆在两次连续激光雷达扫描之间的匀速运动模型，结合几何对齐和连续时间约束，对平移进行优化。通过最小化目标函数，综合考虑点到分布的距离和连续时间平移约束，得到更精确的平移变换。

4

后端建图与回环检测

后端通过扫描到子图的对齐和全局因子图优化来提高定位精度。在扫描到子图对齐中，利用精确的配准方法将最新扫描与积累的局部子图对齐，减少累积误差。在全局优化和回环检测方面，构建因子图模型，将问题建模为最大后验概率问题。通过定义里程计因子和回环因子，调整历史关键帧的位姿，有效消除长期和大规模场景中的累积误差。

图8：ROLO-SLAM的因子图结构。随着车辆移动，建立了两种类型的因子，包括里程计因子和回环闭合因子。

实验

实验设置

实验使用了公开数据集（如 KITTI 里程计数据集）和自行采集的数据集（包括校园和越野场景），并与 LOAM、LeGO - LOAM、CT - ICP、HDL - SLAM 等当前先进方法进行对比。所有算法均在配备 Intel Core i7 CPU、Nvidia RTX2070 GPU 和 32 GB RAM 的联想 Y9000P 笔记本电脑上执行，ROLO - SLAM 系统基于 Robot Operation System (ROS) Noetic 和 Ubuntu 20.04 实现。

图9：越野和校园数据集中的地面车辆及其传感平台。

实验结果

1

定位精度

在 KITTI 里程计数据集的水平方向定位中，ROLO - SLAM 的轨迹与真实轨迹高度重合，相比其他方法表现出色。在垂直方向和 6 - DOF 位姿估计的测试中，使用越野和校园数据集进行评估，ROLO - SLAM 的轨迹与真实轨迹的相似度也高于其他方法，其均方根误差（RMSE）在多个数据集上均低于对比方法，体现了良好的定位精度.

图10：ROLO与其他里程计框架在KITTI-里程计数据集序列轨迹估计。

图11：此方法与其他方法在越野数据集的轨迹估计。

图12：此方法与其他方法在SDU校园数据集的轨迹估计。

2

鲁棒性

通过计算连续帧间变换的绝对误差来评估定位的鲁棒性，ROLO - SLAM 在不同数据集上的误差幅度和均值较小，特别是垂直方向误差接近零，且在越野场景中表现稳定，相比 LOAM 和 HDL - SLAM 等方法，具有更好的鲁棒性。

图13：ROLO在KITTI和越野数据集中的 ER和 Et 曲线。每个维度的 ER和 Et分别在子图中展示。

3

计算效率

对比各方法处理两帧扫描的时间，ROLO - SLAM 和 LeGO - LOAM 的处理时间较为稳定，平均约为 100 ms，满足机器人应用的实时性要求。CT - ICP 由于缺乏后端模块且连续时间约束处理导致处理延迟和不确定性，HDL - SLAM 处理时间变化范围大，而 ROLO - SLAM 比 CT - ICP 和 HDL - SLAM 至少快 10%，在处理效率上具有优势。

图14：各方法每次扫描的处理时间箱型图。每个数据集根据数据帧划分为若干部分。

图15：各方法每次扫描的总体处理时间，CT-ICP仅包含前端处理时间

4

地图构建质量

在地图构建结果方面，ROLO - SLAM 生成的点云图在全局尺度上与卫星地图和实际场景轮廓高度一致，在校园和越野场景中均能准确重建地形和环境，相比 HDL - SLAM 和 LeGO - LOAM 等方法，有效减少了垂直漂移带来的地图扭曲问题。

图16：兴隆数据集的建图结果。（a）显示了Google地图，标示了轨迹和距离指标。（b）展示了整体点云地图，（c）是在特定场景下捕获的。

图17：千佛数据集中的HDL-LOAM、LeGO-LOAM和ROLO的建图结果。在（b）-（d）中，顶部图像显示了整体地图，而底部图像展示了侧视图。

图18：千佛数据集中实际坡道情况的建图结果。（a）-（b）展示了地面车辆的实际场景。（c）-（d）是相应的点云地图以及ROLO的导航轨迹。

图19：Offroad2数据集的建图结果。（a）是来自Google Earth的卫星地图。（b）展示了点云和轨迹。

图20：Offroad3数据集与卫星地图对齐的点云地图。三个快照捕捉了不同驾驶场景，并按顶部编号排序。

5

与 IMU 融合方法对比

与 FAST - LIO2（一种先进的激光雷达 - 惯性 SLAM 方法）相比，ROLO - SLAM 在位置估计精度上更高，在部分数据集上的平移 RMSE 更小，但 FAST - LIO2 在旋转 RMSE 上表现较好。在定位鲁棒性方面，两者整体相似，在不同场景各有优势。在计算效率上，FAST - LIO2 虽高但依赖 IMU，在无 IMU 场景（如 KITTI 序列 00）无法正常工作，而 ROLO - SLAM 仅依靠激光雷达即可实现类似的定位性能。

总结

未来的研究方向可以考虑将ROLO-SLAM方法与其他传感器（如摄像头）相结合，提高定位和地图构建的准确性；也可以尝试将其应用于更复杂的场景中，例如城市道路和室内环境中。此外，还可以探索如何将该方法扩展到多机器人系统中，以实现协同导航和任务分配等功能。

ROLO - SLAM 通过前端的旋转和平移独立估计以及后端的优化，在崎岖地形的定位和建图中取得了显著优于现有方法的性能，在定位精度、鲁棒性和计算效率等方面都表现出色。然而，在某些极端场景下仍存在垂直漂移问题，未来将致力于研究无中心对齐条件下的旋转平移解耦，减少对前向位置预测的依赖，并融合更多先验信息（如坡度和时间序列信息）来进一步提高位姿估计的准确性。

以上内容如有错误请留言评论，欢迎指正交流。如有侵权，请联系删除