[MCM] 基于蚁群算法的三维路径规划

%% 该函数用于演示基于蚁群算法的三维路径规划算法

%% 清空环境
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)