4、刚体变换与位姿表示：旋转矩阵、平移向量、齐次变换矩阵、欧拉角、四元数的数学基础

手眼标定的本质，是求解相机坐标系与机械臂基坐标系（或工具坐标系）之间的刚体变换关系。所谓“刚体”，意味着物体在运动过程中只发生旋转和平移，不发生形变。因此，描述两个坐标系之间的位姿关系，本质上就是描述一次旋转和一次平移。本章将系统梳理旋转矩阵、平移向量、齐次变换矩阵、欧拉角与四元数的数学定义、相互转换关系及其在机器人工程中的应用场景。这些数学工具不仅是手眼标定方程 AX = XB 的基石，也是后续误差补偿、轨迹规划与传感器融合的通用语言。

4.1 核心原理：从三维旋转到刚体变换的数学建模

刚体变换的数学基础建立在三维空间中的旋转与平移之上。旋转矩阵 R 是一个 3×3 的正交矩阵，满足 R^TR = I 且 det(R) = +1，所有这样的矩阵构成特殊正交群 SO(3)。旋转矩阵的每一列代表原坐标系各轴在新坐标系中的方向余弦，因此它完整描述了坐标轴的朝向关系。例如，绕 Z 轴旋转 θ 角的旋转矩阵为：

Rz(θ) = [cosθ  -sinθ  0;
          sinθ   cosθ  0;
          0      0     1]

平移向量 t 是一个 3×1 列向量，表示坐标系原点的位置偏移。将旋转与平移组合，便得到齐次变换矩阵 T：

T = [R  t;
     0  1]    （4×4 矩阵）

齐次变换矩阵属于特殊欧氏群 SE(3)，它允许将旋转和平移统一为一次矩阵乘法，从而支持变换的复合与求逆。例如，点 p 在坐标系 A 中的坐标 p_A 转换到坐标系 B 中，只需计算 p_B = T_AB · p_A（其中 p 为齐次坐标 [x,y,z,1]^T）。

欧拉角将旋转分解为绕三个坐标轴的依次旋转，常见的有 ZYX 顺序（即 RPY 角：滚转 Roll、俯仰 Pitch、偏航 Yaw）。欧拉角直观但存在万向锁问题——当中间旋转角为 ±90° 时，会丢失一个旋转自由度。四元数则用四个参数 (w, x, y, z) 表示旋转，其中 w 为实部，(x,y,z) 为虚部，满足 w²+x²+y²+z²=1。四元数避免了万向锁，且插值平滑，在机器人控制与视觉 SLAM 中广泛使用。

4.2 判断方法与操作流程：如何选择位姿表示方式

在实际手眼标定工程中，选择哪种位姿表示方式取决于具体任务需求。以下是判断流程：

1. 若需进行多次坐标变换复合（如相机→机械臂→基座），优先使用齐次变换矩阵，因为矩阵乘法可直接串联变换。
2. 若需优化求解标定方程 AX = XB，通常将旋转部分用四元数或旋转矩阵表示，平移部分单独提取，便于构造线性方程组。
3. 若需向操作人员展示姿态（如示教器界面），使用欧拉角（RPY）更直观。
4. 若需进行姿态插值或滤波（如视觉伺服平滑运动），使用四元数球面线性插值（SLERP）。

转换关系是工程中的高频操作。例如，从旋转矩阵 R 提取 ZYX 欧拉角：

β = atan2(-R31, sqrt(R11² + R21²))
α = atan2(R21/cosβ, R11/cosβ)
γ = atan2(R32/cosβ, R33/cosβ)

从旋转矩阵转四元数则通过构造迹与元素关系实现，需注意数值稳定性——当迹接近 -1 时需特殊处理。

4.3 案例拆解：六轴机械臂手眼标定中的位姿变换实战

以典型的眼在手上（Eye-in-Hand）标定场景为例。机械臂末端执行器上安装相机，标定板固定在工作台上。我们需要求解相机坐标系 C 到末端工具坐标系 E 的变换 T_CE。

背景与约束：机械臂型号为六轴协作机器人，重复定位精度 ±0.02mm。相机分辨率为 1280×1024，标定板为 7×7 棋盘格（方格边长 30mm）。系统要求标定后视觉引导抓取精度优于 ±1mm。

初始现象：直接使用出厂参数进行视觉引导，抓取误差达到 8~12mm，无法满足精度要求。

关键输入数据：采集 15 组不同位姿下的数据，每组包含：

• 机械臂末端位姿 T_EB（基坐标系 B 到末端 E，从控制器读取，以四元数+平移给出）
• 相机检测到的标定板位姿 T_CB（标定板坐标系到相机坐标系，通过 OpenCV 的 solvePnP 求解）

分析判断过程：根据手眼标定方程 T_CE · T_CB = T_EB · T_CE，将 15 组数据代入，采用 Tsai-Lenz 两步法求解：先利用旋转约束解出 R_CE，再代入平移约束解出 t_CE。

实际操作步骤：

1. 将机械臂末端四元数转换为旋转矩阵，与平移向量组合为 4×4 齐次矩阵 T_EB。
2. 将 solvePnP 返回的旋转向量（罗德里格斯形式）转换为旋转矩阵，组合为 T_CB。
3. 构造 AX = XB 方程组，使用奇异值分解（SVD）求解旋转矩阵。
4. 验证结果：计算重投影误差，若平均误差大于 0.5 像素，则剔除异常数据点后重新求解。

结果对比：标定后视觉引导抓取误差从 8~12mm 降至 0.3~0.6mm，满足 ±1mm 的工艺要求。重投影误差从初始的 3.2 像素降至 0.4 像素。

风险与代价：标定过程中若机械臂位姿选择不当（如所有位姿的旋转轴近似平行），会导致方程组病态，求解结果不稳定。为此，采集数据时应确保位姿差异足够大，覆盖多个旋转自由度。

复盘经验：位姿表示方式的统一是标定成功的前提。工程中建议所有中间计算均使用齐次矩阵，仅在数据采集和结果展示时进行格式转换，避免因四元数符号歧义或欧拉角顺序不一致引入错误。

4.4 常见误区与总结复盘

刚体变换与位姿表示虽然基础，但工程中错误频发。以下为高频问题与规避策略：

• 旋转矩阵正交性丢失：长时间浮点运算累积误差导致 R^TR ≠ I。解决方法是定期对旋转矩阵进行正交化（SVD 分解后重新合成）。
• 四元数未归一化：四元数必须满足单位模长，否则旋转结果会包含缩放。每次更新后应执行归一化操作。
• 齐次矩阵最后一行为 [0,0,0,1] 被误改：某些库函数可能修改该行，导致变换结果异常。建议封装安全的矩阵类，禁止外部直接修改最后一行。
• 欧拉角万向锁：当俯仰角接近 ±90° 时，滚转与偏航无法区分。此时应切换为四元数表示。