企业官网建设流程全解析

1. 项目概述：为什么我力推FAST-LIO的ROS2版本

先抛个结论：做激光雷达建图和定位这一块，FAST-LIO绝对是目前开源方案里性价比最高的选择，没有之一。特别是搭配Livox Mid-360这种非重复扫描的固态激光雷达，FAST-LIO能在算力很有限的小车上跑出非常稳定的实时定位和建图效果。而ROS2版本的FAST-LIO，意味着这套方案不再需要依赖ROS1那种老旧的通信框架，可以直接跑在Ubuntu 22.04 + ROS2 Humble的现代机器人开发栈上，和Nav2、行为树、微控制器生态无缝对接。

这个项目适合谁？三类人必须关注：第一类，被mid360买回来却只会跑官方demo、一换场景就漂移的建图玩家；第二类，实验室或公司要上ROS2但手里只有ROS1时代SLAM代码的机器人工程师；第三类，对紧耦合里程计算法感兴趣、想搞懂卡尔曼滤波到底怎么和点云配准揉在一起的学生。这篇文章我会从算法原理一路拆到部署踩坑，全程用我实际跑通的经历说话，保证你看完能自己动手复现。

坦率讲，FAST-LIO的ROS2版坑不少。网上所谓的“打开即用”基本不存在，从livox_ros_driver2的适配到QoS设置再到代码里几个隐性的时间戳坑，每一步都可能让人卡一整天。所以我这篇不打算只贴几条命令，而是把原理和实操打通了讲，让你知道自己每一步在做什么，出了问题也知道往哪里查。

2. FAST-LIO算法核心原理拆解：紧耦合迭代卡尔曼滤波到底在算什么

2.1 整体框架：IMU做“短期预言家”，激光雷达做“精准修正”

FAST-LIO的全称是Fast LiDAR-Inertial Odometry，思路一句话就能说清：让IMU以高频（通常200Hz~500Hz）做状态预测，再用低频的激光雷达点云（通常10Hz）去修正预测误差。之所以要这么干，是因为IMU短期积分很准但会随时间漂移，激光雷达点云配准很准但频率低且对运动敏感，两者互补堪称完美。

用生活类比解释：IMU像你闭着眼走路时靠内耳前庭感觉“我大概走了几步、往哪偏了”，短时间内感觉挺准，但走久了必然偏。激光雷达则像你每隔一秒睁眼看一下路边的参照物，一睁眼就能发现自己偏了多少。FAST-LIO干的事就是把这两种信息以严格的数学形式融合在一起，而不是简单粗暴地“轮着用”。

状态向量包含位置、速度、姿态四元数、陀螺仪零偏、加速度计零偏，一共18维（也有实现是18维加外参）。IMU读数进来后，通过运动学方程把状态向前传播，同时递推协方差矩阵，这个过程等效于对误差状态的卡尔曼预测步骤。关键设计是，FAST-LIO建立的是误差状态的迭代卡尔曼滤波，也就是IESKF（Iterated Error-State Kalman Filter），它不直接对完整状态做非线性滤波，而是对状态误差做线性化估计，这样既保留了卡尔曼滤波的高效性，又通过迭代消除了线性化误差。

2.2 前端处理：运动去畸变与特征点提取

激光雷达扫描一圈需要时间，比如mid360的10Hz模式一圈100ms。如果雷达在运动，这一圈里的每个点其实对应着不同的雷达位姿，直接把这些点当成同一时刻采集的，点云就会产生“拖影”，这就是运动畸变。FAST-LIO的做法是：利用IMU在100ms内传播出的相对位姿，把每个激光点从它的采集时刻补偿到扫描结束时刻（或起始时刻），从而“矫正”点云。

这一块在代码里体现为点的time字段和IMU预积分结果的配合。livox点云每个点自带OffsetTime，单位纳秒，表示该点相对点云包起始时刻的时间偏移。FAST-LIO拿到一帧点云后，会根据IMU传播的位姿序列，对每个点做插值找到对应时刻的位姿，再把点变换到统一坐标系下。这个细节是很多人忽略的，但如果你的雷达驱动把时间戳设置不对，去畸变会完全失效，表现就是静止时点云锐利、一动起来就糊成一片。

原版FAST-LIO（1.0）还要提取特征点：计算每个点的局部曲率，把曲率大的点划为边缘点，曲率小的划为平面点，然后用边缘点配到地图中的边缘线、平面点配到地图中的平面片。这里有个经验：mid360的点云密度高但噪点多，不要盲目提高特征提取阈值，否则退化环境下（长走廊、空旷场地）特征点数不够，定位很容易飘。这个我后面在调优部分细说。

2.3 后端优化：IESKF迭代更新与残差计算

处理完前端，就进入核心的IESKF更新。流程是这样的：

1. 取当前帧去畸变后的特征点，根据预测位姿把点变换到地图坐标系；
2. 在ikd-tree地图中搜索每个点的最近邻（FAST-LIO 1.0是用局部地图），拟合出直线或平面；
3. 计算点到直线、点到平面的距离残差；
4. 利用这些残差对状态误差进行迭代卡尔曼更新；
5. 更新后的状态再反馈给前端，重新变换点云、重新找对应关系；
6. 迭代数次直到残差收敛，得到这一帧的最优位姿估计。

这个“迭代”是IESKF的灵魂。普通卡尔曼滤波只做一次线性化，如果运动剧烈或初始误差大，线性化点离真实值太远，估计就会发散。IESKF的做法是把更新后的状态当成新的线性化点，重新做一次观测模型线性化和更新，通常迭代1~3次就能收敛。代价是计算量成倍增加，所以FAST-LIO在代码里对迭代次数做了限制，默认是2次，实际跑下来在ARM小车上也扛得住。

残差计算的细节值得展开。点到线的距离用的是叉积模长公式，点到面的距离用的点积。在建图过程中，ikd-tree会随着新点云不断插入而动态更新，不需要维护体素栅格地图，这让地图更新变得非常快。FAST-LIO的“Fast”几个来源就在这里：一是用ikd-tree做增量式地图管理，二是用IESKF避免多次扫描匹配的迭代开销，三是在点云下采样时用体素滤波控制密度。

2.4 与FAST-LIO2的差异：ikd-tree带来的工程简化

如果你搜资料，会看到FAST-LIO2和FAST-LIO并存。2.0版本最大的改动是去掉了特征点提取这一步，直接把原始点云（经过降采样后）拿去配准。它之所以敢这么干，是因为配套实现了一个高效的增量式kd-tree数据结构，叫ikd-tree，支持点的增量插入、动态删除和批量操作。

这意味着2.0不再需要调整特征提取阈值，也不需要针对不同环境（室内室外）重调参数，鲁棒性明显提升。FAST-LIO的ROS2版本基本都是基于2.0代码的，建议你直接新用户用2.0版。2.0的代价是内存占用更高，因为地图点云数量比特征点云多一个数量级，但大部分机器跑起来压力不大。

说到实验对比，我实测在同样的mid360数据包下，FAST-LIO2的建图精度和1.0相比没有明显差距，但环境适应性好很多，尤其是有植被、树叶这类“高曲率但无结构”的户外场景，1.0容易把树叶边缘当边缘特征，2.0用全部点云反而稳。所以后面实战部分我会直接按FAST-LIO2的ROS2版来讲解。

3. ROS2移植的关键改造点：不是把CMakeLists换一换那么简单

3.1 通信层：QoS与DDS选型对点云传输的影响

很多人在ROS2下跑FAST-LIO第一个遇到的诡异问题是：节点起来了，控制台也打印了，但左等右等就是没有地图输出。这种问题八成出在QoS匹配上。

ROS2的通信基于DDS，通信双方必须QoS兼容才能建立连接。激光雷达点云属于大规模周期数据，livox_ros_driver2发布点云时默认的QoS往往是Sensor Data QoS（Best Effort、Depth较小、Durability为Volatile），而FAST-LIO的订阅器如果设成默认的Reliable，两边会直接断开。FAST-LIO官方代码里的nav_msgs::Odometry订阅还好，但PointCloud2的接收要么在launch里设置qos，要么在代码里使用rclcpp::SensorDataQoS()。

还有一个DDS实现的选择问题。ROS2 Humble默认的Fast-DDS在大点云传输上性能一般，如果是在树莓派或Jetson这类设备上跑，建议换CycloneDDS或Iceoryx。CycloneDDS在大包传输时延迟更低，Iceoryx能实现零拷贝共享内存传输。我实测在Jetson Orin上，同样的mid360点云话题，Fast-DDS的CPU占用比CycloneDDS高约15%，建图Delay也更大。配置方法很简单，设置环境变量RMW_IMPLEMENTATION=rmw_cyclonedds_cpp即可，前提是安装了ros-humble-rmw-cyclonedds-cpp。

3.2 节点生命周期与前向声明：ROS2工程的工程化问题

FAST-LIO原始代码是ROS1风格，类内直接维护ros::NodeHandle、ros::Publisher、ros::Subscriber。移植到ROS2后，如果用rclcpp::Node的标准写法，要特别注意Node的生命周期管理。rclcpp::Node必须用std::shared_ptr管理，节点创建后要加入executor才能触发回调，这跟ROS1里ros::spin的全局单线程模型完全不同。

一个常见错误是：在类构造函数里创建了订阅者，但主函数里只写了rclcpp::init和spin，没有把节点指针放进executor。如果订阅回调是单线程executor默认串行执行，点云处理和IMU处理相互阻塞，频率一高就会丢帧。FAST-LIO的IMU回调频率高，点云回调计算量大，强烈建议用MultiThreadedExecutor并配置CallbackGroup，把IMU回调放在一个独立回调组里，点云处理放另一个组。代码里可以用rclcpp::CallbackGroupType::MutuallyExclusive和Reentrant灵活配置，这个环节直接影响建图实时性。

3.3 TF树与坐标变换的ROS2实现

ROS2的TF2 API和ROS1差异很大。ROS1里tf::TransformListener、tf::TransformBroadcaster随处可见，ROS2里全部换成tf2_ros::TransformListener和tf2_ros::TransformBroadcaster，且消息类型从tf2_msgs::msg::TFMessage变成了内置的geometry_msgs/TransformStamped。FAST-LIO的ROS2代码里，如果直接把ROS1的TF代码移植过来，编译会报一堆找不到头文件的错误。

部署时要特别注意：FAST-LIO会广播lidar到odom或map的变换，但在mid360的场景里，还有lidar到imu的外参变换，这个变换要么在代码里显式设置，要么通过launch文件里的static_transform_publisher发布。如果外参不准，建图会立刻体现为旋转时地图拖尾。livox的mid360官方标定文件里有lidar到IMU的外参，一般出厂会标定好，但不同批次可能有细微差异，严谨的做法是用livox_calibration工具重新标定一遍。

3.4 livox_ros_driver2与FAST-LIO的对接

mid360要跑FAST-LIO，必须先用livox_ros_driver2采集点云。这里有个版本匹配问题：livox_ros_driver2支持ROS2 Foxy/Humble，但它的CMakeLists对Fast-DDS的依赖比较强，如果系统里装了多个DDS实现，编译时可能会串台。我踩过一次坑：在已经安装了CycloneDDS的机器上编译livox_ros_driver2，结果它错误地链接到了CycloneDDS的头文件，运行时反复崩溃。解决方法是编译时显式设置CMAKE_PREFIX_PATH，或者干脆先卸载不需要的DDS实现再编译livox_ros_driver2。

驱动配置里几个关键参数需要调：xfer_format决定点云输出格式（建议设成7，即自定义点格式含OffsetTime），publish_freq和pcl_cfg里设置点云话题名，默认是/livox/lidar。FAST-LIO的launch文件里订阅话题名必须和驱动发布的一致，很多新手在这栽跟头——要么是没仔细看launch里pointcloud_topic参数，要么是话题名大小写不对。另外mid360还有IMU数据输出，话题是/livox/imu，注意FAST-LIO配置里imu_topic要填对，否则IMU数据缺失直接闪退。

4. 实战部署：从零跑通Mid-360建图全流程

4.1 环境准备与依赖安装

我推荐的环境组合是Ubuntu 22.04 + ROS2 Humble，这是目前ROS2生态最好用的长期支持版本。安装ROS2 Humble本身在官方文档里有明确步骤，但如果你在国内网络环境，直接用鱼香ROS的一键安装脚本能省不少时间：

wget http://fishros.com/install -O fishros && . fishros

脚本里选“一键安装ROS2 Humble”，它会自动配置软件源并安装基础包。这里提醒一句：鱼香脚本会改系统的源，装完之后建议检查确认，确保编译FAST-LIO需要的python3-colcon-common-extensions等包已经装上。

装完ROS2后，还需要几个额外的依赖包：

sudo apt install ros-humble-pcl-ros ros-humble-pcl-conversions ros-humble-tf2-eigen libeigen3-dev

Eigen建议用系统的3.4版本，如果要装最新Eigen，注意fast_lio代码里可能使用了Eigen 3.3的API，版本太新会出现编译错误。还有livox driver2需要依赖的livox-sdk2，拉源码时用--recursive参数把子模块一并拉下。

4.2 源码拉取与编译流程

FAST-LIO2的ROS2版本仓库在GitHub上的各fork较多，建议直接拉官方FAST-LIO仓库中支持ROS2的分支。具体操作：

mkdir -p ~/fastlio_ws/src && cd ~/fastlio_ws/src git clone https://github.com/hku-mars/FAST-LIO.git git clone https://github.com/Livox-SDK/livox_ros_driver2.git

编译时注意顺序，livox_ros_driver2是依赖包，先编译它，再编FAST-LIO：

cd ~/fastlio_ws colcon build --packages-select livox_ros_driver2 source install/setup.bash colcon build --packages-select fast_lio source install/setup.bash

如果编译FAST-LIO时提示找不到livox_ros_driver2的消息头文件，说明两个包没在同一workspace下source，或者编译顺序不对。更常见的问题是livox_ros_driver2和fast_lio用了不同的消息定义：FAST-LIO的ROS2版要订阅的是livox_ros_driver2/msg/CustomMsg，而不是sensor_msgs/PointCloud2，很多人在launch里忘了把pointcloud_topic参数从默认值改成/livox/lidar。

我建议把驱动发的话题转成sensor_msgs/PointCloud2再给FAST-LIO用，可以少踩很多坑。livox_ros_driver2的配置文件里把xfer_format设成1（即输出PointCloud2格式），然后在FAST-LIO的launch文件里把subscribe_type参数设为1，这样订阅的就是sensor_msgs/PointCloud2类型。

4.3 关键配置文件逐项解析

FAST-LIO的config文件是yaml格式，里面每个参数都值得你花时间理解。我用mid360的默认配置逐项说明：

common: lid_topic: "/livox/lidar" imu_topic: "/livox/imu" time_sync_en: false

lid_topic和imu_topic要和你的驱动话题名一致。time_sync_en是否开启时间同步，如果雷达和IMU不共时基，建议保持false，让FAST-LIO依赖消息头时间戳。

preprocess: lidar_type: 1 scan_line: 16 blind: 0.5

lidar_type设为1表示Livox系列，scan_line在mid360的配置文件里通常是6或16，fast-lio对非重复扫描雷达会自动处理，但scan_line需要和驱动输出的点云行数匹配。blind是盲区半径，0.5米以内的点会被丢弃，可以避免近处杂点干扰。

mapping: acc_cov: 0.1 gyr_cov: 0.1 b_acc_cov: 0.0001 b_gyr_cov: 0.0001 fov_degree: 360 det_range: 50.0 extrinsic_est_en: true

acc_cov和gyr_cov是IMU的噪声方差，如果IMU性能一般或者标定不准确，增大这两个值可以减少IMU信任度。extrinsic_est_en建议打开，让算法在线估计雷达到IMU的外参，但注意它只优化一个初始外参，不是持续在线标定，初始值越准越好。det_range设50米，mid360的测距能力在40米左右，不用设太大反而增加地图内存。

launch文件中还有一个重要参数是 extrinsic_T和extrinsic_R，这是雷达到IMU的手眼标定结果，在config文件对应的yaml里维护。mid360的出厂外参通常存放在雷达的JSON文件中，需要用Livox官方工具读出后填进配置。外参错误是最常见的建图旋转畸变原因，没有之一。

4.4 运行建图与RViz2可视化

一切就绪后，启动流程分三步。第一步启动livox驱动：

source ~/fastlio_ws/install/setup.bash ros2 launch livox_ros_driver2 rviz_HAP.launch

第二步启动FAST-LIO：

source ~/fastlio_ws/install/setup.bash ros2 launch fast_lio mapping.launch.py config_file:=mid360.yaml

第三步用rviz2打开FAST-LIO提供的rviz配置文件，选择global map消息类型为PointCloud2，话题设为/cloud_registered。如果一切正常，会看到点云地图边扫边增长，同时有/ Odometry话题在输出里程计。

一个实测经验：如果你在运行过程中在RViz里不断旋转视角和缩放，地图点云不会消失但会很卡，fast_lio的实际地图输出话题是/cloud_registered，点云密度很高。为了流畅可视化，建议开一个pcl_ros的voxel_grid降采样节点，订阅/cloud_registered再发布降采样后的点云，RViz只订阅降采样结果。这一步能极大改善可视化流畅度，不影响建图质量。

4.5 跑数据包离线建图

没有雷达硬件也可以先玩起来。Livox官方提供了一些开源数据集，mid360的样例数据包也有。跑ros2 bag离线数据的做法是：

ros2 bag play livox_rosbag

注意ros2 bag在播放时会把时间戳恢复成录制时的真实时间，如果你当前的系统时间比录制时间晚，TF和时间同步可能异常。更稳妥的方案是加--clock参数，并让FAST-LIO节点使用ROS2的use_sim_time。在launch文件里加上：

<param name="use_sim_time" value="true"/>

这样所有节点都以bag里的时间为基准，避免时间戳乱跳。

5. 高频问题排查与调优实录

5.1 话题无数据、QoS不匹配

运行后rviz2里看不到/cloud_registered，先按顺序检查：

1. ros2 topic list看是否有/livox/lidar和/cloud_registered；
2. ros2 topic hz /livox/lidar看雷达数据是否持续发布；
3. ros2 topic info /livox/lidar -v看订阅者和发布者的QoS是否匹配。

如果/cloud_registered存在但无更新，一般是FAST-LIO内部处理出错，查看终端输出找报错。如果是雷达话题有数据但FAST-LIO不收，八成是QoS的Reliable和Best Effort不匹配，修改订阅QoS为Best Effort后重启即可。

5.2 建图重影、点云飞行

表现：建图过程中地图边缘出现双重轮廓，或者雷达明明不动地图却在漂移。优先级排查如下。

第一，检查IMU话题频率。livox的IMU是200Hz输出，如果实际只有10Hz，一定是驱动配置问题，FAST-LIO会因缺少IMU数据很快发散。在mid360上要确认imu的发布频率参数没有被人为调低。

第二，检查外参。把配置文件里的extrinsic_T和extrinsic_R打印出来和Livox标定JSON比对，三个平移量误差超过1厘米、旋转量误差超过0.5度，建图就会出明显重影。

第三，检查运动畸变。如果雷达安装的位置震动大或者载体本身剧烈加减速，IMU饱和也会导致建图抖。这时可以适当增大acc_cov，降低IMU的权重，让激光雷达配准占主导。

5.3 里程计漂移严重

在长走廊、隧道这种几何退化环境里，任何LiDAR-Inertial系统都会漂移，FAST-LIO也不例外。缓解措施有几条：

一是确保雷达能“看到”更多结构。mid360的安装角度可以稍微倾斜30度左右，这样天花板和地面同时更多进入视野，比水平安装能获得更好的约束。

二是打开extrinsic_est_en让算法在线估计外参，有时候出厂外参和实际安装有细微偏差，在线估计能帮忙校正一部分。

三是在FAST-LIO输出的里程计后接一个回环检测模块，比如FAST-LIO作者后来提供的FAST-LIO-SLAM或集成ScanContext回环的方案。注意FAST-LIO本身是里程计不是完整SLAM，没有回环优化能力，纯靠它做长时间大范围建图必然漂。

5.4 编译阶段的连环报错

新手最容易在一个环节卡住——colcon build时报找不到catkin包或ament索引错误。这里分享两条保命经验。第一条，如果编译fast_lio时报找不到livox_ros_driver2，先确认你是在一个workspace里同时编译两个包，并且先source了livox的install/setup.bash。第二条，如果编译时报C++标准错误，比如找不到std::filesystem，打开fast_lio的CMakeLists.txt检查是否设置了C++14或C++17，ROS2 Humble默认C++17，一般没问题；但如果系统里有多个GCC版本，建议用update-alternatives切到GCC 11再试。

5.5 实机部署的几个工程化补充

写到这里，再分享几个实机上长期运行的细节。

雷达与IMU的时间同步。mid360本身是激光雷达和IMU紧耦合一体的，时间同步问题不大。但如果你用的是分立的IMU（如MPU6050）和雷达组合，时间同步就是大问题。FAST-LIO对IMU和雷达时间戳的不同步非常敏感，5毫秒的偏差都会让建图出现周期性抖动。建议优先用一体化的lidar-imu方案，或者用硬件同步线把雷达的PPS信号接入IMU板。

计算平台选型。mid360的10Hz点云频率配合FAST-LIO2，在树莓派4B上大概只能跑15~20Hz的处理速度，勉强实时但CPU占用接近临界。Jetson Orin Nano能轻松跑满且有余量做可视化。如果是工业部署，建议x86的工控机，性价比最高；如果有TTL或者强实时性需求，再考虑Jetson或FPGA方案。

相对于ROS1版本，ROS2版FAST-LIO对系统时间的要求更严格。DDS很多传输层基于TCP或共享内存，如果系统时间跳变（比如NTP校准），可能导致消息超时或被丢弃。实机上建议用chrony进行时间同步，并且把NTP服务配置好，避免时间大幅跳变。这个坑我在长时间跑无人车上遇到过，表现为每隔几小时里程计突然跳一下，查了一天才发现是系统时间校时导致。

最后说一句FLV（Fast-Livo Viewer）的坑。FAST-LIO用的点云渲染工具是FLV，在ROS2下需要单独编译Render库，且依赖OpenGL相关开发包。如果你不需要3D实时渲染，完全可以用RViz2代替，省掉FLV的编译时间。RViz2在地图刷新效率上略弱，但足以满足建图和调参需求。

6. 建图质量评估与后续扩展

跑通建图只是开始，怎么判断地图质量好不好才是关键。我习惯用三个指标：回环一致性、点云锐利度、轨迹平滑性。

回环一致性是指走一圈回到原点后，地图上的同一个物体是否重合。把FAST-LIO输出的轨迹画出来，看起点和终点是否闭合，闭合误差小于1%的轨迹长度算及格。点云锐利度是在RViz里观察墙壁和边缘是否有拖影，如果有明显重影，优先怀疑外参和时间同步。轨迹平滑性可以通过plotjuggler工具查看速度曲线，如果速度有高频毛刺，多半是IMU噪声方差设置太小或外参不准。

关于后续扩展，FAST-LIO的里程计输出可以直接接到以下几个方向。

一是接入全局地图后做导航。把FAST-LIO建好的点云地图降采样后作为octomap或grid map，再用Nav2做路径规划，是很多机器人项目的标配链路。在ROS2下，FAST-LIO里程计经robot_localization或直接经EKF融合后，就能为Nav2提供pose信息。

二是FK-LIO或FAST-LIO-SLAM这类方案做回环，弥补FAST-LIO无回环的短板。FAST-LIO-SLAM在FAST-LIO基础上增加了ScanContext全局描述子做回环检测，建图效果在大场景里提升明显，代码也是开源的，推荐进阶使用。如果只是做小场景、短时间建图，纯FAST-LIO完全够用。

三是把FAST-LIO输出作为语义建图的前端，在它的基础上叠加语义分割结果。我之前做过一个项目就是在FAST-LIO的地图上跑3D语义分割，因为里程计精度高，语义点云的时间一致性很好，分割效果比直接用原始扫描拼接好不少。

我个人在实际项目里的体会是：FAST-LIO的ROS2版本真正解决了“从算法到产品”的最后一步，让激光雷达惯性里程计不再是实验室里的论文demo，而是可以直接跑在量产硬件中间件上的基础能力。ROS2带来的分布式通信、动态发现、安全机制这些优势，在机器人产品化的路上是不可逆的大趋势。趁着mid360价格下探和ROS2生态成熟，现在把FAST-LIO吃透，后续做任何移动机器人项目都会顺手很多。

建图这事，摄像头方案怕曝光怕无纹理，纯雷达方案怕退化怕贵，FAST-LIO把雷达和IMU的互补性发挥到了极致。踩过坑、跑通过、上过车，这篇里的每一个字都是我熬夜调出来的经验。如果你在部署过程中遇到了别的问题，看一眼是不是参数名拼错，再看一眼时间戳同步，大概率就有答案了。