基于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核心程序 `function [PathTake, Found]=A_Star_Search(grid,init,goal)

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`