手眼标定简讲

工程实践边界

目前尚未在机械臂上完整跑通 Eye-in-hand 和 Eye-to-hand 两套手眼标定的实机闭环。已经完成或具备的相关基础包括：

相机内参与畸变标定；
solvePnP 的目标 6DoF 位姿解算；
相机到云台的固定外参标定，以及 camera → gimbal → world 坐标转换；
Tsai、Park、Horaud、Daniilidis、RobotWorldHandEye 等方法的比较；
七轴机械臂的 ROS2、TF、MoveIt2、MoveIt Servo 与实机 CAN 控制；
RealSense、ArUco、深度反投影与视觉引导机械臂运动。

这些能力构成了手眼标定所需的视觉、坐标变换和机械臂控制基础；完整的实机手眼闭环仍需在真实硬件上继续验证。

相机内参：从原理到实战

基本原理

相机内参描述“相机坐标系中的三维点如何投影到二维像素”。针孔模型中：

$$ u = f_x \frac{X}{Z} + c_x, \qquad v = f_y \frac{Y}{Z} + c_y $$

内参矩阵为：

$$ K = \begin{bmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{bmatrix} $$

$f_x, f_y$：以像素为单位的焦距；
$c_x, c_y$：主点，通常接近图像中心；
$k_1, k_2, k_3$：径向畸变，主要表现为桶形或枕形弯曲；
$p_1, p_2$：切向畸变，主要来自镜头去心、光轴偏移或成像系统装配不对称。

内参标定的本质是：标定板角点的真实几何位置已知，相机能够检测其像素位置；通过多张不同姿态的图像，同时优化 $K$、畸变参数与每张图的标定板位姿，使所有角点的总重投影误差最小。

不同倾角和距离会产生不同的平面投影约束，有助于分离相机内参与每张图的标定板外参；让标定板覆盖图像边缘，则能更充分地约束镜头畸变。

实战流程

固定分辨率、镜头焦距、对焦、曝光和增益。
使用尺寸准确、平整且刚性的棋盘格或 ChArUco 板。
采集约 20～30 张图，覆盖画面中心、四角、不同距离和不同倾角。
检测角点并进行亚像素优化。
将每张图的标定板三维点 objectPoints、对应二维角点 imagePoints 和 imageSize 输入 cv::calibrateCamera，得到 cameraMatrix、distCoeffs、逐图 rvecs/tvecs；函数返回总体 RMS。
检查逐图重投影误差，删除模糊、角点错误或姿态重复的异常图后重新标定。
使用未参与标定的图像验证去畸变和重投影效果。
保存内参、畸变参数和对应图像尺寸，供 PnP、手眼标定和三维测量使用。

常见错误与链路影响

所有图片都正对标定板、距离相同，会导致焦距与深度约束不足。
标定板只出现在画面中心，会造成边缘畸变约束不足。
打印比例或格子尺寸填写错误，会引入尺度系统性错误。
只看平均误差而不检查边缘与单张图的异常误差。
更换分辨率、镜头、焦距或对焦后，继续使用旧内参。

完整链路是：内参与畸变参数用于 PnP 求每帧 $^C T_T$，再与机器人末端位姿共同进入手眼标定。内参不准确会直接影响 PnP 的距离和姿态，进而把系统误差传递到手眼标定、目标定位和机器人抓取。

手眼标定的本质

相机得到的是“目标在相机坐标系中的位置”，机械臂执行需要的是“目标在机器人基座坐标系中的位置”。手眼标定就是求相机坐标系与机器人坐标系之间固定的 6DoF 刚体变换，包括三个旋转自由度与三个平移自由度。

本文统一采用“左上标表示目标坐标系、右下标表示来源坐标系”的记法：$^A T_B: B \rightarrow A$。它把在 $B$ 坐标系表达的点转换到 $A$ 坐标系：

$$ {}^A p = {}^A R_B {}^B p + {}^A t_B $$

坐标变换按照中间坐标系首尾相接：

$$ {}^A T_C = {}^A T_B {}^B T_C $$

常用坐标系如下：

符号	含义
$B$	机器人基座 `base_link`
$G$	机械臂末端/TCP，例如 `planning_tip_link`
$C$	相机光学坐标系
$T$	标定板坐标系

内参、PnP 和手眼标定的分工不同：

内参：三维相机点如何投影成二维像素；
PnP：求单帧标定板到相机的位姿 $^C T_T$；
手眼标定：结合多帧机器人位姿和 PnP 位姿，求机器人与相机之间的固定外参。

Eye-in-hand：眼在手上

布局与目标

相机固定在机械臂末端，随机械臂一起运动；标定板固定在工作台上。需要求解的手眼外参为：

$$ {}^G T_C: C \rightarrow G $$

它把相机系中的点转换到末端/TCP 系，也就是相机在末端中的位姿。每个姿态需要采集：

$^B T_G$：机器人末端在基座中的位姿，由 FK/TF 获得；
$^C T_T$：标定板在相机中的位姿，由 PnP 获得。

核心不变量与目标转换

每个姿态满足：

$$ {}^B T_G(i) {}^G T_C {}^C T_T(i) = {}^B T_T $$

右侧 $^B T_T$ 表示“标定板在机器人基座中的位姿”。因为标定板固定在工作台上，机械臂运动到不同姿态时，这个量应当保持不变。

需要区分两个固定量：$^G T_C$ 固定，表示相机刚性安装在末端、是待求的手眼外参；$^B T_T$ 固定，表示标定板固定在工作台上、是建立约束和验证结果的不变量。

标定完成后，目标转换为：

$$ {}^B T_O = {}^B T_G {}^G T_C {}^C T_O $$

即：目标 → 相机 → 末端 → 机器人基座。

Eye-to-hand：眼在手外

布局与目标

相机固定在机器人外部；标定板刚性安装在机械臂末端，随机械臂运动。需要求解的外参为：

$$ {}^B T_C: C \rightarrow B $$

它把相机系中的点转换到机器人基座系，也就是相机在基座中的位姿。标定板刚性安装在末端，因此标定板相对末端的安装关系 $^G T_T$ 同样固定。

每个姿态满足：

$$ {}^B T_C {}^C T_T(i) = {}^B T_G(i) {}^G T_T $$

左边表示“标定板 → 相机 → 基座”，右边表示“标定板 → 末端 → 基座”；两条路径描述同一个标定板，因此结果必须相等。

固定量、验证与目标转换

Eye-to-hand 同样有两个固定量：

$^B T_C$ 固定：相机固定在机器人外部，是待求外参；
$^G T_T$ 固定：标定板刚性安装在末端，是对应的验证不变量。

因此，Eye-to-hand 中标定板相对机械臂末端的安装位姿 $^G T_T$ 不变；外部相机相对基座的位姿 $^B T_C$ 也固定，且正是待求的手眼外参。

求出 $^B T_C$ 后，可对每组采样反算标定板安装位姿：

$$ {}^G T_T(i) = ({}^B T_G(i))^{-1} {}^B T_C {}^C T_T(i) $$

若标定正确，不同姿态反算出的 $^G T_T(i)$ 应当基本一致。目标转换为：

$$ {}^B T_O = {}^B T_C {}^C T_O $$

即：目标 → 相机 → 机器人基座。

Eye-to-hand 的相机结构稳定、视野较大，适合先在现有七轴机械臂项目中落地。

AX = XB 的口述理解

比较两个不同姿态，可以消掉固定不变的标定板位姿或安装关系，最终得到：

$$ AX = XB $$

$A$：机器人根据正运动学得到的末端相对运动；
$B$：相机通过观察标定板得到的相对运动；
$X$：相机与机器人之间待求的固定外参。

令 $G_i = {}^B T_G(i)$、$C_i = {}^C T_T(i)$，两种布局下的对应关系为：

$$ \begin{aligned} \text{Eye-in-hand}: &\quad A = G_j^{-1}G_i, \quad B = C_j C_i^{-1}, \quad X = {}^G T_C \\ \text{Eye-to-hand}: &\quad A = G_jG_i^{-1}, \quad B = C_j C_i^{-1}, \quad X = {}^B T_C \end{aligned} $$

物理上，$A$ 和 $B$ 描述同一次真实运动，只是分别使用机器人坐标系和相机坐标系表达；$X$ 用来完成两种表达之间的转换。

OpenCV 的 calibrateHandEye 输入的是每个姿态的绝对位姿，函数内部会构造相对运动 $A$、$B$。标准 Eye-in-hand 的输入输出为：

gripper2base：$^B T_G$；
target2cam：$^C T_T$，即 PnP 直接输出；
返回 cam2gripper：$^G T_C$。

用同一接口计算 Eye-to-hand 时，需要将机器人位姿取逆后输入第一组参数，第二组仍使用 PnP 的 $^C T_T$。接口变量名很容易造成误解，最终必须用固定关系一致性和物理触点实验验证方向。

为什么通常先求旋转，再求平移

将 $AX = XB$ 拆开：

$$ R_A R_X = R_X R_B $$

$$ (R_A - I)t_X = R_X t_B - t_A $$

旋转方程不包含待求平移，因此经典方法通常：

先求相机与机器人之间的旋转 $R_X$；
将旋转代入平移方程；
使用多组数据通过最小二乘求平移 $t_X$。

常见方法包括：

Tsai：经典线性化方案，先旋转后平移；
Park：利用旋转群的对数映射求旋转；
Horaud：使用四元数形式求旋转；
Andreff、Daniilidis：联合估计旋转和平移，其中 Daniilidis 使用对偶四元数；
RobotWorldHandEye：求解 $AX = ZB$，同时估计两个固定坐标关系。

算法选择并非最关键；坐标方向、数据质量和运动是否充分往往更重要。

退化与误差来源

典型退化

纯平移：旋转矩阵为单位阵，$t_X$ 从平移方程中消失，手眼平移不可观。
始终绕同一根轴旋转：沿该轴的平移分量不可观，$R_X$ 绕该轴也存在歧义。
旋转角太小：有效信号弱，结果容易受 PnP 和编码器噪声影响。
姿态数量多但分布相似：增加帧数不能弥补运动维度不足。

更准确地说，不应简单认为“纯旋转一定无法求平移”，而应检查旋转轴是否丰富、角度是否足够以及方程是否病态。两根以上不平行旋转轴可以提供更多约束。

常见误差来源

相机内参和畸变不准；
标定板尺寸或打印比例不准；
平面 PnP 深度不稳定；
图像时间戳与机器人位姿不同步；
相机或标定板安装不牢；
TCP、法兰和末端坐标系混用；
米与毫米、角度与弧度混用；
target2cam、cam2target 方向写反；
机器人零位、关节回差和结构形变。

标准实施流程

固定相机分辨率、焦距、对焦和曝光。
完成相机内参与畸变标定。
明确 base_link、法兰、TCP、camera optical frame 和 target frame。
根据 Eye-in-hand/Eye-to-hand 正确固定相机和标定板。
机械臂到达每个姿态后静止。
同步保存 $^B T_G$、图像、时间戳和 PnP 得到的 $^C T_T$。
至少需要两次轴不平行的相对旋转，也就是至少三个姿态；工程上建议采集约 20～40 组多轴旋转、不同位置和不同深度的数据。
比较多种算法并检查异常样本。
将结果发布为 ROS 静态 TF。
使用留出姿态和实际任务验证。

如何判断标定是否成功

不能只看求解器返回成功或单帧重投影误差，至少应进行三层验证：

相对运动残差：检查 $AX$ 和 $XB$ 是否接近。
固定关系一致性：Eye-in-hand 中，计算出的标定板在基座中的位姿应保持不变；Eye-to-hand 中，计算出的标定板到末端安装位姿应保持不变。
任务空间验证：让机械臂探针/TCP 到达视觉测量点，统计 XYZ 误差、旋转误差和 P95。

最终应以触点、定位或抓取误差为准。

结合现有七轴机械臂的补做方案

优先完成“固定 RealSense 的 Eye-to-hand 视觉定位项目”：

固定 RealSense
  → 末端安装 ChArUco 板
  → MoveIt 自动走多个标定姿态
  → 同步采集机器人末端位姿与标定板视觉位姿
  → 求相机到机器人基座的固定外参
  → 发布静态 TF
  → 检测桌面目标并转换到 base_link
  → MoveIt 到达预抓取点
  → Servo 低速精对准

需要特别注意：

planning_tip_link 是当前规划 TCP，必须确认它是否等于真实使用的 TCP。
现有 ArUco 项目将 aruco_world 数据直接标成 base_link，并通过 X/Y 取反进行人工对齐；这不是严格的外参标定。
手眼项目应输出真实的 $^B T_C$；在 TF 中 parent 为 base_link，child 为 camera_color_optical_frame。
正式采集应使用真实七轴反馈，不能使用 fake/hybrid 数据。
在真实夹爪控制完成前，应称为“视觉引导定位/预抓取对准”，不能称为完整抓取闭环。

技术要点回顾

手眼标定的本质是求相机坐标系与机器人坐标系之间固定的 6DoF 刚体变换。Eye-in-hand 中，相机固定在末端、标定板固定在工作台，要求 $^G T_C$，即将相机系点变到末端系；Eye-to-hand 中，相机固定在外部、标定板安装在末端，要求 $^B T_C$，即将相机系点变到基座系。

通过多个姿态消掉固定不变量，可得到 $AX = XB$：$A$ 是机器人相对运动，$B$ 是相机观察到的相对运动，$X$ 是待求手眼外参。实施时应先完成内参，统一坐标方向和单位，同步采集机器人末端位姿与 PnP 位姿，并保证多轴、非平行且角度充分的运动。求解后不应只看函数返回值，而要检查 $AX = XB$ 残差、固定关系一致性、留出姿态以及 TCP 实际触点误差。

必须记住的八句话

solvePnP 输出的是 target → camera，即 $^C T_T$。
手眼标定求的是两个刚性坐标系之间固定的 6DoF 外参。
Eye-in-hand 求 $^G T_C$：把相机系点转换到末端/TCP 系。
Eye-to-hand 求 $^B T_C$：把相机系点转换到机器人基座系。
$AX = XB$ 来自消掉一个固定不变的坐标关系。
$A$ 是机器人相对运动，$B$ 是相机观察到的相对运动，$X$ 是固定外参。
纯平移、单轴旋转和过小旋转角会导致退化或病态。
标定是否成功最终看 TCP 触点、定位或抓取误差，而不是只看重投影误差。