MULLS: Versatile LiDAR SLAM via Multi-metric Linear Least Square论文阅读

瞻邈

814人浏览 · 2022-12-20 23:21:11

瞻邈 · 2022-12-20 23:21:11 发布

1. 摘要

随着自动驾驶与移动建图的快速发展，实际项目中对现成的激光SLAM建图方案的需求也越来越强烈，并且要求解决方案适用于各种不同规格的激光雷达与各种复杂场景。因此，我们提出了MULLS，一种高效，低漂移，多功能的3D激光SLAM系统。算法的前端使用双阈值地面分类与主成分分析，从每帧点云中提取粗略的特征点（地面，立面，柱，横梁等）；然后使用本文提出的线性最小二乘最近邻迭代算法对当前帧与局部地图配准；在不同的特征类中使用点到点（点到面，点到线）的距离联合建立估计方程来估计运动状态，配准之后的特征点会更新到局部地图中。后端通过对多个局部地图之间进行多级位姿图优化来降低前端里程计的误差；本文的实验涵盖了室内室外的场景、七种不同的激光雷达，三个数据集总共超过100000帧点云。在KITTI的测试中也证明MULLS是性能最好的实时激光SLAM之一。

2. 代码地址

https://github.com/YuePanEdward/MULLS

3. 引言

提出了一个独立于扫描线的激光SLAM方案，在不同的场景都具有低漂移实时的性能，在论文发布的时候，方法在KITTI榜单上排名前十。
提出了一种高效的点云局部配准算法MULLS-ICP，实现了粗分类几何特征点中点对点（平面、线）误差度量的线性最小二乘优化。

4. 方法论

4.1. 运动补偿

这个方法同样考虑帧内运动, 每个点具有自己的时间戳：

在没有IMU的情况下通过匀速运动差值处理:：

4.2. 几何特征点提取与编码

流程如下图所示，输入是每帧的原始点云，输出是六类带有主方向和法向的特征点。

4.2.1. 双阈值地面过滤

把点云投影到参考平面上，该平面是水平面或者上一帧的地面。这里要求激光雷达最好是水平安装的，如果不是水平安装的，则需要知道其安装角。

把参考平面按照固定的分辨率划分成2D栅格；

统计每个栅格的最小高度hmin，以及其3*3邻域的最小高度hneimin，分别设定阈值dh1, dh2，如果一个点的高度超过所在栅格的最小高度dh1或者所在栅格的最小高度超过3*3邻域的最小高度dh2，则改点被判定为非地面点，否则为初始地面点Grough，公式如下：

对每个栅格内的初始地面点做RANSAC平面拟合得到最终的地面点；

4.2.2. 基于PCA的非地面点分类

对上一步分类出的非地面点做PCA，根据其特征值计算特征的线性，平面性以及曲率，计算公式如下：

结合方向向量v与法向量n，特征点可以被分为立面F，屋顶面R，柱P，横梁B，顶点V。为了得到更准确的特征点，分别对线特征(P, B)，面特征(F, R)以及顶点V使用非极大值抑制（NMS）算法。

4.2.3. 邻域类别上下文编码

对提取的特征进行了一个“邻域分类编码”（NCC），其实主要思想是计算不同特征在邻域中的占比，以及归一化强度与高程。公式如下：

NCC作为特征会在后面的全局后端优化中使用到。

4.3. 多尺度线性最小二乘ICP

4.3.1. 多类别的近邻匹配

核心思想还是最近邻查找，创新点在于：

近邻搜索时在每个类别中分开搜索；
对于面点、线点在最近邻搜索时分别加入了法向量、方向向量一致性检测，即过滤掉方向差别过大的特征匹配点。

4.3.2. 变换矩阵估计

要点有两个：

对于不同的特征分别采取了不同的残差计算方法，比如顶点特征V采取点到点距离，面特征(G, F, R)采用点到面距离，线特征（P, B）采取点到线的距离。如下图所示：

对于不同特征采用不同的权重，变换矩阵计算公式如下：

其中，w为权重。接下来介绍权重的设计。

4.3.3. 特征权重设计

特征的权重wi由三部分权重相乘，分别是：残差权重 $w_i^{(residual)}$ ，平衡权重 $w_i^{(balanced)}$ ，强度权重 $w_i^{(intensity)}$ 。

残差权重。残差权重固定为pseudo-Huber核函数，如下所示（k=1）

其中，ei为归一化后的残差。

平衡权重。由于不同类别特征数量的不同，如果不做平衡会导致残差计算时对x, y, z方向的影响不一致，因此对不同类别的特征设计了平衡权重：

G，R分别表示地面点与水平面点，由于实际中该两类点数量较多，因此可以认为是对此两类点的降权处理。

强度权重。强度差别较大的点降权，如下所示：

4.3.4. 匹配好坏的度量

最终使用点云的后验标准差，后验信息矩阵以及非地面点的覆盖率来衡量此次匹配的好坏，公式如下：

4.4. MULLS front-end

如上图所示，前端就是前述的几何特征点提取与编码，多尺度线性最小二乘ICP和地图管理模块的组合。每收到一帧激光点云，先提取特征点并进行分类，然后下采样。此外，还利用历史帧的静态特征维护一个子图，该子图的参考位姿是上一帧。利用上一帧的运动来估计当前帧的位姿作为初始位姿，用当前帧的稀疏特征点和上一帧的稠密特征点进行匹配，估计一个粗位姿。然后再以此为初始位姿，用当前帧与子图匹配估计来估计精位姿。然后过滤掉动态物体后将特征点放到子图中。在这一步中，按类别把离群特征点剔除。然后按固定的半径裁剪子图。

4.5. MULLS back-end

使用TEASER全局对准算法来校验邻接边和回环边。用NCC特征编码的余弦相邻度来决定初始关联。用TEASER估计作为初值，然后用图与图的MULLS-ICP进行精细化，得到带有转换关系和信息矩阵的边。高 $\hat{\sigma}$ 或者 $O_{ts}$ 低于阈值的边会被删除。识别到一个回环边后，用PGO进行回环较正。