【LOAM系列】八：Fast-Lio2论文笔记

最新推荐文章于 2026-07-07 16:28:58 发布

原创

最新推荐文章于 2026-07-07 16:28:58 发布 · 1.3k 阅读

FAST-LIO2是基于误差状态的迭代卡尔曼滤波的雷达-惯性里程计系统，不依赖人工特征，使用ikd-tree进行增量更新和定位。它改进了运动模型和测量模型，实现了高频率定位，并通过ikd-tree优化地图存储和搜索效率，适用于动态和复杂的环境。该方法还包括有效的错误状态更新和卡尔曼增益计算，确保了系统的精度和稳定性。

Fast-Lio2

2022 8 IEEE Transactions on Robotics
Wei Xu; Yixi Cai; Dongjiao He; Jiarong Lin; Fu Zhang 港大火星实验室

论文

FAST-LIO2: Fast Direct LiDAR-Inertial Odometry

1.1 系统框架

紧耦合迭代误差卡尔曼滤波，不提取特征（人工设计的特征对于不雷达的适配性能不同，也容易受环境影响），使用ikd-tree支持增量更新，雷达数据的一次scan可以是10ms~100ms之间，因此定位频率可以修改
在这里插入图片描述

1.2 LIO

与Fast-Lio一样，基于误差状态的迭代卡尔曼滤波，当时增加了外参的估计
$KaTeX parse error: Got group of unknown type: 'internal'$

运动模型

对应着IMU预积分与协方差矩阵传播
$\begin{equation*} \begin{aligned} \mathbf x_{i+1} &= \mathbf x_{i} \boxplus \left(\Delta t \mathbf f(\mathbf x_i, \mathbf u_i, \mathbf w_i) \right) \end{aligned} \tag{4} \end{equation*}$

$KaTeX parse error: Got group of unknown type: 'internal'$

测量模型

运动补偿每个点投影到扫描结束时刻，在投影到全局地图与最近邻点拟合的平面计算残差(这里残差等于0是因为雷达系系的点减去了噪声的影响，因此就是真实值)
$\begin{equation*} \begin{split}\; \mathbf 0 = {}^G \mathbf u_j^T \left({}^G {\mathbf T}_{I_k} {}^{I}{\mathbf T}_{L} \left({}^{L}{\mathbf p}_{j} + {}^{L}{\mathbf n}_{j} \right) - {}^G\mathbf q_j \right) \end{split} \tag{8} \end{equation*}$
对噪声（误差状态）的观测方程
$\begin{equation*} \begin{split}\; \mathbf 0 \! = \! \mathbf h_j\!\left({\mathbf x}_k, \! {}^{L}{\mathbf n}_j \right) \triangleq {}^G \mathbf u_j^T \!\!\left({}^G {\mathbf T}_{I_k}\! {}^{I}{\mathbf T}_{L} ({}^{L}{\mathbf p}_{j} \!+\! {}^{L}{\mathbf n}_{j}) \!- \!{}^G\mathbf q_j \right) \end{split} \tag{9} \end{equation*}$