直接激光雷达里程计：基于稠密点云的快速定位

摘 要

本文提出了一种轻量前端激光雷达里程计解决方案，用于计算能力受限的机器人平台上，具有快速和精确的定位能力。本文的直接激光雷达里程计优先考虑计算效率，并使用稠密的、预处理最少的点云实时提供准确的姿态估计，该方法可以有效地管理历史地图信息。此外，还提供了一个自定义的迭代最近点求解器，用于点云快速配准和数据管理。

01主要贡献

（1）提出了一个定制的“速度优先”的处理流程，这个流程可以实时精确的估计位姿，对预处理的需求比较小，并且IMU是可选项。

（2）提出了一个新的关键帧系统，可以根据环境信号自适应的选择关键帧，并且可以通过凸优化快速的生成子图。

（3）定制迭代点云解算器NanoGICP，用于轻型点云扫描点的匹配，具有跨对象数据共享和数据重用的功能。

02主要内容

下图为本文的整体系统，系统首先使用带有RANSAC外点去除和可选IMU先验旋转的帧到帧(S2S)匹配，恢复两个时间相邻帧间的相对变换。然后，这个初始估计被转换到世界框架中并作为本文的二级GICP模块，用做帧到地图优化(S2M)的初始值，该模块将当前点云Pk与导出的局部地图Sk进行配准，该Sk由附近和边界关键帧的点云组成。其输出是本文的全局一致的姿态估计，然后对几个度量进行检查，以确定是否应该存储为一个新的关键帧。

A.预处理

系统假设输入为由360°激光雷达收集的3D点云数据。首先，通过原点周围单位立方体大小的栅格滤波器删除可能来自机器人本身的所有点。然后，生成的点云通过一个分辨率为0.25m的3D体素网格滤波，在保持周围环境中的主导结构的同时，对下游任务的数据进行轻微下采样；它的输出将用于后续任务。注意，本文没有纠正点云中的运动畸变，因为假设机器人运动的任何畸变由于高帧率都可以忽略不计，而且直接使用密集的点云而不是提取特征。平均经过预处理后，每个云包含少于10,000个点。

B. 基于GICP(Generalized-ICP)的位姿计算

基于激光雷达的雷达里程计可以看作是通过比较内存中连续的点云和点云来恢复机器人的位姿。这个过程通常在两个阶段中执行，首先提供最佳的瞬时位姿估计值，然后被细化为更全局一致。

（1）帧间匹配

在第一阶段，帧到帧的匹配目标是计算雷达坐标系下的源点云Pk和目标点云Pk−1之间的最优相对变换XLk，目标方程定义如下:

其中的残差项定义为:

所以该阶段的整体方程定义如下:

对于点云Pk和Pk-1之间的N个对应点，有:

其中，

表示了点云中的两个点的距离差。指的是两个点云中的两个点的距离差。CSk,i，Ctk,i应指的是每个点对应的估计协方差矩阵。

（2）帧图匹配

在恢复初始机器人运动估计后，运行第二阶段的帧到地图匹配，并遵循与帧到帧匹配相似的流程。然而，与计算帧间的相对变换不同，目标方程为进一步细化前一阶段的运动估计，通过与局部地图匹配来更提高全局一致型。换句话说，这里的任务是计算当前源点云Pk和导出的局部地图Sk之间的最优变换XWk，如下定义：

与帧间匹配类似，这里的输出即为机器人最终的估计位姿。

C.优化先验

通过IMU实现, 将IMU测量的角速度定义为(后面两项分别是静态误差和零白噪声)：


在误差矫正后通过四元数运动学公式可以获得帧间的旋转数据:

D.基于关键帧的快速子图构建

（1）关键帧选择

为了构建局部地图，本文从选定的环境关键帧子集中将相应的点云聚合起来。设Kk是所有关键帧点云的集合，这样 Sk ⊆ Kk。我们定义局部地图Sk由K最近邻关键帧点云QK和L最近邻凸包点云HK。其结果下图所示，其中用绿色突出显示的是关键帧，用红色表示是组成局部地图的关键帧点云。

（2）自适应关键帧

环境关键帧的位置会影响导出的局部地图，进而影响里程计的准确性和鲁棒性。关键帧节点通常使用固定的阈值(例如，每1m或10°的平移或旋转变化)来得到，但最佳位置可以高度依赖于周围环境的结构。因此，根据瞬时点云扫描中的“间隔”概念选择调整新关键帧的平移阈值，定义：

其中Mk是预处理点云中点距离的中位数，经验性的定义α = 0.95和β = 0.05，m是缩放关键帧距离阈值的平滑信号，取值如下：

E.算法的实现

为了便于匹配模块之间的数据交叉，本文组装了NanoGICP，这是一个自定义的GICP求解器，它结合了FastGICP和NanoFLANN开源包，并对上面所述的数据结构共享进行了额外的修改。特别是，NanoGICP使用NanoFLANN有效地构建kdtree数据结构，随后用于点云对应匹配FastGICP。在实践中，数据结构共享是在两个独立的NanoGICP实例之间执行的，这些在算法1中详细阐述。

算法一

04 实 验

为了研究系统模块的功能，包括基于关键帧的子地图、子地图法线近似和数据结构的重用，这里使用DARPA地下挑战赛城市赛道的Alpha Course数据集将每个模块与其对应模块进行比较，该数据集包含来自Velodyne VLP-16传感器的激光雷达扫描点云，以及来自VectorNav VN-100的IMU测量，该测量在位于华盛顿州埃尔马的废弃发电厂中收集60分钟数据，包含了多个感知挑战，如大型或自相似场景（下图）。

上图使用DLO算法在城市Alpha数据集上生成的稠密的3D点云地图的不同视图和角度。每个时间戳处的估计位置用于将提供的扫描点云转换为世界帧，这是对整个数据集的所有扫描点云进行处理后，并通过体素滤波以生成上述结果。

误差比较结果如图：

在1200秒的滑动窗口中绘制绝对姿势误差，显示半径和关键帧子地图方案之间的差异。对每种算法的100个基准校准的平均收敛时间的比较，包括我们的NanoGICP求解器和其他两个开源GICP包，结果如图：

数据回收方案的消融研究。四种不同数据回收方案的处理时间和CPU使用情况的箱形图，范围从无数据结构重用到部分重用和完全重用。如下图：

多场景实验。下图:肯塔基州列克星敦的一个地下矿山的一部分，在运行DLO时，使用我们的定制无人机自动绘制。这种环境包含有挑战性的条件，如:(A)低照度，(B)物体障碍物，(C)潮湿和泥泞的地形。下半部分位于洛杉矶市中心的废弃地铁三层的自上而下(D)和侧面(E)视图，通过DLO使用四足动物上的Velodyne VLP-16绘制。在这次跑步中，我们手动远程操作有腿的机器人上、下和绕每层楼走了总共856米。

与多种方法实验对比结果如下表所示：

05 总 结

这项工作提出了直接激光雷达里程计（DLO），这是一种轻量级和精确的前端定位解决方案，在极端环境中，对于长时间的运行机器人来说，计算开销最小。与其他工作不同的一个关键创新是，如何使用关键帧点云对数据库高效地导出局部子地图，以进行全局姿势优化。本文通过在大规模感知挑战环境中运行的多个平台上进行基准测试和广泛的现场实验，证明了本文方法的可靠性，并邀请其他人使用和评估本文的开源代码。