4、传感器数据融合：加速度计、陀螺仪、磁力计的工作原理、互补滤波与卡尔曼滤波入门、姿态解算（四元数、欧拉角）
各位同学，欢迎来到第四章。这一章，我们聊聊飞控系统的「眼睛」和「小脑」——传感器数据融合。
说实话，我入行那会儿，最头疼的就是这一块。加速度计、陀螺仪、磁力计，三个传感器各有各的脾气。你单独看哪个都挺准，一组合起来就打架。我当年在实验室调一个四轴，陀螺仪积分十分钟就飘了十几度，加速度计一震动就全是噪声。那会儿真想摔遥控器。
后来才明白，这不是传感器坏了，是咱们没学会怎么「哄」它们合作。今天我就把当年踩过的坑，和后来总结的方法，一次性讲清楚。
4.1 三大传感器的「性格」分析
先聊聊这三个传感器的脾气。你只有摸透了它们的性格，才能让它们好好配合。
4.1.1 加速度计
加速度计测量的是比力，说白了就是物体受到的加速度，包括重力。它内部有个质量块，你加速它就变形，通过电容或压阻效应测出电压。
优点：
- 静态时非常准。你把它放桌上不动，它能精确告诉你重力方向。
- 没有长期漂移。放一年，测出来的重力还是9.8m/s²。
缺点：
- 动态响应慢。你猛地一晃，它反应不过来。
- 对振动敏感。电机一转，数据就炸了。
- 无法区分重力加速度和运动加速度。你往前一冲，它分不清是倾斜了还是加速了。
4.1.2 陀螺仪
陀螺仪测量角速度，单位是°/s。它基于科里奥利力原理，内部有个振动的质量块，旋转时会产生一个垂直于振动方向的力。
优点：
- 动态响应极快。你转多快它都能跟上。
- 不受振动影响。电机再抖，角速度数据依然干净。
缺点：
- 有零偏。静止时输出不是0，可能是0.1°/s。
- 有温度漂移。温度一变，零偏也跟着变。
- 积分会累积误差。你想想看，0.1°/s的误差，积分10秒就是1°，一分钟就是6°。飞控能忍？
我建议，陀螺仪的数据只用来做短期姿态跟踪。长期？必须靠加速度计和磁力计来校正。
4.1.3 磁力计
磁力计测量磁场强度，用来确定航向（偏航角）。它就像个电子指南针。
优点：
- 能提供绝对航向参考。不像陀螺仪会漂。
缺点：
- 极易受干扰。电机电流、铁磁物质、甚至地磁场的不均匀性都会影响它。
- 需要校准。我每次换飞控板，第一件事就是做磁力计校准。
4.2 互补滤波：简单粗暴但有效
好，传感器性格摸清了。怎么让它们合作？最简单的方法就是互补滤波。
互补滤波的核心思想：低频信加速度计，高频信陀螺仪。
为什么？加速度计低频准（静态），但高频噪声大。陀螺仪高频准（动态），但低频会漂。两者正好互补。
公式很简单：
角度 = α × (角度 + 陀螺仪角速度 × dt) + (1 - α) × 加速度计角度这里的α是个系数，通常在0.98左右。α越大，越信任陀螺仪；α越小，越信任加速度计。
我当年在STM32F103上跑互补滤波，代码不到50行，效果已经能用了。对于很多入门级飞控，互补滤波完全够用。
4.3 卡尔曼滤波入门
互补滤波虽然简单，但精度有限。如果你想要更好的效果，就得请出卡尔曼滤波。
卡尔曼滤波，说白了就是「用模型预测 + 用测量修正」。它有两个步骤：
- 预测： 根据上一时刻的状态和陀螺仪数据，预测当前时刻的状态。
- 更新： 用加速度计和磁力计的测量值，修正预测结果。
卡尔曼滤波的公式看起来吓人，其实核心就五个：
// 预测
x_pred = A * x_prev + B * u
P_pred = A * P_prev * A^T + Q
// 更新
K = P_pred * H^T * (H * P_pred * H^T + R)^(-1)
x_updated = x_pred + K * (z - H * x_pred)
P_updated = (I - K * H) * P_pred别被这些矩阵吓到。对于一维角度估计，这些矩阵都可以简化成标量。我当年第一次实现卡尔曼滤波时，就是先在一维上跑通的。
4.4 姿态解算：四元数与欧拉角
数据融合完了，我们得到了什么？是角速度和加速度的融合结果。但飞控需要的是姿态——俯仰角、横滚角、偏航角。
这就涉及到姿态表示了。常用的有两种：欧拉角和四元数。
4.4.1 欧拉角
欧拉角就是俯仰角（Pitch）、横滚角（Roll）、偏航角（Yaw）。直观，好理解。
但欧拉角有个致命问题：万向锁。当俯仰角接近±90°时，横滚和偏航会耦合，导致姿态丢失一个自由度。
我建议，在飞控内部计算时，永远不要用欧拉角。只在输出给地面站或用户时，才转换成欧拉角。
4.4.2 四元数
四元数是个四维复数，形式是 q = w + xi + yj + zk，其中 w² + x² + y² + z² = 1。
它没有万向锁问题，计算效率高，适合嵌入式实时计算。
从陀螺仪角速度更新四元数的公式：
// 四元数微分方程
dq/dt = 0.5 * q * ω
// 离散化实现
q_new = q_old + 0.5 * dt * q_old * ω其中ω是角速度的四元数形式 (0, ωx, ωy, ωz)。
四元数转欧拉角的公式：
roll = atan2(2*(w*x + y*z), 1 - 2*(x² + y²))
pitch = asin(2*(w*y - z*x))
yaw = atan2(2*(w*z + x*y), 1 - 2*(y² + z²))4.5 实战建议
最后，给各位一些实战建议：
- 起步用互补滤波： 如果你刚接触飞控，先用互补滤波跑通整个系统。代码简单，调试方便。
- 进阶用卡尔曼滤波： 当你对系统有了感觉，再上卡尔曼滤波。效果确实更好，但调试周期也长。
- 永远用四元数做内部计算： 欧拉角只用于显示和通信。别问我为什么，问就是万向锁。
- 传感器校准不能省： 加速度计要校准零偏和尺度因子，陀螺仪要校准零偏，磁力计要做硬铁和软铁校准。我见过太多人，传感器没校准就急着调滤波，结果调了一周都没调好。
嗯，这一章的内容就到这里。下一章我们会深入飞控的核心——控制律设计。到时候，你会看到姿态解算的结果是如何被PID控制器使用的。
记住，传感器数据融合是飞控的基石。基础打牢了，后面的路就好走了。