机器人～ur5-kinect2自动手眼标定

我们使用Universal Robots公司的UR5工业机器人和Microsoft公司的Kinect v2搭建了一个机械手臂抓取统，这是一个典型的eye to hand结构。为了实现抓取功能，我们首先要对这个系统进行手眼标定，这个标定的目的是确定相机和机器人的相对位置和姿态，从而实现将相机拍到的物品位姿转换到机器人坐标系下进行抓取。

技术总览

在机械臂抓取的过程中，关键的技术是机器视觉，主要分为三个部分：

• 物体识别（Object Recognition）：在图像中检测到物体类型等，这跟 CV 的研究有很大一部分交叉；
• 位姿估计（Pose Estimation）：计算出物体在摄像机坐标系下的位置和姿态，对于机器人而言，需要抓取东西，不仅要知道这是什么，也需要知道它具体在哪里；
• 相机标定（Camera Calibration）：因为上面做的只是计算了物体在相机坐标系下的坐标，我们还需要确定相机跟机器人的相对位置和姿态，这样才可以将物体位姿转换到机器人位姿。

手眼标定在理论上讲是一个被研究地非常成熟的问题，根据相机位置的不同，它主要又分为两种，分别是：

• Eye to Hand：相机与机器人极坐标系固连，不随机械臂运动而运动
• Eye in Hand：相机固连在机械臂上，随机械臂运动而运动

上面的引用[1]在理论上对手眼标定进行了一个非常详细的表述，所以我们不再赘述。在这篇文章中，我们主要是从实践的角度出发，具体地看看如何在一个真实的eye to hand机器人系统中进行手眼标定。

---

软硬件依赖

硬件：

1. UR5工业机器人
2. Kinect V2

软件：

1. ros-kinetic-visp
2. vision_visp
3. aruco_ros
4. easy_handeye

安装步骤

安装ros-kinetic-visp包：

$ sudo apt-get install ros-kinetic-visp

安装vision_visp包

catkin_ws/src$ git clone -b kinetic-devel https://github.com/lagadic/vision_visp.git
catkin_ws$ catkin_make --pkg visp_hand2eye_calibration

安装aruco_ros包

catkin_ws/src$ git clone -b kinetic-devel https://github.com/pal-robotics/aruco_ros
catkin_ws$ catkin_make

安装easy_handeye包

catkin_ws/src$ git clone https://github.com/IFL-CAMP/easy_handeye
catkin_ws$ catkin_make

---

编写启动文件并启动

为了能够同时启动UR5，Kinect和标定程序来进行实际的标定任务，我们需要写一个launch文件。为了保持简单，我们直接在easy_handeye作者所给的样例文件上按照自己的情况进行修改就好。

下面是我的launch文件：

<!-- filename: calibrate.launch -->

<launch>
    <arg name="namespace_prefix" default="chessbot_calibration" />
 
    <arg name="robot_ip" doc="The IP address of the UR5 robot" />
        <!--<arg name="marker_frame" default="aruco_marker_frame"/>-->
    <arg name="marker_size" doc="Size of the ArUco marker used, in meters" default="0.1" />
    <arg name="marker_id" doc="The ID of the ArUco marker used" default="100" />
    <!-- start the Kinect -->
    <include file="$(find kinect2_bridge)/launch/kinect2_bridge.launch" />
    <!--    <arg name="depth_registration" value="true" />
    </include>-->
 
    <!-- start ArUco -->
    <node name="aruco_tracker" pkg="aruco_ros" type="single">
        <remap from="/camera_info" to="/kinect2/hd/camera_info" />
        <remap from="/image" to="/kinect2/hd/image_color_rect" />
        <param name="image_is_rectified" value="true"/>
        <param name="marker_size"        value="$(arg marker_size)"/>
        <param name="marker_id"          value="$(arg marker_id)"/>
        <param name="reference_frame"    value="kinect2_rgb_optical_frame"/>
        <param name="camera_frame"       value="kinect2_rgb_optical_frame"/>
        <param name="marker_frame"       value="camera_marker" />
    </node>
 
    <!-- start the robot -->
    <include file="$(find ur_modern_driver)/launch/ur5_bringup.launch">
        <arg name="limited" value="true" />
        <arg name="robot_ip" value="192.168.199.106" />
    </include>
    <include file="$(find ur5_moveit_config)/launch/ur5_moveit_planning_execution.launch">
        <arg name="limited" value="true" />
    </include>
 
    <!-- start easy_handeye -->
    <include file="$(find easy_handeye)/launch/calibrate.launch" >
        <arg name="namespace_prefix" value="$(arg namespace_prefix)" />
        <arg name="eye_on_hand" value="false" />
 
        <arg name="tracking_base_frame" value="kinect2_rgb_optical_frame" />
        <arg name="tracking_marker_frame" value="camera_marker" />
        <arg name="robot_base_frame" value="base_link" />
        <arg name="robot_effector_frame" value="wrist_3_link" />
 
        <arg name="freehand_robot_movement" value="false" />
        <arg name="robot_velocity_scaling" value="0.5" />
        <arg name="robot_acceleration_scaling" value="0.2" />
    </include>
</launch>

需要注意的是，由于我要制作一个下象棋的机器人，所以在这里我将我的机器人命名为chessbot，大家可以按照自己的情况自由修改。

我们有了启动文件，还差一个标定板，我找到了一个非常好的网页可以在线生成aruco marker，我们要注意的是生成时输入的参数必须和我们在上面的文件中设置的一样，生成好之后直接用浏览器的打印菜单打印到A4纸上即可（可以用尺子量一下尺寸是否正确）。

在启动这个文件之前，还要知道我的机械臂的抓手（或者叫end effector）的坐标和标定板坐标系的变换关系我们还不知道，这要求我们自己设置机器人的TCP。这部分请自行查阅机器人的文档。

好，我们来运行上面的launch文件：

roslaunch chessbot_calibration calibrate.launch

如果成功的话，我们会看到三个窗口跳出来，分别是：

标定窗口1-rziv 标定窗口2-rqt_easy_handeye 标定窗口3-plan

我们还需要在标定窗口1中订阅相机拍到的图片/aruco_tracker/result进行显示，显示后的效果如下：

标定窗口1-rziv打开图片

在打开的图片中，我们可以清楚地看到标定系统已经成功地对标定板进行了识别，到这里我们就可以真正地进入自动标定的环节了。

标定程序

为了安全流畅地操作，我们先在rviz的Motion Planning界面中将机械臂的速度和加速度都设为0.1，并将标定板固定在机械臂上面，移动到相机视野的中心位置。再检查图3中是否显示0/17，如果不是就按check starting pose。

一切都正常设置之后，我们分三步来进行标定：

1. 在图3界面点击next pose，再点击plan，出现绿色就当前位姿可以达到，最后点击execute；如果出现红色则表示当前点不可用，继续点击next pose，重复操作，直至出现绿色；
2. 返回图2界面点击take sample。重复（1）（2）操作，直到17个点全部采集完；
3. 返回图2界面，点击compute，得到一组7个解的变换矩阵（四元数表示），点save。

上面计算的结果保存在~/.ros/easy_handeye/xxx.yaml这个文件里面，接下来我们发布变换，如果使用easy_handey自带的publish.launch一般是会报错的，因为我们在上面的calibrate.launch文件中对保存四元数的yaml文件进行了自定义。这里我们再创建一个publish.launch文件，内容如下：

<!-- filename: publish.launch -->

<?xml version="1.0"?>
<launch>
    <arg name="eye_on_hand" doc="eye-on-hand instead of eye-on-base" default="false" />
    <arg name="namespace_prefix" default="chessbot_calibration" />
    <arg if="$(arg eye_on_hand)" name="namespace" value="$(arg namespace_prefix)_eye_on_hand" />
    <arg unless="$(arg eye_on_hand)" name="namespace" value="$(arg namespace_prefix)_eye_on_base" />

    <!--it is possible to override the link names saved in the yaml file in case of name clashes, for example-->
    <arg if="$(arg eye_on_hand)" name="robot_effector_frame" default="" />
    <arg unless="$(arg eye_on_hand)" name="robot_base_frame" default="" />
    <arg name="tracking_base_frame" default="" />
    
    <arg name="inverse" default="false" />
    
    <!--publish hand-eye calibration-->
    <group ns="$(arg namespace)">
        <param name="eye_on_hand" value="$(arg eye_on_hand)" />
        <param unless="$(arg eye_on_hand)" name="robot_base_frame" value="$(arg robot_base_frame)" />
        <param if="$(arg eye_on_hand)" name="robot_effector_frame" value="$(arg robot_effector_frame)" />
        <param name="tracking_base_frame" value="$(arg tracking_base_frame)" />
        <param name="inverse" value="$(arg inverse)" />
        <node name="$(anon handeye_publisher)" pkg="easy_handeye" type="publish.py" output="screen"/>
    </group>
</launch>

---

查看标定结果

为了查看标定结果，我们需要编写另外个launch文件，通过它将我们的机械臂、kinect、moveit模块启动起来，并发布坐标变换。

<!-- filename: start.launch -->

<launch>
    <!-- start the robot -->
    <include file="$(find ur_modern_driver)/launch/ur5_bringup.launch">
        <arg name="limited" value="true" />
        <arg name="robot_ip" value="192.168.199.106" />
    </include>

    <include file="$(find chessbot_moveit_config)/launch/ur5_moveit_planning_execution.launch">
        <arg name="limited" value="true" />
    </include>
 
    <include file="$(find chessbot_moveit_config)/launch/moveit_rviz.launch">
        <arg name="config" value="true" />
    </include>

    <include file="$(find kinect2_bridge)/launch/kinect2_bridge.launch" />
 
    <include file="$(find chessbot_calibration)/launch/publish.launch" />

</launch>

另外，为了显示点云的octomap，我们还得添加一个点云配置文件sensors_kinect.yaml，放到chessbot_moveit_confit下面，这个chessbot_moveit_config包是我拷贝的ur5_moveit_config包，因为我不想改变原来的文件，当然我们也可以使用moveit_setup_assistant工具自建moveit_config包。sensors_kinect.yaml文件的内容为：

sensors:
  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
    point_cloud_topic: /kinect2/qhd/points
    max_range: 5.0
    point_subsample: 1
    padding_offset: 0.1
    padding_scale: 1.0
    max_update_rate: 1.0
    filtered_cloud_topic: filtered_cloud

注意点云所使用的topic应当按照具体情况做对应的修改。然后我们打开chessbot_moveit_config/launch文件夹下面的sensor_manager.launch.xml文件做相应的修改来使用上面的sensors_kinect.yaml文件。其实就是在后面添加

  <rosparam command="load" file="$(find chessbot_moveit_config)/config/sensors_kinect.yaml" />
  <param name="octomap_frame" type="string" value="odom_combined" />
  <param name="octomap_resolution" type="double" value="0.05" />
  <param name="max_range" type="double" value="5.0" />

用来载入octomap。

好了，现在万事俱备，只欠东风了。在终端输入以下命令启动launch文件：

roslaunch chess_demo start.launch

八叉图

这就是初步的标定结果了，现在我们就可以使用moveit心情地让我们的机械臂有外界交流了。

如果我们还想查看点云，可以先关闭八叉图的server，然后在打开的rviz窗口中，我们添加DepthCloud插件，并订阅kinect2/qhd/image_depth_rect和kinect2/qhd/image_color_rect两个topic，这样我们就看到了以下的结果：

点云

到这里，外参的标定基本上就非常详细地讲完了，谢谢各位阅读！愿为中国机器人的发展有所贡献！

---

备注：
这篇文章重度依赖于另外一篇博客，我是照着该作者的方法自己进行了改进，在此表示感谢！

引用：
[1]. https://www.aiimooc.com/article/show-htm-itemid-34.html