路径规划 matlab

用MATLAB实现路径规划，采用A*算法，三维路径规划也可以。

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% A* ALGORITHM Demo
% Interactive A* search demo
% 04-26-2005
%   Copyright 2009-2010 The MathWorks Inc.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%DEFINE THE 2-D MAP ARRAY
MAX_X=10;
MAX_Y=10;
MAX_VAL=10;
%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
j=0;
x_val = 1;
y_val = 1;
axis（[1 MAX_X+1 1 MAX_Y+1]）
grid on;
hold on;
n=0;%Number of Obstacles

% BEGIN Interactive Obstacle Target Start Location selection
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‘‘blue‘）;
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+.5yval+.5‘bo‘）;
%End of obstacle-Target pickup

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%LISTS USED FOR ALGORITHM
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%OPEN LIST STRUCTURE
%--------------------------------------------------------------------------
%IS ON LIST 1/0 |X val |Y val |Parent X val |Parent Y val |h（n） |g（n）|f（n）|
%--------------------------------------------------------------------------
OPEN=[];
%CLOSED LIST STRUCTURE
%--------------
%X val | Y val |
%--------------
% 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

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件      80240  2005-05-09 09:52  Astar\Astar_tutorial.pdf

     文件       7397  2010-07-03 09:46  Astar\A_Star1.asv

     文件       7396  2010-07-02 20:36  Astar\A_Star1.m

     文件        201  2010-01-05 14:23  Astar\distance.m

     文件       1824  2010-01-05 14:23  Astar\expand_array.m

     文件        596  2010-01-05 14:24  Astar\insert_open.m

     文件       1060  2010-01-05 14:24  Astar\min_fn.m

     文件        297  2010-01-05 14:24  Astar\node_index.m

     文件       1071  2010-01-02 09:50  Astar\ReadMe.txt

     文件      44385  2010-01-02 09:56  Astar\screenShot.jpg

     目录          0  2010-07-03 09:31  Astar

----------- ---------  ---------- -----  ----

               144467                    11