1. 从“盲人摸象”到“心中有图”：为什么我们需要激光雷达惯性里程计？

想象一下，你被蒙上眼睛，在一个完全陌生的房间里行走。你只能依靠两种感觉：一是你双脚迈出的步伐（这就像惯性测量单元IMU，告诉你移动的速度和角度变化），二是你伸出手臂，偶尔触摸到墙壁或家具（这就像激光雷达LiDAR，给你一瞬间的精确位置信息）。如果只靠步伐，走几步后你就会因为微小的误差累积而彻底迷失方向；如果只靠偶尔的触摸，你无法知道两次触摸之间自己是怎么移动的。激光雷达惯性里程计（LIO）要解决的，就是这个“蒙眼行走”的定位难题。它把IMU的高频但会漂移的运动推测，和激光雷达低频但绝对精确的位置测量，紧紧地“绑”在一起，实时为我们构建出周围环境的地图，并同时确定自己在这张地图中的精确位置。这个过程，就是同步定位与建图（SLAM）的核心。

在机器人、自动驾驶、无人机等领域，这个能力是基石。没有稳定可靠的定位，一切自主导航都是空谈。早期的方案，比如单纯用激光雷达做匹配，计算量巨大，在快速运动时容易“跟丢”；而单纯依赖IMU，几分钟后误差就可能大到离谱。因此，将两者数据“紧耦合”起来的LIO算法，成为了高动态场景下的刚需。今天我们要聊的FAST-LIO系列，就是这个领域里一条非常耀眼的技术演进线。它从最初的FAST-LIO，到引入革命性数据结构的FAST-LIO2，再到追求极致速度的FASTER-LIO，每一次迭代都直指实际应用中的痛点：如何更快、更准、更稳地“看清”世界并“知道”自己在哪。我自己在调试移动机器人时，就深刻体会过算法效率带来的天壤之别——一个优化不佳的LIO，能让你的机器人CPU占用率飙升，建图卡顿，最终在转弯时“砰”地一声撞上墙。而一个高效的LIO，则能让机器人在复杂环境中行云流水，仿佛拥有了直觉。

2. FAST-LIO：为紧耦合与效率定下基调

FAST-LIO这个名字，开门见山地表明了它的追求：快。它的出现，在当时很大程度上优化了激光雷达惯性里程计的计算效率，为后续的发展奠定了坚实的框架基础。它主要做了三件关键的事情，我把它们比作是给蒙眼行走者装备的三件“神器”。

2.1 第一件神器：IMU与雷达的“紧耦合”

在FAST-LIO之前，很多方法采用“松耦合”，即IMU和激光雷达各自独立处理，最后再把结果简单融合。这就像你的步伐感知和触觉感知各算各的，最后再对答案，容易产生矛盾和信息损失。FAST-LIO采用了紧耦合策略，将IMU的状态直接纳入到激光雷达的观测模型中。具体来说，它使用了一个叫迭代扩展卡尔曼滤波（IEKF） 的算法框架。在这个框架里，系统的“状态”（就是机器人的位置、姿态、速度等）主要由IMU的高频数据来预测（称为状态方程）。而激光雷达扫描到的每一个特征点（比如一个墙角、一个平面），都被当作是对这个系统状态的“观测”（称为观测方程）。

IEKF的精妙之处在于“迭代”。传统EKF（扩展卡尔曼滤波）在每一帧数据到来时，只做一次线性化估计。但激光雷达的观测模型（点云匹配）是非线性的，一次线性化可能不准。IEKF则会在当前帧数据上反复迭代计算，每次都用更新后的状态重新线性化观测模型，直到估计的变化量小到可以忽略，认为收敛了。这样得到的后验状态估计，要比单次线性化准确得多。FAST-LIO将这个过程与每一帧激光雷达数据紧密同步，实现了深度的传感器融合。

2.2 第二件神器：为运动按下“回放键”（反向传播）

激光雷达扫描一帧数据是需要时间的（通常是几十毫秒）。在这段时间里，机器人本身是在运动的。这就导致一帧点云里的几万个点，其实是在不同时刻、不同位姿下被采集到的。如果直接用这“扭曲”的点云去和地图匹配，就像用一张拍糊了的照片去人脸识别，肯定不准。FAST-LIO的第二个创新点就是运动补偿，它用了一种非常聪明的方法：反向传播。

算法先利用IMU数据，估计出激光雷达帧扫描结束时刻的机器人状态。然后，它沿着时间轴反向推算，利用IMU测量和状态估计，将每一个激光点都“搬回”到它被发射出去的那个精确时刻所对应的机器人位姿上。这个过程，相当于把因为运动而扭曲的点云给“捋直”了。经过运动补偿的点云，再用于与地图匹配，精度会得到质的提升。我在实际项目中对比过，开启和关闭运动补偿，在机器人旋转扫描时，建图边缘的清晰度差异非常明显。

2.3 第三件神器：把大矩阵求逆变成小矩阵求逆（维度变换）

这是FAST-LIO在计算效率上最关键的“魔法”。在卡尔曼滤波的更新步骤中，有一个核心操作是计算“卡尔曼增益”，这需要求一个矩阵的逆。在传统的LIO公式里，这个需要求逆的矩阵，其维度等于观测的维度。什么是观测维度？就是激光雷达提取的特征点（比如点和面特征）的数量。一帧点云里，特征点动辄成千上万，这意味着你要求一个几千维矩阵的逆，计算量是天文数字。

FAST-LIO通过巧妙的数学推导，变换了卡尔曼增益的计算公式。在新的公式里，需要求逆的矩阵维度变成了状态量的维度。状态量有多少？无非是位置、姿态、速度、零偏等，通常就十几二十维。从一个几千维的求逆问题，变成一个十几维的求逆问题，计算复杂度瞬间降低了几个数量级！这个改动看似只是公式变形，但对算法实时性的贡献是决定性的。它让在嵌入式计算平台（如Jetson AGX）上实时运行高精度的LIO成为了可能。我至今还记得第一次在树莓派4B上成功跑起FAST-LIO原型时的那种兴奋——虽然帧率不高，但它证明了这条技术路线的巨大潜力。

3. ikd-Tree：让地图更新从“重建家园”到“添砖加瓦”

FAST-LIO展示了紧耦合框架的威力，但它还有一个性能瓶颈：地图管理。LIO算法需要维护一个全局地图（通常叫submap或local map），用于和新来的激光帧进行匹配。这个地图需要不断地插入新的观测点，删除旧的或无效的点。最朴素的方法是使用一棵静态的k-d树来组织这些地图点。但问题来了：每次插入或删除点，静态k-d树的结构就会“不优”，为了保持高效的查询速度，你必须定期地、或者当树结构太差时，将整棵树推倒重建。这就像你每往书架上放几本书，就要把全部书籍搬下来重新整理一遍，效率极低。

FAST-LIO2的突破，很大程度上归功于它引入了一种全新的数据结构：增量k-d树（ikd-Tree）。ikd-Tree的设计目标，就是实现地图的高效增量更新。

3.1 ikd-Tree的核心思想：给树节点加上“智能标签”

ikd-Tree在传统k-d树的节点上，额外维护了几个属性，比如deleted（该点是否被逻辑删除）、treedeleted（该子树下是否有被删除的点）、pushdown（删除标记是否需要向下传递）、treesize（子树有效节点数）等。这些属性让它拥有了“惰性操作”的能力。

• 插入：新点到来时，ikd-Tree像普通k-d树一样找到叶子节点位置插入。但它会实时地检查从根节点到插入路径上每个节点的子树大小，如果发现某个子树因为插入变得“不平衡”（比如左右子树节点数差异过大），它不会立即重建整棵树，而是会标记这个区域。只有当不平衡程度超过某个阈值，并且系统有空闲计算资源时，才会针对这个不平衡的子树进行局部重建。这就像书架的某一层太满了，你只整理这一层，而不是整理整个书架。
• 删除：当需要删除一个旧点时（比如点移出了地图范围），ikd-Tree并不真的从内存中抹去这个节点，而是简单地将其deleted标记置为真。在后续的查询操作中，算法会自动跳过这些被标记删除的点。只有当被删除的点积累到一定数量，导致树性能下降时，才会触发一次局部的“垃圾回收”和重建。这极大地减少了频繁删除带来的开销。
• 查询：进行最近邻搜索时，ikd-Tree能利用treesize和invalidnum等信息，智能地剪枝那些完全由无效节点构成的子树，加速查询过程。

3.2 性能的飞跃：从数量级上感受差异

根据原论文的实验，在FAST-LIO系统里运行大约7分钟，对比静态k-d树和ikd-Tree：

• 插入操作：ikd-Tree比静态树快得多，因为避免了全局重建。
• 查询+更新操作：这是LIO中最频繁的操作（每帧激光数据都需要查询地图中的最近邻点）。ikd-Tree比静态树方案快了大约7到8倍！

这个提升是颠覆性的。它意味着算法可以将更多计算资源留给状态估计等核心任务，从而允许处理更高线束的激光雷达（如128线），或者在更弱的处理器上运行。我在将项目中的地图管理模块从静态Octree切换到ikd-Tree后，整个SLAM线程的CPU占用率下降了近40%，帧率稳定性也得到了大幅提升，机器人快速转弯时的建图抖动明显减轻。

4. FAST-LIO2：当“去特征化”遇见“ESKF前端”

有了ikd-Tree这把利器，FAST-LIO2的诞生就显得水到渠成。它在FAST-LIO的基础上，做出了两项意义深远的改进，进一步简化了流程并提升了鲁棒性。

4.1 告别“特征提取”：拥抱原始点云

FAST-LIO需要从原始点云中提取“特征点”，比如角点（线特征）和平面点（面特征）。这个过程本身需要计算，而且特征提取的好坏非常依赖于环境——在特征匮乏的长走廊或空旷地带，提取不到足够好的特征，算法性能就会下降。

FAST-LIO2做了一个大胆的决定：不再进行特征提取，直接使用所有原始点云。这听起来有点反直觉，因为点云数据量更庞大了。但它的底气来自于ikd-Tree带来的高效地图管理能力。使用全部点云有两个巨大好处：

1. 信息不丢失：每一个激光点都携带信息，尤其是那些不属于明显角或面的点，它们对约束表面形状同样重要。使用全点云，相当于利用了所有可用的几何信息。
2. 更鲁棒：不再依赖特征提取这个可能失败的环节。无论环境是结构化办公室还是杂乱无章的仓库，算法都能一视同仁地工作。在实际的仓库物流机器人项目中，这一点至关重要，因为现场充满了随机的货箱和托盘，标准的特征提取算法经常“失灵”。

4.2 前端配准的进化：从IEKF到ESKF

FAST-LIO2的另一个核心变化，是在前端点云配准中使用了误差状态卡尔曼滤波（ESKF）。虽然FAST-LIO也用了迭代的思想（IEKF），但FAST-LIO2将ESKF更深地整合到了点云匹配的环节。

简单理解，ESKF的核心思想不是直接估计机器人的“真实状态”，而是估计“真实状态”与一个“名义状态”之间的误差状态。这个误差状态通常很小，其动力学模型更接近线性。在点云配准中，我们不断迭代地估计这个误差状态（即当前点云位姿与地图之间的微小偏差），并用它来修正名义状态。

这样做的好处是，在迭代求解时，数值计算更加稳定，对非线性问题的处理能力更强。FAST-LIO2将这种ESKF迭代作为激光雷达数据的前端配准器，与后端的紧耦合滤波器协同工作，形成了更优雅、更稳定的两级估计架构。实测中，FAST-LIO2在剧烈运动下的收敛速度更快，对于初始值偏差的容忍度也更高。我记得有一次测试，故意给了一个很差的初始位姿，FAST-LIO可能会发散，但FAST-LIO2却能自己“拉”回来，这种鲁棒性让人印象深刻。

5. FASTER-LIO：速度的终极追求与ivoxel的革新

如果说FAST-LIO2已经很快了，那么高翔团队提出的FASTER-LIO，则是在追求极致的道路上又迈出了一大步。它的目标非常明确：在保持相当精度的前提下，进一步成倍地降低计算时间。它的改进主要集中在数据结构和一些工程优化上。

5.1 地图结构的再次革命：从ikd-Tree到ivoxel

FASTER-LIO最大的亮点，是提出了 增量体素（ivoxel） 来替代ikd-Tree作为地图管理的数据结构。为什么又要换？因为树结构（即使是ikd-Tree）的查询复杂度是O(log N)，而体素（voxel）在理想情况下可以达到近似O(1)的查询速度。

ivoxel的基本思想是将空间划分为固定大小的立方体格（体素）。每个体素就像一个“小桶”，存储落在该桶内的所有点。当需要查询一个点的最近邻时，只需要查找这个点所在的体素及其相邻的26个体素（3x3x3的邻域）内的点即可，无需遍历整个地图。这就像你要在图书馆找一本书，如果书按楼层和房间编号摆放（体素），你直接去那个房间找就行；如果是按分类字母排序（树），你可能需要在几个书架间来回查找。

但传统体素也有问题：它是固定网格，无法动态增删点，且会浪费内存。ivoxel做了关键优化：

• 增量管理：它使用类似LRU（最近最少使用）的被动删除策略。当需要为新点腾出空间时，系统会自动淘汰掉那些最久未被访问的体素（或体素内的点）。这非常符合SLAM的特点——我们通常只关心当前所在位置附近的地图。
• 高效检索：查询时，它直接定位到对应的体素邻域，速度极快。
• 内部结构优化：在每个体素内部，当点数较少时，使用简单的线性存储；当点数超过阈值时，切换为一种更高效的PHC（Pseudo Hierarchical Clustering） 数据结构来组织，以加速体素内部的最近邻搜索。

5.2 精度与速度的权衡：一个务实的选择

FASTER-LIO的论文实验结果非常有意思：与FAST-LIO2相比，在大多数公开数据集上，其定位精度几乎持平，但处理时间减少了一半左右。这清晰地表明了FASTER-LIO的设计哲学：通过优化数据结构和工程实现，用极小的精度代价，换取巨大的速度提升。

在 robotics 领域，这通常是一个明智的 trade-off。对于许多实时性要求极高的应用，如高速无人机、自动驾驶汽车，几十毫秒的延迟可能就是撞上与避开的区别。FASTER-LIO 将单帧点云处理时间压缩到极致，为系统留下了更多的计算余量去处理感知、规划等其他任务，或者允许使用更便宜的硬件平台。

我在部署一个需要实时避障的无人机项目时，就深有体会。最初使用FAST-LIO2，在树莓派上勉强能达到10Hz，偶尔在复杂场景会掉帧，导致避障指令延迟。换用FASTER-LIO后，处理频率稳定在20Hz以上，整个系统的响应变得丝滑，飞控也更加跟手。这种从“能用”到“好用”的体验提升，对于产品化至关重要。

6. 实战指南：如何选择与快速上手

面对FAST-LIO、FAST-LIO2、FASTER-LIO这三个优秀的算法，新手可能会困惑：我到底该用哪个？这里我结合自己的踩坑经验，给大家一些选择的建议和上手的关键步骤。

6.1 算法选型：没有最好，只有最合适

• 追求高精度和理论完整性：如果你的研究侧重于算法原理、状态估计的理论边界，或者你的应用场景对精度要求极高，且计算资源相对充裕（比如有高性能工控机），那么从FAST-LIO开始学习是一个非常好的选择。它的代码结构清晰地展示了紧耦合、IEKF、运动补偿等核心思想，是理解整个系列的基础。
• 平衡鲁棒性与实用性：对于大多数机器人应用，如室内服务机器人、仓储AGV、移动底盘等，FAST-LIO2 是目前社区中最流行、最稳健的选择。它去特征化的设计使其对环境适应性极强，ikd-Tree保证了高效运行，开源社区支持好，资料和调参经验丰富。如果你不确定选什么，闭着眼睛选FAST-LIO2通常不会错。
• 追求极致速度和资源受限：如果你的平台计算能力很弱（如树莓派、Jetson Nano），或者你的应用对延迟极度敏感（如竞速无人机、高速自动驾驶），那么FASTER-LIO 是你的首选。它用一点点的精度换来了巨大的速度收益，在边缘设备上的表现往往令人惊喜。

6.2 上手第一步：环境搭建与数据准备

无论选择哪个版本，准备工作是类似的。假设你使用Ubuntu和ROS，核心步骤如下：

1. 安装依赖：确保安装了ROS、PCL、Eigen等基础库。FAST-LIO系列对Eigen版本有一定要求，建议使用较新的版本。

   sudo apt-get install ros-<你的ros版本>-pcl-ros ros-<你的ros版本>-velodyne-pointcloud (根据你的雷达型号调整)
   

2. 下载并编译代码：从GitHub上克隆对应的仓库（如HKUST-Aerial-Robotics/FAST_LIO, hku-mars/FAST_LIO2, hku-mars/FASTER_LIO）。使用catkin_make进行编译。这里以FAST-LIO2为例：

   mkdir -p ~/catkin_ws/src
   cd ~/catkin_ws/src
   git clone https://github.com/hku-mars/FAST_LIO2.git
   cd ..
   catkin_make -j4
   source devel/setup.bash
   

3. 准备配置文件：这是最关键也最容易出错的一步。你需要根据你的传感器修改config目录下的YAML文件。主要修改两项：
   • lid_topic 和 imu_topic：确保与你的雷达和IMU发布的ROS话题名一致。
   • extrinsic_T 和 extrinsic_R：这是雷达相对于IMU的外参（平移和旋转矩阵）。这个参数必须通过标定精确获取，不准确的外参是导致算法失效的最常见原因。可以使用开源工具如lidar_imu_calib进行标定。
4. 运行与可视化：启动算法节点，并播放你的数据包（bag file）。

   roslaunch fast_lio mapping_<你的雷达型号>.launch
   rosbag play your_data.bag
   

   使用Rviz订阅/cloud_registered（注册后的点云）和/Odometry（里程计）话题，即可看到实时建图和定位轨迹。

6.3 调参心得：几个影响巨大的参数

• cube_side_length（地图范围）：这个参数决定了算法维护的局部地图有多大。设得太小，机器人容易丢失跟踪；设得太大，会增加ikd-Tree/ivoxel的负担。一般设为50-100米是一个不错的起点。
• time_offset_lidar_to_imu（时间偏移）：如果雷达和IMU的时间戳没有严格同步，这个参数可以手动补偿微小的硬件时间差。通常先用0，如果发现运动补偿后点云仍有拖影，再微调这个值。
• filter_size_corner 和 filter_size_surf（FAST-LIO特征滤波参数）：在FAST-LIO中，这两个参数控制提取的特征点的密度。增大它们会减少特征点数量，加快计算，但可能降低精度。需要根据雷达线束和环境复杂度权衡。
• 最重要的建议：先确保外参准确，再调其他参数。一个错误的外参会让所有调参工作变得毫无意义。在开始正式使用前，花时间做好传感器标定，是事半功倍的必要投入。

从FAST-LIO到FASTER-LIO，这条技术演进路线清晰地展示了机器人感知领域一个经典的发展模式：先通过创新的理论框架（紧耦合IEKF）解决核心问题，再通过革命性的数据结构（ikd-Tree）突破性能瓶颈，最后通过极致的工程优化（ivoxel，去特征化）追求落地应用的效率极限。每一个版本都不是对前一个的简单否定，而是在不同侧重点上的深化和拓展。在实际项目中，我常常会根据任务需求混合使用它们的思路，比如在FAST-LIO2的框架下，尝试借鉴FASTER-LIO的某些缓存和查询优化。理解它们背后的思想，比单纯调用代码更有价值。当你看到自己的机器人在未知环境中流畅地构建出清晰的地图时，你会觉得所有这些复杂的技术细节，都汇聚成了那一道代表“智能”的光芒。