UR5、realsense手眼标定

参考链接：
【手眼标定】ros/easy_handeye + ur5 + realsense d435i 丁洪凯
UR5 kinect2 eye on base calibration
UR5、Kinect2手眼标定总结
ubuntu16.04kinetic + realsenseD435i相机 + UR3的手眼标定
UR10 RG2机械臂手臂+RealsenseZR300 机器人手眼标定 系列第四篇
UR3机械臂使用kinect2相机利用easy_hand手眼标定
ArUco标定板链接

---

1. 安装

1.1. 新建工作空间

首先新建工作空间，之后均在此工作空间下编译。

mkdir -p ~/catkin_ws/src

1.2. ur5源码编译

源码编译ur5和ur_modern_driver

cd ~/catkin_ws/src
git clone -b kinetic-devel https://github.com/ros-industrial/universal_robot.git
cd universal_robot
git clone -b kinetic-devel https://github.com/ros-industrial/ur_modern_driver.git

1. 将universal_robot路径下的ur_drive文件删除（即用ur_modern_driver替换了ur_drive）。
2. 打开ur_modern_drive/src/ros/ur_hardware_interface.cpp文件。
3. 修改语句，即如下所示（所有都要改）：
   • controller_it->hardware_interface改为controller_it->type
   • stop_controller_it->hardware_interface改为stop_controller_it->type

cd ~/catkin_ws
catkin_make
echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc

1.3. 安装aruco_ros

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

此包现在更新后已不能使用，需要以前原始的包才可标定了……

1.4. 安装vision_visp

在安装之前首先要按照以下步骤安装依赖包（注意版本号）：

sudo apt-get install ros-kinetic-visp

然后我们安装vision_visp

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

1.5. 安装easy_handeye

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

source一下。

2. launch 文件配置

标定过程需启动 ur5 机械臂的相关节点，realsense 节点，aruco 节点，easy_handeye 节点，可以写一个 launch 文件同时启动上述节点，也可以分别启动。easy_handeye 包中给出了用一个 launch 文件实现的示例，在如下的目录中：xxx/easy_handeye/docs/example_launch/ur5_kinect_calibration.launch

也可以到github上下载：链接

下面是我的修改launch文件，文件命名为ur5_realsense_handeyecalibration.launch。

<launch>
    <arg name="namespace_prefix" default="ur5_realsense_handeyecalibration" />

    <arg name="robot_ip" doc="The IP address of the UR5 robot" />

    <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="582"/>

    <!-- start the Kinect -->
    <!--<include file="$(find freenect_launch)/launch/freenect.launch" >
        <arg name="depth_registration" value="true" />
    </include>-->
    <!-- 1. start the Realsense435 -->
    <include file="$(find realsense2_camera)/launch/rs_camera.launch" />

    <!-- 2. start ArUco -->
    <node name="aruco_tracker" pkg="aruco_ros" type="single">
        <!--<remap from="/camera_info" to="/camera/rgb" />-->
        <remap from="/camera_info" to="/camera/color/camera_info" />
        <!--<remap from="/image" to="/camera/rgb/image_rect_color" />-->
        <remap from="/image" to="/camera/color/image_raw" />
        <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="camera_link"/>-->
        <param name="reference_frame"    value="camera_color_frame"/>
        <!--<param name="camera_frame"       value="camera_rgb_optical_frame"/>-->
        <param name="camera_frame"       value="camera_color_frame"/>
        <param name="marker_frame"       value="camera_marker" />
    </node>

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

    <!-- 4. 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="eye_on_hand" value="true" />

        <!--<arg name="tracking_base_frame" value="camera_link" />-->
        <arg name="tracking_base_frame" value="camera_color_frame" />
        <arg name="tracking_marker_frame" value="camera_marker" />
        <!--<arg name="robot_base_frame" value="base_link" />-->
        <arg name="robot_base_frame" value="base" />
        <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>

1、这里面标定板的ID是582，其可以在 aruco里面下载（注意下载打印时选择原始尺寸打印），标定板的尺寸单位是米。
2、修改后的文件放置到~/catkin_ws/src/easy_handeye/easy_handeye/launch文件夹下。

2.1. realsense 相机节点

<!-- 1. start the Realsense435 -->
<include file="$(find realsense2_camera)/launch/rs_camera.launch" />

2.2. aruco_ros 节点

<!-- 2. start ArUco -->
<node name="aruco_tracker" pkg="aruco_ros" type="single">
    <!--<remap from="/camera_info" to="/camera/rgb" />-->
    <remap from="/camera_info" to="/camera/color/camera_info" />
    <!--<remap from="/image" to="/camera/rgb/image_rect_color" />-->
    <remap from="/image" to="/camera/color/image_raw" />
    <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="camera_link"/>-->
    <param name="reference_frame"    value="camera_color_frame"/>
    <!--<param name="camera_frame"       value="camera_rgb_optical_frame"/>-->
    <param name="camera_frame"       value="camera_color_frame"/>
    <param name="marker_frame"       value="camera_marker" />
</node>

标定板从aruco网站下载二维码并打印出来。

注意：

• Dictionary 一定要选 Original ArUco
• marker_id 和 marker_size 自选，在 launch 文件中做相应的修改，本例中 marker_id 为 582，marker_size 为 0.1（单位：m）。
• 打印时，要选择原始大小，否则要测量一下打印出来的真实大小

2.3. ur5 节点

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

注意：

• 这里用了 ur_modern_driver 包，而没有用原始的 ur_bringup 包。
• 修改机器人的真实 ip。

2.4. easy_handeye 节点

<!-- 4. 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="eye_on_hand" value="true" />

    <!--<arg name="tracking_base_frame" value="camera_link" />-->
    <arg name="tracking_base_frame" value="camera_color_frame" />
    <arg name="tracking_marker_frame" value="camera_marker" />
    <!--<arg name="robot_base_frame" value="base_link" />-->
    <arg name="robot_base_frame" value="base" />
    <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>

注意：

• <arg name="eye_on_hand" value="true"/> 眼在手上：value 为 false
• tracking_base_frame 为相机坐标系 camera_color_frame。
  tracking_marker_frame 是标志物的坐标系 camera_marker。
• robot_base_frame 为机器人基座坐标系，示例里写的是 base_link，我在 rviz 中查看 base 才是真实的基座坐标系。
• robot_effector_frame 是末端，为工具坐标系。

3. 标定眼在手上 eye-in-hand

3.1. 启动 launch 文件

roslaunch easy_handeye ur5_realsense_handeyecalibration.launch

若显示无启动文件，则source一下。

此时会同时打开三个界面

其中界面①部分显示了UR5机械臂当前姿态和各坐标系的初始位姿；
界面②部分显示的是相机下ArUco标签码坐标系camera_mark的识别情况，红色为x轴，绿色为y轴，蓝色为z轴；
界面③部分规划机械臂位姿，使相机运动到不同的位姿以进行多次图像采样。

具体标定步骤如下：

1. 放置好标定物ArUco标签码，启动UR5机械臂、相机和标定程序；
2. 点击图中界面③中的check starting pose确定UR5机械臂本姿态下是否能够满足标定条件；
3. 点击界面③中的next pose生成UR5机械臂下一位姿，接着点击plan显示绿色能够规划出从当前位姿到下一位姿的路径后，点击execute执行操作，驱动UR5机械臂运动下一位姿状态；
4. 若此刻界面②中能够识别出标定物ArUco标签码，则点击界面②中的take sample采样，重复步骤(3)和(4)，直到全部采样完毕，如下图所示，有效采样到的数据共十二次；
5. 采样完成后，点击界面②中的computer对所采样的数据进行计算，即可得到相机坐标系camera_color_frame和UR5机械臂末端工具坐标系tool0之间其次变换矩阵，为四元数表示形式。