3、INS基础回顾：IMU工作原理、加速度计与陀螺仪、惯性导航解算（姿态/速度/位置）、IMU误差模型
各位同学，咱们今天聊聊INS的基础。说白了，就是惯性导航系统那点事儿。很多做组合导航的朋友，一上来就搞卡尔曼滤波，结果IMU的底子没打牢，后面全是坑。我当年刚入行时也吃过这个亏，所以今天咱们把基础夯实。
3.1 IMU工作原理：它到底在测什么？
IMU，惯性测量单元。它不依赖任何外部信号，自己闷头测加速度和角速度。你想想看，这玩意儿装在导弹上、无人机上、汽车上，原理都一样。
核心就两个传感器：
- 加速度计：测比力（Specific Force），单位是 m/s²。注意，它测的不是重力，是「载体受到的合力减去重力」。
- 陀螺仪：测角速度，单位是 rad/s 或 °/s。它告诉你载体转得有多快。
我在项目里遇到过一个问题：有同事把加速度计输出的数据直接当速度积分用，结果发散得一塌糊涂。为什么？因为加速度计输出的是「比力」，你得先把重力补偿掉，才能得到真正的运动加速度。
重要概念：加速度计输出 = 运动加速度 + 重力加速度（在导航系下）。积分前必须扣除重力分量。
3.2 加速度计与陀螺仪：两种传感器的脾气
这两种传感器，性格完全不同。我习惯把它们比作「急性子」和「慢性子」。
加速度计：
- 低频特性好，长期稳定。你把它放桌上不动，它输出很稳。
- 但高频噪声大，一有振动就跳。
- 可以用来算俯仰角和横滚角（水平姿态），但算不了航向。
陀螺仪：
- 高频特性好，动态响应快。你猛转一下，它立刻有反应。
- 但低频漂移严重，放着不动它也会慢慢飘。
- 可以用来算航向，但需要积分，误差会累积。
嗯，这里要注意：加速度计不能直接测航向，因为重力方向在水平面内没有投影。你拿手机转个圈，加速度计读数几乎不变。所以航向只能靠陀螺仪积分或者磁力计来辅助。
个人经验：我曾经在一个车载项目里，只用加速度计做姿态估计，结果车一上坡，俯仰角就乱跳。后来加了陀螺仪做互补滤波，才稳住。说白了，单传感器靠不住，必须融合。
3.3 惯性导航解算：姿态、速度、位置
这部分是核心。惯导解算，说白了就是「积分三部曲」：
- 从角速度积分得到姿态
- 从加速度（扣除重力后）积分得到速度
- 从速度积分得到位置
咱们一步步来。
3.3.1 姿态解算
姿态怎么表示？常见的有三种：欧拉角、旋转矩阵、四元数。我个人最推荐四元数，原因很简单——没有万向锁，计算也快。
四元数微分方程：
dq/dt = 0.5 * q ⊗ ω其中 ω 是陀螺仪测得的角速度，⊗ 表示四元数乘法。
实际代码里，我们一般用一阶龙格-库塔法来更新：
// 四元数更新（简化版）
void quaternion_update(Quaternion *q, float gx, float gy, float gz, float dt) {
float q0 = q->w, q1 = q->x, q2 = q->y, q3 = q->z;
float dq0 = 0.5f * (-q1*gx - q2*gy - q3*gz);
float dq1 = 0.5f * ( q0*gx + q2*gz - q3*gy);
float dq2 = 0.5f * ( q0*gy - q1*gz + q3*gx);
float dq3 = 0.5f * ( q0*gz + q1*gy - q2*gx);
q->w += dq0 * dt;
q->x += dq1 * dt;
q->y += dq2 * dt;
q->z += dq3 * dt;
// 别忘了归一化
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;
}警告：四元数更新后必须归一化！我见过有人忘了这步，结果姿态越算越歪，最后直接发散。归一化不是可选项，是必须项。
3.3.2 速度解算
速度更新相对简单。先把加速度计输出的比力转换到导航系，然后扣除重力：
// 速度更新（导航系下）
void velocity_update(float *v_n, float *v_e, float *v_d,
float ax, float ay, float az,
Quaternion *q, float dt) {
// 将加速度从载体系转到导航系
float a_n[3];
rotate_body_to_nav(ax, ay, az, q, a_n);
// 扣除重力（导航系下重力为 [0, 0, g]）
a_n[2] -= GRAVITY;
// 积分
v_n[0] += a_n[0] * dt;
v_n[1] += a_n[1] * dt;
v_n[2] += a_n[2] * dt;
}你想想看，如果姿态算错了，那旋转矩阵就错了，加速度转换到导航系也会错。这就是为什么姿态是惯导解算的「牛鼻子」——姿态一错，速度和位置全完蛋。
3.3.3 位置解算
位置更新就是速度的积分，没什么好说的。但要注意，位置一般用经纬高表示，不是直角坐标。所以积分时需要考虑地球曲率：
// 位置更新（经纬高）
void position_update(double *lat, double *lon, double *alt,
float v_n, float v_e, float v_d, float dt) {
double Rm = 6356752.0; // 子午圈曲率半径
double Rn = 6378137.0; // 卯酉圈曲率半径
*lat += (v_n / Rm) * dt;
*lon += (v_e / (Rn * cos(*lat))) * dt;
*alt += (-v_d) * dt; // 注意：v_d 是向下为正
}避坑指南：我曾经在低纬度地区跑得好好的，到了高纬度，经度更新直接炸了。为什么？因为 cos(lat) 接近0，分母太小，数值不稳定。后来加了纬度阈值判断，才解决。
3.4 IMU误差模型：为什么纯惯导会发散？
纯惯导为什么不能长时间用？因为误差会累积。IMU的误差模型，说白了就是搞清楚「误差从哪来，怎么传」。
主要误差源：
| 误差类型 | 来源 | 影响 |
|---|---|---|
| 零偏（Bias） | 传感器制造偏差、温度变化 | 姿态、速度、位置随时间线性漂移 |
| 比例因子（Scale Factor） | 标定不准确 | 与运动大小相关的误差 |
| 交轴耦合（Misalignment） | 安装角度偏差 | 一个轴的运动耦合到另一个轴 |
| 随机游走（Random Walk） | 噪声积分 | 位置误差随时间平方根增长 |
其中，零偏是最要命的。陀螺仪的零偏会导致姿态误差，姿态误差又会导致加速度积分错误，最终位置误差随时间的三次方增长！
我习惯用一阶马尔可夫过程来建模零偏：
// 零偏模型（简化）
bias_k = bias_k-1 * exp(-dt / tau) + w_k其中 tau 是相关时间，w_k 是高斯白噪声。
核心结论：纯惯导的位置误差随时间的三次方增长。这就是为什么必须用RTK来修正。紧耦合的目的，就是用RTK的位置/速度观测，来估计并补偿IMU的误差。
3.5 本章知识体系图
下面这张图，是我自己总结的惯导解算流程。你把它记牢了，后面讲紧耦合就好理解。
这张图里，最关键的其实是那条虚线——误差反馈。紧耦合的本质，就是用RTK的观测值来估计IMU的误差，然后补偿回去。没有这条虚线，纯惯导就是开环积分，迟早发散。
个人习惯：我在做紧耦合方案时，会把IMU误差模型的状态量（零偏、比例因子）直接放进卡尔曼滤波的状态向量里。这样RTK每来一个观测，就能同时修正导航状态和IMU误差。说白了，就是让RTK帮IMU「擦屁股」。
好了，IMU的基础就回顾到这里。记住三个核心：姿态是基础，误差会累积，紧耦合就是靠外部观测来抑制发散。下一节咱们就正式进入紧耦合的核心——如何设计状态方程和观测方程。
公众号:蓝海资料掘金营，微信deep3321