Dijkstra及A*算法程序

matlab实现Dijkstra及A*路径规划及地图生成。根据程序可一窥路径规划算法原理

%% % set up color map for display 
clc;
clear;
cmap = [1 1 1; ...% 1 - white - clear cell 
        0 0 0; ...% 2 - black - obstacle 
        1 0 0; ...% 3 - red = visited 
        0 0 1; ...% 4 - blue - on list 
        0 1 0; ...% 5 - green - start 
        1 1 0];% 6 - yellow - destination 
colormap（cmap）; 
map = zeros（100）; 
% Add an obstacle 
map （30:35 40:75） = 2; 
map （70:75 40:75） = 2;
map （36:69 71:74） = 2; 
startrow=60;
startcol=20;
tagrow=40;
tagcol=80;
map（startrowstartcol） = 5; % start_coords 
map（tagrowtagcol） = 6; % dest_coords 
image（1.51.5map）; 
grid on; 
axis image; 

% % % % %% % % % % % % %init value% % % % % %% % % % % %
distancemap=inf（length（map））;
distancemapselect=inf（length（map））;
Finaldistancemap=inf（length（map））;


current=sub2ind（size（map）startrowstartcol）;
distancemap（current）=0;
distancemapselect（current）=0;
Finaldistancemap（current）=0;
start_node = sub2ind（size（map）startrowstartcol）; 
dest_node = sub2ind（size（map） tagrowtagcol）; 
neighbor=[01;0-1;10;-10];

openlist=struct（‘row‘0‘col‘0‘dist‘0‘father‘0）;
closelist=struct（‘row‘0‘col‘0‘dist‘0‘father‘0）;

openlist（1）.row=12;
openlist（2）.row=4;
neighborlen=length（neighbor）;
min_dist=0;
[nrowsncols]=size（map）;
BreakFlag=0;
Hvalue=0;
Gvalue=0;
 
% % % % % % % % % % % % % % % % % % % % % % % % %% % % % % %
while BreakFlag~=1
 image（1.5 1.5 map）; 
 grid on; 
 axis image; 
 drawnow; 
 [~current]=min（distancemapselect（:））;
min_dist=distancemap（current）;
%  [min_distindex]=min（distancemap（:））;
 [m n] = ind2sub（size（distancemapselect）current）;
neighbor=[mn+1;...
          mn-1;...
          m+1n;...
          m-1n];
outRangetest = （neighbor（:1）<1） + （neighbor（:1）>nrows） +...
                   （neighbor（:2）<1） + （neighbor（:2）>ncols ） ;
locate = find（outRangetest>0）; 
neighbor（locate:）=[]; 

for i=1:length（neighbor）
    
 neighborsub=sub2ind（size（map）neighbor（i1）neighbor（i2））;
 if neighborsub==dest_node
    BreakFlag=1;
    distancemap（neighborsub）=min_dist+1; 
    Finaldistancemap（neighborsub）=min_dist+1;
    closelist（dest_node）.father=current;
     break;    
 end 
if map（neighborsub）~=2&&map（neighborsub）~=5&&map（neighborsub）~=6
[openlist（i）.rowopenlist（i）.row]=ind2sub（size（map）neighborsub）;
openlist（i）.dist=min_dist+1;
if Finaldistancemap（neighborsub）==inf
distancemap（neighborsub）=min_dist+1; 
Hvalue=abs（tagrow-neighbor（i1））+abs（tagcol-neighbor（i2））;
distancemapselect（neighborsub）=distancemap（neighborsub）+Hvalue;
Finaldistancemap（neighborsub）=min_dist+1;
map（neighborsub）=3;
closelist（neighborsub）.father=current;
end

end
    
end
distancemap（current）=inf;
distancemapselect（current）=inf;
end
path=0;
pathindx=dest_node;
for i=1:Finaldistancemap（dest_node）

path=[pathcloselist（pathindx）.father];
pathindx=closelist（pathindx）.father;
end
        for k = 2:length（path） - 1 
          map（path（k）） = 7; 
                paus

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        3121  2018-11-29 17:07  AstarCsl.m
     文件        2735  2018-11-29 00:26  Dijkstra.m