2. 惯性测量单元(IMU)基础

各位同学好，我是老张。今天咱们聊聊IMU，也就是惯性测量单元。这东西在无人机、机器人、自动驾驶里到处都是，说白了就是给系统装了个「内耳」，让它知道自己怎么动、往哪歪。

我刚开始接触IMU时，觉得不就是个加速度计加陀螺仪嘛，读个数就完事了。结果第一次做四旋翼飞控，飞机刚离地就翻了个跟头……嗯，从那以后我才明白，IMU的水比想象中深得多。

1.1 加速度计工作原理

加速度计测的是什么？很多人以为是「速度变化率」，其实不对。它测的是比力——也就是物体受到的惯性力与重力的合力。

你想想看，把加速度计平放在桌上，它读出来的是0吗？不是。它读出来的是9.8 m/s²，指向地心。这就是重力加速度。所以加速度计在静态时，其实是个「倾角传感器」。

我做过一个项目，把IMU装在机械臂末端。一开始怎么都调不准，后来发现是安装面有0.5度的倾斜。你看，加速度计对重力方向极其敏感，安装误差直接体现在数据里。

1.2 陀螺仪工作原理

陀螺仪测的是角速度，单位是rad/s或°/s。它利用的是科里奥利效应——说白了，就是一个振动的质量块，在旋转时会受到一个垂直于振动方向的力。

MEMS陀螺仪内部有个微小的硅结构，一直在振动。当芯片旋转时，这个振动结构会受到科里奥利力，电容变化被检测出来，就得到了角速度。

嗯，这里要注意：陀螺仪测的是角速度，不是角度。要得到角度，你得积分。但积分会累积误差，这就是为什么纯陀螺仪的姿态会漂移。

1.3 IMU数据读取与预处理

拿到IMU原始数据后，不能直接用。为什么？因为传感器有噪声、有偏置、有温度漂移。我习惯按以下步骤处理：

1. 原始数据读取：通过I2C或SPI读取寄存器值
2. 单位转换：将原始ADC值转换为物理单位
3. 去偏置：减去静态时的平均值
4. 去噪：应用低通滤波器

下面是我常用的预处理代码，Python写的，跑在树莓派上：

import numpy as np
from scipy import signal

class IMUPreprocessor:
    def __init__(self, fs=100, cutoff=5):
        self.fs = fs  # 采样频率 100Hz
        self.cutoff = cutoff  # 截止频率 5Hz
        self.b, self.a = self._design_filter()
        self.accel_bias = np.zeros(3)
        self.gyro_bias = np.zeros(3)
        
    def _design_filter(self):
        """设计巴特沃斯低通滤波器"""
        nyq = 0.5 * self.fs
        normal_cutoff = self.cutoff / nyq
        b, a = signal.butter(2, normal_cutoff, btype='low')
        return b, a
    
    def calibrate_bias(self, accel_data, gyro_data):
        """静态校准：取前100个样本的平均值作为偏置"""
        self.accel_bias = np.mean(accel_data, axis=0)
        self.gyro_bias = np.mean(gyro_data, axis=0)
        print(f"加速度计偏置: {self.accel_bias}")
        print(f"陀螺仪偏置: {self.gyro_bias}")
    
    def preprocess(self, accel_raw, gyro_raw):
        """预处理：去偏置 + 低通滤波"""
        # 去偏置
        accel = accel_raw - self.accel_bias
        gyro = gyro_raw - self.gyro_bias
        
        # 低通滤波
        accel_filtered = signal.filtfilt(self.b, self.a, accel, axis=0)
        gyro_filtered = signal.filtfilt(self.b, self.a, gyro, axis=0)
        
        return accel_filtered, gyro_filtered

1.4 加速度计标定方法

加速度计标定，说白了就是找出「真实值」和「测量值」之间的映射关系。理想情况下，加速度计三个轴应该完全正交，灵敏度完全一致。但现实是——

我遇到过一批IMU，X轴灵敏度比Y轴高了2%。这意味着什么？同样倾斜1度，X轴和Y轴的读数不一样，姿态解算就会出错。

常用的标定方法是六面法：

位置	X轴	Y轴	Z轴	理论值
水平朝上	0	0	+g	9.81
水平朝下	0	0	-g	-9.81
X轴朝上	+g	0	0	9.81
X轴朝下	-g	0	0	-9.81
Y轴朝上	0	+g	0	9.81
Y轴朝下	0	-g	0	-9.81

标定过程其实不复杂：把IMU依次放在六个位置，每个位置采集几秒数据，然后拟合出标定参数。我一般用最小二乘法：

def calibrate_accel(samples, ref_values):
    """
    六面法标定加速度计
    samples: 六个位置的测量值, shape=(6, 3)
    ref_values: 六个位置的理论值, shape=(6, 3)
    返回: 比例因子 s 和偏置 b
    """
    # 构建矩阵 A = [ax, ay, az, 1]
    A = np.hstack([samples, np.ones((6, 1))])
    # 求解最小二乘
    coeff, _, _, _ = np.linalg.lstsq(A, ref_values, rcond=None)
    
    s = coeff[:3, :]  # 3x3 比例/交叉耦合矩阵
    b = coeff[3, :]   # 偏置向量
    
    return s, b

# 使用示例
samples = np.array([
    [0.02, 0.01, 9.78],   # 水平朝上
    [0.01, -0.02, -9.80], # 水平朝下
    [9.76, 0.03, 0.01],   # X朝上
    [-9.79, -0.01, 0.02], # X朝下
    [0.01, 9.77, -0.01],  # Y朝上
    [-0.02, -9.81, 0.03]  # Y朝下
])

ref = np.array([
    [0, 0, 9.81],
    [0, 0, -9.81],
    [9.81, 0, 0],
    [-9.81, 0, 0],
    [0, 9.81, 0],
    [0, -9.81, 0]
])

s, b = calibrate_accel(samples, ref)
print("比例矩阵:\n", s)
print("偏置向量:\n", b)

1.5 本章小结

IMU基础这部分，说白了就三件事：

• 加速度计：静态测倾角，动态测加速度。低频特性好，高频噪声大。
• 陀螺仪：测角速度，动态响应快。但有零偏漂移，不能单独用。
• 预处理：去偏置、去噪声、标定。这三步做不好，后面全是白费功夫。

我个人习惯，每次拿到新IMU，第一件事就是做静态标定和噪声分析。花半小时把传感器摸透，后面能省三天调试时间。你想想看，是不是这个理？

---