第2章惯性导航系统(INS)基础

第2章：惯性导航系统(INS)基础：IMU工作原理、加速度计与陀螺仪、惯性导航解算原理

各位同学，欢迎来到第二章。上一章我们聊了组合导航的整体框架，这一章咱们得把地基打牢——惯性导航系统（INS）到底是怎么工作的？

说实话，我刚入行那会儿，觉得INS就是个黑盒子，给个加速度和角速度就能算出位置，挺神奇的。后来自己动手写过一次捷联惯导解算程序，才发现里面门道不少。今天我就把这些年积累的经验，掰开了揉碎了讲给你听。

2.1 IMU工作原理——它到底在测什么？

IMU，全称Inertial Measurement Unit，惯性测量单元。说白了，它就是用来测量物体运动状态的传感器组合。

一个典型的IMU包含三个加速度计和三个陀螺仪，分别对应三个轴向。为什么是三个？因为我们的运动是三维的，前后、左右、上下，再加上绕这三个轴的旋转，一共六个自由度。

我习惯把IMU比作一个“运动记录仪”。它不依赖任何外部信号，只靠内部传感器感知自身的运动。这就是它最牛的地方——完全自主，不受干扰。

核心要点：IMU测量的是比力（Specific Force）和角速度，不是直接的位置或速度。你需要通过积分才能得到导航信息。

2.2 加速度计——感受“力”的传感器

加速度计测量的是比力，不是我们通常理解的加速度。这个区别很重要，我当年就栽过跟头。

比力 = 物体受到的合力 - 重力加速度

举个例子：你站在地面上不动，加速度计测到的比力是多少？是0吗？不是。它测到的是向上的支撑力，大小等于重力加速度g。所以静止时，加速度计输出的是[0, 0, g]（假设z轴向上）。

为什么会这样？因为加速度计的工作原理是检测质量块的位移。当物体静止时，质量块受到重力向下拉，但壳体给它一个向上的支撑力，质量块相对壳体有位移，传感器就输出了g。

我的经验：在初始化INS时，一定要利用静止时的加速度计读数来估算初始姿态。具体做法是：取几秒钟的静态数据平均，然后通过三角函数反算出俯仰角和横滚角。这个方法简单可靠，我在多个项目中都用过。

加速度计的主要误差源包括：

零偏（Bias）：即使没有输入，传感器也有输出。我见过最差的MEMS加速度计零偏能到几十mg。
刻度因子误差：实际输出与理想输出的比例偏差。
噪声：白噪声和随机游走，积分后会累积成速度误差。

2.3 陀螺仪——测量旋转的利器

陀螺仪测量的是角速度，单位通常是度/秒或弧度/秒。它告诉你物体转得有多快，往哪个方向转。

传统的光纤陀螺和激光陀螺精度很高，但体积大、成本高。现在消费级产品多用MEMS陀螺，虽然精度差一些，但胜在小巧便宜。

我记得有一次做无人机项目，用的MEMS陀螺零偏稳定性只有10度/小时。这意味着什么？如果不做修正，一小时后姿态误差就能漂到10度。对于需要精确控制的无人机来说，这完全不可接受。所以我们后来加了磁力计和GPS来辅助。

陀螺仪的关键指标：

指标	说明	典型值（MEMS）
零偏稳定性	长时间静止时输出的波动	1-100 deg/h
角度随机游走	积分后角度误差的随机增长	0.1-1 deg/√h
带宽	能响应的最高频率	100-1000 Hz

避坑指南：我曾经犯过一个低级错误——直接用陀螺仪的原始角速度积分得到角度。结果发现角度误差越来越大。后来才意识到，陀螺仪有零偏，积分前必须先扣除零偏。否则，一个0.1 deg/s的零偏，积分10秒就产生1度的误差。

2.4 惯性导航解算原理——从IMU到导航信息

有了加速度计和陀螺仪的数据，怎么得到姿态、速度、位置？这就是惯性导航解算要干的事。

整个解算过程可以概括为三个步骤：

姿态更新：用陀螺仪角速度更新姿态矩阵
速度更新：用加速度计比力去除重力后积分得到速度
位置更新：对速度积分得到位置

2.4.1 姿态更新

姿态是惯导解算的核心。姿态错了，后面的速度和位置全错。

常用的姿态表示方法有欧拉角、方向余弦矩阵、四元数。我个人最喜欢用四元数，因为它没有万向锁问题，计算也简单。

四元数更新公式：

q_new = q_old + 0.5 * q_old ⊗ ω * dt

其中ω是角速度向量，⊗表示四元数乘法，dt是采样时间间隔。

嗯，这里要注意：四元数更新后一定要归一化，否则数值误差会累积，导致姿态矩阵不正交。我见过有人忘了这步，结果姿态越算越离谱。

2.4.2 速度更新

速度更新相对直接：

v_new = v_old + (C * f - g) * dt

其中C是姿态矩阵，f是加速度计测到的比力，g是重力加速度。

你想想看，这一步的关键在于：必须用姿态矩阵把比力从载体坐标系转换到导航坐标系，然后减去重力，才能得到真正的加速度。

重要提醒：重力补偿一定要做准确。地球重力随纬度和高度变化，如果你做高精度导航，建议用WGS-84模型计算当地重力值。我见过有人直接用9.8 m/s²，结果在赤道附近误差就大了。

2.4.3 位置更新

位置更新就是对速度积分：

p_new = p_old + v * dt

如果是经纬度表示，还需要考虑地球曲率，用更复杂的公式。

说白了，惯导解算就是不断积分、积分、再积分。每个环节的误差都会累积，这就是为什么纯惯导系统长时间工作精度会下降。

2.5 一个完整的解算流程示例

下面我给一个简化的C语言风格伪代码，展示一个解算周期内要做的事：

// 一个解算周期
void ins_update(imu_data_t *imu, nav_state_t *nav, float dt) {
    // 1. 读取IMU数据
    float wx = imu->gyro_x;   // 角速度
    float wy = imu->gyro_y;
    float wz = imu->gyro_z;
    float fx = imu->accel_x;  // 比力
    float fy = imu->accel_y;
    float fz = imu->accel_z;
    
    // 2. 姿态更新（四元数法）
    quaternion_t q = nav->q;
    quaternion_t dq = quaternion_from_gyro(wx, wy, wz, dt);
    q = quaternion_multiply(q, dq);
    quaternion_normalize(&q);
    nav->q = q;
    
    // 3. 比力坐标转换
    float fn[3];
    attitude_transform(q, fx, fy, fz, fn);
    
    // 4. 速度更新（减去重力）
    nav->vn += (fn[0] - 0) * dt;      // 北向
    nav->ve += (fn[1] - 0) * dt;      // 东向
    nav->vd += (fn[2] - GRAVITY) * dt; // 地向
    
    // 5. 位置更新
    nav->lat += nav->vn / R_EARTH * dt;
    nav->lon += nav->ve / (R_EARTH * cos(nav->lat)) * dt;
    nav->alt -= nav->vd * dt;
}

这段代码虽然简单，但包含了惯导解算的核心逻辑。实际工程中还要考虑地球自转、科里奥利力、圆锥误差补偿等，但基本原理不变。

2.6 本章小结

这一章我们讲了：

IMU测量比力和角速度，不是直接的位置速度
加速度计静止时测到的是重力，可以用来初始化姿态
陀螺仪有零偏，积分前必须补偿
惯导解算三步走：姿态→速度→位置，每一步都是积分
误差会累积，所以纯惯导需要外部辅助

下一章我们会讲GPS的基本原理，然后就可以进入正题——松耦合和紧耦合架构设计了。到时候你会发现，理解了INS的基础，组合导航其实没那么神秘。

好，今天就到这里。有什么问题欢迎在课程群里讨论，或者直接找我。咱们下章见。

📚 课程目录