从FAST-LIO到FASTER-LIO：紧耦合激光惯性里程计的演进之路

1. FAST-LIO紧耦合激光惯性里程计的开山之作第一次接触FAST-LIO是在2019年当时我正在为一个室内移动机器人项目寻找可靠的定位方案。传统LOAM系列算法虽然精度不错但对计算资源要求太高我们的NX开发板根本跑不动。直到看到FAST-LIO的论文我才意识到激光和IMU的紧耦合可以玩出这么多花样。FAST-LIO最核心的创新在于紧耦合设计和反向传播运动补偿。与松耦合方案不同它直接把IMU和激光特征点捆在一起处理。我特别喜欢作者对IEKF迭代扩展卡尔曼滤波的改造——把观测维度的矩阵求逆转换成了状态维度的求逆。这个改动看似简单实际效果却非常惊人。记得当时用16线激光雷达测试传统方法一帧数据处理要50ms而FAST-LIO只需要15ms左右。具体实现上有个细节特别值得说它的状态更新频率完全跟着雷达走。每收到一帧点云就通过IMU的离散传播模型预测状态然后用scan-submap匹配得到的特征点包括线特征和面特征来修正。这里有个精妙的设计是增量式更新每次迭代计算出的状态增量小于阈值才停止确保结果收敛。我在实际项目里发现这个机制对处理快速运动时的点云畸变特别有效。2. 流形计算工程师必须掌握的数学工具第一次看到FAST-LIO论文里的流形运算公式时我也头皮发麻。但真正用起来才发现这些数学工具其实很接地气。简单来说流形运算就是处理旋转矩阵这类特殊空间的加减法。比如你想把两个旋转矩阵相加直接矩阵相乘是不行的需要先用Exp映射转到李代数空间做完加法再用Log映射转回来。FAST-LIO定义了⊞和⊟两个运算符分别对应流形上的加和减。举个例子旋转矩阵R更新时R_new R ⊞ r R * Exp(r)计算两个旋转矩阵差R1 ⊟ R2 Log(R2^T * R1)我在代码实现时踩过一个坑Exp映射中的罗德里格斯公式实现要注意数值稳定性。当旋转向量很小时直接用泰勒展开近似比完整计算更可靠。这个细节论文里没强调但实际使用时非常关键。3. FAST-LIO2当算法遇见数据结构革命2021年看到FAST-LIO2论文时我正被点云管理问题困扰。当时项目用的八叉树地图随着环境扩大内存占用飙升到8GB定位开始出现卡顿。FAST-LIO2带来的ikd-Tree简直就是及时雨。这个版本最大的突破是抛弃了特征提取直接用原始点云配准。这意味着不再依赖环境中的明显线/面特征在空旷走廊也能稳定工作适应各种奇葩雷达我们测试过固态雷达和机械式雷达混用配合ikd-Tree内存占用直降80%实测从8GB降到1.5GBikd-Tree的魔法在于它的增量更新能力。传统k-d树插入新点要重建整棵树而ikd-Tree通过给节点添加treesize、invalidnum等属性实现了局部更新。我做过对比测试在10万点云中插入1000个点ikd-Tree比传统方法快20倍。不过要注意ikd-Tree的查询速度会稍慢建议设置合理的重建阈值。4. FASTER-LIO速度与精度的新巅峰去年测试FASTER-LIO时我专门搭建了一个高速移动平台。在2m/s速度下FAST-LIO2的定位开始出现轻微漂移而FASTER-LIO依然稳如磐石。它的秘密武器是iVox增量体素结构这是对传统体素的智能化改造。iVox有三大绝活支持并行k-NN查询充分利用多核CPU增量插入时自动合并邻近点保持数据紧凑动态调整体素粒度平衡精度和速度实测数据显示在相同硬件上FASTER-LIO比FAST-LIO2快30%且内存占用更低。有个实用技巧对于室内场景将iVox的初始分辨率设为0.2m效果最好室外场景建议0.5m起步可以根据点云密度动态调整。5. 实战经验如何选择适合你的LIO算法经过三个版本的迭代FAST-LIO系列已经形成完整的技术矩阵。根据我的项目经验可以这样选择计算资源有限首选FAST-LIO2ikd-Tree对低配硬件更友好高速运动场景必须上FASTER-LIOiVox的动态性能优势明显特殊雷达适配FAST-LIO2的原始点云方案兼容性最好部署时要注意IEKF的迭代次数建议设为3-5次太少影响精度太多浪费算力。另外所有版本都对IMU-雷达外参标定非常敏感建议先用Kalibr工具精细标定。