一文解决机械臂手眼标定（理论+实操）

【前言】最近在做机械臂的手眼标定，网上的教程杂乱，真正有用的不多，在实际踩坑后进行了记录，本篇能解决机械臂手眼标定的全部问题，从理论到实操。

1. 准备知识及工具介绍

1.1 空间位姿和坐标系转换原理

• 如何在三维空间中描述一个点？
  • 位置（位置可以使用位置向量（x, y, z）来描述） + 姿态（空间中物体的姿态使用固定在物体上的坐标系来描述）
• 任何位姿都需要参考坐标系（parent frame），否则描述是模糊的
• 任何两个不同坐标系下的位姿都可以通过齐次矩阵进行变换
  • 使用齐次坐标可以将所有的变换统一成矩阵乘法的形式。平移、旋转、缩放等操作都可以通过乘以一个4x4的矩阵来实现。而齐次坐标是在x, y, z的基础上添加了一维，构成了齐次阵，s表示x, y, z的缩放，dx, dy, dz表示x, y, z的平移，旋转取决于旋转轴（暂未列出，由左上角3 x 3的矩阵构成）
    $$\left[\begin{array}{cccc}sx& 0& 0& dx\\ 0& sy& 0& dy\\ 0& 0& sz& dz\\ 0& 0& 0& 1\end{array}\right]$$
• 三维空间中位姿的其他表示方式：
  • 位置向量 + 四元数

1.2 手眼标定原理

机械臂的手眼标定主要是确定相机和机械臂末端执行器（end effector）之间的关系，以完成一些操作的任务。根据相机安装位置的不同，分为眼在手上和眼在手外两种方式，此问题实际上涉及如下几个坐标系：

• 眼在手上最终需要求得：相机坐标系到末端执行器之间的变换 ${}^g{T}_c$，其中，target（标定板）到机械臂基底的变换${}^b{T}_g$和相机坐标系到末端执行器之间的变换 ${}^g{T}_c$是两个未知不变量，但${}^b{T}_g$我们并不需要实际求得（通过不移动标定板和机械臂基底保持不变），末端执行器到基底的变换 ${}^b{T}_g$是已知不变量（正运动学求解），target（标定板）到相机的变换 ${}^c{T}_t$是已知变化量，最终得到2个已知量，两个未知固定量，联合求解可得
  

• 眼在手外最终需要求得：相机坐标系到机械臂基底坐标系之间的变换，其中，target（标定板）到机械臂末端执行器的变换 ${}^g{T}_t$和相机到基底的变换 ${}^b{T}_c$是两个未知不变量，但 ${}^g{T}_t$我们不需要实际求得（通过将标定板固定在末端执行器上），末端执行器到基底的变换 ${}^b{T}_g$是已知不变量（正运动学求解），target（标定板）到相机的变换 ${}^c{T}_t$是已知变化量最终得到2个已知量，两个未知固定量，联合求解可得。
  最终可以将上述问题归纳为 $A_{i}X=X{B}_i$的问题， $i$表示测量的次数，即通过多次测量来得到准确的结果

1.3 Aruco marker介绍及aruco_ros package

1.3.1 Aruco marker

Aruco marker生成网站：

https://chev.me/arucogen/

必须选择origin，大小和id自己设置，然后即可打印或者保存
Aruco Marker是一种用于估计三维空间位姿的方形基准标记，黑色边框用于快速检测，内部包含二进制编码能够很好的进行位姿估计

1.3.2 aruco_ros package

https://index.ros.org/p/aruco_ros/
https://github.com/pal-robotics/aruco_ros

实际使用时，需要修改对应的相机节点和其他配置

# 进入aruco_ros package目录下
roscd aruco_ros
cd launch | vim single.launch

将single.launch做如下修改：

....
    <arg name="marker Id" default="xxx">
    <arg name="markerSize" default="xxx">
    ...
    <node pkg="aruco_ros" type="single" name="aruco_single">
        <remap from="/camera_info" to="/xxx">
        <remap from="/image" to="/xxx">
</launch>

启动后，当marker进入相机视野，可以读取/aruco_ros/pose即为相机坐标系下target的位姿，即 ${}^g{T}_c$

1.3 OpenCV calibrateHandEye()接口

https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html

• calibrateHandEye()是一个求解AX=XB的方法，这本身是一个优化问题，结果不具有稳定性，相机位姿的-组数和精度都可能导致最终的标定偏差，实际测试下来，当不同组数据之间的距离变化较大时，结果会出现较大的偏差，OpenCV推荐的求解算法是Tsai，其原论文对标定过程中的机械臂pose做出了几点建议：

• 眼在手外较难标定，不同标定结果之间差别也较大，目前还没有定位到原因
  calibrateHandEye()示例部分只提供了眼在手上的用法，眼在手外也可以使用这个接口，但参数有所区别

2. 眼在手上标定

• 将相机固定在机械臂上，Aruco Marker固定在桌面上保持和机械臂基底的位置不变
• 移动机械臂（拖动示教、遥操、直接控制均可），读取末端位姿，读取aruco pose
• 调用calibrateHandEye()接口，参数区别如下
  

3. 眼在手外

• 将相机固定，保证和机械臂基底的位置不变，将Aruco Marker固定在机械臂末端任意处（只需要保证和末端位姿之间的位置不变）
• 移动机械臂，读取末端位姿，读取aruco pose，获取多组数据
• 调用calibrateHandEye()接口，参数区别如下，与眼在手上不同！
  

4. ROS TF机制使用

ROS使用tf话题接受坐标系发送节点发出的tf关系，/tf话题的消息格式是tf2_msg/TFMessage，其定义为TransformStamped数组

std_mags/Header header
        uint32 seq
        time stamp
        string frame_id
string child_frame_id
geometry_msgs/Transform transform
        geometry_msgs/Vector3 translation
                float64 x
                float64 y
                float64 z
        geometry_msgs/Quaternion rotation
                float64 x
                float64 y
                flaot64 z
                float64 w
如何将一个三维空间的位姿发送为TF话题中？ 

#!/usr/bin/env python
import rospy
import tf
from geometry_msgs.msg import TransformStamped

def publish_tf():
    # 初始化ROS节点
    rospy.init_node('my_tf_broadcaster')

    # 创建TF广播器
    broadcaster = tf.TransformBroadcaster()

    # 循环直到节点关闭
    rate = rospy.Rate(10.0)  # 10Hz
    while not rospy.is_shutdown():
        # 创建TransformStamped消息
        t = TransformStamped()
        t.header.stamp = rospy.Time.now()
        t.header.frame_id = "parent_frame"  # 父坐标系
        t.child_frame_id = "child_frame"    # 子坐标系
        t.transform.translation.x = 1.0  # 相对于父坐标系的x位置
        t.transform.translation.y = 2.0  # 相对于父坐标系的y位置
        t.transform.translation.z = 0.0  # 相对于父坐标系的z位置
        t.transform.rotation.x = 0.0  # 四元数的x分量
        t.transform.rotation.y = 0.0  # 四元数的y分量
        t.transform.rotation.z = 0.0  # 四元数的z分量
        t.transform.rotation.w = 1.0  # 四元数的w分量

        # 发布坐标变换
        broadcaster.sendTransform(t)

        # 按照设定的频率等待
        rate.sleep()

if __name__ == '__main__':
    try:
        publish_tf()
    except rospy.ROSInterruptException:
        pass

5. 标定精度

并没有看到太好的测试精度的方式，稚晖君机械臂的视频里采用百分表进行末端位姿的重复精度测量，感觉这是测量标定精度的可行的方式