4. 惯性导航基础

在FAST-LIO中，IMU不是“辅助传感器”，而是状态传播的主引擎。激光点云提供观测，IMU提供帧间运动先验；若IMU传播模型不准确，后续点云配准会频繁失败。本节围绕四个核心问题展开：IMU测量模型如何定义、预积分如何避免重复积分、噪声与零偏如何建模、离散时间如何递推。

一、IMU测量模型与运动学方程

六轴IMU输出角速度ω̃和比力ã，真实值与测量值的关系为：

ω̃(t) = ω(t) + b_g(t) + n_g(t)
ã(t) = R(t)^T (a_w(t) - g_w) + b_a(t) + n_a(t)

其中b_g、b_a为陀螺与加速度计零偏，n_g、n_a为白噪声，R为机体到世界系的旋转，g_w为世界系重力。FAST-LIO通常把重力固定在状态中，避免与加速度计零偏耦合。运动学方程可写为：

ṗ = v
v̇ = R ã + g_w
Ṙ = R [ω̃]×
ḃ_g = n_bg,  ḃ_a = n_ba

这组方程是连续时间形式，实际计算必须离散化。注意：加速度计测量的是“比力”，不是纯加速度；静止时ã≈-g_w，这是判断IMU安装方向与重力对齐的关键。

二、预积分：把帧间积分从世界系解耦

若每来一帧激光就重新从上一帧状态积分，计算量会随帧数增长。预积分的核心思想是：在相邻激光帧k到k+1之间，仅用IMU测量值积分出相对运动增量Δp、Δv、ΔR，再与上一帧状态组合。这样，当上一帧状态被优化更新时，不需要重新积分IMU数据。

预积分量定义为：

ΔR_ij = ∏ R_i^T R_j
Δv_ij = R_i^T (v_j - v_i - g_w Δt_ij)
Δp_ij = R_i^T (p_j - p_i - v_i Δt_ij - ½ g_w Δt_ij²)

在FAST-LIO中，预积分结果直接作为状态传播的预测值，并与激光残差一起进入迭代卡尔曼滤波。预积分不是“可选项”，而是保证实时性的必要步骤。

三、噪声与零偏建模：从连续到离散

IMU噪声通常建模为高斯白噪声加随机游走零偏。连续时间下，陀螺与加速度计噪声密度为σ_g、σ_a，零偏随机游走密度为σ_bg、σ_ba。离散化时，若采样间隔为Δt，则离散噪声方差为：

σ_g,d² = σ_g² / Δt
σ_a,d² = σ_a² / Δt
σ_bg,d² = σ_bg² · Δt
σ_ba,d² = σ_ba² · Δt

很多初学者直接使用连续噪声密度作为离散方差，导致协方差被严重低估，滤波器过度自信，激光残差被压制。正确做法是：白噪声除以Δt，随机游走乘以Δt。FAST-LIO的配置文件通常给出这些参数，需与IMU数据手册对应。

四、离散时间传播：中值积分与协方差递推

FAST-LIO采用中值积分进行状态传播，比欧拉积分精度更高。设相邻IMU时刻为t_k与t_{k+1}，角速度与加速度取两端平均：

ω_mid = 0.5 * (ω_k + ω_{k+1})
a_mid = 0.5 * (a_k + a_{k+1})
R_{k+1} = R_k * Exp(ω_mid * Δt)
v_{k+1} = v_k + (R_k * a_mid + g_w) * Δt
p_{k+1} = p_k + v_k * Δt + 0.5 * (R_k * a_mid + g_w) * Δt²

协方差传播为P_{k+1} = F P_k F^T + Q，其中F为状态转移雅可比，Q为离散噪声协方差。FAST-LIO在流形上处理旋转，使用右扰动模型，避免欧拉角奇异。传播频率通常为IMU频率（100~200Hz），远高于激光帧率（10Hz），因此两次激光帧之间会执行多次传播。

五、案例与常见误区

某手持设备使用低成本IMU，静止时零偏约0.02 rad/s。若未估计零偏，10秒后姿态漂移约0.2 rad，激光配准会直接失败。FAST-LIO通过将零偏纳入状态向量，在激光观测下持续修正，使零偏收敛到0.001 rad/s以内。

常见误区包括：

• 把加速度计输出直接当加速度使用，忘记减去重力；
• 预积分时未固定重力方向，导致与零偏耦合；
• 离散噪声方差未按Δt缩放，协方差量级错误；
• IMU与激光时间戳未同步，预积分区间错位；
• 使用欧拉角传播旋转，在大角度下出现奇异。

掌握本节内容后，你应能独立配置IMU噪声参数、理解预积分在FAST-LIO中的位置，并判断传播异常是来自零偏、噪声还是时间同步。这是后续激光-惯性紧耦合优化的基础。