像机器人同时处理摄像头、激光雷达和惯性传感器数据来做实时定位这事听起来不难但真正把三者紧耦合在一起、还不丢失精度和速度的业界能拿得出手的方案其实屈指可数。FAST-LIVO2 就是这个方向上非常有代表性的一套开源系统我在移动机器人和无人机平台上前后折腾过它一个多月从源码阅读到数据集复现再到自采数据运行整体体验比大部分多传感器融合框架要顺但坑也不算少。这套系统全称是 Fast, Direct LiDAR-Inertial-Visual Odometry它最大的特点就是三个字直接法。简单说它不提取图像特征点而是直接用图像灰度残差和激光点云残差在一个迭代卡尔曼滤波器里做紧耦合状态估计。这意味着它对弱纹理环境更鲁棒计算量也更可控而且激光雷达和视觉的信息能互相弥补纯雷达会漂的场景、纯视觉会挂的场景它往往能稳住。这篇博文我不打算只做概念导读我会按我的实际落地经验把 FAST-LIVO2 的架构思路、核心模块的代码级细节、从零跑通的完整流程、参数调优心得以及我踩过的坑和排查方法一次性讲清楚。无论你是准备复现它还是想参考它的设计做自己的传感器融合方案这篇文章都能给你省下大量翻源码和查 issue 的时间。1. 整体设计拆解为什么 FAST-LIVO2 能把三种传感器拧成一股绳要理解 FAST-LIVO2先得知道它跟 FAST-LIVO 第一代有什么本质区别。FAST-LIVO 已经实现了 LiDAR-Inertial 和 Visual-Inertial 两条子系统的融合但视觉部分还是传统的特征点法需要提取和匹配图像特征这带来两个问题一是特征点提取在弱纹理、运动模糊场景下非常脆弱二是特征点匹配本身有计算开销在嵌入式平台上很难跑满实时。FAST-LIVO2 把视觉前端整个换掉了改成了稠密直接法配准。什么意思它不再关心图像里哪个角点跟哪个地图点对应而是把当前图像帧的灰度值跟预测出来的顶点投影灰度值直接比较通过最小化灰度差来更新状态。这样做最大的好处是信息利用率高图像里每一个有梯度信息的像素都在参与状态估计而不是只有几十上百个稀疏特征点。从系统架构上看FAST-LIVO2 包含三个核心模块LiDAR-Inertial OdometryLIO用迭代误差状态卡尔曼滤波器IESKF融合激光雷达点云和 IMU 数据维护全局点云地图。Visual-Inertial OdometryVIO同样基于 IESKF 滤波器框架输入图像灰度残差辅助 LIO 修正漂移尤其是在激光退化环境中。体素地图管理激光雷达子系统的核心数据结构用于支撑点云配准和视觉的光度投影配准。这三者之间是紧耦合的不是简单的松耦合拼接。你在代码里会看到 IESKF 的预测步骤由 IMU 驱动更新步骤既包含激光点云的表面残差也包含图像的光度残差权重由协方差矩阵统一调度。也就是说激光帧到了、图像帧到了、IMU 到了所有信息进同一个滤波器做状态更新而不是各跑各的再融合。这种设计带来的直接影响就是系统在退化场景中的表现明显好于单传感器方案。比如无人机飞过一条长直走廊激光雷达几乎只能看到两侧平行墙面前向的平移自由度不确定度很大此时如果视觉还能看到天花板纹理或者走廊尽头的亮窗灰度残差就会把前向漂移拉住。反过来在光照剧烈变化的室外环境视觉可能被过曝或阴影干扰激光点云又提供了稳定的几何约束。这一套互补逻辑在代码里体现得非常直接也是我认为它最值得学习的地方。另外FAST-LIVO2 的建图能力也很强。它的地图不是稀疏特征点集合而是基于 IwMIncremental voxel-wise Mapping机制管理的体素地图每个体素内维护平面参数和残差分布统计。这种地图结构的好处是配准时能快速找到局部平面且地图更新是增量的内存占用可控长时间运行不会像传统图优化那样无限膨胀。需要说明一下这套系统的定位是里程计Odometry不是完整的 SLAM 系统。它没有回环检测和全局优化所以长时间运行会有一定漂移但这在无人机自主导航、机器人底盘控制这类场景里完全够用了。如果需要回环修漂后续可以再接 ScanContext 之类的重定位模块FAST-LIVO2 也专门设计了退化检测机制来配合重定位模块启动。2. 核心细节解析直接法、IESKF 与体素地图的工作原理2.1 直接法视觉从灰度残差到系统更新先细说直接法视觉这部分因为这是 FAST-LIVO2 跟其他多传感器融合方案拉开差距的核心点。传统特征点法会把图像变成特征描述子然后匹配建图整个过程丢掉了图像里大量信息。FAST-LIVO2 的做法是对当前图像帧中的每个有效像素根据当前滤波器的状态预测值把地图中的局部平面反投影到图像平面得到像素位置的预测灰度再与实际观测灰度做差这个差就是光度残差。这里有个很关键的设计它不直接对每个像素做独立残差而是先把 VIO 残差按像素梯度方向搭配激光约束一起注入 IESKF 更新。这样做计算量下降很多因为并不是每帧图像里所有像素都用上而是有选择地使用高梯度像素同时用体素地图平面做几何约束保证光度残差有明确的深度信息支撑。我跑数据集时统计过即使是低端 NUC 也能跑到 10Hz 以上的图像帧率这对视觉惯性系统来说表现相当不错。关于像素点的选择代码里有一套标准只选取灰度梯度幅度大于阈值的像素并且要满足地图点到当前相机光心的距离在合理范围内太远投影噪声大太近视差变化快。这套逻辑在vo_modules相关源文件里能清楚看到核心就是平衡信息量和噪声水平。2.2 IESKF所有传感器数据汇入同一状态估计器IESKF 全称是 Iterated Error-State Kalman Filter它比传统扩展卡尔曼滤波器在强非线性场景下收敛性更好因为是迭代求解相当于在每个时间步做多次局部线性化近似逼近最优估计。FAST-LIVO2 中IESKF 的状态向量包括位置、速度、姿态、陀螺零偏、加速度计零偏、重力向量等基本覆盖了 IMU 惯导解算的全部必要信息。具体到融合过程IMU 负责高频预测100Hz、200Hz 甚至更高频率都能跑激光点云帧和图像帧到的时候触发更新。激光更新用的是点云到地图平面的点到平面残差视觉更新用的是光度残差。两种残差的观测方程不同但都在同一个误差状态模型里表达然后迭加求卡尔曼增益。这种设计的好处是系统对两类传感器的噪声描述是统一的哪个传感器当前更可信滤波器会自动加权不需要人为设置复杂的切换逻辑。实测中当我把相机曝光调低导致图像很暗时视觉残差权重自然下降系统仍然靠激光和 IMU 维持稳定这就是滤波器融合的优雅之处。2.3 体素地图与增量式地图更新策略FAST-LIVO2 能保持配准精度的一个重要支撑是它的地图数据结构。它用空间哈希体素地图存储局部平面特征每个体素大概 0.5m 见方这个参数可配当一个体素里累积的点云足够多时就计算点云的协方差矩阵提取平面法向量与平面中心。在激光帧进来时通过当前位姿预测把地图体素投影到激光坐标系搜索最近的体素平面然后构建点到面的距离残差。由于地图是增量的体素只在需要时创建或更新不会每帧都对全图操作所以即使跑园区级的大场景内存也能控制在合理范围。视觉部分也是一样把体素平面中心重投影到图像平面再取周围像素构造灰度残差。之前看到一些帖子说 FAST-LIVO2 的地图是类似 FAST-LIO2 的 ikd-Tree 结构实际上不完全一样。FAST-LIVO2 对激光帧的当前局部区域建立增量体素地图并用单独的机制维护降低了对内存的需求和配准的搜索耗时。对理解代码来说抓住“增量、局部、体素化”这三个关键词就够了。3. 实操全流程从环境配置到自采数据跑通 FAST-LIVO23.1 环境准备与依赖安装FAST-LIVO2 的主仓库基于 ROS 1推荐环境是 Ubuntu 20.04 ROS Noetic。实测在 18.04 Melodic 下也能编过但会有一些 Eigen 版本相关的编译告警建议直接用 Noetic 省心。依赖项主要是ROS Noeticdesktop-full 版本Eigen 3.3.7 以上PCL 1.10OpenCV 4.x自带的 4.2 即可livox_ros_driverLivox 雷达驱动FAST-LIVO2 对 Livox 支持最好glog、yaml-cpp大多数作为依赖自动装好个人建议用 Docker 跑仓库里提供了Dockerfile一条命令就能把环境拉起来。我自己是在裸机 Ubuntu 上装的踩了不少 OpenCV 版本冲突的坑如果你不想折腾编译链Docker 是更稳的选择。但需要注意用 Docker 跑的时候如果要用自采的 Livox 雷达数据需要把雷达的 USB 设备直接透传给容器否则无法在线实时跑。核心仓库拉取cd ~/catkin_ws/src git clone https://github.com/hku-mars/FAST-LIVO2.git git clone https://github.com/Livox-SDK/livox_ros_driver.git cd ~/catkin_ws catkin_make source devel/setup.bash编译过程中最容易出问题的地方是 PCL 和 OpenCV 的头文件引用冲突常见报错是pcl_conversions里找不到某个头文件。解决办法是把 Eigen、PCL 都装在系统默认路径下然后用catkin_make -DCMAKE_BUILD_TYPERelease编译遇到头文件找不到的问题再逐个include路径检查。3.2 跑通官方数据集复现FAST-LIVO2 官方放了几个数据集包括仓库里自带的hkust数据还有较新的indoor、outdoor场景包。建议先跑hku那个数据集场景复杂度适中跑完可以直观看到点云地图和轨迹输出。数据集的播放命令大体是roslaunch fast_livo2 mapping_hku.launch rosbag play hku.baglaunch文件里需要注意几个参数use_livox设为 true 或 false 取决于数据集的雷达类型point_filter_num控制点云降采样间隔默认是 3也就是每三个点取一个max_iteration是 IESKF 的单帧最大迭代次数官方默认 3 次实测 2 次也能收敛速度更快。启动后打开 RViz订阅/path和/cloud_registered就能看到实时建图效果。我第一次跑通时明显感觉地图收敛速度比 ORB-SLAM3 快而且传感器运动较快时也不太会出现特征丢失的问题这是直接法和紧耦合的功劳。跑数据集时我建议你把record_offline_map打开同时录一份 tf 树方便后面做轨迹误差分析。官方没直接提供 evo 脚本但把/Odometry话题转成 TUM 格式后用 evo 对比 ground truth 完全可行我试过效果很好。3.3 自采数据注意事项与配置调整数据集跑通之后多数人都会想用自己的设备跑。自采数据要特别注意几个点否则容易反复崩溃。第一是外参标定。FAST-LIVO2 需要 LiDAR 到 IMU 的外参、Camera 到 IMU 的外参而且对精度敏感。官方提供了extrinsic参数在 launch 文件里但强烈建议你用lidar_imu_calib或 Kalibr 做一次离线标定不要直接抄别的设备的外参。我实测外参差 1 度可能还能跑差 5 度以上基本几秒内就飘走或炸掉。第二是相机内参和畸变模型。FAST-LIVO2 用 pinhole 模型加equidistant或radtan畸变模型你手里的相机是什么模型就去改对应参数并且要用camera_info话题实时发布否则 VIO 初始化时会直接报错。第三是时间同步。程序里默认认为 LiDAR, IMU, Camera 已经硬件同步好时间戳是同一时钟域。如果不做硬件同步强烈建议在采集时用软件同步至少把三个传感器的话题时间戳统一到同一个 master 时钟上否则滤波器更新时序会乱表现出来的症状就是轨迹跳变或者地图分层。这些准备做完后把 launch 文件里的bag_path换成你的数据包雷达话题名、图像话题名、IMU 话题名都改成你自己的基本就能直接跑。4. 常见问题与排查技巧实录这个部分我挑了几个自己在复现和自采数据时踩过的典型问题每个都是实际发生过并花时间排查过的整理成速查表形式方便你直接对照。4.1 编译问题Eigen 对齐报错与 OpenCV 头文件冲突编译报错最常见的有两类。一类是 Eigen 的 alignment 报错一般出现在pcl::PointCloud与自定义结构体混用时解决办法是在 CMakeLists 里加上-DEIGEN_MAKE_ALIGNED_OPERATOR_NEW或者把涉及 Eigen 类型的容器统一用aligned_allocator。另一类是 OpenCV 3 和 4 混装导致的cv_bridge头文件冲突我建议不要自己编译 cv_bridge直接用 Noetic 自带的同时确保find_package(OpenCV)找到的是 4.x。如果编译时出现一堆undefined reference先检查是否链接了libglog和yaml-cpp这两个库缺失是最常见的原因。4.2 运行启动就崩溃IMU 时间戳跳变或外参异常我在自采数据时遇到过一次启动即崩溃的问题控制台打印了大量NaN在状态向量里。排查到最后发现是 IMU 话题里存在连续两个时间戳完全相同的数据导致滤波器预测步长为 0误差状态协方差更新出现除零。解决办法是在采集脚本里对 IMU 数据做去重和时间戳单调性检查。后来我又在代码里加了一层保护发现时间戳非递增就直接丢弃该帧这样即便数据有问题也不会直接崩掉。外参异常导致崩溃的表现则是启动后状态更新一两秒内位置跳到地图外这种情况优先检查外参文件里旋转矩阵和平移向量是否有明显量纲错误比如平移向量写成米制但标定结果给的是毫米级数值差三个数量级根本跑不了。4.3 定位漂移明显退化场景识别与参数调节策略FAST-LIVO2 有内置的退化检测机制主要基于激光配准的 Hessian 矩阵特征值分析。当检测到退化时系统会降低激光残差的权重更多依赖视觉和 IMU。但如果视觉本身也在弱纹理楼层里漂移还是会出现。我在调试时发现一个有效策略适当增大point_filter_num比如从 3 调到 5减少远距离噪声点对配准的影响同时将max_iteration从 3 提到 4能明显提升穿越退化场景时的稳定性。但代价是单帧计算耗时略微增加在低算力平台上有掉帧风险。还有一种情况是漂移来自雷达外参的 z 轴平移没对齐这种漂移在长时间直线运动中特别明显。官方没有直接的在线外参修正工具但你可以先离线用 FAST-LIO2 跑一段轨迹反向解算出稳定的外参值再填回去实战效果很好。4.4 图像帧率低造成视觉残差稀疏如果你用的相机帧率只有 10Hz 以下视觉约束在快速旋转时会明显不足表现是建图看起来正确但轨迹在猛转时有一段偏出去再拉回来。解决办法有两个方向一是把相机帧率提到 20Hz 以上哪怕降低分辨率也行二是提高图像残差像素的选择数量把max_num_pixels调大默认是 2000 左右。不过后者会直接影响计算耗时建议在性能允许的平台再动。4.5 诡异的灰度异常与自动曝光干扰直接法对图像灰度一致性非常敏感如果相机开了自动曝光或自动白平衡同一场景在不同光照下的灰度响应就会不一致光度残差随之失真。我踩过最明显的坑是室内外切换时地图在地面处出现虚影关掉自动曝光并把曝光时间固定后问题立刻解决。如果你是自采数据务必用固定曝光、固定增益、固定白平衡模式。5. 调参经验与性能优化方向参数调优这块我想重点说两个容易被忽视的方面IMU 噪声参数和配准权重。先看 IMU 噪声参数。在 fast_livo2 的配置 yaml 文件里有一组imu_np、imu_nw之类的噪声密度值它们直接决定 IESKF 里 IMU 预测的置信度。如果你用的 IMU 属于消费级如 BMI088官方默认参数可能偏乐观跑出来的轨迹高频抖动把imu_np调大 3 到 5 倍、imu_nw调大 2 倍左右抖动会被明显压下去。反过来如果用高精度光纤陀螺噪声参数设太大反而会让滤波器过度信任预测而忽略观测更新。配准权重方面laser_weight和visual_weight这两个参数控制激光和视觉残差在更新中的比例。室内场景我习惯把视觉权重调高一些便于修正激光在长走廊里的平移退化室外大场景则回归默认值让激光主导因为视觉在强光下灰度响应不稳定。另外说下性能优化。FAST-LIVO2 对 CPU 多线程优化做得不错默认开了一堆 OpenMP 并行但如果你是在笔记本或无人机板载计算机上跑建议把max_num_pixels、point_filter_num和地图体素分辨率三个参数先调低确保帧率稳定后再逐步上调精度。我在 i7-1165G7 的 NUC 上跑过稳定在 20Hz 左右CPU 占用大约 60%余量还算充足但如果同时跑路径规划、障碍物检测等其他模块资源就有点紧了。6. 对 FAST-LIVO2 后续扩展的个人思考说句实话FAST-LIVO2 发布到现在社区热度一直不低但真正把它用于实际产品级系统的团队不算多。一个原因是它没有回环检测在需要厘米级全局一致性的场景里会露怯另一个原因是它对传感器质量有一定门槛尤其相机必须固定曝光这对某些需要自适应的工业场景不太友好。不过从架构角度看它的直接法理念非常值得借鉴。我最近在自己做的项目里尝试把 FAST-LIVO2 的光度残差模块移植到一个基于因子图的定位框架中效果相当不错说明它的视觉前端模块化做得很好不是只能在 ROS 1 Livox 雷达的封闭环境里跑。如果你正好在考虑要不要读它的源码我的建议是优先读IESKF相关代码这是整个系统的心脏看懂它你就能理解多传感器紧耦合的精髓。其次是LIO模块的体素地图类这是它区别于传统 ikd-Tree 方案的核心设计。至于 VIO 模块调用关系比 LIO 复杂初读可以只看残差计算函数不必逐行啃完整条数据链。最后再分享一个小技巧。如果你想在某个定制传感器组合上快速验证 FAST-LIVO2不需要一开始就自采完整传感器套件的数据可以先用官方数据集降采样播放然后把 IMU 话题替换成你设备的 IMU 话题用 ROS 工具做话题转发看看滤波器在“不同 IMU 官方激光视觉”混搭下的表现。这虽然不能代替完整标定流程但能提前暴露出 IMU 噪声参数是否适配的问题省掉不少标定前的时间。