Matlab航跡規劃仿真——A*算法

文章目錄

• 1. 初始化參數
• 2. 構建地圖
• 3. A*算法搜索路徑
• 4. 路徑優化
• 5. 效果圖
• 6. 下載鏈接

 

基本的A*算法在這裏不再講述，想要了解的朋友可以自己在CSDN搜索，在此主要解釋下代碼。

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.  
64.                 % H=max(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比較簡單的對角線函數
65.                 
66.                 F(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=G(Nextpoint(1)-2+i,Nextpoint(2)-2+j)+H;
67.                 parentx(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(1);
68.                 parenty(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(2);
69.             else distance=((i-2)^2+(j-2)^2)^0.5;
70.                 if(k>(distance+G(Nextpoint(1),Nextpoint(2))))
71.                 k=distance+G(Nextpoint(1),Nextpoint(2));
72.                % H=((Nextpoint(1)-2+i-Epoint(1))^2+(Nextpoint(2)-2+j-Epoint(2))^2)^0.5;  %歐幾里德距離啓發函數
73.                 H_diagonal=min(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比較複雜的對角線啓發函數
74.                 H_straight=abs(Nextpoint(1)-2+i-Epoint(1))+abs(Nextpoint(2)-2+j-Epoint(2));
75.                 H=sqrt(2)*10*H_diagonal+10*(H_straight-2*H_diagonal);
76.  
77.                  % H=max(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比較簡單的對角線函數
78.                 
79.                 F(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=k+H;
80.                 parentx(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(1);
81.                 parenty(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(2);
82.                 end
83.             end
84.             if(((Nextpoint(1)-2+i)==Epoint(1)&&(Nextpoint(2)-2+j)==Epoint(2))|num==inf)
85.                  parentx(Epoint(1),Epoint(2))=Nextpoint(1);
86.                 parenty(Epoint(1),Epoint(2))=Nextpoint(2);
87.                 break;
88.             end
89.         end
90.         if(((Nextpoint(1)-2+i)==Epoint(1)&&(Nextpoint(2)-2+j)==Epoint(2))|num==inf)
91.              parentx(Epoint(1),Epoint(2))=Nextpoint(1);
92.                 parenty(Epoint(1),Epoint(2))=Nextpoint(2);
93.             break;
94.         end
95.     end
96.     if(((Nextpoint(1)-2+i)==Epoint(1)&&(Nextpoint(2)-2+j)==Epoint(2))|num==inf)
97.          parentx(Epoint(1),Epoint(2))=Nextpoint(1);
98.                 parenty(Epoint(1),Epoint(2))=Nextpoint(2);
99.         break;
100.     end
101. end
102.     P=[];
103.     s=1;
104. while(1)
105.     if(num==inf)
106.         break;
107.     end
108.     %subplot(2,2,1);
109.     h4 = plot(Epoint(1),Epoint(2),'b+');
110.     P(s,:)=Epoint;
111.     s=s+1;
112. %      pause(1);
113.     xx=Epoint(1);
114.     Epoint(1)=parentx(Epoint(1),Epoint(2));
115.     Epoint(2)=parenty(xx,Epoint(2));
116.     if(parentx(Epoint(1),Epoint(2))==Spoint(1)&&parenty(Epoint(1),Epoint(2))==Spoint(2))
117.         %subplot(2,2,1);
118.         plot(Epoint(1),Epoint(2),'b+');
119.         P(s,:)=Epoint;
120.         break;
121.     end
122. end
123. P(s+1,:)=Spoint;
124. legend([h1,h2,h3,h4],'起始點','目標點','障礙物','航跡點');
125. count=0;
126. for i=2:12
127.     for j=2:12
128.         if(G(i,j)~=inf&&G(i,j)~=-inf)
129.             count=count+1;
130.         end
131.     end
132. end
133. 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