二维点云配准的交换截取迭代最近点

南开大学统计与数据科学学院 裴晓淞

为解决传统ICP（迭代最近点Iterative Closest Point）[1]算法的存在容易陷入局部最优、误差大、计算量大等问题，本文分析了传统ICP算法缺点的成因，针对问题，提出了截取机制和交换机制，在两者协同作用下，经过大量模拟实验将平均误差减小了83.6%，计算时间减小了42.4%，且由数据显示，能有效地减少一部分局部最优的情况。

点云是通过测量仪器扫描物体而生成的点数据的集合。点云的配准在生活生产中，可以产生许多有价值的应用如：数字自动化生产、自动驾驶、医学图像配准等。点云的配准问题分为粗配准和精确配准两部分[2]，最常见的、工业中使用最广泛的精确配准算法是由Rusu等人提出的ICP[1]算法。二维ICP算法在激光领域的图像处理也产生了很多应用[3]，但是配准的效果还有一定的改进空间。

谢小鹏等人对ICP算法进行了改进，加入了乱序一对一匹配和动态阈值的机制，一定程度上提高了ICP算法的性能[4]，但是也存在动态阈值运行较为不稳定等问题。所以本文针对上述算法的问题，研发出截取机制和交换机制，给出了一个理论说明更加清晰，匹配效果更好的改进算法。

1 基本原理

1.1 传统ICP算法

Input：目标点云A= {Ai}、待配准点云B= {Bi}、终止最小误差δ、最大迭代次数N ；

Step1：对任意点bi=Bi，在点云A找距离最近的点进行匹配，记为{ai}，得到匹配后的一对点云{ai}和{bi}；

Step2：由公式找到变换R和T，使得{ai}和R× {bi} +T的误差err达到最小；

Step3：对B进行变换R×B+T得到新的点云；

Step4：若err没有达到最小误差δ和最大迭代次数N ，将上述步骤中B换成Step3中得到的新的点云回到Step1，重复迭代；否则结束并输出结果；

Output：完成配准的点云，配准的变换矩阵，最终误差，迭代次数。

其中Step2中误差err的定义如式（1）所示：

变换R和T的求取公式推导如下：

旋转矩阵用旋转角度φ表示如式（2）所示：

平移矩阵T表示如式（3）所示：

其中ΔxΔy分别为点云在x轴和y轴上的位移。

记Ca和Cb是点集{ai}和的中心点，如式（4）所示：

令ai′ =ai-Ca，bi′=bi-Cb， 结 合err的 定 义，可以得到新的误差函数如式（5）所示：

这里已经消去了变量T，只需求解旋转矩阵R[3]，对上式进行分解得到如式（6）所示：

对f（φ）求导，如式（7）所示：

进而得到如式（9）所示：

由R的定义即可得到旋转矩阵R。

平移矩阵由公式（10）得到：

1.2 交换截取ICP算法

Alternate and truncated Iterative Closest Point Algorithm简称ATICP。

传统ICP算法有以下几个问题：

（1）迭代容易陷入局部最优解。在面对点云较为对称的情况下，不同旋转方向的梯度可能会近似抵消，迭代有可能会陷入局部最优。

（2）每次迭代的运算量较大。在传统ICP算法中，对于n个点的点云，每个点都需要计算到另一个点云所有点的距离，最近点的距离需要进行6n2次运算，边际计算成本高，收益低，点数越多，模型的运算越复杂。

（3）迭代方向错误。点云可能在中心有一部分点数据，由于点云数据有一定的误差，这样的点更容易导致迭代方向的错误。

针对以上问题，这里给出两点改进：

（1）交换机制（Alternate在后文图标简写为A）。

在上文介绍的传统ICP算法的Step1进行修改：在奇数次迭代中，与传统ICP相同，在偶数次迭代中交换点云A、B的操作，对任意点ai=Ai，在点云B找距离最近的点进行匹配，记为{bi}，得到匹配后的点云{ai}和{bi}。

交换机制对ICP算法的影响：当误差较大时，如果匹配的点云图像较为对称，传统ICP算法的旋转梯度可能会相互抵消，从而陷入局部最优；交换机制从另一个对称匹配的角度，有可能让点云进行正确的变换，从而帮助算法跳出局部最优。 而误差较小时，无论是否交换，都倾向于一对一的正确匹配，交换机制对算法没有影响。

如图1所示，传统ICP算法的旋转梯度抵消，陷入局部最优；而交换机制使得算法跳出局部最优。

图1 对ATICP左为奇数次匹配，右为偶数次匹配；对传统ICP只进行左图的匹配Fig.1 For ATICP, the left is an odd number of matches, and the right is an even number of matches; for traditional ICP,only the left image

（2）截取机制（Truncated在后文图标简写为T）。

在上文介绍的传统ICP算法Step1前加入一步预处理，将点云A、B中距离中心点较小的点按比例去除一部分，再进入算法。

截取机制对ICP算法的影响：当点数较少时，越多的点意味着越多的信息，可以帮助更快更好的配准，但是点数足够时，在相同的点分布的情况下，更多的点数只能徒增计算量。而且由于点云本身的误差，部分距离中心较近的点还会产生错误的梯度信息，对配准不利。

2 实验

编程环境如表1所示。

表1 编程环境Tab.1 Programming environment

随机生成点云数据集的生成规则如式（11）、式（12）所示：

对任意i∈ { 1 ， 2 …50}

这样生成的随机点云较为对称，配准难度大，更容易体现模型的优劣。

最大迭代次数N=10，终止误差δ=3，在截取步骤中截取40%的点云进行迭代。

一次实验具有偶然性，这里进行1000次模拟实验对比，如图3所示展示为误差大于100的模拟。图例中T指截取机制，A指交换机制，前缀表示在ICP算法上的使用对应的技术。

如图2所示，误差大于100的模拟实验中，ICP算法出现的次数最多且误差较大，加入两种机制的ATICP算法出现次数最少且误差较小。两种机制加入后的单独实验也显示对ICP算法有不同程度的优化。

图2 4种算法1000次模拟实验的误差大于100的散点图Fig.2 Scatter plot with errors bigger than 100 for 1000 simulation experiments of four algorithms

err为前文定义的误差均值，Time表示运行的平均时间，T指截取机制，A指交换机制。

如表2所示，分别加入交换机制和截取都对算法的误差和计算量有一定程度的优化，且二者协同后，起到了叠加作用，使得平均误差减小了83.6%，计算时间减小了42.4%。

表2 4种算法的模拟测试误差和时间的均值Tab.2 The mean value of simulated test error and time for 4 algorithms

3 结论

本文在传统ICP算法上进行改进，针对容易陷入局部最优、误差大、计算量大的成因进行分析，提出了截取机制和交换机制。加入算法改进后，平均误差减小了83.6%，计算时间减小了42.4%，提高了算法的性能。

引用

[1]BESL P J,MCKAY H D.A Method for Registration of 3-D Shapes[J].IEEE Transactions on Pattern Analysis and Machine Intelligence,1992,14(2):239-256.

[2]解则晓,徐尚.三维点云数据拼接中ICP及其改进算法综述[J].中国海洋大学学报(自然科学版),2010,40(1):99-103.

[3]渠瀛.基于激光测距仪的移动机器人二维地图创建问题研究[D].湖南:中国人民解放军国防科技大学,2011.

[4]谢小鹏,古家威.一种改进的二维ICP点云配准算法[J].激光与红外,2021,21(7):951-955.