Matlab一键实现A星算法机器人路径规划

强盛机器学习

已于 2025-01-23 17:18:08 修改

阅读量1.8k

点赞数 24

分类专栏：改进优化算法与应用文章标签：人工智能算法 matlab 智能优化算法路径规划机器人栅格地图

于 2025-01-09 19:21:47 首次发布

本文链接：https://blog.youkuaiyun.com/QSJIQIXUEXI/article/details/145040593

版权

改进优化算法与应用专栏收录该内容

46 篇文章

订阅专栏

声明：文章是从本人公众号中复制而来，因此，想最新最快了解各类智能优化算法及其改进的朋友，可关注我的公众号：强盛机器学习，不定期会有很多免费代码分享~

后台很多小伙伴咨询移动机器人路径规划的问题，也给小伙伴们做了很多基于群智能算法（比如WOA、DBO算法）实现移动机器人路径规划的代码。不过要说路径规划中最经典的算法，还得提及A星算法。

因此，今天给大家带了一期基于A星（A*）算法的机器人路径规划通俗易懂详解与Matlab代码，大家也可自行将A星算法与自己的群智能算法进行对比凸显优越性~

需要代码的朋友可直接拉到最后~

您只需做的工作：一键运行main文件！非常适合新手小白！

算法思想

A星算法是一种启发性的算法，即由通过设定评估函数，全面性地对网格的各个节点进行评估。而每个节点就是机器人所到达的位置，对每个位置点都进行智能化评估，找到最好的位置，从而最终找到目标位置。A星算法评估函数如下

其中，F(n)是一种对总过程节点的评估函数，表示从起始节点到目标节点的总的估计代价，G(n)是表示在特定状态空间下，从起始节点到达下一个节点的实际代价；H(n)是预测从当前节点到达目标节点的大致需要多少代价的最佳路径的估计值，最短路径（最优解）的关键在于选取代价函数，所以将其视为核心问题。对于预估启发方法，通常选用曼哈顿预估方法，该方法在A星算法中较为常见使用，比较容易入手。

其中D为曼哈顿距离，ab为下所属的矩形面积常见的A星算法在扩展当前节点的周围节点一般采用八邻域法，即一次同时扩展当前节点周围八个点。在栅格图下，扩展节点图有前后左右表示如下：

算法原理

首先对于搜索区域进行简化成一组可量化的节点，节点可以适应于不同类型的区域之中，因为在划分搜索区域时，不仅仅是标准的矩形，可能是其他多边形，所以此时用节点代替多边形的某一区域则更准确和可靠。

其次A星算法在执行时，会创建两个列表，一个列表叫Openlist（开放列表），另一个列表叫Closelist(封闭列表)。将未扩展的节点放入Openlist中存放，而将已扩展完的节点存放到Closelist中。在Openlist中，将列表中节点根据F值的大小，按照从大到小的顺序进行排列，找到F值中的最小一个，并从Openlist中删除，将其添加到Closelist中。由于起点只有唯一存在的节点，将它放入开放列表，同时让其作为当前节点，通过当前节点搜索邻近八个扩展节点，最后将起点放入封闭列表。

如果这些节点不在开放列表中，则将这些扩展节点全部添加到开放列表中，并根据F值的大小，按照上述操作过程，选出F值最小的节点，添加到封闭列表，作为当前节点，并按照此情况继续搜索其余节点；如果这些扩展点在开放列表中，那么在扩展的节点的开放列表中，令当前节点为父节点，路径中所设计的代价函数将对这个节点进行评估，并且重新计算该节点的G值，与之前的G值进行比较，选择出G值最小，作为当前节点的G值，再对F值进行重新计算，依次排列，循环上述搜索步骤，直至找到目标，将目标点添加到开放列表中，将开放列表中的各个节点逆序排出，从而得到一条搜索路径，即为最佳路径（最短路径）。

A星算法作为一种典型的启发性搜索算法，常常对其进行改进和优化，使在路径规划算法中更富有效率。通常在路径规划中，它不需要把所有的节点都一一搜索出来，速度非常快，但同时，它对于路径规划中机器人移动存在代价预测，所以有的时候会出现搜索不到最适路径，这是其局限性。

算法流程图

为了使大家更好地理解，这边给出算法的流程图和伪代码，非常清晰！

如果实在看不懂，不用担心，可以看下源代码，再结合上文理解就一目了然了！

结果展示

这里以20*20的栅格地图为例，展示以下A*算法求解得到的路径规划结果。大家也可以对照代码自行替换成更大的地图或者更改障碍物的位置~

可以看到，在小型地图中，A*算法求解得到的路线已经非常接近最优解，甚至比一些智能算法还要好

部分代码展示

这里的地图在代码中均用0和1表示，非常清晰，1即代表障碍物，如果需要调整障碍物的位置只要调整1和0的位置即可~

%%  画地图
% 栅格地图的行数、列数定义
m = 20;   
n = 20;   
start_node = [1, 1];
target_node = [20, 20];

G=[0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0; 
   0 0 0 0 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0; 
   0 0 0 0 0 0 1 1 1 0 0 0 0 1 1 0 0 0 0 0; 
   0 0 0 0 0 0 1 1 1 0 0 0 0 1 1 0 0 0 0 0; 
   0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 
   0 1 1 1 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0; 
   0 1 1 1 0 0 1 1 1 0 0 0 0 0 0 0 0 0 0 0;
   0 1 1 1 0 0 1 1 1 0 1 1 1 1 0 0 0 0 0 0; 
   0 1 1 1 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0; 
   0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 0 0 0 0; 
   0 0 0 0 0 0 0 1 1 0 1 1 1 1 0 0 0 0 0 0; 
   0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0; 
   0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 1 1 1 1 0; 
   0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 1 1 1 1 0; 
   1 1 1 1 0 0 0 0 0 0 0 1 1 1 0 1 1 1 1 0; 
   1 1 1 1 0 0 1 1 0 0 1 1 1 0 0 0 0 0 0 0; 
   0 0 0 0 0 0 1 1 0 1 1 1 1 0 0 0 0 1 1 0; 
   0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0; 
   0 0 1 1 0 0 0 0 0 0 1 1 0 0 1 0 0 0 0 0; 
   0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0;];

%%  旋转矩阵
G=rot90(G,3);
[row,col]=find(G==1);
obs=[row,col];
for i = 1:m
    plot([0,n], [i, i], 'k');
    hold on
end
    
for j = 1:n
     plot([j, j], [0, m], 'k');
end