📚 课程目录第3章传感器原理

三、传感器原理：加速度计、陀螺仪、磁力计的工作原理、误差模型、噪声特性及标定方法

姿态解算与稳定控制算法的核心前提，是对物理世界的准确感知。无论是四旋翼无人机、平衡车、机器人关节还是AR/VR设备，其姿态估计的精度上限由传感器本身决定——算法只能逼近传感器所能提供的信息边界，却无法超越它。因此，深入理解加速度计、陀螺仪、磁力计这三种MEMS传感器的物理原理、误差来源与噪声特性，是设计稳定控制系统的第一课。本章将从器件物理机制出发，建立误差模型，并给出工程上可落地的标定与补偿方法。

3.1 加速度计：从比力到姿态参考

3.1.1 核心工作原理

MEMS加速度计基于牛顿第二定律，其敏感元件是一个由硅微机械结构支撑的检验质量块。当载体沿敏感轴方向加速时，质量块因惯性产生位移，通过电容检测或压阻效应将位移转换为电信号。需要特别强调的是，加速度计实际测量的是比力（Specific Force），即物体所受非引力加速度之和。在静止状态下，加速度计输出的是重力加速度的反向矢量（1g），这正是其作为姿态参考的物理基础——通过测量重力在机体坐标系三轴上的投影分量，可以解算出横滚角与俯仰角。

关键公式：静止时，加速度计输出 f = a - g。当 a ≈ 0 时，f ≈ -g。设加速度计三轴输出为 (a_x, a_y, a_z)，则：

roll  = atan2(a_y, a_z)
pitch = atan2(-a_x, sqrt(a_y² + a_z²))

注意：该公式仅在载体无平动加速度时成立。任何线性加速度（如无人机急加速、车辆转弯）都会污染姿态解算结果。

3.1.2 误差模型与噪声特性

加速度计的误差模型可表示为：

a_meas = S_a · M_a · (a_true + b_a + n_a) + ε_a

其中，S_a 为比例因子误差矩阵（通常接近单位阵），M_a 为非正交误差矩阵（三轴安装不完全垂直），b_a 为零偏（bias），n_a 为测量噪声，ε_a 为非线性残差。噪声特性方面，MEMS加速度计主要呈现白噪声+低频随机游走特征，功率谱密度通常在 100-300 μg/√Hz 量级。在频域上，加速度计的低频信号可信度高，高频段噪声明显，因此适合作为姿态的长期参考，但需要低通滤波抑制振动噪声。

3.1.3 六位置静态标定法

六位置标定是加速度计最经典的标定方法，其核心思想是利用重力作为已知基准。操作流程如下：

将传感器水平放置，Z轴朝上（位置1），记录三轴输出均值，持续10秒。
翻转使Z轴朝下（位置2），记录数据。
依次使X轴朝上/朝下（位置3、4），Y轴朝上/朝下（位置5、6）。
利用最小二乘法求解零偏 b 和比例因子 S。

对于非正交误差，需要更精密的十二位置法或使用转台。工程实践中，六位置法可将零偏误差从 ±50mg 降低至 ±2mg 以内，比例因子误差从 1% 降至 0.1% 以内。

误差源	典型量级（消费级）	标定后残余	对姿态的影响
零偏 b	±30~80 mg	< 2 mg	静态姿态误差约 0.1°
比例因子误差	0.5%~1%	< 0.1%	动态加速度解算偏差
非正交误差	0.5°~1°	< 0.05°	三轴耦合误差
随机游走噪声	0.1~0.3 m/s²/√Hz	不可消除	姿态估计抖动

3.2 陀螺仪：角速度测量与积分漂移

3.2.1 科里奥利效应与振动式陀螺

MEMS陀螺仪利用科里奥利力原理测量角速度。其内部有一个持续振动的质量块，当载体绕敏感轴旋转时，质量块受到科里奥利力作用，产生与振动方向垂直的位移，通过电容检测该位移即可得到角速度。与加速度计不同，陀螺仪测量的是角速度，需要积分才能获得角度。这一积分过程使得陀螺仪的零偏误差随时间累积，形成角度漂移——这是姿态解算中最核心的挑战。

3.2.2 误差模型与温漂特性

陀螺仪的误差模型为：

ω_meas = S_g · M_g · (ω_true + b_g + n_g) + ε_g

其中 b_g 是零偏（单位 °/s），其最大特点是随温度漂移。典型消费级陀螺仪（如MPU6050）的零偏温漂可达 ±0.5°/s，在温度变化 20°C 时角度积分误差可达 10°/分钟。此外，陀螺仪还存在g-灵敏度误差——即线性加速度会耦合进角速度输出，这在振动环境中尤为明显。噪声方面，陀螺仪输出包含白噪声和速率随机游走（ARW），ARW 典型值为 0.3~1.0 °/√h。

工程陷阱：许多初学者直接使用陀螺仪原始数据积分求姿态，忽略零偏温漂。在室温下看似正常，一旦设备发热或环境温度变化，姿态角在数分钟内即可漂移数十度。必须配合加速度计/磁力计进行融合修正，或至少实现开机自动零偏校准。

3.2.3 转台速率标定法

转台标定是获取陀螺仪比例因子和非线性的标准方法。操作流程：

将陀螺仪固定在速率转台上，敏感轴与转台旋转轴对齐。
设定转台以固定角速度旋转（如 ±50°/s、±100°/s、±200°/s、±400°/s），每个速率点持续30秒。
记录陀螺仪输出均值，与真值对比，拟合比例因子 S 和零偏 b。
通过多速率点数据评估非线性度。

对于无转台的场景，可采用静态多位置法：将陀螺仪静止放置，采集长时间数据求均值作为零偏。但该方法无法标定比例因子，只能依赖出厂参数。

3.3 磁力计：航向参考与磁场干扰

3.3.1 地磁场测量原理

磁力计测量的是地球磁场在传感器坐标系中的投影矢量。地球磁场在水平面上的分量指向磁北，其与地理北的夹角称为磁偏角（需根据地理位置查表修正）。磁力计提供的是航向参考，可以弥补陀螺仪积分漂移和加速度计无法提供偏航角的问题。常见的MEMS磁力计采用各向异性磁阻（AMR）或巨磁阻（GMR）效应，测量范围通常为 ±4800 μT，分辨率可达 0.15 μT。

3.3.2 硬磁与软磁干扰模型

磁力计误差模型比加速度计和陀螺仪更复杂，主要分为两类：

硬磁干扰（Hard Iron）：由电路板上的永磁体或直流电流产生，表现为固定偏移量，使磁场球心偏离原点。
软磁干扰（Soft Iron）：由铁磁性材料（如电机外壳、螺丝）对磁力线的扭曲产生，表现为椭球变形，即各轴比例因子不一致且存在交叉耦合。

完整误差模型为：

m_meas = S_m · M_m · (m_true + b_hard) + b_soft

其中 b_hard 为硬磁偏移，S_m·M_m 描述软磁椭球变形。

3.3.3 椭圆拟合标定法

磁力计标定最常用的方法是空间旋转采样+椭球拟合：

将设备在空间中缓慢旋转，使磁力计经历所有姿态，采集至少200个采样点。
理想情况下，这些点应分布在一个球面上（地磁场模长恒定）。
实际数据形成椭球，通过最小二乘椭球拟合求解偏移量和变形矩阵。
将拟合结果代入补偿公式，将椭球校正为球体。

标定后，航向角计算为：

yaw = atan2(-m_y·cos(roll) + m_z·sin(roll),
            m_x·cos(pitch) + m_y·sin(pitch)·sin(roll) + m_z·sin(pitch)·cos(roll))

实战建议：磁力计标定必须在最终装配环境中进行，因为PCB布局、电池、扬声器磁铁都会引入干扰。每次硬件改动后都应重新标定。在飞行器上，电机大电流产生的磁场变化无法通过静态标定消除，建议在算法中增加动态磁场检测与异常剔除。

3.4 传感器融合前的预处理与实战案例

3.4.1 数据同步与滤波预处理

在将三组传感器数据送入姿态解算滤波器之前，必须完成以下预处理：

时间同步：确保加速度计、陀螺仪、磁力计的采样时刻对齐，否则会产生动态误差。使用FIFO中断或硬件同步引脚（如MPU9250的SYNC引脚）实现。
低通滤波：加速度计和磁力计对高频振动敏感，需设计截止频率为 20-50Hz 的二阶低通滤波器（Butterworth或Bessel）。
零偏扣除：将标定得到的零偏从原始数据中减去。
单位统一：加速度计转换为 m/s²，陀螺仪转换为 rad/s，磁力计转换为 μT。

3.4.2 案例：四旋翼无人机姿态传感器标定与验证

背景与约束：某四旋翼无人机项目采用 ICM-20602（陀螺仪+加速度计）+ IST8310（磁力计），主控MCU为STM32F405，控制频率1kHz，姿态解算频率500Hz。设计要求：悬停姿态误差 < 1°，航向漂移 < 2°/分钟。

初始现象：样机试飞时发现，悬停时机体缓慢向一侧倾斜，约30秒后倾斜角达5°；航向在1分钟内漂移约8°。通过地面站日志分析，发现陀螺仪零偏在电机启动后从 +0.02°/s 漂移至 +0.35°/s，且加速度计输出在油门变化时出现明显耦合。

分析过程：

首先检查陀螺仪零偏——电机未启动时零偏为 +0.02°/s，启动后变为 +0.35°/s，判断为电机磁场和振动导致的g-灵敏度误差。
检查加速度计——在电机高速旋转时，加速度计输出出现 ±0.3 m/s² 的波动，确认是机械振动耦合。
磁力计在电机启动后输出偏移约 80 μT，判断为电机大电流产生的硬磁干扰。

实际操作步骤：

对陀螺仪执行转台速率标定，获取精确比例因子（0.9987）和零偏（+0.015°/s）。
在飞控固件中增加电机启动后零偏自动校准逻辑：电机怠速稳定后，采集100ms陀螺仪数据求均值作为动态零偏。
对加速度计增加截止频率为 30Hz 的巴特沃斯低通滤波器，并在机械结构上增加减震泡棉。
对磁力计执行椭球拟合标定，并在算法中增加磁场模长异常检测（当 |m| 偏离 45±10 μT 时，降低磁力计融合权重）。

结果对比：优化后，悬停姿态误差从 5° 降至 0.6°，航向漂移从 8°/分钟降至 1.5°/分钟，满足设计要求。电机启动后陀螺仪零偏漂移从 0.35°/s 降至 0.04°/s。

风险与代价：动态零偏校准增加了约 100ms 的启动延迟；低通滤波引入了约 8ms 的相位延迟，需在控制环中补偿。磁力计异常检测在强磁环境下可能误判，需设置合理的阈值滞回。

复盘经验：传感器标定不是一次性工作，必须结合系统级工况（电机振动、温度变化、大电流磁场）进行综合补偿。纯静态标定无法解决动态干扰，需要算法层面的自适应策略。

3.4.3 常见误区与总结

误区	后果	正确做法
忽略陀螺仪温漂	姿态角随时间漂移	温漂补偿或定期零偏校准
磁力计标定后不重新验证	航向误差增大	每次硬件改动后重新标定
加速度计直接用于动态姿态	振动导致姿态抖动	低通滤波+融合算法
三传感器采样时间不同步	动态误差增大	硬件同步或插值对齐
标定数据采集不充分	拟合参数不准确	覆盖全姿态空间，采样点>200

总结而言，加速度计、陀螺仪、磁力计各自具有互补的频域特性：加速度计低频可信、陀螺仪高频可信、磁力计提供航向。理解其物理原理、建立准确的误差模型、实施有效的标定补偿，是后续设计互补滤波或卡尔曼滤波器的前提。在工程实践中，标定的精度直接决定了姿态解算的上限，而融合算法只是逼近这个上限的手段。下一阶段的学习中，我们将基于这些标定后的传感器数据，构建完整的姿态解算滤波器。