一、简介
A算法是一种典型的启发式搜索算法，建立在Dijkstra算法的基础之上，广泛应用于游戏地图、现实世界中，用来寻找两点之间的最短路径。A算法最主要的是维护了一个启发式估价函数，如式(1)所示。
f(n)=g(n)+h(n)(1)
其中，f(n)是算法在搜索到每个节点时，其对应的启发函数。它由两部分组成，第一部分g(n)是起始节点到当前节点实际的通行代价，第二部分h(n)是当前节点到终点的通行代价的估计值。算法每次在扩展时，都选取f(n)值最小的那个节点作为最优路径上的下一个节点。
在实际应用中，若以最短路程为优化目标，h(n)常取作当前点到终点的欧几里得距离(Euclidean Distance)或曼哈顿距离(Manhattan Distance)等。若令h(n)=0，表示没有利用任何当前节点与终点的信息，A算法就退化为非启发的Dijkstra算法，算法搜索空间随之变大，搜索时间变长。
A*算法步骤如下，算法维护两个集合：P表与Q表。P表存放那些已经搜索到、但还没加入最优路径树上的节点；Q表维护那些已加入最优路径树上的节点。
（1）P表、Q表置空，将起点S加入P表，其g值置0，父节点为空，路网中其他节点g值置为无穷大。
（2）若P表为空，则算法失败。否则选取P表中f值最小的那个节点，记为BT，将其加入Q表中。判断BT是否为终点T，若是，转到步骤（3）；否则根据路网拓扑属性和交通规则找到BT的每个邻接节点NT，进行下列步骤:
①计算NT的启发值
f(NT)=g(NT)+h(NT)(2)
g(NT)=g(BT)+cost(BT, NT)(3)
其中,cost(BT, NT)是BT到NT的通行代价。
②如果NT在P表中，且通过式(3)计算的g值比NT原先的g值小，则将NT的g值更新为式(3)结果，并将NT的父节点设为BT。
③如果NT在Q表中，且通过式(3)计算的g值比NT原先的g值小，则将NT的g值更新为式(3)结果，将NT的父节点设为BT，并将NT移出到P表中。
④若NT既不在P表，也不在Q表中，则将NT的父节点设为BT，并将NT移到P表中。
⑤转到步骤（2）继续执行。
（3）从终点T回溯，依次找到父节点，并加入优化路径中，直到起点S，即可得出优化路径。
二、源代码
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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
三、运行结果
四、备注
版本：2014a
