基于栅格地图的A星算法路径规划

用matlab实现基于栅格地图的A星算法路径规划，代码中障碍物为任意障碍物。

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% A* ALGORITHM
% 04-26-2005
% Vivian Paul 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
%DEFINE THE 2-D MAP ARRAY
MAX_X=30;
MAX_Y=30;
MAX_VAL=10;
OPEN_COUNT=0;
CLOSED_COUNT=0;
%This array stores the coordinates of the map and the 
%objects in each coordinate
MAP=2*（ones（MAX_XMAX_Y））;
 
% Obtain Obstacle Target and Robot Position
% Initialize the MAP with input values
% Obstacle=-1Target = 0Robot=1Space=2
i=0;j=0;
x_val = 1;
y_val = 1;
axis（[0 MAX_X0 MAX_Y]）
set（gca‘xtick‘0:1:30‘ytick‘0:1:30‘GridLinestyle‘‘-‘... 
    ‘xGrid‘‘on‘‘yGrid‘‘on‘） 
%grid on;
hold on;
n=0;%Number of Obstacles
 
pause（1）;
h=msgbox（‘Please Select the Target using the Left Mouse button‘）;
uiwait（h5）;
if ishandle（h） == 1
    delete（h）;
end
xlabel（‘Please Select the Target using the Left Mouse button‘‘Color‘‘black‘）;
but=0;
while （but ~= 1） %Repeat until the Left button is not clicked
    [xvalyvalbut]=ginput（1）;
end
xval=floor（xval）;
yval=floor（yval）;
xTarget=xval;%X Coordinate of the Target
yTarget=yval;%Y Coordinate of the Target
 
MAP（xvalyval）=0;%Initialize MAP with location of the target
plot（xval+.5yval+.5‘gd‘）;
text（xval+1yval+.5‘Target‘）
 
pause（2）;
h=msgbox（‘Select Obstacles using the Left Mouse buttonto select the last obstacle use the Right button‘）;
  xlabel（‘Select Obstacles using the Left Mouse buttonto select the last obstacle use the Right button‘‘Color‘‘black‘）;
uiwait（h10）;
if ishandle（h） == 1
    delete（h）;
end
while but == 1
    [xvalyvalbut] = ginput（1）;
    xval=floor（xval）;
    yval=floor（yval）;
    MAP（xvalyval）=-1;%Put on the closed list as well
    plot（xval+.5yval+.5‘ro‘）;
    
 end%End of While loop
  
pause（1）;
 
h=msgbox（‘Please Select the Vehicle initial position using the Left Mouse button‘）;
uiwait（h5）;
if ishandle（h） == 1
    delete（h）;
end
xlabel（‘Please Select the Vehicle initial position ‘‘Color‘‘black‘）;
but=0;
while （but ~= 1） %Repeat until the Left button is not clicked
    [xvalyvalbut]=ginput（1）;
    xval=floor（xval）;
    yval=floor（yval）;
end
xStart=xval;%Starting Position
yStart=yval;%Starting Position
MAP（xvalyval）=1;
 plot（xval+0.5yval+0.5‘bo‘）;
 text（xval+1yval+.5‘start‘）
%--------------------------------------------------------------------------
OPEN=[];
%--------------
% CLOSED=zeros（MAX_VAL2）;
CLOSED=[];
 
%Put all obstacles on the Closed list
k=1;%Dummy counter
for i=1:MAX_X
    for j=1:MAX_Y
        if（MAP（ij） == -1）%障碍物为-1
            CLOSED（k1）=i;
            CLOSED（k2）=j;
            k=k+1;
        end
    end
end
CLOSED_COUNT=size（CLOSED1）;
%set the starting node as the first node
xNode=xval;
yNode=yval;
OPEN_COUNT=1;
path_cost=0;
goal_distance=distance（xNodeyNodexTargetyTarget）;%目标距离并非路径长度
OPEN（OPEN_COUNT:）=insert_open（xNodeyNodexNodeyNodepath_costgoal_distancegoal_distance）;
OPEN（OPEN_COU

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2018-01-19 09:28  A星代码（动态设置）+文档\
     文件         372  2007-01-07 13:57  A星代码（动态设置）+文档\A.caa
     文件         372  2007-01-07 13:59  A星代码（动态设置）+文档\A1.caa
     文件      280122  2007-01-07 13:58  A星代码（动态设置）+文档\A_算法在矢量地图最优路径搜索中的应用_刘浩 （1）.caj
     文件      280122  2007-01-07 13:58  A星代码（动态设置）+文档\A_算法在矢量地图最优路径搜索中的应用_刘浩.caj
     文件      244875  2007-01-07 13:59  A星代码（动态设置）+文档\A_算法改进算法及其应用_张仁平.caj
     目录           0  2018-01-19 09:28  A星代码（动态设置）+文档\A星算法\
     文件        7158  2017-05-19 16:09  A星代码（动态设置）+文档\A星算法\Axing.m
     文件         156  2017-05-09 16:03  A星代码（动态设置）+文档\A星算法\distance.m
     文件        1873  2017-05-03 17:16  A星代码（动态设置）+文档\A星算法\expand_array.m
     文件         543  2017-05-03 17:16  A星代码（动态设置）+文档\A星算法\insert_open.m
     文件        1060  2017-05-03 17:16  A星代码（动态设置）+文档\A星算法\min_fn.m
     文件         208  2017-05-03 17:16  A星代码（动态设置）+文档\A星算法\node_index.m
     文件      231797  2007-01-07 13:59  A星代码（动态设置）+文档\基于加权A_算法的服务型机器人路径规划_赵真明.caj
     文件      686043  2007-01-07 13:58  A星代码（动态设置）+文档\基于平滑A_算法的移动机器人路径规划_王红卫.caj