顶刊JFR｜ROLO-SLAM：首个针对不平坦路面的车载Lidar SLAM系统

摘要
基于激光雷达（LiDAR）的同步定位与地图构建（SLAM）被认为是在恶劣环境中提供定位指导的一种有效方法。然而，现成的基于激光雷达的SLAM方法在经过不平坦地形时，尤其是在垂直方向相关的部分，会出现显著的姿态估计漂移。这种缺陷通常会导致全局地图明显扭曲。在本文中，提出了一种基于激光雷达的SLAM方法，以提高地面车辆在恶劣地形中的姿态估计精度，该方法被称为旋转优化激光雷达仅（ROLO）SLAM。该方法利用前向位置预测粗略地消除连续扫描之间的位置差异，从而能够在前端分别且准确地确定位置和方向。此外，本文采用了一种并行化的空间体素化方法进行对应匹配。本文开发了一种球面对齐引导的旋转配准方法，在每个体素内估计车辆的旋转。通过结合几何对齐，本文将运动约束引入优化公式中，以增强激光雷达平移的快速有效估计。随后提取几个关键帧来构建子地图，并利用当前扫描与子地图之间的对齐进行精确的姿态估计。同时，建立了一个全局规模的因子图，以帮助减少累积误差。在各种场景中，进行了多种实验来评估本文的方法。结果表明，ROLO-SLAM在地面车辆的姿态估计方面表现出色，并且优于现有的最先进的激光雷达SLAM框架。

©️【深蓝AI】编译

论⽂题目：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 Li, Max Q.-H. Meng

论文地址：https://arxiv.org/abs/2501.02166

官方网站：https://github.com/sdwyc/rolo

介绍

在自动驾驶中，精确定位是安全高效导航的基础，尤其在不平坦地形上，车辆运动受波动影响，增加了定位难度。基于激光雷达的SLAM技术通过点云配准提供环境地图和自我定位，适用于复杂环境。该方法分为前端里程计和后端地图优化，前者提供初始姿态估计，后者细化姿态并重建环境。然而，在不平坦地形中，激光雷达SLAM易出现定位漂移，导致地图扭曲。

为解决此问题，本文提出ROLO-SLAM框架，旨在减少垂直方向的姿态漂移，提高不平坦地形中的姿态估计精度。总的来说，本文的主要贡献在于以下几个方面：

本文利用前向位置预测实现了旋转估计和平移估计之间的软解耦，这允许本文独立估计旋转和平移。
在前端，本文提出了一个双阶段范式，用于使用球面对齐和连续时间优化进行旋转和平移估计。它旨在为后端优化提供精确的初始姿态。
通过整合扫描到子地图对齐和全局因子图优化，建立了一个紧凑的激光雷达SLAM框架，这有助于在不平坦地形中定位地面车辆。

实验结果表明，ROLO-SLAM在不平坦地形中表现出色，优于现有方法。图1展示了示例结果和真实场景快照，源代码和视频演示也已提供。

▲图1｜上图显示了一辆真实车辆在越野场景中的移动。下图显示了由ROLO-SLAM输出的点云地图和轨迹。©️【深蓝AI】编译

本文方法

▲图2｜本文的ROLO-SLAM系统的流程管道包含了前端激光雷达里程计模块和后端建图模块。©️【深蓝AI】编译

前向位置预测

在前端，本文将连续扫描的旋转和平移估计解耦。这是通过前向位置预测消除平移差异来实现的。图4显示了车辆在xoz平面上同一扫描间隔期间的快照。给定第k次激光雷达扫描和相应的机器人速度 $\mathbf{v}_k$ ，前一次扫描对应的机器人速度表示为 $\mathbf{v}_{k-1}$ 。由于时间间隔足够小，可以预期在两次连续扫描之间，使用现成的速度控制技术，即 $\mathbf{v}_k \sim \mathbf{v}_{k-1}$ ，会有一致的线速度。两次扫描之间的平移距离可以认为是相同的，即 $\delta \mathbf{d}_{k-1} \sim \delta \mathbf{d}_k$ 。将车辆在经过k次激光雷达扫描后的位置表示为 $\mathbf{P}_{0:k}$ 。本文可以通过以下方式估计车辆位置

$\mathbf{t}_{k+1} = \mathbf{t}_k + \frac{\tau_{k+1} - \tau_k}{\tau_k - \tau_{k-1}} (\mathbf{t}_k - \mathbf{t}_{k-1}),$

其中τi表示第i次扫描的时间戳。当扫描 $\mathcal{P}_{k+1}$ 到达时，使用方程预先估计车辆在时间 $\tau_{k+1}$ 的位置，以形成粗略约束。对于车辆的旋转，车辆的俯仰角 $\theta_i$ 受到地面的约束，这比平移更容易受到地面起伏的影响。在实际应用中，地面的起伏通常是未知和非线性的；因此，在车辆移动过程中面对不平坦地形时，俯仰角变化很难保持相同的值。同样的事情也适用于横滚角的分析。因此，本文不以平移估计的方式来估计旋转。

传统配准方法常常将旋转和平移估计交织在一起，从而模糊了各自相关的独特挑战，可能导致车辆姿态和位置的潜在不准确性。通过引入前向位置预测，本文建立了连续激光雷达扫描之间的粗略平移估计。这在前端解耦了平移和旋转估计，建立了扫描位置的一致基线，并有望提高车辆旋转估计的准确性。

体素化匹配和旋转配准

准确识别扫描间的点对点对应关系是具有挑战性的。为了解决这个问题，本文首先提出使用高斯体素图。高斯体素图是在 $P_t$ 的坐标系中按照算法1中描述的方式构建的。

基于体素化，本文避免了直接考虑点对点对应关系。相反，本文寻找由源点 $p_s^i \in P_s$ 和目标体素 $m_k \in \mathcal{V}$ 组成的对 $\langle p_s^i, m_k \rangle$ 。基于前向位置预测，本文获得了平移信息。

然后，本文将两个连续扫描的传感器中心对齐到同一原点并开始匹配，这在算法2中描述。本文首先按照算法1为 $P_t$ 建立体素图 $V$ 。然后，按照算法2的第4行计算每个源点 $p_s^i \in P_s$ 对应于 $V$ 的索引。如算法2的第5-7行所示。请注意， $V$ 中包含的点不足的体素不符合与任何源点匹配的条件。 $N^+$ 是预设的最少点数阈值。这确保了只有能够充分表示局部几何的体素才被考虑用于建立对应关系。

▲图3｜旋转对齐模型。绿色点是点集Ps中的源点，而蓝色椭球表示mk中的高斯分布。紫色箭头代表可能的旋转方向。©️【深蓝AI】编译

现在获得了源点 $C$ 和目标体素之间的对应关系。然后，通过尝试将 $P_s$ 中的点与存储在体素中的相关高斯分布的均值位置 $\bar{p}_k$ 对齐，获得 $P_s$ 和 $P_t$ 之间的旋转。旋转对齐模型由图5演示。通过前向位置预测，两个连续扫描的传感器中心被定位在同一个原点。旋转点云可以被概念化为每个点沿着球形表面滑动，激光雷达位于中心，点到点的距离作为半径。各种源点 $p_s^i \in P_s$ 沿着球面滑动以对齐存储在体素 $m_k$ 中的高斯分布的均值位置 $\bar{p}_k$ 这个对齐过程估计了旋转。为此，整个旋转对齐表示为

$\mathbf{R} = \arg\min_{\mathbf{R}} \sum \angle(\bar{\mathbf{p}}_k, \mathbf{R} \mathbf{p}_s^i),$

▲图4｜旋转对齐的示例。蓝色平面表示均值位置的高斯分布的投影。©️【深蓝AI】编译

为了概念性地说明这种转换，图4展示了将源点 $p_s^i$ 与相关高斯分布的均值 $p_k$ 对齐的一个示例。这里， $\mathbf{Rp}_s^i$ 表示 $\mathbf{R}$ 通过旋转的源点，而 $\bar{\mathbf{p}}_k'$ 表示 $\bar{\mathbf{p}}_k$ 在与 $\mathbf{Rp}_s^i$ 相交的球面切平面上的投影：

$\sphericalangle(\bar{\mathbf{p}}_k, \mathbf{Rp}_s^i) = \arcsin \frac{\|d_i\|}{\|\bar{\mathbf{p}}_k'\|},$

$\sphericalangle(\bar{\mathbf{p}}_k, \mathbf{Rp}_s^i) \propto \|d_i\|, \quad \text{s.t.,} \quad \sphericalangle(\bar{\mathbf{p}}_k', \mathbf{Rp}_s^i) \leq \frac{\pi}{2},$

本文构建这个高斯噪声的协方差矩阵为

$\mathbf{\Omega}_R' = \mathbf{\Omega}_k + \mathbf{R}^\top \mathbf{\Omega}_s^i \mathbf{R},$

其中 $\mathbf{\Omega}_k$ 和 $\mathbf{\Omega}_s^i$ 分别是 $\bar{\mathbf{p}}_k$ 和 $p_s^i$ 的噪声协方差矩阵。然后本文对 $\mathbf{\Omega}_R'$ 执行奇异值分解（SVD），协方差被重构为

$\mathbf{\Omega}_R = \mathbf{U} \begin{bmatrix} \lambda_{\max} & \\ & \lambda_{\max} \\ & & \ddots \end{bmatrix} \mathbf{V}^\top.$

$\lambda_{\max}$ 是特征值的最大值，鉴 $\|d_i\|$ 量化了 $\mathbf{Rp}_s^i$ 和 $\bar{\mathbf{p}}_k'$ 在球面上的径向距离，本文的主要关注点是径向距离的差异。相反 $\bar{\mathbf{p}}_k'$ 和 $\bar{\mathbf{p}}_k$ 之间的轴向距离对于旋转配准不太相关。因此，SVD被用来正则化协方差，消除轴向距离的影响。这种正则化可以被解释为高斯过程的降维，其中数据从一个3D椭球流形映射到一个2D椭圆流形，如图5所示。

▲图5｜旋转对齐的示例。蓝色平面表示均值位置的高斯分布的投影。©️【深蓝AI】编译

因此，旋转 $\mathbf{R}$ 可以通过以下方式计算：

$\mathbf{R} = \arg\min_{\mathbf{R}} \sum \|d_i\|_{\mathbf{\Omega}_R^{-1}},$

该方程可以通过包括高斯-牛顿（GN）和Levenberg-Marquardt（LM）在内的优化算法迭代求解。

基于连续时间的平移优化

到目前为止，本文已经获得了旋转矩阵 $\mathbf{R}$ 和一个粗略的平移估计 $\mathbf{t}$ 。为了进一步优化平移，本文设计了一个目标函数，该函数结合了基于连续时间的平移约束。这个约束是从车辆在两个连续激光雷达扫描之间连续均匀运动模型中得出的：

$\bar{\mathbf{T}}_{\text{opt}} = \underset{\bar{\mathbf{T}}}{\arg\min} \sum \left( F_{\text{ICP}}[\bar{\mathbf{T}}] + \lambda F_{\text{CT}}[\bar{\mathbf{T}}] \right),$

其中目标函数包含两个部分： $F_{\text{ICP}}[\cdot]$ ，它基于点到分布的距离解决几何对齐问题。 $F_{\text{CT}}[\cdot]$ 表示基于连续时间的平移约束。给定一个对应关系 $\langle \mathbf{p}_s, \mathbf{m}_k \rangle \in \mathcal{C}$ ：

$F_{\text{ICP}}[\bar{\mathbf{T}}] = \left\| N_j \cdot (\bar{\mathbf{p}}_k - \bar{\mathbf{T}} \mathbf{p}_s) \right\|_{\mathbf{\Omega}_{\text{ICP}}^{-1}},$

$\mathbf{\Omega}_{\text{ICP}} = \bar{\mathbf{\Omega}}_k + \bar{\mathbf{T}}^\top \mathbf{\Omega}_s \bar{\mathbf{T}},$

$F_{\text{CT}}[\bar{\mathbf{T}}] = \left\| \mathbf{t}^n - \mathbf{t}^{n-1} \right\|_{\mathbf{\Omega}_{\text{CT}}^{-1}},$

$\mathbf{t}^n = (\bar{\mathbf{T}} \mathbf{p}_i - \check{\mathbf{T}} [\bar{\mathbf{T}}_{\text{flp}}]^{-1} \mathbf{p}_i),$

$\mathbf{\Omega}_{\text{CT}} = \mathbf{t}^n \otimes \mathbf{t}^n.$

$F_{\text{ICP}}(\cdot)$ 用于实现传感器数据的几何对齐，而 $F_{\text{CT}}(\cdot)$ 确保车辆尽可能保持连续和均匀的运动。最终的平移变换 $\bar{\mathbf{T}}$ 是 $\bar{\mathbf{T}}_{\text{flp}}$ 和 $\bar{\mathbf{T}}_{\text{opt}}$ 的组合，计算如下：

$\bar{\mathbf{T}} = \bar{\mathbf{T}}_{\text{opt}} \cdot \bar{\mathbf{T}}_{\text{flp}}.$

后端建图与回环闭合

后端优化前端的变换输出，促进生成高质量的全局位姿和环境地图。后端包括两个主要模块：局部扫描到子地图对齐和全局位姿优化。在局部层面，扫描到子地图对齐采用精确配准方法将最新扫描与累积的局部子地图对齐，从而实现更精确的激光雷达里程计。在全球层面，因子图从累积的关键帧中递增构建，调整每个历史关键帧的位姿以最小化整体历史误差。

扫描到子地图的优化。首先，扫描到子地图对齐用于进一步优化地面车辆的位姿估计。全局点云地图由历史关键帧构建，每个关键帧由边缘特征 $F_e$ 和平面特征 $F_e$ 组成，表示为为了减少内存开销，关键帧以预定义的时间间隔定期选择。在滑动时间窗口内选择预定义数量的关键帧来构建子地图表示为：

$\mathcal{M} = \{\mathcal{M}_e, \mathcal{M}_p\},$

$\mathcal{M}_e = \{F_e^i, F_e^{i-1}, F_e^{i-2}, \ldots, F_e^{i-k+1}\},$

$\mathcal{M}_p = \{F_p^i, F_p^{i-1}, F_p^{i-2}, \ldots, F_p^{i-k+1}\},$

扫描到子地图对齐被转化为一个优化问题，可以表示为

$\mathbf{T}_b = \arg\min_{\mathbf{T}_b} \sum_j \left( F_e[\mathbf{T}_b \mathbf{p}_e] + F_p[\mathbf{T}_b \mathbf{p}_p] \right).$

全局优化和回环闭合。ROLO-SLAM利用因子图（FG）模型来解决MAP问题。整个FG由具有不同因子的节点和边组成。每个节点存储那一刻的状态，本文将状态定义为世界坐标系中车辆的姿态，即 $\mathbf{X}_i = [\mathbf{R}_i \mid \mathbf{t}_i]$ 此外，本文定义了两个因子：里程计因子和回环闭合因子。因子图的整体结构如图8所示。里程计因子约束相邻状态之间的变换，类似于马尔可夫链。相邻节点的变换由扫描到子地图对齐给出。这个因子通过扫描到子地图对齐估计的状态拒绝异常值，并平滑运动轨迹。回环闭合因子用于解决长期和大规模场景中累积的误差。为了构建这个因子，本文建立了一个以当前状态 $\mathbf{X}_c$ 为中心的稳定搜索窗口。在窗口中有状态和关键帧，表示为 $\mathcal{B} = \{\mathbf{F}_0, \mathbf{F}_1, \ldots, \mathbf{F}_i, \ldots\}$ 。在车辆移动过程中，一个检查线程不断执行以评估每个 $\mathbf{F}_i \in \mathcal{B}$ 和当前关键帧之间的相似性。在识别出显著的相似性后，采用特征配准来确定变换 $\widehat{\mathbf{T}}_{\mathbf{F}_c, \mathbf{F}_i}$ 然后，建立回环闭合元组 $\mathcal{L}_{c,i}$ ：

$\mathcal{L}_{c,i} = \langle \mathbf{X}_c, \mathbf{X}_i, \widehat{\mathbf{T}}_{\mathbf{F}_c, \mathbf{F}_i} \rangle.$

这些回环闭合元组被转化为FG中的回环闭合因子，在不同时间实例引入的节点之间建立约束。通过使用FG进行全局姿态优化，细化车辆的姿态，使所有节点能够自适应调整以最小化全局差异，有效消除累积误差。

实验评估

平台和实验设置

为了评估ROLO-SLAM，本文进行了实验，关注姿态估计精度、鲁棒性、计算效率和建图。使用了公共和自建数据集。

KITTI里程计数据集用于评估水平定位精度，包含城市、乡村和高速公路场景的多模态数据。

真实越野场景数据集收集于校园周围山地，包含多种地形，适合评估6D姿态估计。

山东大学校园场景数据集涵盖变化地形，用于评估整体性能。这个数据集被用来评估所开发方法的整体性能。为了在户外环境中收集信息，这些车辆配备了相同的感知平台。感知平台有多个传感器，包括一个VLP-32C Puck 3D激光雷达、一个ZED2i立体相机、一个GW-GNS100 GNSS系统和一个Mti-300 9轴IMU。

ROLO-SLAM与LOAM、LeGO-LOAM、CT-ICP和HDL-SLAM等方法比较，展示其性能。实验在ROS Noetic和Ubuntu 20.04上进行，所有算法在联想Y9000P笔记本上执行。

定位精度评估

本文通过与不同方法的比较来评估ROLO-SLAM的定位精度。在KITTI里程计数据集（序列00、05和08）中，ROLO的轨迹紧密贴合地面实况（GT），尤其在图6(c)中几乎完全重叠。LOAM和CT-ICP也表现出高相似度，但在特征稀疏或表面几何急剧变化的情况下，其他方法如LeGO-LOAM和HDL-SLAM则显示出明显的轨迹误差。相比之下，ROLO与GT的对齐度最高，尤其是在水平方向上表现出色。

为了评估垂直方向上的定位效果，本文在Offroad1、Offroad2和Offroad3数据集上进行了测试。结果显示，ROLO的3D轨迹与GT高度相似，特别是在图7(b)中。此外，ROLO的高程曲线与GT紧密吻合。这些结果表明，ROLO在越野场景中保持了高稳定性和瞬时精度，得益于前端独立估计旋转和平移的能力。

进一步使用SDU校园数据集验证姿态估计性能，结果如图8所示。ROLO的轨迹和高程曲线基本与GT重叠，证明了其在6自由度姿态估计中的优势。表1显示，ROLO在SDU和越野数据集中具有最低的RMSE，而在KITTI数据集中，LOAM和LeGO-LOAM由于丰富的环境特征也表现良好。

为了展示前端配准方法的优势，本文将ROLO与传统点云配准方法（如ICP、NDT和PTP ICP）进行了比较。定量结果表明，ROLO在SDU和越野数据集中具有极小的RMSE，尽管在某些数据集中未能取得最佳表现。

鲁棒性评估

定位鲁棒性反映了复杂场景中维持自定位稳定性的能力。本文利用两个帧间变换的绝对误差 $E_R$ 和 $E_T$ 来测量定位鲁棒性。为了与其它方法比较鲁棒性，本文在SDU校园数据集中收集 $E_R$ 和 $E_T$ 的值。表2显示，ROLO的标准差最小，保证了连续扫描的估计稳定性，且均值最小，证明了其在执行过程中的精确性。相比之下，LOAM和HDL-SLAM的均值和标准差较高，在鲁棒性方面表现不佳。

计算效率评估

图9显示了所有数据集中每次扫描的平均处理时间，每列分别堆叠了前端和后端的处理时间。误差条代表总处理时间的变化范围。结果表明，尽管ROLO的前端被划分为三个连续模块，但它仍保持快速处理速度。总处理时间约为每次扫描100毫秒，满足机器人应用的实时要求。LeGO-LOAM的前端处理时间最短，但在定位精度方面表现不佳。此外，CT-ICP和HDL-SLAM的误差条较长，这意味着它们的处理效率不可靠。ROLO比CT-ICP和HDL-SLAM快至少10%，这证明了所开发方法的效率。

建图结果

为了展示在不平坦地形场景中建图的性能，本文使用千佛山数据集比较了HDL-SLAM、LeGO-LOAM和ROLO的建图性能。结果如图10(b)-(d)所示。在全球尺度上，本文方法生成的点云地图与卫星地图中的场景轮廓高度一致。相比之下，图10(b)-(c)顶部的HDL-SLAM和LeGO-LOAM的结果出现了明显的形态失真，尤其是在建筑物的轮廓上。在图10(d)的侧视图中，ROLO展示了一个相对平坦的建图平面，考虑到本文校园的平坦地面。然而，其他方法的侧视图显示了显著的倾斜，HDL-LOAM甚至在图10(b)中显示了重叠。这些现象反映了SLAM任务中显著的垂直漂移。相比之下，ROLO在减轻垂直漂移问题方面表现更好。

结论和未来工作

本文提出了ROLO-SLAM，以在不平坦地形中获得精确的姿态估计和环境地图。为了减轻地面车辆行驶过程中的垂直漂移，本文基于前向位置预测的粗略平移，独立估计旋转和平移。在后端，本文利用扫描到子地图和因子图来提高最终姿态估计的准确性。实验结果表明，本文的方法与最先进的方法相比表现更为出色。此外，还在不同场景中进行了建图评估，以验证ROLO-SLAM的建图质量。建图结果显示，本文的方法能够在城市和越野场景中生成高度精确的点云地图。与最先进的方法相比，本文方法输出的点云地图与真实场景的相似度最高。这也反映了本文的方法能够输出精确的姿态估计。

在未来的工作中，本文将研究在没有中心对齐条件的情况下旋转和平移的解耦，这减少了对前向位置预测的依赖。此外，本文将专注于由动态和瞬时地面波动引起的姿势变化。可能会将有限观测中提取的未来姿态预测等先验信息融合到框架中，以提高姿态估计的准确性。