FAST - LIO 系列性能发展：ES - IEKF 与 iVox 的更迭探索

AST-LIO系列的演进历程本质上是一场从“精度优先”到“速度与精度并重”的思维范式转换。当LOAM系方法在特征提取与扫描匹配的框架下将精度推向天花板后FAST-LIO系列的三代迭代清晰地揭示了另一条技术主线算法效率正成为新的战场。从FAST-LIO的ES-IEKF静态树到FAST-LIO2的ikd-Tree原始点云直配再到Faster-LIO的iVox哈希查询每一次跨越都是滤波理论与工程实现的深度融合。一、FAST-LIO流形上的迭代卡尔曼滤波与反向传播FAST-LIO的核心算法突破在于将迭代扩展卡尔曼滤波IEKF引入激光-惯性紧耦合框架并将其完整地运行在SO(3)流形上。与传统EKF不同IEKF在每一帧数据到来时反复迭代计算每次都用更新后的状态重新线性化观测模型直到估计的变化量小于阈值后收敛从而有效缓解了激光雷达观测模型的非线性问题。状态向量中的旋转部分不采用欧拉角而是用四元数搭配旋转向量所有更新都通过李代数运算so3::exp和so3::log完成。这种流形上的处理方式不仅彻底规避了万向节锁问题还使IMU预积分与激光匹配的误差传递更加干净利落。系统还设计了动态迭代机制根据残差收敛情况自动控制迭代次数——通常2~4次即可收敛在强运动场景中会主动增加到6次确保非线性误差被压制在毫米级的同时保持实时性。在运动补偿层面FAST-LIO用反向传播策略取代了LOAM系的匀速假设。传统方法在一帧激光数据采集的约100毫秒内假设匀速运动这在快速或非匀速运动中会引入显著畸变。FAST-LIO的做法是利用高频IMU数据200-500Hz从扫描结束时刻反向积分到每个激光点的采样时刻从而为每一个点计算出精确的位姿变换从根本上解决了运动畸变问题。具体的两阶段流程为首先将IMU状态前向传播然后对每个激光点执行反向变换将其统一到帧末坐标系。二、FAST-LIO2ikd-Tree与原始点云直配FAST-LIO2对前作进行了两项革命性改进。第一项是从特征提取转向原始点云直配。FAST-LIO需要先从点云中提取边缘点和平面点进行配准而FAST-LIO2直接拿原始点云与地图匹配。这一转变使系统能够充分利用环境中的细微几何特征无需依赖人工设计的特征提取模块也自然适应不同扫描模式的新兴激光雷达。第二项核心创新是ikd-Tree数据结构。传统静态KD-Tree在SLAM场景下面临致命缺陷随着新点插入和旧点删除树结构逐渐失衡全局重建成本极高且重建操作会阻塞查询线程。ikd-Tree通过精巧的节点设计解决了这一难题——每个节点存储了treedeleted、invalidnum等状态标记配合惰性删除策略在删除点时仅将其标记为删除而非物理移除搜索时自动跳过。只有当“已取走”的节点积累到一定程度时才触发局部子树的重建与再平衡且重建可以在后台进行而不阻塞搜索。这种设计使ikd-Tree在查询和更新整体效率上相比静态KD-Tree提升了约7-8倍。FAST-LIO2将IEKF从“全量迭代”改造为“增量迭代”——只更新被新点云影响的状态维度而非每次激光帧进来都重新计算全部雅可比矩阵。测试数据显示在Jetson AGX Orin上处理10Hz的Livox MID-360点云时FAST-LIO平均耗时87msFAST-LIO2压减至32ms。三、Faster-LIOiVox与哈希体素如果说FAST-LIO2是在树形结构上做增量优化那么Faster-LIO的选择则更加激进放弃树形结构转向体素。核心思想源于一个朴素判断LIO中的最近邻搜索是低维的、增量式的体素类结构天然比树类更适合这种场景。iVoxIncremental Voxel采用哈希表作为底层存储以空间坐标经过体素尺度量化为Key通过空间哈希函数生成唯一索引只在有点云存在的空间位置维护体素避免了对空体素的无效操作。点云配准时的最近邻搜索从树形遍历变为O(1)的哈希查找增和查的效率相比ikd-Tree有了质的提升。Faster-LIO提供了两种可选底层结构线性iVox适用于少量点的快速匹配场景PHC iVox利用伪希尔伯特曲线增强空间局部性适用于大量点的精细配准。性能数据令人印象深刻——在固态激光雷达如Livox Avia上iVox的处理速度达到1000-2000Hz在32线旋转激光雷达上超过200Hz相比FAST-LIO2提升了约1.5-2倍同时保持了相当的精度水平。四、演进脉络与工程启示纵观FAST-LIO系列的三代迭代一条清晰的演进脉络浮现出来FAST-LIO确立了ES-IEKF紧耦合的理论基础FAST-LIO2用ikd-Tree和原始点云直配突破了数据结构瓶颈Faster-LIO则以iVox将性能推向了极致。这一演进路径的深层启示在于当滤波理论趋于成熟后算法效率的主战场已转移到数据结构的设计上。从树形结构的增量式维护到体素结构的哈希索引每一次数据结构层面的突破都带来了一个数量级的性能跃迁。FAST-LIO2提出ikd-Tree时解决了“动态插入删除”问题Faster-LIO则进一步发现树形结构本身就是“过度设计”——LIO场景不需要严格的k-NN查询近似的体素搜索已经足够。这种对问题本质的深刻理解正是算法创新的真正内核。