2、机器人运动学基础：刚体变换、旋转矩阵、欧拉角、四元数、齐次坐标与齐次变换矩阵
手眼标定的本质，是在两个或多个坐标系之间建立精确的数学映射关系。无论是“眼在手上”还是“眼在手外”，最终都要回答同一个问题：机械臂末端执行器上的某一点，在相机坐标系下看到的坐标，如何转换到机器人基坐标系下？ 这个问题的答案，完全建立在刚体变换与齐次变换矩阵的理论基础之上。本章将系统梳理这些核心数学工具，帮助你建立从“物理空间中的旋转和平移”到“矩阵运算”的完整认知链路。
2.1 刚体变换：描述物体在空间中的位置与姿态
刚体变换（Rigid Body Transformation）是机器人学中最基础的数学模型。所谓“刚体”，是指物体在运动过程中不发生形变，其上任意两点之间的距离保持不变。机械臂的连杆、末端法兰、安装在法兰上的相机，在理想情况下都可以视为刚体。刚体变换由两部分组成：旋转（Rotation）和平移（Translation），共6个自由度——3个旋转自由度和3个平移自由度。
在三维空间中，一个刚体相对于参考坐标系的位姿（Pose）可以表示为：
位置：t = [tx, ty, tz]ᵀ （从参考系原点指向刚体原点的平移向量）
姿态：R ∈ SO(3) （刚体坐标系相对于参考系的旋转矩阵）
刚体变换的核心性质包括：
- 保距性：变换前后，刚体上任意两点间的欧氏距离不变。
- 保角性：变换前后，刚体上任意两条线段的夹角不变。
- 可逆性：刚体变换存在逆变换，且逆变换仍为刚体变换。
- 可复合性：多个刚体变换依次作用，等价于一个总的刚体变换。
2.2 旋转矩阵：SO(3)群与正交约束
旋转矩阵 R 是一个 3×3 的正交矩阵，满足 RᵀR = I 且 det(R) = +1。所有满足这些条件的矩阵构成特殊正交群 SO(3)。旋转矩阵的每一列（或每一行）表示刚体坐标系的一个轴在参考坐标系中的方向余弦。
以绕 Z 轴旋转 θ 角为例，旋转矩阵为：
Rz(θ) = [cosθ -sinθ 0]
[sinθ cosθ 0]
[ 0 0 1]
同理，绕 X 轴和 Y 轴的旋转矩阵分别为：
Rx(θ) = [1 0 0 ]
[0 cosθ -sinθ]
[0 sinθ cosθ]
Ry(θ) = [cosθ 0 sinθ]
[ 0 1 0 ]
[-sinθ 0 cosθ]
旋转矩阵的工程意义在于：
- 坐标变换：将向量从一个坐标系变换到另一个坐标系，即 p' = R · p。
- 姿态描述：完整描述刚体在空间中的朝向，无奇异问题。
- 矩阵乘法复合：连续旋转可通过矩阵乘法实现，但注意矩阵乘法不满足交换律，旋转顺序至关重要。
2.3 欧拉角：直观但存在万向锁风险
欧拉角将旋转分解为绕三个坐标轴的依次旋转。常用的约定包括：
- ZYX 欧拉角：先绕 Z 轴旋转 yaw（偏航角），再绕新 Y 轴旋转 pitch（俯仰角），最后绕新 X 轴旋转 roll（横滚角）。
- RPY 角（固定角）：绕固定参考系的 X-Y-Z 轴依次旋转，与 ZYX 欧拉角在数值上存在对应关系。
- ZYZ 欧拉角：常用于机器人运动学分析。
欧拉角到旋转矩阵的转换（以 ZYX 为例）：
R = Rz(yaw) · Ry(pitch) · Rx(roll)
R = [cy·cp cy·sp·sr - sy·cr cy·sp·cr + sy·sr]
[sy·cp sy·sp·sr + cy·cr sy·sp·cr - cy·sr]
[-sp cp·sr cp·cr ]
其中：cy=cos(yaw), sy=sin(yaw), cp=cos(pitch), sp=sin(pitch), cr=cos(roll), sr=sin(roll)
万向锁（Gimbal Lock）是欧拉角最致命的缺陷：当 pitch = ±90° 时，yaw 和 roll 的旋转轴重合，导致丢失一个旋转自由度，系统退化为2自由度。此时，不同的 (yaw, roll) 组合可能产生相同的旋转矩阵，造成奇异性。
2.4 四元数：无奇异性的旋转表示
四元数（Quaternion）是哈密顿在1843年提出的超复数，形式为 q = w + xi + yj + zk，其中 w 为实部，(x, y, z) 为虚部。单位四元数（||q|| = 1）可以唯一表示三维空间中的旋转。
四元数与旋转矩阵的转换关系为：
给定单位四元数 q = (w, x, y, z)，对应的旋转矩阵为：
R = [1-2(y²+z²) 2(xy-wz) 2(xz+wy) ]
[2(xy+wz) 1-2(x²+z²) 2(yz-wx) ]
[2(xz-wy) 2(yz+wx) 1-2(x²+y²)]
四元数的核心优势：
- 无万向锁：不存在奇异姿态，适合全姿态范围工作。
- 紧凑性：仅需4个参数，比旋转矩阵的9个参数更高效。
- 插值平滑：通过球面线性插值（SLERP）可实现姿态的平滑过渡。
- 复合运算简单：四元数乘法对应旋转复合，计算量小于矩阵乘法。
四元数乘法规则：
q₁ ⊗ q₂ = (w₁w₂ - x₁x₂ - y₁y₂ - z₁z₂,
w₁x₂ + x₁w₂ + y₁z₂ - z₁y₂,
w₁y₂ - x₁z₂ + y₁w₂ + z₁x₂,
w₁z₂ + x₁y₂ - y₁x₂ + z₁w₂)
2.5 齐次坐标与齐次变换矩阵：统一旋转与平移
旋转矩阵只能描述姿态，无法直接表达平移。为了将旋转和平移统一到一个矩阵运算中，引入齐次坐标和齐次变换矩阵。
齐次坐标将三维点 (x, y, z) 表示为四维向量 (x, y, z, 1)ᵀ，将三维向量 (vx, vy, vz) 表示为 (vx, vy, vz, 0)ᵀ。这样，平移操作就可以通过矩阵乘法实现。
齐次变换矩阵 T 的形式为：
T = [R t] = [r₁₁ r₁₂ r₁₃ tx]
[0ᵀ 1] [r₂₁ r₂₂ r₂₃ ty]
[r₃₁ r₃₂ r₃₃ tz]
[ 0 0 0 1 ]
其中 R 为 3×3 旋转矩阵，t 为 3×1 平移向量。齐次变换矩阵属于特殊欧氏群 SE(3)。
齐次变换的核心运算规则：
- 坐标变换：p' = T · p，其中 p 和 p' 均为齐次坐标。
- 逆变换：T⁻¹ = [Rᵀ -Rᵀt; 0 1]，表示从目标系变换回参考系。
- 复合变换：T = T₁ · T₂，表示先进行 T₂ 变换，再进行 T₁ 变换。
| 表示方法 | 参数数量 | 优点 | 缺点 | 适用场景 |
|---|---|---|---|---|
| 旋转矩阵 R | 9（3独立） | 无奇异，直接用于坐标变换 | 冗余参数多，需约束 | 坐标变换计算、矩阵复合 |
| 欧拉角 | 3 | 直观，易于理解 | 万向锁，插值不平滑 | 人机交互、简单姿态设定 |
| 四元数 | 4 | 无奇异，插值平滑，紧凑 | 不够直观 | 姿态插补、标定计算、ROS消息 |
| 齐次变换矩阵 T | 16（6独立） | 统一旋转+平移，链式复合 | 参数冗余 | 运动学正解、手眼标定 |
2.6 案例拆解：从相机坐标到机械臂基座的完整变换链
假设一个典型的“眼在手上”视觉抓取场景，我们需要将相机检测到的目标点坐标转换到机械臂基坐标系下。整个变换链如下：
目标点在像素坐标系：p_pixel = (u, v)
步骤1：像素坐标 → 相机坐标系（利用相机内参 K）
p_cam = K⁻¹ · [u, v, 1]ᵀ · depth
步骤2：相机坐标系 → 机械臂末端坐标系（手眼标定结果 T_cam_to_end）
p_end = T_cam_to_end · p_cam
步骤3：机械臂末端坐标系 → 机械臂基坐标系（机器人运动学正解 T_base_to_end）
p_base = T_base_to_end · p_end
最终：p_base 即为机械臂基坐标系下的目标位置，可直接用于路径规划。
在这个变换链中，每一步都是一个齐次变换矩阵的乘法运算。关键点在于：
- T_cam_to_end 是手眼标定要求解的未知量，它是固定不变的。
- T_base_to_end 由机械臂的关节角度通过运动学正解实时计算得出。
- 整个变换链可以合并为一个总变换矩阵：T_base_to_cam = T_base_to_end · T_cam_to_end。
2.7 常见误区与总结复盘
在刚体变换与齐次矩阵的学习和应用中，以下误区最为常见：
总结复盘：本章的核心知识可归纳为以下要点：
- 刚体变换 = 旋转 + 平移，共6自由度，是手眼标定的数学基础。
- 旋转矩阵 是SO(3)群元素，适合坐标变换计算，但参数冗余。
- 欧拉角 直观但存在万向锁问题，不适合全姿态计算。
- 四元数 无奇异、插值平滑，是工程计算的首选旋转表示。
- 齐次变换矩阵 统一了旋转和平移，通过链式乘法实现坐标系间的连续变换。
- 手眼标定的本质就是求解 T_cam_to_end（眼在手上）或 T_cam_to_base（眼在手外）这个齐次变换矩阵。
掌握这些数学工具后，下一阶段我们将进入手眼标定的核心问题：如何通过机械臂的多次运动，采集足够的数据对，求解出这个关键的变换矩阵。届时，本章的齐次变换矩阵将是构建标定方程的基本单元。