Matlab编程技巧：A*算法仿真_a*算法matlab

本文通过Matlab编程实现A* 算法，并通过几个简单的例子，打断点调试输出A*搜索的过程。

1 A* 算法简介

A* 算法是一种全局路径规划算法，通过A*算法可以在一个有障碍物的地图中找到从起点到终点的路径。

网上关于A* 算法的详细介绍有很多，博主也就不再赘述。推荐想学习的博友看一下这篇博文入门：《A*算法（超级详细讲解，附有举例的详细手写步骤）》，博主看完后有种醍醐灌顶的感觉，就想通过自己比较熟悉的Matlab实现一遍，所以就有了本文的内容。

由于博主是初学者，难免有一些错误，如果有博友发现bug恳请私信或评论告知。

2 Matlab编程实现

2.1 输入参数

新建一个Astar()函数如下，其中包含了输入参数和AstarSearch()算法函数。

其中，含义分别如下：

1）X_Length和Y_Length代表网格地图的长和宽，如图的参数就是代表9*6的网格。

2）Start_Node、Target_Node、Obs_Node_List分别为起点、终点、障碍物坐标。

通过Plot函数画图可以直观地看到地图的大小以及黄色的起点、绿色的终点和黑色的障碍物：

这些参数会传递给后面的AstarSearch()函数作为输入，用来进行A* 搜索算法。因此每次试验的时候直接调上面的参数就可以运行Matlab得出结果。

2.2 初始化变量

在A* 搜索算法的开始，博主定义了一些初始化变量，如下图所示：

1）Node是节点的意思，是个结构体变量。其中的成员包括：PositionX，PositionY，代表横坐标和纵坐标；Cost_F，Cost_G，Cost_H，代表3个代价值；Father，表示父节点的坐标。

2）OpenList和CloseList是初始化的固定大小的数组，数组的元素是Node节点的结构体类型数组长度是地图的总网格数量；其实这样定义比较占用资源，不过是仿真也就无所谓了。

3）OpenList_Num和CloseList_Num是OpenList和CloseList中的有效元素个数。由于A* 循环的过程中会不断地增减OpenList和CloseList中的元素，所以Num值也会不断地加减。

2.3 循环过程

循环过程比较长，基本上就是根据上面的链接中的方法来写的函数。其实核心就是将考察过的节点放入CloseList，将待考察的节点放入OpenList。同时在循环的过程中更新Cost的值，直到OpenList中如果出现目标点则代表搜索完成找到路径，或者OpenList为空则代表没找到路径。具体可以参考博主在附录里贴的Matlab代码。

另外，在每次循环结束的时候，会用一个PlotGridArea()函数画出当前循环的信息，例如哪些节点在OpenList，哪些在CloseList。

后文测试的时候，会在画图这里打个断点，来输出每次循环后的信息。

2.4 循环后处理

循环会通过两个条件跳出：OpenList中如果出现目标点则代表搜索完成找到路径，或者OpenList为空则代表没找到路径。跳出循环后会根据两种结果分别进行后处理，找到路径的话就会再画一张从起点到终点的图片，没找到就会弹窗提示。

画出路径的方法是：从OpenList中找到终点（Target_Node），然后找它的父节点，再找父节点的父节点，就这样顺藤摸瓜直到找到起点。这样就形成了一个从终点到起点的完整链路。

3 调试与验证

3.1 案例1

案例1用一个比较简单的9*6的网格来验证。

1）Matlab中定义初始点、结束点和障碍物点如下：

%% 输入参数
X_Length = 9;
Y_Length = 6;

Start_Node = [3,4];
Target_Node = [7,4];
Obs_Node_List = [[5,3];[5,4];[5,5]];
1
2
3
4
5
6
7

2）不打断点直接运行完Matlab程序以后，会分别在两张图中输出最后一次的OpenList、CloseList和画出的路线。

上图中蓝色的格子代表已经放入CloseList中的节点，红色代表放入OpenList中的节点；

上图中是最后画出的从起点到终点的路线。

3）另外，也可以在While循环的最后打个断点，输出每次循环后的OpenList和CloseList网格。

在该案例中，每次输出的图片如下：

3.2 案例2

案例2用一个稍微复杂点的情况来验证。

1）Matlab中定义初始点、结束点和障碍物点如下：

%% 输入参数
X_Length = 12;
Y_Length = 8;

Start_Node = [2,2];
Target_Node = [11,7];
Obs_Node_List = [[4,2];[4,3];[4,4];[4,5];[3,5];[2,5];
    [6,1];[6,2];[6,4];[6,5];[6,7];[6,8];
    [8,5];[9,5];[10,5];[8,4];[9,4];[10,4]];
1
2
3
4
5
6
7
8
9

2）运行程序输出结果：

上图中博主优化了一下代码，把Cost_F代价也打印出来了。

3.3 案例3

案例3用一个找不到路径的情况来验证。

1）Matlab中定义初始点、结束点和障碍物点如下：

%% 输入参数
X_Length = 9;
Y_Length = 6;

Start_Node = [3,4];
Target_Node = [7,4];
Obs_Node_List = [[5,1];[5,2];[5,3];[5,4];[5,5];[5,6]];
1
2
3
4
5
6
7

2）运行程序输出结果：

程序将障碍物左边的所有点都找了一遍，放入了CloseList。此时OpenList中已经没有了点，所以判定为没有合适的路径。

4 总结

本文是博主通过Matlab编程来学习A*算法的总结，虽然学习过程比较坎坷，但也收获了不少。另外，由于这是博主自己的学习过程，肯定有很多不严谨的地方和错误，希望博友可以用批判的眼光看待，如果发现错误和不规范之处，也希望多多交流讨论。

>>返回个人博客总目录

5 附录 Matlab程序代码

下面的代码在Matlab2018a中用文中的3个案例验证过。可以建立一个Astar.m文件，再把代码拷贝进去。

function Astar()
%% 输入参数
X_Length = 9;
Y_Length = 6;

Start_Node = [3,4];
Target_Node = [7,4];
Obs_Node_List = [[5,1];[5,2];[5,3];[5,4];[5,5];[5,6]];

%% Astar算法
AstarSearch(Start_Node,Target_Node,Obs_Node_List,X_Length,Y_Length);

end

%% Astar搜索过程
function AstarSearch(Start_Node,Target_Node,Obs_Node_List,X_Length,Y_Length)
%% 初始化变量
Node = struct('PositionX',0,'PositionY',0,'Cost_F',0,'Cost_G',0,'Cost_H',0,'Father',[0,0]);
OpenList = repmat(Node,X_Length*Y_Length,1);
OpenList_Num = 0;
CloseList = repmat(Node,X_Length*Y_Length,1);
CloseList_Num = 0;
Start_Node_PositionX = Start_Node(1);
Start_Node_PositionY = Start_Node(2);
Target_Node_PositionX = Target_Node(1);
Target_Node_PositionY = Target_Node(2);

%% 将初始节点加入CloseList
Node_Father.PositionX = Start_Node_PositionX;
Node_Father.PositionY = Start_Node_PositionY;
Node_Father.Cost_G = 0;
Node_Father.Cost_H = abs(Start_Node_PositionX - Target_Node_PositionX) + abs(Start_Node_PositionY - Target_Node_PositionY);
Node_Father.Cost_F = Node_Father.Cost_G + Node_Father.Cost_H;
Node_Father.Father = [0,0];

CloseList_Num = CloseList_Num + 1;
CloseList(CloseList_Num) = Node_Father;

%% 打印地图网格和起始、终止、障碍物点
PlotGridArea(X_Length,Y_Length,Start_Node,Target_Node,Obs_Node_List,OpenList,OpenList_Num,CloseList,CloseList_Num)

%% 循环查找路径
while 1
    Node_Child_List = repmat(Node,8,1);
    Node_Child_List(1).PositionX = Node_Father.PositionX + 1;Node_Child_List(1).PositionY = Node_Father.PositionY;
    Node_Child_List(2).PositionX = Node_Father.PositionX + 1;Node_Child_List(2).PositionY = Node_Father.PositionY + 1;
    Node_Child_List(3).PositionX = Node_Father.PositionX;Node_Child_List(3).PositionY = Node_Father.PositionY + 1;
    Node_Child_List(4).PositionX = Node_Father.PositionX - 1;Node_Child_List(4).PositionY = Node_Father.PositionY + 1;
    Node_Child_List(5).PositionX = Node_Father.PositionX - 1;Node_Child_List(5).PositionY = Node_Father.PositionY;
    Node_Child_List(6).PositionX = Node_Father.PositionX - 1;Node_Child_List(6).PositionY = Node_Father.PositionY - 1;
    Node_Child_List(7).PositionX = Node_Father.PositionX;Node_Child_List(7).PositionY = Node_Father.PositionY - 1;
    Node_Child_List(8).PositionX = Node_Father.PositionX + 1;Node_Child_List(8).PositionY = Node_Father.PositionY - 1;
    %计算子节点中的属性
    for i = 1:8
        PositionX = Node_Child_List(i).PositionX;
        PositionY = Node_Child_List(i).PositionY;
        %判断是否超地图范围，如果是则不考虑
        if PositionX <= 0 || PositionX > X_Length || PositionY <= 0 || PositionY > Y_Length
            continue;
        end
        %判断是否障碍物节点，如果是则不考虑
        IsObsNodeFlag = false;
        for j = 1:size(Obs_Node_List,1)
            Obs_Node_PositionX = Obs_Node_List(j,1);
            Obs_Node_PositionY = Obs_Node_List(j,2);
            if PositionX == Obs_Node_PositionX && PositionY == Obs_Node_PositionY
                IsObsNodeFlag = true;
                break;
            end
        end
        if IsObsNodeFlag
            continue;
        end
        %判断是否在CloseList中，如果是则不考虑
        IsInCloseListFlag = false;
        for j = 1:CloseList_Num
            if PositionX == CloseList(j).PositionX && PositionY == CloseList(j).PositionY
                IsInCloseListFlag = true;
                break;
            end
        end
        if IsInCloseListFlag
            continue;
        end
        %计算代价函数
        if abs(PositionX - Node_Father.PositionX) + abs(PositionY - Node_Father.PositionY) == 2
            Cost_G = 1.4 + Node_Father.Cost_G;
        else
            Cost_G = 1 + Node_Father.Cost_G;
        end
        Cost_H = abs(PositionX - Target_Node_PositionX) + abs(PositionY - Target_Node_PositionY);
        Cost_F = Cost_G + Cost_H;
        %判断节点是否在OpenList，是则考虑更新，不是则加入
        IsInOpenListFlag = false;
        Index_InOpenList = 0;
        for j = 1:OpenList_Num
            if PositionX == OpenList(j).PositionX && PositionY == OpenList(j).PositionY
                IsInOpenListFlag = true;
                Index_InOpenList = j;
                break;
            end
        end
        if IsInOpenListFlag %如果节点已经在Openlist，则需要判断新的路径的代价函数是不是更高
            if Cost_F < OpenList(Index_InOpenList).Cost_F
                OpenList(Index_InOpenList).Cost_F = Cost_F;
                OpenList(Index_InOpenList).Cost_G = Cost_G;
                OpenList(Index_InOpenList).Father = [Node_Father.PositionX,Node_Father.PositionY];
            end
        else %直接加入OpenList
            OpenList_Num = OpenList_Num + 1;
            OpenList(OpenList_Num).PositionX = PositionX;
            OpenList(OpenList_Num).PositionY = PositionY;
            OpenList(OpenList_Num).Cost_G = Cost_G;
            OpenList(OpenList_Num).Cost_H = Cost_H;
            OpenList(OpenList_Num).Cost_F = Cost_F;
            OpenList(OpenList_Num).Father = [Node_Father.PositionX,Node_Father.PositionY];
        end
        
    end
    %画出OpenList和CloseList
    PlotGridArea(X_Length,Y_Length,Start_Node,Target_Node,Obs_Node_List,OpenList,OpenList_Num,CloseList,CloseList_Num);
    %判断OpenList是否已经出现了目标节点
    ExistTargetNodeFlag = false;
    for i = 1:OpenList_Num
        if OpenList(i).PositionX == Target_Node_PositionX && OpenList(i).PositionY == Target_Node_PositionY
            ExistTargetNodeFlag = true;
            CloseList_Num = CloseList_Num + 1;%将目标节点放入CloseList以便后面处理
            Target_Node_Cost_F = OpenList(i).Cost_F;
            CloseList(CloseList_Num) = OpenList(i);
        end        
    end
    if ExistTargetNodeFlag %OpenList中如果出现目标点则代表搜索完成找到路径
        break;
    end
    %判断OpenList是否为空
    IsOpenListEmpty = false;
    if OpenList_Num == 0
        IsOpenListEmpty = true;
    end
    if IsOpenListEmpty %OpenList为空则代表没找到路径
        break;
    end
    %挑出代价函数最小的子节点
    Index_Min_Cost = 1;
    Temp_Cost_F = OpenList(1).Cost_F;
    for i = 1:OpenList_Num
        if OpenList(i).Cost_F < Temp_Cost_F
            Temp_Cost_F = OpenList(i).Cost_F;
            Index_Min_Cost = i;
        end        
    end
    %加入CloseList并从OpenList中删除
    CloseList_Num = CloseList_Num + 1;
    CloseList(CloseList_Num) = OpenList(Index_Min_Cost);
    for i = Index_Min_Cost:OpenList_Num
        OpenList(i) = OpenList(i+1);    
    end
    OpenList_Num = OpenList_Num - 1;
    %将该节点作为父节点继续进行搜索
    Node_Father = CloseList(CloseList_Num);    
        
end

%% 循环完成后处理结果
if ExistTargetNodeFlag %OpenList中如果出现目标点则代表搜索完成找到路径,根据CloseList中的数据画出图像
    PositionX = Target_Node_PositionX;
    PositionY = Target_Node_PositionY;
    Path_List = [Target_Node_PositionX,Target_Node_PositionY,Target_Node_Cost_F];
    while 1        
        for i = 1:CloseList_Num % 寻找当前节点的父节点
            if PositionX == CloseList(i).PositionX && PositionY == CloseList(i).PositionY
                PositionX = CloseList(i).Father(1);
                PositionY = CloseList(i).Father(2);        
                break;
            end
        end
        for i = 1:CloseList_Num % 寻找父节点的Cost_F
            if PositionX == CloseList(i).PositionX && PositionY == CloseList(i).PositionY
                Cost_F = CloseList(i).Cost_F;
                Path_List = [[PositionX,PositionY,Cost_F];Path_List];  
                break;
            end
        end        
        if PositionX == Start_Node_PositionX && PositionY == Start_Node_PositionY
            break;
        end
    end
    PlotPath(X_Length,Y_Length,Start_Node,Target_Node,Obs_Node_List,Path_List)
elseif IsOpenListEmpty %OpenList为空则代表没找到路径
    msgbox('未搜索到路径！');
end
    
end

%% 画出每次搜索后的OpenList和CloseList
function PlotGridArea(X_Length,Y_Length,Start_Node,Target_Node,Obs_Node_List,OpenList,OpenList_Num,CloseList,CloseList_Num)
figure(1);
% 画网格线
for x = 1:X_Length
    plot([x,x],[0,Y_Length],'black');
    hold on;
end
for y = 1:Y_Length
    plot([0,X_Length],[y,y],'black');
    hold on;
end
%坐标轴调整
axis equal;
axis([0,X_Length,0,Y_Length]);

%Start_Node
Start_Node_X = Start_Node(1);
Start_Node_Y = Start_Node(2);
fill([Start_Node_X,Start_Node_X,Start_Node_X-1,Start_Node_X-1,Start_Node_X],[Start_Node_Y,Start_Node_Y-1,Start_Node_Y-1,Start_Node_Y,Start_Node_Y],'yellow');
%Target_Node
Target_Node_X = Target_Node(1);
Target_Node_Y = Target_Node(2);
fill([Target_Node_X,Target_Node_X,Target_Node_X-1,Target_Node_X-1,Target_Node_X],[Target_Node_Y,Target_Node_Y-1,Target_Node_Y-1,Target_Node_Y,Target_Node_Y],'green');
%Obs_Node
for i = 1:length(Obs_Node_List)
    Obs_Node_X = Obs_Node_List(i,1);
    Obs_Node_Y = Obs_Node_List(i,2);
    fill([Obs_Node_X,Obs_Node_X,Obs_Node_X-1,Obs_Node_X-1,Obs_Node_X],[Obs_Node_Y,Obs_Node_Y-1,Obs_Node_Y-1,Obs_Node_Y,Obs_Node_Y],'black');    
end
%CloseList
for i = 1:CloseList_Num
    Close_Node = CloseList(i);
    if Close_Node.PositionX == Start_Node(1) && Close_Node.PositionY == Start_Node(2)%如果是Start_Node就不当作CloseNode画
        continue;
    elseif Close_Node.PositionX == Target_Node(1) && Close_Node.PositionY == Target_Node(2)%如果是Target_Node就不当作CloseNode画
        continue;
    end
    Close_Node_X = Close_Node.PositionX;
    Close_Node_Y = Close_Node.PositionY;
    fill([Close_Node_X,Close_Node_X,Close_Node_X-1,Close_Node_X-1,Close_Node_X],[Close_Node_Y,Close_Node_Y-1,Close_Node_Y-1,Close_Node_Y,Close_Node_Y],'blue'); 
end
%OpenList
for i = 1:OpenList_Num
    Open_Node = OpenList(i);
    if Open_Node.PositionX == Start_Node(1) && Open_Node.PositionY == Start_Node(2)%如果是Start_Node就不当作OpenNode画
        continue;
    elseif Open_Node.PositionX == Target_Node(1) && Open_Node.PositionY == Target_Node(2)%如果是Target_Node就不当作OpenNode画
        continue;
    end
    Open_Node_X = Open_Node.PositionX;
    Open_Node_Y = Open_Node.PositionY;
    fill([Open_Node_X,Open_Node_X,Open_Node_X-1,Open_Node_X-1,Open_Node_X],[Open_Node_Y,Open_Node_Y-1,Open_Node_Y-1,Open_Node_Y,Open_Node_Y],'red'); 
end

end

%% 搜索出路径后，通过画图画出来
function PlotPath(X_Length,Y_Length,Start_Node,Target_Node,Obs_Node_List,Path_List)
figure(2);
% 画网格线
for x = 1:X_Length
    plot([x,x],[0,Y_Length],'black');
    hold on;
end
for y = 1:Y_Length
    plot([0,X_Length],[y,y],'black');
    hold on;
end
%坐标轴调整
axis equal;
axis([0,X_Length,0,Y_Length]);

%Start_Node
Start_Node_X = Start_Node(1);
Start_Node_Y = Start_Node(2);
fill([Start_Node_X,Start_Node_X,Start_Node_X-1,Start_Node_X-1,Start_Node_X],[Start_Node_Y,Start_Node_Y-1,Start_Node_Y-1,Start_Node_Y,Start_Node_Y],'yellow');
%Target_Node
Target_Node_X = Target_Node(1);
Target_Node_Y = Target_Node(2);
fill([Target_Node_X,Target_Node_X,Target_Node_X-1,Target_Node_X-1,Target_Node_X],[Target_Node_Y,Target_Node_Y-1,Target_Node_Y-1,Target_Node_Y,Target_Node_Y],'green');
%Obs_Node
for i = 1:length(Obs_Node_List)
    Obs_Node_X = Obs_Node_List(i,1);
    Obs_Node_Y = Obs_Node_List(i,2);
    fill([Obs_Node_X,Obs_Node_X,Obs_Node_X-1,Obs_Node_X-1,Obs_Node_X],[Obs_Node_Y,Obs_Node_Y-1,Obs_Node_Y-1,Obs_Node_Y,Obs_Node_Y],'black');    
end
%Path_List
plot(Path_List(:,1)-0.5,Path_List(:,2)-0.5,'*');
plot(Path_List(:,1)-0.5,Path_List(:,2)-0.5,'-');
%Cost_F
for i = 1:size(Path_List,1)
    PositionX = Path_List(i,1);
    PositionY = Path_List(i,2);
    Cost_F = Path_List(i,3);
    text(PositionX - 1,PositionY - 0.2,num2str(Cost_F));
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
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
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293