3. 刚体变换与位姿表示：旋转矩阵、欧拉角、四元数、平移向量，以及齐次变换矩阵的构建与分解

在3D视觉与手眼标定中，刚体变换是描述物体在三维空间中位置和姿态变化的核心数学工具。一个刚体在运动过程中，其内部任意两点之间的距离保持不变，这种变换由旋转（姿态）和平移（位置）两部分组成。本章将系统梳理旋转矩阵、欧拉角、四元数、平移向量以及齐次变换矩阵的数学定义、相互转换关系及工程应用。

3.1 旋转的三种主流表示方法

旋转描述了刚体坐标系相对于参考坐标系的朝向。在机器人学和计算机视觉中，最常用的三种表示方法为：旋转矩阵、欧拉角和四元数。每种方法都有其独特的优缺点，适用于不同的场景。

3.1.1 旋转矩阵

旋转矩阵 R 是一个3×3的正交矩阵，满足 R^TR = I 且 det(R) = +1。它描述了从一个坐标系到另一个坐标系的线性映射。对于三维空间中的向量 v，旋转后的向量 v' 为：

v' = R · v

绕三个坐标轴的基本旋转矩阵如下：

• 绕X轴旋转角度θ：
  R_x(θ) = [[1, 0, 0], [0, cosθ, -sinθ], [0, sinθ, cosθ]]
• 绕Y轴旋转角度θ：
  R_y(θ) = [[cosθ, 0, sinθ], [0, 1, 0], [-sinθ, 0, cosθ]]
• 绕Z轴旋转角度θ：
  R_z(θ) = [[cosθ, -sinθ, 0], [sinθ, cosθ, 0], [0, 0, 1]]

3.1.2 欧拉角

欧拉角将旋转分解为三个依次绕坐标轴的旋转。常见的顺序有 ZYX（偏航-俯仰-滚转）和 ZXZ 等。以 ZYX 顺序为例：

R = R_z(yaw) · R_y(pitch) · R_x(roll)

其中 yaw（偏航角）、pitch（俯仰角）、roll（滚转角）分别对应绕 Z、Y、X 轴的旋转角度。

3.1.3 四元数

四元数 q 由一个实部和三个虚部组成：q = w + xi + yj + zk，其中 w 为标量部分，(x, y, z) 为向量部分，且满足 w² + x² + y² + z² = 1。四元数表示绕单位向量 u 旋转角度 θ：

q = [cos(θ/2), u_x·sin(θ/2), u_y·sin(θ/2), u_z·sin(θ/2)]

四元数的乘法运算可组合旋转，且避免了欧拉角的奇异性，是机器人运动规划中的首选表示。

3.2 平移向量

平移向量 t = [t_x, t_y, t_z]^T 描述了坐标系原点之间的位移。在刚体变换中，平移与旋转是解耦的：旋转先作用于向量方向，平移再作用于位置。

对于空间中的一点 p，从坐标系 {A} 到坐标系 {B} 的变换为：

p_B = R_AB · p_A + t_AB

其中 R_AB 是 {A} 到 {B} 的旋转矩阵，t_AB 是 {A} 原点在 {B} 中的坐标。

3.3 齐次变换矩阵的构建与分解

为了将旋转和平移统一为线性运算，引入齐次变换矩阵 T，它是一个4×4的矩阵：

T = [[R, t],
     [0, 1]]

其中 R 为3×3旋转矩阵，t 为3×1平移向量，最后一行固定为 [0, 0, 0, 1]。使用齐次坐标 p̃ = [x, y, z, 1]^T，变换可简洁表示为：

p̃_B = T_AB · p̃_A

齐次变换矩阵的逆矩阵为：

T_inv = [[R^T, -R^T · t],
         [0,   1]]

这避免了直接求逆的复杂计算，在标定中频繁使用。

3.4 刚体变换关系图

下图展示了从旋转矩阵、欧拉角、四元数到齐次变换矩阵的完整转换路径，以及它们在手眼标定中的角色。

3.5 齐次变换矩阵的分解

在实际标定中，经常需要从已知的齐次变换矩阵 T 中分解出旋转矩阵 R 和平移向量 t。分解过程非常直接：

R = T[0:3, 0:3]   # 左上角3×3子矩阵
t = T[0:3, 3]     # 右上角3×1子向量

进一步，若需要从 R 中提取欧拉角或四元数，可使用以下方法：

• 旋转矩阵 → 欧拉角（ZYX顺序）：
  pitch = asin(-R[2][0])
  yaw = atan2(R[1][0], R[0][0])
  roll = atan2(R[2][1], R[2][2])
• 旋转矩阵 → 四元数：
  通过求解矩阵的迹和特征值，或使用标准转换公式。

import numpy as np

# 构建齐次变换矩阵
def build_transform(R, t):
    T = np.eye(4)
    T[0:3, 0:3] = R
    T[0:3, 3] = t.flatten()
    return T

# 分解齐次变换矩阵
def decompose_transform(T):
    R = T[0:3, 0:3]
    t = T[0:3, 3]
    return R, t

# 示例：绕Z轴旋转30度，平移(1, 2, 3)
theta = np.radians(30)
R = np.array([[np.cos(theta), -np.sin(theta), 0],
              [np.sin(theta),  np.cos(theta), 0],
              [0, 0, 1]])
t = np.array([1, 2, 3])
T = build_transform(R, t)
print("齐次变换矩阵:\n", T)

R_out, t_out = decompose_transform(T)
print("分解旋转矩阵:\n", R_out)
print("分解平移向量:\n", t_out)

3.6 刚体变换的链式法则与标定闭环

在手眼标定中，机器人末端执行器与相机之间的变换关系通常通过链式法则建立。设：

• T_base_to_ee：机器人基座到末端执行器的变换（由机器人正运动学提供）
• T_ee_to_cam：末端执行器到相机的变换（待标定的手眼矩阵）
• T_cam_to_obj：相机到标定物体的变换（由视觉算法提供）
• T_base_to_obj：机器人基座到标定物体的变换（固定不变）

闭环方程为：

T_base_to_ee · T_ee_to_cam · T_cam_to_obj = T_base_to_obj

通过多次移动机器人并记录对应的变换对，即可求解出 T_ee_to_cam。这正是下一章将要深入讲解的手眼标定核心算法。