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

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

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

关键技术

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

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

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

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

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

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

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

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

图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 实现。