文章目录

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

可以在这里看

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

在此主要解释下代码。

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. 效果图

A*路径

优化后路径

6. 下载链接

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

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

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