MULLS：一种基于多尺度线性最小二乘的激光SLAM算法

点击上方“3D视觉工坊”&＃xff0c;选择“星标”

干货第一时间送达

作者丨paopaoslam

来源丨 泡泡机器人SLAM

标题&＃xff1a;MULLS: Versatile LiDAR SLAM via Multi-metric Linear Least Square

作者&＃xff1a;Yue Pan, Pengchuan Xiao , Yujie He, Zhenlei Shao and Zesong Li

来源&＃xff1a;ICRA2021

编译&＃xff1a;玉玺

审核&＃xff1a;lionheart

摘要

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

代码已开源&＃xff1a; 

https://github.com/YuePanEdward/MULLS

MULLS-SLAM效果示意图

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

特征提取

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

1、将激光点云投影到激光雷达所在水平面上&＃xff1b;

2、将水平面按照固定的分辨率划分成2D栅格&＃xff1b;

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

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

&＃xff08;二&＃xff09;非地面特征提取。对上一步分类出的非地面点做PCA&＃xff0c;根据其特征值计算特征的线性

&＃xff0c;平面性以及曲率&＃xff0c;计算公式如下&＃xff1a;

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

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

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

多尺度线性最小二乘ICP

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

几个关键步骤如下&＃xff1a;

&＃xff08;一&＃xff09;多类别的近邻匹配。核心思想还是最近邻查找&＃xff0c;创新点在于&＃xff1a;

1、近邻搜索时在每个类别中分开搜索&＃xff1b;

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

&＃xff08;二&＃xff09;变换矩阵估计。要点有两个&＃xff1a;

1、对于不同的特征分别采取了不同的残差计算方法&＃xff0c;比如顶点特征V采取点到点距离&＃xff0c;面特征&＃xff08;G&＃xff0c;F&＃xff0c;R&＃xff09;采用点到面距离&＃xff0c;线特征&＃xff08;P,B&＃xff09;采取点到线的距离。如下图所示&＃xff1a;

2、对于不同特征采用不同的权重&＃xff0c;变换矩阵计算公式如下&＃xff1a;

其中&＃xff0c;w为权重。接下来介绍权重的设计。

&＃xff08;三&＃xff09;特征权重设计。特征的权重wi由三部分权重相乘&＃xff0c;分别是&＃xff1a;残差权重wi(residual),平衡权重wi(balanced),强度权重wi(intensity)。

1、残差权重。残差权重固定为pseudo-Huber核函数&＃xff0c;如下所示&＃xff08;k&＃61;1&＃xff09;

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

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

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

3、强度权重。强度差别较大的点降权&＃xff0c;如下所示&＃xff1a;

&＃xff08;四&＃xff09;匹配好坏的度量。最终使用点云的后验标准差&＃xff0c;后验信息矩阵以及非地面点的覆盖率来衡量此次匹配的好坏&＃xff0c;公式如下&＃xff1a;

算法框架

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

实验结果

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

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

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

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

总结

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

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.

本文仅做学术分享&＃xff0c;如有侵权&＃xff0c;请联系删文。

3D视觉精品课程推荐&＃xff1a;

1.面向自动驾驶领域的多传感器数据融合技术

2.面向自动驾驶领域的3D点云目标检测全栈学习路线&＃xff01;(单模态&＃43;多模态/数据&＃43;代码)
3.彻底搞透视觉三维重建&＃xff1a;原理剖析、代码讲解、及优化改进
4.国内首个面向工业级实战的点云处理课程
5.激光-视觉-IMU-GPS融合SLAM算法梳理和代码讲解
6.彻底搞懂视觉-惯性SLAM&＃xff1a;基于VINS-Fusion正式开课啦
7.彻底搞懂基于LOAM框架的3D激光SLAM: 源码剖析到算法优化
8.彻底剖析室内、室外激光SLAM关键算法原理、代码和实战(cartographer&＃43;LOAM &＃43;LIO-SAM)

9.从零搭建一套结构光3D重建系统[理论&＃43;源码&＃43;实践]

10.单目深度估计方法&＃xff1a;算法梳理与代码实现

11.自动驾驶中的深度学习模型部署实战

12.相机模型与标定(单目&＃43;双目&＃43;鱼眼&＃xff09;

13.重磅&＃xff01;四旋翼飞行器&＃xff1a;算法与实战

14.ROS2从入门到精通&＃xff1a;理论与实战

重磅&＃xff01;3DCVer-学术论文写作投稿 交流群已成立

扫码添加小助手微信&＃xff0c;可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群&＃xff0c;旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。

同时也可申请加入我们的细分方向交流群&＃xff0c;目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。

一定要备注&＃xff1a;研究方向&＃43;学校/公司&＃43;昵称&＃xff0c;例如&＃xff1a;”3D视觉 &＃43; 上海交大 &＃43; 静静“。请按照格式备注&＃xff0c;可快速被通过且邀请进群。原创投稿也请联系。

▲长按加微信群或投稿

▲长按关注公众号

3D视觉从入门到精通知识星球&＃xff1a;针对3D视觉领域的视频课程&＃xff08;三维重建系列、三维点云系列、结构光系列、手眼标定、相机标定、激光/视觉SLAM、自动驾驶等&＃xff09;、知识点汇总、入门进阶学习路线、最新paper分享、疑问解答五个方面进行深耕&＃xff0c;更有各类大厂的算法工程人员进行技术指导。与此同时&＃xff0c;星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息&＃xff0c;打造成集技术与就业为一体的铁杆粉丝聚集区&＃xff0c;近4000星球成员为创造更好的AI世界共同进步&＃xff0c;知识星球入口&＃xff1a;

学习3D视觉核心技术&＃xff0c;扫描查看介绍&＃xff0c;3天内无条件退款

 圈里有高质量教程资料、答疑解惑、助你高效解决问题

觉得有用&＃xff0c;麻烦给个赞和在看~