一种基于ICP算法的降维加速点云配准方法

一种基于icp算法的降维加速点云配准方法
技术领域
1.本发明涉及工件在线三维检测技术领域，尤其涉及一种基于icp算法的降维加速点云配准方法。


背景技术：

2.随着现代工业的不断进步，对生产工件的精度要求也不断提升。传统的二维图像检测系统只能获取工件表面反射回的亮度信息，而三维测量可以获取高精度的工件高度信息。工件的某些表面特征的很难通过表面光照信息获得，如平面之间的高度差和曲面轮廓的曲率等，需要依赖三维测量获取的高度信息进行检测。
3.在三维检测领域中，国内外已经有一批企业可以生产高精度的三维传感器，通过三维传感器可以获取工件的三维点云数据，即高度信息，进而完成高度测量，轮廓测量、连续轮廓测量和曲面曲率测量等测量项目。然而，在实际应用到生产线上的在线三维检测中，由于上游生产线的来料位姿不固定，对被测量工件进行检测前需要先进行点云配准，即将被测量工件的点云数据的位姿摆正到与注册的标准点云数据一致。
4.幸运的是，在实际工业生产场景中，上游生产线的来料的位姿差异不大，工件在传送带上测量的过程中，其相对测量基准面不会发生俯仰角和滚动角的变化，即存在一个平面约束，只有偏航角可能发生变化，与此同时，由于工件位于在传送带上，其竖直方向的坐标是固定不动的。所以三维点云数据的配准由6自由度降维成3自由度，即一个旋转角度和两个方向的平移(x,y)。使用传统的icp配准算法耗时较长，难以满足生产线对实时检测的需求，严重缩小了在线三维检测的应用场合，制约了在线高精度检测领域的发展。


技术实现要素：

5.本发明旨在至少在一定程度上解决相关技术中的技术问题之一。为此，本发明的一个目的在于提出一种基于icp算法的降维加速点云配准方法，在考虑生产线的先验条件的基础上，利用降维的方式缩小搜索维度，然后使用二次插值法加速迭代过程，使点云配准过程的时间降低到百毫秒。
6.根据本发明提出的一种基于icp算法的降维加速点云配准方法，所述方法步骤如下：
7.s1：导入点云数据并初始化参数；
8.s2：使用四元数法计算配准参数[rk,tk]；
[0009]
s3：利用二次插值法调整配准参数，加速算法的迭代；
[0010]
s4：更新待配准点云数据以及均方误差；
[0011]
s5：若步骤s4中的所述均方误差小于设定值τ或者迭代次数达到设定值k
max
，则转步骤s3继续迭代，否则结束迭代，返回配准参数[rk,tk]。
[0012]
优选地，步骤s3中所述的利用二次插值法调整配准参数步骤如下：
[0013]
s31：依据迭代第k次时配准参数中的旋转矩阵rk计算绕z轴旋转角度，即
[0014][0015]
推算得到：
[0016][0017]
其中，θk表示第k次迭代时配准参数中的绕z轴旋转角度，此时，令ek、e
k-1
、e
k-2
为θk、θ
k-1
、θ
k-2
对应的均方差，若满足：
[0018][0019]
则可以利用二次插值法的思想来对迭代过程进行加速，令：
[0020]
x
k-2
＝θ
k-2
，x
k-1
＝θ
k-1
+x
k-2
，xk＝θk+x
k-1
[0021]
s32：构建三个以xk为x坐标，以ek为y坐标的坐标点(xk,ek)，(x
k-1
,e
k-1
)，(x
k-2
,e
k-2
)，并以此三点计算抛物线函数e(x)＝ax2+bx＝c，计算所述抛物线函数极值位置的坐标为
[0022]
s33：计算出θ
min
＝x
min-xk，依据上述公式计算出旋转矩阵r
min
，保持平移向量tk不变，应用配准参数[r
min
,tk]，计算均方误差e
min
，若e
min
＜ek，则采用上述配准参数[rk,tk]＝[r
min
,tk]，并继续执行步骤s4。
[0023]
优选地，步骤s1中的所述导入点云数据包括标准点云数据q＝{qj|qj∈r3，j＝1，2
…
n}和待配准点云数据集合p＝{pi|pi∈r3，i＝1，2
…
n}。
[0024]
优选地，步骤s1中的所述初始化参数包括：设定循环次数k＝0，每次迭代后的距离误差ek＝0，初始化配准参数[rk,tk]为：
[0025][0026]
优选地，步骤s2中使用四元数法计算配准参数[rk,tk]的具体方法步骤如下：
[0027]
s21：计算注册点云数据q和待配准点云数据p的质心：
[0028][0029]
s22：计算pi、qi的协方差矩阵e：
[0030][0031]
s23：计算协方差矩阵的循环列向量δ＝[e
12 e
23 e
31
]，列向量可以分别使用e
ij
＝(e-e
t
)
i,j
(i,j＝1,2,3)计算得到；
[0032]
s24：搭建对称矩阵，阶数为四：
[0033][0034]
其中，tr(x)表示矩阵x的迹，即对角线元素之和，i3表示三阶单位矩阵；
[0035]
s25：计算上述四阶矩阵的特征值以及与之对应的特征向量，同时记录特征值取最大值时的特征向量，则该特征向量即为所求四元数向量[a
0 a
1 a
2 a3]
t
；
[0036]
s26：推导出刚性旋转矩阵r即为：
[0037][0038]
同时，依据旋转矩阵与平移矩阵的关联，由t＝c
p-rcq解算出平移矩阵t，至此，得出旋转矩阵和平移矩阵(r,t)。
[0039]
优选地，步骤s4中的所述更新待配准点云数据包括将计算得到的配准参数[rk,tk]作用于p
k-1
，得到新的待配准点云数据pk：
[0040]
pk＝pkp
k-1
+tk。
[0041]
优选地，步骤s4中的所述更新均方误差的步骤如下：
[0042][0043]
优选地，步骤s5中的所述设定值τ为1e-03mm，所述设定值k
max
为20。
[0044]
本发明中的有益效果是：
[0045]
(1)充分利用了生产线上的先验知识，将三维点云数据的配准维度由六维降维成三维，即一个旋转角度和两个方向的平移(x,y)，由于自由度的降低，使算法迭代收敛速度加快，精度提高；
[0046]
(2)利用二次插值法对偏航角的迭代做进一步加速，减少了迭代收敛的次数，使配准耗时降低到百毫秒。
附图说明
[0047]
附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。在附图中：
[0048]
图1为本发明提出的基于icp算法的降维加速点云配准方法的流程框图；
[0049]
图2为本发明提出的利用二次插值法调整配准参数的执行过程流程框图；
[0050]
图3为本发明提出的未配准的点云数据图像；
[0051]
图4为本发明提出的配准后的点云数据图像。
具体实施方式
[0052]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。
[0053]
参照图1-4，一种基于icp算法的降维加速点云配准方法，用于在线三维检测过程中的待测量点云数据与标准点云数据的配准，如图3、图4所示，本实施例采用测量柱面镜表面关键参数弓形高度(sagitta,sag)项目实例来进行验证，图3为未配准的点云数据图像，
图4为配准后的点云数据图像，方法步骤如下：
[0054]
s1：导入点云数据并初始化参数；
[0055]
s2：使用四元数法计算配准参数[rk,tk]，也可采用svd法替代四元数法计算配准参数[rk,tk]；
[0056]
s3：利用二次插值法调整配准参数，加速算法的迭代；
[0057]
s4：更新待配准点云数据以及均方误差；
[0058]
s5：若步骤s4中的所述均方误差小于设定值τ或者迭代次数达到设定值k
max
，则转步骤s3继续迭代，否则结束迭代，返回配准参数[rk,tk]。
[0059]
步骤s3中所述的利用二次插值法调整配准参数步骤如下：
[0060]
s31：依据迭代第k次时配准参数中的旋转矩阵rk计算绕z轴旋转角度，即
[0061][0062]
推算得到：
[0063][0064]
其中，θk表示第k次迭代时配准参数中的绕z轴旋转角度，此时，令ek、e
k-1
、e
k-2
为θk、θ
k-1
、θ
k-2
对应的均方差，若满足：
[0065][0066]
则可以利用二次插值法的思想来对迭代过程进行加速，令：
[0067]
x
k-2
＝θ
k-2
，x
k-1
＝θ
k-1
+x
k-2
，xk＝θk+x
k-1
[0068]
s32：构建三个以xk为x坐标，以ek为y坐标的坐标点(xk,ek)，(x
k-1
,e
k-1
)，(x
k-2
,e
k-2
)，并以此三点计算抛物线函数e(x)＝ax2+bx＝c，计算所述抛物线函数极值位置的坐标为
[0069]
s33：计算出θ
min
＝x
min-xk，依据上述公式计算出旋转矩阵r
min
，保持平移向量tk不变，应用配准参数[r
min
,tk]，计算均方误差e
min
，若e
min
＜ek，则采用上述配准参数[rk,tk]＝[r
min
,tk]，并继续执行步骤s4。
[0070]
步骤s1中的所述导入点云数据包括标准点云数据q＝{qj|qj∈r3，j＝1，2
…
n}和待配准点云数据集合p＝{pi|pi∈r3，i＝1，2
…
n}。
[0071]
步骤s1中的所述初始化参数包括：设定循环次数k＝0，每次迭代后的距离误差ek＝0，初始化配准参数[rk,tk]为：
[0072]
[0073]
步骤s2中使用四元数法计算配准参数[rk,tk]的具体方法步骤如下：
[0074]
s21：计算注册点云数据q和待配准点云数据p的质心：
[0075][0076]
s22：计算pi、qi的协方差矩阵e：
[0077][0078]
s23：计算协方差矩阵的循环列向量δ＝[e
12 e
23 e
31
]，列向量可以分别使用e
ij
＝(e-e
t
)
i,j
(i,j＝1,2,3)计算得到；
[0079]
s24：搭建对称矩阵，阶数为四：
[0080][0081]
其中，tr(x)表示矩阵x的迹，即对角线元素之和，i3表示三阶单位矩阵；
[0082]
s25：计算上述四阶矩阵的特征值以及与之对应的特征向量，同时记录特征值取最大值时的特征向量，则该特征向量即为所求四元数向量[a
0 a
1 a
2 a3]
t
；
[0083]
s26：推导出刚性旋转矩阵r即为：
[0084][0085]
同时，依据旋转矩阵与平移矩阵的关联，由t＝c
p-rcq解算出平移矩阵t，至此，得出旋转矩阵和平移矩阵(r,t)。
[0086]
步骤s4中的所述更新待配准点云数据包括将计算得到的配准参数[rk,tk]作用于p
k-1
，得到新的待配准点云数据pk：
[0087]
pk＝pkp
k-1
+tk。
[0088]
步骤s4中的所述更新均方误差的步骤如下：
[0089][0090]
步骤s5中的所述设定值τ为1e-03mm，所述设定值k
max
为20。
[0091]
在此实例中，与传统icp算法以及accicp算法的计算耗时和配准精度的对比见下表：
[0092]
表1实验对比表
[0093][0094]
从表中可以看出，本发明算法的耗时要大幅低于传统icp法和accicp法，同时配准精度更高。
[0095]
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。