# Matlab_Slam_Sim **Repository Path**: fourzkw/Matlab_Slam_Sim ## Basic Information - **Project Name**: Matlab_Slam_Sim - **Description**: No description available - **Primary Language**: Unknown - **License**: Not specified - **Default Branch**: main - **Homepage**: None - **GVP Project**: No ## Statistics - **Stars**: 0 - **Forks**: 0 - **Created**: 2025-11-04 - **Last Updated**: 2025-11-05 ## Categories & Tags **Categories**: Uncategorized **Tags**: None ## README # MATLAB SLAM 仿真系统 [![MATLAB](https://img.shields.io/badge/MATLAB-R2020a+-blue.svg)](https://www.mathworks.com/products/matlab.html) [![License](https://img.shields.io/badge/License-Educational-green.svg)](LICENSE) --- ## 目录 - [项目简介](#项目简介) - [快速开始](#快速开始) - [配置说明](#配置说明) - [项目结构](#项目结构) - [算法详细说明](#算法详细说明) - [核心算法详解](#核心算法详解) - [算法性能对比](#算法性能对比) - [参数调优指南](#参数调优指南) - [更新日志](#更新日志) - [许可证](#许可证) --- ## 项目简介 本项目实现了四种经典SLAM算法的二维仿真系统，基于激光雷达传感器模型： **特征地图SLAM**（从激光扫描提取特征点）: - **EKF-SLAM**: 扩展卡尔曼滤波SLAM - **Graph-SLAM**: 图优化SLAM - **FastSLAM**: 粒子滤波SLAM **栅格地图SLAM**（基于原始激光扫描）: - **Occupancy Grid + MCL**: 占据栅格建图与蒙特卡洛定位 ### 主要特性 - **统一配置系统** - 集中管理所有参数（`common/get_slam_config.m`） - **统一传感器模型** - 360度激光雷达，120束，最大15米探测距离 - **统一环境地图** - 50x50米场景，包含9个障碍物 - **轨迹一致性保证** - 对比运行时所有算法使用相同真实轨迹 - **平滑运动控制** - 渐进式角速度调整，避免急转弯和走回头路 - **运行时轨迹选择** - 启动时可选择固定路径或动态避障模式 - **完整对比框架** - 包含轨迹误差、运行时间、综合性能评分 - **实时可视化** - 详细进度显示与多维度性能分析 --- ## 快速开始 ### 环境要求 - **MATLAB** R2020a 或更高版本 - 推荐安装工具箱： - Statistics and Machine Learning Toolbox - Optimization Toolbox（可选） ### 单独运行算法 每个算法启动时都会提示选择轨迹模式： - **选项 1**: 固定路径点轨迹（可重复，适合对比） - **选项 2**: 动态避障轨迹（随机探索） #### EKF-SLAM ```matlab cd ekf_slam main_ekf_slam % 提示：请选择轨迹模式 [1/2] (默认1): ``` #### Graph-SLAM ```matlab cd graph_slam main_graph_slam ``` #### FastSLAM ```matlab cd fastslam main_fastslam ``` #### 占据栅格SLAM ```matlab cd occupancy_grid main_grid_slam ``` ### 算法对比 ```matlab cd compare compare_algorithms % 提示：请选择轨迹模式 [1/2] (默认1): % 选项1: 固定路径点轨迹 - 所有算法使用相同预生成轨迹 % 选项2: 动态避障轨迹 - 第一个算法实时生成，后续共享 ``` **对比结果包括**： | 指标 | 说明 | |-----|------| | **轨迹对比** | 真实轨迹 vs 各算法估计轨迹 | | **路标地图** | 特征点分布与估计精度 | | **误差分析** | 平均误差、最大误差统计 | | **运行时间** | 各算法执行时间对比 | | **效率分析** | 精度-速度权衡散点图 | | **综合评分** | 精度50% + 速度30% + 建图20% | | **结果保存** | `comparison_results.mat` | **动态模式轨迹一致性保证**： - EKF-SLAM（第一个）实时生成真实轨迹 - Graph-SLAM、FastSLAM、Grid+MCL 使用相同轨迹 - 确保公平对比，所有算法面对相同观测条件 --- ## 配置说明 ### 轨迹配置 #### 方式 1：运行时选择（推荐） 启动任何主程序时，系统会提示选择轨迹模式： ``` 轨迹模式选择: 1. 固定路径点轨迹（可重复，适合对比） 2. 动态避障轨迹（随机探索） 请选择轨迹模式 [1/2] (默认1): ``` #### 方式 2：配置文件调整 编辑 `common/get_slam_config.m` 可修改轨迹参数： ##### 固定路径点轨迹 ```matlab config.use_fixed_trajectory = true; config.waypoints = [ 25, 45, 45, 15, 15; % x坐标 25, 25, 45, 45, 15 % y坐标 ]; ``` **特点**： - 可重复性好，适合算法对比 - 轨迹固定，结果可复现 ##### 动态避障轨迹 ```matlab config.use_fixed_trajectory = false; config.v_base = 1; % 基础线速度（m/s） config.omega_base = 0.08; % 基础角速度（rad/s） ``` **特点**： - 随机初始方向，每次运行不同 - 平滑运动控制，避免急转弯 - 自动边界和障碍物避让 - 探索性强，适合测试鲁棒性 **动态轨迹特性**： | 特性 | 说明 | |-----|------| | **平滑性** | 渐进式角速度调整（±30%微调）| | **稳定性** | 方向保持60步后才调整 | | **限速** | 最大角速度±0.2 rad/s（约±11.5°/s）| | **避免回头** | 限制最大转角120度 | | **安全距离** | 边界和障碍物保持1.5米 | ### 其他参数 ```matlab % 时间参数 config.dt = 0.1; % 时间步长（秒） config.total_time = 100; % 总仿真时间（秒） % 过程噪声（运动模型） config.Q = diag([0.005, 0.005, 0.002]).^2; % 观测噪声（激光雷达） config.R = diag([0.15, 0.05]).^2; % [距离, 角度] % 激光雷达参数 config.laser_range = 15; % 最大探测距离（米） config.laser_fov = 2*pi; % 视场角（弧度） config.num_beams = 120; % 激光束数量 config.laser_noise = 0.1; % 测距噪声标准差 % 算法特定参数 config.fastslam.N_particles = 50; config.graph_slam.n_iterations = 20; ``` --- ## 项目结构 ``` Matlab_Slam/ ├── common/ # 共享模块 │ ├── get_slam_config.m # 统一配置文件 │ ├── generate_landmarks.m # 环境地图生成 │ ├── generate_waypoint_trajectory.m # 固定路径点轨迹 │ ├── generate_true_trajectory.m # 动态避障轨迹（平滑控制） │ ├── safe_motion_controller.m # 安全运动控制器（避障+平滑） │ ├── motion_model.m # 运动模型 │ ├── observation_model_laser.m # 激光雷达观测模型 │ ├── data_association.m # 数据关联算法（马氏距离） │ ├── extract_features_from_scan.m # 特征提取算法 │ ├── normalize_angle.m # 角度归一化 │ └── simulate_with_fixed_trajectory.m # 固定轨迹仿真 │ ├── ekf_slam/ # EKF-SLAM算法 │ ├── main_ekf_slam.m # 主程序 │ ├── ekf_predict.m # 预测步骤 │ ├── ekf_update.m # 更新步骤 │ └── initialize_landmark.m # 路标初始化 │ ├── graph_slam/ # Graph-SLAM算法 │ ├── main_graph_slam.m # 主程序 │ ├── optimize_pose_graph.m # 位姿图优化 │ ├── estimate_landmark_position.m # 路标位置估计 │ └── plot_graph_slam.m # 可视化 │ ├── fastslam/ # FastSLAM算法 │ ├── main_fastslam.m # 主程序 │ ├── initialize_particles.m # 粒子初始化 │ ├── particle_predict.m # 预测步骤 │ ├── particle_update.m # 更新步骤 │ ├── resample_particles.m # 重采样 │ └── estimate_state.m # 状态估计 │ ├── occupancy_grid/ # 占据栅格SLAM │ ├── main_grid_slam.m # 主程序 │ ├── update_occupancy_grid.m # 地图更新 │ ├── initialize_mcl_particles.m # MCL初始化 │ ├── mcl_predict.m # MCL预测 │ ├── mcl_update.m # MCL更新 │ ├── simulate_laser_scan.m # 激光扫描仿真 │ └── check_collision.m # 碰撞检测 │ ├── compare/ # 算法对比工具 │ ├── compare_algorithms.m # 对比主程序（轨迹一致性保证） │ ├── run_ekf_slam.m # EKF-SLAM对比版 │ ├── run_graph_slam.m # Graph-SLAM对比版 │ ├── run_fastslam.m # FastSLAM对比版 │ ├── run_grid_slam.m # Grid-SLAM对比版 │ ├── plot_comparison.m # 对比可视化（12子图全面分析） │ └── generate_true_trajectory.m # 对比用真实轨迹生成 │ ├── README.md # 项目文档 └── 项目功能及算法实现清单.md # 详细实现清单 ``` --- ## 算法详细说明 ### 1. EKF-SLAM（扩展卡尔曼滤波SLAM） #### 算法原理 EKF-SLAM是最经典的SLAM算法之一，基于扩展卡尔曼滤波（EKF）框架，同时估计机器人位姿和环境中的路标位置。算法将机器人位姿和所有路标位置联合表示为一个状态向量，并维护一个协方差矩阵来表示不确定性。 **状态向量表示**： ``` x = [x_r, y_r, θ_r, x_1, y_1, x_2, y_2, ..., x_N, y_N]^T ``` 其中： - `(x_r, y_r, θ_r)`: 机器人位姿（位置和朝向） - `(x_i, y_i)`: 第i个路标的位置 - `N`: 路标总数 **数学基础**： 1. **预测步骤**：根据运动模型预测下一时刻状态 - 状态预测：`x̂_k|k-1 = f(x_k-1, u_k) + w_k` - 协方差更新：`P_k|k-1 = F_k P_k-1 F_k^T + Q_k` 2. **更新步骤**：根据观测修正预测 - 卡尔曼增益：`K_k = P_k|k-1 H_k^T (H_k P_k|k-1 H_k^T + R_k)^(-1)` - 状态更新：`x̂_k = x̂_k|k-1 + K_k(z_k - h(x̂_k|k-1))` - 协方差更新：`P_k = (I - K_k H_k) P_k|k-1` #### 核心流程 1. **预测阶段** - 根据控制输入和运动模型预测机器人位姿 - 路标位置保持不变（静态假设） - 更新状态协方差矩阵 2. **数据关联** - 使用马氏距离匹配观测与已知路标 - 阈值判断：如果距离小于阈值，则关联成功 - 马氏距离：`d = sqrt((z - ẑ)^T S^(-1) (z - ẑ))` 3. **更新阶段** - 对每个成功关联的观测进行EKF更新 - 同时更新机器人位姿和路标位置估计 - 更新协方差矩阵（减小不确定性） 4. **路标初始化** - 未匹配的观测被视为新路标 - 使用逆观测模型初始化路标位置 - 扩展状态向量和协方差矩阵 #### 优点 - **实时性好** - 递归滤波，每个时间步只需处理当前观测 - **理论成熟** - 有完整的数学推导和理论保证 - **精度高** - 在小规模场景中能提供准确的估计 - **不确定性量化** - 协方差矩阵提供明确的不确定性信息 #### 缺点 - **计算复杂度高** - O(N²)，N为路标数量，不适合大规模场景 - **线性化误差** - EKF假设线性化误差小，对于强非线性系统误差较大 - **数据关联敏感** - 错误的数据关联会导致滤波发散 - **不支持回环闭合** - 无法处理全局一致性问题 #### 适用场景 - 小规模室内环境（路标数量 < 100） - 实时应用（机器人导航、无人车等） - 传感器噪声较小的场景 - 需要不确定性估计的应用 #### 实现细节 本项目中的关键参数： ```matlab Q = diag([0.005, 0.005, 0.002]).^2; % 运动噪声 R = diag([0.15, 0.05]).^2; % 观测噪声 [距离, 角度] ``` --- ### 2. Graph-SLAM（图优化SLAM） #### 算法原理 Graph-SLAM将SLAM问题建模为一个图优化问题。图中的节点表示机器人在不同时刻的位姿，边表示位姿之间的约束关系（来自运动模型或观测）。通过最小化全局约束误差，同时优化所有位姿和路标位置。 **图结构**： - **节点**：机器人位姿 `x_0, x_1, ..., x_T` - **边**： - 里程计边：连接相邻位姿，约束来自运动模型 - 观测边：连接位姿与路标，约束来自传感器观测 - 回环边：连接相隔较远但实际接近的位姿 **优化目标**： ``` min Σ ||f(x_i, u_i) - x_{i+1}||²_Ωu + Σ ||h(x_i, m_j) - z_{ij}||²_Ωz ``` 其中： - `f()`: 运动模型 - `h()`: 观测模型 - `Ωu, Ωz`: 信息矩阵（协方差的逆） #### 核心流程 1. **前端数据处理** - 收集所有位姿节点（从运动模型） - 提取观测特征并进行数据关联 - 构建位姿图（节点和边） 2. **路标初始化** - 从多个观测三角化计算路标位置 - 使用最小二乘法求解过约束问题 - 统计初始估计误差 3. **后端图优化** - 使用Gauss-Newton或Levenberg-Marquardt迭代 - 构建稀疏Hessian矩阵（利用图的稀疏性） - 求解线性系统：`H Δx = -b` - 更新状态：`x ← x + Δx` 4. **迭代优化** - 计算当前误差（所有边的误差平方和） - 如果误差下降，接受更新并继续 - 如果收敛（误差变化 < 阈值），停止迭代 #### 优点 - **精度最高** - 全局批量优化，能充分利用所有观测信息 - **支持回环闭合** - 能处理全局一致性，消除累积误差 - **稀疏性** - 利用图的稀疏结构，计算效率高 - **鲁棒性好** - 对局部误差不敏感，能从整体优化 #### 缺点 - **非实时** - 需要收集所有数据后进行批量优化 - **内存占用大** - 需要存储所有历史位姿 - **依赖初值** - 非凸优化，可能陷入局部最优 - **计算量大** - 迭代优化需要多次矩阵运算 #### 适用场景 - 大规模室内外环境建图 - 离线地图构建（测绘、建筑建模等） - 需要高精度地图的应用 - 存在回环的长轨迹场景 #### 实现细节 本项目中的关键参数： ```matlab n_iterations = 20; % 优化迭代次数 step_size = 0.5; % 梯度下降步长 lambda_pose = 1000; % 位姿约束权重 lambda_obs = 100; % 观测约束权重 ``` **优化技巧**： - 使用信息矩阵加权不同类型的约束 - 自适应步长调整 - 早停策略（误差变化 < 1e-6） --- ### 3. FastSLAM（粒子滤波SLAM） #### 算法原理 FastSLAM是基于粒子滤波（Particle Filter）的SLAM算法。它利用了SLAM问题的关键特性：**给定机器人轨迹，不同路标之间是条件独立的**。因此，FastSLAM用粒子表示位姿分布，每个粒子内部独立维护各个路标的EKF估计。 **状态表示**： ``` 粒子 i: { pose: (x_i, y_i, θ_i) // 机器人位姿 weight: w_i // 粒子权重 landmarks: [EKF_1, EKF_2, ..., EKF_N] // 每个路标的EKF } ``` **Rao-Blackwellization分解**： ``` P(x_{1:t}, m | z_{1:t}, u_{1:t}) = P(x_{1:t} | z_{1:t}, u_{1:t}) × P(m | x_{1:t}, z_{1:t}) ↓ ↓ ↓ 完整后验 粒子滤波 EKF（条件独立） ``` #### 核心流程 1. **粒子预测** - 对每个粒子，根据运动模型采样新位姿 - 添加运动噪声：`x_i ~ p(x_t | x_{t-1}, u_t)` - 粒子轨迹逐步扩展 2. **数据关联**（每个粒子独立） - 使用马氏距离匹配观测与粒子内的路标 - 不同粒子可能有不同的数据关联结果 - 提高鲁棒性，避免单一错误关联导致失败 3. **路标更新**（粒子内EKF） - 对匹配的路标，使用标准EKF更新其位置估计 - 更新路标的均值和协方差 - 新观测到的路标初始化为新EKF 4. **重要性权重计算** - 根据观测似然度更新粒子权重 - `w_i ∝ p(z_t | x_i, m_i)` - 归一化权重：`Σ w_i = 1` 5. **重采样** - 当有效粒子数降低时触发 - 根据权重重新采样粒子（高权重粒子被复制） - 避免粒子退化问题 6. **状态估计** - 加权平均：使用所有粒子的加权平均作为最终估计 - 或选择最高权重粒子 #### 优点 - **非高斯分布** - 粒子可以表示多峰分布，适应复杂场景 - **数据关联鲁棒** - 不同粒子可以尝试不同的关联假设 - **计算复杂度** - O(M·log N)，M为粒子数，比EKF-SLAM更适合大规模场景 - **并行化潜力** - 粒子之间相互独立，易于并行计算 #### 缺点 - **粒子退化** - 随时间推移，只有少数粒子有效 - **重采样损失** - 重采样会丢失粒子多样性 - **粒子数要求** - 需要足够多的粒子才能准确表示分布 - **计算量仍较大** - 每个粒子都要维护完整的路标集 #### 适用场景 - 中等规模环境（路标数 < 500） - 存在数据关联歧义的场景 - 多模态分布场景（多个可能的位置） - 需要鲁棒性的应用 #### 实现细节 本项目中的关键参数： ```matlab N_particles = 50; % 粒子数量 resample_threshold = 0.5; % 重采样阈值（有效粒子比例） ``` **有效粒子数**： ```matlab N_eff = 1 / Σ(w_i²) ``` 当 `N_eff < N_particles × resample_threshold` 时触发重采样。 --- ### 4. Occupancy Grid SLAM + MCL（占据栅格建图与蒙特卡洛定位） #### 算法原理 Occupancy Grid SLAM将环境表示为栅格地图，每个栅格存储该位置被占据的概率。结合蒙特卡洛定位（MCL）进行机器人位姿估计。与基于特征点的SLAM不同，它直接使用原始激光扫描数据，无需特征提取。 **栅格地图表示**： ``` map[i][j] = log-odds(占据概率) log-odds = log(P / (1 - P)) ``` - 优点：可以逐步更新，避免概率0或1的数值问题 - 转换：`P = 1 / (1 + exp(-log-odds))` **MCL定位**： 使用粒子滤波估计机器人位姿，每个粒子表示一个可能的位置和朝向。 #### 核心流程 1. **MCL预测** - 根据运动模型采样粒子位姿 - 添加运动噪声增加多样性 - 粒子云随机器人移动而移动 2. **MCL更新** - 对每个粒子，计算其观测似然度 - 将粒子的激光扫描投影到地图 - 权重 ∝ 观测与地图的匹配程度 - `w_i = Π p(z_k | map, x_i)` 3. **位姿估计** - 加权平均计算最优位姿估计 - 或使用最高权重粒子 4. **占据栅格更新** - 使用估计位姿和激光扫描更新地图 - **逆传感器模型**： ``` P(occupied | z) = { 高 如果点在激光端点附近（障碍物） 低 如果点在激光路径上（自由空间） 0.5 如果点在未观测区域（未知） } ``` - Log-odds更新： ``` l_{t+1}[i][j] = l_t[i][j] + log(P(occ|z) / P(free|z)) - l_0 ``` 5. **重采样** - 根据粒子权重重采样 - 防止粒子退化 #### 优点 - **无需特征提取** - 直接使用原始传感器数据 - **密集地图** - 能表示任意形状的环境，不局限于特征点 - **适应动态环境** - 栅格可以逐步更新，适应环境变化 - **易于理解** - 栅格地图直观，便于可视化和路径规划 #### 缺点 - **内存占用大** - 栅格地图需要大量存储空间 - **分辨率受限** - 栅格分辨率影响精度和内存 - **定位精度一般** - MCL的精度取决于粒子数和地图质量 - **计算量大** - 每个粒子都要进行激光投影计算 #### 适用场景 - 室内结构化环境（建筑物、仓库等） - 密集障碍物环境 - 需要详细环境表示的应用 - 路径规划和导航任务 #### 实现细节 本项目中的关键参数： ```matlab map_size = 50; % 地图尺寸（米） resolution = 0.2; % 栅格分辨率（米） N_particles = 100; % MCL粒子数 ``` **地图更新**： - `l_occ = 0.9`：观测到障碍物时的log-odds增量 - `l_free = -0.7`：观测到自由空间时的log-odds增量 - `l_0 = 0`：先验（未知状态） **激光投影**： 使用Bresenham算法进行光线追踪，标记自由空间和占据空间。 --- ## 核心算法详解 ### 特征提取算法 #### 算法名称：激光角点特征提取 **文件**: `common/extract_features_from_scan.m` **算法原理**: 从360度激光扫描数据中提取显著角点特征，用于特征地图SLAM（EKF-SLAM、Graph-SLAM、FastSLAM）。 **核心步骤**: 1. **扫描数据预处理** ```matlab 有效扫描过滤: - 距离范围: min_range < r < max_range - 击中标记: 必须击中障碍物 - 最小数量: 至少3个有效点 ``` 2. **角点检测（距离突变检测）** ```matlab for i = 3:3:length(scan) % 每3个点采样一次 diff_left = |scan(i) - scan(i-2)| diff_right = |scan(i) - scan(i+2)| if diff_left > threshold OR diff_right > threshold: 标记为角点候选 ``` - **阈值**: 0.8米（距离突变检测） - **窗口**: ±2个点 - **采样**: 每3个点检测一次（减少计算量） 3. **角点密度控制** ```matlab 最小角度间隔: 20个索引（约60度） 最小空间距离: 0.5米 最大特征数: 8个 ``` 4. **空间去重** ```matlab for each 候选角点: 计算到已选特征的空间距离 if 距离 < min_spatial_distance: 舍弃该候选 else: 添加到特征集 ``` 5. **特征分布优化** - **贪心算法**: 选择空间分布最均匀的特征子集 - **策略**: 每次选择距离已选特征最远的点 ```matlab 初始: 选择中等距离的点 迭代: 添加最分散的点 目标: 最大化特征间最小距离 ``` **输出格式**: ```matlab features = [range_1, range_2, ..., range_N; bearing_1, bearing_2, ..., bearing_N] 极坐标表示: (距离, 方位角) ``` **算法特点**: - ✅ 自适应密度控制 - ✅ 空间分布优化 - ✅ 计算效率高（稀疏采样） - ✅ 鲁棒性强（多重过滤） --- ### 路径规划算法 #### 1. 固定路径点规划 **文件**: `common/generate_waypoint_trajectory.m` **算法原理**: 基于预定义路径点序列，生成平滑的机器人控制轨迹，用于可重复的算法对比。 **核心策略**: 1. **路径点到达控制** ```matlab 对每个路径点: 1. 计算目标方向: target_angle = atan2(dy, dx) 2. 计算角度误差: angle_error = target_angle - current_θ 3. 生成控制量: (v, ω) 4. 判断到达: dist < threshold (0.8m) ``` 2. **线速度控制（自适应）** ```matlab if |angle_error| > 60°: v = v_min × 0.3 (几乎停止，主要转向) elif |angle_error| > 30°: v = v_min (减速转向) else: v = v_min + (v_max - v_min) × (1 - |angle_error|/30°) 接近目标时减速: if dist < 3.0m: v = v × (dist / 3.0) ``` 3. **角速度控制（比例控制）** ```matlab ω = clamp(2.0 × angle_error, -ω_max, ω_max) ω_max = 0.5 rad/s (约28.6°/s) ``` **参数配置**: ```matlab v_max = 1.2 m/s 最大线速度 v_min = 0.4 m/s 最小线速度 ω_max = 0.5 rad/s 最大角速度 arrival_threshold = 0.8 m 到达判定阈值 ``` **默认路径点**: ``` 起点: (25, 25) ↓ (45, 25) → (45, 45) → (15, 45) → (15, 15) 总长度: 约120米 ``` **特点**: - ✅ 平滑转向（角度误差越小速度越快） - ✅ 接近目标自动减速 - ✅ 可重复性好 - ✅ 适合算法对比 --- #### 2. 动态避障路径规划 **文件**: `common/safe_motion_controller.m` **算法原理**: 实时检测障碍物和边界，动态调整运动控制，实现安全探索。 **核心模块**: 1. **碰撞检测（实时）** ```matlab for each 障碍物: 计算机器人到矩形的最近距离 if 距离 < safety_margin (1.5m): 触发避障模式 计算逃离方向 ``` 2. **边界检测** ```matlab 检测四个方向的边界距离: - 左边界: x - x_min < margin - 右边界: x_max - x < margin - 下边界: y - y_min < margin - 上边界: y_max - y < margin 触发条件: 任一方向 < 1.5m ``` 3. **智能避障策略** ```matlab if 检测到碰撞或边界: 1. 计算目标方向（逃离方向） 2. 选择最小转向角度（避免大幅转向） 3. 限制最大转角: 120度 4. 设置避障定时器: 30-50步 5. 减速前进: v = v_base × 0.5 ``` 4. **平滑探索运动** ```matlab 正常模式（每60步调整一次）: 速度微调: v = v_base × (0.9 ~ 1.1) 角速度渐进式调整: 初次: ω = ω_base × (-0.5 ~ 0.5) 后续: ω_new = ω_old + Δω Δω = ω_old × (-0.3 ~ 0.3) [±30%微调] 限制: |ω| < 0.2 rad/s ``` **避障决策流程**: ``` 检测阶段: ├─ 障碍物检测 → 计算逃离方向 ├─ 边界检测 → 选择最小转向 └─ 优先级: 障碍物 > 边界 避障阶段: ├─ 转向: ω = sign(angle_diff) × min(|angle_diff|/dt/10, 0.3) ├─ 减速: v = v_base × 0.5 └─ 计时: timer - 1 正常阶段: ├─ 稳定期: 60步 (6秒) ├─ 速度微调: ±10% └─ 角速度微调: ±30% ``` **关键特性**: - ✅ **平滑性**: 渐进式调整，避免突变 - ✅ **稳定性**: 60步才调整一次方向 - ✅ **安全性**: 实时碰撞检测，1.5米安全距离 - ✅ **智能性**: 选择最小转向角度 - ✅ **限速**: 最大角速度±0.2 rad/s，最大转角120° **参数调优历史**: ``` v1.0: 完全随机角速度，频繁调整 → 运动不平滑 v2.0: 渐进式调整±30%，60步周期 → 平滑稳定 最大角速度±0.2，最大转角120° → 避免急转弯 ``` --- ### 碰撞检测算法 **文件**: `occupancy_grid/check_collision.m` **算法原理**: 计算机器人到所有矩形障碍物的最短距离，判定是否在安全距离内。 **核心算法**: 1. **点到矩形最短距离** ```matlab for each 障碍物矩形 [x_min, y_min, x_max, y_max]: # 找到矩形上最接近机器人的点 closest_x = clamp(robot_x, x_min, x_max) closest_y = clamp(robot_y, y_min, y_max) # 计算距离 dx = robot_x - closest_x dy = robot_y - closest_y dist = sqrt(dx² + dy²) if dist < safety_margin: collision = True ``` 2. **逃离方向计算** ```matlab 情况1: 机器人在矩形外 escape_dir = normalize([dx, dy]) 情况2: 机器人在矩形内 找最近的边: distances = [dist_to_left, dist_to_right, dist_to_bottom, dist_to_top] escape_dir = 垂直于最近边的方向 ``` 3. **多障碍物处理** ```matlab 累加所有碰撞障碍物的逃离方向: total_escape_dir = Σ escape_dir_i 归一化: total_escape_dir / ||total_escape_dir|| ``` **数学原理**: ``` 点到矩形距离（Separating Axis Theorem）: 1. 将点投影到矩形 2. 如果投影点在矩形内 → 距离=0 3. 如果投影点在矩形外 → 距离=点到投影点的距离 ``` **应用场景**: - ✅ 动态避障路径规划 - ✅ 轨迹安全性验证 - ✅ 碰撞预警系统 --- ### 激光扫描仿真算法 **文件**: `occupancy_grid/simulate_laser_scan.m` **算法原理**: 基于光线投射（Ray Casting）仿真360度激光雷达扫描。 **核心算法**: 1. **光线投射主循环** ```matlab for i = 1:num_beams: # 计算光线角度 angle = θ_robot + fov_min + i × (fov / num_beams) # 光线方向向量 direction = [cos(angle), sin(angle)] # 查找最近障碍物 min_dist = max_range for each 障碍物: dist = ray_box_intersection(origin, direction, obstacle) if dist > 0 AND dist < min_dist: min_dist = dist # 添加高斯噪声 scan[i] = min_dist + N(0, σ²) ``` 2. **光线-矩形相交检测** ```matlab 光线参数方程: P(t) = P0 + t × d 其中: P0 = (x0, y0) 起点 d = (dx, dy) 方向 t ≥ 0 参数 矩形边界: [x_min, y_min, x_max, y_max] Slab方法: 1. X方向: t_x_min = (x_min - x0) / dx t_x_max = (x_max - x0) / dx 2. Y方向: t_y_min = (y_min - y0) / dy t_y_max = (y_max - y0) / dy 3. 求交集: t_enter = max(t_x_min, t_y_min) t_exit = min(t_x_max, t_y_max) 4. 判定: if t_enter ≤ t_exit AND t_exit > 0: 相交距离 = t_enter (若 t_enter > 0) ``` **算法优化**: - **提前终止**: 找到第一个障碍物后可停止 - **方向预计算**: 只计算一次三角函数 - **并行化潜力**: 每条光线独立计算 **输出格式**: ```matlab scan = [dist_1, dist_2, ..., dist_N]^T 范围: [0, max_range] 噪声: N(0, σ²), σ = 0.1m ``` **真实传感器对比**: ``` 仿真参数 对应真实传感器 ----------------------------------------- 360°视场角 Velodyne VLP-16 / SICK 15米最大距离 中程激光雷达 120束激光 3°分辨率 0.1米噪声 高精度激光雷达 ``` --- ### 数据关联算法 **文件**: `common/data_association.m` **算法原理**: 使用马氏距离（Mahalanobis Distance）匹配观测特征与已知路标。 **算法步骤**: 1. **马氏距离计算** ```matlab 对每个观测 z: 对每个已知路标 m_j: # 预测观测 ẑ = h(x̂, m_j) # 创新 ν = z - ẑ # 创新协方差 S = H P H^T + R # 马氏距离 d² = ν^T S^(-1) ν ``` 2. **最近邻匹配** ```matlab for each 观测: min_distance = inf matched_landmark = null for each 路标: d = mahalanobis_distance(观测, 路标) if d < threshold AND d < min_distance: min_distance = d matched_landmark = 该路标 if matched_landmark != null: 关联成功 else: 新路标 ``` 3. **阈值判定** ```matlab 基于卡方分布: 阈值 = χ²(α, df) 示例: - df=2 (2维观测: 距离+角度) - α=0.95 (95%置信度) - 阈值 ≈ 5.99 ``` **马氏距离 vs 欧氏距离**: ``` 欧氏距离: d = ||z - ẑ|| 问题: 不考虑不确定性，各维度权重相同 马氏距离: d² = (z - ẑ)^T S^(-1) (z - ẑ) 优势: - 考虑观测不确定性 - 自动归一化不同量纲 - 椭圆形匹配区域（适应协方差形状） ``` **应用**: - ✅ EKF-SLAM路标关联 - ✅ Graph-SLAM观测匹配 - ✅ FastSLAM粒子内关联 - ✅ 新路标初始化判定 --- ### 角度归一化算法 **文件**: `common/normalize_angle.m` **算法原理**: 将任意角度归一化到 [-π, π] 区间，处理角度跳变。 **实现**: ```matlab function angle = normalize_angle(angle) while angle > pi angle = angle - 2*pi; end while angle < -pi angle = angle + 2*pi; end end 或使用MATLAB内置: angle = wrapToPi(angle) ``` **应用场景**: - 角度误差计算（避免 -179° 和 179° 的问题） - EKF更新（创新向量的角度分量） - 方向控制（选择最短转向路径） **示例**: ``` 输入: 370° → 输出: 10° (370° - 360°) 输入: -190° → 输出: 170° (-190° + 360°) 输入: 180° → 输出: 180° (边界情况) ``` --- ## 算法性能对比 | 指标 | EKF-SLAM | Graph-SLAM | FastSLAM | Grid+MCL | |------|----------|------------|----------|----------| | **地图类型** | 特征点 | 特征点 | 特征点 | 栅格 | | **估计方式** | 实时滤波 | 批量优化 | 粒子滤波 | 粒子滤波 | | **计算复杂度** | O(N²) | 迭代相关 | O(M·log N) | O(M·K) | | **精度** | 中 | 高 | 中-高 | 中 | | **回环闭合** | 不支持 | 支持 | 部分支持 | 不支持 | | **适用场景** | 小规模实时 | 大规模离线 | 中等规模 | 密集环境 | --- ## 参数调优指南 ### 提高建图精度 - 减小过程噪声 `Q` 和观测噪声 `R` - 增加优化迭代次数（Graph-SLAM） - 增加粒子数量（FastSLAM、Grid+MCL） - 提高栅格分辨率（Grid-SLAM） ### 提高运行速度 - 减少仿真时间 `config.total_time` - 增大时间步长 `config.dt` - 降低可视化更新频率 - 减少粒子数量 - 减少优化迭代次数 ### 增加地图覆盖 - 使用更大范围的路径点 - 增加仿真时间 - 优化轨迹路径 - 使用动态避障模式增强探索 --- ## 更新日志 ### v2.1 (2025-11-05) **文档更新**： - **详细算法介绍** - 为四种SLAM算法添加详细说明 - 算法原理的深入解释 - 数学基础和公式推导 - 核心流程的详细步骤 - 优缺点的全面分析 - 适用场景和实际应用 - 实现细节和关键参数 - **算法对比完善** - 取消Graph-SLAM和FastSLAM的注释，启用完整的四算法对比 - **可视化优化** - 统一栅格地图显示方式，确保单独运行和对比运行的可视化一致 ### v2.0 (2025-11-04) **核心改进**： - **轨迹一致性保证** - 对比运行时动态模式由第一个算法生成轨迹，后续共享 - **平滑运动控制** - 渐进式角速度调整，避免急转弯和走回头路 - 调整频率：40步 → 60步（方向更稳定） - 角速度调整：完全随机 → 基于历史值±30%微调 - 最大角速度：无限制 → ±0.2 rad/s - 最大转角：180° → 120° - **运行时轨迹选择** - 启动时可选择固定路径或动态避障 - **边界安全距离优化** - 从3米减小到1.5米，增加探索空间 **功能增强**： - 增加运行时间对比功能（柱状图 + 散点图） - 增加综合性能评分（精度50% + 速度30% + 建图20%） - 优化可视化界面，统一轨迹图样式（12子图全面分析） - 增强进度显示，提供详细的实时信息 - 修复数据类型错误（整数与浮点数混合） **文档完善**： - 更新README，详细说明轨迹生成逻辑 - 添加动态轨迹特性说明 - 完善FAQ，解答轨迹一致性问题 - 使用更专业的技术语言 ### v1.0 (2024-11-04) - 创建统一配置系统 `get_slam_config.m` - 实现四种SLAM算法（EKF、Graph、FastSLAM、Grid+MCL） - 建立完整的算法对比框架 - 修复数据关联一致性问题 - 修复Graph-SLAM路标初始化 - 优化图优化参数（步长、权重、阻尼） --- ## 许可证 本项目仅供**学习和研究使用**。 --- ## 贡献 欢迎提交 Issue 和 Pull Request！ --- ## 联系方式 如有问题或建议，欢迎通过 Issue 反馈。