基于车载激光雷达的点云匹配定位

摘要
定位是自动驾驶车辆自主导航技术的关键问题之一。

保持稳定、精确、实时的定位能够保证自动驾驶车辆行驶的安全。

自动驾驶车辆的定位方式主要有：基于全球卫星导航系统(Global Navigation Satellite System, GNSS)的定位方式，基于激光雷达点云地图匹配的定位方式等。

目前在自动驾驶领域，全球卫星导航系统是使用最广泛、应用最成熟的定位技术，自动驾驶车辆依靠GNSS定位技术能够在大多数场景下完成精确定位与导航，但仍然存在卫星信号被周边环境的建筑物和树木等障碍物遮挡导致定位失效或者偏移等状况。

目前为了提高定位精度，利用GNSS中的实时差分定位技术（Real-time Kinematic, RTK）结合惯性导航系统（Inertial Navigation System, INS）进行多传感器融合定位，以满足自动驾驶车辆高精度定位的需求。

但是，在复杂地形环境下对定位的干扰依然不可避免，因此在自动驾驶领域基于全球卫星导航系统的定位方式在地形环境复杂的场景下仍受到很大的限制。

车载激光雷达采集的点云地图不仅能够提供全方位的道路特征信息，同时，能够为自动驾驶车辆实现厘米级定位提供数据基础。

基于点云地图匹配的定位技术能够不受天气、地形等环境因素的影响，在定位测算中保持相互独立，在复杂环境下替代GNSS定位，为自动驾驶车辆的安全行驶提供保障。

为此，本文对基于点云地图匹配的定位技术进行研究，并对经典的点云匹配算法进行改进，对比不同算法在复杂环境下的定位精度和效率，验证基于点云地图匹配的定位技术的可行性。

本文的研究数据选取于目前国际上最大的自动驾驶场景数据集(KITTI)中的城市环境点云数据。

首先，对点云数据进行预处理，利用LOAM算法预先构建出点云地图，作为匹配定位实验的基础数据，利用PCL库中的滤波算法对点云数据中存在的噪声点进行过滤优化，增强匹配定位的稳定性；然后，分别利用迭代最近点(Iterative Closest Points，ICP)算法和正态分布变换(Normal Distribution Transform，NDT)算法进行单帧点云与点云地图的匹配定位实验，分析两种算法的稳定性、精度以及效率；最后，在当前完成匹配定位的基础上进行研究和算法改进，利用Gauss-Newton 法改进ICP算法，同时利用K-D树加速查找对应点，并且和NDT算法进行集成，定义为NDT-ICP算法，进行匹配定位实验。

实验结果表明，改进后的NDT-ICP算法结合了ICP算法的高精度和NDT算法的高效率，在定位精度上待配准点云与源点云地图之间的对应点平均距离为4.52cm，可以满足自动驾驶车辆的定位需求；在定位效率上，每一个单帧点云进行匹配定位消耗的时间平均在6.47s，在自动驾驶车辆定位的实时性上还有提升的空间。

关键词：迭代最近点算法，正态分布变换算法，Gauss-Newton法，K-D树，NDT-ICP 算法
Abstract
Positioning is one of the key issues of autonomous navigation technology for autonomous vehicles. Maintaining stable, accurate and real-time positioning can ensure the safety of autonomous vehicles. The main positioning methods for autonomous vehicles include: positioning methods based on the Global Navigation Satellite System (GNSS), positioning methods based on lidar point cloud map matching, etc.
At present, in the field of autonomous driving, the global satellite navigation system is the most widely used and most mature positioning technology. Autonomous vehicles can complete precise positioning and navigation in most scenarios rely on GNSS positioning technology. However, there are still situations where the satellite signal is blocked by obstacles such as buildings and trees in the surrounding environment, resulting in positioning failure or offset. At present, in order to improve the positioning accuracy, the Real-time Kinematic (RTK) in GNSS combined with the inertial navigation system (INS) is used for multi-sensor fusion positioning to meet the needs of high-precision positioning of autonomous vehicles. However, the interference to positioning in a complex terrain environment is still inevitable. Therefore, the positioning method based on the global satellite navigation system in the field of automatic driving is still greatly restricted in the scene of complex terrain environment.
The point cloud maps collected by the on-board lidar can not only provide a full range of road feature information, but also provide a data basis for centrifugal positioning of autonomous vehicles. The positioning technology based on point cloud map matching can be independent of weather, terrain and other environmental factors. It can be independent of each other in positioning calculations. It can replace GNSS positioning in complex environments and provide guarantee for the safe driving of autonomous vehicles. To this end, this paper studies the positioning technology based on point cloud map matching, and improves the classic point cloud matching algorithm, compares the positioning accuracy and efficiency of different algorithms in complex environments, and verifies the feasibility of positioning technology based on point cloud map matching.
The research data of this paper is selected from the urban environment point cloud data in the largest international autonomous driving scene data set (KITTI). First, pre-process the point cloud data, use the LOAM algorithm to construct a point cloud map in advance, as the basic data of the matching positioning experiment, use the filtering algorithm in the PCL library to filter and optimize the noise points in the point cloud data, and enhance the matching Stability of positioning; Then, the iterative closest point (ICP) algorithm and the normal distribution transform (NDT) algorithm are used to perform a single frame point cloud and point cloud map matching positioning experiment. Analyze the stability, accuracy and efficiency of the two algorithms; Finally, based on the current completion of matching positioning, research and algorithm improvement are performed, the Gauss-Newton method is used to improve the ICP algorithm, and the KD tree is used to accelerate the search for the
corresponding point, and integrated with the NDT algorithm. It is defined as the NDT-ICP algorithm to perform matching positioning experiments. The experimental results show that the improved NDT-ICP algorithm combines the high precision of the ICP algorithm and the high efficiency of the NDT algorithm. The average distance between the corresponding point between the point cloud to be registered and the source point cloud map is 4.52 cm in terms of positioning accuracy. It can meet the positioning needs of autonomous driving vehicles. In terms of positioning efficiency, the average time spent by each single-frame point cloud for matching positioning is 6.47s, and there is room for improvement in the real-time positioning of autonomous driving vehicles.
Keywords: Iterative Closest Point Algorithm, Normal Distribution Transformation Algorithm, Gauss-Newton Method, K-D Tree, NDT-ICP Algorithm
目录
北京建筑大学硕士学位论文原创性声明
北京建筑大学硕士学位论文使用授权书
摘要....................................................................................................... I I ABSTRACT (III)
第1章绪论 (1)
1.1研究背景与意义 (1)
1.2自动驾驶车辆定位技术研究现状 (3)
1.3研究内容与组织结构 (6)
第2章点云地图匹配定位算法 (8)
2.1迭代最近点算法 (8)
2.1.1算法原理与特性 (8)
2.1.2算法发展现状 (10)
2.2正态分布变换算法 (10)
2.2.1算法原理与特性 (10)
2.2.2算法发展现状 (12)
2.3NDT-ICP算法 (13)
2.3.1NDT-ICP算法流程 (14)
2.3.2NDT算法粗配准 (15)
2.3.3改进ICP算法精细配准 (16)
第3章三维点云数据预处理 (21)
3.1三维点云数据来源 (21)
3.2三维点云地图创建 (22)
3.2.1LOAM建图算法原理 (22)
3.2.2LOAM算法建图步骤 (24)
3.3非地图要素过滤 (25)
第4章点云匹配定位实验与结果分析 (28)
4.1点云匹配定位结果评价方法 (28)
4.2传统匹配定位实验 (29)
4.2.1ICP匹配定位实验 (29)
4.2.2NDT匹配定位实验 (31)
4.2.3ICP与NDT定位结果对比分析 (33)
4.3NDT-ICP匹配定位实验 (33)
4.3.1实验步骤 (34)
4.3.2实验结果分析 (34)
结论 (37)
参考文献 (39)
致谢 (43)
第1章绪论
1.1研究背景与意义
2016年，谷歌人工智能系统AlphaGo击败世界围棋冠军李世石[1]，标志着人工智能有了跳跃性的发展，在这股浪潮下，各个行业开始接触人工智能，其中有着近两百年历史的汽车行业成为人工智能技术落地的研究热点。

自十九世纪八十年代左右汽车问世以来，人们的日常出行已经离不开汽车这种交通工具。

随着全球经济和科技水平的不断提高，人们对生活质量的要求也越来越高，对汽车的需求也在不断增加。

虽然汽车改善了人们的出行方式，但它们也带来了许多社会问题，如道路交通安全问题、城市交通堵塞问题、环境污染问题等。

世界卫生组织2018年《全球道路安全现状报告》指出，由于道路交通导致死亡的人数呈现逐年攀升的趋势，每年死亡的人数达到135万，且如今5~29岁儿童和年轻人的首要死亡原因就是道路交通伤害，平均每24秒就有一人在道路上失去生命[2]。

许多国家都在道路安全方面都采取了有效措施[3]，包括：修改交通安全立法；设立道路基础设施；完善急救措施等。

然而，这些措施都无法从根本上解决安全问题。

自动驾驶技术的出现，能够使车辆按照规定路线行驶，具有较强的车辆控制和反应能力，不受人为因素干扰。

不仅能从根本上解决车辆行驶安全问题，同时能够满足人们的出行需求。

近几年随着传感器技术成本的降低和质量的提高，自动驾驶技术高速发展，各大高校、车企、图商都开始投入研究自动驾驶车辆。

自动驾驶技术不仅改变人们的出行方式，而且在其他生产行业也能发挥重要作用[4]。

在物流、环卫、采矿、测绘等行业上结合自动驾驶技术，能够进一步减少人工的参与，对生产行业产生革命性的改变。

也就是说，自动驾驶技术能够彻底改善人们的出行以及生产生活方式，因此受到各大高校、车企、互联网企业、厂商等社会各界的广泛关注[5]。

自动驾驶系统的基本架构大体可分为三部分：感知层、规划层和控制层[6]。

如图1所示。

其中，感知层主要分为两部分，其一为环境感知，主要利用多种传感器为自动驾驶车辆提供外界的环境信息，包括静态环境（如建筑物、指示牌等）和动态环境（车辆、行人等）；其二为定位，主要是判断自动驾驶车辆在当前环境中的位置。

定位是自动驾驶技术中的关键基础技术，也为实现后续的路径规划和运动控制等功能提供基础。

对于自动驾驶车辆而言，定位的意义十分重大，是自动驾驶的先决条件，在自动驾驶车辆行驶的过程中，需要不断确定自身与外界环境之间的相对位置关系，为车辆的导航和运动提供参考依据。

因此，定位问题一直以来都是自动驾驶领域的研究重点。

图 1 自动驾驶系统基本架构
Figure 1 Basic architecture of autonomous driving system
现阶段自动驾驶车辆通常使用的是结合GNSS和INS来实现车辆定位。

然而，通过卫星信号进行定位的技术很容易受到环境的影响，在地形环境复杂的场景下，比如高楼耸立的城市、树木茂盛的森林以及高低起伏的山脉，卫星信号会在传播的过程中经历多次反射，形成多路径效应[7]；在阴雨多云的天气下，卫星信号会在穿透云层时受到减弱的影响，这种情况下的定位稳定性根本无法保证。

因此，如何在复杂环境的道路上实现自动驾驶车辆厘米级定位是一个迫切需要解决的难题。

针对复杂环境自动驾驶车辆定位这一难题，目前比较主流的解决方法是结合基于点云地图匹配的定位方法进行车辆定位。

结合上述方法对自动驾驶车辆进行定位所需求的点云地图主要依赖于多线束旋转式激光雷达。

这类激光雷达是一种扫描式传感器，它的主要原理是采用非接触式激光测距技术，通过发射激光光束来探测目标，并搜集反射回来的光束形成点云，经处理后可生成三维点云地图，其包含周边环境360°的距离信息，并且能够直接反映出周边地物的特征[8]。

现阶段市场上主流的激光雷达厂商主要有国外的Velodyne，Sick等和国内的禾
赛科技、速腾聚创等，如图2所示。

图 2 国内外不同厂商激光雷达
Figure 2 Lidar from different manufacturers at home and abroad 在安装有激光雷达的数据采集车行驶并采集的过程中，道路上经常会出现行驶的车辆、来往的行人，这些参与交通的目标完全不属于地图上的要素，然而，在数据采集的过程中，通常会无差别的将这些目标采集并加载到点云地图里；另外，在利用车载激光雷达对点云数据进行采集时，由于环境因素，设备自身，人为操作，以及周边建筑本身材质变化的影响，一些噪声点不可避免的出现在点云数据中，只有将噪点、离群点、孔洞按照后续处理剔除，才能更好的进行匹配，特征提取，曲面重建等操作。

如图3所示，定义这类对点云地图无用且不属于地图的要素为非地图要素。

若这些非地图要素在构建点云地图的过程中没有经过处理，不仅会遮挡车道线、导向箭头等路面标注，影响自动驾驶车辆对道路信息的识别，更重要的是，对基于点云地图匹配定位的稳定性造成干扰[9]。

为此，本文利用PCL库中的滤波算法[10]，对点云地图的噪点、离群点进行过滤，并
结合人工剔除车辆、行人等动态要素，构建路面清晰、仅包含静态地图要素的点云地图。

图 3 点云地图中非地图要素
Figure 3 Non-map features in point cloud maps
1.2 自动驾驶车辆定位技术研究现状
为了解决自动驾驶车辆在复杂环境中的定位问题，自动驾驶领域的许多企业高校都
提出了大量针对自动驾驶车辆的高精度定位方法。

自动驾驶车辆的定位是确定车辆的位置和姿态，定位的方法主要可分为两类：一是绝对定位方法，二是相对定位方法[11]。

绝对定位方法以基于GNSS的定位方法和基于地图匹配的定位方法为代表，主要是通过某种方式获得自动驾驶车辆在某一坐标系（如地图局部坐标系、WGS84世界坐标系）下的绝对坐标，此类方法的每次定位都保持相互独立，互不影响。

而相对定位方法主要以轨迹推算法、惯性导航定位以及即时定位与地图构建(Simultaneous Localization And Mapping, SLAM)等方法为代表，此类方法主要是根据当前车辆的位置姿态，结合传感器测量的某段时间内的姿态变化量，推算预测出下一时刻的位置[12]。

由于传感器噪声以及预测推算过程中噪声的存在，相对定位方法不可避免会出现累计误差，并且随着时间距离的延长，其定位的误差也会不断增大。

因此，在自动驾驶场景下，绝对定位和相对定位都是为了确定车辆与行驶路径的相对位置关系，以此引导车辆前行，其本质是一致的，通过结合绝对定位和相对定位两种方法可以提高自动驾驶车辆的定位精度。

在自动驾驶场景下，往往使用最多的就是基于GNSS的定位方法来获取车辆在当前环境中的绝对位置。

卫星导航定位系统[13]是目前世界上使用最为广泛的定位方法，其广泛应用于各种领域，包括航空航天、船舶定位、车辆导航等领域。

利用卫星信号进行定位的原理是通过测量多个卫星与用户接收机之间的距离，再而利用距离交会的方法，计算出用户的地理位置。

卫星定位系统的组成一般分为三个部分，包括空间部分（卫星）、地面控制部分（检测站和控制站等）和用户设备部分（信号接收机）组成。

目前世界上已在服务的卫星定位系统包括美国的全球卫星定位系统、俄罗斯的格洛纳斯卫星导航系统、欧盟的伽利略卫星导航和我国自行研制的北斗卫星导航系统[14]。

通常情况下，基于GNSS的定位精度会受到多种因素的干扰，包括大气层延迟、卫星星历误差、卫星时钟误差以及多路径效应等，导致其定位误差在数厘米到数十米之间。

为了提升卫星定位的精度，采用RTK技术对采集到的卫星数据进行差分处理，能够得到厘米级的定位结果。

INS是一种辅助导航定位系统，它一般使用陀螺仪来测量物体运动的角速度，用加速度计来测量物体运动的加速度，根据这些运动参数估算出运动物体的位置和姿态。

利用融合后的RTK-INS定位系统，不仅能够减少卫星信号干扰的影响，而且在信号短暂失联后可利用INS继续定位[15]。

然而，在复杂地形环境的影响下，卫星信号可能会出现长时间的减弱或丢失状态，此时，融合后的定位系统同样会出现误差累积或者定位失败的情况。

因此，在环境较为复杂的场景下，自动驾驶车辆定位常采用基于地图匹配的定位方式。

基于地图匹配的定位技术就是通过车辆的传感器感知周边环境信息，并与预先建立的环境地图进行匹配，进而估计车辆在当前环境中的相对位置。

该定位方法可以摆脱对卫星信号的依赖，不受外界环境因素的干扰，有效进行自动驾驶车辆定位。

现阶段传感器技术处在快速发展阶段，激光雷达传感器以其获取信息全面，精度高，轻巧方便等优势快速进入自动驾驶市场，为自动驾驶车辆的感知和定位提供数据支撑。

因此，基于车
载激光雷达采集的点云地图进行匹配定位时，卫星信号对其的干扰可以忽略不计，另外，点云地图中包含丰富的三维信息以及道路的特征信息，包括车道线、道路标牌、建筑物轮廓等信息（如图4所示），该技术被认为是自动驾驶车辆在复杂环境下进行导航定位的关键技术。

图 4 三维点云地图
Figure 4 3D point cloud map
针对复杂环境场景下的自动驾驶车辆定位问题，基于GNSS的定位技术受到卫星信号干扰的局限性，许多国内外研究人员开始对基于地图的定位技术展开研究，以提高车辆位置的准确性。

根据地图进行自动驾驶车辆定位的方法可大致分为两种，一是基于地图特征的匹配定位，二是基于扫描的匹配定位。

（1）基于地图特征的匹配定位方法主要是根据地图中的特征信息来实现匹配定位。

Wulf等人在2004年提出了一种将三维激光传感器与SLAM 算法相结合，从三维点云数据中提取出二维扫描轮廓与并地图实现匹配定位[16]；Maan E. El Najjar等人在2005年提出了利用道路特征匹配的方法，并使用置信理论和卡尔曼滤波来对数字地图中车辆位置进行准确估计，以此来弥补卫星信号在复杂环境中信号受到干扰的问题[17]；Hentschel M等人在2010年提出将OpenStreetMap数据集中的建筑物标签数据集成到机器人的定位任务中，使用激光雷达扫描的二维建筑物轮廓与地图进行匹配实现定位[18]；N.Suganuma等人在2011年提出了一种使用GNSS/INS和车道标记检测的定位方法，在该系统中，GNSS / INS测量的漂移误差通过车道标记测量得到补偿[19]。

（2）基于扫描的匹配定位方法主要是通过比较当前扫描点云和基准点云的相似程度，进一步计算出车辆在点云地图中的位置和姿态。

Besl等人在1992年首次提出了ICP算法，主要是通过对两幅不同的点云寻找其最邻近的点对来计算两幅点云的相对位置关系，最邻近点对的判断依据为两点之间的欧式距离最小[20]；Lu F等人在1997年提出基于两次扫描点云中数据点与切线方向的匹配和最小化距离函数来求解扫描点之间的位移，将位移匹配到前一次扫描，从而计算机器人在未知环境中的相对位置[21]；Peter Biber等人在2003年提出了正态分布变换算法，利用该算法，将二维扫描点云进行匹配[22]；Takeuchi等人在2007年进一步改进了正态分布变换算法，利用该算法进行三维点云的匹配，开始了二维向三维的发展，利用三维正
态分布变换实现了在室内环境中，机器人进行地图构建和匹配定位；张晓等人在2014年提出了一种应用于地面激光扫描中的基于SURF的正态分布变换算法，是NDT算法的改进算法，该算法对不同分辨率的点云具有良好的匹配效果[24]。

结合对基于点云地图进行匹配定位的算法的发展现状可知，本文主要是针对复杂环境下车载激光雷达的匹配定位进行研究分析，故基于扫描的匹配定位方法更适用于本文的研究。

1.3 研究内容与组织结构
为了解决自动驾驶汽车在复杂环境中的定位问题，本文主要以国际上自动驾驶领域广泛使用的KITTI数据集中的车载激光雷达点云为研究数据，首先对KITTI数据集中某城市路段的三维点云数据进行预处理，对点云数据中存在的噪点等非地图要素进行过滤，以提高自动驾驶车辆基于点云地图匹配定位的精度及效率；然后利用两种经典的点云匹配定位算法进行点云匹配定位研究，分析对比两种主流算法的稳定性，准确性与时效性。

结合两种算法的优势对经典算法进行改进，并对改进后的NDT-ICP算法进行对比实验，分析其匹配精度和效率，实验结果验证改进算法在复杂环境场景下具备良好的稳定性和定位精度。

本文研究的主要流程如下图所示：
图 5本文研究主要流程
Figure 5 The main process of this article
本文的章节安排共有五章，以下介绍本篇论文的组织结构：
第一章是绪论部分，首先阐述了在自动驾驶技术快速发展的背景下，确定车辆位置的重要性，然后简述了自动驾驶车辆定位方法的分类，并突出介绍了基于地图匹配的车辆定位方法的国内外研究现状，针对目前自动驾驶车辆在复杂环境场景下定位存在的问题，结合国内外研究现状，提出了本文的研究内容，并对文章的组织结构进行了介绍。

第二章是对点云地图匹配定位算法原理的介绍。

分别介绍了迭代最近点算法和正态
分布变换算法的原理流程，进而实现算法改进，在ICP算法求解目标函数时，利用Gauss-Newton法进行最优解求解，在搜索对应点对的过程中，使用K-D树对该过程进行加速，集成改进的ICP算法与NDT算法，进一步提高点云匹配定位的精度与效率。

第三章主要是对KITTI数据集中的城市道路点云数据进行预处理。

首先介绍了三维点云数据的基本特点，然后对KITTI数据集、点云数据采集设备以及点云数据的格式进行了简单的介绍，详细介绍了基于车载激光雷达点云数据进行地图创建的一些方法，并进行点云地图创建处理，最后重点介绍了对点云数据中存在的噪点等非地图要素的过滤处理。

第四章是点云匹配定位算法的实验部分。

首先对实验测试环境进行简单介绍，然后分别利用迭代最近点(ICP)算法和正态分布变换(NDT)算法进行实验，并对匹配的精度和效率进行评价，最后对改进后的NDT-ICP算法进行实验，与前两次实验进行对比分析，验证其可行性。

第五章是对全文的研究内容进行了归纳总结。

第2章 点云地图匹配定位算法
本章主要是对经典的点云匹配算法进行介绍，包括迭代最近点算法与正态分布变换算法，介绍其原理流程和特性，以及算法的发展现状。

在此基础上，对经典的匹配算法进行了改进，并介绍改进算法的思路和主要流程。

2.1 迭代最近点算法
迭代最近点算法是点云配准的经典算法，该算法的基本思想就是对两幅点云中的数据点建立一一对应的关系，将对应点对进行空间矩阵变换，实现将待配准点云转换到基准点云的坐标系中，完成点云匹配。

2.1.1 算法原理与特性
假设要实现源点云P 和目标点云X 之间的匹配，并计算得到两个点云之间的旋转矩阵R 和平移向量T 。

点云P 中有n 个数据点，记为
,1,2,...,i p i n 。

点云X 中有m 个数据点，记为 ,1,2,...,j x j m 。

ICP 算法的流程总结来说可以分为以下几个阶段：
（1）计算对应点对 对于点云P 中的一个数据点
i p ，可以计算其与点云X 中所有点的最近几何距离(,)i d p X ：
(,)min j i j i x X d p X x p (2-1)
根据公式2-1计算可得，点云P 中的i p 的最近点记为j x 。

这就意味着，对于点云P
中的所有点，均可以在点云X 中找到距离最近的点，可将其构成最近点对。

（2）计算变换矩阵R 与平移向量T
通过步骤（1）可计算得出具有对应关系的两幅点云中的最近点对，为了方便记录，将具有对应关系的最近点对重新排列，使其满足以下条件：令目标点云与源点云的数据点的数量相等，即n m ；将具有对应关系的最近点对的下标使用相同的符号表示，即i
p 的最近点对为i x 。

计算变换矩阵的具体步骤如下所示：
计算点云P 和点云X 的质心：
11n
p i n i u p (2-2) 11m x i m i u x (2-3)
由点云P 和点云X 构造协方差矩阵：
11()()n T px i p i x n i p u x u
(2-4) 由协方差矩阵构造44 对称矩阵：。