CN113421291A - 利用点云配准技术和三维重建技术的工件位置找正方法 - Google Patents

Abstract

一种利用点云配准技术和三维重建技术的工件位置找正方法，首先利用点云技术建立点云模型，其次根据高精度机械臂的运动关系及手眼标定的结果进行多角度点云模型粗拼接，再应用ICP算法对其进行精拼接，通过点云融合得到焊件参考模型，之后利用点云配准技术，将实际场景中的工件模型和建立的参考模型对齐，得到两者的位姿转换关系，使系统能够实时感知工件的位姿，从而实现工件的定位找正。解决了在工件模型未知情况下，工件的定位找正，进而获得准确的机器人运动路径。

Description

利用点云配准技术和三维重建技术的工件位置找正方法

【技术领域】

本发明属于光学测量技术领域，具体涉及一种利用点云配准技术和三 维重建技术的工件位置找正方法。

【背景技术】

近年来，随着工业自动化的需求，工业机器人在辅助加工得到了广泛 的使用。目前，在自动化生产中，应用最多的是“示教再现”和“离线编 程”两种工作方式。这两种模式虽然效果不错，但在实际的生产环境中， 由于目标多变，且工作环境复杂，而这些工作模式无法感知工况，对环境 的变化缺乏一定的灵活性导致效率较低。因此机器人辅助加工的智能化是 未来发展和应用的必然趋势。

机器人辅助加工的智能化，主要是将各类传感器与机器人结合起来， 实时获取实时工况的相关信息，通过对这些信息的实时处理，进而调整加 工过程中相关参数，完成自动加工作业。视觉传感器具有非接触测量，精 度高、速率高，抗弧光等优点，在机器人辅助加工的智能化过程中得到了 大量应用。

视觉传感技术在机器人辅助加工中主要分为被动视觉法和主动视觉传 感技术。主动视觉通常以结构光或者激光作为测量光源，将其投射到测量 物体表面，CCD相机拍摄物体表面图像并进行分析，从而获取工件几何结构 特征。因此，对于测量获取的点云数据是否能够进行合理准确的处理，得 到完整精确的CAD模型，对于机器人运动路径的准确性至关重要。

【发明内容】

针对上述问题，本发明提供一种利用点云配准技术和三维重建技术的 工件位置找正方法，通过对视觉传感器获取的工件点云数据进行处理，建 立工件参考模型，最终实现工件位置找正，解决了在工件模型未知的情况 下，准确装夹工件，进而获得准确的机器人运动路径，确保后期工作得顺 利进行。

本发明是通过以下技术方案实现的，提供一种利用点云配准技术和三 维重建技术的工件位置找正方法，包括以下步骤：

S1对获得的点云数据进行预处理，获得点云模型；

S2利用三维重建技术建立参考模型；

S3对参考模型和点云模型进行点云配准，得到参考模型和点云模型的 位姿变换矩阵；

S4利用位姿变换矩阵将点云模型和参考模型对齐，即完成工件位置找 正。

特别的，所述S1采用视觉传感器获取点云数据，该视觉传感器为二维 传感器。

特别的，所述S1中预处理包括点云去噪和点云简化，所述点云去噪具 体按照以下方案实施：

S111利用点云加权模糊C均值聚类算法计算数据点g_i的密度ρ_i，密度ρ_i具体按如下公式进行计算：

公式(1)中，g_i为数据点，d(g_i,g_j)为两个数据点间的欧氏距离，g_j为 数据点g_i的k个邻域点，当k个数据点距离较近时，d(g_i,g_j)较小，ρ_i较大， 这对聚类影响较大，因此按如下公式对ρ_i进行归一化：

于公式(2)中，r为数据点到聚类中心的距离，Ω_i为数据点g_i对分类 的影响，噪声点离模型越远，Ω_i越小，对分类的影响就越小，最终的聚类 效果越好，因此点云加权模糊C均值聚类算法的目标函数按如下公式表示：

于公式(3)中，u_ij为数据点对应的隶属度，其介于[0，1]之间，b_i为聚 类组i的聚类中心，d_ij＝|b_i-x_j|为第i个聚类中心与第j个数据点间的欧式距 离，x_j为数据点坐标，s＝2，C为聚类中心个数，

S112判断数据点g_i到k近邻点的平均距离是否大于阈值，若是，则表 示该数据点g_i为噪声，删除，结束，若否，则执行S213，

S113对k近邻点进行点云权重聚类，用聚类中心作为新的数据点，即 去除点云数据中的大尺度噪声，结束，通过迭代计算使公式(3)取最小值， 按如下公式计算出聚类中心b_i、对应的隶属度u_ij：

S114按如下公式计算出双边滤波因子μ：

于公式(6)中，k_ij为数据点g_i的临近点，M(g_i)为数据点g_i的邻域点 的隶属度，σ_c衡量了点g_i到其邻域点的欧氏距离对该点的影响，σ_s表示点g_i到邻域点的距离向量在法向上的投影对该点的影响，

为滤波函数，

为特征权重函数，x＝||g_i-k_ij||为数据点g_i到邻域点的距离，＜n_i,g_i-k_ij＞ 为特征权重，所述滤波函数、特征权重函数分别按如下公式表示：

S115按如下公式对滤波数据进行更新：

g_i＝g_i+μn (9)；

S116遍历模型中所有数据点，即去除点云数据中的小尺度噪声，结束；

所述点云简化具体按照以下方案实施：

S121通过原始点云模型在X，Y，Z方向上的x_min，x_max，y_min，y_max， z_min，z_max，来确定Cell的大小，

S122根据实际需要确定栅格大小l_x，l_y，l_z，从而确定模型在X，Y， Z方向被分成m，n，l份，即：

S123确定每个数据点g_i在栅格中的索引号a_i，b_i，c_i，计算方法如下：

按如下公式将数据点g_i在栅格中的索引号转化为一维形式：

S124按如下公式计算体素栅格内所有点的重心，并用该重心代表栅格 内的所有点，

S125遍历所有体素，得到简化后的点云数据。

特别的，所述S2具体按照以下方案实施：

S21将工件固定放置于机械臂前方，控制机械臂运动n次得到点云模型 集合{p₁,p₂,…,p_n}，将各个直接获得的点云模型转换到机械臂初始拍摄位置 的相机坐标系下，分析系统的坐标转换关系如下公式：

于公式(14)中，

均为手眼关系矩阵，分别表示工具末端相对 相机的位姿关系和相机相对工具末端的位姿关系，

表示机械臂每次移动 时的位姿，从机械臂系统获得，

表示机械臂的初始拍摄位置；

S22通过点云融合将模型进行粗拼接，得到粗拼接模型；

S23在点云P中任意选一点P_i，在点云Q中找到距其最近的点Q_j，对于 存在的一些误匹配点对，引入以下约束进行去除，若(p₁,q₁)，(p₂,q₂)为两对 正确匹配的对应点，设e＝||q₁-p₁||₂，则这两对对应点应该满足以下约束：

(1)方向性约束：(p₂-p₁)(q₂-q₁)≥0，

(2)刚性约束：||q₂-q₁||₂≤2(e+||p₂-q₁||₂)，

(3)匹配误差约束：||q₂-q₁||₂≤e+||p₂-q₁||₂；

S24粗拼接之后的两片点云对应的空间点集用{P_i|i＝1,2,…,r}和{Q_i|i＝1,2,…,s}表示，P和Q不用一一对应，r、s无需相等，假设r＞s，且两 片点云有n个正确的对应点，求解这两组点集对应点之间的旋转和平移关系 [R,T]，从而使如下公式的目标函数最小，

S25通过全局优化和点云融合，即得到工件参考模型。

特别的，所述S2在得到工件参考模型后，按如下公式对其拼接精度进 行检验，若满足需要，则结束，若不满足需要，重新进行工件参考模型的 拼接，

于公式(15)中，N为模型点云中的点数量，

为残差e_i的平均值，

特别的，所述S3首先用SHOT描述子对工件参考模型实现粗配准，其 次用ICP对工件参考模型实现精确配准；

所述现粗配准具体按照以下方案实施：

SHOT特征描述子在构建的局部参考系内，统计特征点周围的拓扑特征， 将特征保存在直方图中，并对其进行归一化，特征点p邻域内的点记为p_i， 且满足||p_i-p||≤R，其局部描述符M按如下公式计算：

于公式(15)中，d_i＝||p_i-p||₂，采用特征分解法算法对M进行特征值 分解，得到M的三个特征值及对应的特征向量，分别作为x⁺，y⁺，z⁺，相 反方向记为x^-，y^-，z^-，最终的x轴方向表示为：

同理得到z轴的方向，y轴方向通过z×x确定，即构建完成局部参考系， 其具有旋转和平移不变性；

所述精确配准具体按照以下方案实施：

S321根据不同点云对应点间的距离，建立场景点云和参考点云的对应 点集，

S322求满足对应点集平均距离最小的刚性变换参数，

S323应用得到的刚性变换参数对场景点云进行坐标变换，得到变换后 的场景点云，

S324求参考点云和变换后的点云之间的平均距离，若小于设置的距离 阈值，则迭代结束，否则以变换后的点云作为新的场景点云继续进行迭代 计算，直到参考点云和变换点云之间的平均距离小于设置的距离阈值，即 目标函数满足收敛精度要求。

特别的，所述直方图按如下方法构建：

将局部参考系按照2个径向分区、2个俯仰分区、8个方位角进行分区， 获得32个子区间，分区完成后，记点p的法向量为n，球内点p_i的法向量 记为n_i，设两个法向量的夹角为θ_i，则获得如下公式：

cosθ_i＝n·n_i (21)，

将cosθ_i划分为m份，则可以建立32(m+1)份局部直方图，其中1表示球 内邻域点在球内的位置，通过对邻域点在局部坐标系中的位置和方向角的 计算，即完成直方图的构建。

特别的，所述S4按照以下方案实施：

S41根据两组点云之间的匹配关系，对两组点云之间的位姿转换关系进 行初步估计，首先将参考点云的中心移动至场景点云中心处，可粗略计算 出两组点云之间的平移向量T，假设参考点云关键点移动后为p'，x为场景 点云的关键点坐标，则有：

于公式(22)中，R为单位矩阵，其平方为1，因此上述公式可按如下 公式化为求解H的最大值：

∑2x^TRp'＝tr(R^T∑xp'^T)＝tr(R^TH) (23)，

于公式(23)中，H＝∑xp'^T，对H进行奇异值分解，得到如下公式：

H＝USV^T (24)，

于公式(24)中，当R＝VU^T时，R^TH可对角化，此时迹最大，即通过 分解H，得到旋转矩阵R，并计算出平移矩阵T；

S42利用位姿变换矩阵，将参考模型和场景点云模型对齐，实现工件 的位置找正。

本发明提供一种利用点云配准技术和三维重建技术的工件位置找正方 法，具有以下有益效果：

1、无需知道工件模型，只需要参考点云即可，较为方便；

2、公国点云降噪算法和滤波算法进行点云数据处理，在去除点云数据 噪声的同时保留了图像的细节，使模型更加接近工件；

3、多角度模型重建技术可以对多角度机器人的点云测量数据实现从 粗拼接到ICP精拼接，并通过三维重建得到工件的完整点云模型；

4、点云配准技术可以快速得到参考模型和场景点云模型之间的位姿 转换关系，对场景点云进行位姿估计，从而将参考模型和场景点云模型对 齐，实现工件的位置找正，方便快捷。

【附图说明】

图1为采用本发明经过点云处理后的示意图，其中左侧图为点云去噪 图，右侧为点云简化图；

图2为本发明点云表面误差分布图；

图3采用本发明粗拼接后获得的结果示意图；

图4采用本发明精拼接后获得的结果示意图；

图5采用本发明点云配准后获得的结果示意图。

【具体实施方式】

本发明提供一种利用点云配准技术和三维重建技术的工件位置找正方 法，首先对采集的点云进行去噪及简化等预处理；其次以机械臂作为运动 装置辅助完成工件参考模型的三维重建；最后进行相关点云配准算法，得 到实物模型和参考模型的位姿变换矩阵，从而将实物模型和参考模型对齐， 完成焊件的位置找正。为了使本发明的目的、技术方案及优点更加清楚明 白，以下结合附图及实施例，对本发明进一步详细说明。

S1对获得的点云数据进行预处理，获得点云模型

点云预处理包括点云去噪和点云简化两部分，其中点云去噪采用三维 点云去噪算法：利用点云加权模糊C均值聚类算法去除大尺度噪声，同时 对点云进行聚类，再对那些离点云模型较近的小尺度噪声使用双边滤波算 法进行处理，在去除噪声的同时能够尽可能保留图像的细节。采用体素化 网格法实现点云简化，即模型被划分为若干个体素栅格，每个体素栅格中 的所有点用一个重心点来代替，从而在保证点云特征的情况下实现对点云 的下采样，同时可以通过调整体素栅格的大小来获得不同采样密度的点云 模型。

S11点云去躁

S111利用点云加权模糊C均值聚类算法计算数据点g_i的密度ρ_i，密度ρ_i具体按如下公式进行计算：

公式(1)中，g_i为数据点，d(g_i,g_j)为两个数据点间的欧氏距离，g_j为 数据点g_i的k个邻域点，当k个数据点距离较近时，d(g_i,g_j)较小，ρ_i较大， 这对聚类影响较大，因此按如下公式对ρ_i进行归一化：

于公式(2)中，r为数据点到聚类中心的距离，Ω_i为数据点g_i对分类 的影响，噪声点离模型越远，Ω_i越小，对分类的影响就越小，最终的聚类 效果越好，因此点云加权模糊C均值聚类算法的目标函数按如下公式表示：

于公式(3)中，u_ij为数据点对应的隶属度，其介于[0，1]之间，b_i为聚 类组i的聚类中心，d_ij＝|b_i-x_j|为第i个聚类中心与第j个数据点间的欧式距 离，x_j为数据点的坐标，s＝2，C为聚类中心个数，

S112判断数据点g_i到k近邻点的平均距离是否大于阈值，若是，则表 示该数据点g_i为噪声，删除，结束，若否，则执行S213，

S113对k近邻点进行点云权重聚类，用聚类中心作为新的数据点，即 去除点云数据中的大尺度噪声，结束，通过迭代计算使公式(3)取最小值， 按如下公式计算出聚类中心b_i、对应的隶属度u_ij：

S114按如下公式计算出双边滤波因子μ：

于公式(6)中，k_ij为数据点g_i的临近点，M(g_i)为数据点g_i的邻域点 的隶属度，σ_c衡量了点g_i到其邻域点的欧氏距离对该点的影响，σ_s表示点g_i到邻域点的距离向量在法向上的投影对该点的影响，

为滤波函数，

为特征权重函数，x＝||g_i-k_ij||为数据点g_i到邻域点的距离，＜n_i,g_i-k_ij＞ 为特征权重，所述滤波函数、特征权重函数分别按如下公式表示：

S115将公式(6)带入如下公式对滤波数据进行更新：

g_i＝g_i+μn (9)；

S116遍历模型中所有数据点，即去除点云数据中的小尺度噪声，结束， 如此即在去除噪声的同时保留了图像细节，从而使最终获得的点云模型更 加接近工件；

S12点云简化

S121通过原始点云模型在X，Y，Z方向上的x_min，x_max，y_min，y_max， z_min，z_max，来确定Cell的大小，

S122根据实际需要确定栅格大小l_x，l_y，l_z，从而确定模型在X，Y， Z方向被分成m，n，l份，即：

S123确定每个数据点g_i在栅格中的索引号a_i，b_i，c_i，计算方法如下：

按如下公式将数据点g_i在栅格中的索引号转化为一维形式：

S124按如下公式计算体素栅格内所有点的重心，并用该重心代表栅格 内的所有点，

S125遍历所有体素，得到简化后的点云数据，获得去躁、简化后的点 云模型，点云模型如图1所示。

S2多角度模型重建

利用高精度的机械臂，可以将相机在不同位置处扫描的点云模型转换到 同一坐标系下，从而实现模型的粗拼接，然后利用ICP算法求得相邻点云 的位姿转换关系，实现点云的精拼接，最后进行全局优化，并对拼接后的 模型进行点云融合处理，以去除冗余点云，获得如图2所示的点云表面误 差分布图。

S21将工件固定放置于机械臂前方，控制机械臂运动n次得到点云模型 集合{p₁,p₂,…,p_n}，将各个直接获得的点云模型转换到机械臂初始拍摄位置 的相机坐标系下，分析系统的坐标转换关系如下公式：

于公式(14)中，

均为手眼关系矩阵，分别表示工具末端相对 相机的位姿关系和相机相对工具末端的位姿关系，

表示机械臂每次移动 时的位姿，从机械臂系统获得，

表示机械臂的初始拍摄位置；

S22通过点云融合将模型进行粗拼接，得到粗拼接模型，粗拼接模型 如图3所示；

S23在点云P中任意选一点P_i，在点云Q中找到距其最近的点Q_j，对于 存在的一些误匹配点对，引入以下约束进行去除，若(p₁,q₁)，(p₂,q₂)为两对 正确匹配的对应点，设e＝||q₁-p₁||₂，则这两对对应点应该满足以下约束：

(1)方向性约束：(p₂-p₁)(q₂-q₁)≥0，

(2)刚性约束：||q₂-q₁||₂≤2(e+||p₂-q₁||₂)，

(3)匹配误差约束：||q₂-q₁||₂≤e+||p₂-q₁||₂；

S24粗拼接之后的两片点云对应的空间点集用{P_i|i＝1,2,…,r}和 {Q_i|i＝1,2,…,s}表示，P和Q不用一一对应，r、s无需相等，假设r＞s，且两 片点云有n个正确的对应点，求解这两组点集对应点之间的旋转和平移关系[R,T]，从而使如下公式的目标函数最小，

S25通过全局优化和点云融合，即得到工件参考模型，工件参考模型 如图4所示。

为了确保点云模型和工件更加近似，按如下公式对拼接精度进行检验，

最终计算结果为0.066mm，满足机器人辅助加工作业中工件重建精度的 要求，结束。

S3对参考模型和点云模型进行点云配准，得到参考模型和点云模型的 位姿变换矩阵

S31对降采样得到的关键点计算其SHOT特征描述子

SHOT特征描述子在构建的局部参考系内，统计特征点周围的拓扑特征， 将特征保存在直方图中，并对其进行归一化，特征点p邻域内的点记为p_i， 且满足||p_i-p||≤R，其局部描述符M按如下公式计算：

于公式(15)中，d_i＝||p_i-p||₂，采用特征分解法算法对M进行特征值 分解，得到M的三个特征值及对应的特征向量，分别作为x⁺，y⁺，z⁺，相 反方向记为x^-，y^-，z^-，最终的x轴方向表示为：

同理得到z轴的方向，y轴方向通过z×x确定，即构建完成局部参考系， 其具有旋转和平移不变性；

为了计算特征，将上述局部参考系按照2个径向分区、2个俯仰分区、 8个方位角进行分区，获得32个子区间，分区完成后，记点p的法向量为n， 球内点p_i的法向量记为n_i，设两个法向量的夹角为θ_i，则获得如下公式：

cosθ_i＝n·n_i (21)，

将cosθ_i划分为m份，则可以建立32(m+1)份局部直方图，其中1表示球 内邻域点在球内的位置，通过对邻域点在局部坐标系中的位置和方向角的 计算，即完成直方图的构建。

S32 ICP精确位姿矩阵估计

经过点云初步配准后，两组点云匹配点之间的距离已经在一定范围内， 因此可以利用ICP算法在场景点云中寻找参考模型点云最近的点，从而完 成点云精确配准，执行过程如下：

S321根据不同点云对应点间的距离，建立场景点云和参考点云的对应 点集，

S322求满足对应点集平均距离最小的刚性变换参数，

S323应用得到的刚性变换参数对场景点云进行坐标变换，得到变换后 的场景点云，

S324求参考点云和变换后的点云之间的平均距离，若小于设置的距离 阈值，则迭代结束，否则以变换后的点云作为新的场景点云继续进行迭代 计算，直到目标函数满足收敛精度要求，结果如图5所示。

S4工件找正

S41根据两组点云之间的匹配关系，对两组点云之间的位姿转换关系 进行初步估计，首先将参考点云的中心移动至场景点云中心处，可粗略计 算出两组点云之间的平移向量T，假设参考点云关键点移动后为p'，x为场 景点云的关键点坐标，则有：

于公式(22)中，R为单位矩阵，其平方为1，因此上述公式可按如下 公式化为求解H的最大值：

∑2x^TRp'＝tr(R^T∑xp'^T)＝tr(R^TH) (23)，

于公式(23)中，H＝∑xp'^T，对H进行奇异值分解，得到如下公式：

H＝USV^T (24)，

于公式(24)中，当R＝VU^T时，R^TH可对角化，此时迹最大，即通过 分解H，得到旋转矩阵R，并计算出平移矩阵T，位姿转换矩阵如下：

S42利用位姿变换矩阵，将参考模型和场景点云模型对齐，实现工件 的位置找正。

结合最终获得的示意图，可知采用本发明进行工件位置找正方法可行， 且误差较小，从而可以确保机器人运动路径的准确。

于本发明中，对于有CAD模型的工件，实现预建立的工件参考点云模 型和实际点云模型对齐，使系统可以实时感知工件的位姿情况，实现工件 的定位找正。而对于无CAD模型的工件，利用参考模型的三维重建技术， 根据高精度机械臂的运动关系及手眼标定的结果进行多角度点云模型粗拼 接，再应用ICP算法对其进行精拼接，通过点云融合得到工件件参考模型， 在利用电源配准技术，将实际场景中的工件模型和建立的参考模型对齐， 得到两者的位姿转换关系，使系统能够实时感知工件的位姿，从而实现工 件的定位找正。

Claims (8)

1.利用点云配准技术和三维重建技术的工件位置找正方法，其特征在于，包括以下步骤：

S1对获得的点云数据进行预处理，获得点云模型；

S2利用三维重建技术建立参考模型；

S3对参考模型和点云模型进行点云配准，得到参考模型和点云模型的位姿变换矩阵；

S4利用位姿变换矩阵将点云模型和参考模型对齐，即完成工件位置找正。

2.根据权利要求1所述的利用点云配准技术和三维重建技术的工件位置找正方法，其特征在于，所述S1采用视觉传感器获取点云数据，该视觉传感器为二维传感器。

3.根据权利要求1所述的利用点云配准技术和三维重建技术的工件位置找正方法，其特征在于，所述S1中预处理包括点云去噪和点云简化，所述点云去噪具体按照以下方案实施：

S111利用点云加权模糊C均值聚类算法计算数据点g_i的密度ρ_i，密度ρ_i具体按如下公式进行计算：

公式(1)中，g_i为数据点，d(g_i,g_j)为两个数据点间的欧氏距离，g_j为数据点g_i的k个邻域点，当k个数据点距离较近时，d(g_i,g_j)较小，ρ_i较大，这对聚类影响较大，因此按如下公式对ρ_i进行归一化：

于公式(2)中，r为数据点到聚类中心的距离，Ω_i为数据点g_i对分类的影响，噪声点离模型越远，Ω_i越小，对分类的影响就越小，最终的聚类效果越好，因此点云加权模糊C均值聚类算法的目标函数按如下公式表示：

于公式(3)中，u_ij为数据点对应的隶属度，其介于[0，1]之间，b_i为聚类组i的聚类中心，d_ij＝|b_i-x_j|为第i个聚类中心与第j个数据点间的欧式距离，x_j为数据点坐标，s＝2，C为聚类中心个数，

S112判断数据点g_i到k近邻点的平均距离是否大于阈值，若是，则表示该数据点g_i为噪声，删除，结束，若否，则执行S213，

S113对k近邻点进行点云权重聚类，用聚类中心作为新的数据点，即去除点云数据中的大尺度噪声，结束，通过迭代计算使公式(3)取最小值，按如下公式计算出聚类中心b_i、对应的隶属度u_ij：

S114按如下公式计算出双边滤波因子μ：

于公式(6)中，k_ij为数据点g_i的临近点，M(g_i)为数据点g_i的邻域点的隶属度，σ_c衡量了点g_i到其邻域点的欧氏距离对该点的影响，σ_s表示点g_i到邻域点的距离向量在法向上的投影对该点的影响，

为滤波函数，

为特征权重函数，x＝||g_i-k_ij||为数据点g_i到邻域点的距离，＜n_i,g_i-k_ij＞为特征权重，所述滤波函数、特征权重函数分别按如下公式表示：

S115按如下公式对滤波数据进行更新：

g_i＝g_i+μn (9)；

S116遍历模型中所有数据点，即去除点云数据中的小尺度噪声，结束；

所述点云简化具体按照以下方案实施：

S121通过原始点云模型在X，Y，Z方向上的x_min，x_max，y_min，y_max，z_min，z_max，来确定Cell的大小，

S122根据实际需要确定栅格大小l_x，l_y，l_z，从而确定模型在X，Y，Z方向被分成m，n，l份，即：

S123确定每个数据点g_i在栅格中的索引号a_i，b_i，c_i，计算方法如下：

按如下公式将数据点g_i在栅格中的索引号转化为一维形式：

S124按如下公式计算体素栅格内所有点的重心，并用该重心代表栅格内的所有点，

S125遍历所有体素，得到简化后的点云数据。

4.根据权利要求3所述的利用点云配准技术和三维重建技术的工件位置找正方法，其特征在于，所述S2具体按照以下方案实施：

S21将工件固定放置于机械臂前方，控制机械臂运动n次得到点云模型集合{p₁,p₂,…,p_n}，将各个直接获得的点云模型转换到机械臂初始拍摄位置的相机坐标系下，分析系统的坐标转换关系如下公式：

于公式(14)中，

均为手眼关系矩阵，分别表示工具末端相对相机的位姿关系和相机相对工具末端的位姿关系，

表示机械臂每次移动时的位姿，从机械臂系统获得，

表示机械臂的初始拍摄位置；

S22通过点云融合将模型进行粗拼接，得到粗拼接模型；

S23在点云P中任意选一点P_i，在点云Q中找到距其最近的点Q_j，对于存在的一些误匹配点对，引入以下约束进行去除，若(p₁,q₁)，(p₂,q₂)为两对正确匹配的对应点，设e＝||q₁-p₁||₂，则这两对对应点应该满足以下约束：

(1)方向性约束：(p₂-p₁)(q₂-q₁)≥0，

(2)刚性约束：||q₂-q₁||₂≤2(e+||p₂-q₁||₂)，

(3)匹配误差约束：||q₂-q₁||₂≤e+||p₂-q₁||₂；

S24粗拼接之后的两片点云对应的空间点集用{P_i|i＝1,2,…,r}和{Q_i|i＝1,2,…,s}表示，P和Q不用一一对应，r、s无需相等，假设r＞s，且两片点云有n个正确的对应点，求解这两组点集对应点之间的旋转和平移关系[R,T]，从而使如下公式的目标函数最小，

S25通过全局优化和点云融合，即得到工件参考模型。

5.根据权利要求4所述的利用点云配准技术和三维重建技术的工件位置找正方法，其特征在于，所述S2在得到工件参考模型后，按如下公式对其拼接精度进行检验，若满足需要，则结束，若不满足需要，重新进行工件参考模型的拼接，

于公式(15)中，N为模型点云中的点数量，

为残差e_i的平均值。

6.根据权利要求4所述的利用点云配准技术和三维重建技术的工件位置找正方法，其特征在于，所述S3首先用SHOT描述子对工件参考模型实现粗配准，其次用ICP对工件参考模型实现精确配准；

所述现粗配准具体按照以下方案实施：

SHOT特征描述子在构建的局部参考系内，统计特征点周围的拓扑特征，将特征保存在直方图中，并对其进行归一化，特征点p邻域内的点记为p_i，且满足||p_i-p||≤R，其局部描述符M按如下公式计算：

于公式(15)中，d_i＝||p_i-p||₂，采用特征分解法算法对M进行特征值分解，得到M的三个特征值及对应的特征向量，分别作为x⁺，y⁺，z⁺，相反方向记为x^-，y^-，z^-，最终的x轴方向表示为：

同理得到z轴的方向，y轴方向通过z×x确定，即构建完成局部参考系，其具有旋转和平移不变性；

所述精确配准具体按照以下方案实施：

S321根据不同点云对应点间的距离，建立场景点云和参考点云的对应点集，

S322求满足对应点集平均距离最小的刚性变换参数，

S323应用得到的刚性变换参数对场景点云进行坐标变换，得到变换后的场景点云，

S324求参考点云和变换后的点云之间的平均距离，若小于设置的距离阈值，则迭代结束，否则以变换后的点云作为新的场景点云继续进行迭代计算，直到目标函数满足收敛精度要求。

7.根据权利要求6所述的利用点云配准技术和三维重建技术的工件位置找正方法，其特征在于，所述直方图按如下方法构建：

将局部参考系按照2个径向分区、2个俯仰分区、8个方位角进行分区，获得32个子区间，分区完成后，记点p的法向量为n，球内点p_i的法向量记为n_i，设两个法向量的夹角为θ_i，则获得如下公式：

cosθ_i＝n·n_i (21)，

将cosθ_i划分为m份，则可以建立32(m+1)份局部直方图，其中1表示球内邻域点在球内的位置，通过对邻域点在局部坐标系中的位置和方向角的计算，即完成直方图的构建。

8.根据权利要求6所述的利用点云配准技术和三维重建技术的工件位置找正方法，其特征在于，所述S4按照以下方案实施：

S41根据两组点云之间的匹配关系，对两组点云之间的位姿转换关系进行初步估计，首先将参考点云的中心移动至场景点云中心处，可粗略计算出两组点云之间的平移向量T，假设参考点云关键点移动后为p'，x为场景点云的关键点坐标，则有：

于公式(22)中，R为单位矩阵，其平方为1，因此上述公式可按如下公式化为求解H的最大值：

∑2x^TRp'＝tr(R^T∑xp'^T)＝tr(R^TH) (23)，

于公式(23)中，H＝∑xp'^T，对H进行奇异值分解，得到如下公式：

H＝USV^T (24)，

于公式(24)中，当R＝VU^T时，R^TH可对角化，此时迹最大，即通过分解H，得到旋转矩阵R，并计算出平移矩阵T；

S42利用位姿变换矩阵，将参考模型和场景点云模型对齐，实现工件的位置找正。

Images