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

权利要求书

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

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

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

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

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

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算法获取待抓取物体的姿态估计值，即预测平移量μ₁和预测旋转量，具体实现方式包括：

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的特征值，v′_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)中，v″_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)

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

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

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