4. 旋转表示方法：旋转矩阵、欧拉角、轴角、四元数

在3D视觉与手眼标定中，刚体的旋转是核心数学基础。旋转的表示方法有多种，每种方法在计算效率、直观性、奇异性等方面各有优劣。本章系统梳理四种主流表示方法及其转换关系。

4.1 四种表示方法概述

4.2 旋转矩阵

旋转矩阵 R 是一个 3×3 正交矩阵，满足 RᵀR = I 且 det(R) = +1。它直接作用于三维向量：v' = R · v。

• 优点：无奇异点，可组合（矩阵乘法），可直接用于点云变换。
• 缺点：9个参数冗余，不直观，数值更新需强制正交化。

4.3 欧拉角

欧拉角将旋转分解为绕三个坐标轴的依次旋转。常见顺序有 ZYX（航空）、XYZ（机器人）、ZYZ（经典力学）。

• 优点：直观，仅3个参数，适合人机交互。
• 缺点：万向锁（Gimbal Lock）——当中间角为 ±90° 时，首尾轴重合，丢失一个自由度；旋转顺序依赖，不同顺序结果不同。

4.4 轴角

轴角用一个单位向量 k 和一个标量角 θ 表示旋转：绕 k 轴旋转 θ 弧度。

• 优点：几何意义最直观，参数紧凑（3个）。
• 缺点：θ=0 时旋转轴无定义；插值不自然；组合运算复杂。

4.5 四元数

四元数 q = w + x·i + y·j + z·k，满足 w² + x² + y² + z² = 1。单位四元数表示三维旋转。

• 优点：无奇异，可平滑插值（球面线性插值 Slerp），计算效率高（比矩阵快），存储紧凑（4个浮点数）。
• 缺点：不够直观，需注意双覆盖（q 和 -q 表示同一旋转）。

4.6 表示方法转换关系

四种表示方法之间可以相互转换，以下是核心转换公式与代码实现。

4.6.1 旋转矩阵 ↔ 四元数

// 旋转矩阵 R 转四元数 q (w, x, y, z)
double trace = R[0][0] + R[1][1] + R[2][2];
if (trace > 0) {
    double s = 0.5 / sqrt(trace + 1.0);
    w = 0.25 / s;
    x = (R[2][1] - R[1][2]) * s;
    y = (R[0][2] - R[2][0]) * s;
    z = (R[1][0] - R[0][1]) * s;
} else {
    // 处理 trace ≤ 0 的情况，选取最大对角元
    // ...
}

// 四元数 q 转旋转矩阵 R
double R[3][3];
R[0][0] = 1 - 2*y*y - 2*z*z;
R[0][1] = 2*x*y - 2*w*z;
R[0][2] = 2*x*z + 2*w*y;
R[1][0] = 2*x*y + 2*w*z;
R[1][1] = 1 - 2*x*x - 2*z*z;
R[1][2] = 2*y*z - 2*w*x;
R[2][0] = 2*x*z - 2*w*y;
R[2][1] = 2*y*z + 2*w*x;
R[2][2] = 1 - 2*x*x - 2*y*y;

4.6.2 轴角 ↔ 旋转矩阵（罗德里格斯公式）

// 轴角 (kx, ky, kz, theta) 转旋转矩阵 R
double c = cos(theta);
double s = sin(theta);
double v = 1 - c;
double R[3][3];
R[0][0] = kx*kx*v + c;    R[0][1] = kx*ky*v - kz*s; R[0][2] = kx*kz*v + ky*s;
R[1][0] = kx*ky*v + kz*s; R[1][1] = ky*ky*v + c;    R[1][2] = ky*kz*v - kx*s;
R[2][0] = kx*kz*v - ky*s; R[2][1] = ky*kz*v + kx*s; R[2][2] = kz*kz*v + c;

4.6.3 欧拉角 ↔ 旋转矩阵（以 ZYX 顺序为例）

// 欧拉角 (yaw, pitch, roll) 转旋转矩阵 R
// ZYX: R = Rz(yaw) * Ry(pitch) * Rx(roll)
double R[3][3];
R[0][0] = cos(yaw)*cos(pitch);
R[0][1] = cos(yaw)*sin(pitch)*sin(roll) - sin(yaw)*cos(roll);
R[0][2] = cos(yaw)*sin(pitch)*cos(roll) + sin(yaw)*sin(roll);
R[1][0] = sin(yaw)*cos(pitch);
R[1][1] = sin(yaw)*sin(pitch)*sin(roll) + cos(yaw)*cos(roll);
R[1][2] = sin(yaw)*sin(pitch)*cos(roll) - cos(yaw)*sin(roll);
R[2][0] = -sin(pitch);
R[2][1] = cos(pitch)*sin(roll);
R[2][2] = cos(pitch)*cos(roll);

4.7 转换关系总览图

下图展示了四种旋转表示方法之间的核心转换路径与常用转换公式。

4.8 各表示方法优缺点对比

4.9 手眼标定中的选择策略

4.10 常见转换陷阱

• 欧拉角顺序混淆：不同库（如 Eigen、ROS、MATLAB）默认顺序不同，转换前必须确认。
• 四元数双覆盖：q 和 -q 表示同一旋转，在优化中可能导致符号跳变，需做符号一致性处理。
• 轴角 θ 接近 0：此时旋转轴方向对数值误差敏感，建议使用一阶泰勒展开近似。
• 旋转矩阵正交性漂移：长时间迭代后矩阵可能偏离正交，需定期使用 SVD 或 Gram-Schmidt 正交化。