斯坦福大学AA274A课程《机器人自主性原理》2019年秋季学期加场讲座：粒子滤波SLAM

（原标题：Stanford AA274A Principles of Robotic Autonomy | Autumn 2019 | Extra Session - Particle Filter SLAM）

🎯 核心概述

粒子滤波SLAM（Simultaneous Localization and Mapping）是一种混合架构算法，旨在解决高维状态空间中的同时定位与建图问题。该算法利用粒子集表示机器人轨迹的后验分布，以处理非高斯和多模态特性；同时利用扩展卡尔曼滤波器（EKF）表示地图特征位置的后验分布，以保持计算效率。基于SLAM问题的内在条件独立性属性，全后验分布被精确分解为轨迹后验与多个独立特征后验的乘积。这种分解策略有效规避了朴素粒子滤波因维度灾难导致的计算不可行性，实现了多模态分布处理与计算效率之间的平衡，并通过重采样机制增强了算法对数据关联错误的鲁棒性。

📐 数学基础与架构设计

• 混合表示架构：算法采用分层结构，其中机器人位姿（轨迹）由粒子集近似，每个粒子携带一个完整的地图估计；而地图中的每个特征点则由独立的EKF进行高斯分布建模。这种设计利用了轨迹的非高斯特性和特征位置的高斯特性。
• 后验分解原理：基于贝叶斯规则和马尔可夫假设，全SLAM后验 $P(x_t, X_t | z_{1:t}, u_{1:t})$ 被证明可以分解为轨迹后验 $P(x_t | z_{1:t}, u_{1:t})$ 与 $N$ 个独立特征后验 $\prod_{i=1}^N P(x_i | x_t, z_{1:t}, u_{1:t})$ 的乘积。这一分解将高维联合分布问题转化为低维子问题的集合。
• 维度灾难规避：直接对包含轨迹和所有特征的完整状态空间使用粒子滤波需要指数级数量的粒子，这在计算上是不可行的。通过条件独立性分解，算法仅需维护 $M$ 个粒子（代表轨迹）和每个粒子对应的 $N$ 个EKF滤波器，总计算复杂度从指数级降低至线性或准线性级别。
• 状态空间定义：系统状态由机器人位姿向量 $x_t$ 和地图特征集合 $X_t = \{x_1, ..., x_N\}$ 组成。观测模型假设测量值仅依赖于当前机器人位姿和被观测的特征位置，这一局部性假设是分解成立的关键前提。

⚙️ 算法执行流程

• 预测步骤（Prediction）：利用控制输入 $u_t$ 和状态转移模型对每个粒子的轨迹进行传播。此阶段对应卡尔曼滤波的预测步，仅使用运动模型信息，尚未引入新的测量数据。此时，各特征EKF的状态保持不变，等待观测更新。
• 特征更新步骤（Feature Update）：对于当前时刻的观测 $z_t$，算法检查其与每个粒子所维护的特征集的匹配情况。若某测量与特定特征匹配，则执行标准EKF更新，利用线性化测量模型修正该特征的均值和协方差；若不匹配，则该特征的EKF参数保持不变。此步骤实现了地图信息的增量式更新。
• 权重计算与重采样（Resampling）：根据最新观测值计算每个粒子的重要性权重 $W_t$。权重定义为包含最新测量的目标分布与当前粒子分布之比，简化后即为测量似然 $P(z_t | x_t)$。随后执行重采样步骤，依据权重从当前粒子集中生成新的粒子集，使粒子分布对齐最新观测，避免粒子退化。
• 关联历史维护：每个粒子独立维护其数据关联历史（correspondence history）。这意味着不同粒子可能对应不同的特征匹配假设。这种独立性是算法鲁棒性的核心来源，允许系统同时探索多种可能的地图结构解释。

🛡️ 鲁棒性与优势分析

• 多模态分布处理：与扩展卡尔曼滤波器（EKF）等基于高斯假设的滤波方法不同，粒子滤波能够自然表示和处理多模态后验分布。这在初始定位不确定或存在多个等价地图解释时至关重要，避免了单一均值估计导致的错误收敛。
• 数据关联错误容错：传统EKF依赖单一关联历史，一旦发生错误的特征匹配（data association error），滤波器状态可能整体发散，通常需要硬重置机制。相比之下，粒子滤波中若某粒子的关联历史导致其与观测不兼容，其权重将显著降低，该粒子将在重采样过程中自然淘汰。这种“优雅降级”机制增强了算法在复杂环境中的稳定性。
• 全局定位能力：由于不依赖局部线性化假设的严格有效性，粒子滤波SLAM具备更强的全局收敛能力，适用于大范围导航和初始位姿不确定性较高的场景。
• 计算效率平衡：通过混合架构，算法保留了粒子滤波对非高斯轨迹的处理能力，同时利用EKF的高效性处理特征更新，避免了全状态空间粒子滤波的巨大计算开销。

⚠️ 局限性与挑战

• 维度扩展限制：尽管分解策略缓解了维度灾难，粒子滤波仍难以扩展到极高维状态空间（如包含大量动态对象或密集地图特征的场景）。随着特征数量 $N$ 增加，每个粒子的EKF维护成本线性增长，总计算量可能成为瓶颈。
• Particle Depletion (粒子饥饿)：在重采样过程中，若权重分布过于集中，可能导致大部分粒子被复制自少数高权重粒子，造成样本多样性丧失（particle depletion）。这要求采用更巧妙的采样策略或自适应重采样机制来维持粒子集的有效性。
• Computational Cost：相比纯EKF方法，粒子滤波SLAM的计算成本显著更高，主要源于需要维护 $M \times N$ 个滤波器实例和执行大规模重采样操作。这对实时嵌入式系统提出了更高的硬件要求。

📈 技术演进与对比

• 高斯滤波 vs. 粒子滤波：EKF适用于低不确定性、单模态且局部线性化有效的场景，具有计算高效的优势；而粒子滤波适用于全局定位、多模态分布及存在显著数据关联不确定性的场景，以更高的计算成本换取更强的鲁棒性。
• SLAM发展阶段：SLAM技术演进经历了从基于EKF的经典阶段（关注基本框架建立），到算法分析阶段（深入理解收敛性与误差传播），最终进入强调鲁棒性保证的感知阶段（处理复杂现实世界噪声与异常）。粒子滤波SLAM处于这一演进的关键节点，代表了从理想化假设向实际鲁棒应用的过渡。
• 课程后续方向：在掌握基础SLAM算法后，技术重点将转向状态机管理与决策模块集成，旨在构建完整的自主导航系统。评估方式侧重于基础概念理解与判断能力，强调对核心原理的掌握而非复杂推导。