A*算法航迹规划

function[openclose]=Astar（map）
map=[0030300;0303000]
% plot（map（1:）map（2:））
% axis （[0 30 0 30]）
[rowcol]=size（map）;
close=struct（‘row‘-1‘col‘-1‘g‘0‘h‘0）;%????????????????????????
closelen=1;
open=struct（‘row‘-1‘col‘-1‘g‘-1‘h‘-1）;%????????????????????????
openlen=0;
bindex=1;
for k=1:row
   for j=1:col
       if map（kj）==1
            barrierrow（bindex）=-k;
            barriercol（bindex）=j;
            bindex=bindex+1;
       end
   end
end
%????????????
for i=1:row
   for j=1:col
       if map（ij）==2
            endrow=i;
            endcol=j;
           break;
       end
   end
end
%????????????????????close
for i=1:row
   for j=1:col
     if map（ij）==5
          startrow=i;
          startcol=j;
          close（1）.row=i;
          close（1）.col=j;
         break;
     end
   end
end
%??????????????????open
%????????
direct=[0 -1;0 1;-1 0;1 0];
for i=1:4
   if all（[close（1）.rowclose（1）.col]+direct（i:）>0） && close（1）.row+direct（i1）<=row && close（1）.col+direct（i2）<=col && map（close（1）.row+direct（i1）close（1）.col+direct（i2））~=1
        open（openlen+1）.row=close（1）.row+direct（i1）;
        open（openlen+1）.col=close（1）.col+direct（i2）;
        openlen=openlen+1;
       %????g??????h????????
        open（openlen）.g=1;
        open（openlen）.h=abs（endrow-open（openlen）.row）+abs（endcol-open（openlen）.col）;
   end
end
% close
% open.h
 
%????????open??colse????????????????????????????????????
while openlen>0
   %????????????g+h????open????????
    min = realmax;
   for i=1:openlen
       if open（i）.g+open（i）.h<=min
            min=open（i）.g+open（i）.h;
            sindex=i;
       end
   end
   
   %??s????????close????????open??
    close（closelen+1）.row=open（sindex）.row;
    close（closelen+1）.col=open（sindex）.col;
    close（closelen+1）.g=open（sindex）.g;
    close（closelen+1）.h=open（sindex）.h;
    closelen=closelen+1;
%     openlen=openlen-1;
%     for i=sindex:openlen
%         open（i）