机器人控制系统设计（第三篇-抓取部分-手眼标定、TF坐标转换、位置发布）

在机器人视觉系统，利用AprilTag得到的标签信息，进一步得到物体识别区域的位置，发送给底盘即可驱动底盘移动到识别区域。

也需要用AprilTag进行手眼标定，把子坐标系camera_rgb_optical_frame到基坐标系base_link的相对位置关系标定，完成机器人的手眼标定过程。

      这几章内容主要集中在机器人的视觉控制、三维目标抓取这一部分，也算顺着之前的CT-LineMod算法继续谈一下计算机视觉这一部分的内容。第一节这个总体框架设计在接下来的视觉、抓取、导航部分都会放在前面，以帮助读者理解一下各个部分的功能以及把握整体性。

由于机器人主机NUC并不知道自身机械臂、深度相机的相对位置关系，因此需要借助手眼标定系统完成机器人手（机械臂和机械手）眼（深度相机RealsenseZR300）坐标系的标定。

其中，基坐标系base_tree即主机IntelNUC所在位置为中心的三维坐标系，在基坐标系下则是主机以及机械手臂控制系统所在的坐标系。而相机坐标系camera_tree即以深度相机ZR300所在位置为中心的三维坐标系。

在ROS操作环境中两个坐标系隶属于不同的坐标树。

因此，可以通过标签定位的方法，在机械手上贴有独特信息标签，利用深度相机识别出标签所在的位置（position）和方向（orientation）信息。

通过ROS环境自带的hand_eye标定包得到机械手marker_frame坐标系相对于base_tree的位置关系，进而得到子坐标系camera_rgb_optical_frame到基坐标系base_link的相对位置关系，完成机器人的手眼标定过程。

之后，在真实场景下识别到的三维物体通过主机控制深度相机运行目标识别算法得到在camera_tree坐标系下的物体位置方向信息，通过TF坐标转换模块将识别结果转换到base_tree坐标系，进而用于机械臂的规划及抓取。

手眼标定模块分为深度相机在机械手上以及深度相机在底座上两种情况，本文属于后者。

在标定过程中共采集17组标签在不同位置和方向时的深度相机信息，通过比较base_tree坐标系下机械手移动到对应位置时所处的位置信息，得到base_tree和camera_tree的17组相对位置关系。

通过计算平均值，得到两坐标系的平移和旋转关系。如下图所示为一组坐标位置关系及最终的base_tree和camera_tree的坐标系转换关系

在得到base_tree与camera_tree坐标系的位置转换关系后，需要利用两坐标系平移旋转关系，创建tf_broadcaster来在ROS环境广播其位置关系，并在tf::transform函数中存储两者坐标关系。

之后利用TF坐标转换把深度相机识别到的物体位置和方向信息转换到机械手坐标系下进行规划抓取。

首先，在主机IntelNUC中需要将深度相机识别到的物体位置和方向信息存储所在位置RecognizedObjectArray数组中提取出来，利用rospy.Subscriber函数判断是否有该类信息即可进一步判断是否识别到物体。

接着，定义一用于存放识别到的物体位置信息的临时数组data.object[]，定义一节点tf_listener用于订阅深度相机data.object[]的物体位置消息real_pose，并利用tf::transform中的坐标转换关系将物体位置信息从camera_tree坐标系下转换为base_tree坐标系下。

如下图所示为实时坐标转换关系图。

之后，在tf_listener节点设置发布程序，将转换后的基坐标系下的物体坐标transed_pose发布出去。

最后，机械臂UR10中的节点将订阅tf_listener节点发布的消息，利用Moveit!完成机械手臂的运动规划与抓取。

整个过程中物体位置的消息均为geometry_msgs::PoseStamped消息类型的。如图所示为控制系统话题信息图（不包含机器人底盘）及机器人控制系统全部坐标系连接关系图。

其中，控制系统在执行抓取时的程序主要包含移动底盘驱动到识别区域、物体位置判断、位姿获取、位置偏移计算、机械臂运动规划到达偏移位置处、从偏移位置处移动到真实位置处、机械手手爪闭合到罐体直径宽度、机械手抓取、物体移动放置与桌子平面上、机械手臂收放以及移动底盘返回到停放区域。

      Transform话题所发布的物体位置转换消息如图所示。其中包含物体位置信息，空间位置信息x、y、z以及方向信息x、y、z、w七个位置信息以及坐标对应所在的坐标系。

另外通过增加软件限定的方式，限制机械手臂的移动范围，使机械臂只可以针对深度相机前的范围内进行安全的抓取操作，如超过限制范围系统会重新选取最佳抓取路线，并且在仿真中机械手臂模型会变成红色以警示仿真操作合适，这样也方便了之后增加移动机器人的其他任务的可能（如按电梯等）。

机器人控制系统深度相机与机械手臂部分运行程序后的所有ROS话题如下图所示。其中，camera/driver话题用于深度相机的初始化驱动话题，ur_driver为机械手臂的驱动话题。

将Camera_info消息输入object_recognition_server话题后，识别到的物体6D位姿估计通过recognized_object_array消息传递到tf_listener订阅话题接收位置消息，并通过tf_broadcaster广播到ROS的各个部分模块，实现整个机器人的通信。