一、简介
A算法
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
% 04-26-2005
% Copyright 2009-2010 The MathWorks, Inc.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%DEFINE THE 2-D MAP ARRAY
%
% n=input('Please insert the obstacle numbers');
%
% for i=1:n
%
% ab=input('Please insert the obstacle numbers')
% obstacles(i,:)
clear all
close all
clc
tic
%加载地形数据
data1=load('newMoun300.dat');
l = 100;%地形数据长,即y轴数据长度
w = 150;%地形数据宽，即x轴数据长度
data=reshape(data1,l,w);
mapstep = 5;%网格大小
nl = l/mapstep;
nw = w/mapstep;%nw最大值30
OPEN_COUNT=0;
CLOSED_COUNT=0;
MAP=2*(ones(nw,nl));
for z=1:1:3
% data(nl,nw);
z
p0.x = 20;
p0.y = 60;
p0.z = 1754;%起点坐标
xval=p0.x/mapstep;
yval=p0.y/mapstep;
MAP(p0.x/mapstep,p0.y/mapstep)=1;
% xval=p0.x;
% yval=p0.y;
% MAP(p0.x,p0.y)=1;
xStart=xval;%Starting Position起始点1
yStart=yval;%Starting Position
p1.x = 120;
p1.y = 60;
p1.z = 1342;%终点坐标
xTarget=floor(p1.x/mapstep);
yTarget=floor(p1.y/mapstep);
% zTarget=floor(p1.z);
%
MAP(p1.x/mapstep,p1.y/mapstep)=0;
% xTarget=floor(p1.x);
% yTarget=floor(p1.y);
% % zTarget=floor(p1.z);
%
% MAP(p1.x,p1.y)=0;
% maxpoint = (p1.x-p0.x)/mapstep;%最大点个数clc
switch z
case 1
MAP=dataprocess(MAP);
% % dataprocess(nw,nl,mapstep);%处理地形数据 dataprocess;%处理地形数据
% i=1
% for i1 = 10:1:14
% for j1 = 10:1:12
% MAP(i1,j1)=-1;
%
% end
% end
% for i1 = 6:1:10
% for j1 = 16:1:18
% MAP(i1,j1)=-1;
%
% end
% end
% for i1 = 14:1:18
% for j1 = 16:1:18
% MAP(i1,j1)=-1;
%
% end
% end
% continue;
case 2
% dataprocess1(nw,nl,mapstep);%处理障碍物1地形数据
MAP=dataprocess1(MAP);%处理地形数据
% i=2
% for i1 = 10:1:14
% for j1 = 10:1:12
% MAP(i1,j1)=-1;
%
% end
% end
% for i1 = 6:1:10
% for j1 = 16:1:18
% MAP(i1,j1)=-1;
%
% end
% end
% for i1 = 14:1:18
% for j1 = 16:1:18
% MAP(i1,j1)=-1;
%
% end
% end
% for i1 = 9:1:11
% for j1 = 13:1:15
% MAP(i1,j1)=-1;
%
% end
% end
% continue;
case 3
% dataprocess2(nw,nl,mapstep);%处理障碍物2地形数据
MAP=dataprocess2(MAP);%处理地形数据
% i=3
% for i1 = 10:1:14
% for j1 = 10:1:12
% MAP(i1,j1)=-1;
%
% end
% end
% for i1 = 6:1:10
% for j1 = 16:1:18
% MAP(i1,j1)=-1;
%
% end
% end
% for i1 = 14:1:18
% for j1 = 16:1:18
% MAP(i1,j1)=-1;
%
% end
% end
% for i1 = 19:1:21
% for j1 = 11:1:13
% MAP(i1,j1)=-1;
%
% end
% end
% continue;
end
% for i1 = 10:1:14
% for j1 = 10:1:12
% MAP(i1,j1)=-1;
%
% end
% end
% for i1 = 6:1:10
% for j1 = 16:1:18
% MAP(i1,j1)=-1;
%
% end
% end
% for i1 = 14:1:18
% for j1 = 16:1:18
% MAP(i1,j1)=-1;
%
% end
% end
%
% for i1 = 10:1:14
% for j1 = 10:1:12
% MAP(i1,j1)=-1;
%
% end
% end
% for i1 = 6:1:10
% for j1 = 16:1:18
% MAP(i1,j1)=-1;
%
% end
% end
% for i1 = 14:1:18
% for j1 = 16:1:18
% MAP(i1,j1)=-1;
%
% end
% end
% for i1 = 9:1:11
% for j1 = 13:1:15
% MAP(i1,j1)=-1;
%
% end
% end
% for i1 = 10:1:14
% for j1 = 10:1:12
% MAP(i1,j1)=-1;
%
% end
% end
% for i1 = 6:1:10
% for j1 = 16:1:18
% MAP(i1,j1)=-1;
%
% end
% end
% for i1 = 14:1:18
% for j1 = 16:1:18
% MAP(i1,j1)=-1;
%
% end
% end
% for i1 = 19:1:21
% for j1 = 11:1:13
% MAP(i1,j1)=-1;
%
% end
% end
OPEN=[];
%CLOSED LIST STRUCTURE
%--------------
%X val | Y val |
%--------------
% CLOSED=zeros(MAX_VAL,2);
CLOSED=[];
%Put all obstacles on the Closed list
k=1;%Dummy counter
for i=1:nw
for j=1:nl
if(MAP(i,j) == -1)
CLOSED(k,1)=i;
CLOSED(k,2)=j;
k=k+1;
end
end
end%检测所有的障碍物，放入closed表
CLOSED_COUNT=size(CLOSED,1);
%set the starting node as the first node设置初始节点为第一节点
xNode=xval;
yNode=yval;
OPEN_COUNT=1;
path_cost=0;%h
goal_distance=distance(xNode,yNode,xTarget,yTarget);
OPEN(OPEN_COUNT,:)=insert_open(xNode,yNode,xNode,yNode,path_cost,goal_distance,goal_distance);%将起始点插入open表中，并置open表的第个元素为0，放入close表中。*从open表撤离放入close表时，open表中对应第一个元素置0
OPEN(OPEN_COUNT,1)=0;
CLOSED_COUNT=CLOSED_COUNT+1;
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');
exp_array=expand_array(xNode,yNode,path_cost,xTarget,yTarget,CLOSED,nw,nl);
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
flag=0;
for j=1:OPEN_COUNT
if(exp_array(i,1) == OPEN(j,2) && exp_array(i,2) == OPEN(j,3) )
OPEN(j,8)=min(OPEN(j,8),exp_array(i,5));
if OPEN(j,8)== exp_array(i,5)%当前节点的扩展节点与open表中坐标相同的元素比较，如果扩展节点的f小，则更新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
% if flag == 1
% break;
end;%End of j for
三、运行结果
四、备注
版本：2014a
