3. 基础数学回顾（二）：四元数基础、四元数与旋转矩阵/欧拉角的转换、刚体运动学

好，我们继续。上一章我们把向量、矩阵和欧拉角这些老朋友重新捋了一遍。这一章，咱们来聊聊飞控算法里真正的主角——四元数。

说实话，我刚入行那会儿，看到四元数也是一脸懵。四个数就能表示三维旋转？这玩意儿比欧拉角好在哪？后来在项目里被“万向锁”坑过一次，才彻底服了。嗯，咱们今天就把它彻底搞明白。

3.1 为什么非得用四元数？

你可能会问：欧拉角不是挺直观的吗？偏航、俯仰、横滚，多好理解。

没错，欧拉角直观。但它有个致命的问题——万向锁（Gimbal Lock）。当俯仰角接近 ±90° 时，偏航和横滚的旋转轴会重合，丢失一个自由度。这在飞控里是灾难性的，飞机会突然失控。

我当年调试一个固定翼飞控，在地面站里看着姿态数据突然跳变，飞机直接翻了个跟头。查了两天，最后发现是欧拉角插值在奇异点附近出了问题。从那以后，我所有的核心姿态解算都改用四元数了。

四元数有几个硬核优势：

• 无奇异点：不会出现万向锁
• 计算效率高：乘法比矩阵乘法快，适合嵌入式实时计算
• 插值平滑：可以做球面线性插值（SLERP），姿态过渡非常自然
• 存储紧凑：4个浮点数，比9个元素的旋转矩阵省空间

3.2 四元数基础

四元数本质上是一个超复数。它由一个实部和三个虚部组成：

q = w + xi + yj + zk

其中 i² = j² = k² = ijk = -1。通常我们写成向量形式：

q = [w, x, y, z]ᵀ

或者更常用的：

q = [w, v]  其中 v = [x, y, z] 是向量部分

表示旋转时，我们使用单位四元数，即模长为1：

||q|| = sqrt(w² + x² + y² + z²) = 1

3.2.1 四元数的基本运算

这些是飞控里天天用的，我建议你背下来：

运算	公式	说明
加法	q₁ + q₂ = [w₁+w₂, v₁+v₂]	逐元素相加
乘法	q₁q₂ = [w₁w₂ - v₁·v₂, w₁v₂ + w₂v₁ + v₁×v₂]	不满足交换律！顺序很重要
共轭	q* = [w, -v]	虚部取反
模长	||q|| = sqrt(w² + x² + y² + z²)	单位四元数模长为1
逆	q⁻¹ = q* / ||q||²	单位四元数的逆等于共轭

3.2.2 用四元数旋转向量

这是飞控里最常用的操作。要把一个三维向量 p 用四元数 q 旋转，步骤是：

1. 把向量 p 写成纯四元数：p_quat = [0, p]
2. 执行旋转：p' = q * p_quat * q⁻¹
3. 取 p' 的虚部就是旋转后的向量

写成代码就是：

// 用四元数旋转向量 v
Vector3f rotate_by_quat(Quaternion q, Vector3f v) {
    // 构造纯四元数
    Quaternion p(0, v.x, v.y, v.z);
    // 旋转：q * p * q_conj
    Quaternion result = q * p * q.conjugate();
    // 取虚部
    return Vector3f(result.x, result.y, result.z);
}

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

在实际工程中，我们经常需要在四元数和旋转矩阵之间来回切换。比如，从IMU读出来的是四元数，但做控制律计算时可能需要旋转矩阵。

3.3.1 四元数 → 旋转矩阵

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

R = | 1-2y²-2z²   2xy-2wz     2xz+2wy   |
    | 2xy+2wz     1-2x²-2z²   2yz-2wx   |
    | 2xz-2wy     2yz+2wx     1-2x²-2y² |

这个公式我建议你直接写成函数库，不要每次都手算。我在项目里吃过亏——手算写错一个符号，整个姿态解算全乱套。

3.3.2 旋转矩阵 → 四元数

反过来，从旋转矩阵 R 提取四元数：

w = 0.5 * sqrt(1 + R₁₁ + R₂₂ + R₃₃)
x = (R₃₂ - R₂₃) / (4w)
y = (R₁₃ - R₃₁) / (4w)
z = (R₂₁ - R₁₂) / (4w)

3.4 四元数与欧拉角的转换

这部分是飞控调试的命门。你在地面站上看到的是欧拉角，但代码里跑的是四元数。

3.4.1 四元数 → 欧拉角（ZYX顺序）

假设我们使用 Z-Y-X 顺序（偏航-俯仰-横滚）：

roll  = atan2(2(wx + yz), 1 - 2(x² + y²))
pitch = asin(2(wy - zx))
yaw   = atan2(2(wz + xy), 1 - 2(y² + z²))

这里要注意 asin 的输入范围。当俯仰角接近 ±90° 时，虽然四元数本身没问题，但反算出来的欧拉角会有跳变。我在做无人机编队时遇到过这个问题——两架飞机的欧拉角突然差 180°，但实际上姿态是连续的。

3.4.2 欧拉角 → 四元数

反过来，给定欧拉角 (roll φ, pitch θ, yaw ψ)：

w = cos(φ/2)cos(θ/2)cos(ψ/2) + sin(φ/2)sin(θ/2)sin(ψ/2)
x = sin(φ/2)cos(θ/2)cos(ψ/2) - cos(φ/2)sin(θ/2)sin(ψ/2)
y = cos(φ/2)sin(θ/2)cos(ψ/2) + sin(φ/2)cos(θ/2)sin(ψ/2)
z = cos(φ/2)cos(θ/2)sin(ψ/2) - sin(φ/2)sin(θ/2)cos(ψ/2)

这个公式看着长，但其实就是三个半角旋转四元数的乘积。我习惯先分别算三个轴的四元数，再乘起来，代码可读性更好。

3.5 刚体运动学

好了，有了四元数这个工具，我们终于可以描述刚体怎么动了。

3.5.1 姿态运动学方程

刚体的姿态变化率（角速度）和四元数导数之间的关系是：

dq/dt = 0.5 * q * ω_quat

其中 ω_quat = [0, ωx, ωy, ωz] 是角速度的纯四元数形式。展开成标量形式：

dw/dt = -0.5 * (x*ωx + y*ωy + z*ωz)
dx/dt =  0.5 * (w*ωx + y*ωz - z*ωy)
dy/dt =  0.5 * (w*ωy + z*ωx - x*ωz)
dz/dt =  0.5 * (w*ωz + x*ωy - y*ωx)

这就是飞控里姿态更新的核心方程。每次IMU读出角速度，我们就用这个方程更新四元数。

3.5.2 离散化实现

实际代码里，我们用的是离散版本。假设采样周期为 dt：

// 一阶龙格-库塔（欧拉法）
Quaternion q_dot = 0.5 * q * Quaternion(0, gyro.x, gyro.y, gyro.z);
q += q_dot * dt;
q.normalize();  // 别忘了归一化！

更精确的做法是用二阶龙格-库塔或者解析解。对于大多数飞控应用，一阶方法配合高更新率（>200Hz）已经足够。

3.6 知识体系总览

我把这一章的核心逻辑画了张图，方便你理解各个概念之间的关系：

3.7 工程实践建议

最后，给你几个我在实际项目中总结的经验：

• 初始化：四元数一定要初始化为单位四元数 [1,0,0,0]，不要用零向量
• 归一化频率：每更新一次就归一化一次，不要偷懒
• 数值精度：嵌入式平台用 float 就够了，double 反而可能因为内存对齐问题变慢
• 调试技巧：把四元数转成欧拉角打印出来看，但分析问题时用四元数本身