FAST-LIO2原理与实车部署：直接配准、ikd-Tree与IESKF解析

去年年底做园区低速无人车的时候我在一楼连廊遇到一件挺头疼的事同样一套传感器组合换用某套基于特征提取的激光惯性方案时只要车一进连廊定位轨迹就开始往外飘偶尔还会在玻璃幕墙边原地打转。后来把前端换成FAST-LIO2问题一下就没了。FAST-LIO2全称 Fast Direct LiDAR-inertial Odometry是港大MARS Lab开源的一套激光雷达-惯性紧耦合里程计方案。它对传感器配置的要求不高不管是Livox固态雷达还是常规机械雷达配上普通IMU就能在CPU上实时跑。相比LIO-SAM这类从LOAM继承下来的特征法方案FAST-LIO2最核心的差异是放弃了传统的特征提取流程直接在原始点云层做配准再配合它自研的增量式KD树ikd-Tree做地图管理把鲁棒性和实时性同时推上去了。这篇文章我想把FAST-LIO2从原理到实车的完整链路梳理一遍。内容分成几块直接配准为什么比特征法扛打ikd-Tree在后台到底干了什么IESKF滤波器的融合逻辑以及从零跑通和实际测试中的各种坑。适合正在做移动机器人、无人机或者自动驾驶定位的同学也适合那些准备把激光惯性里程计从demo推向实车的人。1. 直接配准不做特征提取反而更稳这套算法为什么敢这么干1.1 传统方案先提炼特征再匹配问题出在哪先回顾一下LOAM、LIO-SAM这类主流方案的基本流程。它们通常分三步走第一步对原始点云做预处理去畸变、滤掉无效点第二步根据局部曲率把点分成角点和平面点这就是所谓的特征提取第三步拿提取出来的特征去做帧间或帧图匹配通过优化残差解算出位姿。这套流程跑在结构化环境里没有大问题问题往往出在特征提取这个环节。特征提取本质上是一种有损压缩——把一个场景里几万甚至几十万个点压缩成几百个有意义的角点和平面点。一旦环境本身缺乏明显几何特征比如长走廊、玻璃幕墙、灌木丛、开阔广场提取出来的特征点要么数量太少、要么质量不高、要么高度重复匹配约束一下就弱了。我在园区连廊那个场景就是典型的反面教材。连廊两侧是玻璃加瓷砖纹理信息在激光雷达看来几乎是一片光滑的反射面特征提取算法挑出来的平面点虽然不少但全都集中在同一类同质表面上约束方向严重缺失。结果LIO-SAM跑着跑着轨迹就在横向方向上越飘越远最后彻底偏出真实路径。这不是参数没调好的问题而是特征法在信息提取环节就已经把支撑位姿解算的约束信息丢掉了大半。后续无论怎么调匹配权重、优化核函数能用的信息就那么多很难补回来。1.2 直接配准的字面意思与实际含义FAST-LIO2的解法很直接既然特征提取会丢信息那我干脆不提取特征了直接用原始点云来做配准。这就是它论文标题里Direct这个词的来源。具体执行方式是这样的把当前帧里每一个有效点经过采样和预处理之后通过当前估计的状态变换到全局坐标系下然后在ikd-Tree维护的全局地图里搜索最近邻点以点到点的距离构建残差再用迭代误差状态卡尔曼滤波器去更新状态。这个过程中每个点都是一个潜在的约束源不再需要回答这个点是角点还是平面点这个问题。哪怕是墙上非常平坦的区域只要它和一个先前观测到的点形成了几何距离约束它就能参与优化。所以从信息利用的角度看FAST-LIO2是把特征的定义直接简化为点的原始坐标本身。只要这个点不是落在完全均匀分布、毫无区分度的表面上它就提供了至少一个方向上的约束信息。在弱纹理环境下它能拿到的有效约束数量可能是特征法的几十倍这是它在退化场景里明显更稳的根本原因。1.3 不提取特征计算量为什么还扛得住看到这里很多人会问每个点都去地图里找最近邻点云几万甚至几十万个点地图几百上千万个点这计算量不是爆炸了吗答案是两个手段叠加。第一个是采样和过滤FAST-LIO2在预处理阶段就会根据point_filter_num做间隔采样比如每隔4个点取一个单帧实际参与配准的点数通常会降到几千到一两万个已经很可控了。第二个是ikd-Tree这个数据结构本身的高效性——它负责把在高密度地图中查找最近邻这个高频操作的时间复杂度控制在接近对数的水平。这两点配合起来才让不提取特征直接全量配准这件事有了工程上的可行性。2. ikd-Tree增量式更新的地图管理是直接配准能实时跑的底气2.1 从静态KD树到增量维护把每次重建变成只动局部KD树是激光SLAM里做最近邻检索最常用的数据结构。它按维度轮流切分空间把点云组织成一棵二叉树查询一个点的最近邻平均只需要O(log n)的复杂度。但传统的静态KD树有一个硬伤它不支持高效的动态更新。一旦地图里新增了一帧点云理论上要么重建整棵树要么插入后树逐渐失衡导致查询效率下降。大佬们大多数时候的用法是定期重建——地图攒到一定程度整棵树推倒重来。这在点云规模小的时候没有问题但地图一旦涨到百万点级别重建一次的耗时就很可观了直接把实时性拉垮。ikd-Tree的做法是增量维护。它在插入新点的时候沿着树往下走找到合适位置挂上新叶子删除点的时候不马上物理移除而是先打上一个待删除标记。每个子树会记录自己的总点数和待删除点数当某个子树里待删除点的比例超过阈值或者树的高度失衡到一定程度就触发一次局部重建把那棵子树内部重新整理一遍。这整套路子可以理解成整理书架静态KD树是每次放新书进去都要把所有书拿出来重新排一遍ikd-Tree是平时只管往格子塞书删除的书先贴个标签不说破等到某个格子乱到影响找书效率了才把那一格单独整理一遍。这样既避免了频繁全量重建的浪费又保证了查询效率长期处于稳定状态。2.2 点管家的多重职责去重、降采样、区域维护ikd-Tree在FAST-LIO2里承担的角色远不止最近邻查询器它实际上是整个地图的管理者。新点插入之前树会先检查要插入的位置附近是否已经有足够近的点存在如果距离太近就丢弃新点。这个机制控制了全局点云的密度上限避免了不同帧的重复扫描导致同一个位置堆积大量几乎重合的点——否则地图点数量会随着运行时间无限膨胀实时性迟早崩盘。同时FAST-LIO2通过维持一个动态局部地图来控制内存和计算范围。它会以当前估计位置为中心只保留一定范围内的地图点超出范围的旧点会被标记删除。这个范围是由配置文件里的cube_len控制的默认设置通常足够覆盖绝大多数场景。这样整个ikd-Tree的规模就被限制在了一个有界的量级内不会说车跑了十公里地图里就攒了十公里的全部点云。2.3 实际增益百万点地图下的实时表现论文里给出的实测数据是地图累计到7500帧、点云规模在百万级别时FAST-LIO2在一颗普通的i7处理器上单帧处理时间可以控制在几十毫秒以内。这个数字和我自己实测的感觉差不多。换成静态KD树定期重建的策略在相同的点云规模下单帧处理时间会随着地图增长一路爬升很快突破100毫秒甚至更高实时性就保不住了。另外有个常见的认知误区是ikd-Tree比静态KD树检索更快。其实在树结构健康的前提下两者的单次最近邻查询速度差别并不大ikd-Tree真正的优势在于避免了大量重复的全量重建。所以评价ikd-Tree时不要只盯着单次查询耗时要看到它在长时间运行下维持整体性能稳定的价值。3. IESKF状态估计IMU高频猜激光低频纠正3.1 滤波器的分工逻辑FAST-LIO2的状态估计核心是迭代误差状态卡尔曼滤波器也就是IESKF。这套东西贯穿了FAST-LIO系列。先看整体分工。IMU的输出频率通常在一两百赫兹激光雷达帧率通常只有10赫兹左右。滤波器用同一个状态向量——包含姿态、位置、速度、陀螺仪零偏和加速度计零偏——把两者串联起来。IMU负责高频猜在两个激光帧之间利用IMU测得的角速度和加速度做积分不断往前传播状态和协方差。激光负责低频纠正每来一帧点云就和全局地图做一次配准把配准产生的残差拿来修正之前IMU积分累积下来的漂移。这就是紧耦合的含义。松耦合是雷达和IMU各自算出位姿再做加权融合紧耦合是IMU和激光共享同一个状态量IMU传播的结果直接影响激光配准的初值激光配准的结果反过来修正IMU的零偏估计两者在数学上拧成了一个整体而不是两条独立流水线最后拼一下。3.2 误差状态卡尔曼滤波为何更适合激光惯性系统IESKF里的误差状态是一个值得展开的点。它维护的不是状态的绝对值而是估计值与真实值之间的小偏差。姿态误差表示成三维旋转向量而不是四元数或者欧拉角。为什么这么做一个很实际的理由是四元数加法的结果不是合法的四元数直接在四元数上做卡尔曼滤波的线性更新会破坏单位的约束欧拉角又有万向锁问题在俯仰角接近90度的时候直接失效。旋转向量作为so(3)李代数上的元素天然就是一个三维向量可以自由加减它和真实姿态之间的映射又是平滑的。这样滤波器的所有线性化操作都变得干净且稳定。然后是迭代两个字。传统的EKF在做激光点云这种强非线性测量更新时如果初始状态误差比较大一次线性化可能不够准确更新结果会偏离真实解。IEKF的思路是做完一次更新之后用新的状态重新计算雅可比、重新构建残差再更新一次迭代若干轮。每轮都在误差更小的点附近做线性化精度自然会更好。配置文件里的max_iteration参数控制的就是这个迭代次数默认值一般是3次。3.3 从最大后验的角度理解激光-IMU融合把整个滤波过程放到最大后验估计的框架下看会更容易理解为什么参数设置对结果影响这么大。IMU传播给出的是一组带不确定性的状态先验可以理解成我根据之前的状态推测现在大概应该在哪里但这个推测会随时间变模糊。激光配准给出的是另一个独立的证据把当前帧点云按照预测状态变换到地图里点应该贴在地图表面上如果某个点距离最近邻地图点很远说明状态估计可能有偏差。两者都有不确定性。IMU的白噪声和随机游走参数决定了先验有多可信激光的测量噪声方差决定了点云残差有多可信。最终的状态解就是在不能偏离IMU先验太远和尽量让点云贴合地图这两个目标之间找一个加权平衡点。用生活化的说法就是耳听为虚眼见为实IMU短期很准但长期会飘激光点云单帧不一定完整但每次都在纠正绝对位置。滤波器给两者分配权重权重的大小由噪声参数决定。嫌IMU飘得太快就把IMU噪声参数调大一点让滤波器多相信激光觉得激光点云质量一般就把激光测量噪声调大让滤波器别那么敏感。4. 从零跑通FAST-LIO2环境、编译、数据集的完整链路4.1 依赖清单与编译顺序跑FAST-LIO2的环境要求并不高。我日常用的是Ubuntu 20.04加ROS Noetic搭配Eigen、PCL这些标准库就够了。如果用Livox雷达还需要先编译对应ROS驱动。整个编译链路里最容易被坑的就是编译顺序。先建工作空间把源码和驱动都拉下来mkdir -p ~/catkin_fastlio/src cd ~/catkin_fastlio/src git clone https://github.com/hku-mars/FAST_LIO.git git clone https://github.com/Livox-SDK/livox_ros_driver.git然后回到工作空间根目录编译。这一步有一个非常关键的选项cd ~/catkin_fastlio catkin_make -DCMAKE_BUILD_TYPERelease source devel/setup.bash一定要加-DCMAKE_BUILD_TYPERelease。不加的话默认是Debug模式编译出来的程序运行速度会慢好几倍直接表现就是跑官方bag的时候实时性跟不上、点云地图一卡一卡的。很多新手说FAST-LIO2怎么跑不实时八成就是栽在这。如果机器上装的是Ubuntu 18.04配ROS Melodic也是一样的流程。Eigen需要注意版本3.3.x以上基本没问题系统自带的一般都满足。PCL直接用apt装的系统版本就够了不建议自己源码编译PCL版本冲突的问题会让人非常头大。4.2 用官方bag验证流程编译通过之后最快的验证方式是下载官方示例bag。官方仓库的README里提供了M2DGR数据集和示例bag的下载入口建议先拿这个跑通一遍不要一上来就用自己的数据否则出了问题都分不清是代码问题还是数据问题。启动方式很简单roslaunch fast_lio mapping_avia.launch然后另开一个终端回放bagrosbag play m2dgr_xxx.bag启动launch文件之后程序会初始化ikd-Tree和滤波器等待雷达数据和IMU数据输入。rviz可视化可以加载FAST_LIO仓库里自带的fast_lio/rviz/fast_lio.rviz配置文件直接看到全局地图和轨迹。跑第一个bag的时候有一种这就跑起来了的感觉——不需要任何训练、没有任何标定参数要填默认配置一套直接出图。这也是FAST-LIO2在社区里口碑好的原因之一工程完成度确实高。下面这张表是常用的话题调试的时候需要心里清楚话题名内容/Odometry里程计输出的6自由度位姿/path可视化用的轨迹路径/cloud_registered注册到局部坐标系下的已配准点云/cloud_registered_body机体坐标系下的点云/cloud_effected实际参与配准的有效点rviz里的Fixed Frame建议设置成camera_init这样看到的是全局一致的地图和轨迹。如果你把Fixed Frame设成body系地图会跟着车动看起来就是点云绕着车转那是正常的不是代码跑飞了。4.3 自采数据的配置手册跑通官方bag之后换上自己的传感器数据就需要认真改配置文件了。配置文件的路径在launch文件里指定通常是一个YAML文件。核心参数如下参数含义注意事项lidar_type雷达类型1代表Livox固态雷达0代表机械旋转雷达scan_line机械雷达线数比如Velodyne VLP-16填16timestamp_unit点云时间戳单位0秒、1毫秒、2微秒、3纳秒填错直接出问题blind近距离盲区裁剪范围滤掉贴近雷达的无效点det_range最大探测距离滤掉太远的稀疏点point_filter_num点云采样间隔每隔N个点取一个值越大计算量越小filter_size_surf地图体素降采样尺寸控制配准精度的关键参数filter_size_map地图点密度控制影响内存占用和实时性extrinsic_T雷达到IMU的平移外参雷达坐标系原点在IMU坐标系下的位置extrinsic_R雷达到IMU的旋转外参激光帧到IMU帧的旋转矩阵自采数据最常见的翻车点有三个。第一个是时间戳单位填错。常见bag里点云的time字段可能是秒、毫秒、微秒、纳秒四种之一配置里一旦填错去畸变和运动补偿就会错乱表现就是点云撕裂或者快速旋转时出现螺旋状重影。第二个是外参方向搞反。extrinsic_T和extrinsic_R严格来说是雷达坐标系到IMU坐标系的变换如果拿的是手眼标定或者URDF里IMU在雷达系下的坐标需要做一次逆变换再填进去。第三个是IMU噪声参数完全照搬默认值。不同IMU的噪声特性差异很大后面第5章详细说这个问题。5. 实车实测的坑与排查链路从轨迹起飞到地图撕裂5.1 轨迹起飞一次完整的排查链路这是新手最容易遇到的严重问题。现象是启动算法后rviz里的Odometry轨迹瞬间飞出去或者跑几秒之后突然原地转圈、路径画出一朵花。我的排查链路是固定的按顺序逐项排除。第一步看rviz里的Odometry数值是否出现NaN。如果出现基本可以确定是滤波器发散先不要继续跑停下来查配置。第二步检查外参方向。把extrinsic_T和extrinsic_R的数值打印出来对照实际安装情况确认是雷达在IMU系下的坐标。我见过太多人把外参填成相反方向结果算法启动瞬间就崩。外参错90度状态估计大概率直接发散轨迹起飞几乎是必然。第三步检查IMU数据质量。用rostopic hz看IMU话题频率是否正常用rostopic echo看加速度和角速度数据是否连续、是否有突然跳变。有些自制的IMU转接板在初始化时会有大量异常数据这种情况先把bag断开重跑让滤波器稳定初始化再说。第四步确认启动时是否静止。FAST-LIO2在滤波器启动的时候初始状态协方差很大需要一个相对静止的几秒钟让滤波器收敛。如果启动就快速旋转或大幅运动第一帧点云和地图可能完全对不上后续再怎么优化也拉不回来。我自己跑实车时都要在启动前专门停两三秒再发车。5.2 点云撕裂和叠影时间戳问题占大头跑了一段时间后出现的地图叠影或墙体错位大多数人第一反应是配准出了问题其实时间戳原因的占比很大。叠影的典型表现是静止站在原地点云地图里的同一面墙出现两层甚至多层错位或者车辆快速转弯时点云出现螺旋状拖尾。排查链路我建议这么走。先确认配置里的timestamp_unit和bag里的实际时间戳单位一致。常有情况是bag存的是毫秒配置却写了秒导致运动补偿的时间差完全错乱。如果确认了单位没问题再看雷达和IMU的时间基准是否同步。有的采集系统里雷达点云时间戳用硬件时钟IMU时间戳用系统时钟两者之间有未知偏移这种情况下需要打开配置文件里在线时间偏移估计选项或者手动设置time_offset初值。这里要提一个很容易被忽略的点it可以用rqt_bag看一下点云消息里的时间戳对比IMU消息的时间戳检查两者是否在同一个量级且步进一致。很多采集脚本在记录bag时会对时间戳做了转换只要转换逻辑有细微差错FAST-LIO2处理起来就会在快速运动段出现明显错位。5.3 长走廊、隧道和开阔广场退化环境的真实表现很多人以为FAST-LIO2用了直接配准就天下无敌了实际测试下来它在退化环境下确实比特征法强很多但并不是万能的。长直走廊是典型的退化场景。沿走廊长度方向看点云几何几乎是平移不变的也就是说在长度方向上的约束非常弱。FAST-LIO2靠数量众多的原始点撑起了一部分约束所以比特征法撑得久漂移速度也慢得多但在几十米长的走廊里跑下来IMU零偏和轨迹的长度方向误差还是会慢慢积累。开阔广场更麻烦。地面是唯一的大平面四周没有立面除了高度方向的约束水平位置基本靠IMU在撑着。这种场景下任何纯激光雷达方案都会漂FAST-LIO2也不例外。如果工作场景里必然经过这类环境我的经验是两条路一是加入轮速计或者视觉里程计形成真正的多传感器紧耦合二是加装第二个雷达覆盖侧向和后向增加几何约束。单靠FAST-LIO2裸跑硬扛退化环境短期可以用长期还是不推荐。5.4 长距离漂移外参精度和噪声协方差的联合影响还有一种问题不那么炸裂但也很困扰人启动正常、短距离精度很高、轨迹很顺滑但跑个几百米到一公里之后全局轨迹和真值一对比偏差越来越大。这种长距离漂移往往不是滤波器发散而是某些系统性误差在长时间运行中被积分放大了。排查链路从打印估计零偏开始。如果滤波器的陀螺仪零偏和加速度计零偏估计值和IMU出厂标称值差异很大首先要怀疑外参不准。外参误差会被IMU积分过程放大——旋转外参差0.5度短时间内看不出来跑一公里累积的位置误差就会非常可观。这种情况建议用专业的激光-IMU外参标定工具重新标定不要自己拿尺子量就往上填。另一个重要嫌疑是IMU噪声协方差参数设置不当。很多人在配置里填的是IMU手册给出的白噪声和随机游走参数但手册里的数值是基于理想条件测试的实际装在车上的IMU受振动、温漂影响噪声特性会明显变差。如果IMU噪声参数设得太小滤波器会过分相信IMU激光的纠正权重被压低长距离下轨迹就会跟着IMU一起飘。我的做法是先按手册值跑一遍看效果然后把IMU噪声参数调大两到三倍再跑一遍对比轨迹误差。哪个效果好就留哪个。不要一次改太多参数一次只动一个变量否则出了问题根本没法定位。跑FAST-LIO2这类里程计最忌讳的就是参数一把梭全部照着别人的配置抄。同一颗IMU装在无人机上和装在车上的表现完全不同气候冷热变化也会影响噪声特性。把每个参数的含义理解清楚再根据自己实际的数据特点去调这套算法才能真正发挥出它的上限性能。我自己的习惯是每次试验只改一个参数改完跑同一段数据集对比轨迹和地图这样调出来的配置虽然花时间但心里有底换场景也不会莫名其妙地崩。