CN111738261A - 基于位姿估计和校正的单图像机器人无序目标抓取方法 - Google Patents

Abstract

本发明具体公开了一种基于位姿估计和校正的单图像机器人无序目标抓取方法，所述方法包括：S1、生成待抓取物体模型的图像数据集；S2、根据步骤S1的图像数据集构建卷积神经网络模型；S3、将待抓取物体的二维图像导入训练好的卷积神经网络模型中提取对应的置信图和向量场；S4、获取待抓取物体的预测平移量和预测旋转量；S5、找到待抓取物体最优抓取点并计算深度相机的测量平移量；S6、根据待抓取物体的预测平移量和深度相机的测量平移量进行抓取安全距离校正，若校正成功则执行校正数据抓取，若校正失败则进入S7；S7、重复步骤S3‑S6。本发明中无序目标抓取方法具有可靠性高、鲁棒性强和实时性好的特点，能够满足现有工业生产要求，具有较高的应用价值。

Description

基于位姿估计和校正的单图像机器人无序目标抓取方法

技术领域

本发明涉及智能机器人技术领域，尤其涉及一种基于位姿估计和校正的单图像机器人无序目标抓取方法。

背景技术

6D位姿估计是人工智能应用，如：增强现实、自动驾驶、机器人操纵等所需的关键技术。它可以帮助机器人掌握目标位置和目标方向以对目标进行抓取。例如，在亚马逊拣货挑战赛中，机器人从仓库货架上捡拾目标货物的任务与快速可靠的位姿估计密不可分。

基于现有的研究，6D姿态估计的方法可大致分为基于模板的方法和基于特征的方法。传统的基于模板的方法先构造对象的刚性模板；然后使用模板扫描输入图像中的不同位置，并在每个模板中计算相似度得分位置；最后，通过比较这些相似性得分，获得最佳匹配。基于模板的匹配方法可以检测出无纹理的目标，但是当目标存在遮挡，截断等情况时，相似度分数通常会较低，导致可靠性较低。传统的基于特征的方法依赖于手工制作的局部特征，并且通过2D图像和3D模型之间的对应关系生成目标位姿估计。基于特征的方法可以处理遮挡、截断等问题，但是手工特征需要目标具有丰富的纹理并且对照明和场景混乱不稳健，鲁棒性较差。

尽管最近有几种新技术将深度信息用于物体位姿估计，而且取得了较好的结果，但是存在以下两个问题：第一，训练深度卷积神经网络通常需要大量的标记数据，包括使用精确的6自由度姿势进行注释的目标对象。与2D检测相比，基于卷积神经网络的3D检测禁止手动标记数据，因为无法保证手动标记数据的准确性。因此，可以使用合成数据来训练深度卷积神经网络，虽然合成数据保证了数据的准确性，但是合成数据最主要的一个缺点是现实差距。第二，由于RGB-D相机在帧率，视野，分辨率和深度范围方面存在局限性，这使较小，较薄或快速移动的目标很难被检测到，同时移动设备上的有源传感器会消耗过多功率。目前单目相机的6D姿态估计仍然是一个具有挑战性的问题，目标的表面会受到照明，复杂场景和遮挡等诸多方面的影响。因此，单目RGB图像姿态估计的研究更加有价值和实用。

基于实际物体的目标检测和精细位姿估计是成功抓取的前提。尽管基于关键点的传统方法可以得到精确的位姿估计，但其在机器人任务上的适用性依赖于受控环境和有细节信息的刚性物体；另一方面，基于CNN的方法在不受控环境中的物体识别能得到较好的结果，如基于类别的粗糙的位姿估计，但其需要大量的全标记的训练图像数据集，因此采用CNN方法进行实际物体的位姿估计会有困难。

鉴于此，本发明采用合成数据去训练神经网络，从而避免对特定数据集分布的过度拟合，产生了一个对光照变化、相机变化和背景都具有鲁棒性的网络，具有可靠性高、鲁棒性强且实时性好的优点。

发明内容

本发明的目的是提供一种可靠性高、鲁棒性强且实时性好的基于位姿估计和校正的单图像机器人无序目标抓取方法。

为解决上述技术问题，本发明提供一种基于位姿估计和校正的单图像机器人无序目标抓取方法，所述方法包括以下步骤：

S1、获取待抓取物体模型的随机图像数据和真实感图像数据，并生成对应图像数据集；

S2、构建卷积神经网络，并将步骤S1中所获取的图像数据集输入卷积神经网络中进行离线训练，得到卷积神经网络模型；

S3、通过深度相机采集待抓取物体的二维图像并将该二维图像导入卷积神经网络模型中，输出对应的置信图和向量场；

S4、对步骤S3中所输出的置信图进行实例检测以选择出置信图的有效投射顶点，并将有效投射顶点的单位向量与向量场的单位向量进行比较输出有效的2D投射顶点，然后根据有效的2D投射顶点并利用PnP算法获取待抓取物体的姿态估计值，包括预测平移量μ₁和预测旋转量；

S5、通过步骤S2中卷积神经网络模型获取待抓取物体二维图像的感兴趣区域，并将感兴趣区域与图像模板库进行匹配以找到最优抓取点，然后利用深度相机计算最优抓取点的三维距离，从而得到深度相机的测量平移量μ₂；

S6、根据步骤S4中得到的预测平移量μ₁和步骤S5中得到的测量平移量μ₂进行抓取安全距离校正，若校正成功则通过机械臂执行校正数据抓取，若校正失败则进入步骤S7；

S7、重复步骤S3-步骤S6。

优选地，所述步骤S1的具体实现方式包括以下步骤：

S11、制作一个与待抓取物体的物理尺寸及表面纹理均一致的模型；

S12、将接近抓取环境的实验室抓取背景和贴近真实物体场景的户外场景进行1:1结合并与随机背景一起作为虚拟环境的随机图像场景，然后任意选取一组3D模型对待抓取物体模型进行随机遮挡，通过不断调整待抓取物体模型与深度相机之间的距离和角度，获取待抓取物体模型的随机图像数据；

S13、将待抓取物体模型置于具有物理约束的3D背景中获取待抓取物体模型的真实感图像数据；

S14、将步骤S12中获取的随机图像数据与步骤S13中获取的真实感图像数据合并后作为待抓取物体模型的图像数据集。

优选地，所述步骤S2的具体实现方法包括以下步骤：

S21、构建一个具有十二个阶段的卷积神经网络结构，其中前四个阶段为特征提取网络，中间两个阶段为降维处理网络，后六个阶段为任务分支网络；

S22、将图像数据集中待训练的图像数据输入前四个阶段的特征提取网络中进行特征提取，其中前一个特征提取网络的输出为后一个特征提取网络的输入；

S23、将最后一个特征提取网络的输出通过两个降维处理网络中进行降维处理，其中第一个降维处理网络的输出为第二降维处理网络的输入；

S24、将第二个降维处理网络输出的特征输入六个阶段的任务分支网络中，其中前一个任务分支网络的输出为后一个任务分支网络的输入，从而提取出对应图像数据的包括八个投射顶点和一个质心点的置信图以及从八个投射顶点分别到对应质心点方向的向量场，得到卷积神经网络模型。

优选地，所述步骤S4的具体实现方式包括以下步骤：

S41、分别将所述置信图检测到的八个投射顶点与groundtruth进行计算，获取符合设定正态分布的置信度值，其计算公式可表示为：

式(1)中，Confidence(P_k)表示置信度，x_m1,m2表示数据的标签坐标，P_k表示置信图中检测到的第k个投射顶点坐标，k表示投射顶点的序号，k＝1,2,…8，σ²表示设定正态分布的方差；

S42、设定置信度阈值a，将步骤S41中所获得每个投射顶点的置信度值Confidence(P_k)与设定的置信阈值a比较，若Confidence(P_k)≥a，则保留该投射顶点，若Confidence(P_k)＜a，则舍去该投射顶点；

S43、计算

的正切值

将

与设定误差角阈值b比较以判定步骤S42所保留的投射顶点与质心点是否相关联，若

时，则判定该投射顶点与质心点相关联，若

则判定该投射顶点不与质心点相关联，且需要保证同一个待抓取物体至少有4个投射顶点与质心点相关联，其中

的计算公式可表示为：

式(2)中，

表示两个向量之间的误差，

表示从包围框的第k个投射顶点指向质心点的向量，

表示第k个投射顶点对应的向量场；

S44、根据步骤S43得到的有效投射顶点并利用PnP算法获取待抓取物体的姿态估计值，即预测平移量μ₁和预测旋转量。

优选地，所述步骤S44的具体实现方式包括：

S441、将步骤S43中得到的有效投射顶点在世界坐标系中对应点设为空间参考点P_i ^w＝(x_i,y_i,z_i)，其中i＝1,2,…n，4≤n≤8，i表示有效投射顶点的序号，n表示空间参考点个数，然后计算出空间参考点P_i ^w对应向量α_ij，可用公式表示为：

在世界坐标系中，空间参考点可以由4个虚拟控制点表达，利用PnP算法可知，当满足

时，可将空间参考点坐标表示为虚拟控制点的加权和，则式(3)可表示为：

其中，

表示虚拟控制点在世界坐标系中的坐标，j表示虚拟控制点序号(j＝1,2,3,4)，若

表示空间参考点的重心，则

λ_c,j-1表示矩阵A^TA的特征值，ν′_c,j-1表示矩阵A^TA的特征向量，其中矩阵

S442、计算空间参考点在相机坐标系中的坐标

根据图像坐标系与相机坐标系的关系可得：

式(4)和式(5)中，ω_i表示相机捕捉图像时相机光圈到2D坐标点之间距离与相机光圈到3D坐标点之间距离的比值，K表示相机内参矩阵，(u_i,v_i)为空间参考点P_i ^w在图像中的2D投影点，

表示相机坐标系中的虚拟控制点，

则式(5)可表示为：

式(6)中，f_u、f_v、u_c、v_c表示相机内参；

由式(6)可得：

将n个空间参考点P_i ^w串起来可得：

Ml＝0 (8)

式(8)中，M表示矩阵系数，l表示待求参数，即四个虚拟控制点

在相机坐标系中的坐标，且

则式(8)可表示为：

式(9)中，ν″_h表示矩阵M^TM对应零特征值的第h个特征向量，h表示矩阵M^TM对应零特征值特征向量的序号，N表示矩阵M^TM对应零特征值的特征向量的数量，β_h表示待定系数，满足

其中，j₁≠j₂且j₁∈j,j₂∈j，

表示相机坐标系与世界坐标系中两个虚拟控制点之间的距离相等；

由(9)可得相机坐标系中的虚拟控制点

的计算公式：

式(10)中，

表示第j个虚拟控制点所占据的3个元素组成的向量；

S443、计算世界坐标系的质心点

相关矩阵A、相机坐标系的质心点

和相关矩阵B，可用公式表示：

S444、根据所述步骤S443得到的相关矩阵A和相关矩阵B，计算矩阵H并对矩阵H进行奇异值分解，可用公式表示为：

H＝B^TA (15)

H＝UCV^T (16)

式(16)中，U表示一个n×n的正交矩阵，C表示一个n×3的矩阵，V表示一个3×3的正交矩阵；

S445、根据步骤S444中的矩阵H奇异值分解结果计算旋转矩阵R，并通过旋转矩阵R计算出待抓取物体位姿的平移量T，然后利用高斯-牛顿最优化寻找最优解，从而获取待抓取物体的姿态估计值(X_t,Y_t,Z_t,X_r,Y_r,Z_r,W_r)，即预测平移量μ₁(X_t,Y_t,Z_t)和预测旋转量(X_r,Y_r,Z_r,W_r)，可用公式表示：

R＝UV^T (17)

优选地，所述步骤S5中图像模板库的构建包括以下步骤：

S501、获取最优抓取点处任意旋转方向上的待抓取物体图像；

S502、获取最优抓取点处不平衡光照条件下的待抓取物体图像；

S503、获取最优抓取点处不同缩放比例下的待抓取物体图像；

S504、将步骤S501、步骤S502和步骤S503中所获取的最优抓取点处所有待抓取物体图像保存至模板库中，从而完成图像模板库的构建。

优选地，所述步骤S5的具体实现方式包括以下步骤：

S51、通过所述卷积神经网络模型获取待抓取物体二维图像的感兴趣区域，并将待抓取物体感兴趣区域与图像模板库进行标准相关匹配，可用公式表示为：

式(20)中，T′表示图像模板库中模板图像，I′表示待抓取物体图像，R(x,y)表示待抓取物体图像与图像模板库中模板图像之间的匹配程度，(x′,y′)表示图像模板库中模板图像像素点的坐标，T′(x′,y′)表示模板图像在坐标(x′,y′)处的像素值，(x+x′,y+y′)表示待抓取物体图像像素点的坐标，I′(x+x′,y+y′)表示待抓取物体图像在坐标(x+x′,y+y′)处的像素值；

S52、采用矩形框将步骤S51中匹配成功的图像框起来，然后计算该矩形框的质心点，从而找到待抓取物体的最优抓取点及最优抓取点的像素坐标；

S53、将待抓取物体的二维图像与深度相机图像进行匹配，并利用二维图像中最优抓取点像素找到对应深度相机图像的测量距离，从而得到相机的测量平移量μ₂。

优选地，所述步骤S6中抓取安全距离校正的具体实现方式包括以下步骤：

S61、设定误差阈值Q₁与Q₂，且Q₁＜Q₂，同时保证最大误差阈值小于物体平面的直径；

S62、通过预测平移量μ₁和测量平移量μ₂计算平均距离D，可用公式表示为：

式(21)中，x₁,y₁,z₁表示预测平移量μ₁的坐标值，x₂,y₂,z₂表示测量平移量μ₂的坐标值；

S63、根据平均距离D与误差阈值的大小判断是否需要进行抓取安全距离校正，当D＜Q₁时，则判定姿态估计值较准确，进入步骤S64，当Q₁＜D＜Q₂时，则判定姿态估计值存在不构成抓取失败的少许偏差，进入步骤S65；当D＞Q₂时，则判定姿态估计值存在严重偏差，进入步骤S66；

S64、将预测平移量μ₁和预测旋转量输入机械臂中，从而实现待抓取物体的精准抓取；

S65、分别计算预测平移量μ₁与测量平移量μ₂在三个方向上的平均值，获取待抓取物体相对于深度相机的校正平移量μ′，然后将所获取的校正平移量μ′和预测旋转量输入机械臂中，实现待抓取物体的精准抓取，其中校正平移量μ′的计算公式可表示为：

式(22)中，x″,y″,z″表示校正平移量μ′的坐标；

S66、返回步骤S3。

优选地，所述步骤S61中误差阈值Q₁＝0.02m，Q₂＝0.04m。

优选地，所述机械臂实现待抓取物体精准抓取的具体实现方式为：将抓取安全距离校正后的平移量和旋转量与相机内外参结合，计算出机器人的抓取控制量，即抓取平移量和抓取旋转量，然后将抓取控制量输入机械臂中实现待抓取物体的精准抓取，抓取控制量的计算公式可表示为：

式(23)中，

表示抓取平移量，

表示抓取旋转量，

表示世界坐标系与机器人基坐标系的转换关系，其中：

r₁表示X_W轴的旋转矩阵，

θ₁表示机器人基坐标系绕X_W轴旋转的角度，r₂表示Y_W轴的旋转矩阵，

θ₂表示机器人基坐标系绕Y_W轴旋转的角度，r₃表示Z_W轴的旋转矩阵，

θ₃表示机器人基坐标系绕Z_W轴旋转的角度；

表示世界坐标系与相机坐标系之间的转换关系，

表示相机坐标系与机械手坐标系之间的转换关系，

表示机械手坐标系与机器人基坐标系之间的转换关系，

和

可通过机器人标定得到，

表示姿态估计中的旋转矩阵，

(X_r,Y_r,Z_r,W_r)表示预测旋转量，

表示姿态估计中的平移向量，

X,Y,Z表示进行抓取安全校正后的平移量。

与现有技术比较，本发明通过采用合成数据训练卷积神经网络，从而避免对特定数据集分布的过度拟合，能够产生了一个对光照变化、相机变化和背景都具有鲁棒性的网络，具有可靠性高、鲁棒性强且实时性好的优点。

附图说明

图1是本发明一种基于位姿估计和校正的单图像机器人无序目标抓取方法流程图，

图2是本发明中获取待抓取物体模型图像数据集的方法流程图，

图3是本发明中获取卷积神经网络模型的方法流程图，

图4是本发明中置信图和向量场后期处理并对置信图中顶点进行选择分配的流程图，

图5是本发明中获取待抓取物体姿态估计值的方法流程图，

图6是本发明中图像模板库的构建流程图，

图7是本发明中获取深度相机测量平移量的方法流程图，

图8是本发明中通过抓取安全距离校正实现待抓取物体精准抓取的方法流程图，

图9是本发明中抓取安全距离校正算法示意图，

图10是本发明中机械臂抓取过程示意图。

具体实施方式

为了使本技术领域的人员更好地理解本发明的技术方案，下面结合附图对本发明作进一步的详细说明。

如图1所示，一种基于位姿估计和校正的单图像机器人无序目标抓取方法，所述方法包括以下步骤：

S1、获取待抓取物体模型的随机图像数据和真实感图像数据，并生成对应图像数据集；

S2、构建卷积神经网络，并将步骤S1中所获取的图像数据集输入卷积神经网络中进行离线训练，得到卷积神经网络模型；

S3、通过深度相机采集待抓取物体的二维图像并将该二维图像导入卷积神经网络模型中，输出对应的置信图和向量场；

S4、对步骤S3中所输出的置信图进行实例检测以选择出置信图的有效投射顶点，并将有效投射顶点的单位向量与向量场的单位向量进行比较输出有效的2D投射顶点，然后根据有效的2D投射顶点并利用PnP算法获取待抓取物体的姿态估计值，包括预测平移量μ₁和预测旋转量；

S5、通过步骤S2中卷积神经网络模型获取待抓取物体二维图像的感兴趣区域，并将感兴趣区域与图像模板库进行匹配以找到最优抓取点，然后利用深度相机计算最优抓取点的三维距离，从而得到深度相机的测量平移量μ₂；

S6、根据步骤S4中得到的预测平移量μ₁和步骤S5中得到的测量平移量μ₂进行抓取安全距离校正，若校正成功则通过机械臂执行校正数据抓取，若校正失败则进入步骤S7；

S7、重复步骤S3-步骤S6。

本实施例中，为了实现机械臂对待抓取物体的精准抓取，通过采用合成数据去训练神经网络，有效避免了对特定数据集分布的过度拟合，进而产生一个对光照变化、相机变化和背景都具有鲁棒性的网络，然后利用卷积神经网络对待抓取物体的二维图像进行处理以输出待抓取物体的姿态估计值，同时将待抓取物体的二维图像与图像模板库进行匹配获取深度相机的测量平移量，再对预测平移量和测量平移量进行抓取安全距离校正，最后根据校正数据引导机械臂对待抓取物体进行精准抓取，具有可靠性高、鲁棒性强且实时性好的优点。在其他实施例中，也可以采用其他图像获取装置来替代深度相机。其中，所述PnP(Perspective-n-Point)算法是求解3D到2D点对运动的方法。

如图2所示，所述步骤S1的具体实现方式包括以下步骤：

S11、制作一个与待抓取物体的物理尺寸及表面纹理均一致的模型；

S12、将接近抓取环境的实验室抓取背景和贴近真实物体场景的户外场景进行1:1结合并与随机背景一起作为虚拟环境的随机图像场景，然后任意选取一组3D模型对待抓取物体模型进行随机遮挡，通过不断调整待抓取物体模型与深度相机之间的距离和角度，获取待抓取物体模型的随机图像数据；

S13、将待抓取物体模型置于具有物理约束的3D背景中获取待抓取物体模型的真实感图像数据；

S14、将步骤S12中获取的随机图像数据与步骤S13中获取的真实感图像数据合并后作为待抓取物体模型的图像数据集。

本实施例中，所述3D模型包括圆柱体、球体、圆锥等模型，3D背景包括房间、森林和工厂。本实施例主要针对三种不同物体(肥皂、钙片和卫生纸)进行模型制作与数据合成，其中肥皂的尺寸x，y，z分别为：3.2cm，9.4cm和5.8cm，钙片的尺寸x，y，z分别为：4.5cm，4.5cm和8.7cm，卫生纸的尺寸x，y，z分别为10cm，13.5cm和7cm，并将其输入Blender中制作，得到与实际物体尺寸一致的物理模型。通过从复杂背景、遮挡、光照条件、有效抓取距离四个方面考虑将制作数据分为两部分，第一部分是由不同条件进行随机组合的随机化数据；第二部分是放置在真实感场景下的数据。

其中，随机化数据产生过程如下：将接近抓取环境的实验室抓取背景与贴近真实物体场景的户外场景进行1:1结合，作为虚拟环境的随机图像场景；干扰器的数量和类型在一组3D模型(圆柱体、球体、圆锥等)中随机选取，这些3D模型会随机遮挡物体，以保证在这种条件下训练出的神经网络这样可以有效解决抓取中的遮挡问题；在模型与相机之间的距离和角度是在一定范围内不断调整的，因为深度相机捕捉到的深度信息的精度受距离本身的影响，当相机距离目标太近或者太远时，得到的数据不够准确，会影响抓取的成功率。因此，本实施例中将模型与相机之间的范围设定在0.4m-0.8m之间。随机化的优点是不在一个模拟数据集上训练模型，而是以随机的组合方式模拟数据合成，将模型暴露于各种各样的排列数据，并且可以得到完美标记的数据，以便应用于真实世界的图像。

真实感数据生成过程如下：从标准UE4虚拟环境中选择房间、森林和工厂三种3D背景，在这三种背景中获取各种光照条件下的真实感数据，房间背景整体偏暗，用来获取极弱光照条件下的真实感图像；深林背景明暗相间(亮主要来自于阳光，暗主要来自于外界物体的阴影)，此背景具有河流、大树等户外常见物体，可以模拟户外场景，会生成在外界物体阴影下的真实感图像；工厂背景整体偏亮，用来获取强光照条件下的真实感图像。在制作数据集时，会在每个背景中随机选取不同位置进行真实感图像采集。

如图3所示，所述步骤S2的具体实现方法包括以下步骤：

S21、构建一个具有十二个阶段的卷积神经网络结构，其中前四个阶段为特征提取网络，中间两个阶段为降维处理网络，后六个阶段为任务分支网络；

S22、将图像数据集中待训练的图像数据输入前四个阶段的特征提取网络中进行特征提取，其中前一个特征提取网络的输出为后一个特征提取网络的输入；

S23、将最后一个特征提取网络的输出通过两个降维处理网络中进行降维处理，其中第一个降维处理网络的输出为第二降维处理网络的输入；

S24、将第二个降维处理网络输出的特征输入六个阶段的任务分支网络中，其中前一个任务分支网络的输出为后一个任务分支网络的输入，从而提取出对应图像数据的包括八个投射顶点和一个质心点的置信图以及从八个投射顶点分别到对应质心点方向的向量场，得到卷积神经网络模型。

本实施例中，所述卷积神经网络的构建步骤如下：整个卷积神经网络结构一共有十二个阶段，前四个阶段属于特征提取网络，中间两阶段用来进行降维处理，后六个阶段属于任务分支网络，特征提取网络用于提取特征，之后两个阶段进行降维处理，以降低模型的复杂度，提高训练效率，同时减少过拟合程度，提高模型的鲁棒性和泛化性，然后将特征输入到任务分支网络分别提取置信图和向量场，最终输出为3D包围框的8个投射顶点和一个质心点，以及从8个投射顶点分别到对应的质心点的方向。任务分支网络每个阶段都使用图像特征和前一阶段的输出作为输入。得到一个越来越到的有效接收域，使网络能够通过在后期合并越来越多的上下文，解决在早期阶段由于接受域小而导致的歧义。

其中，特征提取具体过程包括：

第一次特征提取阶段：按照网络设计顺序，需要训练的图像依次经过：输入层-卷积层-Relu层-卷积层-Relu层；输入层的输入大小为400×400×3；卷积层的卷积核大小设置为3×3，卷积核的数目设置为64，步长设置为1，padding(像素填充)设置为1。第一次特征提取阶段的输出为400×400×64，然后进入第二次特征提取阶段；

第二次特征提取阶段：按照网络设计顺序，需要训练的图像依次经过：最大池化层-卷积层-Relu层-卷积层-Relu层；第二阶段的输入为第一阶段的输出；卷积层的卷积核大小设置为3×3，卷积核的数目设置为128，步长设置为1，padding设置为1；最大池化层大小设置为2×2，步长设置为2。第二次特征提取阶段的输出为200×200×128，然后进入第三次特征提取阶段；

第三次特征提取阶段：按照网络设计顺序，需要训练的图像依次经过：最大池化层-卷积层-Relu层-卷积层-Relu层-卷积层-Relu层-卷积层-Relu层；第三阶段的输入为第二阶段的输出；卷积层的卷积核大小设置为3×3，卷积核的数目设置为256，步长设置为1，padding设置为1；最大池化层大小设置为2×2，步长设置为2。第三次特征提取阶段的输出为100×100×256，然后进入第四次特征提取阶段。

第四次特征提取阶段：按照网络设计顺序，需要训练的图像依次经过：最大池化层-卷积层-Relu层-卷积层-Relu层；第四阶段的输入为第三阶段的输出；卷积层的卷积核大小设置为3×3，卷积核的数目设置为512，步长设置为1，padding设置为1；最大池化层大小设置为2×2，步长设置为2。第四次特征提取阶段的输出为50×50×512，然后进入第一次降维阶段。

降维处理具体过程包括：

第一次降维处理阶段：按照网络设计顺序，需要训练的图像依次经过：卷积层-Relu层；第一次降维处理阶段的输入为第四次特征提取阶段的输出；卷积层的卷积核大小设置为3×3，卷积核的数目设置为256，步长设置为1，padding设置为1；第一次降维处理阶段的输出为50×50×256，然后进入第二次降维阶段；

第二次降维处理阶段：按照网络设计顺序，需要训练的图像依次经过：卷积层-Relu层；第二次降维处理阶段的输入为第一次降维处理阶段的输出；卷积层的卷积核大小设置为3×3，卷积核的数目设置为128，步长设置为1，padding设置为1；第二次降维处理阶段的输出为50×50×128，即为所得特征图。

从任务分支网络的六个阶段中分别提取置信图和向量场的过程包括：

任务分支阶段一：按照网络设计顺序，需要训练的图像同时进入向量通道与分类通道，分别依次经过：卷积层-Relu层-卷积层-Relu层-卷积层-Relu层-卷积层-Relu层，任务分支阶段一的输入为第二次降维处理阶段的输出，阶段一通过分支产生两个不同输出，分别为置信图和向量场。置信图的输出大小为50×50×9，它包括了3D包围框的8个投射顶点和一个质心点，向量场的输出大小为50×50×16，它表明了从8个顶点分别到对应的质心点的方向，然后将这两个分支的输出与第二次降维处理阶段的输出进行合并，将其维度相加，任务分支阶段一的输出为50×50×153，然后进入任务分支阶段二；

任务分支阶段二：按照网络设计顺序，需要训练的图像同时进入向量通道与分类通道，分别依次经过：卷积层-Relu层-卷积层-Relu层-卷积层-Relu层-卷积层-Relu层-卷积层-Relu层-卷积层-Relu层，任务分支阶段二的输入为任务分支阶段一的输出，阶段二通过分支产生两个不同输出，分别为置信图和向量场，置信图的输出大小为50×50×9，它包括了3D包围框的8个投射顶点和一个质心点，向量场的输出大小为50×50×16，它表明了从8个顶点分别到对应的质心点的方向，然后将这两个分支的输出与第二次降维处理阶段的输出进行合并，将其维度相加，任务分支阶段二的输出为50×50×153，然后进入任务分支阶段三；

任务分支阶段三：按照网络设计顺序，需要训练的图像同时进入向量通道与分类通道，分别依次经过：卷积层-Relu层-卷积层-Relu层-卷积层-Relu层-卷积层-Relu层-卷积层-Relu层-卷积层-Relu层，任务分支阶段三的输入为任务分支阶段二的输出，阶段三通过分支产生两个不同输出，分别为置信图和向量场。置信图的输出大小为50×50×9，它包括了3D包围框的8个投射顶点和一个质心点，向量场的输出大小为50×50×16，它表明了从8个顶点分别到对应的质心点的方向，然后将这两个分支的输出与第二次降维处理阶段的输出进行合并，将其维度相加，任务分支阶段三的输出为50×50×153，然后进入任务分支阶段四；

任务分支阶段四：按照网络设计顺序，需要训练的图像同时进入向量通道与分类通道，分别依次经过：卷积层-Relu层-卷积层-Relu层-卷积层-Relu层-卷积层-Relu层-卷积层-Relu层-卷积层-Relu层，任务分支阶段四的输入为任务分支阶段三的输出，阶段四通过分支产生两个不同输出，分别为置信图和向量场，置信图的输出大小为50×50×9，它包括了3D包围框的8个投射顶点和一个质心点，向量场的输出大小为50×50×16，它表明了从8个顶点分别到对应的质心点的方向，然后将这两个分支的输出与第二次降维处理阶段的输出进行合并，将其维度相加，任务分支阶段四的输出为50×50×153，然后进入任务分支阶段五；

任务分支阶段五：按照网络设计顺序，需要训练的图像同时进入向量通道与分类通道，分别依次经过：卷积层-Relu层-卷积层-Relu层-卷积层-Relu层-卷积层-Relu层-卷积层-Relu层-卷积层-Relu层，任务分支阶段五的输入为任务分支阶段四的输出，阶段五通过分支产生两个不同输出，分别为置信图和向量场，置信图的输出大小为50×50×9，它包括了3D包围框的8个投射顶点和一个质心点，向量场的输出大小为50×50×16，它表明了从8个顶点分别到对应的质心点的方向，然后将这两个分支的输出与第二次降维处理阶段的输出进行合并，将其维度相加，任务分支阶段五的输出为50×50×153，然后进入任务分支阶段六；

任务分支阶段六：按照网络设计顺序，需要训练的图像同时进入向量通道与分类通道，分别依次经过：卷积层-Relu层-卷积层-Relu层-卷积层-Relu层-卷积层-Relu层-卷积层-Relu层-卷积层-Relu层，任务分支阶段六的输入为任务分支阶段五的输出，阶段五通过分支产生两个不同输出，分别为置信图和向量场，置信图的输出大小为50×50×9，它包括了3D包围框的8个投射顶点和一个质心点，向量场的输出大小为50×50×16，它表明了从8个投射顶点分别到对应的质心点的方向。

如图4所示，所述步骤S4的具体实现方式包括以下步骤：

S41、分别将所述置信图检测到的八个顶点与groundtruth(参考标准，即数据的标签坐标)进行计算，获取符合设定正态分布的置信度值，其计算公式可表示为：

式(1)中，Confidence(P_k)表示置信度，x_m1,m2表示数据的标签坐标，P_k表示置信图中检测到的第k个投射顶点坐标，k表示投射顶点的序号，k＝1,2,…8，σ²表示设定正态分布的方差；其中，在进行数据合成过程中会生成描述图片信息的标签文件，每张图像均有与其对应的标签文件，其里面包含了很多该图片的信息(如坐标值，图像名等)，所述数据的标签坐标指的是标签文件中所包含的坐标值；

S42、设定置信度阈值a，将步骤S41中所获得每个投射顶点的置信度值Confidence(P_k)与设定的置信阈值a比较，若Confidence(P_k)≥a，则保留该投射顶点，若Confidence(P_k)＜a，则舍去该投射顶点，其中置信度阈值a是一个超参数，可根据实际情况进行相应调整，以得到最优的结果，当某一投射顶点置信度越大，则表示该投射顶点的可靠性就越高，从而通过设定置信图阈值可以舍去那些不可靠的置信图投射顶点，有效避免了不可靠的置信图投射顶点对最后结果所造成较大的误差；

S43、计算

的正切值

将

与设定误差角阈值b比较以判定步骤S42所保留的投射顶点与质心点是否相关联，若

时，则判定该投射顶点与质心点相关联，若

则判定该投射顶点不与质心点相关联，且需要保证同一个待抓取物体至少有4个投射顶点与质心点相关联，其中

的计算公式可表示为：

式(2)中，

表示两个向量之间的误差，

表示从包围框的第k个投射顶点指向质心点的向量，

表示第k个投射顶点对应的向量场；

S44、根据步骤S43得到的有效投射顶点利用PnP算法获取待抓取物体的姿态估计值，即预测平移量μ₁和预测旋转量。

如图5所示，所述步骤S44的具体实现方式包括：

S441、将步骤S43中得到的有效投射顶点在世界坐标系中对应点设为空间参考点P_i ^w＝(x_i,y_i,z_i)，其中i＝1,2,…n，4≤n≤8，i表示有效投射顶点的序号，n表示空间参考点个数，然后计算出空间参考点P_i ^w对应向量α_ij，可用公式表示为：

在世界坐标系中，空间参考点可以由4个虚拟控制点表达，利用PnP算法可知，当满足

时，可将空间参考点坐标表示为虚拟控制点的加权和，则式(3)可表示为：

其中，

表示虚拟控制点在世界坐标系中的坐标，j表示虚拟控制点序号(j＝1,2,3,4)，若

表示空间参考点的重心，则

λ_c,j-1表示矩阵A^TA的特征值，ν′_c,j-1表示矩阵A^TA的特征向量，其中矩阵

从而可根据空间参考点P_i ^w求出虚拟控制点在世界坐标系中的坐标

再代入式(3)即可计算出空间参考点P_i ^w对应向量α_ij；

S442、计算空间参考点P_i ^w在相机坐标系中的坐标P_i ^c，根据图像坐标系与相机坐标系的关系可得：

式(4)和式(5)中，ω_i表示相机捕捉图像时相机光圈到2D坐标点之间距离与相机光圈到3D坐标点之间距离的比值，K表示相机内参矩阵，(u_i,v_i)为空间参考点P_i ^w在图像中的2D投影点，

表示相机坐标系中的虚拟控制点，

则式(5)可表示为：

式(6)中，f_u、f_v、u_c、v_c表示相机内参，由式(6)可分别求出4个虚拟控制点

在相机坐标系中的坐标

由式(6)可得：

将n个空间参考点P_i ^w串起来可得：

Ml＝0 (8)

式(8)中，M表示矩阵系数，l表示待求参数，即四个虚拟控制点

在相机坐标系中的坐标，且

则式(8)可表示为：

式(9)中，ν″_h表示矩阵M^TM对应零特征值的第h个特征向量，h表示矩阵M^TM对应零特征值特征向量的序号，N表示矩阵M^TM对应零特征值的特征向量的数量，β_h表示待定系数，满足

其中，j₁≠j₂且j₁∈j,j₂∈j，

表示相机坐标系与世界坐标系中两个虚拟控制点之间的距离相等；

由(9)可得相机坐标系中的虚拟控制点

的计算公式：

式(10)中，

表示第j个虚拟控制点所占据的3个元素组成的向量；

S443、计算世界坐标系的质心点

相关矩阵A、相机坐标系的质心点

和相关矩阵B，可用公式表示：

S444、根据所述步骤S443得到的相关矩阵A和相关矩阵B，计算矩阵H并对矩阵H进行奇异值分解，可用公式表示为：

H＝B^TA (15)

H＝UCV^T (16)

式(16)中，U表示一个n×n的正交矩阵，矩阵U中的向量称为左奇异向量，C表示一个n×3的矩阵，矩阵C中除了主对角线上以外的元素全为0，且主对角线上的元素称为奇异值，V表示一个3×3的正交矩阵，正交矩阵V中的向量称为右奇异向量；

S445、根据步骤S444中的矩阵H奇异值分解结果计算旋转矩阵R，并通过旋转矩阵R计算出待抓取物体位姿的平移量T，然后利用高斯-牛顿最优化寻找最优解，从而获取待抓取物体的姿态估计值(X_t,Y_t,Z_t,X_r,Y_r,Z_r,W_r)，即预测平移量μ₁(X_t,Y_t,Z_t)和预测旋转量(X_r,Y_r,Z_r,W_r)，可用公式表示：

R＝UV^T (17)

本实施例中，首先计算卷积神经网络模型输出的置信图中顶点的置信度值，并通过设定置信度阈值舍去可靠性不高的置信图投射顶点，以避免最后结果出现较大误差，然后将得到的有效投射顶点的单位向量与卷积神经网络模型输出的向量场的单位向量进行比较，进而输出有效的2D投射顶点，最后根据输出的有效2D投射顶点并利用PnP算法计算出待抓取物体的姿态估计值。

如图6所示，所述步骤S5中图像模板库的构建包括以下步骤：

S501、获取最优抓取点处任意旋转方向上的待抓取物体图像；

S502、获取最优抓取点处不平衡光照条件下的待抓取物体图像；

S503、获取最优抓取点处不同缩放比例下的待抓取物体图像；

S504、将步骤S501、步骤S502和步骤S503中所获取的最优抓取点处所有待抓取物体图像保存至模板库中，从而完成图像模板库的构建。

本实施例中，所述图像模板库的构建过程中，任意旋转方向目的是保证物体的任意旋转角度依然可以识别；不平衡光照目的是解决物体表面反光、暗光和不平衡光照问题，去除对识别产生的干扰影响；尺度缩放的目的是保证在机械臂的有效抓取距离浮动下，依然保证待抓取图像的识别。

其中为了减少计算量，通过图像采集获取一系列不同旋转角度的图像，相比逐度旋转搜索方式，改进方式速度提升；

不平衡光照采取的措施是：针对模板的图像亮度设置了3个梯度，分别是光照降低30％，光照提升20％和50％，同时针对获取图像本来会将亮度降低10％，为了降低反光的严重影响，破坏有效识别；

尺度缩放采取的措施是：设置了两个尺度分别是大小缩小是原来的0.8和扩大是原来的1.2，不能设置过多或者设置更多尺度，因为会导致计算加大和图像缩放的严重变形。

如图7所示，所述步骤S5的具体实现方式包括以下步骤：

S51、通过所述卷积神经网络模型获取待抓取物体二维图像的感兴趣区域，并将待抓取物体感兴趣区域与图像模板库进行标准相关匹配，可用公式表示为：

式(20)中，T′表示图像模板库中模板图像，I′表示待抓取物体图像，R(x,y)表示待抓取物体图像与图像模板库中模板图像之间的匹配程度，(x′,y′)表示图像模板库中模板图像像素点的坐标，T′(x′,y′)表示模板图像在坐标(x′,y′)处的像素值，(x+x′,y+y′)表示待抓取物体图像像素点的坐标，I′(x+x′,y+y′)表示待抓取物体图像在坐标(x+x′,y+y′)处的像素值；本实施例中，当模板图像刚开始滑动时，模板图像位于待抓取物体图像的左上角，此时x＝0,y＝0，通过模板图像逐个像素匹配，即先从像素坐标x′＝1,y′＝1开始，直至完成所有相应位置匹配程度值R的计算，得到可以代表待抓取图像匹配区域1的匹配程度值，然后在待匹配图像上横向滑动一个像素(即x+1)，按照上述过程同样进行逐模板匹配，得到可以代表待抓取图像匹配区域2的匹配程度值，然后继续滑动，计算下一区域的匹配程度，滑动完整行后再换至下一行(即y+1,)，依旧从左至右滑动，直至完成所有位置的滑动及其相应区域匹配程度值R的计算。

S52、采用矩形框将步骤S51中匹配成功的图像框起来，然后计算该矩形框的质心点，从而找到待抓取物体的最优抓取点及最优抓取点的像素坐标；

S53、将待抓取物体的二维图像与深度相机图像进行匹配，并利用二维图像中最优抓取点的像素找到对应深度相机图像的测量距离，从而得到相机的测量平移量μ₂。

本实施例中，将待抓取物体感兴趣区域与图像模板库进行标准相关匹配是通过将模板图像在待匹配的待抓取物体图像(即感兴趣区域)逐个像素滑动，同时计算滑动过程中每一个匹配区域的匹配程度，最终找到模板图像的最佳匹配区域。当R＝1表示完美匹配，-1表示糟糕匹配，0表示表示没有任何相关性。

如图8所示，所述步骤S6中抓取安全距离校正的具体实现方式包括以下步骤：

S61、设定误差阈值Q₁与Q₂，且Q₁＜Q₂，同时保证最大误差阈值小于物体平面的直径；

S62、通过预测平移量μ₁和测量平移量μ₂计算平均距离D，可用公式表示为：

式(21)中，x₁,y₁,z₁表示预测平移量μ₁的坐标值，x₂,y₂,z₂表示测量平移量μ₂的坐标值；

S63、根据平均距离与误差阈值的大小判断是否需要进行抓取安全距离校正，当D＜Q₁时，则判定姿态估计值较准确，进入步骤S64，当Q₁＜D＜Q₂时，则判定姿态估计值存在不构成抓取失败的少许偏差，进入步骤S65；当D＞Q₂时，则判定姿态估计值存在严重偏差，进入步骤S66；

S64、将预测平移量μ₁和预测旋转量输入机械臂中，从而实现待抓取物体的精准抓取；

S65、分别计算预测平移量μ₁与测量平移量μ₂在三个方向上的平均值，获取待抓取物体相对于深度相机的校正平移量μ′，然后将所获取的校正平移量μ′和预测旋转量输入机械臂中，实现待抓取物体的精准抓取，其中校正平移量μ′的计算公式可表示为：

式(22)中，x″,y″,z″表示校正平移量μ′的坐标；

S66、返回步骤S3。

其中，所述步骤S61中误差阈值Q₁＝0.02m，Q₂＝0.04m。

其中，所述机械臂实现待抓取物体精准抓取的具体实现方式为：将抓取安全距离校正后的平移量和旋转量与相机内外参结合，计算出机器人的抓取控制量，即抓取平移量和抓取旋转量，然后将抓取控制量输入机械臂中实现待抓取物体的精准抓取，抓取控制量的计算公式可表示为：

式(23)中，

表示抓取平移量，

表示抓取旋转量，

表示世界坐标系与机器人基坐标系的转换关系，其中：

表示机器人基坐标系O_W-X_WY_WZ_W中的X_W，Y_W和Z_W三轴旋转效果的和，r₁表示X_W轴的旋转矩阵，

θ₁表示机器人基坐标系绕X_W轴旋转的角度，r₂表示Y_W轴的旋转矩阵，

θ₂表示机器人基坐标系绕Y_W轴旋转的角度，r₃表示Z_W轴的旋转矩阵，

θ₃表示机器人基坐标系绕Z_W轴旋转的角度；

表示世界坐标系与相机坐标系之间的转换关系，

表示相机坐标系与机械手坐标系之间的转换关系，

表示机械手坐标系与机器人基坐标系之间的转换关系，

和

可通过机器人标定得到，

表示姿态估计中的旋转矩阵，

(X_r,Y_r,Z_r,W_r)表示预测旋转量，

表示姿态估计中的平移向量，

X,Y,Z表示进行抓取安全校正后的平移量。

本实施例中，通过对预测平移量μ₁和测量平移量μ₂进行抓取安全距离校正后获取机械臂抓取待抓取物体的平移量和旋转量，然后结合相机内外参计算出机械臂的抓取控制量，即抓取平移量

和抓取旋转量

从而有效保证机械臂对待抓取物体的精准抓取。

以上对本发明所提供的一种基于位姿估计和校正的单图像机器人无序目标抓取方法进行了详细介绍。本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。

Claims (10)

1.基于位姿估计和校正的单图像机器人无序目标抓取方法，其特征在于，所述方法包括以下步骤：

S1、获取待抓取物体模型的随机图像数据和真实感图像数据，并生成对应图像数据集；

S2、构建卷积神经网络，并将步骤S1中所获取的图像数据集输入卷积神经网络中进行离线训练，得到卷积神经网络模型；

S3、通过深度相机采集待抓取物体的二维图像并将该二维图像导入卷积神经网络模型中，输出对应的置信图和向量场；

S4、对步骤S3中所输出的置信图进行实例检测以选择出置信图的有效投射顶点，并将有效投射顶点的单位向量与向量场的单位向量进行比较输出有效的2D投射顶点，然后根据有效的2D投射顶点并利用PnP算法获取待抓取物体的姿态估计值，包括预测平移量μ₁和预测旋转量；

S5、通过步骤S2中卷积神经网络模型获取待抓取物体二维图像的感兴趣区域，并将感兴趣区域与图像模板库进行匹配以找到最优抓取点，然后利用深度相机计算最优抓取点的三维距离，从而得到深度相机的测量平移量μ₂；

S6、将步骤S4中得到的预测平移量μ₁和步骤S5中得到的测量平移量μ₂进行抓取安全距离校正，若校正成功则通过机械臂执行校正数据抓取，若校正失败则进入步骤S7；

S7、重复步骤S3-步骤S6。

2.如权利要求1所述的基于位姿估计和校正的单图像机器人无序目标抓取方法，其特征在于，所述步骤S1的具体实现方式包括以下步骤：

S11、制作一个与待抓取物体的物理尺寸及表面纹理均一致的模型；

S12、将接近抓取环境的实验室抓取背景和贴近真实物体场景的户外场景进行1:1结合并与随机背景一起作为虚拟环境的随机图像场景，然后任意选取一组3D模型对待抓取物体模型进行随机遮挡，通过不断调整待抓取物体模型与深度相机之间的距离和角度，获取待抓取物体模型的随机图像数据；

S13、将待抓取物体模型置于具有物理约束的3D背景中获取待抓取物体模型的真实感图像数据；

S14、将步骤S12中获取的随机图像数据与步骤S13中获取的真实感图像数据合并后作为待抓取物体模型的图像数据集。

3.如权利要求2所述的基于位姿估计和校正的单图像机器人无序目标抓取方法，其特征在于，所述步骤S2的具体实现方法包括以下步骤：

S21、构建一个具有十二个阶段的卷积神经网络结构，其中前四个阶段为特征提取网络，中间两个阶段为降维处理网络，后六个阶段为任务分支网络；

S22、将图像数据集中待训练的图像数据输入前四个阶段的特征提取网络中进行特征提取，其中前一个特征提取网络的输出为后一个特征提取网络的输入；

S23、将最后一个特征提取网络的输出通过两个降维处理网络中进行降维处理，其中第一个降维处理网络的输出为第二降维处理网络的输入；

S24、将第二个降维处理网络输出的特征输入六个阶段的任务分支网络中，其中前一个任务分支网络的输出为后一个任务分支网络的输入，从而提取出对应图像数据的包括八个投射顶点和一个质心点的置信图以及从八个投射顶点分别到对应质心点方向的向量场，得到卷积神经网络模型。

4.如权利要求3所述的基于位姿估计和校正的单图像机器人无序目标抓取方法，其特征在于，所述步骤S4的具体实现方式包括以下步骤：

S41、分别将所述置信图检测到的八个投射顶点与groundtruth进行计算，获取符合设定正态分布的置信度值，其计算公式可表示为：

式(1)中，Confidence(P_k)表示置信度，x_m1,m2表示数据的标签坐标，P_k表示置信图中检测到的第k个投射顶点坐标，k表示投射顶点的序号，k＝1,2,…8，σ²表示设定正态分布的方差；

S42、设定置信度阈值a，将步骤S41中所获得每个投射顶点的置信度值Confidence(P_k)与设定的置信阈值a比较，若Confidence(P_k)≥a，则保留该投射顶点，若Confidence(P_k)＜a，则舍去该投射顶点；

S43、计算

的正切值

将

与设定误差角阈值b比较以判定步骤S42所保留的投射顶点与质心点是否相关联，若

时，则判定该投射顶点与质心点相关联，若

则判定该投射顶点不与质心点相关联，且需要保证同一个待抓取物体至少有4个投射顶点与质心点相关联，其中

的计算公式可表示为：

式(2)中，

表示两个向量之间的误差，

表示从包围框的第k个投射顶点指向质心点的向量，

表示第k个投射顶点对应的向量场；

S44、根据步骤S43得到的有效投射顶点利用PnP算法获取待抓取物体的姿态估计值，即预测平移量μ₁和预测旋转量。

5.如权利要求4所述的基于位姿估计和校正的单图像机器人无序目标抓取方法，其特征在于，所述步骤S44的具体实现方式包括：

S441、将步骤S43中得到的有效投射顶点在世界坐标系中对应点设为空间参考点P_i ^w＝(x_i,y_i,z_i)，其中i＝1,2,…n，4≤n≤8，i表示有效投射顶点的序号，n表示空间参考点个数，然后计算出空间参考点P_i ^w对应向量α_ij，可用公式表示为：

在世界坐标系中，空间参考点可以由4个虚拟控制点表达，利用PnP算法可知，当满足

时，可将空间参考点坐标表示为虚拟控制点的加权和，则式(3)可表示为：

其中，

表示虚拟控制点在世界坐标系中的坐标，j表示虚拟控制点序号(j＝1,2,3,4)，若

表示空间参考点的重心，则

λ_c,j-1表示矩阵A^TA的特征值，ν′_c,j-1表示矩阵A^TA的特征向量，其中矩阵

S442、计算空间参考点P_i ^w在相机坐标系中的坐标P_i ^c，根据图像坐标系与相机坐标系的关系可得：

式(4)和式(5)中，ω_i表示相机捕捉图像时相机光圈到2D坐标点之间距离与相机光圈到3D坐标点之间距离的比值，K表示相机内参矩阵，(u_i,v_i)为空间参考点P_i ^w在图像中的2D投影点，

表示相机坐标系中的虚拟控制点，

则式(5)可表示为：

式(6)中，f_u、f_v、u_c、v_c表示相机内参；

由式(6)可得：

将n个空间参考点P_i ^w串起来可得：

Ml＝0 (8)

式(8)中，M表示矩阵系数，l表示待求参数，即四个虚拟控制点

在相机坐标系中的坐标，且

则式(8)可表示为：

式(9)中，ν″_h表示矩阵M^TM对应零特征值的第h个特征向量，h表示矩阵M^TM对应零特征值特征向量的序号，N表示矩阵M^TM对应零特征值的特征向量的数量，β_h表示待定系数，满足

其中，j₁≠j₂且j₁∈j,j₂∈j，

表示相机坐标系与世界坐标系中两个虚拟控制点之间的距离相等；

由(9)可得相机坐标系中的虚拟控制点

的计算公式：

式(10)中，

表示第j个虚拟控制点所占据的3个元素组成的向量；

S443、计算世界坐标系的质心点

相关矩阵A、相机坐标系的质心点

和相关矩阵B，可用公式表示：

式中，

表示世界坐标系的质心点，

表示相机坐标系的质心点；

S444、根据所述步骤S443得到的相关矩阵A和相关矩阵B，计算矩阵H并对矩阵H进行奇异值分解，可用公式表示为：

H＝B^TA (15)

H＝UCV^T (16)

式(16)中，U表示一个n×n的正交矩阵，C表示一个n×3的矩阵，V表示一个3×3的正交矩阵；

S445、根据步骤S444中的矩阵H奇异值分解结果计算旋转矩阵R，并通过旋转矩阵R计算出待抓取物体位姿的平移量S，然后利用高斯-牛顿最优化寻找最优解，从而获取待抓取物体的姿态估计值(X_t,Y_t,Z_t,X_r,Y_r,Z_r,W_r)，即预测平移量μ₁(X_t,Y_t,Z_t)和预测旋转量(X_r,Y_r,Z_r,W_r)，可用公式表示：

R＝UV^T (17)

6.如权利要求5所述的基于位姿估计和校正的单图像机器人无序目标抓取方法，其特征在于，所述步骤S5中图像模板库的构建包括以下步骤：

S501、获取最优抓取点处任意旋转方向上的待抓取物体图像；

S502、获取最优抓取点处不平衡光照条件下的待抓取物体图像；

S503、获取最优抓取点处不同缩放比例下的待抓取物体图像；

S504、将步骤S501、步骤S502和步骤S503中所获取的最优抓取点处所有待抓取物体图像保存至模板库中，从而完成图像模板库的构建。

7.如权利要求6所述的基于位姿估计和校正的单图像机器人无序目标抓取方法，其特征在于，所述步骤S5的具体实现方式包括以下步骤：

S51、通过所述卷积神经网络模型获取待抓取物体二维图像的感兴趣区域，并将待抓取物体感兴趣区域与图像模板库进行标准相关匹配，可用公式表示为：

式(20)中，T′表示图像模板库中模板图像，I′表示待抓取物体图像，R(x,y)表示待抓取物体图像与图像模板库中模板图像之间的匹配程度，(x′,y′)表示图像模板库中模板图像像素点的坐标，T′(x′,y′)表示模板图像在坐标(x′,y′)处的像素值，(x+x′,y+y′)表示待抓取物体图像像素点的坐标，I′(x+x′,y+y′)表示待抓取物体图像在坐标(x+x′,y+y′)处的像素值；

S52、采用矩形框将步骤S51中匹配成功的图像框起来，然后计算该矩形框的质心点，从而找到待抓取物体的最优抓取点及最优抓取点的像素坐标；

S53、将待抓取物体的二维图像与深度相机图像进行匹配，并利用二维图像中最优抓取点像素找到对应深度相机图像的测量距离，从而得到相机的测量平移量μ₂。

8.如权利要求7所述的基于位姿估计和校正的单图像机器人无序目标抓取方法，其特征在于，所述步骤S6中抓取安全距离校正的具体实现方式包括以下步骤：

S61、设定误差阈值Q₁与Q₂，且Q₁＜Q₂，同时保证误差阈值的最大误差小于物体平面的直径；

S62、通过预测平移量μ₁和测量平移量μ₂计算平均距离D，可用公式表示为：

式(21)中，x₁,y₁,z₁表示预测平移量μ₁的坐标值，x₂,y₂,z₂表示测量平移量μ₂的坐标值；

S63、根据平均距离D与误差阈值的大小判断是否需要进行抓取安全距离校正，当D＜Q₁时，则判定姿态估计值较准确，进入步骤S64，当Q₁＜D＜Q₂时，则判定姿态估计值存在不构成抓取失败的少许偏差，进入步骤S65；当D＞Q₂时，则判定姿态估计值存在严重偏差，进入步骤S66；

S64、将预测平移量μ₁和预测旋转量输入机械臂中，从而实现待抓取物体的精准抓取；

S65、分别计算预测平移量μ₁与测量平移量μ₂在三个方向上的平均值，获取待抓取物体相对于深度相机的校正平移量μ′，然后将所获取的校正平移量μ′和预测旋转量输入机械臂中，实现待抓取物体的精准抓取，其中校正平移量μ′的计算公式可表示为：

式(22)中，x″,y″,z″表示校正平移量μ′的坐标；

S66、返回步骤S3。

9.如权利要求8所述的基于位姿估计和校正的单图像机器人无序目标抓取方法，其特征在于，所述步骤S61中误差阈值Q₁＝0.02m，Q₂＝0.04m。

10.如权利要求9所述的基于位姿估计和校正的单图像机器人无序目标抓取方法，其特征在于，所述机械臂对待抓取物体精准抓取的具体实现方式为：将抓取安全距离校正后的平移量和旋转量与相机内外参结合，计算出机器人的抓取控制量，即抓取平移量和抓取旋转量，然后将抓取控制量输入机械臂中实现待抓取物体的精准抓取，抓取控制量的计算公式可表示为：

式(23)中，

表示抓取平移量，

表示抓取旋转量，

表示世界坐标系与机器人基坐标系的转换关系，其中：

r₁表示X_W轴的旋转矩阵，

θ₁表示机器人基坐标系绕X_W轴旋转的角度，r₂表示Y_W轴的旋转矩阵，

θ₂表示机器人基坐标系绕Y_W轴旋转的角度，r₃表示Z_W轴的旋转矩阵，

θ₃表示机器人基坐标系绕Z_W轴旋转的角度；

表示世界坐标系与相机坐标系之间的转换关系，

表示相机坐标系与机械手坐标系之间的转换关系，

表示机械手坐标系与机器人基坐标系之间的转换关系，

和

可通过机器人标定得到，

表示姿态估计中的旋转矩阵，

(X_r,Y_r,Z_r,W_r)表示预测旋转量，

表示姿态估计中的平移向量，

X,Y,Z表示进行抓取安全校正后的平移量。

Images