基于激光雷达的SLAM和融合定位方法综述

代凯 申棋仁 吴官朴 赵阳 马芳武

（1.吉林大学 汽车仿真与控制国家重点实验室，长春130022；2.吉林大学 汽车工程学院，长春130022）

主题词：激光雷达 无人驾驶 同步定位和建图 多传感器融合

缩略语

SLAM Simultaneous Localization And Mapping

LiDAR Light Detection and Ranging

GNSS Global Navigation Satellite System

IMU Inertial Measurement Unit

RTK Real Time Kinematic

ESKF Error State Kalman Filter

LIO LiDAR Inertial Odometry

UWB Ultra Wide Band

V2I Vehicle-to-Infrastructure

LOAM LiDAR Odometry and Mapping

NDT Normal Distribution Transform

DR Dead Reckoning

VINS Visual Inertial System

MSCKF Multi-State Constraint Kalman Filter

1 引言

自动驾驶是当下研究的热点，车辆定位是自动驾驶必须解决的关键问题，良好的定位系统不仅能够有效帮助车辆完成自动驾驶功能，更能够有效提高车辆的安全性。目前，无人驾驶车辆广泛采用多传感器融合定位方案，以覆盖不同的行驶工况。主流方案有2种，分别为基于传感器的定位方法和基于车联网的定位方法[1]。使用多传感器融合定位时，主要采用的是基于GNSS、IMU、摄像头、激光雷达、车辆编码盘等传感器融合定位的方案。IMU/GPS 融合定位方法，能够弥补GPS 多路径干扰、信号丢失等问题，增强基于卫星定位方法的鲁棒性、稳定性。文献[2]提出了LiDAR/GNSS/IMU 组合导航系统，采用ESKF 滤波器融合64线激光雷达定位信息、GNSS-RTK 定位信息、IMU 信息，实现5～10 cm 的连续稳定车道级定位。文献[3]提出了紧耦合3D激光惯性里程计（LIO），有效解决了纯激光雷达定位长航程数据漂移、环境特征稀疏定位失效等问题。在基于车联网定位研究领域，文献[4]提出了基于多模型滤波算法的车辆协作定位增强方法，有利于提高车辆在“高楼峡谷”、隧道等弱卫星信号区域的定位精度；文献[5]提出了基于UWB的单锚点V2I定位算法，有利于提高基于GPS、V2I组网的车联网定位精度。

在众多的定位方法中，利用激光雷达构建地图，并基于地图定位的方法被广泛运用在无人驾驶车辆开发中。在美国早期举办的DARPA 无人车挑战赛中，包括斯坦福等在内的高校利用激光雷达实现建图及定位[6]，并借此提高定位精度及鲁棒性，取得了较好的成绩。文献[7]提出了一种改进的激光里程计，结合了文献[8]和[9]的优点，采用SegMap 基于已知地图检测闭环，与LOAM[8]的里程计进行融合，减少纯激光定位漂移问题，获得良好的定位效果。

本文主要包括3 部分内容：首先对基于激光雷达的同步定位和建图进行讨论，其次对如何利用车辆模型辅助无人驾驶车辆进行定位展开叙述，最后对基于地图的融合定位技术进行分析，并对应用于无人驾驶车辆的激光SLAM及融合定位技术前景提出展望。

2 基于激光雷达的同步定位和建图

高精度地图是无人驾驶车辆开发与使用中必不可少的一环，也是定位和导航规划的前提，当前，激光雷达是构建高精度地图必不可少的传感器。在文献[6]中，作者采用GPS、IMU、轮式里程计、激光雷达等多种传感器融合生成高精度地图，也有像文献[8]这样采用纯激光雷达构建地图的方法。在地图的构建方法中，最常用的是同步定位和建图（SLAM），SLAM 又分为前端和后端，前端主要完成点云配准工作，本质上来说是一个激光里程计，里程计是靠当前帧和地图参考帧的帧间匹配算法完成位姿估计的，但是位姿估计会随着里程计的增加产生漂移，导致建图不准，后端会建立外部观测约束，以此来减少前端里程计产生的漂移[10]。

2.1 激光雷达前端配准算法

2.1.1 基于最近邻点迭代（ICP）的点云配准算法

ICP(Iterative Closest Point )算法被广泛应用于初始位姿已知的三维点云几何对准，其原理如下：假设2个点云数据集合P 和G，要通过P 转换到G（2 组点云存在局部几何特征相似的部分），可以通过P 叉乘四元矩阵进行旋转平移变换到G，或者采用SVD 法将P转换到G 位置，总体思想都是需要一个4×4 的旋转平移矩阵。对于每次旋转平移变换后计算P 的所有点到G 对应点的距离，用最小二乘法求出最小二乘误差，看是否在要求的范围内，如果最小二乘误差小于设定的值，则计算结束，否则继续进行迭代。随着匹配效率和精度的要求逐步提高，衍生出了多种改进算法[11]。文献[12]提出了Generalized-ICP（GICP），将ICP 和PPICP 融合到了一个概率框架下，具有更灵活的参数配置方案，相比于单个系统，其鲁棒性更高；文献[13]提出了一种基于尺度的扫描匹配算法，通过实验证明Point-to-Line ICP（PLICP）在计算效率和匹配精度方面均有提高，但该方法存在初始化误差较大的缺点，为解决该问题，文献[14]提出了一种改进的PLICP 算法，提高了初始化匹配时的鲁棒性和计算效率。

ICP算法被广泛运用在SLAM算法中，文献[15]提出了一种基于ICP 的SLAM 算法，该算法仅利用激光点云数据完成定位和建图工作；在文献[15]的基础上，文献[16]加入了3自由度IMU 信息进行点云校正和位置预测，该方法对旋转运动具有较强的鲁棒性，适用于传感器位置和方向变化幅度较大的场景。

2.1.2 基于正态分布变换（NDT）的点云配准算法

正态分布变换（NDT）算法[17]是一个配准算法，它应用于三维点的统计模型，使用标准最优化技术来确定2个点云间的最优的匹配，因为其在配准过程中不利用对应点的特征计算和匹配，所以时间比其他方法快。

文献[18]提出了一种3D-NDT 扫描配准算法，该方法改进了由Biber[17]等人提出的2D-NDT算法，作者将该算法与标准ICP 算法进行了定性和定量的比较，对比结果表明该方法速度更快，可靠性更高；文献[19]提出了一种快速、准确的点云配准算法，显著提高了计算效率和精度，此外，作者还提出了一个基于3DNDT的全局描述子，用于实现迭代算法可靠的初始位姿估计。为了评价NDT 和ICP 的性能，文献[20]通过实验对2 种三维扫描配准算法进行了深入的对比分析，并提出了一种改进的NDT算法。

由于NDT 3D 点云配准算法计算效率高，初始化效果较好，因此备受开发者青睐，hdl_graph_slam 是使用3D LiDAR的实时6 DOF（Degree Of Freedom）SLAM的开源ROS 软件包，它基于NDT 扫描匹配的方法实现位姿估计和闭环检测。在理论研究方面，文献[21]提出了一种基于NDT 的SLAM 方法，在动态环境中，该方法能够实现长航程高精度建图和定位；文献[22]介绍了基于NDT 的图优化SLAM 在复杂城市工况下的性能，经过试验分析，结果表明，采用NDT 的激光SLAM 算法，其性能与交通环境和城镇化率有明显相关性，在复杂的交通环境下，定位和建图性能明显下降。

2.1.3 基于特征的点云配准算法

激光点云特征提取有2 种思路：模型驱动的模式和数据固有特征模式。

模型驱动模式的核心是确定点云数据与模型在全局或局部的匹配关系，典型的参数域提取方法有Hough、Randon 等，原本是用在二维图像处理中，用于检测直线、圆形、椭圆等，后有研究人员将其引入到点云数据处理中，可用于检测点云中蕴含的空间平面、球体、柱体等。但此种方法缺乏普适性，只能实现对特定参数的提取，而且需要有集合模型。

利用数据固有特征描述点云特征不需要预先建模，几乎可以应用于所有场景，即充分利用几何特性及微分几何特性，例如曲面的平均曲率、高斯曲率、表面法向等。根据点云选取范围的不同，可分为全局特征和局部特征，对于激光雷达特征，由于无法预先获取全局场景信息，采用局部特征更为合适。由于点云表面的法线、曲率等计算受点云噪声影响较大，因此，在进行特征提取时，需要对点云进行平滑操作，去除噪声的同时保留特征，主流的方法有：基于法向约束和PCA[23]的面拟合、特征边缘识别算法；采用KNN[24]邻域算法实现法向量的多尺度平滑计算，并利用平滑后的法向量重构表面提取特征。

在基于特征匹配的前端激光里程计算法中，LOAM[8]是纯激光匹配算法中较为出色的，长期霸占KITTI[25]数据集纯激光里程计算法榜首，作者利用角和面2种特征，建立特征点云地图，新加入的点云特征与已建立的2 种特征点云地图匹配得到相对精确的位姿。为获得最优位姿，通过点-线和点-面关联共同构建约束,以点线距离和点面距离作为损失函数，基于非线性最小二乘的方法进行优化。LeGO-LOAM[26]在LOAM 的基础上对地面特征进行提取，主要利用平面和边特征，提出了一种优化的轻量级里程计和建图方法，相比于LOAM，该方法减少了计算资源，且精度有所提升。文献[27]提出了一种LiDAR-IMU 紧耦合融合方法，在LOAM 的基础上加入IMU，通过联合优化LiDAR、IMU 测量值，配合旋转约束优化算法（LIOmapping），使雷达位姿与全局地图对齐，有效减少长航程里程计漂移，得到良好的建图效果。LIO-SAM[28]在LeGO-LOAM[26]、LIO[27]的基础上，采用因子图优化算法，结合滑动窗口（Sliding window）方法，融合了IMU预积分因子、激光里程计因子、GPS因子、闭环检测因子，实现多传感器融合及全局优化，以获得良好的位姿估计和地图构建效果，作者对该算法进行了开源，在该算法框架下，可以再加入航向计、高度计等绝对测量仪，辅助降低里程计漂移。

2.2 激光雷达SLAM后端优化

对于一个建图系统来讲，有了后端才有核心，前端能够根据相邻的2帧点云判断出此时此刻的位姿，且可以给出一个增量式的地图，由于不可避免的误差累计，这个地图在长时间内是不准确的。而SLAM 致力于构建一个长生命周期的可靠的解决方案，因此只有前端是远远不够的。当地图增长到一定程度后，累计误差会使后来的数据越来越不准确。这时我们需要把所有地图数据放到一起做一次完整的优化，从而降低各部分的误差。

后端优化有很多种方案，过去采用以扩展卡尔曼滤波（Extended Kalman Filter，EKF）为主的滤波器方案，现在大多都采用非线性优化方案。EKF由于假设了马尔可夫性质，只利用前一时刻状态来估计当前状态的值，很难做到全局的优化。目前常用的非线性优化方法则是把所有数据都考虑进来，进行全局优化，虽然会增大计算量，但融合精度较高。

2.2.1 基于滤波法的后端

在计算资源受限、待估计量比较简单的情况下，以扩展卡尔曼滤波（EKF）为代表的滤波法比较有效，通常应用在激光SLAM中。但它的一大缺点是存储量和状态量是平方增长关系，因此不适合大型场景。而现在基于视觉的SLAM 方案，路标点（特征点）数据很大，所以此时滤波的方法效率非常低。其余滤波算法也被应用在SLAM 算法中，文献[29]提出的UKFSLAM 降低了计算的复杂度，解决了状态估计不一致问题，应用基于可观性约束的UKF 算法，保证了滤波器的系统模型有一个适当维数的不可观测子空间；文献[30]提出了一种自适应减少粒子数的粒子滤波器，以分布式计算精度的方式，有选择地进行重采样，减少粒子耗尽问题，在室内外大尺度场景中取得了良好的建图效果。

2.2.2 基于优化法的后端

当前，优化法是SLAM 算法后端的主流方案，SLAM 的状态估计是一个非线性最小二乘问题，一般使用梯度下降法、高斯牛顿法(G-N)、列文伯格-马夸尔特方法(Levenberg-Marquardt Method, LM)、以及Dog-Leg 方法等。在实际应用中，通常使用谷歌的Ceres 库以及基于图优化的g2o 库[31]来实现这些求解方法。基于优化的方法一般利用所有的观测信息来优化估计机器人完整的运动轨迹，因此也称为全SLAM 方法。

文献[32]提出的2D激光SLAM算法，采用Ceres求解器实现后端优化，其开源的Cartographer 算法在2D SLAM 中应用较为广泛。在基于优化法的SLAM 后端中，g2o的应用是更为广泛的，基于g2o的SLAM方法，是利用图模型对SLAM 问题进行建模，模型中的节点对应不同时刻机器人及环境组成系统的状态，图优化的边则描述了系统状态(节点)之间的约束关系[33]。当前基于紧耦合的激光SLAM 算法LIO[27]、LIO-SLAM[28]均采用g2o方法实现后端优化。

3 基于车辆模型定位技术

基于车辆模型的定位技术在自动驾驶车辆上已被广泛使用。无论是基于传感器定位还是基于车联网定位，车辆模型均可为融合定位系统提供稳定可靠的数据源，它不需要依赖外部条件，自主性和适用性较强，且不增加车辆硬件成本。文献[34]分别对2 自由度车辆运动学、动力学模型进行理论分析和仿真试验，并基于2自由度车辆模型将Kalman滤波器运用到车辆定位方法中，以提高基于航迹推算（DR）定位的精度；文献[35]分别分析了2 自由度车辆运动学与动力学模型的适用范围与优缺点，并利用交互式多模型（IMM）概率算法结合车辆运动学与动力学模型，提高基于车辆模型定位精度；文献[36]采用2 自由度车辆运动学模型与GPS、LiDAR 融合定位，并成功应用在高速自动驾驶赛车上；文献[37]采用2 自由度车辆模型进行航迹推算（DR），并利用GNSS 定位结果与DR结果进行对比，得出DR 精度高，但准确度较低的结论，采用无迹卡尔曼滤波（UKF）融合GNSS、DR 定位结果，仿真结果表明，融合算法能够有效弥补2者的缺点，提高定位精度及稳定性；文献[38]提出了同时采用2自由度车辆运动学模型和车辆动力学模型辅助惯导系统的方法，以提高导航定位精度，增强定位系统稳定性；文献[39]以3 自由度非线性车辆动力学模型为基础，建立了车辆动力学模型辅助的惯性导航系统，用于提高车辆航向角精度；文献[40]以9 自由度车辆动力学模型为基础，为车载融合定位系统提供状态信息。

以上基于车辆模型辅助的定位方法在各自领域均不同程度地提升了自动驾驶车辆的定位性能，但在车辆模型选择上，并未系统论述选择车辆模型的依据。文献[41]从理论角度分析了车辆模型对导航系统的性能影响，基于2自由度车辆运动学模型拓展了无侧滑、有侧滑、有明确指向性侧滑3个类型车辆模型，并进行了仿真试验，分析了不同模型的定位误差，得出高阶车辆模型对导航定位性能有正面促进作用，且需要合理选择参数可观性较高的车辆模型。

车辆模型在SLAM 领域中也有应用。文献[42]提出了一种结合2自由度车辆动力学模型的视觉惯性估计器，以解决车辆在匀速、匀加速等工况下视觉惯性导航系统（VINS）不可观测导致定位精度降低等问题。作者通过将车辆运动学信息扩展到IMU 预积分文献[43]模型中，提高VINS 的初始化能力，同时采用基于滑动窗口的优化法，提高车辆动力学辅助VINS系统的里程计精度；同理，为解决VINS 性能退化问题，文献[44,45]在VINS的基础上加入轮速信息；文献[46]在滤波紧耦合算法MSCKF的基础上，加入车辆阿克曼模型，结合标定后的车速、转角信息，提高了车辆视觉惯性里程计精度。在车辆工程领域，车辆滑移是比较常见的现象，如何准确估计滑移率，给轮速合理的补偿，以提高轮速精度，是值得探讨的问题。文献[47]提出了一个带有滑移的车辆模型，准确估计车辆滑移率，避免由于车辆滑移导致VINS 系统定位精度降低的问题。文献[48]采用交互式多模型扩展卡尔曼滤波器融合车辆运动学模型和车辆动力学模型，实现车辆模型优势互补，将该模型作为预测模型，以视觉、IMU、GPS 为观测更新模型，实现多传感器融合，以提高车辆定位性能。

4 基于地图的多传感器融合定位技术

基于地图的定位是无人驾驶车辆中常用的技术方法，以激光雷达为例，通过当前激光扫描与预先构建的点云地图匹配，可获得无人驾驶车辆在地图中的位置。对于室外无人驾驶车辆来说，由于环境特征减少或相似的环境特征导致扫描匹配定位信息错误，容易造成重大安全事故，因此需要融合来自GPS、IMU等外部传感器信息，提高定位精度和鲁棒性。

4.1 地图种类

一般地图分为2类，第1类是平面地图，即依赖地理信息系统上的图层或平面的地图，例如高精度地图（HD map），如图1。这类地图一般分4层：第1层代表静态地图，包含路网、建筑地标等信息，第2层包含准静态信息，如路侧基础设施、可变限速等，第3层包含通过与基础设施交互获得的瞬间动态数据，如天气信息、红绿灯时间、拥挤程度等，第4层是一个高度动态的局部动态地图，通过V2X 通讯可以获得车与车之间、车与路之间、车与云之间等万物互联的信息。第2类是点云地图，一般由3D扫描仪或激光传感器生成，用来表示物体在3D 空间中的外表面特征信息，数据包含X、Y、Z坐标及点云强度信息[1]。

图1 HD map分层示意[49]

图2 车载激光雷达获取的点云地图

4.2 基于地图的定位方法

地图是无人车导航、定位的基础。在平面地图中，车辆从车载传感器和V2X设备收集有关环境和其它道路使用者的信息，与预先存储的高精度地图（HD map）进行对比，从而实现车辆自身定位。

点云地图是目前无人驾驶车辆中应用较为广泛的地图，可以实时与当前激光扫描帧进行匹配。文献[50]提出了3D-NDT扫描匹配算法，相比于ICP点云扫描匹配算法[51]，NDT算法将扫描点分割为体素，每个体素格内的扫描点近似为正态分布，其效率及精度更高，且更容易完成初始化，当前点云与预先存储的点云地图扫描匹配，即可得到车辆相对于点云地图的位姿。文献[6]提出利用粒子滤波器将当前激光测量值预先存储的地图进行关联，实验结果表明，该方法的相对精度比传统的基于GPS-IMU 定位的方法高出一个数量级，在10 cm范围内实现了可靠的实时定位，提升了车辆在GPS干扰环境、恶劣天气和密集交通条件下的定位性能。相比于只利用X、Y、Z 坐标点进行距离匹配的方式，百度自动驾驶团队[2]提出了利用激光雷达强度值和高度信息与点云地图匹配的方法，显著提高了定位性能。

4.3 多源融合定位方法

利用互补性的传感器进行融合定位是当前无人驾驶车辆常用的技术，LiDAR、IMU、GNSS、Camera 是较为常用的传感器。文献[2]提出了一种基于误差状态卡尔曼滤波器的多传感器融合定位系统，主要融合来自激光雷达与地图匹配的位姿信息、RTK-GNSS 信息、IMU 信息，输出位置、速度、航向（PVA）信息，实现稳定可靠的厘米级定位。除高楼、密集交通、极端气候等复杂环境，在一般的交通场景下，现有传感器（例如差分惯导）采用一个标准的滤波的方式即可获得较高的定位精度。但是，要实现全场景的高精度定位，比如城市密集区域，一个稳定的数据融合框架非常重要。文献[52]提出了基于因子图优化的融合定位框架，并且加入了场景变化检测和实时激光惯性里程计，即使在GNSS 信号丢失或者高精度地图由于环境变化失效的情况下，激光里程计依旧可以在短时间内提供可靠的里程计信息；此外，作者提出的图优化融合定位框架，相比于滤波法，可以有效减少线性化的误差，且多次迭代可以更好地趋近最优解，在复杂场景下，定位系统稳定性更高。

5 结论

纵览激光SLAM和融合定位30余年的研究发展，不难发现，从传感器配置到算法均取得了长足发展。

（1）在传感器配置方面经历了从单一传感器到多传感器互补的发展历程，激光雷达经历了从2D 到3D 的变化，GNSS 定位精度经历了从米级到厘米级的提升；

（2）在激光雷达扫描匹配算法方面，经历了从ICP迭代匹配到NDT和基于特征匹配，再到当前基于点云深度学习匹配的方式；

（3）数据融合算法从线性滤波法逐步向非线性优化方法发展，其中因子图优化与滑动窗口结合的方法以其良好的实时性和鲁棒性，被广泛应用在各类融合定位和建图系统中。

未来，以激光雷达为基础的SLAM 和融合定位系统仍然是无人驾驶技术发展的重要研究方向，尤其是基于点云的深度学习定位和基于非线性联合优化的算法，将加速该技术的发展；在高速无人驾驶车辆中，如何利用车辆运动学和动力学模型弥补自动驾驶车辆定位系统的稳定性也会是值得探讨的课题。