CN114299150A - 一种深度6d位姿估计网络模型及工件位姿估计方法 - Google Patents

Abstract

本发明公开一种深度6D位姿估计网络模型及工件位姿估计方法，该工件位姿估计方法通过物理引擎生成模拟数据集的方法解决了大体量点云数据集获取困难的问题，以纯几何点云坐标直接将完整场景点云作为输入，通过点云的语义与实例分割部分，可以提取输入点云的局部与全局特征，提升了网络对场景的理解能力，并通过多层特征融合的位姿估计网络输出准确姿态，在一定程度上解决了物体的堆叠和自遮挡问题，且针对各种对称物体具有鲁棒性，经过模拟真实场景的数据集的实验验证，本发明提出的方法在整体精度和稳定性上具有明显优势，具有更高的鲁棒性。

Description

一种深度6D位姿估计网络模型及工件位姿估计方法

技术领域

本发明涉及机器视觉技术领域，具体提供一种深度6D位姿估计网络模型及工件位姿估计方法。

背景技术

随着技术的进步，机器人在抓取、装配、包装、加工、物流分拣等方面得到日益广泛的应用。其中抓取装配操作是机器人最常见的应用场景，面临的一大挑战是复杂环境下对象的准确抓取问题。在传统的结构化环境中，通过预先由人工寻找示教点编入程序，再按固定程序运行的方式，虽然可达到很高的精度与成功率，但由于缺少环境感知与交互，难以在非结构化或半结构化等复杂场景下完成精密抓取装配作业。

可靠的机器人抓取装配系统，需要准确地获得目标在场景点云中的位置和姿态，即6D位姿。现有的方法，主要分为类别级识别和实例级识别。类别级姿态估计是指，同类物体拥有不同外形、或是在有某类下新的物体加入的情况下也能完成物体识别与姿态估计任务。实例级识别则反之，每类物体需要有固定的外形形状，如CAD模型等。

而工业零件大都为具有标准生产模型的刚性物体，几乎无类内的变化，降低了识别难度，但工业场景中的大多数零件有弱纹理、颜色相近或相同的特点，在这种情况下，RGB颜色信息变得不再可靠。因此，为了提高识别的可靠性，本文从仅包含3D几何信息的点云入手，从物体的边缘信息、几何关系中挖掘物体6D位姿；另一方面待抓取物体因为经常在杂乱的场景中，物体间相互遮挡和堆叠不可避免，这对基于视觉引导的机器人抓取等任务仍带来较大困难。

发明内容

本发明的目的是为了解决现有技术中的上述缺陷，设计了一种深度6D位姿估计网络模型及工件位姿估计方法；该网络模型结合点云分割方法和点云姿态估计方法来实现复杂场景下对多种多个非规则工件的位姿估计。

本发明解决所述技术问题的技术方案是：设计一种深度6D位姿估计网络模型，其特征在于，该网络模型包含点云分割器、特征聚类采样器和姿态估计器三个子模块：

所述点云分割器包括特征提取器、特征生成器和特征判别器，其中特征提取器由多个SA层构成，特征生成器由多个FP层构成，特征判别器由两个MLP构成；

特征提取器的多个SA层对输入的点云数据进行多层逐次采样提取特征点，输入的点云数据除了输入到第一个SA层之外还输入到特征生成器的最后一个FP层，第一个SA层提取到的特征点除了输入到第二个SA层之外还输入到特征生成器的倒数第二个FP层，以此类推，每一个SA层提取到的特征点除了输入到下一个SA层还输入到距离其最远的未连接SA层的FP层，最后一个SA层提取到的特征点输入到第一个FP层。输入的N×3的点云数据经过特征提取器与特征生成器的处理后，最终输出N×128维的特征向量。

所述两个MLP的尺寸分别为[64，128]与[64，32]，将特征生成器的最后一个FP层的输出分别输入到两个MLP，分别获得输入点云数据的点级高维实例特征和语义标签预测。

所述特征聚类采样器由基于密度聚类的机器学习方法以及采样方法组合而成，对于点云分割器生成的高维实例特征，特征聚类采样器将具有相似特征的实例点聚类，生成实例，并通过最远点迭代下采样方法或球查询上采样方法将点数控制到统一值M个，结合语义标签送入下一环节。具体的，若生成实例的点数大于M，使用迭代最远点采样法；而生成实例的点数小于M时，则使用球查询的方法，将点云围绕中心点划分成N个球形区域，将预测实例的点与输入的原始点云合并，并在每个预测点的查询半径内查找距离最近的原始点，循环查找直至找到M个点。对于数量过于小的点簇，即点数小于100的点，不进行球查询以及后续的保存处理。

所述姿态估计器包含两个姿态特征提取模块，该两个姿态特征提取模块结构相同，聚类采样器输出的M个点数的实例及相应的语义标签输入到姿态估计器，姿态估计器对其进行两个方面的处理：第一方面是将M个点数的实例及相应的语义标签直接输送到第一姿态特征提取模块，通过第一姿态特征提取模块获得预测的带语义标签的点云实例的旋转向量；第二方面是求取M个点数的实例的均值，然后将M个点数的实例及相应的语义标签减去M个点数的实例的均值后输送到第二姿态特征提取模块，获得预测的相对偏移，然后再将相对偏移与M个点数的实例的均值加和，获得预测的带语义标签的点云实例的平移向量。

姿态特征提取模块基于改进的PointNet结构，其工作原理为：首先将输入点云实例通过一个尺寸为[64,64,128,256]的MLP进行特征提取，然后将MLP的每层所提取到的多个维度特征进行拼接，获得一个512维度的多层次特征；接着将获得的512维度的多层次特征输入到一个全连接层，获得一个1024维度的多层次特征，之后将其输入到池化层，获取全局特征；最后，将全局特征输入到尺寸为[512,256,3]的MLP，获得预测的带语义标签的点云实例的旋转向量或平移向量。

进一步，本发明设计一种工件位姿估计方法，其特征在于，该方法基于如上所述的深度6D位姿估计网络模型，其包括如下步骤：

步骤1：包含实例及6D位姿标签的点云数据准备：

根据待识别物体的三维模型，利用物理引擎模拟物体从固定高度随机位置自然掉落，模拟与地面以及物体相互之间的碰撞参数，待碰撞稳定后，利用虚拟相机采集点云，并生成具有点级实例标签以及准确6D位姿的点云数据；最后，剔除数据中出现严重遮挡的物体，即一类物体的点云点数进行统计排序后，剔除掉点数最少的30％数据，得到网络模型的点云数据集；将网络模型的点云数据集按一定比例划分，将其中不少于60％的部分划分为训练集，余下部分为测试集；

步骤2:深度6D位姿估计网络模型的训练

2.1利用步骤1所得的训练集对深度6D位姿估计网络模型的点云分割器、姿态估计器分别进行训练。

训练集中的点云数据及对应的点级语义与实例分割标签经过参数初始化的点云分割器处理，获得点级高维实例特征和语义标签预测，然后分别计算语义标签与实例特征生成的损失函数，将两者加权求和得到点云分割器的损失函数；根据点云分割器的损失函数值，反向传播完成一次参数更新，接着用参数更新后的点云分割器再一次处理训练集中的点云数据及对应的点级语义与实例分割标签，并利用当前参数值对应的点云分割器的损失函数值继续反向传播，完成一次参数更新，不断循环参数更新过程直至达到设定的循环次数，完成点云分割器的参数训练。

将训练集中的点云实例以及对应的准确6D姿态标签输入到参数初始化的姿态估计器中，获得预测的对应点云实例旋转向量和平移向量，进而根据旋转和平移的损失函数计算姿态估计器总体的损失函数，根据姿态估计器总体的损失函数值反向传播，实现一次姿态估计器的参数更新；接着用参数更新后的姿态估计器再一次处理训练集中的点云实例以及对应的准确6D姿态标签，并利用当前参数值对应的姿态估计器总体的损失函数值继续反向传播，实现一次姿态估计器的参数更新，不断循环参数更新过程直至达到设定的循环次数，完成姿态估计器的参数训练。

2.2利用步骤1所得的测试集对深度6D位姿估计网络模型进行测试

将步骤1所的测试集中的点云数据及对应的点级语义与实例分割标签输入到完成参数训练的点云分割器，得到预测的点云数据的点级高维实例特征和语义标签预。接着将预测的点云数据的点级高维实例特征和语义标签输入到特征聚类采样器中，得到M个点数的实例及相应的语义标签；最后将聚类采样器输出的M个点数的实例及相应的语义标签输入到完成参数训练的姿态估计器，得到预测的对应点云实例的旋转向量和平移向量。

物体位姿估计误差的评价指标为平均点对距离，具体如下所示：

其中M表示3D模型的点云集合，m是点数，x表示3D模型的点云中的任意一个点；x₁为物体模型的点云中任意一个点，x₂为将物体模型的点云按深度6D位姿估计网络模型预测的6D位姿变换到场景中的与x₁最邻近的点；R和t分别表示真实的旋转矩阵和平移向量，R和

分别表示预测的旋转矩阵和平移向量；评估时，将预测的旋转向量转换为旋转矩阵进行评估。

6D位姿估计的预测质量评估，表示为平均点对距离小于各物体对应模型直径d的10％的度量上准确率；当准确率大于90，则该深度6D位姿估计网络模型为有效模型。

步骤3:工件位姿估计

将相机采集到的多个工件的场景点云输入到步骤2中测试为有效的深度6D位姿估计网络模型中，经过点云分割器对场景点云进行实例分割，获取到整个场景点云的高维实例特征，送入特征聚类采样器，获取到每个工件的点云实例，进而送入姿态估计器，得到每个工件的高精度6D姿态，该姿态即可用于工业分拣抓取作业过程。

与现有技术相比，本发明的有益效果是：

本发明方法将虚拟数据生成方法应用在工业抓取的6D姿态估计问题中，拥有深度学习技术高效的检测效果而又缓解了深度学习对大批量数据的依赖性，在保证识别准确性及实时性的前提下，不需要大量的带有标签的训练数据，减少了人力成本，利用实例分割的方法提前将工件单独分割出来再进行估计，其训练好的模型对各类轴对称、中心对称、以及其他非规则物体的识别具有更好的泛化能力，其性能远远优于传统机器视觉和一般的姿态估计方法。

附图说明

图1为本发明一种深度6D位姿估计网络模型一种实施例的工作原理示意图；

图2为本发明一种深度6D位姿估计网络模型的点云分割器的工作原理示意图；

图3为本发明一种深度6D位姿估计网络模型的特征聚类采样器的工作原理示意图；

图4为本发明一种深度6D位姿估计网络模型的姿态估计器工作原理示意图；

图5为本发明一种深度6D位姿估计网络模型的姿态估计器的姿态特征提取模块的工作原理示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明提供一种深度6D位姿估计网络模型，该网络模型包含点云分割器、特征聚类采样器和姿态估计器三个子模块：

所述点云分割器(参见图2)包括特征提取器、特征生成器和特征判别器，其中特征提取器由多个SA(Set Abstraction的缩写，特征点采样与特征提取模块，下同)层构成，特征生成器由多个FP(Feature Propagation，特征传播模块，为跨层连接，实现特征点对应特征向量生成，下同)层构成，特征判别器由两个MLP(多层感知机，下同)构成；

特征提取器的多个SA层对输入的点云数据进行多层逐次采样提取特征点，输入的点云数据除了输入到第一个SA层之外还输入到特征生成器的最后一个FP层，第一个SA层提取到的特征点除了输入到第二个SA层之外还输入到特征生成器的倒数第二个FP层，以此类推，每一个SA层提取到的特征点除了输入到下一个SA层还输入到距离其最远的未连接SA层的FP层，最后一个SA层提取到的特征点输入到第一个FP层。

特征提取器与特征生成器以跳接特征层的方式，不断采样特征点，提取高维特征，SA层用于点云的采样、点组合、组合点处的局部特征提取任务，而FP层则用于将多尺度的特征进行传递，最终输出每个点的预测特征。输入的N×3的点云数据经过特征提取器与特征生成器的处理后，最终输出N×128维的特征向量。

所述两个MLP的尺寸分别为[64，128]与[64，32]，将特征生成器的最后一个FP层的输出分别输入到两个MLP，分别获得输入点云数据的点级高维实例特征和语义标签预测。

点云分割器的损失函数由两个分支的损失函数加权求和而成：

其中θ，η是平衡参数,

分别代表语义标签与实例特征生成的损失函数，语义标签分支的损失函数

为交叉熵损失函数，实例特征生成分支的损失函数

定义如下：

其中α，β，γ为权重参数，表示若场景下共有C个实例，N_C为其中的第c个，μ_C为该实例的特征的均值；x_i为第i个点的实例特征；

为计算每个实例的点朝向其自身实例中心点的损失，使得每个实例点的预测向其真实中心点靠近；

为计算各个实例的中心点之间的距离损失，增加预测的各个实例中心点的距离；δ_v和δ_d为

和

的损失界限，

为一个使得每个点向着原点靠近的拉力，以保持激活有界；点云分割器在训练时，采用随机梯度下降的方式使

最小化。

所述特征聚类采样器由基于密度聚类的机器学习方法以及采样方法组合而成，对于点云分割器生成的高维实例特征，特征聚类采样器将具有相似特征的实例点聚类，生成实例，并通过最远点迭代下采样方法或球查询上采样方法将点数控制到统一值M(本专利中M＝4096，下同)个，结合语义标签送入下一环节。具体的，若生成实例的点数大于M，使用迭代最远点采样法，而生成实例的点数小于M时，则使用球查询的方法，将点云围绕中心点划分成N个球形区域，将预测实例的点与输入的原始点云合并，并在每个预测点的查询半径内查找距离最近的原始点，循环查找直至找到M个点。对于数量过于小的点簇，即点数小于100的点，不进行球查询以及后续的保存处理。其流程结构如图3所示，首先，对于送入的高维特征，使用MeanShift算法进行特征聚类，生成逐点实例预测，在评估时，会出现虽预测的是同一个实例，但是预测编号不一致，导致评估为错误的数据关联问题，引入KM算法，其对应权重设定为预测实例的点数统计集合与真实实例的点数统计集合的对应数据方差，其目的在于使得点数越相近的两组点拥有越高的连接权重，从而使得同一实例的预测标签与真实标签数值相同。

在聚类生成实例后，由于每个实例的点个数都不相同，需将其采样至统一值，送入后续网络，这里采用最远点采样与球查询方法，其中若实例的点数大于M，使用FPS采样法，相比于其他点云采样方式，该方法可以更好的覆盖空间中的所有点，尽可能的保留源点云的特征，便于后续的网络分析，而实例的点数小于M时，则使用球查询的方法，将点云围绕中心点划分成N个球形区域，将预测实例的点与输入的原始点云合并，并在每个预测点的查询半径内查找距离最近的原始点，循环查找直至找到M个点，将查找到的原始点集作为实例送入后续进行分析。

所述姿态估计器包含两个姿态特征提取模块，该两个姿态特征提取模块结构相同，聚类采样器输出的M个点数的实例及相应的语义标签输入到姿态估计器，姿态估计器对其进行两个方面的处理：第一方面是将M个点数的实例及相应的语义标签直接输送到第一姿态特征提取模块，通过第一姿态特征提取模块获得预测的带语义标签的点云实例的旋转向量；第二方面是求取M个点数的实例的均值，然后将M个点数的实例及相应的语义标签减去M个点数的实例的均值后输送到第二姿态特征提取模块，获得预测的相对偏移(平移残差)，然后再将相对偏移与M个点数的实例的均值加和，获得预测的带语义标签的点云实例的平移向量。

姿态特征提取模块基于改进的PointNet结构，其工作原理为：首先将输入点云实例通过一个尺寸为[64,64,128,256]的MLP进行特征提取，然后将MLP的每层所提取到的多个维度特征进行拼接，获得一个512维度的多层次特征；接着将获得的512维度的多层次特征输入到一个全连接层，获得一个1024维度的多层次特征，之后将其输入到池化层，获取全局特征；最后，将全局特征输入到尺寸为[512,256,3]的MLP，获得预测的带语义标签的点云实例的旋转向量或平移向量。

在计算Loss时由于网络对输入的点云会去除输入点云均值再进行计算，则输出的平移Δt为平移残差，故预测平移向量

由网络预测的残差平移Δt与输入的M×3点云求均值获得的μ_t加和构成：

平移向量T的损失函数

为真实平移向量t与预测平移向量

之间距离的二范数：

旋转向量r的损失函数为将预测旋转向量

与真实旋转向量r转换为对应旋转矩阵

与R后，计算两者在三维空间中的旋转差，即测地距离：

姿态估计器总体的损失函数是由旋转和平移的损失函数加权求得，具体为将平移损失函数进行一定比例的缩放之后，与旋转损失函数相加而得，如式(9)所示：

其中，α为比例常数，取值范围为2.5～3.5，本实施例取值为3。

姿态估计器在训练的时候，通过调整两个姿态特征提取模块的网络参数，使得姿态估计器总体的损失函数为最小值。

姿态估计器主要任务是完成输入点云实例的6D物体位姿估计任务，其目的是找到待估计物体从物体坐标系变换到相机坐标系的变换关系，即旋转和平移参数，从而进行后续的处理，如机械臂抓取等，而平移与旋转彼此相对独立，其中，平移参数一般以3×1的平移向量T表示，代表了物体变换时沿着三个坐标轴的位移，旋转指定了围绕三个坐标轴的旋转，本发明在网络中采用了旋转向量的输出方式，其具体结构如图4所示，其中，姿态特征提取模块将输入点云通过多层感知机提取特征后，将多个维度的特征拼接起来获得512维度的多层次特征，实现网络对物体多层特征的学习感知，进而通过全连接层将特征维度升至1024维，送入池化层后获取全局特征，进而通过多层感知机回归物体的旋转或平移向量。

进一步的，本发明提供一种工件位姿估计方法，该方法基于如上所述的深度6D位姿估计网络模型，其包括如下步骤：

步骤1：包含实例及6D位姿标签的点云数据准备：

根据待识别物体的三维模型，利用物理引擎模拟物体从固定高度随机位置自然掉落，模拟与地面以及物体相互之间的碰撞参数，待碰撞稳定后，利用虚拟相机采集点云，并生成具有点级实例标签以及准确6D位姿的点云数据，最后，剔除数据中出现严重遮挡的物体，即一类物体的点云点数进行统计排序后，剔除掉点数最少的30％数据，得到网络模型的点云数据集；将网络模型的点云数据集按一定比例划分，将其中不少于60％的部分划分为训练集，余下部分为测试集；

本实施例中，网络模型的点云数据集利用开源物理引擎Blensor获得，其过程具体如下：

(1)随机导入4～7个可重复的零件的CAD网格模型，并在预设的抓取平面上方，随机产生各模型的6自由度位姿，用于初始化零件的位态，待拍摄平面为长宽均为256mm的正方形区域，并在四周设置了碰撞挡板，防止物体从平面掉落导致穿模而无法获取准确数据。

(2)为仿真场景中模型加入物理引擎，设定对应刚体碰撞参数、动态参数，保留零件在重力作用下，工件做自由落体的最后状态，以模拟工件在平面上的散乱、有堆叠的摆放。

(3)添加传感器模型，设定传感器分辨率、焦距参数，并加入噪声参数以提高数据集与实际传感器采集数据的相似程度。

(4)保存传感器坐标系下场景内的表面点云，作为数据集样本；保存各零件的6自由度姿态信息与点级语义信息，作为数据集标签。

(5)针对每个场景下的有多个同类工件的点云，依据语义信息，将该类工件分离出来，并将点云模型通过6自由度姿态信息变换到实际位置，使用KNN(K近邻点算法)的算法，获取变换后的每个点云模型在原场景中临近的一个或多个点，并提取为单个实例，从而获得整个场景点云的准确实例标签。

(6)针对严重遮挡或因碰撞、穿模而无法在场景点云中有效呈现物体的标签，予以去除。严重遮挡判定阈值为所有生成物体表面点云统计均值的30％，以保证识别抓取的可靠性。

步骤2:深度6D位姿估计网络模型的训练

2.1利用步骤1所得的训练集对深度6D位姿估计网络模型的点云分割器、姿态估计器分别进行训练。

训练集中的点云数据及对应的点级语义与实例分割标签经过参数初始化的点云分割器处理，获得点级高维实例特征和语义标签预测，然后分别计算语义标签与实例特征生成的损失函数，将两者加权求和得到点云分割器的损失函数；根据点云分割器的损失函数值，反向传播完成一次参数更新，接着用参数更新后的点云分割器再一次处理训练集中的点云数据及对应的点级语义与实例分割标签，并利用当前参数值对应的点云分割器的损失函数值继续反向传播，完成一次参数更新，不断循环参数更新过程直至达到设定的循环次数，完成点云分割器的参数训练。

将训练集中的点云实例以及对应的准确6D姿态标签输入到参数初始化的姿态估计器中，获得预测的对应点云实例旋转向量和平移向量，进而根据旋转和平移的损失函数计算姿态估计器总体的损失函数，根据姿态估计器总体的损失函数值反向传播，实现一次姿态估计器的参数更新；接着用参数更新后的姿态估计器再一次处理训练集中的点云实例以及对应的准确6D姿态标签，并利用当前参数值对应的姿态估计器总体的损失函数值继续反向传播，实现一次姿态估计器的参数更新，不断循环参数更新过程直至达到设定的循环次数，完成姿态估计器的参数训练。

2.2利用步骤1所得的测试集对深度6D位姿估计网络模型进行测试

将步骤1所的测试集中的点云数据及对应的点级语义与实例分割标签输入到完成参数训练的点云分割器，得到预测的点云数据的点级高维实例特征和语义标签。接着将预测的点云数据的点级高维实例特征和语义标签输入到特征聚类采样器中，得到M个点数的实例及相应的语义标签；最后将聚类采样器输出的M个点数的实例及相应的语义标签输入到完成参数训练的姿态估计器，得到预测的对应点云实例的旋转向量和平移向量。

对于物体位姿估计误差常用的评价指标是平均点对距离，即将3D模型点云分别做真实位姿和预测位姿的刚体变换后点对的平均欧氏距离。由于对称物体真实位姿变换与预测位姿变换后的3D模型点云之间点对的对应关系不确定，因此区分非对称物体和对称物体，平均点对距离的计算也相应分为了针对非对称物体的AD和针对对称物体的AD-S，具体如下所示：

其中M表示3D模型的点云集合，m是点数，x表示3D模型的点云中的任意一个点；x₁为物体模型的点云中任意一个点，x₂为将物体模型的点云按深度6D位姿估计网络模型预测的6D位姿变换到场景中的与x₁最邻近的点；R和t分别表示真实的旋转矩阵和平移向量，R和

分别表示预测的旋转矩阵和平移向量；评估时，将预测的旋转向量转换为旋转矩阵进行评估。

6D位姿估计的预测质量评估，通常表示为平均点对距离(AD或AD-S值)小于各物体对应模型直径d的10％的度量上准确率，即当平均点对距离小于物体模型的最小外接圆的直径d的0.1倍，则该物体的6D位姿估计为成功；当准确率大于90，则该深度6D位姿估计网络模型为有效模型。

将本发明所提出的深度6D位姿估计网络模型(简称Ours+ICP)预测方法与其他三种做比较，其它三种分别是FPFH+RANSAC+ICP(简称FPFH+ICP)、PPF+3D-Hough+ICP(简称PPF+ICP)和CloudPose+ICP方法。其中前两种是在传统的基于人工描述子的配准方法中常见的识别方案，第三种是基于直接在点云数据上使用深度学习方法进行位姿估计。为了全面比较本发明所提出的网络模型的性能，在输入数据形式上四种方法也有不同，FPFH+RANSAC+ICP和CloudPose+ICP是在已经在真实标签下分割好的点云块上进行识别，并且已知该点云块对应的语义，而PPF+3D-Hough+ICP和本发明提出的网络模型均在整个场景点云上做出多个物体的位姿估计，四种预测方法所采用的数据集相同。

表1四种预测方法的准确率

四种预测方法的准确率如表1所示，根据预测结果可以看到，除了物体D(非对称物体)与物体F(对称物体)的准确率PPF+ICP稍高一点(均为0.3以内)，其余物体的准确率本发明所提出的网络模型具有明显的优势(准确率提高最大为25左右，与其它方法最优相比提高为10左右，加粗字体为该物体对应的类型)。从结果上看，本本发明所提出的网络模型预测效果更加稳定。

步骤3:工件位姿估计

将相机采集到的多个工件的场景点云输入到步骤2中测试为有效的深度6D位姿估计网络模型中，经过点云分割器对场景点云进行实例分割，获取到整个场景点云的高维实例特征，送入特征聚类采样器，获取到每个工件的点云实例，进而送入姿态估计器，得到每个工件的高精度6D姿态(由三维旋转向量和三维平移向量构成)，该姿态即可用于工业分拣抓取作业过程。

作为一种实施例，深度6D位姿估计网络模型训练的时候，采用的数据集数量配置如下所示。

表2训练基本配置表

本实施例使用的电脑配置是酷睿i9-9820X CPU、32GB内存，单张NVIDIA GeForceRTX 2080ti显卡，测试结果表明，点云分割器对训练集的语义分割精度不低于98％，实例分割精度不低于92％，在点云分割器输出高维实例特征后，再经过特征聚类采样器处理后输出实例，然后送入姿态估计器，结果显示，该网络模型对非对称物体的识别精度不低于96％，针对对称物体的识别精度不低于95％。

本实施例一共对七种不同物体的6D姿态进行了识别测试，其中包含轴对称物体、中心对称物体、以及其他大小尺度不一的非规则物体，本发明方法能够准确识别物体的6D位姿，对深度学习方法在工业中的应用提供了低成本解决办法，能够用于工业分拣、抓取装配等环节。

本发明中应用了具体实施例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

本发明未述及之处适用于现有技术。

Claims (6)

1.一种深度6D位姿估计网络模型，其特征在于，该网络模型包含点云分割器、特征聚类采样器和姿态估计器三个子模块：

所述点云分割器包括特征提取器、特征生成器和特征判别器，其中特征提取器由多个SA层构成，特征生成器由多个FP层构成，特征判别器由两个MLP构成；

特征提取器的多个SA层对输入的点云数据进行多层逐次采样提取特征点，输入的点云数据除了输入到第一个SA层之外还输入到特征生成器的最后一个FP层，第一个SA层提取到的特征点除了输入到第二个SA层之外还输入到特征生成器的倒数第二个FP层，以此类推，每一个SA层提取到的特征点除了输入到下一个SA层还输入到距离其最远的未连接SA层的FP层，最后一个SA层提取到的特征点输入到第一个FP层；输入的N×3的点云数据经过特征提取器与特征生成器的处理后，最终输出N×128维的特征向量；

所述两个MLP的尺寸分别为[64，128]与[64，32]，将特征生成器的最后一个FP层的输出分别输入到两个MLP，分别获得输入点云数据的点级高维实例特征和语义标签预测；

所述特征聚类采样器由基于密度聚类的机器学习方法以及采样方法组合而成，对于点云分割器生成的高维实例特征，特征聚类采样器将具有相似特征的实例点聚类，生成实例，并通过最远点迭代下采样方法或球查询上采样方法将点数控制到统一值M个，结合语义标签送入下一环节；具体的，若生成实例的点数大于M，使用迭代最远点采样法；而生成实例的点数小于M时，则使用球查询的方法，将点云围绕中心点划分成N个球形区域，将预测实例的点与输入的原始点云合并，并在每个预测点的查询半径内查找距离最近的原始点，循环查找直至找到M个点；对于数量过于小的点簇，即点数小于100的点，不进行球查询以及后续的保存处理；

所述姿态估计器包含两个姿态特征提取模块，该两个姿态特征提取模块结构相同，聚类采样器输出的M个点数的实例及相应的语义标签输入到姿态估计器，姿态估计器对其进行两个方面的处理：第一方面是将M个点数的实例及相应的语义标签直接输送到第一姿态特征提取模块，通过第一姿态特征提取模块获得预测的带语义标签的点云实例的旋转向量；第二方面是求取M个点数的实例的均值，然后将M个点数的实例及相应的语义标签减去M个点数的实例的均值后输送到第二姿态特征提取模块，获得预测的相对偏移，然后再将相对偏移与M个点数的实例的均值加和，获得预测的带语义标签的点云实例的平移向量；

姿态特征提取模块基于改进的PointNet结构，其工作原理为：首先将输入点云实例通过一个尺寸为[64,64,128,256]的MLP进行特征提取，然后将MLP的每层所提取到的多个维度特征进行拼接，获得一个512维度的多层次特征；接着将获得的512维度的多层次特征输入到一个全连接层，获得一个1024维度的多层次特征，之后将其输入到池化层，获取全局特征；最后，将全局特征输入到尺寸为[512,256,3]的MLP，获得预测的带语义标签的点云实例的旋转向量或平移向量。

2.根据权利要求1所述的一种深度6D位姿估计网络模型，其特征在于，点云分割器的损失函数由两个分支的损失函数加权求和而成：

其中θ，η是平衡参数,

分别代表语义标签与实例特征生成的损失函数，语义标签分支的损失函数

为交叉熵损失函数，实例特征生成分支的损失函数

定义如下：

其中α，β，γ为权重参数，表示若场景下共有C个实例，N_C为其中的第c个，μ_C为该实例的特征的均值；x_i为第i个点的实例特征；

为计算每个实例的点朝向其自身实例中心点的损失，使得每个实例点的预测向其真实中心点靠近；

为计算各个实例的中心点之间的距离损失，增加预测的各个实例中心点的距离；δ_v和δ_d为

和

的损失界限，

为一个使得每个点向着原点靠近的拉力，以保持激活有界；点云分割器在训练时，采用随机梯度下降的方式使

最小化。

3.根据权利要求1所述的一种深度6D位姿估计网络模型，其特征在于，特征聚类采样器中点数M＝4096。

4.根据权利要求1所述的一种深度6D位姿估计网络模型，其特征在于，姿态估计器在计算Loss时由于网络对输入的点云会去除输入点云均值再进行计算，则输出的平移Δt为平移残差，故预测平移向量

由网络预测的残差平移Δt与输入的M×3点云求均值获得的μ_t加和构成：

平移向量T的损失函数

为真实平移向量t与预测平移向量

之间距离的二范数：

旋转向量r的损失函数为将预测旋转向量

与真实旋转向量r转换为对应旋转矩阵

与R后，计算两者在三维空间中的旋转差，即测地距离：

姿态估计器总体的损失函数是由旋转和平移的损失函数加权求得，具体为将平移损失函数进行一定比例的缩放之后，与旋转损失函数相加而得，如式(9)所示：

其中，α为比例常数，取值范围为2.5～3.5，本实施例取值为3；

姿态估计器在训练的时候，通过调整两个姿态特征提取模块的网络参数，使得姿态估计器总体的损失函数为最小值。

5.一种工件位姿估计方法，其特征在于，该方法基于如权利要求1-4任一项所述的深度6D位姿估计网络模型，其包括如下步骤：

步骤1：包含实例及6D位姿标签的点云数据准备：

根据待识别物体的三维模型，利用物理引擎模拟物体从固定高度随机位置自然掉落，模拟与地面以及物体相互之间的碰撞参数，待碰撞稳定后，利用虚拟相机采集点云，并生成具有点级实例标签以及准确6D位姿的点云数据；最后，剔除数据中出现严重遮挡的物体，即一类物体的点云点数进行统计排序后，剔除掉点数最少的30％数据，得到网络模型的点云数据集；将网络模型的点云数据集按一定比例划分，将其中不少于60％的部分划分为训练集，余下部分为测试集；

步骤2:深度6D位姿估计网络模型的训练

2.1利用步骤1所得的训练集对深度6D位姿估计网络模型的点云分割器、姿态估计器分别进行训练；

训练集中的点云数据及对应的点级语义与实例分割标签经过参数初始化的点云分割器处理，获得点级高维实例特征和语义标签预测，然后分别计算语义标签与实例特征生成的损失函数，将两者加权求和得到点云分割器的损失函数；根据点云分割器的损失函数值，反向传播完成一次参数更新，接着用参数更新后的点云分割器再一次处理训练集中的点云数据及对应的点级语义与实例分割标签，并利用当前参数值对应的点云分割器的损失函数值继续反向传播，完成一次参数更新，不断循环参数更新过程直至达到设定的循环次数，完成点云分割器的参数训练；

将训练集中的点云实例以及对应的准确6D姿态标签输入到参数初始化的姿态估计器中，获得预测的对应点云实例旋转向量和平移向量，进而根据旋转和平移的损失函数计算姿态估计器总体的损失函数，根据姿态估计器总体的损失函数值反向传播，实现一次姿态估计器的参数更新；接着用参数更新后的姿态估计器再一次处理训练集中的点云实例以及对应的准确6D姿态标签，并利用当前参数值对应的姿态估计器总体的损失函数值继续反向传播，实现一次姿态估计器的参数更新，不断循环参数更新过程直至达到设定的循环次数，完成姿态估计器的参数训练；

2.2利用步骤1所得的测试集对深度6D位姿估计网络模型进行测试

将步骤1所的测试集中的点云数据及对应的点级语义与实例分割标签输入到完成参数训练的点云分割器，得到预测的点云数据的点级高维实例特征和语义标签预；接着将预测的点云数据的点级高维实例特征和语义标签输入到特征聚类采样器中，得到M个点数的实例及相应的语义标签；最后将聚类采样器输出的M个点数的实例及相应的语义标签输入到完成参数训练的姿态估计器，得到预测的对应点云实例的旋转向量和平移向量；

物体位姿估计误差的评价指标为平均点对距离，具体如下所示：

其中M表示3D模型的点云集合，m是点数，x表示3D模型的点云中的任意一个点；x₁为物体模型的点云中任意一个点，x₂为将物体模型的点云按深度6D位姿估计网络模型预测的6D位姿变换到场景中的与x₁最邻近的点；R和t分别表示真实的旋转矩阵和平移向量，R和

分别表示预测的旋转矩阵和平移向量；评估时，将预测的旋转向量转换为旋转矩阵进行评估；

6D位姿估计的预测质量评估，表示为平均点对距离小于各物体对应模型直径d的10％的度量上准确率；当准确率大于90，则该深度6D位姿估计网络模型为有效模型；

步骤3:工件位姿估计

将相机采集到的多个工件的场景点云输入到步骤2中测试为有效的深度6D位姿估计网络模型中，经过点云分割器对场景点云进行实例分割，获取到整个场景点云的高维实例特征，送入特征聚类采样器，获取到每个工件的点云实例，进而送入姿态估计器，得到每个工件的高精度6D姿态，该姿态即可用于工业分拣抓取作业过程。

6.根据权利要求5所述的一种工件位姿估计方法，其特征在于，步骤1中，网络模型的点云数据集利用开源物理引擎Blensor获得，其过程具体如下：

(1)随机导入4～7个可重复的零件的CAD网格模型，并在预设的抓取平面上方，随机产生各模型的6自由度位姿，用于初始化零件的位态，待拍摄平面为长宽均为256mm的正方形区域，并在四周设置了碰撞挡板；

(2)为仿真场景中模型加入物理引擎，设定对应刚体碰撞参数、动态参数，保留零件在重力作用下，工件做自由落体的最后状态，以模拟工件在平面上的散乱、有堆叠的摆放；

(3)添加传感器模型，设定传感器的分辨率、焦距参数，并加入噪声参数以提高数据集与实际传感器采集数据的相似程度；

(4)保存传感器坐标系下场景内的表面点云，作为数据集样本；保存各零件的6自由度姿态信息与点级语义信息，作为数据集标签；

(5)针对每个场景下的有多个同类工件的点云，依据语义信息，将该类工件分离出来，并将点云模型通过6自由度姿态信息变换到实际位置，使用K近邻点算法，获取变换后的每个点云模型在原场景中临近的一个或多个点，并提取为单个实例，从而获得整个场景点云的准确实例标签；

(6)针对严重遮挡或因碰撞、穿模而无法在场景点云中有效呈现物体的标签，予以去除；严重遮挡判定阈值为所有生成物体表面点云统计均值的30％，以保证识别抓取的可靠性。

Images