最近在做一台室内巡检机器人的回归位功能，说白了就是让车在工作区域内转完一整圈之后，还能自己开回充电桩附近。跑了几版方案，最后回到了FAST_LIO_LOCALIZATION这条开源链路上，配合手上的Livox MID-360，把“建图—录包—重定位”这条完整流水线从头趟了一遍。
这篇文章写给正在被“回充定位”“定点回归”“重复定位”折磨的朋友，尤其是手上有 Livox 雷达（MID-360、Avia 都适用）、想在已有地图上做激光重定位，但又不想自己从零写定位算法的人。我会把从依赖安装、参数配置、PCD 地图保存，到自定义 bag 录制与回放、重定位启动与排错的整个实操过程写清楚，也会把测试中踩过的坑一并交代。
先说明一点：FAST_LIO_LOCALIZATION 并不是一个和 FAST_LIO 完全独立的新工程，它是在 FAST_LIO 建图基础上扩展出来的“重定位模式”。核心思路是先预先建好一张全局点云地图，然后让机器人在运行时把当前雷达帧和这张全局地图做配准，从而修正累积漂移，而不是像纯前端里程计那样一帧帧地增量推算。理解了这一点，后面配置参数时才不会犯方向性错误。
1. 先同步一下背景：重定位到底解决什么问题，为什么我选了这条开源链路
1.1 建图与重定位的根本区别
很多刚开始接触 SLAM 的朋友会把“建图”和“定位”混在一起理解。其实对机器人项目来说，这是两个完全不同的阶段，解决的问题也不一样。
建图解决的是“这个空间长什么样”的问题。机器人带着雷达在场景里走一圈，利用 FAST_LIO 之类的算法，把每一帧点云按里程计推算的位姿拼到同一个坐标系下，形成一张全局点云地图。这个过程允许有一定的累积漂移，只要最终地图看着不重影、拓扑结构没坏，一般就能接受。
重定位解决的是“我在这张地图里的哪个位置”的问题。地图已经是现成的，机器人再进入同一个场景时，需要实时估计自己的全局位姿。这时候如果继续靠里程计积分，轮子打滑、IMU 温漂、地面不平等因素都会让位置慢慢跑偏。重定位要做的是把当前雷达帧和全局地图进行全局匹配，把漂移“拉”回来。
FAST_LIO_LOCALIZATION 相当于把这两件事拆开了：先用 FAST_LIO 建图，再把建图时的全局点云地图加载进来，运行阶段用每一帧扫描和全局地图做配准，输出修正后的里程计位姿。
1.2 为什么是 FAST_LIO_LOCALIZATION，而不是别的方案
市面上能实现激光重定位的开源方案不算少，NDT、icp_localization、LIO-SAM 的重定位模式都能做。我最终选择这条链路，有几个很现实的原因。
第一是Livox 雷达的适配度。FAST_LIO 本身就是为 Livox 系列雷达（MID-360、Avia、Horizon 等）设计的，驱动和点云处理流程都针对非重复扫描特性做了优化。其他方案如果要支持 Livox，通常还得自己改驱动、改点云格式，工作量不小。
第二是地图管理方式。FAST_LIO_LOCALIZATION 使用 ikd-Tree 管理全局地图，支持动态增删点，重定位时当前帧和全局地图的配准效率比较高。实测在室内场景，加载一张几百 MB 的点云地图，匹配频率能做到 10 Hz 左右，基本满足低速机器人导航需求。
第三是不需要额外传感器。如果你有相机，可以后面再做视觉和激光融合；但即使只有一台雷达加内置 IMU，重定位也能跑起来。这对于不想引入太多标定工作的项目特别友好。
1.3 我用的硬件和软件组合
我这台测试机器用的是Livox MID-360，车载一个 Jetson Orin NX，跑 Ubuntu 20.04 + ROS Noetic。MID-360 自带 IMU（BMI088），所以不需要另外接 IMU，这对快速搭建原型很有帮助。Livox Avia 我也在实验室另一台设备上验证过，配置过程几乎一样，只是点云话题名和部分参数略有差异。
如果你手头是 Avia 或者其他 Livox 雷达，下面大部分内容依然适用。遇到雷达型号相关的配置差异，我会单独指出来。
2. 从驱动到三个仓库：环境与依赖的搭建顺序很重要
2.1 Livox 雷达驱动：livox_ros_driver2 是当前唯一推荐
早期项目里有人还在用老的 livox_ros_driver，但对 MID-360 来说，官方已经明确转向livox_ros_driver2，尤其是 MID-360 需要依赖新版驱动里的扩展配置。
安装方式很简单，直接 clone 源码编译：
cd ~/catkin_ws/src git clone https://github.com/Livox-SDK/livox_ros_driver2.git cd ~/catkin_ws catkin_make编译完成后，需要确认驱动里的雷达型号配置。在livox_ros_driver2/config目录下会有一个MID360_config.json（或者config.json），里面最重要的是lidar_type和frame_id这两个字段。MID-360 通常对应lidar_type: 7，Avia 对应lidar_type: 1，不同驱动版本略有差异，打开 JSON 文件看一下注释就能确认。
实际操作中，我建议先单独启动驱动，确认点云话题能正常发布再继续下一步：
roslaunch livox_ros_driver2 rviz MID360.launch如果 rviz 里能看到非重复扫描的点云画面，说明驱动工作正常。此时用rostopic list记下点云话题名和 IMU 话题名，后面配置 FAST_LIO 要用。
2.2 FAST_LIO：建图模式先跑通
FAST_LIO 是重定位链路的地图来源，必须先编译并在实机上跑通建图。
cd ~/catkin_ws/src git clone https://github.com/hku-mars/FAST_LIO.git cd ~/catkin_ws catkin_make编译时注意两个常见问题。
一是Eigen 版本。FAST_LIO 依赖 Eigen3，如果系统里是 Eigen 3.3 以下版本，编译会报alignas相关错误。Ubuntu 20.04 系统自带的 Eigen 3.3.7 通常没问题，如果自己手动装过旧版本，要留意路径覆盖问题。
二是PCL 版本。FAST_LIO 用 PCL 做点云处理，Noetic 自带 PCL 1.10，兼容性没问题。如果用的是 ROS Melodic + PCL 1.8，部分较新版本的 FAST_LIO 源码可能需要小改，建议直接搜一下对应分支或 issue。
2.3 FAST_LIO_LOCALIZATION：重定位本体
这是整条链路的核心仓库：
cd ~/catkin_ws/src git clone https://github.com/HKUST-Aerial-Robotics/FAST_LIO_LOCALIZATION.git cd ~/catkin_ws catkin_make这个仓库的编译依赖 FAST_LIO 的一些头文件，所以必须先编译 FAST_LIO，再编译 FAST_LIO_LOCALIZATION，顺序反了会找不到依赖。
编译完成后，在FAST_LIO_LOCALIZATION/launch里能看到一个类似localization_mid360.launch的启动文件，在config里能看到对应的mid360.yaml。后面所有参数调整都会集中在这两个文件里。
2.4 三个仓库的版本匹配经验
源码仓库更新比较频繁，直接git clone master分支一般能编译过，但不同 commit 之间接口可能有变化。我的建议是：三个仓库尽量在同一时间段拉取，不要一个仓库拉最新、另一个仓库拉一年前的版本。如果你在编译 FAST_LIO_LOCALIZATION 时遇到fastlio_lio相关头文件找不到的问题，多半是 FAST_LIO 仓库更新了目录结构，去两个仓库的 issue 里搜一下关键字就能找到对应 commit。
3. FAST_LIO_LOCALIZATION配置里最容易跑偏的四个参数
3.1 lid_topic 和 imu_topic：话题名必须严格对应
FAST_LIO_LOCALIZATION 和 FAST_LIO 一样，通过参数文件订阅雷达点云和 IMU 数据。打开config/mid360.yaml，开头通常是这样的：
common: lid_topic: "/livox/lidar" imu_topic: "/livox/imu" time_sync_en: false这里有两个坑。
第一是话题名要和 livox_ros_driver2 实际发布的话题名完全一致，一个字母都不能差。如果你启动驱动后雷达话题是/livox/lidar，而配置文件里写的是/livox/left/lidar，算法接收不到数据，程序会一直等待，日志里只显示点云频率为 0。
第二是time_sync_en参数。默认 false 表示使用雷达和 IMU 各自的时间戳，不强制对齐。对于 MID-360 这种一体化设备，官方驱动已经做了硬件同步，保持 false 没问题。如果你把驱动里的时间同步功能打开，又在这里把time_sync_en设为 true，反而可能导致时间戳异常，在 bag 回放时尤其明显。
3.2 雷达型号相关参数：不是所有 Livox 都一样
在 FAST_LIO 和 FAST_LIO_LOCALIZATION 的 yaml 里，有几个参数和雷达型号强相关：
lidar_type：1 代表 Livox 系列，对于 FAST_LIO 来说，Avia、MID-360 都填 1。scan_line：代表“扫描线”数。Avia 通常是 6，MID-360 是非重复扫描模式，实际配scan_line: 4或者按仓库默认值即可。point_filter_num：隔几个点取一个。这个值越大，参与计算的点越少，CPU 占用越低，但精度也会下降。MID-360 默认点频大约是 200k 点/秒，我实测point_filter_num: 6在室内场景比较平衡。
不要照搬网上别人的 yaml 就完事。同一型号雷达在不同驱动版本下点频可能不同，如果运行后发现 CPU 占用过高、frame 掉帧严重，优先把point_filter_num调大，比如从 6 改到 10，再观察配准质量。
3.3 map_path 和 global_map_voxel：地图从哪来、多久抽一次点
FAST_LIO_LOCALIZATION 定位时需要加载一张预先保存的全局点云地图，这里有两个关键配置：
mapping: global_map_voxel: 0.2 map_path: "/home/yourname/maps/global_map.pcd"global_map_voxel是体素降采样的大小。加载全局地图时，算法会把地图体素化，体素越小保留的细节越多，但内存占用和匹配耗时也越高。我建议先设 0.2 试试，如果定位频繁漂移，再降到 0.1；如果 CPU 占用太高，就调到 0.3。
map_path指向 PCD 文件的绝对路径。这个路径经常被忽略，尤其是在 launch 文件里通过$(find ...)方式传参时，容易出现相对路径解析错误。我自己的习惯是直接写绝对路径，简单粗暴不出错。
3.4 雷达-IMU 外参：livox_calibration 还是手动量
FAST_LIO 这类紧耦合算法对雷达和 IMU 之间的外参比较敏感。MID-360 内置 IMU，出厂时官方会提供外参，但实际安装时雷达可能有一定旋转角度，原厂外参不一定完全贴合。Livox 官方提供了livox_calibration工具，可以用来标定雷达和 IMU 之间的旋转外参。
git clone https://github.com/Livox-SDK/Livox_ros_driver2.git # 或者单独拉取 livox_calibration 仓库不过对于 MID-360 这种内置 IMU 的一体化设备，大多数情况下官方外参已经够用。如果你的安装方式是把雷达通过转接板斜装、倒装，就不能依赖出厂值了，必须重新标定。外参错了最典型的表现是：建图时地图分层重影、重定位时当前帧点云和全局地图怎么都对不齐。
4. “能用来重定位的地图”是怎么来的：建图与PCD保存实录
4.1 建图时的动作规范，决定了地图质量
很多人以为 FAST_LIO 建图就是拎着机器人在场地里随便走一圈，按下保存就完事。实际上建图时的手法和路径规划，直接决定这张地图能不能用于重定位。
FAST_LIO 本质上是里程计建图，它会把每一帧点云按当前估计的位姿拼接到地图上。如果移动太快、转弯太急，或者遇到大平面、长走廊这类退化场景，里程计估计会产生漂移，地图就会“飘”。一旦地图飘了，后面重定位做得再好也补不回来。
我在室内办公室场景建图时，遵循这几个原则：
- 移动速度控制在0.3 到 0.5 m/s，不要快走更不要跑。
- 转弯时放慢速度，避免大角度急转。
- 场景里的每一个区域尽量走到，不要只走一条直线就结束。
- 终点尽量落在起点附近，形成一个闭环路径，这样算法能利用回环把累积误差压下来。
4.2 pcd_save_enable 的正确用法
FAST_LIO 保存地图的方式是把pcd_save_enable设为 true，然后在程序退出时自动保存 PCD 文件。
在FAST_LIO/launch/mapping_mid360.launch里找到：
<param name="pcd_save_enable" type="bool" value="true" />确保这一项是 true，然后启动建图。跑完路径后，不要直接关闭终端，而是按Ctrl+C让节点正常退出。FAST_LIO 会在结束时把当前全局地图写入 PCD 文件，保存在 FAST_LIO 包的 PCD 目录下，文件名带时间戳。
这里有个容易踩的坑：如果你在跑图过程中直接拔电源、杀掉进程，或者用kill -9，PCD 文件不会生成，之前跑的时间全白费。必须用正常方式退出节点。
4.3 地图降采样：PCD 文件太大会把重定位卡死
FAST_LIO 保存的原始地图点数量非常大，办公室一个小区域随便就是几千万个点，直接把这么大的 PCD 丢给 FAST_LIO_LOCALIZATION，加载时内存占用会飙升，配准频率也会掉到一帧要好几秒。
所以建完图后，我习惯先用 PCL 工具做一次体素降采样：
pcl_voxel_grid input.pcd output_voxel_0.2.pcd -leaf 0.2 0.2 0.2如果系统里没装 pcl-tools，可以用pcl_voxel_grid对应的 ROS 节点，或者直接在 FAST_LIO_LOCALIZATION 里把global_map_voxel调大。我推荐先离线降采样，再配合global_map_voxel做二次抽稀，这样加载速度和匹配精度都比较可控。
一张几千万点的地图，降到体素 0.2 之后通常剩下几十万到一两百万点，对 ikd-Tree 来说负担小很多。
5. 自定义bag录制技巧：比想象中更容易翻车的环节
5.1 为什么做重定位调试一定要先录 bag
重定位调试是一个非常反复的过程。第一次跑，位姿可能飞；改完参数，又要跑一遍；再改，再跑。如果每次都是推着实机跑，不仅体力消耗大，而且实机状态不稳定，很难控制变量。
我的做法是先把场景数据录成 bag，之后所有参数调试都在同一份 bag 上回放。这样既保证每次调试输入数据完全一致，又能解放双手，坐在电脑前反复跑。
5.2 录制前必须确认的三个东西
录 bag 之前，先确认这几个问题：
第一，点云话题和 IMU 话题的频率是否正常。
rostopic hz /livox/lidar rostopic hz /livox/imuMID-360 的点云频率一般在 10 Hz 左右，IMU 在 200 Hz 左右。如果 IMU 频率只有几十赫兹，说明驱动配置有问题，bag 录下来之后算法表现会很差。
第二，TF 是否完整。
FAST_LIO_LOCALIZATION 虽然主要靠雷达和 IMU 做紧耦合，但 ROS 架构下 RVIZ 显示、地图坐标系发布都需要 TF。录制时把/tf和/tf_static一起录进去，回放时才不会在可视化环节出问题。
第三，要不要录参考里程计。
如果你车上还有轮式里程计或者其他定位源，建议把对应的/odom话题也录进去。后面判断重定位效果时，可以把 FAST_LIO_LOCALIZATION 输出的轨迹和参考轨迹做对比，直观看到漂移是否被修正。
5.3 录制命令与话题过滤
一个比较标准的录制命令长这样：
rosbag record /livox/lidar /livox/imu /tf /tf_static -O office_loop.bagbag 文件不宜过大，录制时尽量只录必要的传感器话题。如果你把 rviz 可视化话题、导航栈的话题全部录进去，文件体积会迅速膨胀，回放时也会拖慢系统。
录制时长我一般控制在3 到 10 分钟。太短覆盖不全，太长文件太大。录制路径和建图路径保持一致，最好在起点和终点处停留几秒，让算法有充分的静止观测，有利于后续初始化。
5.4 回放时的时钟问题：use_sim_time 到底开不开
这是 bag 调试里最容易翻车的地方。
FAST_LIO 和 FAST_LIO_LOCALIZATION 都依赖完整的时间戳来做 IMU 积分和点云配准。如果用rosbag play回放，系统会按 bag 里的时间戳发布消息，算法内部的时间必须和 bag 时间对齐。
回放时我推荐这样做：
rosparam set /use_sim_time true rosbag play --clock --delay 2 office_loop.bag--clock让 bag 发布/clock话题，/use_sim_time true让所有节点使用模拟时钟而不是系统时钟。--delay 2给启动阶段留一点缓冲时间，避免一启动就涌入大量消息。
但这里有个需要注意的地方：如果你是在实机上同时运行多个节点，不是所有节点都支持 sim time。比如某些硬件驱动的 watchdog、看门狗逻辑，如果收到的时间戳长时间不动，会误判为通信故障。所以如果只调试 FAST_LIO_LOCALIZATION，建议把无关节点全部关掉，只跑 bag 回放和定位节点。
如果不开use_sim_time会怎样？最典型的现象是：启动定位节点后，算法一直等不到点云，或者点云和 IMU 时间戳不连续，导致初始化失败、位姿不发散也不收敛，日志里全是时间戳异常警告。
6. 启动顺序与重定位效果判断：怎么看配准是不是真的稳
6.1 建议的启动顺序
无论你用实机还是 bag，启动顺序都有讲究。
先用实机的情况：
# 终端1：启动雷达驱动 roslaunch livox_ros_driver2 rviz MID360.launch # 终端2：启动重定位节点 roslaunch fast_lio_localization localization_mid360.launch如果是 bag 回放：
# 终端1：设置 sim time 并播放 bag rosparam set /use_sim_time true rosbag play --clock --delay 2 office_loop.bag # 终端2：启动重定位节点 roslaunch fast_lio_localization localization_mid360.launch不建议一开始就把 FAST_LIO 建图节点和 FAST_LIO_LOCALIZATION 重定位节点同时启动，因为两个节点会抢雷达数据，而且地图发布关系容易混淆。重定位只需要 FAST_LIO_LOCALIZATION 一个算法进程，不需要同时跑 FAST_LIO 建图进程。
6.2 如何在 RVIZ 里确认重定位状态
FAST_LIO_LOCALIZATION 启动后，默认会在 RVIZ 里显示几个核心话题：
/map：加载的全局地图/cloud_registered：当前帧配准后的点云/Odometry：重定位输出的里程计/path：累积轨迹
最直观的判断标准是：当前帧点云（/cloud_registered）和全局地图（/map）是否贴合。如果重定位成功，当前帧点云应该像拼图一样嵌在全局地图里，边缘轮廓完全对齐。如果错位、重影，说明配准有问题。
6.3 判断重定位效果，不能只看当前帧
有些朋友看到 rviz 里当前帧和地图重合了就以为没问题，其实不够。我建议做两个更严格的测试。
测试一：长时间运行稳定性。
让 bag 循环播放或者让实机持续运行 10 分钟以上，观察/Odometry输出的轨迹是否平滑。重点看每个房间的转角处，轨迹有没有明显的跳变。如果轨迹在地图里来回飘，说明配准在每个局部区域都有可能被带偏。
测试二：回到起点验证闭环。
如果 bag 里的路径是闭环的（起点终点重合），重定位稳定的话，最终输出的终点位姿应该和起点位姿接近。我一般会在起点放一个明显的标志物，通过 rviz 里的射线工具量一下起点和终点的坐标差，偏差在 0.1 米以内算合格，0.05 米以内算优秀。
6.4 初始化失败时怎么用 2D Pose Estimate 手动救场
FAST_LIO_LOCALIZATION 内置的全局匹配（基于 Scan Context 类似思路）在大部分场景能自动给出初始位姿，但也存在完全失效的情况，比如地图本身对称、场景重复度过高、或者雷达刚启动时视角还没覆盖足够特征。
这时可以打开 RVIZ 的2D Pose Estimate工具，在地图上点一下当前帧大致位置，并拉一个朝向。这个操作会给算法一个粗略的初始位姿，之后当前帧会在附近搜索并收敛到正确位置。
我测试时遇到最典型的案例是：在一层楼里有两个结构几乎一样的电梯厅，自动初始化时算法很容易认错，被对称结构骗过去。手动给一个接近真实位置的初始位姿后，配准立刻收敛。
7. 排查实录：重定位漂移、地图错位和点云断裂
7.1 重影和地图错位，先查外参而不是查算法
如果你的当前帧点云和全局地图之间出现整体偏移，尤其是同一个物体在帧里出现“两个轮廓”，这通常不是配准算法的问题，而是外参标定有偏差。
FAST_LIO_LOCALIZATION 内部用当前帧点云和全局地图匹配时，是在已经带有外参变换的点云基础上做的。如果雷达和 IMU 之间的旋转外参偏差大于两三度，点云就会被“扭”一下，导致帧和地图之间永远差一个小角度，怎么调匹配参数都收不回来。
排查步骤：
- 用 livox_calibration 重新标定雷达-IMU 外参。
- 确认 yaml 里的外参值和标定结果一致，注意单位是弧度。
- 重新建图，重新保存 PCD，再加载到重定位节点。
7.2 点云断裂、时间戳跳变，多半是 bag 回放方式不对
bag 回放时如果出现当前帧点云断裂、位置跳变、轨迹分段，最常见的根源是use_sim_time没设置，或者 bag 播放时 CPU 占用过高导致消息延迟。
我之前在一次调试中，所有节点都用系统时间运行，而 bag 里是另一套时间戳，结果算法把两段时间戳混在一起，IMU 积分全部错乱。后来统一改成rosparam set /use_sim_time true再rosbag play --clock，问题立刻消失。
另外，回放 bag 时电脑 CPU 占用可能非常高，尤其是打开 RVIZ 显示大量点云时。如果发现消息延迟明显增大，可以关掉 RVIZ 的全局地图显示，只保留当前帧点云，等算法稳定后再打开。
7.3 配准发散、位姿突然飞走，问题可能出在初始位姿
重定位运行一段时间后突然发散，或者一开始就飞走，常见原因有三个：
- 初始位姿给得太偏，算法在错误区域反复搜索。
- 当前帧点云退化，比如雷达对着白墙或空旷区域，特征不足。
- 全局地图体素太大，细节太少，匹配残差容易陷入局部极小值。
我的排查顺序是：先把global_map_voxel从 0.2 降到 0.1，重新加载地图；然后给一个更准的初始位姿；最后检查当前雷达视角是否在特征丰富的区域。如果三个都做了还飞，才去怀疑算法本身或者地图质量问题。
7.4 一个可以快速验证问题范围的技巧
当定位效果不理想时，先用同一份 bag 在 FAST_LIO 建图节点里跑一遍，看建图轨迹是否平滑。如果建图本身就飘，说明是数据质量或外参问题，重定位再怎么调也没用。如果建图很稳，但 FAST_LIO_LOCALIZATION 重定位飘，说明问题出在地图加载、初始位姿或配置参数上。这个二分法能快速缩小排查范围，节省大量时间。
个人经验里最值得强调的一点是：重定位的地图必须专门为“重定位”准备，而不是随便拿一张建图输出直接用。建图时走慢一点、覆盖全面一点、保存前先确认地图干净，后面做重定位会省掉无数麻烦。还有一个小习惯，我会把每张 PCD 地图的路径、体素大小和对应的建图时间记录在文件名里，方便对比不同参数下重定位的效果差异。你如果也在折腾 FAST_LIO_LOCALIZATION，不妨也试试这个习惯。
