论文的标题是：LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain

• 标题给出的应用场景是 可变地形
• 重点是 轻量级 并 利用 地面优化
• 本质依然是一个 激光雷达里程计和建图

摘要部分

文章提出了一种轻量级、基于地面优化的激光里程计和建图算法LeGo-Loam,能够实时的进行六自由度位姿估计，应用在地面的车辆上面。

强调应用在车辆上面就是因为在应用该算法的时候，雷达必须水平安装（主要设计地面优化部分），像LOAM和LIO-SAM的话对于安装角点是没有要求的，并且在运行过程中角度也可以改变。

LeGo-Loam是一套轻量级的算法，可以在低功耗嵌入式系统中进行实时的位姿估计。LOAM的实时性是要求在x86性能比较好的平台的上进行，LeGo-Loam在算力下降的平台上依然可以实时进行

该算法基于地面优化，在分割和优化的过程中利用地面的存在

首先进行点云分类，并利用分类去除了一部分噪声点，通过特征提取获得面点和角点（这部分和LOAM一样）
通过两步的LM优化方法，利用角点和面点，进行相邻帧的位姿计算。—前端部分

用地面小车在可变地形上采集的数据，和LOAM进行比较，LeGO-LOAM在降低计算量的同时达到了相似或更好的精度。

把LeGO-LOAM集成到一个SLAM的框架里面去，用关键帧的概念对后端进行一个管理，来消除累计误差，用回环检测的方法（LOAM没有的部分）—后端部分。

简介部分

技术背景：

地图构建和状态估计是智能机器人中很重要的一个功能

有很多人对此付出了很多努力，通过两种方法：

• 基于视觉
• 基于激光雷达

视觉SLAM的优势可以很好的进行回环检测，但是对光照和视角的变换很敏感

激光SLAM的优势是可以在晚上依然可用，并且可以得到高精度的测量

因此论文用3d激光雷达进行实时的SLAM

传统的求解相邻两帧位姿的方式就是通过迭代最近邻点的方式（ICP），当场景非常大时，包含很多的点，那么ICP方法会非常耗时。针对ICP有几种升级的方法，将点与局部平面进行匹配，面到地图的匹配，并利用并行计算，使效率得到提升。

后面继续介绍了些点云配准的算法

LOAM的优势

LOAM是一种低漂移并且实时的激光雷达里程计和建图的方法

LOAM是通过提取角点和面点建立约束，来求取帧间的位姿变换。特征点是通过计算点的曲率进行判断的

LOAM的实时性是通过将估计问题分成了两个独立的算法进行，一种算法以高频运行，低精度估计传感器运动。另一种算法运行频率较低，但返回高精确运动估计。

在KITTI数据集上，仅通过激光雷达的估计，LOAM的精度是最好的。

LOAM的问题

指出LOAM在该工作场景中的问题

工作场景描述：
该工作场景是在地面的小车上装一个3D的激光雷达，来获得实时可靠的6自由度位姿估计。并且将算法部署到小规模的嵌入式系统中。

问题：

• 1 计算量的问题
  许多无人机驾驶的车辆上无法安装强大的计算单元

• 2 运动激烈时候的问题
  当小车在多种场景下跑的时候，由于颠簸，运动并不是十分平滑，导致数据有运动畸变。（LOAM是通过匀速模型进行的畸变去除，此时不再适用）由于运动强烈也会导致联系两帧的特征点匹配出现异常。
  另外，大量的激光点云对于低功耗的嵌入式平台很难达到实时性

当把LOAM直接用到上面的场景上，当UGV运动比较平稳，并且特征稳定，计算资源足够的时候，可以实现低漂移的运动估计

但是资源受到限制，LOAM的表现就会退化。在场景点云比较多的时候，要计算每个点的曲率是比较耗时的，低算力的平台上计算就跟不上了

• 3 噪点的问题
  UGV 的运行环境有很多的噪点。如果雷达和地面比较近，那么地面上的噪声也会影响LOAM的表现
  比如在草地上跑，可能会把草提取成角点，这样很难再找到匹配对。树叶是不是稳定的特征点

提出LeGO-LOAM的解决办法

因此提出了一种轻量的，通过地面优化的的LOAM（LeGO-LOAM）

点云的分类是通过先地面分割后，之后去除不可靠的特征点（解决噪点问题）

由于是基于地面的优化，LeGO-LOAM通过两步的优化来进行位姿的估计。（解决轻量化问题）
第一步，通过从地面点中提取面点，然后进行z roll pitch 的估计（不估计 x y yaw）
第二步，通过角点来进行 x y yaw的估计

并且集成了回环优化来修正位姿漂移。

硬件系统

论文的框架由两种激光雷达的数据进行的测

激光雷达

• 1 VLP-16：
  测量范围最远100米，精度为±3厘米
  30°（±15°）的垂直视场（FOV）和360°的水平FOV
  16通道传感器提供2°的垂直角分辨率
  水平角分辨根据旋转速率不同，从0.1°到0.4°变化
  10Hz的扫描速率，水平角分辨率为0.2°

• 2 HDL-64E
  60°的水平FOV
  48个通道
  垂直FOV为26.9°

移动车辆

使用的UGV是Clearpath Jackal
锂电池供电
最大速度为2.0米/秒，最大有效载荷为20千克
集成低成本惯性测量单元（IMU），型号为CH Robotics UM6
如下图所示

计算单元

计算单元进行验证的有两个

• 1 嵌入式计算设备 Nvidia Jetson TX2
  Jetson TX2是一款嵌入式计算设备，配备ARM Cortex-A57 CPU。

• 2 台式计算机 2.5GHz i7-4710MQ
  电脑CPU以与LOAM中使用的计算硬件相同

轻量级激光雷达里程计和建图方法

系统框架

整体框架如下：

• 输入：三维激光点云
• 输出：六自由度位姿估计

整个系统可以分为五个模块：

• 分割：指获取单次扫描的点云，并将其投影到距离图像上进行分割。161800个点用cvmat来对点进行初步管理*
• 特征提取：分割的点云被发送到特征提取模块。
• 激光雷达里程计：使用从先前模块中提取的特征点进行帧间位姿估计
• 激光建图部分：这些特征在激光建图部分被转换到全局点云地图中进一步处理。
• 变换集成模块：融合来自激光雷达里程计和激光雷达建图的姿态估计结果，并输出最终的姿态估计。以里程计高频率输出精确位姿

相对于LOAM中的原始通用框架，该系统旨在提高地面车辆的效率和精度。

点云分割

分割方法：
Pt是t时刻的一帧点云数据

pi是Pt中的一个点

首先需要把Pt映射到深度图像上去

映射的深度图像的分辨率是 1800*16 .1800就是一帧激光雷达点云里面的每一个scan上有1800个点，16就是有16个scan
VLP的水平分辨率为0.2° 垂直分辨率为2°

每个有效的点为深度图像的一个像素，像素的值就是该点到雷达中心的距离

就相当于:

地面点分类

由于在场景中可能是在斜坡上面进行运动，所以没有假设地面点是一个水平面

通过深度图像的每一列进行地面点的提取，然后再做点云的分类。

在后面的点云分类中，地面点就不参与了，因为它已经分完了

其它点分类及过滤

之后基于图像的分割方法，将点云分成很多簇

同一簇的点分配上唯一的标签

地面点是一种特殊类型的簇

对点云进行聚类分割可以提高处理效率和特征提取精度

假设机器人在一个噪声很大的环境下，有些物体的尺寸很小比如树叶，或者其它不可靠的特征点。相邻帧几乎不可能看到同一个树叶
所以做好先去掉这些点

为了实现快速和可靠的特征提取，把小于30个点的聚类进行过滤，即不会在这些点中进行特征提取

测试

其可视化的效果 如下

a图是原始的点云可视化效果

做完聚类之后，去除聚类小于30的点后保留的点，红色的点是地面点

在聚类之后剩下的点，代表比较大的目标，比如建筑物、树干、地面点，可以用这些点做里程计的位姿估计
同时这些点会被存储到深度图像中去，过滤的点则会被深度图像删去。

保留的点会存储三种属性

• 是否是地面点的标签
• 在深度图像的行列索引
• 距离，点到雷达中心的距离

然后利用这些信息再做特征提取

特征提取

这一部分和LOAM差不多

区别是 LeGO-LOAM 不是从原始点云中提取特征，而是从地面点和分割点中提取特征。

先算一个曲率

这个曲率和一个阈值比较

大于阈值就是角点，小于阈值就是面点

和LOAM一样，也把每个scan分成6段子图
那每个子图就是
300*16的点
在每个子图中选取

• 角点 2 个
• 面点4个
• 弱角点 40个
• 弱面点 80个
  

激光雷达里程计

激光雷达里程计模块的功能就是：估计相邻帧之间的位姿变换。

估计的方式：在相邻帧之间做点到线的约束和点到面的约束
具体的方式和LOAM一样

针对LOAM的改进

• 1 基于标签的匹配
  在特征提取部分提取的特征点都会有个标签（在点云分割时分配的）
  因此在找对应点时，标签必须一致
  对于面点仅在上一帧中找地面点与之匹配
  对于角点在上一帧对于的标签中找对应得角点
  这种特征点匹配得方式会提高匹配精度

• 2 两步LM优化
  首先做地面点得优化，再做角点得优化。该方法实现了相邻帧间得位姿估计
  具体两步LM优化方法如下：
  （1）第一步通过当前帧地面点和上一帧匹配得地面点估计出 z roll pitch
  （2）第二步通过当前帧角点和上一帧匹配得角点估计出 x y yaw，并利用第一步估计得 z roll pitch
  虽然在第一步就可以估计出x y yaw，但是其精度步高，不能用于第二步得估计
  最后把两步估计得结果加起来，就是6自由度得位姿变换结果
  优化方法意义：通过这种方式得优化，计算时间相比于原始LOAM可以减少35%。

激光雷达建图

原理和LOAM一样：当前帧和地图进行低频率得配准，得到最优得当前帧位姿变换

和原始LOAM不一样地方： 地图得存储。
LOAM是通过一个栅格地图进行局部地图得管理。
LeGO-LOAM是通过关键帧得概念进行局部地图管理，保存了激光雷达一些帧，和该帧的位姿。
局部地图建立就是通过根据当前帧的位置，提取与当前帧位置小于100m的关键帧拼接（根据每帧的位姿拼接）在一起作为局部地图

进行回环检测集成位姿图优化：
最后将SLAM里面的位姿图优化集成到了LeGO-LOAM中

• 回环检测的方法：就是通过检测历史帧的位姿和当前帧的位姿比较接近，则认为形成一个回环。（因为激光雷达里程计在短时间内的漂移比较小）
• 优化的方法就是通过ICP计算历史帧和当前帧的位姿变换，然后通过LM的优化方法就行 GT-SAM的位姿图优化。

实验对比

户外小场景下

户外的小场景，其中
i是一个树
ii 石头墙
iii UGV

LOAM把一些树叶提取成了面点，这种面点很不稳定

LeGO-LOAM通过距离滤波的方式把树叶去除了

LOAM把草地上的草提取成了角点，这种角点也不稳定

LeGO-LOAM不提取地面上的角点

所以LeGO-LOAM提取的特征点相比于LOAM更加稳定

最后形成的地图LOAM中杂点多，且一个树形成了3个数，
LeGO-LOAM效果更好些

户外大场景

在大范围场景下，LOAM的杂点要多些

文章来源: blog.csdn.net，作者：月照银海似蛟龙，版权归原作者所有，如需转载，请联系作者。

原文链接：blog.csdn.net/qq_32761549/article/details/125995841