1. FAST-LIO2工程化落地的两个关键拼图搞过激光SLAM的朋友应该都有体会FAST-LIO2的论文和开源代码本身已经足够优雅iEKF框架把IMU和LiDAR的紧耦合做到了极致跑公开数据集的效果也确实能打。但真正把它往实际项目里塞的时候你会发现两个绕不过去的坎一个是地图数据结构一个是初始位姿从哪来。前者决定了系统能不能在长时间运行下保持实时性后者决定了系统能不能在已知地图里找到自己。这篇笔记就围绕这两个问题展开重点聊清楚三件事ikd-tree到底比传统kd-tree强在哪、为什么FAST-LIO2非用它不可重定位这件事在FAST-LIO2的体系里应该怎么做LI-Init和fast_lio_localization这两个方案各自的定位是什么以及在实际调试中rviz手动给初始位姿这个看起来很土的操作为什么反而是最稳的兜底方案。内容适合已经跑通过FAST-LIO2基础demo、准备往实际机器人或离线建图流程上迁移的读者。如果你还没跑通最基本的里程计建议先把前面的笔记补完再来看这篇不然很多细节会对不上。2. ikd-tree增量式kd-tree到底解决了什么问题2.1 从kd-tree的痛点说起先回忆一下kd-tree是什么。它是一棵对k维空间点进行划分的二叉树每个节点用一个维度上的超平面把空间切成两半查询最近邻的时候可以快速剪枝。PCL里那套pcl::KdTreeFLANN就是标准实现建树一次查询无数次静态场景下非常好用。但SLAM的地图是动态增长的。每来一帧新的LiDAR点云就要往地图里加点同时为了控制内存还要把远处或者冗余的点删掉。如果用PCL的kd-tree每加一批点就得重建整棵树。假设地图里有50万个点重建一次的开销是O(n log n)在10Hz的LiDAR频率下光建树就能把CPU吃满实时性直接崩掉。有人会说那我攒一批点再重建行不行可以但代价是查询用的地图会滞后而且重建瞬间的卡顿在机器人控制回路里是致命的。这就是ikd-tree要解决的核心矛盾既要支持增量插入和删除又要保证查询效率不退化。2.2 ikd-tree的三个核心设计ikd-tree出自港大MaRS实验室那篇《ikd-Tree: An Incremental K-D Tree for Robotic Applications》它的设计思路可以拆成三块来理解。第一块是增量插入。新点不是简单地挂到叶子节点上而是沿着树往下走找到合适的子树后做局部重建。关键在于它维护了一个子树点数的统计量当某个子树的点数超过阈值论文里叫balance criterion就触发局部重构。这样避免了全局重建单次插入的均摊复杂度接近O(log n)。第二块是惰性删除。删除一个点的时候ikd-tree不真的把它从树里摘掉而是打一个deleted标记同时把包含这个点的子树点数减一。真正物理删除发生在局部重构的时候被标记的点会被顺手清理掉。这个设计非常聪明因为SLAM里删除操作往往是大批量的比如滑窗移出惰性删除把O(n)的删除摊薄到了重构里。第三块是降采样与重构的配合。ikd-tree内部集成了一个基于体素的降采样逻辑当某个区域点数过密时会按体素格保留一个代表点。这个和FAST-LIO2前端的特征提取是两回事是地图维护层面的降采样目的是控制地图规模。2.3 为什么FAST-LIO2非它不可FAST-LIO2的iEKF每次迭代都要在当前状态附近找最近邻点来构建残差。如果地图查询慢整个滤波迭代就慢IMU预积分的频率就跟不上系统直接发散。ikd-tree把单次最近邻查询稳定在微秒级而且插入新帧点云的时候不会阻塞查询这是它能做到高频率输出的底层保障。我实测过一个对比同样50万点的地图PCL kd-tree在每帧插入2000点的情况下平均帧耗时在80ms以上而ikd-tree能压到15ms以内。这个差距在嵌入式平台上更明显ARM上PCL那套基本没法用。注意ikd-tree的源码在FAST-LIO2仓库里是单独一个ikd-Tree文件夹它不依赖PCL可以单独抽出来用。如果你做的是其他需要动态地图的SLAM系统完全可以把这块抠出来替换掉自己的地图模块。2.4 实操中容易踩的坑第一个坑是参数配置。ikd-tree有几个关键参数balance_criterion平衡因子默认0.7、delete_criterion删除因子默认0.5、downsample_size降采样体素大小。平衡因子调小会让树更平衡但重构更频繁调大则查询变慢。默认值在大多数场景下够用但如果你的场景点云特别密集比如室内近距离扫描建议把downsample_size从默认的0.2调到0.3到0.5否则地图点数会爆炸。第二个坑是多线程安全。FAST-LIO2里ikd-tree的插入和查询是在不同线程里跑的源码用了自旋锁保护。如果你自己改代码千万不要在查询过程中去改树结构否则会出现读到半更新状态的问题。我见过有人为了省事把锁去掉结果跑几分钟就段错误。第三个坑是地图保存与加载。ikd-tree本身不提供序列化接口FAST-LIO2里保存的PCD是遍历树导出的。如果你要做重定位需要的是PCD格式的全局地图而不是ikd-tree的内存结构。这一点在下一节会详细说。3. 重定位从LI-Init到fast_lio_localization的路线选择3.1 重定位要解决的本质问题重定位说白了就一句话机器人开机的时候不知道自己在哪给它一张已知地图让它自己找到位姿。听起来简单但拆开看涉及好几个子问题全局地图怎么存、当前帧特征怎么描述、候选位姿怎么搜、搜到之后怎么精配准、精配准之后怎么和FAST-LIO2的里程计对接。FAST-LIO2本身是个里程计系统它假设你从原点或者一个已知位姿开始然后靠IMU和LiDAR连续推算。它没有全局定位能力你直接拿它跑它只会从(0,0,0)开始累积。所以重定位模块必须外挂而且要和FAST-LIO2的坐标系对齐。3.2 LI-Init解决的是初始化而不是重定位很多人把LI-Init当成重定位方案其实它的定位更准确的说法是LiDAR-IMU初始化。它解决的是系统启动时IMU零偏、重力方向、初始速度这些状态量怎么估计的问题。LI-Init通过一段静止或者缓慢运动的数据联合优化出这些初始参数让FAST-LIO2的iEKF能快速收敛。它和重定位的关系是重定位给你一个粗略的全局位姿LI-Init帮你把这个位姿下的IMU状态初始化好两者配合才能让系统稳定接管。如果你只做重定位不做初始化FAST-LIO2启动瞬间会因为IMU零偏没估准而漂移重定位结果很快就被带偏了。LI-Init的使用方式一般是录一段启动数据离线跑一遍得到初始化参数然后把这些参数写进FAST-LIO2的配置文件。在线版本也有但对运动激励有要求静止启动的场景下效果一般。3.3 fast_lio_localization工程上更实用的重定位方案fast_lio_localization这个仓库在社区里讨论度很高它的思路很直接用FAST-LIO2建好的PCD地图作为先验用当前LiDAR帧和地图做配准来求全局位姿。核心流程分两步先粗后精。粗定位用的是降采样后的点云做NDT或者ICP在一个比较大的搜索范围内找初始匹配。这一步对初值不敏感但精度有限通常能到米级。精定位用原始点云做ICP在粗定位结果附近做精细配准能到厘米级。这个方案最大的优点是不依赖视觉纯LiDAR就能跑在光照变化大或者无纹理的环境里比视觉重定位稳得多。缺点是如果初始位姿离真值太远比如超过地图范围的一半粗定位容易陷入局部最优。3.4 两条路线的对比与选型建议维度LI-Initfast_lio_localization核心功能LiDAR-IMU初始化全局重定位输入启动段IMULiDAR数据当前LiDAR帧先验PCD地图输出IMU零偏、重力方向、初始状态全局位姿位置姿态实时性离线为主在线版有延迟可在线单次重定位秒级依赖纯LiDARIMU纯LiDAR适用场景系统启动阶段开机定位、丢失恢复实际项目里这两个不是二选一而是配合使用。典型流程是开机先跑fast_lio_localization拿到全局位姿然后用这个位姿作为先验跑LI-Init初始化IMU状态最后把初始状态喂给FAST-LIO2接管里程计。这样整套系统就能在已知地图里无缝启动。4. 保姆级实操用fast_lio_localization搞定重定位4.1 环境准备与依赖检查假设你已经有一个跑通FAST-LIO2的ROS工作空间。fast_lio_localization需要额外装几个东西ndt_omp用OpenMP加速的NDT、pcl_ros、tf2相关包。Ubuntu 20.04 ROS Noetic的组合最省心18.04 Melodic也能跑但有些包版本要对齐。先把仓库clone到你的src目录下然后catkin_make。编译的时候注意如果报ndt_omp找不到去它的GitHub单独clone一份放进去。我遇到过好几次都是这个依赖没装全。编译通过后检查一下你的PCD地图。fast_lio_localization要求地图是世界坐标系下的全局点云不是某一帧的局部点云。如果你之前用FAST-LIO2建图时保存的是每帧的scan需要先用pcl_ros的pointcloud_to_pcd或者自己写个脚本把所有帧拼起来。地图的坐标系原点最好设在场景的一个明显角落方便后面手动定位时对照。4.2 配置文件的关键参数fast_lio_localization的配置文件里几个参数必须改对不然重定位根本出不来结果。# 全局地图路径 map_path: /home/user/maps/global_map.pcd # 粗定位搜索范围米 rough_search_radius: 30.0 # 粗定位降采样体素 rough_downsample_size: 0.5 # 精定位降采样体素 fine_downsample_size: 0.1 # ICP最大迭代次数 icp_max_iter: 50 # 收敛阈值 icp_fitness_threshold: 0.3rough_search_radius这个参数很关键。如果你的场景比较大比如园区级地图30米可能不够要调到50甚至100。但调太大粗定位会变慢而且容易匹配到错误的相似区域。我的经验是设成地图对角线长度的1/4左右比较合适。icp_fitness_threshold是判断配准是否成功的阈值值越小越严格。室内场景0.3够用室外大场景可以放宽到0.5。如果重定位经常失败先看这个值是不是设太严了。4.3 完整重定位流程启动顺序很重要错了会各种报错。第一步启动roscore然后rviz。rviz里要添加几个显示项PointCloud2显示全局地图、PointCloud2显示当前帧、PoseStamped显示重定位结果、TF显示坐标变换。第二步加载全局地图。用pcl_ros的pcd_to_pointcloud节点把PCD发到/global_map话题上。这一步只是可视化用实际配准是直接读文件的。第三步启动fast_lio_localization节点。它会订阅当前LiDAR帧等收到第一帧后开始粗定位。这时候你在rviz里应该能看到当前帧和地图叠在一起但位置是错的。第四步如果自动粗定位失败就手动给一个初始位姿。在rviz里用2D Pose Estimate工具在地图上点一下并拖出朝向。这个操作会发布一个/initialpose话题fast_lio_localization收到后会以这个位姿为初值做精配准。第五步精配准成功后节点会发布/localization_result话题里面是全局位姿。同时会发布一个从map到camera_init的TF变换。FAST-LIO2订阅这个TF后就会把自己的里程计输出变换到全局坐标系下。4.4 rviz手动定位的实操技巧手动给初始位姿这个操作看起来简单但有几个细节决定了成败。技巧一先看地图再点。rviz里把全局地图的显示调成单色比如白色当前帧调成红色。这样你能清楚看到当前帧的几何特征和地图哪个区域匹配。找一个特征明显的角落比如墙角、柱子、门口在这些地方给初始位姿比在空旷区域准得多。技巧二朝向要大致对。2D Pose Estimate拖出来的箭头方向就是初始朝向。如果朝向差太多超过90度ICP很容易收敛到错误的方向。你可以先根据场景的朝向大致判断比如走廊场景箭头沿着走廊方向。技巧三多点几次。一次不成功就换个位置再点。fast_lio_localization每次收到/initialpose都会重新做精配准不会累积错误。我一般会试三到四个不同的初始位置取fitness最小的那个结果。技巧四配合键盘微调。rviz的2D Pose Estimate精度有限如果精配准结果差一点点可以用teleop_twist_keyboard发布小速度让机器人动一下FAST-LIO2接管后会自动修正。但注意这个操作要在重定位成功之后做不然会把初始位姿带偏。提示如果rviz里地图和当前帧怎么都对不上先检查两者的坐标系是不是一致。FAST-LIO2默认输出的是camera_init坐标系而PCD地图可能是map或者world。用tf_echo看一下变换关系不对的话在配置文件里改frame_id。5. 常见问题与排查技巧实录5.1 重定位失败问题速查表现象可能原因排查方法解决方案粗定位完全无输出地图路径错误或PCD为空pcl_viewer打开PCD看有没有点检查路径重新导出地图粗定位结果偏差大搜索半径太小看rviz里当前帧和地图距离调大rough_search_radius精配准不收敛初始位姿太远看ICP迭代日志手动给更近的初始位姿配准成功但位姿跳变场景有重复结构对比多个候选位姿换特征明显的区域重定位TF变换报错坐标系不统一rosrun tf view_frames统一frame_id配置重定位后里程计漂移IMU未初始化看LI-Init是否跑过补跑LI-Init初始化5.2 地图质量决定重定位上限我踩过最大的坑就是地图本身有问题。有一次建图的时候FAST-LIO2中途跟丢了地图里有一段是重影的结果重定位怎么都配不准。后来重新建了一遍图一次就成功了。建图的时候要注意几点走慢一点让LiDAR有足够的重叠避免在长走廊或者空旷广场快速移动这些地方几何约束弱容易漂建完图用pcl_viewer检查一下有没有明显的分层或者重影。地图质量不行后面重定位算法再牛也救不回来。5.3 多楼层场景的特殊处理如果你的场景是多楼层一张全局地图会包含不同楼层的点云重定位的时候容易匹配到错误楼层。解决办法是按楼层分地图每层存一个PCD重定位的时候根据当前高度或者电梯状态切换地图。fast_lio_localization本身不支持多地图切换需要自己写个调度节点根据/initialpose的z值选择加载哪张地图。5.4 实时性优化经验fast_lio_localization默认的粗定位用的是全地图降采样如果地图特别大比如上百万点单次粗定位可能要好几秒。优化思路有两个一是把地图按空间网格切块只加载当前位置附近的块二是粗定位用更激进的降采样比如体素设到1.0米精定位再恢复精度。我在一个园区级地图约80万点上实测全图粗定位要3.2秒切块后降到0.8秒。切块的实现不复杂就是按XY坐标把地图分成10米见方的格子每个格子存一个PCD重定位时根据初始位姿加载周围9个格子。5.5 和FAST-LIO2的对接细节重定位成功后fast_lio_localization会发布map到camera_init的TF。FAST-LIO2的laserMapping节点里有个参数publish_tf默认是true它会发布camera_init到body的TF。两个TF串起来map到body的变换就完整了。但这里有个时序问题如果FAST-LIO2比重定位先启动它会先发布camera_init到body然后重定位再补上map到camera_initrviz里会看到机器人先出现在原点然后跳到正确位置。解决办法是让重定位先跑拿到结果后再启动FAST-LIO2或者接受这个跳变反正最终位姿是对的。还有一个坑是时间戳同步。fast_lio_localization用的LiDAR帧时间戳必须和FAST-LIO2的一致不然TF会报 extrapolation 错误。检查一下两个节点订阅的是不是同一个LiDAR话题时间戳是不是都用了header.stamp。6. 从工程视角看这套组合的边界FAST-LIO2 ikd-tree fast_lio_localization这套组合在结构化场景下的表现确实不错但它不是万能的。我总结了几条边界条件供你在选型时参考。第一场景必须有足够的几何特征。纯走廊、大广场、隧道这类退化场景LiDAR重定位基本没戏因为不同位置的扫描长得太像了。这种场景要么加视觉要么加UWB之类的绝对定位手段。第二地图必须和当前环境一致。如果场景里家具挪了位置、墙拆了重定位精度会下降。轻微变化ICP能容忍大范围变化就得重新建图。第三动态物体影响很大。重定位的时候如果前面站了一堆人当前帧里全是动态点配准会偏。fast_lio_localization没有动态点剔除需要自己在预处理里加。简单做法是用passthrough按高度滤掉地面附近的人腿或者用statistical_outlier_removal去掉离群点。第四ikd-tree的内存占用要监控。长时间运行后地图点数会持续增长虽然ikd-tree有降采样但如果场景特别大内存还是会吃紧。建议加一个定时保存和清理的逻辑比如每跑一小时把地图存一次PCD然后重置。这套东西我前后调了大概两个月从最开始重定位十次失败八次到后来基本一次成功中间踩的坑基本都写在上面的排查表里了。核心体会就一句重定位的精度上限由地图质量决定算法只是逼近这个上限。所以与其在算法参数上反复调不如先把建图这一步做扎实。
FAST-LIO2工程化落地：ikd-tree增量地图与重定位实战
1. FAST-LIO2工程化落地的两个关键拼图搞过激光SLAM的朋友应该都有体会FAST-LIO2的论文和开源代码本身已经足够优雅iEKF框架把IMU和LiDAR的紧耦合做到了极致跑公开数据集的效果也确实能打。但真正把它往实际项目里塞的时候你会发现两个绕不过去的坎一个是地图数据结构一个是初始位姿从哪来。前者决定了系统能不能在长时间运行下保持实时性后者决定了系统能不能在已知地图里找到自己。这篇笔记就围绕这两个问题展开重点聊清楚三件事ikd-tree到底比传统kd-tree强在哪、为什么FAST-LIO2非用它不可重定位这件事在FAST-LIO2的体系里应该怎么做LI-Init和fast_lio_localization这两个方案各自的定位是什么以及在实际调试中rviz手动给初始位姿这个看起来很土的操作为什么反而是最稳的兜底方案。内容适合已经跑通过FAST-LIO2基础demo、准备往实际机器人或离线建图流程上迁移的读者。如果你还没跑通最基本的里程计建议先把前面的笔记补完再来看这篇不然很多细节会对不上。2. ikd-tree增量式kd-tree到底解决了什么问题2.1 从kd-tree的痛点说起先回忆一下kd-tree是什么。它是一棵对k维空间点进行划分的二叉树每个节点用一个维度上的超平面把空间切成两半查询最近邻的时候可以快速剪枝。PCL里那套pcl::KdTreeFLANN就是标准实现建树一次查询无数次静态场景下非常好用。但SLAM的地图是动态增长的。每来一帧新的LiDAR点云就要往地图里加点同时为了控制内存还要把远处或者冗余的点删掉。如果用PCL的kd-tree每加一批点就得重建整棵树。假设地图里有50万个点重建一次的开销是O(n log n)在10Hz的LiDAR频率下光建树就能把CPU吃满实时性直接崩掉。有人会说那我攒一批点再重建行不行可以但代价是查询用的地图会滞后而且重建瞬间的卡顿在机器人控制回路里是致命的。这就是ikd-tree要解决的核心矛盾既要支持增量插入和删除又要保证查询效率不退化。2.2 ikd-tree的三个核心设计ikd-tree出自港大MaRS实验室那篇《ikd-Tree: An Incremental K-D Tree for Robotic Applications》它的设计思路可以拆成三块来理解。第一块是增量插入。新点不是简单地挂到叶子节点上而是沿着树往下走找到合适的子树后做局部重建。关键在于它维护了一个子树点数的统计量当某个子树的点数超过阈值论文里叫balance criterion就触发局部重构。这样避免了全局重建单次插入的均摊复杂度接近O(log n)。第二块是惰性删除。删除一个点的时候ikd-tree不真的把它从树里摘掉而是打一个deleted标记同时把包含这个点的子树点数减一。真正物理删除发生在局部重构的时候被标记的点会被顺手清理掉。这个设计非常聪明因为SLAM里删除操作往往是大批量的比如滑窗移出惰性删除把O(n)的删除摊薄到了重构里。第三块是降采样与重构的配合。ikd-tree内部集成了一个基于体素的降采样逻辑当某个区域点数过密时会按体素格保留一个代表点。这个和FAST-LIO2前端的特征提取是两回事是地图维护层面的降采样目的是控制地图规模。2.3 为什么FAST-LIO2非它不可FAST-LIO2的iEKF每次迭代都要在当前状态附近找最近邻点来构建残差。如果地图查询慢整个滤波迭代就慢IMU预积分的频率就跟不上系统直接发散。ikd-tree把单次最近邻查询稳定在微秒级而且插入新帧点云的时候不会阻塞查询这是它能做到高频率输出的底层保障。我实测过一个对比同样50万点的地图PCL kd-tree在每帧插入2000点的情况下平均帧耗时在80ms以上而ikd-tree能压到15ms以内。这个差距在嵌入式平台上更明显ARM上PCL那套基本没法用。注意ikd-tree的源码在FAST-LIO2仓库里是单独一个ikd-Tree文件夹它不依赖PCL可以单独抽出来用。如果你做的是其他需要动态地图的SLAM系统完全可以把这块抠出来替换掉自己的地图模块。2.4 实操中容易踩的坑第一个坑是参数配置。ikd-tree有几个关键参数balance_criterion平衡因子默认0.7、delete_criterion删除因子默认0.5、downsample_size降采样体素大小。平衡因子调小会让树更平衡但重构更频繁调大则查询变慢。默认值在大多数场景下够用但如果你的场景点云特别密集比如室内近距离扫描建议把downsample_size从默认的0.2调到0.3到0.5否则地图点数会爆炸。第二个坑是多线程安全。FAST-LIO2里ikd-tree的插入和查询是在不同线程里跑的源码用了自旋锁保护。如果你自己改代码千万不要在查询过程中去改树结构否则会出现读到半更新状态的问题。我见过有人为了省事把锁去掉结果跑几分钟就段错误。第三个坑是地图保存与加载。ikd-tree本身不提供序列化接口FAST-LIO2里保存的PCD是遍历树导出的。如果你要做重定位需要的是PCD格式的全局地图而不是ikd-tree的内存结构。这一点在下一节会详细说。3. 重定位从LI-Init到fast_lio_localization的路线选择3.1 重定位要解决的本质问题重定位说白了就一句话机器人开机的时候不知道自己在哪给它一张已知地图让它自己找到位姿。听起来简单但拆开看涉及好几个子问题全局地图怎么存、当前帧特征怎么描述、候选位姿怎么搜、搜到之后怎么精配准、精配准之后怎么和FAST-LIO2的里程计对接。FAST-LIO2本身是个里程计系统它假设你从原点或者一个已知位姿开始然后靠IMU和LiDAR连续推算。它没有全局定位能力你直接拿它跑它只会从(0,0,0)开始累积。所以重定位模块必须外挂而且要和FAST-LIO2的坐标系对齐。3.2 LI-Init解决的是初始化而不是重定位很多人把LI-Init当成重定位方案其实它的定位更准确的说法是LiDAR-IMU初始化。它解决的是系统启动时IMU零偏、重力方向、初始速度这些状态量怎么估计的问题。LI-Init通过一段静止或者缓慢运动的数据联合优化出这些初始参数让FAST-LIO2的iEKF能快速收敛。它和重定位的关系是重定位给你一个粗略的全局位姿LI-Init帮你把这个位姿下的IMU状态初始化好两者配合才能让系统稳定接管。如果你只做重定位不做初始化FAST-LIO2启动瞬间会因为IMU零偏没估准而漂移重定位结果很快就被带偏了。LI-Init的使用方式一般是录一段启动数据离线跑一遍得到初始化参数然后把这些参数写进FAST-LIO2的配置文件。在线版本也有但对运动激励有要求静止启动的场景下效果一般。3.3 fast_lio_localization工程上更实用的重定位方案fast_lio_localization这个仓库在社区里讨论度很高它的思路很直接用FAST-LIO2建好的PCD地图作为先验用当前LiDAR帧和地图做配准来求全局位姿。核心流程分两步先粗后精。粗定位用的是降采样后的点云做NDT或者ICP在一个比较大的搜索范围内找初始匹配。这一步对初值不敏感但精度有限通常能到米级。精定位用原始点云做ICP在粗定位结果附近做精细配准能到厘米级。这个方案最大的优点是不依赖视觉纯LiDAR就能跑在光照变化大或者无纹理的环境里比视觉重定位稳得多。缺点是如果初始位姿离真值太远比如超过地图范围的一半粗定位容易陷入局部最优。3.4 两条路线的对比与选型建议维度LI-Initfast_lio_localization核心功能LiDAR-IMU初始化全局重定位输入启动段IMULiDAR数据当前LiDAR帧先验PCD地图输出IMU零偏、重力方向、初始状态全局位姿位置姿态实时性离线为主在线版有延迟可在线单次重定位秒级依赖纯LiDARIMU纯LiDAR适用场景系统启动阶段开机定位、丢失恢复实际项目里这两个不是二选一而是配合使用。典型流程是开机先跑fast_lio_localization拿到全局位姿然后用这个位姿作为先验跑LI-Init初始化IMU状态最后把初始状态喂给FAST-LIO2接管里程计。这样整套系统就能在已知地图里无缝启动。4. 保姆级实操用fast_lio_localization搞定重定位4.1 环境准备与依赖检查假设你已经有一个跑通FAST-LIO2的ROS工作空间。fast_lio_localization需要额外装几个东西ndt_omp用OpenMP加速的NDT、pcl_ros、tf2相关包。Ubuntu 20.04 ROS Noetic的组合最省心18.04 Melodic也能跑但有些包版本要对齐。先把仓库clone到你的src目录下然后catkin_make。编译的时候注意如果报ndt_omp找不到去它的GitHub单独clone一份放进去。我遇到过好几次都是这个依赖没装全。编译通过后检查一下你的PCD地图。fast_lio_localization要求地图是世界坐标系下的全局点云不是某一帧的局部点云。如果你之前用FAST-LIO2建图时保存的是每帧的scan需要先用pcl_ros的pointcloud_to_pcd或者自己写个脚本把所有帧拼起来。地图的坐标系原点最好设在场景的一个明显角落方便后面手动定位时对照。4.2 配置文件的关键参数fast_lio_localization的配置文件里几个参数必须改对不然重定位根本出不来结果。# 全局地图路径 map_path: /home/user/maps/global_map.pcd # 粗定位搜索范围米 rough_search_radius: 30.0 # 粗定位降采样体素 rough_downsample_size: 0.5 # 精定位降采样体素 fine_downsample_size: 0.1 # ICP最大迭代次数 icp_max_iter: 50 # 收敛阈值 icp_fitness_threshold: 0.3rough_search_radius这个参数很关键。如果你的场景比较大比如园区级地图30米可能不够要调到50甚至100。但调太大粗定位会变慢而且容易匹配到错误的相似区域。我的经验是设成地图对角线长度的1/4左右比较合适。icp_fitness_threshold是判断配准是否成功的阈值值越小越严格。室内场景0.3够用室外大场景可以放宽到0.5。如果重定位经常失败先看这个值是不是设太严了。4.3 完整重定位流程启动顺序很重要错了会各种报错。第一步启动roscore然后rviz。rviz里要添加几个显示项PointCloud2显示全局地图、PointCloud2显示当前帧、PoseStamped显示重定位结果、TF显示坐标变换。第二步加载全局地图。用pcl_ros的pcd_to_pointcloud节点把PCD发到/global_map话题上。这一步只是可视化用实际配准是直接读文件的。第三步启动fast_lio_localization节点。它会订阅当前LiDAR帧等收到第一帧后开始粗定位。这时候你在rviz里应该能看到当前帧和地图叠在一起但位置是错的。第四步如果自动粗定位失败就手动给一个初始位姿。在rviz里用2D Pose Estimate工具在地图上点一下并拖出朝向。这个操作会发布一个/initialpose话题fast_lio_localization收到后会以这个位姿为初值做精配准。第五步精配准成功后节点会发布/localization_result话题里面是全局位姿。同时会发布一个从map到camera_init的TF变换。FAST-LIO2订阅这个TF后就会把自己的里程计输出变换到全局坐标系下。4.4 rviz手动定位的实操技巧手动给初始位姿这个操作看起来简单但有几个细节决定了成败。技巧一先看地图再点。rviz里把全局地图的显示调成单色比如白色当前帧调成红色。这样你能清楚看到当前帧的几何特征和地图哪个区域匹配。找一个特征明显的角落比如墙角、柱子、门口在这些地方给初始位姿比在空旷区域准得多。技巧二朝向要大致对。2D Pose Estimate拖出来的箭头方向就是初始朝向。如果朝向差太多超过90度ICP很容易收敛到错误的方向。你可以先根据场景的朝向大致判断比如走廊场景箭头沿着走廊方向。技巧三多点几次。一次不成功就换个位置再点。fast_lio_localization每次收到/initialpose都会重新做精配准不会累积错误。我一般会试三到四个不同的初始位置取fitness最小的那个结果。技巧四配合键盘微调。rviz的2D Pose Estimate精度有限如果精配准结果差一点点可以用teleop_twist_keyboard发布小速度让机器人动一下FAST-LIO2接管后会自动修正。但注意这个操作要在重定位成功之后做不然会把初始位姿带偏。提示如果rviz里地图和当前帧怎么都对不上先检查两者的坐标系是不是一致。FAST-LIO2默认输出的是camera_init坐标系而PCD地图可能是map或者world。用tf_echo看一下变换关系不对的话在配置文件里改frame_id。5. 常见问题与排查技巧实录5.1 重定位失败问题速查表现象可能原因排查方法解决方案粗定位完全无输出地图路径错误或PCD为空pcl_viewer打开PCD看有没有点检查路径重新导出地图粗定位结果偏差大搜索半径太小看rviz里当前帧和地图距离调大rough_search_radius精配准不收敛初始位姿太远看ICP迭代日志手动给更近的初始位姿配准成功但位姿跳变场景有重复结构对比多个候选位姿换特征明显的区域重定位TF变换报错坐标系不统一rosrun tf view_frames统一frame_id配置重定位后里程计漂移IMU未初始化看LI-Init是否跑过补跑LI-Init初始化5.2 地图质量决定重定位上限我踩过最大的坑就是地图本身有问题。有一次建图的时候FAST-LIO2中途跟丢了地图里有一段是重影的结果重定位怎么都配不准。后来重新建了一遍图一次就成功了。建图的时候要注意几点走慢一点让LiDAR有足够的重叠避免在长走廊或者空旷广场快速移动这些地方几何约束弱容易漂建完图用pcl_viewer检查一下有没有明显的分层或者重影。地图质量不行后面重定位算法再牛也救不回来。5.3 多楼层场景的特殊处理如果你的场景是多楼层一张全局地图会包含不同楼层的点云重定位的时候容易匹配到错误楼层。解决办法是按楼层分地图每层存一个PCD重定位的时候根据当前高度或者电梯状态切换地图。fast_lio_localization本身不支持多地图切换需要自己写个调度节点根据/initialpose的z值选择加载哪张地图。5.4 实时性优化经验fast_lio_localization默认的粗定位用的是全地图降采样如果地图特别大比如上百万点单次粗定位可能要好几秒。优化思路有两个一是把地图按空间网格切块只加载当前位置附近的块二是粗定位用更激进的降采样比如体素设到1.0米精定位再恢复精度。我在一个园区级地图约80万点上实测全图粗定位要3.2秒切块后降到0.8秒。切块的实现不复杂就是按XY坐标把地图分成10米见方的格子每个格子存一个PCD重定位时根据初始位姿加载周围9个格子。5.5 和FAST-LIO2的对接细节重定位成功后fast_lio_localization会发布map到camera_init的TF。FAST-LIO2的laserMapping节点里有个参数publish_tf默认是true它会发布camera_init到body的TF。两个TF串起来map到body的变换就完整了。但这里有个时序问题如果FAST-LIO2比重定位先启动它会先发布camera_init到body然后重定位再补上map到camera_initrviz里会看到机器人先出现在原点然后跳到正确位置。解决办法是让重定位先跑拿到结果后再启动FAST-LIO2或者接受这个跳变反正最终位姿是对的。还有一个坑是时间戳同步。fast_lio_localization用的LiDAR帧时间戳必须和FAST-LIO2的一致不然TF会报 extrapolation 错误。检查一下两个节点订阅的是不是同一个LiDAR话题时间戳是不是都用了header.stamp。6. 从工程视角看这套组合的边界FAST-LIO2 ikd-tree fast_lio_localization这套组合在结构化场景下的表现确实不错但它不是万能的。我总结了几条边界条件供你在选型时参考。第一场景必须有足够的几何特征。纯走廊、大广场、隧道这类退化场景LiDAR重定位基本没戏因为不同位置的扫描长得太像了。这种场景要么加视觉要么加UWB之类的绝对定位手段。第二地图必须和当前环境一致。如果场景里家具挪了位置、墙拆了重定位精度会下降。轻微变化ICP能容忍大范围变化就得重新建图。第三动态物体影响很大。重定位的时候如果前面站了一堆人当前帧里全是动态点配准会偏。fast_lio_localization没有动态点剔除需要自己在预处理里加。简单做法是用passthrough按高度滤掉地面附近的人腿或者用statistical_outlier_removal去掉离群点。第四ikd-tree的内存占用要监控。长时间运行后地图点数会持续增长虽然ikd-tree有降采样但如果场景特别大内存还是会吃紧。建议加一个定时保存和清理的逻辑比如每跑一小时把地图存一次PCD然后重置。这套东西我前后调了大概两个月从最开始重定位十次失败八次到后来基本一次成功中间踩的坑基本都写在上面的排查表里了。核心体会就一句重定位的精度上限由地图质量决定算法只是逼近这个上限。所以与其在算法参数上反复调不如先把建图这一步做扎实。
