第4章传感器校准验证

第4章传感器校准验证：加速度计、陀螺仪、磁力计校准流程与验证方法

传感器校准，说白了就是给飞控的「眼睛」和「耳朵」做一次精准的视力听力测试。我做了这么多年飞控验证，见过太多因为传感器没校准好导致的炸机事故。嗯，这章咱们就聊聊怎么把这事做扎实。

核心观点：校准不是走流程，而是建立传感器输出与真实物理量之间的精确映射关系。跳过这一步，后面所有验证都是空中楼阁。

4.1 为什么要校准？—— 我踩过的坑

你想想看，MEMS传感器出厂时都有偏差。加速度计可能有零偏，陀螺仪可能有温漂，磁力计更麻烦，周围铁磁物质会干扰它。我记得有一次，一个客户反馈飞机悬停时总往一边飘。查了两天，最后发现是加速度计X轴偏了0.05g。0.05g啊，看着不大，但积分到速度上，几秒钟就偏出去了。

所以校准的目的就三个：

消除零偏：传感器静止时输出不为零的问题
修正比例因子：让输出值与真实值成正确的比例关系
正交化：三个轴之间的安装角度不是完美的90度，需要补偿

我的习惯：每次飞控上电后，先检查校准参数是否有效。如果发现参数异常，我会直接拒绝解锁。这个习惯救过我不少次。

4.2 加速度计校准流程

加速度计校准，最常用的方法是六位置法。说白了，就是把传感器分别朝上、朝下、朝左、朝右、朝前、朝后，每个方向采集数据。

4.2.1 六位置法操作步骤

将飞控水平放置，Z轴朝上，采集30秒数据
翻转180度，Z轴朝下，采集30秒数据
侧放，X轴朝上，采集30秒数据
侧放，X轴朝下，采集30秒数据
侧放，Y轴朝上，采集30秒数据
侧放，Y轴朝下，采集30秒数据

每个位置的理论加速度值应该是±1g。实际采集到的值跟理论值之间的差异，就是我们需要补偿的偏差。

4.2.2 校准模型与代码实现

我一般用这个六参数模型：

// 加速度计校准模型
// raw_data: 原始ADC值
// calibrated_data: 校准后的加速度值（单位：g）
// bias[3]: 零偏参数
// scale[3]: 比例因子参数

void accel_calibrate(float raw[3], float calibrated[3], 
                     float bias[3], float scale[3]) {
    for (int i = 0; i < 3; i++) {
        calibrated[i] = (raw[i] - bias[i]) * scale[i];
    }
}

参数怎么算？用最小二乘法拟合。我在项目中写过一套自动校准脚本，核心逻辑是这样的：

// 伪代码：六位置法参数计算
// 六个位置的测量值：m[0]~m[5]，每个位置对应一个理论值t[0]~t[5]
// 理论值：+1g, -1g, +1g, -1g, +1g, -1g

for each axis (X, Y, Z):
    // 该轴正方向测量值 + 负方向测量值 = 2 * bias
    bias[axis] = (m[positive] + m[negative]) / 2.0
    
    // 该轴正方向测量值 - 负方向测量值 = 2 * scale * 1g
    scale[axis] = 2.0 / (m[positive] - m[negative])

注意：实际操作时，每个位置要保证传感器完全静止。我见过有人用手拿着晃来晃去采集数据，那校准出来肯定不准。建议用水平台或者3D打印的夹具固定。

4.3 陀螺仪校准

陀螺仪校准比加速度计简单一些。为什么？因为陀螺仪测量的是角速度，静止时理论输出就是0。所以校准的核心就是采集静止数据，计算零偏。

4.3.1 静态零偏校准

操作很简单：把飞控放在水平桌面上，保持绝对静止，采集30秒到1分钟的数据，然后取平均值。

// 陀螺仪零偏校准
// 采集N个样本，计算平均值作为零偏
float gyro_bias[3] = {0};
int sample_count = 1000;  // 假设采样率100Hz，采集10秒

for (int i = 0; i < sample_count; i++) {
    gyro_bias[0] += raw_gyro[0][i];
    gyro_bias[1] += raw_gyro[1][i];
    gyro_bias[2] += raw_gyro[2][i];
}

gyro_bias[0] /= sample_count;
gyro_bias[1] /= sample_count;
gyro_bias[2] /= sample_count;

我曾经遇到过：有一次校准完陀螺仪，发现零偏值特别大，有5度/秒。排查了半天，发现是飞控板下面有个风扇在转，产生的振动干扰了陀螺仪。所以校准环境一定要安静，没有振动源。

4.3.2 温漂补偿

陀螺仪的零偏会随温度变化。我建议做温度补偿，特别是用在工业级产品上。方法是在温箱里从-40°C到85°C，每5°C采集一组零偏数据，然后拟合出一条曲线。

温度(°C)	X轴零偏(°/s)	Y轴零偏(°/s)	Z轴零偏(°/s)
-40	0.12	-0.08	0.05
-20	0.08	-0.05	0.03
0	0.03	-0.02	0.01
25	0.00	0.00	0.00
50	-0.04	0.03	-0.02
85	-0.10	0.07	-0.05

4.4 磁力计校准

磁力计校准是最容易被忽视的，但也是最容易出问题的。磁力计测量的是地球磁场，但周围任何铁磁物质都会扭曲磁场。你想想看，飞控板上的螺丝、电机、甚至PCB走线都会影响它。

4.4.1 硬铁与软铁干扰

硬铁干扰：由永磁体引起，表现为固定偏移。校准方法就是旋转飞控，采集各个方向的数据，找到球心偏移。
软铁干扰：由铁磁材料引起，表现为磁场扭曲。校准需要更复杂的椭球拟合算法。

4.4.2 椭球拟合校准法

实际操作中，我推荐做三维旋转校准。把飞控拿在手里，在空中画「8」字，确保每个方向都覆盖到。采集到的数据点应该分布在一个球面上。如果有硬铁干扰，球心会偏移；如果有软铁干扰，球会变成椭球。

// 磁力计椭球拟合校准
// 采集N个样本，拟合椭球参数
// 输出：偏移量offset[3]，缩放矩阵scale[3][3]

// 椭球方程： (x - ox)^2/a^2 + (y - oy)^2/b^2 + (z - oz)^2/c^2 = 1
// 使用最小二乘法求解参数

void mag_calibrate(float samples[][3], int n, 
                   float offset[3], float scale[3][3]) {
    // 1. 构建矩阵A和向量b
    // 2. 求解线性方程组
    // 3. 提取椭球参数
    // 4. 转换为偏移量和缩放矩阵
}

我曾经踩过的坑：有一次在室内校准磁力计，怎么校都不准。后来发现是办公桌的金属框架干扰了磁场。所以磁力计校准一定要在开阔、远离金属物体的地方进行。我一般建议在室外空旷场地做。

4.5 校准验证方法

校准完了，怎么知道校得准不准？不能光看参数算出来了就完事。我有一套验证流程：

4.5.1 加速度计验证

把飞控水平放置，检查Z轴输出是否为1g，X、Y轴是否为0
翻转各个方向，检查输出是否与理论值一致
计算模长：sqrt(x^2 + y^2 + z^2)，应该接近1g，误差小于0.01g

4.5.2 陀螺仪验证

静止时，三个轴输出应接近0，波动小于0.1°/s
用转台给一个已知角速度，比如100°/s，检查输出是否准确
积分测试：旋转90度，检查角度变化是否准确

4.5.3 磁力计验证

旋转飞控，检查模长是否恒定，波动小于5%
对比已知地磁场方向，检查航向角是否准确
做一次完整的360度旋转，检查航向角误差是否在3度以内

我的验证清单：每次校准后，我都会跑一遍自动化验证脚本。脚本会检查所有指标，如果任何一项超标，就自动标记为校准失败，需要重新校准。这个流程帮我拦截了至少20%的「假校准」情况。

4.6 传感器校准知识体系

下面这张图是我总结的传感器校准验证知识体系，你可以看到整个流程从校准到验证的闭环关系：

这张图展示了三个传感器的校准方法以及统一的验证流程。注意那个反馈迭代的箭头——校准不是一次性的，验证发现问题后要回到校准环节重新做。这个闭环很重要。

我的建议：把校准验证做成自动化脚本，每次飞控上电后自动执行。如果验证不通过，直接禁止起飞。这个机制在PX4和ArduPilot里都有实现，但很多二次开发的人把它关掉了。我个人强烈建议保留这个功能。

好了，传感器校准验证就聊到这儿。记住一句话：校准是基础，验证是保障。两者缺一不可。

📚 课程目录