顶刊JFR最新ROLO-SLAM：专为复杂地形下车辆的位姿漂移设计

最新推荐文章于 2025-06-05 20:21:44 发布

自动驾驶之心

最新推荐文章于 2025-06-05 20:21:44 发布

阅读量515

点赞数

CC 4.0 BY-SA版权

原文链接：https://mp.weixin.qq.com/s?__biz=Mzg2NzUxNTU1OA==&mid=2247649382&idx=4&sn=87e7df59dfa71fbf6542144ba3523573&chksm=cf6b241f2d67a435070da82126bf2c64e8197b46a15524a77eaa734f84406ce20c684324f83c&scene=126&sessionid=0

点击下方卡片，关注“3D视觉之心”公众号

第一时间获取3D视觉干货

>>点击进入→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 的坐标系分别表示为和。位姿由变换矩阵表示，记为 ([R \mid t])，其中表示旋转矩阵，表示平移向量。对于一个车辆与 LiDAR 刚性连接的系统，车辆在世界坐标系下的位姿可以通过 LiDAR 在世界坐标系下的位姿计算得到：

其中表示连接车辆和 LiDAR 的连接关系。此外，每次 LiDAR 扫描生成的点云由点集 ({p_i \in P}) 组成，表示点云地图。

我们假设车辆与 LiDAR 之间的刚性连接始终保持不变。我们的目标是确定 LiDAR 的位姿，然后通过公式（1）推导车辆的位姿。

大多数基于 LiDAR 的 SLAM 方法在地面车辆经过不平整地形时会产生明显的垂直方向位姿漂移。导致这一问题的主要原因包括以下两个方面：

不平整地形表面：车辆的姿态，尤其是滚转角（roll）和俯仰角（pitch），随着地形表面的起伏而发生变化。在这种情况下，LiDAR 传感器在垂直方向上会产生较大的角位移。然而，由于 LiDAR 在垂直方向上的分辨率有限，这些位姿误差会逐渐累积。
点云配准方法的局限性：大多数用于 LiDAR SLAM 的点云配准方法采用迭代优化来逼近解。图 2 展示了在不平整地形上自动驾驶的一个示例。浅橙色车辆表示上一时刻的位置，深橙色车辆表示当前时刻的位置。LiDAR 在这两个时刻生成的点云分别为和。从上一时刻到当前时刻的位姿变换可通过以下公式计算：

其中 (\langle p_i^t, p_j^{t-1} \rangle) 表示点对的对应关系，其正确性直接影响公式（2）的解的质量。正确的点对对应关系要求点的变换与位姿变换一致，而错误的对应关系则会导致结果不一致。

然而，在不平整地形中，由于车辆剧烈摇晃，来自两次扫描的点云容易产生错误的对应关系。图 2 所示，蓝色点来自，绿色点和红色点来自。基于最近邻匹配规则，蓝色点被匹配到红色点生成对应关系。然而，由于无法反映和之间的俯仰角变化，该对应关系是错误的。正确的对应关系应为，能够有效反映变化。此外，优化变量需要考虑 6 维位姿的变化，而不仅仅是平坦场景中的 3 维变化。这些因素导致最大共识集合的大小减少，从而影响解的质量。

本研究的目标是针对不平整地形中的车辆定位问题，如何利用单一 LiDAR 传感器提高车辆定位精度。为此，我们重构了整个前端并将其划分为多个模块以优化位姿估计。随后，我们引入了扫描到子地图的对齐和因子图，以在地图内优化车辆的位姿。

具体方法

ROLO-SLAM 系统流程

ROLO-SLAM 的体系架构如图 3 所示。该框架由两个主要组件组成：前端 LiDAR 里程计模块和后端建图模块。

首先，利用后端的里程计数据对 LiDAR 扫描数据进行矫正，以消除运动畸变。在前端，基于光滑度指标提取边缘和平面特征。随后，开发了一种前向位置预测，用于快速对 LiDAR 平移进行初步估计，从而实现旋转和平移的松散解耦。在此基础上，通过开发的体素化方法确定点云的对应关系。旋转和平移分别独立估计，其中旋转通过球面对齐模型注册，平移通过连续时间优化获得。

此外，后端通过关键帧的聚合来构建子地图，并通过扫描到子地图的对齐进行优化。最后，利用因子图进一步优化 LiDAR 的全局位姿和点云地图。

前向位置预测

在前端，我们通过前向位置预测实现了连续扫描之间旋转和平移估计的解耦。这是通过消除扫描之间的平移差异来实现的。

图 4 展示了车辆在平面上的快照，记录了相同扫描间隔内的车辆状态。给定第次 LiDAR 扫描及其对应的机器人速度，上一时刻的机器人速度记为。由于时间间隔足够小，利用现成的速度控制技术，可认为两次扫描之间的线速度保持一致，即。因此，扫描之间的平移距离可以视为相同，即。将车辆在第次扫描时的位置表示为，其在次扫描时的位置可通过以下公式预估：

其中表示第次扫描的时间戳。接收到后，可利用公式（3）在时间点预估车辆位置，从而为扫描和的后续旋转与平移独立估计提供粗略约束。

对于车辆的旋转，俯仰角（pitch angle）受到地面表面约束，相较于平移，更容易受到地面起伏的影响。在实际应用中，地面起伏通常是未知的且非线性的，因此，在车辆移动过程中，俯仰角变化很难保持一致。类似地，滚转角（roll angle）的分析也适用此情形。因此，我们在平移估计时不以同样的方式估计旋转。

传统的配准方法通常将旋转和平移估计结合起来，使得各自的独立挑战被掩盖，从而可能导致车辆姿态和位置估计的潜在不准确。通过引入前向位置预测，我们对连续 LiDAR 扫描之间的平移进行粗略估计，从而在前端实现了旋转和平移估计的解耦。这为扫描位置建立了一致的基准，预计能够提高车辆旋转估计的准确性。

体素化匹配与旋转配准

在扫描之间准确识别点对点的对应关系是一个具有挑战性的问题。为了解决这一问题，我们首先提出了使用高斯体素地图的方法。稍作符号滥用，我们将两次相邻的扫描和分别表示为源点云和目标点云。高斯体素地图在目标点云的坐标系中构建，其详细过程如 算法 1 所述。

我们构建了一个空体素地图和一个体素索引集合，用于存储体素的索引值。目标点云中的每个点都会被分配到一个特定的体素，其索引计算公式为：

其中：

是一个参考点，其坐标值分别为点云中各方向上的最小坐标值 ([x_\text{min}, y_\text{min}, z_\text{min}])；
、和分别表示体素地图的宽度、高度和分辨率；
表示取整操作。

我们将任何受高斯白噪声影响的空间点表示为：

其中是点的位置，是高斯白噪声的协方差矩阵。

对于目标点云中的点，每个体素不仅封装了一组空间点，还通过高斯分布描述了这些点的特性。对于每个体素，用一个高斯分布近似体素中的点的空间特性，其表达为：

其中：

和分别表示体素中点的均值位置和协方差矩阵；
是体素中点的数量。

上述过程详细描述于 算法 1 的第 8-12 行。

基于体素化的匹配

基于体素化处理后，我们避免直接考虑点对点的对应关系。相反，我们在此研究中寻找由源点和目标体素组成的对应对。基于前向位置预测，我们获得平移信息。然后，我们将两个连续扫描的传感器中心对齐到同一原点并开始匹配，其详细过程如 算法 2 所述。

首先，我们根据 算法 1 为目标点云构建体素地图。然后，计算每个源点在中的对应体素索引，其计算公式为（见 算法 2 第 4 行）：

匹配规则如 算法 2 第 5-7 行所示：当的体素索引存在于中且体素点数时，生成对应对。这里是预设的最小点数阈值，以确保仅考虑能充分代表局部几何的体素来建立对应关系。

旋转配准

通过匹配过程，我们得到了源点和目标体素之间的对应关系。接下来，通过将源点云中的点与目标体素中的高斯分布均值对齐，计算扫描之间的旋转。旋转配准模型如 图 5 所示。

通过前向位置预测，将两次扫描的传感器中心对齐到同一原点。点云的旋转可以看作每个点沿着以 LiDAR 为中心的球面滑动，其半径等于点到传感器的距离。源点云中的点沿球面滑动以对齐目标体素的高斯分布均值。

旋转对齐过程可表示为以下优化目标：

其中：

表示源点与目标体素均值之间的球面角。

为了将角度度量转换为距离度量，我们提出了以下变换：

其中是目标点在球面切平面上的投影。其计算公式为：

其中是目标点的单位向量。

为进一步优化旋转，我们引入马氏距离定义优化目标函数：

其中：

表示基于协方差矩阵的马氏距离；
。

这里和分别为目标点和源点的协方差矩阵。

最终，该优化问题可通过高斯-牛顿（GN）或 Levenberg-Marquardt（LM）算法迭代求解。

基于连续时间的平移优化

在前向位置预测之后，车辆的平移分量已经得到了初步估计。然而，由于崎岖地形对位姿的影响，直接使用初步预测的平移可能会导致估计误差累积。因此，我们提出了一种基于连续时间的平移优化方法，以进一步提高位姿估计的准确性。

平移模型

假设在两个连续扫描时刻和之间，车辆的平移可以通过以下公式表示为：

其中：

表示车辆在时刻的线速度；
表示车辆的加速度；
为时间间隔。

上述公式描述了车辆在连续扫描之间的平移变化，并将其分解为线速度和加速度的贡献。

误差度量

为了优化平移估计，我们定义了误差度量函数，用于描述预测平移与实际平移之间的差异。误差函数可以表示为：

其中：

和分别表示目标点云和源点云中的点；
是从时刻到的位姿变换矩阵。

通过最小化上述误差函数，我们可以优化平移分量，以更好地适应车辆的运动模式。

连续时间优化

为了实现连续时间的平移优化，我们利用插值方法对车辆的速度和加速度进行建模。例如，可以采用样条插值（spline interpolation）来平滑速度和加速度的变化，从而在优化过程中提供连续的运动约束。

优化目标函数可以表示为：

其中：

是基于协方差矩阵的马氏距离；
用于表示车辆运动的不确定性。

优化求解

该优化问题可以通过非线性最小二乘方法（如高斯-牛顿或 Levenberg-Marquardt 算法）求解。通过迭代优化，我们能够获得更为准确的平移分量，从而提高车辆在崎岖地形中的定位精度。

后端建图与回环检测

为了减少累积误差并进一步优化位姿估计，我们在 ROLO-SLAM 的后端设计了一个建图与回环检测模块。该模块结合局部子地图构建和全局因子图优化，确保车辆的定位在长期运行中保持精确和一致。

局部子地图构建

在后端，我们基于关键帧聚合构建局部子地图。关键帧是经过挑选的 LiDAR 扫描帧，能够充分代表车辆在环境中的运动状态。在生成局部子地图时，关键帧间的重叠率需要达到预设阈值，以确保每个子地图能够包含足够的环境特征。

局部子地图的生成公式如下：

其中：

是第个关键帧的位姿；
是对应的点云数据；
是包含在子地图中的关键帧数量。

局部子地图的构建能够显著减少处理点云的计算量，同时为全局对齐和回环检测提供可靠的局部环境描述。

回环检测与因子图优化

回环检测

回环检测的目的是识别车辆在某一时刻重新访问了先前经过的地点。这一过程通过当前扫描与全局地图的匹配来实现。当检测到回环时，我们将生成一个回环约束以减少累积误差。

回环检测的步骤包括：

从全局地图中提取候选关键帧；
通过快速点云描述子（如 FPFH 或 ISS 特征）进行粗匹配；
利用点对点的精确 ICP 算法进行精细配准，计算当前扫描与历史关键帧之间的相对位姿。

回环约束的数学表达式为：

其中：

是当前关键帧的位姿；
是检测到的历史关键帧的位姿。

因子图优化

为了整合局部子地图和回环约束，我们采用因子图（Factor Graph）来建模和优化全局位姿。因子图由节点和因子组成：

节点表示车辆的位姿；
因子表示来自前端的位姿约束、回环检测约束以及传感器噪声的影响。

优化目标是最小化所有因子的误差函数，公式如下：

其中：

表示来自前端的里程计约束；
表示回环检测约束；
和分别是前端约束和回环约束的数量。

因子图优化通常通过非线性最小二乘方法（如 Ceres 或 GTSAM）求解，以同时校正所有关键帧的全局位姿。

全局地图生成

在优化完成后，所有关键帧的位姿被校正，并可用于生成全局地图。全局地图的公式如下：

其中：

是优化后的关键帧位姿；
是对应的点云数据；
是关键帧数量。

全局地图不仅能够准确描述环境，还可以用于导航、路径规划以及后续的定位任务。

实验效果

总结一下

ROLO-SLAM是一种新颖的 LiDAR 专用 SLAM 框架，针对复杂地形下地面车辆的位姿漂移问题进行了优化。我们的框架通过前端和后端的协同设计，能够有效减少垂直方向的漂移，提高定位和建图精度。在实验中，ROLO-SLAM 在多种场景下展示了卓越的性能，尤其是在地形崎岖环境中的精度和鲁棒性。未来，我们计划将多传感器融合引入系统中，以进一步提升在极端场景下的鲁棒性和实时性。此外，开发更加高效的算法以进一步优化计算性能也是未来的研究方向。

参考

【1】ROLO-SLAM: Rotation-Optimized LiDAR-Only SLAM in Uneven Terrain with Ground Vehicle

本文仅做学术分享，论文汇总于『3D视觉之心知识星球』，欢迎加入交流！

【3D视觉之心】技术交流群

3D视觉之心是面向3D视觉感知方向相关的交流社区，由业内顶尖的3D视觉团队创办！聚焦三维重建、Nerf、点云处理、视觉SLAM、激光SLAM、多传感器标定、多传感器融合、深度估计、摄影几何、求职交流等方向。扫码添加小助理微信邀请入群，备注：学校/公司+方向+昵称（快速入群方式）

扫码添加小助理进群

【3D视觉之心】知识星球

3D视觉之心知识星球主打3D感知全技术栈学习，星球内部形成了视觉/激光/多传感器融合SLAM、传感器标定、点云处理与重建、视觉三维重建、NeRF与Gaussian Splatting、结构光、工业视觉、高精地图等近15个全栈学习路线，每天分享干货、代码与论文，星球内嘉宾日常答疑解惑，交流工作与职场问题。