我最初接触FAST-LIO2这个项目，是因为课题组要做一款室内外都能跑的巡检机器人，激光雷达和IMU的融合定位是刚需。当时对比了好几个开源方案，最后在FAST-LIO2上稳定跑通了，而且从源码到实车移植，踩了不少坑，也积累了不少理解。这篇内容就当作一次项目复盘，把FAST-LIO2这套LiDAR-Inertial Odometry系统的核心设计、实操步骤、参数调试和常见问题一次性讲透，给准备入坑或者已经在调里程计的朋友做个参考。
先说清楚这套东西到底能干什么。FAST-LIO2是港大MARS Lab开源的激光雷达惯性里程计，核心功能就是紧耦合激光雷达点云和IMU数据，实时估计传感器在三维空间里的位姿，同时增量式地构建一张全局点云地图。它最突出的特点有两个：一是“直接法”，不提取角点、平面点这些特征，直接在原始点云上做配准；二是用了自研的ikd-tree数据结构来维护地图，增量更新、动态删除，计算效率和内存占用都比传统做法好很多。对做机器人、无人车、无人机定位导航，或者手持三维重建设备的人来说，这套系统几乎是必看的参考实现。
1. 整体设计拆解：FAST-LIO2为什么值得研究
1.1 从LOAM到FAST-LIO2：激光惯性里程计的演进逻辑
要理解FAST-LIO2，最好先回顾一下它之前的主流方案。LOAM是激光里程计的经典之作，它把问题拆成两个算法并行跑：一个高频低精度的里程计做帧间匹配，一个低频高精度的建图做scan-to-map匹配。这种“双线程”结构后来被很多系统沿用，但它存在一个明显短板：没有引入IMU，在快速旋转、剧烈颠簸的时候，点云畸变很难处理，匹配容易发散。
后续的LIO-SAM、LINS这些系统开始把IMU加进来。LIO-SAM的做法是因子图优化，IMU预积分、激光里程计、GPS、回环检测都作为因子加入图里，精度很高，但框架偏重，在大规模地图上实时性压力较大。LIO-SAM本质上还是基于特征匹配，需要从点云里提取边缘点和平面点，这对雷达的扫描模式、点云密度、场景结构都有一定要求，特征退化环境下容易出问题。
FAST-LIO2的思路则更直接。它放弃了特征提取这一步，直接把当前帧的原始点云和全局地图做配准，用迭代误差状态卡尔曼滤波器（IESKF）完成状态估计。这套设计的好处在于：省掉了特征提取的算力开销和参数调优成本，对低线束雷达、非重复扫描雷达这些“特征不那么典型”的传感器也友好得多。用一句话概括，就是“不做特征工程，直接拟合原始几何”。
1.2 直接法配准：为什么“不用特征”反而是优势
很多刚开始接触FAST-LIO2的人会疑惑：不做特征提取，直接拿几万甚至几十万个原始点去匹配地图，计算量不是更大吗？
这里有个关键细节。FAST-LIO2的配准并不是暴力地把所有点都拿去做最近邻搜索，而是利用ikd-tree维护的全局地图，把当前帧每个点投影到地图上，计算点到局部平面（或者说点到近邻点拟合平面）的残差。这个残差模型和LOAM里的点到面约束很像，只不过LOAM只在地图点里找角点/平面点对应的特征约束，FAST-LIO2是逐个原始点去找它的近邻地图点，然后拟合一个平面来计算距离残差。
直接法带来的直接好处是：系统对点云的“质量分布”不敏感。特征法依赖环境中存在明显的角点、棱边，如果场景是空旷的草地、雪地，或者碰到只有单一平面的走廊，特征提取就抓瞎了。直接法则只要局部点云能拟合成面，就能提供约束，退化场景的鲁棒性高出一截。代价当然也有，就是每一帧的最近邻搜索量很大，所以ikd-tree的效率直接决定了系统能否实时跑。
1.3 和FAST-LIO1的区别：一次“减法”带来的性能飞跃
FAST-LIO2是从FAST-LIO1迭代过来的。FAST-LIO1同样使用IESKF，但前面有一层特征提取模块，取的是环境中几何特征明显的点。这个模块在Livox雷达上表现不错，但在Velodyne、Ouster这类旋转式雷达上，特征提取的效率和稳定性就没那么理想了。
FAST-LIO2干脆把特征提取模块整个拿掉，设计了一个“直接配准”的流程。论文里给的对比数据很直观，在同样的数据集上，FAST-LIO2的精度和FAST-LIO1相当，但在计算耗时上有明显下降，尤其是在点云规模较大的场景下，受益于ikd-tree的增量更新，整体效率提升非常明显。这种“功能做减法、结构做优化”的演进思路，在做系统设计的时候很值得借鉴，不是堆积模块就能变强，砍掉不必要的中间环节往往更有效。
2. 核心模块深度拆解：状态估计、ikd-tree与点云畸变处理
2.1 IESKF：迭代误差状态卡尔曼滤波到底在算什么
FAST-LIO2的状态估计核心是IESKF。可以先把它理解成一个“能应对非线性系统的卡尔曼滤波器”，只不过它估计的不是状态本身，而是状态的误差量。
具体的状态向量包含IMU的姿态、位置、速度、陀螺仪零偏和加速度计零偏。流程上，每一帧激光数据到来之前，系统用IMU的角速度和线加速度做状态传播，得到当前时刻的预测位姿。由于IMU的频率通常有200Hz甚至更高，这期间会积分出很多中间帧，也就得到了点云扫描过程中每个时刻的近似的传感器位姿。激光帧到达后，把当前帧点云按各自时间戳对应的位姿变换到全局坐标系，和ikd-tree地图做配准，计算点面残差，再用迭代卡尔曼更新对预测状态进行修正。
这里“迭代”两个字是精髓。普通的扩展卡尔曼滤波只做一次线性化，遇到初始误差大的情况容易发散。IESKF在更新步骤里反复线性化和更新，类似于高斯牛顿迭代，把配准问题和状态估计问题融合在同一个框架里求解。这样即使初始化不够准，或者运动比较剧烈，系统也能在几次迭代里收敛回来，鲁棒性比单次EKF要好很多。
3.2 ikd-tree：增量式地图管理的核心设计
ikd-tree是整个系统性能的关键，值得单独拿出来讲。传统的静态KD-tree建好后就不能动了，每来一帧新点云，如果要把新点插入，通常需要重建整棵树，复杂度很高。FAST-LIO2用的ikd-tree支持增量式点插入、动态点删除和自动重平衡，三个特点对应三个实际需求：
- 增量插入：每帧新点云直接插入现有树中，而不用重建整棵树。
- 动态删除：当机器人移动后，旧地图点可能距离当前位置很远，这些点对配准没有贡献，还拖慢最近邻搜索。系统会基于当前位姿把地图范围之外的旧点标记删除，控制地图规模。
- 自动重平衡：长时间增量插入可能让树退化，ikd-tree和平衡二叉树类似，维护平衡因子，在局部子树失衡时自动做重平衡，保证查询复杂度稳定在O(log n)级别。
ikd-tree的存在，让FAST-LIO2可以对超大场景做持久化的地图管理和配准。实测中，我在一个约几百米长的园区环境里跑，地图点总量几十万，每帧的最近邻搜索仍然能保持实时，这在静态KD-tree重建方案里是很难想像的。
3.3 点云畸变补偿：IMU传播带来的“免费午餐”
点云畸变这个问题，做过激光SLAM的朋友肯定不陌生。机械旋转式雷达扫描一帧需要几十毫秒甚至上百毫秒，这期间机器人一直在运动，如果把这帧内所有点都当成同一时刻采样的，配准结果就会有偏差，严重的时候地图边缘会糊。
FAST-LIO2对畸变的处理非常优雅。因为IMU在以几百赫兹的频率做状态传播，系统天然知道点云扫描过程中任意时刻的传感器位姿，所以在把当前帧的点变换到全局坐标系时，每个点都使用它自己那一时刻的位姿去变换，而不是用帧头或帧尾的位姿。这就相当于在配准前已经把畸变“隐式”补偿掉了，不需要单独做一次运动补偿运算，也不用像有些方案那样需要额外做线性插值。
我实际对比过，在快速摇晃传感器的情况下，FAST-LIO2输出的地图边缘依然清晰，没有明显的拖影。对于用手持设备扫室内环境这种场景，这个特性非常实用。
4. 传感器硬件特性对FAST-LIO2的影响：从光学架构到点云质量
虽然是算法项目，但FAST-LIO2对传感器输入的敏感度很高，实际部署的时候，激光雷达的硬件架构和点云质量直接决定系统表现。这里结合激光雷达的光学系统设计，讲几个容易踩的坑。
4.1 同轴与非同轴架构：点云分布差异很大
机械旋转式激光雷达是典型的非同轴架构，发射和接收光路在旋转机构上，点云按固定的角分辨率均匀分布。这种雷达的点云覆盖范围大、分布规则，FAST-LIO2适配起来很轻松，只需在配置里把点云类型设成XYZI或XYZIRT。
而Livox系列用的则是非同轴但带旋转棱镜的方案，非重复扫描模式让点云在空间上呈现“花瓣式”覆盖，长时间积分后能形成非常致密的扫描图案。这种特性对直接法很友好，因为近邻点更多、拟合成平面的约束更可靠。但要注意的是，Livox雷达的视场角通常比较小，比如Horizon视场只有81.7°×25.1°，如果传感器安装方向不对，很容易出现视野盲区，激光里程计在这种盲区下约束不足，会造成漂移。装雷达之前最好先根据机器人可能的运动范围，想想雷达朝哪个方向能覆盖到最多的环境结构。
另外还有一些激光雷达采用同轴收发共光路的设计（monostatic），发射和接收共用一套光学系统，这类方案通常体积更小、光路对准更稳定，但在近距离探测盲区和远距离信噪比上各有取舍。同轴架构在远距离时信号衰减明显，点云噪声变大，这直接导致FAST-LIO2匹配时点到面残差变大，长走廊加上远距离高噪声点云，精度会明显下降。相反，双站式（bistatic）架构在近距离有更好的信噪比，但光路对准要求高。选传感器时不要只看线数，要结合实际使用场景，近距离室内为主就选近距离盲区小的，室外远距离就优先考虑远距离点云噪声低的。
4.2 点云噪声、射程与扫描频率的匹配原则
FAST-LIO2对点云噪声不是无限鲁棒的。点云噪声大，意味着拟合出来的平面参数也不可靠，残差里就多了很多错误约束，严重时会引起状态估计偏差。我自己踩过一个坑：某款雷达在近距离测距时存在系统性偏移，导致机器人静止时地图边缘反复抖动，最后是标定了测距误差补偿表才解决。
扫描频率也要注意。高线束雷达帧率通常只有10Hz，低线束雷达可以到20Hz甚至更高。FAST-LIO2每帧激光数据都会触发一次状态更新，如果雷达帧率太低，两帧之间IMU传播时间太长，误差积累就会增加。反过来，帧率太高、点云又密，计算负载也会上去。一般10Hz~20Hz范围内是FAST-LIO2的最佳工作区间，低于5Hz的时候就要考虑是不是该换传感器方案了。
5. 从零跑通FAST-LIO2：环境配置、数据测试与自采标定
5.1 编译环境和依赖项：这些都是必须的
FAST-LIO2的官方代码基于ROS1编写，常用的运行环境是Ubuntu 18.04或20.04，加ROS Melodic或Noetic。核心依赖有PCL、Eigen3，另外需要对应激光雷达的驱动，比如Livox雷达就用livox_ros_driver，Velodyne就用velodyne_driver。
编译过程本身不复杂，但有几个细节容易出问题。Eigen版本要保证3.3以上，太老的版本在编译时会出现一堆模板报错。PCL的版本尽量和ROS发行版自带的匹配，不要自己从源码装一套新的，否则cmake查找依赖时可能会出现混乱。我第一次编译时，就是因为在系统里装了多个版本的PCL，链接阶段各种符号冲突，最后把非系统自带的PCL卸掉才顺利通过。
官方仓库里给的编译命令很直接：
cd ~/catkin_ws/src git clone https://github.com/hku-mars/FAST_LIO.git cd .. catkin_make source devel/setup.bash编译成功之后，先跑官方提供的数据集验证流程。官方仓库里放了几个rosbag，下载完直接：
roslaunch fast_lio mapping_ouster64.launch rosbag play your_bag.bag就能看到实时的位姿轨迹和增量地图输出。这一步跑通了，说明环境没大问题，再开始用自己的设备。
5.2 自采数据集之前的“标定功课”：IMU内参、外参和时间同步
自采数据才是真正开始踩坑的地方。第一关是IMU标定，这一点怎么强调都不过分。FAST-LIO2对IMU内参噪声和零偏估计有自己的在线估计机制，但如果IMU本身没标定过，比如加速度计和陀螺仪存在明显的尺度误差、安装偏角，系统在线估计很难完全收敛。
建议先做静止放置采集，采集半小时以上IMU数据，用imu_utils或者Kalibr的imu标定流程，算出噪声密度和随机游走，填进yaml配置文件里。这个参数对滤波器表现影响很大，给一个离谱的噪声参数，滤波器会认为IMU数据非常不可信，完全依赖雷达匹配，反而不稳定。
外参标定同样重要，即激光雷达和IMU之间的相对位姿。官方提供了一组粗略标定方法，但精度一般，不少人在调试的时候发现地图倾斜或轨迹弯曲，最后查到是外参里有几度的偏差。推荐用lidar_align这个工具做离线标定，操作也不复杂：准备一个结构丰富、有棱有角的室内环境，手持设备慢速运动十分钟左右，记录rosbag，然后跑标定工具，它会输出雷达到IMU的旋转和平移。标定后的外参直接写进yaml里，效果立竿见影。
时间同步是个不太起眼但很致命的问题。FAST-LIO2假设激光雷达点云时间戳和IMU时间戳是同一时钟基准，如果硬件上是两个独立设备，时间戳不同步，相当于每帧点云和IMU数据之间有一个未知的延迟，严重时初始化就发散。解决办法有两种：硬件上可以在嵌入式端做PTP同步，或者软件上用time_offset校准工具，在数据预处理阶段对齐时间戳。我在自制设备上遇到过10ms级别的时间偏差，表现就是静止时位姿漂移很小，但一走起来轨迹就歪，排查了很久才发现是时间戳问题。
5.3 参数配置要点：max_iteration、帧率与地图更新
yaml里的参数看着多，但真正影响核心表现的并不多。常用的几个：
- max_iteration：IESKF迭代的最大次数，默认值一般是3到5。如果传感器噪声大、初始化不准，可以适当调大，代价是计算更慢。
- 地图范围相关参数：比如local_map_size之类，控制ikd-tree保留的地图大小。室外大场景可以调大，但要注意内存占用。
- 外参矩阵和噪声协方差：前者必须准确标定，后者可以先用官方默认值，再根据实际数据微调。
- 降采样分辨率：如果点云太密，配准计算压力大，可以设置合理的体素降采样，比如0.5m分辨率，保持地图点密度适中。
原则上，参数调整要一次只改一个，改完观察轨迹和地图效果，不要同时动好几个，否则出了问题都不知道是哪一步引起的。我见过有人把噪声协方差改大了十倍，系统直接变得“迟钝”，位姿更新滞后，地图拖影严重，其实就是参数失配。
6. 常见问题与排查技巧实录
6.1 一启动位姿就飞掉，或者初始化失败
这是最高频的问题，几乎每个第一次跑FAST-LIO2的人都会遇到。最常见的原因是IMU数据和雷达数据时间戳没对齐，其次是外参给得特别离谱，再有就是IMU内参参数填错。
排查建议是先离线分析rosbag，查看话题的时间戳跨度，确认IMU和雷达的时间戳是否匹配。再检查外参标定结果，看看旋转和平移向量是否在合理范围内。如果这几项都正常，可以试着把设备放在静止平面上启动，给系统一个干净的初始化条件。
6.2 建图出现重影、轨迹漂移
地图重影通常有两个来源：一是退化场景，比如狭长走廊、空旷停车场，激光约束在某个方向上不足，里程计就会沿退化方向漂移，地图看起来就是“糊”的。这种情况只靠FAST-LIO2本身很难完全解决，常见做法是加回环检测和全局优化，后续接SC-PGO或者Faster-LIO这种带回环的方案，效果会好很多。
另一个来源是传感器外参漂移，尤其是经常拆装传感器的设备，每次安装后外参都会有轻微变化，如果不重新标定，时间久了地图就会越来越歪。建议养成习惯：设备拆装之后，先跑一遍快速标定再出门采集数据。
6.3 计算延迟高、帧率跑不满
如果是在嵌入式设备或者算力有限的工控机上跑，计算负载问题很突出。优先检查降采样分辨率是不是太小，过密的点云会让ikd-tree查询和残差计算都变慢。其次可以把激光雷达的扫描频率适当降低，或者关闭可视化窗口，实测中RVIZ的渲染开销也很大。还有一个小技巧是调整ikd-tree的平衡因子，加速重平衡过程，但一般不建议动，除非你明确知道瓶颈在树维护上。
我最后一次优化的时候，把降采样从0.2m调整到0.4m，帧率从8Hz提升到了15Hz，地图质量肉眼几乎无差别。对于大多数机器人应用，15Hz的里程计输出已经完全够用。
6.4 快速旋转和剧烈颠簸时的处理
FAST-LIO2对快速运动鲁棒性比较强，但也不是无限度的。如果设备旋转角速度过快，一帧激光点云覆盖角度跨度太大，畸变补偿的近似模型也会出现误差。实际测试中，手持设备快速甩动时，轨迹偶尔会有少量跳变，降速或增加IMU频率可以缓解。对无人机这类高速运动载体，建议选择高帧率IMU，同时确保陀螺仪量程足够，不然IMU数据在剧烈转动时已经饱和削顶了，神仙算法也救不回来。
7. 一些值得后续尝试的扩展方向
FAST-LIO2本身是一个干净的里程计系统，它不做回环检测，不做全局优化，官方定位就是“前端”。这个定位其实很清晰，也方便了二次开发。
最常见的扩展是接回环检测模块，用FAST-LIO2的地图做scan context描述子，在后端维护关键帧，检测到回环就做位姿图优化，把整个系统的全局一致性提上来。这类方案在GitHub上有很多实现，比如FAST-LIO2_LC、FAST_LIO_SAM等，都是直接在FAST-LIO2基础上加回环和因子图，改动不大但效果明显。
另外就是多雷达融合方向。FAST-LIO2的ikd-tree结构和直接法配准天然适合多雷达输入，只需要把多个雷达的点云统一到同一个外参坐标系下，然后合并成一帧输入。不过多雷达的时间同步和外参标定复杂度更大，适合有一定经验的开发者尝试。
还有人在FAST-LIO2基础上做语义分割和动态物体剔除，先跑语义模型识别动态目标，再把动态点从地图里滤掉，避免动态物体残差污染位姿估计。这在城区道路场景里很实用，毕竟真车测试时，来往车辆行人都是动态点。
我自己后续的计划是尝试把FAST-LIO2接上惯性导航系统做紧耦合的卫导融合，用它在城市峡谷的弱GNSS场景下做连续定位。这个方向目前开源实现还不多，值得持续关注。
8. 写在最后：我对FAST-LIO2的一些个人体会
从工程角度看，FAST-LIO2给我的最大启发是“架构简洁”。整个系统没有堆砌特别玄的模块，但每个设计都踩在点上：直接法省掉了特征工程的调参负担，ikd-tree解决了地图维护的性能瓶颈，IMU传播顺带把点云畸变问题消化掉了。这种环环相扣、彼此支撑的系统设计，比单纯堆精度指标更值得学习。
如果你正准备入坑激光惯性里程计，我建议不要只跑官方的demo就结束，一定把源码读一遍，尤其是IESKF更新流程、ikd-tree插入删除逻辑、激光帧和地图的配准残差模型这三块。读懂了这三块，你对整个激光SLAM前端的理解会上一个台阶，后面再去看LIO-SAM、Point-LIO这些系统，会顺畅很多。
最后再分享一个小技巧。调试FAST-LIO2的时候，一定要保留好rosbag和对应的可视化，每次修改参数之前先保存当前的地图和轨迹，修改后再对比，不要凭感觉判断好坏。单靠肉眼观察RVIZ界面，很容易被视觉欺骗，尤其是地图缩放到远处时，几厘米的漂移根本看不出来。用evo这类工具量化评估轨迹精度，哪怕只是一个粗略的数据，也比纯肉眼靠谱得多。
