【路径规划】基于matlab A_star算法机器人避障自动寻路路径规划【含Matlab源码 496期】

原创

佛怒火莲 2021-11-08 12:48:12 ©著作权

文章标签 matlab 算法自动驾驶 sed 搜索 文章分类 数据结构与算法人工智能

©著作权归作者所有：来自51CTO博客作者佛怒火莲的原创作品，请联系作者获取转载授权，否则将追究法律责任

一、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, 且可以沿对角移动 (估值不考虑对角移动) .其搜索路径流程如下:

【路径规划】基于matlab A_star算法机器人避障自动寻路路径规划【含Matlab源码 496期】_搜索

图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所示.

【路径规划】基于matlab A_star算法机器人避障自动寻路路径规划【含Matlab源码 496期】_搜索_02

图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摆放的图像与存入的数组不一样，需要先逆时针旋转90*3=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