A*算法原理及matlab代码_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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225

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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28