该 MATLAB 代码实现了一个移动机器人在静态与动态障碍物环境下的全局路径规划与局部避障仿真系统。
一、研究背景
随着智能机器人技术的发展，机器人在复杂环境中的自主导航 成为研究热点。该代码结合了全局路径规划（A*算法） 与局部动态避障（DWA算法） ，旨在解决机器人在已知静态地图 中遇到未知静止障碍物 和移动障碍物时的路径重规划与安全行驶问题。
二、主要功能
- 地图建模：使用二维网格表示环境，1为障碍物，0为可通行区域。
- 全局路径规划：使用A*算法生成从起点到目标点的最优路径。
- 动态障碍物路径生成：为移动障碍物设置起点与终点，并规划其运动轨迹。
- 未知静止障碍物设置：用户可交互式添加未知障碍物。
- 局部路径规划与避障 ：使用DWA算法（Dynamic Window Approach）实现机器人的局部路径跟踪与避障。
- 运动学模型仿真：模拟机器人运动（位置、速度、角速度）。
- 结果可视化：绘制路径、速度变化、姿态变化等。
三、算法步骤
1. 初始化阶段
- 定义静态障碍物网格地图。
- 设置坐标系与绘图参数。
2. 用户交互阶段
- 鼠标点击选择目标点 和起点。
- 设置移动障碍物起点与终点，并生成其路径。
- 设置未知静止障碍物。
3. 全局路径规划
- 调用
Astar_G_du函数，生成从起点到目标点的全局路径Line_path。
4. 动态障碍物路径规划
- 使用
Astar_G为移动障碍物生成路径。 - 调用
Line_obst根据速度生成移动障碍物在各时刻的位置。
5. 局部路径规划与动态避障
- 调用
DWA_ct_dong函数，结合：- 全局路径
- 动态障碍物轨迹
- 未知静止障碍物
- 机器人运动学模型
- 实时计算机器人的下一时刻位置、速度与角速度。
6. 结果输出与可视化
- 绘制最终路径、速度曲线、姿态角变化。
- 计算机器人实际行驶路径长度。
四、技术路线
| 模块 | 方法 |
|---|---|
| 地图表示 | 栅格法（Grid Map） |
| 全局路径规划 | A* 算法（启发式搜索） |
| 动态障碍物建模 | 路径 + 速度模型 |
| 局部路径规划 | DWA（动态窗口法） |
| 机器人运动学 | 非完整约束模型（差速驱动） |
| 避障策略 | 评价函数法（方位角、障碍物距离、速度） |
| 用户交互 | MATLAB ginput 鼠标点选 |
五、公式原理
DWA算法在速度空间 (v, ω) 中采样，预测轨迹并评估：
-
评价函数 ：
G(v,ω)=α⋅heading(v,ω)+β⋅dist(v,ω)+γ⋅velocity(v,ω) G(v, ω) = α \cdot \text{heading}(v, ω) + β \cdot \text{dist}(v, ω) + γ \cdot \text{velocity}(v, ω) G(v,ω)=α⋅heading(v,ω)+β⋅dist(v,ω)+γ⋅velocity(v,ω)heading：朝向目标的角度差dist：与最近障碍物的距离velocity：当前速度大小
-
动态窗口 ：
v∈[vcurrent−amax⋅Δt,vcurrent+amax⋅Δt] v \in [v_{\text{current}} - a_{\text{max}} \cdot \Delta t, v_{\text{current}} + a_{\text{max}} \cdot \Delta t] v∈[vcurrent−amax⋅Δt,vcurrent+amax⋅Δt]限制在机器人运动学与动力学约束范围内。
六、参数设定
| 参数 | 含义 | 示例值 |
|---|---|---|
Kinematic |
机器人运动学参数 | [1.5 m/s, 20°/s, 0.2 m/s², 50°/s², 0.02, 1°] |
evalParam |
DWA评价函数系数 | [0.05, 0.2, 0.3, 3.0] |
L_obst |
移动障碍物速度因子 | 0.026 m/0.1s |
MAX |
静态障碍物网格 | 30×30 矩阵 |
Area_MAX |
地图尺寸 | [30, 30] |
七、运行环境
- 软件：MATLAB2020（需支持图形交互）
八、应用场景
- 移动机器人导航仿真（如扫地机器人、AGV小车）
- 智能交通系统中的车辆避障
- 多机器人协同环境下的路径规划研究
- 教学与科研实验：验证A*与DWA结合的有效性
- 复杂环境下路径重规划算法开发
