简介FAST-LIO 是基于 LiDAR 与 IMU 的高精度 SLAM 算法这份资源是作者针对论文算法与开源代码的个人理解整理适合正在学习多传感器融合、激光里程计或准备复现 SLAM 系统的研究者参考。文档先梳理基础概念李群与李代数、流形、数据格式以及 velodyne VLP-16 LiDAR、ROS、C 在数据流中的作用再拆解 FAST-LIO 框架围绕 LiDAR 数据预处理、IMU 初始化、点云特征点提取、状态估计与 ikd-tree 地图维护展开帮助读者理清前端配准与后端优化的整体流程随后进入源码层结合主程序、IMU_Processing.hpp、laserMapping.cpp 与 esekfom 核心模块解释论文公式到代码实现的对应关系并附带参考文献与附录便于按目录逐节深读。资源包含 1 个 docx 文档约 5.09MB已有 3103 人浏览学习整体结构清晰既能帮助初学者建立 SLAM 数学基础也能为分析具体工程代码提供可循的线索。1. Fast-LIO 是什么一咬牙读完论文和代码后我留下的一句话结论两年前我第一次跑通 Fast-LIO 时脑子里只有一句话原来紧耦合的 LiDAR-Inertial 里程计可以在一台普通笔记本上实时跑而且精度比纯激光配准高出一个量级。Fast-LIO 的核心是把 IMU 的误差状态卡尔曼滤波ESKF和激光点云配准揉进同一个迭代框架用 IMU 做运动补偿用点云残差反复修正状态估计。它解决的是这样一个问题当机器人快速旋转、剧烈加减速时纯激光里程计会因点云畸变和帧间位移过大而丢匹配而 Fast-LIO 用 IMU 预积分撑住帧间运动再用迭代卡尔曼把激光观测压进状态更新两个传感器互为冗余、互相校正。这篇文章适合两类人一类是准备复现 Fast-LIO 做建图定位的从业者另一类是读论文时被 ESKF 和迭代更新公式卡住的初学者。我会顺着这条线把论文里的关键推导、代码里对应的模块、跑通数据集的完整流程以及我踩过的坑一次讲清楚。2. 先立住核心算法线ESKF 与迭代卡尔曼为什么要绑在一起2.1 为什么是 ESKF 而不是普通 EKF误差状态的三个好处Fast-LIO 的状态估计没有用传统的 EKF 直接估计位置、速度、姿态而是估计真实状态与预测状态之间的误差。这个选择不是炫技而是被 IMU 的数值特性逼出来的。IMU 的加速度计和陀螺仪输出频率通常在 200Hz 以上积分一次姿态变化量很小误差积累也慢所以用误差状态做滤波时线性化点始终贴着真实轨迹不会像普通 EKF 那样在状态离真值较远时线性化失效。另一个好处是误差状态的维度可以裁剪位置、速度、姿态、陀螺零偏、加速度计零偏都进了状态向量但旋转部分用微小角度增量表示避免了四元数在加法运算里的约束问题。实际编程时ESKF 的预测步就是把 IMU 的角速度和比力积分到当前状态同时把误差状态的协方差按线性化模型往前传播。代码里最直观的体现是 imuPreintegration 这个模块每一帧 IMU 数据到达后程序会累加预积分量、更新状态和协方差。注意协方差的传播矩阵里包含 IMU 噪声项如果噪声方差设得太小滤波会过度相信 IMU后续激光残差被强行掰回去时会出现跳变设得太大IMU 的补偿作用又会被削弱。这块我一般会花时间反复调因为它是整个系统稳定性的地基。# esKF 预测步的关键结构伪代码对应 fast-lio 的 imuPreintegration def predict(state, imu_data, dt): # 状态: [p, v, R, bg, ba]R 为旋转矩阵 # 1. 陀螺零偏补偿后的角速度 omega imu_data.gyro - state.bg # 2. 加计零偏补偿后转到世界系 acc_w state.R (imu_data.accel - state.ba) - state.g # 3. 更新位置、速度 state.p state.v * dt 0.5 * acc_w * dt * dt state.v acc_w * dt # 4. 更新旋转一阶近似工程上可接受 delta_R exp_so3(omega * dt) state.R state.R delta_R # 5. 协方差传播F * P * F^T Q # F 由 IMU 误差方程确定Q 由陀螺/加计噪声方差确定 return state这段伪代码展示了预测步的信息流位置和速度更新依赖旋转矩阵旋转更新依赖补偿后的角速度而零偏估计来自激光观测的更新步。参数上坐标系约定必须和代码里一致——某个开发者曾经在配置里把加速度计的重力符号写反结果建出来的地图整体倾斜一个固定角度光看数值根本发现不了。另外exp_so3是指数映射代码里通常用罗德里格斯公式展开不需要真正做矩阵指数运算这也是保证实时性的一个细节。2.2 迭代卡尔曼做了什么重复线性化让残差收敛传统 EKF 的更新步只做一次线性化把观测函数在当前状态处展开成雅可比矩阵然后算卡尔曼增益。问题在于激光点云配准的观测模型是强非线性的尤其在走廊、树林这类几何结构重复的场景里一次线性化后的残差可能离真实极小值很远滤波结果直接被带偏。Fast-LIO 的做法是把更新步做成一个迭代过程每次迭代里重新计算点云到地图的残差、重新算雅可比、重新更新状态直到残差的变化小于阈值或者达到迭代次数上限。这么做相当于把 ICP 式配准的迭代逻辑塞进了卡尔曼框架。迭代开始时初始状态来自 IMU 预测残差通常不大迭代收敛很快如果系统受到外部冲击IMU 预测已经偏离第一轮迭代的残差会很大这时需要更多轮迭代才能拉回来。代码里的iter_cfg配置项控制最大迭代次数和收敛阈值我习惯在震动剧烈的载体上把迭代次数从默认值调大两到三倍但代价是 CPU 占用上升需要看实时性是否扛得住。# 迭代卡尔曼更新步简化为数值迭代对应 fast-lio 的 laserMapping 部分 def update_ikf(state, P, points, map, max_iter): for i in range(max_iter): # 1. 将当前状态下的点云转换到地图系 transformed transform_points(points, state.p, state.R) # 2. 计算每个点到局部地图最近面的残差 residuals, jacobians compute_residuals(transformed, map) # 3. 残差均值判断是否收敛 if mean(abs(residuals)) convergence_threshold: break # 4. 用残差和雅可比更新状态本质是加权最小二乘 gain_k compute_kalman_gain(P, jacobians, noise) state state - gain_k residuals P update_covariance(P, gain_k, jacobians) return state, P这里noise是激光观测的噪声矩阵它平衡了激光和 IMU 的权重。噪声取值如果比实际传感器误差小一个数量级点云残差会主导状态更新IMU 预积分的作用被削弱退化场景就容易飘反过来如果调大激光修正力度变弱快速运动时误差会慢慢积累。我想强调一个容易忽略的点迭代卡尔曼里每次迭代都重新计算雅可比计算量比单次 EKF 更新大不少这就是为什么 Fast-LIO 的代码里点云要先降采样降采样参数直接影响实时性和精度之间的取舍没有免费午餐。2.3 前端时间对齐点云畸变补偿为什么依赖 IMU 消息时间戳Fast-LIO 区别于早期 Loam 系方案的一个重要改进是点云畸变补偿完全依赖 IMU 的时间戳对齐。机械式激光雷达扫描一圈时载体可能已经移动了十几厘米这个位移如果不消除每一帧点云都会被“涂抹”。代码中收到一帧完整点云后会按每个点的相对时间戳结合 IMU 预积分得到的位姿增量把点云重新投影到扫描起始时刻的坐标系下。时间戳对齐是工程里最容易翻车的环节。激光雷达的驱动发出点云帧时帧头时间戳和帧尾时间戳之间的 IMU 数据必须完整如果 IMU 驱动丢数据或者话题受到调度延迟运动补偿可能出现跳变。我常用的核对手段是把 IMU 话题的发布频率和点云话题的帧周期拉出来对比确认 IMU 消息时间戳覆盖了每一帧点云的时间范围。另一个坑是不同传感器驱动的时间基准不一致比如 GeForce 串口输出的 IMU 时间是设备上电计时而激光雷达是系统时间两个时间源不对齐时代码里的time_offset参数就派上用场了。3. 从公式到代码把 fast-lio 仓库的数据流与关键模块对上号3.1 代码结构的三条主线预处理、预测、更新读 Fast-LIO 的代码如果没有主线容易被一堆函数绕晕。我一般把仓库里的代码分成三条线scanRegistration负责点云运动补偿和特征提取imuPreintegration负责 ESKF 预测和预积分量管理laserMapping负责迭代卡尔曼更新和全局地图维护。这三条线通过共享的状态变量和消息回调串联起来本质上是一个事件驱动的状态机。// 用结构体描述核心状态量对应的就是论文里的误差状态向量 struct EsKFState { Eigen::Vector3d p; // 位置 Eigen::Vector3d v; // 速度世界系 Eigen::Matrix3d R; // 旋转世界系到机体系 Eigen::Vector3d bg; // 陀螺零偏 Eigen::Vector3d ba; // 加计零偏 Eigen::Matrixdouble, 15, 15 P; // 误差状态协方差15维 };这段结构体声明对应论文中的 15 维误差状态向量位置 3 维、速度 3 维、姿态 3 维、陀螺零偏 3 维、加计零偏 3 维。代码里的P矩阵如果对角线数值异常通常意味着初始化不充分或者噪声参数不合理。我见过某开发者在跑自己的传感器数据时P矩阵发散导致估计位置跳了几百米最后定位到是 IMU 噪声标准差给得比真实值小了两个数量级滤波过于自信把错误预测当成真值一路传。这三个模块的通信关系也值得梳理IMU 数据到达后触发预测点云帧到达后触发更新。两者的频率差很大IMU 通常是 200Hz激光是 10Hz所以预测步执行次数远多于更新步。代码里如果看到 IMU 回调里面调用了多次传播而激光回调里才执行一次迭代更新说明设计是合理的。如果反过来点云回调里跑了很多次预测就要检查是不是 IMU 话题断流了。3.2 launch 文件与配置项一个参数对应一个物理量Fast-LIO 的 ROS 包装把参数集中在 yaml 配置文件和 launch 文件里跑通样例前先把这些参数和物理量对上调试起来会省很多时间。common参数组里最重要的是外部参数extR和extT它们描述 IMU 坐标系与激光雷达坐标系之间的转换关系。这个外部参数如果标定不准点云畸变补偿和残差计算都是错的地图会出现轻微的重影或系统性扭曲。# fast-lio 的典型配置片段参数含义逐一解释 common: lid_topic: /livox/lidar # 激光点云话题名按驱动修改 imu_topic: /livox/imu # IMU 话题名 extrinsic_T: [0.0, 0.0, 0.0] # 雷达在 IMU 坐标系下的平移 extrinsic_R: [1, 0, 0, 0, 1, 0, 0, 0, 1] # 雷达在 IMU 坐标系下的旋转 preprocess: lidar_type: 1 # 1 表示机械式雷达2 表示固态雷达 scan_line: 16 # 雷达线数机械式一般 16/32/64 blind: 0.5 # 距离小于该值的点直接丢弃 max_line: 3 # 特征提取相关非主要调参对象 mapping: max_iteration: 3 # 迭代卡尔曼最大迭代次数 convergence_threshold: 0.1 # 残差变化收敛阈值 laser_point_cov: 0.001 # 激光观测噪声方差lidar_type这一项要特别留意机械式雷达和固态雷达在点云组织方式上不同代码里对应的处理分支完全不一样。某团队在用固态雷达跑数据时没改这个参数程序不停报点云尺寸异常排查了半天才发现类型没有改过来。laser_point_cov是激光噪声方差它的物理含义是“单个匹配点的置信度”设得越小代表越相信点云残差但这个值要和传感器真实噪声水平匹配凭空给一个极小值只会让系统在退化场景里剧烈抖动。3.3 从消息回调到异步处理为什么更新不在点云回调里一条路走到黑看代码时你会发现 Fast-LIO 的激光回调里并不是简单地做完更新就返回而是把计算量较大的迭代卡尔曼更新放到了多线程里。原因很朴素激光回调频率虽然是 10Hz但一帧点云可能有数万个点加上迭代多次的雅可比计算一次回调的耗时可能超过 100ms这会把 ROS 回调线程卡死导致 IMU 消息积压实时性彻底崩溃。代码里常见的做法是异步队列加锁保护共享状态让激光处理线程和 IMU 处理线程互不阻塞。// 多线程处理的关键点锁保护状态避免 IMU 回调读写时激光线程同时修改 void LaserCallback(const PointCloud::ConstPtr msg) { // 前置过滤与降采样 // 将当前状态和点云交给处理线程而不是在回调内直接更新 processing_thread_.post([this, cloud] { std::lock_guardstd::mutex lock(mtx_); ikf.update(cloud); // 迭代卡尔曼更新发生在工作线程 }); }这里mtx_的存在容易引发一个常见问题线程锁竞争导致 IMU 回调等待间接引入时间延迟。如果你看到系统延时增大不是 CPU 不够而是锁粒度过大——整个更新过程都包在锁里200Hz 的 IMU 回调只能排队等后面的预测步。我见过的方案里有人把锁的范围缩小到只保护状态变量的读写瞬间把点云坐标转换这类耗时计算挪到锁外延时立刻降下来这个优化思路值得记住。4. 把 Fast-LIO 跑起来数据集、launch 文件与必调参数4.1 本地编译与工作空间准备最小命令集Fast-LIO 依赖 ROS 环境和一些第三方库编译前先确认环境变量已经加载。我第一次编译时遇到pcl_ros版本不匹配定位半天才发现是依赖库顺序问题。常见做法是先把依赖装齐再创建独立的工作空间编译避免和系统里的其它 ROS 包冲突。# 创建工作空间并拉取代码 mkdir -p ~/catkin_fastlio/src cd ~/catkin_fastlio/src git clone fast-lio 的仓库地址 # 这里替换为实际仓库 cd ~/catkin_fastlio catkin_make -j4 # 4 线程并行编译避免内存不足 source devel/setup.bash编译完成后启动程序前需要确认配置文件里的话题名和传感器驱动一致。最常见的翻车现场是安装了驱动后话题名带前缀比如driver/points而配置文件里写的还是points_raw程序一直收不到数据。我的排查习惯是先rostopic list列出所有话题再对比 yaml 里的配置项宁可多花十秒钟确认不要在运行时抓耳挠腮。4.2 用公开数据包复现播放数据与结果可视化公开的 LiDAR-Inertial 数据集通常带有 ground truth 轨迹跑一遍后可以和真值对比判断代码是否工作正常。启动 Fast-LIO 后用rosbag play播放数据包同时打开 RViz 查看点云地图和轨迹这个过程能看到算法实时运行状态。# 终端 1启动 fast-lio 节点 roslaunch fast_lio mapping.launch # 终端 2播放数据包注意 --clock 选项保证时间同步 rosbag play --clock your_dataset.bag # 终端 3打开 RViz 加载配置 rviz -d src/fast_lio/rviz/fast_lio.rviz如果你播放数据后 RViz 里地图迟迟不出现优先怀疑 TF 关系没有发布。Fast-LIO 一般会发布camera_init到body的 TF如果数据包本身没有提供 TF可以从配置里找到发布 TF 的开关。另一个常见问题是数据包时间戳乱序表现为地图突然错位叠加解决办法是检查数据包录制时是否开了--clock或者把rosbag play --clock改成--clock --hz100来调整消息发布节奏。4.3 必调参数先调噪声再调迭代最后调降采样参数调优的次序往往比参数本身更关键。我一般遵循“先噪声、后迭代、再降采样”的顺序先用保守的噪声方差跑通观察轨迹是否平滑然后把迭代次数加大看精度是否有提升最后再调降采样体素大小在精度和实时性之间找平衡。mapping: max_iteration: 2 # 先保持 2 次跑通后再按需调大 convergence_threshold: 0.05 laser_point_cov: 0.001 # 激光噪声起点给 0.001 量级 preprocess: filter_size_surf: 0.15 # 面点点云降采样体素大小 filter_size_map: 0.15 # 地图体素大小越大细节丢失越多filter_size_map值得单独说一下地图体素大小决定增量地图的分辨率设得太小会保留大量点导致内存暴涨设得太大则残差计算失去精度。公开数据包我通常从 0.2 起步根据 CPU 占用和轨迹精度的实际表现逐步下调。如果发现 CPU 占用率在算法启动瞬间冲到 100%多半是地图体素太小而全局地图不断膨胀这时先调大filter_size_map比优化代码逻辑立竿见影。5. Fast-LIO 实战避坑复现与改代码时最常见的 5 个翻车点5.1 现象地图整体倾斜一个固定角度跑通后的地图在平面上看着正常但墙面与地面明显不垂直倾斜角度用肉眼看就能分辨。最初我以为是激光雷达安装偏了重新标定外部参数后依然如故。原因IMU 加速度计的重力参考方向处理错误。代码里默认加速度计测量的是“比力”当地面静止时读数应该指向天而有些数据集在录制时已经把重力加速度从读数中剔除重复剔除后重力被当成真实加速度积分导致位置和姿态偏离。解决检查加速度计原始读数静止时三个轴的均方根确认重力分量是否已经存在如果存在代码里不再额外补偿把配置里的重力对齐开关关掉。5.2 现象快速旋转时轨迹剧烈抖动甚至发散机器人在原地快速转动时地图轨迹出现锯齿状抖动停顿下来后轨迹又恢复正常。这个问题在室内场景特别容易触发因为墙面结构弱纹理区域较多。原因快速旋转时点云畸变补偿依赖 IMU 时间戳对齐精度而 IMU 消息延迟抖动导致补偿后的点云里有系统性误差加上迭代卡尔曼的收敛阈值在旋转大残差下过早停止迭代。解决先检查 IMU 话题延迟通过rostopic delay查看抖动幅度再把max_iteration从 3 调到 6 或 8同时把convergence_threshold调小一个量级强制算法在旋转场景多迭代几次。如果还不行检查降采样体素是否太大点云过于稀疏时残差约束不足配准容易陷入局部极小值。5.3 现象地图出现重影同一面墙有双层结构跑长走廊数据时墙壁和柱子出现明显的双层轮廓远看像没对齐的照片。这通常不是建图模块的问题反而和里程计跳变有关。原因里程计在某一段发生了几厘米级别的位姿跳变后续地图叠加时旧点云和新点云错开。跳变的根源大概率在退化场景中激光约束不足迭代卡尔曼更新后状态没能被正确修正IMU 零偏估计趁机漂移。解决把闭环或回环检测加入流程但这超纲了最直接的办法是提高特征丰富区域的约束——调小filter_size_surf让面点保留更多细节同时检查面特征提取的max_line参数是否滤掉了过多有效点。5.4 现象运行十分钟后内存持续上涨程序运行的初始阶段很流畅但二十分钟后内存占用接近系统上限界面操作卡顿。这通常不是内存泄漏而是地图管理的机制问题。原因全局地图没有做体素化压缩所有历史帧的点云都被累积保存地图点数量随时间线性增长。Fast-LIO 的增量地图只保留当前帧附近的局部体素但如果配置里地图体素尺寸设得偏小或map_accumulate开关被打开就会退化为全量地图存储。解决检查配置文件里是否有地图累积限幅参数把地图范围限制在当前位置附近的体素集合另外把filter_size_map调大旧地图点会被合并内存增长速度显著下降。5.5 现象程序编译通过但启动后在等待消息处卡住终端显示节点已启动但一直没有任何输出日志停在等待第一条消息。我遇到过好几次每次都以为是传感器驱动没起来最后发现是别的原因。原因最常见的是话题名不匹配配置里监听的话题根本不存在其次是 ROS 网络环境变量没配对节点和驱动不在同一台机器或同一 ROS domain 内。解决先执行rostopic list看实际话题名和 yaml 配置文件逐一比对确认拼写一致后检查环境变量ROS_MASTER_URI用rqt_graph查看节点是否连接。如果话题存在但依然收不到用rostopic hz 话题名看发布频率频率为零说明驱动侧没有输出数据问题不在 Fast-LIO 而在上游驱动。6. 进阶用法把 Fast-LIO 当工具而不是黑匣子6.1 用轨迹评估工具量化精度而不是只看地图地图看起来漂亮不等于定位精度可靠。在长走廊这类视觉上容易“以假乱真”的场景地图可能整体漂移但局部观感依然连贯。我的习惯是保存程序输出的轨迹文件用轨迹评估工具把它和 ground truth 做时间对齐计算绝对轨迹误差和相对位姿误差。# 保存轨迹后用轨迹评估工具计算误差指标 evo_ape kitti trajectory.txt ground_truth.txt -a --plot这段命令把 Fast-LIO 输出的trajectory.txt与数据集提供的真值对齐并计算绝对轨迹误差。注意时间戳对齐很重要因为 ROS 消息时间戳与真值时间戳通常存在固定偏移工具里-a参数会自动对齐。看到误差数值后你才能判断参数调整是否真的有效——有一次我发现某组参数把地图“修”得很干净但 APE 反而变差了说明调参只是掩盖了问题而非消除问题。6.2 吃透一处代码把残差计算改成可视化输出真正理解 Fast-LIO 最快的方式是不要把它当成一个标准建图工具而是当成一个可拆卸的算法框架。我做过一个练习把迭代卡尔曼更新里的点云残差和雅可比矩阵单独抽取出来保存成文本文件然后在 Python 里画出来。当残余误差在迭代过程中不收敛时你能直接看到是哪一轮迭代出了问题——是初始状态太差导致残差一直减小不了还是收敛阈值设得过紧让迭代提前退出。# 把残差变化量记录为文本用于事后分析收敛行为 residual_history [] for i in range(max_iter): residuals compute_residuals(transformed, map) residual_history.append(np.mean(np.abs(residuals))) if residual_history[-1] residual_history[0] * 0.1: break通过这个简单的统计你能判断代码的迭代终止条件是“收敛到了足够小”还是“撞到了迭代上限”。我见过有的场景里残差在第 4 次迭代才突然下降默认的 3 次迭代上限刚好把改善卡掉了这种情况下把参数调大立刻见效。这套调试思路对任何基于卡尔曼的估计系统都通用先看到数值行为再决定改哪个参数。6.3 状态机视角同一套代码在静态场景和动态场景的不同表现最后想分享一个容易被忽略的视角Fast-LIO 本质上是一个状态估计器它的行为强烈依赖于输入激励。载体静止不动时激光点云重合度极高残差很小协方差持续收缩此时如果把载体突然推一把位置估计会出现短暂的滞后因为滤波协方差被压缩得太小对新的观测权重不足。我在长期静止后忽然运动的场景里会先在原地做一次小幅晃动“唤醒”协方差而不是期望系统凭空响应。理解了这个特性很多现场定位跳变的问题就有了解释路径。这套参数和方法论是我自己反复跑数据、反复看残差曲线之后沉淀下来的每次换到新的传感器组合我都会按“先噪声、后迭代、再降采样”的顺序重新调一遍。固定的一组合适参数不存在但一套可靠的调试流程可以复用到每一次实验里。希望帮到你。本文还有配套的精品资源点击获取
看内容编号体系：设计逻辑与实操方法)