第三章 惯性导航系统(INS)原理：加速度计与陀螺仪工作原理、捷联惯导解算

各位同学，欢迎来到第三章。这一章咱们要啃的，是整个组合导航系统里最核心、也最“硬”的一块——惯性导航系统，简称INS。

说实话，我刚开始接触惯导那会儿，觉得这东西挺玄乎的。一个盒子，里面几个传感器，就能算出你从哪儿来、到哪儿去、姿态怎么样？后来自己动手搭过几套系统，踩过不少坑，才慢慢摸透了它的脾气。今天我就把这些年积累的经验，掰开了揉碎了讲给你们听。

3.1 加速度计工作原理

加速度计，说白了就是测量“比力”的传感器。什么叫比力？就是除了重力以外的力，作用在单位质量上产生的加速度。

你可能会问：“那重力呢？”嗯，这里有个关键点——加速度计测不到重力。它测的是物体受到的惯性力。比如你站在地面上，加速度计感受到的是地面给你的支撑力，这个力正好抵消了重力。所以静止时，加速度计输出的是1g（重力加速度）向上。

我当年第一次做实验，把IMU平放在桌上，看到Z轴输出9.8 m/s²，还以为是传感器坏了……后来才明白，这就是比力的概念。

常见的加速度计有几种原理：

• 电容式：利用质量块位移改变电容值。便宜、功耗低，消费级产品常用。
• 压阻式：利用硅微结构受力变形，电阻变化。响应快，但温漂大。
• 谐振式：利用谐振频率变化测加速度。精度高，但贵。

实际项目中，我建议你重点关注两个指标：零偏稳定性和噪声密度。零偏决定了你静止时读数的稳定性，噪声密度则影响你积分后的速度误差。

a_measured = a_true + b_a + n_a + S_a * a_true + ...

3.2 陀螺仪工作原理

陀螺仪测量的是角速度。注意，不是角度，是角速度。要得到角度，你得积分。

传统的机械陀螺靠高速旋转的转子来保持方向稳定，但那个东西又大又贵。现在咱们用的基本都是MEMS陀螺，微机电系统，指甲盖大小。

MEMS陀螺的原理其实挺有意思——它利用科里奥利效应。简单说，就是一个质量块在振动，如果系统在旋转，质量块会受到一个垂直于振动方向和旋转轴方向的力。测这个力的大小，就能算出角速度。

我记得有一次在无人机上做测试，陀螺数据突然跳变，飞机差点翻过去。排查了半天，发现是电机振动频率和陀螺的谐振频率接近了，产生了共振。从那以后，我每次选型都会仔细看陀螺的带宽和抗振性能。

陀螺仪的几个关键参数：

• 量程：一般±250°/s到±2000°/s。无人机用±500°/s就够了，汽车用可能需要更大。
• 零偏不稳定性：这个指标直接决定了你能保持姿态精度多久。好的工业级陀螺能做到0.1°/h以下。
• 角度随机游走：积分后角度误差随时间平方根增长。说白了，时间越长，角度越不准。

3.3 捷联惯导解算——姿态更新

好，传感器原理讲完了，接下来是重头戏——怎么用这些数据算出姿态、速度和位置。

捷联惯导，英文叫Strapdown INS。意思是传感器“绑”在载体上，跟着一起动。不像平台式惯导那样有个物理平台保持水平。

姿态更新，说白了就是解微分方程：

q_dot = 0.5 * q ⊗ ω

其中 q 是四元数，ω 是角速度。⊗ 表示四元数乘法。

实际代码里，我一般用四元数，因为它没有欧拉角的万向锁问题，计算量也比方向余弦矩阵小。

姿态更新的步骤：

1. 读取陀螺仪当前角速度 ω
2. 计算角增量 Δθ = ω * dt
3. 用四元数更新公式：q_new = q_old ⊗ [cos(Δθ/2), (Δθ/Δθ)*sin(Δθ/2)]
4. 归一化四元数，防止累积误差

void attitude_update(quat_t *q, float gx, float gy, float gz, float dt) {
    float half_dt = 0.5f * dt;
    float dq[4];
    
    // 计算角增量
    float dtheta = sqrtf(gx*gx + gy*gy + gz*gz) * half_dt;
    
    if (dtheta > 1e-6f) {
        float sin_half = sinf(dtheta) / dtheta;
        dq[0] = cosf(dtheta);
        dq[1] = gx * half_dt * sin_half;
        dq[2] = gy * half_dt * sin_half;
        dq[3] = gz * half_dt * sin_half;
    } else {
        // 小角度近似
        dq[0] = 1.0f;
        dq[1] = gx * half_dt;
        dq[2] = gy * half_dt;
        dq[3] = gz * half_dt;
    }
    
    // 四元数乘法
    quat_multiply(q, dq, q);
    
    // 归一化
    quat_normalize(q);
}

这里有个坑——小角度近似。当角速度很小时，sin(θ) ≈ θ，可以省去三角函数计算。但如果你在高速旋转的场景下还用这个近似，姿态误差会迅速累积。我建议设个阈值，超过阈值就用精确计算。

3.4 速度更新

速度更新，核心就是积分加速度，但要扣除重力。

公式很简单：

v_new = v_old + (R * a_imu - g) * dt

其中 R 是姿态旋转矩阵，把加速度从机体坐标系转到导航坐标系。a_imu 是加速度计读数。g 是重力向量。

你可能会问：“为什么要把加速度转到导航系？”因为重力方向是导航系下的，你得在同一个坐标系里做减法。

实际项目中，我见过不少人直接对加速度计读数积分，结果速度越飞越快，位置飞到了天边。原因就是没扣除重力。记住，加速度计测的是比力，不是真正的运动加速度。

3.5 位置更新

位置更新最简单，就是对速度积分：

p_new = p_old + v * dt

但这里有个细节——地球曲率。如果你跑的距离很短（几百米以内），可以忽略。但如果是长距离导航，就得考虑地球是圆的。

考虑地球曲率时，位置更新公式变成：

lat_new = lat_old + v_north / (R_e + h) * dt
lon_new = lon_old + v_east / ((R_e + h) * cos(lat)) * dt
h_new = h_old + v_up * dt

其中 R_e 是地球半径，约6371km。h 是高度。

你看，经度更新公式里分母有个 cos(lat)。这意味着在极地附近，cos(lat)趋近于0，经度变化会变得非常敏感。这也是为什么极地导航是个难题。

3.6 惯导解算的误差特性

最后，咱们聊聊惯导的“命门”——误差随时间累积。

惯导解算的误差有三个主要来源：

• 初始对准误差：初始姿态、速度、位置不准，后面全偏。
• 传感器误差：零偏、噪声、刻度因子等。
• 算法误差：离散化误差、截断误差等。

这些误差会怎么传播？

你看，陀螺零偏对位置的影响是时间的三次方！这就是为什么纯惯导系统几分钟就飞了。必须用GPS、视觉或其他传感器来修正。

好了，这一章的内容就到这儿。惯导原理是组合导航的基石，后面讲卡尔曼滤波、GPS/INS融合时，你会反复用到这些知识。下一章，咱们聊聊视觉里程计——另一种在GPS拒止环境下非常有用的导航手段。