从零推导FAST-LIO2：h_share_model中的数学之美与工程实践

从零推导FAST-LIO2：h_share_model中的数学之美与工程实践

在激光惯性里程计（LIO）领域，FAST-LIO2以其卓越的计算效率和稳定性成为众多研究者和工程师的首选方案。其核心创新之一在于巧妙地将迭代卡尔曼滤波（IKFOM）与紧耦合的激光-IMU融合相结合，而h_share_model函数正是这一融合过程中的数学精髓所在。这个函数不仅承载着观测模型的构建任务，更是一个将李群李代数理论、右扰动模型和工程实践完美结合的典范。

对于SLAM算法工程师和研究者而言，深入理解h_share_model的数学推导过程，就像是掌握了一把解开现代激光惯性里程计技术奥秘的钥匙。本文将带你从第一性原理出发，逐步推导观测方程对位移、旋转及外参的偏导数计算，揭示其中的数学之美和工程智慧。

1. FAST-LIO2系统框架与状态表示

FAST-LIO2采用基于IKFOM的滤波框架，其状态向量包含了位姿、速度、传感器偏置等关键参数。系统的状态表示如下：

state_ikfom {
    vect3 pos;          // 世界坐标系下的位置
    SO3 rot;            // 世界坐标系下的旋转（李群表示）
    SO3 offset_R_L_I;   // IMU到激光雷达的旋转外参
    vect3 offset_T_L_I; // IMU到激光雷达的平移外参
    vect3 vel;          // 速度矢量
    vect3 bg;           // 陀螺仪偏置
    vect3 ba;           // 加速度计偏置
    S2 grav;            // 重力向量（S2流形表示）
}

这种状态表示方法充分考虑了传感器外参的可辨识性，为后续的在线外参标定奠定了数学基础。

1.1 坐标系变换链理解

在FAST-LIO2中，点云数据需要经历一系列坐标系变换：

1. 激光雷达坐标系（Body系）：原始点云数据所在的坐标系
2. IMU坐标系：惯性测量单元所在的坐标系
3. 世界坐标系：全局固定的参考坐标系

坐标变换的数学表达为： $$ \mathbf{p}w = \mathbf{R}{w}^{i} \cdot (\mathbf{R}{i}^{l} \cdot \mathbf{p}l + \mathbf{t}{i}^{l}) + \mathbf{t}{w}^{i} $$

其中$\mathbf{p}w$是世界坐标系下的点，$\mathbf{p}l$是激光雷达坐标系下的点，$\mathbf{R}{i}^{l}$和$\mathbf{t}{i}^{l}$是激光雷达到IMU的外参。

2. 观测模型与残差函数构建

h_share_model函数的核心任务是构建点云到地图的匹配残差及其雅可比矩阵。这个过程可以分为三个主要步骤：点云变换、最近邻搜索和平面拟合。

2.1 点云坐标变换

首先需要将激光雷达坐标系下的点云转换到世界坐标系：

// 激光雷达系到IMU系的变换
V3D p_imu = s.offset_R_L_I * p_body + s.offset_T_L_I;

// IMU系到世界系的变换
V3D p_global = s.rot * p_imu + s.pos;