从可观测性的角度研究基于扩展卡尔曼滤波器（EKF）的同时定位与地图构建（SLAM）中的不一致性问题附Matlab代码-优快云博客

本文链接：https://blog.youkuaiyun.com/matlab_daizuo/article/details/148315254

✅作者简介：热爱科研的Matlab仿真开发者，擅长数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。

🍎 往期回顾关注个人主页：Matlab科研工作室

🍊个人信条：格物致知,完整Matlab代码及仿真咨询内容私信。

🔥 内容介绍

同时定位与地图构建（SLAM）是移动机器人领域的核心问题，旨在使机器人在未知环境中构建地图的同时，估计自身的位置和姿态。扩展卡尔曼滤波器（EKF）因其高效性，长期以来是解决SLAM问题的主流方法之一。然而，基于EKF的SLAM算法普遍存在估计不一致性问题，即估计协方差矩阵对真实误差的低估，从而导致对估计结果置信度的错误评估。本文将从可观测性理论的角度深入剖析EKF-SLAM中的不一致性问题。首先，我们将回顾EKF-SLAM的基本原理，并阐述不一致性的产生机制。其次，我们将详细讨论可观测性对EKF-SLAM一致性的影响，包括线性化误差、数据关联以及特征初始化等因素。最后，本文将探讨一些针对EKF-SLAM不一致性问题的改进策略和未来研究方向。

1. 引言

同时定位与地图构建（SLAM）技术是实现自主导航的关键。它赋予移动机器人在没有先验地图信息的未知环境中，通过传感器数据实时构建环境地图并同步确定自身位姿的能力。SLAM技术在机器人、自动驾驶、虚拟现实等领域具有广泛的应用前景。

在众多SLAM方法中，基于滤波器的解决方案，尤其是基于扩展卡尔曼滤波器（EKF）的方法，因其实时性高、计算复杂度相对较低的特点，在早期研究中占据了主导地位。EKF-SLAM通过卡尔曼滤波器的框架，将机器人位姿和地图特征状态融合到一个状态向量中，并通过预测和更新步骤迭代地估计这些状态。

然而，EKF-SLAM的一个长期存在的挑战是其估计的不一致性问题。不一致性表现为EKF估计的协方差矩阵（表示估计误差的不确定性）系统性地低于实际的误差，这意味着EK尔夫（EKF）对自身的估计过于自信。这种不一致性不仅会影响后续的决策和规划，甚至可能导致系统发散。因此，深入理解和解决EKF-SLAM中的不一致性问题，对于提高SLAM系统的鲁棒性和可靠性至关重要。

本文将从可观测性理论的角度，对EKF-SLAM中的不一致性问题进行系统性研究。可观测性是控制理论中的一个核心概念，它描述了从系统输出中推断系统内部状态的能力。我们将探讨EKF-SLAM中可观测性如何影响估计的一致性，以及在实际应用中如何通过优化可观测性来缓解不一致性问题。

2. EKF-SLAM基本原理与不一致性机制

2.1 EKF-SLAM概述

EKF-SLAM通常将机器人位姿和环境中观测到的特征点（或地标）的状态作为状态向量的一部分进行估计。系统模型包括运动模型和观测模型。

EKF-SLAM的迭代过程包括预测和更新两个阶段：

预测：
根据运动模型和上一时刻的估计值，预测当前时刻的机器人位姿和地图状态，并更新其协方差。这涉及到对非线性函数进行线性化。
更新：
当机器人观测到环境中的特征点时，利用观测模型和当前的观测值来修正预测的状态和协方差。这同样涉及到对非线性函数进行线性化。

2.2 不一致性产生机制

EKF-SLAM中不一致性的主要原因在于对非线性系统进行线性化处理。EKF通过在当前估计点进行一阶泰勒展开来近似非线性函数，这引入了线性化误差。当系统的高度非线性或估计误差较大时，这种线性化误差会变得显著，导致卡尔曼滤波器对状态和协方差的估计产生偏差。具体来说，不一致性体现在以下几个方面：

协方差矩阵的低估：
线性化误差导致EKF计算的协方差矩阵（表示不确定性）系统性地低于实际的误差协方差。这意味着EKF对自己的估计过于“自信”，错误地认为估计结果的精度很高。
雅可比矩阵的近似：
EKF使用雅可比矩阵来近似非线性函数的局部梯度，而雅可比矩阵的计算依赖于当前的状态估计。如果状态估计存在偏差，那么雅可比矩阵的近似也会不准确，从而传播误差。
数据关联问题：
在EKF-SLAM中，将当前观测与已知地图特征进行匹配（数据关联）是一个关键步骤。错误的数据关联会导致状态估计和协方差的错误更新，从而加剧不一致性。
特征初始化问题：
新观测到的特征点需要被初始化并添加到地图中。不准确的特征初始化会引入误差，并扩散到整个状态向量。

3. 可观测性对EKF-SLAM一致性的影响

可观测性是系统理论中的一个基本概念，它描述了从系统的输出（传感器观测）中推断系统内部状态（机器人位姿和地图特征）的能力。对于EKF-SLAM而言，良好的可观测性是确保一致性估计的前提。

3.1 线性化误差与可观测性

线性化误差是EKF不一致性的根本原因。当系统高度非线性时，即使可观测性条件满足，线性化误差也可能导致协方差的低估。然而，可观测性不足会进一步放大线性化误差的影响。

轨迹的可观测性：
机器人轨迹的可观测性是指仅凭传感器观测能否唯一确定机器人的运动轨迹。例如，在直线运动或纯旋转运动中，如果缺少足够的特征点观测，机器人的位姿可能存在退化，导致轨迹的可观测性不足。
地图特征的可观测性：
地图特征的可观测性是指能否从机器人的观测中准确估计特征点的位置。如果一个特征点只被短暂观测到或被多次观测到的视角变化不大，其位置估计的精度可能较低，从而影响整个地图的一致性。

当系统在某些方向上不可观测时，EKF将无法准确地更新相关状态的协方差，导致这些方向上的不确定性被低估。这种低估会随着时间累积，最终导致估计值与真实值之间的显著偏差。

3.2 数据关联与可观测性

数据关联是EKF-SLAM中一个挑战性的问题。错误的关联会严重破坏EKF的一致性。可观测性在数据关联中扮演着重要角色：

混淆的观测：
当环境中存在相似的特征点时，可能会导致数据关联的模糊性。例如，在走廊或对称的环境中，多个相同的门或窗户可能导致EKF难以区分它们。此时，如果特征点的可观测性（例如，它们在不同视角下的独特几何信息）不足，错误关联的风险就会增加。
不确定性传播：
如果某个特征点的协方差被EKF低估，那么在进行数据关联时，其门限（Mahalanobis距离）也会相应地变小，从而更容易导致错误关联。这种错误关联的后果是，系统会错误地更新状态，进一步加剧不一致性。

良好的可观测性可以帮助EKF更准确地估计特征点的协方差，从而为数据关联提供更可靠的判断依据，降低错误关联的概率。

3.3 特征初始化与可观测性

新特征点的初始化是将它们添加到地图中的过程。不准确的初始化会对EKF-SLAM的一致性产生负面影响。

视差不足：
当机器人只从一个视角或视差很小的情况下观测到新特征点时，其深度信息往往不够精确。此时，如果不对特征点进行足够多的不同视角观测，其位置的可观测性会很差，导致初始化的不确定性很大。
误差的累积：
初始化误差会传播到整个状态向量中。如果一个特征点初始化不准确，那么当机器人再次观测到它时，将导致EKF的更新步引入更大的线性化误差，从而加速不一致性的累积。

为了确保一致性，在特征初始化时应尽可能地确保其可观测性，例如通过多视角观测来提高初始化的精度。

4. 解决EKF-SLAM不一致性问题的策略

针对EKF-SLAM的不一致性问题，研究者们提出了多种改进策略，其中许多都与提高可观测性或更有效地处理线性化误差相关：

4.1 改进线性化方法

无迹卡尔曼滤波器（UKF）：
UKF使用确定性采样点（sigma points）来近似非线性函数的均值和协方差，避免了雅可比矩阵的计算和线性化误差。UKF在处理高度非线性系统方面表现出比EKF更好的一致性。
容积卡尔曼滤波器（CKF）：
CKF是另一种无迹卡尔曼滤波器，使用球面-径向容积规则选择采样点。CKF在某些情况下可以提供比UKF更优的性能。

4.2 优化数据关联策略

多假设数据关联（MHT）：
MHT考虑多个数据关联假设，并使用概率来评估它们的可能性。通过延迟决策，MHT可以减少错误关联的风险，但计算复杂度较高。
门限调整与自适应：
动态调整数据关联的门限，使其能够更好地适应不确定性，可以降低错误关联的概率。这通常需要更精确地估计协方差矩阵。
特征描述子的改进：
使用更具辨识度和鲁棒性的特征描述子可以降低相似特征点之间的混淆，从而提高数据关联的准确性。

4.3 提高特征初始化的鲁棒性

延迟初始化：
不立即初始化新观测到的特征点，而是等待多次观测并获得足够视差后再进行初始化，可以显著提高特征点的初始精度。
多视角几何约束：
利用多视角几何（例如，三角测量）来更精确地估计新特征点的位置，从而降低初始化的误差。

4.4 考虑系统可观测性分析

系统状态分解：
将SLAM问题分解为独立的子问题（例如，位姿估计和地图构建），并分析每个子问题的可观测性。
选择合适的传感器：
选择能够提供丰富且多样观测信息的传感器，例如，结合视觉和惯性测量单元（IMU）可以提高系统的可观测性。IMU可以提供独立的运动信息，从而缓解纯视觉SLAM在退化运动中的可观测性问题。