1. 项目缘起与整体方案设计1.1 为什么选择FAST-LIO2加Mid360这套组合室内场景做SLAM最头疼的从来不是算法本身而是传感器和环境的匹配问题。我这次的任务是给一个室内巡检机器人做定位底盘场地是一栋三层办公楼走廊长、房间多、玻璃幕墙多还有大量重复结构的工位隔断。之前用2D激光雷达跑Gmapping走廊里一进玻璃区域就飘回到起点能差出两米多重定位更是基本靠运气。换3D方案是必然的。选型阶段我对比过几个主流方案LOAM系列太老维护成本高LIO-SAM依赖GPS和九轴IMU室内没有GPS信号直接排除LVI-SAM加了视觉但室内光照变化大视觉反而成了拖累。最后锁定FAST-LIO2核心原因有三个第一它是紧耦合的激光惯性里程计IMU和激光互相校正室内这种激光特征退化长走廊的场景下IMU能顶住短时漂移第二它不需要回环检测就能输出相对干净的里程计建图阶段省心第三它对雷达的兼容性好尤其是固态雷达。雷达选Mid360这是Livox的固态激光雷达水平360度、垂直-7到52度视场最远探测70米80%反射率近处盲区很小。室内场景最怕的就是盲区Mid360的垂直视场覆盖了大部分墙面和地面对走廊和房间的建图很友好。而且它体积小、重量轻装在机器人上不占地方功耗也低。实测下来Mid360在室内10米范围内的点云密度足够支撑FAST-LIO2的特征匹配不会出现点太少导致里程计跳变的情况。系统环境用Ubuntu 20.04加ROS Noetic这是目前最稳的组合。ROS Noetic是ROS1的最后一个长期支持版本生态完整FAST-LIO2的官方仓库对Noetic的支持最好编译基本不会遇到依赖地狱。Ubuntu 20.04的内核版本对USB和网口驱动兼容性好Mid360的驱动跑起来很顺。1.2 整体架构与数据流设计整个系统的数据流是这样的Mid360通过网口输出原始点云和内置IMU数据Livox SDK2驱动把数据打包成ROS话题FAST-LIO2订阅点云话题和IMU话题输出高频里程计和全局点云地图。重定位部分我单独做了一个模块把建图阶段保存的点云地图做降采样和特征提取运行时用当前帧和地图做匹配输出位姿修正。这里有个关键设计决策建图和重定位分离。很多教程喜欢把建图和定位放在一个节点里跑建完图直接切定位模式。但实际用下来建图阶段FAST-LIO2的位姿是递推的没有全局优化长时间跑会有累积误差。我的做法是建图阶段跑完一圈后把点云地图保存下来用离线工具做一次全局优化比如用PCL的ICP做帧间配准生成一张干净的地图。重定位时加载这张地图用NDT或者ICP做匹配这样定位精度比直接切模式高一个档次。重定位的触发策略也做了设计。机器人启动时不知道自己在哪先做全局重定位用当前帧和地图做粗匹配找到大概位置后再切到局部跟踪模式。局部跟踪用里程计递推加定期匹配修正这样既保证了实时性又不会因为每帧都做全局匹配而卡顿。2. 环境搭建与驱动配置实操2.1 Ubuntu 20.04与ROS Noetic安装要点Ubuntu 20.04的安装没什么好说的但有几个坑要提前避开。分区的时候给根目录至少留50GROS的包和编译中间文件很占空间我一开始只给了30G编译FAST-LIO2到一半就满了。另外安装时勾选“安装第三方软件”这样显卡驱动和网卡驱动会一起装好省得后面折腾。ROS Noetic的安装按官方文档走就行但国内网络环境需要换源。我用的是清华的镜像源速度稳定。安装命令如下sudo sh -c echo deb http://mirrors.tuna.tsinghua.edu.cn/ros/ubuntu $(lsb_release -sc) main /etc/apt/sources.list.d/ros-latest.list sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 sudo apt update sudo apt install ros-noetic-desktop-full装完之后记得初始化rosdep这一步很多人会卡住因为rosdep update要访问外网。我的做法是先用手机热点连一下把rosdep update跑完后面就不需要了。如果实在不行可以手动下载rosdep的索引文件放到本地但那样比较麻烦不推荐。环境变量配置在~/.bashrc里加一行source /opt/ros/noetic/setup.bash然后source ~/.bashrc生效。验证安装是否成功开一个终端跑roscore再开一个终端跑rosrun turtlesim turtlesim_node能看到小乌龟就说明ROS没问题。2.2 Mid360驱动安装与网络配置Mid360的驱动安装是第一个大坑。Livox官方提供了Livox SDK2和livox_ros_driver2两个仓库但版本匹配很关键。我用的组合是Livox SDK2 v2.3.0加livox_ros_driver2的master分支这个组合在ROS Noetic下编译最稳。先装Livox SDK2git clone https://github.com/Livox-SDK/Livox-SDK2.git cd Livox-SDK2 mkdir build cd build cmake .. make -j$(nproc) sudo make install然后装livox_ros_driver2。注意这个驱动要放在ROS工作空间的src目录下编译mkdir -p ~/catkin_ws/src cd ~/catkin_ws/src git clone https://github.com/Livox-SDK/livox_ros_driver2.git cd .. catkin_make source devel/setup.bash网络配置是Mid360最容易出问题的地方。Mid360默认IP是192.168.1.1XXXX是序列号后两位电脑的网口要设成同网段的静态IP比如192.168.1.50子网掩码255.255.255.0。我一开始用DHCP结果雷达和电脑不在一个网段死活连不上。改成静态IP后一次成功。验证连接ping 192.168.1.1XX能通就说明网络没问题。然后跑驱动roslaunch livox_ros_driver2 msg_MID360.launch如果能看到/livox/lidar和/livox/imu两个话题在发布说明驱动正常。用rostopic hz /livox/lidar看一下频率Mid360默认10Hz如果只有几Hz可能是网络带宽不够检查网线是不是千兆的。2.3 FAST-LIO2编译与依赖处理FAST-LIO2的仓库地址是https://github.com/hku-mars/FAST_LIO但直接clone下来编译会报错因为它的依赖项版本要求比较严。需要先装几个依赖sudo apt install libeigen3-dev libpcl-dev libyaml-cpp-devEigen版本要3.3以上Ubuntu 20.04自带的3.3.7刚好够用。PCL要1.10以上20.04自带1.10.0没问题。编译FAST-LIO2cd ~/catkin_ws/src git clone https://github.com/hku-mars/FAST_LIO.git cd FAST_LIO git submodule update --init cd ../.. catkin_make这里有个细节FAST-LIO2的CMakeLists.txt里默认用C14但有些依赖需要C17。如果编译报错把set(CMAKE_CXX_STANDARD 14)改成17。另外如果用的是Mid360需要修改config/mid360.yaml里的参数主要是lid_topic和imu_topic要对应上驱动发布的话题名。编译完成后跑一下测试roslaunch fast_lio mapping_mid360.launch如果能看到Rviz里点云在动里程计话题/Odometry有数据说明FAST-LIO2跑起来了。3. 建图流程与参数调优3.1 建图前的场地准备与雷达安装建图质量很大程度上取决于雷达的安装位置。Mid360我装在机器人顶部离地1.2米水平安装没有倾斜。这个高度是有讲究的太低的话地面点云太多墙面特征被稀释太高的话天花板点云占比大对定位帮助不大。1.2米刚好在大多数室内场景的“黄金视场”能同时看到墙面、地面和部分天花板。雷达的朝向也要注意。Mid360的零度方向是线缆出口的反方向安装时让零度方向对准机器人正前方这样建图时点云的坐标系和机器人坐标系一致后面做导航不用再转来转去。场地准备方面建图前把走廊里的移动物体比如推车、纸箱清走这些动态物体会在地图上留下“鬼影”重定位时匹配到这些鬼影会导致位姿跳变。玻璃幕墙区域要特别注意激光在玻璃上大部分会穿透少量反射点云会很稀疏。我的做法是在玻璃上贴一些哑光胶带增加反射点这样建图时玻璃区域不会出现大空洞。3.2 FAST-LIO2关键参数解析与调整FAST-LIO2的配置文件里有几个参数直接决定建图效果我逐个说明。common/lidar_typeMid360填1这个不能错填错了点云格式解析会出问题。preprocess/blind盲区距离默认0.5米。Mid360的近处盲区很小我设成0.3米这样近处的点也能用上。但设太小会把机器人自身的点云也框进去所以要在机器人上盖一块挡板挡住雷达下方的区域。preprocess/filter_size_surf表面降采样尺寸默认0.2米。室内场景我调到0.15米这样墙面特征更细匹配更准。但调太小会增加计算量Mid360的点云密度本来就高0.15米已经够用了。mapping/acc_cov、mapping/gyr_covIMU的加速度计和陀螺仪噪声协方差。这两个参数决定了IMU在优化中的权重。Mid360内置IMU的噪声比较大我实测下来acc_cov设0.1、gyr_cov设0.01比较合适。如果设太小IMU权重过高激光匹配会被带偏设太大IMU不起作用长走廊里会漂。mapping/extrinsic_T和extrinsic_R雷达和IMU的外参。Mid360的IMU和雷达是集成的外参理论上应该是零但实际安装有微小偏差。我的做法是先用默认零外参跑一圈看里程计有没有明显漂移如果有再用lidar_align工具标定一下。实测下来Mid360的外参偏差很小默认值就能用。mapping/max_iteration迭代次数默认3。室内场景我调到5这样匹配更充分但计算量增加不多因为Mid360的点云经过降采样后每帧只有几千个点。3.3 建图实操过程与地图保存建图时机器人走的速度要慢尤其是转弯的时候。FAST-LIO2的里程计是递推的走太快会导致帧间匹配失败点云地图出现重影。我一般控制在0.3米/秒转弯时降到0.1米/秒。建图路线要规划好尽量走“回”字形让雷达从不同角度扫描同一区域。走廊要来回走两遍这样墙面特征更完整。房间要进去转一圈确保角落都扫到。我这次建图跑了三圈第一圈粗扫第二圈补漏第三圈验证一致性。建图过程中用Rviz实时监控点云和里程计。如果发现点云突然“炸开”或者里程计跳变说明匹配失败了要停下来检查。常见原因是走到玻璃区域或者长走廊激光特征退化。这时候可以原地转一圈让雷达重新找到特征再继续走。建图完成后保存地图。FAST-LIO2提供了/laser_map话题用rosbag record录下来然后用PCL的工具转成PCD文件rosrun pcl_ros pointcloud_to_pcd input:/laser_map保存的PCD文件就是全局地图。但这时候的地图还包含累积误差需要做一次离线优化。我用PCL的ICP做帧间配准把地图切成小段每段和相邻段做配准然后全局优化。这一步比较耗时但优化后的地图重定位精度能提升30%以上。4. 重定位模块实现与调试4.1 重定位方案选型与原理重定位的本质是“当前帧和地图的匹配”。方案上有几种选择NDT、ICP、特征匹配。NDT对初值不敏感适合全局重定位ICP精度高但需要好的初值适合局部跟踪。我的方案是两级先NDT粗匹配再ICP精匹配。NDT的原理是把地图划分成网格每个网格用高斯分布表示点云当前帧的点云和这些高斯分布做匹配优化位姿。它的好处是不需要点对点对应对初值容忍度高。ICP是点对点匹配精度高但容易陷入局部最优。重定位的触发条件机器人启动时或者里程计置信度低于阈值时。里程计置信度用FAST-LIO2输出的协方差矩阵判断如果协方差突然变大说明里程计可能漂了触发重定位。4.2 重定位节点代码实现重定位节点我用C写核心逻辑是订阅FAST-LIO2的当前帧点云和里程计加载PCD地图做NDT匹配输出修正后的位姿。关键代码如下// 加载地图 pcl::PointCloudpcl::PointXYZ::Ptr map_cloud(new pcl::PointCloudpcl::PointXYZ); pcl::io::loadPCDFile(/path/to/map.pcd, *map_cloud); // NDT初始化 pcl::NormalDistributionsTransformpcl::PointXYZ, pcl::PointXYZ ndt; ndt.setInputTarget(map_cloud); ndt.setResolution(1.0); // 网格大小1米 ndt.setMaximumIterations(30); ndt.setTransformationEpsilon(0.01); // 当前帧匹配 ndt.setInputSource(current_cloud); ndt.align(*aligned_cloud, initial_guess); Eigen::Matrix4f transform ndt.getFinalTransformation();这里有几个参数要调setResolution设1.0米室内场景这个粒度合适太小会过拟合太大会丢细节。setMaximumIterations设30NDT收敛慢迭代次数要给够。setTransformationEpsilon设0.01这是收敛阈值设太小会浪费时间。ICP精匹配在NDT之后做初值用NDT的结果pcl::IterativeClosestPointpcl::PointXYZ, pcl::PointXYZ icp; icp.setInputTarget(map_cloud); icp.setInputSource(current_cloud); icp.setMaximumIterations(50); icp.setMaxCorrespondenceDistance(0.5); // 对应点最大距离0.5米 icp.align(*aligned_cloud, ndt_transform);ICP的setMaxCorrespondenceDistance很关键设0.5米是因为NDT的结果已经有了一定精度ICP只需要在小范围内微调。设太大容易匹配到错误点设太小可能找不到对应点。4.3 重定位调试与精度验证调试重定位时我遇到的最大问题是“匹配到错误位置”。室内场景有很多重复结构比如相同的工位隔断、相同的门框NDT容易匹配到相似但不正确的位置。解决方法是加一个“位置先验”机器人不会瞬移当前帧的位置和上一帧不会差太远。所以我在匹配时加了一个约束如果NDT的结果和上一帧位姿差超过2米就认为匹配失败用里程计递推的结果。精度验证的方法在场地里选10个已知坐标的点机器人开到这些点看重定位输出的坐标和已知坐标的偏差。我实测下来NDT加ICP的方案在大多数点上的偏差在5厘米以内玻璃幕墙附近偏差在15厘米左右这个精度对室内巡检够用了。重定位的频率也要控制。全局NDT匹配很耗时Mid360一帧点云降采样后还有几千个点NDT匹配一次要50毫秒左右。如果每帧都做10Hz的雷达就跟不上了。我的做法是每5帧做一次全局匹配中间用里程计递推这样既保证了实时性又不会漂太多。5. 常见问题与排查技巧实录5.1 建图阶段典型问题速查问题现象可能原因排查方法解决方案点云地图重影帧间匹配失败看Rviz里点云是否突然跳变降低行走速度转弯时更慢里程计漂移快IMU噪声参数不对检查acc_cov和gyr_cov调大协方差降低IMU权重玻璃区域空洞激光穿透玻璃看点云在玻璃处是否稀疏贴哑光胶带增加反射长走廊里漂移激光特征退化看走廊方向点云是否成线原地转圈重新找特征建图卡顿点云降采样不够看CPU占用率调大filter_size_surf这个表是我踩坑后总结的每一条都对应实际遇到的问题。比如“点云地图重影”我一开始以为是雷达问题后来发现是走太快了FAST-LIO2的帧间匹配跟不上。降到0.3米/秒后就没再出现。5.2 重定位阶段常见故障与解决重定位最怕的是“匹配到错误位置”。我遇到过一次机器人在走廊里NDT匹配到了隔壁走廊因为两条走廊结构几乎一样。解决方法是加“位置先验”约束前面已经说了。还有一个方法是加“方向先验”机器人不会瞬间掉头如果匹配结果的方向和上一帧差超过90度也认为失败。另一个问题是“重定位后位姿跳变”。这是因为NDT和ICP的结果有微小差异切换时位姿会跳一下。我的做法是加一个低通滤波把匹配结果和里程计结果做加权平均权重根据匹配的置信度动态调整。置信度高的时候多用匹配结果置信度低的时候多用里程计。还有一个坑是“地图加载慢”。PCD地图如果太大加载要好几秒机器人启动时会有延迟。我的做法是把地图做降采样保存一个低精度的版本用于重定位高精度版本只在需要时加载。降采样用PCL的VoxelGrid体素大小0.1米地图文件从几百兆降到几十兆加载快很多。5.3 实操心得与避坑建议第一个心得建图时一定要录bag。我一开始觉得建图跑完就行了没录bag结果后来发现地图有问题想重新处理都没数据。录bag的好处是后面调参、优化、重定位都可以用同一份数据反复跑不用重新建图。第二个心得Mid360的IMU数据要检查。Mid360内置IMU但有些批次的IMU零偏比较大。我的做法是先把机器人静置5分钟录一段IMU数据看零偏是否稳定。如果零偏漂移大要在FAST-LIO2里加零偏估计或者换外置IMU。第三个心得重定位的初值很重要。如果机器人启动时位置完全未知NDT的全局匹配可能很慢。我的做法是给一个大概的初值比如“机器人在一楼大厅”这样NDT的搜索范围小很多匹配快且准。第四个心得地图要定期更新。室内场景会变比如家具挪动、隔断调整旧地图重定位会不准。我的做法是每个月重新建一次图或者用增量式建图把新数据融合到旧地图里。第五个心得Rviz的配置要保存。调试时经常要切换显示项每次重新配置很麻烦。我把常用的配置保存成rviz文件启动时直接加载省时间。6. 系统集成与性能优化6.1 多节点协同与话题管理整个系统跑起来有多个节点Mid360驱动、FAST-LIO2、重定位节点、Rviz。节点多了之后话题管理很重要。我用rqt_graph看节点连接关系确保没有多余的话题订阅。比如FAST-LIO2订阅/livox/lidar和/livox/imu重定位节点订阅/Odometry和/cloud_registeredRviz订阅/laser_map和/Odometry。话题的队列长度也要调。Mid360的点云数据量大如果队列太长会积压导致延迟。我把/livox/lidar的队列长度设成1这样只处理最新帧不积压。IMU的队列可以设长一点因为IMU频率高丢几帧影响不大。6.2 计算资源占用与优化FAST-LIO2和重定位都是计算密集型任务CPU占用高。我的工控机是Intel i7-107008核16线程跑起来CPU占用在60%左右。优化方法有几个第一点云降采样。Mid360原始点云每帧有2万多个点降采样到0.15米后只剩几千个点计算量降了一个数量级。第二用多线程。FAST-LIO2本身用了OpenMP编译时确保-fopenmp开启。重定位节点我也用了多线程NDT和ICP分开跑互不阻塞。第三降低重定位频率。前面说了每5帧做一次全局匹配中间用里程计递推这样CPU占用降了一半。第四用GPU加速。如果工控机有NVIDIA显卡可以用CUDA版的PCLNDT和ICP能快好几倍。我这次没用GPU因为工控机没独显但如果有条件强烈建议上GPU。6.3 长时间运行稳定性测试系统集成后我做了72小时连续运行测试。测试方法是让机器人在场地里自动巡逻每圈约10分钟跑72小时。测试结果前24小时没问题重定位精度稳定在5厘米以内24到48小时出现两次重定位失败原因是走廊里有人走动动态物体干扰了匹配48到72小时里程计开始有轻微漂移但重定位能纠正回来。针对动态物体干扰我加了“动态点滤除”模块用半径滤波把孤立的点云去掉这些点通常是行人或移动物体。加了之后重定位失败率从每天2次降到每天0.5次。针对长时间漂移我加了“定期全局重定位”每10分钟强制做一次全局匹配不管里程计置信度如何。这样即使里程计漂了也能被拉回来。7. 后续扩展与个人体会这套系统目前跑在巡检机器人上效果稳定。后续还可以做几个扩展一是加视觉融合用相机做辅助重定位在激光特征退化的区域比如长走廊用视觉顶一下二是加语义信息把地图里的物体标注出来重定位时用语义特征匹配精度更高三是做多机协同建图多台机器人同时建图然后融合成一张大图。我个人在实际操作中的体会是SLAM这件事算法只占三成七成在调试和工程化。FAST-LIO2的论文写得再好实际跑起来还是会遇到各种问题比如雷达和IMU的时间同步、外参标定、动态物体干扰。这些问题没有标准答案只能靠一次次试。我建图跑了十几遍重定位调了上百次参数才达到现在的效果。最后分享一个小技巧建图时在场地里放几个“标志物”比如不同形状的纸箱这些标志物在地图里有独特的几何特征重定位时能快速区分相似区域。我放了五个纸箱重定位的首次匹配时间从3秒降到1秒效果很明显。