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

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

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

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

🔥 内容介绍

一、研究背景与意义

同时定位与地图构建（SLAM）是自主移动机器人实现自主导航、环境感知与交互的核心技术，其核心目标是让机器人在未知环境中，通过自身传感器数据同时完成自身位姿估计与环境地图构建。扩展卡尔曼滤波器（EKF）因能有效处理非线性系统的状态估计问题，被广泛应用于SLAM系统（即EKF-SLAM），成为早期SLAM研究的主流框架。

然而，EKF-SLAM在实际应用中普遍存在不一致性问题，表现为状态估计误差的协方差矩阵被严重低估，导致估计结果看似“可信”实则偏离真实值，最终引发机器人导航失准、地图失真等严重问题，限制了EKF-SLAM在高精度、高可靠性场景（如工业巡检、自动驾驶、深空探测）中的应用。

可观测性作为系统理论的核心概念，描述了通过输出数据（传感器观测）能否唯一确定系统状态（机器人位姿与地图特征）的能力。从可观测性角度出发，EKF-SLAM的不一致性本质上与系统可观测性的特性、可观测性矩阵的构造缺陷以及EKF对可观测性的错误假设密切相关。因此，深入剖析可观测性与EKF-SLAM不一致性之间的内在关联，提出基于可观测性优化的不一致性抑制策略，对提升EKF-SLAM的估计精度与可靠性具有重要的理论价值与实际意义。

二、核心理论基础

2.1 可观测性基本概念

对于非线性系统$\dot{\mathbf{x}} = f(\mathbf{x}, \mathbf{u}, t)$，$\mathbf{z} = h(\mathbf{x}, t)$（其中$\mathbf{x}$为状态向量，$\mathbf{u}$为控制输入，$\mathbf{z}$为观测输出，$f$为状态转移函数，$h$为观测函数），其可观测性定义为：若存在有限时间$t_1 > t_0$，通过$[t_0, t_1]$时间段内的输入$\mathbf{u}(t)$和输出$\mathbf{z}(t)$，能够唯一确定$t_0$时刻的初始状态$\mathbf{x}(t_0)$，则称系统在$\mathbf{x}(t_0)$处是局部可观测的；若系统在所有状态处均满足局部可观测性，则称系统全局可观测。

对于线性化后的系统（EKF核心思想之一），可通过构造可观测性矩阵$\mathcal{O}$判断可观测性：$\mathcal{O} = \begin{bmatrix} \mathbf{C} \\ \mathbf{C}\mathbf{A} \\ \mathbf{C}\mathbf{A}^2 \\ \vdots \\ \mathbf{C}\mathbf{A}^{n-1} \end{bmatrix}$，其中$\mathbf{A}$为状态转移矩阵线性化后的雅可比矩阵，$\mathbf{C}$为观测矩阵线性化后的雅可比矩阵。若$\mathcal{O}$满秩，则线性化系统完全可观测；否则存在不可观测子空间，对应状态无法通过观测唯一确定。

2.2 EKF-SLAM的基本框架

EKF-SLAM将机器人位姿与环境地图特征统一纳入状态向量$\mathbf{x} = [\mathbf{x}_r^T, \mathbf{x}_m^T]^T$，其中$\mathbf{x}_r = [x, y, \theta]^T$为机器人二维位姿（x、y为平面坐标，$\theta$为航向角），$\mathbf{x}_m = [\mathbf{x}_{m1}^T, \mathbf{x}_{m2}^T, \dots, \mathbf{x}_{mn}^T]^T$为n个环境特征点的坐标。其核心流程分为预测与更新两步：

1. 预测阶段：基于机器人运动模型，利用控制输入$\mathbf{u}$（如轮式机器人的线速度、角速度）预测状态的先验估计$\hat{\mathbf{x}}_{k|k-1}$和先验协方差矩阵$\mathbf{P}_{k|k-1}$，即$\hat{\mathbf{x}}_{k|k-1} = f(\hat{\mathbf{x}}_{k-1|k-1}, \mathbf{u}_k)$，$\mathbf{P}_{k|k-1} = \mathbf{F}_k \mathbf{P}_{k-1|k-1} \mathbf{F}_k^T + \mathbf{Q}_k$，其中$\mathbf{F}_k$为状态转移函数$f$的雅可比矩阵，$\mathbf{Q}_k$为过程噪声协方差矩阵。

2. 更新阶段：基于传感器观测模型（如激光雷达、相机的观测方程），计算观测残差$\mathbf{r}_k = \mathbf{z}_k - h(\hat{\mathbf{x}}_{k|k-1})$、观测雅可比矩阵$\mathbf{H}_k$和残差协方差矩阵$\mathbf{S}_k = \mathbf{H}_k \mathbf{P}_{k|k-1} \mathbf{H}_k^T + \mathbf{R}_k$（$\mathbf{R}_k$为观测噪声协方差矩阵），进而计算卡尔曼增益$\mathbf{K}_k = \mathbf{P}_{k|k-1} \mathbf{H}_k^T \mathbf{S}_k^{-1}$，最终得到后验估计$\hat{\mathbf{x}}_{k|k} = \hat{\mathbf{x}}_{k|k-1} + \mathbf{K}_k \mathbf{r}_k$和后验协方差矩阵$\mathbf{P}_{k|k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k|k-1}$（$\mathbf{I}$为单位矩阵）。

2.3 EKF-SLAM的可观测性特性

经典EKF-SLAM系统存在固有不可观测性：在无绝对定位参考（如GPS）的情况下，系统的全局位姿（机器人初始位置、航向角）和地图的全局坐标是不可观测的，仅机器人相对地图的位姿关系和地图特征间的相对位置是可观测的。这一特性源于SLAM的“同时定位与建图”本质——系统无法区分“机器人移动+地图静止”与“机器人静止+地图反向移动”两种状态，导致可观测性矩阵$\mathcal{O}$存在零特征值，对应不可观测子空间。

此外，EKF-SLAM的可观测性还受传感器类型（如单目相机存在尺度不可观测性，激光雷达可观测绝对距离）、观测约束数量（特征点数量过少会降低可观测性）、运动轨迹（如机器人直线运动时部分状态不可观测）等因素影响，这些因素均可能加剧系统的不一致性。

三、基于可观测性的EKF-SLAM不一致性根源分析

EKF-SLAM的不一致性并非单一因素导致，从可观测性角度来看，其核心根源在于EKF的线性化假设与系统真实可观测性特性不匹配，以及可观测性矩阵构造与利用过程中的缺陷，具体可分为以下4类：

3.1 线性化误差引发的可观测性偏差

EKF通过一阶泰勒展开将非线性的状态转移函数$f$和观测函数$h$线性化，但其前提是“线性化点附近的误差较小”。在实际场景中，当机器人运动轨迹复杂、观测距离较远或环境特征分布密集时，线性化误差会显著增大，导致线性化后的系统可观测性与真实非线性系统的可观测性出现偏差——线性化系统可能误判某些状态为“可观测”，进而在更新阶段对这些状态进行过度校正，同时低估协方差矩阵，引发不一致性。

2 不可观测子空间的错误处理

如前文所述，EKF-SLAM存在固有不可观测子空间（如全局位姿、地图全局坐标），这些子空间的状态无法通过观测唯一确定，其估计误差理论上应保持不变。但经典EKF并未对不可观测子空间进行特殊处理：在预测阶段，不可观测子空间的状态误差会随运动模型传播；在更新阶段，卡尔曼增益会错误地对这些状态进行校正，导致后验协方差矩阵被不合理地缩小，而真实误差却未得到有效抑制，最终表现为不一致性。

例如，当机器人在封闭环境中长时间运动时，全局位姿的不可观测性会导致误差不断累积，而EKF的协方差矩阵却因持续更新而逐渐减小，形成“误差累积但协方差低估”的矛盾，最终引发导航失效。

3.3 可观测性矩阵的构造缺陷

EKF的更新阶段依赖观测雅可比矩阵$\mathbf{H}_k$（可观测性矩阵的核心组成部分）来衡量观测对状态的约束能力。但在实际应用中，$\mathbf{H}_k$的构造存在诸多缺陷：

• **特征点匹配误差**：当传感器观测存在噪声（如相机图像的特征点匹配错误、激光雷达的点云误匹配）时，$\mathbf{H}_k$会基于错误的观测信息构造，导致可观测性矩阵的秩被错误估计（如伪观测会使可观测性矩阵“伪满秩”），进而导致卡尔曼增益计算偏差，引发过度校正和协方差低估。

• **观测信息冗余/不足**：当观测到的特征点数量过少时，可观测性矩阵的秩不足，无法为所有可观测状态提供足够约束，导致部分状态的误差无法被有效抑制；当观测到的特征点存在大量冗余（如多个特征点对应同一环境目标）时，$\mathbf{H}_k$会包含重复信息，导致可观测性矩阵的条件数增大，数值稳定性下降，进而影响协方差矩阵的更新精度，加剧不一致性。

3.4 过程/观测噪声协方差的不准确设定

EKF的性能依赖于过程噪声协方差$\mathbf{Q}_k$和观测噪声协方差$\mathbf{R}_k$的准确设定，而这两个参数与系统可观测性密切相关：$\mathbf{Q}_k$决定了预测阶段状态误差的传播强度，$\mathbf{R}_k$决定了更新阶段观测信息的权重（即观测对可观测性的贡献程度）。

在实际应用中，$\mathbf{Q}_k$和$\mathbf{R}_k$通常基于经验设定，难以准确匹配真实环境的噪声特性：若$\mathbf{Q}_k$设定过小，会低估运动模型的不确定性，导致预测误差被低估；若$\mathbf{R}_k$设定过小，会过度放大观测信息的权重，导致更新阶段对状态的校正过度；若两者均设定不准确，则会导致可观测性矩阵与真实系统的匹配度进一步下降，最终引发不一致性。

四、基于可观测性的EKF-SLAM不一致性抑制策略

针对上述根源，从可观测性角度出发，可通过“优化可观测性感知、校正不可观测子空间、提升可观测性矩阵可靠性”三大思路，提出以下不一致性抑制策略：

4.1 基于可观测性分析的线性化优化

核心目标是减小线性化误差对可观测性的影响，使线性化系统的可观测性尽可能贴近真实非线性系统：

• **自适应线性化策略**：通过实时分析可观测性矩阵的秩变化，判断线性化误差的大小——当可观测性矩阵的秩发生突变时（表明线性化误差较大），采用更高阶的线性化方法（如无迹卡尔曼滤波UKF的Sigma点采样方法）替代一阶泰勒展开，提升线性化精度，减少可观测性偏差。

• **线性化点优化**：将线性化点从“先验估计值”调整为“基于可观测性的最优估计点”——通过可观测性矩阵筛选出对观测约束最强的状态子集，以该子集的后验估计值作为线性化点，降低线性化误差对可观测性的影响。

4.2 不可观测子空间的分离与约束

通过可观测性分解，将状态空间划分为可观测子空间和不可观测子空间，对两者进行差异化处理，避免不可观测子空间的错误更新：

• **可观测性分解算法**：利用奇异值分解（SVD）对可观测性矩阵$\mathcal{O}$进行分解，得到可观测子空间的正交基和不可观测子空间的正交基，进而将状态向量$\mathbf{x}$投影到两个子空间中，得到$\mathbf{x} = \mathbf{x}_o + \mathbf{x}_{\bar{o}}$（$\mathbf{x}_o$为可观测状态，$\mathbf{x}_{\bar{o}}$为不可观测状态）。

• **子空间差异化更新**：在EKF的更新阶段，仅对可观测子空间的状态$\mathbf{x}_o$进行校正，保持不可观测子空间的状态$\mathbf{x}_{\bar{o}}$及其协方差矩阵不变；同时，通过投影矩阵将卡尔曼增益限制在可观测子空间内，避免对不可观测状态的错误校正，从而抑制协方差低估。

4.3 基于可观测性的观测优化与噪声自适应估计

通过提升可观测性矩阵的可靠性，优化观测信息的利用效率，同时实现噪声协方差的自适应调整：

• **观测信息筛选与加权**：基于可观测性矩阵的奇异值大小，对观测到的特征点进行筛选——保留奇异值较大（对可观测性贡献大）的特征点，剔除奇异值较小（观测约束弱）或无效的特征点（如匹配错误的特征点）；同时，根据奇异值的大小为不同观测分配权重，构造加权观测雅可比矩阵$\mathbf{H}_k^w$，提升可观测性矩阵的可靠性。

• **基于可观测性的噪声自适应估计**：利用可观测性矩阵的秩变化和观测残差$\mathbf{r}_k$的统计特性，实时调整$\mathbf{Q}_k$和$\mathbf{R}_k$——当可观测性矩阵的秩降低（观测约束不足）时，增大$\mathbf{R}_k$以降低观测权重；当观测残差增大（过程噪声增大）时，增大$\mathbf{Q}_k$以提升预测阶段的误差容忍度，使噪声协方差与系统可观测性特性动态匹配。

4.4 融合绝对观测的可观测性增强

针对EKF-SLAM的固有不可观测性，可通过引入绝对观测（如GPS、视觉惯导、地标导航）增强系统的可观测性，减少不可观测子空间的范围：

• **多传感器融合**：将GPS（提供绝对位置）、IMU（惯性测量单元，提供高频运动信息）与传统视觉/激光传感器融合，通过绝对观测为系统提供全局参考，消除全局位姿的不可观测性，使可观测性矩阵满秩，从而从根源上抑制因不可观测子空间导致的不一致性。

• **人工地标辅助**：在环境中部署已知坐标的人工地标（如二维码、激光反射板），通过传感器对地标进行观测，为系统提供绝对约束，增强可观测性，降低误差累积，抑制协方差低估。

五、总结与展望

从可观测性角度来看，EKF-SLAM的不一致性根源在于线性化误差引发的可观测性偏差、不可观测子空间的错误处理、可观测性矩阵的构造缺陷以及噪声协方差的不准确设定。通过优化线性化策略、分离并约束不可观测子空间、提升可观测性矩阵可靠性以及融合绝对观测增强可观测性等策略，可有效抑制系统的不一致性，提升估计精度与可靠性。

未来的研究方向可聚焦于以下方面：

• **非线性可观测性的精确建模**：现有研究多基于线性化系统的可观测性分析，未来可探索基于李群、李代数的非线性可观测性建模方法，更准确地描述EKF-SLAM的真实可观测性特性。

• **实时可观测性评估与自适应调整**：针对动态环境（如行人、车辆移动的场景），研究实时、高效的可观测性评估算法，实现抑制策略的动态自适应调整，提升系统在动态环境中的鲁棒性。

• **多智能体SLAM的可观测性与一致性**：扩展单智能体的可观测性分析框架至多智能体SLAM，研究多智能体间的观测交互对可观测性的影响，提出多智能体协同的不一致性抑制策略，提升群体导航的可靠性。

⛳️ 运行结果

🔗 参考文献

[1] 苏涛.面向越野环境的多传感器融合SLAM技术研究[D].中国科学技术大学,2022.

[2] 于宁波,王石荣.利用双RGB-D传感器融合增强对未知环境的自主探索和地图构建[J].Engineering, 2019, v.5(01):355-373.DOI:CNKI:SUN:GOCH.0.2019-01-022.

[3] 张书景.大尺度环境中自主式水下机器人同时定位与地图构建算法研究[D].中国海洋大学[2026-01-01].

📣 部分代码

🎈 部分理论引用网络文献，若有侵权联系博主删除

 👇 关注我领取海量matlab电子书和数学建模资料 

🏆团队擅长辅导定制多种科研领域MATLAB仿真，助力科研梦：

🌈 各类智能优化算法改进及应用

生产调度、经济调度、装配线调度、充电优化、车间调度、发车优化、水库调度、三维装箱、物流选址、货位优化、公交排班优化、充电桩布局优化、车间布局优化、集装箱船配载优化、水泵组合优化、解医疗资源分配优化、设施布局优化、可视域基站和无人机选址优化、背包问题、 风电场布局、时隙分配优化、 最佳分布式发电单元分配、多阶段管道维修、 工厂-中心-需求点三级选址问题、 应急生活物质配送中心选址、 基站选址、 道路灯柱布置、 枢纽节点部署、 输电线路台风监测装置、 集装箱调度、 机组优化、 投资优化组合、云服务器组合优化、 天线线性阵列分布优化、CVRP问题、VRPPD问题、多中心VRP问题、多层网络的VRP问题、多中心多车型的VRP问题、 动态VRP问题、双层车辆路径规划（2E-VRP）、充电车辆路径规划（EVRP）、油电混合车辆路径规划、混合流水车间问题、 订单拆分调度问题、 公交车的调度排班优化问题、航班摆渡车辆调度问题、选址路径规划问题、港口调度、港口岸桥调度、停机位分配、机场航班调度、泄漏源定位

🌈 机器学习和深度学习时序、回归、分类、聚类和降维

2.1 bp时序、回归预测和分类

2.2 ENS声神经网络时序、回归预测和分类

2.3 SVM/CNN-SVM/LSSVM/RVM支持向量机系列时序、回归预测和分类

2.4 CNN|TCN|GCN卷积神经网络系列时序、回归预测和分类

2.5 ELM/KELM/RELM/DELM极限学习机系列时序、回归预测和分类

2.6 GRU/Bi-GRU/CNN-GRU/CNN-BiGRU门控神经网络时序、回归预测和分类

2.7 ELMAN递归神经网络时序、回归\预测和分类

2.8 LSTM/BiLSTM/CNN-LSTM/CNN-BiLSTM/长短记忆神经网络系列时序、回归预测和分类

2.9 RBF径向基神经网络时序、回归预测和分类

2.10 DBN深度置信网络时序、回归预测和分类

2.11 FNN模糊神经网络时序、回归预测

2.12 RF随机森林时序、回归预测和分类

2.13 BLS宽度学习时序、回归预测和分类

2.14 PNN脉冲神经网络分类

2.15 模糊小波神经网络预测和分类

2.16 时序、回归预测和分类

2.17 时序、回归预测预测和分类

2.18 XGBOOST集成学习时序、回归预测预测和分类

2.19 Transform各类组合时序、回归预测预测和分类

方向涵盖风电预测、光伏预测、电池寿命预测、辐射源识别、交通流预测、负荷预测、股价预测、PM2.5浓度预测、电池健康状态预测、用电量预测、水体光学参数反演、NLOS信号识别、地铁停车精准预测、变压器故障诊断

🌈图像处理方面

图像识别、图像分割、图像检测、图像隐藏、图像配准、图像拼接、图像融合、图像增强、图像压缩感知

🌈 路径规划方面

旅行商问题（TSP）、车辆路径问题（VRP、MVRP、CVRP、VRPTW等）、无人机三维路径规划、无人机协同、无人机编队、机器人路径规划、栅格地图路径规划、多式联运运输问题、 充电车辆路径规划（EVRP）、 双层车辆路径规划（2E-VRP）、 油电混合车辆路径规划、 船舶航迹规划、 全路径规划规划、 仓储巡逻

🌈 无人机应用方面

无人机路径规划、无人机控制、无人机编队、无人机协同、无人机任务分配、无人机安全通信轨迹在线优化、车辆协同无人机路径规划

🌈 通信方面

传感器部署优化、通信协议优化、路由优化、目标定位优化、Dv-Hop定位优化、Leach协议优化、WSN覆盖优化、组播优化、RSSI定位优化、水声通信、通信上传下载分配

🌈 信号处理方面

信号识别、信号加密、信号去噪、信号增强、雷达信号处理、信号水印嵌入提取、肌电信号、脑电信号、信号配时优化、心电信号、DOA估计、编码译码、变分模态分解、管道泄漏、滤波器、数字信号处理+传输+分析+去噪、数字信号调制、误码率、信号估计、DTMF、信号检测

🌈电力系统方面

微电网优化、无功优化、配电网重构、储能配置、有序充电、MPPT优化、家庭用电

🌈 元胞自动机方面

交通流 人群疏散 病毒扩散 晶体生长 金属腐蚀

🌈 雷达方面

卡尔曼滤波跟踪、航迹关联、航迹融合、SOC估计、阵列优化、NLOS识别

🌈 车间调度

零等待流水车间调度问题NWFSP 、 置换流水车间调度问题PFSP、 混合流水车间调度问题HFSP 、零空闲流水车间调度问题NIFSP、分布式置换流水车间调度问题 DPFSP、阻塞流水车间调度问题BFSP

👇