深度解析：多传感器融合SLAM技术全景剖析

本文剖析了卡尔曼滤波系列及滑动窗口优化方法，并系统回顾了视觉-惯性、LiDAR-惯性、视觉-LiDAR与LiDAR-视觉-惯性，四类SLAM融合方案及开源算法。

作者：

Jun Zhu, Hongyi Li,Tao Zhang, Qing Su

01   状态估计算法简介

1.1 卡尔曼滤波（KF）

SLAM中，先验值通常从一系列传感器获得，比如惯性测量单元（IMU）和编码器，而观测值则是通过GPS、相机和激光雷达等其他传感器获取的，后验值是融合了先验信息与观测数据之后得到的结果，它代表了根据所有已知信息计算出的最优定位输出。可以表达为：

其中，表示第k时刻机器人的状态向量，即位置、姿态等信息，是初始状态向量，代表从第一次到第k次的输入向量，是从初始状态到第k时刻的所有观测向量。

卡尔曼滤波器（KF）在解决机器人状态估计问题上是一种常用的方法，它被公认为贝叶斯滤波研究技术中的佼佼者，但其仅适用于线性高斯系统。

卡尔曼滤波算法流程

● 扩展卡尔曼滤波（EKF）

EKF是卡尔曼滤波器的一个重要变体，特别适用于处理非线性系统。

扩展卡尔曼滤波算法流程

● 迭代卡尔曼滤波（IEKF）

在处理非线性问题时，IEKF通过多次迭代改进线性化点以减小误差。它反复计算并更新卡尔曼增益及状态估计，直到达到收敛或变化极小为止，虽增加计算量，但能有效弥补了标准EKF方法的不足。

迭代卡尔曼滤波算法流程

● 误差状态卡尔曼滤波（ESKF）

ESKF是一种针对非线性问题优化设计的状态估计算法。与标准的EKF相比，ESKF通过将真实状态和名义状态分离，将误差作为状态变量进行估计，从而简化了非线性系统的处理。

误差状态卡尔曼滤波算法流程

1.2 滑动窗口优化

滑动窗口优化就是优化滑动窗口内的所有状态量，对于一个有个状态量的窗口，其最优解可以通过优化以下残差来获得：

其中是由IMU与积分得来的残差项，是视觉或者雷达的残差项，和是对应的方差，是IMU的量测集，是视觉或者雷达的量测集。是由上一个滑窗边缘化后留下的先验残差。

02   多传感器融合算法

多传感器融合可分为四个类别：视觉+IMU，LiDAR+IMU，视觉+LiDAR和视觉+LiDAR+IMU，每个分类中SOTA的算法如下表。

目前SOTA的多传感器融合算法 编译注：（1）类型：FB-滤波，OB-优化，LC-松耦合，TC-紧耦合。（2）回环方式：FAST和ORB-特征点，DBoW-词袋模型。(3)传感器类型：MC-单目，ML-机械雷达，SL-固态雷达，RC-RGB-D相机。（4）融合策略：FGO-因子图优化，BA-光束调整法，SWO-滑窗优化，PGO-姿态图有优化。

2.1 视觉和IMU的融合

● 基于滤波的方法

MSCKF：采用了一种无结构化的方法来处理视觉信息，在优化过程中考虑了最近一窗口内的所有相机姿态以及相关的特征点观测。它允许系统在实时运行的同时，利用已知的静态特征点实现对六自由度机器人的位姿进行高效估计，而不必将所有的历史特征点都包含在滤波器的状态向量中，从而避免了计算复杂度随着特征点数量增加而呈二次增长的问题。

MSCKF 2.0：由于在线性化测量模型时使用的是不断更新的估计值，这会导致线性化系统的可观测性质与实际非线性系统不匹配。改进版的MSCKF 2.0算法，在计算雅可比矩阵时使用首次可用的每种状态估计值，以确保系统的适当可观测性。

● 基于优化的方法

OKVIS：利用了IMU预积分技术将IMU测量整合到相对运动约束中，以此避免因状态估计不断变化而重复计算IMU传播的繁琐过程。此外，该系统引入了关键帧的概念，仅对最近的关键帧进行详细的特征点跟踪和优化，从而提高了系统的实时性和效率。

VINS-mono：在单目视觉场景中，初始化是显著挑战，因为需要加速度激励才能观测到尺度信息，意味着单目VINS无法从静止初始状态直接启动。此外，还需要考虑摄像头-IMU外参数校准以及数据处理等问题。这些问题在VINS-mono算法中得到了有效解决，该算法包括五个主要部分：预处理、初始化、基于非线性优化的VIO、环路闭合以及全局位姿图优化。

2.2 雷达与IMU的融合

● 松耦合方法

LOAM：LOAM是一个经典的三维LiDAR SLAM方法，其结构包含特征提取、里程计计算和地图构建三个主要模块。在LOAM中，通过比较当前扫描周期内提取的边缘点和平面点与前一扫描周期中的对应关系来更新姿态变换。然而，在高速运动场景下，LOAM的准确性会显著下降，此时可以通过整合IMU测量数据来改善，IMU提供的高频率运动信息有助于补偿动态环境下的运动失真，从而大大提高系统的准确性和鲁棒性。

LION：LION算法设计了用于处理低计算负荷的轻量化结构，特别关注在无法使用视觉传感器时如何依赖LiDAR和IMU融合实现准确且鲁棒的定位。通过结合LiDAR观测到的点云特征以及IMU提供的连续高频测量信息，尤其是在快速运动情况下，能够有效补偿因单纯依靠LiDAR带来的运动畸变问题，从而提升整体导航系统的精度和稳定性。

● 紧耦合方法

LIOM：LIOM借鉴了视觉-惯性融合技术中的思想，它采用滑动窗口的方式处理连续的LiDAR扫描帧，并对每个帧的姿态以及后续姿态进行联合优化。为了处理动态环境下的快速运动问题，LIOM引入了IMU预积分技术，即预先整合IMU测量数据以校正由于快速运动导致的点云失真，从而提升定位精度和稳定性。

LINS：LINS采用了迭代扩展卡尔曼滤波器的形式来进一步优化估计过程。这种迭代形式使得系统能够在每次迭代中更接近真实状态，减少线性化带来的误差。同时，LINS还考虑了机器人中心坐标系下算法的表述方式，以适应不同应用场景的需求。

此外还有LIO-SAM，LILI-OM，FAST-LIO系列算法。

2.3 雷达与视觉融合

● 松耦合算法

DEMO：DEMO首先通过估计出的相机姿态将LiDAR点云转换到与相机坐标系相对应的空间中，并生成一个深度图。在该过程中，相机前方点云中新产生的点被添加到地图构建中。为了高效处理和匹配这些点，地图中的点采用了球面坐标系统表示，并存储在一个基于两个角度坐标的2D k-d树结构中。对于每个从图像中提取的特征点，可以通过投影到由k-d树中该特征点最近邻三点构成的平面片上，获得相应的深度值。

LIMO：LIMO算法结合了单目相机和激光雷达（LiDAR）的数据，采用深度学习来识别并剔除环境中移动物体的特征，这样可以避免这些不可靠的特征对估计机器人自身运动状态的影响。该方法能够有效改善SLAM（同时定位与建图）过程中因误匹配动态对象而导致的轨迹漂移问题，从而提高视觉和LiDAR数据融合后的定位准确度。

● 紧耦合算法

V-LOAM：利用了相机采集频率远高于LiDAR的特点，通过增强后的视觉里程计获取可观察尺度信息来校正LiDAR点云的畸变。同时，针对视觉里程计在一次扫描内线性运动产生的漂移进行了建模，改进了去畸变过程的性能。接着，对校正后的点云进行匹配并注册到当前构建的地图中，进一步优化估计出的机器人姿态。

TVL-SLAM：TVL-SLAM利用高精度的时间戳同步保证了视觉帧和LiDAR扫描在同一时刻对应，进而实现基于因子图优化的紧耦合状态估计。这样可以有效消除由于传感器独立工作而带来的误差累积，并且通过同时考虑视觉特征和LiDAR几何特征，提升了对动态物体、光照变化以及无纹理区域的适应能力。

2.4 雷达，视觉与IMU融合

● 松耦合算法

VIL-SLAM：利用立体相机作为视觉传感器，在某些退化场景（例如长隧道）中表现优异，能够弥补纯LiDAR系统在此类环境中容易失效的问题。算法采用紧耦合的方式整合立体匹配和IMU测量数据，通过固定滞后平滑滤波技术，输出了基于IMU速率和相机速率的VIO位姿估计，这些估计结果用于去除运动畸变并执行LiDAR点云到地图的配准操作。

● 紧耦合算法

LIC-Fusion 2.0：首先利用IMU测量预测运动状态，然后通过视觉-惯性里程计进一步细化估计，最后使用LiDAR扫描到地图匹配来微调估计结果。还引入了一种新颖的平面特征跟踪算法至LIC-Fusion 2.0版本中，该算法从经由IMU测量去畸变后的LiDAR点云中提取平面点，并在一个滑动窗口中采用包含异常点剔除准则的数据关联策略，从而高效且鲁棒地处理LiDAR数据。

Super odometry：采用了以IMU为中心的数据处理管道，其包括三个主要组成部分：IMU里程计、视觉-惯性里程计以及LiDAR-惯性里程计。IMU偏差受到视觉-惯性和LiDAR-惯性里程计提供的位姿先验约束，同时，LiDAR-惯性里程计接收来自IMU里程计的运动预测。为实现实时高效率运行，该系统应用了动态八叉树结构。这种设计背后的洞察是，只要其他传感器能很好地约束IMU的偏置漂移，那么基于IMU的估计就能保持相当高的准确性，因为IMU产生的数据虽然带有噪声但很少出现离群值。

此外还有Super odometry，LVI-SAM和R3LIVE等算法。

03   未来研究方向

● 通用高效传感器融合策略：当前先进的融合框架算法通常针对特定平台设计，这导致它们难以在具有相似传感器的其他平台上部署。为了实现通用性和高效性，亟需研发一种灵活多变且高效的融合框架，使其能够适应不同平台的需求和配置。

● 深度学习在SLAM中的应用：利用深度学习进行特征提取、噪声抑制、动态物体识别和位姿估计等方面的改进。

● 分布式协作方式：多个机器人配备不同类型的传感器来共同执行相同的SLAM（同时定位与建图）任务，这样可以显著减轻单个机器人的工作负担。

审核编辑：黄飞