259 lines
7.4 KiB
Plaintext
259 lines
7.4 KiB
Plaintext
|
附录B 蚁群算法实现路径规划MATLAB代码
|
|||
|
|
1. function main()
|
|||
|
|
2. G=[0 0 0 1 0 0;
|
|||
|
|
3. 0 1 0 0 0 0;
|
|||
|
|
4. 0 1 1 0 1 0;
|
|||
|
|
5. 0 0 0 0 1 0;
|
|||
|
|
6. 0 1 1 0 1 0;
|
|||
|
|
7. 0 0 0 0 0 0;];
|
|||
|
|
8. MM=size(G,1); % 总的空间为01矩阵，如果为1表示有障碍物
|
|||
|
|
9. Tau=ones(MM*MM,MM*MM); % Tau 初始信息素矩阵
|
|||
|
|
10. Tau=8.*Tau;
|
|||
|
|
11. K=100; %迭代次数（指蚂蚁出动多少波）
|
|||
|
|
12. M=10; %蚂蚁个数
|
|||
|
|
13. S=1 ; %最短路径的起始点
|
|||
|
|
14. E=MM*MM; %最短路径的目的点
|
|||
|
|
15. Alpha=1; % Alpha 表征信息素重要程度的参数
|
|||
|
|
16. Beta=5; % Beta 表征启发式因子重要程度的参数
|
|||
|
|
17. Rho=0.3 ; % Rho 信息素蒸发系数
|
|||
|
|
18. Q=1; % Q 信息素增加强度系数
|
|||
|
|
19. minkl=inf;
|
|||
|
|
20. mink=0;
|
|||
|
|
21. minl=0;
|
|||
|
|
22. D=G2D(G);
|
|||
|
|
23. N=size(D,1); %N表示问题的规模（像素个数）
|
|||
|
|
24. a=1; %小方格像素的边长
|
|||
|
|
25. Ex=a*(mod(E,MM)-0.5); %终止点横坐标
|
|||
|
|
26. if Ex==-0.5
|
|||
|
|
27. Ex=MM-0.5;
|
|||
|
|
28. end
|
|||
|
|
29. Ey=a*(MM+0.5-ceil(E/MM)); %终止点纵坐标
|
|||
|
|
30. Eta=zeros(N); %启发式信息，取为至目标点的直线距离的倒数
|
|||
|
|
31. %%%%%%以下为启发式信息矩阵%%%%%%%%%%%
|
|||
|
|
32. for i=1:N
|
|||
|
|
33. ix=a*(mod(i,MM)-0.5);
|
|||
|
|
34. if ix==-0.5
|
|||
|
|
35. ix=MM-0.5;
|
|||
|
|
36. end
|
|||
|
|
37. iy=a*(MM+0.5-ceil(i/MM));
|
|||
|
|
38. if i~=E
|
|||
|
|
39. Eta(i)=1/((ix-Ex)^2+(iy-Ey)^2)^0.5;
|
|||
|
|
40. else
|
|||
|
|
41. Eta(i)=10;
|
|||
|
|
42. end
|
|||
|
|
43. end
|
|||
|
|
44. ROUTES=cell(K,M); %用细胞结构存储每一代的每一只蚂蚁的爬行路线
|
|||
|
|
45. PL=zeros(K,M); %用矩阵存储每一代的每一只蚂蚁的爬行路线长度
|
|||
|
|
46. %%%%%%%启动K轮蚂蚁觅食活动，每轮派出M只蚂蚁%%%%%%%
|
|||
|
|
47. for k=1:K
|
|||
|
|
48. for m=1:M
|
|||
|
|
49. %%%%%%%%%%%%状态初始化%%%%%%%%%%%%%%%%
|
|||
|
|
50. W=S; %当前节点初始化为起始点
|
|||
|
|
51. Path=S; %爬行路线初始化
|
|||
|
|
52. PLkm=0; %爬行路线长度初始化
|
|||
|
|
53. TABUkm=ones(N); %禁忌表初始化
|
|||
|
|
54. TABUkm(S)=0; %已经在初始点了，因此要排除
|
|||
|
|
55. DD=D; %邻接矩阵初始化
|
|||
|
|
56. %%%%%%%%%%%下一步可以前往的节点%%%%%%%%%%
|
|||
|
|
57. DW=DD(W,:);
|
|||
|
|
58. DW1=find(DW);
|
|||
|
|
59. for j=1:length(DW1)
|
|||
|
|
60. if TABUkm(DW1(j))==0
|
|||
|
|
61. DW(DW1(j))=0;
|
|||
|
|
62. end
|
|||
|
|
63. end
|
|||
|
|
64. LJD=find(DW);
|
|||
|
|
65. Len_LJD=length(LJD); %可选节点的个数
|
|||
|
|
66. %%%%%%%%%%%%蚂蚁未遇到食物或者陷入死胡同或者觅食停止%%%%%%%%%%%
|
|||
|
|
67. while W~=E&&Len_LJD>=1
|
|||
|
|
68. %%%%%%%%转轮赌法选择下一步怎么走%%%%%%%%%%%
|
|||
|
|
69. PP=zeros(Len_LJD);
|
|||
|
|
70. for i=1:Len_LJD
|
|||
|
|
71. PP(i)=(Tau(W,LJD(i))^Alpha)*((Eta(LJD(i)))^Beta);
|
|||
|
|
72. end
|
|||
|
|
73. sumpp=sum(PP);
|
|||
|
|
74. PP=PP/sumpp; %建立概率分布
|
|||
|
|
75. Pcum(1)=PP(1);
|
|||
|
|
76. for i=2:Len_LJD
|
|||
|
|
77. Pcum(i)=Pcum(i-1)+PP(i);
|
|||
|
|
78. end
|
|||
|
|
79. Select=find(Pcum>=rand);
|
|||
|
|
80. to_visit=LJD(Select(1));
|
|||
|
|
81. %%%%%%%%%%%%状态更新和记录%%%%%%%%%%%%
|
|||
|
|
82. Path=[Path,to_visit]; %路径增加
|
|||
|
|
83. PLkm=PLkm+DD(W,to_visit); %路径长度增加
|
|||
|
|
84. W=to_visit; %蚂蚁移到下一个节点
|
|||
|
|
85. for kk=1:N
|
|||
|
|
86. if TABUkm(kk)==0
|
|||
|
|
87. DD(W,kk)=0;
|
|||
|
|
88. DD(kk,W)=0;
|
|||
|
|
89. end
|
|||
|
|
90. end
|
|||
|
|
91. TABUkm(W)=0; %已访问过的节点从禁忌表中删除
|
|||
|
|
92. DW=DD(W,:);
|
|||
|
|
93. DW1=find(DW);
|
|||
|
|
94. for j=1:length(DW1)
|
|||
|
|
95. if TABUkm(DW1(j))==0
|
|||
|
|
96. DW(j)=0;
|
|||
|
|
97. end
|
|||
|
|
98. end
|
|||
|
|
99. LJD=find(DW);
|
|||
|
|
100. Len_LJD=length(LJD); %可选节点的个数
|
|||
|
|
101. end
|
|||
|
|
102. %%%%记下每一代每一只蚂蚁的觅食路线和路线长度%%%%%
|
|||
|
|
103. ROUTES{k,m}=Path;
|
|||
|
|
104. if Path(end)==E
|
|||
|
|
105. PL(k,m)=PLkm;
|
|||
|
|
106. if PLkm<minkl
|
|||
|
|
107. mink=k;minl=m;minkl=PLkm;
|
|||
|
|
108. end
|
|||
|
|
109. else
|
|||
|
|
110. PL(k,m)=0;
|
|||
|
|
111. end
|
|||
|
|
112. end
|
|||
|
|
113. %%%%%%%%%%%%%%更新信息素%%%%%%%%%%%%%%%%
|
|||
|
|
114. Delta_Tau=zeros(N,N);%更新量初始化
|
|||
|
|
115. for m=1:M
|
|||
|
|
116. if PL(k,m)
|
|||
|
|
117. ROUT=ROUTES{k,m};
|
|||
|
|
118. TS=length(ROUT)-1;%跳数
|
|||
|
|
119. PL_km=PL(k,m);
|
|||
|
|
120. for s=1:TS
|
|||
|
|
121. x=ROUT(s);
|
|||
|
|
122. y=ROUT(s+1);
|
|||
|
|
123. Delta_Tau(x,y)=Delta_Tau(x,y)+Q/PL_km;
|
|||
|
|
124. Delta_Tau(y,x)=Delta_Tau(y,x)+Q/PL_km;
|
|||
|
|
125. end
|
|||
|
|
126. end
|
|||
|
|
127. end
|
|||
|
|
128. Tau=(1-Rho).*Tau+Delta_Tau;%信息素挥发一部分，新增加一部分
|
|||
|
|
|
|||
|
|
129. end
|
|||
|
|
130. % 绘图 %
|
|||
|
|
131. plotif=1; %是否绘图的控制参数
|
|||
|
|
132. if plotif==1 %绘收敛曲线
|
|||
|
|
133. minPL=zeros(K);
|
|||
|
|
134. for i=1:K
|
|||
|
|
135. PLK=PL(i,:);
|
|||
|
|
136. Nonzero=find(PLK);
|
|||
|
|
137. PLKPLK=PLK(Nonzero);
|
|||
|
|
138. minPL(i)=min(PLKPLK);
|
|||
|
|
139. end
|
|||
|
|
140. figure(1)
|
|||
|
|
141. plot(minPL);
|
|||
|
|
142. hold on
|
|||
|
|
143. grid on
|
|||
|
|
144. title('收敛曲线变化趋势');
|
|||
|
|
145. xlabel('迭代次数');
|
|||
|
|
146. ylabel('最小路径长度'); %绘爬行图
|
|||
|
|
147. figure(2)
|
|||
|
|
148. axis([0,MM,0,MM])
|
|||
|
|
149. for i=1:MM
|
|||
|
|
150. for j=1:MM
|
|||
|
|
151. if G(i,j)==1
|
|||
|
|
152. x1=j-1;y1=MM-i;
|
|||
|
|
153. x2=j;y2=MM-i;
|
|||
|
|
154. x3=j;y3=MM-i+1;
|
|||
|
|
155. x4=j-1;y4=MM-i+1;
|
|||
|
|
156. fill([x1,x2,x3,x4],[y1,y2,y3,y4],[0.2,0.2,0.2]);
|
|||
|
|
157. hold on
|
|||
|
|
158. else
|
|||
|
|
159. x1=j-1;y1=MM-i;
|
|||
|
|
160. x2=j;y2=MM-i;
|
|||
|
|
161. x3=j;y3=MM-i+1;
|
|||
|
|
162. x4=j-1;y4=MM-i+1;
|
|||
|
|
163. fill([x1,x2,x3,x4],[y1,y2,y3,y4],[1,1,1]);
|
|||
|
|
164. hold on
|
|||
|
|
165. end
|
|||
|
|
166. end
|
|||
|
|
167. end
|
|||
|
|
168. hold on
|
|||
|
|
169. title('最优运动轨迹');
|
|||
|
|
170. xlabel('坐标x');
|
|||
|
|
171. ylabel('坐标y');
|
|||
|
|
172. ROUT=ROUTES{mink,minl};
|
|||
|
|
173. LENROUT=length(ROUT);
|
|||
|
|
174. Rx=ROUT;
|
|||
|
|
175. Ry=ROUT;
|
|||
|
|
176. for ii=1:LENROUT
|
|||
|
|
177. Rx(ii)=a*(mod(ROUT(ii),MM)-0.5);
|
|||
|
|
178. if Rx(ii)==-0.5
|
|||
|
|
179. Rx(ii)=MM-0.5;
|
|||
|
|
180. end
|
|||
|
|
181. Ry(ii)=a*(MM+0.5-ceil(ROUT(ii)/MM));
|
|||
|
|
182. end
|
|||
|
|
183. plot(Rx,Ry)
|
|||
|
|
184. end
|
|||
|
|
185. plotif2=0; %绘各代蚂蚁爬行图
|
|||
|
|
186. if plotif2==1
|
|||
|
|
187. figure(3)
|
|||
|
|
188. axis([0,MM,0,MM])
|
|||
|
|
189. for i=1:MM
|
|||
|
|
190. for j=1:MM
|
|||
|
|
191. if G(i,j)==1
|
|||
|
|
192. x1=j-1;y1=MM-i;
|
|||
|
|
193. x2=j;y2=MM-i;
|
|||
|
|
194. x3=j;y3=MM-i+1;
|
|||
|
|
195. x4=j-1;y4=MM-i+1;
|
|||
|
|
196. fill([x1,x2,x3,x4],[y1,y2,y3,y4],[0.2,0.2,0.2]);
|
|||
|
|
197. hold on
|
|||
|
|
198. else
|
|||
|
|
199. x1=j-1;y1=MM-i;
|
|||
|
|
200. x2=j;y2=MM-i;
|
|||
|
|
201. x3=j;y3=MM-i+1;
|
|||
|
|
202. x4=j-1;y4=MM-i+1;
|
|||
|
|
203. fill([x1,x2,x3,x4],[y1,y2,y3,y4],[1,1,1]);
|
|||
|
|
204. hold on
|
|||
|
|
205. end
|
|||
|
|
206. end
|
|||
|
|
207. end
|
|||
|
|
208. for k=1:K
|
|||
|
|
209. PLK=PL(k,:);
|
|||
|
|
210. minPLK=min(PLK);
|
|||
|
|
211. pos=find(PLK==minPLK);
|
|||
|
|
212. m=pos(1);
|
|||
|
|
213. ROUT=ROUTES{k,m};
|
|||
|
|
214. LENROUT=length(ROUT);
|
|||
|
|
215. Rx=ROUT;
|
|||
|
|
216. Ry=ROUT;
|
|||
|
|
217. for ii=1:LENROUT
|
|||
|
|
218. Rx(ii)=a*(mod(ROUT(ii),MM)-0.5);
|
|||
|
|
219. if Rx(ii)==-0.5
|
|||
|
|
220. Rx(ii)=MM-0.5;
|
|||
|
|
221. end
|
|||
|
|
222. Ry(ii)=a*(MM+0.5-ceil(ROUT(ii)/MM));
|
|||
|
|
223. end
|
|||
|
|
224. plot(Rx,Ry)
|
|||
|
|
225. hold on
|
|||
|
|
226. end
|
|||
|
|
227. end
|
|||
|
|
228. function D=G2D(G)
|
|||
|
|
229. l=size(G,1);
|
|||
|
|
230. D=zeros(l*l,l*l);
|
|||
|
|
231. for i=1:l
|
|||
|
|
232. for j=1:l
|
|||
|
|
233. if G(i,j)==0
|
|||
|
|
234. for m=1:l
|
|||
|
|
235. for n=1:l
|
|||
|
|
236. if G(m,n)==0
|
|||
|
|
237. im=abs(i-m);jn=abs(j-n);
|
|||
|
|
238. if im+jn==1||(im==1&&jn==1)
|
|||
|
|
239. D((i-1)*l+j,(m-1)*l+n)=(im+jn)^0.5;
|
|||
|
|
240. end
|
|||
|
|
241. end
|
|||
|
|
242. end
|
|||
|
|
243. end
|
|||
|
|
244. end
|
|||
|
|
245. end
|
|||
|
|
246. end
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|
|||
|
|
|