激光SLAM框架LeGO-LOAM

LeGO-LOAM全称为：Lightweight and Groud-Optimized Lidar Odometry and Mapping on Variable Terrain，从标题可以看出 LeGO-LOAM 为应对可变地面进行了地面优化，同时保证了轻量级。LeGO-LOAM是专门为地面车辆设计的SLAM算法，要求在安装的时候Lidar能以水平方式安装在车辆上；如果是倾斜安装的话，也要进

瞻邈

2954人浏览 · 2020-12-18 15:40:04

瞻邈 · 2020-12-18 15:40:04 发布

1. LOAM存在的问题

LeGO-LOAM全称为：Lightweight and Groud-Optimized Lidar Odometry and Mapping on Variable Terrain，从标题可以看出 LeGO-LOAM 为应对可变地面进行了地面优化，同时保证了轻量级。

LeGO-LOAM是专门为地面车辆设计的SLAM算法，要求在安装的时候Lidar能以水平方式安装在车辆上；如果是倾斜安装的话，也要进行位姿转换到车辆上。而LOAM对Lidar的安装方式没有要求，即使手持都没有关系。

作者的实验平台是一个移动小车 (UGA)，挂载了一个 Velodyne VLP-16 线激光雷达，还配有一个低精度的 IMU；选用的硬件平台是 Nvidia Jetson TX2 (ARM Cortex-A57 CPU)；整体负载是 20Kg；移动速度为：2.0m/s；测试场景为：地面不平（比较颠簸）的草地。

LOAM框架在这样的硬件环境和使用场景中会存在一些问题：

由于搭载的是嵌入式系统，计算能力将会受限，LOAM的计算需要将难以满足，致使无法做到实时；
如果使用LOAM框架，系统计算每个点曲率的处理频率（数据量很大，VLP-16一条线是1800个点）将难以跟上传感器的更新频率；
UGA行驶的路面是非平滑连续的（运动是颠簸的），采集的数据将会失真（运动畸变，匀速运动模型无法适用于颠簸场景），使用LOAM很难在两帧之间找到可靠的特征对应；
在噪杂的环境中操作UGV也会给LOAM带来一些挑战，例如：浮动的草丛和摆动的树叶的点云将被误提取为角点或面点，这些特征是不可靠的，难以在连续帧之间获取准确的匹配，从而会造成较大的漂移。

2. LOAM和LeGO-LOAM对比

3. LeGO-LOAM的设计思路

LeGO-LOAM的设计想法中利用了地平面信息。首先，对采集到的点云进行聚类分割，分享出地面点云（地面点代表了绝大部分不稳定的特征点），同时滤出异常点。然后，使用LM进行两步优化解决连续帧之间的６自由度的变换：第一步是使用地面点云估算出平面变换参数 $[t_z,\theta_{roll},\theta_{pitch}]$ ；第二步是对分割后点云中边缘点和面点进行匹配得到 $[t_{x},t_{y},\theta_{yaw}]$ 。最后，作者还进行了回环检测以纠正运动估计的漂移。

对地面点云的配准主要使用的是面点特征；在分割后的点云配准主要使用的是边缘点和面点特征。从中可以看出使用边缘点的数量是要远小于平面点的数量，这也是能实现加速的主要原因。

LeGO_LOAM的软件系统输入 3D Lidar 的点云，输出 6 DOF 的位姿估计。整个软件系统分为 5 个部分：

Segmentation：这一部分的主要操作是分离出地面点云；同时对剩下的点云进行聚类，滤除数量较少的点云簇。
Feature Extraction：对分割后的点云（已经分离出地面点云）进行边缘点和面点特征提取，这一步和LOAM里面的操作一样。
Lidar 里程计：在连续帧之间进行（边缘点和面点）特征匹配找到连续帧之间的位姿变换矩阵。
Lidar Mapping：对feature进一步处理，然后在全局的 point cloud map 中进行配准。
Transform Integration：Transform Integration 融合了来自 Lidar Odometry 和 Lidar Mapping 的 pose estimation 进行输出最终的 pose estimate。

4. LeGO-LOAM的算法细节

上图展示了一条scan的噪声提取过程：(a)原始点云；(b)红色的点云是地面点云，其他点云是分割去噪之后保留下来的点云；(c)蓝色和黄色点云表示明显的边缘点 $(\operatorname{in}F_e)$ 和面点 $(\operatorname{in}F_p)$ 特征；(d)绿色和粉色分别表示非明显的边缘点 $(\operatorname{in}F_e)$ 和面点 $(\operatorname{in}F_p)$ 。

4.1. Segmentation

$P_t=\{p_1,p_2,\ldots,p_n\}$ 视为t时刻扫描得到的点云。把 $P_t$ 投射为一个距离图像，其中 $P_t$ 中每个有效点在距离图像中都有一个唯一的像素点与之对应，像素值 $r_i$ 表示对应点 $p_i$ 与sensor的距离值。在很多环境中，倾斜的地形很常见，论文中小车的实验场景也并不是平整的。对距离图像的 column-wise evaluation（柱状评价）可以视为是对地面估计，从而实现对地面的提取。

提取地面点之后，再对剩下的距离图像进行聚类（分簇），过滤掉点云数量小于 30 的点云簇，对保留下来的点云簇分配不同的标签。地面点云属于特殊的一类点云簇（一开始我们就提取出来了）。对点云进行聚类再处理，可以提高运行效率和提取更稳定的特征。例如，小车运行于嘈杂的环境，树叶将会产生不可靠的特征，相同的叶子不太可能在连续两帧扫描中看到。图 3 (a) 是原始点云，包含了很多植被点云；经过处理之后变成了图3(b)，只剩下大物体点云，例如：树干。地面点云将被保留以作进一步处理。此时，保留下来的每个点将具备三种属性：(1) 点云的标签；(2) 在距离图像中的行列数；(3) 距离值。

4.2. Feature Extraction

这一步主要是从地面点云和分割出来的点云中提取特征，与LOAM中的操作一样。主要要搞清楚几个特征点集合的概念。

设S是距离图像同一行连续点的集合， $|S|=10$ ， $p_i$ 两边分布着一半的点云， $p_i$ 的曲率计算公示为：

$c=\frac1{|S|\cdot||r_i||}||\sum_{j\in S,j\neq i}(r_j-r_i)||$

为了从各个方向均匀地提取特征，我们将距离图像水平分割成几个相等的子图像，将360°均匀分成6等分，每一等分的精度是 $300 \times 16$ （因为VLP-16线Lidar，一条scan是1800个点）。

计算子图中每一行的点的曲率值，对曲率值进行排序分类，> Cth，分为边缘点特征；< Cth 分为面点特征。设置以下几个集合(论文中这部分语句相近，以下是我个人的理解，可能不准确)：

$n_{F_e}$ 表示从子图每一行中提取具有最大c值（不包含地面）的边缘特征集合；
$n_{F_p}$ 表示从子图每一行中提取具有最小c值（包含地面的）的面点特征集合；
$F_e$ 和 $F_p$ 表示所有明显的边缘点特征和面点特征的集合；
$n_{\mathbb{F}_e}$ 表示每个子图中每一行次大c值的边缘点（不包括地面）集合；
$n_{\mathbb{F}_p}$ 表示每个子图中每一行次小c值的面点集合（包括地面）；
$\mathbb{F}_e$ 和 $\mathbb{F}_p$ 表示所有子图的非明显的边缘点和面点（包括地面），如图3(d)；
针对明显的特征点集合 $n_{F_e}$ ， $n_{F_p}$ ，我们分布取数量2和4；
针对非明显的特征点集合 $n_\mathbb{F}$ 。和 $n_\mathbb{F}$ 。我们分布取数量40和80。

4.3. Lidar Odometry

Lidar Odometry 与在LOAM中类似，用来估计sensor在连续两帧之间的位姿；其中，使用了 point-to-edge 和 point-to-plane 的帧间匹配。换句话说，我们需要在当前帧特征点集合 $[F_e^t,F_p^t]$ 和上一帧特征集 $[F_e^{t-1},F_p^{t-1}]$ 之间建立对应关系。LeGO-LOAM在 Lidar Odometry 中做了一些改变，提高了特征匹配的精度和效率。

(1）Label Matching：LeGO-LOAM对点云进行了聚类分簇，不同的点云簇具有不同的Label。Label 信息可以作为两帧匹配的约束条件，连续两帧之间只有同类标签点云簇才能进行配准。这种方式可以提高配准的精度和效率。

(2) 两步L-M优化：在LOAM中，它把当前帧和上一帧边缘点和面点特征匹配的问题综合为了一个复杂距离向量，然后应用LM直接求得最短距离下的位姿矩阵。在LeGO-LOAM中，作者应用了两步LM优化来求得齐次变换矩阵T：step1: 在连续两帧面点特征 $[F_p^t,F_p^{t-1}]$ 中进行匹配得到 $[ t_z, \theta _{roll}, \theta _{pitch}]$

4.4. Lidar Mapping

在LOAM论文中，Lidar Mapping 的工作是执行低频高精度配准：首先，把当前特征的 $\{\mathbb{F}_e^t,\mathbb{F}_p^t\}$ 与周围点云图 $\hat{Q}^{t-1}$ 进行匹配来进一步改善位姿变换；然后，再次使用LM方法获取最终的变换结果。

LeGO-LOAM与LOAM在此处最大的区别在于点云地图的存贮。LeGO-LOAM放弃了存储单张点云地图，转而存储单个特征集 $\{\mathbb{F}_e^t,\mathbb{F}_p^t\}$ 。让 $M^{t-1}=\{\{\mathbb{F}_e^1,\mathbb{F}_p^1\},\ldots,\{\mathbb{F}_e^{t-1},\mathbb{F}_p^{t-1}\}\}$ 保存着先前所有的特征集，其中 $M^{t-1}$ 中每一个特征集与 sensor 的位姿进行了绑定。全局地图 $\hat{Q}^{t-1}$ 可以通过两种方式从 $M^{t-1}$ 中得到。

在第一种方法中：进行局部地图匹配。一种简单的方法是，我们选择sensor当前位置100m之内的特征集作为 key-frame 拼接到点云地图 $\hat{Q}^{t-1}$ 上。这种地图的拼接方式与LOAM类似。

另一种方法是使用因子图位姿优化框架来更新位姿估计。构建了一个 pose-graph，每个特征集的 sensor pose 是 graph 中的一个Node，特征集 $\{\mathbb{F}_e^t,\mathbb{F}_p^t\}$ 被视为 Node 的测量值。在 lidar mapping 中位姿估计的漂移很低，我们假设在这个很短的时间内没有漂移现象。在这种方法中， $\hat{Q}^{t-1}$ 由最近的$k$组特征集构成， $\hat{Q}^{t-1}=\{\{\mathbb{F}_e^{t-k},\mathbb{F}_p^{t-k}\},\ldots,\mathbb{F}_e^{t-1},\mathbb{F}_p^{t-1}\}\}$ 。然后，利用L-M优化后得到的变换，作为新节点与$\hat{Q}^{t-1}$中所选节点之间的空间约束。通过执行闭环检测进一步消除漂移。在这种情况下，如果使用ICP在当前特征集和以前的特征集之间找到匹配，则会添加新的约束。最后，把这些约束丢入姿态图优化系统中来进一步更新 sensor 的位姿估计。注意，作者只有第IV(D)节中的实验使用了位姿图优化创建周围的地图。

5. LeGO-LOAM的性能表现

作者为测试LeGO-LOAM的性能，设计了一系列的实验来对比LeGO-LOAM和LOAM的表现性能。

作者在户外的小场景和大场景中，分别对LeGO-LOAM和LOAM测试平台执行激烈和平缓的控制，来查看两者的建图效果和效率。

在小场景的激烈运动过程中：LOAM 会把草丛、树叶提取为边缘点特征（草丛和树叶是不稳定特征的主要来源）；而LeGOU-LOAM会过滤掉这些不稳定的特征，只会在树干，地面，台阶等上提取稳定特征。如下图所示（绿色是边缘点，粉色是面点）：

在LOAM框架中，剧烈的运动容易造成点云地图的发散，如下图中 (a) LOAM，有三个树干。

作者在大场景的都市环境（是一个学校，不同地点的海拔误差在19m之内）里也进行多种测试以验证建图的精度，有人行道，水泥路，土路和草丛。

LOAM 在人行道上的建图效果并不好，可能是一端存在树木叶子的干扰，如下图所示：

在其他三种场景中，LeGO-LOAM的偏差（终点相对于初始位置的偏差）表现都要优于LOAM。

整个测试的结果如下：

特征点数量对比：LeGO-LOAM特征点整体下降幅度超过：29%，40%，68%，72%。

迭代次数对比：里程计的迭代次数降低了34%，48%。

运行时间对比：降低了 60%

位姿误差对比：LeGOLOAM可以用更少的计算时间实现可比或更好的位置估计精度。

参考文献

SC-LEGO-LOAM 扩展以及深度解析（一）

一文详解激光SLAM框架LeGO-LOAM-CSDN博客

2048 AI社区

有“AI”的1024 = 2048，欢迎大家加入2048 AI社区

更多推荐

招商霸屏哪家售后优？企业必备实战复盘TOP榜

2048 AI社区

AI技术唾手可得的时代，挖掘新需求是产品突围的关键——某知名聚合DNS管理系统的需求洞察

例如，为Cloudflare提供更细粒度的API Token支持（而非仅限Global API Key）、为华为云DNS增加按省份/国家地区的精细化线路选项、在SSL证书部署中支持更多特定的应用场景（如宝塔邮局、宝塔云WAF v6.8+、群晖等）。（1）需求1：用户希望支持更多新型的DNS/CDN服务商，以扩展系统的管理范围。例如，增加对Spaceship、DNSPod国际版、Cloudns.ne