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

最新推荐文章于 2025-12-04 22:09:52 发布

原创最新推荐文章于 2025-12-04 22:09:52 发布 · 804 阅读

15 ·

CC 4.0 BY-SA版权

文章标签：

#matlab #算法 #开发语言

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

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

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

🔥 内容介绍

同时定位与地图构建（SLAM）作为移动机器人自主导航的核心技术，旨在让机器人在未知环境中通过传感器数据同时估计自身位姿和环境地图。扩展卡尔曼滤波器（EKF）因能处理非线性系统而成为 SLAM 领域的经典解决方案，但实际应用中常出现不一致性问题—— 滤波器估计的协方差矩阵与真实误差不匹配，表现为过度自信（协方差偏小）或过度保守（协方差偏大），严重影响导航精度和系统稳定性。从可观测性角度剖析这一问题，是理解其本质并提出改进策略的关键。

可观测性与 SLAM 系统的基础关联

可观测性是控制系统理论中的核心概念，描述通过系统输出（传感器测量）能否唯一确定系统状态的能力。在 SLAM 中，系统状态通常包含机器人位姿（位置和姿态）和环境特征（如路标坐标），传感器测量（如激光雷达点云、视觉特征匹配）则是状态的非线性函数。SLAM 的可观测性直接决定了状态估计的理论精度上限：若状态完全可观测，理论上可通过足够的测量消除初始不确定性；若存在不可观测子空间，状态的某些维度将始终保留模糊性，这是导致 EKF-SLAM 不一致性的根本原因之一。

SLAM 系统的可观测性具有鲜明特点：当机器人处于静态时，仅通过自身传感器无法区分 “机器人静止而环境移动” 与 “机器人移动而环境静止”，此时位姿与路标坐标的绝对位置存在全局尺度不确定性；当机器人运动轨迹存在冗余（如重复路径）时，部分状态维度的观测信息会重叠，可能导致可观测性矩阵秩亏。这些特性为 EKF-SLAM 的不一致性埋下伏笔。

EKF-SLAM 的基本原理与不一致性表现

EKF 通过线性化非线性系统方程和测量方程，近似实现对非线性状态的递归估计，其核心步骤包括预测和更新：

预测阶段：基于运动模型预测机器人位姿，并根据协方差传播定律更新状态不确定性；

更新阶段：利用传感器测量修正预测状态，通过卡尔曼增益融合测量信息与预测不确定性。

EKF-SLAM 的不一致性主要表现为两种形式：

滤波发散：估计误差随时间累积并超出协方差矩阵的边界，导致机器人位姿或路标位置的估计严重偏离真实值；

协方差失配：估计协方差矩阵无法反映真实误差的统计特性，例如实际定位误差已显著增大，但协方差仍保持较小值，使系统丧失对误差的预警能力。

从可观测性角度看，这些现象的本质是 EKF 的线性化近似破坏了系统真实的可观测性结构，导致滤波器对状态不确定性的估计与实际可观测性脱节。

可观测性视角下不一致性的成因分析

1. 线性化误差对可观测性的扭曲

EKF 在更新阶段需对测量方程进行一阶泰勒展开（线性化），但 SLAM 系统的测量模型（如相机投影、激光测距）通常具有强非线性。线性化误差会导致可观测性矩阵的近似偏差：实际系统中不可观测的状态维度，在 EKF 的线性化模型中可能被误判为可观测，或反之。

例如，当机器人在平面内运动并观测远处路标时，测量方程的非线性程度随距离增加而增强。EKF 的线性化处理可能高估观测对路标深度的修正能力（即错误提升该维度的可观测性），导致更新后的协方差矩阵过度缩小，而实际估计误差并未按预期降低，最终引发不一致性。

2. 不可观测子空间的协方差错误收缩

SLAM 系统存在固有的不可观测子空间，例如全局坐标系下的绝对位置和方向（除非引入外部参考，如 GPS）。这些子空间的状态误差无法通过传感器测量消除，理论上其协方差应保持恒定或仅受噪声影响。

但 EKF 的更新机制可能错误地对不可观测子空间的协方差进行收缩：卡尔曼增益的计算基于线性化的可观测性矩阵，若该矩阵错误包含不可观测维度的 “伪观测信息”，滤波器会误以为这些维度可通过测量修正，从而不合理地减小其协方差。例如，在无全局定位的室内 SLAM 中，EKF 可能因线性化误差错误地降低机器人绝对位置的协方差，导致系统对累积漂移的估计过于乐观。

3. 运动与观测耦合的可观测性缺失

SLAM 的可观测性不仅取决于传感器类型，还与机器人的运动轨迹紧密相关（即主动可观测性）。当机器人运动轨迹无法提供足够的激励（如匀速直线运动）时，系统可观测性矩阵的秩会降低，导致部分状态维度无法被有效观测。

EKF 虽能通过协方差矩阵反映不确定性，但无法主动识别轨迹导致的可观测性不足。例如，机器人沿直线重复往返时，路标在运动方向上的坐标估计可能因观测信息冗余而出现协方差虚降，而实际误差因轨迹约束无法被修正，最终引发不一致性。

基于可观测性的不一致性改进策略

1. 保持可观测性结构的滤波设计

通过重构 EKF 的状态空间，显式分离可观测子空间与不可观测子空间，避免对不可观测维度的错误更新。例如，采用相对坐标系而非全局坐标系定义状态（如以机器人初始位姿为原点），可消除全局位置的不可观测性，使 EKF 的协方差估计更贴合实际可观测性。

2. 非线性可观测性分析的引入

利用微分几何工具（如李导数）分析 SLAM 系统的非线性可观测性，替代 EKF 依赖的线性化可观测性矩阵。通过预先识别不可观测子空间，在滤波更新中限制对这些维度的协方差调整，从理论上避免协方差的错误收缩。

3. 基于轨迹优化的可观测性增强

通过规划机器人运动轨迹（如加入转向、变速等激励动作）提升系统可观测性，确保 EKF 能获得足够的观测信息区分状态维度。例如，在自主导航中引入随机轨迹扰动，避免长时间匀速直线运动导致的可观测性退化。

4. 鲁棒滤波与协方差校正

针对线性化误差，采用鲁棒 EKF 变体（如无味卡尔曼滤波器 UKF、粒子滤波器）减少非线性近似误差；同时引入协方差校正机制，通过在线估计真实误差与协方差的偏差，动态调整滤波器增益，使协方差矩阵更真实地反映可观测性约束下的不确定性。