基于三维激光雷达的智能车辆SLAM技术研究

巴浩麟，马晓录，吴立辉

（河南工业大学 机电工程学院，河南 郑州 450001）

安全行驶是智能车辆最重要的性能要求之一。为保证智能车辆的安全行驶，定位精度及其可靠性已成为智能车辆设计中的关键。传统的智能车辆定位技术主要基于卫星定位，其定位精度仅为 5～10 m[1]。通过利用差分技术的实时动态（Real Time Kinematic, RTK）虽然将定位精度提升至厘米级，但在具有峡谷特征的环境下依旧无法获取准确定位，不能满足智能车辆的需求。同步定位与建图（Simultaneous Localization And Mapping, SLAM）[2]在建立地图的同时通过匹配对位姿进行估计，在过去的几年里，智能车辆采用视觉传感器的SLAM技术进行定位取得了重大进展，通过运用快速特征点提取和描述的算法（Oriented Fast and Rotated Brief, ORB）对图像中的特征点进行提取，再对特征点进行匹配定位，但由于其光学性的特点，在缺乏显著特征的环境下，其提取特征能力差，且在恶劣的天气环境下无法正常工作，很难提供可靠的定位。

美国国防部高级研究计划局地面挑战赛[3]推动将多线激光雷达用于智能汽车。于是用于智能汽车的三维激光SLAM应运而生。随后基于三维激光雷达的智能车辆定位技术大力发展。例如：谷歌公司的智能车辆采用激光雷达定位方案，百度Apollo公司采用激光雷达与视觉传感器共同定位的方案等。但当前传统基于概率特征的匹配定位方式很容易陷入局部迭代中，无法获得最优的全局定位。随着三维激光雷达的量产和处理器算力的提升，可以相信三维激光SLAM技术在未来的智能车辆制造和使用中将担负重要角色。故本文着重对三维激光SLAM中的关键模块、部分开源算法以及应用过程中的一些关键问题进行分析总结。

1 三维激光SLAM算法框架及关键模块

基于滤波器的SLAM方案利用从P(xi)中提取一组随机状态样本。

其表达式为

式中，x1:i为从1时刻到i时刻的姿态；u1:i为时刻i-1为机器人在时刻i达到位姿xi的控制信号；z1:i为在未知环境中行驶会观测到的许多观测值，在i时刻观测到的观测值；为一个标准化常数，与位姿无关。

在式（1）中第二行和第三行分别使用了贝叶斯全概率规则和马尔可夫假设。假设P(xi)为在i时刻姿态的先验数据，则可表达为

此式也依赖于贝叶斯全概率规则和马尔可夫假设。贝叶斯分为两个部分：

式（3）表示预测步骤，式（4）表示更新步骤，在预测步骤中，为给定i-1时刻的位姿xi-1和i时刻控制信号ui，估计i时刻的位姿xi。在更新步骤中，对前一步的估计进行误差修正，使机器人能预测更加准确的位置和姿态信息。

基于图优化的SLAM方案是三维激光SLAM主要采取的方案，如图1所示。

图1 三维激光SLAM框架

优化的原理如图2所示。其中三角形代表智能车辆的位姿，六角形代表智能车辆观测到的观测值，位姿与观测值作为图中的节点，实线为边对SLAM系统起到约束的作用。

图2 图优化原理

三维激光SLAM通常由三维激光雷达为主传感器、搭配轮式里程计和惯性测量单元（InertialMeasurement Unit, IMU）等其余车载传感器进行同步定位与建图。基于图优化的三维激光 SLAM框架分为扫描匹配（即激光里程计）、闭环检测、后端优化、地图构建四个部分。扫描匹配计算激光雷达等传感器相邻帧之间的数据关系，估计当前时刻智能车辆的位姿。扫描匹配是一个随时间增长的数据问题，存在误差累积的问题。后端优化对扫描匹配进行修正优化，减少误差的产生。闭环检测是对全局的数据处理，通过对当前时刻与过去时刻的对比，识别是否到达过当前时刻环境，在校准累积误差的同时构建闭环地图。地图构建生成与环境一致的全局地图。

1.1 扫描匹配

目前三维激光SLAM的扫描匹配方法主要分为基于距离、基于概率和基于特征的方法。

1）基于距离的扫描匹配方法，即通过求解两临近时刻点云的欧式距离极小值，求解点云的相对转换关系，如 CHEN等人[4]提出的迭代最近点算法（Iterative Closest Point, ICP）。但ICP假设点云在迭代过程中数量及对应关系不变，当传感器采样频率较低时，这种假设是不正确的。CENSI[5]在ICP的基础上提出Point to Line ICP（PL-ICP），修改误差函数为点到线的距离使得收敛速度更快，精度更高，但对初值更加敏感。LOW[6]提出的Point to Plane ICP（PP-ICP）构建顶点到目标顶点平面的距离为优化函数，通过非线性优化的方式进行优化求解，提高了收敛速度。

Generalized ICP（G-ICP）[7]假设源点云与目标点云中的每个点与其相邻的点组成的集合满足高斯分布，同时假定每个点所处表面均可以拟合为平面，通过法向给目标函数进行匹配赋值，降低了错误匹配的概率，提高了匹配精度和匹配速率。

Normal ICP（N-ICP）[8]除了利用对应点的距离作为约束，还通过计算该点所在面的法向量和曲率作为约束关系，在很大程度上去除了错误的匹配关系，在减少计算量的同时提高了精确度。Implicit Moving Least Square ICP（IMLS-ICP）[9]使用高斯拟合和最小二乘重建出隐含曲面，通过点到对应点在该曲面的投影点距离构建误差函数，提高了精确度。

传统的 ICP及改进算法假设一帧点云中的所有点是在同一时刻获取的，但实际点云是从起始角度到结束角度依次获得的，当传感器频率较低或运动速度较快时，传感器的位姿不是一成不变的。所以，传统的 ICP及改进算法会估计到不正确的位姿。Velocity ICP（VICP）[10]提出在进行对应点集合间的关系计算之前对点云数据进行畸变去除，通过在 ICP迭代求解的同时估计传感器运动速度，通过该速度补偿点云的畸变，该方法有效去除了异常点，提供了更准确的位姿估计。

随着ICP及其改进算法的不断发展，可将ICP算法流程大致归纳，如图3所示。

图3 ICP大致流程图

2）基于概率的扫描匹配方法主要是正态分布变换（Normal Distribution Transformation, NDT）。该方法对点云数据进行体素化处理，建立点云的高斯分布模型，不建立点与点之间的数学关系，极大了缩减了因错误对应关系而对位姿估计产生的误差。

MAGNUSSON等[11]将其应用于三维扫描匹配，该方法首先把三维点云数据划分为大小均匀的立方体，把每个立方体内的点云数据转换为概率密度函数，通过矩阵法求解出转换关系。其流程如图4所示。由于NDT配准算法不需要两组点云完全相同且计算速率高，被广泛应用于三维激光SLAM中。

图4 NDT大致流程图

NDT算法相比ICP类算法有更高的效率，但也存在因初值不佳而陷入局部极值，无法获得最优解的情况。针对此情况，通常在进行NDT流程前增加粗配准算法，对点云进行初始粗配准得到一个初值，以提高NDT配准的精度及消除局部极值。

3）基于特征的方法根据点云的结构，构建不同特点的点云集合，例如平面、边缘等，通过对各点云集合的点云进行匹配，提高计算效率，如图5所示。ZHANG等[12]通过基于曲率的方式提取边缘线特征和平面特征用于配准，实现了激光里程计，在多种环境中均取得出色的表现。刘今越等[13]提出从点云中提取面元，建立不确定性模型，使用隐式最小二乘求解帧间运动和全局位姿定位性能好，实时性好。

图5 基于特征的扫描匹配方法流程图

不同于直接对点云数据进行特征提取，SEHGAL 等人[14]提出将点云数据转化为图像数据，对图像数据进行特征提取，极大地增强了提取出特征的正确性，提高了匹配精度。

1.2 后端优化

后端优化的本质是一个非线性优化问题，可以分为基于滤波器和基于图优化两类。基于滤波器的SLAM方法主要应用于工作场景较小的服务型机器人，对于智能车辆在室外环境下表现效果差。基于图优化的方法将智能车辆的位姿作为节点，位姿之间构建约束即边。图优化方法估计整个路径和地图上的所有位姿x0:i，而不是当前的位姿xi。该方法通过一段时间内的所有观测值zi和控制信号ui，对所有的姿态进行全局最优估计，如图6所示。该方法计算当前位姿xi+1时，将考虑x0和xi之间所有可用的观测值和控制信号。

图6 基于图优化的SLAM方案

王忠立等[15]对基于图优化的后端优化方法作了详细介绍，基于图优化的后端优化方法可以分为基于最小二乘法的优化方法、基于松弛迭代的优化方法、基于随机梯度下降的优化方法及基于流形迭代方法。

1）基于最小二乘法的优化方法目标是求解误差的最小平方和，找一个（组）估计值，使得实际值与估计值之差的平方加权之后的值最小。SLAM 是一个非线性问题，通过对目标函数的泰勒展开式线性化再利用高斯牛顿迭代法或莱文伯格-马夸特（Levenberg-Marquardt, L-M）算法对其进行求解。但在实际应用时不能满足实时性，于是 DELLAERT等[16]提出利用稀疏 Cholesky分解方法对SLAM问题进行求解，极大地增加了实时性。

2）基于松弛迭代的优化方法将构建的图中每个节点与其相邻节点间的关系进行计算，每次迭代计算都遍历全部节点，实验证明了该方法必定收敛于最优解[17]。但当构建的图中约束误差量大时容易陷入多次迭代。

3）基于随机梯度下降的优化方法根据构建的图中随机选取一个约束边作为计算目标函数的梯度下降方向，在每次迭代计算时都朝着正确方向行进，在该方向上进行目标寻优。该方法有较高的鲁棒性且不易陷入局部极值。

4）基于流形迭代的方法不同于假定优化过程在欧氏空间，GRISETTI等[18]提出一种在流形空间中进行优化的思想即我们观察到的数据实际上是由一个低维流形映射到高维空间，由于数据内部特征的限制，一些高维中的数据会产生维度上的冗余，实际上只需要比较低的维度就能唯一地表示。目前，General Graphic Optimization（g2o）是用于流形优化的一个开源工具。

1.3 闭环检测

闭环检测是实现SLAM鲁棒性的关键问题，通过判断是否到达历史环境使地图闭环。BESL等[19]提出ICP算法，直接对当前帧和历史帧的数据逐个匹配，通过重合度判断是否回环，该方法检测精度高但实时性差。HESS等[20]提出子图概念通过帧与子图进行匹配消除累积误差。张剑华等[21]提出一种基于点云片段匹配约束的方法提升了回环效率但精度不足。KIM等[22]提出Scan Context算法通过行列向量分析特征矩阵提升了闭环检测的精确度。李炯等[23]采用MF-RANSAC算法改进Scan Context，利用区域生长算法对扇形栅格化后的点云进行分割，简化特征匹配计算，相比 Scan Context方法提升了60%的效率。

除了通过算法进行闭环检测，研究学家们提出通过其余传感器对激光雷达的难闭环问题进行解决，将其余传感器的信息数据通过图因子的方式加入图优化中对其进行约束和优化达到闭环的目的。例如，通过高精度惯性传感器、视觉传感器等。本文主要说明基于激光雷达的SLAM技术，对其余传感器不做过多介绍。

1.4 地图构建

三维激光 SLAM 通过点云数据构建点云地图，如图7所示，它可以更加详细地描述环境，但因点云数据量过大及无法表示障碍物信息很难直接用于智能车辆的导航。根据不同的使用目的对点云地图进行处理，通过体素滤波和特征聚类可以将点云地图处理为用于定位的特征地图，也可以通过八叉树[24]的方法构建八叉树栅格占据地图用于导航。近些年，研究学者通过卷积神经网络构建训练模型对点云地图进行分割，检测出运动目标实现了避障的功能。

图7 点云地图

2 三维激光SLAM算法

目前开源的三维激光SLAM包括雷达定位与建图（Lidar Odometry and Mapping, LOAM）、Lightweight and Groud-Optimized LOAM（LeGOLOAM）、Multi LOAM（M-LOAM）、Fast LOAM（F-LOAM）、Range-Monte Carlo Localization（Range-MCL）、Multi-Metric Linear Least Square（MULLS）等。

2.1 LOAM算法原理

LOAM 算法[12]提出了一种实时的 SLAM 方法，通过两个算法将定位与建图分成两个问题进行求解。定位算法以高频率得到一个精度较低的里程计，建图算法以低频率构建精度较高的地图数据，二者结合实现一个实时高精度的定位。

算法首先基于点的曲率进行特征点的提取，再对所提取的点进行分类，分为边缘点与平面点。然后进行特征匹配，通过scan to scan的方式对每帧扫描数据的特征点进行匹配，求解得到一个旋转矩阵R和一个平移矩阵T。当获取到若干相邻帧的位姿信息后，将位姿信息与全局地图进行匹配，从而优化位姿信息得到一个更加精准的定位数据。但是由于其缺少闭环检测功能，积累误差会随着时间的增长不断增长。

2.2 LeGO-LOAM算法原理

LeGO-LOAM 算法[25]提出了一种基于地面优化的轻量级SLAM方法，实现实时六自由度里程计。首先根据激光雷达的水平精度和线数，将点云数据投影为一幅深度图像，然后进行地面的提取，这一过程去除了一些不稳定的特征点。使用L-M 算法进行优化计算位姿。同时利用 Point Cloud Library（PCL）库中基于半径的近邻搜索算法，实现了闭环检测功能。

2.3 M-LOAM算法原理

基于激光雷达的方法常受到数据稀疏和垂直视场有限的问题。针对这些问题，M-LOAM算法[26]提出了多激光雷达的SLAM技术。整个系统首先对点云进行分割聚类、去除噪声、提取边缘特征和平面特征。其中分割与LeGO-LOAM相同，特征提取与LOAM相同。然后进行系统初始化，不需要事先进行配置，对多个激光雷达进行标定，移植性得到了提升。同时提出了一种基于滑动窗口的里程计，联合利用多个激光雷达数据，减少了飘移量。

2.4 F-LOAM算法原理

基于激光雷达的SLAM通常将问题分为两个部分，scan-to-scan匹配和scan-to-map优化，这两个模块依赖迭代的方法进行计算，大量的迭代降低了计算效率。针对这个问题，F-LOAM算法[27]提出了采用非迭代两步法实现畸变补偿降低计算效率。其假设上一帧的运动速度作为当前帧的预测，对当前帧所有特征点进行去畸变，得到位姿估计后利用得到的位姿重新对特征点进行去畸变。相比较LOAM和LeGO-LOAM，在一帧数据的处理周期中，有效降低了参与迭代的点数和迭代的次数，提升了实时性。

2.5 rang-mcl算法原理

range-mcl算法[28]的主要思路是将地图利用结构更加紧凑的三角形网格对地图进行表示。range-mcl不使用原始的点云数据，而是对三维激光雷达扫描的数据通过球坐标进行投影生成深度图像。然后将用三角形网格表示的地图和当前扫描得到的深度图像进行合成渲染，同时建立一种新的观测模型，通过粒子滤波实现基于先验地图的精确定位。

其中球面投影法的原理如下：

式中，r为扫描范围；f=fup+fdown为传感器垂直角度；ω和h为将点云投影到顶点图VD的宽度和高度。当给定VD和每个坐标（u,v）上点的范围r，即生成深度图像。

2.6 MULLS算法原理

针对现有的激光SLAM缺乏通用性，MULLS算法[29]提出了一种通用的三维激光SLAM系统。首先对点云进行投影到参考平面，将参考平面划分为等格的2D体素格，记录每个体素格内最小点的高度和对应的三维体素格，根据值的大小判断其是否属于地面，再使用RANSAC对地面拟合进行优化。地面拟合完成后使用主成分分析（Principal Component Analysis, PCA）的方法进行特征提取，最后利用多度量线性最小二乘迭代最临近点算法实现当前扫描帧与目标点云的配准。构建点点、点线、点面误差，通过高斯马尔可夫参数估计的方法进行优化求解。

2.7 几种开源三维激光SLAM的优缺点

表1对几种开源三维激光SLAM方法的优缺点进行了比较。

表1 开源三维激光SLAM方法的优缺点对比

3 三维激光SLAM应用中的关键问题

激光雷达具有不受光照影响可靠性强的特点，但也具有局部点云稀疏、Z轴的漂移以及动态对象引发的噪声影响的问题，这成了三维激光SLAM在应用于智能车辆时需要解决的关键问题。

3.1 地面分割

对于智能车辆车载激光雷达由于其工作在室外环境，地面分割起到了关键性作用，分割的地面对Z轴的位姿进行约束，同时以地面为基准可以过滤掉一些噪声及对局部稀疏点云进行聚类。

地面分割方法通常分为基于栅格地图、基于平面模型及基于深度图像。基于栅格地图的方法通过计算投影到栅格的差值，根据人为设定阀值去判断是否为地面，该方法简单高效但栅格大小以及阀值单一易造成过分割或欠分割[30]。HIMMELSBACH等[31]对原始点云进行划分栅格，对离散点进行直线拟合，通过距离分割出地面，快速且高效。基于平面模型的方法通过建立一个模型表示地面，将符合该模型的点归为地面点。李孟迪等[32]通过抽样随机一致性算法构建平面模型对地面进行分割，可以很好地处理异常值问题。基于深度图像的方法通过将三维点云转换为深度图像，利用相邻两点连线与水平线间的夹角构建阀值，当角度小于经验值时将该点归为平面。但上述方法都存在阀值设置的问题，不能自适应。张凯等[33]针对复杂道路提出一种基于自适应阀值的三维激光点云地面分割的方法，通过构建路面波动幅度方程，实现了自适应阀值。朱株等[34]提出一种基于马尔可夫随机场的路面分割算法，通过采用最大模糊线段法对每条激光雷达扫描线在X-Y平面投影，利用角点定位线段端点，建立能量方程，用图分割的方法将线段标记为地面与非地面。

3.2 运动目标检测

现在的大多数 SLAM 方法都是假设静态环境，对于智能车辆而言其所处的环境不可能是静态环境，于是环境中的运动目标会对SLAM过程产生噪声导致定位和建图精度下降甚至失真。CHEN等[35]提出对每帧点云集成语义信息利用RangeNet++分割出动态物体并进行剔除，提高了构建地图的精确度。ZHAO等[36]通过建立动态目标检测模块，通过卷积神经网络（Convolution Neural Network, CNN）从每帧扫描中剔除动态目标，但实时性较差。王忠立等[37]提出融合激光雷达与惯性传感器，利用惯性传感器补偿激光雷达运动误差，在补偿后的点云中通过 Fully CNN（FCNN）检测运动目标基于无迹卡尔曼滤波的方法区分动、静目标。

4 总结与展望

激光SLAM的精度和可靠性是智能车辆实现自主导航的前提。传统的SLAM模块还有很大的提高空间。实时的高精度SLAM方法将是未来智能车辆大众化的关键。

随着智能车辆的大力发展，激光SLAM发展趋势大致如下：

1）激光SLAM与深度学习的结合。深度学习具有学习能力强，可应用场景广，可移植性强的特点。目前二者的结合多用于对动态物体的剔除，将扫描配准与深度学习相结合，实现端到端的帧间估计将是一个热点问题。

2）对于场景复杂的环境，多传感器的融合必然是一个趋势。视觉传感器具有良好的语义信息可以给激光雷达提供高精度的深度信息帮助激光进行闭环。高精度惯性传感器在短时间内的运动估计十分准确，可以用来修正激光雷达通过扫描匹配方式得到的位姿估计误差。

3）在范围较大的环境下，多车同时构建SLAM 是一个必然趋势。目前，通过单个智能汽车构建局部地图，再对所有的局部地图进行融合以达到多车 SLAM，但其在动态环境中易出现错误。因此如何解决多车SLAM中动态环境下的错误是一个研究难点。