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的安装没什么好说的，但有几个坑要提前避开。分区的时候给根目录至少留50G，ROS的包和编译中间文件很占空间，我一开始只给了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 SDK2：
git 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.1XX（XX是序列号后两位），电脑的网口要设成同网段的静态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-LIO2：
cd ~/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里默认用C++14，但有些依赖需要C++17。如果编译报错，把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_type：Mid360填1，这个不能错，填错了点云格式解析会出问题。
preprocess/blind：盲区距离，默认0.5米。Mid360的近处盲区很小，我设成0.3米，这样近处的点也能用上。但设太小会把机器人自身的点云也框进去，所以要在机器人上盖一块挡板，挡住雷达下方的区域。
preprocess/filter_size_surf：表面降采样尺寸，默认0.2米。室内场景我调到0.15米，这样墙面特征更细，匹配更准。但调太小会增加计算量，Mid360的点云密度本来就高，0.15米已经够用了。
mapping/acc_cov、mapping/gyr_cov：IMU的加速度计和陀螺仪噪声协方差。这两个参数决定了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::PointCloud<pcl::PointXYZ>::Ptr map_cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::io::loadPCDFile("/path/to/map.pcd", *map_cloud); // NDT初始化 pcl::NormalDistributionsTransform<pcl::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设30，NDT收敛慢，迭代次数要给够。setTransformationEpsilon设0.01，这是收敛阈值，设太小会浪费时间。
ICP精匹配在NDT之后做，初值用NDT的结果：
pcl::IterativeClosestPoint<pcl::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_registered，Rviz订阅/laser_map和/Odometry。
话题的队列长度也要调。Mid360的点云数据量大，如果队列太长，会积压导致延迟。我把/livox/lidar的队列长度设成1，这样只处理最新帧，不积压。IMU的队列可以设长一点，因为IMU频率高，丢几帧影响不大。
6.2 计算资源占用与优化
FAST-LIO2和重定位都是计算密集型任务，CPU占用高。我的工控机是Intel i7-10700，8核16线程，跑起来CPU占用在60%左右。优化方法有几个：
第一，点云降采样。Mid360原始点云每帧有2万多个点，降采样到0.15米后只剩几千个点，计算量降了一个数量级。
第二，用多线程。FAST-LIO2本身用了OpenMP，编译时确保-fopenmp开启。重定位节点我也用了多线程，NDT和ICP分开跑，互不阻塞。
第三，降低重定位频率。前面说了，每5帧做一次全局匹配，中间用里程计递推，这样CPU占用降了一半。
第四，用GPU加速。如果工控机有NVIDIA显卡，可以用CUDA版的PCL，NDT和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秒，效果很明显。
