ICP配准算法的影响因素及评价指标分析

陈春旭，漆钰晖，朱一帆，裴 凌，徐昌庆

(1.上海交通大学 上海市北斗导航与位置服务重点实验室，上海 200240；2.南昌大学 信息工程学院，南昌 330031)

0 引言

近年来，随着无人车技术[1]的发展，同时定位与地图构建(Simultaneous Localization and Mapping, SLAM)[2]成为重要课题和研究热点。由于传感器的不同，SLAM主要分为视觉SLAM[3]和激光SLAM[4]。相较于RGB-D相机传感器，激光雷达传感器具有精度高、速度快，对于光照环境要求低的特点。目前，高分辨率多线激光雷达传感器广泛应用于机器人自主导航和三维场景重建两大领域，基于高分辨率多线激光雷达传感器的SLAM技术为测绘行业提供了更加成熟可靠的解决方案，其技术的关键环节之一即局部三维激光点云配准问题，其点云配准的精度和效率会对移动机器人后续的定位与建图产生关键性的影响。

目前常用的三维空间点云配准方法多采用迭代最邻近点(Iterative Closest Point，ICP)[5]算法，该算法是采用点点(点面)距离寻找邻近点，再通过均方误差函数求取多帧点云间的旋转和平移矩阵以达到配准的目的，实质是一种迭代收敛的算法。该算法最初提出是用于对象重建，但近年来在机器人领域已广泛应用于全局场景重建的配准。由于ICP原始算法中存在很多缺陷和不足，后来就引发了对其算法的各种改进和各类变种算法的出现[6]。

本文主要研究了ICP算法在三维激光点云配准中的应用，首先对ICP算法的原理及其最小二乘求解过程进行了详细分析；其次分析研究了影响ICP配准方法的各类因素，并提出了相应的配准评价指标；最后通过实验验证的方法总结了不同因素对配准精度的影响，利用评价指标对实验结果进行了分析总结。

1 面向三维激光点云的ICP配准算法

三维激光点云配准过程，就是求一个两组点云之间的旋转平移矩阵(刚性变换或欧式变换)，将源点云变换到目标点云相同的坐标系下[7]。三维点云配准原理可以表示为方程:Pt=RPs+t；其中，Pt、Ps就是目标点云与源点云中的一对对应点。其中，R是旋转矩阵，t是平移矩阵。图1所示为三维点云配准的算法流程图。

1.1 ICP算法原理介绍

ICP算法是目前三维空间点云配准中应用最为广泛的配准算法，其目的在于寻找最小二乘逼近的三维空间坐标变换矩阵。ICP算法本质上是基于最小二乘法的最优配准方法，该算法通过选择对应关系点对并反复进行迭代，直到满足算法的收敛条件，从而得到最优的刚体变换。

定义2个给定的点云集合：源点云集P，目标点云集Q，pi∈P,qj∈Q,其中i=1,2,…,M,j=1,2,…,N，M和N分别为点集P和Q的大小。寻找一个变换矩阵T，使得源点云中的点经过T变换，能够与目标点云中的点距离误差最小，目标函数如式(1)所示, 其中,pi∈P，qi为pi在Q点集中找到的配准点，L为合适的配准对的个数。

(1)

本文使用的ICP算法迭代收敛的约束条件主要有3个：最大迭代次数、两次变换矩阵之间的差值(ΔT)和均方误差(Mean Squared Error，MSE)。ICP算法的具体实现步骤如下：

1)获取2个待配准的点云P和Q，变换矩阵为T(初始值默认为单位阵)，首先将点云集P使用变换矩阵T进行变换，得到新的点集P′；

2)建立新点集P′和Q的相关性，利用点云关联算法，从Q中寻找pi的对应最近点qi；

3)构建最小二乘的约束条件

(2)

4)求解变换矩阵ΔT，得到新的变换矩阵T=ΔT·T；

5)返回步骤1)，重复以上步骤，直到满足收敛条件。

1.2 目标函数求解分析

传统ICP算法的目标函数求解常用奇异值分解(Singular Value Decomposition，SVD)的方法。根据1.1节中ICP算法的问题描述[8]，首先定义第i对点的误差项ei：

ei=qi-(Rpi+t)

(3)

其中，pi是源点云集中的任一点，qi是目标点云集中与pi对应的任一点。

然后构建最小二乘式(4)，求使得误差平方和达到极小的旋转矩阵R和平移向量t：

(4)

定义源点集和目标点集的质心：

(5)

将2组点集的质心代入式(4)进行处理，则误差函数可以简化成如下形式

(6)

由式(6)可以看出，第一项只与旋转矩阵R有关，而第二项既有R也有t，但只和质心相关。因此先求得R，再令第二项为0就能得到t。

(7)

则求最优变换矩阵T的问题转为先求解式(8)中的最优旋转矩阵

(8)

对式(8)展开如下：

由上式可以看出，第一项和第二项均与R无关，那么问题转化为：

(9)

W=UΣVT

(10)

其中，Σ为奇异值组成的对角矩阵，对角线元素从大到小排列，而U和V为对角矩阵。当W满秩时，R=UVT为所求最优值，根据R继而求得t。

2.ICP配准的影响因素及评价指标

2.1 ICP配准的影响因素

ICP配准算法主要分为两部分：三维点云关联算法和约束条件构建求解算法。三维点云关联算法的目标在于尽可能地寻找相似的点对，并剔除异常点对。目前已有的关联点对的算法有寻找近邻点的KD-Tree算法[9]、八叉树算法[10]以及利用各种描述子的相似性，例如浙江大学戴静兰提出的利用曲率特征提高ICP算法的效率和算法的稳定性[11]。

目前ICP及其变种的配准方法构建的约束算法都是基于最小二乘算法，基于最小二乘的迭代收敛算法是一个容易陷入局部优化的算法。在实际实验中，在不提供相对准确的初值情况下，当转弯角度或者移动距离较大时，经常会出现ICP算法陷入局部收敛而导致配准失败的情况。为了提高配准效率和准确度，通常借助于其他传感器如IMU[12]给ICP提供初值。本文主要选取旋转角度和平移距离2个参数作为影响因素对ICP配准效果进行分析。

2.2 ICP配准的评价指标

ICP算法的配准实则是一种优化策略的配准过程，算法要求两帧点云数据都存在彼此之间的对应点，即一个点集是另一个点集的严格子集。寻找对应点对的过程是ICP算法计算代价最大的一步，点云数据间若只存在部分重叠区，则能够找寻到的正确的配准点对的数量是有限的，同时正确配准点对的数量会直接影响ICP算法的配准精度。因此,本文将在实验部分对不同平移和旋转参数下的2组点云集计算正确配准的点对数量，并将迭代过程中配准点对数量的变化趋势作为ICP配准的一个评价指标进行分析和比较。

(11)

S的值越小，则认为ICP的配准效果越好，本文测试实验效果的得分低于0.01时，可以认为配准精度较高。

3 实验验证

3.1 实验说明

本文的实验平台为Ubuntu16.04系统和Velodyne16线激光采集平台，为保证实验结果的准确性，选择多组三维激光点云数据进行实验测试。实验数据包含2组室内外场景的开源数据集和2组Velodyne16线实地采集的室内外数据集。将原始数据作为源点云，对源点云进行旋转平移，并加入高斯白噪声，作为目标点云。测试算法使用经典ICP算法，同时使用KD-Tree算法对最近邻点搜索进行加速。

在实际机器人运动中，机器人主要是进行平面上的运动，因此考虑偏航角和平面位移对ICP配准效果的影响。设置ICP的迭代次数为100，确保足够的迭代次数，ΔT收敛阈值为10-12，误差均方差为10-12。

首先经过数据测试，得到每组数据集的得分阈值为0.03，得分越高，则配准效果越差，得分超过0.03则判定为配准失效，结束进程。图2～图5所示为各个场景数据集的配准效果图，红色为源点云，绿色为目标点云。图2所示为PCL库[13]提供的室内房间激光点云测试数据；图3所示为KITTI数据集中提取的一帧十字路口的激光点云数据；图4所示为Velodyne16线实地采集的实验室场景；图5所示为实地采集的室外场景。实验测试前，在空间直角坐标系下，将源点云绕z轴旋转30°后沿x轴和y轴方向均平移10m得到目标点云，并加入高斯白噪声。

3.2 实验结果及分析

为测试旋转角度和平移距离2个参数对ICP配准算法的影响效果，分别设定单个变量去评估ICP算法的配准精度。

实验一：固定x轴和y轴方向的平移距离为1m，从零开始逐步增加偏航角度，步长为1°，直到ICP配准失效，图6所示为各个场景数据集的配准得分折线图。

实验二：根据实验一得到的旋转角度的变化范围，除KITTI数据集点云的旋转角度设置为20°，其他场景的旋转角度固定为30°，y轴方向的固定位移均为1m，逐步增大x轴方向上的位移，步长为1m，得到ICP算法随x轴方向位移增大的配准得分折线图，实验结果如图7所示。

实验三：选取图2所用的房间数据集作为测试数据集，得到不同旋转和平移参数下的正确配准点对数量的变化趋势。设定2组点集之间x和y方向的位移均为1m，旋转角度只考虑偏航角，从零开始依次增大到80°。实验结果如图8、图9所示。

从图6和图7的实验结果中可以看出，虽然各个测试数据集场景的折线拐点不同，但图中折线的变化走势基本一致。当旋转角度或者平移距离在一定范围内变化时，ICP配准精度得分有所波动，但均能达到较好的配准效果。如图2～图5，配准得分低于0.01的实验效果图，四类场景数据均达到较好的配准效果。这说明ICP算法在一定的角度变换和位移内，配准结果可靠，不易陷入局部优化。

当超过一定的角度变换和位移时，ICP算法在没有提供准确初值的情况下，即使参与配准的点云数量足够多，配准也会失效，如图6和图7所示的得分会在拐点处急剧增加，表1所示为四类场景数据集中相应拐点处的得分指标。

表1 拐点处ICP配准得分

参与配准的点对数量和点对质量会直接影响ICP的配准效果，本文以算法每次迭代中正确配准的点对数量变化来代表点对质量对ICP配准效果的影响，计算配准真值点与ICP搜索到的配准点的距离差值，在一定差值范围内，则认为该点对配准成功，实验中阈值设定为0.5。经典ICP算法中使用KD-Tree算法关联对应点对，实验三中根据经验值设置KD-Tree的距离阈值以确保足够的配准点对。图8是偏航角分别为20°、40°、60°时每次迭代的正确匹配对个数，图9是偏航角为65°、70°、75°时每次迭代的正确匹配对个数。当偏航角大于等于65°时，ICP配准失效。在65°偏航角时的ICP配准失效结果如图10所示，此时得分指标为1.58575。

由图8和图9可以提出用正确配准点对数量的变化趋势作为ICP配准效果的评价指标。

图8为ICP配准成功情况下的实验结果图，从图中可以看出，正确配准的点对数量呈增长趋势，直到参与配准的点对全部完成配准从而收敛。其中，每段折线都存在一个正确配准点对急剧增长的中间过程，这说明ICP找寻配准点对的方向是正确的。图9为ICP配准失效情况下每次迭代中正确配准点对数量，初始状态下虽然正确配准点对的数量有所增加，但涨幅变化有限，到达峰值后点对数量下降，最终趋于稳定且数量较少。因此，在多次迭代之后，若正确点对个数增长幅度不大且有下降趋势，则可以认为ICP配准失效。

4 结束语

三维激光点云配准方法是激光SLAM研究中极其重要的内容，局部点云配准精度的提高有利于整个SLAM过程中的轨迹恢复[14]。本文通过仿真移动距离参数和旋转角度参数进行测试实验，在此基础上分析ICP的配准效果，研究ICP配准的影响因素；在对实验结果进行分析的过程中，得到误差距离和正确配准点对数量变化趋势这2个配准的评价指标。实验结果表明，ICP配准算法可以在一定角度和距离的变化范围内避免受到局部最优的影响，同时，正确的配准点对数量对于ICP是否失效也有着重大影响。从实验结果中可以得出2个评价ICP是否失效的评判标准，根据ICP的计算结果得分，若超过经验阈值(一般设为传感器精度)，则ICP配准失效；或者根据每次迭代的正确配准点对的变化趋势来提前预判ICP的结果。这对研究SLAM技术中关键帧的筛选具有一定的参考价值。目前国内外学者都对ICP算法在对应点对的关联性和配准的收敛速度上作了改进，提高了ICP算法的配准精度和配准效率[15]，但在SLAM应用上仍需要对ICP配准算法的鲁棒性和配准效率做进一步提升。