姿态解算基础：从欧拉角到EKF
姿态解算，说白了就是回答一个问题：飞行器现在朝哪边？
我刚开始做飞控那会儿，觉得这问题挺简单——不就是测个角度嘛。后来真上了硬件才发现，传感器数据全是噪声，加速度计抖得像筛子，陀螺仪漂得没边。嗯，这里面的门道，比想象中深得多。
这一章，咱们把姿态解算的底裤扒干净。从数学工具到工程实现，一条线捋下来。
3.1 坐标系定义——先定规矩
没有坐标系，谈姿态就是耍流氓。飞控里最常用的两套坐标系，你得刻在脑子里。
3.1.1 机体坐标系（Body Frame）
原点在飞行器重心，跟着飞机转。我习惯这么记：
- X轴：指向机头（前进方向）
- Y轴：指向右翼
- Z轴：指向机腹（向下）
注意，Z轴向下是航空界的惯例。你想想看，如果Z轴向上，那滚转正方向就得反过来，容易搞混。
3.1.2 地理坐标系（Navigation Frame / NED）
也叫导航坐标系，原点在地面某点，不跟着飞机转。
- X轴：指向北
- Y轴：指向东
- Z轴：指向地心（向下）
这就是常说的NED坐标系（North-East-Down）。
核心概念：姿态解算的本质，就是求机体坐标系到地理坐标系的旋转关系。这个关系，可以用欧拉角、旋转矩阵、四元数三种方式表达。
3.2 三种姿态表达方式
同一个旋转，三种数学工具。各有各的脾气。
3.2.1 欧拉角——最直观，但最危险
欧拉角就是三个角度：横滚（Roll φ）、俯仰（Pitch θ）、偏航（Yaw ψ）。
旋转顺序我建议固定为 Z-Y-X（偏航→俯仰→横滚），这是航空界标准。顺序错了，姿态就全乱了。
⚠ 万向锁（Gimbal Lock）：当俯仰角接近 ±90° 时，横滚和偏航的旋转轴重合，丢失一个自由度。我在做固定翼飞控时遇到过这个问题——飞机垂直爬升时姿态解算直接崩了。所以，全姿态飞行（比如四旋翼特技）千万别只用欧拉角。
3.2.2 旋转矩阵——数学上完美，计算上沉重
3×3 的正交矩阵，没有奇点。但每次更新要算 9 个元素，嵌入式 MCU 扛不住。
从欧拉角到旋转矩阵的转换公式（Z-Y-X 顺序）：
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.2.3 四元数——工程首选
四元数用四个数表达旋转：q = [q0, q1, q2, q3]，其中 q0 是标量部分。
为什么选它？
- 无奇点（万向锁不存在）
- 计算量小（只更新4个参数）
- 插值平滑（做姿态平滑时特别好用）
我几乎所有的飞控项目，姿态解算核心都用四元数。只在最后输出给上层控制时，才转成欧拉角。
四元数转欧拉角（Z-Y-X 顺序）：
φ = atan2(2*(q0*q1 + q2*q3), 1 - 2*(q1² + q2²))
θ = asin(2*(q0*q2 - q3*q1))
ψ = atan2(2*(q0*q3 + q1*q2), 1 - 2*(q2² + q3²))💡 我的习惯：在代码里用四元数做所有内部运算，只在日志记录和地面站显示时转成欧拉角。这样既避免了万向锁，又方便人看。
3.3 姿态解算算法
传感器各有各的毛病：
- 陀螺仪：动态响应快，但积分会漂移
- 加速度计：长期稳定，但受振动干扰大
- 磁力计：提供偏航参考，但容易受磁场干扰
姿态解算的核心，就是融合这些传感器的优点，抑制它们的缺点。
3.3.1 互补滤波——简单粗暴，够用就好
思路很直白：低频信加速度计，高频信陀螺仪。
公式就一行：
角度 = α * (角度 + 陀螺仪积分) + (1-α) * 加速度计角度其中 α 是滤波系数，一般取 0.98 左右。
优点：代码量少，计算量小，适合资源受限的 MCU。
缺点：精度一般，对振动敏感。我曾经在一个四旋翼上试过，悬停还行，一做大机动就飘。
3.3.2 Mahony 滤波——互补滤波的升级版
Mahony 滤波在互补滤波基础上，加入了PI 反馈修正。它用加速度计和磁力计的测量值，计算出一个误差，然后用 PI 控制器去修正陀螺仪的偏差。
核心步骤：
- 用当前四元数估计重力方向
- 用加速度计测量实际重力方向
- 两者叉积得到误差
- 误差经过 PI 控制器，修正陀螺仪角速度
- 用修正后的角速度更新四元数
// Mahony 滤波核心伪代码
void MahonyUpdate(float gx, float gy, float gz, float ax, float ay, float az) {
// 1. 估计重力方向
float halfvx = q1*q3 - q0*q2;
float halfvy = q0*q1 + q2*q3;
float halfvz = q0*q0 - 0.5f + q3*q3;
// 2. 计算误差（叉积）
float ex = ay * halfvz - az * halfvy;
float ey = az * halfvx - ax * halfvz;
float ez = ax * halfvy - ay * halfvx;
// 3. PI 修正
integralFBx += Ki * ex * dt;
integralFBy += Ki * ey * dt;
integralFBz += Ki * ez * dt;
gx += ex * Kp + integralFBx;
gy += ey * Kp + integralFBy;
gz += ez * Kp + integralFBz;
// 4. 更新四元数
// ... 四元数微分方程
}💡 参数调优经验：Kp 决定收敛速度，Ki 决定稳态精度。我一般先调 Kp 让姿态能跟上，再加一点点 Ki 消除静差。Ki 太大反而会引入低频振荡。
3.3.3 EKF（扩展卡尔曼滤波）——精度天花板
EKF 是姿态解算的终极方案。它用状态空间模型，把陀螺仪偏差也作为状态量一起估计。
状态向量（7维）：
x = [q0, q1, q2, q3, bgx, bgy, bgz]前4个是四元数，后3个是陀螺仪零偏。
观测向量：加速度计（3轴）+ 磁力计（3轴）
EKF 的流程：
- 预测步：用陀螺仪角速度更新状态
- 更新步：用加速度计和磁力计修正状态
我做过对比测试：在同样硬件上，EKF 的姿态精度比 Mahony 高约 30%，但计算量大了 5 倍。所以选型时要权衡——STM32F4 以上跑 EKF 没问题，F103 就老老实实用 Mahony。
⚠ 我曾经踩过的坑：EKF 的协方差矩阵初始化很关键。如果初始协方差设得太大，滤波器收敛慢；设得太小，又跟不上动态。我后来总结了一个经验：先跑一段静态数据，用 Allan 方差分析出传感器噪声特性，再用这些参数初始化协方差矩阵。效果立竿见影。
3.4 三种算法对比
| 算法 | 精度 | 计算量 | 适用场景 | 我推荐的使用条件 |
|---|---|---|---|---|
| 互补滤波 | 低 | 极低 | 玩具、低成本飞控 | MCU 主频 < 72MHz |
| Mahony | 中 | 低 | 大多数消费级飞控 | STM32F1/F3 级别 |
| EKF | 高 | 高 | 工业级、自动驾驶 | STM32F4/H7 或更高 |
3.5 姿态解算知识体系
下面这张图，把这一章的核心逻辑串起来了：
3.6 工程选型建议
说了这么多，到底用哪个？我个人的决策逻辑是这样的：
- 玩具/教育级：互补滤波，10行代码搞定
- 消费级无人机：Mahony 滤波，性价比最高
- 工业级/自动驾驶：EKF，精度就是生命
还有一个容易被忽略的点：传感器质量。再好的算法也救不了烂传感器。我曾经在一个项目里花了两周调 EKF 参数，最后发现是加速度计噪声太大。换了传感器，问题迎刃而解。
嗯，姿态解算这块，数学是基础，工程是艺术。多动手调，多上机试，慢慢就有感觉了。