跑激光SLAM的圈子里FAST-LIO这两个词这几年几乎避不开。我第一次真正把它跑通是在某台移动底盘真机上当时前面用的激光里程计在快速转弯和空旷走廊里飘得没法看换成FAST-LIO之后定位轨迹稳定得让人有点不适应。后来细读了原理和代码再回头看这个框架才明白它强的地方不光是精度而是把IMU和激光点云从底层融合做到了足够干净、足够快并且把整个状态估计过程设计得让工程实现也能落地。这篇文章就把FAST-LIO系列的原理从动机到公式再到工程细节完整拆一遍适合那些已经跑过开源代码但想搞懂内部原理的人也适合还在选型、纠结要不要入坑的开发者。1. 纯激光里程计撑不住的时候正是FAST-LIO出场的时候要理解FAST-LIO得先理解它要解决什么。很多人第一反应是我有激光雷达直接匹配两帧点云不就能算出位姿吗确实能但现实场景里纯激光里程计会遇到一系列问题FAST-LIO的整套设计几乎都是冲着这些问题去的。1.1 纯激光里程计的三座大山第一座大山是运动畸变。激光雷达扫描一圈需要几十毫秒到上百毫秒这期间车或机器人一直在动。如果你用一帧点云里所有点都是同一时刻采集的假设去做配准快速运动时点云边缘就会出现明显拖影。低速环境还勉强能接受一旦遇到高速旋转的机械雷达或者车辆急加速畸变误差会直接爆炸。第二座大山是退化场景。激光点云的几何信息分布不均匀是常态长走廊里沿走廊方向几乎没有几何约束空旷场地里能匹配的特征点少得可怜玻璃墙面和反光物体又会产生大量飞点。纯激光配准在这些场景里要么发散要么干脆给你一个看似平滑实则跑偏的轨迹。第三座大山是初始化和丢失恢复。纯激光里程计每次配准都从上一帧位姿出发一旦某帧匹配失败后续所有帧都会跟着崩。你可以在数据集里看到那种明显跳变的轨迹基本都是这一帧没跟上。这三座大山叠加在一起就导致一个问题仅仅靠点云自匹配稳定性很难满足实际部署需求尤其是对于需要长期连续工作的移动平台。1.2 IMU为什么能救场以及松耦合为什么不够IMU能在一定程度上救场因为它的核心优势是高频和连续200Hz甚至更高频率的加速度和角速度输出可以让你在两次激光帧之间以较高频率递推位姿。这不仅能用来消除运动畸变还能在激光配准失败时给一个还算靠谱的预测位姿保住系统的连续性。但IMU的致命缺陷是漂移纯积分姿态和位置加速度在几十秒内就会偏到离谱。所以正确的方向从来都不是选雷达还是选IMU而是把两者融合。很多人以为把GNSS、IMU、激光数据各自算一个结果最后做加权平均或者做因子图合并就是融合了。这种思路一般被称为松耦合。松耦合的问题在于它把每个传感器的输出当作独立的、已经稳定的结果来看待但实际情况是激光配准结果本身带有很强的先验依赖IMU积分结果又带有持续漂移你把两个有问题的东西在高层做个加权误差没消除只是被掩盖了。真正有效的做法是紧耦合让IMU状态参与激光残差的构建让激光点云反过来修正IMU的递推误差两者在同一个状态估计框架里互相约束。FAST-LIO的核心贡献就是把这个紧耦合过程用迭代误差状态卡尔曼滤波IESKF做得既严密又高效并且把计算量和工程复杂度压到了可以实时运行的程度。2. IESKF到底是什么从普通EKF到迭代误差状态的跨越FAST-LIO的状态估计核心不是很多人以为的因子图优化也不是粒子滤波而是迭代误差状态卡尔曼滤波。我第一次看这个名词的时候也绕了很久拆开来看其实没那么玄乎。2.1 误差状态卡尔曼滤波的思路传统扩展卡尔曼滤波EKF处理的是完整状态比如位置、速度、姿态四元数、陀螺仪零偏等直接对完整状态做预测和更新。问题在于姿态这种东西是流形上的量四元数的加法不是普通的向量加法。如果你直接对四元数做加一个高斯噪声的操作很容易产生非归一化和奇异问题。误差状态卡尔曼滤波ESKF的思路是维护一个名义状态这个状态可以被预测、被积分同时再维护一个误差状态表示真实状态与名义状态之间的偏差。由于这个偏差通常很小可以用向量形式表示其协方差传递也就自然成立。姿态的误差状态用旋转向量表示规避了四元数加法的各种坑。FAST-LIO在此基础上做了迭代更新。普通的卡尔曼更新是一锤子买卖用线性化的雅可比算一次卡尔曼增益然后完成修正。如果线性化点距离真值比较远这个一次修正就不够准。迭代的思想就是反复更新线性化点每次修正完重新线性化、重新算增益再修正一次类似于高斯牛顿法里的多次迭代直到收敛。提示IESKF的迭代过程可以通俗理解为预测一个位姿后不急着当作定论而是把点云在这个位姿附近反复贴合每贴合一次就调整位姿一次最终贴合得最好的那个位姿就是滤波器认为最可能的真实位姿。2.2 状态向量里到底装了什么FAST-LIO的状态向量包含两部分名义状态位置、速度、姿态四元数、陀螺仪零偏、加速度计零偏误差状态对应位置误差、速度误差、旋转向量误差、零偏误差等此外还有两个关键量的处理重力向量和IMU外参。重力向量在初始化时估计并作为常量处理因为对大多数轮式或地面机器人来说重力方向相对稳定。IMU到激光雷达的外参被放进滤波器和在线标定这一点非常重要因为很多跑数据集效果很好、上真机就发散的情况根因就是外参不准。2.3 IESKF更新环节的本质一个等价的最优化问题IESKF的更新步在数学上等价于求解一个带先验的最小二乘问题$$\min_{\delta x} \left( | \delta x |^2_{P^{-1}} | z H \delta x |^2_{R^{-1}} \right)$$这里的$x$是误差状态P是误差状态的协方差矩阵z是激光点云配准残差H是残差对误差状态的雅可比R是残差的噪声协方差。第一项约束修正量不能距离当前状态太远第二项约束修正量要尽量让点云残差变小。迭代求解这个问题的好处是它天然考虑了状态先验的约束迭代收敛后得到的修正量就是当前信息量下的最优估计。这和我第一次理解卡尔曼滤波时的感受完全不同卡尔曼滤波不是一个黑盒取加权平均的过程而是在相信预测和相信观测之间不断折中并用协方差来定量刻画两边的不确信程度。有了IESKF作为骨架接下来最关键的问题就是激光点云是怎么跟这个状态滤波过程产生联系的这就涉及到配准残差的设计。3. 激光点云是这样被塞进滤波器里的配准残差全流程FAST-LIO的激光部分不是单独算一个位姿再喂给滤波器而是直接构建当前帧点云到局部地图的距离残差然后让这个残差反向修正IMU状态和位姿。整个过程可以拆成几个步骤理解。3.1 特征提取只选有用的点最初的FAST-LIO版本在拿到原始点云之后会先做特征提取。雷达扫描的原始点云里有大量无效点远距离噪声点、低反射率点、遮挡边缘点等。特征提取的目的是从一堆点里选出两类最有代表性的点平面特征点局部区域内点云分布呈现明显平面形状比如地面、墙面边缘特征点局部区域点云分布呈现明显线状/角状形状比如墙角、柱边提取方式通常是计算局部区域的曲率。把每个点周围邻域拟合一个平面计算点到拟合平面的距离距离大的可能是边缘距离小且邻域点分布规律的是平面。FAST-LIO对特征点的数量有控制避免送入状态估计的点过多导致计算量失控。3.2 点到面距离而不是点到点距离很多配准算法用的是点到点的最近邻距离但FAST-LIO用的是点到面距离。为什么要用点到面因为点到点的距离在平面区域对垂直方向的约束非常弱而且离散采样噪声会被放大。点到面距离的定义是把当前帧某个特征点p经过预测位姿变换到局部地图坐标系里找到局部地图中最近的几个点拟合出一个微平面计算点p到这个微平面的距离d作为残差。这个残差既有明确的几何意义又比点到点对噪声更鲁棒。当点p正好落在某个真实平面上时d应该接近0当预测位姿有偏时d就会变大方向也包含位姿误差的信息。3.3 雅可比的构建和噪声协方差设置有了残差d之后我们需要知道d对状态修正量x的敏感程度也就是计算雅可比H。这里的推导比较绕因为残差是点云坐标的函数而点云坐标又经过外参、位姿、IMU状态层层变换。反向传播这一串偏导就是雅可比的计算过程。这也是FAST-LIO代码里比较复杂的一部分几乎每篇讲解都会在这里打转。噪声协方差R怎么设这直接关系到卡尔曼增益的大小。如果R设得过大系统会过度相信预测激光残差对状态的修正作用减小容易出现累计漂移如果R设置过小系统会过度相信单次点云匹配震动和噪声会被放大。实际工程中R通常根据激光雷达的测距噪声来定一般取厘米级别的方差量级同时可以在线根据残差分布做自适应调整。3.4 迭代让配准和滤波不再是两件事关键来了IESKF的更新过程中每迭代一次当前的位姿预测就会改变点云在局部地图中的最近邻关系也会改变。如果迭代次数足够最终收敛时位姿已经使点云和地图贴合得非常好此时得到的修正量才是最终修正量。这个过程把配准和滤波彻底融合成了一件事不再先算一个位姿更新再把这个更新塞进滤波器而是让滤波器自己完成配准优化。这也是FAST-LIO即使在快速运动、剧烈颠簸场景中依然能保持稳定的原因之一。4. FAST-LIO2的大改去掉特征提取之后地图更新反而更快如果只是到上面的程度FAST-LIO已经是一个不错的激光惯性里程计了。但它的续作FAST-LIO2做了一次更激进的改动把特征提取模块整个拿掉直接用全部点云进行配准配合一种增量式维护的kd-tree来提速。这个改动初看很反直觉点云数量多了那么多速度不是应该更慢吗实际效果却是又快又稳值得仔细聊聊。4.1 特征提取的隐形成本特征提取虽然能减少配准点数但有两个隐形成本。第一是特征提取本身有计算开销而且这个开销随着点云分辨率提高而增大。第二是特征提取是一个人为设计的过滤过程场景里的信息量并没有被完全利用。比如精细的结构纹理、不均匀的平面可能因为曲率不够大就被滤掉了。全点云配准的思路是既然雷达能提供这么多点那就全部用来约束状态让匹配信息量最大化。只要算法能把计算复杂度控制住全点云的精度和鲁棒性自然会优于手筛特征点。4.2 增量kd-tree是怎么做到提速的传统的kd-tree建树开销大如果每来一帧点云都重建一棵树或者大规模re-balance计算量依然吃不消。FAST-LIO2引入的增量kd-tree维护策略是树的节点可以动态增删每次只更新局部受影响的区域而不是整体重建。它类似局部动态调整周期性整体平衡的机制既能保证查询效率又不会让每次更新都重新算一遍全树。配合ikd-Tree系统会在局部地图中维护一个滑动窗口太久远的点云体素被淘汰新帧的点云被增量加入。查询最近邻时直接在树里找到候选点集再用拟合微平面的方式计算点到面残差。4.3 全点云配准为何稳定性更好全点云配准带来的另一个好处是无论什么环境几乎都能找到足够的几何约束。空旷房间里地面点可以约束俯仰和滚转房间边缘的墙点能约束偏航和水平位置复杂结构里更是有大量冗余约束即使一部分点云被遮挡或者噪声污染其他点仍然能维持整体状态的稳定。从精度指标上看在全点云配准之后FAST-LIO2在公开数据集上的表现普遍优于带特征提取的版本尤其在高动态和复杂结构的场景里差距更明显。4.4 代价和选型全点云配准也不是没有代价。它的实时性依然严重依赖硬件算力在算力受限的嵌入式平台上需要配合降采样策略比如体素降采样到一定分辨率再送入滤波器。此外处理全点云让单帧计算量不再那么可预测对实时系统调度有一定压力。个人建议是如果平台算力充足优先用FAST-LIO2如果平台是老旧的嵌入式设备且场景相对简单保留特征提取的FAST-LIO反而更容易调。5. 从公式到工程初始化、外参、退化和调试心得原理看完很多人会兴冲冲跑代码然后发现跑数据集挺顺一上真机就开始各种飘。这一章集中讲我实际使用中总结的工程经验内容包括初始化、外参、参数调试和退化场景处理。5.1 初始化前几帧就定生死FAST-LIO对初始姿态比较敏感。初始姿态误差如果很大比如偏了十几度滤波器需要一定时间收敛这个过程里轨迹可能明显跳动。建议导入代码后先用静止数据初始化重力方向和IMU零偏再开启正式运动估计。很多数据集本身提供了初始位姿能直接用真机场景下建议在启动前保持平台静止数秒给滤波器一个静止初始化窗口。5.2 外参真机发散的头号杀手IMU与激光雷达之间的外参是FAST-LIO真机调试中最容易出问题的地方。手动测量和理论安装位置之间通常存在几毫米到几厘米的误差以及毫米级别的角度误差这些误差在高速运动时会放大成明显的轨迹漂移。FAST-LIO把外参纳入在线标定能一定程度上修正误差但不能完全替代离线标定。我的经验是先做一次离线外参标定把外参初值尽量标准再打开在线标定让它微调。两者配合下来稳定性会明显提升。外参标定工具市面上有好多其中基于Apriltag 多帧点云联合优化的方法比较适合多数场景这里不展开但建议一定要做不要拿纸面安装尺寸直接上。5.3 关键参数怎么调FAST-LIO有几组参数需要重点关注我列了一个表方便对照。参数影响调试建议IMU采样频率决定预测频率影响畸变消除效果尽量用真机IMU的满频不要降频加速度计噪声密度控制对IMU加速度的信任度过大则轨迹平滑但延迟大过小则易振动根据IMU手册初设陀螺仪噪声密度控制角速度信任度过大会让姿态跟随噪声过小则偏航漂移明显局部地图体素大小决定地图更新粒度和计算量小则精细但慢大则快但丢失细节通常从0.5m起步调迭代次数上限控制配准收敛程度太少精度不够太多增加算力负担默认值一般够用调参时一次只动一个参数改完跑同一条录制的数据包对比避免同时乱改最后不知道怎么调回去的。真机调试和数据集调试要区分开真机数据里的噪声模式和数据集的差异往往比想象中大得多。5.4 退化场景长走廊、玻璃墙和高速运动即便FAST-LIO有IMU加持退化场景依然不能完全幸免。长走廊场景沿走廊方向的约束基本靠IMU积分撑着如果IMU零偏没有标定好走廊尽头的方向容易慢慢偏转。处理方法是利用走廊上的灯光、门框等几何结构以及不要长时间高速直线运行让滤波器有机会通过其他方向约束来修正。玻璃墙和透明材质会把点云击穿产生飞点。建议在点云预处理阶段加一层基于反射强度的过滤去掉反射率异常低的点再送入FAST-LIO。如果场景里大面积玻璃无法避免可以考虑在玻璃区域加少量人工特征点或配合其他传感器信息比如轮式里程计作为额外约束。高速运动场景最考验运动畸变消除和IMU递推质量。FAST-LIO理论设计已经很强但工程实现上还是要保证IMU时间戳对齐精度不能有较大的时间偏差。时间偏差会导致IMU递推和激光帧之间的耦合出现系统性误差这种误差在低速时不容易察觉高速时会突然放大。5.5 代码阅读顺序建议如果要从代码层面吃透FAST-LIO不建议直接从主循环开始读很容易被大量矩阵运算淹没。我建议按这个顺序先看IMU预测模块搞明白状态是怎么每秒几百次递推的再看点云预处理理解降采样、过滤和特征提取逻辑然后看残差构建找到点到面距离的计算入口重点理解雅可比是怎么链式传导的最后看滤波器更新结合论文里的IESKF公式把卡尔曼增益计算和迭代循环对上这样读下来公式和代码能一一对应上不会云里雾里。6. 最后聊聊我对这套方案的看法拉通FAST-LIO系列这么多年我自己最大的感触是它真正把工程可落地做到了一个很高的位置。很多研究的方案上了真机就垮FAST-LIO之所以在社区里被反复讨论恰恰是因为它在公开基准和真实场景都能拿出让人信服的表现这个输出背后是大量系统设计的取舍和调优。举个例子它把迭代的机制放进滤波更新里本质上就是在经典滤波的效率和优化方法的精度之间取了一个非常好的平衡而不是把两者硬拼在一起。如果你正在做机器人的定位导航方案选型我觉得FAST-LIO一定值得作为首选之一。它的代码可控性高社区讨论多遇到问题能找到参考经验调参思路也相对好梳理。但有一点要提醒先把它的原理读懂再动手调代码会节省你大量时间。纯粹靠改参数跑通一两次很容易在外面绕圈遇到新环境就又开始瞎试。按我个人经验拿到一个新平台第一件事永远是标定做好尤其是时间同步和外参。这两样做扎实了FAST-LIO的稳定性已经成功了一半。剩下的事情无非是让算法在熟悉你的平台风格之后好好替你打工。