4. IMU惯性导航原理：加速度计与陀螺仪、捷联惯导解算、IMU误差模型

各位同学，今天我们来啃一块硬骨头——IMU惯性导航原理。说实话，我刚入行那会儿，觉得惯导这东西玄乎得很，一个加速度计加一个陀螺仪，怎么就能算出位置了呢？后来亲手调过几套系统，踩过坑，才慢慢摸清了门道。这一章，我就把核心原理掰开了讲给你听。

4.1 加速度计与陀螺仪：IMU的两只眼睛

IMU的核心传感器就两个：加速度计和陀螺仪。加速度计测的是比力，说白了就是物体受到的惯性力与重力的合力。陀螺仪测的是角速度，也就是物体转动的快慢。

加速度计的工作原理

加速度计内部有个质量块，当载体加速时，质量块会因惯性产生位移。通过检测这个位移，就能反推出加速度。我常用的MEMS加速度计，输出的是数字信号，直接读寄存器就行。

陀螺仪的工作原理

陀螺仪利用科里奥利效应来测量角速度。MEMS陀螺仪内部有振动的质量块，当载体旋转时，质量块会受到科里奥利力，产生垂直于振动方向的位移。检测这个位移，就能得到角速度。

我记得第一次用陀螺仪积分算角度，发现漂移得厉害。后来才明白，陀螺仪的零偏误差会随时间累积，这就是为什么纯惯导系统需要其他传感器辅助。

4.2 捷联惯导解算：从原始数据到位置姿态

捷联惯导，说白了就是把IMU直接固定在载体上，通过数学计算来模拟一个虚拟的导航平台。解算过程分三步：姿态更新、速度更新、位置更新。

姿态更新

姿态更新是核心中的核心。我们用陀螺仪的角速度数据，通过四元数或方向余弦矩阵来更新姿态。我个人习惯用四元数，因为它没有欧拉角的万向锁问题，计算量也小。

// 四元数姿态更新示例
void updateAttitude(gyro_data_t *gyro, quaternion_t *q, float dt) {
    float wx = gyro->x * dt / 2.0f;
    float wy = gyro->y * dt / 2.0f;
    float wz = gyro->z * dt / 2.0f;
    
    // 四元数微分方程
    quaternion_t dq;
    dq.w = -q->x * wx - q->y * wy - q->z * wz;
    dq.x =  q->w * wx + q->y * wz - q->z * wy;
    dq.y =  q->w * wy - q->x * wz + q->z * wx;
    dq.z =  q->w * wz + q->x * wy - q->y * wx;
    
    // 更新四元数
    q->w += dq.w;
    q->x += dq.x;
    q->y += dq.y;
    q->z += dq.z;
    
    // 归一化
    float norm = sqrt(q->w*q->w + q->x*q->x + q->y*q->y + q->z*q->z);
    q->w /= norm;
    q->x /= norm;
    q->y /= norm;
    q->z /= norm;
}

速度更新

速度更新需要把加速度计的比力转换到导航坐标系，然后减去重力，再积分。这里有个坑：加速度计数据必须经过姿态矩阵转换，否则算出来的速度全是错的。

位置更新

位置更新最简单，对速度积分就行。但要注意，位置误差会随时间二次方增长，这就是为什么纯惯导系统几分钟就飞了。

4.3 IMU误差模型：零偏、比例因子、随机游走

IMU不是完美的，它有各种误差。理解这些误差，是调优的基础。我把常见的误差模型总结成了一张图。

零偏（Bias）

零偏是IMU输出中不随运动变化的常值误差。说白了，就是传感器静止时，输出不为零。加速度计零偏会导致速度误差随时间线性增长，陀螺仪零偏会导致姿态误差随时间线性增长。

比例因子（Scale Factor）

比例因子是输入输出之间的比例关系偏差。比如你转90度，陀螺仪输出可能只有89.5度。这个误差在高速运动时特别明显。

随机游走（Random Walk）

随机游走是噪声积分的结果。角度随机游走（ARW）是陀螺仪白噪声积分后产生的角度误差，速度随机游走（VRW）是加速度计白噪声积分后产生的速度误差。

4.4 误差补偿实战：标定与滤波

知道了误差模型，接下来就是怎么补偿。我一般分两步走：离线标定和在线滤波。

离线标定

标定就是确定误差参数。对于加速度计，我用六位置法：把IMU分别朝六个方向放置，记录输出，然后解算出零偏和比例因子。对于陀螺仪，我用速率法：把IMU放在转台上，以不同角速度旋转，记录输出。

标定项目	方法	所需设备	精度要求
加速度计零偏	六位置法	水平台面	±0.1 mg
加速度计比例因子	六位置法	水平台面	±0.1%
陀螺仪零偏	静态采集	静止基座	±0.01 °/s
陀螺仪比例因子	速率法	转台	±0.05%

在线滤波

标定只能补偿确定性误差，随机误差还得靠滤波。我常用的方法是卡尔曼滤波，把IMU误差作为状态量，和导航状态一起估计。

// 卡尔曼滤波中的IMU误差状态
typedef struct {
    float gyro_bias[3];      // 陀螺仪零偏
    float accel_bias[3];     // 加速度计零偏
    float gyro_scale[3];     // 陀螺仪比例因子
    float accel_scale[3];    // 加速度计比例因子
} imu_error_state_t;

好了，这一章的内容就到这里。IMU惯导原理是融合定位的基石，理解透了，后面的组合导航才能玩得转。记住：误差模型是调优的起点，标定是基础，滤波是手段。三者缺一不可。

可可精品资料库