一种改进的二维ICP点云配准算法

谢小鹏,古家威

(1.东莞理工学院城市学院,广东 东莞 523419；2.季华实验室,广东 佛山 528000)

1 引 言

点云配准技术在逆向工程、机器人导航及无人驾驶等领域具有广泛的应用,点云配准一般来说包括两个步骤,粗配准和精配准,粗配准主要实现目标点集与参考点集之间的快速配准,但是精度不高,常用的方法有NDT(Normal Distribution Transform)方法[1],王庆闪等实现了NDT与ICP结合的点云配准方法[2],精配准的实现方法主要是迭代最近点(ICP,Iterative Closest Point)算法[3],由Besl于1992年提出。近些年来,很多学者对ICP算法进行了改进,Censi采用点到线的度量方式进而提出了PL-ICP(Point-to-Line ICP)方法[4],该方法提高了ICP算法的精度；解则晓改进了点到面ICP即P-ICP(Point-to-Plain ICP)[5]； Chetverikov提出了Trimmed ICP[6],该方法将n个匹配点对的欧氏距离进行排序,去除距离最大的ηn(0<η<1)个匹配点对,该方法需要对η值做出平衡,过大过小都会影响最终结果的精度；孙翌将[7]k邻域搜索方法应用到ICP算法中,提高了查找最近点的效率,进而加快了ICP算法的处理速度。

本文针对传统ICP 算法存在的问题进行了改进,首先介绍了传统ICP的主要步骤,发现传统ICP算法有以下几种问题:迭代方向错误,迭代优化过程中易出现局部最优的情况,迭代次数过多,增加了算法的处理时间。本文提出了改进方案,首先打乱目标点集中点序号的顺序,然后在接下来的寻找最近点过程中采用一对一的方式进行,增强了算法的稳定性,对每个匹配点对进行筛选,使用动态阈值去过滤那些欧氏距离过大的一些匹配点对,加快了算法收敛速度。

2 传统二维ICP算法

传统的二维ICP算法的一般步骤如下:

步骤一:选取参考点集和目标点集。

步骤二:遍历目标点集中所有的点,在参考点集中选择欧氏距离最小的一个点。

步骤三:建立目标函数,对目标函数进行优化求解,得到目标点集的旋转矩阵Rj和平移矩阵Tj,进而得到新的目标点集。

步骤四:判断是否达到了最大迭代次数或者误差条件,是则停止迭代,输出最终的结果,否则转到步骤一,继续迭代。

在步骤三中,需要建立目标函数并优化求解,下面是目标函数的建立和优化过程:

(1)

将目标点集与参考点集中所有对应点对的欧氏距离的平方和累加起来得到目标函数,并使之达到最小,于是目标函数为:

(2)

接下来对目标函数进行求解:

对目标函数中的3个待定系数分别求偏导并令其为0:

-sinθyi-t1)(sinθxi-cosθyi)=0

(3)

(4)

(5)

对上述3个方程联立即可得到3个待定系数的值,进而得到本次迭代的旋转矩阵Rj和平移矩阵Tj,将目标点集中所有的点代入到式(1)中作旋转平移变换即可得到新的目标点集。

当算法结束后,最后一次迭代得到的目标点集即为最终的目标点集,最终的旋转矩阵和平移矩阵由公式(6)得到:

(6)

其中,m是迭代次数。

3 改进的二维ICP算法

传统的二维ICP算法容易出现以下一些问题:

(1)在寻找最近点的过程中,会出现目标点集与参考点集多对一的情况,进而使得算法的迭代方向发生错误,最终的误差无法收敛。在图1中,箭头所指圆圈指的是目标点集,其它圆圈指的是参考点集,绿色带箭头线段指的是目标点集与最近点的配对关系,可以发现目标点集中所有的点都指向了同一个最近点。

图1 当目标点集与参考点集以多对一方式匹配时

(2)本文将目标点集中的某个点与其找到的最近点称作匹配点对,寻找到最近点后,有些匹配点对的欧式距离过大,会影响算法的迭代方向,增加算法的迭代次数。

在图2中黑色连线的匹配点对间的欧氏距离远大于其他匹配点对的欧氏距离,这将大大地改变ICP算法的迭代方向。

图2 当出现匹配点对间欧氏距离过大的情况

(3)传统的二维ICP算法在优化过程中容易陷入局部最优的情况,导致求解出来的结果错误。

本文提出的一种改进的二维ICP扫描匹配算法针对传统的二维ICP算法进行了改进。

(1)为避免出现目标点集与参考点集多对一的情况,采取目标点集与参考点集一对一的方式,具体实施方法则是当目标点集中的某个点在参考点集中找到最近点后,将参考点集中的这个最近点排除在参考点集之外。

(2)打乱目标点集的顺序。一般来说,用于ICP匹配的点集的点序号存在着某种规律性,比如从上到下或者从左到右排列,目标点集中序号最前的点能够在参考点集中正确找到最近点,由于序号靠前的点具有优先选择权,目标点集中序号靠后的点找到的最近点有很大的可能性不是真实的最近点。由于点集客观存在的排列规律性,会导致大多数点都找不到真实的最近点,为了让目标点集中大多数点都能找到真实的最近点,本文将目标点集中的点序号打乱,增加了目标点集中能找到真实最近点的个数,如图3所示。

在图3(a)中目标点集的点序号按照从上往下的顺序排列,由此形成了图中的匹配点对,可以发现这些匹配点对间的欧氏距离都相对要更大,对于这些欧氏距离过大的匹配点对,算法会对其进行筛选,进而发生大范围误过滤的情况,这将影响算法的最终效果。在图3(b)中目标点集的点序号经过了打乱,此时目标点集中有更多的点找到了真实最近点。

图3 目标点集打乱顺序前后对比

(7)

(8)

图4 改进的二维ICP算法流程图

4 实 验

为验证本文改进算法的优点,进行对比实验。实验中点集数据来自于一款旋转二维激光扫描器在实际环境中所采集的数据,该款激光扫描器每转一圈获取360个数据。实验环境的测试环境如下:CPU为Intel(R)Core(TM)i5-6500U,8G内存,Windows10系统,编程环境是MATLAB7.0。

图5是传统二维ICP算法的ICP迭代过程,图中的点集数据来源于二维激光扫描器,图中的点集数据来源于二维激光扫描器,图中有两簇点集,分别是目标点集及参考点集,其中参考点集由目标点集经过旋转平移变换所得到,连线表示表示匹配点对间的匹配关系。迭代过程中,目标点集与参考点集不断靠近,最终几乎完全重合。图6是本文改进的二维ICP算法的ICP迭代过程,采用的数据与图5中的数据相同,都是从初始状态到最终目标点集与参考点集的无限接近,可以发现本文算法所需的迭代次数明显要更少,特别是在无限接近阶段,本文算法能够迅速地收敛。

图5 传统二维ICP算法的迭代过程

图6 本文改进的二维ICP算法的迭代过程

为具体量化本文改进ICP算法的优势,在获得多组点集数据之后,使用这些数据在传统ICP算法与本文改进ICP算法上分别处理,表1是传统ICP算法与本文改进ICP算法在平均迭代次数与平均处理时间上的对比,可以发现本文所需的迭代次数要更少,处理时间也要更短。本文改进ICP算法处理较快的原因在于算法的后续收敛阶段,正常的匹配点对的欧氏距离趋于0,因此一些误匹配点对的出现会极大地影响算法的收敛,而本文算法设有自适应变化的阈值用来过滤一些误匹配点对,从而加快了算法收敛,减少了迭代次数。

表1 传统ICP算法与本文改进ICP算法的迭代次数与处理时间

5 结 论

本文针对传统ICP算法出现的一些问题,对其进行了改进。本文采用目标点集与参考点集一对一的方式进行最近点配对,并对目标点集的点序号进行打乱,增强了ICP算法的稳定性；在ICP算法每次迭代过后,设置一个新的阈值用于过滤一些误匹配点对,进而减小算法的迭代次数,加快了算法处理速度,实验结果表明,本文改进的ICP算法在处理速度具有明显地提升。