Matlab编程技巧：A*算法仿真
本文通过Matlab编程实现A* 算法，并通过几个简单的例子，打断点调试输出A*搜索的过程。
文章目录
- 1 A* 算法简介
- 2 Matlab编程实现
-
- 2.1 输入参数
- 2.2 初始化变量
- 2.3 循环过程
- 2.4 循环后处理
- 3 调试与验证
-
- 3.1 案例1
- 3.2 案例2
- 3.3 案例3
- 4 总结
- 5 附录 Matlab程序代码
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]];
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]];
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]];
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