CN109345620A - 融合快速点特征直方图的改进icp待测物体点云拼接方法 - Google Patents

Abstract

本发明公开了一种融合快速点特征直方图的改进ICP待测物体点云拼接方法。投射标准正弦数字光栅到待测物体表面，通过CCD相机不同视角拍摄物体表面被投射标准正弦数字光栅后的条纹图像，获得多视角的拍摄点云；针对需要拼接的两个拍摄点云，由每个拍摄点云构建k‑d树并插值获得插值后点云；针对需拼接两个插值后点云，计算快速点特征直方图，进行随机采样一致变换获得点云结果；采用改进的迭代最近点方式处理获得第一个插值后点云经精配准的点云；叠加点云并网格划分，实现两个不同视角的拍摄点云的拼接。本发明对于拼接点云初始位置要求低，显著提高鲁棒性不易陷入局部最优，提高拼接精度，实现多视角下点云的精确拼接，可满足实际工业应用需求。

Description

融合快速点特征直方图的改进ICP待测物体点云拼接方法

技术领域

本发明涉及一种点云处理方法及***，尤其涉及了一种融合快速点特征直方图的改进迭代最近点(ICP)点云拼接方法。

背景技术

多视角点云拼接是重建物体表面数据的重要环节，该技术一直是逆向工程、计算机视觉、曲面质量检测及摄影测量学等领域的研究热点和难点。点云拼接的核心是求坐标变换参数R(旋转矩阵)和T(平移矩阵)，使得两视角下测得的三维数据经坐标变换后的距离最小。

目前，点云拼接方法主要有以下两类。在人工辅助的基础上自动拼接，利用运动定位装置计算点云旋转平移矩阵，对拍摄计算的点云进行对应变换，该方法的不足在于配准精度易受运动装置影响；或者通过在物体上粘贴标志点进行匹配，该方法精度较高但是操作麻烦、会在物体表面形成孔洞，严重降低点云的拼接效率，破坏物体的完整性。出于配准效率和精度两方面考虑，无辅助的自动拼接近年来在工业和学术领域获得广泛关注。无辅助自动拼接主要有三类方法：基于特征求对应点法、基于统计学求刚体变换、迭代最近点法。基于特征求对应点法通常利用点特征直方图计算点云几何特征，如点云法向量、密度分布、邻域点间位置关系等，基于采样一致性方法采样匹配，该算法具有较好的稳定性，但是拼接精度较低，若采样点数增加会显著降低点云配准效率；基于统计学求刚体变换是将变换空间离散化，并建立某种配准误差评价函数，典型算法是正态分布变换(NDT)，该方法不利用对应点特征，运算速度快，但是精度较低；利用迭代最近点(ICP)算法实现自动拼接，该算法基于最小二乘法的优化思想，计算点云间的配准矩阵，该算法运行速度和精度主要取决于给定的初始变换估计，若两拼接点云没有一个较好的初始位置和姿态，ICP算法不仅速度会明显降低而且精度也会严重下降。

针对以上三种方法都无法实现点云有效快速的拼接，本发明综合特征求对应点及迭代最近点两类方法，采用快速点特征直方图(FPFH)并通过利用点云欧氏距离及方向矢量夹角阈值去除错误点对来改进迭代最近点方法，最终实现点云的自适应拼接。

发明内容

本发明为解决点云拼接过程拼接精度较低、稳定性不足以及拼接效率低下等问题，提出了一种融合快速点特征直方图的改进迭代最近点(ICP)点云拼接方法，同时在传统最近点迭代的基础上加以改进，使用k-d树的搜寻方式，结合点云的欧氏距离以及方向夹角阈值去除错误点对，实现点云的高效精确拼接。

如图2所示，本发明的技术方案包括以下步骤：

步骤1)，投射标准正弦数字光栅到待测物体表面，通过CCD相机从不同的视角拍摄物体表面被投射标准正弦数字光栅后的条纹图像，利用反正切函数对每个条纹图像进行求解得到相位值，将所求的相位值进行解包裹反求得到待测物体的表面点云，然后进行预处理获得多个拍摄点云，从而获得了多视角的拍摄点云；

步骤2)，拍摄点云迭代插值：针对需要拼接的两个拍摄点云，由每个拍摄点云构建k-d树，遍历构建三角网格并插值，多次迭代直至满足点云密度要求，获得插值后点云；

步骤3)，粗配准阶段：针对需要拼接的两个插值后点云，计算第一个插值后点云的快速点特征直方图，在第二个插值后点云中找到与快速点特征直方图相似的点，进行随机采样一致变换获得第一个插值后点云经粗配准的点云结果；

步骤4)，精配准阶段：第一个插值后点云经粗配准的点云和第二个插值后点云大致重合，采用改进的迭代最近点方式对第一个插值后点云经粗配准的点云进行处理获得第一个插值后点云经精配准的点云；

具体是搜索近邻点结合对应点间的欧氏距离以及方向矢量夹角对比预设阈值，判断点集对应关系去除错点对，再基于最小二乘法迭代求解点云最优坐标变换。

步骤5)，叠加阶段：采用均匀网格精简方式对第一个插值后点云经精配准的点云和第二个插值后点云叠加并进行网格划分，对网格内的所有点求取中心点替换覆盖网格内的所有点，实现两个不同视角的拍摄点云的拼接。

本发明方法对输入点云进行迭代插值，以快速点特征直方图作为特征，通过采样一致性进行粗配准；在结合欧氏距离以及方向法矢量夹角对比预设阈值去除错误点对的基础上，进行最近点迭代精配准，将两拼接点集配准到同一三维坐标系下，叠加精简完成拼接。

所述的步骤1)具体是获取平面模型采用采样一致的方法提取背景平面点云加以去除；对保留下的点云进行体素下采样滤波，降低点云密度；最后进行统计滤波，通过搜寻近邻点数和离群阈值去除离群噪点。

所述的步骤1)中，进行预处理获得拍摄点云，具体是对待测物体的表面点云依次进行基于随机采样一致性的背景点云分割、体素单元下采样滤波、统计滤波去除离群噪点，实现对拍摄得到待测物体表面点云进行无效背景点以及离群噪点去除并降低密度的操作：

1.2)针对待测物体的表面点云，以桌面背景作为基元，预设桌面背景的基元平面形状及平面误差阈值，采用随机采样方法拟合桌面背景的基元平面形状并进行多次处理获得不同的平面模型，计算平面模型中的点数量，选取点数量最多的平面模型作为最优平面，将最优平面中的点作为无效背景点从待测物体的表面点云中去除，这样处理能够提高后续点云处理效率和精度；

1.3)对去除无效背景点后的点云进行体素下采样处理，根据待测物体大小生成体素单元，以体素下采样中每个体素单元的所有点的重心代替覆盖体素单元内所有点，这样能保留表面形状特征的同时降低点云密度；

1.4)对体素下采样后的点云进行统计滤波，设置搜寻近邻点数和离群阈值，遍历点云中的所有点，采用以下方式针对每个点进行处理：建立以点为球心、以离群阈值为球半径的球体，球体内的点数量和搜寻近邻点数进行比较：若点数小于搜寻近邻点数，则将该点视为离群噪点，进行去除；若点数大于等于搜寻近邻点数，则将该点不视为离群噪点，进行保留；最终保留得到的为拍摄点云。

上述步骤对原始点云进行以上三步预处理，背景点去除，去除桌面点云引起的干扰；统计滤波去除离群点，提高后续配准精度以及缺陷检测的效果；体素下采样滤波，降低点云密度，提高点云处理效率。

所述的离群阈值为设备精度、环境因素引起的允许误差。

所述的步骤2)具体为：

2.1)以拍摄点云中的点作为k-d树的节点，根据拍摄点云构建k-d树；

2.2)遍历搜索k-d树中每一个点p_i，将点p_i与近邻于点p_i的两个点q_i建立三角网格，在三角网格中心插值建立插值点，加入到k-d树的节点中；

2.3)重复上述步骤2.2)不断迭代直至k-d树对应的拍摄点云满足点云密度要求，获得插值后点云。这样能够有效解决因为相机分辨率限制导致点云密度不足，可以显著提高点云拼接精度。

在点云库(Point Cloud Library)配置环境下，采用点云数据处理库接口，将点云数据转换为.pcd文件格式读入。由于受到相机分辨率的限制，实物目标上的点并不完全在点云上显示，在实物目标上的某一点可能出现在其中一幅点云或同时不出现在两次拍摄的点云上，通过这样寻找到的对应点存在扫描位置引起的误差，是引起配准误差的重要原因，但是随着点云密度的增加，对应点间错位距离明显减少，配准精度显著提高，满足拼接精度要求。

所述的步骤1)中，在点云库(Point Cloud Library)配置环境下，采用点云数据处理库程序接口，将点云数据转换为.pcd文件格式读入。由于受到相机分辨率的限制，实物目标上的点并不完全在点云上显示，在实物目标上的某一点可能出现在其中一幅点云或同时不出现在两次拍摄的点云上，这样寻找到的对应点存在扫描位置引起的误差，是引起配准误差的重要原因，但是随着点云密度的增加，对应点间错位距离明显减少，配准精度显著提高，满足拼接精度要求。

所述的步骤3)具体是：

3.1)从第一个插值后点云P中任意选择s个点作为样本点，同时使得s个样本点任意两个点之间的距离均大于预先设定的最小阈值d_min；

3.2)根据所有样本点的每个点p与其在第一个插值后点云P中近邻的其他点之间的关系，计算简化点特征直方图(SPFH)，根据距离权重计算获得样本点的快速点特征直方图(FPFH)；

3.3)将第二个插值后点云中的点和样本点进行随机匹配：

3.3.1)对于每个样本点，在第二个插值后点云找到满足与快速点特征直方图(FPFH)相似的其中一个点作为匹配点，由所有样本点与其各自对应的匹配点构成一组对应点组，计算一组对应点组中所有样本点与其对应的匹配点之间的刚体变换矩阵，再计算用刚体变换矩阵对插值后点云进行变换获得的点云结果和第二个插值后点云之间的距离差，进而采用以下公式计算获得度量误差：

式中，H(l_i)表示第i组对应点组的度量误差，||l_i||表示距离差，m_l为预设比较阈值，l_i为第i组对应点组获得的点云结果和标准点云之间的距离；

所述的距离差为从用刚体变换矩阵对插值后点云进行变换获得的点云结果中的点与其在第二个插值后点云中最近的点之间的距离。

3.3.2)重复上述步骤3.3.1)进行多次，即进行随机匹配多次，获得多组对应点组及其度量误差，再计算度量误差和函数其中n表示对应点组的总组数，i表示对应点组的组序数；取度量误差和函数最小的对应点组对应的刚体变换矩阵对插值后点云进行变换作为粗配准点云。

计算FPFH特征时，近邻点集不宜取得过小，否则无法准确估计出该点的特征值，导致粗配准精度过低；近邻点集也不宜取得过大，否则会导致计算量增大，同时会使得FPFH的计算失去意义(过大的近邻点集合不能反映局部特征)。

本发明的步骤3)能在获得快速点特征直方图之后，利用点云数据旋转不变的特征，在初始配准阶段引入采样一致性初始配准算法(SAC-IA)，保持相同的对应关系中进行大量的采样，SAC-IA得到变换矩阵进行粗匹配。上述这样方式处理能提高采样的均匀性，使算法更加稳定。

所述的步骤4)具体是：

4.1)以第一个插值后点云经粗配准的点云作为源点云，以第二个插值后点云作为目标点云，采用k-d树搜索方式处理：以源中的点作为样本点，针对每一个样本点，在目标点云中找到和该样本点欧式距离最近一个点作为匹配点，由一个样本点与其对应的匹配点构成一对对应点，计算一对对应点的欧氏距离和方向矢量夹角，一对对应点的欧氏距离和方向矢量夹角是指样本点与其对应的匹配点之间的欧式距离和点方向向量之间的矢量夹角；将欧氏距离和方向矢量夹角与预设的距离阈值和夹角阈值分别进行比较，取欧氏距离大于预设距离阈值或者方向矢量夹角大于预设夹角阈值的样本点作为正常点，并将正常点以移动步长向自身对应的匹配点移动；

这样能够去除因为视角遮挡、相机一起噪声等不存对应点的情况，降低无法迭代至全局最优的可能性，提高配准精度。

4.2)采用以下公式计算获得正常点到目标点云的均方差e，以避免无效迭代浪费硬件资源，提高迭代效率；

其中，q_i、p_i分别为对应点在源点云、目标点云中点的坐标，e为目标距离均方差，i表示正常点的序数，N表示正常点的总数，R表示旋转矩阵，T表示平移矩阵；

上述公式旋转矩阵R和平移矩阵T是采用以下公式的奇异值分解法(SVD)求解坐标变换得到，通过旋转矩阵R和平移矩阵T将源点云配准到目标点云的坐标系下：

Q＝RP+T

其中，R表示旋转矩阵，是一个3*3的旋转矩阵，T表示平移矩阵，是一个1*3的平移矩阵，P表示变换前的点云，Q表示变换后的点云。

在粗配准的基础上使用基于去除错误点对的最近点迭代进行精配准。首先采用点对点对应点集搜索方法，利用k-d树搜索方式，对比显著提高了对应点对的搜索效率：传统的ICP算法中，使用蛮力法查找对应点对，其算法复杂度为O(N²),N为配准点云点数，采用k-d树的方法，将点云按照坐标分类组成树状结构，算法的复杂度为O(Nlog₂ N),显著提高了搜索效率。

搜索近邻点设置阈值进行比较中，当两个对应点之间的距离大于设置阈值，则认为是错误的匹配点对其进行剔除。选取经过欧氏距离判定后得到的对应点集P′和Q′，通过邻域协方差分析法计算各对应点对法矢量，并求出两矢量的夹角α对比预设阈值θ，若大于预设阈值θ则去除该点对。形成错误点对的原因主要有：因为待测物体形状遮挡，部分点云之出现在其中一幅数据中；光学设备、环境因素等引入的噪声点。

其中阈值设置需要根据点云密度以及拼接精度要求进行选择，若阈值设置过大，则无法去除错误点对；若阈值设置过小，则引起正确点对误删除。引入错误点对的阈值判断，大大降低配准点云重合度的要求，提高配准精度。

所述的步骤5)具体为：将第一个插值后点云经精配准的点云和第二个插值后点叠加，叠加后进行体素下采样，设置体素单元作为网格，根据待测物体大小生成体素单元，若体素单元设置过大，会导致点云的过度稀疏，点云特征点丢失，降低后续的缺陷率；若体素单元设置过小，将无法滤去点云，无法降低点云密度，重叠区域点云密度过高，遮挡区域点云密度不足。以体素下采样中每个体素单元的所有点的重心代替覆盖体素单元内所有点，使点云密度均一化并且保留点云的形状特征，这样能保留表面形状特征的同时降低点云密度。

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

1)本发明利用快速点特征直方图作为匹配特征，基于采样一致性的粗配准，提高算法稳定性，降低对点云初始位置的要求，为迭代最近点配准提供较好的初始位置和姿态；

2)本发明在传统迭代最近点基础上，采用k-d树的近邻点搜寻方式，将对应点查找算法复杂度从O(N²)降为O(Nlog₂ N),提高点云拼接效率。

3)本发明在传统迭代最近点基础上，利用结合欧式距离以及方向向量的阈值判断去除错误点对，将由噪声点和拍摄遮挡引起的错误点对剔除，既避免迭代过程陷入局部最优，又提高配准精度，降低对待拼接点云重合度的要求。

综合来说，本发明对于拼接点云初始位置要求低，显著提高鲁棒性不易陷入局部最优，提高拼接精度，实现多视角下点云的精确拼接，可满足实际工业应用需求。

附图说明

图1为本发明实施例的结构光检测***结构图；

图2为本发明方法流程图；

图3为待拼接点云相对初始位置图；

图4是为传统最近点迭代拼接结果图；

图5是为本发明方法拼接结果图。

具体实施方式

下面结合附图和实施例对本发明作进一步说明。

如图2所示，按照本发明方法完整实施的实施例及其实施过程包含以下步骤：

具体实施采用结构光检测***，如图1所示，结构光检测***包括投影仪、计算机、CCD相机和平台，待测物体置于平台上，投影仪连接计算机，投影仪和相机分别置于待测物体上方的两侧，投影仪的镜头和CCD相机的镜头均朝向待测物体；待测物体放置在桌面上，计算机中发出输入光栅模式的信号，输入到投影仪中产生条纹光栅作为光栅光源照射到待测物体和桌面上，投影方向和待测物体不垂直，通过CCD相机采集条纹光栅照射到待测物体和桌面后的图像，示意图如图1所示。投影仪将条纹光栅投射在待测物体上，待测物体使条纹光栅发生畸变，相机拍摄畸变条纹，由计算机解相位后重构物体表面点云。

第一步输入点云插值迭代。

在点云库(Point Cloud Library)配置环境下，采用点云数据处理库程序接口，将点云数据转换为.pcd文件格式读入，对输入点云进行合适预处理操作，如背景点分割，离群点滤除等。分析输入点集数据结构，构建k-d树，建立M×K查询表矩阵，存储由k-d树寻找到的每一个点的K近邻点索引值(M是点云中点数)；遍历搜索点p_i，与p_i近邻点q_i之间建立插值点，构建三角网格计算网格中心插值，并进行迭代直至点云密度满足要求。

第二步粗配准阶段。

拍摄不同视角下的待测物体表面点云，两两拼接。

选择任一为源点云，另一为目标点云。对源点云每个查询点p计算其快速点特征直方图(SPFH)，特征有点的三维坐标、法向量以及周围点关系估计得到；根据距离权重计算快速点特征直方图。利用点云数据旋转不变的特征，在初始配准阶段引入采样一致性初始配准算法(SAC-IA)，保持相同的对应关系中进行大量的采样，并通过以下步骤对他们进行排名：1)从待配准点云P中选择s个样本点，同时确定他们的配对距离大于设定的最小值0.005m；2)对于每个样本点，在目标点云Q找到满足直方图个样本点直方图相似的点存入一个列表中，从这些点中随机选择一些代表采样点的对应关系；3)计算通过采样点定义的刚体变换和其对应变换，计算点云的度量错误来评价转换质量。此处度量误差和函数表示计算为，记为其中：

式中：m_l为预设对比值，l_i为第i组对应点组获得的点云结果和标准点云之间的距离。

第三步精配准阶段。经过快速点特征直方图粗配准之后，待配准的两片点云已经大致重合，但是仍然存在偏差，配准的精度也比较低，为了提高配准精度，本发明采用基于传统最近点迭代方法，在迭代过程中搜索近邻点设置阈值进行比较去除错误点对。

首先，引入k-d树的近邻点搜索方式，将搜索算法的复杂度由O(N²)降到O(Nlog₂N),显著提高ICP算法速度，缩短查找时间；设置错误点对阈值，基于实验相机拍摄分辨率和实际拍摄效果，错误点对欧式阈值设为0.003m、法矢量夹角阈值0.05，能够有效保留正确对应点，同时去除错误点对。引入迭代评价标准，用于判断精配准精度，同时作为迭代中止条件。以均方差作为评价配准的指标，对应点p_i和q_i间距与预设阈值比较，若大于阈值，则不计入配准评价；若小于阈值，对应点对计数加一，叠加误差，遍历配准点集，求均值。

设置最近点迭代中止条件：最大迭代次数100、两次变换矩阵之间差值1e-10以及均方误差0.2，达到任意一个标准迭代停止。

使用奇异值分解法(SVD)求解坐标变换，将两拼接点集配准到同一三维坐标系下。

第四步点云数据叠加精简阶段。在精确配准之后，对两点集叠加，再次采用体素单元下采样，设置体素单元边长为0.001m，将体素单元内所有点的重心代表栅格内所有点，保证点云的均匀性：不会因为重合处点云数据的叠加导致该处点云密度的成倍上升，同时对于特征点保留程度较高，能够保留大部分的特征。

实验验证：

以藏族佛塔点云作为实例加以验证，对点集进行任意角度旋转平移变换，采用融合快速点特征直方图的改进迭代最近点自适应点云拼接方法，效果如图2所示，配准结果均方差为4×e^-7，配准时间为4.359s，配准精度和效率高于传统迭代最近点算法。

对完整佛塔点云(38504)进行任意切割，得到两有重叠单不相同点集，点云个数分别为23263、33706，两幅不完整且初始位置相互偏离的藏族佛塔点云如图3所示，分别采用传统迭代最近点算法拼接以及融合快速点特征直方图的改进迭代最近点拼接方法，点云效果对比如图4、5所示，拼接结果对比如表1所示。

表1

上表中可见，采用本发明拼接方法，从拼接的效率和精度两个角度都明显优于传统ICP的拼接方式，同时本发明对不同视角下的待拼接点云重合度要求更低，算法更加稳定。

虽然本发明已以较佳实施例揭露如上，然其并非用以限定本发明。本发明所属技术领域中具有通常知识者，在不脱离本发明的精神和范围内，当可作各种的更动与润饰。因此，本发明的保护范围当视权利要求书所界定者为准。

Claims (6)

1.一种融合快速点特征直方图的改进ICP待测物体点云拼接方法，其特征在于：

步骤1)，投射标准正弦数字光栅到待测物体表面，通过CCD相机从不同的视角拍摄物体表面被投射标准正弦数字光栅后的条纹图像，利用反正切函数对每个条纹图像进行求解得到相位值，将所求的相位值进行解包裹反求得到待测物体的表面点云，然后进行预处理获得多个拍摄点云，从而获得了多视角的拍摄点云；

步骤2)，拍摄点云迭代插值：针对需要拼接的两个拍摄点云，由每个拍摄点云构建k-d树，遍历构建三角网格并插值，多次迭代直至满足点云密度要求，获得插值后点云；

步骤3)，粗配准阶段：针对需要拼接的两个插值后点云，计算第一个插值后点云的快速点特征直方图，在第二个插值后点云中找到与快速点特征直方图相似的点，进行随机采样一致变换获得第一个插值后点云经粗配准的点云结果；

步骤4)，精配准阶段：采用改进的迭代最近点方式对第一个插值后点云经粗配准的点云进行处理获得第一个插值后点云经精配准的点云；

步骤5)，叠加阶段：对第一个插值后点云经精配准的点云和第二个插值后点云叠加并进行网格划分，对网格内的所有点求取中心点替换覆盖网格内的所有点，实现两个不同视角的拍摄点云的拼接。

2.根据权利要求1所述的一种融合快速点特征直方图的改进ICP待测物体点云拼接方法，其特征在于：所述的步骤1)中，进行预处理获得拍摄点云，具体是：

1.2)针对待测物体的表面点云，以桌面背景作为基元，预设桌面背景的基元平面形状，采用随机采样方法拟合桌面背景的基元平面形状并进行多次处理获得不同的平面模型，计算平面模型中的点数量，选取点数量最多的平面模型作为最优平面，将最优平面中的点作为无效背景点从待测物体的表面点云中去除；

1.3)对去除无效背景点后的点云进行体素下采样处理，以体素下采样中每个体素单元的所有点的重心代替覆盖体素单元内所有点；

1.4)对体素下采样后的点云进行统计滤波，设置搜寻近邻点数和离群阈值，遍历点云中的所有点，采用以下方式针对每个点进行处理：建立以点为球心、以离群阈值为球半径的球体，球体内的点数量和搜寻近邻点数进行比较：若点数小于搜寻近邻点数，则将该点视为离群噪点，进行去除；若点数大于等于搜寻近邻点数，则将该点不视为离群噪点，进行保留；最终保留得到的为拍摄点云。

3.根据权利要求1所述的一种融合快速点特征直方图的改进ICP待测物体点云拼接方法，其特征在于：所述的步骤2)具体为：

2.1)以拍摄点云中的点作为k-d树的节点，根据拍摄点云构建k-d树；

2.2)遍历搜索k-d树中每一个点p_i，将点p_i与近邻于点p_i的两个点q_i建立三角网格，在三角网格中心插值建立插值点，加入到k-d树的节点中；

2.3)重复上述步骤2.2)不断迭代直至k-d树对应的拍摄点云满足点云密度要求，获得插值后点云。

4.根据权利要求1所述的一种融合快速点特征直方图的改进ICP待测物体点云拼接方法，其特征在于：所述的步骤3)具体是：

3.1)从第一个插值后点云P中任意选择s个点作为样本点，同时使得s个样本点任意两个点之间的距离均大于预先设定的最小阈值d_min；

3.2)根据所有样本点的每个点p与其在第一个插值后点云P中近邻的其他点之间的关系，计算获得快速点特征直方图(FPFH)；

3.3)将第二个插值后点云中的点和样本点进行随机匹配：

3.3.1)对于每个样本点，在第二个插值后点云找到满足与快速点特征直方图(FPFH)相似的其中一个点作为匹配点，由所有样本点与其各自对应的匹配点构成一组对应点组，计算一组对应点组中所有样本点与其对应的匹配点之间的刚体变换矩阵，再计算用刚体变换矩阵对插值后点云进行变换获得的点云结果和第二个插值后点云之间的距离差，进而采用以下公式计算获得度量误差：

式中，H(l_i)表示第i组对应点组的度量误差，||l_i||表示距离差，m_l为预设比较阈值，l_i为第i组对应点组获得的点云结果和标准点云之间的距离；

3.3.2)重复上述步骤3.3.1)进行多次，获得多组对应点组及其度量误差，再计算度量误差和函数其中n表示对应点组的总组数，i表示对应点组的组序数；取度量误差和函数最小的对应点组对应的刚体变换矩阵对插值后点云进行变换作为第一个插值后点云经粗配准的点云。

5.根据权利要求1所述的一种融合快速点特征直方图的改进ICP待测物体点云拼接方法，其特征在于：

所述的步骤4)具体是：

4.1)以第一个插值后点云经粗配准的点云作为源点云，以第二个插值后点云作为目标点云，采用k-d树搜索方式处理：以源中的点作为样本点，针对每一个样本点，在目标点云中找到和该样本点欧式距离最近一个点作为匹配点，由一个样本点与其对应的匹配点构成一对对应点，计算一对对应点的欧氏距离和方向矢量夹角，一对对应点的欧氏距离和方向矢量夹角是指样本点与其对应的匹配点之间的欧式距离和点方向向量之间的矢量夹角；将欧氏距离和方向矢量夹角与预设的距离阈值和夹角阈值分别进行比较，取欧氏距离大于预设距离阈值或者方向矢量夹角大于预设夹角阈值的样本点作为正常点，并将正常点以移动步长向自身对应的匹配点移动；

4.2)采用以下公式计算获得正常点到目标点云的均方差e；

其中，q_i、p_i分别为对应点在源点云、目标点云中点的坐标，e为目标距离均方差，i表示正常点的序数，N表示正常点的总数，R表示旋转矩阵，T表示平移矩阵；

上述公式旋转矩阵R和平移矩阵T是采用以下公式的奇异值分解法(SVD)求解坐标变换得到：

Q＝RP+T

其中，R表示旋转矩阵，T表示平移矩阵，P表示变换前的点云，Q表示变换后的点云。

6.根据权利要求1所述的一种融合快速点特征直方图的改进ICP待测物体点云拼接方法，其特征在于：所述的步骤5)具体为：将第一个插值后点云经精配准的点云和第二个插值后点叠加，叠加后进行体素下采样，设置体素单元作为网格，以体素下采样中每个体素单元的所有点的重心代替覆盖体素单元内所有点。