TRO 2025 | 港大揭秘：误差状态卡尔曼滤波 + 稀疏直接法，多模态传感器融合 SLAM 实现毫米级精度
点击下方“大模型与具身智能”，关注我们
在机器人导航、三维重建、航空测绘等领域，同时定位与地图构建（SLAM）技术始终是核心支撑。单一传感器的SLAM系统早已无法满足复杂场景需求——视觉SLAM怕光照、缺深度，激光雷达SLAM少纹理、易受几何约束限制。而融合激光雷达、IMU、相机的多传感器SLAM，又面临着传感器数据维度不匹配、图像对齐精度低、极端场景鲁棒性差等难题。
近期，一篇名为《FAST-LIVO2: Fast, Direct LiDAR–Inertial–Visual Odometry》的论文给出了全新解决方案！该研究提出的FAST-LIVO2框架，在精度、鲁棒性和效率上全面超越现有主流SLAM系统，还落地了无人机自主导航、航空测绘、三维模型渲染三大实用场景。今天，我们就来深度拆解这款“六边形战士”级别的SLAM方案。
一、核心痛点：多传感器SLAM的四大卡脖子问题
在激光雷达-惯性-视觉（LIVO）融合SLAM领域，长期存在四大核心挑战：
数据处理效率低：激光雷达每秒数千个点云、高帧率高分辨率图像，机载算力有限却要处理海量数据；
特征提取依赖强：传统系统需分别提取激光雷达和视觉特征，无纹理/无结构环境下特征点稀缺；
统一地图难构建：激光雷达和相机的异构数据特性，让统一管理点云与图像测量的地图设计难度拉满；
像素级精度难实现：硬件同步、外参标定、光照适应、融合策略等，任一环节出问题都会丢失像素级位姿精度。
而前代方案FAST-LIVO还存在额外短板：异步更新导致稳定性差、图像块深度假设粗糙、参考块选择不合理、无光照适应机制、激光雷达近距盲区无应对策略。FAST-LIVO2正是针对这些痛点，实现了全方位升级。
二、整体架构：一张图看懂FAST-LIVO2的核心逻辑
想要理解FAST-LIVO2的创新，首先得掌握它的整体架构。如下图所示，这是FAST-LIVO2的系统总体结构图，整个框架分为四大核心模块：误差状态迭代卡尔曼滤波器（ESIKF）、局部地图构建、激光雷达测量模型、视觉测量模型，各模块环环相扣，实现多传感器数据的高效融合。
异步采样的激光雷达点先通过扫描重组（如下图），在相机采样时刻整合为完整扫描帧，保证相机与激光雷达数据同频同步。随后基于ESIKF框架，先融合激光雷达测量更新系统状态，再融合视觉测量完成二次更新，全程依托统一的体素地图采用“直接方法”——无需提取特征，直接用原始数据做融合，大幅提升效率。
三、五大核心创新：精准破解SLAM行业痛点
FAST-LIVO2的核心优势，来自于五个针对性的技术突破，每一个都直击传统方案的软肋：
1. 顺序更新ESIKF：解决传感器数据维度不匹配难题
传统ESIKF会一次性融合激光雷达和视觉测量，但两种传感器数据维度不同、更新逻辑有差异，容易导致稳定性问题。FAST-LIVO2提出顺序更新策略：先把激光雷达测量与IMU传播的先验信息融合，得到收敛后的状态和协方差；再用这个结果融合视觉测量。
这种方式在理论上与一次性更新等价，但大幅提升了灵活性和鲁棒性——激光雷达更新专注解决几何定位，视觉更新聚焦像素级精度优化，分工明确，彻底解决了异步更新带来的稳定性问题。
2. 激光雷达平面先验：让图像对齐精度“更上一层楼”
前代方案假设图像块内所有像素深度相同，这个粗糙假设直接拉低了图像对齐的精度。FAST-LIVO2则充分利用激光雷达点云的优势：从激光雷达点中提取局部平面法向量作为先验，指导图像块的仿射变换。
不仅如此，系统还会在后台线程中对这些法向量做进一步优化——通过最小化参考图像块与其他图像块的光度误差，迭代优化平面法向量，让图像对齐的仿射变换更精准，彻底摆脱“深度一致”的不合理假设。
3. 动态参考块更新：选对参考，精度翻倍
参考图像块的选择直接决定图像对齐的效果。传统方案只看“与当前视角的接近程度”选参考块，选出来的块往往约束力弱、精度低。FAST-LIVO2设计了基于评分机制的动态更新策略：
评分同时考虑两个关键维度：一是归一化互相关（NCC）衡量的光度相似性（优先选与多数视角图像块相似的块），二是视角方向（优先选垂直于所在平面的视角）。最终选出的参考块，既有充足的纹理细节，又有大视差，让图像对齐的约束更强、精度更高。
4. 按需体素光线投射：搞定激光雷达“近距盲区”
激光雷达在近距离会出现无返回点的“盲区”，且相机视场往往无法被激光雷达完全覆盖，这会导致视觉地图点丢失。FAST-LIVO2提出按需体素光线投射（如下图）：
先把图像划分为30×30像素的网格，对没有激光雷达点覆盖的网格单元，从中心像素反向投射光线，在深度范围内均匀采样；只要采样点命中体素地图中的有效点，就将其纳入视觉子图。这一操作能召回更多视觉地图点，让系统在激光雷达盲区也能稳定工作。
5. 实时曝光估计：对抗光照剧烈变化
光照变化是视觉SLAM的“老敌人”，前代方案完全没解决这个问题，导致光照突变时图像对齐收敛性极差。FAST-LIVO2新增实时图像曝光时间估计模块，能动态适配环境光照变化，确保视觉模块在强光、逆光、明暗交替等场景下，依然保持稳定的收敛性和精度。
四、更实用的细节：让框架落地更简单
除了核心创新，FAST-LIVO2在工程实现上也做了大量优化，让系统更易落地：
1. 高效的局部体素地图
系统采用自适应体素结构构建地图：哈希表管理根体素（0.5×0.5×0.5m），每个根体素下嵌套八叉树，可细分出不同尺寸的叶子体素。这种结构既能适配不同尺度的局部平面，又能通过“环形缓冲区”控制地图大小——只保留当前位置周围的局部地图，超出范围的区域自动释放内存，保证机载运行的内存稳定性（如下图）。
2. 更精准的激光雷达测量模型
为了让激光雷达点云的利用更精准，FAST-LIVO2还考虑了激光光束发散角的影响（如下图）：
光束发散会导致测距不确定性随方位角增大而增加，系统通过建模这一误差，让激光雷达点的位姿估计更精准，尤其在地面、墙面等场景下效果显著。
五、实测表现：精度、鲁棒性、效率全面领先
作者在25个公开数据集（Hilti、NTU-VIRAL）和多个私有数据集上做了全面测试，对比R3LIVE、LVI-SAM、FAST-LIO2等主流方案，FAST-LIVO2展现出压倒性优势：
精度更高：像素级的位姿估计精度，在航空测绘场景中体现得淋漓尽致；
鲁棒性更强：在无纹理、光照突变、激光雷达盲区等极端场景下稳定运行；
效率更优：核心模块经Intel/ARM处理器优化，可实时机载运行。
更重要的是，FAST-LIVO2已落地三大实用场景：
无人机自主导航：首个实现激光雷达-惯性-视觉系统的真实无人机自主飞行；
航空测绘：在无结构环境下实现像素级精度的测绘；
三维模型渲染：支持高质量网格生成、纹理映射、NeRF建模。
六、总结：多传感器SLAM的新标杆
FAST-LIVO2的出现，为激光雷达-惯性-视觉融合SLAM树立了新标杆：它既保留了多传感器融合的优势，又通过顺序更新ESIKF、平面先验、动态参考块、光线投射、曝光估计五大创新，解决了传统方案的核心痛点。
更可贵的是，该研究的代码和数据集已完全开源，不仅能助力机器人领域的学术研究，也能为工业界的无人机导航、三维重建、航空测绘等应用提供直接的技术支撑。相信在FAST-LIVO2的基础上，多传感器SLAM会朝着更精准、更鲁棒、更易用的方向持续进化。
论文信息
题目：FAST-LIVO2: Fast, Direct LiDAR–Inertial–Visual Odometry 作者：Chunran Zheng, Wei Xu, Zuhao Zou, Tong Hua, Chongjian Yuan, Dongjiao He, Bingyang Zhou, Zheng Liu, Jiarong Lin, Fangcheng Zhu, Yunfan Ren, Rong Wang, Fanle Meng, Fu Zhang