3. IMU传感器原理：加速度计、陀螺仪、磁力计工作原理与数据融合
IMU，全称惯性测量单元，是四足机器人感知自身运动状态的核心器件。说白了，它就是机器人的「内耳前庭」——没有它，机器人不知道自己是在跑、在跳，还是已经摔了个四脚朝天。
我个人习惯把IMU拆成三个独立的传感器来看：加速度计测「平动」，陀螺仪测「转动」，磁力计测「方向」。它们各自有短板，但合在一起，就能给出一个相对靠谱的姿态估计。
3.1 加速度计：感知重力与线加速度
加速度计的原理其实挺直观的。它内部有一个微小的质量块，被弹簧悬在硅结构上。当传感器加速时，质量块会偏移，电容值随之变化，最终输出一个电压信号。
我遇到过不少刚入行的同学问：「加速度计静止时输出是0吗？」
不是的。静止时它测的是重力加速度g，大约9.8 m/s²。你把它平放，Z轴读数是g；竖起来，X轴或Y轴就是g。这个特性，恰恰是我们计算俯仰角和横滚角的基础。
但加速度计有个硬伤——它怕振动。四足机器人走路时，每条腿落地都会产生冲击加速度。这时候加速度计读出来的，是重力加速度和运动加速度的混合体。你直接用这个算角度，结果会剧烈抖动。
3.2 陀螺仪：测量角速度
陀螺仪用的是科里奥利效应。一个微小的音叉结构在高速振动，当传感器旋转时，振动平面会产生偏移，电容检测到变化，输出角速度。
陀螺仪的优点很明显：响应快，不受振动影响。你给它一个瞬间的旋转，它立刻就能测出来。四足机器人需要快速调整姿态时，陀螺仪是主力。
但陀螺仪的缺点也很致命——它漂移。我做过一个实验：把IMU放在桌面上静止不动，陀螺仪积分出来的角度，10分钟后偏了15度。为什么会这样？因为陀螺仪有零偏误差，积分会把误差不断累积。
| 传感器 | 优点 | 缺点 | 适用场景 |
|---|---|---|---|
| 加速度计 | 无漂移，长期稳定 | 易受振动干扰 | 静态/慢速姿态参考 |
| 陀螺仪 | 响应快，抗振动 | 积分漂移 | 动态姿态跟踪 |
| 磁力计 | 提供航向 | 易受磁场干扰 | 偏航角校正 |
3.3 磁力计：电子罗盘
磁力计测量的是地球磁场方向。它和加速度计配合，可以给出完整的3D姿态——包括偏航角。
但磁力计是个「娇气」的传感器。你把它放在电机旁边，读数就乱跳；你把它靠近铁质结构，偏航角就偏了。我在四足机器人上遇到过最头疼的问题：机器人原地转圈，磁力计读出来的航向却在来回摆动。后来发现是电机电流产生的磁场干扰。
3.4 数据融合：互补滤波与卡尔曼滤波
单个传感器都有短板，那怎么办？融合。把加速度计的低频稳定性和陀螺仪的高频响应结合起来，就是数据融合的核心思想。
3.4.1 互补滤波
互补滤波是最简单、最实用的融合方法。它的思路是：
- 陀螺仪积分得到的角度，通过高通滤波器（保留快速变化）
- 加速度计计算得到的角度，通过低通滤波器（保留长期稳定）
- 两者相加，得到最终姿态
# 互补滤波核心代码
def complementary_filter(gyro_rate, acc_angle, dt, alpha=0.98):
"""
gyro_rate: 陀螺仪角速度 (rad/s)
acc_angle: 加速度计计算的角度 (rad)
dt: 时间步长 (s)
alpha: 融合系数，通常0.96~0.99
"""
# 陀螺仪积分
gyro_angle += gyro_rate * dt
# 融合
fused_angle = alpha * gyro_angle + (1 - alpha) * acc_angle
return fused_anglealpha取0.98意味着：98%信任陀螺仪的短期变化，2%信任加速度计的长期参考。这个系数我调过很多次，0.98是个不错的起点，具体要看你的机器人振动情况。
3.4.2 卡尔曼滤波
卡尔曼滤波更复杂，但效果更好。它把姿态估计建模成一个状态空间问题：
- 状态向量： 角度 + 陀螺仪零偏
- 预测： 用陀螺仪积分更新状态
- 更新： 用加速度计观测值修正状态
我建议初学者先从互补滤波入手。卡尔曼滤波虽然精度高，但调参很痛苦——过程噪声协方差Q、观测噪声协方差R，这两个矩阵调不好，结果还不如互补滤波。
3.5 IMU数据融合流程总览
下面这张图，是我自己总结的IMU数据融合流程。你照着这个走，基本不会出错。
3.6 实际项目中的注意事项
最后，分享几个我在四足机器人项目中踩过的坑：
- IMU安装位置很重要。 尽量靠近机器人的质心。装在腿上？别想了，那数据根本没法用。
- 采样率要匹配。 陀螺仪和加速度计的采样率最好一致，否则融合时时间戳对不上，角度会跳变。
- 温漂不可忽视。 机器人跑10分钟后，IMU温度升高，零偏会变化。我习惯在代码里加一个简单的温漂补偿模型。
- 磁力计远离大电流线。 电源线、电机线都会产生磁场。布线时尽量让磁力计远离这些干扰源。