热门标签
热门文章
- 1Ubuntu18.04melodic安装Arbotix出现failed to connect to github.com443：拒绝连接_ubuntu下载arbotix
- 2如果王自如支持友推，而罗永浩不支持的话......
- 3洛谷B2142题解
- 4安路FPGA-TangDynasty软件学习笔记一_安路fpga怎么创建工程
- 5Elasticsearch 权威指南一书 总结_elasticsearch 排序的书
- 6集群小文件太多问题（spark-sql优化）_conf spark.sql.shuffle.partitions=10。
- 7向量空间模型 2022-1-17_人工智能 向量空间模型
- 8【改进YOLOv8】融合Gold-YOLO的车辆未礼让行人检测系统_yolov8-goldyolo-asf
- 9零基础如何学习挖漏洞？白帽黑客从零入门，看这篇就够了_新手如何挖第一个漏洞？
- 1010分钟带你深入理解Transformer原理及实现
当前位置: article > 正文
基于蚁群算法的栅格地图路径规划-考虑机器人体积
作者：weixin_40725706 | 2024-04-22 23:15:50
赞
踩
基于蚁群算法的栅格地图路径规划-考虑机器人体积
基于蚁群算法的栅格地图路径规划-考虑机器人体积
**
一、基础栅格地图路径规划*
clc;clear all; G=[0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 1 0 0 0 0 1 1 0 0 0 0 0; 0 0 0 0 0 0 1 1 0 0 0 0 1 0 0 1 0 0 0 0; 0 0 0 0 0 0 1 0 1 0 0 0 0 0 0 0 0 0 0 0; 0 1 1 1 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0; 0 1 1 1 0 0 0 1 1 0 0 0 0 0 0 1 0 0 0 0; 0 1 1 1 0 0 0 0 1 1 0 0 0 1 0 1 0 0 0 0; 0 1 1 0 0 0 0 0 0 0 0 0 1 1 0 1 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0; 0 0 0 0 0 0 0 1 1 0 0 0 1 1 0 0 0 0 0 0; 0 0 0 0 1 1 0 0 1 1 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 1 1 0 0 0 0 0 0 0 1 0 0 1 1 1 0; 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0; 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0; 0 0 1 1 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;]; MM=size(G,1); % G 地形图为01矩阵，如果为1表示障碍物 ，size()返回矩阵G的行数，MM=20。如果size中的1改为2，则返回矩阵G的列数 Tau=ones(MM*MM,MM*MM); % Tau 初始信息素矩阵，规模为400*400,。记录每个栅格到其他栅格的信息素量。ones产生一个全是1的矩阵 Tau=8.*Tau; %矩阵全为8 K=100; %迭代次数（指蚂蚁出动多少波） M=50; %蚂蚁个数 S=1; %最短路径的起始点，第一个栅格点为起始地，这里用1-400的序号代表点的位置，默认左上角是第一个 E=400; %最短路径的目的点 Alpha=1; % Alpha 表征信息素重要程度的参数 Beta=8; % Beta 表征启发式因子重要程度的参数 Rho=0.4; % Rho 信息素蒸发系数 Q=1; % Q 信息素增加强度系数 minkl=inf; %inf代表无穷大量 mink=1; minl=1; D=G2D(G); %G2D栅格矩阵转换成邻接矩阵的函数，D用于存储每个点到各自相邻无障碍栅格点的代价值（该点到其他点的可行性） N=size(D,1); %N表示问题的规模（像素个数），N=400 a=1; %小方格像素的边长 Ex=a*(mod(E,MM)-0.5); %终止点横坐标，这一行E=-0.5。这是栅格地图的直角坐标表示法和序号表示法的映射关系。 if Ex==-0.5 Ex=MM-0.5; %这一行Ex=19.5 end Ey=a*(MM+0.5-ceil(E/MM)); %终止点纵坐标，栅格地图的直角坐标表示法和序号表示法的映射关系。 Eta=zeros(N); %启发式信息矩阵，取为至目标点的直线距离的倒数，N=400，Eta大小为400*400 %以下启发式信息矩阵 %下面的for循环就是得到每个栅格点的坐标，并构建启发式信息矩阵（每个栅格点到目标点的距离） for i=1:N ix=a*(mod(i,MM)-0.5); %mod求i除以MM的余数 if ix==-0.5 ix=MM-0.5; %得到每一个栅格点的横坐标，一共400个 end iy=a*(MM+0.5-ceil(i/MM)); %得到每一个栅格点的纵坐标，一共400个，这里ix，iy都是有400个元素的向量 if i~=E %~=不等号，E代表目标点这里是400 Eta(i)=1/((ix-Ex)^2+(iy-Ey)^2)^0.5;%计算该点到目标的距离长度，并输入到启发式信息矩阵Eta，一共400个 else Eta(i)=100; end end %到这里，步骤2完成 ROUTES=cell(K,M); %用细胞结构存储每一代的每一只蚂蚁的爬行路线。K迭代次数，M蚂蚁个数。cell类型每个单元可以储存任何数据 %把ROUTES初始化为一个K行M列的空cell类型数据 %比如ROUTES{1,1}=rand(5)，就是ROUTES的1行1列的单元中存储的就是一个随机的5×5的方阵 PL=zeros(K,M); %用矩阵存储每一代的每一只蚂蚁的爬行路线长度 %启动K轮蚂蚁觅食活动，每轮派出M只蚂蚁 for k=1:K for m=1:M %状态初始化 W=S; %当前节点初始化为起始点，S=1是最短路径起始点，W用于表示当前所在节点 Path=S; %爬行路线初始化 PLkm=0; %爬行路线长度初始化 TABUkm=ones(N); %禁忌表初始化，N=400 TABUkm(S)=0; %已经在初始点了，因此要排除 DD=D; %邻接矩阵初始化，DD是400*400大小 %下一步可以前往的节点，蚂蚁走出第一步 DW=DD(W,:); %取行操作，取矩阵DD第W行的全部元素，矩阵DD的第一行代表第一个栅格点的邻接矩阵（也就是第一个栅格点能走的下一个点存储在第一行） %第二行就是存储第二个栅格点到其他栅格点的可行性，DW大小是400*1 DW1=find(DW); %find函数，返回向量DW中非0元素的位置（就是非0元素是第几个数） %DW1就得到了所以可以去的栅格点的序号，这样就筛选出当前可选择的点 for j=1:length(DW1) if TABUkm(DW1(j))==0 %将可选择的点与禁忌表对比，如果这个点在禁忌表在是0，则这个点不能走 DW(DW1(j))=0; %更新这一行的邻接矩阵（蚂蚁当前所在点与其他所有点的邻接矩阵为W这一行），经过和禁忌表对比，如果不能走则更新为0 end end %到这里步骤3结束 LJD=find(DW); %找到经过与禁忌表对比以后还能选择的栅格点有哪些 Len_LJD=length(LJD);%可选节点的个数 %蚂蚁未遇到食物或者陷入死胡同（可选栅格点Len_LJD<1 ）或者觅食停止 while W~=E&&Len_LJD>=1 %当前所在点不是目标点并且可选择节点数大于等于1，则进行下一步 %转轮赌法选择下一步怎么走 PP=zeros(Len_LJD); %得到可选节点数*可选节点数的0矩阵 for i=1:Len_LJD PP(i)=(Tau(W,LJD(i))^Alpha)*((Eta(LJD(i)))^Beta); %概率选择公式的分子，PP（i）给矩阵第i个中的元素赋值 %Tau是信息素矩阵，W就是当前点，LJD（i）代表下一栅格点的位置（概率公式中的下标j）。Eta启发式信息矩阵 end sumpp=sum(PP); %求概率公式的分母，将所有可访问栅格点的求和 PP=PP/sumpp; %建立概率分布，完整的概率公式 Pcum(1)=PP(1); for i=2:Len_LJD Pcum(i)=Pcum(i-1)+PP(i); %这个循环求所有路径概率的累加和 end Select=find(Pcum>=rand); %Pcum是路径概率的累加和，来模仿各条路径对应轮盘扇面大小。rand是[0,1]之间的随机数 %find(Pcum>=rand)返回所有大于 rand 的扇面 to_visit=LJD(Select(1)); %选择第一个大于 rand 的扇面 %到这里步骤4结束 %状态更新和记录 Path=[Path,to_visit]; %路径增加，记录蚂蚁走的路径比如[1,3,5,10]从序号1走到序号3再走到序号5 PLkm=PLkm+DD(W,to_visit); %路径长度增加。知道起始点W和下一步走的点就可以在DD中找到两点的距离 W=to_visit; %蚂蚁移到下一个节点，把W定义为初始点，蚂蚁移动后初始节点也就变化了 for kk=1:N if TABUkm(kk)==0 %禁忌表 DD(W,kk)=0; %DD是对称矩阵，禁忌表中0表示走过的或者不能走的栅格点，将走过的点在邻接矩阵中更新为0 DD(kk,W)=0; %根据禁忌表来更新邻接矩阵，因为蚂蚁走过一个点之后这个点不能走了吧，那么禁忌表中这个点就变了，那么下一步要走的点的邻接矩阵也变化了 end end TABUkm(W)=0; %已访问过的节点从禁忌表中删除（禁忌表大小应该是20*20） %到这里步骤5结束 %开始第10代第20只蚂蚁的下一步 DW=DD(W,:); DW1=find(DW); for j=1:length(DW1) if TABUkm(DW1(j))==0 DW(j)=0; end end LJD=find(DW); Len_LJD=length(LJD);%可选节点的个数 end %while循环结束，第2代第3只蚂蚁走完这条路径 %记下每一代每一只蚂蚁的觅食路线和路线长度 ROUTES{k,m}=Path; %ROUTES是cell结构 if Path(end)==E %判断蚂蚁是否走到了终点，前面while循环中还有陷入死胡同的可能 PL(k,m)=PLkm; %PL矩阵存储每一代每一只蚂蚁的路径长度 if PLkm<minkl %步骤7中的与当前已知最短路径长度作比较，最开始mink1定义为无穷大的量（到这里我明白了这么定义的意义了） mink=k;minl=m;minkl=PLkm; %mink存储蚂蚁代数，min1储存第几只蚂蚁，mink1储存最短路径长度 end else PL(k,m)=0; end end %到这里一代的所有蚂蚁循环结束 %上面下面两部分是步骤8 %更新信息素，一代的所有蚂蚁完成一次路径搜索后按照公式更新信息素,这是全局信息素更新 Delta_Tau=zeros(N,N); %更新量初始化，N=400 for m=1:M %M=50代表蚂蚁个数 if PL(k,m) %PL(k,m)路径长度非0时条件成立（这条语句意味着到达目标终点了） ROUT=ROUTES{k,m}; %这只蚂蚁走的路径给ROUT TS=length(ROUT)-1; %跳数，意思就是蚂蚁从起始点到目标点跳了多少步，比如[1,4,12,9,10]跳了4步 PL_km=PL(k,m); for s=1:TS x=ROUT(s); y=ROUT(s+1); Delta_Tau(x,y)=Delta_Tau(x,y)+Q/PL_km; %信息素更新公式Q/蚂蚁K走的路径总长度PL_km Delta_Tau(y,x)=Delta_Tau(y,x)+Q/PL_km; %上面的for循环，每一只蚂蚁走过路的都要更新信息素 end end end %经过一次迭代，所有蚂蚁走完一次，更新信息素 Tau=(1-Rho).*Tau+Delta_Tau;%信息素挥发一部分，新增加一部分 end %到这里所有代所有蚂蚁循环结束 %绘图 plotif=1;%是否绘图的控制参数 if plotif==1 %绘收敛曲线 minPL=zeros(K); %K=100 for i=1:K PLK=PL(i,:); %PL路径长度，PL是k*m的矩阵 Nonzero=find(PLK); %PLK大小是1*m PLKPLK=PLK(Nonzero); %PLKPLK储存第5代所有蚂蚁走的路径长度 minPL(i)=min(PLKPLK); %minPL中储存各代的最短路径长度，1*100 end figure(1) %创建一个图像显示窗口 plot(minPL); hold on %在当前图的轴（坐标系）中画了一幅图，再画另一幅图时，原来的图还在，与新图共存，都看得到 grid on %显示轴网格 title('收敛曲线变化趋势'); xlabel('迭代次数'); ylabel('最小路径长度'); %绘爬行图 figure(2) axis([0,MM,0,MM]) %左下角为原点 for i=1:MM for j=1:MM if G(i,j)==1 %左上角是原点，如果该点是障碍 x1=j-1;y1=MM-i; x2=j;y2=MM-i; x3=j;y3=MM-i+1; x4=j-1;y4=MM-i+1; fill([x1,x2,x3,x4],[y1,y2,y3,y4],[0.2,0.2,0.2]); %前两个是填充的范围，最后是填充的颜色 hold on else %该点是空白点 x1=j-1;y1=MM-i; x2=j;y2=MM-i; x3=j;y3=MM-i+1; x4=j-1;y4=MM-i+1; fill([x1,x2,x3,x4],[y1,y2,y3,y4],[1,1,1]); hold on end end end hold on title('机器人运动轨迹'); xlabel('坐标x'); ylabel('坐标y'); ROUT=ROUTES{mink,minl}; %ROUT存放的是路径的索引序号不是点的坐标 LENROUT=length(ROUT); Rx=ROUT; Ry=ROUT; for ii=1:LENROUT Rx(ii)=a*(mod(ROUT(ii),MM)-0.5); if Rx(ii)==-0.5 Rx(ii)=MM-0.5; end Ry(ii)=a*(MM+0.5-ceil(ROUT(ii)/MM)); end plot(Rx,Ry) end plotif2=0;%绘各代蚂蚁爬行图 if plotif2==1 figure(3) axis([0,MM,0,MM]) for i=1:MM for j=1:MM if G(i,j)==1 x1=j-1;y1=MM-i; x2=j;y2=MM-i; x3=j;y3=MM-i+1; x4=j-1;y4=MM-i+1; fill([x1,x2,x3,x4],[y1,y2,y3,y4],[0.2,0.2,0.2]); hold on else x1=j-1;y1=MM-i; x2=j;y2=MM-i; x3=j;y3=MM-i+1; x4=j-1;y4=MM-i+1; fill([x1,x2,x3,x4],[y1,y2,y3,y4],[1,1,1]); hold on end end end for k=1:K PLK=PL(k,:); minPLK=min(PLK); pos=find(PLK==minPLK); m=pos(1); ROUT=ROUTES{k,m}; LENROUT=length(ROUT); Rx=ROUT; Ry=ROUT; for ii=1:LENROUT Rx(ii)=a*(mod(ROUT(ii),MM)-0.5); if Rx(ii)==-0.5 Rx(ii)=MM-0.5; end Ry(ii)=a*(MM+0.5-ceil(ROUT(ii)/MM)); end plot(Rx,Ry) hold on end end %栅格地图转换成邻接矩阵的函数 function D=G2D(G) l=size(G,1); D=zeros(l*l,l*l); for i=1:l for j=1:l if G(i,j)==0 for m=1:l for n=1:l if G(m,n)==0 im=abs(i-m);jn=abs(j-n); if im+jn==1||(im==1&&jn==1) D((i-1)*l+j,(m-1)*l+n)=(im+jn)^0.5; end end end end end end end
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
- 85
- 86
- 87
- 88
- 89
- 90
- 91
- 92
- 93
- 94
- 95
- 96
- 97
- 98
- 99
- 100
- 101
- 102
- 103
- 104
- 105
- 106
- 107
- 108
- 109
- 110
- 111
- 112
- 113
- 114
- 115
- 116
- 117
- 118
- 119
- 120
- 121
- 122
- 123
- 124
- 125
- 126
- 127
- 128
- 129
- 130
- 131
- 132
- 133
- 134
- 135
- 136
- 137
- 138
- 139
- 140
- 141
- 142
- 143
- 144
- 145
- 146
- 147
- 148
- 149
- 150
- 151
- 152
- 153
- 154
- 155
- 156
- 157
- 158
- 159
- 160
- 161
- 162
- 163
- 164
- 165
- 166
- 167
- 168
- 169
- 170
- 171
- 172
- 173
- 174
- 175
- 176
- 177
- 178
- 179
- 180
- 181
- 182
- 183
- 184
- 185
- 186
- 187
- 188
- 189
- 190
- 191
- 192
- 193
- 194
- 195
- 196
- 197
- 198
- 199
- 200
- 201
- 202
- 203
- 204
- 205
- 206
- 207
- 208
- 209
- 210
- 211
- 212
- 213
- 214
- 215
- 216
- 217
- 218
- 219
- 220
- 221
- 222
- 223
- 224
- 225
- 226
- 227
- 228
- 229
- 230
- 231
- 232
- 233
- 234
- 235
- 236
- 237
- 238
- 239
- 240
- 241
- 242
- 243
- 244
- 245
- 246
- 247
- 248
- 249
- 250
- 251
- 252
- 253
- 254
- 255
- 256
- 257
- 258
- 259
- 260
- 261
- 262
- 263
- 264
- 265
- 266
- 267
- 268
- 269
- 270
- 271
- 272
- 273
- 274
- 275
- 276
- 277
- 278
- 279
- 280
- 281
- 282
- 283
- 284
- 285
- 286
- 287
- 288
- 289
- 290
- 291
- 292
- 293
- 294
- 295
- 296
- 297
- 298
- 299
- 300
- 301
- 302
- 303
- 304
- 305
- 306
- 307
- 308
- 309
- 310
求解结果如下图所示（G2D函数需要与求解程序分开保存），紧贴障碍物，未考虑机器人自身体积。
二、考虑机器人体积
考虑机器体积即不紧贴障碍物，绕开障碍物边界，求解如下：
clc;clear all; G=[0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 1 0 0 0 0 1 1 0 0 0 0 0; 0 0 0 0 0 0 1 1 0 0 0 0 1 0 0 1 0 0 0 0; 0 0 0 0 0 0 1 0 1 0 0 0 0 0 0 0 0 0 0 0; 0 1 1 1 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0; 0 1 1 1 0 0 0 1 1 0 0 0 0 0 0 1 0 0 0 0; 0 1 1 1 0 0 0 0 1 1 0 0 0 1 0 1 0 0 0 0; 0 1 1 0 0 0 0 0 0 0 0 0 1 1 0 1 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0; 0 0 0 0 0 0 0 1 1 0 0 0 1 1 0 0 0 0 0 0; 0 0 0 0 1 1 0 0 1 1 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 1 1 0 0 0 0 0 0 0 1 0 0 1 1 1 0; 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 0; 0 0 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0; 0 0 1 1 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;]; MM=size(G,1); % G 地形图为01矩阵，如果为1表示障碍物 ，size()返回矩阵G的行数，MM=20。如果size中的1改为2，则返回矩阵G的列数 Tau=ones(MM*MM,MM*MM); % Tau 初始信息素矩阵，规模为400*400,。记录每个栅格到其他栅格的信息素量。ones产生一个全是1的矩阵 Tau=8.*Tau; %矩阵全为8 K=100; %迭代次数（指蚂蚁出动多少波） M=50; %蚂蚁个数 S=1; %最短路径的起始点，第一个栅格点为起始地，这里用1-400的序号代表点的位置，默认左上角是第一个 E=400; %最短路径的目的点 Alpha=1; % Alpha 表征信息素重要程度的参数 Beta=8; % Beta 表征启发式因子重要程度的参数 Rho=0.4; % Rho 信息素蒸发系数 Q=1; % Q 信息素增加强度系数 minkl=inf; %inf代表无穷大量 mink=1; minl=1; Dt=G2D(G); %G2D栅格矩阵转换成邻接矩阵的函数，D用于存储每个点到各自相邻无障碍栅格点的代价值（该点到其他点的可行性） Dtt=ones(440,440); for si=1:400; sj =1:400; Dtt(si+20,sj+20)=Dt(si,sj); end for ki=1:400; if sum(Dt(ki,:))==0; Dtt(ki+19,ki)=0; Dtt(ki+19,ki+40)=0; Dtt(ki+21,ki)=0; Dtt(ki+21,ki+40)=0; Dtt(ki,ki+21)=0; Dtt(ki,ki+19)=0; Dtt(ki+40,ki+21)=0; Dtt(ki+40,ki+19)=0; end end for gi=1:400; gj=1:400; D(gi,gj)=Dtt(gi+20,gj+20); %若这两个节点在栅格地图中是自由栅格且是相邻或者对角的，则两点的距离则为邻接矩阵D的元素值，否则D的元素值为0 end N=size(D,1); %N表示问题的规模（像素个数），N=400 a=1; %小方格像素的边长 Ex=a*(mod(E,MM)-0.5); %终止点横坐标，这一行E=-0.5。这是栅格地图的直角坐标表示法和序号表示法的映射关系。 if Ex==-0.5 Ex=MM-0.5; %这一行Ex=19.5 end Ey=a*(MM+0.5-ceil(E/MM)); %终止点纵坐标，栅格地图的直角坐标表示法和序号表示法的映射关系。 Eta=zeros(N); %启发式信息矩阵，取为至目标点的直线距离的倒数，N=400，Eta大小为400*400 %以下启发式信息矩阵 %下面的for循环就是得到每个栅格点的坐标，并构建启发式信息矩阵（每个栅格点到目标点的距离） for i=1:N ix=a*(mod(i,MM)-0.5); %mod求i除以MM的余数 if ix==-0.5 ix=MM-0.5; %得到每一个栅格点的横坐标，一共400个 end iy=a*(MM+0.5-ceil(i/MM)); %得到每一个栅格点的纵坐标，一共400个，这里ix，iy都是有400个元素的向量 if i~=E %~=不等号，E代表目标点这里是400 Eta(i)=1/((ix-Ex)^2+(iy-Ey)^2)^0.5;%计算该点到目标的距离长度，并输入到启发式信息矩阵Eta，一共400个 else Eta(i)=100; end end %到这里，步骤2完成 ROUTES=cell(K,M); %用细胞结构存储每一代的每一只蚂蚁的爬行路线。K迭代次数，M蚂蚁个数。cell类型每个单元可以储存任何数据 %把ROUTES初始化为一个K行M列的空cell类型数据 %比如ROUTES{1,1}=rand(5)，就是ROUTES的1行1列的单元中存储的就是一个随机的5×5的方阵 PL=zeros(K,M); %用矩阵存储每一代的每一只蚂蚁的爬行路线长度 %启动K轮蚂蚁觅食活动，每轮派出M只蚂蚁 for k=1:K for m=1:M %状态初始化 W=S; %当前节点初始化为起始点，S=1是最短路径起始点，W用于表示当前所在节点 Path=S; %爬行路线初始化 PLkm=0; %爬行路线长度初始化 TABUkm=ones(N); %禁忌表初始化，N=400 TABUkm(S)=0; %已经在初始点了，因此要排除 DD=D; %邻接矩阵初始化，DD是400*400大小 %下一步可以前往的节点，蚂蚁走出第一步 DW=DD(W,:); %取行操作，取矩阵DD第W行的全部元素，矩阵DD的第一行代表第一个栅格点的邻接矩阵（也就是第一个栅格点能走的下一个点存储在第一行） %第二行就是存储第二个栅格点到其他栅格点的可行性，DW大小是400*1 DW1=find(DW); %find函数，返回向量DW中非0元素的位置（就是非0元素是第几个数） %DW1就得到了所以可以去的栅格点的序号，这样就筛选出当前可选择的点 for j=1:length(DW1) if TABUkm(DW1(j))==0 %将可选择的点与禁忌表对比，如果这个点在禁忌表在是0，则这个点不能走 DW(DW1(j))=0; %更新这一行的邻接矩阵（蚂蚁当前所在点与其他所有点的邻接矩阵为W这一行），经过和禁忌表对比，如果不能走则更新为0 end end %到这里步骤3结束 LJD=find(DW); %找到经过与禁忌表对比以后还能选择的栅格点有哪些 Len_LJD=length(LJD);%可选节点的个数 %蚂蚁未遇到食物或者陷入死胡同（可选栅格点Len_LJD<1 ）或者觅食停止 while W~=E&&Len_LJD>=1 %当前所在点不是目标点并且可选择节点数大于等于1，则进行下一步 %转轮赌法选择下一步怎么走 PP=zeros(Len_LJD); %得到可选节点数*可选节点数的0矩阵 for i=1:Len_LJD PP(i)=(Tau(W,LJD(i))^Alpha)*((Eta(LJD(i)))^Beta); %概率选择公式的分子，PP（i）给矩阵第i个中的元素赋值 %Tau是信息素矩阵，W就是当前点，LJD（i）代表下一栅格点的位置（概率公式中的下标j）。Eta启发式信息矩阵 end sumpp=sum(PP); %求概率公式的分母，将所有可访问栅格点的求和 PP=PP/sumpp; %建立概率分布，完整的概率公式 Pcum(1)=PP(1); for i=2:Len_LJD Pcum(i)=Pcum(i-1)+PP(i); %这个循环求所有路径概率的累加和 end Select=find(Pcum>=rand); %Pcum是路径概率的累加和，来模仿各条路径对应轮盘扇面大小。rand是[0,1]之间的随机数 %find(Pcum>=rand)返回所有大于 rand 的扇面 to_visit=LJD(Select(1)); %选择第一个大于 rand 的扇面 %到这里步骤4结束 %状态更新和记录 Path=[Path,to_visit]; %路径增加，记录蚂蚁走的路径比如[1,3,5,10]从序号1走到序号3再走到序号5 PLkm=PLkm+DD(W,to_visit); %路径长度增加。知道起始点W和下一步走的点就可以在DD中找到两点的距离 W=to_visit; %蚂蚁移到下一个节点，把W定义为初始点，蚂蚁移动后初始节点也就变化了 for kk=1:N if TABUkm(kk)==0 %禁忌表 DD(W,kk)=0; %DD是对称矩阵，禁忌表中0表示走过的或者不能走的栅格点，将走过的点在邻接矩阵中更新为0 DD(kk,W)=0; %根据禁忌表来更新邻接矩阵，因为蚂蚁走过一个点之后这个点不能走了吧，那么禁忌表中这个点就变了，那么下一步要走的点的邻接矩阵也变化了 end end TABUkm(W)=0; %已访问过的节点从禁忌表中删除（禁忌表大小应该是20*20） %到这里步骤5结束 %开始第10代第20只蚂蚁的下一步 DW=DD(W,:); DW1=find(DW); for j=1:length(DW1) if TABUkm(DW1(j))==0 DW(j)=0; end end LJD=find(DW); Len_LJD=length(LJD);%可选节点的个数 end %while循环结束，第2代第3只蚂蚁走完这条路径 %记下每一代每一只蚂蚁的觅食路线和路线长度 ROUTES{k,m}=Path; %ROUTES是cell结构 if Path(end)==E %判断蚂蚁是否走到了终点，前面while循环中还有陷入死胡同的可能 PL(k,m)=PLkm; %PL矩阵存储每一代每一只蚂蚁的路径长度 if PLkm<minkl %步骤7中的与当前已知最短路径长度作比较，最开始mink1定义为无穷大的量（到这里我明白了这么定义的意义了） mink=k;minl=m;minkl=PLkm; %mink存储蚂蚁代数，min1储存第几只蚂蚁，mink1储存最短路径长度 end else PL(k,m)=0; end end %到这里一代的所有蚂蚁循环结束 %上面下面两部分是步骤8 %更新信息素，一代的所有蚂蚁完成一次路径搜索后按照公式更新信息素,这是全局信息素更新 Delta_Tau=zeros(N,N); %更新量初始化，N=400 for m=1:M %M=50代表蚂蚁个数 if PL(k,m) %PL(k,m)路径长度非0时条件成立（这条语句意味着到达目标终点了） ROUT=ROUTES{k,m}; %这只蚂蚁走的路径给ROUT TS=length(ROUT)-1; %跳数，意思就是蚂蚁从起始点到目标点跳了多少步，比如[1,4,12,9,10]跳了4步 PL_km=PL(k,m); for s=1:TS x=ROUT(s); y=ROUT(s+1); Delta_Tau(x,y)=Delta_Tau(x,y)+Q/PL_km; %信息素更新公式Q/蚂蚁K走的路径总长度PL_km Delta_Tau(y,x)=Delta_Tau(y,x)+Q/PL_km; %上面的for循环，每一只蚂蚁走过路的都要更新信息素 end end end %经过一次迭代，所有蚂蚁走完一次，更新信息素 Tau=(1-Rho).*Tau+Delta_Tau;%信息素挥发一部分，新增加一部分 end %到这里所有代所有蚂蚁循环结束 %绘图 plotif=1;%是否绘图的控制参数 if plotif==1 %绘收敛曲线 minPL=zeros(K); %K=100 for i=1:K PLK=PL(i,:); %PL路径长度，PL是k*m的矩阵 Nonzero=find(PLK); %PLK大小是1*m PLKPLK=PLK(Nonzero); %PLKPLK储存第5代所有蚂蚁走的路径长度 minPL(i)=min(PLKPLK); %minPL中储存各代的最短路径长度，1*100 end figure(1) %创建一个图像显示窗口 plot(minPL); hold on %在当前图的轴（坐标系）中画了一幅图，再画另一幅图时，原来的图还在，与新图共存，都看得到 grid on %显示轴网格 title('收敛曲线变化趋势'); xlabel('迭代次数'); ylabel('最小路径长度'); %绘爬行图 figure(2) axis([0,MM,0,MM]) %左下角为原点 for i=1:MM for j=1:MM if G(i,j)==1 %左上角是原点，如果该点是障碍 x1=j-1;y1=MM-i; x2=j;y2=MM-i; x3=j;y3=MM-i+1; x4=j-1;y4=MM-i+1; fill([x1,x2,x3,x4],[y1,y2,y3,y4],[0.2,0.2,0.2]); %前两个是填充的范围，最后是填充的颜色 hold on else %该点是空白点 x1=j-1;y1=MM-i; x2=j;y2=MM-i; x3=j;y3=MM-i+1; x4=j-1;y4=MM-i+1; fill([x1,x2,x3,x4],[y1,y2,y3,y4],[1,1,1]); hold on end end end hold on title('机器人运动轨迹'); xlabel('坐标x'); ylabel('坐标y'); ROUT=ROUTES{mink,minl}; %ROUT存放的是路径的索引序号不是点的坐标 LENROUT=length(ROUT); Rx=ROUT; Ry=ROUT; for ii=1:LENROUT Rx(ii)=a*(mod(ROUT(ii),MM)-0.5); if Rx(ii)==-0.5 Rx(ii)=MM-0.5; end Ry(ii)=a*(MM+0.5-ceil(ROUT(ii)/MM)); end plot(Rx,Ry) end plotif2=0;%绘各代蚂蚁爬行图 if plotif2==1 figure(3) axis([0,MM,0,MM]) for i=1:MM for j=1:MM if G(i,j)==1 x1=j-1;y1=MM-i; x2=j;y2=MM-i; x3=j;y3=MM-i+1; x4=j-1;y4=MM-i+1; fill([x1,x2,x3,x4],[y1,y2,y3,y4],[0.2,0.2,0.2]); hold on else x1=j-1;y1=MM-i; x2=j;y2=MM-i; x3=j;y3=MM-i+1; x4=j-1;y4=MM-i+1; fill([x1,x2,x3,x4],[y1,y2,y3,y4],[1,1,1]); hold on end end end for k=1:K PLK=PL(k,:); minPLK=min(PLK); pos=find(PLK==minPLK); m=pos(1); ROUT=ROUTES{k,m}; LENROUT=length(ROUT); Rx=ROUT; Ry=ROUT; for ii=1:LENROUT Rx(ii)=a*(mod(ROUT(ii),MM)-0.5); if Rx(ii)==-0.5 Rx(ii)=MM-0.5; end Ry(ii)=a*(MM+0.5-ceil(ROUT(ii)/MM)); end plot(Rx,Ry) hold on end end %栅格地图转换成邻接矩阵的函数 function D=G2D(G) l=size(G,1); D=zeros(l*l,l*l); for i=1:l for j=1:l if G(i,j)==0 for m=1:l for n=1:l if G(m,n)==0 im=abs(i-m);jn=abs(j-n); if im+jn==1||(im==1&&jn==1) D((i-1)*l+j,(m-1)*l+n)=(im+jn)^0.5; end end end end end end end
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
- 85
- 86
- 87
- 88
- 89
- 90
- 91
- 92
- 93
- 94
- 95
- 96
- 97
- 98
- 99
- 100
- 101
- 102
- 103
- 104
- 105
- 106
- 107
- 108
- 109
- 110
- 111
- 112
- 113
- 114
- 115
- 116
- 117
- 118
- 119
- 120
- 121
- 122
- 123
- 124
- 125
- 126
- 127
- 128
- 129
- 130
- 131
- 132
- 133
- 134
- 135
- 136
- 137
- 138
- 139
- 140
- 141
- 142
- 143
- 144
- 145
- 146
- 147
- 148
- 149
- 150
- 151
- 152
- 153
- 154
- 155
- 156
- 157
- 158
- 159
- 160
- 161
- 162
- 163
- 164
- 165
- 166
- 167
- 168
- 169
- 170
- 171
- 172
- 173
- 174
- 175
- 176
- 177
- 178
- 179
- 180
- 181
- 182
- 183
- 184
- 185
- 186
- 187
- 188
- 189
- 190
- 191
- 192
- 193
- 194
- 195
- 196
- 197
- 198
- 199
- 200
- 201
- 202
- 203
- 204
- 205
- 206
- 207
- 208
- 209
- 210
- 211
- 212
- 213
- 214
- 215
- 216
- 217
- 218
- 219
- 220
- 221
- 222
- 223
- 224
- 225
- 226
- 227
- 228
- 229
- 230
- 231
- 232
- 233
- 234
- 235
- 236
- 237
- 238
- 239
- 240
- 241
- 242
- 243
- 244
- 245
- 246
- 247
- 248
- 249
- 250
- 251
- 252
- 253
- 254
- 255
- 256
- 257
- 258
- 259
- 260
- 261
- 262
- 263
- 264
- 265
- 266
- 267
- 268
- 269
- 270
- 271
- 272
- 273
- 274
- 275
- 276
- 277
- 278
- 279
- 280
- 281
- 282
- 283
- 284
- 285
- 286
- 287
- 288
- 289
- 290
- 291
- 292
- 293
- 294
- 295
- 296
- 297
- 298
- 299
- 300
- 301
- 302
- 303
- 304
- 305
- 306
- 307
- 308
- 309
- 310
- 311
- 312
- 313
- 314
- 315
- 316
- 317
- 318
- 319
- 320
- 321
- 322
- 323
- 324
- 325
- 326
- 327
- 328
求解效果如下图：
声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/weixin_40725706/article/detail/470976
推荐阅读
相关标签
