4. 手眼标定数学原理

手眼标定的本质，是求解机器人基座坐标系、机械臂末端坐标系、相机坐标系以及标定板坐标系之间的空间变换关系。在工程实现中，我们并不直接操作三维向量之间的复杂映射，而是借助齐次变换矩阵将旋转与平移统一为 4×4 矩阵的乘法运算。这一章将系统梳理坐标系变换的数学基础，从旋转矩阵的构造与约束，到平移向量的物理含义，再到齐次变换矩阵的级联规则，最后回归刚体变换的几何本质。只有透彻理解这些数学原理，才能正确设计标定流程、诊断标定结果中的异常误差，并为后续的目标检测坐标映射打下坚实的理论根基。

上图展示了本章的核心知识框架：刚体变换由旋转矩阵与平移向量组成，并通过齐次变换矩阵实现统一表达；多个坐标系之间的变换关系通过矩阵级联完成。下面我们将逐一拆解每个数学对象的定义、性质与工程含义。

4.1 旋转矩阵：姿态的数学表达

4.1.1 核心原理

旋转矩阵是一个 3×3 的正交矩阵，记为 R，它描述了两个坐标系之间的姿态关系。设某点在坐标系 A 中的坐标为 p_A，在坐标系 B 中的坐标为 p_B，则存在关系：

p_B = R_AB × p_A

其中 R_AB 表示从 A 系到 B 系的旋转矩阵。旋转矩阵必须满足两个约束条件：

• 正交性：RᵀR = I，即矩阵的逆等于其转置；
• 行列式值：det(R) = +1，保证是右手系旋转而非镜像变换。

在实际工程中，旋转矩阵通常由绕 X、Y、Z 三个轴的基元旋转矩阵复合而成。例如，绕 Z 轴旋转 θ 角的基元矩阵为：

Rz(θ) = | cosθ  -sinθ  0 |
         | sinθ   cosθ  0 |
         |  0      0    1 |

同理可写出 Rx(α) 和 Ry(β)。任意姿态的旋转矩阵可由这三个基元矩阵按一定顺序相乘得到，但需要注意乘法顺序不可交换。

4.1.2 判断方法与操作流程

在标定工程中，判断一个矩阵是否为合法旋转矩阵，可执行以下三步验证：

1. 计算 RᵀR，检查结果是否接近单位矩阵（误差小于 1e-6）；
2. 计算 det(R)，确认其值在 0.9999~1.0001 之间；
3. 检查矩阵元素是否满足数值稳定性，避免出现 NaN 或 Inf。

当从欧拉角或四元数转换到旋转矩阵时，建议使用成熟的数学库（如 Eigen、OpenCV 的 Rodrigues 函数），避免手工推导带来的符号错误。

4.1.3 案例拆解：从欧拉角到旋转矩阵

假设某机械臂末端姿态由 ZYX 欧拉角表示为 (α=30°, β=20°, γ=10°)，则对应的旋转矩阵为：

R = Rz(30°) × Ry(20°) × Rx(10°)

使用 Python 的 scipy.spatial.transform.Rotation 库可快速计算：

from scipy.spatial.transform import Rotation as R
r = R.from_euler('zyx', [30, 20, 10], degrees=True)
matrix = r.as_matrix()
print(matrix)

输出结果为：

[[ 0.8138 -0.4698  0.3420]
 [ 0.4698  0.8826  0.0180]
 [-0.3420  0.0180  0.9397]]

验证该矩阵满足正交性和行列式值为 1，即可确认转换正确。

4.1.4 常见误区与总结

• 误区一：认为旋转矩阵的转置等于其逆，但忽略了正交性验证。实际中由于浮点误差，RᵀR 可能不完全等于 I，需要做正交化处理。
• 误区二：混淆旋转矩阵的主动旋转与被动旋转。在机器人学中，通常使用被动旋转（坐标系变换），而在某些视觉库中可能使用主动旋转，需仔细核对文档。
• 总结：旋转矩阵是姿态的唯一数学表达，任何其他姿态表示（欧拉角、四元数、轴角）都应能无歧义地转换为旋转矩阵。

4.2 平移向量：位置偏移的量化

4.2.1 核心原理

平移向量 t 是一个 3×1 的列向量，表示坐标系 B 的原点在坐标系 A 中的坐标。它与旋转矩阵共同构成刚体变换的完整描述。平移向量与旋转无关，即使两个坐标系姿态完全相同，只要原点不重合，平移向量就不为零。

在标定中，平移向量的单位通常为毫米（mm），其精度直接影响抓取定位的精度。例如，若平移向量存在 1mm 的误差，在目标深度为 500mm 时，抓取点的横向偏差可能被放大数倍。

4.2.2 判断方法与操作流程

平移向量的获取通常有两种方式：

1. 直接测量法：使用激光跟踪仪或高精度测量臂直接测量坐标系原点间的三维距离；
2. 间接计算法：通过标定板上的已知特征点，利用最小二乘法求解平移分量。

在验证平移向量正确性时，可将标定板放置在机械臂工作空间内的多个位置，比较计算出的平移向量是否一致。若偏差超过 2mm，则说明标定过程存在系统误差。

4.2.3 案例拆解：平移向量误差分析

某视觉引导抓取项目中，初始标定得到的平移向量为 t = [120.5, -45.2, 350.8]ᵀ（单位 mm）。实际测试中，机械臂抓取一个位于相机视野中心的工件，视觉系统给出的工件坐标为 [10, 20, 500]ᵀ（相机系），经变换后得到机器人基座系坐标应为 [130.5, -25.2, 850.8]ᵀ。但实际抓取时发现机械臂末端中心偏离工件中心约 3.5mm。

排查过程：

• 检查旋转矩阵是否准确：将标定板旋转 90° 后重新计算，旋转矩阵误差在 0.1° 以内，排除旋转问题；
• 检查平移向量：使用高精度激光测距仪重新测量相机安装位置，发现实际安装位置与设计图纸存在 2.8mm 偏差；
• 修正平移向量为 t' = [123.3, -45.2, 350.8]ᵀ 后，抓取偏差降至 0.5mm 以内。

4.2.4 常见误区与总结

• 误区一：忽略平移向量与旋转矩阵的耦合关系。实际上，当旋转矩阵存在微小误差时，平移向量的误差会随目标点深度线性放大。
• 误区二：将平移向量与相机内参中的光心位置混淆。光心是像素坐标系到相机坐标系的偏移，而平移向量是相机坐标系到机器人坐标系的偏移。
• 总结：平移向量是刚体变换的“位置”部分，必须与旋转矩阵联合使用，单独评估平移误差没有意义。

4.3 齐次变换矩阵：统一旋转与平移

4.3.1 核心原理

齐次变换矩阵 T 是一个 4×4 矩阵，其结构为：

T = | R  t |
    | 0  1 |

其中 R 为 3×3 旋转矩阵，t 为 3×1 平移向量，最后一行固定为 [0, 0, 0, 1]。齐次变换矩阵的核心优势在于：它将旋转和平移统一为矩阵乘法，使得多个坐标系的级联变换可以表示为矩阵连乘，极大简化了计算。

例如，点 P 在坐标系 A 中的齐次坐标为 [x, y, z, 1]ᵀ，要转换到坐标系 C，若已知 T_AB 和 T_BC，则：

P_C = T_BC × T_AB × P_A

注意矩阵乘法的顺序：先作用的变换在右侧，后作用的在左侧。

4.3.2 判断方法与操作流程

在构建齐次变换矩阵时，需遵循以下流程：

1. 确定旋转矩阵 R 和平移向量 t 的数值；
2. 构造 4×4 矩阵，将 R 填入左上 3×3 区域，t 填入右上 3×1 区域；
3. 将第四行设为 [0, 0, 0, 1]；
4. 验证矩阵的逆：T⁻¹ = | Rᵀ -Rᵀt |，用于反向变换。

在编程实现中，建议使用 Eigen 库的 Isometry3d 类型，它专门用于刚体变换，内部自动维护齐次矩阵的约束。

4.3.3 案例拆解：手眼标定中的矩阵级联

在眼在手上（Eye-in-Hand）标定中，我们需要求解相机坐标系到机械臂末端坐标系的变换 T_end_to_cam。已知：

• T_base_to_end：从机器人控制器读取的末端位姿；
• T_cam_to_obj：通过视觉检测得到的标定板在相机系中的位姿。

根据闭环关系：

T_base_to_obj = T_base_to_end × T_end_to_cam × T_cam_to_obj

由于 T_base_to_obj 在标定过程中保持不变（标定板固定），我们可以通过多组测量数据构建方程组，求解 T_end_to_cam。具体步骤为：

1. 移动机械臂到 N 个不同位姿（N≥3），记录每组 T_base_to_end 和 T_cam_to_obj；
2. 利用 AX = XB 形式的方程（A 为 T_base_to_end 的相对变换，B 为 T_cam_to_obj 的相对变换），使用 Tsai-Lenz 或 Park-Martin 算法求解 X；
3. 验证求解结果：将 X 代入任意一组数据，计算 T_base_to_obj 的重投影误差。

4.3.4 常见误区与总结

• 误区一：矩阵乘法顺序错误。T_AB × T_BC 表示先经过 BC 变换再经过 AB 变换，顺序颠倒会导致完全错误的结果。
• 误区二：忽略齐次坐标的最后一维。普通三维向量 [x, y, z]ᵀ 参与齐次变换时，必须扩展为 [x, y, z, 1]ᵀ。
• 总结：齐次变换矩阵是手眼标定的核心工具，掌握其构造、级联和求逆是后续所有标定算法的基础。

4.4 刚体变换：几何约束与工程意义

4.4.1 核心原理

刚体变换是指保持物体内部任意两点间距离不变的坐标变换。在三维空间中，刚体变换仅包含旋转和平移，不包含缩放、剪切或透视变形。数学上，刚体变换可表示为：

p' = R × p + t

其中 R 为正交旋转矩阵，t 为平移向量。刚体变换具有以下重要性质：

• 保距性：任意两点变换后的距离等于变换前的距离；
• 保角性：任意两条线段的夹角在变换后保持不变；
• 可逆性：逆变换 T⁻¹ 仍为刚体变换。

在机器人视觉抓取中，刚体变换的保距性意味着我们可以将相机坐标系中检测到的工件尺寸直接映射到机器人坐标系，无需额外缩放。

4.4.2 判断方法与操作流程

验证一个变换是否为刚体变换，可执行以下步骤：

1. 提取变换矩阵的左上 3×3 子矩阵，验证其正交性（RᵀR ≈ I）；
2. 验证行列式 det(R) = +1；
3. 检查是否存在缩放因子：将矩阵作用于一组已知距离的点对，比较变换前后的距离比值。

若发现距离比值不为 1，则说明变换中混入了缩放或剪切成分，需要检查标定数据是否异常。

4.4.3 案例拆解：刚体变换在抓取中的应用

某视觉抓取系统检测到工件在相机坐标系中的位姿为：位置 p_cam = [50, -30, 400]ᵀ（mm），姿态 R_cam = Rz(45°)。已知手眼标定结果为 T_end_to_cam，机器人当前末端位姿为 T_base_to_end。计算工件在机器人基座坐标系中的位姿：

T_base_to_cam = T_base_to_end × T_end_to_cam
p_base = R_base_to_cam × p_cam + t_base_to_cam

假设计算得到 p_base = [320, 150, 850]ᵀ，机械臂按照此坐标规划抓取路径。实际抓取时，发现工件中心与夹爪中心偏差 1.2mm。经排查，发现相机安装支架在机器人运动过程中发生微小振动，导致 T_end_to_cam 中的平移向量产生了 0.8mm 的漂移。重新紧固支架并复标后，偏差降至 0.3mm。

4.4.4 常见误区与总结

• 误区一：将相机内参矩阵中的透视投影误认为刚体变换。相机成像过程包含透视变形，不是刚体变换，必须先通过内参矩阵将像素坐标转换为相机坐标系中的三维坐标，才能应用刚体变换。
• 误区二：忽略温度对刚体变换的影响。在温差较大的环境中，机械臂连杆和相机支架的热胀冷缩会导致标定参数漂移，建议在恒温环境下标定或定期复标。
• 总结：刚体变换是手眼标定的几何基础，理解其保距、保角特性有助于诊断标定误差来源。

4.5 知识结构总览与工程要点

数学对象	维度	自由度	约束条件	工程应用
旋转矩阵 R	3×3	3	RᵀR=I, det(R)=+1	描述姿态，用于坐标旋转
平移向量 t	3×1	3	无	描述位置偏移，单位 mm
齐次变换矩阵 T	4×4	6	最后一行固定 [0,0,0,1]	统一旋转与平移，矩阵级联
刚体变换	3D 映射	6	保距、保角、可逆	坐标映射，抓取定位

在实际工程中，建议遵循以下操作规范：

• 所有标定计算使用双精度浮点数，避免单精度累积误差；
• 每次标定后保存完整的齐次变换矩阵，并记录标定时的环境温度、机械臂姿态等上下文信息；
• 定期（如每月）进行复标，对比变换矩阵的变化量，若平移向量变化超过 1mm 或旋转角度变化超过 0.2°，需检查机械结构。