A星算法matlab源码及详细注释(共14页)

1、精选优质文档-倾情为你奉上A星算法matlab源码及详细注释 function astardemo%ASTARDEMO Demonstration of ASTAR algorithm% Copyright Bob L. Sturm, Ph. D., Assistant Professor% Department of Architecture, Design and Media Technology% formerly Medialogy% Aalborg University i Ballerup% formerly Aalborg University Copenhagen% $Revi

2、sion: 0.1 $ $Date: 2011 Jan. 15 18h24:24$n = 20; % field size n x n tiles 20*20的界面wallpercent = 0.45; % this percent of field is walls 45%的界面作为阻碍物（墙）% create the n x n FIELD with wallpercent walls containing movement costs, % a starting position STARTPOSIND, a goal position GOALPOSIND, the costs % A

3、 star will compute movement cost for each tile COSTCHART, % and a matrix in which to store the pointers FIELDPOINTERSfield, startposind, goalposind, costchart, fieldpointers = .initializeField(n,wallpercent); %初始化界面% initialize the OPEN and CLOSED sets and their costssetOpen = startposind; setOpenCo

4、sts = 0; setOpenHeuristics = Inf;setClosed = ; setClosedCosts = ;movementdirections = 'R','L','D','U'% keep track of the number of iterations to exit gracefully if no solutioncounterIterations = 1;% create figure so we can witness the m

5、agicaxishandle = createFigure(field,costchart,startposind,goalposind);% as long as we have not found the goal or run out of spaces to explorewhile max(ismember(setOpen,goalposind) && isempty(setOpen) %ismember(A,B)返回与A同大小的矩阵，其中元素1表示A中相应位置的元素在B中也出现，0则是没有出现% for the element in OPEN wit

6、h the smallest costtemp, ii = min(setOpenCosts + setOpenHeuristics); %从OPEN表中选择花费最低的点temp,ii是其下标(也就是标号索引)% find costs and heuristic of moving to neighbor spaces to goal% in order 'R','L','D','U'costs,heuristics,posinds = findFValue(setO

7、pen(ii),setOpenCosts(ii), .field,goalposind,'euclidean'); %扩展temp的四个方向点，获得其坐标posinds，各个方向点的实际代价costs，启发代价heuristics% put node in CLOSED and record its costsetClosed = setClosed; setOpen(ii); %将temp插入CLOSE表中setClosedCosts = setClosedCosts; setOpenCosts(ii); %将temp的花费计入ClosedCosts% upd

8、ate OPEN and their associated costs 更新OPEN表 分为三种情况if (ii > 1 && ii < length(setOpen) %temp在OPEN表的中间，删除tempsetOpen = setOpen(1:ii-1); setOpen(ii+1:end);setOpenCosts = setOpenCosts(1:ii-1); setOpenCosts(ii+1:end);setOpenHeuristics = setOpenHeuristics(1:ii-1); setOpenHeuri

9、stics(ii+1:end);elseif (ii = 1)setOpen = setOpen(2:end); %temp是OPEN表的第一个元素，删除tempsetOpenCosts = setOpenCosts(2:end);setOpenHeuristics = setOpenHeuristics(2:end);else %temp是OPEN表的最后一个元素，删除tempsetOpen = setOpen(1:end-1);setOpenCosts = setOpenCosts(1:en d-1);setOpenHeuristics = setOpenHeuristics(1:end-

10、1);end% for each of these neighbor spaces, assign costs and pointers; % and if some are in the CLOSED set and their costs are smaller, % update their costs and pointersfor jj=1:length(posinds) %对于扩展的四个方向的坐标% if cost infinite, then it's a wall, so ignoreif isinf(costs(jj) %如果此点的实际代价不为Inf,也就是没

11、有遇到墙% if node is not in OPEN or CLOSED then insert into costchart and % movement pointers, and put node in OPENif max(setClosed; setOpen = posinds(jj) %如果此点不在OPEN表和CLOSE表中fieldpointers(posinds(jj) = movementdirections(jj); %将此点的方向存在对应的fieldpointers中costchart(posinds(jj) = costs(jj); %将实际代价值存入对应的cost

12、chart中setOpen = setOpen; posinds(jj); %将此点加入OPEN表中setOpenCosts = setOpenCosts; costs(jj); %更新OPEN表实际代价setOpenHeuristics = setOpenHeuristics; heuristics(jj); %更新OPEN表启发代价% else node has already been seen, so check to see if we have% found a better route to it.elseif max(setOpen = posinds(jj) %如果此点在OP

13、EN表中I = find(setOpen = posinds(jj); %找到此点在OPEN表中的位置% update if we have a better routeif setOpenCosts(I) > costs(jj) %如果在OPEN表中的此点实际代价比现在所得的大costchart(setOpen(I) = costs(jj); %将当前的代价存入costchart中，注意此点在costchart中的坐标与其自身坐标是一致的（setOpen(I)其实就是posinds(jj)），下同fieldpointerssetOpenCosts(I) = costs(jj);

14、 %更新OPEN表中的此点代价，注意此点在setOpenCosts中的坐标与在setOpen中是一致的，下同setOpenHeuristicssetOpenHeuristics(I) = heuristics(jj); %更新OPEN表中的此点启发代价(窃以为这个是没有变的)fieldpointers(setOpen(I) = movementdirections(jj); %更新此点的方向 end% else node has already been CLOSED, so check to see if we have% found a better route to it.else %如

15、果此点在CLOSE表中，说明已经扩展过此点% find relevant node in CLOSEDI = find(setClosed = posinds(jj); % update if we have a better routeif setClosedCosts(I) > costs(jj) %如果在CLOSE表中的此点实际代价比现在所得的大（有一个问题，经过此点扩展的点还需要更新当前代价呢!）costchart(setClosed(I) = costs(jj); %将当前的代价存入costchart中setClosedCosts(I) = costs(jj); %更新

16、CLOSE表中的此点代价fieldpointers(setClosed(I) = movementdirections(jj); %更新此点的方向end endendendif ise mpty(setOpen) break; end %当OPEN表为空，代表可以经过的所有点已经查询完毕set(axishandle,'CData',costchart costchart(:,end); costchart(end,:) costchart(end,end);% hack to make image look rightset(gca,'CLim&

17、amp;#39;,0 1.1*max(costchart(find(costchart < Inf); %CLim将CData中的值与colormap对应起来: cmin cmax Color axis limits （不过不太明白为什么要*1.1）drawnow; %cmin is the value of the data mapped to the first color in the colormap. cmax is the value of the data mapped to the last color in the colormapendif max(ismem

18、ber(setOpen,goalposind) %当找到目标点时disp('Solution found!'); %disp： Display array， disp(X)直接将矩阵显示出来，不显示其名字，如果X为string，就直接输出文字X% now find the way back using FIELDPOINTERS, starting from goal positionp = findWayBack(goalposind,fieldpointers);% plot final pathplot(p(:,2)+0.5,p(:,1)+0.5,&

19、;#39;Color',0.2*ones(3,1),'LineWidth',4);drawnow;elseif isempty(setOpen)disp('No Solution!'); end% end of the main function%function p = findWayBack(goalposind,fieldpointers)% This function will follow the pointers from the goal position to the% starting posit

20、ionn = length(fieldpointers); % length of the fieldposind = goalposind;% convert linear index into row columnpy,px = ind2sub(n,n,posind);% store initial positionp = py px;% until we are at the starting positionwhile strcmp(fieldpointersposind,'S') %当查询到的点不是'S'起点时switc

21、h fieldpointersposindcase 'L' % move left 如果获得该点的来源点方向为左时px = px - 1;case 'R' % move rightpx = px + 1;case 'U' % move uppy = py - 1;case 'D' % move downpy = py + 1;endp = p; py px;% convert row column to linear indexposind = sub2ind(n n

22、,py,px);end% end of this function%function cost,heuristic,posinds = findFValue(posind,costsofar,field, .goalind,heuristicmethod)% This function finds the movement COST for each tile surrounding POSIND in% FIELD, returns their position indices POSINDS. They are ordered: right,% left, down, up.n = len

23、gth(field); % length of the field% convert linear index into row columncurrentpos(1) currentpos(2) = ind2sub(n n,posind); %获得当前点的行列坐标，注意currentpos(1)是列坐标，currentpos(2)是行坐标goalpos(1) goalpos(2) = ind2sub(n n,goalind); %获得目标点的行列坐标% places to store movement cost value and positioncost = Inf*ones(4,1);

24、heuristic = Inf*ones(4,1); pos = ones(4,2); % if we can look left, we move from the right 向左查询，那么就是从右边来newx = currentpos(2) - 1; n ewy = currentpos(1); if newx > 0 %如果没有到边界pos(1,:) = newy newx; %获得新的坐标switch lower(heuristicmethod)case 'euclidean' %欧几里得距离(不像啊，亲)heuristic(1) = a

25、bs(goalpos(2)-newx) + abs(goalpos(1)-newy); %heuristic(1)为启发函数计算的距离代价case 'taxicab'heuristic(1) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy); endcost(1) = costsofar + field(newy,newx); %costsofar为之前花费的代价，field(newy,newx)为环境威胁代价，cost(1)为经过此方向点的真实代价end% if we can look right, we move f

26、rom the left 向右查询newx = currentpos(2) + 1; newy = currentpos(1);if newx <= npos(2,:) = newy newx;switch lower(heuristicmethod)case 'euclidean'heuristic(2) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy);case 'taxicab'heuristic(2) = abs(goalpos(2)-newx) + abs(goal

27、pos(1)-newy);endcost(2) = costsofar + field(newy,newx);end% if we can look up, we move from down 向上查询newx = currentpos(2); newy = currentpos(1)-1;if newy > 0pos(3,:) = newy newx;switch lower(heuristicmethod)case 'euclidean'heuristic(3) = abs(goalpos(2)-newx) + abs(goalpos(1)-n

28、ewy);case 'taxicab'heuristic(3) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy);endcost(3) = costsofar + field(newy,newx);end% if we can look down, we move from up 向下查询newx = currentpos(2); newy = currentpos(1)+1;if newy <= npos(4,:) = newy newx;switch lower(heuristicmethod)case

29、 'euclidean'heuristic(4) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy);case 'taxicab'heuristic(4) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy);endcost(4) = costsofar + field(newy,newx);end% return row column to linear indexposinds = sub2ind(n n,pos(:,1),pos(:,2); %po

30、sinds为此点扩展的四个方向上的坐标% end of this function%初始化界面function field, startposind, goalposind, costchart, fieldpointers = .initializeField(n,wallpercent) % This function will create a field with movement costs and walls, a start% and goal position at random, a matrix in which the algorithm will store % f v

31、alues, and a cell matrix in which it will store pointers% create the field and place walls with infinite cost 初始化界面和墙field = ones(n,n) + 10*rand(n,n); % field(ind2sub(n n,ceil(n2.*rand(floor(n*n*wallpercent),1) = Inf; %floor(x)下取整，即舍去正小数至最近整数，ceil(x)上取整，即加入正小数至最近整数，Inf代表正无穷field(ceil(n2.*rand(floor(

32、n*n*wallpercent),1) = Inf; %ind2sub是用来将线性坐标(总体位置序号) 转为多维坐标(包含行列的坐标)的，发现其实不用转为多维坐标就可以，矩阵field可以访问线性坐标% create random start position and goal position 随机选择行列作为起点与终点startposind = sub2ind(n,n,ceil(n.*rand),ceil(n.*rand); %sub2ind用来将行列坐标转换为线性坐标，这里是必要的，因为如果把startposind设置成x,y的形式，访问field(x,y)的时候goalposind =

33、 sub2ind(n,n,ceil(n.*rand),ceil(n.*rand); %它并不是访问x行y列元素，而是访问线性坐标为x和y的两个元素% force movement cost at start and goal positions to not be walls 将初始坐标设置为0，以免成为墙field(startposind) = 0; field(goalposind) = 0; % put not a numbers (NaN) in cost chart so A* knows where to look costchart = NaN*ones(n,n); %costc

34、hart用来存储各个点的实际代价，NaN代表不是数据（不明确的操作）% set the cost at the starting position to be 0costchart(startposind) = 0; %起点的实际代价% make fieldpointers as a cell array 生成n*n的元胞fieldpointers = cell(n,n); %fieldpointers用来存储各个点的来源方向% set the start pointer to be "S" for start, "G"

35、for goal 起点设置为"S",终点设置为"G"fieldpointersstartposind = 'S' fieldpointersgoalposind = 'G'% everywhere there is a wall, put a 0 so it is not considered 墙设置为0fieldpointers(field = Inf) = 0; %很好的方式，field = Inf 返回墙的位置，fieldpointers(field =

36、 Inf)设置相应的位置% end of this function% function axishandle = createFigure(field,costchart,startposind,goalposind)% This function creates a pretty figure% If there is no figure open, then create oneif isempty(gcbf) %gcbf是当前返回图像的句柄f1 = figure('Position',450 150 500 500,'Units&

37、#39;,'Normalized', . 'MenuBar','none'); %这里的Position属性值为一个四元数组 rect = left, bottom, width, height，第一、二个参数表示窗口位置，都是从屏幕的左下角计算的%normalized Units map the lower-left corner of the figure window to (0,0) and the upper-right corner to (1.0,1.0). Caxes2 = axes

38、('position', 0.01 0.01 0.98 0.98,'FontSize',12, .'FontName','Helvetica'); %position根据前面figure设置的单位，in normalized units where (0,0) is the lower-left corner and (1.0,1.0) is the upper-rightelse% get the current figure, and clear it 获得当前图

39、像并清空gcf; cla;endn = length(field);% plot field where walls are black, and everything else is white 0是黑色field(field < Inf) = 0; %注意，虽然修改了field，但是这里的field属于局部变量，根本没有影响主函数中的fieldpcolor(1:n+1,1:n+1,field field(:,end); field(end,:) f ield(end,end); %多了一行一列% set the colormap for the ploting the cost and looking really nicecmap = flipud(colormap('jet'); %flipud用于反转矩阵 colormap为产生jet类型的颜色表 jet ranges from blue to red% make first entry be white, an