【A*路径搜索算法】基于A星的最优避障路径搜索算法的MATLAB仿真+GUI界面

1.软件版本

MATLAB2021a

2.基本原理

A算法是启发式算法重要的一种，主要是用于在两点之间选择一个最优路径，而A的实现也是通过一个估值函数

F=G+H

• G表示该点到起始点位所需要的代价
• H表示该点到终点的曼哈顿距离。
• F就是G和H的总和，而最优路径也就是选择最小的F值，进行下一步移动（后边会做详细介绍）

 3.核心代码

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Author:Michael Jacob Mathew
% The following code illustrates the A star search algorithm.
% The code is self explanatory
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% clc; clear all; clear classes;
function [PathTake, Found]=A_Star_Search(grid,init,goal)
tic;
cost=1;
Found=false;
Resign=false;
% grid=NewGrid();
%
% init = [1,1]; %Start
% goal = [size(grid,1), size(grid,2)]; % Goal.
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

4.操作步骤与仿真结论

 D217