第四章：IMU惯性导航——IMU器件原理与捷联惯导算法
各位同学，欢迎来到第四章。这一章我们聊聊IMU，也就是惯性测量单元。说实话，IMU是V2X定位里最“磨人”的传感器。它不像GPS那样直接告诉你位置，而是靠“猜”——猜你从A点怎么走到B点的。嗯，这里面的门道可不少。
4.1 IMU器件原理：加速度计与陀螺仪
IMU的核心就两个东西：加速度计和陀螺仪。我习惯把它们比作“感觉器官”。加速度计感受的是“推背感”，陀螺仪感受的是“旋转感”。
4.1.1 加速度计的工作原理
加速度计测量的是比力，不是单纯的运动加速度。什么意思呢？你想想看，静止放在桌面上时，加速度计测到的是1g（重力加速度），而不是0。这个坑我刚开始做项目时踩过——以为静止时加速度就是0，结果算出来的位置飞到了天上去。
常见的MEMS加速度计内部有个质量块，像弹簧一样挂着。外界有加速度时，质量块会偏移，电容值就变了。测出电容变化，反推加速度。说白了，就是个微型的“弹簧秤”。
关键点：加速度计输出的是“比力”，即物体受到的合力减去重力。公式上：a_measured = a_true + g。所以做惯导解算时，第一步就是扣除重力分量。
4.1.2 陀螺仪的工作原理
陀螺仪测量角速度，单位是°/s。MEMS陀螺仪用的是科里奥利效应——一个振动的质量块，如果发生旋转，会产生一个垂直于振动方向的力。测这个力的大小，就知道转得多快了。
我记得第一次看陀螺仪数据时，发现静止时输出也不是0，而是有个小偏置。我当时以为是传感器坏了，后来才知道——这就是零偏，所有陀螺仪都有的毛病。
个人经验：选型时，我建议重点关注陀螺仪的零偏稳定性和角度随机游走。这两个参数直接决定了你能“纯惯导”撑多久。消费级的IMU可能几秒就飘了，战术级的能撑几分钟。
4.2 捷联惯导算法
捷联惯导，说白了就是把IMU“绑”在车身上，用数学方法模拟出一个虚拟的导航平台。没有物理转台，全靠算。
4.2.1 姿态更新
姿态是惯导的核心。你连车头朝哪都不知道，后面算位置全是扯淡。姿态更新用的是陀螺仪的角速度积分。
常用的方法有：欧拉角法、方向余弦法、四元数法。我个人强烈推荐四元数法——没有万向锁问题，计算量也小。
// 四元数姿态更新示例（离散化）
// q: 当前姿态四元数, w: 角速度(rad/s), dt: 采样间隔
void quaternion_update(double q[4], double w[3], double dt) {
double wx = w[0], wy = w[1], wz = w[2];
double norm = sqrt(wx*wx + wy*wy + wz*wz);
double half_theta = 0.5 * norm * dt;
// 小角度近似，实际项目建议用精确公式
double s = sin(half_theta) / norm;
double qw = cos(half_theta);
double qx = wx * s;
double qy = wy * s;
double qz = wz * s;
// 四元数乘法更新
double q0 = q[0], q1 = q[1], q2 = q[2], q3 = q[3];
q[0] = q0*qw - q1*qx - q2*qy - q3*qz;
q[1] = q0*qx + q1*qw + q2*qz - q3*qy;
q[2] = q0*qy - q1*qz + q2*qw + q3*qx;
q[3] = q0*qz + q1*qy - q2*qx + q3*qw;
// 归一化
double inv_norm = 1.0 / sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
q[0] *= inv_norm; q[1] *= inv_norm; q[2] *= inv_norm; q[3] *= inv_norm;
}注意：四元数一定要记得归一化！我曾经因为忘记归一化，导致姿态越算越歪，最后定位误差累积到几十米。查了两天才找到这个bug。
4.2.2 速度与位置更新
有了姿态，就可以把加速度计的比力从载体坐标系转换到导航坐标系，然后扣除重力，再积分得到速度和位置。
公式很简单：
- 速度更新：v_new = v_old + (C * a_measured - g) * dt
- 位置更新：p_new = p_old + v_avg * dt
其中C是姿态矩阵，把载体坐标系的加速度转到导航坐标系。这里有个细节——速度更新时，建议用梯形积分，比矩形积分更准。
4.3 IMU误差模型
IMU不是完美的。它有一堆误差，你得了解它们，才能在融合算法里补偿掉。
4.3.1 确定性误差
| 误差类型 | 描述 | 典型值（消费级） |
|---|---|---|
| 零偏（Bias） | 静止时输出不为0的常值偏移 | 加速度计：±50 mg 陀螺仪：±5 °/s |
| 比例因子（Scale Factor） | 输入输出比例不准确 | 0.1% ~ 1% |
| 轴间对准误差 | 三个轴不是严格正交 | 0.1° ~ 1° |
这些误差可以在实验室标定出来，然后做硬补偿。我建议每次上电后做一次静态初始化，估算零偏。
4.3.2 随机误差
随机误差才是真正头疼的。它们没法完全消除，只能用滤波算法去估计。
- 角度随机游走（ARW）：陀螺仪噪声积分后，角度误差像随机游走一样增长。单位是°/√h。
- 速度随机游走（VRW）：加速度计噪声积分后，速度误差增长。单位是m/s/√h。
- 零偏不稳定性（Bias Instability）：零偏随时间缓慢漂移，是惯导精度的核心指标。
避坑指南：我曾经在一个项目中，只关注了零偏，忽略了角度随机游走。结果纯惯导模式下，前10秒还行，30秒后角度误差就大到不可接受。后来换了低ARW的IMU，问题才解决。选型时，这两个指标都要看。
4.4 零偏与噪声特性
零偏和噪声，是IMU误差模型里最重要的两个概念。我单独拿出来讲，因为它们在融合算法里是状态量。
4.4.1 零偏的建模
零偏不是常数，它会缓慢变化。通常建模为一阶马尔可夫过程或随机游走。在卡尔曼滤波里，我们把零偏作为状态变量，实时估计和补偿。
零偏的典型特性：
- 上电重复性：每次上电的零偏可能不同
- 温度相关性：温度变化1°C，零偏可能漂移几mg
- 长期稳定性：好的IMU，零偏几个月漂移不超过0.1°/s
我的习惯：每次上电后，让车辆静止5秒钟，采集IMU数据取平均，作为初始零偏估计。虽然不完美，但比直接用出厂标定值好很多。尤其是在温度变化大的场景下。
4.4.2 噪声特性分析
IMU噪声通常用Allan方差来分析。你可以把IMU静止放几个小时，采集数据，然后画Allan方差曲线。从曲线上能读出ARW、零偏不稳定性、量化噪声等参数。
为什么要做这个？因为你要给卡尔曼滤波设置噪声协方差矩阵。设小了，滤波发散；设大了，滤波太保守，定位精度差。
// Allan方差计算简化流程
// 1. 采集N个静止数据点，采样率fs
// 2. 对不同的聚类时间tau = m/fs，计算方差
// 3. 在log-log图上，斜率-1/2处读出ARW
// 4. 曲线最低点读出零偏不稳定性
// 伪代码示意
for (int m = 1; m < N/2; m *= 2) {
double tau = m / fs;
double sum_var = 0;
for (int k = 0; k < N - 2*m; k += m) {
double theta1 = mean(data, k, k+m);
double theta2 = mean(data, k+m, k+2*m);
sum_var += (theta2 - theta1) * (theta2 - theta1);
}
allan_var[m] = sum_var / (2 * (N/m - 1));
}注意：Allan方差分析需要足够长的静态数据。我建议至少采集2小时。如果只采集几分钟，低频噪声特性根本看不出来。别问我怎么知道的——我当年就因为这个，把滤波器的噪声参数设错了，导致定位精度一直上不去。
4.5 本章小结
这一章我们聊了IMU的核心原理和误差特性。总结几个要点：
- 加速度计测比力，陀螺仪测角速度，两者都有零偏和噪声
- 捷联惯导的核心是姿态更新，推荐用四元数法
- IMU误差分确定性和随机性，随机误差用Allan方差分析
- 零偏是卡尔曼滤波里的重要状态量，需要实时估计
下一章，我们会把这些IMU数据和GPS、视觉等传感器融合起来。嗯，那才是真正有意思的部分。到时候见。