devbox
在线问答5
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

AStar算法（MATLAB实现）_astar matlab

作者：在线问答5 | 2024-07-18 10:01:32

赞

踩

astar matlab

A*算法简介：

A*（A-Star)算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法。算法中的距离估算值与实际值越接近，最终搜索速度越快。

结合Dijkstra算法与BFS算法的优点，得到的就是A星算法，A*算法是一种启发式搜索算法，它是在状态空间中的搜索，首先对每一个搜索的位置进行评估，得到最好的位置，再从这个位置进行搜索直到目标。这样可以省略大量无谓的搜索路径，提高了效率。在启发式搜索中，对位置的估价是十分重要的。采用了不同的估价可以有不同的效果。公式表示为： f(n)=g(n)+h(n),其中， f(n) 是从初始状态经由状态n到目标状态的代价估计，g(n) 是在状态空间中从初始状态到状态n的实际代价，h(n) 是从状态n到目标状态的最佳路径的估计代价，对于路径搜索问题，状态就是图中的节点，代价就是距离。h(n)的选取：保证找到最短路径（最优解的）条件，关键在于估价函数f(n)的选取（或者说h(n)的选取）。我们以d(n)表达状态n到目标状态的距离，那么h(n)的选取大致有如下三种情况：

（1）如果h(n)< d(n)到目标状态的实际距离，这种情况下，搜索的点数多，搜索范围大，效率低。但能得到最优解。

（2）如果h(n)=d(n)，即距离估计h(n)等于最短距离，那么搜索将严格沿着最短路径进行， 此时的搜索效率是最高的。

（3）如果 h(n)>d(n)，搜索的点数少，搜索范围小，效率高，但不能保证得到最优解。

  

此外：，如果h(n)为0，则只g(n)起作用，A*变成Dijkstra算法，保证找到最短路径。如果h(n)总是低于（或等于）从目标移动n到目标的成本，则保证 A* 找到最短路径。越低h(n)，节点 A* 扩展得越多，速度越慢。如果h(n)正好等于从n到目标的移动成本，那么 A* 将只遵循最佳路径，而不会扩展其他任何东西，使其非常快。尽管您不能在所有情况下都做到这一点，但您可以在某些特殊情况下做到这一点。很高兴知道给定完美的信息，A* 将表现完美。如果h(n)有时大于从移动n到目标的成本，则不能保证 A* 找到最短路径，但它可以运行得更快。在另一个极端，如果h(n)相对于 非常高g(n)，则仅h(n)起作用，并且 A* 变成贪婪的最佳优先搜索。

A*算法伪代码：

       把起始格添加到 "开启列表"

do{

        寻找开启列表中F值最低的格子, 我们称它为当前格.

        把它切换到关闭列表.

        对当前格相邻的8格中的每一个

        if (它不可通过 || 已经在 "关闭列表" 中) {

                什么也不做.

        }

        if (它不在开启列表中) {

                把它添加进 "开启列表", 把当前格作为这一格的父节点, 计算这一格的 FGH

        }

        if (它已经在开启列表中) {

                if (用 G 值为参考检查新的路径是否更好, 更低的G值意味着更好的路径) {

                        把这一格的父节点改成当前格, 并且重新计算这一格的 GF 值.

                }

        }

} while( 目标格已经在 "开启列表", 这时候路径被找到)

如果开启列表已经空了, 说明路径不存在.

最后从目标格开始, 沿着每一格的父节点移动直到回到起始格, 这就是路径.

代码：

  1. %% 单次路径规划
  2. clear all;
  3.  
  4. %% 初始化智能体
  5. fig = figure();				% 创建图形窗口
  6. hold on;
  7.  
  8. % 设置区域边界
  9. Estart_x = 0;  Estart_y = 0;
  10. Weight = 80;  High = 80;
  11. dL = 1;   %设置网格大小
  12.  
  13. axis([Estart_x-5 Weight 0 High]);
  14. axis equal;  %设置屏幕高宽比，使得每个坐标轴的具有均匀的刻度间隔
  15. %添加模糊刻度
  16. set(gca,'XMinorTick','on')
  17. set(gca,'YMinorTick','on')
  18. xlabel('X(m)');    ylabel('Y(m)'); 
  19. grid minor   %在刻度线之间添加次网格线
  20. rectangle('position',[Estart_x,Estart_y,Weight,High],'LineWidth',2);   %画边界
  21.  
  22. %% 区域网格化
  23. xL = 0:dL:Weight;
  24. yL = 0:dL:High;
  25. [XL , YL] = meshgrid(xL,yL);   %获得网格化后的X矩阵和Y矩阵
  26. C = zeros(size(XL));    %存储网格的高度值
  27. %--------------------%
  28. C(1,:) = 100;   C(:,1) = 100;
  29. C(end,:) = 100; C(:,end) = 100;
  30.  
  31. %% 设置障碍物覆盖值
  32. %--------------------%
  33. a1(1:21) = 20;
  34. C((20:40),20)=80;
  35. plot((20:40),a1,'k-','LineWidth',2);
  36. %--------------------%
  37. a2(1:30) = 30;
  38. C((1:29),30)=80;
  39. plot((0:29),a2,'k-','LineWidth',2);
  40. %--------------------%
  41. a3(1:20) = 50;
  42. C(50,(31:50))=80;
  43. plot(a3,(31:50),'k-','LineWidth',2);
  44. %--------------------%
  45. a4(1:21) = 60;
  46. C((40:60),60)=80;
  47. plot((40:60),a4,'k-','LineWidth',2);
  48. %--------------------%
  49. a5(1:21) = 25;
  50. C(25,(40:60))=80;
  51. plot(a5,(40:60),'k-','LineWidth',2);
  52.  
  53. close_list=0;open_list=0;                    %初始化节点容器
  54. start_x=70;start_y=3;                         %设置起点位置
  55. end_x=5;end_y=70;                            %设置终点位置
  56.  
  57. i=1;
  58. %本节点x坐标
  59. father_node(:,1)=start_x;                                                     
  60. %本节点y坐标
  61. father_node(:,2)=start_y;                                                    
  62. %本节点f(n)
  63. father_node(:,3)=norm([father_node(:,1),father_node(:,2)]-[start_x,start_y])+(father_node(:,1)-end_x)^2+(father_node(:,2)-end_y)^2;    
  64. %本节点g(n)
  65. father_node(:,4)=norm([father_node(:,1),father_node(:,2)]-[start_x,start_y]);
  66. %父节点y坐标
  67. father_node(:,5)=start_x;                                                     
  68. %父节点y坐标
  69. father_node(:,6)=start_y;   
  70. close_list=father_node;
  71.  
  72.  %返回8个邻居节点
  73. [N,index]=land(father_node(:,1),father_node(:,2),start_x,start_y,end_x,end_y,Estart_x,Estart_x+Weight,Estart_y,Estart_x+High,C,close_list);
  74. last_open_list = sortrows(N,3);                                         %排序并存入临时open_list
  75. father_node=last_open_list(1,:);                                        %f(n)最小邻居设为父节点
  76. last_open_list(1,:)=[];                                                 %删除临时open_list中最小f(n)节点
  77. cend=1;
  78.  
  79. while close_list(cend,1)~=end_x||close_list(cend,2)~=end_y
  80.     cend=cend+1;
  81.     x=father_node(1,1);                                                     %返回f(n)最小节点坐标
  82.     y=father_node(1,2);
  83.     [N,index]=land(father_node(:,1),father_node(:,2),start_x,start_y,end_x,end_y,Estart_x,Estart_x+Weight,Estart_y,Estart_x+High,C,close_list);
  84.     for i=1:index
  85.         a=N(i,1)==father_node(1,1); 
  86.         b=N(i,1)==father_node(1,2); 
  87.         f=N(i,1);
  88.         g=N(i,1);
  89.         if a>0&&b>0
  90.             if father_node(1,3)>g
  91.                 father_node(1,1)=N(i,1); 
  92.                 father_node(1,1)=N(i,2); 
  93.                 father_node(1,1)=N(i,3); 
  94.                 father_node(1,1)=N(i,4); 
  95.                 father_node(1,1)=N(i,5); 
  96.                 father_node(1,1)=N(i,6); 
  97.             end
  98.         else
  99.             last_open_list(i,:)=N(i,:);
  100.             plot(N(i,1),N(i,2),'bs');     %正方形(表示待检测节点)
  101.         end
  102.         
  103.     end
  104.     last_open_list = sortrows(last_open_list,3);
  105.     close_list(cend,1)=father_node(1,1);
  106.     close_list(cend,2)=father_node(1,2);
  107.     close_list(cend,3)=father_node(1,3);
  108.     close_list(cend,4)=father_node(1,4);
  109.     close_list(cend,5)=father_node(1,5);
  110.     close_list(cend,6)=father_node(1,6);
  111.     
  112.     father_node=last_open_list(1,:);
  113.     last_open_list(1,:)=[];
  114.     
  115. end
  116. plot(close_list(:,1),close_list(:,2),'r*');   %当前节点标绿色*号
  117.  
  118.  
  119.  
  120. function [N,index]=land(x,y,start_x,start_y,end_x,end_y,xmin,xmax,ymin,ymax,C,close_list)
  121.     index=0;
  122.     %上邻域
  123.     if judge(x,y+1,xmin,xmax,ymin,ymax,C,close_list)==1
  124.         index=index+1;
  125.         N(index,1)=x;
  126.         N(index,2)=y+1;
  127.         N(index,3)=norm([x,y+1]-[start_x,start_y])+(x-end_x)^2+(y+1-end_y)^2;
  128.         N(index,4)=norm([x,y+1]-[start_x,start_y]);
  129.         N(index,5)=x;
  130.         N(index,6)=y;
  131.         
  132.     end
  133.     %下邻域
  134.     if judge(x,y-1,xmin,xmax,ymin,ymax,C,close_list)==1
  135.         index=index+1;
  136.         N(index,1)=x;
  137.         N(index,2)=y-1;
  138.         N(index,3)=norm([x,y-1]-[start_x,start_y])+(x-end_x)^2+(y-1-end_y)^2;
  139.         N(index,4)=norm([x,y-1]-[start_x,start_y]);
  140.         N(index,5)=x;
  141.         N(index,6)=y;
  142.  
  143.     end
  144.     %左邻域
  145.     if judge(x-1,y,xmin,xmax,ymin,ymax,C,close_list)==1
  146.         index=index+1;
  147.         N(index,1)=x-1;
  148.         N(index,2)=y;
  149.         N(index,3)=norm([x-1,y]-[start_x,start_y])+(x-1-end_x)^2+(y-end_y)^2;
  150.         N(index,4)=norm([x-1,y]-[start_x,start_y]);
  151.         N(index,5)=x;
  152.         N(index,6)=y;
  153.  
  154.     end
  155.     %右邻域
  156.     if judge(x+1,y,xmin,xmax,ymin,ymax,C,close_list)==1
  157.         index=index+1;
  158.         N(index,1)=x+1;
  159.         N(index,2)=y;
  160.         N(index,3)=norm([x+1,y]-[start_x,start_y])+(x+1-end_x)^2+(y-end_y)^2;
  161.         N(index,4)=norm([x+1,y]-[start_x,start_y]);
  162.         N(index,5)=x;
  163.         N(index,6)=y;
  164.  
  165.     end
  166.     %左上邻域
  167.     if judge(x-1,y+1,xmin,xmax,ymin,ymax,C,close_list)==1
  168.         index=index+1;
  169.         N(index,1)=x-1;
  170.         N(index,2)=y+1;
  171.         N(index,3)=norm([x-1,y+1]-[start_x,start_y])+(x-1-end_x)^2+(y+1-end_y)^2;
  172.         N(index,4)=norm([x-1,y+1]-[start_x,start_y]);
  173.         N(index,5)=x;
  174.         N(index,6)=y;
  175.  
  176.     end
  177.     %右上邻域
  178.     if judge(x+1,y+1,xmin,xmax,ymin,ymax,C,close_list)==1
  179.         index=index+1;
  180.         N(index,1)=x+1;
  181.         N(index,2)=y+1;
  182.         N(index,3)=norm([x+1,y+1]-[start_x,start_y])+(x+1-end_x)^2+(y+1-end_y)^2;
  183.         N(index,4)=norm([x+1,y+1]-[start_x,start_y]);
  184.         N(index,5)=x;
  185.         N(index,6)=y;
  186.     end
  187.     %左下邻域
  188.     if judge(x-1,y-1,xmin,xmax,ymin,ymax,C,close_list)==1
  189.         index=index+1;
  190.         N(index,1)=x-1;
  191.         N(index,2)=y-1;
  192.         N(index,3)=norm([x-1,y-1]-[start_x,start_y])+(x-1-end_x)^2+(y-1-end_y)^2;
  193.         N(index,4)=norm([x-1,y-1]-[start_x,start_y]);
  194.         N(index,5)=x;
  195.         N(index,6)=y;
  196.     end
  197.     %右下邻域
  198.     if judge(x+1,y-1,xmin,xmax,ymin,ymax,C,close_list)==1
  199.         index=index+1;
  200.         N(index,1)=x+1;
  201.         N(index,2)=y-1;
  202.         N(index,3)=norm([x+1,y-1]-[start_x,start_y])+(x+1-end_x)^2+(y-1-end_y)^2;
  203.         N(index,4)=norm([x+1,y-1]-[start_x,start_y]);
  204.         N(index,5)=x;
  205.         N(index,6)=y;
  206.     end
  207. end
  208.  
  209. function ruselt=judge(x,y,xmin,xmax,ymin,ymax,C,close_list)
  210.     if x<xmin||x>xmax||y<ymin||y>ymax
  211.         ruselt=0;
  212.         return
  213.     end
  214.     if C(x,y)==100||C(x,y)==80
  215.         ruselt=0;
  216.         return
  217.     end
  218.  
  219.     [i,j]=size(close_list);
  220.  
  221.     for ii=1:i
  222.         if close_list(ii,1)==x&&close_list(ii,2)==y
  223.         ruselt=0;
  224.         return
  225.         end
  226.     end
  227.     
  228.     ruselt=1;
  229.  
  230.  
  231. end
  232.

参考：

A星(A*、A Star)路径规划算法详解（附MATLAB代码）

声明：本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：【wpsshop博客】
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号