【泡泡点云时空】MULLS：一种基于多尺度线性最小二乘的通用激光SLAM算法

泡泡点云时空，带你精读点云领域顶级会议文章

标题：MULLS: Versatile LiDAR SLAM via Multi-metric Linear Least Square

作者：Yue Pan, Pengchuan Xiao , Yujie He, Zhenlei Shao and Zesong Li

来源：ICRA2021

编译：玉玺

审核：lionheart

这是泡泡点云时空推送的第253篇文章，欢迎个人转发朋友圈；其他机构或自媒体如需转载，后台留言申请授权

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

代码已开源： 

https://github.com/YuePanEdward/MULLS

MULLS-SLAM效果示意图

（a）单帧激光（b）MULLS提取的特征示意图（地面，立面，立柱，横柱）（c）配准后的局部地图（d）局部地图特征（e）子地图之间的TEASER配准（f）闭环约束的边（f）最终生成的地图与轨迹

（一）地面特征提取。采用双阈值滤波算法，默认激光雷达是水平安装（如果不是水平，则需要标定出激光与地面的角度），分以下几步：

1、将激光点云投影到激光雷达所在水平面上；

2、将水平面按照固定的分辨率划分成2D栅格；

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

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

（二）非地面特征提取。对上一步分类出的非地面点做PCA，根据其特征值计算特征的线性

，平面性以及曲率，计算公式如下：

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

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

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

这是本文提出的一种基于多类别点云的ICP方法，主要流程如下图所示：

几个关键步骤如下：

（一）多类别的近邻匹配。核心思想还是最近邻查找，创新点在于：

1、近邻搜索时在每个类别中分开搜索；

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

（二）变换矩阵估计。要点有两个：

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

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

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

（三）特征权重设计。特征的权重wi由三部分权重相乘，分别是：残差权重wi(residual),平衡权重wi(balanced),强度权重wi(intensity)。

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

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

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

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

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

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

MULLS-SLAM算法框架如图5所示。可见算法前端主要做特征提取、帧帧匹配，帧图匹配，而算法后端主要做回环检测与图图匹配。后端中使用第一部分提到的特征编码做回环检测，图图匹配中使用TEASER方法做初始匹配，使用MULLS-ICP做精匹配。

作者选取了KITTI,MIMAP,HESAI三个数据集，包含室内外不同的场景以及7种不同的激光雷达进行了实验，实验结果如下表所示：

可以看到在满足实时性的基础上，MULLS-SLAM精度与现有算法比都取得了接近最优的水平。同时作者还分析了不同的特征以及权重对效果的影响。

表3种可以看到顶点特征其实会导致相对精度降低，因此作者也提到了在实际中顶点的特征只用在后端的回环检测与匹配中。另外横梁B特征虽然会导致一定程度的位置精度的损失，但是会抑制姿态的发散。

表4中可以看到当三种权重都有效时效果最优，也证明了权重设计的有效性。

本文提出了一种通用的MULLS-SLAM纯激光雷达建图的方法，设计了多尺度最小二乘ICP匹配算法MULLS-ICP，并且通过实验证明了该方法在室内外以及不同激光设备下都可以在保证实时性的同时保证较低的漂移，证明了该算法的有效性。

Abstract

The rapid development of autonomous driving andmobile mapping calls for off-the-shelf LiDAR SLAM solutionsthat are adaptive to LiDARs of different specifications onvarious complex scenarios. To this end, we propose MULLS, anefficient, low-drift, and versatile 3D LiDAR SLAM system. Forthe front-end, roughly classified feature points (ground, facade,pillar, beam, etc.) are extracted from each frame using dual-threshold ground filtering and principal components analysis.Then the registration between the current frame and the localsubmap is accomplished efficiently by the proposed multi-metric linear least square iterative closest point algorithm.Point-to-point (plane, line) error metrics within each point classare jointly optimized with a linear approximation to estimatethe ego-motion. Static feature points of the registered frame areappended into the local map to keep it updated. For the back-end, hierarchical pose graph optimization is conducted amongregularly stored history submaps to reduce the drift resultingfrom dead reckoning. Extensive experiments are carried out onthree datasets with more than 100,000 frames collected by seventypes of LiDAR on various outdoor and indoor scenarios. Onthe KITTI benchmark, MULLS ranks among the top LiDAR-only SLAM systems with real-time performance.

如果你对本文感兴趣，想要下载完整文章进行阅读，可以关注【泡泡机器人SLAM】公众号。

欢迎来到泡泡论坛，这里有大牛为你解答关于SLAM的任何疑惑。

有想问的问题，或者想刷帖回答问题，泡泡论坛欢迎你！

泡泡网站：www.paopaorobot.org

泡泡论坛：http://paopaorobot.org/bbs/

商业合作及转载请联系[email protected]