FAST-LIO 系列是激光惯性 SLAM 里绕不开的一套开源方案，我第一次真正跑通它，是在一台算力很弱的板载电脑上做室外建图。当时项目时间紧，激光雷达帧率低、IMU 噪声还大，结果这套算法硬是把地图建出来了，边缘清晰、轨迹闭环也对得上。从那之后我就把 FAST-LIO 当成了移动机器人定位建图的默认候选方案之一。

这篇内容我不讲那些浮在表面的介绍，直接拆它的原理、版本差异、关键实现和落地时最容易翻车的点。适合正在选型激光惯性 SLAM 方案的同学、刚接触 FAST-LIO 想搞懂里面到底怎么回事的开发者，以及已经跑通代码但想调参调得更顺手的人。看完你至少能明白：FAST-LIO 为什么能做到高精度实时建图，FAST-LIO2 相比初版到底改了哪些核心东西，以及你拿到一套新传感器时应该按什么顺序去调参。

1. FAST-LIO 系列全景：它到底解决了什么问题

1.1 为什么纯激光 SLAM 打不过退化场景

先说一个很直观的现象。纯激光 SLAM 的核心是点云配准，也就是把当前帧激光点云和已有地图做对齐，用最近邻搜索和迭代最近点或特征关联来求位姿。平时没问题，但一旦环境变得“退化”，比如一条长长的走廊、雪地、隧道、开阔广场，激光点云里没有足够的几何约束，配准就会在某个自由方向上“漂”。

打个比方，你闭着眼睛走路，脚下感觉平平稳稳，实际上方向已经偏了，只有碰到墙你才知道错了。纯激光 SLAM 在退化场景里就是那个闭眼走路的人——它有高频的激光距离观测，但环境不给你足够的信息来约束所有维度，于是位姿估计慢慢往外飘。

IMU 正好补上这个缺口。IMU 提供角速度和加速度，频率高、短时间漂移小，可以在两帧激光之间做运动传播。就算环境退化，IMU 也能在短期内把姿态和位置“拉住”，不至于让误差瞬间炸开。把激光和 IMU 放在一个紧耦合框架里，就是用 IMU 的预测顶住退化方向的空洞，用激光的观测修正累积漂移，两者取长补短。FAST-LIO 干的就是这件事。

1.2 从 FAST-LIO 到 FAST-LIO2 再到 FAST-LIO-SAM

FAST-LIO 初版可以理解成一套“强特征 + 紧耦合 IESKF”的方案。它先提取点云里的边缘点和平面点，然后做配准。这样做的优点是计算量可控，缺点是特征提取在退化场景会失效，而且丢失了原始点云里的很多信息。

FAST-LIO2 做了一个我认为很关键的决定：去掉特征提取，直接把原始点云用于配准。当时看到这个设计我是有点意外的，因为不用特征点通常意味着计算量更大，但 FAST-LIO2 用一个自研的增量式 kd 树，也就是 ikd-Tree，把地图管理和最近邻查询做成了动态更新，整体复杂度反而降了下来。代价是理解门槛变高了，你需要先搞清楚 ikd-Tree 的内部机制，否则参数调起来会一头雾水。

后续还有 FAST-LIO-SAM 这类延展版本，把因子图回环检测加了进来。它更适合大场景长时间建图，但体积和依赖也会变多。选型的时候可以参考这个表格：

我的建议是：如果只是做课题验证或者无人机小范围建图，从 FAST-LIO 开始读代码，它逻辑更短；如果要上线跑真实机器人、做复杂环境建图，直接用 FAST-LIO2 作为基线更省事。

2. 核心原理：迭代误差状态卡尔曼滤波（IESKF）拆解

2.1 卡尔曼滤波浓缩版：导航里的“GPS 加车速表”

要把 IESKF 讲清楚，得先回到最基础的卡尔曼滤波。想象你开车导航，车上有两个信息来源：一个是 GPS，每隔一秒给一个位置，精度一般；一个是车速表加方向盘转角，你能推算出相对上一刻走了多远。

GPS 像激光雷达，绝对值准但帧率低；里程推算像 IMU，帧率高但有累积误差。卡尔曼滤波的思想就是：根据车速表预测现在的位置，再拿 GPS 测到的位置做校正，两者加权平均。权重大小取决于各自的不确定性，越可信的信息权重越大。这种“预测 + 更新”的循环，就是卡尔曼滤波的骨架。

FAST-LIO 用的不是普通卡尔曼滤波，而是误差状态卡尔曼滤波。名字听起来吓人，但思路不复杂：我不直接估计“你现在在哪”，而是估计“预测值和真实值之间的差”。这个差值通常很小，数值上更稳定，线性化误差也更小。就像你用一个粗略地图导航，先知道大概位置，再不断修正“我刚才是不是偏了一点点”。

2.2 误差状态与流形更新：为什么偏要估计“偏差”

先看状态向量里都有什么。FAST-LIO 维护的无非是这些量：位置、速度、姿态、陀螺仪零偏、加速度计零偏，以及重力向量。其中姿态是旋转，它不在普通的欧几里得空间里，不能简单做加减。

这就是流形概念的来源。你可以把旋转理解成一个球面上的点，你只能在这个点上施加“小转动”来更新它，而不是像普通向量一样直接加一个差值。误差状态滤波器的做法是：把姿态的误差定义为一个小的旋转增量，整体状态用四元数或旋转矩阵维护，误差状态用小的向量近似。这样既避免欧拉角的万向锁问题，又让线性化模型足够准确。

我早期看这段代码时，花了不少时间才想明白一个点：为什么要绕一圈去估计误差状态，而不直接估计完整状态？原因主要有两个。第一，误差状态数值很小，协方差矩阵的更新不容易被数值误差干扰；第二，旋转修正天然适合用流形上的增量来表示，如果直接对四元数做加减，结果往往是错的。

2.3 预测与更新：IMU 递推、激光修正的一次完整循环

整个 IESKF 的运行节拍可以这样理解。每次拿到一帧 IMU 数据，系统就做一次预测：用当前状态和 IMU 读数积分，更新位置、速度和姿态，也更新协方差。这个阶段不需要激光参与，是纯粹的惯性递推。

拿到一帧激光点云之后，进入更新阶段。系统把激光点投影到当前估计位置附近，在 ikd-Tree 地图里找最近邻点，计算残差。然后根据残差和卡尔曼增益，对状态做一次修正。这一步和普通卡尔曼滤波的唯一区别是，修完之后再用新状态重新做一次点云关联，接着再修正一次，反复迭代几次直到残差收敛。这就是“迭代”二字的来源。

这种做法很像你用瞄准镜打靶：先粗略瞄准打一发，看到弹着点偏了，再调整方向，再打一发，逐步逼近目标。迭代误差状态卡尔曼滤波做的就是这个事，每一次迭代都在用更准的位姿去重新寻找匹配点，从而减少配准误差。实际效果是，在剧烈运动或者点云噪声大的情况下，收敛速度和精度都明显优于一次性更新。

3. FAST-LIO2 的新引擎：ikd-Tree 与直接配准

3.1 传统 kd-Tree 的“存量焦虑”

说到最近邻搜索，最常见的工具就是 kd-Tree。它把空间用超平面递归划分，查询出一个点附近最近的邻居非常快。但这个数据结构有一个让人头疼的问题：它基本是静态的。建好一棵树之后，如果你想插入一个新点，保持树的平衡性需要大量调整；如果你要删除地图中过时的点，那就更麻烦了，甚至可能需要重建整棵树。

FAST-LIO2 使用全局地图，每一帧都会往里加成千上万个新点，如果每帧都重建一次 kd-Tree，实时性根本保不住。这就是我常说的“存量焦虑”：地图越来越大，传统树结构的维护成本越来越高，直到成为瓶颈。

3.2 ikd-Tree 的增量更新设计

ikd-Tree 的核心思路是：不追求全局完美平衡，而是做局部重平衡。它允许新点被增量地插入到树中，每个节点记录子树的信息；当某个子树的插入次数太多，导致它严重不平衡时，才触发一次局部重建，把这个子树重新构建成平衡结构。

删除也不是立刻物理删除，而是先把节点标记为“已删除”。标记节点仍然占据空间，但在查询时会被跳过。当整个子树里被删除的点足够多时，重建会把它们真正清理掉。这种懒删除加定时重组的策略，避免了频繁的物理删除操作，同时地图点的增加和淘汰都变成了可控成本。

除此之外，ikd-Tree 还内置了地图管理功能。你可以设置体素大小，插入新点前先检查该体素内是否已有足够多的点，如果没有就插入，否则丢弃或者降采样。这样能有效控制全局地图的点密度，防止地图膨胀到内存爆炸。

我在实测中观察到，用 ikd-Tree 管理地图之后，配准单帧的耗时变得非常平稳，即使地图点规模到了几十万级别，每帧查询耗时也只是微小波动。相比经典 kd-Tree 加局部地图的方案，省下的时间主要是“不用每帧重新建树”和“不用手工维护局部滑窗”这两块。

3.3 直接配准：不用特征点也能建图

传统特征法激光 SLAM，比如早期 LOAM 思路，会从点云里提取角点和平面点，再用这些几何特征做配准。特征是干净的，但代价是丢失信息。比如在森林、灌木丛这类无规则环境中，能稳定提取的特征点很少，配准就变得脆弱。

FAST-LIO2 直接拿原始点云做配准。不区分边缘点、平面点，每一个点都参与残差计算。它的底气来自两个地方：一个是 ikd-Tree 让全局地图最近邻查询足够快，另一个是迭代更新让错误匹配在大批量迭代中被逐步剔除。

直接配准的思路和深度学习的某些想法很相似：特征提取是人工设计的归纳偏置，保留全部数据反而让模型更鲁棒。当然，它不是没有代价，原始点云离群点、动态障碍物都会干扰配准，所以后面还要做动态点剔除和距离阈值过滤。但整体上，这个取舍在复杂环境下明显更划算。

4. 实操落地：从零跑通 FAST-LIO2

4.1 环境准备：依赖、编译与常见坑

先说软硬件环境。FAST-LIO2 基于 ROS 开发，建议先装好 ROS 和对应版本的 Eigen、PCL、OpenCV。编译顺序有个讲究：先保证 ROS 核心和依赖库没问题，再编译项目本身。

我见过最多的编译失败都是这类问题：PCL 版本和已有的其他库冲突，或者 Eigen 的 C++ 对齐问题。解决方案是在 CMake 里加上-DNDEBUG关闭 Eigen 的调试断言，避免运行时读取未对齐内存地址崩溃。如果你用的编译器版本太老，建议升级到支持 C++14 的版本。

还有一个小坑是内存。编译工程时如果板载电脑内存只有 4G，开多线程并行编译很容易卡死。我一般先catkin_make -j2限制并行度，等第一次编译成功之后再考虑用更多线程增量编译。

雷达驱动也要提前准备好。如果你用的是固态激光雷达，需要先编译对应的 ROS 驱动，并且确认点云话题能正常发布。驱动和 FAST-LIO 的时间戳不同步，是隐藏得很深的坑，后面会细说。

4.2 数据集试跑与效果验证

没有真机之前，最好的验证方式就是跑公开数据集。找一段包含校园、室内、动态行人场景的数据包，离线回放给算法吃。我用过好几个开源数据包，包括小型手持设备扫描的室内数据和中大型车辆跑园区场景的数据，效果差异很大，建议多试几个，不要只盯着一个跑通了就以为万事大吉。

启动流程可以这样组织：先启动驱动节点，再启动 FAST-LIO2 节点，最后用rosbag play --clock回放数据包。注意一定要带上--clock选项，否则时间戳跳变会导致 IMU 积分混乱，地图直接飞掉。这是一个很多人会踩的坑。

运行过程中观察 RViz 里的点云地图和轨迹。好的效果应该是：轨迹连续平滑，重复扫描区域点云重叠良好，地图边缘清晰。如果轨迹出现突然的跳变，或者地图产生明显分层，优先检查参数和外参，不要急着怀疑算法本身。

4.3 参数调整：从默认参数到适配自己的传感器

拿到新传感器组合，参数一定是需要调的。调参顺序比我个人经验是有讲究的：先外参、再体素与地图、最后噪声协方差。

外参是雷达到 IMU 的旋转和平移。这个值错一点，地图就会“拖影”甚至“重影”。你可以先用标定工具做离线标定，如果条件不允许，至少把旋转矩阵的初始值设准，平移量可以先给个量级正确的估计。外参不准时，调后续参数都没有意义。

体素大小直接决定地图密度。一个经验值是：近距离室内场景设 0.1 到 0.2 米，室外空旷场景可以放到 0.3 到 0.5 米。体素太密，地图点数量暴涨，内存和算力都受不住；体素太大，地图细节丢失，配准精度下降。这是一个很典型的取舍，没有万能值，只能根据你的场景反复试。

噪声协方差参数决定了 IMU 数据和激光数据的信任程度。如果你发现轨迹抖动明显，可能是激光噪声权重太低，把激光更新调强一些；如果你发现短时间预测发散，可以适当减小 IMU 噪声，让它更信任 IMU。这些参数在配置文件中都有对应项，调的时候一次只改一个，并且记录效果，不然调完根本不知道是谁的功劳。

5. 高频问题与排查实录

5.1 漂移、分层、回环错位怎么排查

建图最常见的问题是地图漂移和分层。漂移的表现是轨迹缓慢偏离真实路径，地图整体扭曲；分层的表现是同一个区域被重复扫过时，点云出现了明显的“两层皮”。

排查顺序我一般固定下来。第一步看外参是否准确，这是最容易被忽略的根因。第二步看时间戳，确认激光和 IMU 的时间没有明显延迟。第三步看退化场景，如果建图过程经过长走廊或开阔广场，漂移是大概率事件，这时可以尝试提高激光配准权重，或者在退化严重的方向人为减少自由度约束。第四步才是检查体素大小和帧率。

回环错位一般出现在长时间扫描后回到起始点的场景。如果没有回环模块，纯里程计累积误差会让首尾地图对不上。这时候需要考虑使用带回环的版本，或者在数据采集时多次往返，让算法有更多重叠观测。

5.2 初始化失败与 IMU 外参标定

初始化失败的表现通常是轨迹起飞、点云糊掉或者系统直接崩溃。原因多为 IMU 数据质量差、初始状态不准确、或者外参错误。

IMU 数据质量检查有一个很直接的手段：把设备静止放置，打印加速度计三轴输出。理论上模长应该近似等于重力加速度，且数据平稳。如果静止时数据乱跳，说明 IMU 没校准好或者数据处理有问题。

初始化还有一个技巧：启动时让设备静止几秒钟，给系统足够时间让加速度计确定重力方向，再做初始陀螺零偏估计。我见过不少项目急着开机就猛转设备，导致初始姿态收敛失败，后面全崩。

外参标定如果暂时没有条件做完整标定，可以用一种笨办法：先手动测量传感器之间的相对安装位置做初值，然后把设备放在已知结构的房间里来回扫描，观察地图是否清晰。如果地图出现结构性扭曲，就微调外参旋转量，每次调一点，直到地图质量明显改善。这个方法笨，但实测量很有效。

5.3 算力紧张时的调优经验

板载电脑算力有限的时候，FAST-LIO2 也得做“减法”。我自己在无人机上调试时，最常用的是这几招：降低点云处理频率，比如每隔两帧才做一次完整配准，中间帧用 IMU 预测顶住；增大体素大小，减少地图点总数；降低地图可见范围，只保留设备周围一定半径内的点参与配准。

还有一个容易被忽视的优化：压缩激光点数。如果雷达原始点云每一圈有几十万点，可以先做简单降采样再喂给算法。点数量少一半，配准时间能明显下降，而精度损失在大部分场景下并不大。

观察 CPU 占用时要注意：FAST-LIO2 的配准线程是主要瓶颈，如果你看到它的执行时间持续超过激光帧间隔，说明循环跑不满，需要按上面的方法做减法。实时性是硬门槛，调完参数后要始终关注每帧处理耗时，而不是只看地图好不好看。

6. 影响范围与行业扩展

6.1 它能用在哪些地方

FAST-LIO 系列的应用场景非常广。无人机领域的自主避障和精准降落到复杂环境建图，地面机器人的室内外一体化巡检，移动测绘背包的实时三维建模，自动驾驶中的低速园区定位，这些都是典型场景。

它的核心价值在于：把高精度激光和低成本的 IMU 组合在一起，用比较轻量的计算资源实现实时高精度定位与建图。尤其适合算力受限、传感器便宜、环境复杂的机器人产品。我看到不少创业团队直接拿 FAST-LIO2 做基线，在它上面叠加回环检测、地面分割和动态障碍物过滤，形成自己的定位导航原型。

这里要提醒一句：FAST-LIO2 本身不是“一切场景通吃”。如果地图规模超大、轨迹跨度几十公里，还是需要回环检测和后端优化模块。它的定位是高性能前段里程计和建图核心，不是完整的回环和后端系统。

6.2 与同类激光惯性方案的横向对比

把 FAST-LIO2 和 LIO-SAM、R3LIVE 这类方案放在一起对比，能更清楚它的位置。LIO-SAM 的特点是因子图和回环检测做得好，建大图更稳定，但它在帧间配准上相对保守，遇到复杂运动时精度不如迭代方案。R3LIVE 加了视觉信息，纹理丰富的环境能进一步提升精度，但代价是多一个相机，系统复杂度上了一个台阶。

FAST-LIO2 的优势在于简洁和鲁棒。它没有复杂的因子图后端，也没有视觉分支，就用一个迭代误差状态卡尔曼滤波把激光惯性的紧耦合做到极致。如果你的场景没有超长距离回环需求，它的精度和实时性平衡做得非常好。

从学术和工程生态来看，FAST-LIO 系列启发了大量后续工作，很多新方案都是先跑通 FAST-LIO2，再往上叠加自己的模块。它能成为社区里的“标准底座”，原因不只是精度高，更在于代码结构清晰、文档相对完整，这给二次开发省了很多时间。

最后分享一点个人体会

调试这套算法的过程中，我踩过最大的坑其实是时间同步。有一次地图在转弯时反复出现毛刺，我调了两天的外参和噪声协方差都没有起色，最后发现是雷达驱动的时戳不是真实传感器时戳，IMU 和激光之间有将近二十毫秒的延迟。修好这个问题后，地图一下子就干净了。所以拿到任何新传感器组合，第一件事永远是确认时间戳和坐标系外参，这两个问题不解决，后面所有调参都是事倍功半。

另一个小技巧是，调参时养成记录的习惯。每改一个参数，存一份对应的配置文件，截图记录地图效果。否则你会陷入“调来调去改回原样”的死循环。参数是组合拳，只有组合不变，你才能知道每个变量的贡献。

FAST-LIO 系列的价值，在于它把复杂的紧耦合多传感器融合做成了一套能实际跑起来、能改、能上线的东西。读懂了它的原理，你不仅会用它，还能在它上面玩出自己的花样。