CN108830902A - 一种基于点云处理的散乱工件识别与定位方法 - Google Patents

Abstract

本发明公开了一种基于点云处理的散乱工件识别与定位方法，用以解决随机箱体抓取过程中散乱工件的位姿估计问题。该方法分为离线模板库建立和在线特征配准两个部分。模板点云数据集和场景点云由3D点云获取系统得到。离线状态下提取模板点云的特征信息可以用于场景点云的预处理、分割以及配准阶段，提高了算法的运行速度。将点云配准分为初始配准和精确配准两个阶段，初始配准提出几何特征和统计特征融合的特征描述子，实现对关键点特征的唯一性描述，通过在模板库中搜索与特征点特征描述最相近的点为对应点，得到对应点集，进而计算初始变换矩阵；精确配准阶段，添加几何约束实现对应点选取，减少精配准的迭代次数，降低算法陷入局部最优的概率。

Description

一种基于点云处理的散乱工件识别与定位方法

技术领域

本发明涉及机器视觉领域中的目标识别和定位，尤其涉及一种基于点云处理的散乱工件识别与定位方法，可以实现姿态任意工件的位姿估计。

背景技术

为了使机器人能从随机放置且未排序的箱体中以正确的方向定位和抓取工件，需要对目标工件进行位姿估计。近年来，使用三维扫描设备(如三维激光扫描仪、立体视觉系统等)获取工件表面的三维点云数据(即工件点云)，并将工件点云和位姿已知模板点云进行配准得到两者之间的刚性变换，从而实现工件的识别和定位成为了机器视觉领域的研究热点。利用三维点云处理算法计算单个工件位姿，引导机器人进行抓取作业，效率高、速度快，更能提高工业生产效率。

最常见的配准算法迭代最近点算法(Iterative Closest Point，ICP)及后续研究者的改进算法，ICP算法原理简单易实现，配准精度高，缺点为：(1)当两幅点云之间不存在包含关系或初始位置偏差较小时，算法易陷入局部最优；(2)迭代收敛速度慢。因此，在进行精确配准之前先进行初始配准很有必要。工业上常见的初始配准算法多是基于特征,基于特征的方法可以分为全局特征和局部特征，通过测量和比较3D形状的几何形状或拓扑属性来区分3D对象。基于全局特征描述的配准算法对于杂乱、有遮挡场景下的抗干扰能力不强，而通过局部特征匹配建立的对应关系对视点、杂波和遮挡的变化具有很好的鲁棒性，工业上应用最多。常见的局部特征描述子有快速点特征直方图描述子(FPFH，Fast PointFeature Histograms)、3D形状内容描述子(3D Shape Context)、旋转图像(Spin image)、方向直方图签名描述子(SHOT，Signature of Histograms of OrienTations)、旋转投影统计(RoPS，Rotational Projection Statistics)等。其中，RoPS描述子具有很好的抗噪能力，鉴别力最好，但实时性差；在对实时性要求高的场合下，FPFH和SHOT是最好的选择。当局部表面上的点较少时，FPFH描述子是最佳选择，这是由于FPFH鉴别力强、运算速度快且占用存储资源少。反之，SHOT描述子是最佳选择，其在鉴别力与计算效率之间能取得很好的折中。

针对不同的对象的几何特征,选择相应的配准算法可以实现工件的精确快速定位,但是目前仍然没有一种通用的目标识别和定位算法可以适用于所有对象,需要根据对象的不同不断地调整解决方案。因此，不断开发新的目标识别和定位算法成为当前研究人员的研究热点。

发明内容

本发明为实现散乱堆放工件的位姿估计,提供了一种基于点云处理的散乱工件识别与定位方法,该方法在满足实时性要求的同时，能够准确的估计箱体内任意工件的位姿信息,从而为机器人提供抓取点。

本发明的技术方案如下：

一种基于点云处理的散乱工件识别与定位方法，具体过程如下：

步骤1、对模板点云和场景点云进行预处理，得到去除背景点和离群点后的模板点云和场景点云；

步骤2、在离线状态下，建立模板点云特征库，对于每个模板点云，提取其关键点并对关键点进行特征描述；

步骤3、在在线状态下，对场景点云进行分割，将具有碰撞、遮挡、包含多个工件的场景点云分割成多个只包含单个工件的场景点云子集，对场景点云子集进行筛选得到候选可抓取点云子集；

步骤4、在线状态下，对候选点云和模板点云进行初始配准，对于每个候选点云子集，根据全局特征相似性度量确定工件点云在模板库中对应的模板点云，计算候选工件点云关键点的特征描述子与对应模板点云中对应点的特征描述子的差异，找出特征描述子最相近的点作为对应点；

步骤5、通过对应点之间的几何关系判断对应关系是否正确，从而去除误匹配点对，根据两幅点云之间的对应点对求解模板点云到场景点云的变换矩阵R和T，即初始变换矩阵；

步骤6、使用改进ICP算法进行点云精确配准实现目标识别和定位，通过对应点在两幅点云中的几何约束得到正确的对应点，进而得到两者之间的最佳变换矩阵，从而得到工件点云的位姿信息。

其进一步的技术方案为，所述步骤2具体包括：

步骤2.1、计算模板点云密度；

设模板点云M有n个点，对任意一点p_i∈M，其r邻域半径内邻近点集合p_i ^k，记作{p_i1,p_i2,p_i3,...,p_ik}，计算每个邻近点距p_i点的平均距离d_i，如下式所示：

式(1)中，d_ij表示第j个邻近点到点p_i的距离，遍历点云M中所有的点，得到n个平均距离值，根据下式得到点云M在k邻域下的平均距离均值，即点云密度；

步骤2.2、计算模板点云中各点的法线；采用主成分分析法求取点云中所有点的法线，具体步骤是：对任意一点p_i∈M，构件其k个邻近点{p_i1,p_i2,p_i3,...,p_ik}对应的协方差矩阵M：

式(3)中，表示最邻近元素的三维质心，λ_j和分别代表协方差矩阵的特征值和特征向量，λ₀<λ₁<λ₂；具有最小特征值的特征向量即p_i点的法线，记作

步骤2.3、计算模板点云特征描述子；具体过程为：

步骤2.3.1、关键点提取；求模板点云的关键点具体步骤为：首先，求取点云的方向包围盒OBB，方向包围盒OBB由基准角C(x_c,y_c,z_c)和三个矢量CC₁(x_max,y_max,z_max)，CC₂(x_mid,y_mid,z_mid)，CC₃(x_min,y_min,z_min)表示；三个矢量CC₁(x_max,y_max,z_max)，CC₂(x_mid,y_mid,z_mid)，CC₃(x_min,y_min,z_min)分别对应于方向包围盒OBB的最大、中间和最小维度；然后，取方向包围盒OBB的高的一半为阈值，利用直通滤波器进行滤波，去除小于阈值的点，保留余下的点记作候选关键点；最后对关键点云进行均匀采样，提高匹配速度；

步骤2.3.2、对关键点进行特征描述；具体的有：

步骤2.3.2.1、计算关键点的SHOT特征描述子；首先，根据特征点球邻域信息建立局部参考坐标系，对特征点的球邻域分别沿径向、方位角方向和高度方向进行区域划分；分别统计每个网格内的法向量夹角余弦值分布；最终得到SHOT特征描述子；

cosθ_q＝z_kn_q (5)

式(5)中，z_k为r半径球邻域内局部坐标系的Z轴，即特征点的法线；n_q是当前网格标签内所包含点的法线；

步骤2.3.2.2、引入关键点的空间位置描述子，该空间位置描述子在计算对应匹配点对时考虑特征点和重心点的关系；点云重心表示为特征点和点云重心的对应关系为：

其中，l表示点云中特征点的数目，表示当前点的法线；

步骤2.4、对于模板点云集中的所有模板点云，重复步骤2.1～步骤2.3，得到每个模板点云的特征集第i个模板点云的特征描述子表示为其中C,CC₁,CC₂,CC₃为点云方向包围盒特征，f0为SHOT特征，f1,f2为空间位置特征。

其进一步的技术方案为，所述步骤4和步骤5具体包括如下内容：

在步骤4中有：

步骤4.1、计算各点云子集的特征描述子第j个点云子集的特征描述子表示为其中C_o,CC_o1,CC_o2,CC_o3为方向包围盒特征，f0_o为SHOT特征，f1_o,f2_o为空间位置特征；

步骤4.2、对工件点云子集中的任意特征点，在模板点云中搜索其对应点；具体步骤为：首先，计算工件点云的方向包围盒特征，在模板点云库中搜索对应模板点云，两点云之间的方向包围盒特征应满足如下公式：

然后，对比工件点云和与其对应的模板点云的特征描述子差异，查找对应点，设工件点云中任意点q_i在模板点云中的对应点为p_i，需满足如下公式，得到特征点在模板点云中的对应点，若该组对应点的空间位置描述子的差异在预设阈值范围内，则保留当前对应点，若该组对应点的空间位置描述子的差异在预设阈值范围之外则舍弃；

式(9)中，ε₀,ε₁,ε₂为阈值；

在步骤5中，对步骤4所述的对应点对采用奇异值分解算法求解模板点云到目标点云之间的旋转R和平移矩阵T，即初始配准结果。

其进一步的技术方案为，所述步骤6包括：

步骤6.1、在工件点云O中随机选取一定数量的点组成工件点云随机点集q^k，

步骤6.2、在对应模板点云M中寻找与点距离最近的点组成目标点云对应点集，随机选取3组对应点对，对应点在工件点云和模板点云中的分布应近似相同，该分布可以用对应点之间的距离表示，若满足既定条件则为正确对应点，采用的四元数法计算出旋转矩阵R^k和平移矩阵T^k；

步骤6.3、计算配准误差，公式如下：

如果误差函数保持不变或者达到设定要求，则停止迭代，当前旋转矩阵旋转矩阵R^k和平移矩阵T^k就是最终的配准参数矩阵；如果没有达到设定要求，则O^k+1＝R^kQ^k+T^k，重复上述步骤；式中k表示迭代次数，n表示点云O的随机点集中元素的个数。

本发明的有益技术效果是：

本发明提出一种基于点云处理的散乱工件识别与定位方法，实现随机箱体抓取过程中的目标工件的位姿定位。该算法将工件识别与定位分为离线模板特征库的建立和在线点云特征配准两个部分，离线建立模板特征库大大降低了算法的运行速度。配准过程中，提出OBB裁剪方法提取关键点，消除边缘稀疏点特征描述差异带来的影响；针对点云相似区域单一的特征匹配识别度低的问题，提出基于特征融合的特征描述子，实现对特征点的唯一性描述，从而降低误匹配的概率，并提高初始配准的准确性；精确配准阶段，添加几何约束实现对应点选取，从而减少精配准的迭代次数，降低算法陷入局部最优的概率。

附图说明

图1是离线流程图。

图2是在线流程图。

图3是球形域示意图。

具体实施方式

为了更清楚的说明本发明的技术方案和优点，下面结合具体实施例，并参附图，阐述本发明的具体实施方式。

本发明公开了一种基于点云处理的散乱工件识别与定位方法。目的是引导机器人从随机放置且未排序的箱体中以正确的方向定位和抓取工件。本发明的整体步骤中，包括在离线状态下处理的流程与在线状态下处理的流程。结合图1、图2，本发明的整体步骤包括：

步骤1、对模板点云和场景点云进行预处理，得到去除背景点和离群点后的模板点云和场景点云。场景点云是指散乱放置的多个工件的表面点云。

如图1和图2所示，模板点云和场景点云均需要进行预处理步骤。

模板点云的预处理步骤如下：

步骤1.1、去除背景点。通过拟合箱体底部平面方程，去除模板点云中距离背景平面较近的点。

步骤1.2、去除离群点。使用半径滤波方法去除离群点的思想是：首先，构建模板点云M的kd树，便于邻近点的快速查找；然后，对任意一点p_i∈M，搜索其r邻域半径内的邻近点集合l表示点p_i的r邻域半径内邻近点的个数，将l值大小和预先设定的模板点云密度阈值n_th比较。若k<n_th，则点p_i判定为离群点，并从模板点云M中剔除。其中，i指模板点云M中的第i个点。

场景点云的预处理步骤与模板点云的预处理步骤方法相同。

步骤2、在离线状态下，建立模板点云特征库，对于每个模板点云，提取其关键点并对关键点进行特征描述。具体的，如图1所示，步骤2包括：

步骤2.1、计算模板点云密度；

设模板点云M有n个点，对任意一点p_i∈M，其r邻域半径内邻近点集合记作{p_i1,p_i2,p_i3,...,p_ik}，计算每个邻近点距p_i点的平均距离如下式所示：

式(1)中，d_ij表示第j个邻近点到点p_i的距离。

遍历模板点云M中所有的点，得到n个平均距离值，根据下式得到模板点云M在k邻域下的平均距离均值，即点云密度；

步骤2.2、计算模板点云中各点的法线；采用主成分分析法(PCA，PrincipalComponent Analysis)求取点云中所有点的法线，具体步骤是：对任意一点p_i∈M，构件其k个邻近点{p_i1,p_i2,p_i3,...,p_ik}对应的协方差矩阵M：

式(3)中，表示最邻近元素的三维质心，λ_j和分别代表协方差矩阵的特征值和特征向量，λ₀<λ₁<λ₂。具有最小特征值的特征向量即p_i点的法线，记作

步骤2.3、计算模板点云的特征描述子；具体过程为：

步骤2.3.1、关键点提取；求模板点云的关键点具体步骤为：

步骤2.3.1.1、求取点云的方向包围盒(OBB，Oriented bounding box)。方向包围盒由基准角C(x_c,y_c,z_c)和三个矢量CC₁(x_max,y_max,z_max)，CC₂(x_mid,y_mid,z_mid)，CC₃(x_min,y_min,z_min)表示。三个矢量CC₁(x_max,y_max,z_max)，CC₂(x_mid,y_mid,z_mid)，CC₃(x_min,y_min,z_min)分别对应于方向包围盒的最大、中间和最小维度。

步骤2.3.1.2、对方向包围盒进行裁剪，去除稀疏边缘点。取方向包围盒的高的一半为阈值，利用直通滤波器进行滤波，去除小于阈值的点，保留余下的点记作候选关键点。

步骤2.3.1.3、对关键点云进行均匀(US，Uniform Sampling)采样，提高匹配速度。

步骤2.3.2、对关键点进行特征描述。如图1所示，也就是计算关键点特征描述子。具体的有：

步骤2.3.2.1、计算关键点的SHOT特征描述子。根据特征点球邻域信息建立局部参考坐标系。对特征点的球邻域分别沿径向、方位角方向和高度方向进行区域划分，分别统计每个网格内的法向量夹角余弦值分布，每个网格内统计一个包含11个单元格的直方图。直方图内各个单元格的值由特征点处法线与邻域点法线夹角余弦统计，如式(5)所示，最终得到长度为32x11＝352的SHOT描述子。

cosθ_q＝z_kn_q (5)

式(5)中，z_k为r半径球邻域内局部坐标系的Z轴，也是特征点的法线；n_q是当前网格标签内所包含点的法线。

在本实施例中，区域划分的方法是：径向划分为2，方位角方向划分为8，高度方向划分为2，总共32个网格，如图3所示。

步骤2.3.2.2、为保证特征的唯一描述，引入关键点的空间位置描述子，该空间位置描述子在计算对应匹配点对时考虑特征点和重心点的关系，以降低误匹配率；

点云重心表示为

特征点和点云重心的对应关系为：

其中，l表示点云中特征点的数目，表示当前点的法线；

步骤2.4、对于模板点云集中的所有模板点云，重复步骤2.1～步骤2.3，得到每个模板点云的特征集m表示模板点云的个数。第i个模板点云的特征描述子表示为其中C,CC₁,CC₂,CC₃为点云方向包围盒特征，属于全局特征，f0为SHOT特征，f1,f2为空间位置特征。

步骤3、如图2所示，在线状态下，对具有碰撞、遮挡、包含多个工件的场景点云进行分割，分割成多个只包含单个工件的场景点云子集(即候选点云集)O＝{O_j,j＝0,1,...,g}。其中，g为场景点云的个数。采用步骤2.3.1的方法对场景点云子集进行关键点提取。

步骤4、在线状态下，将候选点云集和模板点云进行初始配准。对候选点云集中的每个候选点云O_j，根据方向包围盒特征的相似性度量确定其在模板库中对应的模板点云，计算候选点云关键点的特征描述子与对应模板点云中关键点特征描述子的差异，找出特征描述子最相近的点作为对应点。

具体的，所述步骤4包括：

步骤4.1、计算各点云子集的特征描述子第j个点云子集的特征描述子表示为其中C_o,CC_o1,CC_o2,CC_o3为方向包围盒特征，f0_o为SHOT特征，f1_o,f2_o为空间位置特征；

步骤4.2、对工件点云子集中的任意特征点，在模板点云中搜索其对应点；具体步骤为：首先，计算工件点云的方向包围盒特征，在模板点云库中搜索对应模板点云，两点云之间的方向包围盒特征应满足如下公式：

然后，对比工件点云和与其对应的模板点云的特征描述子差异，查找对应点，设工件点云中任意点q_i在模板点云中的对应点为p_i，需满足如下公式，得到特征点在模板点云中的对应点，若该组对应点的空间位置描述子的差异也在预设阈值范围内，则保留当前对应点，反之则舍弃；

式(9)中，ε₀,ε₁,ε₂为阈值。

步骤5、通过对应点之间的几何关系判断对应关系是否正确，从而去除误匹配点对，根据两幅点云之间的对应点对求解模板点云到场景点云的变换矩阵R和T，即初始变换矩阵；具体的，对对应点对采用奇异值分解算法求解模板点云到目标点云之间的旋转R和平移矩阵T，即初始配准结果；

其中，为对应点对，对初试配准结果进行性能评估，公式如下：

步骤6、利用改进ICP算法进行点云精确配准实现目标识别和定位，通过对应点在两幅点云中的几何约束得到正确的对应点，从而减少迭代次数，提高配准效果，评估配准效果，得到两者之间的最佳变换矩阵，从而得到工件点云的位姿信息。

具体的，步骤6包括：

步骤6.1、在工件点云O中随机选取一定数量的点组成工件点云随机点集q^k，

步骤6.2、在对应模板点云M中寻找与点距离最近的点组成目标点云对应点集，随机选取3组对应点对，对应点在工件点云和模板点云中的分布应近似相同，该分布可以用对应点之间的距离表示，若满足既定条件则为正确对应点，采用的四元数法计算出旋转矩阵R^k和平移矩阵T^k；

步骤6.3、计算配准误差，公式如下：

如果误差函数保持不变或者达到设定要求，则停止迭代，当前旋转矩阵旋转矩阵R^k和平移矩阵T^k就是最终的配准参数矩阵；如果误差函数没有达到设定要求，则O^k+1＝R^kQ^k+T^k，重复上述步骤；式(12)中k表示迭代次数，n表示点云O的随机点集中元素的个数。

以上所述的仅是本发明的优选实施方式，本发明不限于以上实施例。可以理解，本领域技术人员在不脱离本发明的精神和构思的前提下直接导出或联想到的其他改进和变化，均应认为包含在本发明的保护范围之内。

Claims (4)

1.一种基于点云处理的散乱工件识别与定位方法，其特征在于，具体过程如下：

步骤1、对模板点云和场景点云进行预处理，得到去除背景点和离群点后的模板点云和场景点云；

步骤2、在离线状态下，建立模板点云特征库，对于每个模板点云，提取其关键点并对关键点进行特征描述；

步骤3、在在线状态下，对场景点云进行分割，将具有碰撞、遮挡、包含多个工件的场景点云分割成多个只包含单个工件的场景点云子集，对场景点云子集进行筛选得到候选可抓取点云子集；

步骤4、在线状态下，对候选点云和模板点云进行初始配准，对于每个候选点云子集，根据全局特征相似性度量确定工件点云在模板库中对应的模板点云，计算候选工件点云关键点的特征描述子与对应模板点云中对应点的特征描述子的差异，找出特征描述子最相近的点作为对应点；

步骤5、通过对应点之间的几何关系判断对应关系是否正确，从而去除误匹配点对，根据两幅点云之间的对应点对求解模板点云到场景点云的变换矩阵R和T，即初始变换矩阵；

步骤6、使用改进ICP算法进行点云精确配准实现目标识别和定位，通过对应点在两幅点云中的几何约束得到正确的对应点，进而得到两者之间的最佳变换矩阵，从而得到工件点云的位姿信息。

2.如权利要求1所述的一种基于点云处理的散乱工件识别与定位方法，其特征在于，所述步骤2具体包括：

步骤2.1、计算模板点云密度；

设模板点云M有n个点，对任意一点p_i∈M，其r邻域半径内邻近点集合记作{p_i1,p_i2,p_i3,...,p_ik}，计算每个邻近点距p_i点的平均距离如下式所示：

式(1)中，d_ij表示第j个邻近点到点p_i的距离，遍历点云M中所有的点，得到n个平均距离值，根据下式得到点云M在k邻域下的平均距离均值，即点云密度；

步骤2.2、计算模板点云中各点的法线；采用主成分分析法求取点云中所有点的法线，具体步骤是：对任意一点p_i∈M，构件其k个邻近点{p_i1,p_i2,p_i3,...,p_ik}对应的协方差矩阵M：

式(3)中，表示最邻近元素的三维质心，λ_j和分别代表协方差矩阵的特征值和特征向量，λ₀<λ₁<λ₂；具有最小特征值的特征向量即p_i点的法线，记作

步骤2.3、计算模板点云特征描述子；具体过程为：

步骤2.3.1、关键点提取；求模板点云的关键点具体步骤为：首先，求取点云的方向包围盒OBB，方向包围盒OBB由基准角C(x_c,y_c,z_c)和三个矢量CC₁(x_max,y_max,z_max)，CC₂(x_mid,y_mid,z_mid)，CC₃(x_min,y_min,z_min)表示；三个矢量CC₁(x_max,y_max,z_max)，CC₂(x_mid,y_mid,z_mid)，CC₃(x_min,y_min,z_min)分别对应于方向包围盒OBB的最大、中间和最小维度；然后，取方向包围盒OBB的高的一半为阈值，利用直通滤波器进行滤波，去除小于阈值的点，保留余下的点记作候选关键点；最后对关键点云进行均匀采样，提高匹配速度；

步骤2.3.2、对关键点进行特征描述；具体的有：

步骤2.3.2.1、计算关键点的SHOT特征描述子；首先，根据特征点球邻域信息建立局部参考坐标系，对特征点的球邻域分别沿径向、方位角方向和高度方向进行区域划分；分别统计每个网格内的法向量夹角余弦值分布；最终得到SHOT特征描述子；

cosθ_q＝z_kn_q (5)

式(5)中，z_k为r半径球邻域内局部坐标系的Z轴，即特征点的法线；n_q是当前网格标签内所包含点的法线；

步骤2.3.2.2、引入关键点的空间位置描述子，该空间位置描述子在计算对应匹配点对时考虑特征点和重心点的关系；点云重心表示为特征点和点云重心的对应关系为：

其中，l表示点云中特征点的数目，表示当前点的法线；

步骤2.4、对于模板点云集中的所有模板点云，重复步骤2.1～步骤2.3，得到每个模板点云的特征集第i个模板点云的特征描述子表示为其中C,CC₁,CC₂,CC₃为点云方向包围盒特征，f0为SHOT特征，f1,f2为空间位置特征。

3.如权利要求1所述的一种基于点云处理的散乱工件识别与定位方法，其特征在于，所述步骤4和步骤5具体包括如下内容：

在步骤4中有：

步骤4.1、计算各点云子集的特征描述子第j个点云子集的特征描述子表示为其中C_o,CC_o1,CC_o2,CC_o3为方向包围盒特征，f0_o为SHOT特征，f1_o,f2_o为空间位置特征；

步骤4.2、对工件点云子集中的任意特征点，在模板点云中搜索其对应点；具体步骤为：首先，计算工件点云的方向包围盒特征，在模板点云库中搜索对应模板点云，两点云之间的方向包围盒特征应满足如下公式：

然后，对比工件点云和与其对应的模板点云的特征描述子差异，查找对应点，设工件点云中任意点q_i在模板点云中的对应点为p_i，需满足如下公式，得到特征点在模板点云中的对应点，若该组对应点的空间位置描述子的差异在预设阈值范围内，则保留当前对应点，若该组对应点的空间位置描述子的差异在预设阈值范围之外则舍弃；

式(9)中，ε₀,ε₁,ε₂为阈值；

在步骤5中，对步骤4所述的对应点对采用奇异值分解算法求解模板点云到目标点云之间的旋转R和平移矩阵T，即初始配准结果。

4.如权利要求1所述的一种基于点云处理的散乱工件识别与定位方法，其特征在于，所述步骤6包括：

步骤6.1、在工件点云O中随机选取一定数量的点组成工件点云随机点集q^k，

步骤6.2、在对应模板点云M中寻找与点距离最近的点组成目标点云对应点集，随机选取3组对应点对，对应点在工件点云和模板点云中的分布应近似相同，该分布可以用对应点之间的距离表示，若满足既定条件则为正确对应点，采用的四元数法计算出旋转矩阵R^k和平移矩阵T^k；

步骤6.3、计算配准误差，公式如下：

如果误差函数保持不变或者达到设定要求，则停止迭代，当前旋转矩阵旋转矩阵R^k和平移矩阵T^k就是最终的配准参数矩阵；如果没有达到设定要求，则O^k+1＝R^kQ^k+T^k，重复上述步骤；式中k表示迭代次数，n表示点云O的随机点集中元素的个数。