astarm.m

航迹规划中的AStar算法

%
%
%           A - S T A R   By Emad Hasan
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Astar_path, ts, e_time] = Astar(Astar_coord, Astar_connect, s, d);
tic;
%%
openlist = [];

closedlist = [];

%1. Add the source node to the open list

openlist = [s s 0]';




% Define what is targe is in closed list (tiscl)
tiicl=0;
openlempty=0; ts=0;
%%



while ( tiicl==0 || openlempty==0)
    
%%
    ts=ts+1;
    [widop,lenol]=size(openlist);
    olcst=zeros(4,lenol);
    % Generate Cost function for Open list (open list cost) storing G H F
    olcst(1,:)=openlist(1,:);
    % Calculate G for the Open list
    d2cost=abs(Astar_coord(:,openlist(2,:))-Astar_coord(:,openlist(1,:)));
    olcst(2,:)=d2cost(1,:)+d2cost(2,:)+openlist(3,:);
    % Caculate H for the open list
    d2cost=Astar_coord(:,d)*ones(1,lenol)-Astar_coord(:,openlist(1,:));
    olcst(3,:)=abs(d2cost(1,:)+d2cost(2,:));
    % Calculate F = G + H
    olcst(4,:)=olcst(2,:)+olcst(3,:);
 
    
    % a) Look for the lowest F cost square on the open list. We refer to
    % this as the current square. 
    x=find(olcst(4,:)==min(olcst(4,:)),1);
    curnode=olcst(1,x); % the node that has the lowest cost
    
    olindex=find(openlist(1,:)==curnode);
    curnodecst=openlist(3,olindex);
    
  % b) Switch it to the closed list.% 
    [clw,lencl]=size(closedlist);
    % add node and its parent to closed list
    closedlist(:,lencl+1)=[olcst(1,x) openlist(2,olindex) openlist(3,olindex)]';
    % Remove it from the open list
    openlist(:,olindex)=[];
    
        
% c) For each of the squares adjacent to this current square