3. 传感器标定：六面法标定加速度计、角速率标定、椭球拟合磁力计标定、温漂补偿

传感器标定，说白了就是给传感器“纠偏”。

你想想看，一个加速度计，静止放在桌上，理论上只感受到重力。但实际读出来的数，三个轴往往不是(0,0,g)。为什么？因为芯片有零偏，有刻度误差，还有安装歪斜。我刚开始做飞控那会儿，就吃过这个亏——传感器没标定，飞控悬停都抖得跟筛子似的。

3.1 六面法标定加速度计

六面法，名字听着唬人，其实原理很简单。就是把加速度计分别朝六个方向摆放，记录每个方向的读数，然后反算出零偏和刻度系数。

具体哪六个方向？

• X轴朝上、X轴朝下
• Y轴朝上、Y轴朝下
• Z轴朝上、Z轴朝下

每个方向，理论上加速度计感受到的矢量是已知的。比如Z轴朝上，理论值就是(0, 0, +g)。实际读数是(ax, ay, az)。

我个人习惯用最小二乘法来拟合。假设模型是：

a_meas = K * a_true + b

其中K是对角矩阵（刻度系数），b是零偏。六个方程，六个未知数，解起来很稳。

|a_meas_x|   |Kx  0   0 | |a_true_x|   |bx|
|a_meas_y| = |0   Ky  0 | |a_true_y| + |by|
|a_meas_z|   |0   0   Kz| |a_true_z|   |bz|

3.2 角速率标定

陀螺仪的标定，核心是零偏和刻度。零偏好办，静止时采集几百个数据取平均就行。刻度标定就麻烦一点。

为什么？因为陀螺仪测的是角速度，你需要一个已知的角速度源。我常用的方法有两种：

• 转台法： 用精密转台，给一个固定转速，比如100°/s，然后对比读数。
• 积分法： 让飞控绕某个轴旋转固定角度（比如360°），积分角速度得到角度，反推刻度。

我个人更推荐转台法，精度高。但如果没有转台，积分法也能凑合。注意积分法对零偏很敏感，零偏没标好，积分结果会漂。

3.3 椭球拟合磁力计标定

磁力计标定，和加速度计不太一样。加速度计的理论值是个球面（半径为g），而磁力计的理论值是个椭球面。

为什么是椭球？因为地磁场本身是均匀的，但传感器周围的铁磁物质会扭曲磁场，导致三个轴的灵敏度不一样，还有软磁和硬磁干扰。

硬磁干扰，说白了就是传感器附近有永磁体或电流，产生一个固定的偏置磁场。软磁干扰，则是铁磁物质改变了磁力线的走向。

标定方法：

1. 拿着飞控在空中画“8”字，让磁力计采集各个方向的数据。
2. 这些数据点会落在一个椭球面上。
3. 用最小二乘法拟合椭球，得到中心点（硬磁干扰）和半轴长度（软磁干扰和刻度）。

椭球方程：

(x - cx)^2 / a^2 + (y - cy)^2 / b^2 + (z - cz)^2 / c^2 = 1

其中(cx, cy, cz)就是硬磁补偿值，a、b、c是刻度系数。

import numpy as np

# 采集的数据：N x 3 矩阵
data = np.loadtxt('mag_data.txt')

# 椭球拟合
def ellipsoid_fit(data):
    # 构造矩阵 A 和 b
    # 解最小二乘
    # 返回中心点和半轴
    pass

center, radii = ellipsoid_fit(data)
print("硬磁补偿:", center)
print("刻度系数:", radii)

3.4 温漂补偿

温漂，是所有传感器的通病。温度一变，零偏和刻度都会变。尤其是陀螺仪的零偏，温度每变化1°C，可能漂移0.01°/s。飞控飞个十分钟，温度升个20°C，零偏就漂了0.2°/s，积分出来的角度误差会累积到好几度。

温漂补偿的常用方法：

• 查表法： 在温箱里采集不同温度下的零偏，做成温度-零偏对照表。飞控运行时，根据当前温度查表插值。
• 多项式拟合： 用二次或三次多项式拟合零偏随温度的变化曲线。
• 在线估计： 飞控静止时，实时更新零偏估计。但这个方法在运动时不能用。

我个人习惯用查表法加线性插值。简单，可靠，计算量小。

知识体系总览

下面这张图，把传感器标定的核心逻辑串起来了。你可以看到，加速度计、陀螺仪、磁力计各有各的标定方法，但最终都指向一个目标：让测量值更接近真实值。

嗯，到这里，传感器标定的核心内容就讲完了。六面法、角速率标定、椭球拟合、温漂补偿，每一个都是实战中绕不开的坎。我个人觉得，标定这件事，花的时间越多，飞控飞得越稳。别偷懒，老老实实做。