1. 为什么要盯上FAST-LIVO2它不是又一个LOAM变体先交代一下背景。我去年在一个厂区巡检项目里被定位精度折磨了很久。车上有16线机械式激光雷达、一颗全局快门工业相机和一块消费级IMU。最初用的是经典的LOAM系里程计加视觉特征做松耦合刚跑出来的头两分钟还行一旦到了两边都是铁皮板、白墙或者纹理稀疏的通道点云匹配就开始发散轨迹画出来像一盘散线。后来我把方案换成FAST-LIVO2同样一套传感器情况完全不一样了。FAST-LIVO2的全称是Fast, Direct LiDAR-Inertial-Visual Odometry也就是快速、直接法的激光-惯性-视觉里程计。它和市面上常见方案的差别可以浓缩成四个字直接融合。它不提取视觉特征点不用描述子做匹配而是把激光雷达点云、图像像素灰度、IMU测量值一股脑塞进同一个迭代误差状态卡尔曼滤波器IESKF里做紧耦合状态估计。换句话说它解决的核心问题只有一个在高速运动、退化场景和光照变化下怎么让里程计仍然能实时输出稳定、不漂的位姿。这篇内容不是论文翻译而是我在复现FAST-LIVO2过程中攒下来的实操记录。从传感器选型、标定方法、环境构建到参数调试和踩坑恢复尽量把每一处容易翻车的地方讲清楚。适合正在自己搭多传感器融合定位方案、准备在实车上跑SLAM或者被特征法匹配搞到头大的同学参考。2. FAST-LIVO2核心原理拆解直接法凭什么更稳2.1 从FAST-LIVO到FAST-LIVO2升级在哪FAST-LIVO第一版就已经把激光雷达-惯性-Visual三个传感器做进了同一个滤波器框架里但当时的视觉部分仍然依赖特征点提取和描述子匹配整体管线还是“先提特征、后做关联”的思路。到了FAST-LIVO2作者把视觉部分换成了直接法也就是不需要提取角点、不计算描述子直接拿像素灰度值和激光投到图像上的深度约束参与状态更新。这么做的好处很直观特征提取和匹配是最费时间也最容易受环境干扰的一环。纯色墙皮、大片暗光、动态物体遮挡都很容易让特征点数量骤降。直接法绕开了这个瓶颈只要求场景里存在梯度变化哪怕纹理很弱也能提供足够的几何约束。配合LiDAR测距带来的深度信息像素和地图之间的关联精度比单纯二维图像法高不少。2.2 IESKF框架下传感器怎么“捏”在一起FAST-LIVO2的内核是迭代误差状态卡尔曼滤波器这套东西你可以把它理解成一个“会自我修正的加权融合器”。IMU以较高的频率推送角速度和加速度滤波器用这些测量做状态预测LiDAR和相机则在帧率低的时刻提供观测修正。关键的创新点在观测模型这一层。LiDAR部分的拟合对象不是直接的三维点坐标而是点云局部拟合出来的平面特征。相机部分则对图像做稀疏直接法配准利用像素强度残差和LiDAR量测的深度把视觉信息转化为对状态向量的直接约束。整个过程不产生“中间层”的地图描述子也就减少了误差逐级累积的环节。我个人的理解是这套方案之所以工程上健壮是因为它对误差的定义特别“直白”。任何传感器的测量残差都被统一投影到状态空间里哪个传感器提供的信息更确定卡尔曼增益就会在融合时给予更高权重。这意味着即使某一路传感器短暂失效比如视觉被强光过曝系统也不会瞬间崩掉。2.3 直接法代价与边界条件直接法省掉了特征提取但它对初始化质量非常敏感。滤波器启动时如果位姿估计偏得离谱后续像素投影的初值就会错位迭代优化很容易掉进局部极小而不是收敛到真值。所以跑FAST-LIVO2的时候启动阶段必须给系统一个运动激励比如小幅晃动传感器几秒让滤波器的可观性尽快建立起来。另外直接法假设光度一致性。简单说就是同一个世界点在不同帧画面中的灰度值应该差不多。如果相机自动曝光调得太激进或者场景里光照变化剧烈灰度一致性前提就被破坏了。这点在后面参数配置章节还会详细说怎么缓解。3. 硬件选型与LiDAR-IMU标定90%的定位问题都出在这3.1 激光雷达选型机械式与非重复扫描怎么选FAST-LIVO2的参考实现里官方主要适配的是Livox系列固态雷达比如AVIA、Mid-360这类。这类雷达采用非重复扫描模式随着时间推移点云会逐渐“填满”视场角短时间窗口内点云分布看起来不如机械式均匀但胜在视场角大、盲区小而且点云密度在连续积分后并不吃亏。机械式16线/32线雷达也能跑这套框架吗我的实测结论是能跑但要处理好帧率匹配和点云畸变校正。旋转机械雷达单圈扫完需要100ms甚至更久如果载体本身运动速度快一帧点云内部的畸变就很严重。外参标定和运动畸变补偿没做好的话匹配残差会明显变大。对比维度机械旋转式如16线Velodyne非重复扫描固态如Livox Mid-360点云覆盖每帧固定重复扫描线随时间填充视场短时稀疏畸变风险整帧扫描时间长畸变明显积分窗口可控畸变相对小硬件寿命旋转结构易磨损无机械旋转结构耐久性好与视觉融合线与线间距大深度投影稀疏短时点云可更好辅助像素深度成本相对高近年来降得很快还有一类是近几年业内经常讨论的单站/双站光学架构也就是同轴雷达和旁轴雷达的区别。同轴结构收发光路完全重合近距盲区更小、测量精度更稳定旁轴结构在近距会有明显的视差盲区。如果你在室内做部署优先选同轴架构的型号这样近距离墙面和障碍物也能稳定出点FAST-LIVO2初始化阶段才不会因为初始点云太少而迟迟不收敛。3.2 相机和IMU的取舍视觉部分FAST-LIVO2支持单目相机不做双目也不依赖深度相机这是我非常喜欢的一点。相机选择上我强烈建议用全局快门而不是卷帘快门。因为系统在运动过程中对像素灰度梯度做直接法配准卷帘快门在快速转动时会出现明显的果冻效应同一帧图像不同行对应不同时刻的姿态这会直接污染像素残差。全局快门相机虽然贵一点但省掉很多后期对齐的麻烦。IMU方面没有太多玄学注意两点第一输出频率尽量不低于100Hz最好在200Hz附近给滤波器预测提供足够密集的激励第二量程要匹配实际运动。无人机这类高动态平台选加速度计量程大一些地面机器人则可以选择灵敏度更高的IMU。量程不足导致削顶失真比噪声大更致命。3.3 LiDAR-IMU标定实操流程LiDAR和IMU的外参标定不做好FAST-LIVO2基本跑不出理想轨迹。很多人在厂区跑出S型漂移第一反应是调滤波器参数其实大概率是外参偏了几厘米、旋转角偏了一两度。差之毫厘谬以千里。标准的标定流程分三步数据采集找一个纹理丰富、结构明显的环境避免四周全是一模一样的墙面。采集时把传感器装在一个刚性支架上手握支架或装在车载平台上沿空间的三个轴做充分的旋转和平移运动典型的动作是画“8”字同时故意做一些俯仰偏航运动。时长控制在2-3分钟动作幅度宁愿大一些也不要全程匀速平移。使用开源工具解算常用的有li_calib、li_calib_gpu这类雷达惯性标定工具也有把相机-激光雷达-IMU一起标定的方案比如direct_visual_lidar_calibration。我自己的习惯是先分别标定LiDAR到IMU的外参然后再做一次相机到LiDAR的外参标定最后放到系统里联合优化一遍。标定结果验证把标定出的外参填入FAST-LIVO2的配置文件播放同一段数据看初始化收敛速度和轨迹闭合误差。如果轨迹起点和终点能在运行几圈后基本重合说明外参基本没问题如果偏差明显回头检查标定时是否动作太单一、是否有长时间静止段。提示LiDAR到IMU的外参标定需要非常精准的时间同步。如果雷达驱动时间戳和IMU时间戳不对齐外参解算结果会是错的。建议在标定前先单独验证一遍系统时间同步误差通常要控制在毫秒级。3.4 时间同步被很多人忽略的隐形杀手FAST-LIVO2这类紧耦合方案对时间同步极为敏感。它假设所有传感器测量都对应同一时刻的系统状态。如果相机的时间戳滞后30ms而载体在快速旋转那么计算出的像素投影初值就会偏差数像素直接导致直接法配准迭代不收敛或者收敛到错误解。时间同步这块的通用做法是硬件PPS同步加PTP网络授时。没有硬件同步条件的也需要在软件层面做插值对齐。具体操作上我可以提供一套简易验证手段让传感器对着一个有秒针的大钟或者闪烁的LED灯同时采集雷达和相机数据观察时间戳错位程度。误差大于5ms的建议先解决同步再谈精度。4. 从源码到跑通demo完整环境构建与踩坑记录4.1 为什么我不建议在Windows上死磕源码编译看了一圈最近关于FAST-LIVO2的网络热词很多人卡在Visual Studio、Visual C Redistributable这些关键词上。这说明有不少同学拿到的是一台Windows电脑想直接在Windows上把源码编译起来。我的态度很明确在Windows上直接编译FAST-LIVO2是一个非常痛苦的路径因为它的依赖生态基本生长在Linux环境下包括ROS、PCL、Livox SDK、Ceres Solver这些库在Windows上的维护都不太积极。如果你手里只有Windows机器最优解是装WSL2或者使用Docker容器在虚拟的Ubuntu 20.04环境里完成编译和运行而不是在本机装一堆VC运行库去硬撸源码。WSL2的GPU/IO性能经过多年优化跑数据集绰绰有余。装好WSL之后再装一个VS Code按着“Remote-WSL”插件连进去体验和原生Linux差距不大。网上经常搜索“microsoft visual c redistributable 2015”“visual studio 2022”之类的多半是在安装别的依赖软件时被弹窗卡住。这里给一个判断原则只有当你要编译的原生Windows程序报错缺少VCRUNTIME140.dll、MSVCP140.dll这些动态库时才需要安装对应版本的Visual C Redistributable。如果是为了FAST-LIVO2请直接跳过这些安装直奔Linux环境。4.2 依赖安装和编译步骤我在Ubuntu 20.04 ROS Noetic环境下完整跑通过FAST-LIVO2以下是经过验证的依赖安装顺序# 1. 安装ROS Noetic如果还没装 # 参考ROS官方wiki完成桌面完整版安装确保rosdep init和rosdep update都成功 # 2. 安装基础编译工具 sudo apt-get install cmake build-essential libeigen3-dev # 3. 安装PCL、OpenCV、Ceres等核心库 sudo apt-get install libpcl-dev libopencv-dev libceres-dev libfmt-dev # 4. 安装livox_ros_driverLivox雷达驱动 # 从Livox官方仓库克隆并编译版本需要与雷达固件匹配依赖装好之后创建一个catkin工作空间并编译FAST-LIVO2mkdir -p ~/catkin_ws/src cd ~/catkin_ws/src git clone https://github.com/hku-mars/FAST-LIVO2.git cd ~/catkin_ws catkin_make source devel/setup.bash这里有一个容易踩的坑catkin_make默认可能找不到Ceres和PCL的Lib路径。如果编译报错提示找不到Ceres检查是否安装了libceres-dev或者手动在CMakeLists里指定CERES_DIR路径。如果提示OpenCV版本冲突确认系统里只有一个主版本OpenCV避免Anaconda或其它虚拟环境把路径污染掉。4.3 编译报错汇总与对策我整理了一张表覆盖我实际遇到过以及周围人高频遇到的编译期问题报错信息常见原因解决办法Could not find a package configuration file provided by livox_ros_driver没有先编译Livox驱动或路径未加载先编译livox_ros_driver把devel/setup.bash写入~/.bashrcfatal error: pcl_conversions/pcl_conversions.h: No such filePCL头文件路径未配置安装ros-noetic-pcl-conversions包undefined reference toceres::CostFunctionCeres版本过旧或未正确链接检查ceres版本需不低于1.14或者重新编译cereserror: ‘IAMUData’ was not declared依赖包版本不匹配同步更新所有依赖包到与FAST-LIVO2仓库readme一致的版本CMake Error: The following variables are used in this project, but they are set to NOTFOUND某个依赖库未找到逐个检查OpenCV_DIR、PCL_DIR、CERES_DIR等全局变量编译完成后接下来找一个官方的Rosbag数据集进行验证。官方文档里通常会附带下载链接一般选室内带纹理结构、包含匀速和加减速运动的一段数据。播放数据包rosbag play 你的数据包.bag再打开另一个终端roslaunch fast_livo2 mapping.launch正常情况下RVIZ里会逐渐出现点云地图和相机轨迹。发现没有建图时先确认launch文件里雷达和相机的topic名称是否和bag里发布的一致。最容易出问题的是topic重映射和坐标系名称不一致这两个对不上直接导致系统拿不到数据。4.4 配置参数文件该怎么读FAST-LIVO2的配置集中在launch文件与yaml参数文件里核心参数大概分五类传感器外参雷达到IMU、相机到IMU的相对变换矩阵这是最先要确认的相机内参焦距、主点、畸变模型直接用标定板结果滤波器参数包括初始噪声协方差、过程噪声、测量模型噪声这些对应激光雷达测距精度和相机像素噪声水平直接法视觉参数pyramid层数、灰度梯度阈值、最大迭代次数等LiDAR配准参数平面拟合距离阈值、最大匹配距离、鲁棒核函数系数等。我的调试建议是先跑通默认参数再用自己的数据逐步调。不要一上来就把某个阈值改得很激进。比如平面拟合距离阈值默认值在室内结构环境表现良好改小确实能让匹配更精细但如果点云噪声偏大反而会因为拟合不出平面导致特征缺失。5. 实际场景中常见的跟踪失败与我的排查思路5.1 轨迹发散不能只怪算法先检查这三项轨迹发散也就是常说的“跑飞”是自采数据时最让人崩溃的现象。我每次排查都按固定的三步走。第一步看外参。之前有个案例我把标定板放在另一个车间采集标定数据结果标定时环境太单调解算的外参旋转角偏了0.8度。实际跑线时FAST-LIVO2在直道上看不出问题一到转弯就累积横向误差。重新标定后问题瞬间消失。所以轨迹发散先质疑外参而不是滤波器里的协方差系数。第二步看时间戳。如果LiDAR和IMU的时间戳在驱动层就没有对齐滤波器每次预测和更新之间会带上一段随机的“传输延迟”。你可以把各传感器topic的接收时间差打出来看如果差值抖动超过10ms优先修时间同步。第三步看运动退化。FAST-LIVO2虽然融合了三类传感器但如果在极长的隧道里匀速直线行驶LiDAR平面约束在前进方向依然很弱单目相机灰度梯度在纯色墙面也给不出有效约束。这种场景下位姿估计必然在前进方向漂移。合理做法是配合轮速计做运动约束或者把运行轨迹设计成有更多转向路径尽量避免十几秒的纯直线。5.2 初始化失败启动阶段不要“稳如老狗”有一次我在工厂现场启动FAST-LIVO2结果系统一直卡在初始化过了30秒还没输出位姿。检查半天发现我为了“保护设备”把小车放在原地静止了快一分钟才推走。这类紧耦合系统在静止状态下缺乏运动激励惯性导航的可观测性不足激光视觉匹配残差又缺少视角变化自然无法完成初始化。正确做法是启动后立刻让设备在空间中做几个小幅度的俯仰、横滚和偏航动作尽量让六自由度都被激励起来。动作幅度不需要大但速度要明显持续两三秒即可。初始化成功后再平稳起步行驶。5.3 直接法视觉配准发散光照与曝光是隐形推手直接法对光照变化的容忍度确实不如特征法高。我在室外傍晚光线渐变的环境里跑过一次相机自动曝光导致图像亮度持续变化视觉部分的灰度残差一直在微小振荡虽然没有秒崩但轨迹精度明显下滑。应对方案有两个。一是在相机驱动里把曝光模式从自动改为手动固定曝光时间和ISO分清晨、白天、傍晚设定三组参数运行时手动切换。二是降低视觉残差在整体状态更新里的权重让LiDAR-IMU在光照条件差时承担更多主导。第三个进阶思路是使用带HDR功能的工业相机它可以在大动态范围场景下保持更多可用的灰度梯度信息直接法配准的稳定性会好很多。提示直接法视觉配准的退化表现和特征法不一样。特征法是“特征点不够所以匹配失败”直接法是“梯度还在但灰度一致性被破坏导致残差放大”。判断到底属于哪一种可以把视觉残差的统计值打出来如果残差一直在阈值边缘徘徊并且随图像亮度同步变化优先处理曝光问题。5.4 算力不足导致实时性下降FAST-LIVO2被称为“Fast”不是没有道理但“Fast”的前提是分配给它的算力足够。实际部署时如果CPU占用持续逼近100%会出现帧率抖动进而导致滤波器更新滞后、地图错位。优先检查三块性能瓶颈点云预处理点云去畸变和下采样如果做得很重会占用大量CPU视觉金字塔图像缩放直接法在多分辨率图像上迭代层数越多计算量越大地图更新与协方差传播滤波器维护的地图体素数量增长过快时匹配过程会变慢。我的策略是先把图像金字塔层数调到2层减少高分辨率层的迭代次数再把LiDAR配准的最近邻搜索范围限制在合理的体素范围避免每次匹配都要访问全部地图。把CPU占用从85%降到60%之后系统的整体稳定性明显上来了。5.5 实战中的操作节奏部署FAST-LIVO2到实际项目里建议按以下节奏推进先在官方数据集上跑通并理解各个参数的作用用自己的传感器测试平台做标定和时间同步在环境可控、有闭环路线的场景里采集第一段自测数据验证初始化、跟踪和最终轨迹闭合确认精度满足需求后再拓展到更复杂的环境和更大的运动范围每次只改一个参数记录前后轨迹差异不要多个参数一起调否则出了新问题都不知道是哪一步引起的。我自己在跑第2步和第3步时花的时间最多也是整个系统能否稳定落地的分水岭。标定做扎实、数据采集方式合理后面几乎不用反复折腾参数反过来标定草草了事后面所有调试都会变成徒劳。这个项目后续还可以往两个方向扩展一个是给系统加入回环检测模块把建图精度进一步提升另一个是适配更多型号的雷达和相机组合比如换成更高分辨率的工业相机或者加入Bom板载同步信号。如果已经在FAST-LIVO2上跑通了你自己的传感器组合欢迎多交流一些数据采集和参数调优的心得。
