4、传感器与测量：IMU原理（加速度计、陀螺仪）、磁力计、传感器融合基础

各位同学，欢迎来到第四讲。

前面几章我们聊了控制器的结构和参数整定，但有个问题我一直憋着没说——你拿什么去控制？说白了，飞控的大脑再聪明，也得靠眼睛和耳朵去感知世界。对于多旋翼来说，这个「感知器官」就是IMU和磁力计。

这一章，我带你把这些传感器的底裤扒干净。嗯，别想歪了，就是搞清楚它们怎么工作的，以及怎么把一堆乱七八糟的数据揉成有用的姿态信息。

4.1 加速度计：你感受到的是重力，不是运动

加速度计测量的是什么？很多人第一反应是「测量物体的加速度」。其实不对。

加速度计测量的是 比力（Specific Force），也就是物体受到的惯性力与重力的合力。你想想看，当你的四轴悬停在空中时，加速度计读数是（0, 0, -g）——它测到了重力，而不是运动加速度。

我在项目中遇到过一件事：有个同学把加速度计直接积分算速度，结果发现飞机明明没动，速度却一直在漂。为什么？因为重力分量一直在那里，积分一次就变成了速度，再积分一次就变成了位置，越漂越远。这就是典型的「积分漂移」。

加速度计的输出模型可以写成：

a_measured = a_true + g + b_a + n_a

其中：

• a_true 是真实运动加速度
• g 是重力加速度向量
• b_a 是零偏（bias）
• n_a 是测量噪声

在悬停或匀速飞行时，a_true ≈ 0，所以加速度计直接给出了重力方向。利用这个，我们可以算出俯仰角和横滚角：

roll  = atan2(ay, az)
pitch = atan2(-ax, sqrt(ay^2 + az^2))

注意，这里算不出偏航角。为什么？因为重力方向始终向下，你绕着重力轴转，加速度计是感觉不到的。

4.2 陀螺仪：角速度的忠实记录者

陀螺仪测量的是角速度，单位通常是 °/s 或 rad/s。它不像加速度计那样受运动加速度干扰，所以动态响应特别好。

但陀螺仪有个致命弱点——零偏漂移。你把它放在桌上一动不动，它输出的角速度也不是零，而是一个小值。这个值随时间缓慢变化，积分之后就会产生角度误差。

我曾经调试一架六轴，发现悬停时飞机慢慢往一个方向偏。查了半天，最后发现是陀螺仪的零偏没校准好。每次上电后，我习惯让飞机静置5秒，采集1000个样本取平均，把这个零偏值减掉。效果立竿见影。

陀螺仪的测量模型：

ω_measured = ω_true + b_g + n_g

其中 b_g 是陀螺零偏，n_g 是噪声。

从角速度到姿态，我们需要做积分。常用的方法是四元数更新：

q_new = q_old + 0.5 * dt * q_old ⊗ ω_measured

这里 ⊗ 表示四元数乘法。每次更新后，记得把四元数归一化，否则数值误差会累积。

4.3 磁力计：帮你找到北

磁力计测量的是地球磁场。有了它，我们才能得到偏航角的绝对参考。

但磁力计是个「娇气」的传感器。它怕什么？怕电机电流产生的磁场，怕铁磁材料，怕附近的高压线。我在一个工业现场调试时，飞机一靠近金属管道，磁力计读数就乱跳，偏航角直接歪了30度。

磁力计的校准是必修课：

1. 硬铁校准：消除传感器自身的零偏。方法是在水平面旋转360度，采集数据，找到椭球中心。
2. 软铁校准：消除周围铁磁材料造成的尺度误差。这个比较复杂，一般用椭球拟合算法。

磁力计计算偏航角的公式（假设已经水平校准）：

mag_x_compensated = mag_x * cos(pitch) + mag_y * sin(roll) * sin(pitch) + mag_z * cos(roll) * sin(pitch)
mag_y_compensated = mag_y * cos(roll) - mag_z * sin(roll)
yaw = atan2(-mag_y_compensated, mag_x_compensated)

4.4 传感器融合：把三个臭皮匠凑成诸葛亮

好了，现在我们有了三个传感器：

• 加速度计：低频准，高频噪
• 陀螺仪：高频准，低频漂
• 磁力计：绝对参考，但易受干扰

单独用哪一个都不行。怎么办？融合。

最经典的融合方法是 互补滤波器：

angle = alpha * (angle + gyro * dt) + (1 - alpha) * acc_angle

其中 alpha 是融合系数，一般取0.98左右。这个滤波器简单、计算量小，适合资源受限的MCU。我最早做飞控时就用这个，效果还不错。

但如果你追求更高的精度和鲁棒性，那就得上 卡尔曼滤波器。这里我给出一个简化版的姿态卡尔曼滤波器模型：

状态向量: x = [roll, pitch, yaw, gyro_bias_x, gyro_bias_y, gyro_bias_z]
观测向量: z = [acc_roll, acc_pitch, mag_yaw]

预测步:
x_pred = A * x + B * u
P_pred = A * P * A^T + Q

更新步:
K = P_pred * H^T * (H * P_pred * H^T + R)^(-1)
x = x_pred + K * (z - H * x_pred)
P = (I - K * H) * P_pred

看着复杂，其实核心思想就一句话：用陀螺仪做预测，用加速度计和磁力计做修正。

4.5 实战中的坑与经验

最后，分享几个我踩过的坑：

• 振动问题：多旋翼的电机振动会直接耦合到IMU上。我见过有人把IMU用海绵垫安装，结果振动是隔离了，但姿态响应也变慢了。正确的做法是用减震胶圈，同时配合陷波滤波器（Notch Filter）滤除特定频率的振动。
• 温度漂移：陀螺仪的零偏随温度变化。我习惯在飞控里加一个温度补偿表，每5度一个点，线性插值。效果很明显。
• 传感器同步：IMU数据和外部的视觉或GPS数据有时间戳对齐问题。我一般用MCU的硬件定时器给每个传感器数据打时间戳，确保融合时不会出现「张冠李戴」。

嗯，这一章的内容就到这里。传感器是飞控的基石，搞不懂它们，后面的控制参数整定就是空中楼阁。下一章我们开始讲执行器——电调和电机，看看怎么把控制信号变成实实在在的推力。

记住：好的姿态估计，是飞控稳定的一半。