基于一致点漂移算法的三维非刚体点云配准方法与流程

本发明涉及基于点云配准的喷漆机器人位置配准方法。

背景技术：

随着现代工业对产品质量和生产效率的要求，计算机辅助制造工程已经成为现代计算机科学的重要组成部分，而三维点云配准技术则是计算机辅助制造工程中较为重要的环节。在工业生产中，通过点云配准可以起到控制工业生产测量精度误差、快速检测工业产品缺陷和加快工业生产进度等作用。此外，在医疗诊断、文物修复等非工业领域，点云配准技术依然有着十分重要的作用，尤其是在喷漆机器人领域，利用三维点云配准技术可以极大提高机器人喷漆效率。

三维点云配准技术就是对采集回的点云数据进行匹配，找到两组点云数据集的一系列空间变换关系，并通过这些空间变换，使得两个点云数据集的对应点在空间位置上尽最大可能的一致。根据待配准点云形变模型的复杂程度，点云配准可以分为刚体点云配准和非刚体点云配准。对于刚体点云的配准问题已经取得了很多理论成果，然而，现实中普遍存在的是非刚体形变，所以对非刚体的研究十分重要。

对于刚体点云配准，由beslpj和mckaynd于1987年提出的迭代最近点算法(iteratedclosestpointsalgorithm,icp)使用最为广泛，该算法运用最小二乘的最优匹配原理，当初值较好时，具有极高的配准精度。而相对于刚体点云配准，非刚体点云的配准涉及到更多的自由度，也更加复杂。为有效解决这一难题，相关的基于概率的软匹配方法被相继提出，即利用概率值来描述点对点之间可能的匹配对应关系。其中较为成功的是mcneill与vijayakumar等提出的基于高斯混合模型(gaussianmixturemodels,gmm)的em算法,以及在此基础上，andriymyronenko和xubosong提出了一致点漂移算法(coherentpointdrift,cpd)，两者的核心都是将匹配矩阵的求解转化为gmm的质心点集与观测数据点集进行拟合的最大似然估计问题，前指利用em(期望最大值)算法，后者基于运动一致性原理(motioncoherenttheory,mct)求解优化，因此对于噪声、出格点以及缺失点的影响具有较强鲁棒性。

然而，对于实际中采集到的非刚体三维点云，因为其点云数据量大、变形复杂、有噪声、出格点和缺失点的影响，所以大部分三维点云非刚体配准算法存在配准精度低、鲁棒性差、计算时间长。

技术实现要素：

本发明的目的是为了解决现有三维点云非刚体配准算法在点云数据量大、变形复杂、有噪声、出格点和缺失点影响的情况下，存在配准精度低、鲁棒性差、计算时间长的缺点，而提出基于一致点漂移算法的三维非刚体点云配准方法。

基于一致点漂移算法的三维非刚体点云配准方法具体过程为：

步骤一、利用喷漆机器人中的图像采集设备对待喷漆物体进行扫描，采集得到一组三维点云数据作为待配准点云；

步骤二、对步骤一中采集得到的待配准点云进行预处理，得到的点云数据作为参考点集；

步骤三、计算步骤二中得到的参考点集与已有的模板点集的协方差σ，并初始化一致点漂移算法的相关参数；

一致点漂移算法的相关参数包括系数矩阵w，反应噪声影响占比的权值ω，高斯滤波器的宽度β和最大似然拟合参数λ；

步骤四、通过步骤三得到的协方差和初始化后的相关参数，构造高斯核矩阵g；

步骤五、利用步骤四中得到的高斯核矩阵，迭代更新模板点集直至收敛，得到最终配准的结果点集，根据最终的配准结果点集对待喷漆物体进行喷漆作业。

本发明的有益效果为：

1)本发明针对现有的主流三维点云非刚体配准算法在点云数据量大、变形复杂、有噪声、出格点和缺失点影响的情况下，存在配准精度低、鲁棒性差、计算时间长的问题，提出来了基于一致点漂移算法的三维非刚体点云配准并行处理方法，保证了三维非刚体点云配准的强鲁棒性和高配准精度，可以精准完成对待喷漆物体进行喷漆。

2)本发明提高了配准精度，在有噪声、出格点和缺失点的情况下，配准误差小于0.01个单位长度。

3)本发明采用的gpu并行处理极大的缩短了程序的运行时间，较现有的国内外广泛使用的cpd点云配准库，在程序运行效率上有较大的优越性：对于有四千多个点的两组点云，cpd点云配准库中的非刚体配准算法完成一次配准的时间约为35min，而本发明的运行时间仅为8min。

附图说明

图1是具体实施方式一中基于一致点漂移算法的三维非刚体点云配准并行处理方法的流程图。

图2是步骤一中使用kineat采集到的一组点云图；

图3是步骤二中对原始点云数据进行无关背景移除和降采样等预处理后的点云图；

图4是具体实施方式五中将模板点云配准到参考点云后的最终效果图。

具体实施方式

具体实施方式一：结合图1说明本实施方式，本实施方式的基于一致点漂移算法的三维非刚体点云配准并行处理方法具体过程为：

步骤一、利用喷漆机器人中的图像采集设备对待喷漆物体进行扫描，采集得到一组三维点云数据作为待配准点云；如图2；

步骤二、对步骤一中采集得到的待配准点云进行预处理，得到的点云数据作为参考点集；如图3；

步骤三、计算步骤二中得到的参考点集与已有的模板点集的协方差σ，并初始化一致点漂移算法的相关参数；

一致点漂移算法的相关参数包括系数矩阵w，反应噪声影响占比的权值ω，高斯滤波器的宽度β和最大似然拟合参数λ；

步骤四、通过步骤三得到的协方差和初始化后的相关参数，构造高斯核矩阵g；

步骤五、利用步骤四中得到的高斯核矩阵，迭代更新模板点集直至收敛，得到最终配准的结果点集，根据最终的配准结果点集对待喷漆物体进行喷漆作业。

具体实施方式二：本实施方式与具体实施方式一不同的是：所述步骤二中对步骤一中采集得到的待配准点云进行预处理,得到的点云数据作为参考点集；具体过程为：

步骤二一、对步骤一中采集得到的待配准点云中不需要配准的背景点云数据进行删除，得到去背景后的点云；

步骤二二、使用统计滤波器和半径滤波器对步骤二一中得到的去背景后的点云中的离群点进行删除，得到滤波后的点云；

步骤二三、对步骤二二中得到的滤波后的点云进行降采样，得到降采样后的点云；以稀疏点云数据，达到减少点云数据量的目的；

步骤二四、将步骤二三中得到的降采样后的点云以矩阵的形式保存为一个数据集，做为参考点集。

其它步骤及参数与具体实施方式一相同。

具体实施方式三：本实施方式与具体实施方式一或二不同的是：所述步骤三中计算步骤二中得到的参考点集与已有的模板点集的协方差σ，并初始化一致点漂移算法的相关参数；具体过程为：

步骤三一、参考点集和模板点集分别表示为xn×d＝(x1,…xn)^t，ym×d＝(y1,…ym)^t，则两点集的协方差初始化为：

其中，m,n分别为模板点集和参考点集中点的数目，取值为正整数；d为点集的维数；xn为参考点集中第n个点的d维向量，ym为模板点集中第m个点的d维向量；

步骤三二、对一致点漂移算法的相关参数初始化，系数矩阵w初始化为零矩阵，即w＝0；对ω,β,λ初始化，满足0≤ω≤1，β＞0，λ＞0。

其它步骤及参数与具体实施方式一或二相同。

具体实施方式四：本实施方式与具体实施方式一至三之一不同的是：所述步骤四中通过步骤三得到的协方差和初始化后的相关参数，构造高斯核矩阵g；具体过程为：

式中，gij表示高斯核矩阵g第i行第j列元素，e是自然对数的底数，β为高斯滤波器的宽度，yi,yj分别表示模板点集中第i个和第j个点的d维向量。

其它步骤及参数与具体实施方式一至三之一相同。

具体实施方式五：本实施方式与具体实施方式一至四之一不同的是：所述步骤五中利用步骤四中得到的高斯核矩阵，迭代更新模板点集直至收敛，得到最终配准的结果点集；具体过程为：

步骤五一、利用cuda核函数计算高斯关联矩阵k：

先将模板点集ym×d和参考点集xn×d拷贝到显存，并为cuda核函数分配m×n个线程，所有线程同时计算矩阵k中的对应元素，如索引为(m,n)的线程应计算矩阵k的第m行第n列元素，计算方法为

式中，kmn为高斯关联矩阵k的第m行第n列元素；

步骤五二、基于步骤五一得到的高斯关联矩阵k，利用cuda核函数在gpu中并行计算下列三个向量：

p^tl＝l-ca；pl＝ka；pxn×d＝k(a.*xn×d)

其中，p为高斯混合模型的后验概率矩阵，符号.*表示点乘，上角标t表示矩阵的转置，l为元素全为1的列向量，a＝1./(k^t1+cl)，符号./表示点除，xn×d则是参考点集；

步骤五三、对步骤五二得到的三个向量，利用cuda核函数中的cusolver库进行如下线性方程求解：

aw＝b

其中a＝g+λσ²d(pl)^-1，b＝d(pl)^-1pxn×d-ym×d，d(pl)表示由向量pl生成的对角矩阵；a、b为中间变量；

步骤五四、利用步骤五三得到的系数矩阵w，对模板点集进行更新，更新后的模板点集为：

其中，ym×d为模板点集，g为高斯核矩阵；

步骤五五、利用步骤五四中得到的更新后的模板点集，计算并更新参考点集和模板点集的协方差：

其中，np＝l^tpl；符号tr(a)表示矩阵a的迹，a为x^tm×dd(p^tl)xn×d、或

步骤五六、重复步骤五一到步骤五五，直至满足收敛条件，求出满足收敛条件时的g和w；

收敛条件为协方差σ²小于预设阈值，预设阈值设置为计算机最小非零浮点数的十倍(根据实际情况决定，推荐设置为计算机最小非零浮点数的十倍)；

步骤五七、根据步骤五六迭代的结果，计算得到最终配准的结果点集：res＝ym×d+gw。如图4。

其它步骤及参数与具体实施方式一至四之一相同。

具体实施方式六：本实施方式与具体实施方式一至五之一不同的是：所述步骤五三中系数矩阵w具体求解过程为：

1.初始化cusolver和cublas求解器；

2.将矩阵a和b拷贝进显存；

3.计算所需的工作缓冲区的大小；

4.对矩阵a进行qr分解，即a＝qr；

5.计算q^tb的值；

6.求解多右端线性方程组：rw＝q^tb，得到系数矩阵w的值；

7.释放缓冲区和显存。

其它步骤及参数与具体实施方式一至五之一相同。

采用以下实施例验证本发明的有益效果：

实施例一：

本实施例基于一致点漂移算法的三维非刚体点云配准并行处理方法用于对喷漆机器人采集到的三维点集x4436×3与已有的模板点集y4672×3进行位置配准。具体步骤与本发明具体实施过程一致，其中相关参数设置如下：

反应噪声影响占比的权值ω＝0.1，高斯滤波器的宽度β＝3.0，最大似然拟合参数λ＝3.0；

步骤一、利用喷漆机器人中的图像采集设备对待喷漆物体进行扫描，采集得到一组三维点云数据作为待配准点云；

步骤二、对步骤一中采集得到的待匹配点云进行预处理，得到的点云数据作为参考点集；

步骤三、计算步骤二中得到的参考点集与已有的模板点集的协方差σ，并初始化一致点漂移算法的相关参数；

一致点漂移算法的相关参数包括系数矩阵w，反应噪声影响占比的权值ω，高斯滤波器的宽度β和最大似然拟合参数λ；

步骤四、通过步骤三得到的协方差和初始化后的相关参数，构造高斯核矩阵g；

步骤五、利用步骤四中得到的高斯核矩阵，迭代更新模板点集直至收敛，得到最终配准的结果点集，根据最终的配准结果点集对待喷漆物体进行喷漆作业。

最终，在intel酷睿i53230m处理器、nvidiageforcegt740m的显卡配置下，程序运行时间约为8min，相比其他主流算法效率更高。此外，配准误差小于0.01m，配准精度较高。

本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。