2. 欧拉角与四元数：姿态表示的两种语言

做机器人控制，绕不开姿态表示。我见过不少新手，上来就用欧拉角，结果在某个奇异点附近系统直接飞了。嗯，今天我们就来把欧拉角和四元数彻底讲透。

2.1 欧拉角定义与万向锁

欧拉角说白了，就是用三个角度来描述刚体在空间中的朝向。常见的定义方式有 ZYX、ZYZ、XYZ 等。我个人最常用的是 ZYX 顺序，也就是先绕 Z 轴转（偏航 yaw），再绕 Y 轴转（俯仰 pitch），最后绕 X 轴转（滚转 roll）。

你想想看，为什么会有万向锁？我记得第一次在六轴机器人上遇到这个问题时，调试了整整两天。万向锁发生在中间轴旋转到 ±90° 时，导致第一个轴和第三个轴的旋转平面重合，丢失了一个自由度。

旋转顺序	万向锁条件	丢失自由度
ZYX	β = ±90°	偏航与滚转耦合
ZYZ	β = 0° 或 180°	两个 Z 轴旋转合并
XYZ	β = ±90°	X 与 Z 旋转耦合

2.2 四元数运算

四元数是什么？说白了就是一个超复数。它由四个分量组成：一个实部和三个虚部。写成 q = w + xi + yj + zk，或者更常见的 (w, x, y, z)。

我为什么偏爱四元数？因为它没有万向锁，而且插值平滑。在实时控制中，这太重要了。

2.2.1 四元数基本运算

• 模长：||q|| = sqrt(w² + x² + y² + z²)，单位四元数模长为 1
• 共轭：q* = (w, -x, -y, -z)，用于求逆
• 乘法：q₁ ⊗ q₂，不满足交换律，顺序很重要
• 逆：q⁻¹ = q* / ||q||²，单位四元数就是共轭

// 四元数乘法示例（C++风格伪代码）
Quaternion multiply(Quaternion q1, Quaternion q2) {
    Quaternion result;
    result.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z;
    result.x = q1.w*q2.x + q1.x*q2.w + q1.y*q2.z - q1.z*q2.y;
    result.y = q1.w*q2.y - q1.x*q2.z + q1.y*q2.w + q1.z*q2.x;
    result.z = q1.w*q2.z + q1.x*q2.y - q1.y*q2.x + q1.z*q2.w;
    return result;
}

2.2.2 四元数插值

做轨迹规划时，经常需要在两个姿态之间平滑过渡。欧拉角线性插值会出问题，但四元数有 SLERP（球面线性插值）。

SLERP 的公式其实不复杂：

slerp(q1, q2, t) = (sin((1-t)θ) * q1 + sin(tθ) * q2) / sin(θ)

其中 θ 是两个四元数之间的夹角。如果 θ 接近 0，直接用线性插值就行，避免除零。

2.3 四元数与旋转矩阵互转

在实际工程中，我们经常需要在四元数和旋转矩阵之间来回切换。为什么？因为四元数适合做插值和滤波，而旋转矩阵适合做坐标变换和雅可比计算。

2.3.1 四元数 → 旋转矩阵

给定单位四元数 q = (w, x, y, z)，对应的旋转矩阵 R 为：

位置	表达式
R₁₁	1 - 2(y² + z²)
R₁₂	2(xy - wz)
R₁₃	2(xz + wy)
R₂₁	2(xy + wz)
R₂₂	1 - 2(x² + z²)
R₂₃	2(yz - wx)
R₃₁	2(xz - wy)
R₃₂	2(yz + wx)
R₃₃	1 - 2(x² + y²)

2.3.2 旋转矩阵 → 四元数

反过来，从旋转矩阵提取四元数，我建议用稳定数值方法：

// 从旋转矩阵提取四元数
Quaternion fromRotationMatrix(Matrix3x3 R) {
    Quaternion q;
    float trace = R[0][0] + R[1][1] + R[2][2];
    
    if (trace > 0) {
        float s = 0.5f / sqrt(trace + 1.0f);
        q.w = 0.25f / s;
        q.x = (R[2][1] - R[1][2]) * s;
        q.y = (R[0][2] - R[2][0]) * s;
        q.z = (R[1][0] - R[0][1]) * s;
    } else {
        // 处理 trace <= 0 的情况，选择最大对角元
        // 篇幅原因，这里省略分支代码
    }
    return q;
}

2.4 知识体系总览

下面这张图，是我自己整理的四元数与欧拉角的关系图。每次做新项目，我都会先确认：这个场景适合用哪种表示？

2.5 工程选型建议

说了这么多，到底什么时候用哪个？我个人的经验是：

• 人机交互界面：用欧拉角，操作员看得懂
• 内部姿态滤波：用四元数，IMU 数据融合首选
• 运动学计算：用旋转矩阵，雅可比矩阵推导方便
• 轨迹插值：用四元数 SLERP，保证路径最短