4. 传感器基础（一）：加速度计原理与模型、陀螺仪原理与模型、磁力计原理与模型

各位同学，欢迎来到传感器基础的第一讲。做姿态控制，说白了就是跟这三个传感器打交道：加速度计、陀螺仪、磁力计。它们就像飞行器的眼睛和耳朵，告诉你现在“头朝哪”、“转得快不快”、“北在哪边”。

我刚开始做飞控那会儿，总觉得算法是核心，传感器嘛，买现成的模块焊上去就行了。结果第一次试飞，飞机在天上像个醉汉，晃得我头皮发麻。后来才发现，不是算法写错了，是压根没读懂传感器的“脾气”。

这一章，我们就来彻底搞懂这三个传感器的物理原理和数学模型。你只有知道它们怎么工作的，才知道怎么用好它们，怎么避开那些坑。

4.1 加速度计：感知“重力”与“运动”

4.1.1 工作原理：弹簧上的小方块

加速度计的核心，其实就是一个微小的“质量-弹簧-阻尼”系统。你想象一下，在一个小盒子里，有一个小方块被弹簧拉着。当盒子加速时，小方块会因为惯性而移动，挤压弹簧。我们测量这个位移，就能算出加速度。

嗯，这里要注意，加速度计测量的不是“速度”，而是“加速度”。而且，它测的是 比力（Specific Force），也就是物体受到的除了重力以外的合力。但有趣的是，它无法区分“重力”和“运动加速度”。

4.1.2 数学模型：从物理到代码

我们通常用三轴加速度计，输出三个轴的比力值。在理想情况下，它的模型可以写成：

a_measured = a_true + g + b_a + n_a

其中：

• a_measured：传感器输出的原始值
• a_true：真实的运动加速度
• g：重力加速度矢量（在导航系下是[0, 0, -g]）
• b_a：零偏（Bias），一个缓慢变化的常值误差
• n_a：高斯白噪声

这个模型非常重要。它告诉我们，想从加速度计里提取姿态信息，前提是 a_true 必须为0，也就是飞行器处于静止或匀速运动状态。否则，你算出来的“姿态”其实是运动加速度和重力的混合物，完全不可信。

4.1.3 避坑指南：加速度计的“软肋”

我曾经在一个项目中，发现飞机悬停时姿态角一直在低频抖动。查了三天，最后发现是机架的共振频率和加速度计的采样频率产生了耦合。解决办法很简单：在硬件上加减振海绵，在软件上加一个低通滤波器。

另外，加速度计对温度很敏感。零偏会随着温度漂移。我建议你在上电后，等传感器温度稳定了再开始初始化，或者做一个简单的温度补偿模型。

4.2 陀螺仪：测量“旋转”的利器

4.2.1 工作原理：科里奥利力的魔法

现代MEMS陀螺仪，利用的是科里奥利效应。简单说，就是一个高速振动的质量块，当它所在的平台发生旋转时，质量块会受到一个垂直于振动方向和旋转轴方向的力，这个力就是科里奥利力。测量这个力的大小，就能算出角速度。

你想想看，这个原理决定了陀螺仪天生对振动敏感。因为任何外部振动都会干扰质量块的振动状态，从而引入误差。

4.2.2 数学模型：积分是天使也是魔鬼

陀螺仪的模型相对简单：

ω_measured = ω_true + b_g + n_g

• ω_measured：测量的角速度
• ω_true：真实的角速度
• b_g：零偏（Bias），陀螺仪最大的敌人
• n_g：高斯白噪声

陀螺仪最大的问题在于，我们用它来积分得到角度。积分会放大误差，尤其是零偏。哪怕是一个微小的零偏，积分几秒钟后，角度误差就会大到不可接受。

4.2.3 避坑指南：陀螺仪的“温漂”与“振动”

我个人习惯，每次上电后，先让飞机静止几秒钟，采集一段陀螺仪数据，取平均作为初始零偏。这叫“静态初始化”。但要注意，这个零偏会随着温度变化而漂移。如果你对精度要求高，可以考虑在飞行过程中实时估计零偏（比如用卡尔曼滤波器）。

另外，螺旋桨的高速旋转会产生强烈振动。如果减振没做好，陀螺仪的输出会叠加一个高频噪声。我建议在硬件上使用减振支架，在软件上使用陷波滤波器（Notch Filter）来滤除特定频率的振动。

4.3 磁力计：寻找“北”的指南针

4.3.1 工作原理：地球磁场与霍尔效应

磁力计，也叫电子罗盘，它测量的是地球磁场在传感器三个轴上的分量。地球磁场本身很微弱（约0.5高斯），而且方向是从地理南极指向地理北极（磁北极）。

大多数MEMS磁力计基于霍尔效应或磁阻效应。当外部磁场变化时，传感器内部的电阻或电压会发生变化，通过测量这个变化就能算出磁场强度。

4.3.2 数学模型：一个“脆弱”的矢量

磁力计的模型比前两个复杂一些，因为它容易受到干扰：

m_measured = R * m_earth + m_hard + m_soft + n_m

• m_measured：测量的磁场矢量
• R：从导航系到机体系的旋转矩阵（包含姿态信息）
• m_earth：当地的地球磁场矢量（已知，可查表）
• m_hard：硬铁干扰（由永磁体或带电流的导线产生，是常值偏移）
• m_soft：软铁干扰（由铁磁性材料产生，会扭曲磁场形状）
• n_m：高斯白噪声

这个模型告诉我们，想用磁力计求航向角，必须先做校准，消除硬铁和软铁干扰。否则，你算出来的航向会随着飞机旋转而变化，完全不准。

4.3.3 避坑指南：磁力计的“天敌”

我曾经在一个项目中，发现磁力计的航向角在某个方向总是跳变。排查后发现，是机臂上的大功率电机导线在通电时产生了强磁场，干扰了磁力计。解决办法是把磁力计尽量远离大电流导线和电机，或者使用磁屏蔽材料。

另外，室内环境有很多铁质结构（钢筋、管道），会导致局部磁场畸变。在室内飞行时，磁力计的航向只能作为参考，不能完全依赖。

4.4 知识体系总览

为了让你更直观地理解这三个传感器的关系，我画了一张图。你可以看到，它们各有优劣，需要互补使用。

4.5 传感器对比总结

最后，我用一张表格来总结这三个传感器的核心特性，方便你对比记忆：

特性	加速度计	陀螺仪	磁力计
测量物理量	比力（加速度）	角速度	磁场强度
主要用途	修正横滚/俯仰（静态）	短时姿态积分	修正偏航（航向）
主要误差	运动加速度干扰、振动	零偏漂移、温漂	硬铁/软铁干扰、环境磁场
动态响应	慢（需低通滤波）	快	中等
长期稳定性	好（无漂移）	差（积分漂移）	好（但易受干扰）
校准方式	六面静态校准	静态初始化	椭球拟合（画8字）