3. 惯性导航原理

在无GPS环境下，视觉惯性导航系统之所以能够持续输出姿态、速度和位置，核心依赖的是IMU（惯性测量单元）。IMU不依赖外部信号，只通过感知载体自身的角速度和比力来推算运动状态，因此它既是系统的高频“内感受器”，也是短时精度最高、长时漂移最明显的传感器。理解IMU的组成、测量模型、积分递推过程以及误差来源，是后续视觉与惯性融合的前提。

一、IMU的组成与测量对象

典型六轴IMU由三轴陀螺仪和三轴加速度计组成，二者通常正交安装在同一芯片或模块内。陀螺仪测量的是载体相对于惯性系的角速度，单位一般为rad/s或°/s；加速度计测量的是比力，即非引力加速度，单位常用m/s²或g。若IMU还集成三轴磁力计，则构成九轴MEMS单元，但磁力计易受电机、金属结构干扰，在无人机上通常只作为辅助。

需要特别区分两个概念：加速度计输出的不是“运动加速度”，而是比力。当无人机静止水平放置时，加速度计z轴输出约为+9.81 m/s²，这来自重力反作用，而不是无人机在向上加速。只有扣除重力分量后，才能得到真正的运动加速度。这个细节直接决定了惯性递推中重力补偿的正确性。

二、加速度计与陀螺仪的测量模型

理想情况下，陀螺仪输出角速度真值，加速度计输出比力真值。但真实MEMS器件存在零偏、噪声、尺度因子误差和轴间非正交误差。工程上常写成如下简化模型：

陀螺仪：  ω_meas = ω_true + b_g + S_g · ω_true + n_g
加速度计：f_meas = f_true + b_a + S_a · f_true + n_a

其中：
  b_g, b_a   ：零偏（bias），缓慢变化，可建模为随机游走
  S_g, S_a   ：尺度因子与轴间耦合矩阵
  n_g, n_a   ：白噪声，服从零均值高斯分布

零偏是惯性导航中最关键的误差项。它分为静态零偏和动态零偏：静态零偏可在开机时通过静止平均估计；动态零偏随温度和时间缓慢变化，需要在线估计。在VIO系统中，陀螺仪零偏通常与视觉观测联合优化，而加速度计零偏由于与重力耦合，可观测性较弱，需要载体充分运动才能有效估计。

三、积分递推：从角速度到位置

惯性递推的本质是“积分链”：陀螺仪积分得到姿态，姿态用于将比力旋转到世界系，扣除重力后积分得到速度，再积分得到位置。离散时间下，常用中值积分或龙格-库塔积分提高精度。以中值积分为例：

// 伪代码：IMU中值积分递推
for each IMU sample (ω, f, dt):
    // 1. 姿态更新
    ω_mid = 0.5 * (ω_prev + ω_curr)
    q = q ⊗ Exp(ω_mid * dt)
    R = quat2rot(q)

    // 2. 比力旋转并扣除重力
    f_mid = 0.5 * (f_prev + f_curr)
    a_w = R * f_mid + g_world

    // 3. 速度与位置更新
    v = v + a_w * dt
    p = p + v * dt + 0.5 * a_w * dt * dt

    // 4. 保存上一时刻量
    ω_prev = ω; f_prev = f

这段递推每毫秒执行一次，输出高频位姿预测。视觉惯性导航中，IMU预积分技术将两帧图像之间的多个IMU采样压缩为一个相对约束，避免在优化中重复积分，同时保留零偏可修正性。

四、漂移与误差来源

惯性导航的误差具有明显的“时间累积”特征。陀螺仪零偏导致姿态角随时间线性漂移；姿态误差又使重力方向投影错误，产生加速度误差；加速度误差积分后速度误差线性增长，位置误差则呈二次增长。因此，纯惯性导航在数秒到数十秒内就会发散，必须依靠视觉、气压计或GNSS进行修正。

五、案例：室内无人机悬停中的惯性漂移

假设一架室内无人机以200 Hz采集IMU，陀螺仪z轴零偏为0.5°/s。若不做任何修正，10秒后偏航角误差约为5°，30秒后约15°。偏航误差会进一步导致速度估计在水平方向旋转，位置轨迹逐渐弯曲。若加速度计x轴零偏为0.02 m/s²，10秒后速度误差约0.2 m/s，位置误差约1 m；30秒后位置误差可达9 m。这正是VIO必须引入视觉观测的原因：视觉提供绝对位置和姿态约束，将惯性漂移限制在可控范围内。

综上，IMU是视觉惯性导航的“高频骨架”，其测量模型、积分递推和误差特性决定了整个系统的短时精度与长期稳定性。掌握零偏建模、重力补偿和预积分思想，才能在无GPS环境中实现可靠的位姿估计。