2. 惯性导航基础
在IMU预积分与因子图融合定位系统中，惯性测量单元（IMU）是整个状态估计链路的最前端传感器。理解IMU的物理测量原理、坐标系约定、姿态表示方法以及运动学方程，是后续推导预积分模型、构建因子图约束的数学前提。本章从传感器物理层出发，逐层建立从原始测量到姿态描述的完整认知框架。
一、IMU传感器原理与测量模型
典型的6轴IMU由三轴加速度计和三轴陀螺仪组成，分别测量载体在惯性空间中的比力（specific force）和角速度。加速度计的核心是MEMS质量块-弹簧结构，当载体存在加速度时，质量块因惯性产生位移，通过电容变化换算出比力值。陀螺仪则利用科里奥利力效应，测量绕三轴旋转的角速度。
需要特别强调的是，加速度计测量的并非纯粹的“运动加速度”，而是比力，即除去重力后的非引力外力加速度。其理想测量模型可表示为：
a_m = R_wb^T · (a_w - g_w) + b_a + n_a
ω_m = ω_b + b_g + n_g其中 a_m、ω_m 为加速度计和陀螺仪的原始测量值，R_wb 为世界系到机体系的旋转矩阵，g_w 为世界系重力向量，b_a、b_g 为加速度计和陀螺仪的零偏，n_a、n_g 为高斯白噪声。零偏是IMU误差的主要来源，也是预积分中必须在线估计的关键状态量。
二、坐标系定义与变换关系
惯性导航涉及多个坐标系的定义与转换，工程中必须严格区分：
- 惯性系（i系）：原点在地心，坐标轴指向恒星，牛顿力学定律严格成立。
- 世界系/导航系（w系或n系）：原点在载体初始位置，东北天（ENU）或北东地（NED）指向，重力方向固定，是融合定位的参考基准。
- 机体系（b系）：原点在IMU中心，前-右-下（FRD）或前-左-上（FLU）指向，与传感器固连。
- 相机系（c系）：原点在相机光心，z轴沿光轴方向，用于视觉惯性对齐。
各坐标系之间的变换由旋转矩阵 R 和平移向量 t 描述。机体系到世界系的变换 T_wb = [R_wb | t_wb] 即为待估计的位姿状态。IMU预积分的本质，就是在世界系下对机体系测量进行积分，从而建立两个关键帧之间的相对运动约束。
三、姿态表示方法：欧拉角、旋转矩阵与四元数
姿态表示是惯性导航的核心数学工具，三种主流方法各有优劣：
| 表示方法 | 参数个数 | 优点 | 缺点 | 典型用途 |
|---|---|---|---|---|
| 欧拉角 | 3 | 直观易读，便于人机交互 | 存在万向节死锁，非线性强 | 姿态显示、航向输出 |
| 旋转矩阵 | 9 | 线性变换，无奇异点 | 冗余参数，需正交约束 | 坐标变换、误差状态推导 |
| 四元数 | 4 | 紧凑无奇异，插值平滑 | 物理意义不直观，需归一化 | 预积分、因子图优化 |
在因子图优化中，四元数是最常用的姿态表示。其乘法更新规则为：
q_new = q_old ⊗ δq
δq ≈ [1, 0.5·δθ]^T其中 δθ 为姿态误差的小角度扰动。这种局部参数化方式避免了四元数模长约束，使优化问题转化为无约束最小二乘，是VINS-Mono、ORB-SLAM3等系统的标准做法。
四、惯性运动学方程与预积分思想
惯性导航的运动学方程描述了位姿、速度、零偏随时间的演化关系。在连续时间下，世界系中的运动学方程为：
ṗ_wb = v_wb
v̇_wb = R_wb · (a_m - b_a - n_a) + g_w
Ṙ_wb = R_wb · [ω_m - b_g - n_g]×
ḃ_a = n_ba
ḃ_g = n_bg其中 [·]× 表示反对称矩阵。直接对上述方程积分会带来两个问题：一是积分结果依赖初始状态，每次优化迭代都需重新积分；二是计算量随关键帧数量增长。预积分技术通过将测量积分到相对运动量中，使积分结果仅依赖零偏，从而大幅降低重复计算。
预积分量的定义为：
Δp_ij = ∫∫ R_ib · (a_m - b_a) dt²
Δv_ij = ∫ R_ib · (a_m - b_a) dt
Δq_ij = ∫ q_ib ⊗ [0, 0.5·(ω_m - b_g)] dt这三个预积分量构成了因子图中IMU约束的核心，与视觉观测约束共同参与位姿、速度、零偏的联合优化。
五、工程案例：从原始IMU到预积分因子的数据流
以VINS-Mono系统为例，IMU数据以200Hz频率进入系统，视觉帧率为30Hz。在相邻两个视觉关键帧之间，通常有6～7个IMU测量区间。系统首先对IMU测量进行预积分，得到 Δp_ij、Δv_ij、Δq_ij 及其对零偏的雅可比矩阵，然后构建IMU残差因子：
r_imu = [
r_p, r_v, r_q, r_ba, r_bg
]^T该残差与视觉重投影残差、边缘化先验残差共同构成因子图的目标函数，通过Ceres或GTSAM求解。整个过程中，坐标系定义的一致性、四元数更新的正确性、零偏雅可比的准确性，直接决定融合定位的精度和鲁棒性。
六、复盘与要点归纳
本节从IMU物理原理出发，建立了“传感器测量→坐标系变换→姿态表示→运动学方程→预积分”的完整知识链路。核心要点包括：加速度计测量的是比力而非纯加速度；世界系、机体系、相机系的定义必须严格统一；四元数是因子图优化中最优的姿态表示；预积分将高频测量转化为关键帧间的相对约束，是IMU与视觉融合的数学桥梁。掌握这些基础，才能正确推导预积分残差及其雅可比，为后续因子图构建打下坚实基础。