[发明专利]一种基于双目视觉的双机械臂协作控制方法

权利要求书

1.一种基于双目视觉的双机械臂协作控制方法，其特征在于包括以下步骤：

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

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

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

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

式中Z_c表示子步骤(a)获得的深度信息d，f表示双目视觉相机摄像头焦距，d_x表示每个像素在x轴上的物理尺寸，d_y表示每个像素在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)设定空间一位置，右机械臂到达这一位置，使右机械臂关节向右旋转到达目标位置，根据这一位置及水杯、水瓶的大小计算出左机械臂应到达的位置，使左机械臂关节向左旋转到达目标位置，左、右机械臂各自向自己外侧旋转到达位置，避免发生碰撞，最后转动左机械臂最末端关节完成倒水动作。