Matlab航迹规划仿真——A*算法

可以在这里看。在此主要解释下代码。主要参数:地图大小。起始点和目标点坐标。clear all。%%构建地图。

文章目录

  • 1. 初始化参数
  • 2. 构建地图
  • 3. A*算法搜索路径
  • 4. 路径优化
  • 5. 效果图
  • 6. 下载链接

可以在这里看

画仆:A星算法详解(个人认为最详细,最通俗易懂的一个版本)zhuanlan.zhihu.com

Matlab航迹规划仿真——A*算法

在此主要解释下代码。

1. 初始化参数

主要参数:

  • 地图大小
  • 起始点和目标点坐标
  1. clc
  2. clear all
  3. m = 30;n = 30;
  4. Spoint = [3 3]; %起始点坐标
  5. Epoint = [29 22]; %目标点坐标

2. 构建地图

-inf表示不可到达的障碍物点

  1. %%构建地图
  2. for i = 1:m+2
  3. if i == 1
  4. for j = 1:n+2
  5. Matrix(i,j) = -inf;
  6. end
  7. elseif i == m+2
  8. for j = 1:n+2
  9. Matrix(i,j) = -inf;
  10. end
  11. else
  12. for j = 1:n+2
  13. if ((j == 1)|(j == n+2))
  14. Matrix(i,j) = -inf;
  15. else
  16. Matrix(i,j) = inf;
  17. end
  18. end
  19. end
  20. end
  21. %%障碍
  22. for j=2:10
  23. Matrix(5,j)=-inf;
  24. for j=2:15
  25. Matrix(24,j)=-inf;
  26. for j=9:24
  27. %for j=6:24
  28. Matrix(10,j)=-inf;
  29. for j=20:31
  30. Matrix(15,j)=-inf;
  31. for j=5:20
  32. Matrix(20,j)=-inf;
  33. for j=18:27
  34. Matrix(28,j)=-inf;
  35. for i=2:6
  36. Matrix(i,18)=-inf;
  37. for i=17:20
  38. Matrix(i,5)=-inf;
  39. for i=23:25
  40. Matrix(i,20)=-inf;
  41. for i=13:17
  42. Matrix(i,13)=-inf;
  43. end
  44. end
  45. end
  46. end
  47. end
  48. end
  49. end
  50. end
  51. end
  52. end
  53. %end
  54. % 显示地图
  55. %subplot(2,2,1);
  56. h1 = plot(Spoint(1),Spoint(2),’gO’);
  57. hold on
  58. h2 = plot(Epoint(1),Epoint(2),’rO’);

3. A*算法搜索路径

  1. %%寻路
  2. Matrix(Spoint(1),Spoint(2))=0;
  3. Matrix(Epoint(1),Epoint(2))=inf;
  4. G=Matrix;
  5. F=Matrix;
  6. openlist=Matrix;
  7. closelist=Matrix;
  8. parentx=Matrix;
  9. parenty=Matrix;
  10. openlist(Spoint(1),Spoint(2)) =0;
  11. %closelist(Epoint(1),Epoint(2))=inf;
  12. for i = 1:n+2
  13. for j = 1:m+2
  14. k = Matrix(i,j);
  15. if(k == -inf)
  16. %subplot(2,2,1);
  17. h3 = plot(i,j,’k.’);
  18. % elseif(k == inf) % show green feasible point
  19. % %subplot(2,2,1);
  20. % plot(i,j,’gh’);
  21. % else
  22. % %subplot(2,2,1);
  23. % plot(i,j,’gh’);
  24. end
  25. hold on
  26. end
  27. end
  28. axis([0 m+3 0 n+3]);
  29. %subplot(2,2,1);
  30. plot(Epoint(1),Epoint(2),’b+’);
  31. %subplot(2,2,1);
  32. plot(Spoint(1),Spoint(2),’b+’);
  33. while(1)
  34. num=inf;
  35. for p=1:m+2
  36. for q=1:n+2
  37. if(openlist(p,q)==0&&closelist(p,q)~=1)
  38. Outpoint=[p,q];
  39. if(F(p,q)>=0&&num>F(p,q))
  40. num=F(p,q);
  41. Nextpoint=[p,q];
  42. end
  43. end
  44. end
  45. end
  46. closelist(Nextpoint(1),Nextpoint(2))=1;
  47. for i = 1:3
  48. for j = 1:3
  49. k = G(Nextpoint(1)-2+i,Nextpoint(2)-2+j);
  50. if(i==2&&j==2|closelist(Nextpoint(1)-2+i,Nextpoint(2)-2+j)==1)
  51. continue;
  52. elseif (k == -inf)
  53. G(Nextpoint(1)-2+i,Nextpoint(2)-2+j) = G(Nextpoint(1)-2+i,Nextpoint(2)-2+j);
  54. closelist(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=1;
  55. elseif (k == inf)
  56. distance=((i-2)^2+(j-2)^2)^0.5;
  57. G(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=G(Nextpoint(1),Nextpoint(2))+distance;
  58. openlist(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=0;
  59. % H=((Nextpoint(1)-2+i-Epoint(1))^2+(Nextpoint(2)-2+j-Epoint(2))^2)^0.5;%欧几里德距离启发函数
  60. H_diagonal=min(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比较复杂的对角线启发函数
  61. H_straight=abs(Nextpoint(1)-2+i-Epoint(1))+abs(Nextpoint(2)-2+j-Epoint(2));
  62. H=sqrt(2)*H_diagonal+(H_straight-2*H_diagonal);
  63. % H=max(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比较简单的对角线函数
  64. F(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=G(Nextpoint(1)-2+i,Nextpoint(2)-2+j)+H;
  65. parentx(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(1);
  66. parenty(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(2);
  67. else distance=((i-2)^2+(j-2)^2)^0.5;
  68. if(k>(distance+G(Nextpoint(1),Nextpoint(2))))
  69. k=distance+G(Nextpoint(1),Nextpoint(2));
  70. % H=((Nextpoint(1)-2+i-Epoint(1))^2+(Nextpoint(2)-2+j-Epoint(2))^2)^0.5; %欧几里德距离启发函数
  71. H_diagonal=min(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比较复杂的对角线启发函数
  72. H_straight=abs(Nextpoint(1)-2+i-Epoint(1))+abs(Nextpoint(2)-2+j-Epoint(2));
  73. H=sqrt(2)*10*H_diagonal+10*(H_straight-2*H_diagonal);
  74. % H=max(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比较简单的对角线函数
  75. F(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=k+H;
  76. parentx(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(1);
  77. parenty(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(2);
  78. end
  79. end
  80. if(((Nextpoint(1)-2+i)==Epoint(1)&&(Nextpoint(2)-2+j)==Epoint(2))|num==inf)
  81. parentx(Epoint(1),Epoint(2))=Nextpoint(1);
  82. parenty(Epoint(1),Epoint(2))=Nextpoint(2);
  83. break;
  84. end
  85. end
  86. if(((Nextpoint(1)-2+i)==Epoint(1)&&(Nextpoint(2)-2+j)==Epoint(2))|num==inf)
  87. parentx(Epoint(1),Epoint(2))=Nextpoint(1);
  88. parenty(Epoint(1),Epoint(2))=Nextpoint(2);
  89. break;
  90. end
  91. end
  92. if(((Nextpoint(1)-2+i)==Epoint(1)&&(Nextpoint(2)-2+j)==Epoint(2))|num==inf)
  93. parentx(Epoint(1),Epoint(2))=Nextpoint(1);
  94. parenty(Epoint(1),Epoint(2))=Nextpoint(2);
  95. break;
  96. end
  97. end
  98. P=[];
  99. s=1;
  100. while(1)
  101. if(num==inf)
  102. break;
  103. end
  104. %subplot(2,2,1);
  105. h4 = plot(Epoint(1),Epoint(2),’b+’);
  106. P(s,:)=Epoint;
  107. s=s+1;
  108. % pause(1);
  109. xx=Epoint(1);
  110. Epoint(1)=parentx(Epoint(1),Epoint(2));
  111. Epoint(2)=parenty(xx,Epoint(2));
  112. if(parentx(Epoint(1),Epoint(2))==Spoint(1)&&parenty(Epoint(1),Epoint(2))==Spoint(2))
  113. %subplot(2,2,1);
  114. plot(Epoint(1),Epoint(2),’b+’);
  115. P(s,:)=Epoint;
  116. break;
  117. end
  118. end
  119. P(s+1,:)=Spoint;
  120. legend([h1,h2,h3,h4],’起始点’,’目标点’,’障碍物’,’航迹点’);
  121. count=0;
  122. for i=2:12
  123. for j=2:12
  124. if(G(i,j)~=inf&&G(i,j)~=-inf)
  125. count=count+1;
  126. end
  127. end
  128. end
  129. count

4. 路径优化

  1. %将得到的折现曲线拟合成光滑的曲线
  2. P=P’;
  3. a=[];
  4. b=[];
  5. a=P(1,:);
  6. b=P(2,:);
  7. figure
  8. %subplot(2,2,3);
  9. plot(a,b);
  10. axis([0,n+3,0,n+3]);
  11. values = spcrv([[a(1) a a(end)];[b(1) b b(end)]],3);
  12. figure
  13. %subplot(2,2,4);
  14. plot(values(1,:),values(2,:),’r’);
  15. axis([0,m+3,0,m+3]);

5. 效果图

Matlab航迹规划仿真——A*算法

A*路径

Matlab航迹规划仿真——A*算法

优化后路径

Matlab航迹规划仿真——A*算法

6. 下载链接

直接复制到matlab即可使用,或者也可以点击下载

https://blog.csdn.net/qq_16775293/article/details/87654586

Matlab航迹规划仿真——A*算法

如果觉得有用可以点赞收藏哦~~~

免责声明:本站所有文章内容,图片,视频等均是来源于用户投稿和互联网及文摘转载整编而成,不代表本站观点,不承担相关法律责任。其著作权各归其原作者或其出版社所有。如发现本站有涉嫌抄袭侵权/违法违规的内容,侵犯到您的权益,请在线联系站长,一经查实,本站将立刻删除。 本文来自网络,若有侵权,请联系删除,如若转载,请注明出处:https://itzsg.com/10051.html

(0)
上一篇 2023年 4月 22日 下午11:55
下一篇 2023年 4月 22日 下午11:55

相关推荐

发表回复

您的邮箱地址不会被公开。 必填项已用 * 标注

联系我们YX

mu99908888

在线咨询: 微信交谈

邮件:itzsgw@126.com

工作时间:时刻准备着!

关注微信