IMU基础：加速度计、陀螺仪、磁力计的工作原理、测量模型与关键性能指标

在机器人平衡控制系统中，IMU（惯性测量单元）是感知自身姿态与运动状态的核心传感器。无论是两轮自平衡车、双足人形机器人，还是四足机器人，其平衡控制回路都高度依赖IMU提供的姿态角、角速度和线加速度信息。本章将深入剖析加速度计、陀螺仪、磁力计三种传感器的工作原理、测量模型与关键性能指标，帮助读者建立对IMU的工程级理解，为后续的传感器融合与平衡控制算法打下坚实基础。

3.1 加速度计：感知比力与重力方向

3.1.1 工作原理与测量模型

加速度计测量的是物体受到的比力（Specific Force），即物体所受非引力加速度与重力加速度的矢量差。在机器人平衡控制中，加速度计最核心的作用是感知重力矢量在机体坐标系中的投影方向，从而推算横滚角与俯仰角。

以MEMS电容式加速度计为例，其内部包含一个由硅微机械加工而成的质量块，质量块通过弹性梁悬挂在固定电极之间。当加速度作用于传感器时，质量块因惯性发生位移，导致差分电容发生变化。通过测量电容差值，即可反推出加速度大小。其测量模型可表示为：

a_meas = a_true + b_a + n_a + s_a · a_true

其中，a_meas为传感器输出，a_true为真实比力，b_a为零偏（Bias），n_a为测量噪声，s_a为尺度因子误差。在平衡控制应用中，零偏和噪声是影响姿态估计精度的主要因素。

3.1.2 姿态解算方法

当机器人静止或匀速运动时，加速度计测得的比力即为重力加速度的反向矢量。设机体坐标系下加速度计输出为 [a_x, a_y, a_z]，则横滚角与俯仰角可通过以下公式计算：

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

需要注意的是，当机器人处于加速或减速运动状态时，加速度计会同时感受到运动加速度与重力加速度，此时直接使用上述公式会引入显著误差。因此，在动态平衡控制中，加速度计通常只用于低频姿态修正，而高频姿态变化由陀螺仪主导。

3.1.3 关键性能指标

3.2 陀螺仪：角速度测量与姿态积分

3.2.1 工作原理与测量模型

陀螺仪测量物体绕各轴的角速度。MEMS陀螺仪基于科里奥利效应工作：当质量块在驱动方向做高频振动时，若同时存在垂直于振动方向的角速度，则会产生科里奥利力，使质量块在检测方向发生位移。通过电容检测该位移，即可得到角速度值。

陀螺仪的测量模型为：

ω_meas = ω_true + b_g + n_g + s_g · ω_true

其中，b_g为角速度零偏，n_g为角速度白噪声，s_g为尺度因子误差。陀螺仪的零偏会随时间缓慢变化（称为零偏漂移），这是导致姿态积分发散的根本原因。

3.2.2 姿态积分与漂移问题

通过将角速度对时间积分，可以得到姿态角的变化量。以一维情况为例：

θ(t) = θ(0) + ∫₀ᵗ ω(τ) dτ

若陀螺仪存在0.1°/s的零偏，则积分10秒后会产生1°的姿态误差，积分60秒后误差达到6°。对于平衡控制而言，这显然是不可接受的。因此，陀螺仪必须与加速度计（或磁力计）进行融合，利用加速度计的低频准确性来持续修正陀螺仪的积分漂移。

3.2.3 关键性能指标

3.3 磁力计：绝对航向参考

3.3.1 工作原理与测量模型

磁力计测量地球磁场在传感器坐标系中的矢量分量。地球磁场在水平面上的投影总是指向磁北方向，因此通过磁力计可以确定机器人的绝对航向角（偏航角）。MEMS磁力计通常采用各向异性磁阻（AMR）或霍尔效应原理，三轴正交排列以测量三维磁场矢量。

磁力计的测量模型为：

m_meas = R · m_earth + b_m + n_m + H_i · m_meas

其中，R为姿态旋转矩阵，m_earth为地磁场矢量，b_m为硬铁干扰（传感器附近的固定磁场源），n_m为噪声，H_i为软铁干扰矩阵（由铁磁材料对磁力线的扭曲引起）。

3.3.2 航向角计算与校准

在水平状态下，偏航角可通过以下公式计算：

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))

磁力计在使用前必须进行校准，主要包括：

1. 硬铁校准：将机器人旋转360°，记录磁场数据，计算球心偏移量并扣除。
2. 软铁校准：通过椭圆拟合修正磁场矢量的各向异性缩放。
3. 倾斜补偿：结合加速度计的姿态信息，将磁场矢量投影到水平面后再计算航向角。

3.3.3 关键性能指标与局限性

3.4 传感器融合策略与工程实践

3.4.1 互补滤波原理

互补滤波是平衡控制中最常用的传感器融合方法。其核心思想是：加速度计和磁力计提供低频准确、高频噪声大的姿态信息；陀螺仪提供高频准确、低频漂移的角速度信息。通过高通滤波器和低通滤波器的组合，将两者的优势互补：

θ_est = α · (θ_est + ω · dt) + (1 - α) · θ_acc

其中，α为滤波系数，通常取0.95~0.98。滤波系数越大，对陀螺仪的信任度越高，姿态响应越快，但低频漂移也越大；反之，滤波系数越小，对加速度计的信任度越高，姿态越稳定，但动态响应变慢。

3.4.2 案例：两轮自平衡车的姿态估计

背景与约束：某两轮自平衡车采用MPU6050（三轴加速度计+三轴陀螺仪），控制周期为5ms（200Hz），要求姿态估计误差小于0.5°，且不能使用磁力计（因电机磁场干扰严重）。

初始现象：直接使用加速度计计算姿态角时，由于车体频繁加速/减速，姿态角出现大幅波动（±5°以上）；直接积分陀螺仪角速度时，姿态角在30秒内漂移超过10°。

分析判断过程：

• 加速度计噪声主要来自车体振动和运动加速度，属于高频干扰，应通过低通滤波抑制。
• 陀螺仪零偏漂移属于低频误差，应通过高通滤波消除。
• 互补滤波的截止频率应设置在0.5~2Hz之间，既能有效抑制加速度计高频噪声，又能快速修正陀螺仪漂移。

实际操作步骤：

1. 采集静止状态下的加速度计数据1000个样本，计算零偏并扣除。
2. 采集静止状态下的陀螺仪数据1000个样本，计算平均零偏并扣除。
3. 设置互补滤波系数α=0.98，对应截止频率约0.65Hz。
4. 在车体上施加已知角度（如10°、20°、30°）进行静态验证。
5. 进行动态骑行测试，观察姿态角是否平滑且无滞后。

结果对比：

风险与代价：互补滤波参数α需要根据实际振动环境调整。α过大会导致姿态响应迟钝，车体在急加速时姿态估计滞后；α过小则无法有效抑制加速度计噪声。此外，互补滤波对陀螺仪零偏的估计能力有限，若零偏随温度变化较大，需要增加温度补偿环节。

复盘经验：在平衡控制中，姿态估计的延迟比噪声更致命。应优先保证姿态估计的实时性，再通过滤波平滑噪声。建议在控制回路中直接使用原始角速度进行微分控制，而姿态角仅用于比例控制，这样可减少滤波带来的相位滞后。

3.4.3 常见误区与总结