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

本文通过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]];

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