机器人抓取（五）—— 手眼标定 hand eye calibration

1. 原理篇

参考：机器人手眼标定的基础理论分析
3D视觉之手眼标定
胡春旭：“手眼”结合完成物体抓取应用
在实际应用中，我们通常需要将相机观察到的外界环境中物体的姿态从相机坐标系转换到机械臂的坐标系中，辅助机械臂规划一些后续动作（如抓取）。为了得到机器人坐标系和相机坐标系的转换矩阵，我们就要对机器人进行手眼标定。手眼标定有两种常见的标定方法。第一种是通过特殊的标定物，第二种是通过机器人运动建立约束。

1.1 手眼标定通用原理

机器人手眼标定其实就是求解机器人坐标系和相机坐标系转换关系。假设现在我们有机器人基坐标系 {B}，和相机坐标系 {C}，相机坐标系到机器人基坐标系的转换矩阵为 ${}^B{T}_C$，已知空间中固定点 $P$ 在这两个坐标系中的坐标为 ${}^{B}P和{}^{C}P$，那么根据坐标转换关系有：
$${}^{B}P={}^B{T}_C{}^{C}P$$
其中，${}^{B}P和{}^{C}P$为补1后的其次坐标$[x,y,z,1{]}^{\prime }$，这样 ${}^B{T}_C$便可以同时包含旋转和平移变换。
只要点$P$的个数大于我们求解的转换矩阵维度，同时这些点线性不相关，我们便可以通过伪逆矩阵计算${}^B{T}_C$。
$${}^B{T}_C={}^{B}P({}^{C}P{)}^{-1}$$
由上述分析可知，只要我们能够同时测量出多组固定点 $P$在两个坐标系的坐标 ${}^{B}P和{}^{C}P$ 数据，就可以很方便地计算出坐标变换矩阵。当然在实际过程中我们有可能很难同时测量出来固定点$P$在两个坐标系的坐标。后面我们针对不同情况做具体讨论。

1.2 眼在手外的标定原理

• eye-to-hand 眼在手外：相机固定在机器人外的固定底座上。这种情况下，需要将标定板固定在机器人末端，求解的量为相机坐标系和机器人坐标系之间的位姿关系。
• eye-in-hand 眼在手上：相机固定在机器人末端。这种情况下，需要将标定板固定在地面上，机器人坐标系和标定板坐标系的转换关系始终不变。求解的量为相机坐标和机器人末端坐标系的位姿关系。

本文着重讲解眼在手外的情况：
如前文所述，我们接下来需要测量几组固定点 $P$ 在机器人基座坐标系和相机坐标系下的坐标 ${}^{B}P和{}^{C}P$。那么怎么去测量这些点的坐标呢？我们需要用到一个标定神器：棋盘格标定板。通过视觉算法可以准确的识别棋盘格的角点，具体算法在 opencv，ros 等平台都有相应功能包，调用即可。
识别棋盘格可以得到棋盘格角点在图像坐标系{I}中的二维坐标 ${}^{I}P$，通过相机内参和物体的深度信息可以计算出 ${}^{C}P$。如何从${}^{I}P$得到${}^{C}P$前文已有论述。此处我们假设已经得到了${}^{C}P$。

eye-to-hand 的情况下，会将标定板固定在机器人末端，如图所示，相我们便会得到如图所示这些坐标系：
{C}：相机坐标系
{B}：机器人基坐标系
{E}：机器人末端坐标系
{K}：标定板坐标系

坐标系之间的转换关系：
$A$ : 机器人末端坐标系到机器人基座标系的转换矩阵，机器人运动学正解可知。
$B$ ： 标定板坐标系到机器人末端坐标系的转换矩阵，标定板是固定安装在机器人末端的，所以固定不变，但未知。
$C$ ：相机坐标系到标定板坐标系的转换矩阵，即相机外参，可知。
$D$ ：相机坐标系到机器人基坐标系的转换矩阵，待求量。

$${}^{B}P=A\cdot B\cdot C\cdot {}^{C}P$$

$${}^{B}P=D\cdot {}^{C}P$$
所以：
$$D=A\cdot B\cdot C$$
$D$和$B$ 都是未知，但固定不变的量。$A$和$C$则遂机器人的末端位姿变化而变化。
我们让机器人走两个位置，于是有：
$$A_1\cdot B\cdot C_1=A_2\cdot B\cdot C_2$$,
经变换可得：
$$(A_2^{-1}\cdot A_1)\cdot B=B\cdot (C_2\cdot C_1^{-1})$$
这是一个典型的 AX = XB 问题。

2. 实践篇：利用 easy_handeye 标定 ur5+Azure kinect

参考：

easy_handeye

UR5、Kinect2手眼标定总结

ROS下UR5、usb_cam单目外参标定（使用easy_handeye、眼在手上eye-in-hand）
ubuntu16.04kinetic + realsenseD435i相机 + UR3的手眼标定

UR5 kinect2 eye on base calibration

easy_handeye 是一个ros功能包，可以标定眼在手上和眼在手外两种情况, 而且设计了两个GUI，机器人自主移动界面可以自动规划采样姿态，并控制机器人运动；标定界面可实现位姿数据采集，然后计算标定结果。使用easy_handeye的一般步骤：

1. 首先需要把UR5跟相机跑起来.
2. 用aruco_ros识别标定板上的标志物(ArUco marker), 得到camera_frame到marker_frame的转换.
3. 之照GUI控制机械臂在相机视野内产生16个姿态, 在开始姿态前后左右上下8个位置各2个角度，每移至一个新位姿进行一次采样。
4. 采样完成后点击求解, easy_handeye 会调用 vision_visp 的代码得到标定结果. 然后点 save, 标定结果会被保存在~/.ros/easy_handeye/---.yaml。
5. 之后就可以关掉标定程序, 再跑 publish.launch 即可发布 base_frame 到camera_frame 的转换。

2.1 标定前的准备工作

2.1.1 安装easy_handeye及其依赖

按照官方步骤：

• clone this repository into your catkin workspace:

cd ~/catkin_ws/src  # replace with path to your workspace
git clone https://github.com/IFL-CAMP/easy_handeye

• 安装依赖 （aruco_ros，visp_hand2eye_calibration）

cd ..  # now we are inside ~/catkin_ws
rosdep install -iyr --from-paths src

若安装不成功，可手动安装。或去github下载功能包源码至~/catkin_ws/src，catkin_make编译即可。

sudo -H apt-get install -y ros-melodic-visp-hand2eye-calibration
sudo -H apt-get install -y ros-melodic-aruco-ros

• build

catkin_make

2.1.2 打印 ArUco marker

ArUco marker

ArUco marker是一种汉明码方格图。它由一个宽的黑边和一个内部的二进制矩阵组成，黑色的边界有利于快速检测到图像，Marker ID是他的二进制矩阵编码，Marker size是图片的大小。有个网站可以生成ArUco marker图案，可打印下来做标定板，打印时注意：
Dictionary 要选 Original ArUco；
Marker ID 和 Marker size 自选，注意在launch 文件中做相应的修改；
打印时要选择原始大小，否则要测量一下打印出来的真实大小。

若眼在手上，则将标定板固定在地面上。若眼在手外，则将标定板固定在手上。

aruco_ros

aruco_ros 是用来识别 ArUco marker 图案的，启动节点后会发布
/aruco_tracker/marker
/aruco_tracker/pixel : ArUco marker 在图像坐标系的坐标
/aruco_tracker/pose ：/marker_frame 在 /reference_frame 的位姿
/aruco_tracker/transform
…
等话题。主要用于目标位姿估计和视觉伺服领域。 /marker_frame 的 TF 遵循rviz约定，X轴为红色，Y轴为绿色，Z轴为蓝色；但在相机视图中的显示 x为蓝色，y 为绿色，z 为红色。

2.1.3 添加 launch 文件

安装完成后为了能够同时启动 robot, camera, ArUco 和 easy_handeye，需要重新写一个launch文件。easy_handeye包中给出了一个示例的ur5_kinect_calibration.launch。可将此文件拷贝到easy_handle/launch文件夹并重命名一下，然后按个人需求修改文件内容。

经试验发现easy_handeye和前三者同时启动会出现采集窗口无法打开的情况，因此拆分成两个 launch 文件。

ur5 机器人+ azure kinect 相机，launch 文件参考如下：

• ur5_k4a_calibration_setup.launch
  kinematics_config加载的是ur机器人标定后的配置文件。

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

    <arg name="robot_ip" default="192.168.1.2" doc="The IP address of the UR5 robot" />
    <arg name="limited" default="true" doc="If true, limits joint range [-PI, PI] on all joints." />
    <arg name="kinematics_config" default="$(find ur_description)/config/ur5_calibration.yaml"/>

    <arg name="marker_size" value="0.050" doc="Size of the ArUco marker used, in meters" />
    <arg name="marker_id" value="25" doc="The ID of the ArUco marker used" />

    
    <include file="$(find azure_kinect_ros_driver)/launch/driver.launch" >
        <arg name="depth_mode" value="WFOV_2X2BINNED" />
    include>

    
    <node name="aruco_tracker" pkg="aruco_ros" type="single">
        <remap from="/camera_info" to="/depth_to_rgb/camera_info" />
        <remap from="/image" to="/rgb/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="rgb_camera_link"/>
        <param name="camera_frame"       value="rgb_camera_link"/>
        <param name="marker_frame"       value="camera_marker" />
    node>

    
    <include file="$(find ur_robot_driver)/launch/ur5_bringup.launch">
        <arg name="limited" value="$(arg limited)"/>
        <arg name="robot_ip" value="$(arg robot_ip)" />
        <arg name="kinematics_config" value="$(arg kinematics_config)"/>
    include>
    <include file="$(find ur5_moveit_config)/launch/ur5_moveit_planning_execution.launch">
        <arg name="limited" value="$(arg limited)"/>
    include>

launch>

• ur5_k4a_calibration.launch

注意事项：

1. namespace_prefix 参数与ur5_k4a_calibration_setup.launch保持一致。
2. eye_on_hand 参数， false 眼在手外，true 眼在手上。
3. 几个坐标系的选择：
   眼在手外的手眼标定是获得tracking_base_frame（相机坐标系）和robot_base_frame（机器人基坐标系）的坐标变换；
   眼在手上的手眼标定获得的是tracking_base_frame（相机坐标系）和robot_effector_frame（机器人工具坐标系）的坐标变换。

<launch>
	<arg name="namespace_prefix" default="ur5_k4a_handeyecalibration" />
    
    <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="rgb_camera_link" />
        <arg name="tracking_marker_frame" value="camera_marker" />
        <arg name="robot_base_frame" value="base" />
        <arg name="robot_effector_frame" value="tool0" />

        <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>

2.1.4 提前解除一些报错

报错：RLException: roslaunch file contains multiple nodes named [/robot_state_publisher].
原因：azure_kinect_ros_driver 和 ur_robot_driver 都启动了robot_state_publisher 节点。

将 ur_robot_driver/launch/ur_common.launch 或Azure_Kinect_ROS_Driver/launch/driver 中的下面内容注释掉即可。

<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />

2.2 手眼标定步骤

1.依次启动 launch 文件

roslaunch easy_handeye ur5_k4a_calibration_setup.launch
roslaunch easy_handeye ur5_k4a_calibration.launch

会打开三个界面：
a. 标定界面

b. rviz界面

c. 机器人自主移动界面

2.在标定界面中，点击菜单栏的 Plugins -> Visulization -> Image View，选择 /aruco_tracker/result 话题，界面会如下所示。

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

标定完成，此时可关闭标定程序。

2.3 发布 TF

方法 1.

easy_handeye 功能包提供了 publish.launch 文件，可以将标定好的 TF 发布出。注意修改 namespace_prefix 参数与标定的 launch 文件中的保持一致，这样才能找到标定好的 YAML 文件。眼在手外"eye_on_hand" 参数设为 false。

roslaunch easy_handeye publish.launch eye_on_hand:=false namespace_prefix:=ur5_k4a_handeyecalibration

# 查看 TF（基坐标系到相机坐标系）
rosrun tf tf_echo /base /rgb_camera_link

在实际应用中可在机器人launch 文件中这样发布标定信息。

<include file="$(find easy_handeye)/launch/publish.launch" >
	<arg name="namespace_prefix" default="ur5_k4a_handeyecalibration" />
    <arg name="eye_on_hand" value="false" />
include>

方法 2.

直接添加static_transform_publisher节点，实时（100hz）发布位置转换关系

<node pkg="tf" type="static_transform_publisher" name="link1_broadcaster" args="0.7810954111863362 0.4362160638790268 0.16301827937001387 -0.20452635132067903 -0.7688849381709477 0.5396310162123635 0.275287649216088 base_link camera_link 100" />

这里的args 分别对应x y z qx qy qz qw frame_id child_frame_id period_in_ms.

2.4 debug记录

1.Take Sample 报错

Error processing request: "tool0_controler" passed to lookupTransform argument target_frame does not exist。

解决方法：将robot_effector_frame 的值改为 tool0 (或 ee_link 或 wrist_3_link, 取决于你机器人的 urdf 文件中的定义的末端执行器是啥)，报错解除。

可能原因：tool0_controler 是/ur_hardware_interface 发布的，而其他机器人TF 都是/robot_state_publisher 发布的。

注：执行指令rosrun tf view_frames ，可生成tf树信息的 frames.pdf 文件。view_frames 是TF 树可视化的调试工具。

2. Compute 报错

DeserializationError cannot deserialize: unknown error handler name 'rosmsg'。

问题在于混用了旧的和更新的Debian软件包， sudo apt-get install ros-melodic-genpy 将genpy更新至最新版本即可解决。

【手眼标定】ros / easy_handeye + ur5 + realsense d435i