CN106553195B - 工业机器人抓取过程中的物体6自由度定位方法及系统 - Google Patents

Abstract

本发明公开了一种工业机器人抓取过程中的物体6自由度定位方法及系统，该方案能够在图像中提取待抓取物体表面的自然特征——局部特征点，进而利用特征进行识别和定位。其次，该方法能够根据机器人的关节角和运动学模型，计算比较合适的机器人的下一步运动并使机器人执行。最后，根据机器人的运动过程和两次识别结果，计算物体表面的顶点的3D坐标，建立相应的物体坐标系并获得物体6自由度位姿，从而准确的抓取相应的物体。

Description

工业机器人抓取过程中的物体6自由度定位方法及系统

技术领域

本发明涉及机器人系统的物体感知技术领域，尤其涉及一种工业机器人抓取过程中的物体6自由度定位方法及系统。

背景技术

对于机器人在真实环境下抓取物体，一个合适的视觉系统是至关重要的。在事先未知物体信息的环境中，视觉系统不仅要识别出物体，而且还必须在机器人基坐标系中给出足够准确的物体位姿。考虑刚性的3D物体模型，物体位姿是物体的位置和姿态组成的6自由度信息。

按使用的视觉特征，基于视觉的物体定位方法主要分为基于人工标记和基于自然特征。基于人工标记的方法需要在物体上贴人工图案做标记：十字叉、圆形、棋盘格、二维码等等，然后在图像中提取这些人工模式作为物体的特征。基于自然特征的方法直接用物体本身模式作为物体的特征：颜色，轮廓，局部纹理等等。两者相比较，人工标记的检测过程比较容易，检测结果更稳定，但是需要对物体的外观进行额外处理使得这种方法的应用场合受到限制。而自然特征广泛存在于各种物体上，更加适合于根据外观定位物体的场合。

按照使用的成像系统分类，面向机器人的物体定位方法主要有单目视觉和多目立体视觉两类。一方面，多目摄像机系统可以直接构成立体视觉。在物体定位中，立体视觉可以直接获取空间中点的3D信息进行物体定位，例如使用自然光的双目立体视觉摄像机和使用红外线的RGBD摄像机。另一方面，单目视觉如果没有其他辅助信息，单幅图像无法获取物体的3D信息。因此，单目的物体定位主要集中在从摄像机运动产生的多幅图像中恢复3D结构(SFM)上，或者事先已知物体上部分几何信息——部分点的坐标信息，根据PNP算法求解物体的位姿。尽管这些方法本身更加复杂，但是单目摄像机的体积和质量比多目摄像机小得多，改造和安装更加灵活。

现有的立体视觉设备价格较高，一般在数千元以上，好一些的甚至达到数十万。立体视觉设备价格高，体积和质量较大，在一些场合不宜使用。而现有的基于单目视觉的物体定位方法，仅从运动过程的图像出发或利用已知的坐标信息，导致计算过程过于复杂或需要额外工作获得物体先验知识。

发明内容

本发明的目的是提供一种工业机器人抓取过程中的物体6自由度定位方法及系统，可以准确定位目标物体的6自由度位姿并成功抓取相应物体。

本发明的目的是通过以下技术方案实现的：

一种工业机器人抓取过程中的物体6自由度定位方法，包括：

预先根据单目摄像机采集到的物体表面图像，进行多个物体顶点的2自由度定位，从而训练得到相应的物体模型；

根据训练得到的物体模型识别单目摄像机采集到的物体图像，定位得到物体的多个顶点位置作为物体顶点的2自由度定位结果，且记录工业机器人的当前关节角向量；再结合物体顶点的2自由度定位结果与工业机器人的当前关节角向量，来计算期望的工业机器人的下一步关节角向量，进而使所述工业机器人运动到所述期望的下一步关节角向量处；

再根据所述物体模型对下一步关节角向量处单目摄像机采集到的物体图像进行物体顶点的2自由度定位，对两次物体顶点的2自由度定位结果所构成的封闭区域内的特征点进行相互匹配，并计算每一对匹配的特征点的3D坐标，然后通过拟合物体表面平面的方式来计算物体每一顶点的最佳位置；最终根据物体每一顶点的最佳位置构建物体坐标系，来表示物体的6自由度位姿。

所述预先根据单目摄像机采集到的物体表面图像，进行多个物体顶点的2自由度定位，从而训练得到相应的物体模型包括：

获取单目摄像机采集到的一幅物体表面图像，再标定修正后的物体表面图像中的四个顶点，从而完成物体顶点的2自由度定位，顶点位置记为p_A,p_B,p_C,p_D；

这四个顶点构成一个四边形以表示物体上的待训练区域；在待训练区域外的部分被当作无用背景而丢掉，在该训练区域内的所有SURF特征构成该物体的模型，并被存储为数据库中的物体模型文件；其中，提取SURF特征时，设定海森矩阵特征值阈值t_he。

所述根据训练得到的物体模型识别单目摄像机采集到的物体图像，定位得到物体的多个顶点位置包括：

在初始时刻1，从单目摄像机采集到物体图像I1中提取SURF特征点；然后对于每个特征，使用近似最近邻搜索方法在物体模型中找出它匹配的最近邻；再使用比率测试去除错误匹配，一个特征的比率测试是检查它到最近邻的距离与它到次近邻的距离之商，这个商需要小于一个阈值t_r；然后采用Hough变换去除可能存在的错误匹配；

再使用单应性变换矩阵H将物体模型中标定的多个顶点与物体图像I1中待定位的顶点联系起来：

其中，和分别为物体模型中顶点和待定位顶点的齐次坐标，H是3×3的单应性矩阵，H的任意倍数仍然表示同一个单应性变换，求解H时直接令h₉＝1；根据成对的和使用RANSAC方法求解单应性矩阵；根据平面的平整程度设定RANSAC算法求解时的阈值t_ho；然后根据上式从物体图像I1中定位出四个顶点的坐标^I1p_A,^I1p_B,^I1p_C,^I1p_D。

所述结合物体顶点的2自由度定位结果与工业机器人的当前关节角向量，来计算工业机器人期望的下一步关节角向量，进而使所述工业机器人运动到期望的下一步关节角向量处包括：

首先，根据工业机器人的当前关节角向量和机器人的运动学模型计算单目摄像机的位姿，其过程如下：

工业机器人的当前关节角向量记为q1，关节角向量是一个n维向量q1＝(q₁,q₂,…,q_n)，机器人末端执行器上单目摄像机的坐标系相对于机器人末端执行器坐标系的变换矩阵为^ET_C；

在初始时刻1，末端执行器E的位姿^BT_E1为：

其中，L_i是第i根连杆末端的坐标系，i＝1,2,...,n；

则，此时单目摄像机坐标系F_C1的位姿^BT_C1为：

^BT_C1＝^BT_E1 ^ET_C；

然后，根据物体顶点的2自由度定位结果，来计算期望的单目摄像机的下一步位姿，其过程如下：

从物体图像I1中定位出四个顶点，其坐标为^I1p_A,^I1p_B,^I1p_C,^I1p_D，再计算物体中心坐标^I1p_O：

^I1p_O＝(^I1p_A+^I1p_B+^I1p_C+^I1p_D)/4；

如果^I1p_O在图像的左半区域，则期望的单目摄像机的下一步位姿否则，期望的单目摄像机的下一步位姿其中，d_C为运动量；

最后，根据期望的单目摄像机的下一步位姿和机器人的运动学模型，进行运动学反解，求期望的工业机器人的下一步关节角向量q1^*；再使所述工业机器人运动到所述期望的下一步关节角向量处。

所述对两次物体顶点的2自由度定位结果所构成的封闭区域内的特征进行相互匹配，并计算每一对匹配的特征点的3D坐标包括：

两次物体顶点的2自由度定位结果即为初始时刻1与初始时刻2定位得到的两组顶点坐标：^I1p_A,^I1p_B,^I1p_C,^I1p_D，以及^I2p_A,^I2p_B,^I2p_C,^I2p_D；

计算每一对匹配的特征点的3D坐标的步骤如下：

步骤a、推导点的成像过程：设在世界坐标系F_W中一点P的坐标为^Wp＝(Wx,^Wy,^Wz)^T，任意时刻，它在单目摄像机坐标系F_C中的成像过程如下：首先，点P以摄像机坐标系F_C为参考系的坐标为：

其中，分别为点P在世界坐标系F_W和摄像机坐标系F_C中的齐次坐标， ^Cp＝(^Cx,^Cy,^Cz), ^CT_W为单目摄像机坐标系F_C在世界坐标系F_W中的位姿；然后，坐标被投影到成像平面上，并被记录到图像上一点

其中，是图像坐标系F_I中的齐次坐标，L是投影矩阵，K是单目摄像机的内参数矩阵，^Cz是齐次坐标的z分量,是一个可缩放的因子；整合上面两步，得成像过程为：

步骤b、设摄像机分别在初始时刻1与初始时刻2对物体进行拍摄，并取初始时刻1的单目摄像机坐标系F_C1为世界坐标系W，那么，F_C1与F_W相同，即F_C1在世界坐标系F_W中的位姿^C1T_W是单位阵G；设初始时刻1物体的2自由度定位结果所构成的封闭区域内的特征点与初始时刻2物体的2自由度定位结果所构成的封闭区域内的特征点是一对匹配的特征点，然后对这一对匹配的特征点套用步骤a成像过程公式得到两条限制点P坐标的射线，其方程为：

上述两公式中，为点P以单目摄像机坐标系F_C1为参考系的齐次坐标，^C1z为中的Z轴坐标，^C1T_W为单目摄像机坐标系F_C1在世界坐标系F_W中的位姿；为点P以单目摄像机坐标系F_C2为参考系的齐次坐标，^C2z为中的Z轴坐标，^C2T_W为单目摄像机坐标系F_C2在世界坐标系F_W中的位姿，为坐标被投影到成像平面上，并被记录到图像上一点的齐次坐标；

上述两个公式中以^C1p的分量^C1x,^C1y,^C1z为未知数，^C2T_C1为初始时刻2的单目摄像机坐标系C2相对于C1的变换矩阵；根据初始时刻1、初始时刻2单目摄像机的位姿^BT_C1、^BT_C2来计算^C2T_C1：

步骤c、如果图像中有误差或者得到的单目摄像机位姿不准确，上述两条射线方程表示的两条射线可能不相交；则利用使重投影误差最小的优化问题来求解点P的最优3D坐标^Wp^*：

其中，π₁,π₂分别为步骤a中初始时刻1与初始时刻2的单目摄像机成像过程计算结果；

步骤d、把顶点坐标^I2p_A,^I2p_B,^I2p_C,^I2p_D构成的四边形区域内的每个特征点与顶点坐标^I1p_A,^I1p_B,^I1p_C,^I1p_D构成的四边形区域内的特征点进行匹配，形成匹配对集合；

步骤e，重复前述步骤b～步骤c，计算匹配对集合中每一对匹配的特征点的3D坐标，构成3D点集。

所述并通过拟合物体表面平面的方式来计算物体每一顶点的最佳位置包括：

从3D点集中拟合出一个平面C1S，然后在平面^C1S上计算物体四个顶点A,B,C,D的最佳位置^C1p_i,i＝A,B,C,D。

在平面^C1S上计算物体四个顶点A,B,C,D的最佳位置包括：

设平面^C1S的方程为：ax+by+cz+d＝0；求该平面^C1S的参数如下：

其中，共有m个3D点在该平面上，采用RANSAC方法对方程上述求解；

以穿过初始时刻1的单目摄像机坐标系C1原点和物体图像I1中四个顶点的射线与平面^C1S的交点作为顶点的最优位置，已知4条射线的方向向量为：

则求解交点^C1p_i＝(x,y,z)^T的方程组为：

ax+by+cz+d＝0

d_zx-d_xz＝0

d_zy-d_yz＝0

述根据物体每一顶点的最佳位置构建物体坐标系，来表示物体的6自由度位姿包括：

通过初始时刻1时单目摄像机的位姿^BT_C1，将顶点在单目摄像机坐标系F_C1中的坐标转换到机器人基坐标F_B中：

再根据顶点定义物体坐标系，以顶点A为原点，以AB为X轴方向计算单位向量e_x；在平面ABD内以AD方向为参考通过正交化建立Y轴方向和单位向量e_y；Z轴与X，Y轴构成右手，e_z＝e_x×e_y；原点A的坐标就是物体的3自由度位置；而物体坐标系的姿态矩阵为R＝(e_x,e_y,e_z)，由姿态矩阵R导出的表示姿态的欧拉角即为物体的3自由度姿态。

一种工业机器人抓取过程中的物体6自由度定位系统，用于实现前述的一种工业机器人抓取过程中的物体6自由度定位方法，该系统包括：

物体标定模块，用于预先根据单目摄像机采集到的物体表面图像，进行多个物体顶点的2自由度定位，从而训练得到相应的物体模型；以及根据训练得到的物体模型识别单目摄像机采集到的物体图像，定位得到物体的多个顶点位置作为物体顶点的2自由度定位结果，且记录工业机器人的当前关节角向量；

下一步运动模块，用于结合物体顶点的2自由度定位结果与工业机器人的当前关节角向量，来计算工业机器人期望的下一步关节角向量，进而使所述工业机器人运动到所述期望的下一步关节角向量处；

所述物体标定模块，还用于根据所述物体模型对下一步关节角向量处单目摄像机采集到的物体图像进行物体顶点的2自由度定位；

6自由度定位模块，用于对两次物体顶点的2自由度定位结果所构成的封闭区域内的特征点进行相互匹配，并计算每一对匹配的特征点的3D坐标，然后通过拟合物体表面平面的方式来计算物体每一顶点的最佳位置；最终根据物体每一顶点的最佳位置构建物体坐标系，来表示物体的6自由度位姿。

由上述本发明提供的技术方案可以看出，1)针对几何结构未知的物体，使得原本难以或无法获得3D信息的单目视觉系统能够更容易地获得物体表面点的3D坐标信息。2)结合了机器人的关节传感器和运动学模型，主动计算机器人下一步的运动量，能够主动控制机器人下一步运动到有利的位置上。3)在两个时刻测量机器人的关节角向量，并利用运动学模型，能够直接计算出两个时刻间摄像机的位姿的变化量。4)测量3D坐标时，采用了使图像内的重投影误差最小的优化目标，对测量结果进行了优化。在定位物体顶点时，进行了平面拟合，然后计算直线与平面的交点，能够进一步抵抗随机误差的干扰。5)相对于传统单目视觉方案而言，计算更加简单且不需要先验的物体几何信息，可以准确定位目标物体的6自由度位姿并成功抓取该物体。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他附图。

图1是本发明实施例提供的典型作业环境和坐标系定义图；

图2为本发明实施例提供的工业机器人抓取过程中的物体6自由度定位方法的流程图；

图3为本发明实施例提供的在准备阶段训练物体模型时标定物体图像各个顶点的示意图；

图4为本发明实施例提供的进入物体抓取过程中，标定物体图像各个顶点的示意图；

图5为本发明实施例提供的平面拟合和求解交点的示意图。

具体实施方式

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

本发明实施例提供一种工业机器人抓取过程中的物体6自由度定位方法，该方法能够实现工业机器人在抓取物体前对一类物体进行6自由度定位，其主要步骤如下：预先根据单目摄像机采集到的物体表面图像，进行多个物体顶点的2自由度定位，从而训练得到相应的物体模型；根据训练得到的物体模型识别单目摄像机采集到的物体图像，定位得到物体的多个顶点位置作为物体顶点的2自由度定位结果，且记录工业机器人的当前关节角向量；再结合物体顶点的2自由度定位结果与工业机器人的当前关节角向量，来计算工业机器人期望的下一步关节角向量，进而使所述工业机器人运动到期望的下一步关节角向量处；再根据所述物体模型对下一步关节角向量处单目摄像机采集到的物体图像进行物体顶点的2自由度定位，对两次物体顶点的2自由度定位结果所构成的封闭区域内的特征点进行相互匹配，并计算每一对匹配的特征点的3D坐标，然后通过拟合物体表面平面的方式来计算物体每一顶点的最佳位置；最终根据物体每一顶点的最佳位置构建物体坐标系，来表示物体的6自由度位姿。

简单来说，该方法能够在图像中提取待抓取物体表面的自然特征——局部特征点，进而利用特征进行识别和定位。其次，该方法能够根据机器人的关节角和运动学模型，计算比较合适的机器人的下一步运动并使机器人执行。最后，根据机器人的运动过程和两次识别结果，计算物体表面的顶点的3D坐标，建立相应的物体坐标系并获得物体6自由度位姿，从而准确地抓取相应的物体。

本领域技术人员可以理解，本发明实施例所述的物体至少包含一个平整的表面，该表面上有足够的纹理信息用于物体识别。

本发明实施例中的方案，可以应用于由工业机器人、单目摄像机、实验台、待抓取物体所组成的作业环境中，典型作业环境和坐标系定义如图1所示，在实验台的一侧设置有工业机器人，在工业机器人的末端执行器上设置有单目摄像机；单目摄像机的视场为工业机器人的作业范围；在作业范围内设置有待抓取物，

本发明实施例提供一种工业机器人抓取过程中的物体6自由度定位方法的流程图如图2所示，其主要包括：

在最初的准备阶段(未在图2中示出)，一方面，需要对单目摄像机进行标定，得到所述单目摄像机的扭曲参数向量d、内参数矩阵K和所述单目摄像机坐标系F_C相对于机器人末端执行器坐标系F_E的变换矩阵^ET_C；同时，后文所涉及的各个物体图像均被使用扭曲参数向量d校正；另一方面，需要根据物体表面图像训练得到相应的物体模型，其方法如下：如图3所示，获取单目摄像机采集到的一幅物体表面图像，并使用单目摄像机的扭曲参数向量d对物体表面图像进行修正，再标定出(可以由用户直接选定)修正后的物体表面图像中的四个顶点(图3中的A、B、C、D)，从而完成物体顶点的2自由度定位，顶点位置记为p_A,p_B,p_C,p_D；这四个顶点构成一个四边形以表示物体上的待训练区域；在待训练区域外的部分被当作无用背景而丢掉，在该训练区域内的所有SURF特征构成该物体的模型，并被存储为数据库中的物体模型文件；其中，提取SURF特征时，设定合适的海森矩阵特征值阈值t_he(例如，可以设为100)。

在完成上述准备阶段后，可以进入物体抓取过程。

一、初始时刻1

1、根据训练得到的物体模型识别单目摄像机采集到的物体图像，定位得到物体的多个顶点位置。

在初始时刻1，从单目摄像机采集到的物体图像I1中提取SURF特征点；然后对于每个特征，使用近似最近邻搜索方法在物体模型中找出它匹配的最近邻；再使用比率测试去除错误匹配，一个特征的比率测试是检查它到最近邻的距离与它到次近邻的距离之商，这个商需要小于一个阈值t_r(例如，可以设为0.667)；然后采用Hough变换去除可能存在的错误匹配；

再使用单应性变换矩阵H将物体模型中标定的多个顶点与物体图像I1中待定位的顶点联系起来：

其中，和分别为物体模型中顶点和待定位顶点的齐次坐标，H是3×3的单应性矩阵，H的任意倍数仍然表示同一个单应性变换，求解H时直接令h₉＝1；根据成对的和使用RANSAC方法求解单应性矩阵；根据平面的平整程度设定RANSAC算法求解时的阈值t_ho(例如，可以设为3.0px)；然后根据上式从物体图像I1中定位出四个顶点的坐标^I1p_A,^I1p_B,^I1p_C,^I1p_D，其结果如图4所示。

2、结合物体的定位结果与工业机器人的当前关节角向量，来计算工业机器人的下一步关节角向量，进而使所述工业机器人运动到所述下一步关节角向量处包括：

首先，根据工业机器人的当前关节角向量和机器人的运动学模型计算单目摄像机的位姿，其过程如下：

工业机器人的当前关节角向量记为q1，关节角向量是一个n维向量q1＝(q₁,q₂,…,q_n)，单目摄像机坐标系相对于机器人末端执行器坐标系的变换矩阵为^ET_C；

在初始时刻1，末端执行器E的位姿^BT_E1为：

其中，L_i是第i根连杆末端的坐标系，i＝1,2,...,n；

则，此时单目摄像机坐标系F_C1的位姿^BT_C1为：

^BT_C1＝^BT_E1 ^ET_C；

然后，根据物体顶点的2自由度定位结果，来计算期望的单目摄像机的下一步位姿，其过程如下：

根据^I1p_A,^I1p_B,^I1p_C,^I1p_D计算物体中心坐标^I1p_O：

^I1p_O＝(^I1p_A+^I1p_B+^I1p_C+^I1p_D)/4；

如果^I1p_O在图像的左半区域，则期望的单目摄像机的下一步位姿也即下一步使摄像机坐标系向左运动d_C(例如，可以设为0.0225m)；否则，期望的单目摄像机的下一步位姿也即下一步使摄像机坐标系向右运动d_C。

最后，根据期望的单目摄像机的下一步位姿和机器人的运动学模型，进行运动学反解，求工业机器人期望的下一步关节角向量q1^*；再使所述工业机器人运动到所述下一步关节角向量处。

本领域技术人员可以理解，上文所标识的“1～2”并非限定执行顺序，而是为了便于描述所进行的区分，在实际过程中，上文“1”中所描述的物体顶点定位过程，可以与上文“2”中计算单目摄像机位姿同步执行，也可以任意一前一后执行，任意执行方式均不会对本发明造成影响。

二、初始时刻2

初始时刻2所要执行的步骤与初始时刻1类似，故不再赘述。

三、计算物体的6自由度位姿

1、对两次物体的2自由度定位结果所构成的封闭区域内的特征点进行相互匹配，并计算每一对匹配的特征点的3D坐标。

两次物体顶点的2自由度定位结果即为初始时刻1与初始时刻2定位得到的两组顶点坐标：^I1p_A,^I1p_B,^I1p_C,^I1p_D，以及^I2p_A,^I2p_B,^I2p_C,^I2p_D；

计算物体每一对匹配的特征点的3D坐标的步骤如下：

步骤a、推导点的成像过程。设在世界坐标系F_W中一点P的坐标为^Wp＝(^Wx,^Wy,^Wz)^T，任意时刻，它在单目摄像机坐标系F_C中的成像过程如下：首先，点P以摄像机坐标系F_C为参考系的坐标为：

其中，分别为点P在世界坐标系F_W和摄像机坐标系F_C中的齐次坐标， ^Cp＝(^Cx,^Cy,^Cz), ^CT_W为单目摄像机坐标系F_C在世界坐标系F_W中的位姿；然后，坐标被投影到成像平面上，并被记录到图像上一点

其中，是图像坐标系F_I中的齐次坐标，L是投影矩阵，K是单目摄像机的内参数矩阵，^Cz是齐次坐标的z分量,是一个可缩放的因子；整合上面两步，得成像过程为：

步骤b、设摄像机分别在初始时刻1与初始时刻2对物体进行拍摄，并取初始时刻1的单目摄像机坐标系F_C1为世界坐标系F_W，那么，F_C1与F_W相同即F_C1在世界坐标系F_W中的位姿^C1T_W是单位阵G；设初始时刻1物体的2自由度定位结果所构成的封闭区域内的特征点与初始时刻2物体的2自由度定位结果所构成的封闭区域内的特征点是一对匹配的特征点，然后对这一对匹配的特征点套用在步骤a末尾的公式得到两条限制点P坐标的射线，其方程为：

上述两个公式中，各个参数上标实际是F_C1、F_C2的表示初始时刻1、2时的单目摄像机坐标系，即各个参数含义如下：为点P以单目摄像机坐标系F_C1为参考系的齐次坐标，^C1z为中的Z轴坐标，^C1T_W为单目摄像机坐标系F_C1在世界坐标系F_W中的位姿；为点P以单目摄像机坐标系F_C2为参考系的齐次坐标，^C2z为中的Z轴坐标，^C2T_W为单目摄像机坐标系F_C2在世界坐标系F_W中的位姿，为坐标被投影到成像平面上，并被记录到图像上一点的齐次坐标；

上述两个公式中以的分量^C1x,^C1y,^C1z为未知数，^C2T_C1为初始时刻2的单目摄像机坐标系C2相对于C1的变换矩阵；根据初始时刻1、初始时刻2单目摄像机的位姿^BT_C1、^BT_C2可以计算^C2T_C1：

步骤c、前述的射线方程与^C2T_C1计算方程构成恢复点P的3D坐标的计算公式；实际中，如果图像中有误差或者得到的单目摄像机位姿不准确，上述两条射线方程表示的两条射线可能不相交；则利用使重投影误差最小的优化问题来求解点P的最优3D坐标^Wp^*：

其中，π₁,π₂分别为步骤a中初始时刻1与初始时刻2的单目摄像机成像过程计算结果(即)。

步骤d、把顶点坐标^I2p_A,^I2p_B,^I2p_C,^I2p_D构成的四边形区域内的每个特征点(也就是前文计算的SURF特征点)与顶点坐标^I1p_A,^I1p_B,^I1p_C,^I1p_D构成的四边形区域内的特征点进行匹配，可以形成匹配对集合。

步骤e，重复前述步骤b～步骤c，计算匹配对集合中每一对匹配的特征点的3D坐标，构成3D点集。

2、通过拟合物体表面平面的方式来计算物体每一顶点的最佳位置。

从3D点集中拟合出一个平面^C1S，然后在平面^C1S上计算物体四个顶点A,B,C,D的最佳位置^C1p_i,i＝A,B,C,D；具体过程为：

在平面^C1S上计算物体四个顶点A,B,C,D的最佳位置包括：

设平面^C1S的方程为：ax+by+cz+d＝0；求该平面^C1S的参数如下：

其中，共有m个3D点在该平面上，采用RANSAC方法对方程上述求解；

以穿过初始时刻1的单目摄像机坐标系C1原点和物体图像I1中四个顶点的射线与平面^C1S的交点作为顶点的最优位置，已知4条射线的方向向量为：

则求解交点^C1p_i＝(x,y,z)^T的方程组为：

ax+by+cz+d＝0

d_zx-d_xz＝0

d_zy-d_yz＝0

上述的平面拟合和求解交点的示意图如图5所示。

3、根据物体每一顶点的最佳位置构建物体坐标系，来表示物体的6自由度位姿。

通过初始时刻1时单目摄像机的位姿^BT_C1，将顶点在单目摄像机坐标系F_C1中的坐标转换到机器人基坐标F_B中：

再根据顶点定义物体坐标系，以顶点A为原点，以AB为X轴方向计算单位向量e_x；在平面ABD内以AD方向为参考通过正交化建立Y轴方向和单位向量e_y；Z轴与X，Y轴构成右手，e_z＝e_x×e_y；原点A的坐标就是物体的3自由度位置；而物体坐标系的姿态矩阵为R＝(e_x,e_y,e_z)，由姿态矩阵R导出的表示姿态的欧拉角即为物体的3自由度姿态；物体的3自由度位置与3自由度姿态构成了物体的6自由度定位。

本领域技术人员可以理解，前文的“步骤一”、“步骤二”是在图像内对物体顶点进行的2自由度定位、“步骤三”中的第“1”个过程是在空间中进行特征点的3自由度定位，“步骤三”中的第“2”个过程是在空间中进行物体顶点的3自由度定位，“步骤三”中的第“3”个过程是在空间中进行物体位姿的6自由度定位，上述每一次定位就是计算一次相应的坐标。

另一方面，还基于本发明实施例的上述方案进行了试验，试验中将目标物体随意放在单目摄像机的视野中的6个不同位置处，用6自由度定位算法对每个情况进行5次定位。这里使用的欧拉角是绕旋转坐标轴的偏航角(yaw)，横滚角(pitch)和俯仰角(roll)。所有定位结果的统计量如表1。

表1对6自由度定位算法的结果的误差的统计表

*:length:目标物体沿x轴方向的长度；width:目标物体沿y方向的宽度。

本发明实施例的上述方案，主要具有如下优点：

1)针对几何结构未知的物体，使得原本难以或无法获得3D信息的单目视觉系统能够更容易地获得物体表面点的3D坐标信息。

2)结合了机器人的关节传感器和运动学模型，主动计算机器人下一步的运动量，能够主动控制机器人下一步运动到有利的位置上。

3)在两个时刻测量机器人的关节角向量，并利用运动学模型，能够直接计算出两个时刻间摄像机的位姿的变化量。

4)测量3D坐标时，采用了使图像内的重投影误差最小的优化目标，对测量结果进行了优化。在定位物体顶点时，进行了平面拟合，然后计算直线与平面的交点，能够进一步抵抗随机误差的干扰。

5)相对于传统单目视觉方案而言，可以在使计算更简单且不需要先验的物体几何信息的情况下，准确定位目标物体的6自由度位姿并成功抓取该物体。

本发明另一实施例还提供一种工业机器人抓取过程中的物体6自由度定位系统，该实施例可用于实现前述实施例的方法，其主包括：

物体标定模块，用于预先根据单目摄像机采集到的物体表面图像，进行多个物体顶点的2自由度定位，从而训练得到相应的物体模型；以及根据训练得到的物体模型识别单目摄像机采集到的物体图像，定位得到物体的多个顶点位置作为物体顶点的2自由度定位结果，且记录工业机器人的当前关节角向量；

下一步运动模块，用于结合物体顶点的2自由度定位结果与工业机器人的当前关节角向量，来计算工业机器人期望的下一步关节角向量，进而使所述工业机器人运动到所述期望的下一步关节角向量处；

所述物体标定模块，还用于根据所述物体模型对下一步关节角向量处单目摄像机采集到的物体图像进行物体顶点的2自由度定位；

6自由度定位模块，用于对两次物体顶点的2自由度定位结果所构成的封闭区域内的特征点进行相互匹配，并计算每一对匹配的特征点的3D坐标，然后通过拟合物体表面平面的方式来计算物体每一顶点的最佳位置；最终根据物体每一顶点的最佳位置构建物体坐标系，来表示物体的6自由度位姿。

需要说明的是，上述系统中包含的各个功能模块所实现的功能的具体实现方式在前面的实施例中已经有详细描述，故在这里不再赘述。

所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，仅以上述各功能模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能模块完成，即将系统的内部结构划分成不同的功能模块，以完成以上描述的全部或者部分功能。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例可以通过软件实现，也可以借助软件加必要的通用硬件平台的方式来实现。基于这样的理解，上述实施例的技术方案可以以软件产品的形式体现出来，该软件产品可以存储在一个非易失性存储介质(可以是CD-ROM，U盘，移动硬盘等)中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述的方法。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明披露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求书的保护范围为准。

Claims (9)

1.一种工业机器人抓取过程中的物体6自由度定位方法，其特征在于，包括：

预先根据单目摄像机采集到的物体表面图像，进行多个物体顶点的2自由度定位，从而训练得到相应的物体模型；

根据训练得到的物体模型识别单目摄像机采集到的物体图像，定位得到物体的多个顶点位置作为物体顶点的2自由度定位结果，且记录工业机器人的当前关节角向量；再结合物体顶点的2自由度定位结果与工业机器人的当前关节角向量，来计算期望的工业机器人的下一步关节角向量，进而使所述工业机器人运动到所述期望的下一步关节角向量处；

再根据所述物体模型对下一步关节角向量处单目摄像机采集到的物体图像进行物体顶点的2自由度定位，对两次物体顶点的2自由度定位结果所构成的封闭区域内的特征点进行相互匹配，并计算每一对匹配的特征点的3D坐标，然后通过拟合物体表面平面的方式来计算物体每一顶点的最佳位置；最终根据物体每一顶点的最佳位置构建物体坐标系，来表示物体的6自由度位姿。

2.根据权利要求1所述的一种工业机器人抓取过程中的物体6自由度定位方法，其特征在于，所述预先根据单目摄像机采集到的物体表面图像，进行多个物体顶点的2自由度定位，从而训练得到相应的物体模型包括：

获取单目摄像机采集到的一幅物体表面图像，再标定修正后的物体表面图像中的四个顶点，从而完成物体顶点的2自由度定位，顶点位置记为p_A,p_B,p_C,p_D；

这四个顶点构成一个四边形以表示物体上的待训练区域；在待训练区域外的部分被当作无用背景而丢掉，在该训练区域内的所有SURF特征构成该物体的模型，并被存储为数据库中的物体模型文件；其中，提取SURF特征时，设定海森矩阵特征值阈值t_he。

3.根据权利要求1所述的一种工业机器人抓取过程中的物体6自由度定位方法，其特征在于，所述根据训练得到的物体模型识别单目摄像机采集到的物体图像，定位得到物体的多个顶点位置包括：

在初始时刻1，从单目摄像机采集到物体图像I1中提取SURF特征点；然后对于每个特征，使用近似最近邻搜索方法在物体模型中找出它匹配的最近邻；再使用比率测试去除错误匹配，一个特征的比率测试是检查它到最近邻的距离与它到次近邻的距离之商，这个商需要小于一个阈值t_r；然后采用Hough变换去除可能存在的错误匹配；

再使用单应性变换矩阵H将物体模型中标定的多个顶点与物体图像I1中待定位的顶点联系起来：

其中，和分别为物体模型中顶点和待定位顶点的齐次坐标，H是3×3的单应性矩阵，H的任意倍数仍然表示同一个单应性变换，求解H时直接令h₉＝1；根据成对的和使用RANSAC方法求解单应性矩阵；根据平面的平整程度设定RANSAC算法求解时的阈值t_ho；然后根据上式从物体图像I1中定位出四个顶点的坐标^I1p_A,^I1p_B,^I1p_C,^I1p_D。

4.根据权利要求1所述的一种工业机器人抓取过程中的物体6自由度定位方法，其特征在于，所述结合物体顶点的2自由度定位结果与工业机器人的当前关节角向量，来计算工业机器人期望的下一步关节角向量，进而使所述工业机器人运动到期望的下一步关节角向量处包括：

首先，根据工业机器人的当前关节角向量和机器人的运动学模型计算单目摄像机的位姿，其过程如下：

工业机器人的当前关节角向量记为q1，关节角向量是一个n维向量q1＝(q₁,q₂,…,q_n)，机器人末端执行器上单目摄像机的坐标系相对于机器人末端执行器坐标系的变换矩阵为^ET_C；

在初始时刻1，末端执行器E的位姿^BT_E1为：

其中，L_i是第i根连杆末端的坐标系，i＝1,2,...,n；

则，此时单目摄像机坐标系F_C1的位姿^BT_C1为：

^BT_C1＝^BT_E1 ^ET_C；

然后，根据物体顶点的2自由度定位结果，来计算期望的单目摄像机的下一步位姿，其过程如下：

从物体图像I1中定位出四个顶点，其坐标为^I1p_A,^I1p_B,^I1p_C,^I1p_D，再计算物体中心坐标^I1p_O：

^I1p_O＝(^I1p_A+^I1p_B+^I1p_C+^I1p_D)/4；

如果^I1p_O在图像的左半区域，则期望的单目摄像机的下一步位姿否则，期望的单目摄像机的下一步位姿其中，d_C为运动量；

最后，根据期望的单目摄像机的下一步位姿和机器人的运动学模型，进行运动学反解，求期望的工业机器人的下一步关节角向量q1^*；再使所述工业机器人运动到所述期望的下一步关节角向量处。

5.根据权利要求1或2或3或4所述的一种工业机器人抓取过程中的物体6自由度定位方法，其特征在于，所述对两次物体顶点的2自由度定位结果所构成的封闭区域内的特征进行相互匹配，并计算每一对匹配的特征点的3D坐标包括：

两次物体顶点的2自由度定位结果即为初始时刻1与初始时刻2定位得到的两组顶点坐标：^I1p_A,^I1p_B,^I1p_C,^I1p_D，以及^I2p_A,^I2p_B,^I2p_C,^I2p_D；

计算每一对匹配的特征点的3D坐标的步骤如下：

步骤a、推导点的成像过程：设在世界坐标系F_W中一点P的坐标为^Wp＝(^Wx,^Wy,^Wz)^T，任意时刻，它在单目摄像机坐标系F_C中的成像过程如下：首先，点P以摄像机坐标系F_C为参考系的坐标为：

其中，分别为点P在世界坐标系F_W和摄像机坐标系F_C中的齐次坐标， ^CT_W为单目摄像机坐标系F_C在世界坐标系F_W中的位姿；然后，坐标被投影到成像平面上，并被记录到图像上一点

其中，是图像坐标系F_I中的齐次坐标，L是投影矩阵，K是单目摄像机的内参数矩阵，^Cz是齐次坐标的z分量,是一个可缩放的因子；整合上面两步，得成像过程为：

步骤b、设摄像机分别在初始时刻1与初始时刻2对物体进行拍摄，并取初始时刻1的单目摄像机坐标系F_C1为世界坐标系W，那么，F_C1与F_W相同，即F_C1在世界坐标系F_W中的位姿^C1T_W是单位阵G；设初始时刻1物体的2自由度定位结果所构成的封闭区域内的特征点与初始时刻2物体的2自由度定位结果所构成的封闭区域内的特征点是一对匹配的特征点，然后对这一对匹配的特征点套用步骤a成像过程公式得到两条限制点P坐标的射线，其方程为：

上述两公式中，为点P以单目摄像机坐标系F_C1为参考系的齐次坐标，^C1z为中的Z轴坐标，^C1T_W为单目摄像机坐标系F_C1在世界坐标系F_W中的位姿；为点P以单目摄像机坐标系F_C2为参考系的齐次坐标，^C2z为中的Z轴坐标，^C2T_W为单目摄像机坐标系F_C2在世界坐标系F_W中的位姿，为坐标被投影到成像平面上，并被记录到图像上一点的齐次坐标；

上述两个公式中以^C1p的分量^C1x,^C1y,^C1z为未知数，^C2T_C1为初始时刻2的单目摄像机坐标系C2相对于C1的变换矩阵；根据初始时刻1、初始时刻2单目摄像机的位姿^BT_C1、^BT_C2来计算^C2T_C1：

步骤c、如果图像中有误差或者得到的单目摄像机位姿不准确，上述两条射线方程表示的两条射线可能不相交；则利用使重投影误差最小的优化问题来求解点P的最优3D坐标^Wp^*：

其中，π₁,π₂分别为步骤a中初始时刻1与初始时刻2的单目摄像机成像过程计算结果；

步骤d、把顶点坐标^I2p_A,^I2p_B,^I2p_C,^I2p_D构成的四边形区域内的每个特征点与顶点坐标^I1p_A,^I1p_B,^I1p_C,^I1p_D构成的四边形区域内的特征点进行匹配，形成匹配对集合；

步骤e，重复前述步骤b～步骤c，计算匹配对集合中每一对匹配的特征点的3D坐标，构成3D点集。

6.根据权利要求5所述的一种工业机器人抓取过程中的物体6自由度定位方法，其特征在于，所述通过拟合物体表面平面的方式来计算物体每一顶点的最佳位置包括：

从3D点集中拟合出一个平面^C1S，然后在平面^C1S上计算物体四个顶点A,B,C,D的最佳位置^C1p_i,i＝A,B,C,D。

7.根据权利要求6所述的一种工业机器人抓取过程中的物体6自由度定位方法，其特征在于，在平面^C1S上计算物体四个顶点A,B,C,D的最佳位置包括：

设平面^C1S的方程为：ax+by+cz+d＝0；求该平面^C1S的参数如下：

其中，共有m个3D点在该平面上，采用RANSAC方法对方程上述求解；

以穿过初始时刻1的单目摄像机坐标系C1原点和物体图像I1中四个顶点的射线与平面^C1S的交点作为顶点的最优位置，已知4条射线的方向向量为：

则求解交点^C1p_i＝(x,y,z)^T的方程组为：

ax+by+cz+d＝0

d_zx-d_xz＝0

d_zy-d_yz＝0。

8.根据权利要求6或7所述的一种工业机器人抓取过程中的物体6自由度定位方法，其特征在于，所述根据物体每一顶点的最佳位置构建物体坐标系，来表示物体的6自由度位姿包括：

通过初始时刻1时单目摄像机的位姿^BT_C1，将顶点在单目摄像机坐标系F_C1中的坐标转换到机器人基坐标F_B中：

再根据顶点定义物体坐标系，以顶点A为原点，以AB为X轴方向计算单位向量e_x；在平面ABD内以AD方向为参考通过正交化建立Y轴方向和单位向量e_y；Z轴与X，Y轴构成右手，e_z＝e_x×e_y；原点A的坐标就是物体的3自由度位置；而物体坐标系的姿态矩阵为R＝(e_x,e_y,e_z)，由姿态矩阵R导出的表示姿态的欧拉角即为物体的3自由度姿态。

9.一种工业机器人抓取过程中的物体6自由度定位系统，其特征在于，用于实现前述权利要求1-8任一项所述的一种工业机器人抓取过程中的物体6自由度定位方法，该系统包括：

物体标定模块，用于预先根据单目摄像机采集到的物体表面图像，进行多个物体顶点的2自由度定位，从而训练得到相应的物体模型；以及根据训练得到的物体模型识别单目摄像机采集到的物体图像，定位得到物体的多个顶点位置作为物体顶点的2自由度定位结果，且记录工业机器人的当前关节角向量；

下一步运动模块，用于结合物体顶点的2自由度定位结果与工业机器人的当前关节角向量，来计算工业机器人期望的下一步关节角向量，进而使所述工业机器人运动到所述期望的下一步关节角向量处；

所述物体标定模块，还用于根据所述物体模型对下一步关节角向量处单目摄像机采集到的物体图像进行物体顶点的2自由度定位；

6自由度定位模块，用于对两次物体顶点的2自由度定位结果所构成的封闭区域内的特征点进行相互匹配，并计算每一对匹配的特征点的3D坐标，然后通过拟合物体表面平面的方式来计算物体每一顶点的最佳位置；最终根据物体每一顶点的最佳位置构建物体坐标系，来表示物体的6自由度位姿。