CN111267095A - 一种基于双目视觉的机械臂抓取控制方法 - Google Patents
一种基于双目视觉的机械臂抓取控制方法 Download PDFInfo
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 25
- 230000036544 posture Effects 0.000 claims abstract description 37
- 239000012636 effector Substances 0.000 claims abstract description 29
- 239000011159 matrix material Substances 0.000 claims description 21
- 230000005484 gravity Effects 0.000 claims description 13
- 238000013519 translation Methods 0.000 claims description 12
- 238000002474 experimental method Methods 0.000 claims description 6
- 238000006243 chemical reaction Methods 0.000 claims description 4
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 3
- 238000013507 mapping Methods 0.000 claims description 3
- 230000011218 segmentation Effects 0.000 claims description 3
- 230000000694 effects Effects 0.000 abstract description 9
- 238000001514 detection method Methods 0.000 abstract description 5
- 239000003086 colorant Substances 0.000 abstract description 3
- 238000000605 extraction Methods 0.000 description 6
- 238000010586 diagram Methods 0.000 description 5
- 230000001678 irradiating effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
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)进行描述,
其中,z为双目视觉相机深度值,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)进行描述,
(e)方向向量v的参考系是双目视觉相机世界坐标系map,要将右目标物的方向向量v转换到右机械臂底部坐标系right中,方向向量为v2,转换公式通过式(9)进行描述,
式中,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平面夹角为通过式(11)进行描述,
其中,v1x,v1y,v1z分别为v1在左机械臂底座坐标系下x轴、y轴、z轴上的投影;
步骤4、机械臂末端执行器识别并抓取目标物体,实验状态下的左目标物体所对应的位置为水平放置,与水平面夹角为零,式(11)求解方向向量v1与左机械臂底座坐标系x-z平面夹角Δ,左机械臂末端执行器z轴与左机械臂底部坐标系z轴夹角为170°-180°,左机械臂末端执行器绕其关节轴旋转角度Gr控制,通过式(12)进行描述,
实验状态下的右目标物倾斜于桌面放置,在右机械臂底部坐标系下方向向量为v2,v2与x-y平面的夹角为α,右机械臂末端执行器z轴与右机械臂底部坐标系的z轴夹角σ,求解通过式(13),(14)进行描述,
其中,delta为球坐标中纬度值,v2右机械臂底部坐标系z-x平面的夹角β,右机械臂末端执行器绕其关节轴旋转角度Gr,通过式(15),(16)进行描述,
姿态控制完毕之后,需要控制机械臂末端执行器到达目标位置,其中方位角定义是从x轴正方向开始,顺时针方向到目标方向线之间的水平夹角,由于方位角θ在[0,2π],所以需要判断(x,y)位于四象限中的象限,通过式(17)计算出方位角θ;
为了避免在抓取过程中与目标物体产生碰撞,设计了预抓取过程,提前设计好抓取的位置和姿态,通过式(18)进行描述,
式中(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)进行描述,
其中,z为双目视觉相机深度值,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)进行描述,
(e)方向向量v的参考系是双目视觉相机世界坐标系map,要将右目标物的方向向量v转换到右机械臂底部坐标系right中,方向向量为v2,转换公式通过式(9)进行描述,
式中,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平面夹角为通过式(11)进行描述,
其中,v1x,v1y,v1z分别为v1在左机械臂底座坐标系下x轴、y轴、z轴上的投影;
步骤4、机械臂末端执行器识别并抓取目标物体,实验状态下的左目标物体所对应的位置为水平放置,与水平面夹角为零,式(11)求解方向向量v1与左机械臂底座坐标系x-z平面夹角Δ,左机械臂末端执行器z轴与左机械臂底部坐标系z轴夹角为170°-180°,左机械臂末端执行器绕其关节轴旋转角度Gr控制,通过式(12)进行描述,
实验状态下的右目标物倾斜于桌面放置,在右机械臂底部坐标系下方向向量为v2,v2与x-y平面的夹角为α,右机械臂末端执行器z轴与右机械臂底部坐标系的z轴夹角σ,求解通过式(13),(14)进行描述,
其中,delta为球坐标中纬度值,v2右机械臂底部坐标系z-x平面的夹角β,右机械臂末端执行器绕其关节轴旋转角度Gr,通过式(15),(16)进行描述,
姿态控制完毕之后,需要控制机械臂末端执行器到达目标位置,其中方位角定义是从x轴正方向开始,顺时针方向到目标方向线之间的水平夹角,由于方位角θ在[0,2π],所以需要判断(x,y)位于四象限中的象限,通过式(17)计算出方位角θ;
为了避免在抓取过程中与目标物体产生碰撞,设计了预抓取过程,提前设计好抓取的位置和姿态,通过式(18)进行描述,
式中(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)进行描述,
其中,z为双目视觉相机深度值,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)进行描述,
(e)方向向量v的参考系是双目视觉相机世界坐标系map,要将右目标物的方向向量v转换到右机械臂底部坐标系right中,方向向量为v2,转换公式通过式(9)进行描述,
式中,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平面夹角为通过式(11)进行描述,
其中,v1x,v1y,v1z分别为v1在左机械臂底座坐标系下x轴、y轴、z轴上的投影;
步骤4、机械臂末端执行器识别并抓取目标物体,实验状态下的左目标物体所对应的位置为水平放置,与水平面夹角为零,式(11)求解方向向量v1与左机械臂底座坐标系x-z平面夹角Δ,左机械臂末端执行器z轴与左机械臂底部坐标系z轴夹角为170°-180°,左机械臂末端执行器绕其关节轴旋转角度Gr控制,通过式(12)进行描述,
实验状态下的右目标物倾斜于桌面放置,在右机械臂底部坐标系下方向向量为v2,v2与x-y平面的夹角为α,右机械臂末端执行器z轴与右机械臂底部坐标系的z轴夹角σ,求解通过式(13),(14)进行描述,
其中,delta为球坐标中纬度值,v2右机械臂底部坐标系z-x平面的夹角β,右机械臂末端执行器绕其关节轴旋转角度Gr,通过式(15),(16)进行描述,
姿态控制完毕之后,需要控制机械臂末端执行器到达目标位置,其中方位角定义是从x轴正方向开始,顺时针方向到目标方向线之间的水平夹角,由于方位角θ在[0,2π],所以需要判断(x,y)位于四象限中的象限,通过式(17)计算出方位角θ;
为了避免在抓取过程中与目标物体产生碰撞,设计了预抓取过程,提前设计好抓取的位置和姿态,通过式(18)进行描述,
式中(xgoal,ygoal,zgoal)T为目标重心点相对机械臂底部坐标系的位置坐标,(x′,y′,z′)T为相对机械臂底部坐标系的预抓取位置坐标点,L是人为设定的一段距离;θ为目标点在机械臂坐标系中x-y平面投影与x轴的夹角,σ为机械臂末端执行器z轴与机械臂底部坐标系z轴之间的夹角;利用式(11)-(18)得到预设目标位置和姿态,控制机械臂末端执行器以最佳的姿态抓取目标物;
步骤5、机械臂抓取目标后到指定关节角状态,机械臂动作客户端发送指定关节角到动作服务器,执行完毕,动作结束。
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 (10)
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 | 西安工业大学 | 视觉伺服机械臂物品抓取最佳位姿检测方法 |
CN114485397A (zh) * | 2022-01-11 | 2022-05-13 | 重庆固高科技长江研究院有限公司 | 基于移动机构运动轨迹的线激光扫描状态控制方法 |
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)
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 | 浙江工业大学 | 一种基于自适应位姿估计的移动机器人跟随控制方法 |
-
2020
- 2020-01-14 CN CN202010037021.6A patent/CN111267095B/zh active Active
Patent Citations (5)
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 (15)
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 | 北京百度网讯科技有限公司 | 一种实时距离确定方法、装置、设备及介质 |
CN111814634B (zh) * | 2020-06-29 | 2023-09-08 | 北京百度网讯科技有限公司 | 一种实时距离确定方法、装置、设备及介质 |
CN112667823A (zh) * | 2020-12-24 | 2021-04-16 | 西安电子科技大学 | 一种机械臂任务执行序列的语义解析方法、***及计算机可读介质 |
CN112894815B (zh) * | 2021-01-25 | 2022-09-27 | 西安工业大学 | 视觉伺服机械臂物品抓取最佳位姿检测方法 |
CN112894815A (zh) * | 2021-01-25 | 2021-06-04 | 西安工业大学 | 视觉伺服机械臂物品抓取最佳位姿检测方法 |
WO2023082990A1 (zh) * | 2021-11-09 | 2023-05-19 | 极限人工智能有限公司 | 一种确定机械臂工作位姿的方法和装置 |
CN114516045A (zh) * | 2021-11-25 | 2022-05-20 | 苏州永固智能科技有限公司 | 一种基于物联网的无人库房机械臂的控制方法和*** |
CN114485397A (zh) * | 2022-01-11 | 2022-05-13 | 重庆固高科技长江研究院有限公司 | 基于移动机构运动轨迹的线激光扫描状态控制方法 |
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相机的堆叠工件姿态识别及拾取方法 | |
CN107590836B (zh) | 一种基于Kinect的充电桩动态识别与定位方法及*** | |
CN111775152B (zh) | 基于三维测量引导机械臂抓取散乱堆叠工件的方法及*** | |
CN112396664B (zh) | 一种单目摄像机与三维激光雷达联合标定及在线优化方法 | |
CN110340891B (zh) | 基于点云模板匹配技术的机械臂定位抓取***及方法 | |
CN111598916A (zh) | 一种基于rgb-d信息的室内占据栅格地图的制备方法 | |
CN111721259B (zh) | 基于双目视觉的水下机器人回收定位方法 | |
CN113096094B (zh) | 三维物体表面缺陷检测方法 | |
CN111862201A (zh) | 一种基于深度学习的空间非合作目标相对位姿估计方法 | |
CN112907735B (zh) | 一种基于点云的柔性电缆识别与三维重建方法 | |
CN111178138B (zh) | 基于激光点云和双目视觉的配网导线作业点检测方法及装置 | |
JPWO2009028489A1 (ja) | 物体検出方法と物体検出装置およびロボットシステム | |
CN113177983B (zh) | 基于点云几何特征的角焊缝定位方法 | |
CN111127613B (zh) | 基于扫描电子显微镜的图像序列三维重构方法及*** | |
CN114241269B (zh) | 一种用于岸桥自动控制的集卡视觉融合定位*** | |
CN111784655A (zh) | 一种水下机器人回收定位方法 | |
CN112819883A (zh) | 一种规则对象检测及定位方法 | |
CN111402330A (zh) | 一种基于平面靶标的激光线关键点提取方法 | |
CN112734844A (zh) | 一种基于正八面体的单目6d位姿估计方法 | |
CN112767481B (zh) | 一种基于视觉边缘特征的高精度定位及建图方法 | |
CN111198563B (zh) | 一种用于足式机器人动态运动的地形识别方法及*** | |
Ogas et al. | A robotic grasping method using convnets | |
CN116579955B (zh) | 一种新能源电芯焊缝反光点去噪和点云补全方法及*** | |
CN117589145A (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 |