3、刚体姿态描述：欧拉角、四元数、旋转向量与旋转矩阵的相互转换

在六轴机械臂的运动学求解中，描述末端执行器或各关节连杆在三维空间中的朝向，是正解与逆解算法的基础。姿态描述方式的选择直接影响计算效率、数值稳定性以及工程实现的复杂度。本章将系统梳理欧拉角、四元数、旋转向量与旋转矩阵四种主流姿态表示方法，重点讲解它们之间的数学转换关系、适用场景以及工程中的常见陷阱。理解这些转换是后续推导机械臂DH参数表、构建雅可比矩阵和实现轨迹规划的前提。

3.1 四种姿态表示的核心原理与数学定义

刚体姿态描述的本质是建立从物体固连坐标系到参考坐标系的旋转变换。旋转矩阵是最基础的表示形式，它是一个3×3的正交矩阵，满足 R^TR = I 且 det(R) = +1。旋转矩阵的9个元素之间存在6个约束方程，因此实际只有3个自由度。在机械臂正解中，相邻连杆间的姿态变换通常通过矩阵连乘实现，但直接使用9个参数会带来冗余存储和数值漂移问题。

欧拉角通过三次绕坐标轴的旋转来分解任意姿态。以工业机器人常用的ZYX顺序为例，先绕Z轴旋转偏航角(yaw)，再绕新Y轴旋转俯仰角(pitch)，最后绕新X轴旋转滚转角(roll)。这种分解方式直观且便于人类理解，但存在著名的万向节锁问题：当俯仰角接近±90°时，第一次和第三次旋转的轴重合，导致一个自由度丢失。四元数则采用超复数形式 q = w + xi + yj + zk 表示旋转，其中 w 为标量部分，向量部分 (x,y,z) 对应旋转轴方向，且满足 w²+x²+y²+z²=1。旋转向量（轴角表示）将姿态描述为绕单位向量 n 旋转 θ 角，形式上最为紧凑，但向量加法不满足旋转合成规则。

3.2 旋转矩阵与欧拉角的相互转换

从欧拉角到旋转矩阵的转换遵循"绕固定轴还是绕动轴"的约定。工业机器人通常采用内旋（绕动轴）方式，即每次旋转都绕当前坐标系的新轴进行。对于ZYX内旋顺序，合成旋转矩阵为：

R = Rz(α) · Ry(β) · Rx(γ)
其中：
Rz(α) = [cosα  -sinα  0;  sinα  cosα  0;  0  0  1]
Ry(β) = [cosβ  0  sinβ;  0  1  0;  -sinβ  0  cosβ]
Rx(γ) = [1  0  0;  0  cosγ  -sinγ;  0  sinγ  cosγ]

展开后得到完整的3×3矩阵元素。反向求解时，从旋转矩阵提取欧拉角需要利用反正切函数 atan2 来保证角度范围正确。设 R 的元素为 r_ij，则：

β = atan2(-r31, sqrt(r11² + r21²))
α = atan2(r21/cosβ, r11/cosβ)
γ = atan2(r32/cosβ, r33/cosβ)

当 cosβ ≈ 0 时（即 β = ±90°），出现万向节锁，此时 α 和 γ 无法唯一确定，通常约定 γ = 0，则 α = atan2(r12, r22)。

3.3 旋转矩阵与四元数的转换

从四元数 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²)]

反向转换时，从旋转矩阵提取四元数需要处理数值稳定性问题。最稳健的方法是先计算矩阵的迹 trace = r11+r22+r33，然后分四种情况：

1. 若 trace > 0，则 w = 0.5·sqrt(1+trace)，x = (r32-r23)/(4w)，y = (r13-r31)/(4w)，z = (r21-r12)/(4w)
2. 若 trace ≤ 0 且 r11 最大，则 x = 0.5·sqrt(1+r11-r22-r33)，w = (r32-r23)/(4x)，y = (r12+r21)/(4x)，z = (r13+r31)/(4x)
3. 若 trace ≤ 0 且 r22 最大，则 y = 0.5·sqrt(1+r22-r11-r33)，w = (r13-r31)/(4y)，x = (r12+r21)/(4y)，z = (r23+r32)/(4y)
4. 若 trace ≤ 0 且 r33 最大，则 z = 0.5·sqrt(1+r33-r11-r22)，w = (r21-r12)/(4z)，x = (r13+r31)/(4z)，y = (r23+r32)/(4z)

这种分支处理避免了除以接近零的数。转换后需对四元数进行归一化处理，防止累积误差导致非单位四元数。

3.4 旋转向量与旋转矩阵、四元数的转换

旋转向量 θ·n 到旋转矩阵的转换由罗德里格斯公式给出：

R = cosθ·I + (1-cosθ)·n·nᵀ + sinθ·[n]×
其中 [n]× 为 n 的反对称矩阵：
[n]× = [0  -nz  ny;  nz  0  -nx;  -ny  nx  0]

反向提取时，旋转角 θ = acos((trace(R)-1)/2)，旋转轴 n 由矩阵的反对称部分求得：n = (1/(2sinθ))·[r32-r23; r13-r31; r21-r12]。当 θ 接近 0 或 π 时，sinθ 趋近于零，需要特殊处理：θ 接近 0 时，旋转轴方向不确定，可近似取 R 的列向量；θ 接近 π 时，使用 R+I 的特征向量求解。

旋转向量与四元数的转换更为直接：

q = (cos(θ/2), n·sin(θ/2))
反向：θ = 2·acos(w)，n = (x,y,z)/sin(θ/2)

这种转换在机械臂的微分运动学中尤为重要，因为角速度向量本质上就是旋转向量的导数。

3.5 实战案例：六轴机械臂姿态插补中的转换策略

在某型六轴焊接机械臂的轨迹规划中，要求末端执行器在笛卡尔空间从姿态 A 平滑过渡到姿态 B。初始姿态 A 的欧拉角为 (30°, 45°, 60°)，目标姿态 B 的欧拉角为 (80°, -30°, 120°)。若直接在欧拉角空间线性插值，中间姿态会出现明显的抖动，且当俯仰角经过 90° 附近时产生奇异。

实际处理流程如下：

1. 将姿态 A 和 B 的欧拉角分别转换为旋转矩阵 R_A 和 R_B；
2. 计算相对旋转 ΔR = R_A^T·R_B；
3. 将 ΔR 转换为旋转向量 θ·n，得到旋转轴 n = (0.577, 0.577, 0.577) 和旋转角 θ = 85.4°；
4. 在插补周期内对 θ 进行线性插值，每个插补点计算 q(t) = (cos(θ(t)/2), n·sin(θ(t)/2))；
5. 将四元数 q(t) 转换为旋转矩阵，再结合位置插补得到末端位姿。

通过这种"欧拉角→旋转矩阵→旋转向量→四元数→旋转矩阵"的转换链路，避免了欧拉角插值的非线性和奇异问题。实测结果表明，插补姿态的角速度波动从直接欧拉角插值的 12°/s 降低至 1.5°/s，焊接轨迹的平滑度显著提升。

3.6 常见误区与数值稳定性复盘

在姿态转换的工程实践中，以下问题最为常见：

• 欧拉角顺序混淆：不同厂家机器人（如ABB、KUKA、FANUC）使用不同的欧拉角约定，直接套用转换公式会导致姿态错误。必须确认是ZYX、ZYZ还是XYZ顺序，以及是内旋还是外旋。
• 四元数符号歧义：q 和 -q 表示相同的旋转，但在插值时若未处理符号一致性，会导致旋转路径绕远路。应在插值前检查 q_A·q_B 的符号，若为负则取反。
• 旋转矩阵漂移：长时间运行后，矩阵连乘产生的数值误差会破坏正交性。应定期对旋转矩阵进行正交化修正（如SVD分解后强制奇异值为1）。
• 旋转向量在零角度的退化：当 θ 接近 0 时，旋转轴方向无定义，直接计算会产生数值噪声。工程上通常设置阈值（如 θ < 1e-6），此时直接返回单位四元数或单位矩阵。

为验证转换代码的正确性，建议建立闭环测试：随机生成四元数 → 转旋转矩阵 → 转欧拉角 → 转回四元数，比较前后误差。对于六轴机械臂，还应在典型姿态（如 home 位、奇异位形附近）进行专项测试，确保转换算法在全工作空间内数值稳定。