%% 该函数用于演示基于蚁群算法的三维路径规划算法 %% 清空环境 clc clear %% 数据初始化 HeightData = [2000 1400 800 650 500 750 1000 950 900 800 700 900 1100 1050 1000 1150 1300 1250 1200 1350 1500; 1100 900 700 625 550 825 1100 1150 1200 925 650 750 850 950 1050 1175 1300 1350 1400 1425 1450; 200 400 600 600 600 900 1200 1350 1500 1050 600 600 600 850 1100 1200 1300 1450 1600 1500 1400; 450 500 550 575 600 725 850 875 900 750 600 600 600 725 850 900 950 1150 1350 1400 1450; 700 600 500 550 600 550 500 400 300 450 600 600 600 600 600 600 600 850 1100 1300 1500; 500 525 550 575 600 575 550 450 350 475 600 650 700 650 600 600 600 725 850 1150 1450; 300 450 600 600 600 600 600 500 400 500 600 700 800 700 600 600 600 600 600 1000 1400; 550 525 500 550 600 875 1150 900 650 725 800 700 600 875 1150 1175 1200 975 750 875 1000; 800 600 400 500 600 1150 1700 1300 900 950 1000 700 400 1050 1700 1750 1800 1350 900 750 600; 650 600 550 625 700 1175 1650 1275 900 1100 1300 1275 1250 1475 1700 1525 1350 1200 1050 950 850; 500 600 700 750 800 1200 1600 1250 900 1250 1600 1850 2100 1900 1700 1300 900 1050 1200 1150 1100; 400 375 350 600 850 1200 1550 1250 950 1225 1500 1750 2000 1950 1900 1475 1050 975 900 1175 1450; 300 150 0 450 900 1200 1500 1250 1000 1200 1400 1650 1900 2000 2100 1650 1200 900 600 1200 1800; 600 575 550 750 950 1275 1600 1450 1300 1300 1300 1525 1750 1625 1500 1450 1400 1125 850 1200 1550; 900 1000 1100 1050 1000 1350 1700 1650 1600 1400 1200 1400 1600 1250 900 1250 1600 1350 1100 1200 1300; 750 850 950 900 850 1000 1150 1175 1200 1300 1400 1325 1250 1125 1000 1150 1300 1075 850 975 1100; 600 700 800 750 700 650 600 700 800 1200 1600 1250 900 1000 1100 1050 1000 800 600 750 900; 750 775 800 725 650 700 750 775 800 1000 1200 1025 850 975 1100 950 800 900 1000 1050 1100; 900 850 800 700 600 750 900 850 800 800 800 800 800 950 1100 850 600 1000 1400 1350 1300; 750 800 850 850 850 850 850 825 800 750 700 775 850 1000 1150 875 600 925 1250 1100 950; 600 750 900 1000 1100 950 800 800 800 700 600 750 900 1050 1200 900 600 850 1100 850 600]; %网格划分 LevelGrid=10; PortGrid=21; %起点终点网格点 starty=10;starth=4; endy=8;endh=5; m=10; %算法参数 PopNumber=100; %种群个数 BestFitness=[]; %最佳个体 %初始信息素 pheromone=ones(21,21,21); %% 初始搜索路径 [path,pheromone]=searchpath(PopNumber,LevelGrid,PortGrid,pheromone, HeightData,starty,starth,endy,endh); fitness=CacuFit(path); %适应度计算 [bestfitness,bestindex]=min(fitness); %最佳适应度 bestpath=path(bestindex,:); %最佳路径 BestFitness=[BestFitness;bestfitness]; %适应度值记录 %% 信息素更新 rou=0.2; cfit=100/bestfitness; for i=2:PortGrid-1 pheromone(i,bestpath(i*2-1),bestpath(i*2))= (1-rou)*pheromone(i,bestpath(i*2-1),bestpath(i*2))+rou*cfit; end %% 循环寻找最优路径 for kk=1:200 %这里改迭代次数 kk %迭代次数 %% 路径搜索 [path,pheromone]=searchpath(PopNumber,LevelGrid,PortGrid,pheromone,HeightData,starty,starth,endy,endh); %% 适应度值计算更新 fitness=CacuFit(path); [newbestfitness,newbestindex]=min(fitness); if newbestfitness<bestfitness bestfitness=newbestfitness; bestpath=path(newbestindex,:); end BestFitness=[BestFitness;bestfitness]; %% 更新信息素 cfit=100/bestfitness; for i=2:PortGrid-1 pheromone(i,bestpath(i*2-1),bestpath(i*2))=(1-rou)* pheromone(i,bestpath(i*2-1),bestpath(i*2))+rou*cfit; end end %% 最佳路径 for i=1:21 a(i,1)=bestpath(i*2-1); a(i,2)=bestpath(i*2); end figure(1) x=1:21; y=1:21; [x1,y1]=meshgrid(x,y); mesh(x1,y1,HeightData) axis([1,21,1,21,0,2000]) hold on k=1:21; plot3(k(1)',a(1,1)',a(1,2)'*200,'-o','LineWidth',2,'MarkerEdgeColor','k','MarkerFaceColor','r','MarkerSize',7) plot3(k(21)',a(21,1)',a(21,2)'*200,'-o','LineWidth',2,'MarkerEdgeColor','k','MarkerFaceColor','r','MarkerSize',7) text(k(1)',a(1,1)',a(1,2)'*200,' 起点'); text(k(21)',a(21,1)',a(21,2)'*200,' 终点'); xlabel('km','fontsize',12); ylabel('km','fontsize',12); zlabel('m','fontsize',12); title('三维路径规划空间','fontsize',12) set(gcf, 'Renderer', 'ZBuffer') hold on plot3(k',a(:,1)',a(:,2)'*200,'r-o') %% 适应度变化 figure(2) plot(BestFitness) title('最佳个体适应度变化趋势') xlabel('迭代次数') ylabel('适应度值')
function [path,pheromone]=searchpath(PopNumber,LevelGrid,PortGrid,pheromone,HeightData,starty,starth,endy,endh) %% 该函数用于蚂蚁蚁群算法的路径规划 %LevelGrid input 横向划分格数 %PortGrid input 纵向划分个数 %pheromone input 信息素 %HeightData input 地图高度 %starty starth input 开始点 %path output 规划路径 %pheromone output 信息素 %% 搜索参数 ycMax=2; %蚂蚁横向最大变动 hcMax=2; %蚂蚁纵向最大变动 decr=0.9; %信息素衰减概率 %% 循环搜索路径 for ii=1:PopNumber path(ii,1:2)=[starty,starth]; %记录路径 NowPoint=[starty,starth]; %当前坐标点 %% 计算点适应度值 for abscissa=2:PortGrid-1 %计算所有数据点对应的适应度值 kk=1; for i=-ycMax:ycMax for j=-hcMax:hcMax NextPoint(kk,:)=[NowPoint(1)+i,NowPoint(2)+j]; if (NextPoint(kk,1)<20)&&(NextPoint(kk,1)>0)&&(NextPoint(kk,2)<20)&&(NextPoint(kk,2)>0) qfz(kk)=CacuQfz(NextPoint(kk,1),NextPoint(kk,2),NowPoint(1),NowPoint(2),endy,endh,abscissa,HeightData); qz(kk)=qfz(kk)*pheromone(abscissa,NextPoint(kk,1),NextPoint(kk,2)); kk=kk+1; else qz(kk)=0; kk=kk+1; end end end %选择下个点 sumq=qz./sum(qz); pick=rand; while pick==0 pick=rand; end for i=1:25 pick=pick-sumq(i); if pick<=0 index=i; break; end end oldpoint=NextPoint(index,:); %更新信息素 pheromone(abscissa+1,oldpoint(1),oldpoint(2))=0.5*pheromone(abscissa+1,oldpoint(1),oldpoint(2)); %路径保存 path(ii,abscissa*2-1:abscissa*2)=[oldpoint(1),oldpoint(2)]; NowPoint=oldpoint; end path(ii,41:42)=[endy,endh]; end
function qfz=CacuQfz(Nexty,Nexth,Nowy,Nowh,endy,endh,abscissa,HeightData) %% 该函数用于计算各点的启发值 %Nexty Nexth input 下个点坐标 %Nowy Nowh input 当前点坐标 %endy endh input 终点坐标 %abscissa input 横坐标 %HeightData input 地图高度 %qfz output 启发值 %% 判断下个点是否可达 if HeightData(Nexty,abscissa)<Nexth*200 S=1; else S=0; end %% 计算启发值 %D距离 D=50/(sqrt(1+(Nowh*0.2-Nexth*0.2)^2+(Nexty-Nowy)^2)+sqrt((21-abscissa)^2 ... +(endh*0.2-Nexth*0.2)^2+(endy-Nowy)^2)); %计算高度 M=30/abs(Nexth+1); %计算启发值 qfz=S*M*D;
function fitness=CacuFit(path) %% 该函数用于计算个体适应度值 %path input 路径 %fitness input 路径 [n,m]=size(path); for i=1:n fitness(i)=0; for j=2:m/2 %适应度值为长度加高度 fitness(i)=fitness(i)+sqrt(1+(path(i,j*2-1)-path(i,(j-1)*2-1))^2 +(path(i,j*2)-path(i,(j-1)*2))^2)+abs(path(i,j*2)); end end
改进
clc clear h=[1800 2200 1900 2400 2300 2100 2500 2400 2700 2600 2900 1600 2000 2000 2600 2900 2000 2000 2500 2700 3000 2800 2100 1900 2000 1900 1700 2000 2000 2000 2000 2500 2900 1700 2000 2000 2000 1800 2000 2200 2000 2000 2000 2800 2200 1800 2000 3100 2300 2400 1800 3100 3200 2300 2000 1900 2100 2200 3000 2300 3000 3500 3100 2300 2600 2500 1700 1400 2300 2900 2400 2800 1800 3500 2600 2000 3200 2300 2500 2400 3100 3000 2600 3000 2300 3000 2500 2700 2000 2200 2100 2000 2200 3000 2300 2500 2400 2000 2300 2300 2200 2000 2300 2200 2200 2200 2500 2000 2800 2700 2000 2300 2500 2200 2200 2000 2300 2600 2000 2500 2000]; h=h-1400; [n,m]=size(h); for i=3:n+2 for j=3:n+2 H(i,j)=h(i-2,j-2); end end H(3:m+2,2)=(290*H(3:m+2,3)-366*H(3:m+2,4)+198*H(3:m+2,5)-38*H(3:m+2,6))/84; H(3:m+2,1)=(7211*H(3:m+2,3)-12813*H(3:m+2,4)+8403*H(3:m+2,5)-1919*H(3:m+2,6))/882; H(3:m+2,n+3)=-(21*H(3:m+2,n-1)-101*H(3:m+2,n)+177*H(3:m+2,n+1)-135*H(3:m+2,n+2))/38; H(3:m+2,n+4)=-(2079*H(3:m+2,n-1)-8403*H(3:m+2,n)+12013*H(3:m+2,n+1)-6411*H(3:m+2,n+2))/722; H(2,:)=(290*H(3,:)-366*H(4,:)+198*H(5,:)-38*H(6,:))/84; H(1,:)=(7211*H(3,:)-12813*H(4,:)+8403*H(5,:)-1919*H(6,:))/882; H(n+3,:)=-(21*H(n-1,:)-101*H(n,:)+177*H(n+1,:)-135*H(n+2,:))/38; H(n+4,:)=-(2079*H(n-1,:)-8403*H(n,:)+12013*H(n+1,:)-6411*H(n+2,:))/722; %二维四次卷积插值 [n,m]=size(h); D=[-21 59 -32 -48 61 -19 63 -261 386 -222 15 19 -63 366 -600 354 -57 0 21 -164 6 156 -19 0 0 0 240 0 0 0]; for i=1:10*(n-1) for j=1:10*(m-1) indexi=floor(i/10)+3; indexj=floor(j/10)+3; s=mod(i,10)*0.1; if j==100 indexj=indexj-1; end if i==100 indexi=indexi-1; end % if s==0 % indexi=indexi-1; % end t=mod(j,10)*0.1; % if t==0 % indexj=indexj-1; % end S=[s^4,s^3,s^2,s 1]; T=[t^4,t^3,t^2,t,1]; C=[H(indexi-2,indexj-2) H(indexi-2,indexj-1) H(indexi-2,indexj) H(indexi-2,indexj+1) H(indexi-2,indexj+2) H(indexi-2,indexj+3) H(indexi-1,indexj-2) H(indexi-1,indexj-1) H(indexi-1,indexj) H(indexi-1,indexj+1) H(indexi-1,indexj+2) H(indexi-1,indexj+3) H(indexi ,indexj-2) H(indexi ,indexj-1) H(indexi ,indexj) H(indexi ,indexj+1) H(indexi ,indexj+2) H(indexi ,indexj+3) H(indexi+1,indexj-2) H(indexi+1,indexj-1) H(indexi+1,indexj) H(indexi+1,indexj+1) H(indexi+1,indexj+2) H(indexi+1,indexj+3) H(indexi+2,indexj-2) H(indexi+2,indexj-1) H(indexi+2,indexj) H(indexi+2,indexj+1) H(indexi+2,indexj+2) H(indexi+2,indexj+3) H(indexi+3,indexj-2) H(indexi+3,indexj-1) H(indexi+3,indexj) H(indexi+3,indexj+1) H(indexi+3,indexj+2) H(indexi+3,indexj+3)]; HH(i,j)=S*D*C*D'*T'/57600; end end [n,m]=size(HH); x=1:n; y=1:m [xx,yy]=meshgrid(x,y); mesh(xx,yy,HH) xlabel('km') ylabel('km') zlabel('m') a =[ 50 600 65 600 50 600 40 600 30 800 30 600 35 800 35 600 35 800 25 800 30 1200 15 1200 20 1000 15 1000 30 1000 35 1200 35 1000 40 1000 30 1400 35 1600 40 1000]; b=[0 5 10 15 20 25 30 35 40 45 50 55 60 65 70 75 80 85 90 95 100]; hold on plot3(b',a(:,1)-2,a(:,2)+300,'r-o','linewidth',1)
