【路径规划】A星算法结合floyd和动态窗口法机器人栅格地图路径规划【含Matlab源码 2721期】

✅博主简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，Matlab项目合作可私信。
🍎个人主页：海神之光
🏆代码获取方式：
海神之光Matlab王者学习之路—代码获取方式
⛳️座右铭：行百里者，半于九十。

更多Matlab仿真内容点击👇
Matlab图像处理（进阶版）
路径规划（Matlab）
神经网络预测与分类（Matlab）
优化求解（Matlab）
语音处理（Matlab）
信号处理（Matlab）
车间调度（Matlab）

⛄一、A星算法结合floyd和动态窗口法

结合A*算法、Floyd算法和动态窗口法人栅格地图路径规划的步骤如下：

创建栅格地图：将地图转换为栅格形式，每个栅格表示一个状态节点。标记障碍物、起点和目标位置。

运行Floyd算法：Floyd算法计算出任意两个节点之间的最短路径，生成路径距离矩阵或路径图。

初始化起点和目标点：将起点设置为当前节点，目标点设置为目标位置。

初始化开启列表和关闭列表：开启列表存储待探的节点，关闭列表存储已经探索过的节点。

开始循环：

a. 将当前加入关闭列表，标记为已被访问。

b. 遍历当前节点相邻的节点：

若该节点是目标节点，结束搜索，保存路径。

若该节点在关闭列表中，忽略。

若该节点不在开启列表中，添加到开启列表，并计算该节点的G值、H值和F值。G值是从起始节点到当前节点的实际代价，H值是从当前节点目标节点的预估代价，F值为G值和H值的总和。

若该已经在开启列表中，计算新的G值。若新的G值比原来的更小，则更新该节点的G值和父节点。

c. 根据节点的F值对开启列表中的节点进行排序。

d. 从开启列表中选择F值最小的节点作为新的当前节点，重复步骤a-d直到到达目标节点或开启列表为空。

使用训练好的动态窗口法规划路径：将获得的路径与实时传感器数据结合，使用动态窗口法进行实时路径规划和避障。

需要注意的是，以上步骤涉及到多个算法的结，具体的实现可能比较复杂，需要根据具体场景进行相应的调整和优化。同时，路径规划还考虑机器人的运动能力、环境障碍物的变化等素。

⛄二、部分源代码

clear
close all
figure

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%改进A星算法+Floyd+DWA（躲避未知静态障碍物）
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% 地图建模
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
MAX0 =[
0 0 0 0 0 1 0 1 0 0 1 1 0 0 0 1 0 0 0 1 1 1 0 0 0 1 0 0 0 1;
1 0 0 1 0 0 0 0 0 0 1 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 1;
0 0 0 1 1 0 0 1 1 0 0 0 0 1 1 0 0 0 1 0 0 0 0 0 0 0 0 1 0 1;
0 0 0 0 1 0 1 0 0 0 1 0 0 0 0 0 0 0 1 0 0 0 0 1 1 1 0 0 0 0;
1 1 0 0 0 1 1 0 0 0 0 0 0 0 1 1 0 0 0 0 1 0 0 0 0 0 0 0 1 0;
1 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 1 1 0 0 1 1;
0 0 1 1 0 0 1 1 0 1 0 1 1 0 0 0 0 0 0 1 0 0 0 0 1 0 0 0 0 0;
1 0 0 1 0 0 0 0 0 0 0 0 1 0 0 1 0 0 1 1 0 0 0 0 0 0 1 0 0 0;
0 0 0 0 0 0 1 0 0 0 1 0 0 0 0 1 0 0 1 0 0 0 0 0 1 0 0 0 1 1;
1 0 0 0 0 0 0 0 1 0 0 0 0 1 1 1 0 0 0 0 0 1 0 0 1 0 0 0 1 0;
1 0 1 1 0 0 0 0 0 1 1 0 0 0 0 0 0 1 0 0 0 1 0 1 1 0 1 0 0 0;
1 0 0 0 0 1 1 0 0 0 1 0 1 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0;
0 0 0 0 1 1 1 1 0 0 1 0 0 1 0 0 0 1 1 1 0 0 0 0 0 0 0 1 0 0;
0 0 0 0 0 0 0 1 0 1 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 1 0 0 0 0 0 0 1 1 0 0 1 1 1 0 0 0 1 0 0 0 0 1 1 0 0 0 0;
0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 1 0 0 0 0 0 0 0 0;
0 0 1 0 0 0 1 1 1 0 0 1 1 0 0 1 1 0 0 0 0 1 0 0 0 0 0 1 1 0;
0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 1 0 1 0 0;
1 0 0 0 0 0 0 0 0 0 0 0 1 0 0 1 0 0 0 0 0 1 1 1 0 0 0 0 0 0;
0 0 0 1 1 0 0 1 0 1 0 0 1 0 0 0 0 1 1 0 0 1 0 0 0 0 0 1 0 0;
1 1 0 0 0 0 0 1 0 1 0 0 1 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0;
0 0 0 1 0 0 0 1 0 0 1 1 0 0 0 0 0 0 1 0 0 0 1 0 0 0 0 1 0 0;
0 0 1 1 1 0 0 1 0 0 0 0 0 0 1 0 0 0 1 0 0 1 1 0 0 0 1 1 0 0;
1 0 0 0 0 0 0 1 0 0 0 0 0 1 1 1 0 0 0 0 0 1 0 0 0 0 0 0 0 0;
1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 1 1;
0 0 0 1 0 0 1 0 0 1 1 0 0 0 1 1 0 0 0 1 0 0 0 1 1 0 0 0 1 1;
0 0 0 1 0 0 1 0 0 0 0 0 0 1 1 0 0 0 1 1 0 0 0 0 0 0 0 0 0 1;
1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 1 0 0 0 0 0;
1 0 0 0 0 0 1 1 0 1 0 0 0 0 1 0 1 0 0 0 0 1 0 0 0 0 1 0 0 0;
1 0 1 1 0 0 0 1 0 0 0 1 1 0 0 0 1 0 0 1 0 0 0 0 0 0 1 1 0 0;
];

%%% 通道设置为 0 ；障碍点设置为 1 ；起始点设置为 2 ；目标点设置为 -1 。
MAX=rot90(MAX0,3); %%%设置0,1摆放的图像与存入的数组不一样，需要先逆时针旋转90*3=270度给数组，最后输出来的图像就是自己编排的图像
MAX_X=size(MAX,2); %%% 获取列数，即x轴长度
MAX_Y=size(MAX,1); %%% 获取行数，即y轴长度
MAX_VAL=10; %%% 返回由数字组成的字符表达式的数字值，就是函数用于将数值字符串转换为数值

x_val = 1;
y_val = 1;
axis([1 MAX_X+1, 1 MAX_Y+1]) %%% 设置x，y轴上下限
set(gca,‘xtick’,1:1:MAX_X+1,‘ytick’,1:1:MAX_Y+1,‘GridLineStyle’,‘-’,…
‘xGrid’,‘on’,‘yGrid’,‘on’)
grid on; %%% 在画图的时候添加网格线
hold on; %%% 当前轴及图像保持而不被刷新，准备接受此后将绘制的图形，多图共存
n=0;%Number of Obstacles %%% 障碍的数量

k=1; %%%% 将所有障碍物放在关闭列表中；障碍点的值为1;并且显示障碍点
CLOSED=[];
for j=1:MAX_X
for i=1:MAX_Y
if (MAX(i,j)==1)
%%plot(i+.5,j+.5,‘ks’,‘MarkerFaceColor’,‘b’); 原来是红点圆表示
fill([i,i+1,i+1,i],[j,j,j+1,j+1],‘k’); %%%改成 用黑方块来表示障碍物
CLOSED(k,1)=i; %%% 将障碍点保存到CLOSE数组中
CLOSED(k,2)=j;
k=k+1;
end
end
end

⛄三、运行结果

⛄四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1]钱程,许映秋,谈英姿.A Star算法在RoboCup救援仿真中路径规划的应用[J].指挥与控制学报. 2017,3(03)

3 备注
简介此部分摘自互联网，仅供参考，若侵权，联系删除

🍅 仿真咨询
1 各类智能优化算法改进及应用
生产调度、经济调度、装配线调度、充电优化、车间调度、发车优化、水库调度、三维装箱、物流选址、货位优化、公交排班优化、充电桩布局优化、车间布局优化、集装箱船配载优化、水泵组合优化、解医疗资源分配优化、设施布局优化、可视域基站和无人机选址优化

2 机器学习和深度学习方面
卷积神经网络（CNN）、LSTM、支持向量机（SVM）、最小二乘支持向量机（LSSVM）、极限学习机（ELM）、核极限学习机（KELM）、BP、RBF、宽度学习、DBN、RF、RBF、DELM、XGBOOST、TCN实现风电预测、光伏预测、电池寿命预测、辐射源识别、交通流预测、负荷预测、股价预测、PM2.5浓度预测、电池健康状态预测、水体光学参数反演、NLOS信号识别、地铁停车精准预测、变压器故障诊断

3 图像处理方面
图像识别、图像分割、图像检测、图像隐藏、图像配准、图像拼接、图像融合、图像增强、图像压缩感知

4 路径规划方面
旅行商问题（TSP）、车辆路径问题（VRP、MVRP、CVRP、VRPTW等）、无人机三维路径规划、无人机协同、无人机编队、机器人路径规划、栅格地图路径规划、多式联运运输问题、车辆协同无人机路径规划、天线线性阵列分布优化、车间布局优化

5 无人机应用方面
无人机路径规划、无人机控制、无人机编队、无人机协同、无人机任务分配

6 无线传感器定位及布局方面
传感器部署优化、通信协议优化、路由优化、目标定位优化、Dv-Hop定位优化、Leach协议优化、WSN覆盖优化、组播优化、RSSI定位优化

7 信号处理方面
信号识别、信号加密、信号去噪、信号增强、雷达信号处理、信号水印嵌入提取、肌电信号、脑电信号、信号配时优化

8 电力系统方面
微电网优化、无功优化、配电网重构、储能配置

9 元胞自动机方面
交通流 人群疏散 病毒扩散 晶体生长

10 雷达方面
卡尔曼滤波跟踪、航迹关联、航迹融合