ROS下UR+Realsense相机进行手眼标定

手眼标定主要是用来确定相机坐标系和机械臂坐标系之间的转换关系，从而实现后续的抓取等任务。

手眼标定原理

手眼标定有两个类型：

1. Eye-to-hand 眼在手外：即相机和机械臂分离，需要确认相机坐标系和机器人的基坐标系之间的转换关系。
2. Eye-on-hand 眼在手上：即相机在机械臂上，需要确认相机坐标系和机器人的末端之间的转换关系。
   

在眼在手上的标定系统中，四个坐标系根据图示的关系，可以得到等式
$${}_{base}^{tool}T{}_{tool}^{cam}T{}_{cam}^{cal}T={}_{base}^{cal}T$$
在标定过程中，标定板固定不动，机械臂末端进行移动，然后通过记录各个位姿之间的变化关系进行求解，由于标定板和基坐标系之间的转换关系在移动过程中保持不变。在两次变化间：
$$\begin{gathered} {}_{base}^{tool1}T{}_{tool1}^{cam1}T{}_{cam1}^{cal}T={}_{base}^{cal}T \\ {}_{base}^{tool2}T{}_{tool2}^{cam2}T{}_{cam2}^{cal}T={}_{base}^{cal}T \end{gathered}$$
两式联立，就可以求出相机和机械臂末端之间的转换关系。

功能实现

下载Aruco_ros工具包：

git clone https://github.com/pal-robotics/aruco_ros.git

这个包主要是生成给定大小的 AR 标记，利用该包生成标定纸，生成网址：https://chev.me/arucogen/。

安装easy_handeye:

git clone https://github.com/IFL-CAMP/easy_handeye.git

安装visip:

git clone  https://github.com/lagadic/vision_visp.git

编译完成后，修改easy_handeye下的launch文件，自己创建一个Ur_realsensed45.launch文件：

<?xml version="1.0"?>
<launch>
    <arg name="namespace_prefix" default="auboi5_realsense_handeyecalibration" />

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

    <arg name="marker_size" doc="Size of the ArUco marker used, in meters" default="0.12"/>
	<arg name="marker_id" doc="The ID of the ArUco marker used" default="100"/>

	<!--start realsense-->
	<!-- <include file="$(find realsense2_camera)/launch/demo_pointcloud.launch">
		<arg name="filters" value="pointcloud"/>
	</include> -->

    <!-- 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_link"/>
        <param name="camera_frame"       value="camera_color_optical_frame"/>
        <param name="marker_frame"       value="camera_marker" />
    </node>

    <!-- start the robot -->
  <!-- <include file="$(find ur_robot_driver)/launch/ur3_bringup.launch">
        <arg name="robot_ip" value="192.168.31.84" />
    </include>
    <include file="$(find ur3_moveit_config)/launch/moveit_planning_execution.launch">
    </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="true" />

        <arg name="tracking_base_frame" value="camera_link" />
        <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>

原始的文件是打开相机、机械臂、和手眼标定这三个程序，但是同时打开电脑容易出现卡死的现象。因此我的建议是提前打开相机、机械臂的运行文件。下面重点讲解一下关键的部分：

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

    <arg name="marker_size" doc="Size of the ArUco marker used, in meters" default="0.12"/>
	<arg name="marker_id" doc="The ID of the ArUco marker used" default="100"/>

这个需要改成自己的机械臂的网址，同时将标定文件的大小和id号进行修改，修改成和自己相匹配的。

  <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_link" />
        <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>

<arg name="eye_on_hand" value="true" />这个是选择相机是否在机械臂上，true即为眼在手上，flase即为眼在手外。其余相机和机械臂的坐标系参数可以根据自己的设备进行修改。

具体实现

运行相机：

roslaunch realsense2_camera rs_camera.launch

运行机械臂：

roslaunch ur_modern_driver ur3_bringup.launch limited:=true robot_ip:=192.168.31.84
roslaunch ur3_moveit_config ur3_moveit_planning_execution.launch limited:=true

然后运行手眼标定程序：

roslaunch easy_handeye ur_realsense_calibration.launch

打开后有三个界面：



第一个为Rviz界面，第二个为手眼标定的显示和计算界面，第三个为控制机械臂进行运动的界面。

在第二个界面中，窗口选择plugins->Visualization->image_View，打开image_view界面。选择/aruco_ros/tracker/result话题，确认能检测到aruco码。然后在第三个窗口中检测可行性，进行运动规划，控制机械臂不断发生位姿改变，从而进行采样。

机械臂每次进行一次运动，都可以进行采样：Take sample。多次采样完成后，就可以点击Compute进行求解。
最后求出标定结果，并点击Save进行保存，数据保存在$HOME/.ros/easy_handeye 目录下的yaml文件中。