企业官网建设流程全解析

去年年底做园区低速无人车的时候，我在一楼连廊遇到一件挺头疼的事：同样一套传感器组合，换用某套基于特征提取的激光惯性方案时，只要车一进连廊，定位轨迹就开始往外飘，偶尔还会在玻璃幕墙边原地打转。后来把前端换成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_TYPE=Release source devel/setup.bash

一定要加-DCMAKE_BUILD_TYPE=Release。不加的话默认是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

然后另开一个终端回放bag：

rosbag play m2dgr_xxx.bag

启动launch文件之后，程序会初始化ikd-Tree和滤波器，等待雷达数据和IMU数据输入。rviz可视化可以加载FAST_LIO仓库里自带的fast_lio/rviz/fast_lio.rviz配置文件，直接看到全局地图和轨迹。

跑第一个bag的时候有一种"这就跑起来了？"的感觉——不需要任何训练、没有任何标定参数要填，默认配置一套，直接出图。这也是FAST-LIO2在社区里口碑好的原因之一，工程完成度确实高。

下面这张表是常用的话题，调试的时候需要心里清楚：

rviz里的Fixed Frame建议设置成camera_init，这样看到的是全局一致的地图和轨迹。如果你把Fixed Frame设成body系，地图会跟着车动，看起来就是"点云绕着车转"，那是正常的，不是代码跑飞了。

4.3 自采数据的配置手册

跑通官方bag之后，换上自己的传感器数据，就需要认真改配置文件了。配置文件的路径在launch文件里指定，通常是一个YAML文件。核心参数如下：

自采数据最常见的翻车点有三个。第一个是时间戳单位填错。常见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装在无人机上和装在车上的表现完全不同，气候冷热变化也会影响噪声特性。把每个参数的含义理解清楚，再根据自己实际的数据特点去调，这套算法才能真正发挥出它的上限性能。我自己的习惯是每次试验只改一个参数，改完跑同一段数据集，对比轨迹和地图，这样调出来的配置虽然花时间，但心里有底，换场景也不会莫名其妙地崩。