怼上所有传感器不一定好！Selective Kalman Filter：优雅的融合

编辑 | 自动驾驶之心

如何处理个别传感器的退化

为了解决复杂环境中的挑战并提高定位的鲁棒性和环境感知能力，越来越多的研究集中于融合多种传感器的SLAM系统。这些系统通常集成了多种传感器，如LiDAR、视觉、RGB-D相机和IMU，代表性的系统包括R3LIVE、FAST-LIVO、LVIO-Fusion、RIVER等。

现有关于卡尔曼滤波器的研究主要集中在多传感器信息融合方面，这些研究提出了使用扩展卡尔曼滤波器（EKF）和自适应模糊逻辑系统来融合里程计和声纳信号的方案，其核心思想是自适应调整增益。然而，这些工作主要集中于如何利用和融合来自多传感器的信息，而未考虑是否有必要融合或是否需要融合所有可用数据。在非退化条件下，当高精度数据与中等精度数据融合时，精度的提升往往是微小的，甚至可能是负面的，同时还会增加额外的计算开销。我们将这种对所有测量数据不加区分进行融合的方法称为“全包含”（“all-in”）方法。这种方法面临以下两个主要问题：

1. 首先，“全包含”方法处理的信息量比单传感器SLAM大得多，通常导致计算负担显著增加，影响实时性能。
2. 其次，尽管该方法结合了各种传感器的优势，但也引入了其劣势。例如，视觉SLAM易受运动模糊、剧烈光照变化和动态物体的影响，从而产生较大的误差，甚至导致系统失效。

因此，通过探讨何时以及如何选择性地融合多传感器信息，以LiDAR与视觉融合SLAM系统为例，解决上述两个问题。在多传感器SLAM融合中，单一传感器易出现定位退化，而整合不同类型的传感器可以提供额外的约束，防止SLAM退化。

• 对于“何时”融合，我们的解决方案是仅在更为精确和鲁棒的LiDAR SLAM子系统表现出退化时，才引入视觉信息，否则仅依赖LiDAR子系统进行位姿估计。
• 关于“如何”融合，如果直接融合所有信息，视觉子系统的较低精度可能会降低LiDAR系统的整体精度。因此，我们的解决方案是仅在LiDAR子系统退化的方向上融合视觉信息。
• 解决“何时”问题需要准确判断LiDAR SLAM是否发生退化。现有方法未考虑点面约束中旋转与平移的高耦合性，导致退化方向的评估不准确。

Selective Kalman Filter(选择性卡尔曼滤波器)[1]的解决方案通过准确判断LiDAR SLAM退化的方向，仅在这些退化方向上引入视觉观测数据。相比之下，LION的融合方法在LiDAR退化时停止使用LiDAR里程计，转而依赖其他里程计信息。这种方法较为粗糙，因为在非退化方向上依赖较低精度和鲁棒性的传感器信息可能会降低系统性能。Zhang、Hinduja和X-ICP的方法主张仅在退化方向融合其他传感器信息，这被我们认为是优化的策略。然而，这些优化方法基于优化框架，未能在滤波框架中精确识别退化的具体方向，可能导致实际退化方向未被充分约束。

具体贡献包括：

• 提出了一种新型的多传感器融合方法——选择性卡尔曼滤波器，设计用于解决数据“何时”与“如何”选择性融合的问题。这种方法仅在LiDAR数据退化时引入必要的视觉信息，大幅提升实时性能，同时确保鲁棒性和精度。
• 提出了一种创新高效的LiDAR里程计退化检测方法，可同时确定退化方向。该方法充分考虑了旋转与平移约束之间的耦合关系。
• 通过实验验证了所提退化检测方法的精确性以及选择性卡尔曼滤波器的效率。
• 开源了代码以促进社区参与和广泛应用。

系统概述

首先介绍LiDAR-惯性-视觉SLAM的“全包含”方法，该方法由FAST-LIVO和R3LIVE等方法所代表。在没有视觉或LiDAR测量数据的情况下，类似于FAST-LIO2，系统利用IMU的加速度和角速度进行状态传播，作为卡尔曼滤波的预测步骤。当LiDAR或视觉测量数据到达时，系统在卡尔曼滤波中执行数据融合更新。

据我们所知，在没有退化的情况下，与视觉-惯性SLAM（VIO）相比，LiDAR-惯性里程计（LIO）系统通常表现出更高的精度和更强的鲁棒性。在将选择性卡尔曼滤波器集成到LiDAR-惯性-视觉里程计（LIVO）系统中时，我们优先将LIO子系统作为核心，将VIO子系统作为辅助。系统的工作流程如图2所示。与“全包含”方法不同，我们的方法基于LiDAR测量进行退化检测与分析：

• 如果LIO子系统未发生退化，我们选择不融合视觉测量数据。这种方法有助于避免因视觉问题（如运动模糊、过度曝光和错误特征匹配）引入的误差。此外，它还减少了计算负担，从而提高了实时性能。

• 如果LIO子系统发生退化，为了尽量减少引入的视觉信息并防止状态估计恶化，我们分析LiDAR的退化方向，然后选择性地融合与此退化方向相关的视觉数据，并舍弃与非退化方向相关的数据。这有助于防止LiDAR的非退化维度的精度下降。

我们将这种选择性更新信息融合的方法称为“选择性卡尔曼滤波器”（Selective Kalman Filter）。

LiDAR SLAM中的退化检测

首先，我们通过加权线性最小二乘法对LiDAR测量方程(2)进行变换，得到以下形式：

其中，为状态的估计值。为了简化表示，我们做出以下定义：

因此，上述方程可以简化为：

由于实际场景中的噪声影响，矩阵通常为满秩矩阵，因此是一个6×6的对称正定矩阵。

Hinduja方法通过比较的最小特征值与预设阈值来检测退化。然而需要注意的是，由于的前三维表示旋转，后三维表示平移，其特征值是旋转和平移的耦合值，缺乏明确的物理意义，且单位同时与旋转和平移相关。同一特征值分解中的特征值单位不同，因此难以设定合理的退化检测阈值。此外，与这些特征值对应的特征向量也无法代表实际方向，这使得该方法在实际部署中往往不切实际。

为了解决这一问题，LION提出通过分析的对角元素来确定退化及其方向。首先，将矩阵表示为分块矩阵的形式：

LION分别对和进行特征值分解：

其中，和为对角矩阵，包含旋转和平移部分的特征值。然后通过设定两个阈值，评估旋转和平移是否退化。这种方法有效解决了Hinduja方法中阈值缺乏物理意义的问题，但其忽略了耦合项和，从而忽视了旋转和平移约束之间的相互影响。这种忽略导致在大多数情况下，退化程度及方向的估计不准确。

例如，如图3(a)所示，根据LION的观点，y方向的估计仅依赖于点A在y方向的点面残差雅可比，导致y方向估计较差（如所示）。但实际上，点B的点面残差可以改善旋转的估计，从而增强y方向的估计（如所示）。可以推断，这种考虑了平移和旋转耦合的后者方法，对于不确定的估计具有更高的准确性。

为了解决上述问题，我们提出了一种基于协方差信息的新型、简单且具有物理意义的退化检测方法。我们首先对矩阵求逆，得到状态的协方差矩阵：

需要注意的是：

从以上公式可以看出，我们的方法与LION使用局部Hessian矩阵的方法不同。我们的方法考虑了旋转与平移之间的耦合关系。此外，该方法具有实际的物理意义：协方差矩阵的对角块表示状态的方差，而非对角块表示不同状态之间的相关性。方差可以作为退化程度的直观量度，例如，高方差表示该方向上的估计存在较大的不确定性，从而表明退化。

我们对和进行特征值分解：

其中，和的特征值分别具有物理单位，旋转部分为，平移部分为。对应的特征向量代表退化方向，此时，和分别为旋转矩阵，能够将原始坐标系旋转到这些特征方向。

最后，我们基于实际需求和经验设置两个方差阈值和。如果，则表示发生旋转退化，对应的特征向量表示退化方向；平移部分同理。

综上所述，本节提出了一种具有实际物理意义并考虑耦合约束的退化检测方法，理论上能够更准确地判断退化程度及方向。

视觉测量的选择

我们首先对视觉测量方程 (3) 应用加权线性最小二乘法，得到以下形式：

其中， 为状态 的估计值。为了简化表示，我们定义如下：

因此，方程可以简化为：

需要注意的是，根据实际场景中的噪声，矩阵 总是可逆的。因此，可以得到估计状态 为：

利用协方差矩阵的特征向量矩阵 和 ，我们定义一个新的旋转矩阵 ：

该旋转矩阵能够将 的坐标系旋转至旋转和平移的主分量方向上：

在已知 LiDAR 子系统退化维度的情况下，我们定义一个选择矩阵 ，以选择视觉测量方程中与退化方向相关的信息，从而滤除无关数据：

例如，如果 LiDAR 子系统在一个平移方向和一个旋转方向上发生退化，例如 和 ，我们可以将选择矩阵 设置为：

在该矩阵中，设置为1的对角元素对应退化状态的维度，而设置为0的对角元素对应非退化维度。我们将状态恢复到原始坐标系，并同时恢复原始测量方程：

为了保持信息矩阵的对称性，我们进一步处理，得到：

可以简化为：

其中：

此时，我们已完成对视觉数据的选择，仅保留与 LiDAR 退化方向相关的信息。

选择性卡尔曼滤波器

首先，我们描述传统卡尔曼滤波器的形式。对于视觉子系统，假设已存在先验估计，记为 和协方差 。按照FAST-LIO的方法，卡尔曼增益 的计算公式为：

得到更新方程为：

将卡尔曼增益 代入更新方程并进行简化，可以得到：

根据式 (17)，可以进一步简化为：

其中， 是后验估计状态。

对于后验协方差 ，其公式为：

将 的表达式代入并简化，可以得到：

通过观察视觉信息的测量方程在选择前（式 (18)）和选择后（式 (26)）的变化，我们只需将 替换为 ，将 替换为 ，即可得到新的选择性卡尔曼滤波器公式：

备注：在本文中，所有进行特征值分解的矩阵均为实对称矩阵，因此其特征值分解和奇异值分解的结果是等价的。我们的主要目标是确定这些矩阵的主方向，这本质上与奇异值相关的物理含义一致。为了简化表达并减少变量的数量，本文统一采用特征值分解描述整个过程。

LiDAR-惯性-视觉里程计

基于出色的LIVO框架R3LIVE，我们集成了选择性卡尔曼滤波器，构建了SKF-Fusion，其系统框架如图4所示。原始R3LIVE框架包含两个并行子系统：LiDAR-惯性里程计（LIO）和视觉-惯性里程计（VIO）。本文未重复描述R3LIVE中的相同内容。在此基础上，我们对LIO子系统进行退化检测。如果未检测到退化，仅将视觉信息用于上色，同时保存地图的颜色信息，以便在退化状态下使用。如果检测到退化，则在帧与帧之间以及帧与地图之间进行序列更新，专注于退化状态的相关成分。在随后的实验中，我们使用R3LIVE数据集对SKF-Fusion进行了评估。

系统工作流程：

1. 未检测到退化时：

• 系统仅使用LiDAR子系统进行状态估计和更新。
• 视觉信息仅用于地图的颜色渲染，无需进行视觉状态更新。
• 这种方式避免了视觉子系统带来的计算开销，并减少了因视觉特性（如运动模糊、过度曝光等）引起的误差。

2. 检测到退化时：

• 系统分析LiDAR的退化方向。
• 针对退化方向，选择性地融合视觉观测数据，仅对退化状态进行更新。
• 非退化方向的数据被舍弃，从而防止引入不必要的误差。

通过引入选择性卡尔曼滤波器，SKF-Fusion实现了高效的退化处理，显著提升了系统的鲁棒性和实时性能。

实验效果

总结一下

基于传统的“全包含”融合SLAM（涉及LiDAR、视觉和IMU），本研究引入了一种基于协方差信息的创新退化检测模块，以及用于退化状态选择性更新的选择性卡尔曼滤波器。与现有退化检测方法相比，实验结果表明，该方法显著提升了退化检测的准确性，从而提高了定位的精度和鲁棒性。此外，与“全包含”方法相比，方法显著提升了视觉子系统的实时性能。需要注意的是，当前的阈值选择未能适应不同体素分辨率下的LIO，这为未来的研究提供了方向。

参考

[1] Selective Kalman Filter: When and How to Fuse Multi-Sensor Information to Overcome Degeneracy in SLAM