1. 从建图到定位FAST-LIO2 落地绕不开的两块硬骨头做过激光雷达 SLAM 的人大概都有这个体会跑通一个开源建图算法只是万里长征第一步真正把它塞进一个能用的机器人系统里中间还隔着好几道坎。FAST-LIO2 本身在里程计精度和实时性上已经做得相当出色iEKF迭代扩展卡尔曼滤波的框架加上直接法配准让它在很多场景下都能稳定输出高频位姿。但如果你只停留在“跑 demo 看轨迹”的阶段那它对你来说还只是个玩具。真正把 FAST-LIO2 用起来绕不开两个核心问题。第一个是地图的组织与查询效率也就是 ikd-tree 这个增量式 KD 树到底在干什么、为什么 FAST-LIO2 要自己造一个数据结构而不用现成的 PCL 或者 nanoflann。第二个是重定位机器人关机再开机、或者被搬动之后怎么让它知道自己在地图的哪个位置而不是傻乎乎地从原点重新开始建图。这两个问题一个关乎“建图时快不快、省不省内存”一个关乎“用图时准不准、能不能接上”恰好对应了 FAST-LIO2 从算法走向应用的两端。这篇笔记面向的是已经跑过 FAST-LIO2、想进一步把它集成到自己系统里的朋友。我会把 ikd-tree 的设计动机和关键操作讲透再把重定位的完整链路拆开包括 LI-Init 的作用、fast_lio_localization 这个包怎么用、rviz 里手动给初始位姿的技巧以及实际调试中那些文档里不会写的坑。内容偏实操但该讲的原理我会用生活化的方式说清楚保证你看完能直接上手改自己的工程。2. ikd-tree 深度拆解FAST-LIO2 的地图为什么这么快2.1 为什么现成的 KD 树不够用先说结论FAST-LIO2 需要的是一个支持增量更新、支持降采样、支持最近邻和范围查询、并且内存可控的动态地图结构。普通的静态 KD 树建好之后就不动了你要往里加点或者删点基本只能重建整棵树。而激光雷达 SLAM 每来一帧点云地图就要更新一次如果每次都重建计算量直接爆炸。有人会说那用 PCL 的 octree 或者 nanoflann 不行吗nanoflann 的查询确实快但它同样不擅长频繁的插入和删除。octree 虽然支持动态更新但在最近邻查询的常数因子上往往不如 KD 树而且体素化的粒度控制不够灵活。FAST-LIO2 的作者在论文里明确对比过ikd-tree 在增量更新场景下的综合性能明显优于这些通用结构。ikd-tree 的核心思路是惰性删除加局部重建。当你删除一个点的时候它不立刻把节点从树里摘掉而是打一个“已删除”的标记同时把这个点从有效点数里扣掉。只有当某个子树的无效点比例超过阈值默认是 30%才对这个子树做一次重建把真正有效的点重新组织成一棵平衡的树。插入也是类似新点先挂上去必要时触发重建来维持平衡。这样就把频繁的小改动摊薄成了偶尔的大操作均摊复杂度就下来了。2.2 降采样与地图规模控制ikd-tree 另一个关键能力是基于体素格的降采样。激光雷达一帧就是几万个点如果全塞进地图几分钟下来内存就撑不住了。ikd-tree 在插入新点时会检查它所在的体素格里是否已经有点如果已经有了就根据配置决定是丢弃新点还是替换旧点。这个体素大小filter_size是个非常重要的参数直接决定了地图的稠密程度和查询速度。我一般会把 filter_size 设成 0.2 到 0.5 米之间。太小了地图太稠查询慢、内存涨得快太大了地图太稀配准的时候特征不够精度会掉。具体取值要看你的场景室内结构化环境可以小一点比如 0.2室外大场景可以放到 0.5 甚至更大。这个参数在 FAST-LIO2 的配置文件里叫filter_size_map改完之后一定要重新跑一遍看看轨迹有没有跳变。还有一个参数是cube_side_length它决定了地图的局部范围。ikd-tree 维护的是一个以当前位置为中心的立方体区域超出这个范围的点会被裁掉。这样做的好处是内存有上界不会无限增长。默认值通常是 1000 米对于大多数场景够用了。但如果你做的是大范围室外建图可能需要适当调大代价是内存占用上升。2.3 最近邻查询的实现细节ikd-tree 的最近邻查询用的是标准的 KD 树递归下降加剪枝。从根节点开始先找到点所在的叶子然后回溯用当前最近距离去剪掉那些不可能更近的子树。这部分逻辑和经典 KD 树一致没什么特别的。真正有意思的是它和 iEKF 的配合方式。FAST-LIO2 在配准的时候对每个当前帧的点需要在地图里找最近的几个点来构建点到面或者点到线的残差。ikd-tree 提供的Nearest_Search接口可以一次返回 K 个最近邻K 通常取 5。这里有个细节返回的点里可能包含被标记为删除的点所以调用方需要自己过滤。另外如果最近邻距离超过某个阈值比如 1 米这个点对就不应该参与配准否则会引入错误约束。这个阈值在代码里通常叫max_dist设得太大会让配准被离群点带偏设得太小又会导致有效约束不足。提示调试 ikd-tree 相关问题时可以打开ikd_tree的调试输出观察每次重建的触发频率。如果重建过于频繁说明删除比例阈值设得太低或者场景中动态物体太多需要针对性调整。2.4 实操中关于 ikd-tree 的几个经验第一个经验是不要在配准线程里做地图重建。ikd-tree 的重建虽然均摊下来不慢但单次重建是有明显耗时的。如果重建恰好发生在配准的关键路径上会导致某一帧的处理时间突然拉长表现为轨迹抖动或者丢帧。稳妥的做法是把地图更新放到单独的线程或者至少在时间上错开配准的高峰期。第二个经验是注意内存的释放。ikd-tree 删除点只是打标记真正的内存回收发生在重建时。如果你的场景里动态物体特别多比如人来人往的商场无效点会快速累积触发频繁重建。这时候可以考虑适当降低重建阈值让回收更及时代价是重建次数增加。这是一个需要根据实际场景权衡的取舍。第三个经验是保存和加载地图。ikd-tree 本身支持把树结构序列化到磁盘但格式是它自己定义的不同版本之间可能不兼容。如果你要长期维护地图建议同时保存一份 PCD 格式的原始点云作为备份需要的时候可以重新构建 ikd-tree。这样即使树结构出了问题也不至于把辛苦建的地图弄丢。3. 重定位全链路从开机到找回自己的位置3.1 重定位到底在解决什么问题建图的时候FAST-LIO2 从原点出发一边走一边把点云拼到地图里同时输出每一时刻的位姿。这个过程是连续的位姿是递推出来的。但一旦系统重启或者机器人被搬到了别的地方递推的链条就断了。这时候如果直接开始新的建图就会得到一张和旧地图对不上的新地图之前建的东西全白费。重定位要做的就是在已知地图的前提下估计机器人当前在地图中的位姿。这个问题本质上是一个全局配准问题比里程计那种局部配准难得多因为没有任何先验告诉你机器人在哪。常见的做法是先做一个粗定位比如用描述子或者穷举搜索再做一个精配准比如 ICP 或者 NDT。但在 FAST-LIO2 的生态里有一套更轻量的方案就是结合 LI-Init 和 fast_lio_localization 来做。LI-Init 的作用是给系统一个可靠的初始状态。它通过静止状态下的一段激光和 IMU 数据估计出重力方向、初始姿态和陀螺零偏。这一步很重要因为 iEKF 对初始值敏感如果初始姿态歪得太多滤波器可能收敛到错误的地方。LI-Init 相当于帮你把起跑线摆正后面的重定位才有意义。3.2 fast_lio_localization 的工作流程fast_lio_localization 这个包的核心思路是用 FAST-LIO2 做里程计用已知地图做全局约束通过一个额外的位姿图或者滤波环节把两者融合起来。具体来说它订阅 FAST-LIO2 输出的里程计位姿和当前帧点云然后在地图里做配准得到一个全局位姿的观测再用这个观测去修正里程计的漂移。这个包通常包含几个关键节点。一个是地图加载节点负责把预先建好的 PCD 地图读进来构建成 KD 树供查询。一个是全局定位节点负责接收当前帧点云做粗配准和精配准输出全局位姿。还有一个是坐标变换节点负责把 FAST-LIO2 的局部坐标系和地图的全局坐标系对齐。实际使用的时候你需要先确认地图的坐标系和 FAST-LIO2 输出的坐标系之间的关系。如果建图时就是从原点开始的那地图坐标系和里程计坐标系在初始时刻是重合的。但重定位时机器人可能在任何位置所以需要一个初始的猜测。这个猜测可以来自 GPS、来自上一次的位姿、或者最简单粗暴的——手动在 rviz 里给。3.3 rviz 手动定位看似笨拙但极其可靠很多人觉得手动给初始位姿很 low但在实际调试和很多应用场景里这是最靠谱的办法。原因很简单自动全局定位在对称环境、重复结构、或者地图质量不好的时候很容易失败而人眼一看就知道机器人在哪。rviz 里的2D Pose Estimate工具就是干这个的点一下拖一下给一个带方向的箭头系统就知道大概位置了。手动定位的操作要点有几个。第一箭头的位置要尽量准最好对着机器人实际所在的物理位置误差控制在半米以内这样精配准才容易收敛。第二箭头的方向要对着机器人的朝向如果方向反了配准可能会收敛到 180 度翻转的解。第三给完初始位姿后不要立刻让机器人快速移动先静止几秒让配准稳定下来观察 rviz 里的点云和地图是否对齐。如果对齐了再慢慢移动。注意手动定位时如果发现点云和地图始终对不上先检查地图的坐标系设置是否正确再检查 FAST-LIO2 输出的位姿是否已经稳定。有时候问题不在重定位本身而在里程计还没收敛。3.4 自动重定位的触发与切换逻辑手动定位适合调试和简单场景但产品化的时候还是需要自动重定位。常见的做法是设置一个重定位触发条件比如系统启动后自动尝试全局定位或者检测到里程计位姿和地图匹配度低于阈值时触发。触发之后先做粗定位可以用 Scan Context 之类的描述子做候选搜索也可以用多分辨率的地图做穷举。粗定位给出几个候选位姿然后对每个候选做精配准取匹配得分最高的作为结果。这里有个工程上的细节重定位成功后如何切换。如果直接从里程计位姿跳到重定位位姿可能会有一个大的跳变导致控制模块收到一个突变的目标。稳妥的做法是做一个平滑过渡比如在几秒内把位姿从旧值插值到新值或者用一个滤波器把跳变限幅。这个逻辑需要根据你的下游模块来设计没有通用答案。4. 实操过程一步步把重定位跑起来4.1 环境准备与依赖确认在开始之前先确认你的环境里已经装好了 ROS推荐 Noetic 或 Melodic、FAST-LIO2 本体、以及 fast_lio_localization 包。FAST-LIO2 的编译依赖包括 Eigen、PCL、Sophus 等这些在它的 README 里都有说明。fast_lio_localization 通常还依赖一些额外的包比如用于位姿图优化的 gtsam 或者 ceres具体看你的版本。编译的时候有个常见坑PCL 版本冲突。如果你的系统里同时装了 ROS 自带的 PCL 和手动编译的 PCL可能会出现找不到符号或者版本不匹配的问题。解决办法是在 CMakeLists 里显式指定 PCL 的路径或者用find_package(PCL REQUIRED COMPONENTS common io kdtree)这样的方式精确控制。我一般会在编译前先echo $PCL_DIR确认一下用的是哪个版本。4.2 地图的准备与格式转换重定位需要一张预先建好的地图。如果你之前用 FAST-LIO2 建过图通常会得到一个cloud.pcd或者类似的点云文件。但 fast_lio_localization 可能需要特定格式的地图比如带法向量的点云或者经过降采样的版本。这时候就需要做格式转换。转换可以用 PCL 的工具也可以自己写个小脚本。关键是要保证地图的坐标系和重定位时使用的坐标系一致。如果建图时用了extrinsic_R和extrinsic_T做外参变换那地图里的点已经是变换后的坐标了重定位时不需要再变。如果没做变换就需要在加载地图时补上。地图的降采样也很重要。原始地图可能非常稠密直接加载会占用大量内存查询也慢。我一般会用体素滤波降到 0.2 米左右的粒度这样既保留了结构特征又控制了规模。降采样之后记得检查一下地图有没有明显的空洞或者缺失特别是墙角、柱子这些特征丰富的地方。4.3 启动顺序与参数配置启动顺序对重定位的成功率影响很大。我的习惯是先启动地图加载节点确认地图加载完成再启动 FAST-LIO2让它先跑起来输出稳定的里程计最后启动重定位节点给初始位姿。这个顺序能避免重定位节点在里程计还没稳定的时候就开始配准导致错误的收敛。参数配置方面重点看几个。filter_size_map控制地图降采样粒度前面说过。max_iteration控制配准的迭代次数一般 30 到 50 够用太大了浪费时间。transformation_epsilon是收敛判据太小了可能不收敛太大了精度不够通常设 1e-6 到 1e-8 之间。还有correspondence_distance控制最近邻搜索的最大距离这个要根据地图的稠密程度来调。4.4 手动定位的完整操作记录假设你已经把地图加载好了FAST-LIO2 也在跑rviz 里能看到当前帧点云和地图点云。这时候在 rviz 的工具栏里找到2D Pose Estimate点击它然后在机器人大概的位置按住鼠标左键拖出一个箭头箭头的方向就是机器人的朝向。松开鼠标后重定位节点会收到这个初始位姿开始做精配准。配准的过程在终端里能看到输出通常会打印迭代次数、残差、以及最终的位姿。如果残差在下降说明在收敛如果残差震荡或者不降可能是初始位姿给得太偏或者地图和当前场景差异太大。这时候可以重新给一个初始位姿稍微调整一下位置和方向。配准成功后rviz 里的当前帧点云应该和地图点云基本重合。你可以移动机器人观察点云是否始终贴合地图。如果贴合得好说明重定位稳定了如果慢慢漂移可能是里程计的漂移没有被完全修正需要检查重定位的更新频率和权重。4.5 自动重定位的配置与测试自动重定位的配置主要在 launch 文件里。你需要设置触发条件比如启动后自动执行一次全局定位或者订阅一个话题来手动触发。全局定位的实现可能是基于 Scan Context 的也可能是基于多分辨率搜索的具体看包的实现。测试的时候我建议先在已知位置启动看看自动重定位能不能找回正确的位姿。然后再把机器人搬到别的位置看看能不能重新定位。如果失败先检查地图是否覆盖了当前位置再检查描述子的参数是否合适。有时候场景太相似描述子区分不开就需要引入额外的信息比如 IMU 的重力方向或者轮式里程计的粗略位置。5. 常见问题与排查技巧实录5.1 重定位失败的典型原因重定位失败最常见的原因是初始位姿给得太离谱。如果初始位置离真实位置超过几米或者方向差了 90 度以上精配准很容易陷入局部最优。解决办法就是手动给一个靠谱的初始值或者用粗定位先缩小范围。第二个原因是地图和当前场景不一致。比如建图之后环境发生了变化多了或者少了一些结构配准就会受影响。这种情况下可以考虑更新地图或者在配准时降低对变化区域的权重。第三个原因是点云质量太差。如果激光雷达被遮挡或者场景里特征太少比如长走廊配准的约束不足也会失败。这时候可以尝试增加配准的迭代次数或者引入其他传感器的信息。5.2 ikd-tree 相关的报错与处理ikd-tree 常见的报错是内存不足。如果地图太大或者降采样不够内存会迅速耗尽。解决办法是调大filter_size_map或者调小cube_side_length限制地图的局部范围。另一个常见问题是查询返回空结果。这通常是因为地图里没有点或者查询范围太小。检查一下地图是否加载成功以及max_dist是否设得太小。还有一个坑是多线程访问冲突。如果配准线程和地图更新线程同时访问 ikd-tree可能会崩溃。解决办法是加锁或者用无锁的数据结构。FAST-LIO2 本身对这个问题做了处理但如果你自己改代码要特别注意。5.3 位姿跳变与平滑处理重定位成功后位姿可能会有一个跳变。如果下游的控制模块直接用了这个跳变后的位姿可能会导致机器人突然转向或者加速。解决办法是加一个平滑环节比如用一阶低通滤波或者用插值的方式在几秒内过渡。平滑的参数需要根据机器人的动力学来调。过渡太快机器人反应不过来过渡太慢重定位的效果体现不出来。我一般会设一个 2 到 3 秒的过渡时间具体看机器人的最大速度和加速度。5.4 常见问题速查表问题现象可能原因排查方向解决办法重定位后点云与地图不重合初始位姿偏差大检查 rviz 中箭头位置和方向重新给初始位姿靠近真实位置配准残差不下降地图与场景差异大对比地图和当前帧特征更新地图或降低变化区域权重ikd-tree 内存持续增长降采样不足查看 filter_size_map 配置调大降采样粒度重定位后位姿跳变切换逻辑太生硬检查位姿发布频率和下游处理加平滑过渡或限幅自动重定位失败描述子区分度不够检查场景是否有重复结构引入 IMU 或轮式里程计辅助编译时报 PCL 符号错误PCL 版本冲突确认 PCL_DIR 和链接库显式指定 PCL 路径5.5 几个文档里不会写的避坑技巧第一个技巧建图时就把地图质量搞好。很多人建图的时候随便跑一圈地图里全是重影和噪声重定位的时候自然对不上。建图时尽量慢速、平稳避免快速旋转和剧烈晃动这样建出来的地图干净重定位成功率会高很多。第二个技巧重定位前先让 IMU 静止初始化。LI-Init 需要一段静止数据来估计重力方向和零偏如果一开机就动初始化不准后面的重定位也会受影响。我一般会等系统静止 3 到 5 秒看到初始化完成的提示再开始操作。第三个技巧rviz 里把地图和当前帧用不同颜色显示。这样对齐的时候一眼就能看出偏差。地图用灰色当前帧用红色或者绿色重合的地方颜色会叠加偏的地方一眼就能看出来。第四个技巧保存重定位成功后的位姿。如果每次开机都要重新定位很麻烦。可以把成功定位后的位姿存下来下次开机直接加载作为初始值这样重定位会快很多成功率也高。6. 从能跑到好用我的几点个人体会把 FAST-LIO2 从 demo 跑到实际可用的系统ikd-tree 和重定位这两块是必须啃下来的。ikd-tree 决定了你的系统能不能长时间稳定运行重定位决定了你的系统能不能反复使用同一张地图。这两个问题解决好了FAST-LIO2 才真正算是一个能落地的定位方案。我在实际项目里踩过的最大坑是低估了地图质量对重定位的影响。一开始觉得建图随便跑跑就行结果重定位怎么都调不好后来老老实实重新建了一张干净的地图问题迎刃而解。所以如果你正在为重定位发愁不妨先回头看看你的地图是不是够好。另一个体会是不要迷信全自动。手动给初始位姿虽然看起来不智能但在很多场景下它就是最可靠的。产品化的时候可以做成“自动为主、手动兜底”的模式自动失败了让人点一下用户体验反而更好。最后分享一个小技巧如果你用的是 fast_lio_localization可以试着把重定位的更新频率调低一点比如 1 Hz 或者 2 Hz而不是每帧都做。这样既能维持全局约束又不会占用太多计算资源。重定位本质上是一个慢过程没必要跟里程计一样快。