3、姿态表示（四元数）：四元数定义、乘法、共轭、旋转操作，为什么飞控偏爱四元数？
聊到飞控的姿态表示，四元数是个绕不开的话题。我记得刚入行那会儿，看到四元数公式就头疼——四个数怎么就能表示旋转了？后来在项目里吃过欧拉角万向锁的亏，才真正体会到四元数的好。今天咱们就把这东西掰开揉碎了讲清楚。
3.1 四元数的定义
说白了，四元数就是一个超复数。普通复数有一个实部一个虚部，四元数有一个实部和三个虚部：
q = w + xi + yj + zk其中 i、j、k 满足：
i² = j² = k² = ijk = -1
ij = k, jk = i, ki = j
ji = -k, kj = -i, ik = -j嗯，这里要注意，乘法不满足交换律。我在项目中第一次写四元数乘法时，就因为这个吃了亏——顺序搞反了，结果姿态解算直接炸了。
通常我们把四元数写成向量形式：
q = [w, x, y, z]ᵀ 或 q = w + vw 是标量部分，v = (x, y, z) 是矢量部分。
3.2 四元数的基本运算
3.2.1 四元数乘法
两个四元数 q₁ = w₁ + v₁ 和 q₂ = w₂ + v₂ 相乘：
q₁ ⊗ q₂ = (w₁w₂ - v₁·v₂) + (w₁v₂ + w₂v₁ + v₁×v₂)写成矩阵形式更直观：
q₁ ⊗ q₂ =
[ w₁ -x₁ -y₁ -z₁ ] [ w₂ ]
[ x₁ w₁ -z₁ y₁ ] [ x₂ ]
[ y₁ z₁ w₁ -x₁ ] [ y₂ ]
[ z₁ -y₁ x₁ w₁ ] [ z₂ ]我的经验：实际写代码时，我习惯用这个矩阵形式。它比直接算叉积点积要快，而且不容易出错。你想想看，在飞控里每秒要算几千次姿态更新，这点效率差异就放大了。
3.2.2 共轭与模
四元数的共轭很简单，就是把矢量部分取反：
q* = w - xi - yj - zk模长：
||q|| = √(w² + x² + y² + z²)单位四元数的模为1，这是飞控里最常用的。为什么？因为单位四元数才能表示纯旋转，不引入缩放。
3.2.3 逆四元数
对于单位四元数，逆就等于共轭：
q⁻¹ = q*这个性质太棒了。我在做姿态插值时，经常用这个来算相对旋转。
3.3 用四元数表示旋转
这是核心。一个三维向量 p = (x, y, z)，用四元数表示为 p_q = (0, x, y, z)。绕单位轴 u 旋转 θ 角度：
q = cos(θ/2) + u·sin(θ/2)旋转后的向量：
p' = q ⊗ p_q ⊗ q*注意顺序：先乘 q，再乘 q*，不能搞反。
避坑指南：我曾经在代码里把 q 和 q* 的顺序写反了，结果旋转方向完全反了。调试了整整一个下午才发现。所以建议你写个单元测试，旋转一个已知向量验证一下。
3.4 为什么飞控偏爱四元数？
这个问题面试常考。我总结了几个关键点：
| 特性 | 四元数 | 欧拉角 | 旋转矩阵 |
|---|---|---|---|
| 万向锁 | 无 | 有（俯仰±90°时） | 无 |
| 参数数量 | 4个 | 3个 | 9个 |
| 计算效率 | 高 | 中 | 低 |
| 插值平滑 | 容易（球面插值） | 困难 | 困难 |
| 奇异性 | 无 | 有 | 无 |
说白了，飞控选四元数就三个原因：
- 没有万向锁——飞机做大机动时不会突然丢失自由度
- 计算量小——嵌入式MCU资源有限，少算一点是一点
- 容易融合——IMU数据融合时，四元数更新公式简洁优雅
你想想看，飞机在做筋斗动作时，俯仰角会经过90°，如果用欧拉角，这时候就炸了。但四元数完全没问题。
3.5 代码实现
下面是我在飞控里常用的四元数操作代码：
// 四元数结构体
typedef struct {
float w;
float x;
float y;
float z;
} Quaternion;
// 四元数乘法
Quaternion quat_mult(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;
}
// 用四元数旋转向量
Vector3 quat_rotate(Quaternion q, Vector3 v) {
Quaternion p = {0, v.x, v.y, v.z};
Quaternion q_inv = {q.w, -q.x, -q.y, -q.z};
Quaternion temp = quat_mult(q, p);
Quaternion result = quat_mult(temp, q_inv);
return (Vector3){result.x, result.y, result.z};
}
// 从角速度更新四元数（一阶龙格库塔）
Quaternion quat_update(Quaternion q, Vector3 gyro, float dt) {
float wx = gyro.x * dt / 2.0f;
float wy = gyro.y * dt / 2.0f;
float wz = gyro.z * dt / 2.0f;
Quaternion dq = {1, wx, wy, wz};
Quaternion q_new = quat_mult(q, dq);
// 归一化，防止累积误差
float norm = sqrt(q_new.w*q_new.w + q_new.x*q_new.x +
q_new.y*q_new.y + q_new.z*q_new.z);
q_new.w /= norm;
q_new.x /= norm;
q_new.y /= norm;
q_new.z /= norm;
return q_new;
}注意：每次更新后一定要归一化！我见过有人忘了这步，结果姿态越飘越远。浮点误差会累积，归一化就是定期校准。
3.6 知识体系总览
下面这张图把四元数的核心逻辑串起来了：
从这张图能看出来，四元数从定义出发，通过基本运算支撑起姿态解算和控制输出，最终成为飞控系统的核心。我个人觉得，理解四元数的关键不在于背公式，而在于理解它为什么能避免欧拉角的那些坑。
一个小建议：刚开始接触四元数时，别急着看复杂的推导。先写个简单的旋转demo，转几个向量看看效果。等有了直观感受，再回头看公式，会发现豁然开朗。
公众号:蓝海资料掘金营，微信deep3321