基于激光雷达的点云配准算法综述

衣明悦，石 晶，赵梓杉

（辽宁工业大学 汽车与交通工程学院，辽宁 锦州 121001）

前言

目前主流的自动驾驶定位技术有基于全球导航卫星系统（GNSS）、基于航迹推算的定位方法，以及基于环境特征匹配的定位方法三种。其中基于环境特征匹配的定位方法主要采用车载相机和激光雷达获取RGB图像与激光点云数据，然后与高精地图中存储数据进行特征匹配，从而实现精确定位。

在点云地图构建过程中由于激光雷达扫描距离存在限制，所以会进行多次连续扫描，然后将每一次扫描的点云数据进行配准拼接，最终形成完整连续的激光点云地图。

点云配准是点云数据处理的基础性工作。同时激光扫描的点云数据能够以较小的内存获得物体准确的拓扑结构和几何结构，因而获得越来越多学者的广泛关注。在实际采集过程中，因为被测物体尺寸过大，物体表面被遮挡或者三维扫描设备的扫描角度等因素，单次的扫描往往得不到物体完整的集合信息。从而，为了获得被测物体的完整几何信息，就需要通过求解坐标之间转换关系，将连续扫描的两帧或多帧激光点云转换到同一坐标系（scan-to-scan），或者将当前扫描点云已建立的地图进行配准（scan-to-map）从而最终恢复车身位置和姿态的变化。

1 发展现状

1.1 迭代最近点（ICP）算法国内外发展现状

Besl[1]等在1992年提出的迭代最近点（ICP）算法，是最经典的点云配准算法，该算法计算较为简单，但是该算法的缺点是不仅对初始位置要求较高，而且容易陷入局部最优结果。此外显示测量中很难满足传统ICP算法在两片点云间存在子集关系的要求[2]。因此针对ICP算法的缺陷，很多学者提出了ICP的改进算法。TakeshiMasuda等在1994年为了提高对应点搜索效率，提出了在当前帧的点云中选择有代表性的点，再进行搜索对应点的步骤，减小对应点搜索的范围，减少了点云匹配消耗的时间[3]。DevrimAkca在2005年为了加快对应点搜索，提升算法效率，提出了一种用于匹配三维点云的最小二乘匹配的算法，使用高斯一马尔可夫模型估计点云之间的转换参数，同时，提出了有效的空间划分方法[4]。

为了解决实际中点云配准问题，国内一些学者也提出了改进匹配算法。张鸿宾等在2005年，提出了一种基于物体表面距离的匹配算法，主要思想是将物体表面用三角网格来近似表示，将网格间最近距离的均值作为评价标准，进而估计运动参数，该算法对物体匹配的准确性有所提高[5]。近年，在2019年赵明富等[6]提出了融合采样一致性和ICP算法对初始位置相差较大的点云进行配准。2021年李慧慧等提出一种改进的ICP激光点云精确配准方法，该方法可以对初始位置相差较大且具有部分重叠的点云进行精确配准，同时提高运行效率并对噪声具有相应的鲁棒性[6]。

1.2 正态分布变换（NDT）算法国内外发展现状

Biber和Straber在2003年第一次提出了NDT算法，当时主要用于二维平面点云数据之间的匹配[7]。厄勒布鲁大学的Magnus-son在2006年首次在三维空间中使用NDT算法，并提出了3D-NDT算法，使得NDT算法突破了维度的限制[8]，能够适用于利用机器人进行采集的点云数据配准。Cihan等在2013年提出了多层正态分布变换（ML-NDT）算法[9]，改进了正态分布变换算法，同年，JariSaarinen等人提出了一种新的三维建图方法，NDT-OM算法主要采用递归更新策略在大规模动态环境中实现精确的三维地图创建，并且可以无缝提供多种分辨率的地图[10]。

国内也有很多学者也进一步研究和改进了正态分布变换（NDT）算法。蔡则苏在2005年利用NDT算法根据自然环境特征有效地完成了室内环境下的自主定位，有效避免了点云之间的复杂匹配问题[11]。浙江大学的胡凤军等在2014年提出了将牛顿迭代算法应用到三维正态分布变换算法中，解决了在离散点云中配准算法收敛性差，容易产生局部最优的问题[12]。2021年荆路等提出了基于SAC-IA和NDT融合的点云配准方法，依据FPFH特征和SAC-IA算法对点云进行初始配准，然后在此基础上使用NDT算法进行精配准该方法的配准精度不仅显著提高，同时效率也有一定的提升。

2 两种经典点云配准算法

在同步定位和建图（SLAM）中，分为前端和后端，前端主要完成的工作就是点云配准工作，本章将分别介绍迭代最近点（ICP）算法与正态分布变换（NDT）算法的基本思想与流程。

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

迭代最近点ICP（IterativeClosestPoint）的点云配准算法重复进行“确定对应关系的点集—计算最优刚体变换”的过程，直到收敛条件被满足，其实质就是基于最小二乘法的最优匹配。该算法的基本原理是：求解两组点云数据重叠区域内源点云P与目标点云Q之间的空间变换，使两组点云模型之间的距离最小。设E（R，t）为源点云P在变换矩阵（R，t）下与目标点云Q之间的误差，称E（R，t）为目标函数，其中R为旋转矩阵，t为平移矩阵。则求解最优变换矩阵的问题可以转化为满足minE（R，t）的最优解（R，t）。可用下列公式表示：

通过最小化目标函数来求解最优变换矩阵（R，t）。具体步骤为：

（1）计算源点云P中的每个点在目标点云Q中的最近点；

（2）利用SVD分解计算旋转矩阵R和平移矩阵t，使得目标函数E（R，t）最小；

（3）对源点云P通过求得的旋转矩阵R和平移矩阵t进行旋转和平移变换，得到新的对应点集P'；

（4）计算新的点集P'与目标点云Q之间的平均距离d；

（5）如果平均距离d小于给定的阈值或大于预设的最大迭代次数，则停止迭代计算。否则返回第一步，直到满足收敛条件为止。

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

正态分布变换，是一种统计学模型。如果一组随机向量满足正态分布，那么概率密度函数可用下列公式表示：

其中D表示维数，表示均值向量，Σ表示随机向量的协方差矩阵。NDT算法能够通过概率的形式描述点云的分布情况，这种形式有利的减少了配准所需要的时间。具体算法流程为：

（1）网格化目标点云。利用立方体将激光点云所在的空间进行栅格化，使激光点云处于相应的立方体中；

（2）目标点云划分好后，求出每一个网格内的正态分布概率密度函数；其中网格太大或太小都有风险，因此一般保证网格内至少有5个点；

（3）求出源点云相对目标点云的初始坐标变换参数，即旋转矩阵R和平移矩阵t；这一步是为下一步变换参数的迭代提供了距离最优点较近的初值；

（4）源点云进行初始坐标转换，并计算在目标点云网格中的概率；源点云根据上一步求出的（R，t），将坐标转换到目标点云网格上，转换后的源点云坐标，对应所在网格的正态分布概率密度函数，求出转换后激光点坐标的概率；

（5）将每个点云的概率乘积作为目标似然函数。概率乘积最大时的似然函数，就是最优的坐标转换，如式：

其中－logφ为最大似然函数的负对数形式，这种形式是为了使之后求导更方便。

最后利用牛顿迭代法，找出最优变换参数完成点云配准。

3 总结

本文主要介绍了两种点云匹配算法，两种算法特点具体对比如表1所示：

表1 两种经典点云匹配方法特点

基于迭代最近点（ICP）的点云配准算法，可以获得非常精确的配准效果不必对处理的点集进行分割和特征提取，同时有较好的初值时，可以得到很好的算法收敛性。相比于基于正态分布变换（NDT）的点云配准算法，ICP算法在搜索对应点的过程中，计算量非常大，且收敛域小，运行速度慢，同时因为标注ICP算法中寻找对应点时，认为欧氏距离最近的点就是对应点，这种假设也会生成的错误对应点。同 时基于正态分布变换（NDT）算法不仅拥有更好的鲁棒性，相比于ICP算法效率也有了提高。