---

前言

代码：github
参考资料：介绍
原文：原文

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

图1. 上部图像显示了一辆在越野场景中行驶的真实车辆。下部图像显示了由ROLO-SLAM生成的点云地图和轨迹。

一. 引言

定位在自动驾驶的背景下具有极其重要的意义。它是安全高效导航的基础构件，使车辆能够在其环境中精确确定自身的位置。对于不平坦地形的导航，车辆的运动不可避免地受到地形起伏的影响。刚性安装在地面车辆上的传感器尤其容易受到这些运动的影响，从而使离开地形的定位成为一个特别具有挑战性的任务。同时定位与地图构建（SLAM）技术允许对传感器的自我姿态进行定位，同时提供环境地图。这种方法为解决不平坦和未知环境中的定位问题提供了有效的解决方案。本研究的重点是利用基于LiDAR的SLAM方法进行不平坦地形导航。基于LiDAR的方法通常利用连续的LiDAR扫描在局部和全局尺度上进行点云配准，从而估计精确的传感器自运动。这些方法具有环境条件不敏感、传感范围远、测量噪声低的优点，特别适用于室外不平坦场景。现成的基于LiDAR的SLAM通常包括两个模块：前端的LiDAR里程计和后端的地图优化[1]。前端通过逐帧配准提供初始姿态估计，而后端在全局尺度上采用对齐和优化方法来细化姿态估计并重建周围环境。这种方法使框架能够实现从粗到细的定位。它通常适用于正常场景，例如城市平坦道路。然而，在不平坦的场景中，部署在地面车辆上的基于LiDAR的SLAM方法在定位中存在不可忽略的漂移，导致地图的失真或倾斜。主要原因在于车辆因地形表面抖动导致的姿态分量在垂直方向上发生显著变化。这些变化直接导致逐帧匹配期间共识集减少，从而使前端的初始姿态估计不准确。尽管已有研究[2-4]取得了显著进展，但该主题仍然具有挑战性，需要进一步的增强解决方案。
为了解决这一问题，我们提出了ROLO-SLAM：一种旋转优化的LiDAR仅SLAM框架，旨在减少垂直方向的姿态漂移，并更精确地估计地面车辆在不平坦地形中的姿态。我们将前端分为三个独立的模块，基于对粗糙地形场景中姿态漂移的观察。在前端，开发的前向位置预测用于粗略平移估计，以解耦旋转和平移。随后，采用体素化匹配和旋转注册来独立估计连续两次扫描之间的精确旋转。一个基于连续时间的平移估计方法被用来获得扫描的精确平移。我们随后将此方法集成到一个高效的SLAM框架中，其中扫描到子地图的对齐和全局因子图作为后端。总体而言，我们的主要贡献包括以下几个方面：

• 利用前向位置预测实现旋转估计和平移估计的软解耦，使得旋转和平移可以独立估计。
• 在前端，提出了一种双相位的旋转和平移估计方案，利用球形对齐和连续时间优化，旨在为后端优化提供精确的初始姿态。
• 建立了一个紧凑的LiDAR SLAM框架，通过将扫描到子地图对齐和全局因子图优化相结合，促进地面车辆在不平坦地形中的定位。

我们进行了广泛的实验以验证所提出方法的有效性，结果表明，我们的方法在与现有最先进SLAM框架的统一性能比较中表现最佳。图1展示了一个示例结果和越野场景中的真实快照。此外，我们提供了源代码和方法的视频演示[1]。

本文结构如下：第II节讨论了相关研究。第III节分析了问题的提出及潜在原因，随后介绍了SLAM系统的流程及详细机制。第IV节介绍了实验环境的构建。第V节展示了实验结果的分析。最后，第VI节总结了本文的结论，并讨论了未来的研究方向。

二. 相关工作

尽管基于LiDAR的SLAM在地面车辆中的应用已催生了许多实际应用，例如搜索与救援、自动驾驶和地下探测[5-7]，但在恶劣环境中，关于车辆SLAM或定位的挑战依然存在。对于开创性的基于LiDAR的方法[8]，显而易见的是，当车辆穿越不平坦地形时，会发生显著的垂直漂移，导致点云地图的失真和重叠。目前，已有大量研究致力于提升基于LiDAR的SLAM的精度和实时性能[9-11]，但在这些具有挑战性的环境中，垂直误差的减少仍然是一个局限性。一种缓解上述问题的潜在策略是提高点云配准的精度。迭代最近点（ICP）算法[12]及其变体通常用于完成基于LiDAR的SLAM中的对齐任务。例如，[13]提出了一种改进的LiDAR SLAM框架，基于广义ICP（GICP）算法，利用空间体素化技术增强点云匹配过程。为了提高LiDAR点云的鲁棒性，[14]采用正态分布匹配，通过追求联合概率的最大值来替代点到点匹配。最近，[15]提出了一种CT-ICP方法，在优化函数中添加了连续时间约束，以实现更平滑的姿态变换。此方法在处理多变地形方面表现出卓越的效率，并成为减少恶劣环境中垂直漂移的潜在候选方案。此外，基于特征的方法[16-18]通过对齐环境中的边缘特征和平面特征等代表性特征，增强了配准的效率和效果。[19]开发了FEVO-LOAM框架，用于解决垂直漂移问题，增强特征提取以捕获有效的线、平面和地面特征点。[20]构建了一个稀疏特征提取方法，并将其纳入因子图，以实现不平坦地形中的优化姿态估计。然而，这些方法均同时估计平移和旋转，导致优化问题的解空间巨大，阻碍了快速收敛。[2]提出了一种LeGO-LOAM方法，用于准确估计多变地形中的姿态。此方法将估计问题划分为独立步骤，并优化结果的分辨率，从而缩小解空间并提升解的覆盖质量。[21]利用平移-旋转不变原则独立估计旋转和平移，此方法已被验证可以缩小优化解空间并通过大量实验取得改进的结果。
另一种解决垂直漂移问题的方法是多传感器信息融合。该方法通常利用环境的多尺度特征来弥补单一传感器（如LiDAR、摄像头和IMU）观察中的垂直漂移缺陷[22, 23]。[24]提出了一种紧耦合的LiDAR-惯性SLAM方法，使用IMU和LiDAR紧密跟踪姿态变换，并采用因子图融合这些观测结果。SDV-LOAM[25]利用丰富的摄像头观测，在前端实现了半直接里程计，而后端使用LiDAR传感器执行基于扫描匹配的优化。然而，需要注意的是，在多传感器融合中，如果未实现精确校准，不同传感器数据的整合可能引入额外误差。因此，传感器的精确校准与同步对于确保配准结果的准确性和可靠性至关重要。随着深度学习（DL）技术的日益普及，研究人员越来越认识到DL方法在应对环境感知和数据关联挑战中的潜力。通用方法[26-28]利用深度神经网络完成复杂的匹配和识别任务，即使在传感器观测稀疏的情况下。例如，[29]提出了SuMa++，以实现高质量的建图和里程计测量任务。SuMa++通过网络提取语义信息，并将其与曲面特征相结合，构建连续的语义地图。同样，[30]和[31]利用神经网络将点云转换为网格地图，采用最先进的3D重建技术。虽然基于DL的方法为复杂配准问题提供了有前景的解决方案，但在精度、效率和鲁棒性之间保持平衡，对于这些技术在实际环境感知应用中的广泛采用至关重要。

图2. 在不平坦地形中出现匹配问题的一个简单案例。

三. 问题定义

在世界坐标系 $\mathcal{W}\in {\mathbb{R}}^3$ 中，分别用 $\mathcal{B}$ 和 $\mathcal{L}$ 表示车辆和LiDAR的坐标系。姿态由变换矩阵 $\mathbf{T}\in SE(3)$ 表示，记为 $[\mathbf{R}|\mathbf{t}]$，其中 $\mathbf{R}\in SO(3)$ 是旋转矩阵，$\mathbf{t}\in {\mathbb{R}}^3$ 是平移向量。对于车辆与LiDAR刚性连接的系统，车辆在世界坐标系下的姿态 ${\mathcal{T}}_{\mathcal{W}}^{\mathcal{B}}$ 可以从LiDAR在世界坐标系下的姿态 ${\mathcal{T}}_{\mathcal{W}}^{\mathcal{L}}$ 推导，计算公式为：

$${\mathcal{T}}_{\mathcal{W}}^{\mathcal{B}}={\mathcal{T}}_{l_1}^{l_2}{\mathcal{T}}_{l_2}^{l_3}\cdots {\mathcal{T}}_{l_{n-1}}^{l_n}{\mathcal{T}}_{\mathcal{W}}^{\mathcal{L}},\text{\hspace{1em}(1)}$$

其中， l i , i ∈ { 1 , 2 , … , n } l_i, i \in \{1, 2, \dots, n\} li​,i∈{1,2,…,n} 表示连接车辆和LiDAR的链路。此外，每次扫描 $\mathcal{P}$ 由LiDAR生成的点集 { p i ∈ P } \{p_i \in \mathcal{P}\} {pi​∈P} 构成，$\mathcal{M}$ 表示点云地图。我们假设车辆和LiDAR之间的刚性连接始终保持稳定。目标是确定LiDAR的姿态 ${\mathcal{T}}_{\mathcal{W}}^{\mathcal{L}}$，然后通过公式(1)推导车辆的姿态 ${\mathcal{T}}_{\mathcal{W}}^{\mathcal{B}}$。大多数基于LiDAR的SLAM方法在部署到地面车辆时，会在垂直方向上产生明显的姿态漂移，尤其是在不平坦地形中行驶时。这种现象的根源在于：

1. 垂直方向的姿态分量随着车辆在地形表面的移动而变化；
2. LiDAR的垂直分辨率有限，导致姿态误差逐渐积累。

在另一种情况下，大多数点云配准方法在基于LiDAR的SLAM中采用迭代优化以逼近解。图2显示了车辆在不平坦地形中行驶的场景。浅橙色代表上一个时刻的位置，深橙色代表当前时刻的位置，分别记为 $B_1$ 和 $B_2$。LiDAR生成的点云 $L_{t-1}$ 和 $L_t$ 分别为 ${\mathcal{P}}_{t-1}$ 和 ${\mathcal{P}}_t$。在这种情况下，从上一时刻到当前时刻的姿态变换 ${\mathcal{T}}_{t-1}^t$ 可通过以下公式计算：

$${\mathcal{T}}_{t-1}^t=\underset{\mathbf{T}}{\mathrm{argmin}}\sum _i\Vert p_i^t-\mathbf{T}{p}_i^{t-1}\Vert ,p_i^t\in {\mathcal{P}}_t,p_i^{t-1}\in {\mathcal{P}}_{t-1}.\text{\hspace{1em}(2)}$$

其中，$(p_i^t,p_i^{t-1})$ 被称为对应点，其正确性直接影响公式(2)的解质量。正确的对应点表明点变换 $\mathbf{T}$ 与姿态变换 ${\mathcal{T}}_{t-1}^t$ 一致，而错误的对应点则表示两者不一致。然而，由于不平坦地形中车辆的剧烈抖动，来自两个扫描的点往往会产生错误的对应点。如图2所示，蓝点来自 ${\mathcal{P}}_{t-1}$，绿色点和红点来自 ${\mathcal{P}}_t$。根据传统的最近点匹配规则，蓝点与红点配准生成了对应点。绿色点比蓝点更接近红点，因此它们不匹配。然而，配准点对 $(B,R)$ 是错误的对应关系，因为它未能反映 $B_1$ 和 $B_2$ 之间俯仰角变化的真实情况。$(B,G)$ 是正确的对应关系，有效地反映了变化。为了解决不平坦地形中的车辆定位问题，我们的目标是探索如何通过单一LiDAR传感器提高车辆定位精度。为此，我们重新构建了整个前端并将其划分为多个模块以细化姿态估计。随后，引入扫描到子地图的对齐与因子图优化，以优化车辆在地图中的姿态。

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

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

四. 方法论

A. ROLO-SLAM 系统流程

ROLO-SLAM 的架构如图 3 所示。所开发的框架由两个模块组成：前端 LiDAR 里程计模块和后端建图模块。最初，利用后端提供的里程计数据对 LiDAR 扫描数据进行矫正，以修正运动畸变。在前端，根据几何特征的边缘和平面特性，通过平滑度指标 [8] 提取特征。随后，开发了前向位置预测，用于快速初步估计 LiDAR 的平移，从而实现旋转和平移的松耦合解耦。该过程在第 IV-B 节详细说明。通过开发的体素化方法确定对应关系，旋转和平移分别独立确定，其中旋转通过球形对齐模型注册，平移基于连续时间优化获得（详见第 IV-C 和 IV-D 节）。此外，后端通过聚合关键帧构建子地图，这些子地图用于扫描到子地图的对齐，并进一步利用因子图优化 LiDAR 的全局姿态和点云地图（详见第 IV-E 节）。

B. 前向位置预测

在前端，我们解耦了连续扫描之间旋转和平移的估计。这是通过前向位置预测消除平移差异来实现的。图 4 展示了车辆在 $xoz$ 平面中相同扫描间隔期间的快照。给定第 $k$ 次 LiDAR 扫描及其对应的机器人速度 $v_k$，前一次扫描的机器人速度表示为 $v_{k-1}$。由于时间间隔足够小，可以假设两次连续扫描之间的线速度是恒定的，即 $v_k\sim v_{k-1}$。两次扫描之间的平移距离也可以视为相同，即 $\delta d_k\sim \delta d_{k-1}$。设定车辆在第 $k$ 次位置为 $t_k$，经过 $k$ 次 LiDAR 扫描 $P_{0:k}$ 后的位置为 $t_k$。通过接收到扫描 $P_{k+1}$，可以通过以下公式估计车辆位置 $t_{k+1}$：

$$t_{k+1}=t_k+\frac{{\tau }_{k+1}-{\tau }_k}{{\tau }_k-{\tau }_{k-1}}(t_k-t_{k-1}),\text{\hspace{1em}(3)}$$

其中，${\tau }_i$ 表示第 $i$ 次扫描的时间戳。当扫描 $P_{k+1}$ 到达时，车辆在时间点 ${\tau }_{k+1}$ 的位置通过公式 (3) 进行预估，以形成粗略约束，从而约束扫描 $P_k$ 和 $P_{k+1}$ 之间的平移差异。此过程为 $P_k$ 和 $P_{k+1}$ 的旋转和平移的独立后续估计奠定了基础。对于车辆的旋转，俯仰角 ${\theta }_i$ 受地表约束的影响更容易受到地表起伏的影响，相比于平移，旋转的影响更加显著。在实际应用中，地表的起伏通常是未知的且非线性的；因此，俯仰角变化 $\delta \theta$ 在车辆行驶时面对不同地形时难以保持一致。同样的情况也适用于横滚角的分析。因此，旋转估计未采用平移估计的方式进行。传统的配准方法通常将旋转和平移估计交织在一起，因此模糊了各自相关的挑战，可能导致车辆姿态和位置的潜在不准确性。通过引入前向位置预测，我们在前端对连续 LiDAR 扫描之间的平移进行了粗略估计。这解耦了旋转和平移的估计，构建了一个一致的扫描位置基线，从而有望提高车辆旋转估计的精度。

C. 体素化匹配与旋转注册

在扫描之间准确识别点到点对应关系是一项挑战。为了解决这一问题，我们首先提出使用高斯加权网络。通过体素化表示，我们将两个相邻扫描 $P_{t-1}$ 和 $P_t$ 分别映射到源点云 ${\mathcal{P}}_s$ 和目标点云 ${\mathcal{P}}_t$。构建的高斯加权网络如算法 1 所描述，并且体素映射如图 1 所示，在点云 $P_t$ 的坐标系中。建立一个空体素地图 $\mathcal{V}$ 和一个体素索引集合 $\mathcal{I}(\mathcal{V})$，以存储每个体素 ${\mathbf{m}}_k\in \mathcal{V}$ 的索引值。每个点 $p_t\in {\mathcal{P}}_t$ 被分配到一个特定的体素，其索引通过以下公式计算：

$${\mathbf{p}}^{+}=\frac{{\mathbf{p}}_t-{\mathbf{p}}_{\text{min}}}{\text{Res}},\text{\hspace{1em}(4)}$$

$$\text{voxel\_index}={\left[1,W_v,W_v\cdot H_v\right]}^{\top }\cdot \text{round}({\mathbf{p}}^{+}),\text{\hspace{1em}(5)}$$

其中，${\mathbf{p}}_{\text{min}}$ 是一个参考点，其坐标值在不同方向 $[x_{\text{min}},y_{\text{min}},z_{\text{min}}]$ 被分别指定为点集 ${\mathcal{P}}_t$ 中的最低坐标值。$W_v$, $H_v$ 和 $\text{Res}$ 分别表示体素地图的长度、高度和分辨率。$\text{round}(\cdot )$ 表示取整操作。

我们表示任何空间点 $p_i\in {\mathbb{R}}^3$ 受到白噪声影响的形式为：

$$p_i\sim \mathcal{N}({\hat{p}}_i,{\Omega }_i),\text{\hspace{1em}(6)}$$

其中，${\hat{p}}_i$ 是点的位置，${\Omega }_i$ 是高斯白噪声的协方差矩阵。对于目标点集，每个体素不仅封装了一组空间点的聚类特性，还通过高斯分布来表示这些点的统计特性。对于每个体素 ${\mathbf{m}}_k$，利用高斯分布近似表示体素内点的空间特性，该高斯分布描述为：

$${\mathbf{m}}_k\sim \mathcal{N}({\hat{\mathbf{p}}}_k,{\hat{\Omega }}_k),\text{\hspace{1em}(7)}$$

其中：

$${\hat{\mathbf{p}}}_k=\frac{{\sum }_i{\mathbf{p}}_i}{N_k},{\hat{\Omega }}_k=\frac{{\sum }_i{\Omega }_i}{N_k},\text{\hspace{1em}(8)}$$

$N_k$ 表示属于体素 ${\mathbf{m}}_k$ 的点的总数。体素索引 $voxel\_index$ 存在于 $\mathcal{I}(\mathcal{V})$ 中的 $\mathcal{V}$。请注意，在 $\mathcal{V}$ 中包含点数不足的体素不适合与任何源点匹配。$N^{+}$ 是预设的最小点数阈值。这确保了只有能够充分表示局部几何形状的体素会被考虑用于建立对应关系。现在我们获得了源点和目标体素之间的对应关系 $\mathcal{C}$。然后，通过尝试将 ${\mathcal{P}}_s$ 中的点与存储在体素中的相关高斯分布的均值位置 ${\hat{\mathbf{p}}}_k$ 对齐，计算 ${\mathcal{P}}_s$ 和 ${\mathcal{P}}_t$ 之间的旋转。旋转对齐模型如图5所示。通过前向位置预测，两次连续扫描的传感器中心被定位在同一原点 $O$。旋转点云可以被概念化为每个点沿球面滑动，其中 LiDAR 位于球心，点到球心的距离为半径。来自 ${\mathcal{P}}_s$ 的各种源点 $p_j^i\in {\mathcal{P}}_s$ 沿球面的表面滑动以与体素 ${\mathbf{m}}_k$ 中存储的高斯分布的均值位置 ${\hat{\mathbf{p}}}_k$ 对齐。该对齐过程估计了 ${\mathcal{P}}_s$ 和 ${\mathcal{P}}_t$ 之间的传感器旋转。为此，整体旋转对齐可表示为：

$$\mathbf{R}=\underset{\mathbf{R}}{\mathrm{argmin}}\sum ⟨{\hat{\mathbf{p}}}_k,\mathbf{R}{p}_j^i⟩,\text{\hspace{1em}(9)}$$

其中，$⟨\cdot ⟩$ 表示与原点 $O$ 相对的关联点 $p_j^i$ 与对应高斯分布的均值 ${\hat{\mathbf{p}}}_k$ 之间的球面角度。$⟨\cdot ⟩$ 代表一种角度度量，而在本研究中，空间点的坐标通过距离度量表示。随后，我们提出了一种转换，将 $⟨\cdot ⟩$ 用距离度量表达出来，并通过使用马氏距离（Mahalanobis distance）构建的优化目标函数，推导出最优旋转估计。

图5. 旋转对齐模型。绿色点为源点集 ${\mathcal{P}}_s$ 中的点，蓝色椭球表示体素 ${\mathbf{m}}_k$ 中的高斯分布，紫色箭头表示可能的旋转方向。

图6. 一个旋转对齐的示例。蓝色平面表示均值位置为 ${\hat{\mathbf{p}}}_k$ 的高斯分布的投影。

为直观说明这种转换，图6展示了一个对齐源点 $p_j^i$ 和高斯分布均值 ${\hat{\mathbf{p}}}_k$ 的示例。这里，$\mathbf{R}{p}_j^i$ 表示通过旋转 $\mathbf{R}$ 后的源点，${\hat{\mathbf{p}}}_k^{\prime }$ 表示 ${\hat{\mathbf{p}}}_k$ 在与 $\mathbf{R}{p}_j^i$ 相交的球面切平面上的投影。我们有：

$$⟨{\hat{\mathbf{p}}}_k,\mathbf{R}{p}_j^i⟩=\arcsin \frac{\Vert {\mathbf{d}}_i\Vert }{\Vert {\hat{\mathbf{p}}}_k\Vert },\text{\hspace{1em}(10)}$$

$$⟨{\hat{\mathbf{p}}}_k,\mathbf{R}{p}_j^i⟩\propto \Vert {\mathbf{d}}_i\Vert ,\text{s.t. }⟨{\hat{\mathbf{p}}}_k,\mathbf{R}{p}_j^i⟩\le \frac{\pi }{2},\text{\hspace{1em}(11)}$$

其中 ${\mathbf{d}}_i={\hat{\mathbf{p}}}_k-\mathbf{R}{p}_j^i$。公式 (11) 表明从角度度量到距离度量的转换。${\hat{\mathbf{p}}}_k^{\prime }$ 可通过以下公式计算：

$${\hat{\mathbf{p}}}_k^{\prime }=\left(\Vert {\hat{\mathbf{p}}}_k\Vert -({\hat{\mathbf{p}}}_k-\mathbf{R}{p}_j^i)\cdot \frac{{\hat{\mathbf{p}}}_k}{\Vert {\hat{\mathbf{p}}}_k\Vert }\right){\mathbf{n}}_k,\text{\hspace{1em}(12)}$$

其中 ${\mathbf{n}}_k$ 表示 ${\hat{\mathbf{p}}}_k$ 的单位向量。需要注意的是，${\mathbf{d}}_i$ 本质上受到高斯白噪声的影响。我们构建高斯噪声的协方差矩阵为：

$${\Omega }_k^{\mathbf{R}}={\Omega }_k+{\mathbf{R}}^{\top }{\Omega }_j^{\mathbf{i}}\mathbf{R},\text{\hspace{1em}(13)}$$

其中 ${\Omega }_k$ 和 ${\Omega }_j^{\mathbf{i}}$ 分别是 ${\hat{\mathbf{p}}}_k$ 和 $p_j^i$ 的噪声协方差矩阵。随后，我们对 ${\Omega }_k^{\mathbf{R}}$ 执行奇异值分解（SVD），并将协方差 ${\Omega }_{\mathbf{R}}$ 重构为：

$${\Omega }_{\mathbf{R}}=\mathbf{U}\left[\begin{array}{cc}{\lambda }_{\text{max}}& 0\\ 0& {\lambda }_{\text{max}}\end{array}\right]\mathbf{V},\text{\hspace{1em}(14)}$$

其中 ${\lambda }_{\text{max}}$ 是通过 SVD 获得的最大特征值，$\mathbf{U}$ 和 $\mathbf{V}$ 是对应的正交矩阵。给定 $\Vert {\mathbf{d}}_i\Vert$ 量化了球面上 $\mathbf{R}{p}_j^i$ 和 ${\hat{\mathbf{p}}}_k^{\prime }$ 的径向距离，该方法主要聚焦于径向距离的差异性。相反，$\Vert {\mathbf{d}}_i\Vert$ 与 ${\hat{\mathbf{p}}}_k$ 和 ${\hat{\mathbf{p}}}_k^{\prime }$ 的距离越小，旋转的相关性越低。因此，使用 SVD 能够有效减少这种影响。为了正则化协方差，消除轴向距离的影响。该正则化可以解释为对高斯过程的降维操作，从而将数据从3D椭球流形映射到图7所示的2D椭圆流形。因此，旋转矩阵 $\mathbf{R}$ 可以通过以下公式计算：

$$\mathbf{R}=\underset{\mathbf{R}}{\mathrm{argmin}}\sum \Vert {\mathbf{d}}_i{\Vert }_{{\Omega }_{\mathbf{R}}^{-1}},\text{\hspace{1em}(15)}$$

其中，$\Vert \cdot {\Vert }_{{\Omega }_{\mathbf{R}}^{-1}}$ 表示马氏距离，${\Omega }_{\mathbf{R}}$ 是协方差矩阵。公式 (15) 可以通过优化算法迭代求解，包括高斯-牛顿（Gaussian-Newton, GN）和列文伯格-马夸尔特（Levenberg–Marquardt, LM）。

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

D. 基于连续时间的平移优化

到目前为止，我们已经获得了旋转矩阵 $\mathbf{R}$ 和粗略的平移估计 $\mathbf{T}$。为了进一步优化平移，我们设计了一个包含基于连续时间平移约束的目标函数。该约束源于连续均匀运动模型，用于描述连续两次 LiDAR 扫描之间的车辆运动。为了简化表示，引入两个矩阵 $\dot{\mathbf{T}}$ 和 $\mathbf{T}$，分别表示仅考虑旋转和平移的简单变换矩阵，即 $\dot{\mathbf{T}}=[\mathbf{I}|\mathbf{t}]$ 和 $\mathbf{T}=[\mathbf{R}|0]$。

包含目标函数的平移优化方程表示为：

$${\dot{\mathbf{T}}}_{\text{opt}}=\underset{\dot{\mathbf{T}}}{\mathrm{argmin}}\sum _i\left(F_{\text{ICP}}[\dot{\mathbf{T}}]+\lambda F_{\text{CT}}[\dot{\mathbf{T}}]\right),\text{\hspace{1em}(16)}$$

其中，目标函数包含两个部分：

1. $F_{\text{ICP}}[\cdot ]$：基于点到分布距离的几何对齐。
2. $F_{\text{CT}}[\cdot ]$：基于连续时间平移约束。

$\lambda$ 是 $F_{\text{CT}}[\cdot ]$ 的可调权重。

给定对应关系 $({\mathcal{P}}_s,{\mathbf{m}}_k)\in \mathcal{C}$，$F_{\text{ICP}}[\cdot ]$ 表示为：

$$F_{\text{ICP}}[\dot{\mathbf{T}}]=\Vert N_j\cdot ({\hat{\mathbf{p}}}_k-\dot{\mathbf{T}}{\hat{\mathbf{p}}}_s){\Vert }_{{\Omega }_{\text{ICP}}^{-1}},\text{\hspace{1em}(17)}$$

$${\Omega }_{\text{ICP}}={\Omega }_k+{\dot{\mathbf{T}}}^{\top }{\Omega }_s\dot{\mathbf{T}},\text{\hspace{1em}(18)}$$

其中，$N_j$ 表示体素 ${\mathbf{m}}_k$ 中包含的点数量，$\Omega$ 表示协方差矩阵。基于均匀运动模型，两次扫描之间的平移随时间保持线性关系。满足此假设时，$F_{\text{CT}}[\cdot ]$ 对 ${\mathcal{P}}_t$ 中的每个点 $p_i$ 的表示为：

$$F_{\text{CT}}[\dot{\mathbf{T}}]=\Vert t^n-t^{n-1}{\Vert }_{{\Omega }_{\text{CT}}^{-1}},\text{\hspace{1em}(19)}$$

$$t^n=({\mathbf{T}}_{\text{flp}}\mathbf{T}[f|p_i]),\text{\hspace{1em}(20)}$$

$${\Omega }_{\text{CT}}=t^n\otimes t^n.\text{\hspace{1em}(21)}$$

这里，${\mathbf{T}}_{\text{flp}}$ 的平移分量和 $\dot{\mathbf{T}}$ 的旋转分量分别由前向位置预测和平移注册中估计得出。协方差矩阵 ${\Omega }_{\text{CT}}$ 表示为 $t^n$ 本身的张量积，其中 $t^n$ 越大，优化过程中对应的维度受到的惩罚越大。$F_{\text{ICP}}[\cdot ]$ 用于实现传感器数据的几何对齐，而 $F_{\text{CT}}[\cdot ]$ 确保车辆尽可能保持连续和均匀的运动。最终的平移变换 $\dot{\mathbf{T}}$ 是 ${\dot{\mathbf{T}}}_{\text{flp}}$ 和 ${\dot{\mathbf{T}}}_{\text{opt}}$ 的组合，其计算公式为：

$$\dot{\mathbf{T}}={\dot{\mathbf{T}}}_{\text{opt}}\cdot {\dot{\mathbf{T}}}_{\text{flp}}.\text{\hspace{1em}(22)}$$

需要注意的是，公式 (16) 中的问题求解不涉及第 IV-C 节中描述的匹配过程，而是直接继承了旋转注册中的对应关系 $\mathcal{C}$。该方法旨在加速前端的处理速度。

图8. ROLO-SLAM 的因子图结构。在车辆移动过程中，建立了两种因子：里程计因子和回环闭合因子。

E. 后端建图与回环检测

后端优化来自前端的变换输出，便于生成高质量的全局姿态和环境地图。后端包括两个主要模块：局部的扫描到子地图对齐和全局的因子图优化。在局部层面，扫描到子地图的对齐方法提供了将最近的扫描与累积的局部子地图对齐的精确配准方法，从而实现更准确的LiDAR里程计。在全局层面，因子图基于累积的关键帧逐步构建，通过调整每个历史关键帧的姿态来最小化整体历史误差。

1) 扫描到子地图优化

首先，扫描到子地图的对齐被用于进一步优化地面车辆的姿态估计。全局点云地图由历史关键帧构成，每个关键帧 $\mathbf{F}$ 包含边缘特征 ${\mathbf{F}}_e$ 和平面特征 ${\mathbf{F}}_p$，表示为 F = { F e , F p } \mathbf{F} = \{\mathbf{F}_e, \mathbf{F}_p\} F={Fe​,Fp​}。为了减少内存开销，关键帧以预定时间间隔周期性选取。在滑动时间窗口内选取的预定义数量的关键帧用于构建子地图 $\mathcal{M}$，其表示为：

M = { M e , M p } , (23) \mathcal{M} = \{\mathcal{M}_e, \mathcal{M}_p\}, \tag{23} M={Me​,Mp​},(23)

M e = { F e k , F e k − 1 , F e k − 2 , … , F e k − l + 1 } , (24) \mathcal{M}_e = \{\mathbf{F}_e^k, \mathbf{F}_e^{k-1}, \mathbf{F}_e^{k-2}, \dots, \mathbf{F}_e^{k-l+1}\}, \tag{24} Me​={Fek​,Fek−1​,Fek−2​,…,Fek−l+1​},(24)

M p = { F p k , F p k − 1 , F p k − 2 , … , F p k − l + 1 } , (25) \mathcal{M}_p = \{\mathbf{F}_p^k, \mathbf{F}_p^{k-1}, \mathbf{F}_p^{k-2}, \dots, \mathbf{F}_p^{k-l+1}\}, \tag{25} Mp​={Fpk​,Fpk−1​,Fpk−2​,…,Fpk−l+1​},(25)

其中，${\mathcal{M}}_e$ 是一个由关键帧中的边缘特征组成的子集，${\mathcal{M}}_p$ 是由平面特征组成的平面子集。扫描到子地图的对齐被视为一个优化问题，其表达为：

$${\mathbf{T}}_b=\underset{{\mathbf{T}}_b}{\mathrm{argmin}}\sum \left({\mathbf{F}}_e[{\mathbf{T}}_b{\mathbf{p}}_e]+{\mathbf{F}}_p[{\mathbf{T}}_b{\mathbf{p}}_p]\right),\text{\hspace{1em}(26)}$$

其中，${\mathbf{T}}_b$ 是扫描与子地图对齐的最终变换矩阵。需要优化的变量是扫描与子地图之间的变换矩阵 ${\mathbf{T}}_b$。${\mathbf{p}}_e$ 和 ${\mathbf{p}}_p$ 分别是 ${\mathbf{F}}_e$ 和 ${\mathbf{F}}_p$ 中的点。这里，${\mathbf{F}}_e[\cdot ]$ 和 ${\mathbf{F}}_p[\cdot ]$ 分别是边缘特征和平面特征的代价函数。在我们的方法中，${\mathbf{F}}_e[\cdot ]$ 测量当前边缘特征点 { p e ∈ P } \{\mathbf{p}_e \in \mathcal{P}\} {pe​∈P} 与子地图中对应点 { p e ′ ∈ M e } \{\mathbf{p}'_e \in \mathcal{M}_e\} {pe′​∈Me​} 之间的距离。${\mathbf{F}}_p[\cdot ]$ 测量当前平面特征点 { p p ∈ P } \{\mathbf{p}_p \in \mathcal{P}\} {pp​∈P} 与子地图中对应点 { p p ′ ∈ M p } \{\mathbf{p}'_p \in \mathcal{M}_p\} {pp′​∈Mp​} 之间的距离。这些距离计算如下：

$${\mathbf{F}}_e[{\mathbf{T}}_b{\mathbf{p}}_e]={\beta }_e\frac{\Vert ({\mathbf{p}}_e^{\prime }-{\mathbf{T}}_b{\mathbf{p}}_e)\times {\mathbf{n}}_e\Vert }{\Vert {\mathbf{n}}_e\Vert },\text{\hspace{1em}(27)}$$

$${\mathbf{F}}_p[{\mathbf{T}}_b{\mathbf{p}}_p]={\beta }_p\frac{({\mathbf{T}}_b{\mathbf{p}}_p-{\mathbf{p}}_p^{\prime })\cdot {\mathbf{n}}_p}{\Vert {\mathbf{n}}_p\Vert },\text{\hspace{1em}(28)}$$

$${\beta }_e=\frac{\sum \Vert {\mathbf{n}}_e\Vert }{\Vert ({\mathbf{p}}_e^{\prime }-{\bar{\mathbf{p}}}_e^{\prime })\times {\mathbf{n}}_e\Vert },\text{\hspace{1em}(29)}$$

$${\beta }_p=\frac{1}{\sum {\mathbf{n}}_p{\mathbf{p}}_p^{\prime }+1},\text{\hspace{1em}(30)}$$

其中，${\mathbf{n}}_e$ 是边缘特征的单位向量，而 ${\mathbf{n}}_p$ 是平面特征的法向量。此外，${\beta }_e$ 和 ${\beta }_p$ 作为权重参数，用于单独的残差，优先考虑与对应边缘或平面距离较小的特征。

2) 全局优化与回环检测

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

$${\mathcal{L}}_{\mathbf{c},i}=⟨{\mathbf{X}}_c,{\mathbf{X}}_i,{\hat{\mathbf{T}}}_{{\mathbf{F}}_c,{\mathbf{F}}_i}⟩.\text{\hspace{1em}(31)}$$

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