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]];

  • 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
文章知识点与官方知识档案匹配，可进一步学习相关知识
算法技能树首页概览50988 人正在系统学习中
注：本文转载自blog.csdn.net的chhttty的文章"https://blog.csdn.net/u013288925/article/details/122628387"。版权归原作者所有，此博客不拥有其著作权，亦不承担相应法律责任。如有侵权，请联系我们删除。
复制链接
相关推荐
发表评论

评论记录：

未查询到任何数据！