第3章：传感器校准与数据融合

各位同学，欢迎来到第三讲。前两章我们聊了飞控的硬件选型和坐标系，今天要啃的这块骨头，是垂直起降阶段最核心的底层技术——传感器校准与数据融合。

说实话，我见过太多新手飞手，一上来就调PID参数，结果飞机在地上疯狂抽搐。为什么？因为传感器数据都是错的。你想想看，用一把不准的尺子去量东西，后面算得再精细也是白搭。所以，这一章我们先把“尺子”校准好。

3.1 加速度计校准

加速度计测量的是比力，说白了就是重力加上运动加速度。在垂直起降阶段，飞机悬停时，加速度计主要感受重力。

校准目标：消除零偏和刻度因子误差。

我的做法：

1. 把飞控水平静置，记录三轴输出。
2. 理论上，水平时X、Y轴应为0g，Z轴应为1g。
3. 翻转六个面（±X, ±Y, ±Z），分别采集数据。
4. 用最小二乘法拟合出零偏和尺度因子。

a_calibrated = (a_raw - bias) * scale_factor

我在项目中遇到过一个问题：某款加速度计温度漂移特别大，冬天和夏天校准结果差很多。后来我加了个温度补偿表，才搞定。嗯，这里要注意，如果你用的是工业级传感器，最好做一下温补。

3.2 陀螺仪校准

陀螺仪测量角速度。它的主要误差是零偏，也就是静止时输出不为0。

校准方法：

• 静止采集1000个样本，取平均值作为零偏。
• 对于高精度应用，还需要考虑温度影响。

我个人习惯在飞控上电后，先让飞机静置5秒，自动完成陀螺仪零偏校准。这样每次起飞前都能保证数据干净。

3.3 磁力计校准

磁力计用来测航向。但它特别娇气，容易受周围铁磁物质干扰。

校准原理：

磁力计的理想输出是一个球体。但实际因为硬铁和软铁干扰，会变成一个椭球。校准就是把这个椭球拉回球体。

操作步骤：

1. 拿着飞机在空中画“8”字，让磁力计采集各个方向的数据。
2. 用椭球拟合算法，求出偏移和缩放矩阵。
3. 把校准参数写入飞控参数。

3.4 数据融合入门

单个传感器都有缺点：加速度计高频噪声大，陀螺仪低频漂移，磁力计容易受干扰。所以我们需要融合它们，取长补短。

下面这张图展示了传感器融合的基本思路：

3.5 互补滤波

互补滤波是最简单、最实用的融合方法。它的核心思想是：

• 陀螺仪负责高频姿态更新（短时准确）
• 加速度计和磁力计负责低频修正（长时稳定）

代码实现：

// 互补滤波核心代码
float complementary_filter(float gyro_rate, float acc_angle, float dt) {
    float alpha = 0.98;  // 权重系数
    float angle = alpha * (angle + gyro_rate * dt) + (1 - alpha) * acc_angle;
    return angle;
}

alpha值怎么选？我一般取0.98到0.995之间。alpha越大，越信任陀螺仪；越小，越信任加速度计。你想想看，如果飞机震动大，alpha就设大一点，不然加速度计的噪声会污染姿态。

3.6 卡尔曼滤波入门

卡尔曼滤波听起来高大上，其实核心就五条公式。我尽量用大白话讲清楚。

核心思想：用模型预测 + 测量修正，不断迭代。

五条公式：

1. 预测状态：根据上一时刻的状态，预测当前状态
2. 预测协方差：估计预测的不确定性
3. 计算卡尔曼增益：决定相信预测还是测量
4. 更新状态：用测量值修正预测值
5. 更新协方差：减小不确定性

// 一维卡尔曼滤波
float kalman_filter(float measurement, float dt) {
    // 预测
    float x_pred = x_est;
    float p_pred = p_est + q;
    
    // 更新
    float k = p_pred / (p_pred + r);
    x_est = x_pred + k * (measurement - x_pred);
    p_est = (1 - k) * p_pred;
    
    return x_est;
}

这里q是过程噪声，r是测量噪声。q越大，说明模型越不准，更相信测量；r越大，说明测量噪声大，更相信预测。

3.7 两种滤波对比

我个人建议：初学者先用互补滤波，把姿态控制搞明白。等遇到性能瓶颈了，再上卡尔曼滤波。别一上来就搞复杂算法，容易把自己绕晕。

好了，这一章的内容就到这里。传感器校准是飞控的基石，数据融合是姿态估计的灵魂。把这些搞扎实了，后面调PID才能事半功倍。