机器人早上开机地图还在里程计却从零开始。轮子一动点云哗啦一下散开导航栈里那个“当前位置”和真实位置差出半条走廊——这就是我在仓库里第一次做FAST_LIO_LOCALIZATION之前遇到的典型场景。FAST_LIO_LOCALIZATION这个项目就是专门解决“建好图之后机器人重新开机不知道自己在哪里”的问题它用Livox雷达的实时点云在预先构建好的全局点云地图里重新找回机器人的位姿让你不用每次都人工把机器人推到原点也不用从头重新建图。这篇文我从环境配置、ROS bag录制、离线建图到重定位的完整流程都盘了一遍中间夹了大量实测踩坑记录。如果你手里有Livox Mid-360或Avia雷达正在被“重定位发散”“bag数据没法用”“开机丢位姿”这类事折磨这篇文章应该能帮你省掉不少弯路。1. 动手之前先搞清楚这套重定位系统在做什么1.1 它和FAST_LIO不是同一个东西很多人第一次接触FAST_LIO_LOCALIZATION时习惯性把它当成“FAST_LIO的另一个分支”用起来才发现完全不是一回事。FAST_LIO本身是紧耦合的LiDAR-Inertial里程计它靠雷达点云和IMU数据做状态估计输出的是相对轨迹。问题在于FAST_LIO构建和维护的是局部地图时间一长或者场景特征变差轨迹会慢慢飘而且一旦程序重启里程计初始状态就丢了。它自己没有能力告诉你“机器人在地图的哪个位置”。FAST_LIO_LOCALIZATION做的事情是用FAST_LIO作前端把当前实时点云与一个预先构建好的全局点云地图做匹配通过配准结果不断修正位姿让机器人始终知道自己在这个全局地图中的绝对位置。你可以这样理解FAST_LIO是“闭着眼往前走靠脚下感觉猜自己走了多远”FAST_LIO_LOCALIZATION则是“手里拿着一张高清地图每走几步就抬头对一下地标”。前者有累积误差后者每一帧都在用地图做全局校正。这个区别也决定了使用方式的差异。跑FAST_LIO建图阶段你不需要任何先验地图跑FAST_LIO_LOCALIZATION定位阶段你必须先有一张全局地图而且这张地图的质量会直接决定后续重定位的成败。1.2 完整数据链路长什么样整套系统的数据流我用一句话就能说完Livox雷达原始点云和IMU数据进FAST_LIO前端前端输出已校正的点云和里程计随后定位模块把当前帧点云放进全局地图里做配准得到一个校正后的全局位姿。拆开来看有四个关键环节。第一数据输入。雷达点云话题通常是/livox/lidarIMU话题通常是/livox/imu。这两个话题是FAST_LIO和FAST_LIO_LOCALIZATION都需要的原始输入。Livox雷达内置IMU所以不需要额外接独立的IMU传感器。第二前端里程计。FAST_LIO完成点云畸变校正、IMU传播、迭代状态更新输出当前帧相对起始时刻的位姿以及经过配准后的点云。这个环节输出频率不固定一般跟随雷达帧率10Hz左右。第三全局地图匹配。定位模块把当前帧点云与预先构建的全局点云地图做最近邻匹配计算当前帧在地图坐标系下的最优位姿。这部分用的是类似point-to-plane的优化思路不是传统的NDT也不是暴力ICP它对Livox非重复扫描造成的点云密度不均匀有比较好的适应性。第四结果输出。定位模块会把匹配后的全局位姿发布出来供导航栈或者其他业务模块使用。正常工作时你在RVIZ里能看到点云和地图贴合得非常紧密轨迹平滑稳定。理解这条链路很重要因为后面所有配置和调试本质上都是在保证这条链路里的每个环节都不掉链子。1.3 适用场景与硬件要求这套方案在几种场景里特别吃香。最典型的是AGV和AMR机器人晚上在仓库里跑完任务回到充电桩第二天重新上电不需要人工把车推到“Home点”开机后在原地稍微转一下系统就能自己找回位姿。其次是园区巡检、割草机器人这类室外半室外场景。Livox雷达在白天强光、晚上无光的环境下都能工作比纯视觉方案稳定得多。也正是因为这点很多人拿它和“ros相机重定位”做对比相机重定位对光照变化很敏感夜班、逆光、阴影里经常掉链子而Livox雷达方案没有这个问题。硬件上最常用的是Livox Mid-360和Avia。Mid-360水平视场角360度垂直70度左右特别适合室内外兼顾的场景机身自带IMU安装方便Avia视场角小一些但点云分辨率更高适合对细节要求更高的环境。算力方面普通x86工控机就行Jetson系列也能跑只是要在参数里加大点云滤波强度。2. 从零配置依赖安装、编译与雷达标定2.1 环境与依赖准备我在Ubuntu 18.04和20.04上都验证过这套流程ROS对应使用Melodic和Noetic。如果你用的是Ubuntu 22.04配合ROS2那需要另外看FAST_LIO_LOCALIZATION的ROS2分支这里只讲最常见的ROS1方案。建议准备一个干净的工作空间不要和一堆其他功能包混在一起因为后面编译时经常出现依赖版本冲突。命令很简单mkdir -p ~/fastlio_ws/src cd ~/fastlio_ws catkin_make编译前需要确保这些依赖已安装PCL、Eigen、Ceres Solver。Ceres是重灾区Ubuntu自带的版本往往不够新建议从源码编译1.14.0以上的版本。我自己遇到过因为Ceres版本老导致编译FAST_LIO时线性代数相关头文件缺失的问题折腾了半个下午后来统一升级到源码编译的Ceres才解决。另外Livox雷达驱动建议用livox_ros_driver2。Mid-360和Avia都支持这个驱动不是老的livox_ros_driver。安装方式可以单独建一个工作空间编译也可以直接放进fastlio_ws的src里一起编译。驱动编译需要依赖Livox SDK2编译前先把子模块拉全cd src git clone https://github.com/Livox-SDK/livox_ros_driver2.git cd livox_ros_driver2 ./build.sh ROS12.2 Livox驱动与雷达识别驱动编译好后第一步不是急着跑算法而是确认雷达数据能不能正常发出来。Livox的驱动有一个user_config.json配置文件里面需要填写雷达的SN码和型号。MID-360的配置项和Avia不太一样你装驱动的时候一定要看清楚文档里对应你雷达型号的配置方式。配置完成后启动驱动roslaunch livox_ros_driver2 msg_MID360.launch然后用rostopic检查数据rostopic hz /livox/lidar rostopic hz /livox/imu正常情况是点云频率在10Hz左右IMU频率在200Hz左右。如果IMU没有数据多半是user_config.json里imu相关的使能开关没打开如果点云频率远低于10Hz先检查雷达的网口连接和SN配置。这一步看起来简单却是很多人第一步就卡住的地方。点云话题名、IMU话题名、时间戳格式任何一个不对后面FAST_LIO和FAST_LIO_LOCALIZATION都会跑不起来或者跑起来全是乱数据。2.3 雷达-IMU外参标定Livox雷达和它内置IMU之间有一个外参代表两个传感器坐标系之间的旋转和平移。驱动会提供一个出厂默认外参但实际安装到机器人上以后由于安装应力、结构件公差、温度变化等原因这个外参可能并不精确。对FAST_LIO这种紧耦合系统来说外参不准带来的问题非常严重表现是静止时点云看起来没问题一旦运动起来点云就开始拖影建图出现双重影甚至直接发散。如果你想认真标定Livox官方提供livox_calibration工具也有的团队用手眼标定或者“先跑FAST_LIO在线估计外参再把收敛值回填”的方法。我的习惯是分两步第一步用出厂外参先跑FAST_LIO把extrinsic_est_en设为true让它在线估计外参。找一个特征丰富的环境走几个“S”弯和“8”字形跑一两分钟然后看日志里外参的收敛值。第二步把这个收敛值和出厂外参做对比。如果差异很小直接沿用出厂值把extrinsic_est_en设为false如果差异较大把收敛值填入配置文件重启FAST_LIO仍然设extrinsic_est_en为false。这样做的原因是在线外参估计在弱特征环境下容易被带偏长期运行时不建议一直开着。外参在FAST_LIO配置里通常体现为旋转矩阵和平移向量两项具体字段名不同版本略有差别但含义一致。你只需要把标定得到的值填进去并且确保FAST_LIO建图和FAST_LIO_LOCALIZATION定位用的是同一份外参。2.4 编译两个工程时的常见坑把FAST_LIO和FAST_LIO_LOCALIZATION放在同一个工作空间里编译大概率会遇到几个坑。首先是包名冲突。FAST_LIO和FAST_LIO_LOCALIZATION内部都包含名为fast_lio的功能包目录如果两个工程同时放到src下catkin会报“package already exists”的错误。解决办法是不要在同一个工作空间同时放两个完整工程我的做法是建图用单独的fastlio_ws定位用单独的localization_ws两者各自完整编译互不干扰。其次是依赖版本。FAST_LIO_LOCALIZATION对PCL和Ceres的依赖要求比较新Ubuntu 18.04自带的系统版本可能不够。建议先编译并运行原版FAST_LIO确认建图流程完全跑通再编译FAST_LIO_LOCALIZATION。这样万一编译失败你能快速定位是环境问题还是代码问题。还有一个坑是launch文件里的node名。如果你以前装过其他版本的FAST_LIOrosmaster里可能有同名的node导致launch启动时报“port already in use”。我习惯在运行定位之前先执行killall -9 rosmaster roscore清掉所有残留节点再干净地启动。3. 录制一份“教科书级”的自定义bag3.1 录bag前先确认话题列表和时间戳FAST_LIO_LOCALIZATION的输入数据本质上和FAST_LIO建图一样所以bag里至少要包含雷达点云和IMU两个话题。我一般录制前会先做一次“预检”rostopic list rostopic hz /livox/lidar rostopic hz /livox/imu rostopic echo /livox/lidar/header/stamp -n1为什么要看时间戳因为FAST_LIO对时间同步非常敏感。如果点云消息里的时间戳是“1970年”的零值或者IMU消息的时间戳和雷达消息的时间戳来自完全不同源的时钟FAST_LIO前端在做IMU传播时就会得到完全不合理的角速度和加速度状态估计直接崩溃。如果你是通过bag回放跑定位回放时建议加上--clock并记得在启动FAST_LIO或FAST_LIO_LOCALIZATION的launch里打开use_sim_time。不加--clockbag里的时间戳和你系统时钟对不上IMU积分会乱成一锅粥。录制命令很简单rosbag record /livox/lidar /livox/imu -O scene_01.bag如果你希望后续能离线分析机器人的运动轨迹也可以把/tf和/tf_static一起录进去但这两个话题不是必须的。3.2 数据采集路线设计的四个原则bag数据质量直接决定建图质量而地图质量直接决定重定位效果。很多人在算法上花大把时间最后发现是当初录数据的时候偷懒了。我总结出四个原则。第一务必回到起点。你录的这段数据应该构成一个闭环哪怕中间绕了一大圈最后一定要回到出发的位置附近。闭环是检验里程计漂移最直观的方式如果回到起点时建图轨迹跟起点没有重合说明这段数据建图质量可能有问题。第二覆盖特征丰富的区域。走廊拐角、货架边缘、柱子、墙面转角这些都是特征点密集的地方。不要长时间在空旷的大厅中央晃也不要总在一面大白墙面前来回走。Livox的匹配需要几何特征纯平面环境会让位姿退化。第三控制运动速度。我录数据时习惯把底盘线速度控制在1m/s以内旋转速度尽量慢。太快的旋转会让点云畸变严重IMU积分误差也会变大。再好的算法也扛不住你拿着雷达甩来甩去。第四录完一段“初始化”。正式采集前让机器人在原地静止5到10秒然后缓慢旋转一下给系统一个良好的初始姿态估计。这个动作能显著降低后面建图发散的概率。3.3 用rosbag工具控制bag体积和完整性Livox点云加高频IMU的数据量不小。Mid-360的原始点云加IMU实测下来一分钟大约100到200MB不等录个十分钟的bag就是把一两个G的存储吃掉了。我习惯用分卷录制rosbag record /livox/lidar /livox/imu --split --size1024 -O scene_01.bag这样每个分卷1GB避免单个bag文件过大导致后续读取和回放卡顿。录制完成后一定要做一次完整性检查rosbag info scene_01.bag看duration、topics、每条消息数量是否合理。如果duration和你实际录制时间差太多说明中途掉过话题或者bag写坏了建议重录。还有一个小细节录制结束的时候不要直接拔电源线或者kill掉终端。先CtrlC让rosbag把缓冲区的数据写盘等它自己退出再断电。我吃过一次亏拔线太急bag损坏花了大半天采集的数据直接报废。4. 离线建图把bag变成全局点云地图4.1 基于bag回放跑FAST_LIO建图有了合格的bag下一步就是用FAST_LIO把它变成全局点云地图。我建图的流程是先启动FAST_LIO的mapping launch文件然后在新终端里回放bagroslaunch fast_lio fastlio_mapping.launchrosbag play scene_01.bag --clock在RVIZ里主要看两个东西/cloud_registered当前帧配准后的点云和轨迹。理想状态是点云在运动过程中平滑地拼接墙体边缘不抖动轨迹没有明显突变。如果看到点云突然跳变或者轨迹来回折返先停下来不要继续录检查一下是不是外参写错了、IMU话题频率不对、或者bag时间戳有问题。不要指望“多跑一会儿它自己就好了”建图发散的问题只会越来越严重。这里要特别注意一个参数filter_size_map。这个值控制地图点云的分辨率或者说体素滤波大小一般是0.5。建图和定位两阶段的filter_size_map要尽量保持一致否则地图密度和定位匹配时采样的尺度不匹配会影响匹配精度。4.2 地图保存与质量检查FAST_LIO建图过程中全局地图会持续发布。等bag回放结束我需要把这张地图保存下来。我这里说一个最笨但通用的方法订阅全局地图话题用pcl_ros把点云落盘rosrun pcl_ros pointcloud_to_pcd topic:/cloud_registered不同版本的FAST_LIO全局地图话题名可能不一样我见过叫/Map的也见过叫/cloud_registered_all的以你的源码发布的话题名为准。跑完bag后话题里就会生成一个pcd文件。保存完之后我强烈建议用CloudCompare打开看一眼。检查三个指标一是墙和地面是否平整。如果墙面有“虚胖”的双重影或者地面像被橡皮泥捏过说明建图过程有漂移或者外参有偏差。二是地图边缘是否锐利。货架立柱、门框这些结构件应该轮廓清晰边缘不模糊。三是点云密度是否覆盖了整个工作区域。如果某个区域完全没点云或者密度特别低说明建图时没有走过那里后续定位在那个区域会很吃力。这一步有问题的话不要急着进入定位回到采集和建图环节把地图修好否则后面所有重定位调试都是在地图上“屎上雕花”。4.3 地图精度如何影响后续重定位有个概念我希望新手朋友能尽早明白重定位不会改善地图它只是在现有地图里找一个最符合当前点云的位姿。地图本身漂移了、歪了、畸变了重定位出来的位姿也会跟着歪而且点云会被强行“按”到错误的地图位置上看起来贴合实际整体飘移。地图精度的上限决定了重定位精度的上限。这也是为什么我在前面花了整整一章强调数据采集和建图质量。你后期在定位参数上纠结来纠结去可能还不如把当初的bag重新录一遍把地图修得更准一点。另外地图不是越密越好。地图点云太密匹配时的计算量上去了实时性会变差太稀疏特征不够匹配容易走偏。我习惯把最终保存的地图做一次降采样让墙面的平均点间距在3到5厘米左右兼顾精度和计算效率。5. 重定位实战配置、启动与调参5.1 localization配置文件的几个关键项FAST_LIO_LOCALIZATION的工程目录里有一个config文件夹里面是针对不同雷达型号的yaml文件比如livox_avia.yaml、livox_mid360.yaml一类的名字。修改之前建议先复制一份原文件改成自己的版本避免把默认配置搞坏。我每次必改四项。第一雷达型号相关参数。lidar_type要选Livox对应的值scan_line和point_filter_num要和建图时保持一致。注意Mid-360和Avia的scan_line并不一样网上很多默认配置是从特定型号拷出来的直接套用不出问题则已出了问题第一个查这里。第二外参。把你在2.3里确定好的雷达-IMU外参填进去确保和建图阶段完全一致。定位阶段extrinsic_est_en建议设为false让它用固定外参减少在线估计的不确定性。第三地图路径。指定之前保存的pcd文件路径。注意路径不要写错很多版本如果地图文件找不到会直接在一个空地图上做匹配然后给你发一堆发散警告。第四初始位姿。有些版本支持在yaml里写一个初始位姿有些版本只能靠RVIZ里的“2D Pose Estimate”设置。前一版适合你知道机器人大致位置的场景后一版更灵活。我习惯用RVIZ手动给。另外filter_size_map、点云滤波相关参数、IMU加速度和角速度噪声参数都和建图时保持一致。不要定位时随手改大改小否则你很难判断效果变化到底来自哪里。5.2 初始位姿给法和失败后的补救FAST_LIO_LOCALIZATION的全局匹配不是全球搜索它需要一个足够好的初始值。如果初始位姿偏差太大点云和地图匹配时会陷入局部最优表现是点云散开、轨迹跳变、位姿乱飘。我给初始位姿的方法是启动定位后先在RVIZ里找到地图然后观察实时点云用“2D Pose Estimate”在地图上标出机器人当前的大致位置和朝向。关键是朝向不要搞反。位置差个两三米问题不大但朝向差了四五十度系统很可能收敛不回来。如果给了初始位姿之后点云依然发散我的处理流程是先撤销错误位姿重新观察机器人周围的特征再给一次更准的初始值。如果反复试了几次都失败检查这几项一是地图文件是否加载成功。在RVIZ里能看到地图点云才能确认地图加载成功。二是当前环境是否发生剧烈变化。如果几年前建的地图今天货架全搬走了那重定位当然不容易成功。三是点云话题是否和建图时一致。可能驱动换过点云帧率或话题名变了。5.3 启动顺序与可视化判断启动定位有一个稳定顺序先启动FAST_LIO_LOCALIZATION的launch再打开RVIZ加载对应配置最后回放bag或启动实时雷达。启动后我在RVIZ里重点盯三个迹象。第一点云是否贴合地图。这是最直观的。如果实时点云和地图点云高度重合说明位姿估计很准如果出现双影、偏移说明还没收敛。第二轨迹是否平滑。观察系统发布的位姿轨迹如果轨迹出现来回抖动或者突然大跳说明匹配不稳定可能被某些错误点带偏了。第三点云是否均匀地“贴”在地图上。有时你能看到点云基本贴合但某个局部区域有明显的“凸起”或“凹陷”这往往是初始位姿的微小偏差没消除干净。可以尝试重新给一次更精确的初始位姿或者让机器人缓慢移动一小段距离让系统自己修正。当这三个迹象都稳定基本可以认定重定位成功了。这时候把定位模块发布出来的全局位姿接到导航栈里机器人就能在已有地图上自然运行。6. 常见问题排查与实战心得6.1 高频问题速查表我在实际调试中遇到过的坑挑一部分整理成表格方便你对照排查。现象可能原因解决方向雷达点云话题无数据驱动未启动或SN配置错误检查livox_ros_driver2的launch和user_config.jsonIMU话题无数据或频率低驱动配置中IMU使能没打开检查驱动配置里的imu相关开关建图时点云拖影、重影外参不准或scan_line错误重新标定外参回填配置核对scan_linebag回放后时间对不上没有启用use_sim_time或没加--clock在launch中设use_sim_time为true回放加--clockFAST_LIO建图轨迹发散数据采集时运动过激或特征太少控制速度增加特征丰富路线重新录bag重定位点云散开初始位姿偏差过大重新用2D Pose Estimate给更准的初始位姿重定位后轨迹缓慢漂移地图本身有畸变或外参有偏差回查建图阶段的地图质量定位结果与实际位置差一个固定距离地图坐标系与机器人的初始定位基准不一致确认地图坐标系原点和机器人工作原点的定义表格里的每一项本质都能往前追溯到我们前面讲的某一环。遇到问题时不要一头扎进参数里反复试先定位是硬件、数据、地图还是算法的问题。6.2 关于FAST_LIO_LOCALIZATION定位精度的一些体会这套方案我实际用下来正常环境下重定位精度能达到厘米级到十厘米级具体取决于地图密度、环境特征丰富度和传感器标定精度。但要注意它不是“每次开机都一样完美”的魔法。环境变化是重定位最大的敌人茶几被挪了位置、货架被移走、墙面新增了一块巨大广告牌都会让匹配质量下降。我的经验是给自己留一个“定位参考区域”。在机器人经常工作的环境中挑出几个特征稳定、不易被改动的区域比如承重柱边、固定货架端头、门框附近这些地方作为开机后的首选位置。每次开机让机器人先走到这些区域附近再启动重定位成功率会大大提升。另外如果你是从“ros相机重定位”的方案转过来的最直观的感受可能是雷达方案不挑光线夜里也能稳定跑但代价是建图阶段的功夫不能省。相机重定位对纹理变化敏感而雷达重定位对几何结构变化敏感两者各有边界不能互相覆盖。6.3 后续扩展多地图切换、GPS融合等方向FAST_LIO_LOCALIZATION单地图定位跑通之后很多人会想把地图扩展到多楼层、多车间。做法不复杂准备多个pcd文件在上层业务逻辑里根据机器人大致位置或任务区域动态切换地图。切换时要注意把定位模块复位重新给一次新地图下的初始位姿。另一个常见的扩展方向是和GPS融合。室外大场景下完全依赖LiDAR地图定位可能不够灵活如果先把GPS给一个粗糙的初始位姿再用FAST_LIO_LOCALIZATION做精细化匹配就能兼顾全局和局部。这个思路在园区无人车项目里很常见等于用“粗定位精定位”两级策略。如果手里有相机又想融合视觉信息也可以做雷达-视觉融合定位但前提是把雷达重定位这条路先走通。它足够稳定以后视觉只是作为一个辅助置信源用来解决特定场景下的退化问题而不是替代雷达。6.4 最后分享一点个人体会我踩过很多次坑之后最大的体会是FAST_LIO_LOCALIZATION调起来其实没什么玄学它的调试时间大头往往不是算法本身而是前面的数据质量和外参精度。外参没标好地图建出来就是歪的后面怎么做重定位都别扭bag录制得潦草全局地图里缺少关键区域的特征等到实际定位时就会在那些区域疯狂掉点。所以如果你现在正在被重定位问题折磨不要急着去改一堆定位参数先回到源头拿着雷达好好录一段干净、完整、有闭环、运动温和的数据把地图建到让你自己满意的程度。地图好了外参准了重定位就是水到渠成的事。反过来地图一塌糊涂你花再多时间调匹配阈值都是白费。这套流程我用了很长时间希望这篇文能帮你把最耗时的坑提前绕开。