CN111267095A - 一种基于双目视觉的机械臂抓取控制方法 - Google Patents

一种基于双目视觉的机械臂抓取控制方法 Download PDF

Info

Publication number
CN111267095A
CN111267095A CN202010037021.6A CN202010037021A CN111267095A CN 111267095 A CN111267095 A CN 111267095A CN 202010037021 A CN202010037021 A CN 202010037021A CN 111267095 A CN111267095 A CN 111267095A
Authority
CN
China
Prior art keywords
mechanical arm
coordinate system
target object
axis
target
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202010037021.6A
Other languages
English (en)
Other versions
CN111267095B (zh
Inventor
王东
杨冬
董永祥
连捷
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Dalian University of Technology
Original Assignee
Dalian University of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Dalian University of Technology filed Critical Dalian University of Technology
Priority to CN202010037021.6A priority Critical patent/CN111267095B/zh
Publication of CN111267095A publication Critical patent/CN111267095A/zh
Application granted granted Critical
Publication of CN111267095B publication Critical patent/CN111267095B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1612—Programme controls characterised by the hand, wrist, grip control
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/1648—Programme controls characterised by the control loop non-linear control combined or not with linear control
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
    • B25J9/1697—Vision controlled systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Physics & Mathematics (AREA)
  • Nonlinear Science (AREA)
  • Health & Medical Sciences (AREA)
  • General Health & Medical Sciences (AREA)
  • Orthopedic Medicine & Surgery (AREA)
  • Automation & Control Theory (AREA)
  • Image Analysis (AREA)

Abstract

本发明涉及kinova机械臂和ZED视觉技术领域，一种基于双目视觉的机械臂抓取控制方法，包括以下步骤：(1)实验平台搭建，(2)目标物倾斜放置时的位置和姿态的识别，(3)目标物水平放置时的位置和姿态的识别，(4)机械臂末端执行器识别并抓取目标物体，(5)机械臂抓取目标后到指定关节角状态。本发明具有以下优点：一是，估计目标物体的姿态角度，方向向量与坐标轴平面的夹角偏差在‑5～+5度之内，检测的正确率大大提高。二是，采用颜色+形状可以避免颜色相同或相近的干扰项对目标物体识别的干扰。三是，设定双机械臂抓取左右目标物体，最终获得左右机械臂抓取目标物姿态的效果。

Description

一种基于双目视觉的机械臂抓取控制方法
技术领域
本发明涉及一种基于双目视觉的机械臂抓取控制方法，属于kinova机械臂和ZED视觉技术领域。
背景技术
双目视觉位姿测量作为一种重要的目标姿态方式，但是对倾斜放置的目标物姿态测量存在一定的难度，现存的一些方法有：圆提取方法和特征点提取方法。
圆形截面提取法：现有方法是使用3个工业CCD相机来对圆柱体目标物的上下截面的圆心进行提取，定义坐标系，计算两个截面圆心连线与坐标系的夹角来描述位置姿态，这种方法的准确性受目标物外形影响较大，且被遮挡的圆形截面的圆心不易提取。
特征点提取法：分为两种情况：一，人为设定特征点；二，使用激光照射获取特征点。第一种情况有人对目标物表面设置角锥棱镜作为特征点，利用一定波长的激光照射棱镜来获取目标物的位置和姿态，但是这种方法条件苛刻，只有让拍摄前景图像和背景图像时间间隔缩短才可达到一定的精度。第二种情况有人使用激光射线照射零件表面来人为创造特征点，这种方法的精度很大程度依赖于发射激光的准确性和稳定性，也增加了检测成本。
发明内容
针对现有技术中存在的不足，本发明目的是提供一种基于双目视觉的机械臂抓取控制方法。由于圆形截面的轮廓提取精度不高，造成圆心提取存在较大的偏差。在实际的情况中，有时也会存在圆形截面被遮挡等一些情况，这些都会使得圆心连线法估计圆柱体姿态变得不切实际。为解决提取截面圆困难和精度不高等问题，本发明提出了利用前景提取的方法来获取目标物截面圆弧顶点，以顶点坐标差估计目标物姿态，最终控制机械臂能够类似于人手一样灵活地抓取目标物。
为了实现上述发明目的，解决己有技术中存在的问题，本发明采取的技术方案是：一种基于双目视觉的机械臂抓取控制方法，包括以下步骤：
步骤1、实验平台搭建，将双目视觉相机安装在实验平台正上方1-1.5m处，启动双目视觉相机，启动kinova机械臂并加载左右机械臂模型urdf，该模型描述了kinova机械臂的各关节位置和姿态关系，其中左机械臂安装在水平桌面左手边，其底部坐标系定义为left，右机械臂安装在水平桌面右手边，其底部坐标系定义为right，左右机械臂距离为0.8-1.2m。根坐标系在左右机械臂中间位置，定义为root，双目视觉相机世界坐标系定义为map，以root为根坐标系，利用平移和旋转确定left、right和map坐标位置和姿态，以根坐标系原点为中心点，旋转到左机械臂底部坐标系的旋转矩阵为R1，平移矩阵为T1，旋转到右机械臂底部坐标系的旋转矩阵为R2，平移矩阵T2，旋转到双目视觉相机坐标系的旋转矩阵为R3，平移矩阵为T3，并通过式(1)至(3)进行描述，
left＝R1*root+T1 (1)
right＝R2*root+T2 (2)
map＝R3*root+T3 (3)
步骤2、目标物倾斜放置时的位置和姿态的识别，具体包括以下子步骤：
(a)订阅双目视觉相机发布的RGB图像，对图片进行二值化，由于图像外杂质物体较多，人为设置一个像素框，利用opencv中grabCut算法将框内的前景提取出来，其中设置迭代次数为7-12；
(b)将提取的前景图像像素pi(u,v),i∈N(N为前景提取到的像素点个数)与深度图像进行匹配得到世界坐标系下对应的世界坐标Pi＝[XW YW ZW]T,i∈N。将N个Pi存放在容器中，从所有像素坐标系映射到双目视觉相机的世界坐标的映射关系，通过式(4)进行描述，
Figure BDA0002366394030000021
其中，z为双目视觉相机深度值，
Figure BDA0002366394030000031
f为双目视觉相机的焦距，dx为横向单位像素所对应的物理距离，单位为mm，dy为纵向单位像素所对应的物理距离，单位为mm，式中参数u0为图像像素横向中点，v0为图像像素纵向中点。将转换后的世界坐标存储在容器2中，遍历整个容器2，求出所有元素中x值最小的元素世界坐标P1＝[X,Y,Z],X∈min{XW}，该元素代表两个圆弧最高顶点P1，以P1为中心点；
(c)以瓶子长度L为约束条件，遍历容器2，找出与中心点P1距离满足长度为L约束条件的所有点，存放于容器3中，再从容器3中查找x轴坐标最小值所对应的目标P2，即后面圆弧最高顶点P2，方向向量v，通过式(5)计算得到，
v＝P1-P2 (5)
(d)计算目标物重心点Q3，设Q2点为子步骤(c)中P1和P2的中点，其中瓶子截面半径设为r，Q2与正下方点Q之间的距离为h，Q，Q2，Q3组成直角三角形的几何关系并且满足Q3-Q＝kv,k≠0，vx，vy，vz分别为方向向量v在双目视觉相机世界坐标系x轴，y轴，z轴上的投影，通过式(6)至(8)进行描述，
Figure BDA0002366394030000032
Figure BDA0002366394030000033
Figure BDA0002366394030000034
(e)方向向量v的参考系是双目视觉相机世界坐标系map，要将右目标物的方向向量v转换到右机械臂底部坐标系right中，方向向量为v2，转换公式通过式(9)进行描述，
Figure BDA0002366394030000035
其中，
Figure BDA0002366394030000036
为双目视觉相机世界坐标系map转换到右机械臂底部坐标系right的旋转矩阵，计算方向向量v2与right各个坐标平面的夹角，通过式(10)进行描述，
Figure BDA0002366394030000041
式中，v2x，v2y，v2z，分别为v2在右机械臂底座坐标系下x轴、y轴、z轴投影，α为方向向量v2与x-y平面夹角，β为方向向量v2与x-z平面夹角，χ为方向向量v2与y-z平面夹角；
步骤3、目标物水平放置时的位置和姿态的识别，具体包括以下子步骤：
(a)需要利用颜色分割的方式提取出左目标物的二值图像，这是由于这一过程会存在颜色相同的无关物体的干扰，也会提取到二值图像里面；
(b)为了解决上述中关于相同颜色对目标物识别造成干扰的问题，采取目标物的形状作为对象识别辅助条件，利用形状特征对提取的图像进行筛选，保留具有四边形特征的二值图像；
(c)使用opencv中多边形逼近轮廓函数cv::approxPoly()，得到二维数组M×N,其中M表示多边形组数，N表示每组多边型边界像素点的集合；
(d)判断是否为四边形，因为所需要提取出来的目标物体是四边形，但是实际工程上可能会存在一定的偏差，在这里选择目标物体的边数为3-5，在这个范围内都可认为符合目标物体的形状特征，予以保留；
(e)使用findContours()函数求出所有形状信息，存储轮廓的向量；
(f)使用approxPolyDP()函数求解每个轮廓的边数，遍历整个容器，将边数不为3-5的干扰物删除，保留容器中形状近似为四边形的元素，则使用polylines()函数画出边框，并求出重心点goal的像素坐标，再利用深度图像匹配计算出重心点在双目视觉相机的世界坐标系下的位置坐标；
(g)当目标物的轮廓信息确定后，设矩形框四个顶点为A,B,C,D，若AB为最长边，选取AB边顶点坐标之差作为方向向量，将左目标物方向向量v转换到左机械臂底座坐标系下v1，计算方向向量与左机械臂底座坐标系x-z平面夹角为
Figure BDA0002366394030000051
通过式(11)进行描述，
Figure BDA0002366394030000052
其中，v1x，v1y，v1z分别为v1在左机械臂底座坐标系下x轴、y轴、z轴上的投影；
步骤4、机械臂末端执行器识别并抓取目标物体，实验状态下的左目标物体所对应的位置为水平放置，与水平面夹角为零，式(11)求解方向向量v1与左机械臂底座坐标系x-z平面夹角Δ，左机械臂末端执行器z轴与左机械臂底部坐标系z轴夹角为170°-180°，左机械臂末端执行器绕其关节轴旋转角度Gr控制，通过式(12)进行描述，
Figure BDA0002366394030000053
实验状态下的右目标物倾斜于桌面放置，在右机械臂底部坐标系下方向向量为v2，v2与x-y平面的夹角为α，右机械臂末端执行器z轴与右机械臂底部坐标系的z轴夹角σ，求解通过式(13)，(14)进行描述，
Figure BDA0002366394030000054
Figure BDA0002366394030000055
其中，delta为球坐标中纬度值，v2右机械臂底部坐标系z-x平面的夹角β，右机械臂末端执行器绕其关节轴旋转角度Gr，通过式(15)，(16)进行描述，
Figure BDA0002366394030000056
Figure BDA0002366394030000061
姿态控制完毕之后，需要控制机械臂末端执行器到达目标位置，其中方位角定义是从x轴正方向开始，顺时针方向到目标方向线之间的水平夹角，由于方位角θ在[0,2π]，所以需要判断(x,y)位于四象限中的象限，通过式(17)计算出方位角θ；
Figure BDA0002366394030000062
为了避免在抓取过程中与目标物体产生碰撞，设计了预抓取过程，提前设计好抓取的位置和姿态，通过式(18)进行描述，
Figure BDA0002366394030000063
式中(xgoal,ygoal,zgoal)T为目标重心点相对机械臂底部坐标系的位置坐标，(x′,y′,z′)T为相对机械臂底部坐标系的预抓取位置坐标点，L是人为设定的一段距离；θ为目标点在机械臂坐标系中x-y平面投影与x轴的夹角，σ为机械臂末端执行器z轴与机械臂底部坐标系z轴之间的夹角；利用式(11)-(18)得到预设目标位置和姿态，控制机械臂末端执行器以最佳的姿态抓取目标物；
步骤5、机械臂抓取目标后到指定关节角状态，机械臂动作客户端发送指定关节角到动作服务器，执行完毕，动作结束。
本发明有益效果是：一种基于双目视觉的机械臂抓取控制方法，包括以下步骤：(1)实验平台搭建，(2)目标物倾斜放置时的位置和姿态的识别，(3)目标物水平放置时的位置和姿态的识别，(4)机械臂末端执行器识别并抓取目标物体，(5)机械臂抓取目标后到指定关节角状态。与已有技术相比，本发明具有以下优点：一是，对目标物体的前景提取到前后圆弧的顶点坐标，利用坐标的差值来估计目标物的方向向量，从而估计目标物体的姿态角度，方向向量与坐标轴平面的夹角偏差在-5～+5度之内，检测的正确率大大提高。二是，采用颜色+形状可以避免颜色相同或相近的干扰项对目标物体识别的干扰，克服干扰项对目标检测的影响，重心点比较稳定，可以稳定地检测目标物的位置和姿态信息。三是，设定双机械臂抓取左右目标物体，左目标物的位姿可以引导左机械臂以最佳的姿态去抓取目标物。右目标物的位姿可以引导右机械臂以最佳的姿态去抓取该目标物，最终获得左右机械臂抓取目标物姿态的效果。
附图说明
图1是本发明方法步骤流程图。
图2是右目标物体姿态检测效果图。
图中：(a)是右目标物体姿态图，(b)是选取矩形框并且只在框内提取目标物体前景椭圆图像图，(c)是双目视觉相机的世界坐标系下右目标的几何关系图，(d)是求解出右目标物体方向向量的结果图。
图3是左目标物体姿态检测效果图。
图中：(a)是左目标物体姿态图，(b)是使用颜色+形状识别效果图，(c)是左机械臂底部坐标系下左目标的几何关系图，(d)是求解出左目标物体方向向量的结果图。
图4是左右机械臂系统坐标系转换图。
图5是左右机械臂抓取特殊姿态的目标物效果图。
图中：(a)是左机械臂抓取特殊姿态的目标物效果图，(b)是右机械臂抓取特殊姿态的目标物效果图。
图6是左右机械臂抓取两个目标物体完成后的效果图。
具体实施方式
下面结合附图对本发明作进一步说明。
如图1所示，一种基于双目视觉的机械臂抓取控制方法，包括以下步骤：
步骤1、实验平台搭建，将双目视觉相机安装在实验平台正上方1-1.5m处，启动双目视觉相机，启动kinova机械臂并加载左右机械臂模型urdf，该模型描述了kinova机械臂的各关节位置和姿态关系，其中左机械臂安装在水平桌面左手边，其底部坐标系定义为left，右机械臂安装在水平桌面右手边，其底部坐标系定义为right，左右机械臂距离为0.8-1.2m。根坐标系在左右机械臂中间位置，定义为root，双目视觉相机世界坐标系定义为map，以root为根坐标系，利用平移和旋转确定left、right和map坐标位置和姿态，以根坐标系原点为中心点，旋转到左机械臂底部坐标系的旋转矩阵为R1，平移矩阵为T1，旋转到右机械臂底部坐标系的旋转矩阵为R2，平移矩阵T2，旋转到双目视觉相机坐标系的旋转矩阵为R3，平移矩阵为T3，并通过式(1)至(3)进行描述，
left＝R1*root+T1 (1)
right＝R2*root+T2 (2)
map＝R3*root+T3 (3)
步骤2、目标物倾斜放置时的位置和姿态的识别，具体包括以下子步骤：
(a)订阅双目视觉相机发布的RGB图像，对图片进行二值化，由于图像外杂质物体较多，人为设置一个像素框，利用opencv中grabCut算法将框内的前景提取出来，其中设置迭代次数为7-12；
(b)将提取的前景图像像素pi(u,v),i∈N(N为前景提取到的像素点个数)与深度图像进行匹配得到世界坐标系下对应的世界坐标Pi＝[XW YW ZW]T,i∈N。将N个Pi存放在容器中，从所有像素坐标系映射到双目视觉相机的世界坐标的映射关系，通过式(4)进行描述，
Figure BDA0002366394030000091
其中，z为双目视觉相机深度值，
Figure BDA0002366394030000092
f为双目视觉相机的焦距，dx为横向单位像素所对应的物理距离，单位为mm，dy为纵向单位像素所对应的物理距离，单位为mm，式中参数u0为图像像素横向中点，v0为图像像素纵向中点。将转换后的世界坐标存储在容器2中，遍历整个容器2，求出所有元素中x值最小的元素世界坐标P1＝[X,Y,Z],X∈min{XW}，该元素代表两个圆弧最高顶点P1，以P1为中心点；
(c)以瓶子长度L为约束条件，遍历容器2，找出与中心点P1距离满足长度为L约束条件的所有点，存放于容器3中，再从容器3中查找x轴坐标最小值所对应的目标P2，即后面圆弧最高顶点P2，方向向量v，通过式(5)计算得到，
v＝P1-P2 (5)
(d)计算目标物重心点Q3，设Q2点为子步骤(c)中P1和P2的中点，其中瓶子截面半径设为r，Q2与正下方点Q之间的距离为h，Q，Q2，Q3组成直角三角形的几何关系并且满足Q3-Q＝kv,k≠0，vx，vy，vz分别为方向向量v在双目视觉相机世界坐标系x轴，y轴，z轴上的投影，通过式(6)至(8)进行描述，
Figure BDA0002366394030000093
Figure BDA0002366394030000094
Figure BDA0002366394030000095
(e)方向向量v的参考系是双目视觉相机世界坐标系map，要将右目标物的方向向量v转换到右机械臂底部坐标系right中，方向向量为v2，转换公式通过式(9)进行描述，
Figure BDA0002366394030000101
其中，
Figure BDA0002366394030000102
为双目视觉相机世界坐标系map转换到右机械臂底部坐标系right的旋转矩阵，计算方向向量v2与right各个坐标平面的夹角，通过式(10)进行描述，
Figure BDA0002366394030000103
式中，v2x，v2y，v2z，分别为v2在右机械臂底座坐标系下x轴、y轴、z轴投影，α为方向向量v2与x-y平面夹角，β为方向向量v2与x-z平面夹角，χ为方向向量v2与y-z平面夹角；
步骤3、目标物水平放置时的位置和姿态的识别，具体包括以下子步骤：
(a)需要利用颜色分割的方式提取出左目标物的二值图像，这是由于这一过程会存在颜色相同的无关物体的干扰，也会提取到二值图像里面；
(b)为了解决上述中关于相同颜色对目标物识别造成干扰的问题，采取目标物的形状作为对象识别辅助条件，利用形状特征对提取的图像进行筛选，保留具有四边形特征的二值图像；
(c)使用opencv中多边形逼近轮廓函数cv::approxPoly()，得到二维数组M×N,其中M表示多边形组数，N表示每组多边型边界像素点的集合；
(d)判断是否为四边形，因为所需要提取出来的目标物体是四边形，但是实际工程上可能会存在一定的偏差，在这里选择目标物体的边数为3-5，在这个范围内都可认为符合目标物体的形状特征，予以保留；
(e)使用findContours()函数求出所有形状信息，存储轮廓的向量；
(f)使用approxPolyDP()函数求解每个轮廓的边数，遍历整个容器，将边数不为3-5的干扰物删除，保留容器中形状近似为四边形的元素，则使用polylines()函数画出边框，并求出重心点goal的像素坐标，再利用深度图像匹配计算出重心点在双目视觉相机的世界坐标系下的位置坐标；
(g)当目标物的轮廓信息确定后，设矩形框四个顶点为A,B,C,D，若AB为最长边，选取AB边顶点坐标之差作为方向向量，将左目标物方向向量v转换到左机械臂底座坐标系下v1，计算方向向量与左机械臂底座坐标系x-z平面夹角为
Figure BDA0002366394030000111
通过式(11)进行描述，
Figure BDA0002366394030000112
其中，v1x，v1y，v1z分别为v1在左机械臂底座坐标系下x轴、y轴、z轴上的投影；
步骤4、机械臂末端执行器识别并抓取目标物体，实验状态下的左目标物体所对应的位置为水平放置，与水平面夹角为零，式(11)求解方向向量v1与左机械臂底座坐标系x-z平面夹角Δ，左机械臂末端执行器z轴与左机械臂底部坐标系z轴夹角为170°-180°，左机械臂末端执行器绕其关节轴旋转角度Gr控制，通过式(12)进行描述，
Figure BDA0002366394030000113
实验状态下的右目标物倾斜于桌面放置，在右机械臂底部坐标系下方向向量为v2，v2与x-y平面的夹角为α，右机械臂末端执行器z轴与右机械臂底部坐标系的z轴夹角σ，求解通过式(13)，(14)进行描述，
Figure BDA0002366394030000114
Figure BDA0002366394030000115
其中，delta为球坐标中纬度值，v2右机械臂底部坐标系z-x平面的夹角β，右机械臂末端执行器绕其关节轴旋转角度Gr，通过式(15)，(16)进行描述，
Figure BDA0002366394030000121
Figure BDA0002366394030000122
姿态控制完毕之后，需要控制机械臂末端执行器到达目标位置，其中方位角定义是从x轴正方向开始，顺时针方向到目标方向线之间的水平夹角，由于方位角θ在[0,2π]，所以需要判断(x,y)位于四象限中的象限，通过式(17)计算出方位角θ；
Figure BDA0002366394030000123
为了避免在抓取过程中与目标物体产生碰撞，设计了预抓取过程，提前设计好抓取的位置和姿态，通过式(18)进行描述，
Figure BDA0002366394030000124
式中(xgoal,ygoal,zgoal)T为目标重心点相对机械臂底部坐标系的位置坐标，(x′,y′,z′)T为相对机械臂底部坐标系的预抓取位置坐标点，L是人为设定的一段距离；θ为目标点在机械臂坐标系中x-y平面投影与x轴的夹角，σ为机械臂末端执行器z轴与机械臂底部坐标系z轴之间的夹角；利用式(11)-(18)得到预设目标位置和姿态，控制机械臂末端执行器以最佳的姿态抓取目标物；
步骤5、机械臂抓取目标后到指定关节角状态，机械臂动作客户端发送指定关节角到动作服务器，执行完毕，动作结束。

Claims (1)

1.一种基于双目视觉的机械臂抓取控制方法，其特征在于包括以下步骤：
步骤1、实验平台搭建，将双目视觉相机安装在实验平台正上方1-1.5m处，启动双目视觉相机，启动kinova机械臂并加载左右机械臂模型urdf，该模型描述了kinova机械臂的各关节位置和姿态关系，其中左机械臂安装在水平桌面左手边，其底部坐标系定义为left，右机械臂安装在水平桌面右手边，其底部坐标系定义为right，左右机械臂距离为0.8-1.2m。根坐标系在左右机械臂中间位置，定义为root，双目视觉相机世界坐标系定义为map，以root为根坐标系，利用平移和旋转确定left、right和map坐标位置和姿态，以根坐标系原点为中心点，旋转到左机械臂底部坐标系的旋转矩阵为R1，平移矩阵为T1，旋转到右机械臂底部坐标系的旋转矩阵为R2，平移矩阵T2，旋转到双目视觉相机坐标系的旋转矩阵为R3，平移矩阵为T3，并通过式(1)至(3)进行描述，
left＝R1*root+T1 (1)
right＝R2*root+T2 (2)
map＝R3*root+T3 (3)
步骤2、目标物倾斜放置时的位置和姿态的识别，具体包括以下子步骤：
(a)订阅双目视觉相机发布的RGB图像，对图片进行二值化，由于图像外杂质物体较多，人为设置一个像素框，利用opencv中grabCut算法将框内的前景提取出来，其中设置迭代次数为7-12；
(b)将提取的前景图像像素pi(u,v),i∈N(N为前景提取到的像素点个数)与深度图像进行匹配得到世界坐标系下对应的世界坐标Pi＝[XW YW ZW]T,i∈N。将N个Pi存放在容器中，从所有像素坐标系映射到双目视觉相机的世界坐标的映射关系，通过式(4)进行描述，
Figure FDA0002366394020000011
其中，z为双目视觉相机深度值，
Figure FDA0002366394020000021
f为双目视觉相机的焦距，dx为横向单位像素所对应的物理距离，单位为mm，dy为纵向单位像素所对应的物理距离，单位为mm，式中参数u0为图像像素横向中点，v0为图像像素纵向中点。将转换后的世界坐标存储在容器2中，遍历整个容器2，求出所有元素中x值最小的元素世界坐标P1＝[X,Y,Z],X∈min{XW}，该元素代表两个圆弧最高顶点P1，以P1为中心点；
(c)以瓶子长度L为约束条件，遍历容器2，找出与中心点P1距离满足长度为L约束条件的所有点，存放于容器3中，再从容器3中查找x轴坐标最小值所对应的目标P2，即后面圆弧最高顶点P2，方向向量v，通过式(5)计算得到，
v＝P1-P2 (5)
(d)计算目标物重心点Q3，设Q2点为子步骤(c)中P1和P2的中点，其中瓶子截面半径设为r，Q2与正下方点Q之间的距离为h，Q，Q2，Q3组成直角三角形的几何关系并且满足Q3-Q＝kv,k≠0，vx，vy，vz分别为方向向量v在双目视觉相机世界坐标系x轴，y轴，z轴上的投影，通过式(6)至(8)进行描述，
Figure FDA0002366394020000022
Figure FDA0002366394020000023
Figure FDA0002366394020000024
(e)方向向量v的参考系是双目视觉相机世界坐标系map，要将右目标物的方向向量v转换到右机械臂底部坐标系right中，方向向量为v2，转换公式通过式(9)进行描述，
Figure FDA0002366394020000025
其中，
Figure FDA0002366394020000026
为双目视觉相机世界坐标系map转换到右机械臂底部坐标系right的旋转矩阵，计算方向向量v2与right各个坐标平面的夹角，通过式(10)进行描述，
Figure FDA0002366394020000031
式中，v2x，v2y，v2z，分别为v2在右机械臂底座坐标系下x轴、y轴、z轴投影，α为方向向量v2与x-y平面夹角，β为方向向量v2与x-z平面夹角，χ为方向向量v2与y-z平面夹角；
步骤3、目标物水平放置时的位置和姿态的识别，具体包括以下子步骤：
(a)需要利用颜色分割的方式提取出左目标物的二值图像，这是由于这一过程会存在颜色相同的无关物体的干扰，也会提取到二值图像里面；
(b)为了解决上述中关于相同颜色对目标物识别造成干扰的问题，采取目标物的形状作为对象识别辅助条件，利用形状特征对提取的图像进行筛选，保留具有四边形特征的二值图像；
(c)使用opencv中多边形逼近轮廓函数cv::approxPoly()，得到二维数组M×N,其中M表示多边形组数，N表示每组多边型边界像素点的集合；
(d)判断是否为四边形，因为所需要提取出来的目标物体是四边形，但是实际工程上可能会存在一定的偏差，在这里选择目标物体的边数为3-5，在这个范围内都可认为符合目标物体的形状特征，予以保留；
(e)使用findContours()函数求出所有形状信息，存储轮廓的向量；
(f)使用approxPolyDP()函数求解每个轮廓的边数，遍历整个容器，将边数不为3-5的干扰物删除，保留容器中形状近似为四边形的元素，则使用polylines()函数画出边框，并求出重心点goal的像素坐标，再利用深度图像匹配计算出重心点在双目视觉相机的世界坐标系下的位置坐标；
(g)当目标物的轮廓信息确定后，设矩形框四个顶点为A,B,C,D，若AB为最长边，选取AB边顶点坐标之差作为方向向量，将左目标物方向向量v转换到左机械臂底座坐标系下v1，计算方向向量与左机械臂底座坐标系x-z平面夹角为
Figure FDA0002366394020000041
通过式(11)进行描述，
Figure FDA0002366394020000042
其中，v1x，v1y，v1z分别为v1在左机械臂底座坐标系下x轴、y轴、z轴上的投影；
步骤4、机械臂末端执行器识别并抓取目标物体，实验状态下的左目标物体所对应的位置为水平放置，与水平面夹角为零，式(11)求解方向向量v1与左机械臂底座坐标系x-z平面夹角Δ，左机械臂末端执行器z轴与左机械臂底部坐标系z轴夹角为170°-180°，左机械臂末端执行器绕其关节轴旋转角度Gr控制，通过式(12)进行描述，
Figure FDA0002366394020000043
实验状态下的右目标物倾斜于桌面放置，在右机械臂底部坐标系下方向向量为v2，v2与x-y平面的夹角为α，右机械臂末端执行器z轴与右机械臂底部坐标系的z轴夹角σ，求解通过式(13)，(14)进行描述，
Figure FDA0002366394020000044
Figure FDA0002366394020000045
其中，delta为球坐标中纬度值，v2右机械臂底部坐标系z-x平面的夹角β，右机械臂末端执行器绕其关节轴旋转角度Gr，通过式(15)，(16)进行描述，
Figure FDA0002366394020000051
Figure FDA0002366394020000052
姿态控制完毕之后，需要控制机械臂末端执行器到达目标位置，其中方位角定义是从x轴正方向开始，顺时针方向到目标方向线之间的水平夹角，由于方位角θ在[0,2π]，所以需要判断(x,y)位于四象限中的象限，通过式(17)计算出方位角θ；
Figure FDA0002366394020000053
为了避免在抓取过程中与目标物体产生碰撞，设计了预抓取过程，提前设计好抓取的位置和姿态，通过式(18)进行描述，
Figure FDA0002366394020000054
式中(xgoal,ygoal,zgoal)T为目标重心点相对机械臂底部坐标系的位置坐标，(x′,y′,z′)T为相对机械臂底部坐标系的预抓取位置坐标点，L是人为设定的一段距离；θ为目标点在机械臂坐标系中x-y平面投影与x轴的夹角，σ为机械臂末端执行器z轴与机械臂底部坐标系z轴之间的夹角；利用式(11)-(18)得到预设目标位置和姿态，控制机械臂末端执行器以最佳的姿态抓取目标物；
步骤5、机械臂抓取目标后到指定关节角状态，机械臂动作客户端发送指定关节角到动作服务器，执行完毕，动作结束。
CN202010037021.6A 2020-01-14 2020-01-14 一种基于双目视觉的机械臂抓取控制方法 Active CN111267095B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010037021.6A CN111267095B (zh) 2020-01-14 2020-01-14 一种基于双目视觉的机械臂抓取控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010037021.6A CN111267095B (zh) 2020-01-14 2020-01-14 一种基于双目视觉的机械臂抓取控制方法

Publications (2)

Publication Number Publication Date
CN111267095A true CN111267095A (zh) 2020-06-12
CN111267095B CN111267095B (zh) 2022-03-01

Family

ID=70994170

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010037021.6A Active CN111267095B (zh) 2020-01-14 2020-01-14 一种基于双目视觉的机械臂抓取控制方法

Country Status (1)

Country Link
CN (1) CN111267095B (zh)

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111751136A (zh) * 2020-06-29 2020-10-09 伯肯森自动化技术（上海）有限公司 一种基于双目视觉组件的pos机测试系统
CN111814634A (zh) * 2020-06-29 2020-10-23 北京百度网讯科技有限公司 一种实时距离确定方法、装置、设备及介质
CN112667823A (zh) * 2020-12-24 2021-04-16 西安电子科技大学 一种机械臂任务执行序列的语义解析方法、系统及计算机可读介质
CN112894815A (zh) * 2021-01-25 2021-06-04 西安工业大学 视觉伺服机械臂物品抓取最佳位姿检测方法
CN114516045A (zh) * 2021-11-25 2022-05-20 苏州永固智能科技有限公司 一种基于物联网的无人库房机械臂的控制方法和系统
CN115256019A (zh) * 2022-06-25 2022-11-01 北京建工集团有限责任公司 一种支护板自动拼装对齐装置
WO2023082990A1 (zh) * 2021-11-09 2023-05-19 极限人工智能有限公司 一种确定机械臂工作位姿的方法和装置
CN117163421A (zh) * 2023-11-03 2023-12-05 山东新华医疗器械股份有限公司 用于消毒供应中心的多臂协作智能打包机器人
CN117464692A (zh) * 2023-12-27 2024-01-30 中信重工机械股份有限公司 一种基于结构光视觉系统的衬板抓取机械臂控制方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH05288884A (ja) * 1992-04-13 1993-11-05 Toshiba Corp ロボット操作プラント
US5887121A (en) * 1995-04-21 1999-03-23 International Business Machines Corporation Method of constrained Cartesian control of robotic mechanisms with active and passive joints
CN102902271A (zh) * 2012-10-23 2013-01-30 上海大学 基于双目视觉的机器人目标识别与抓取系统及方法
CN108582075A (zh) * 2018-05-10 2018-09-28 江门市思远信息科技有限公司 一种智能机器人视觉自动化抓取系统
CN109102525A (zh) * 2018-07-19 2018-12-28 浙江工业大学 一种基于自适应位姿估计的移动机器人跟随控制方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH05288884A (ja) * 1992-04-13 1993-11-05 Toshiba Corp ロボット操作プラント
US5887121A (en) * 1995-04-21 1999-03-23 International Business Machines Corporation Method of constrained Cartesian control of robotic mechanisms with active and passive joints
CN102902271A (zh) * 2012-10-23 2013-01-30 上海大学 基于双目视觉的机器人目标识别与抓取系统及方法
CN108582075A (zh) * 2018-05-10 2018-09-28 江门市思远信息科技有限公司 一种智能机器人视觉自动化抓取系统
CN109102525A (zh) * 2018-07-19 2018-12-28 浙江工业大学 一种基于自适应位姿估计的移动机器人跟随控制方法

Cited By (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111814634B (zh) * 2020-06-29 2023-09-08 北京百度网讯科技有限公司 一种实时距离确定方法、装置、设备及介质
CN111814634A (zh) * 2020-06-29 2020-10-23 北京百度网讯科技有限公司 一种实时距离确定方法、装置、设备及介质
CN111751136A (zh) * 2020-06-29 2020-10-09 伯肯森自动化技术（上海）有限公司 一种基于双目视觉组件的pos机测试系统
CN112667823A (zh) * 2020-12-24 2021-04-16 西安电子科技大学 一种机械臂任务执行序列的语义解析方法、系统及计算机可读介质
CN112894815A (zh) * 2021-01-25 2021-06-04 西安工业大学 视觉伺服机械臂物品抓取最佳位姿检测方法
CN112894815B (zh) * 2021-01-25 2022-09-27 西安工业大学 视觉伺服机械臂物品抓取最佳位姿检测方法
WO2023082990A1 (zh) * 2021-11-09 2023-05-19 极限人工智能有限公司 一种确定机械臂工作位姿的方法和装置
CN114516045A (zh) * 2021-11-25 2022-05-20 苏州永固智能科技有限公司 一种基于物联网的无人库房机械臂的控制方法和系统
CN115256019A (zh) * 2022-06-25 2022-11-01 北京建工集团有限责任公司 一种支护板自动拼装对齐装置
CN115256019B (zh) * 2022-06-25 2023-07-14 北京建工集团有限责任公司 一种支护板自动拼装对齐装置
CN117163421A (zh) * 2023-11-03 2023-12-05 山东新华医疗器械股份有限公司 用于消毒供应中心的多臂协作智能打包机器人
CN117163421B (zh) * 2023-11-03 2024-01-23 山东新华医疗器械股份有限公司 用于消毒供应中心的多臂协作智能打包机器人
CN117464692A (zh) * 2023-12-27 2024-01-30 中信重工机械股份有限公司 一种基于结构光视觉系统的衬板抓取机械臂控制方法
CN117464692B (zh) * 2023-12-27 2024-03-08 中信重工机械股份有限公司 一种基于结构光视觉系统的衬板抓取机械臂控制方法

Also Published As

Publication number Publication date
CN111267095B (zh) 2022-03-01

Similar Documents

Publication Publication Date Title
CN111267095B (zh) 一种基于双目视觉的机械臂抓取控制方法
CN108555908B (zh) 一种基于rgbd相机的堆叠工件姿态识别及拾取方法
CN111775152B (zh) 基于三维测量引导机械臂抓取散乱堆叠工件的方法及系统
CN112396664B (zh) 一种单目摄像机与三维激光雷达联合标定及在线优化方法
CN111598916A (zh) 一种基于rgb-d信息的室内占据栅格地图的制备方法
CN110648367A (zh) 一种基于多层深度与彩色视觉信息的几何体目标定位方法
JP5458885B2 (ja) 物体検出方法と物体検出装置およびロボットシステム
CN113096094B (zh) 三维物体表面缺陷检测方法
CN111721259B (zh) 基于双目视觉的水下机器人回收定位方法
CN111882612A (zh) 一种基于三维激光检测车道线的车辆多尺度定位方法
CN111507390A (zh) 一种基于轮廓特征的仓储箱体识别与定位方法
CN111862201A (zh) 一种基于深度学习的空间非合作目标相对位姿估计方法
CN111178138B (zh) 基于激光点云和双目视觉的配网导线作业点检测方法及装置
CN112907735B (zh) 一种基于点云的柔性电缆识别与三维重建方法
CN113177983B (zh) 基于点云几何特征的角焊缝定位方法
CN111127613B (zh) 基于扫描电子显微镜的图像序列三维重构方法及系统
CN114241269B (zh) 一种用于岸桥自动控制的集卡视觉融合定位系统
CN110648359A (zh) 一种果实目标定位识别方法及系统
CN112734844A (zh) 一种基于正八面体的单目6d位姿估计方法
JPH07103715A (ja) 視覚に基く三次元位置および姿勢の認識方法ならびに視覚に基く三次元位置および姿勢の認識装置
CN117589145A (zh) 一种基于远距二维码的地图创建方法及系统
Ogas et al. A robotic grasping method using convnets
CN111198563A (zh) 一种用于足式机器人动态运动的地形识别方法及系统
CN116021519A (zh) 一种基于tof相机采摘机器人手眼标定方法以及装置
CN112767481B (zh) 一种基于视觉边缘特征的高精度定位及建图方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant