基于RBPF框架的FastSLAM 1.0与2.0对比仿真系统

资 源 简 介

本项目在MATLAB开发环境下完整实现了FastSLAM 1.0和FastSLAM 2.0两种经典的同时定位与地图构建（SLAM）算法的仿真演示。系统利用Rao-Blackwellized粒子滤波器（RBPF）框架，将复杂的SLAM分解为机器人路径估计与地图路标估计两个独立部分。其核心功能包括模拟机器人在带有随机分布路标点的二维平面内移动，并利用传感器获取环境观测信息。FastSLAM 1.0部分实现了基于运动学模型的粒子采样，为每个粒子维护一组独立的扩展卡尔曼滤波器（EKF）来更新地图路标；FastSL

详 情 说 明

FastSLAM 1.0 与 FastSLAM 2.0 算法仿真系统

项目介绍

本系统是一个基于 MATLAB 开发的机器人同时定位与地图构建（SLAM）仿真平台。它实现了粒子滤波框架下的两种核心算法：FastSLAM 1.0 和 FastSLAM 2.0。通过模拟机器人并在带有已知路标点的环境中运动，该系统直观地展示了如何利用 Rao-Blackwellized 粒子滤波器（RBPF）将高维 SLAM 问题分解为机器人轨迹估计（通过粒子滤波）和路标位置估计（通过每个粒子维护的独立扩展卡尔曼滤波器 EKF）两个子问题。

功能特性

• 双算法并行对比：在同一仿真环境下同时运行 FastSLAM 1.0 和 2.0，直接对比两者的收敛速度与定位精度。
• 运动与观测模拟：集成标准的机器人非线性运动模型（控制线速度与角速度）和带有噪声的传感器观测模型（距离与方位角）。
• 传感器感知限制：模拟真实传感器的探测距离上限（10.0单位）和探测视场角（180度），仅更新视野内的路标。
• 进阶建议分布：FastSLAM 2.0 实现了引入最新测量值来优化粒子采样建议分布的功能，显著提高了采样效率。
• 动态可视化：实时生成轨迹对比图、粒子群分布动态演化以及机器人位姿估计的均方根误差（RMSE）曲线。
• 数据关联处理：内部建立了基于索引的路标识别机制，确保观测值与地图点之间的准确匹配。

使用方法

1. 环境配置：确保您的计算机上已安装 MATLAB（推荐 R2016b 或更高版本），并安装了 Statistics and Machine Learning Toolbox（用于处理概率分布函数）。
2. 运行仿真：将代码保存并在 MATLAB 当前文件夹中打开。在命令行窗口直接键入 main 并回车。
3. 结果观察：程序将弹出一个双子图窗口。左侧展示机器人轨迹、路标点及粒子分布的实时动态；右侧展示 FastSLAM 1.0 与 2.0 随时间变化的 RMSE 误差曲线。
4. 参数调整：用户可以直接修改仿真参数设置部分，如改变粒子数量（N_particles）、调整运动/观测噪声（Q, R）或增加探测范围（max_range）。

系统要求

• 软件平台：MATLAB R2016b 及以上版本。
• 依赖工具箱：Statistics and Machine Learning Toolbox。
• 硬件建议：粒子数设为 30-50 时，普通办公电脑即可流畅运行。

核心功能实现逻辑

本项目模拟了一个时长 60 秒、步长 0.1 秒的移动机器人 SLAM 过程。

1. 状态与环境初始化：

系统首先定义了 15 个随机分布的静态路标点作为真实环境。初始状态下，FastSLAM 1.0 和 2.0 分别维护一个包含 30 个粒子的粒子群。每个粒子记录当前机器人的位姿（x, y, theta）和一组用于估计路标位置的均值与协方差矩阵。

1. 运动预测与真实状态更新：

在每一时间步，机器人根据既定的线速度（含正弦波动）和角速度进行运动。真实位姿通过运动学方程精确计算。而粒子群则根据带有噪声的控制量进行预测采样。

1. 环境观测生成：

系统计算机器人与所有路标之间的几何关系。只有在最大探测距离内且在 180 度视野范围内的路标才会被生成带噪声的观测向量（包括欧氏距离和相对角度）。

1. FastSLAM 1.0 更新逻辑：

对于每个粒子，程序首先基于运动模型进行盲采样（不考虑当前观测）。接收到观测值后，系统利用 EKF 更新对应路标点的状态。粒子的权重根据观测值在预测分布下的似然概率（mvnpdf）进行更新。

1. FastSLAM 2.0 更新逻辑：

这是系统的核心改进点。在采样阶段，2.0 算法不仅使用运动模型，还结合了当前的观测信息。通过线性化观测方程并计算建议分布，生成一个更贴合真实后验分布的采样。随后计算增益权重，并按照与 1.0 类似的方式更新地图路标的均值和方差。

1. 重采样机制：

系统实时计算粒子的有效权重。当有效粒子数下降到总数的一半以下时，触发系统性重采样。这一机制通过复制高权重粒子、剔除低权重粒子，有效地解决了粒子耗散问题，保持了粒子群的多样性与有效性。

算法实现分析

• 建议分布优化（FastSLAM 2.0）：相比于 1.0 算法直接从预测分布采样，2.0 算法通过 Jacobian 矩阵计算观测对位姿的影响，利用当前观测值调整采样的位置，使粒子更聚集于真实位姿附近。
• 坐标变换与雅可比计算：代码中实现了专门的函数用于计算观测模型对路标状态以及机器人位姿的 Jacobian 矩阵。这些矩阵是 EKF 地图更新及建议分布优化的数学基础。
• 角度标准化处理：在所有涉及机器人朝向和观测角度的计算中，系统通过专门函数将角度严格限制在 [-pi, pi] 范围内，避免了角度跳变导致的滤波器发散。
• 评估指标：系统通过计算每个时间步估计轨迹与真实轨迹之间的欧氏距离，生成 RMSE 曲线，直观证明了 FastSLAM 2.0 在相同粒子数下具有更优的定位性能。