我最初接触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_driverVelodyne就用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_iterationIESKF迭代的最大次数默认值一般是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这类工具量化评估轨迹精度哪怕只是一个粗略的数据也比纯肉眼靠谱得多。