手眼标定原理与流程

1 硬件配置

机械臂安装在机器狗背上，并在机械臂link6位置安装了D435i深度相机。

2 软件驱动

2.1 机械臂驱动

1、 从 Unitree 机械臂 DDS 反馈 → 发布 /joint_states

让 RViz / MoveIt / TF / RobotStatePublisher 都能“看到真实机械臂状态”

2、 订阅你前一个节点发的 IK 结果

话题：

/infore/robotDog/targetIkResult   (sensor_msgs/JointState)

把 MoveIt 解出来的关节角 转成 Unitree 的 JSON 控制协议 通过 DDS 发给真实机械臂

3、 接收「单关节控制指令」（夹爪 / 单轴微调）

话题：

/infore/robotArm/jointControl   (std_msgs/String JSON)

这是机械臂Moveit规划程序里：

ControlGipperStatus()
SendJointValue()
ControlGipperPos()

这些函数的“下游接收者”

4、 提供标准 ROS Action 接口（MoveIt 必需）

Action Server：

/arm_controller/follow_joint_trajectory
/gripper_controller/follow_joint_trajectory

这一步 让 MoveIt 能“识别”自己在控制一个标准 ROS 机械臂

5、 把 FollowJointTrajectory 转成 Unitree 私有 DDS 控制协议

关键适配层

  • ROS 标准：JointTrajectory
  • Unitree 实际要：JSON + angle0~angle6

6、 自动从 URDF 中解析关节结构

从 /robot_description 中：

  • 自动识别 arm joints
  • 自动识别 gripper joints
  • 自动排序

2.2 相机驱动

1、 发布图像与深度图

发布RGB图像"/color/image_raw"和进行了深度对齐的深度图"/aligned_depth_to_color/image_raw"，并发布相机的内参矩阵"/color/camera_info"

2、 接收相机控制指令

接收控制指令"/infore/mainsystem/controlcmd"，控制相机的开关从而实现降低CPU占用的情况

3 标定原理

相机安装在机械臂末端——眼在手上

手眼标定要解的核心关系

A · X = X · B 

世界 / Base        
     ↑        
     |   (A)        
     |    
机械臂末端（Hand）        
     |        
     |   (X)  ← 手眼标定要解的        
     |     
相机（Eye）        
     |        
     |   (B)        
     |   
标定板 / ArUco

各个矩阵的真实含义

符号含义
A机械臂移动前后机械臂末端的坐标变换关系
B机械臂移动前后标定板在相机坐标系的变换关系
X相机 ↔ 机械臂末端 的固定变换（未知）

刚体固定变换矩阵 =

X = | R  t |
    | 0  1 |
部分含义
R3×3 旋转矩阵
t3×1 平移向量

常用求解算法对比

AX=XB方程的求解算法有多种，不同算法在精度、效率和鲁棒性上各有优劣，适用于不同场景：

算法名称核心特点精度效率适用场景
Tsai-Lenz算法分离旋转和平移求解，经典算法中高高工业场景实时标定、算力有限的嵌入式平台
迭代法（如Newton-Raphson）基于非线性优化，迭代收敛高中高精度需求场景（如精密装配）
解析法（Procrustes+最小二乘）闭式解，无迭代，稳定性强中高快速标定、多组数据融合
基于李代数的方法将旋转和平移转化为向量空间求解，数学严谨高中学术研究、复杂场景标定

在实际应用中，Tsai-Lenz算法因兼顾精度和效率，是工业界应用最广泛的方法；若需更高精度，可采用迭代法或融合多组数据的解析法。

4 标定流程

4.1 标定功能包组成

机械臂驱动包——————aruco_ros————————easy_handeye————————tf_pubblish————————bbox2catch
               (aruco码定位)       (手眼标定包)         （发布X矩阵）       (转换坐标系)

4.2 标定步骤

1、启动机械臂驱动

source devel/setup.bash
rosrun robotArmClient robotArmClient_node 
roslaunch infore_moveit_config demo.launch

此时出现机械臂moveit界面，并与机械臂实际位置相同

2、启动相机

roslaunch realsense_camera realsense_camera.launch

3、启动标定模块

启动前需要注意的是，eye_in_hand.launch 中机械臂的和相机的frame_id是否与实际/tf中的对应。

roslaunch aruco_ros single.launch 
roslaunch easy_handeye eye_in_hand.launch

4、开始标定

在moveit界面选择不同的机械臂姿态，对准固定位置的标定板。

在rqt的界面中采集20-30组数据，注意机械臂的姿态变化应始终保证尽量多的旋转。

最后点击compute计算得到标定数据。

填入tf_publish.launch

5 标定精度提升

5.1 标定技巧

使得每个pose之间的旋转变化足够大

使得每个pose之间的平移变化足够小

多采样 pose使得校准值收敛

使得目标物体尽可能地贴近相机（但应当在相机可行的工作空间内）

尽量使得标定图案不要正对着相机，而是要有一些偏转，避免它对称，由于其对称性，位姿估计可能不明确，并且输出可能变得不稳定

校准相机的内参（如果内参不够准确的话，但通常出厂时已经校准）

校准机械臂自身使得它足够准确

5.2 标定注意事项

标定板选用aruco码或者是棋盘格，aruco码需要核对好aruco码的ID，由于实际打印与电脑设定的有误差，故需要严格测量打印出的码的尺寸，精度保证在1mm以内。

相机安装位置在机械臂夹爪上方，因此标定实际是将相机标定到link6上，而不是link7_right或者link7_left。

相机驱动发布了两个frame：camera_color_optical_frame、camera_link，camera_color_optical_frame是相机图像坐标系，而camera_link是相机坐标系。

标定是将aruco码定位得到的camera_marker坐标系从camera_link标定到link6上，因此在easy_handeye中，需要填写机械臂的base坐标系base_link、机械臂安装相机的位置坐标系link6、相机坐标系camera_link、以及aruco码坐标系camera_marker。

标定完成前以及完成后， 都可以输入指令查看tf树，若/tf树显示了camera_link到baselink的完整链接，则实现了标定的转换

rosrun tf view_frames