移动机器人的路径规划以及避障

移动机器人避障，实现路径优化，平滑转折点

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% A* ALGORITHM
% 04-26-2005
% Vivian Paul 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%DEFINE THE 2-D MAP ARRAY
MAX_X=10;
MAX_Y=10;
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（[1 MAX_X+1 1 MAX_Y+1]）
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‘‘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
        if（MAP（ij） == -1）
            CLOSED（k1）=i;
            CLOSED（k2）=j;
       

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

     文件       7288  2005-04-28 16:13  Astar\A_star1.m

     文件        156  2005-04-28 16:13  Astar\distance.m

     文件       1870  2005-04-28 16:13  Astar\expand_array.m

     文件        543  2005-04-28 16:13  Astar\insert_open.m

     文件       1051  2005-04-28 16:13  Astar\min_fn.m

     文件        206  2005-04-28 16:13  Astar\node_index.m

     目录          0  2010-09-15 10:37  Astar

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

                11114                    7