FAST-LIO-SAM 系列的记录2按道理应该比记录1更逼近生产环境一些。上次我老老实实把 FAST-LIO 的单激光雷达惯性里程计跑通也承认前端精度确实能打但在面对需要闭环、需要长时间反复扫描同一区域的建图场景时前端里程计只能给你局部很平、全局慢慢歪掉的美丽错觉。所以这次我把注意力放在了 FAST-LIO-SAM 上一个保留 FAST-LIO 紧耦合前端同时把 LIO-SAM 那套基于因子的全局优化搬过来的融合派方案。如果你已经大概理解 LIO-SAM 和 FAST-LIO但对它们之间到底怎么分工、FAST-LIO-SAM 相对两者的增益在哪、实际落地时有哪些绕不开的参数和坑点仍然模糊这篇记录应该正对你的需求。1. 从 LIO-SAM 和 FAST-LIO 各自把我逼疯的瞬间说起先说一下我自己的经历基线。早在接触 FAST-LIO-SAM 之前我已经分别在 FAST-LIO 和 LIO-SAM 上做过不少实验。这两个项目在各自体系里都算成熟但放到真实场景里都有让人头疼的时刻。FAST-LIO 那套紧耦合机制主要体现在迭代误差状态卡尔曼滤波把 IMU 和激光雷达点云观测一起估计状态前端稳得让人很有安全感。可它本质上还是一个增量式的里程计——它没有把全局地图之间的关系做闭环修正。什么意思呢就是当我在一个形状比较恶心的场景里走一个大的环路比如园区里绕一圈又回到起点附近前端里程计大体上能保持不错的局部姿态但由于传感器噪声、点云配准误差、以及长时间运行中 IMU 零偏慢慢漂移整个轨迹会像一块没有闭合的布一样起点和终点之间逐渐出现一条缝。地图前端看起来挺整齐放大以后其实有一层明显的错位或重影。对做纯里程计估算来讲这也许还能接受但做建图就完全不能忍。LIO-SAM 也有它的优势它把激光惯性里程计和回环检测、GPS 因子放在一个因子图框架里能做全局优化。做闭环检测时它会把当前帧跟过去的关键帧做一次配准得到一个闭环约束然后把这个约束扔给后端的 GTSAM 做批量优化从而把累积漂移压回去。这套思想本身非常漂亮可 LIO-SAM 前端的鲁棒性又不如 FAST-LIO 那么强尤其在雷达运动过快或者环境特征退化的时候它那种依赖特征点提取和点到面匹配的前端容易把自己带跑偏。我遇到过的情况很典型带着 LIO-SAM 走过一段单调长走廊特征本来就少相邻雷达帧之间的约束弱连续几帧的配准误差悄悄累积最后系统突然在一个转角处彻底丢失方向后端再强大也救不回来。于是 FAST-LIO-SAM 的价值就变得非常现实它相当于让 FAST-LIO 的紧耦合前端干重活让 LIO-SAM 风格的全局因子图负责修正长漂移两侧的优势互补。你可以把它理解成一种短期记忆非常好、长期方向感很差的人配上一个专门负责记地图和纠错的后勤团队的组合——短期状态估计交给前端那个快而稳的脑子全局一致性问题交给后端那张图。这也可以解释为什么现在很多人在自己的机器人平台上搭 SLAM 都不再只跑单一口径的里程计或只依赖回环检测而是想要这种前端紧耦合、后端全局优化的混合架构。尤其是在成本有限的 DIY 平台上IMU 精度一般、雷达又不是特别高端的型号前端状态估计必须足够抗造后端又不能用太重的全局优化策略把实时性拖死。FAST-LIO-SAM 式的设计恰恰给出了一条非常适合借鉴的中间路线。2. 拆开看它解决的核心问题前端谁来做、后端谁来管我第一次看 FAST-LIO-SAM 相关源码时最大的感受是它并没有发明一种全新的算法范式而是把已经验证过的两套方案在系统层做了一次非常有意识的整合。从模块划分上我把它拆成了三层来看这样整个系统的脉络会非常清晰。第一层是传感器数据预处理。到这一步激光雷达点云和 IMU 数据要先被导入统一的时间轴里。雷达点云通常还带有运动畸变因为一帧点云的采集并不是瞬间完成的雷达在旋转或扫描的过程中载体自身也在运动。如果直接拿原始点云去和地图匹配在快速运动时会出现明显的拖影。所以这套系统里IMU 数据除了用于状态估计还要承担一部分点云畸变校正的职责。我的理解是这里本质上不是简单地给每一帧打一个时间戳而是将点云中的每个点都补偿到同一参考时刻的位姿下从而让后续的帧到地图匹配能拿到一套干净的观测。如果你在实验中忽略了这条链路或者时间同步没做好你会发现在转弯和变速场景下地图开始发飘原因就是畸变没被校正好。第二层是紧耦合的激光雷达惯性里程计。这一块基本延续了 FAST-LIO 的路线用迭代误差状态卡尔曼滤波把激光点云观测和 IMU 预测放进同一个状态空间里解算。理解这层时有一个特别容易被混淆的点这里的雷达观测并不是传统意义上的特征点匹配而是用当前估计位姿把雷达点云投影到局部地图中再通过点到面的残差来更新状态。这意味着它绕开了很多 LIO 系统里先提取特征、再匹配特征的流程保留了更多的原始点云信息因此在环境特征不太丰富的时候依然能拿到相对稳定的相对位姿。前端每输出一个位姿局部地图也会被增量式地维护起来以保证下一次匹配有足够的几何约束。第三层是全局图层面的因子图优化。这一层里有几个关键因子里程计因子、回环因子、GPS 因子。里程计因子把前端输出的相邻关键帧之间的相对位姿约束加入因子图回环因子则是在检测到当前关键帧和历史关键帧有足够多空间重合时通过一次配准产生一个跨时间尺度的约束。GPS 因子则通常在室外场景下提供绝对位置参考能有效地把轨迹拉回真实地理坐标附近同时也能对累积漂移形成一种弱校正。从这条主线可以看出FAST-LIO-SAM 的前端和后端并不是完全解耦的。前端产生的点云地图、关键帧位姿都要作为后端图优化节点的输入而后端每一次全局优化完成后又要把修正后的位姿更新回当前活动的地图甚至通过反馈机制通知前端重置或修正。这个过程比单跑 FAST-LIO 复杂不少因为系统里存在两条反馈路径一条是高频的里程计状态更新一条是低频的全局修正。如果没有理解这两条路径的交互想在代码里调参就会有些无从下手。我个人的阅读建议是不要一上来就钻公式或者从 main 函数逐行看。先按预处理节点—前端节点—后端优化节点的三段式思路把节点和它们之间的 topic 关系画在脑子里再带着问题去代码里找对应实现。你会发现所谓系统集成本质上是在协调不同时间尺度上的数据流和反馈而不是单纯把两个算法库拼在一起。3. 从源码到跑起来依赖、编译和第一次试运行的痛苦记忆FAST-LIO-SAM 的编译安装过程不算特别复杂但对环境的一致性要求比较高。我在自己的机器上用的是 Ubuntu 20.04 和 ROS Noetic另外还需要 PCL、Eigen3、GTSAM。这三个库的版本最让人头疼尤其是 Eigen 和 GTSAM 之间有时会因为编译标准和 ABI 不兼容出现各种莫名其妙的链接错误。我的建议是尽量不要手动去系统里乱装最新版 GTSAM尤其不要直接从源码 master 分支编。虽然 GTSAM 官方一直在迭代但在某些版本组合下会和项目原本依赖的接口不一致。我自己到最后干脆把整个工作空间搬进 Docker 容器里用 ROS Noetic 官方镜像加特定版本的依赖一次编译通过。如果你只是为了先跑通数据、验证算法效果容器方案比在一台正在日常使用的电脑里反复折腾依赖要省心得多。编译完成之后第一件要紧的事是确认传感器数据的话题类型和时间同步。这里我踩过的坑是激光雷达和 IMU 如果不做硬件或者软件时间同步话题之间可能出现几毫秒到几十毫秒的延迟。对于比较慢速的机器人平台几十毫秒延迟也许只是地图轻微模糊但在高速场景下会导致姿态估计跳动甚至发散。FAST-LIO 系列对 IMU 和雷达之间的时间偏差相对敏感所以使用前一定要先把时间轴对齐。还有一个非常值得注意的配置项是雷达类型和线数。很多刚接触这类系统的朋友拿到配置文件的第一个反应是直接照搬示例但不同雷达的扫描方式差异很大比如机械旋转雷达和固态雷达在点云分布、视场角、盲区等方面完全不同。我一开始用某款 16 线机械雷达跑一个默认偏向于 64 线或者固态雷达的参数结果前端很容易因为局部点云太稀而退化表现为地图出现大量飞点、轨迹在某一段明显跳动。后来把线数、最大扫描距离、最小有效距离等参数按实际雷达规格重新调整问题才基本消失。这里要提醒一句盲区参数尤其关键如果雷达本身盲区大却还允许极近距离的点参与匹配那些带有严重测量噪声的近距点会像撒进面糊里的石子一样把配准结果搅乱。我第一次试运行时用的是数据集回放而不是实车实采。回放数据的好处是可以反复调整参数并对比不同配置下的输出不用担心安全问题。我选择了一条包含明显回环的园区数据刚跑完一遍时 RViz 里的地图看起来很干净几乎挑不出毛病。但这恰恰是我后来学会警惕的第一印象陷阱视觉效果会骗人看起来很漂亮的地图其绝对轨迹误差可能已经高到不适合导航使用。真正能说明问题的是把轨迹导出来和地面真值做数值对比这一点放在后面单独聊。4. 真正决定建图效果的几个隐蔽参数外参、协方差和回环阈值跑通只是起点把图建好才是难点。我在调参过程中慢慢形成了一个认识影响 FAST-LIO-SAM 最终效果的往往不是那些最显眼的大开关而是一些容易被忽略的小旋钮。其中最重要的有三个雷达和 IMU 之间的外参、系统噪声协方差初值、以及回环检测的接受阈值。先说外参。雷达和 IMU 之间通常存在一个固定的旋转和平移关系这个关系如果标定不准相当于前端在融合 IMU 时对状态预测用了一个有偏差的观测模型。把外参误差想象成一个戴眼镜的人看东西时镜片没有完全对准平时的低速慢走可能影响不大一旦高速运动、加减速或者强振动这种系统误差就会持续积累。很多人在雷达和 IMU 支架重新拆装之后忘了重新标定就会发现自己再怎么调内参建图质量也没有明显改善。所以建议在拿到一套新传感器组合时先用标定工具把外参重新算一遍不要迷信图纸上的固定值。再说协方差初值。卡尔曼滤波类系统的表现非常依赖噪声协方差的设置。IMU 的加速度计和陀螺仪噪声、雷达点云的测量噪声如果设置得跟传感器实际物理特性相差太远滤波器会变成两个极端要么过于相信 IMU导致激光观测无法修正漂移要么过于相信激光观测导致状态估计里混入大量点云噪声。我有个简单的调参经验先在静止状态下观察系统输出的姿态稳定性如果静止时姿态漂移或抖动明显优先检查 IMU 噪声协方差是不是设得太小或者外参是否出了问题。如果运动过程中轨迹总是落后于真实位置则要适当调低雷达测量噪声或者调高 IMU 数据的权重让前端反应更快一些。回环检测阈值是另一个容易被忽略但影响很大的地方。回环检测并不是检测到回环就一定是正确约束如果阈值设置得过于宽松系统会把本不该闭合的帧当闭环引入错误的因子导致地图被强行拉歪甚至出现明显的褶皱。如果阈值过紧又会漏掉本来可以闭合的环让累积漂移一直得不到修正浪费了后端强大的优化能力。怎么判断阈值合适不合适我常用的方法是观察运行时是否频繁输出回环信息。如果长时间跑完一个完整回环却完全没有回环触发大概率是阈值太严如果回环频率高得不像话每隔几十秒就来一次而且地图在回环后出现轻微变形那就要仔细检查是不是把误匹配放进来了。还有一个我后来才意识到的重要因素GPS 因子并不是加了就一定好。如果你手里只有消费级 GPS或者根本没有 GPS就不要轻易启用 GPS 因子。GPS 数据如果本身漂移严重、更新频率又低把它作为强约束放进因子图反而会把激光雷达辛苦积累出来的相对一致轨迹拉得歪歪扭扭。FAST-LIO-SAM 这类框架给了你灵活的因子配置能力但能力多不代表都该开。我的做法是室外场景先纯跑雷达惯性加回环确认前端和后端单独工作正常再在特定测试条件下谨慎开启 GPS 因子对比差异之后再决定要不要在正式任务里使用。5. 不靠肉眼判断效果evo 评估一条轨迹的完整思路记录1 时候我基本是靠肉眼来验收地图那时候觉得地图看起来不缺胳膊少腿就是成功。但记录2 这次我换了个态度建图质量的最终标准是轨迹精度轨迹精度的最终标准是比较数值而不是视觉感受。所以要把系统输出的轨迹保存下来并用合适的评测工具去和地面真值做对比。我之前提过的方法是用 evo 这套工具。它能够直接读取 TUM、KITTI、EuRoC 等多种轨迹格式然后计算绝对轨迹误差ATE和相对位姿误差RPE。ATE 比较的是整条轨迹和真值之间的一致性它能直观告诉你系统整体漂移有多大RPE 衡量的是轨迹中局部时间段内的相对运动误差更能反映系统短时间内的稳定性。我们的测评场景通常都是非城市环境所以一般使用TUM格式的轨迹文件里面每一行的时间戳以及 xyz和qxyzw格式如下timestamp tx ty tz qx qy qz qw在 FAST-LIO-SAM 里跑完数据后需要把算法输出的位姿写到这个格式的文本文件中。之后可以用类似这条命令来评估evo_ape tum groundtruth.txt estimate.txt -va --plot-v 是 verbose把每次对齐后的详细误差都打印出来-a 表示在评估前做一次轨迹对齐这样可以去除因坐标系原点不一致而造成的整体位移。--plot 会直接画出真实轨迹和估计轨迹之间的误差曲线。我还会顺手加一条 --save_results 选项把结果存成 json 或 zip 文件方便后续对比不同参数组的成绩。不过这里有个容易踩的坑地面真值的坐标系和估计轨迹的坐标系往往不是同一个。比如真值来自 GPS 或动作捕捉系统估计轨迹来自系统的第一帧位姿两者不仅原点不同旋转方向也可能完全不一致。直接用原始轨迹去做 evo 对比是没有意义的必须先用 --align 或者手动求变换关系把它们对齐。evo 的 -a 参数做的就是这件事它会对整条轨迹做一次刚体变换对齐使两条轨迹尽可能重合。但这也有副作用如果系统轨迹漂移不是整体性的而是某一段特别差完全对齐后误差会被分摊到其他段上导致数值结果看起来比实际更乐观或更悲观。所以我觉得比较稳妥的做法是先看 RPE因为 RPE 不受全局坐标对齐的影响再看 ATE并且要注意报告的均方根误差和最大值分别是什么。从评测结果里能看到很有意思的现象。比如我在一组没有开启回环检测的测试中轨迹末端误差大概在两米左右建图时肉眼看局部细节还能接受但其实这条轨迹如果用于导航末端已经偏到了墙里。而在开启回环检测后同样的数据末端误差缩小到二十厘米以内地图整体闭合效果明显更好。这种对比能非常直观地告诉你后端闭环到底起了多大作用也能帮你说服团队成员为什么不能只看地图不跑评估。我自己还会额外做一个小实验把数据集切成多个长度不同的片段分别跑评测。这样可以观察到误差是随着路径长度线性增长还是主要集中在一两个关键转折点。如果是后者说明系统本身没有大问题问题往往出在特定几何条件下的退化场景比如长直走廊或空旷停车场。针对这些场景再去看看前端输出和后端回环是否正常工作会比盲目调一遍全局参数有效得多。6. 写下记录2之后还在纠结的几个问题到目前为止FAST-LIO-SAM 的基本链路我已经比较清楚了也能在自己采集的数据集上得到比单跑 FAST-LIO 稳定得多的全局地图。但它并不是一个拿来就能解决所有情况的银弹我自己脑子里还有几个悬而未决的问题准备留在后续继续验证。第一个是回环检测在大场景下的计算负担。按我当前的理解回环越频繁、关键帧数量越多后端因子图的规模会持续扩张优化耗时也会随之上涨。虽然实际项目里可以通过降低关键帧频率来缓解但关键帧少了又会降低回环检测的召回率。这里面的平衡关系目前还没有一个特别通用的解法大概率还是要根据传感器频率和场景大小去试。比如我在园区测试时发现稍微降低关键帧频率对最终精度几乎没有损失但优化耗时明显下降这个趋势在高楼环绕的封闭场景里可能又会反过来。第二个是关于不同雷达扫描模式对预处理环节的影响。FAST-LIO-SAM 在设计时对机械旋转雷达和固态雷达都有一定兼容能力但我总感觉在实际配置不同雷达时参数之间的耦合关系比我最初想的复杂得多。同样的算法框架换一套分辨率或者视场角差异明显的雷达可能需要连带去调整畸变校正策略、盲区过滤范围、地图分辨率等一系列参数而不是简单地改一个雷达类型选项就能完事。这背后的原因应该跟点云分布密度和运动畸变模型有关但我也还在实践中积累经验。第三个是 GPS 因子真正能发挥作用的场景边界。我在不开 GPS 的小范围、有回环数据上表现很满意但在一些大范围、几乎没有明显回环的野外场地如果没有 GPS 参与误差还是不可避免。问题在于消费级 GPS 噪声和更新频率会带来新的不确定性并非每一条绝对观测都有正收益。要找到一个合理的加权策略让系统在依赖雷达惯性漂移和信任 GPS 绝对定位之间平滑切换可能需要更系统地观察不同噪声等级对后端的扰动。这些想法与其说是本文的结论不如说是我自己下一步的实验清单。做 SLAM 学习和工程验证的乐趣也正在于此当你觉得一个系统原理讲通了、地图也看着干净的时候总会有新场景和新数据把你拉回原来这个环节我还没有真正吃透的状态。如果你也在折腾 FAST-LIO-SAM希望这篇记录能帮你少走几步弯路也欢迎你在自己平台上跑完以后对比一下那些容易让人误判的环节是否和我遇到的情况一致。