A星算法matlab源碼及詳細(xì)注釋

1、A星算法matlab源碼及詳細(xì)注釋 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% $Revision: 0.1 $ $

2、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 star will co

3、mpute 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; setOpenCosts = 0; setO

4、penHeuristics = 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 magicaxishandl

5、e = 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中相應(yīng)位置的元素在B中也出現(xiàn)，0則是沒有出現(xiàn)% for the element in OPEN with the smalles

6、t costtemp, ii = min(setOpenCosts + setOpenHeuristics); %從OPEN表中選擇花費最低的點temp,ii是其下標(biāo)(也就是標(biāo)號索引)% find costs and heuristic of moving to neighbor spaces to goal% in order 'R','L','D','U'costs,heuristics,posinds = findFValue(setOpen(ii),setOp

7、enCosts(ii), .field,goalposind,'euclidean'); %擴(kuò)展temp的四個方向點，獲得其坐標(biāo)posinds，各個方向點的實際代價costs，啟發(fā)代價heuristics% put node in CLOSED and record its costsetClosed = setClosed; setOpen(ii); %將temp插入CLOSE表中setClosedCosts = setClosedCosts; setOpenCosts(ii); %將temp的花費計入ClosedCosts% update OPEN and

8、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); setOpenHeuristics(ii+1:en

9、d);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-1);end% for e

10、ach 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) %對于擴(kuò)展的四個方向的坐標(biāo)% if cost infinite, then it's a wall, so ignoreif isinf(costs(jj) %如果此點的實際代價不為Inf,也就是沒有遇到墻% if node

11、 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); %將此點的方向存在對應(yīng)的fieldpointers中costchart(posinds(jj) = costs(jj); %將實際代價值存入對應(yīng)的costchart中setOpen

12、 = setOpen; posinds(jj); %將此點加入OPEN表中setOpenCosts = setOpenCosts; costs(jj); %更新OPEN表實際代價setOpenHeuristics = setOpenHeuristics; heuristics(jj); %更新OPEN表啟發(fā)代價% else node has already been seen, so check to see if we have% found a better route to it.elseif max(setOpen = posinds(jj) %如果此點在OPEN表中I = find(

13、setOpen = posinds(jj); %找到此點在OPEN表中的位置% update if we have a better routeif setOpenCosts(I) > costs(jj) %如果在OPEN表中的此點實際代價比現(xiàn)在所得的大costchart(setOpen(I) = costs(jj); %將當(dāng)前的代價存入costchart中，注意此點在costchart中的坐標(biāo)與其自身坐標(biāo)是一致的（setOpen(I)其實就是posinds(jj)），下同fieldpointerssetOpenCosts(I) = costs(jj); %更新OPEN表中的此點

14、代價，注意此點在setOpenCosts中的坐標(biāo)與在setOpen中是一致的，下同setOpenHeuristicssetOpenHeuristics(I) = heuristics(jj); %更新OPEN表中的此點啟發(fā)代價(竊以為這個是沒有變的)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 %如果此點在CLOSE表中，說

15、明已經(jīng)擴(kuò)展過此點% find relevant node in CLOSEDI = find(setClosed = posinds(jj); % update if we have a better routeif setClosedCosts(I) > costs(jj) %如果在CLOSE表中的此點實際代價比現(xiàn)在所得的大（有一個問題，經(jīng)過此點擴(kuò)展的點還需要更新當(dāng)前代價呢!）costchart(setClosed(I) = costs(jj); %將當(dāng)前的代價存入costchart中setClosedCosts(I) = costs(jj); %更新CLOSE表中的此點代價f

16、ieldpointers(setClosed(I) = movementdirections(jj); %更新此點的方向end endendendif ise mpty(setOpen) break; end %當(dāng)OPEN表為空，代表可以經(jīng)過的所有點已經(jīng)查詢完畢set(axishandle,'CData',costchart costchart(:,end); costchart(end,:) costchart(end,end);% hack to make image look rightset(gca,'CLim',0 1.

17、1*max(costchart(find(costchart < Inf); %CLim將CData中的值與colormap對應(yīng)起來: 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(ismember(setOpen,g

18、oalposind) %當(dāng)找到目標(biāo)點時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,'Color&am

19、p;#39;,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 positionn = length

20、(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') %當(dāng)查詢到的點不是'S'起點時switch fieldpointe

21、rsposindcase '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,py,px);end%

22、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 = length(field); %

23、 length of the field% convert linear index into row columncurrentpos(1) currentpos(2) = ind2sub(n n,posind); %獲得當(dāng)前點的行列坐標(biāo)，注意currentpos(1)是列坐標(biāo)，currentpos(2)是行坐標(biāo)goalpos(1) goalpos(2) = ind2sub(n n,goalind); %獲得目標(biāo)點的行列坐標(biāo)% places to store movement cost value and positioncost = Inf*ones(4,1); heuristic = I

24、nf*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; %獲得新的坐標(biāo)switch lower(heuristicmethod)case 'euclidean' %歐幾里得距離(不像啊，親)heuristic(1) = abs(goalpos(2)

25、-newx) + abs(goalpos(1)-newy); %heuristic(1)為啟發(fā)函數(shù)計算的距離代價case 'taxicab'heuristic(1) = abs(goalpos(2)-newx) + abs(goalpos(1)-newy); endcost(1) = costsofar + field(newy,newx); %costsofar為之前花費的代價，field(newy,newx)為環(huán)境威脅代價，cost(1)為經(jīng)過此方向點的真實代價end% if we can look right, we move from the left

26、向右查詢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(goalpos(1)-newy);

27、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)-newy);case &am

28、p;#39;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 'euc

29、lidean'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); %posinds為此點擴(kuò)展的四個

30、方向上的坐標(biāo)% 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 values, and a

31、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)下取整，即舍去正小數(shù)至最近整數(shù)，ceil(x)上取整，即加入正小數(shù)至最近整數(shù)，Inf代表正無窮field(ceil(n2.*rand(floor(n*n*wallperce

32、nt),1) = Inf; %ind2sub是用來將線性坐標(biāo)(總體位置序號) 轉(zhuǎn)為多維坐標(biāo)(包含行列的坐標(biāo))的，發(fā)現(xiàn)其實不用轉(zhuǎn)為多維坐標(biāo)就可以，矩陣field可以訪問線性坐標(biāo)% create random start position and goal position 隨機(jī)選擇行列作為起點與終點startposind = sub2ind(n,n,ceil(n.*rand),ceil(n.*rand); %sub2ind用來將行列坐標(biāo)轉(zhuǎn)換為線性坐標(biāo)，這里是必要的，因為如果把startposind設(shè)置成x,y的形式，訪問field(x,y)的時候goalposind = sub2ind(n,n,

33、ceil(n.*rand),ceil(n.*rand); %它并不是訪問x行y列元素，而是訪問線性坐標(biāo)為x和y的兩個元素% force movement cost at start and goal positions to not be walls 將初始坐標(biāo)設(shè)置為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); %costchart用來存儲各個點的實

34、際代價，NaN代表不是數(shù)據(jù)（不明確的操作）% 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" for goal 起點設(shè)置

35、為"S",終點設(shè)置為"G"fieldpointersstartposind = 'S' fieldpointersgoalposind = 'G'% everywhere there is a wall, put a 0 so it is not considered 墻設(shè)置為0fieldpointers(field = Inf) = 0; %很好的方式，field = Inf 返回墻的位置，fieldpointers(field = Inf)設(shè)置相應(yīng)的位置%

36、 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是當(dāng)前返回圖像的句柄f1 = figure('Position',450 150 500 500,'Units',&#39

37、;Normalized', . 'MenuBar','none'); %這里的Position屬性值為一個四元數(shù)組 rect = left, bottom, width, height，第一、二個參數(shù)表示窗口位置，都是從屏幕的左下角計算的%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('pos

38、ition', 0.01 0.01 0.98 0.98,'FontSize',12, .'FontName','Helvetica'); %position根據(jù)前面figure設(shè)置的單位，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 獲得當(dāng)前圖像并清空gcf; cla;

39、endn = length(field);% plot field where walls are black, and everything else is white 0是黑色field(field < Inf) = 0; %注意，雖然修改了field，但是這里的field屬于局部變量，根本沒有影響主函數(shù)中的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用于反轉(zhuǎn)矩陣 colormap為產(chǎn)生jet類型的顏色表 jet ranges from blue to red% make first entry be wh