A*算法原理及matlab代码

A算法与D算法在路径规划中应是用较为广泛的一类算法，D是A的增强版本，先来看看A算法。
一、A原理
A算法结构相对来说较为复杂。
（1）首先注意区分两个词，当前已选定的栅格&当前已选定栅格的相邻栅格（后面简称相邻栅格）。
（2）两个概念：
启发距离：h=该相邻栅格到目标点的距离。
代价距离：g=已选定栅格到已选定栅格的某一相邻栅格的代价距离+已选定栅格与起点之间已生成的代价距离。
f=g+h
(3)两个列表：
openlist:该表中存放的栅格点都是以可达的相邻栅格的身份进入的。
closelist:该表中的栅格都是以可以作为父栅格身份进入的，父栅格很重要，因为直到寻路完成，所得到的所有父栅格构成起点至终点的路线。
A过程大致如下：（与之后程序中的变量名称相对应）
1、从起点出发，进行初始化。
将起点（start）放入open list中。初始化将其作为当前已选定栅格（cell），并将起点本身作为自己的父栅格从openlist中移除放入closelist中。
2、提取cell的8个相邻栅格（s），判断每个相邻点是否为可达相邻点，即是否在地图范围内，是否为障碍物。
3、计算可达相邻栅格的h、g、f，并将当前选定栅格的可达相邻栅格放入openlist中。过程：
依次检查每个可达相邻栅格（s）是否已经存在于openlist中。
若已存在，比较两者的f值，（1）若当前值小，则将对应的已存在的栅格的父栅格替换为当前已选定栅格（cell），并更新对应的f、h、g值。（2）若原有值小，则不对其进行任何操作。
若不存在，则将其加入至openlist。
4、在当前的openlist中选择f值最小的相邻栅格作为下一个已选定栅格，并将当前已选定栅格（cell）作为该相邻栅格的父栅格，移入closelist中。
5、循环至将终点作为相邻栅格的身份加入到openlist中。
6、将父栅格依次连接起来，完成寻路。

二、具体代码：其中valid中的第一位为列表标志位，1为openlist成员，0为closelist成员
valid=[flag|s_x |s_ y | parent_x(cell) | parent_y(cell) | h(n) | g(n) | f(n)
1、主程序：

%valid中的第一列为列表标志位，1：代表该栅格在openlist中，0：代表该栅格在closelist中。
%in_valid中为障碍物节点+当前点作为夫栅格的点。
%(cell_x,cell_y)当前选定点
%(s_x,s_y)当前选定点的相邻点
clc
clear
close all

%坐标轴最大范围
max_x=20; 
max_y=20;

environment=2*(ones(max_x,max_y));%20*20矩阵中每个元素值为2

j=0;
x_val = 1; 
y_val = 1;
axis([1 max_x+1 1 max_y+1]) %axis设置坐标轴范围
grid on;
hold on;
n=0;

%设置终点坐标
xval=18;
yval=18;
x_target=xval; 
y_target=yval;

environment(xval,yval)=0; %初始化环境中的目标点置0
plot(xval+.5,yval+.5,'gd'); %将目标点设置为绿色菱形
text(xval+1,yval+.5,'Target')%并在目标点上做Target标注

% 设置障碍物位置，障碍物位置处元素值为-1,显示效果填充为黑色
for i = 3:5
    for j = 13:16
        environment(i,j) = -1; 
        %plot(i+.5,j+.5,'rd'); 
    end
end
x = [3, 6, 6, 3];
y = [13, 13, 17, 17];
fill(x,y,'k'); %fill：填充为黑色（k）
% obstacle 2
for i = 7:9
    for j = 11:14 
        environment(i,j) = -1;
        %plot(i+.5,j+.5,'rd'); 
    end
end
x = [7, 10, 10, 7];
y = [11, 11, 15, 15];
fill(x,y,'k');
% obstacle 3
for i = 11:13
    for j = 11:17 
        environment(i,j) = -1;
        %plot(i+.5,j+.5,'rd'); 
    end
end
x = [11, 14, 14, 11];
y = [11, 11, 18, 18];
fill(x,y,'k');
% obstacle 4
for i = 12:13
    for j = 6:9 
        environment(i,j) = -1;
        %plot(i+.5,j+.5,'rd'); 
    end
end
x = [12, 14, 14, 12];
y = [6, 6, 10, 10];
fill(x,y,'k');
% obstacle 5
for i = 15:16
    for j = 6:17 
        environment(i,j) = -1;
        %plot(i+.5,j+.5,'rd'); 
    end
end
x = [15, 17, 17, 15];
y = [6, 6, 18, 18];
fill(x,y,'k');
%plot(19+.5,19+.5,'gd');

%设置起始点位置
xval=3;
yval=3;
x_start=xval;%Starting Position
y_start=yval;%Starting Position

environment(xval,yval)=1;%起始点位置元素值为1
 plot(xval+.5,yval+.5,'bo');%起点处用蓝色圆圈表示

valid=[]; % x | y | parent_x | parent_y | h(n) | g(n) | f(n) 

in_valid=[]; % x | y

%获取障碍物位置
k=1;
for i=1:max_x
    for j=1:max_y
        if(environment(i,j) == -1) % check if obstacle
            in_valid(k,1)=i; 
            in_valid(k,2)=j; 
            k=k+1;
        end
    end
end
in_valid_size=size(in_valid,1);%障碍物个数

%A*过程开始
cell_x=x_start;%将起点作为当前已选定
cell_y=y_start;
valid_size=1; % initialize size of valid_list
path_cost=0;
goal_distance=sqrt((cell_x-x_target)^2 + (cell_y-y_target)^2);%起点与终点的欧式距离

new_row=[1,8];
new_row(1,1)=1;
new_row(1,2)=cell_x;%cell_x为当前选定点坐标
new_row(1,3)=cell_y;%cell_y当前选定点坐标
new_row(1,4)=cell_x; % parent x
new_row(1,5)=cell_y; % parent y 
new_row(1,6)=path_cost;
new_row(1,7)=goal_distance;
new_row(1,8)=goal_distance;

valid(valid_size,:)=new_row; % 用开始位置初始化valid
valid(valid_size,1)=0;

in_valid_size=in_valid_size+1;
in_valid(in_valid_size,1)=cell_x; % make it invalid for further iterations
in_valid(in_valid_size,2)=cell_y;

path_not_found=1;

while((cell_x ~= x_target || cell_y ~= y_target) && path_not_found == 1)
   %h=distance（当前选定点，该当前选定点的一个相邻点），g=distance(目标点，该当前选定点的一个相邻点)，f=h+g
    successors=explore_successors(cell_x,cell_y,path_cost,x_target,y_target,in_valid,max_x,max_y);%successors返回当前点的所有周围可选点的 的x | y | h | g | f 信息
    successors_size=size(successors,1);%获取当前选定点相邻点可达的个数
    for i=1:successors_size%遍历当前选定点的可达相邻点
    flag=0;%标志位清零
    for j=1:valid_size %遍历valid中之前的点，判断当前选定点的可达相邻点是否已经存在
        if(successors(i,1) == valid(j,2) && successors(i,2) == valid(j,3) ) % 判断当前点的周围可达点是否已存在于valid中

        valid(j,8)=min(valid(j,8),successors(i,5)); %如果已经存在，判断当前的f值与之前的比哪个小，若以前的小则不管，若现在的小，将当前选定点
            if valid(j,8) == successors(i,5) %在判断一遍valid中的f值是否与当前的f值相同，若上一步判断现在的f值小，则不相等，需要执行以下步骤
                valid(j,4)=cell_x;% 将当前选定点替换该点作为父节点parent x，并更新记录其h,g值
                valid(j,5)=cell_y;% parent y
                valid(j,6)=successors(i,3); % h
                valid(j,7)=successors(i,4); % g
            end;
            flag=1;%该标志位置1表示valid矩阵中已经含有该节点。
        end;
    end;
    if flag == 0 % if new cell with minimum f(n) then add to valid_path，若当前点周围可选点不在valid中，标志位为0，并将其加入valid中
        valid_size= valid_size+1;
        new_row=[1,8];
        new_row(1,1)=1;%valid标志位，为0相当于该点已经移入closelist，不可在用，为1可用相当于在openlist中
        new_row(1,2)=successors(i,1);
        new_row(1,3)=successors(i,2);
        new_row(1,4)=cell_x; % valid中4，5位为当前选定点相邻点的当前点，作为parent x
        new_row(1,5)=cell_y; % parent y
        new_row(1,6)=successors(i,3); % h
        new_row(1,7)=successors(i,4); % g
        new_row(1,8)=successors(i,5); % f
        valid(valid_size,:)= new_row; 
     end;
    end;

    index_min_cell = min_f(valid,valid_size,x_target,y_target);%获取valid中的最小值
    if (index_min_cell ~= -1) % 得到下一个最小值的点，if index with minimum fn is obstacle no path exists    
        cell_x=valid(index_min_cell,2);%将f值最小的相邻点设为新的当前点
        cell_y=valid(index_min_cell,3);
        path_cost=valid(index_min_cell,6);%记录目前的h值

        in_valid_size=in_valid_size+1; % 将最小f值的相邻点移入in_valid中，put the cell in_valid so we dont come back on it again
        in_valid(in_valid_size,1)=cell_x;
        in_valid(in_valid_size,2)=cell_y;
        valid(index_min_cell,1)=0;%将该相邻点在valid中所在行的第一个元素置0，不在检查。其对应的当前选定点设为父节点
    else
        path_not_found=0;
    end;
end;  %

% backtracking to find the path回溯找到路径
i=size(in_valid,1);%i为parents个数
path=[];
xval=in_valid(i,1); % 取in_valid最后一点，pick last in in_valid_list that must be target
yval=in_valid(i,2);
i=1;
path(i,1)=xval;%取in_valid最后一个点
path(i,2)=yval;
i=i+1;

%若in_valid中最后一点为目标点，则将其作为父节点
if ( (xval == x_target) && (yval == y_target)) %判断in_valid最后一点是否为终点
    inode=0;
   parent_x=valid(find((valid(:,2) == xval) & (valid(:,3) == yval),1),4);%find返回valid的第二列、第3列中第一个同时等于xval，yval的行数，即找到终点的上一个点，即终点的父节点
   parent_y=valid(find((valid(:,2) == xval) & (valid(:,3) == yval),1),5);
   
   while( parent_x ~= x_start || parent_y ~= y_start)  %如果当前父节点不是起始点
           path(i,1) = parent_x;%将当前父节点写入路径
           path(i,2) = parent_y;
           
           inode=find((valid(:,2) == parent_x) & (valid(:,3) == parent_y),1);%查找valid中第一个等于当前父节点的点所在的行数，inode为行数
           parent_x=valid(inode,4);%取当前父节点的父节点，循环往复，直到当前的夫节点为起点，路点记录为从终点向起点方向
           parent_y=valid(inode,5);
           i=i+1;
    end;
    
% plottin path画图
%path中记录的路点从上往下依次为由终点向起点方向的路点，因此此处path取值应从下想上取才为从起点向终点运动的方向
j=size(path,1);
 p=plot(path(j,1)+.5,path(j,2)+.5,'bo');
  j=j-1;
 for i=j:-1:1%画从起点向终点的运动过程
  pause(.25);%pause(n) 暂停执行 0.25秒，然后继续执行。
  set(p,'XData',path(i,1)+.5,'YData',path(i,2)+.5);
 drawnow ;%刷新屏幕，实时显示plot
 end;
 plot(path(:,1)+.5,path(:,2)+.5);%得出路径
else
disp( 'Sorry, No path exists to the Target!');
end

explore_successors.m

for k= 1:-1:-1    %分别获取当前点周围8个相邻点的信息
        for j= 1:-1:-1
            if (k~=j || k~=0)  %把当前选定点排除掉，只获取当前选定点周围的可选点信息
                s_x = x_cell+k;  %（s_x,s_y）是8个相邻点坐标
                s_y = y_cell+j;
                if( (s_x >0 && s_x <=max_x) && (s_y >0 && s_y <=max_y)) % 该相邻点是否在地图范围内
                    flag=1;    %当前点在地图范围内标志位为1                
                    for c1=1:c2 % 遍历所有障碍物位置，检查当前点是否为障碍物
                        if(s_x == in_valid(c1,1) && s_y == in_valid(c1,2)) 
                            flag=0;%当前点为不可达点，标志位为0
                        end;
                    end;
                    if (flag == 1) %当前点可达，将当前点加入successors中
                        successors(successor_count,1) = s_x;
                        successors(successor_count,2) = s_y;
                        successors(successor_count,3) = h+distance(x_cell,y_cell,s_x,s_y);%h
                        successors(successor_count,4) = distance(x_target,y_target,s_x,s_y);%g
                        successors(successor_count,5) = successors(successor_count,3)+successors(successor_count,4);%f
                        successor_count=successor_count+1;
                    end
                end
            end
        end
    end

min_f.m

%   相当于取openlist中f值的最小值。index_min_cell = min_f(valid,valid_size,x_target,y_target)
function index_of_min = min_f(valid,valid_len,x_target,y_target)
 temp_array=[];
 k=1;
 flag=0;%标志位清零
 goal_index=0;
 for j=1:valid_len  %遍历valid中已加入的所有值，判断是否为终点
     if (valid(j,1)==1)%若节点状态标志位为1相当于该相邻点在openlist中，为1该相邻点的父节点已经移入close list，不必再看
         temp_array(k,:)=[valid(j,:) j]; %将valid中第一位=1的自由点依次加入到temp_array中，并在该行最后一位记录其在valid中的行数
         if (valid(j,2)==x_target && valid(j,3)==y_target)   %判断该相邻点是否为为终点，若是，标志位置1
             flag=1;
             goal_index=j;%目标索引指向valid中对应的该相邻点的行数
         end;
         k=k+1;
     end;
 end;
 if flag == 1 %若是终点，最小值索引为终点
     index_of_min=goal_index;  
 end
 
 if size(temp_array ~= 0)%统计自由节点的个数
  [min_f,temp_min]=min(temp_array(:,8)); %取其中f值最小的点的最小值及在temp_array中的行数，
  %min_f
  index_of_min=temp_array(temp_min,9);%取该行最后对应的valid的行号
  fprintf('Cell with minimum f found to be x : %d y : %d with f = %f',valid(index_of_min,2),valid(index_of_min,3),valid(index_of_min,8));
 else
     index_of_min=-1;
 end