3. 传感器原理：加速度计、陀螺仪、磁力计的工作原理、测量模型、误差特性及坐标系
姿态估计系统的输入质量直接决定卡尔曼滤波的收敛速度与稳态精度。若传感器原理不清、误差模型失真，再精妙的滤波算法也无法输出可信姿态。本章聚焦惯性测量单元（IMU）与磁力计的核心原理，建立统一的测量模型与误差描述框架，为后续卡尔曼滤波的状态方程与观测方程设计奠定物理基础。
3.1 加速度计：比力测量与重力参考
3.1.1 工作原理与测量模型
加速度计测量的是比力（Specific Force），即单位质量上受到的惯性力与引力之差。微机电系统（MEMS）加速度计通常采用电容式检测原理：质量块受加速度作用发生位移，导致差分电容变化，通过ASIC转换为数字输出。其理想测量模型为：
f_b = a_b - g_b其中 f_b 为加速度计输出的比力（m/s²），a_b 为载体运动加速度，g_b 为重力加速度在机体坐标系下的投影。当载体静止或匀速运动时，a_b ≈ 0，此时加速度计直接测量到重力矢量在机体轴上的分量，可用于计算横滚角与俯仰角：
roll = atan2(-f_y, -f_z)
pitch = atan2(f_x, sqrt(f_y² + f_z²))注意：当载体存在线性加速度（如车辆急加速、无人机高速平飞）时，加速度计输出包含运动加速度分量，此时直接使用加速度计解算姿态会引入显著误差。这是卡尔曼滤波中需要融合陀螺仪积分的原因之一。
3.1.2 误差特性与标定
MEMS加速度计的主要误差源包括：
- 零偏（Bias）：静态输出偏离真实比力的常值分量，典型值 ±50 mg（1 mg = 0.0098 m/s²），受温度影响明显。
- 噪声（VRW，速度随机游走）：高频白噪声积分后表现为速度随机游走，典型值 0.1~0.5 m/s/√h。
- 尺度因子误差：输出与输入的比值偏离1，通常为 0.1%~1%。
- 轴失准角：三个敏感轴不完全正交，失准角典型值 0.1°~1°。
六位置静态标定法是工程中最常用的加速度计标定方法：将IMU分别以±X、±Y、±Z六个方向朝上静置，采集各轴输出，利用最小二乘法求解零偏、尺度因子和失准角。标定后可将姿态解算精度提升一个数量级。
3.2 陀螺仪：角速度积分与漂移挑战
3.2.1 工作原理与测量模型
MEMS陀螺仪基于科里奥利效应：质量块在驱动轴方向做高频振动，当载体旋转时，科里奥利力使质量块在检测轴方向产生位移，通过电容检测得到角速度。理想测量模型为：
ω_b = ω_ib^b + b_g + n_g其中 ω_ib^b 为机体相对惯性系的角速度在机体系的投影，b_g 为陀螺零偏，n_g 为测量噪声。姿态更新通过四元数微分方程实现：
q_dot = 0.5 * q ⊗ [0, ω_x, ω_y, ω_z]陀螺仪短时精度极高（积分1分钟姿态误差通常小于1°），但零偏的存在导致姿态误差随时间线性累积。例如，零偏为 0.01°/s 的陀螺仪，积分10分钟后姿态误差可达6°。因此，陀螺仪必须与加速度计/磁力计融合，利用参考持续修正漂移。
3.2.2 误差特性与Allan方差分析
陀螺仪误差特性比加速度计更复杂，常用Allan方差分析来辨识各类误差源：
| 误差类型 | Allan方差特征 | 典型值（消费级） | 对姿态的影响 |
|---|---|---|---|
| 角度随机游走（ARW） | 斜率 -1/2 | 0.3~1.0 °/√h | 短时噪声，积分后缓慢累积 |
| 零偏不稳定性 | 曲线底部平坦区 | 5~20 °/h | 长时漂移的主要来源 |
| 速率随机游走（RRW） | 斜率 +1/2 | 1~10 °/h/√h | 低频漂移，难以估计 |
| 量化噪声 | 斜率 -1 | 取决于ADC位数 | 高频噪声，可低通滤波 |
在卡尔曼滤波设计中，ARW对应过程噪声协方差矩阵 Q 中的角速度噪声项，零偏不稳定性则对应陀螺零偏状态的过程噪声。正确设置这些参数是滤波器收敛速度和稳态精度的关键。
3.3 磁力计：航向参考与干扰挑战
3.3.1 工作原理与测量模型
磁力计测量载体所在位置的地磁场矢量。常用的磁阻传感器（如HMC5883L）利用各向异性磁阻效应：外部磁场改变薄膜电阻的阻值，通过惠斯通电桥输出与磁场强度成比例的电压。理想测量模型为：
m_b = C_n^b * m_n + b_m + n_m其中 m_n 为导航系下的地磁场矢量（在北东地坐标系中，水平分量指向磁北，垂直分量指向下），C_n^b 为姿态旋转矩阵，b_m 为硬磁干扰（由铁磁材料产生），n_m 为测量噪声。航向角可由磁力计水平分量计算：
yaw = atan2(-m_y, m_x) + declination其中 declination 为磁偏角（磁北与真北的夹角），需根据地理位置查表修正。
3.3.2 误差特性与校准
磁力计误差分为硬磁干扰和软磁干扰两类：
- 硬磁干扰：由永磁体或铁磁材料产生，表现为测量数据球心偏移，可通过椭球拟合校准。
- 软磁干扰：由可磁化材料产生，表现为测量数据球体变形为椭球，需通过最小二乘椭球拟合求解9个参数（3×3尺度矩阵+3×1偏移）。
现场校准流程：将设备在空间中缓慢旋转，采集覆盖各方向的磁场数据（至少200个点），使用椭球拟合算法计算校准参数。校准后航向精度可从 ±10° 提升至 ±1°~2°。
3.4 坐标系定义与转换
姿态估计涉及多个坐标系，清晰定义是避免混乱的前提：
| 坐标系 | 定义 | 用途 |
|---|---|---|
| 导航系（n系） | NED（北东地）或ENU（东北天），原点在载体质心 | 姿态解算的参考系，重力与地磁场在此系中定义 |
| 机体系（b系） | 原点在载体质心，Xb前、Yb右、Zb下（NED约定） | 传感器直接测量值所在坐标系 |
| 传感器系（s系） | 各传感器自身封装定义的轴向 | 实际输出数据所在坐标系，需标定与b系的安装失准角 |
姿态矩阵 C_n^b 将导航系矢量转换到机体系，其参数化形式包括欧拉角、方向余弦矩阵和四元数。卡尔曼滤波中通常使用四元数作为状态量，避免欧拉角的万向锁问题，同时保持计算效率。
3.5 案例拆解：消费级IMU误差标定与滤波参数设定
背景：某四旋翼无人机项目采用MPU9250（三轴加速度计+三轴陀螺仪+三轴磁力计），要求姿态精度：横滚/俯仰 < 0.5°，航向 < 2°（静态），动态飞行姿态发散时间 > 5分钟。
初始现象：直接使用芯片默认参数，静态姿态误差达 2.5°（横滚），航向误差达 8°，且飞行30秒后姿态明显漂移。
分析过程：
- 采集静止1小时数据，计算加速度计零偏：X轴 +35 mg，Y轴 -22 mg，Z轴 +18 mg，远超预期。
- Allan方差分析陀螺仪：ARW = 0.45 °/√h，零偏不稳定性 = 12 °/h，属于典型消费级水平。
- 磁力计在无人机中心旋转采集数据，椭球拟合发现硬磁偏移 [45, -30, 80] mGauss，软磁尺度差异达 8%。
操作步骤：
- 执行六位置静态标定，求解加速度计12参数（3零偏+3尺度+3失准+3交叉轴），标定后静态横滚/俯仰误差降至 0.3°。
- 对陀螺仪进行温度补偿（-20°C~60°C分段线性拟合），零偏随温度变化从 ±0.05 °/s 降至 ±0.01 °/s。
- 磁力计椭球拟合校准，航向误差从 8° 降至 1.5°。
- 根据Allan方差结果设置卡尔曼滤波参数：
Q_gyro_noise = (0.45/60)²（转换为 rad²/s），Q_bias_walk = (12/3600)²，R_acc = (0.05)²，R_mag = (0.03)²。
结果对比：标定与参数优化后，静态姿态误差降至 0.2°（横滚/俯仰）和 1.2°（航向）；动态飞行5分钟，姿态漂移 < 1.5°，满足设计要求。
复盘经验：传感器标定是姿态估计精度的基石，滤波算法只能修正残余误差。标定过程应记录温度数据，建立温度补偿模型；滤波参数必须基于实测Allan方差数据，而非经验猜测。
3.6 常见误区与总结
误区二：忽视陀螺仪零偏的时变性。零偏不是常数，而是随温度和时间缓慢变化的随机过程，必须在滤波器中建模估计。
误区三：磁力计校准一次即可永久使用。平台结构变化、电池位置移动、外部磁场环境改变都会导致校准失效，应定期重新校准。
误区四：坐标系定义混乱。传感器安装方向、导航系选择不一致，会导致姿态解算结果完全错误，务必在代码中明确注释坐标系约定。