F-LOAM:基于激光雷达的快速里程计和建图

文章：F-LOAM : Fast LiDAR Odometry and Mapping

作者：Han Wang, Chen Wang, Chun-Lin Chen, and Lihua Xie

github：https://github.com/wh200720041/floam

编译：点云PCL

本文仅做学术分享，如有侵权，请联系删除。内容如有错误欢迎评论留言，未经允许请勿转载！

摘要

同时定位与建图（SLAM）在机器人领域有着广泛的应用，如自动驾驶和无人驾驶等领域。一个好的SLAM系统其计算效率和定位精度是至关重要。现有的基于激光雷达的SLAM研究通常将问题描述为两个模块：点云帧到点云帧匹配和点云帧到地图匹配。这两个模块都是通过迭代计算来解决的，计算量很大。

在本文提出了一个通用的解决方案，旨在为基于激光雷达的SLAM提供一个计算效率高、精度高的框架。具体来说，我们采用了一种非迭代的两级失真补偿方法来降低计算量。对于每个扫描输入，提取边缘和平面特征，分别匹配到局部边缘图和局部平面图，并考虑局部平滑度进行迭代姿态优化。通过深入的实验评估了其在具有挑战性的场景中的性能，包括仓库自动导引车（AGV）的定位和自动驾驶的公共数据集。该方法在公共数据集评估中以10hz以上的处理率获得了很好的定位精度，为实际应用提供了性能和计算成本之间的良好折衷。该方法目前是KITTI数据集排名中最准确、最快的开源SLAM系统之一。

图1:在KITTI数据集上提出的方法的示例(a） 显示序列05上的建图的结果。（b）是通过整合摄像机视图重建的三维道路景观(c） 绘制从F-LOAM到地面的轨迹。

介绍

估计两个点云帧之间变换的最经典方法是迭代最近点（ICP），其中两个点云帧通过最小化点云距离迭代对齐。然而，优化过程中涉及大量的点，计算效率低下。另一种方法是匹配计算效率更高的特征。一个典型的例子是激光雷达里程计和建图（LOAM），它主要思路是提取边缘和平面特征，并通过最小化点到平面和点到边缘的距离来计算姿态。然而，畸变补偿和激光里程计都需要迭代计算，这仍然是昂贵的计算。

本文介绍了一种轻量型激光雷达SLAM，旨在为公众提供一种实用的实时激光雷达SLAM解决方案。提出了一种结合特征提取、失真补偿、姿态优化和建图的新框架。与传统方法相比，本文采用了非迭代的两级失真补偿方法来代替计算效率低下的迭代失真补偿方法。在连续扫描点云帧中，具有较高局部平滑度的边缘特征和具有较低平滑度的平面特征通常是一致的。这些点对于匹配更为重要。为了提高定位精度，在迭代姿态估计中还考虑了局部几何特征。它能够在低功耗的嵌入式计算单元上实现高达20hz的实时性能。

为了证明该方法的鲁棒性，对该方法进行了全面的评价，包括室内和室外实验。与现有的最新方法相比，该方法能够以较低的计算代价获得具有竞争力的定位精度，这是性能和速度之间的一个很好的折衷。值得注意的是，所提出的方法是KITTI基准中最准确和最快的开源方法之一。

主要内容

A 传感器模型与特征提取

机械式三维激光雷达通过旋转一个尺寸为M的垂直排列的激光束阵列来感知周围的环境。它用M个平行读数扫描垂直面。在每个扫描间隔期间，激光阵列在水平面上以恒定速度旋转，同时激光测量按顺时针或逆时针顺序进行。

原始点云匹配方法（如ICP）对噪声和动态对象（如人类）非常敏感，可用于自动驾驶。此外，激光雷达扫描包含数以万计的点，这使得ICP计算效率低下。与ICP等原始点云匹配方法相比，特征点匹配在实际应用中更具鲁棒性和效率。为了提高匹配精度和匹配效率，这里利用点云数据的表面特征和边缘特征，同时剔除了噪声或不太显著的点。如上所述，三维机械激光雷达返回的点云在垂直方向上是稀疏的，在水平方向上是稠密的。因此，水平方向的特征更加明显，在水平方向上进行错误特征检测的可能性较小。对于每一个点云，我们聚焦在水平面上，并通过计算局部曲面的平滑度。

B 运动估计与失真补偿

在LOAM和LeGO LOAM等现有方案中，通过点云帧对点云帧匹配来校正失真，该匹配方法迭代地估计两个连续激光扫描点之间的转换。然而，要找到变换矩阵需要迭代计算，计算效率很低。在本篇论文中提出使用两级失真补偿来降低计算成本。请注意，大多数现有的三维激光雷达能够以10赫兹以上的频率运行，两次连续激光雷达扫描之间的时间通常非常短。因此，我们可以先假设短时间内的角速度和线速度恒定，来预测运动并校正畸变。在第二阶段，在姿态估计过程之后，将重新计算畸变，并且将重新计算的未畸变特征更新到最终的地图。

C 姿态估计

全局特征点地图由边缘特征和平面特征组成，分别进行更新和维护。为了减少搜索的计算量，边缘特征地图和平面特征地图都存储在三维KD树中。通过从边缘和平面特征图中收集附近的点来估计全局线和平面。对于每个边缘特征点， 从全局边缘特征图中计算其附近点的协方差矩阵。当点分布在一条直线上时，协方差矩阵包含一个更大的特征值。将与最大特征值相关的特征向量视为直线方向，直线的位置视为附近点的几何中心。类似地，对于每个平面特征点，这里可以得到一个具有位置和曲面范数的全局平面。注意，与全局边不同，全局平面的范数作为与最小特征值相关的特征向量。

D 建图与失真补偿

全局地图由边缘特征地图和平面特征地图组成，并基于关键帧进行更新。当平移变化大于预定义的平移阈值或旋转变化大于预定义的旋转阈值时，将选择作为关键帧。与逐帧更新相比，基于关键帧的地图更新可以减少计算量。如上所述，为了降低计算成本，基于恒定速度模型而不是迭代运动估计来执行失真补偿。

实验

A 实验设备

为了验证该算法，我们在大尺度室外环境和中等尺度室内环境下对F-LOAM进行了评估。对于大规模实验，我们在KITTI数据集上评估了我们的方法，KITTI数据集是SLAM评估中最流行的数据集之一。然后将该算法集成到仓储物流中。首先在模拟仓库环境下进行了验证，然后在AGV平台上进行了测试。

B 在公共数据集的评估

首先在广泛用于户外定位评估的KITTI数据集上测试了我们的方法。数据集是从一辆装有Velodyne HDL-64激光雷达、相机和GPS的汽车上收集的。大多数最先进的SLAM方法都是在此数据集上进行评估的，例如ORB-SLAM、VINS Fusion、LIMO和LSD-SLAM。为了验证算法的鲁棒性，我们对KITTI数据集的所有序列（包括高速公路、市中心、县道、居民区等不同场景）进行了评估。

图3:KITTI数据集序列00-10上不同定位方法的比较。

C 仓储物流实验

在本实验中，我们的目标是建立一个自主的仓储机器人来取代人工主导的制造业。AGV设计用于执行日常任务，如运输。这就要求机器人平台在复杂的环境中主动定位。

1） 仿真环境：首先在一个仿真环境中验证我们的算法。仿真环境建立在Gazebo和Linux-ubuntu18.04上。如图4（a）所示，我们使用虚拟先锋机器人和虚拟Velodyne VLP-16作为地面车辆平台。模拟环境重建了一个复杂的仓库环境，包括各种对象，如移动的工人、货架、机器等。

图4：具有静态和动态对象的模拟仓库环境(a） 露台模拟环境(b） 模拟先锋机器人和Velodyne激光雷达进行评估(c） F-LOAM与地面真值的轨迹比较。

2） 实验：为了进一步验证我们的方法的性能，我们在一个实际的用于智能制造的AGV上实现了F-LOAM。如图5（b-e）所示，仓库环境由三个主要区域组成：自动充电站、物料搬运区和制造站。一个完全自主的工厂需要机器人将材料运送到制造机器上进行装配和收集产品。所有这些操作都需要精确定位，以确保可靠性和安全性。用于测试的机器人平台如图5（a）所示，并配有Intel NUC微型计算机和Velodyne VLP-16传感器。定位和建图结果如图5所示。在这种情况下，机器人自动探索仓库，并使用所提出的方法同时构建地图。

图5：仓库环境中的壤土(a） 用于实验的自动引导车(b-e）为AGV操作构建的先进工厂环境，包括操作机器、自动充电站和存储架。中心图像：仓库定位和建图的F-LOAM结果。

3） 性能评估

本文提出的方法也在装有 VICON 系统的室内进行了测试，以评估其定位精度。机器人被遥控在测试区域内移动。结果如图6所示，其中F-LOAM轨迹和地面真实轨迹分别以绿色和红色绘制。可以看出，我们的方法可以精确地跟踪机器人的姿态。与 VICON 系统提供的地面真值相比，它的平均定位精度达到2厘米。

图6：所提出的方法与地面真值的比较。该方法能精确跟踪机器人的位姿，平均定位误差为2cm

定位精度和计算成本的对比

总结

本文提出了一个计算效率高的LiDAR-SLAM框架，旨在为有限计算资源的机器人应用提供一个良好的解决方案。与传统方法相比，我们提出使用非迭代的两级失真补偿来减少计算量。另外，在连续扫描点云帧过程中，通常会提取局部平滑度较高的边缘特征和平滑度较低的平面特征，这对于扫描帧与地图的匹配更为重要。因此，在迭代姿态估计中也考虑了局部几何特征。为了验证该方法在实际应用中的鲁棒性，进行了深入的实验，包括仿真、室内AGV试验和室外自主驾驶试验。在室内测试中平均定位精度达到2cm，是KITTI数据集中最准确、最快的开源方法之一。