【路径规划】基于matlab A_star算法机器人栅格地图最短路径规划【含Matlab源码 496期】

已于 2023-10-28 10:43:08 修改
阅读量2.2k 收藏 22
点赞数 6
分类专栏： Matlab路径规划（进阶版） 文章标签： matlab
于 2021-03-12 00:00:28 首次发布
代码事宜私信博主
本文链接：https://blog.csdn.net/TIQCmatlab/article/details/114669854
版权

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

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

⛄一、A_star算法简介

1 A Star算法及其应用现状
进行搜索任务时提取的有助于简化搜索过程的信息被称为启发信息.启发信息经过文字提炼和公式化后转变为启发函数.启发函数可以表示自起始顶点至目标顶点间的估算距离, 也可以表示自起始顶点至目标顶点间的估算时间等.描述不同的情境、解决不同的问题所采用的启发函数各不相同.我们默认将启发函数命名为H (n) .以启发函数为策略支持的搜索方式我们称之为启发型搜索算法.在救援机器人的路径规划中, A Star算法能结合搜索任务中的环境情况, 缩小搜索范围, 提高搜索效率, 使搜索过程更具方向性、智能性, 所以A Star算法能较好地应用于机器人路径规划相关领域.

2 A Star算法流程
承接2.1节, A Star算法的启发函数是用来估算起始点到目标点的距离, 从而缩小搜索范围, 提高搜索效率.A Star算法的数学公式为:F (n) =G (n) +H (n) , 其中F (n) 是从起始点经由节点n到目标点的估计函数, G (n) 表示从起点移动到方格n的实际移动代价, H (n) 表示从方格n移动到目标点的估算移动代价.

如图2所示, 将要搜寻的区域划分成了正方形的格子, 每个格子的状态分为可通过(walkable) 和不可通过 (unwalkable) .取每个可通过方块的代价值为1, 且可以沿对角移动 (估值不考虑对角移动) .其搜索路径流程如下:
在这里插入图片描述
图2 A Star算法路径规划
Step1:定义名为open和closed的两个列表;open列表用于存放所有被考虑来寻找路径的方块, closed列表用于存放不会再考虑的方块;
Step2:A为起点, B为目标点, 从起点A开始, 并将起点A放入open列表中, closed列表初始化为空;
Step3:查看与A相邻的方格n (n称为A的子点, A称为n的父点) , 可通过的方格加入到open列表中, 计算它们的F, G和H值.将A从open移除加入到closed列表中;
Step4:判断open列表是否为空, 如果是, 表示搜索失败, 如果不是, 执行下一步骤;
Step5:将n从open列表移除加入到closed列表中, 判断n是否为目标顶点B, 如果是, 表示搜索成功, 算法运行结束;
Step6:如果不是, 则扩展搜索n的子顶点:
a.如果子顶点是不可通过或在close列表中, 忽略它.
b.子顶点如果不在open列表中, 则加入open列表, 并且把当前方格设置为它的父亲, 记录该方格的F, G和H值.
Step7:跳转到步骤Step4;
Step8:循环结束, 保存路径.从终点开始, 每个方格沿着父节点移动直至起点, 即是最优路径.A Star算法流程图如图3所示.
在这里插入图片描述
图3 A Star算法流程

⛄二、部分源代码

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% A* ALGORITHM Demo
% Interactive A* search demo
% 1 避开障碍物，不斜线过障碍物顶点
% 2 改进栅格实心表示障碍点，在简化设置障碍点，对同一地图不同起始点进行研究
% 3 改进折线转弯为圆弧
% 11-13-2018
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clc ;
figure(1)
%%%只能设置正方形矩阵，行和列相等，否则旋转时会出现错误
% MAX0 = [ 0 0 0 1 1 0 0 0 1 0 0 0 0 0 1
% 0 0 0 1 1 1 0 0 0 0 0 1 0 0 0
% 0 0 0 0 0 0 1 1 1 1 1 1 1 1 0
% 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0
% 0 0 0 0 0 0 0 0 1 1 0 1 0 0 0
% 0 0 0 0 0 0 0 1 0 0 0 1 0 0 0
% 0 0 0 0 0 0 0 1 0 1 1 0 0 0 0
% 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0
% 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
% 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
% 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
% 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
% 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
% 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
% 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ] ;
% MAX0 = [ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
% 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 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 1 1 1 0 0 0 0 0 0 0 0 0 0 0
% 0 0 0 0 0 0 1 1 1 0 0 0 0 0 1 1 1 1 0 0
% 0 0 0 0 0 0 0 0 0 0 0 0 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 1 1 1 0 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0
% 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0
% 0 0 1 1 0 0 1 1 1 1 1 1 1 0 0 0 1 0 0 0
% 0 0 0 0 0 0 0 0 1 1 1 1 1 1 0 0 1 0 0 0
% 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0
% 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 1 0 0 0
% 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0 0 0 0 0 0
% 0 0 1 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 1 1
% 0 0 1 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0
% 0 0 0 0 0 0 1 1 1 0 1 1 0 0 1 1 0 0 0 0
% 0 0 0 0 0 0 0 0 0 0 1 1 0 0 1 1 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 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 ] ;
MAX0 = [ 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0

     0  1 1 1 0 0 1 1 1 1 1 1 1 1 1 0 0 1 1 1 1    0 0 0   1 1 1 1 1 0 0 0 1 1 1 1 1 1 1 1 0 0 1 1  0
     0  1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1    0 0 0   1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1  0
     0  1 0 0 1 1 0 0 0 1 1 0 0 0 0 1 0 0 0 0 1    0 0 0   1 0 0 1 0 1 0 0 0 0 1 1 1 0 1 0 0 1 0 1  0
     0  1 0 0 1 1 0 0 0 0 1 0 0 0 0 1 1 1 0 0 1    0 0 0   1 0 0 1 1 1 0 0 0 0 1 0 1 1 1 0 0 1 1 1  0
     0  1 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0 0 1    0 0 0   1 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 1 0 0 0 0 0 1 0 0 1 1 0 0 0 0    0 0 0   0 0 0 1 1 1 1 0 0 1 0 1 0 0 0 1 0 1 0 1  0
     0  0 0 0 0 1 1 1 0 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 1 0 0 0 1 0 1 0 1  0
     0  0 0 0 0 0 1 0 0 0 1 1 1 0 0 0 1 0 0 0 1    0 0 0   1 0 0 1 1 1 1 0 0 1 1 1 0 0 0 1 1 1 0 1  0
     0  1 0 1 1 0 0 0 0 0 0 0 1 0 0 1 1 0 0 0 1    0 0 0   1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1  0
     0  1 0 0 1 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0 1    0 0 0   1 1 0 1 1 1 1 0 0 1 0 0 0 1 1 1 0 0 0 1  0
     0  1 0 0 1 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0 1    0 0 0   1 0 0 1 0 0 1 0 0 1 0 0 0 1 0 1 0 0 0 1  0
     0  1 1 1 1 0 0 0 0 0 1 0 0 1 0 1 0 0 0 0 1    0 0 0   1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1  0
     0  1 0 0 1 0 0 1 1 0 1 0 0 1 0 1 0 0 0 0 1    0 0 0   1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0  0
     0  1 0 1 1 0 0 0 1 0 1 0 0 1 1 1 0 0 0 0 1    0 0 0   1 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 1 0 0 0  0
     0  1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1    0 0 0   0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 1 0 0 0  0
     0  1 0 0 0 0 0 1 1 1 0 0 0 0 0 1 1 1 0 0 0    0 0 0   0 0 0 1 1 0 0 0 0 0 0 1 0 0 0 0 1 0 0 0  0
     0  1 0 0 0 1 0 1 0 0 0 0 0 0 0 0 0 1 0 0 0    0 0 0   1 0 1 1 1 1 0 0 0 1 1 1 1 0 0 0 1 0 0 1  0
     0  1 0 0 1 1 0 0 0 0 0 0 0 0 0 1 1 1 0 0 1    0 0 0   1 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 0 0 0 0 0 0 0 0 0 0 0 0 0 1    0 0 0   1 1 0 0 0 0 0 0 1 0 0 1 0 0 0 0 0 0 0 1  0
     0  1 1 1 1 0 0 0 1 1 1 1 1 1 1 0 0 1 1 1 1    0 0 0   1 1 1 1 1 0 0 0 1 1 1 1 1 1 0 0 1 1 1 1  0
     
     0  0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0    0 0 0   0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0  0
     0  0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0    0 0 0   0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0  0
     0  0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0    0 0 0   0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0  0
       
       
     0  1 1 1 1 0 0 1 1 1 1 1 1 1 0 0 1 1 1 1 1    0 0 0   1 1 1 1 1 1 1 0 0 0 0 1 1 1 1 1 1 1 1 1  0
     0  1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1    0 0 0   1 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 0 0 0 0 0 0 0 0 0 0 0 0 0 1    0 0 0   1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1  0
     0  1 0 0 1 0 1 0 1 0 1 0 1 0 0 1 0 1 0 0 1    0 0 0   1 0 0 0 1 1 1 1 1 1 1 0 0 1 1 1 1 1 1 1  0
     0  1 0 0 1 0 1 0 1 0 1 0 1 0 0 1 0 1 0 0 1    0 0 0   1 0 0 0 1 0 0 0 0 0 1 0 0 1 0 0 0 0 0 1  0
     0  1 0 0 1 0 1 0 1 0 1 0 1 0 0 1 0 1 0 0 0    0 0 0   0 0 0 0 1 0 0 0 0 0 1 0 0 1 0 0 0 0 0 1  0
     0  1 0 0 1 0 1 0 1 0 0 0 0 0 0 0 0 0 0 0 0    0 0 0   0 0 0 0 1 1 1 1 1 0 1 0 0 1 1 1 0 1 1 1  0
     0  0 0 0 0 0 0 0 0 0 1 0 1 0 0 1 0 1 0 0 1    0 0 0   1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1  0
     0  0 0 0 0 0 0 0 0 0 1 0 1 0 0 1 0 1 0 0 1    0 0 0   1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1  0
     0  1 0 0 1 0 1 0 1 0 0 0 0 0 0 0 0 0 0 0 1    0 0 0   1 0 0 0 1 1 1 0 1 1 1 0 0 1 1 1 0 1 1 1  0
     0  1 0 0 1 0 1 0 1 0 1 0 1 0 0 1 0 1 0 0 1    0 0 0   1 0 0 0 1 0 0 0 0 0 1 0 0 1 0 0 0 0 0 1  0
     0  1 0 0 1 0 1 0 1 0 1 0 1 0 0 1 0 1 0 0 0    0 0 0   1 0 0 0 1 0 0 0 0 0 1 0 0 1 0 0 0 0 0 1  0
     0  1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0    0 0 0   1 0 0 0 1 1 1 0 1 1 1 0 0 1 1 1 1 1 1 1  0
     0  1 0 0 1 0 1 0 1 0 1 0 1 0 0 1 0 1 0 0 1    0 0 0   1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0  0
     0  1 0 0 1 0 1 0 1 0 1 0 1 0 0 1 0 1 0 0 1    0 0 0   0 0 0 0 1 1 1 1 1 1 1 0 0 0 1 1 1 1 1 1  0
     0  1 0 0 1 0 1 0 1 0 1 0 1 0 0 1 0 1 0 0 1    0 0 0   0 0 0 0 1 0 0 0 0 0 1 0 0 0 1 0 0 0 0 1  0
     0  1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1    0 0 0   1 0 0 0 1 1 1 0 1 1 1 0 0 0 1 1 0 1 1 1  0
     0  1 1 0 1 0 1 0 1 0 0 0 1 0 0 1 0 1 1 0 1    0 0 0   1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1  0
     0  1 1 0 1 0 1 0 1 0 0 0 1 0 0 1 0 1 1 0 1    0 0 0   1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1  0
     0  1 1 1 1 1 1 1 1 1 0 1 1 1 1 1 1 1 1 1 1    0 0 0   1 1 1 1 1 1 0 0 1 1 1 1 1 1 0 0 1 1 1 1  0
       
     0  0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0    0 0 0   0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0  0 ] ;

%%% 通道设置为 0 ；障碍点设置为 1 ；起始点设置为 2 ；目标点设置为 -1 。
MAX=rot90(MAX0,3); %%%设置0,1摆放的图像与存入的数组不一样，需要先逆时针旋转903=270度给数组，最后输出来的图像就是自己编排的图像
MAX_X=size(MAX,2); %%% 获取列数，即x轴长度
MAX_Y=size(MAX,1); %%% 获取行数，即y轴长度
MAX_VAL=10; %%% 返回由数字组成的字符表达式的数字值，就是函数用于将数值字符串转换为数值
%This array stores the coordinates of the map and the
%Objects in each coordinate
%%% 这个数组存储地图的坐标和每个坐标中的对象。
% // MAP=2(ones(MAX_X+1,MAX_Y+1)); %%%%% 生成MAX_X行,MAX_Y列，且全部元素为2
%%%// 改进2 自己设置地图

% Obtain Obstacle, Target and Robot Position
% Initialize the MAP with input values
% Obstacle=-1,Target = 0,Robot=1,Space=2

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

%%% 选择目标位置
pause(1); %%% 程序暂停1秒
h=msgbox(‘请使用鼠标左键选择目标’); %%% 显示提示语 原句是：Please Select the Target using the Left Mouse button
uiwait(h,5); %%% 程序暂停
if ishandle(h) == 1 %%% ishandle(H) 将返回一个元素为 1 的数组；否则，将返回 0。
delete(h);
end
xlabel(‘请使用鼠标左键选择目标’,‘Color’,‘black’); %%% 显示图x坐标下面的提示语 原句是：Please Select the Target using the Left Mouse button
but=0;
while (but ~= 1) %Repeat until the Left button is not clicked %%% 重复，直到没有单击“向左”按钮
[xval,yval,but]=ginput(1); %%% ginput提供了一个十字光标使我们能更精确的选择我们所需要的位置，并返回坐标值。
end
xval=floor(xval); %%% floor（）取不大于传入值的最大整数，向下取整
yval=floor(yval);
xTarget=xval;%X Coordinate of the Target %%% 目标的坐标
yTarget=yval;%Y Coordinate of the Target

MAP(xval,yval) = -1 ; %%% 目标坐标点位置的值设为-1
plot(xval+.5,yval+.5,‘go’); %%% 目标点颜色b 蓝色 g 绿色 k 黑色 w白色 r 红色 y黄色 m紫红色 c蓝绿色
% text(xval+1,yval+.5,‘Target’) %%% text(x,y,‘string’)在二维图形中指定的位置(x,y)上显示字符串string

%%% 选择起始位置
h=msgbox(‘请使用鼠标左键选择车辆初始位置’); %%%原文 Please Select the Vehicle initial position using the Left Mouse button
uiwait(h,5);
if ishandle(h) == 1
delete(h);
end
xlabel('请选择车辆初始位置 ',‘Color’,‘black’); %%% 原文 Please Select the Vehicle initial position
but=0;
while (but ~= 1) %Repeat until the Left button is not clicked %%%重复，直到没有单击“向左”按钮
[xval,yval,but]=ginput(1);
xval=floor(xval);
yval=floor(yval);
end
xStart=xval;%Starting Position
yStart=yval;%Starting Position
MAP(xval,yval)=2; %%% 起始点位置的值设置为1；目标点为0，障碍点为-1，其余空白点为2
plot(xval+.5,yval+.5,‘b^’);

xlabel('起始点位置标记为 △ ，目标点位置标记为 o ',‘Color’,‘black’);
%End of obstacle-Target pickup
tic
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%LISTS USED FOR ALGORITHM %%%用于算法的列表
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%OPEN LIST STRUCTURE %%%开放列表结构
%--------------------------------------------------------------------------
%IS ON LIST 1/0 |X val |Y val |Parent X val |Parent Y val |h(n) |g(n)|f(n)|
%--------------------------------------------------------------------------
OPEN=[];
%CLOSED LIST STRUCTURE %%% 封闭的列表结构
%--------------
%X val | Y val |
%--------------
% CLOSED=zeros(MAX_VAL,2); %%% 生成MAX_VAL行，2列的0矩阵
CLOSED_COUNT=size(CLOSED,1); %%% CLOSED的行数，即障碍点的个数
Nobs=CLOSED_COUNT;
%set the starting node as the first node %%%将起始节点设置为第一个节点
xNode=xval; %%% =xStart
yNode=yval; %%% =yStart
OPEN_COUNT=1; %%% OPEN_COUNT 开启列表的行数标志
path_cost=0;
goal_distance=distance(xNode,yNode,xTarget,yTarget); %%% 调用distance（）函数，求两坐标点之间的笛卡尔距离
OPEN(OPEN_COUNT,:)=insert_open(xNode,yNode,xNode,yNode,path_cost,goal_distance,goal_distance); %%% 插入到开放列表
%%% OPEN（第一行的元素）=（1，xNode,yNode,xNode,yNode,path_cost,goal_distance,goal_distanc）；
OPEN(OPEN_COUNT,1)=0; %%% OPEN(1,1)=0
CLOSED_COUNT=CLOSED_COUNT+1; %%% CLOSED 存储完障碍点后，下一个单元
CLOSED(CLOSED_COUNT,1)=xNode; %%% 下一个存储起始点的 坐标
CLOSED(CLOSED_COUNT,2)=yNode;
NoPath=1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% START ALGORITHM 开始算法
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
while((xNode ~= xTarget || yNode ~= yTarget) && NoPath == 1) %%% 判断当前点是否等于目标点
% plot(xNode+.5,yNode+.5,‘go’);
% xnode=xNode,ynode=yNode %%%输出当前节点，用来学习了解A算法的运算过程* ///不需要知道过程可注释掉///
exp_array=expand_array(xNode,yNode,path_cost,xTarget,yTarget,CLOSED,MAX_X,MAX_Y,Nobs); %%% 不在关闭列表的子节点，（x,y,gn,hn,fn）,列数是个数
exp_count=size(exp_array,1); %%% 可选择的子节点个数
%UPDATE LIST OPEN WITH THE SUCCESSOR NODES
%OPEN LIST FORMAT
%--------------------------------------------------------------------------
%IS ON LIST 1/0 |X val |Y val |Parent X val |Parent Y val |h(n) |g(n)|f(n)|
%--------------------------------------------------------------------------
%EXPANDED ARRAY FORMAT 扩展阵列格式
%--------------------------------
%|X val |Y val ||h(n) |g(n)|f(n)|
%--------------------------------
for i=1:exp_count %%% 把exp_array内的元素添加到 开启列表 里面
flag=0; %%% 将exp_array内的点的标志位设为0
for j=1:OPEN_COUNT %%% OPEN_COUNT 从1开始，自加
if(exp_array(i,1) == OPEN(j,2) && exp_array(i,2) == OPEN(j,3) ) %%%判断可选子节点是否与OPEN[]中的点相同
OPEN(j,8)=min(OPEN(j,8),exp_array(i,5)); %%%如果相同，比较两个fn的值的大小，并将fn小的坐标点赋值给OPEN(j,8)
if OPEN(j,8)== exp_array(i,5) %%% 表示，上一步比较中 exp_array(i,5)小，则把exp_array(i,：）中的值赋给OPEN
%UPDATE PARENTS,gn,hn
OPEN(j,4)=xNode;
OPEN(j,5)=yNode;
OPEN(j,6)=exp_array(i,3);
OPEN(j,7)=exp_array(i,4);
end;%End of minimum fn check
flag=1; %%%将与ＯＰＥＮ相同的ｆｌａｇ＝０
end;%End of node check

⛄三、运行结果

在这里插入图片描述
在这里插入图片描述

⛄四、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 雷达方面
卡尔曼滤波跟踪、航迹关联、航迹融合

海神之光
关注
  • 6
    点赞
  • 踩
  • 22
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 1
    评论
专栏目录
MATLAB算法实战应用案例精讲-【自动驾驶】路径规划
qq_36130719的博客
08-03 1940
轨迹规划是在考虑临时或移动障碍物的前提下做出的规划，解决的是如何在避开障碍物的同时尽量沿着路径走的问题，和你看到地图上指示的路线后虽然是沿着路线走但是仍需要看路看车看人看红绿灯一样，重点在避障和遵守交规。在下面的图中，越黄的结点代表越高的启发式值（移动到目标的代价高），而越黑的结点代表越低的启发式值（移动到目标的代价低）。来选择最终车辆状态。再回答为什么有的论文没有路径规划，可能是因为是室内机器人，园区机器人等小型封闭空间内的运动规划，或者是其他缺少移动障碍物的场景，所以轨迹约等于路径，没有两者之分。
A*（A星）搜索路径规划教程（matlab实现）
你的Matlab小助手
03-10 1033
A*（A-Star）路径搜索算法是一种常用的启发式搜索算法，用于解决图形和图形网络中的最短路径问题。它结合了Dijkstra算法的全局最短路径搜索和贪心最佳优先搜索的优点，具有高效性和可扩展性。路径规划是人工智能和计算机科学中的一个重要问题，应用于许多领域，如机器人导航、游戏开发、交通规划等。[1]袁中锋,汪朝亮. 探索新路径 为民解心结[N]. 安徽日报,2024-03-09(012).文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。行百里者，半于九十。
matlab路径规划代码.zip
11-29
源码参考学习使用，
A*算法与DWA算法融合的改进路径规划算法：实现动态避障并提供MATLAB源文件，改进A星算法与DWA算法融合，实现机器人动态路径规划导航及避障（附matlab源文件）
最新发布
tBjEtmysBb的博客
06-23 2287
DWA算法根据感知到的动态障碍物的信息和机器人的运动能力，实时调整机器人的速度和转向角度，以避免与障碍物发生碰撞。另外，DWA算法能够快速地生成局部最优路径，并通过不断更新路径信息适应动态环境，使得机器人能够在复杂环境中灵活导航。未来，我们可以进一步研究该算法在更复杂环境下的应用，提高算法的效率和性能。算法和DWA算法融合，可以实现动态避障，这为机器人的自主导航提供了更高效和可靠的解决方案。改进A星算法，机器人路径规划导航A星算法和DWA算法融合，可以实现动态避障，有算法matlab源文件，可以直接用。
基于Matlab实现路径规划算法（附上10多个案例源码）
热门推荐
m0_62143653的博客
06-19 2万+
基于Matlab实现路径规划算法（附上10多个案例源码）
路径规划算法（matlab实现）
你的Matlab小助手
03-04 2076
路径规划，也称为运动规划或轨迹规划，是机器人学和计算机科学中的一个基本问题。常用的技术如Dijkstra算法和A*搜索，用于在该网格上考虑穿越每个单元格的代价，找到最短路径。[2]张强,王翱航,吉祥.高校教育评价改革社会支持的学理渊源、动力支撑与优化路径[J].上海教育评估研究,2024,13(01):14-18.DOI:10.13794/j.cnki.shjee.2024.0002.文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。行百里者，半于九十。
【路径规划】Astart算法——图文直观解析
Jimmyshis
06-16 6890
Astart算法详解1. Astart算法原理 1.1. Astart算法路径搜索原理 1.2. 栅格网络的Astart算法2. Astart程序实现 Java 1. Astart算法原理  1.1. Astart算法路径搜索原理   Dijkstra算法已经是非常经典的求解最短路径的动态规划算法，其可在解空间的可行域内求解出唯一确定最短路径。但Dijkstra算法需要遍历解空间内所有的点，这在解空间节点数巨大的情况下十分占用计算资源，求解效率低下。而对于在三维构型空间或运动空间内路径规划常见的栅格网络法，
【路径规划】基于matlab A_star算法机器人栅格地图最短路径规划【含Matlab源码 496期】.mp4
02-22
步骤一：将所有文件放到Matlab的当前文件夹中； 步骤二：双击打开main.m文件； 步骤三：点击运行，等程序运行完得到结果； 4、仿真咨询 如需其他服务，可私信博主或扫描视频QQ名片； 4.1 博客或资源的完整代码...
【路径规划】基于matlab Theta_star算法机器人栅格地图最短路径规划【含Matlab源码 2618期】.mp4
02-22
步骤一：将所有文件放到Matlab的当前文件夹中； 步骤二：双击打开main.m文件； 步骤三：点击运行，等程序运行完得到结果； 4、仿真咨询 如需其他服务，可私信博主或扫描视频QQ名片； 4.1 博客或资源的完整代码...
【路径规划】 A_star算法机器人栅格地图最短路径规划【含Matlab源码 1388期】.zip
06-23
Matlab领域上传的全部代码均可运行，亲测可用，直接替换数据即可，适合小白； 1、代码压缩包内容 主函数：main.m； 调用函数：其他m文件；无需运行 ...4.4.5 萤火虫算法FA/差分算法DE避障路径规划
【路径规划】 A_star算法机器人栅格地图路径规划【含Matlab源码 116期】.zip
06-23
Matlab领域上传的全部代码均可运行，亲测可用，直接替换数据即可，适合小白； 1、代码压缩包内容 主函数：main.m； 调用函数：其他m文件；无需运行 ...4.4.5 萤火虫算法FA/差分算法DE避障路径规划
【路径规划】 A_Star和D_Star算法求解机器人栅格地图最短路径规划【含Matlab源码 2472期】.zip
06-23
Matlab领域上传的全部代码均可运行，亲测可用，直接替换数据即可，适合小白； 1、代码压缩包内容 主函数：main.m； 调用函数：其他m文件；无需运行 ...4.4.5 萤火虫算法FA/差分算法DE避障路径规划
【无标题】Matlab 之axes函数——创建笛卡尔坐标区
weixin_44202064的博客
02-16 1017
要求1：指定第一个 Axes 对象的位置，使其左下角位于点 (0.1 0.1) 处，宽度和高度均为 0.7。总结：axes(parent,Name,Value) 在由 parent 指定的图窗、面板或选项卡中创建坐标区，而不是在当前图窗中创建。总结：函数axes(Name,Value) 使用一个或多个名称-值对组参数修改坐标区的外观，或控制数据的显示方式。在坐标区上添加一个线图。应用场景2：将坐标区设置为当前坐标区。**基本用法：**axes 在当前图窗中创建默认的笛卡尔坐标区，并将其设置为当前坐标区。
【TWVRP】基于matlab遗传和粒子群算法求解带时间窗的车辆路径规划问题【含Matlab源码 1037期】
订阅付费专栏（初级版），可赠送该付费专栏任意代码1份；
06-20 2824
遗传和粒子群算法求解带时间窗的车辆路径规划问题 完整的代码，方可运行；可提供运行操作视频！适合小白！
【路径规划】蚁群算法机器人栅格地图最短路径规划【含Matlab源码 1618期】
Matlab领域的博客
01-28 2029
蚁群算法机器人栅格地图最短路径规划 完整的代码，方可运行；可提供运行操作视频！适合小白！
【A_star三维路径规划】基于matlab A_star算法无人机三维路径规划【含Matlab源码 003期】
订阅付费专栏（初级版），可赠送该付费专栏任意代码1份；
12-30 2871
A_star算法无人机三维路径规划 完整代码，直接运行，适合小白！可提供运行操作视频！
在matlab中使用A*算法进行二维路径规划
qq_63079839的博客
09-15 2808
增设参数Corner_obstacles，在计算斜向拓展代价时，判断垂直其拓展方向的两侧有没有障碍物，如向父节点左上方拓展时，需判断父节点的左方以及上方是否有障碍物，若有则不能拓展，以此获得不触碰障碍物四角的安全路线，如右下图。考虑无人机等在进行直角拐弯时，速度会受到影响，进而增加耗能，因此增设参数quarter_turn表示每一次直角拐弯的代价，使规划的路径尽可能减少直角拐弯的次数，也使的路径更加圆滑。在findvalue函数中，将原有的四方向增加为八方向，并根据之前定义的每个方向的含义编写代码。
Astart寻路算法
DotNetFirst的专栏
07-21 1804
use A* to find path.../*written by hjjboyemail:tianmashuangyi@163.comqq:156809986*/var closelist=new Array(),openlist=new Array();var gw=10,gh=10,gwh=14;var p_start=new Array(2),p_end=new Array(2);var
【VRP】基于matlab遗传算法求解多车辆路径规划问题【含Matlab源码 1249期】
订阅付费专栏（初级版），可赠送该付费专栏任意代码1份；
08-24 1万+
遗传算法求解多车辆路径规划问题 完整代码，直接运行，适合小白！可提供运行操作视频！
依据象限搜索及混合预计耗费的A*改进算法，包含8邻域及24邻域的改进
slandarer的博客
06-24 1378
注： 以下改进皆为在传统 A* 算法基础下的改进，请先了解传统 A* 算法后再对本文进行了解。 1 邻域改进 当终点相对于当前点在不同象限时，采取不同的搜索邻域能够减小检验节点数量。 首先要确定终点相对于当前节点象限，我们定义向量： V=[goal.x−cur.x2，goal.y−cur.y2] V=[\frac{goal.x-cur.x}{2}，\frac{goal.y-cur.y}{2}] V=[2goal.x−cur.x​，2goal.y−cur.y​] 其中goalgoalgoal为目标结点，cur
评论 1
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

海神之光

有机会获得赠送范围1份代码

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值