基于Astar算法的栅格地图目标最短路径搜索算法MATLAB仿真,带GUI界面

1.算法描述

   Astar算法是一种图形搜索算法,常用于寻路。它是个以广度优先搜索为基础,集Dijkstra算法与最佳优先(best fit)算法特点于一身的一种 算法。它通过下面这个函数来计算每个节点的优先级,然后选择优先级最高的节点作为下一个待遍历的节点。

   AStar（又称 A*），它结合了 Dijkstra 算法的节点信息（倾向于距离起点较近的节点）和贪心算法的最好优先搜索算法信息（倾向于距离目标较近的节点）。可以像 Dijkstra 算法一样保证找到最短路径，同时也像贪心最好优先搜索算法一样使用启发值对算法进行引导。简单点说，AStar的核心在于将游戏背景分为一个又一个格子，每个格子有自己的靠谱值，然后通过遍历起点的格子去找到周围靠谱的格子，接着继续遍历周围…… 最终找到终点。

实现步骤:

1.把起始格添加到开启列表。

2.重复如下的工作：

a) 寻找开启列表中估量代价F值最低的格子。我们称它为当前格。

b) 把它切换到关闭列表。

c) 对相邻的8格中的每一个进行如下操作

• 如果它不可通过或者已经在关闭列表中，略过它。反之如下。
• 如果它不在开启列表中，把它添加进去。把当前格作为这一格的父节点。记录这一格的F,G,和H值。
• 如果它已经在开启列表中，用G值为参考检查新的路径是否更好。更低的G值意味着更好的路径。如果是这样，就把这一格的父节点改成当前格，并且重新计算这一格的G和F值。如果你保持你的开启列表按F值排序，改变之后你可能需要重新对开启列表排序。

d) 停止，

• 把目标格添加进了关闭列表(注解)，这时候路径被找到，或者
• 没有找到目标格，开启列表已经空了。这时候，路径不存在。

3.保存路径。从目标格开始，沿着每一格的父节点移动直到回到起始格。这就是你的路径。

2.仿真效果预览
matlab2022a仿真结果如下：

3.MATLAB核心程序

tic;
cost=1;
Found=false;
Resign=false;
 
 
Heuristic=CalculateHeuristic(grid,goal); %Calculate the Heuristic   
 
ExpansionGrid(1:size(grid,1),1:size(grid,2)) = -1; % to show the path of expansion
 
ActionTaken=zeros(size(grid)); %Matrix to store the action taken to reach that particular cell
 
OptimalPath(1:size(grid,1),1:size(grid,2))={' '}; %Optimal Path derived from A Star
 
%how to move in the grid
 
delta = [-1,  0; % go up
          0, -1; % go left
          1,  0; %go down
          0,  1]; % go right
%           1,  1; %diagonal down
%          -1, -1]; %diagonal up
 
 
 
 for i=1:size(grid,1)
     for j=1:size(grid,2)
         gridCell=search();
         if(grid(i,j)>0)
            gridCell=gridCell.Set(i,j,1,Heuristic(i,j));
         else
             gridCell=gridCell.Set(i,j,0,Heuristic(i,j));
         end
         GRID(i,j)=gridCell;
         clear gridCell;
     end
 end
 
% drawEnvironment(grid,init,goal);
 
Start=search();
Start=Start.Set(init(1),init(2),grid(init(1),init(2)),Heuristic(init(1),init(2)));
Start.isChecked=1;
GRID(Start.currX,Start.currY).isChecked=1;
Goal=search();
Goal=Goal.Set(goal(1),goal(2),grid(goal(1),goal(2)),0);
 
OpenList=[Start];
ExpansionGrid(Start.currX,Start.currY)=0;
 
small=Start.gValue+Start.hValue;
 
count=0;
 while(Found==false || Resign==false) 
    
 small=OpenList(1).gValue+OpenList(1).hValue+cost;
 
for i=1:size(OpenList,2)
        fValue=OpenList(i).gValue+OpenList(i).hValue;
        if(fValue<=small)
            small=fValue;
            ExpandNode=OpenList(i);
            OpenListIndex=i;
        end
    end
    
   
    OpenList(OpenListIndex)=[];
 
    
    ExpansionGrid(ExpandNode.currX,ExpandNode.currY)=count;
    count=count+1;
    
    for i=1:size(delta,1)
        direction=delta(i,:);
        if(ExpandNode.currX+ direction(1)<1 || ExpandNode.currX+direction(1)>size(grid,1)|| ExpandNode.currY+ direction(2)<1 || ExpandNode.currY+direction(2)>size(grid,2))
            continue;
        else
            NewCell=GRID(ExpandNode.currX+direction(1),ExpandNode.currY+direction(2));
            
             if(NewCell.isChecked~=1 && NewCell.isEmpty~=1)
                GRID(NewCell.currX,NewCell.currY).gValue=GRID(ExpandNode.currX,ExpandNode.currY).gValue+cost;
                GRID(NewCell.currX,NewCell.currY).isChecked=1; %modified line from the v1
                OpenList=[OpenList,GRID(NewCell.currX,NewCell.currY)]; 
                ActionTaken(NewCell.currX,NewCell.currY)=i;
             end
            
             if(NewCell.currX==Goal.currX && NewCell.currY==Goal.currY && NewCell.isEmpty~=1)
                Found=true;
                Resign=true;
                disp('Search Successful');
                GRID(NewCell.currX,NewCell.currY).isChecked=1;
                ExpansionGrid(NewCell.currX,NewCell.currY)=count;
                GRID(NewCell.currX,NewCell.currY);
                break;
            end
            
        end
    end
     if(isempty(OpenList) && Found==false)
         Resign=true;
         disp('Search Failed');
         break;
     end
 end
 PathTake=[]; %For stroring the values taken for the path.
 if(Found==true) %further process only if there is a path
     Policy={'Up','Left','Down','Right','Diag Down','Diag Up'};
     X=goal(1);Y=goal(2);
     OptimalPath(X,Y)={'GOAL'};
     while(X~=init(1)|| Y~=init(2))
         x2=X-delta(ActionTaken(X,Y),1);
         y2=Y-delta(ActionTaken(X,Y),2);
         OptimalPath(x2,y2)=Policy(ActionTaken(X,Y));
         PathTake=[PathTake;[X,Y]];
         X=x2;
         Y=y2;
     end
     PathTake=[PathTake;[init(1),init(2)]]; % add the start state to the end
     Total_Elapsed_Time=toc
 
%     figure;
    plot(fliplr((PathTake(:,2))'),fliplr((PathTake(:,1))'));
    set(gca,'XLim',[-1,size(grid,2)+2],'YLim',[-1,size(grid,1)+2]);
    set(gca,'YDir','reverse');
 
   % SmoothPath(PathTake,size(grid));
 
%  ExpansionGrid; %to see how the expansion took place
%     OptimalPath %to see the optimal path taken by the Search Algo
 else
 
     disp('No Path to Display');
     Total_Elapsed_Time=toc
 end
end
A103