一种基于双目视觉的双机械臂协作控制方法与流程

本发明涉及一种基于双目视觉的双机械臂协作控制方法，属于机器人学和计算机视觉技术领域。

背景技术：

自1961年美国研制出第一台机器人以来，机器人技术历经五十多年的发展，已经取得了长足的进步。随着机器人的相关技术日益娴熟，机器人已经渗入到各行各业中，逐渐改变着人们的生产生活方式并为世界的工业发展贡献重要的力量。

单个机器人在结构化场景的应用存在很大的局限性，己无法满足当前智能化的需求，而双臂协调系统具有负载能力强、工作效率高、工作空间大等优势，得到了国内外研究人员的广泛关注。但双臂的协调控制是相对困难的，双臂协调控制能力的高低决定了机器人智能化的程度。

计算机视觉是在图像处理的基础上发展而来的新兴领域，是一门通过研究图像或视频观察周围世界的学科。立体视觉的基本原理是通过两个或者多个视点观察同一景物并获取不同视角下的图像，利用三角测量原理计算图像像素间的位置偏差以提取三维信息。

技术实现要素：

针对现有技术中存在的不足，本发明目的是提供一种基于双目视觉的双机械臂协作控制方法。该方法是利用两个机械手臂组成一个双臂系统，双目视觉对环境进行感知，得到多个目标的三维空间位置，驱动双机械臂完成协作。

为了实现上述发明目的，解决己有技术中存在的问题，本发明采取的技术方案是：一种基于双目视觉的双机械臂协作控制方法，包括以下步骤：

步骤1、安装双目视觉相机和双机械臂，其中双目视觉相机自上而下垂直放置，要求双目视觉相机位置与识别区域的距离为0.8-20.0m，安装双机械臂，要求双机械臂底座中心相距为0.7-0.9m，且双机械臂中心与双目视觉相机的中心位于同一平面，该平面垂直于水平面，便于标定；

步骤2、获得多目标物体在双目视觉相机坐标系的相对位置信息，具体包括以下子步骤：

(a)订阅双目视觉相机图像话题获得图像，在hsv色彩空间采用颜色特征分别识别一个用红色标识的水瓶和一个用绿色标识的水杯，取识别到的目标物体图像的中心点为测距点，该点的像素坐标为(u，v)，与深度图像进行匹配获得该点的深度信息d；

(b)获取目标物体在双目视觉相机坐标系中的三维空间位置,设目标物体在双目视觉相机坐标系中的三维空间位置为(x，y，z),目标点坐标(u，v)为三维坐标在成像平面的成像点，二者转换关系通过公式(1)进行描述，

式中zc表示子步骤(a)获得的深度信息d，f表示双目视觉相机摄像头焦距，dx表示每个像素在x轴上的物理尺寸，dy表示每个像素在y轴上的物理尺寸，最终获得目标物体在双目视觉相机坐标系的三维空间位置为(x，y，z)；

步骤3、双机械臂协作设计实现，具体包括以下子步骤：

(a)构建双机械臂urdf模型，在两个机械臂底座中点位置建立三维原点坐标系，在各自的底座位置分别建立各自的根坐标系，在模型中建立原点坐标系与各自根坐标系的位置姿态关系，然后在各自的根坐标系下按照单机械臂模型建立各关节与根坐标的关系；

(b)双机械臂联合驱动，在驱动文件中首先加载双机械臂模型，再分为两个group，一个为左机械臂，另一个为右机械臂，在各自的group中加载各自驱动程序，并运行robot_state_publish和joint_state_publish节点，将各自机械臂模型和关节信息发布出去，并在rviz人机交互平台显示，由于两个机械臂各关节名称不同，就可向不同的话题下达控制命令，从而完成双机械臂的联合驱动；

步骤4、计算目标物体在双机械臂坐标中的空间位置，在步骤1中，双目视觉相机相对于双机械臂的位置关系是确定的，其中将位于双机械臂底座中心的根坐标系root绕其x轴正半轴顺时针旋转90°，再绕z轴正半轴逆时针旋转90°得到双目视觉相机的根坐标系map，再将二者的关系利用机器人操作系统(ros)中tf坐标变换机制发布并在rviz人机交互平台；同样利用tf可以完成目标物体位置在双目视觉相机坐标系到双机械臂根坐标系的转换，得到目标物体在双机械臂根坐标系的位置；

步骤5、计算双机械臂的抓取姿态，由方位角、极角和旋转角组成的欧拉角确定姿态，由步骤2子步骤(b)获得目标物体在双目视觉相机坐标系的三维空间位置(x,y,z)得到角度信息，其中：方位角α是从x轴正方向开始，顺时针方向到目标物体方向线之间的水平夹角，范围为[0,2π]；极角β是指z轴正半轴双机械臂末端向量的角度，范围为[0,π]，由于目标物体水平放置，即β＝π；旋转角ω是指双机械臂最末端关节的转动角度，范围是[0,2π]，由于目标物体水平放置，即ω＝π/2；

步骤6、完成双机械臂联合动作，具体包括以下子步骤：

(a)设定右机械臂抓取水杯，左机械臂抓取水瓶，再进行一次坐标转换得到水杯相对于右机械臂底座的坐标位置和水瓶相对于左机械臂底座的坐标位置；

(b)根据不同的话题同时发布控制命令，双机械臂同时对各自的目标物体进行抓取，根据得到的目标物体坐标设置一个预先抓取位置，即z轴坐标增加0.1m，避免在双机械臂抓取过程中触碰到物体，并提前调整好双机械臂的姿态，再将真实的目标物体坐标发送至双机械臂，双机械臂到达目标位置，完成抓取；

(c)设定空间一位置，右机械臂到达这一位置，使右机械臂关节向右旋转到达目标位置，根据这一位置及水杯、水瓶的大小计算出左机械臂应到达的位置，使左机械臂关节向左旋转到达目标位置，左、右机械臂各自向自己外侧旋转到达位置，避免发生碰撞，最后转动左机械臂最末端关节完成倒水动作。

本发明有益效果是：一种基于双目视觉的双机械臂协作控制方法，包括以下步骤：(1)安装双目视觉相机和双机械臂，(2)获得多目标物体在双目视觉相机坐标系的相对位置信息，(3)双机械臂协作设计实现，(4)计算目标物体在双机械臂坐标中的空间位置，(5)计算双机械臂的抓取姿态，(6)完成双机械臂联合动作。与已有技术相比，本发明具有以下优点：一是，相比传统的单机械臂抓取，双机械臂系统具有更大的灵活性，能够完成单机械臂完成不了的任务；二是，建立了双机械臂的模型，可有效地避免双机械臂发生碰撞；三是，抓取物体时，采用了先到达预先位置的方式，避免触碰到目标物体。

附图说明

图1是本发明方法步骤流程图。

图2是双目视觉相机和双机械臂安装效果示意图。

图3是目标物体识别效果图。

图4是双机械臂系统坐标系转换图。

图5是双机械臂模型图。

图6是双机械臂及双目视觉相机的坐标系转换图。

图7是双机械臂抓取效果图。

图中：(a)是双机械臂到达预先位置效果图。

(b)是双机械臂到达目标位置效果图。

(c)是双机械臂抓取物体效果图。

图8是双机械臂到达指定位置效果图。

图中：(a)是右机械臂到达指定位置效果图。

(b)是左机械臂到达指定位置效果图。

图9是双机械臂完成倒水动作效果图。

具体实施方式

下面结合附图对本发明作进一步说明。

如图1所示，一种基于双目视觉的双机械臂协作控制方法，包括以下步骤：

步骤1、安装双目视觉相机和双机械臂，其中双目视觉相机自上而下垂直放置，要求双目视觉相机位置与识别区域的距离为10.0m，安装双机械臂，要求双机械臂底座中心相距为0.8m，且双机械臂中心与双目视觉相机的中心位于同一平面，该平面垂直于水平面，便于标定，详细安装如图2所示。

步骤2、获得多目标物体在双目视觉相机坐标系的相对位置信息，具体包括以下子步骤：

(a)订阅双目视觉相机图像话题获得图像，在hsv色彩空间采用颜色特征分别识别一个用红色标识的水瓶和一个用绿色标识的水杯，获得二者的轮廓信息如图3所示，取识别到的目标物体图像的中心点为测距点，该点的像素坐标为(u，v)，与深度图像进行匹配获得该点的深度信息d。

(b)获取目标物体在双目视觉相机坐标系中的三维空间位置,设目标物体在双目视觉相机坐标系中的三维空间位置为(x，y，z),目标点坐标(u，v)为三维坐标在成像平面的成像点，二者转换关系通过公式(1)进行描述，

式中zc表示子步骤(a)获得的深度信息d，f表示双目视觉相机摄像头焦距，dx表示每个像素在x轴上的物理尺寸，dy表示每个像素在y轴上的物理尺寸，最终获得目标物体在双目视觉相机坐标系的三维空间位置为(x，y，z)。

步骤3、双机械臂协作设计实现，具体包括以下子步骤：

(a)构建双机械臂urdf模型，在两个机械臂底座中点位置建立三维原点坐标系，在各自的底座位置分别建立各自的根坐标系，在模型中建立原点坐标系与各自根坐标系的位置姿态关系，如图4所示；然后在各自的根坐标系下按照单机械臂模型建立各关节与根坐标的关系，最后得到的双机械臂模型如图5所示。

(b)双机械臂联合驱动，在驱动文件中首先加载双机械臂模型，再分为两个group，一个为左机械臂，另一个为右机械臂，在各自的group中加载各自驱动程序，并运行robot_state_publish和joint_state_publish节点，将各自机械臂模型和关节信息发布出去，并在rviz人机交互平台显示，由于两个机械臂各关节名称不同，就可向不同的话题下达控制命令，从而完成双机械臂的联合驱动。

步骤4、计算目标物体在双机械臂坐标中的空间位置，在步骤1中，双目视觉相机相对于双机械臂的位置关系是确定的，其中将位于双机械臂底座中心的根坐标系root绕其x轴正半轴顺时针旋转90°，再绕z轴正半轴逆时针旋转90°得到双目视觉相机的根坐标系map，再将二者的关系利用机器人操作系统(ros)中tf坐标变换机制发布并在rviz人机交互平台，如图6所示。同样利用tf可以完成目标物体位置在双目视觉相机坐标系到双机械臂根坐标系的转换，得到目标物体在双机械臂根坐标系的位置。

步骤5、计算双机械臂的抓取姿态，由方位角、极角和旋转角组成的欧拉角确定姿态，由步骤2子步骤(b)获得目标物体在双目视觉相机坐标系的三维空间位置(x,y,z)得到角度信息，其中：方位角α是从x轴正方向开始，顺时针方向到目标物体方向线之间的水平夹角，范围为[0,2π]；极角β是指z轴正半轴双机械臂末端向量的角度，范围为[0,π]，由于目标物体水平放置，即β＝π；旋转角ω是指双机械臂最末端关节的转动角度，范围是[0,2π]，由于目标物体水平放置，即ω＝π/2。

步骤6、完成双机械臂联合动作，具体包括以下子步骤：

(a)设定右机械臂抓取水杯，左机械臂抓取水瓶，再进行一次坐标转换得到水杯相对于右机械臂底座的坐标位置和水瓶相对于左机械臂底座的坐标位置；

(b)根据不同的话题同时发布控制命令，双机械臂同时对各自的目标物体进行抓取，根据得到的目标物体坐标设置一个预先抓取位置，即z轴坐标增加0.1m，避免在双机械臂抓取过程中触碰到物体，并提前调整好双机械臂的姿态，如图7(a)所示；再将真实的目标物体坐标发送至双机械臂，双机械臂到达目标位置，如图7(b)所示，完成抓取，如图7(c)所示。

(c)设定空间一位置，右机械臂到达这一位置，使右机械臂关节向右旋转到达目标位置，如图8(a)所示；根据这一位置及水杯、水瓶的大小计算出左机械臂应到达的位置，使左机械臂关节向左旋转到达目标位置，如图8(b)所示；左、右机械臂各自向自己外侧旋转到达位置，避免发生碰撞，最后转动左机械臂最末端关节完成倒水动作，如图9所示。