如果你在SLAM相关的技术社区里逛过应该早就见过FAST-LIO 这个名字。这个系列开源之后几乎成了激光惯性里程计LIO方向最常被拿来当基线的方案跑数据集、做毕业设计、换传感器、做真机调试大家都是先把这套代码拉起来跑通再去谈别的算法。它到底强在哪里为什么能在一众LIO方案里脱颖而出这篇文章不打算停留在“框架很好用”这种层面而是把FAST-LIO 系列背后几个真正关键的原理拆开讲清楚紧耦合为什么比松耦合稳迭代卡尔曼滤波和普通卡尔曼滤波差在哪反向传播怎么做运动补偿以及FAST-LIO2换上的ikd-Tree 到底解决了什么问题。适合谁来读已经会启动一个LIO节点、但还没把内部公式和代码对应起来的人正在做相关方向、想快速吃透核心逻辑的人。也可以作为你调参之前的一份“避坑地图”帮你少走很多弯路。1. 为什么 FAST-LIO 系列成了 LIO 圈子的“默认起点”1.1 LIO 到底在解什么题激光惯性里程计简单说就是同时拿激光雷达点云和IMU数据估计传感器在三维空间里的位置和姿态并在这个过程中拼出环境地图。听起来和纯激光里程计没什么区别但加一个IMU事情的性质就变了。纯激光方案在大多数时候也能跑但它有三个绕不过去的痛点一是角速度太快或者转弯太猛的时候一帧点云里每个点的采集时间不同点云会“糊”掉二是环境里如果出现长走廊、空旷广场这类几何退化场景激光匹配本身给不出约束轨迹就会慢慢漂走三是初始化阶段位姿完全未知纯激光面临“鸡生蛋”问题。IMU恰好能从这三个方向上补位它提供高频的旋转和加速度信息可以把一帧点云按时间戳做运动补偿在退化方向上虽然激光约束缺失但IMU的重力矢量和惯性积分能兜住一部分漂移对初始化来说IMU能快速把重力方向找回来给激光一个可用的初始姿态。但IMU也有自己的毛病它本身带零偏和噪声纯积分只是几秒就会漂得离谱。所以LIO的核心任务不是“用好激光”或者“用好IMU”而是把两者放在一个状态估计框架里让它们互相纠正。这就是FAST-LIO系列的核心出发点。1.2 系列升级除了算法还给工程留了什么FAST-LIO系列并不是单个算法而是一条演进线。大致可以分成两个阶段第一阶段把“激光配准”和“IMU传播”放进一个统一的迭代卡尔曼滤波框架用紧耦合的方式解决此前松耦合方案在快速运动下精度下降的问题同时把计算量压到一个实时可跑的水平第二阶段换掉整个地图管理结构不再依赖提取特征点而是用增量式kd-tree来管理地图直接拿点云配准。这一步让系统在保持精度的同时适应更多传感器类型也更容易在嵌入式平台上长时间运行。很多同学看代码时会把注意力放在各种滤波公式上实际上换个角度把这套系统看成“传感器数据处理流水线”更容易理解点云进来先做运动补偿IMU积分给出预测激光匹配给出修正修正量再反馈给状态地图增量更新。后面几章我会按照这条流水线把原理一节一节讲通。2. 紧耦合核心FAST-LIO 的 IEKF 到底怎么工作2.1 状态向量为什么要把外参也塞进去先看一个最容易被忽略的问题状态向量里到底装了什么。FAST-LIO的状态量不只是位置、姿态、速度这三样它还把陀螺仪零偏、加速度计零偏以及“激光雷达相对IMU的外参旋转和平移”一起放进估计状态里。为什么要把外参也塞进去因为在实际安装中外参很难量得绝对准。哪怕差个一两度在远距离点云上造成的匹配误差也会被放大接着IMU和激光之间的约束关系就会出现系统性偏差。让外参作为状态变量参与在线估计等于给系统一个“自动校准”的能力。代价是状态维度上升常见的实现里会来到18维左右如果加上完整外参估计可能到21维上下。这里有个概念要拎清估计器里的状态不是“真值”而是带协方差的随机变量。所以滤波过程本质上是在维护“对状态的信念”IMU传播会让这个信念的方差变大激光观测会把方差重新压小。紧耦合的含义也体现在这里——不是先用激光单独算一次位姿再用IMU积分结果去融合而是在同一个更新步骤里让激光残差直接去修正IMU预测出来的状态。2.2 一次配准迭代里发生了什么把一次完整的IEKF迭代拆开看大致是以下几步。第一步是IMU传播。用陀螺仪和加速度计的读数从上一个状态预测下一个状态同时更新协方差。因为IMU频率通常比激光高很多这一传播过程会跨多个IMU采样周期。第二步是残差构建。把一帧点云里的每个有效点通过当前估计出的位姿变换到世界坐标系然后在已有地图中查找最近邻点。FAST-LIO里一般不是简单找单个最近点而是取周围几个点拟合一个局部平面然后计算该点到平面的距离作为残差。第三步是迭代修正。理论上激光残差和状态之间是非线性关系。普通扩展卡尔曼滤波只做一次线性化如果初值偏差大就容易发散。FAST-LIO的做法是循环执行“线性化-计算增益-更新状态”每次更新后重新计算残差直到收敛或达到最大迭代次数。这就是“迭代”两个字的意思。这个流程和常见的“先ICP配准、再松耦合融合”有本质区别。松耦合方案通常先用ICP求出一个位姿再把它当作观测值送入滤波器而FAST-LIO让配准本身直接参与卡尔曼增益的计算相当于“匹配即估计、估计即匹配”。代码里能看到这样的设计迭代更新的过程中点云配准使用的变换矩阵也在同步更新两者之间没有明显边界。关键还是理解卡尔曼增益在干什么。普通卡尔曼滤波里增益决定“预测”和“观测”各自相信多少。IEKF里观测其实是“当前状态预测下点云到地图应该对齐”这个约束。于是选什么样的地图点做残差、残差模型够不够准直接决定系统能修正到什么程度。2.3 计算效率的关键把求逆从测量空间“挪”到状态空间很多刚接触FAST-LIO的人都会问一个问题一帧点云有几千个点测量维度几千起步这个卡尔曼更新怎么算得动这正是FAST-LIO最巧妙的地方。如果直接用常规公式卡尔曼增益的计算要面对一个“测量维度×测量维度”的矩阵求逆。几千点就是几千维矩阵求逆的复杂度是维度的三次方实时性基本没戏。FAST-LIO把更新公式改写了一下核心是下面这个等价形式K (P^{-1} H^T R^{-1} H)^{-1} H^T R^{-1}这里P是预测协方差H是观测模型R是测量噪声。注意看要逆的矩阵变成了P^{-1} H^T R^{-1} H它的维度等于状态维度通常只是十几或二十几和测量点数量无关。于是计算量从“几千维矩阵求逆”降到了“二十来维矩阵求逆”一下子变得实时可跑。这个细节就是FAST-LIO能跑在嵌入式平台上的关键转折点。也提醒我们一个道理在看这类算法时不要只盯着“精度高不高”计算图结构往往决定了这套系统能不能落地。工程上真正让算法可用的往往不是新概念而是把老概念算得足够便宜。2.4 运动补偿把一帧点云按时间“拉直”再往下挖一层就是运动补偿。虽然激光雷达每秒出很多点但一帧扫描其实有一个时间跨度。载体在采集期间一直在动如果把这几十毫秒内的所有点都认为是在同一时刻采的点云就会出现拖影。FAST-LIO里补偿用的工具就是IMU状态。具体做法是在一帧扫描的起点和终点各保留一个状态对扫描区间内每一个点根据它的相对时间戳反推出“从该点采集时刻到帧参考时刻”的位姿变换然后把点云变换过去相当于把一帧“弯”了的点云拉直。伪代码层面大致是这样for each point p in scan: t p.time # 该点在扫描内的相对时间戳 T_t_to_ref 反向积分(t, 帧参考时刻, IMU状态序列) p_corrected T_t_to_ref * p_raw这里最需要注意的问题是“帧参考时刻”到底选在哪个点。有的代码用扫描开始时刻有的用结束时刻细节不同会影响IMU状态序列的插值方式。实际调参时如果发现点云边缘总是虚的或者在静止环境下都出现“微小抖动”优先怀疑运动补偿和时间戳参考没对齐。运动补偿做得好不好直接决定配准残差里有多少是“真误差”多少是“时间拖影带来的假误差”。假误差会骗过滤波器让它把噪声当成运动去修正表现出来就是轨迹漂移和建图模糊。3. FAST-LIO2 凭什么敢不做特征提取3.1 直接配准每个点都能当“特征”早期很多LIO和LOAM类方案会先做特征提取把点云分成边缘点和平面点然后只拿这些“有代表性”的点去配准。这样做的理由是减少计算量、剔除不可靠点。但特征提取本身是个信息损失过程如果场景里纹理少、结构弱提取出来的特征点稀疏匹配质量就直线下降。FAST-LIO2走的是另一条路不做显式特征提取直接对原始点云做配准。每个点只要在有效范围内都可以参与残差计算。你可能会想几千个点全参与计算这不是更慢吗这里的关键在于配准也不是每个点都独立去最近邻搜索而是借助地图组织结构和滤波计算技巧把总体开销压了下来。直接配准带来的收益很有意思弱纹理环境下比如一面白墙、一条长廊传统特征提取可能只能找到零星几个点而直接法依然能从整面墙的表面获取足够的约束信息。这解释了不少用户的实际体验——FAST-LIO2在“看起来没什么特征”的室内环境下反而比需要特征点的方法更稳。3.2 ikd-Tree增量地图查询的关键既然不做特征提取地图的“组织方式”就成了头等大事。配准的本质是不断查询“当前点附近的已建图区域在哪里”。如果地图里点的规模一味增长每次查询都遍历全图系统迟早卡死。最朴素的想法是用一个经过下采样的体素地图先删掉太密的点再靠哈希去索引。FAST-LIO1就是这么做的体素地图在中等规模下表现不错。但到了更大场景、长时间运行体素地图的更新代价会逐渐抬高。FAST-LIO2换上的核心结构是增量kd-tree。普通kd-tree是一种静态结构建好之后如果要插入新点往往得把整棵树重建一遍这在实时跑图时不可接受。ikd-tree的思路是“动态维护平衡树”先按常规kd-tree的规则建树插点时只影响局部路径删除点时不完全物理删掉而是先打个“删除标记”等某个子树里标记过期的点达到一定比例再触发一次局部重建Rebuild。这个过程可以用一个容易理解的类比厨房里不会做完一道菜就把所有碗碟重新摆一遍而是先把要扔的东西放在一个待处理区等垃圾堆到一定量再集中倒掉并整理台面。ikd-tree的“增量删除、延后重建”就是这个逻辑的分层版。由于地图点总规模被限制在一个可控范围内配准查询的耗时能得到很好的保证。这也是FAST-LIO2能长时间建图跑大场景的底气之一。3.3 退化场景下的特别价值很多人实测下来有个感觉FAST-LIO2在隧道、走廊这类“退化环境”里比纯激光算法稳。这还真不是玄学。退化环境的意思是激光在某个方向上没有提供足够约束。比如一条笔直长廊沿着宽度方向还有点约束但沿着长度方向环境看起来都一样激光基本说不出“我走了多远”。纯激光里程计在这个方向上的误差会越积越大。IMU在这里起了什么作用一方面加速度计感受到的重力方向始终是一个稳定约束能防止姿态在小尺度上随意漂移另一方面IMU的短时积分在几秒内相对可信相当于在激光约束缺失的方向上“垫”了一块信息。卡尔曼滤波会按协方差自动地权衡激光约束强的方向多信激光激光约束弱的方向多信IMU。这就是紧耦合在退化场景下的核心价值。但别把它神话了。IMU本身有偏置误差时间一长也会漂。FAST-LIO2能扛住的退化场景周期一般在几十秒量级如果一条隧道几公里长中间没有转角、没有特殊特征光靠IMU撑还是很吃力。实用做法是外部叠加闭环检测或者轮速器等传感器。4. 把原理落成可跑的系统配置、标定与调参经验4.1 外参、IMU 参数和时间轴启动前先做三件事代码跑不跑得动头三件事决定成败外参对不对、IMU噪声参数对不对、时间戳对不对。先说外参。FAST-LIO系列虽然支持在线估计外参但初值必须在合理范围内。如果你把激光和IMU之间的安装角度写错了几十度在线估计大概率直接发散。我的建议是先用尺子和简单标定工具把旋转和平移初值量到误差不超过几厘米、几度的程度再让系统在线微调。不要指望在线估计能从零猜出外参。然后是IMU噪声参数。这里最容易踩的坑是拿着别人配置文件里的数字直接抄。不同IMU的白噪声和随机游走差别很大哪怕同一型号不同个体、不同采样率也会有差别。如果噪声参数设得过小滤波器会过于相信IMU实验结果就是轨迹跟着IMU漂设得过大滤波器又说不敢用IMU退化场景下很快完蛋。正规做法是用Allan方差分析出陀螺仪和加速度计的白噪声、零偏随机游走再把单位换算成代码里的量纲。最后是时间轴。激光点和IMU数据必须换算到同一时间基准系。常见传感器在驱动里会给点云时间戳但点云输出时刻和真实采集时刻之间可能有固定延时这个延时差个几毫秒在高速旋转下就可能让配准残差明显变大。启动前先看驱动文档或者跑静置实验观察点云在静止时是否有规律变形来估计时延。4.2 参数文件里容易被低估的几个数字FAST-LIO系列参数文件里藏着几个看起来不起眼、实际影响很大的参数。盲区距离很多雷达近处一堆杂点特别是墙角、地面反射直接参与配准会污染残差。设一个合适的盲区把 0.5米或更近的点滤掉通常能立刻改善稳定性。最大测距范围远点误差大角度误差在远处会被放大。设一个合理的最大范围比如几十米比无脑用满量程更稳。地图下采样分辨率地图点太密ikd-tree节点多查询变慢太疏配准精度下降。建议从 0.2到0.5米之间试起看实时运行单圈耗时和建图厚度再定。迭代最大次数一般2到3次就能收敛过度增加迭代次数对精度帮助很有限反而拉高CPU。如果残差一直不降优先怀疑残差模型和外参而不是盲调迭代次数。最大迭代间隔/scan间隔这个和雷达帧率相关不同传感器差异很大不要照抄默认值。调参顺序上我习惯先固定外参和时间同步再调滤波噪声参数最后才碰地图相关的阈值。顺序反了很容易出现“改一个参数好一点改另一个又坏回去”的振荡局面。4.3 从仿真到实车的工作流一个比较稳妥的落地流程是先在公开仿真环境里跑通再迁移到真实设备。仿真环境的好处是外参、真实位姿、噪声参数全都是“已知的”你可以用它验证自己对参数文件的理解。比如把外参故意写偏一点观察轨迹怎么变把IMU噪声写大一点观察退化场景下系统是不是更早发散。这些实验能帮你建立“参数-现象”之间的直觉比直接拿真机瞎试安全得多。迁移到真机之后首次实验原则只有一条让运动温柔一点。先慢速走直线再转小圈再加速转弯。不要一上来就做激烈动作否则系统一旦崩溃你根本看不出是外参问题、时间同步问题还是参数过激。我见过不少开发者的调试过程最后发现是IMU安装方向写反、点云时间戳固定延迟相差十几毫秒这类小问题而这在温柔运动下很容易暴露。真机调试时建议导出每帧的配准残差均值、状态估计方差、点云匹配内点数量这几个指标能非常直观地告诉你系统是不是健康。残差突然抬升而内点数骤降通常意味着地图匹配到了错误地方方差长期不收敛大概率是IMU参数或外参没设对。5. 常见问题与排查技巧实录5.1 里程计刚开始正常几十秒后开始飘这是提问率最高的问题。最常见的原因有四种重力方向初始化没做好。启动瞬间如果包含较大加速度估计出的重力方向就偏了之后轨迹会在重力方向上慢慢漂。解决方法是让系统在静止状态下静置几秒等IMU初始化稳定再开始运动。加速度计零偏估计被污染。如果初始化期间车身没静止滤波会把运动加速度当成零偏吸收后面越跑越偏。外参平移或旋转初值差太多。在线估计虽然能修正一部分但修正范围有限初值太离谱只会引发发散。角速度饱和或者IMU量程不够。激烈旋转时IMU数据削顶积分结果失真系统会把假运动当成真残差去配准。排查顺序建议先看启动时IMU加速度模长是不是接近当地重力值再看外参初值再看运行轨迹是否在某个方向上有单调漂移最后再怀疑代码逻辑。5.2 地图重影、轨迹跳变多半和时间轴有关地图重影和轨迹跳变的问题重点怀疑两个方向层间时间同步和运动畸变补偿。如果你用的是支持点级时间戳的雷达要确认点的时间戳单位、起点、方向对不对。有的雷达把第一个点当作0有的把最后一个点当作0有的时间戳是浮点秒有的是整型微秒。单位差着千倍补偿就是灾难。另外要检查IMU时间戳和激光时间戳是不是同一个时钟源。如果雷达有自己的时钟而未同步毫秒级偏差在高速运动时非常致命。一个非常简单的判断方法让设备静止不动如果静止时点云都出现重复边缘那就大概率不是配准问题而是时间戳或运动补偿问题。5.3 空旷与长走廊退化场景怎么判断和绕开在线判断退化最直接的办法是看状态协方差在激光更新后的变化。如果某个方向上的方差一直降不下来或者残差在某个方向上对位姿变化不敏感基本可以判定这个方向退化。实际操作中可以同时观察点云匹配残差和IMU置信度。如果残差均值不高但轨迹在横向漂移很可能是退化方向缺少约束而不是参数没调好。这时候再调滤波器参数意义不大更值得做的是加入轮速约束、引入闭环检测、或者调整运动策略让载体尽量多转身、多制造一些非退化运动。5.4 问题速查表启动问题的常见归因下面这张表是我在几次调试过程中整理出来的遇到问题可以先对着表排查一轮。现象优先排查项常见处理手段启动后立刻飞掉外参初值、IMU方向、时间戳量外参检查IMU安装矩阵固定延时补偿正常跑一会儿慢慢漂重力初始化、IMU零偏、加速度噪声静止启动重新标定IMU参数地图重影运动补偿、时间戳单位校正时间戳基准检查扫描参考时刻高速转向时发散IMU量程、角速度饱和、外参旋转检查数据是否削顶降低运动速度验证长走廊横向漂移退化方向、IMU随机游走加回环或轮速调低IMU噪声CPU占用异常高ikd-tree规模、地图下采样增大下采样分辨率检查删除区域设置这只是一张起点地图真正排查时还是要结合残差曲线和协方差数据去联动判断。时序问题、参数问题和外参问题常常表现得非常像草率下结论反而容易浪费时间。最后再分享一个我自己的体会FAST-LIO 系列最大的门槛其实不是公式而是把状态变量、时间戳、地图结构这三条线从代码里拎出来。你不需要背出每一步矩阵推导但一定要知道IMU传播之后谁在负责“给预测纠偏”点云补偿用的是哪个时间参考系地图点又是怎么被动态维护的。把这几个问题在代码里逐个对应上绝大多数调试问题都能从现象反推回原因。之后再调参就不是靠碰运气而是每条参数都有明确的物理意义了。
如果你在SLAM相关的技术社区里逛过应该早就见过FAST-LIO 这个名字。这个系列开源之后几乎成了激光惯性里程计LIO方向最常被拿来当基线的方案跑数据集、做毕业设计、换传感器、做真机调试大家都是先把这套代码拉起来跑通再去谈别的算法。它到底强在哪里为什么能在一众LIO方案里脱颖而出这篇文章不打算停留在“框架很好用”这种层面而是把FAST-LIO 系列背后几个真正关键的原理拆开讲清楚紧耦合为什么比松耦合稳迭代卡尔曼滤波和普通卡尔曼滤波差在哪反向传播怎么做运动补偿以及FAST-LIO2换上的ikd-Tree 到底解决了什么问题。适合谁来读已经会启动一个LIO节点、但还没把内部公式和代码对应起来的人正在做相关方向、想快速吃透核心逻辑的人。也可以作为你调参之前的一份“避坑地图”帮你少走很多弯路。1. 为什么 FAST-LIO 系列成了 LIO 圈子的“默认起点”1.1 LIO 到底在解什么题激光惯性里程计简单说就是同时拿激光雷达点云和IMU数据估计传感器在三维空间里的位置和姿态并在这个过程中拼出环境地图。听起来和纯激光里程计没什么区别但加一个IMU事情的性质就变了。纯激光方案在大多数时候也能跑但它有三个绕不过去的痛点一是角速度太快或者转弯太猛的时候一帧点云里每个点的采集时间不同点云会“糊”掉二是环境里如果出现长走廊、空旷广场这类几何退化场景激光匹配本身给不出约束轨迹就会慢慢漂走三是初始化阶段位姿完全未知纯激光面临“鸡生蛋”问题。IMU恰好能从这三个方向上补位它提供高频的旋转和加速度信息可以把一帧点云按时间戳做运动补偿在退化方向上虽然激光约束缺失但IMU的重力矢量和惯性积分能兜住一部分漂移对初始化来说IMU能快速把重力方向找回来给激光一个可用的初始姿态。但IMU也有自己的毛病它本身带零偏和噪声纯积分只是几秒就会漂得离谱。所以LIO的核心任务不是“用好激光”或者“用好IMU”而是把两者放在一个状态估计框架里让它们互相纠正。这就是FAST-LIO系列的核心出发点。1.2 系列升级除了算法还给工程留了什么FAST-LIO系列并不是单个算法而是一条演进线。大致可以分成两个阶段第一阶段把“激光配准”和“IMU传播”放进一个统一的迭代卡尔曼滤波框架用紧耦合的方式解决此前松耦合方案在快速运动下精度下降的问题同时把计算量压到一个实时可跑的水平第二阶段换掉整个地图管理结构不再依赖提取特征点而是用增量式kd-tree来管理地图直接拿点云配准。这一步让系统在保持精度的同时适应更多传感器类型也更容易在嵌入式平台上长时间运行。很多同学看代码时会把注意力放在各种滤波公式上实际上换个角度把这套系统看成“传感器数据处理流水线”更容易理解点云进来先做运动补偿IMU积分给出预测激光匹配给出修正修正量再反馈给状态地图增量更新。后面几章我会按照这条流水线把原理一节一节讲通。2. 紧耦合核心FAST-LIO 的 IEKF 到底怎么工作2.1 状态向量为什么要把外参也塞进去先看一个最容易被忽略的问题状态向量里到底装了什么。FAST-LIO的状态量不只是位置、姿态、速度这三样它还把陀螺仪零偏、加速度计零偏以及“激光雷达相对IMU的外参旋转和平移”一起放进估计状态里。为什么要把外参也塞进去因为在实际安装中外参很难量得绝对准。哪怕差个一两度在远距离点云上造成的匹配误差也会被放大接着IMU和激光之间的约束关系就会出现系统性偏差。让外参作为状态变量参与在线估计等于给系统一个“自动校准”的能力。代价是状态维度上升常见的实现里会来到18维左右如果加上完整外参估计可能到21维上下。这里有个概念要拎清估计器里的状态不是“真值”而是带协方差的随机变量。所以滤波过程本质上是在维护“对状态的信念”IMU传播会让这个信念的方差变大激光观测会把方差重新压小。紧耦合的含义也体现在这里——不是先用激光单独算一次位姿再用IMU积分结果去融合而是在同一个更新步骤里让激光残差直接去修正IMU预测出来的状态。2.2 一次配准迭代里发生了什么把一次完整的IEKF迭代拆开看大致是以下几步。第一步是IMU传播。用陀螺仪和加速度计的读数从上一个状态预测下一个状态同时更新协方差。因为IMU频率通常比激光高很多这一传播过程会跨多个IMU采样周期。第二步是残差构建。把一帧点云里的每个有效点通过当前估计出的位姿变换到世界坐标系然后在已有地图中查找最近邻点。FAST-LIO里一般不是简单找单个最近点而是取周围几个点拟合一个局部平面然后计算该点到平面的距离作为残差。第三步是迭代修正。理论上激光残差和状态之间是非线性关系。普通扩展卡尔曼滤波只做一次线性化如果初值偏差大就容易发散。FAST-LIO的做法是循环执行“线性化-计算增益-更新状态”每次更新后重新计算残差直到收敛或达到最大迭代次数。这就是“迭代”两个字的意思。这个流程和常见的“先ICP配准、再松耦合融合”有本质区别。松耦合方案通常先用ICP求出一个位姿再把它当作观测值送入滤波器而FAST-LIO让配准本身直接参与卡尔曼增益的计算相当于“匹配即估计、估计即匹配”。代码里能看到这样的设计迭代更新的过程中点云配准使用的变换矩阵也在同步更新两者之间没有明显边界。关键还是理解卡尔曼增益在干什么。普通卡尔曼滤波里增益决定“预测”和“观测”各自相信多少。IEKF里观测其实是“当前状态预测下点云到地图应该对齐”这个约束。于是选什么样的地图点做残差、残差模型够不够准直接决定系统能修正到什么程度。2.3 计算效率的关键把求逆从测量空间“挪”到状态空间很多刚接触FAST-LIO的人都会问一个问题一帧点云有几千个点测量维度几千起步这个卡尔曼更新怎么算得动这正是FAST-LIO最巧妙的地方。如果直接用常规公式卡尔曼增益的计算要面对一个“测量维度×测量维度”的矩阵求逆。几千点就是几千维矩阵求逆的复杂度是维度的三次方实时性基本没戏。FAST-LIO把更新公式改写了一下核心是下面这个等价形式K (P^{-1} H^T R^{-1} H)^{-1} H^T R^{-1}这里P是预测协方差H是观测模型R是测量噪声。注意看要逆的矩阵变成了P^{-1} H^T R^{-1} H它的维度等于状态维度通常只是十几或二十几和测量点数量无关。于是计算量从“几千维矩阵求逆”降到了“二十来维矩阵求逆”一下子变得实时可跑。这个细节就是FAST-LIO能跑在嵌入式平台上的关键转折点。也提醒我们一个道理在看这类算法时不要只盯着“精度高不高”计算图结构往往决定了这套系统能不能落地。工程上真正让算法可用的往往不是新概念而是把老概念算得足够便宜。2.4 运动补偿把一帧点云按时间“拉直”再往下挖一层就是运动补偿。虽然激光雷达每秒出很多点但一帧扫描其实有一个时间跨度。载体在采集期间一直在动如果把这几十毫秒内的所有点都认为是在同一时刻采的点云就会出现拖影。FAST-LIO里补偿用的工具就是IMU状态。具体做法是在一帧扫描的起点和终点各保留一个状态对扫描区间内每一个点根据它的相对时间戳反推出“从该点采集时刻到帧参考时刻”的位姿变换然后把点云变换过去相当于把一帧“弯”了的点云拉直。伪代码层面大致是这样for each point p in scan: t p.time # 该点在扫描内的相对时间戳 T_t_to_ref 反向积分(t, 帧参考时刻, IMU状态序列) p_corrected T_t_to_ref * p_raw这里最需要注意的问题是“帧参考时刻”到底选在哪个点。有的代码用扫描开始时刻有的用结束时刻细节不同会影响IMU状态序列的插值方式。实际调参时如果发现点云边缘总是虚的或者在静止环境下都出现“微小抖动”优先怀疑运动补偿和时间戳参考没对齐。运动补偿做得好不好直接决定配准残差里有多少是“真误差”多少是“时间拖影带来的假误差”。假误差会骗过滤波器让它把噪声当成运动去修正表现出来就是轨迹漂移和建图模糊。3. FAST-LIO2 凭什么敢不做特征提取3.1 直接配准每个点都能当“特征”早期很多LIO和LOAM类方案会先做特征提取把点云分成边缘点和平面点然后只拿这些“有代表性”的点去配准。这样做的理由是减少计算量、剔除不可靠点。但特征提取本身是个信息损失过程如果场景里纹理少、结构弱提取出来的特征点稀疏匹配质量就直线下降。FAST-LIO2走的是另一条路不做显式特征提取直接对原始点云做配准。每个点只要在有效范围内都可以参与残差计算。你可能会想几千个点全参与计算这不是更慢吗这里的关键在于配准也不是每个点都独立去最近邻搜索而是借助地图组织结构和滤波计算技巧把总体开销压了下来。直接配准带来的收益很有意思弱纹理环境下比如一面白墙、一条长廊传统特征提取可能只能找到零星几个点而直接法依然能从整面墙的表面获取足够的约束信息。这解释了不少用户的实际体验——FAST-LIO2在“看起来没什么特征”的室内环境下反而比需要特征点的方法更稳。3.2 ikd-Tree增量地图查询的关键既然不做特征提取地图的“组织方式”就成了头等大事。配准的本质是不断查询“当前点附近的已建图区域在哪里”。如果地图里点的规模一味增长每次查询都遍历全图系统迟早卡死。最朴素的想法是用一个经过下采样的体素地图先删掉太密的点再靠哈希去索引。FAST-LIO1就是这么做的体素地图在中等规模下表现不错。但到了更大场景、长时间运行体素地图的更新代价会逐渐抬高。FAST-LIO2换上的核心结构是增量kd-tree。普通kd-tree是一种静态结构建好之后如果要插入新点往往得把整棵树重建一遍这在实时跑图时不可接受。ikd-tree的思路是“动态维护平衡树”先按常规kd-tree的规则建树插点时只影响局部路径删除点时不完全物理删掉而是先打个“删除标记”等某个子树里标记过期的点达到一定比例再触发一次局部重建Rebuild。这个过程可以用一个容易理解的类比厨房里不会做完一道菜就把所有碗碟重新摆一遍而是先把要扔的东西放在一个待处理区等垃圾堆到一定量再集中倒掉并整理台面。ikd-tree的“增量删除、延后重建”就是这个逻辑的分层版。由于地图点总规模被限制在一个可控范围内配准查询的耗时能得到很好的保证。这也是FAST-LIO2能长时间建图跑大场景的底气之一。3.3 退化场景下的特别价值很多人实测下来有个感觉FAST-LIO2在隧道、走廊这类“退化环境”里比纯激光算法稳。这还真不是玄学。退化环境的意思是激光在某个方向上没有提供足够约束。比如一条笔直长廊沿着宽度方向还有点约束但沿着长度方向环境看起来都一样激光基本说不出“我走了多远”。纯激光里程计在这个方向上的误差会越积越大。IMU在这里起了什么作用一方面加速度计感受到的重力方向始终是一个稳定约束能防止姿态在小尺度上随意漂移另一方面IMU的短时积分在几秒内相对可信相当于在激光约束缺失的方向上“垫”了一块信息。卡尔曼滤波会按协方差自动地权衡激光约束强的方向多信激光激光约束弱的方向多信IMU。这就是紧耦合在退化场景下的核心价值。但别把它神话了。IMU本身有偏置误差时间一长也会漂。FAST-LIO2能扛住的退化场景周期一般在几十秒量级如果一条隧道几公里长中间没有转角、没有特殊特征光靠IMU撑还是很吃力。实用做法是外部叠加闭环检测或者轮速器等传感器。4. 把原理落成可跑的系统配置、标定与调参经验4.1 外参、IMU 参数和时间轴启动前先做三件事代码跑不跑得动头三件事决定成败外参对不对、IMU噪声参数对不对、时间戳对不对。先说外参。FAST-LIO系列虽然支持在线估计外参但初值必须在合理范围内。如果你把激光和IMU之间的安装角度写错了几十度在线估计大概率直接发散。我的建议是先用尺子和简单标定工具把旋转和平移初值量到误差不超过几厘米、几度的程度再让系统在线微调。不要指望在线估计能从零猜出外参。然后是IMU噪声参数。这里最容易踩的坑是拿着别人配置文件里的数字直接抄。不同IMU的白噪声和随机游走差别很大哪怕同一型号不同个体、不同采样率也会有差别。如果噪声参数设得过小滤波器会过于相信IMU实验结果就是轨迹跟着IMU漂设得过大滤波器又说不敢用IMU退化场景下很快完蛋。正规做法是用Allan方差分析出陀螺仪和加速度计的白噪声、零偏随机游走再把单位换算成代码里的量纲。最后是时间轴。激光点和IMU数据必须换算到同一时间基准系。常见传感器在驱动里会给点云时间戳但点云输出时刻和真实采集时刻之间可能有固定延时这个延时差个几毫秒在高速旋转下就可能让配准残差明显变大。启动前先看驱动文档或者跑静置实验观察点云在静止时是否有规律变形来估计时延。4.2 参数文件里容易被低估的几个数字FAST-LIO系列参数文件里藏着几个看起来不起眼、实际影响很大的参数。盲区距离很多雷达近处一堆杂点特别是墙角、地面反射直接参与配准会污染残差。设一个合适的盲区把 0.5米或更近的点滤掉通常能立刻改善稳定性。最大测距范围远点误差大角度误差在远处会被放大。设一个合理的最大范围比如几十米比无脑用满量程更稳。地图下采样分辨率地图点太密ikd-tree节点多查询变慢太疏配准精度下降。建议从 0.2到0.5米之间试起看实时运行单圈耗时和建图厚度再定。迭代最大次数一般2到3次就能收敛过度增加迭代次数对精度帮助很有限反而拉高CPU。如果残差一直不降优先怀疑残差模型和外参而不是盲调迭代次数。最大迭代间隔/scan间隔这个和雷达帧率相关不同传感器差异很大不要照抄默认值。调参顺序上我习惯先固定外参和时间同步再调滤波噪声参数最后才碰地图相关的阈值。顺序反了很容易出现“改一个参数好一点改另一个又坏回去”的振荡局面。4.3 从仿真到实车的工作流一个比较稳妥的落地流程是先在公开仿真环境里跑通再迁移到真实设备。仿真环境的好处是外参、真实位姿、噪声参数全都是“已知的”你可以用它验证自己对参数文件的理解。比如把外参故意写偏一点观察轨迹怎么变把IMU噪声写大一点观察退化场景下系统是不是更早发散。这些实验能帮你建立“参数-现象”之间的直觉比直接拿真机瞎试安全得多。迁移到真机之后首次实验原则只有一条让运动温柔一点。先慢速走直线再转小圈再加速转弯。不要一上来就做激烈动作否则系统一旦崩溃你根本看不出是外参问题、时间同步问题还是参数过激。我见过不少开发者的调试过程最后发现是IMU安装方向写反、点云时间戳固定延迟相差十几毫秒这类小问题而这在温柔运动下很容易暴露。真机调试时建议导出每帧的配准残差均值、状态估计方差、点云匹配内点数量这几个指标能非常直观地告诉你系统是不是健康。残差突然抬升而内点数骤降通常意味着地图匹配到了错误地方方差长期不收敛大概率是IMU参数或外参没设对。5. 常见问题与排查技巧实录5.1 里程计刚开始正常几十秒后开始飘这是提问率最高的问题。最常见的原因有四种重力方向初始化没做好。启动瞬间如果包含较大加速度估计出的重力方向就偏了之后轨迹会在重力方向上慢慢漂。解决方法是让系统在静止状态下静置几秒等IMU初始化稳定再开始运动。加速度计零偏估计被污染。如果初始化期间车身没静止滤波会把运动加速度当成零偏吸收后面越跑越偏。外参平移或旋转初值差太多。在线估计虽然能修正一部分但修正范围有限初值太离谱只会引发发散。角速度饱和或者IMU量程不够。激烈旋转时IMU数据削顶积分结果失真系统会把假运动当成真残差去配准。排查顺序建议先看启动时IMU加速度模长是不是接近当地重力值再看外参初值再看运行轨迹是否在某个方向上有单调漂移最后再怀疑代码逻辑。5.2 地图重影、轨迹跳变多半和时间轴有关地图重影和轨迹跳变的问题重点怀疑两个方向层间时间同步和运动畸变补偿。如果你用的是支持点级时间戳的雷达要确认点的时间戳单位、起点、方向对不对。有的雷达把第一个点当作0有的把最后一个点当作0有的时间戳是浮点秒有的是整型微秒。单位差着千倍补偿就是灾难。另外要检查IMU时间戳和激光时间戳是不是同一个时钟源。如果雷达有自己的时钟而未同步毫秒级偏差在高速运动时非常致命。一个非常简单的判断方法让设备静止不动如果静止时点云都出现重复边缘那就大概率不是配准问题而是时间戳或运动补偿问题。5.3 空旷与长走廊退化场景怎么判断和绕开在线判断退化最直接的办法是看状态协方差在激光更新后的变化。如果某个方向上的方差一直降不下来或者残差在某个方向上对位姿变化不敏感基本可以判定这个方向退化。实际操作中可以同时观察点云匹配残差和IMU置信度。如果残差均值不高但轨迹在横向漂移很可能是退化方向缺少约束而不是参数没调好。这时候再调滤波器参数意义不大更值得做的是加入轮速约束、引入闭环检测、或者调整运动策略让载体尽量多转身、多制造一些非退化运动。5.4 问题速查表启动问题的常见归因下面这张表是我在几次调试过程中整理出来的遇到问题可以先对着表排查一轮。现象优先排查项常见处理手段启动后立刻飞掉外参初值、IMU方向、时间戳量外参检查IMU安装矩阵固定延时补偿正常跑一会儿慢慢漂重力初始化、IMU零偏、加速度噪声静止启动重新标定IMU参数地图重影运动补偿、时间戳单位校正时间戳基准检查扫描参考时刻高速转向时发散IMU量程、角速度饱和、外参旋转检查数据是否削顶降低运动速度验证长走廊横向漂移退化方向、IMU随机游走加回环或轮速调低IMU噪声CPU占用异常高ikd-tree规模、地图下采样增大下采样分辨率检查删除区域设置这只是一张起点地图真正排查时还是要结合残差曲线和协方差数据去联动判断。时序问题、参数问题和外参问题常常表现得非常像草率下结论反而容易浪费时间。最后再分享一个我自己的体会FAST-LIO 系列最大的门槛其实不是公式而是把状态变量、时间戳、地图结构这三条线从代码里拎出来。你不需要背出每一步矩阵推导但一定要知道IMU传播之后谁在负责“给预测纠偏”点云补偿用的是哪个时间参考系地图点又是怎么被动态维护的。把这几个问题在代码里逐个对应上绝大多数调试问题都能从现象反推回原因。之后再调参就不是靠碰运气而是每条参数都有明确的物理意义了。
