【手眼标定】ros/easy_handeye+ur5e+realsense D435i（eye on/to hand）

前言

此前有尝试过使用Matlab Tsai原理标定珞石工业机械臂，效果不是很好，也没有找到原因在哪里。终于来了北京实验室可以用UR5e在ROS框架下使用easy_handeye标定了，十分简洁哈哈哈哈哈

---

提示：以下是内容严重参考链接：https://zhuanlan.zhihu.com/p/92339362
十分感谢前人大佬的分享

0. 手眼标定原理

参考大佬深入浅出的解释

重点：

• Eye-to-hand 眼在手外：标定的是相机坐标系相对于机器人基座坐标系的位姿
• Eye-in-hand 眼在手上：标定的是相机坐标系相对于机器人工具坐标系的位姿
• 自己总结的标定原则
  • 确定哪些是已知的？哪些是未知的？哪些是变化的？哪些是不变的？哪些是需要求解的？哪些是不需要求解的？
  • 标定的是未知且不变的 TF（Transformation）
  • 恒等式关系
    • eye-to-hand：相机坐标系 <> 机器人基座坐标系（未知，不变，需要求解）
    • eye-in-hand：Marker坐标系 <> 机器人基座坐标系（未知，不变，不需要求解）

1. 安装相应依赖驱动库

# 创建catkin_workspace
mkdir calibration_catkin_workspace
cd calibration_catkin_workspace
mkdir src
cd src
 
# 下载机械臂驱动
git clone https://github.com/UniversalRobots/Universal_Robots_ROS_Driver.git Universal_Robots_ROS_Driver
git clone -b calibration_devel https://github.com/fmauch/universal_robot.git fmauch_universal_robot
# aruco
git clone -b noetic-devel https://github.com/pal-robotics/aruco_ros.git
# vision_visp
git clone -b noetic-devel https://github.com/lagadic/vision_visp.git
# easy_hand_eye
git clone https://github.com/IFL-CAMP/easy_handeye
 
# 下载依赖
cd ..
sudo apt update
rosdep update
rosdep install --from-paths src --ignore-src -y
# empy
pip install empy
# pyqt
pip install PyQt5
 
# 编译
catkin_make
# 切换环境
source ./devel/setup.bash

2. 开始标定（eye on/to hand）

2.1 标定Launch文件设置

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

```cpp
```cpp
<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="571"/>
    <!-- 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/color/camera_info" />
        <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_color_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_robot_driver)/launch/ur5e_bringup.launch">
        <arg name="robot_ip" value="101.6.64.35" />
    </include>
    <include file="$(find ur5e_moveit_config)/launch/ur5e_moveit_planning_execution.launch">
    </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="true" />
        <arg name="tracking_base_frame" value="camera_color_frame" />
        <arg name="tracking_marker_frame" value="camera_marker" />
        <arg name="robot_base_frame" value="base" />
        <arg name="robot_effector_frame" value="tool0_controller" />
        <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>为启动相机节点
• <2>启动aruco_ros节点，需要提前打印标定纸，网站
  • 注：Dictionary 一定要选 Original ArUco；Marker ID 和 Marker size 自选，在launch 文件中做相应的修改；打印时，要选择原始大小，否则要测量一下打印出来的真实大小。
• <3>UR5e节点，设计连接IP并开启moveit路径规划
• <4>easy_handeye节点
  • eye_on_hand的参数值为true则代表眼在手上
  • tracking_base_frame为相机坐标系camera_color_frame
  • robot_base_frame为机器人基坐标系，示例中为base_link，前辈避坑改为了base【未验证】
  • robot_effector_frame是工具坐标系，使用了robotiq，在示教器中定义了该夹具，为tool0_controller

2.2 开始标定

1. 启动launch文件，如果正常会弹出三个界面（rviz和两个弹窗）

roslaunch easy_handeye eye_on_hand_calibration.launch

2. 在rviz中添加image，topic选择result方便观察标定板位置
3. 手动调节机械臂，使 aruco 二维码移动至相机视野中心处附近，作为 home config。在界面 3 中，点击 check starting pose，若检查成功，界面会出现： 0/17，ready to start
4. 界面 3 中依次点击 Next Pose，Plan，Execute，机械臂会移动至新的位置，若二维码在相机视野范围内，且能检测成功，则进行下一步
5. 界面 2 中点击 Take Sample，若 Samples 对话框中出现有效信息，说明第一个点标定成功
6. 重复执行步骤 2 和步骤 3，直至 17 个点全部标定完毕
7. 界面 2 中点击 Compute，则 Result 对话框中会出现结果
8. 界面 2 中 Save，会将结果保存为一个 YAML 文件，路径为 ~/.ros/easy_handeye

2.3 发布tf验证

easy_handeye 功能包提供了 publish.launch 文件，可以将标定好的 TF 发布出
注：要修改 “namespace_prefix” 参数，与眼在手外标定 launch 文件中的 “namespace_prefix” 一致，这样才能找到标定好的 YAML 文件，并且同样修改eye_on_hand为True

<?xml version="1.0"?>
<launch>
    <arg name="eye_on_hand" doc="eye-on-hand instead of eye-on-base" default="true"/>
    <arg name="namespace_prefix" default="ur5_realsense_handeyecalibration" />
    <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>

标定结果TF发布如下：可以看出二者还是有一些旋转角度，平移自不用说，至于精度如何，后面有时间看能不能测试一下。

# 开启 publish.luanch 文件，以眼在手上为例
roslaunch easy_handeye publish.launch

# 查看 TF（改成自己的工具坐标系与相机坐标系）
rosrun tf tf_echo /tool0_controller /camera_color_frame

Question

尝试标定相机和tool0（而非tool0_controller），tool0即末端法兰，结果误差很大，在旋转角度上明显不对，不知道原因

参考：
https://zhuanlan.zhihu.com/p/33441113
https://zhuanlan.zhihu.com/p/63757762
https://blog.csdn.net/txy12029047/article/details/99323620
https://blog.csdn.net/ecH0o0/article/details/95245858
https://zhuanlan.zhihu.com/p/92339362