传感器校准（二）：陀螺仪校准——零偏与温漂处理。磁力计校准——椭圆拟合与干扰排除
上一章我们把加速度计搞定了，这章接着啃剩下的两块硬骨头——陀螺仪和磁力计。说实话，这两兄弟的脾气比加速度计大得多。陀螺仪怕温度，磁力计怕干扰，你要是没摸透它们的性子，飞控在天上分分钟给你脸色看。
我个人习惯把传感器校准分成两类：一类是出厂后做一次就行的，另一类是每次上电都得做的。陀螺仪的零偏属于后者，温漂属于前者。磁力计更麻烦，环境一变就得重来。嗯，咱们一个一个说。
核心观点：陀螺仪校准的核心是消除零偏和温漂；磁力计校准的核心是把受干扰的测量值“拉回”到标准球面上。两者都是飞控数据质量的基石。
陀螺仪校准：零偏的来龙去脉
陀螺仪静止时，理想输出应该是0。但现实中没有完美的传感器，哪怕你把它放在桌上纹丝不动，它也会输出一个非零值。这个值就是零偏（Bias）。
零偏的来源很多：制造公差、温度变化、甚至焊接应力。我遇到过一块飞控板，刚焊好时陀螺仪零偏只有0.5°/s，放了一晚上变成2°/s。查了半天，原来是板子冷却过程中应力释放导致的。
零偏校准的原理很简单：采集静止状态下的N组数据，取平均，然后从后续测量值中减去这个平均值。
// 陀螺仪零偏校准伪代码
#define SAMPLE_COUNT 200
void calibrate_gyro_bias(void) {
float sum_x = 0, sum_y = 0, sum_z = 0;
// 确保无人机静止
for (int i = 0; i < SAMPLE_COUNT; i++) {
sum_x += gyro_raw_x;
sum_y += gyro_raw_y;
sum_z += gyro_raw_z;
delay(5); // 5ms采样间隔
}
gyro_bias_x = sum_x / SAMPLE_COUNT;
gyro_bias_y = sum_y / SAMPLE_COUNT;
gyro_bias_z = sum_z / SAMPLE_COUNT;
// 保存到参数
param_set_float("GYRO_BIAS_X", gyro_bias_x);
param_set_float("GYRO_BIAS_Y", gyro_bias_y);
param_set_float("GYRO_BIAS_Z", gyro_bias_z);
}
这里有个坑：采样时间不能太短。我见过有人只采50组数据，结果零偏误差大得离谱。为什么？因为陀螺仪噪声是随机的，样本量不够，平均值就不稳定。我个人建议至少采200组，时间跨度1秒以上。
经验之谈：零偏校准必须在无人机完全静止的状态下进行。哪怕有一点点振动，结果都会偏。我习惯把无人机放在泡沫垫上，再盖上防尘罩，最大限度隔绝气流扰动。
温漂处理：温度补偿才是真功夫
零偏校准做完就万事大吉了吗？太天真了。陀螺仪的零偏会随着温度变化而漂移，这就是温漂。你想想看，无人机在地面25°C起飞，飞到300米高空可能只有10°C，零偏早就不是当初那个值了。
温漂的处理方式有两种：
- 硬件补偿：使用带温度传感器的陀螺仪，芯片内部自动补偿。比如ICM-20689就内置了温度补偿。
- 软件补偿：建立温度-零偏模型，运行时根据当前温度查表或插值。
大多数开源飞控用的是软件补偿。做法是：在温箱里采集不同温度下的零偏数据，拟合出一条曲线。实际项目中，我见过有人用简单的线性拟合，也有人用三阶多项式。说实话，线性拟合在±20°C范围内够用了，再宽就得用高阶。
| 温度范围 | 零偏变化量（°/s） | 推荐补偿方式 |
|---|---|---|
| -10°C ~ 10°C | ±0.3 | 线性插值 |
| 10°C ~ 40°C | ±0.8 | 二阶多项式 |
| 40°C ~ 70°C | ±2.5 | 三阶多项式或查表 |
警告：千万不要忽略温漂！我曾经在一个项目中，无人机在高温环境下飞行，陀螺仪零偏漂了3°/s，导致姿态估计严重偏差，最后炸机了。从那以后，我每次做飞控都会把温漂补偿加上。
磁力计校准：椭圆拟合的原理
磁力计比陀螺仪更娇气。它测量的是地球磁场，但周围任何铁磁性物质都会干扰它。理想情况下，磁力计在三维空间中旋转时，测量值应该落在一个球面上。但现实中，由于硬铁干扰和软铁干扰，这个球会被拉成椭球。
硬铁干扰：比如电机、电池、螺丝刀等永久磁铁或带磁物体，它们会给测量值加一个固定的偏移量。效果是把球心从原点移开。
软铁干扰：比如铁质机架、碳纤维等导磁材料，它们会改变磁场的方向和强度。效果是把球拉成椭球。
校准的目标就是把这个椭球拟合出来，然后通过数学变换把它变回标准球。说白了，就是找到椭球的中心和半径，然后做归一化。
// 磁力计椭圆拟合核心步骤
// 1. 采集数据：让无人机绕各个轴旋转，采集磁力计数据
// 2. 拟合椭球：使用最小二乘法拟合椭球参数
// 3. 计算变换矩阵：将椭球映射到单位球
// 椭球方程：Ax² + By² + Cz² + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz = 1
// 拟合得到参数 A~I，然后计算中心偏移和缩放矩阵
void mag_calibration_ellipsoid_fit(float *data, int n) {
// 构建矩阵并求解
// 这里省略具体数学推导，实际工程中常用现成库
// 比如 PX4 的 mag_calib.cpp 中的 ellipsoid_fit() 函数
}
关键点：椭圆拟合需要足够多的数据点，而且数据要覆盖三维空间的各个方向。我建议至少采集500个点，并且确保无人机在旋转过程中覆盖了所有姿态。
干扰排除：实战中的那些坑
椭圆拟合的理论很完美，但实战中干扰无处不在。我总结了几种常见的干扰场景和应对方法：
- 电机干扰：电机通电后会产生强磁场。校准磁力计时，一定要在电机断电状态下进行。如果必须在电机转动时使用磁力计，那就得做动态补偿。
- 大电流干扰：电池放电电流大时，导线周围会产生磁场。我遇到过一架无人机，油门一推磁力计数据就跳。后来发现是电源线离磁力计太近了，重新布线才解决。
- 地磁异常：某些地方有铁矿或建筑物钢筋，会导致局部地磁异常。这种情况下，磁力计校准会失败。我的建议是换个地方再试。
避坑指南：我曾经在室内校准磁力计，结果怎么都拟合不好。折腾了半天才发现，地板下面有钢筋网。后来我都是去室外空旷的地方校准，远离建筑物和金属物体。
知识体系总览
下面这张图把本章的核心逻辑串起来了。你可以看到，陀螺仪和磁力计的校准虽然方法不同，但目标一致——消除系统误差，提高数据质量。
校准流程实操
说了这么多理论，咱们来点实际的。下面是我在项目中常用的校准流程：
- 上电预热：飞控上电后等待30秒，让陀螺仪温度稳定。我见过有人一上电就开始校准，结果温漂导致零偏误差很大。
- 陀螺仪零偏校准：确保无人机静止，采集200组数据取平均。
- 陀螺仪温漂校准：如果条件允许，在温箱中采集不同温度下的零偏数据。没有温箱的话，可以用飞行日志反推温漂曲线。
- 磁力计校准：到室外空旷处，让无人机绕各个轴缓慢旋转，采集至少500个点。
- 验证校准结果：校准完成后，检查磁力计模长是否接近1（归一化后），陀螺仪静止时输出是否接近0。
注意：磁力计校准后，如果更换了机架、电机或电池位置，必须重新校准。因为这些部件的磁场分布已经变了。
好了，这一章的内容就到这里。陀螺仪和磁力计的校准看似繁琐，但只要你掌握了核心原理，实际操作起来并不复杂。记住一句话：校准做得越细致，飞控在天上就越听话。