4、姿态解算：基于四元数的姿态更新算法，从传感器数据到姿态角的计算流程
各位同学，欢迎来到第四章。
这一章，我们聊聊姿态解算。说白了，就是回答一个问题：飞控板上的陀螺仪、加速度计、磁力计，这些传感器读出来的原始数据，怎么变成我们需要的横滚角、俯仰角、偏航角？
我个人习惯把姿态解算比作「搭积木」。传感器数据是零散的积木块，而四元数算法就是那个能把积木拼成完整模型的图纸。今天，我们就来拆解这张图纸。
4.1 为什么不用欧拉角？
很多初学者会问：欧拉角不是更直观吗？横滚、俯仰、偏航，多好理解。
嗯，这里要注意。欧拉角有个致命问题——万向锁。当俯仰角接近±90°时，横滚和偏航会失去一个自由度。你想想看，飞机在做筋斗动作时，如果姿态解算突然卡住，那后果……
我在项目中遇到过类似情况。早期用欧拉角做姿态解算，调试时一切正常，结果一上天，飞机做大机动动作，姿态直接崩了。后来换成四元数，问题迎刃而解。
四元数没有奇点，计算效率也高。所以，现代飞控几乎清一色用四元数。
4.2 四元数是什么？
四元数，你可以理解为一个「超复数」。它有一个实部和三个虚部：
q = w + x*i + y*j + z*k其中，w是实部，x、y、z是虚部。i、j、k满足：
i² = j² = k² = i*j*k = -1是不是看着有点眼熟？没错，和复数很像。只不过复数用一个虚部表示旋转，四元数用三个虚部表示三维空间旋转。
核心理解：四元数描述的是「绕某个轴旋转多少角度」。这个轴是三维空间中的任意方向向量，角度是绕该轴旋转的大小。
举个例子。绕单位向量 u = (ux, uy, uz) 旋转 θ 角度，对应的四元数是：
q = cos(θ/2) + (ux*sin(θ/2))*i + (uy*sin(θ/2))*j + (uz*sin(θ/2))*k这个公式，就是整个姿态解算的基石。
4.3 姿态更新：从陀螺仪数据到四元数
陀螺仪输出的是角速度，单位是 rad/s。我们要做的，就是把这些角速度积分，得到姿态变化。
具体流程是这样的：
- 读取陀螺仪数据：得到三轴角速度 ωx、ωy、ωz。
- 构建角速度四元数：
ω_q = 0 + ωx*i + ωy*j + ωz*k- 四元数微分方程：
dq/dt = 0.5 * q ⊗ ω_q这里的 ⊗ 表示四元数乘法。别怕，代码实现时就是几个乘法和加法。
- 离散化更新：
q_new = q_old + dq/dt * Δt或者用更精确的龙格-库塔法。我个人习惯用一阶龙格-库塔，简单够用。
避坑指南：我曾经在更新四元数后忘记归一化，结果姿态越飘越远。记住，每次更新完四元数，一定要做归一化：q = q / ||q||。否则，四元数的模长会慢慢偏离1，导致姿态失真。
4.4 从四元数到欧拉角
虽然我们用四元数做计算，但最终输出给控制算法时，还是需要欧拉角。毕竟PID控制器不认识四元数。
转换公式如下：
| 欧拉角 | 计算公式 |
|---|---|
| 横滚角 φ | φ = atan2(2*(w*x + y*z), 1 - 2*(x² + y²)) |
| 俯仰角 θ | θ = asin(2*(w*y - z*x)) |
| 偏航角 ψ | ψ = atan2(2*(w*z + x*y), 1 - 2*(y² + z²)) |
注意，atan2 是四象限反正切函数，能正确处理角度范围。asin 的值域是 [-π/2, π/2]，刚好对应俯仰角的物理范围。
4.5 传感器融合：为什么需要加速度计和磁力计？
你可能会问：只用陀螺仪不行吗？
不行。陀螺仪有零偏漂移。时间一长，积分出来的姿态就会跑偏。我做过测试，纯陀螺仪积分，10秒后姿态误差就能到5°以上。
所以，我们需要用加速度计和磁力计来「修正」陀螺仪的误差。
- 加速度计：测量重力方向。在静止或匀速运动时，它能提供横滚角和俯仰角的参考。
- 磁力计：测量地磁场方向。它能提供偏航角的参考。
融合方法有很多，比如互补滤波、卡尔曼滤波。这里我推荐一种简单实用的方法——Mahony互补滤波。
4.6 Mahony互补滤波算法流程
这个算法是我个人非常喜欢的。它简单、高效、鲁棒性好。我在多个飞控项目里都用它。
核心思想：用加速度计和磁力计的测量值，计算出一个「误差」，然后用PI控制器修正陀螺仪的角速度。
// 伪代码示例
// 1. 从加速度计得到重力方向参考
g_ref = [0, 0, 1]; // 地理坐标系下的重力方向
// 2. 用当前四元数将重力方向转到机体坐标系
g_body = q * g_ref * q_conj;
// 3. 计算误差：加速度计测量值与g_body的叉积
error_acc = acc_meas × g_body;
// 4. 类似地，用磁力计计算偏航误差
// ...（略，原理相同）
// 5. PI控制器修正角速度
omega_corrected = omega_gyro + Kp * error + Ki * ∫error dt;
// 6. 用修正后的角速度更新四元数
// ...（同4.3节步骤）注意：加速度计不能修正偏航角。因为绕重力轴旋转时，重力方向不变。所以偏航角的修正必须依赖磁力计。如果你的环境磁场干扰严重，偏航角会飘。我曾经在室内飞无人机，旁边有根钢筋柱子，偏航角直接偏了30°。
4.7 完整计算流程图
下面这张图，把整个流程串起来了。从传感器数据输入，到姿态角输出，一目了然。
4.8 代码实现要点
最后，我分享几个代码实现时的关键点：
- 四元数乘法：别自己手写，容易出错。用现成的库函数，或者写一个经过充分测试的函数。
- 归一化：每次更新后必须做。我习惯在更新函数末尾加一行
q = q_normalize(q); - PI参数整定：Kp 决定收敛速度，Ki 决定稳态误差。我一般先调 Kp，等姿态能快速跟踪了，再加一点 Ki 消除静差。
- 传感器同步：陀螺仪和加速度计的数据时间戳要对齐。否则融合出来的姿态会有延迟。
个人经验：调试姿态解算时，先把飞控放在桌面上静止，看姿态角是否稳定在0°附近。如果漂移，检查零偏补偿。如果抖动，检查滤波参数。如果偏航角不准，检查磁力计校准。
好了，这一章的内容就到这里。姿态解算是飞控的核心，理解了四元数更新和传感器融合，你就掌握了飞控的「大脑」。
公众号:蓝海资料掘金营，微信deep3321