【路径规划】人工蜂群之无人机三维路径规划【Matlab 017期】

一、简介

自然界中的蜜蜂总能在任何环境下以极高的效率找到优质蜜源，且能适应环境的改变。蜜蜂群的采蜜系统由蜜源、雇佣蜂、非雇佣蜂三部分组成，其中一个蜜源的优劣有很多要素，如蜜源花蜜量的大小、离蜂巢距离的远近、提取的难易程度等；雇佣蜂和特定的蜜源联系并将蜜源信息以一定概率形式告诉同伴；非雇佣蜂的职责是寻找待开采的蜜源，分为跟随蜂和侦查蜂两类，跟随峰是在蜂巢等待而侦查蜂是探测蜂巢周围的新蜜源。蜜蜂采蜜时，蜂巢中的一部分蜜蜂作为侦查蜂，不断并随机地在蜂巢附近寻找蜜源，如果发现了花蜜量超过某个阈值的蜜源，则此侦査蜂变为雇佣蜂开始釆蜜，采蜜完成后飞回蜂巢跳摇摆舞告知跟随峰。摇摆舞是蜜蜂之间交流信息的一种基本形式，它传达了有关蜂巢周围蜜源的重要信息如蜜源方向及离巢距离等，跟随峰利用这些信息准确评价蜂巢周围的蜜源质量。当雇佣蜂跳完摇摆舞之后，就与蜂巢中的一些跟随蜂一起返回原蜜源采蜜，跟随蜂数量取决于蜜源质量。以这种方式，蜂群能快速且有效地找到花蜜量最高的蜜源。

蜜蜂采蜜的群体智能就是通过不同角色之间的交流转换及协作来实现的。具体采蜜过程如图所示。在最初阶段，蜜蜂是以侦查蜂的形式出现，且对蜂巢周闱的蜜源没有任何了解，由于蜜蜂内在动机和外在的条件不同侦查蜂有两种选择：①成为雇佣蜂，开始在蜂巢周围随机搜索蜜源，如图中路线②成为跟随峰，在观察完摇摆舞后开始搜索蜜源，如图中路线。假设发现两个蜜源和，在发现蜜源后，该侦查蜂变成一只雇佣蜂，雇佣蜂利用其自身属性记住蜜源的位置，并立刻投入到采蜜中。采蜜完成后蜜蜂带着满载花蜜返回蜂巢，将花蜜卸载到卸蜜房，卸载完成后雇佣蜂有三种可能的行为①放弃自己发现的花蜜量不高的蜜源，变成一个不受约束的非雇佣蜂，如图中的路线；②在招募区跳摇摆舞，招募一些待在蜂巢中跟随峰，带领其再次返回所发现的蜜源如图中的路线；③不招募其他蜜蜂，继续回到原来的蜜源采蜜如图中的路线。在现实生活中并不是所有的蜜蜂一开始就立刻采蜜，另外大多数蜜蜂在一次采蜜完成后都会选择回到招募区跳摇摆舞来招募更多的蜜蜂去采蜜。
算法模型
人工蜂群算法就是模拟蜜蜂的采蜜过程而提出的一种新型智能优化算法，它也是由食物源、雇佣蜂和非雇佣蜂三部分组成。
食物源：食物源即为蜜源。在任何一个优化问题中，问题的可行解都是以一定形式给出的。在人工蜂群算法中，食物源就是待求优化问题的可行解，是人工蜂群算法中所要处理的基本对象。食物源的优劣即可行解的好坏是用蜜源花蜜量的大小即适应度来评价的。
雇佣蜂：雇佣蜂即为引领蜂与食物源的位置相对应，一个食物源对应一个引领蜂。在人工蜂群算法中，食物源的个数与引领蜂的个数相等；引领蜂的任务是发现食物源信息并以一定的概率与跟随蜂分享；概率的计算即为人工蜂群算法中的选择策略，一般是根据适应度值以轮盘赌的方法计算。
非雇佣蜂：非雇佣蜂包括跟随蜂和侦査蜂跟随蜂在蜂巢的招募区内根据引领蜂提供的蜜源信息来选择食物源，而侦查蜂是在蜂巢附近寻找新的食物源。在人工蜂群算法中，跟随蜂依据引领蜂传递的信息，在食物源附近搜索新食物源，并进行贪婪选择。若一个食物源在经过次后仍未被更新，则此引领蜂变成侦査蜂，侦查蜂寻找新的食物源代替原来的食物源。

算法搜索过程

领蜂、跟随蜂和侦查蜂三类，每一次搜索过程中，引领蜂和跟随蜂是先后开采食物源，即寻找最优解，而侦查蜂是观察是否陷入局部最优，若陷入局部最优则随机地搜索其它可能的食物源。每个食物源代表问题一个可能解，食物源的花蜜量对应相应解的质量（适应度值fiti）。
一、人工蜂群算法搜索过程中，首先需要初始化，其中包括确定种群数、最大迭代次数MCN、、控制参数limit和确定搜索空间即解的范围，在搜索空间中随机生成初始解xi(i=1,2,3,……,SN)，SN为食物源个数，每个解xi是一个D维的向量，D是问题的维数。初始化之后，整个种群将进行引领蜂、跟随蜂和侦查蜂搜寻过程的重复循环，直到达到最大迭代次数MCN或误差允许值 ε。
二、在搜索过程开始阶段，每个引领蜂由式(2-3)产生一个新解即新食物源，
vij=xij+Φij(xij-xkj) (2-3)

式中，k∈﹛1,2,…,SN﹜,j∈{1,2,…,D},且k ≠i;Φij为[-1,1]之间的随机数。计算新解的fiti并评价它，若新解的fiti优于旧解，则引领蜂记住新解忘记旧解。反之，它将保留旧解。
三、在所有引领蜂完成搜寻过程之后，引领蜂会在招募区跳摇摆舞把解的信息及信息与跟随蜂分享。跟随蜂根据式计算每个解的选择概率，
pi=fiti/∑k=1SNfitk。 (2-4)
然后在区间[-1,1]内随机产生一个数，如果解的概率值大于该随机数，则跟随蜂由式(2-3)产生一个新解，并检验新解的fiti，若新解的fiti比之前好，则跟随蜂将记住新解忘掉旧解；反之，它将保留旧解。
四、在所有跟随蜂完成搜寻过程之后，如果一个解经过limit次循环仍然没有被进一步更新，那么就认为此解陷入局部最优，该食物源就会被舍弃。设食物源xi被舍弃，则此食物源对应的引领蜂转成一个侦查蜂。侦察蜂由(2-5)式产生一个新的食物源代替它。
xij=xminj+rand(0,1)(xmaxj-xminj) (2-5)
其中j∈{1,2…,D}。然后返回引领蜂搜索过程，开始重复循环。
五、人工蜂群算法的食物源质量一般是越大越好，即适应度值越大越好，而对应于要优化的问题，需要分两种情况考虑：即最小值问题、最大值问题。设fi是优化问题的目标函数，所以若优化最小值问题时，适应度函数为fi的变形，一般用式(2-6)表示；若优化最大值问题，适应度函数即目标函数。

                                                        fiti={1+abs(fi)     fi>=01/1+fi               fi>0                                (2-6)

人工蜂群算法在评价食物源时一般进行贪婪选择按式(2-7)进行。
vi={xi fit(xi)<=fit(vi)vi fit(vi)>fit(xi) (2-7)
人工蜂群算法就是通过循环搜索，最终找到最优食物源或最优解。
算法步骤
人工蜂群算法具体实现步骤：
步骤1：初始化种群：初始化各个参数，蜂群总数SN、食物源被采集次数即最大迭代次数MCN及控制参数limit，确定问题搜索范围，并且在搜索范围内随机产生初始解xi(i=1,2,…SN) 。
步骤2：计算并评估每个初始解的适应度。
步骤3：设定循环条件并开始循环
步骤4：引领蜂对解xi按照式（2-3）进行邻域搜索产生新解（食物源）vi，并计算其适应度值；
步骤5：按照式(2-7)进行贪婪选择：如果vi的适应度值优于xi，则利用vi替换xi，将vi作为当前最好的解，否则保留xi不变；
步骤6：根据式(2-4)计算食物源的概率pi；
步骤7：跟随蜂依照概率pi选择解或食物源，按照式(2-3)搜索产生新解（食物源）vi，并计算其适应度。
步骤8：按式(2-7)进行贪婪选择；如果vi的适应度优于xi，则用vi代替xi，将vi作为当前最好解，否则保留xi不变；
步骤9：判断是否有要放弃的解。若有，则侦查蜂按式(2-5)随机产生新解将其替换；
步骤10：记录到目前为止的最优解；
步骤11：判断是否满足循环终止条件，若满足，循环结束，输出最优解，否则返回步骤4继续搜索。

二、源代码

clc
close all
clear all
%================================参数设置=============================================%
global boundary setstart setfinal node delta_H danger_xi danger_yi danger_zi danger_ri weight setstart_2 setfinal_2 ;
%node=12;%离终点最近的起始点到终点的节点个数(偶数)
delta_H=[20 40];%起飞(落地)高度、飞行过程最低高度
danger_xi=[207.1 393.9];%设置威胁区域坐标                
danger_yi=[333.3 414.1];
danger_zi=[389.9 349.2];
danger_ri=[30 50];%设置威胁半径
weight=[1 0.05 0.6];%路径长度、离地面高度、夹角所占权重
boundary=[500 0];%设置环境上下限
%setfinal_ALL=[444 459 483];
setstart=[15.15 30.3 295.9];
setfinal=[449.5 459.6 422]; 
%==============确定节点数=================%
L_ALL=sqrt(((setfinal-setstart).^2)*ones(3,1));
L_FEN=17;%设置节点间的间隔距离
node=floor(L_ALL/L_FEN);
if mod(node,2)==1
    node=node+1;
end;
%====================================================================================%       
%======================================地形绘图=======================================%
SETenvironment;
surf(X,Y,Z);
box on;
rotate3d on;
xi=linspace(0,500,100);
yi=linspace(0,500,100);
[XI,YI]=meshgrid(xi,yi);
ZI=interp2(X,Y,Z,XI,YI,'cubic');
surf(XI,YI,ZI) %光滑曲面+等高线  
hold on;
% for i=1:2
% 
%       h(i)=surf(X,Y,Z);
%       alpha(h(i),0.2*i)
%       Z=Z-100;
%     
% end
%=====================================威胁区域绘图====================================%
[x,y,z]=sphere(200);
for k=1:size(danger_xi,2)
 surf(danger_ri(k)*x+danger_xi(k),danger_ri(k)*y+danger_yi(k),danger_ri(k)*z+danger_zi(k));
 hold on;
end
%====================================================================================%
 
maxCycle=20;%算法迭代次数
%/* Problem specific variables*/
D=3; 
NP=40; 
FoodNumber=NP/2; 
limit=NP*D;
ub=ones(1,D)*(boundary(1)-10); 
lb=ones(1,D)*(boundary(2)+10);
setfinal_2=setfinal+[0 0 delta_H(1)];
setstart_2=setstart+[0 0 delta_H(1)];
runtime=node+1;
objfun='UAV2'; 
 
GlobalMins=zeros(1,runtime);
GlobalParams_s=[setstart_2;zeros(node+1,3);setfinal_2];
 
for r=1:2*runtime
Range = repmat((ub-lb),[FoodNumber 1]);
Lower = repmat(lb, [FoodNumber 1]);
Foods = rand(FoodNumber,D) .* Range + Lower;
 
%====================================================================================%
if r<=node/2%Step1:将奇数位置的节点双向规划（基于X坐标均分）
   X_ave=setstart_2(1):((setfinal_2(1)-setstart_2(1))/(node/2+1)):setfinal_2(1);
   X_ave=X_ave(2:end-1);
   if rem(r,2)~=0
     Foods(:,1)=X_ave((r+1)/2);
     ObjVal=feval(objfun,Foods,GlobalParams_s(r,:),GlobalParams_s(node+4-r,:));   
   else
     Foods(:,1)=X_ave(node/2+1-r/2);    
     ObjVal=feval(objfun,Foods,GlobalParams_s(r+1,:),GlobalParams_s(node+5-r,:));   
   end
else
    if r>node/2&&r<=runtime%Step2:将偶数位置的节点单向插缝规划（基于Step1所产生节点间的Y坐标中值）
   feckParams_s=[0;0;GlobalParams_s(1:(end-2),2)];
   feckParams_s=(GlobalParams_s(:,2)-feckParams_s)/2;
   feckParams_s=[feckParams_s(3:end);0;0];
   Y_ave=GlobalParams_s(:,2)+feckParams_s;
   Foods(:,2)=Y_ave(2*(r-node/2)-1);
   ObjVal=feval(objfun,Foods,GlobalParams_s(2*(r-node/2)-1,:),GlobalParams_s(2*(r-node/2)+1,:));   
    elseif r>runtime&&r<=runtime+node/2%Step3:重新将奇数位置的节点单向插缝规划（基于Step2所产生节点间的X坐标中值）
   feckParams_s=[0;GlobalParams_s(4:end,1);0;0];     
   feckParams_s=(feckParams_s-GlobalParams_s(:,1))/2;  
   X_ave=GlobalParams_s(:,1)+feckParams_s;
   Foods(:,1)=X_ave(2*(r-runtime));
   ObjVal=feval(objfun,Foods,GlobalParams_s(2*(r-runtime),:),GlobalParams_s(2*(r-runtime)+2,:));   
    else%Step4:重新将偶数位置的节点单向插缝规划（基于Step3所产生节点间的Y坐标中值）
   feckParams_s=[0;0;GlobalParams_s(1:(end-2),2)];
   feckParams_s=(GlobalParams_s(:,2)-feckParams_s)/2;
   feckParams_s=[feckParams_s(3:end);0;0];
   Y_ave=GlobalParams_s(:,2)+feckParams_s;
   Foods(:,2)=Y_ave(2*(r-node/2-runtime)-1);
   ObjVal=feval(objfun,Foods,GlobalParams_s(2*(r-node/2-runtime)-1,:),GlobalParams_s(2*(r-node/2-runtime)+1,:));            
    end    
end
%====================================================================================%
 
Fitness=calculateFitness(ObjVal);
%reset trial counters
trial=zeros(1,FoodNumber);
 
%/*The best food source is memorized*/
BestInd=find(ObjVal==min(ObjVal(find(ObjVal>0))));
BestInd=BestInd(end);
GlobalMin=ObjVal(BestInd);
GlobalParams=Foods(BestInd,:);
 
iter=1;
while (iter <= maxCycle),
%雇佣蜂先采一批蜜源回去供观察蜂观察
%%%%%%%%% EMPLOYED BEE PHASE %%%%%%%%%%%%%%%%%%%%%%%%
    for i=1:(FoodNumber)
        Param2Change=fix(rand*D)+1;
        neighbour=fix(rand*FoodNumber)+1;      
            while(neighbour==i)%即如果随机生成的整数与i相等则再生成一个，直到不相等为止
                neighbour=fix(rand*FoodNumber)+1;
            end;
        
       sol=Foods(i,:);
       %与第个蜜源相对应的采蜜蜂依据如下公式寻找新的蜜源
       sol(Param2Change)=Foods(i,Param2Change)+(Foods(i,Param2Change)-Foods(neighbour,Param2Change))*(rand-0.5)*2;%(rand-0.5)*2  即产生一个[-1:1]的随机数
     
        ind=find(sol<(lb));%如果超出边界，则将值转变为边界值
        sol(ind)=lb(ind);
        ind=find(sol>(ub));
        sol(ind)=ub(ind);
        
        %evaluate new solution
        if r<=node/2
            if rem(r,2)~=0
          ObjValSol=feval(objfun,sol,GlobalParams_s(r,:),GlobalParams_s(node+4-r,:)); 
            else
          ObjValSol=feval(objfun,sol,GlobalParams_s(r+1,:),GlobalParams_s(node+5-r,:)); 
            end
      else
        if r>node/2&&r<=runtime
        ObjValSol=feval(objfun,sol,GlobalParams_s(2*(r-node/2)-1,:),GlobalParams_s(2*(r-node/2)+1,:));   
        elseif r>runtime&&r<=runtime+node/2
        ObjValSol=feval(objfun,sol,GlobalParams_s(2*(r-runtime),:),GlobalParams_s(2*(r-runtime)+2,:));   
        else
        ObjValSol=feval(objfun,sol,GlobalParams_s(2*(r-node/2-runtime)-1,:),GlobalParams_s(2*(r-node/2-runtime)+1,:));   
        end
       end
       
        FitnessSol=calculateFitness(ObjValSol);
        
       % /*a greedy selection is applied between the current solution i and its mutant 
       if (FitnessSol>Fitness(i)) %/*If the mutant solution is better than the current solution i, replace the solution with the mutant and reset the trial counter of solution i*/
            Foods(i,:)=sol;
            Fitness(i)=FitnessSol;
            ObjVal(i)=ObjValSol;
            trial(i)=0;
        else
            trial(i)=trial(i)+1; %/*if the solution i can not be improved, increase its trial counter*/
       end;
     end;
 
%%%%%%%%%%%%%%%%%%%%%%%% CalculateProbabilities %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
prob=(0.9.*Fitness./max(Fitness))+0.1;%"."运算为矩阵运算
%prob=Fitness./sum(Fitness);
%%%%%%%%%%%%%%%%%%%%%%%% ONLOOKER BEE PHASE %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
i=1;
t=0;
while(t<FoodNumber)
    if(rand<prob(i))
        t=t+1;
        Param2Change=fix(rand*D)+1;%随机生成[1：D]上的一个整数
        neighbour=fix(rand*(FoodNumber))+1;
        %/*Randomly selected solution must be different from the solution i*/        
            while(neighbour==i)
                neighbour=fix(rand*(FoodNumber))+1;
            end;
        
       sol=Foods(i,:);
       sol(Param2Change)=Foods(i,Param2Change)+(Foods(i,Param2Change)-Foods(neighbour,Param2Change))*(rand-0.5)*2;
       %  /*if generated parameter value is out of boundaries, it is shifted onto the boundaries*/
        ind=find(sol<(lb));%如果超出边界，则将值转变为边界值
        sol(ind)=lb(ind);
        ind=find(sol>(ub));
        sol(ind)=ub(ind);
        
        %evaluate new solution
        if r<=node/2
            if rem(r,2)~=0
          ObjValSol=feval(objfun,sol,GlobalParams_s(r,:),GlobalParams_s(node+4-r,:)); 
            else
          ObjValSol=feval(objfun,sol,GlobalParams_s(r+1,:),GlobalParams_s(node+5-r,:)); 
            end
      else
        if r>node/2&&r<=runtime
        ObjValSol=feval(objfun,sol,GlobalParams_s(2*(r-node/2)-1,:),GlobalParams_s(2*(r-node/2)+1,:));   
        elseif r>runtime&&r<=runtime+node/2
        ObjValSol=feval(objfun,sol,GlobalParams_s(2*(r-runtime),:),GlobalParams_s(2*(r-runtime)+2,:));   
        else
        ObjValSol=feval(objfun,sol,GlobalParams_s(2*(r-node/2-runtime)-1,:),GlobalParams_s(2*(r-node/2-runtime)+1,:));   
        end
       end
        
        FitnessSol=calculateFitness(ObjValSol);
        
       % /*a greedy selection is applied between the current solution i and its mutant*/
       if (FitnessSol>Fitness(i)) %/*If the mutant solution is better than the current solution i, replace the solution with the mutant and reset the trial counter of solution i*/
            Foods(i,:)=sol;
            Fitness(i)=FitnessSol;
            ObjVal(i)=ObjValSol;
            trial(i)=0;
        else
            trial(i)=trial(i)+1; %/*if the solution i can not be improved, increase its trial counter*/
       end;
    end;
    
    i=i+1;
     if (i==(FoodNumber)+1)
         i=1;
     end;   
end; 
 
 
%/*The best food source is memorized*/
         ind=find(ObjVal==min(ObjVal(find(ObjVal>0))));
         ind=ind(end);
         if (ObjVal(ind)<GlobalMin)
         GlobalMin=ObjVal(ind);
         GlobalParams=Foods(ind,:);%储存最小函数值对应参数
         end;
         
         
%%%%%%%%%%%% SCOUT BEE PHASE %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%侦察蜂用于防止算法陷入局部最优
ind=find(trial==max(trial));
ind=ind(end);
if (trial(ind)>limit)
    Bas(ind)=0;
    
    sol=(ub-lb).*rand(1,D)+lb;%随机生成各参数值（即食物）
    
     if r<=node/2
            if rem(r,2)~=0
          ObjValSol=feval(objfun,sol,GlobalParams_s(r,:),GlobalParams_s(node+4-r,:)); 
            else
          ObjValSol=feval(objfun,sol,GlobalParams_s(r+1,:),GlobalParams_s(node+5-r,:)); 
            end
      else
        if r>node/2&&r<=runtime
        ObjValSol=feval(objfun,sol,GlobalParams_s(2*(r-node/2)-1,:),GlobalParams_s(2*(r-node/2)+1,:));   
        elseif r>runtime&&r<=runtime+node/2
        ObjValSol=feval(objfun,sol,GlobalParams_s(2*(r-runtime),:),GlobalParams_s(2*(r-runtime)+2,:));   
        else
        ObjValSol=feval(objfun,sol,GlobalParams_s(2*(r-node/2-runtime)-1,:),GlobalParams_s(2*(r-node/2-runtime)+1,:));   
        end
       end
     
    FitnessSol=calculateFitness(ObjValSol);%计算适宜度
    Foods(ind,:)=sol;%即超过‘limit’限制的食物被随机生成的食物代替
    Fitness(ind)=FitnessSol;
    ObjVal(ind)=ObjValSol;
end;
 
fprintf('代数=%d GlobalMin=%g\n',iter,GlobalMin);
iter=iter+1;
end ;% End of ABC
 
GlobalMins(r)=GlobalMin;
 
if r<=node/2
    if rem(r,2)~=0
    GlobalParams_s(r+2,:)=GlobalParams;
    else
    GlobalParams_s(node+3-r,:)=GlobalParams;        
    end
else
  if r>node/2&&r<=runtime
  GlobalParams_s(2*(r-node/2),:)=GlobalParams;
  elseif r>runtime&&r<=runtime+node/2
  GlobalParams_s(2*(r-runtime)+1,:)=GlobalParams;  
  else
  GlobalParams_s(2*(r-node/2-runtime),:)=GlobalParams;    
  end
end
 
 
end; %end of runs
%=====================================路径绘图========================================%
scatter3([setstart(1) setfinal(1)],[setstart(2) setfinal(2)],[setstart(3) setfinal(3)],'*','r');
% line([setfinal(1) GlobalParams_s(node+2,1) ],[setfinal(2) GlobalParams_s(node+2,2)],[setfinal(3) GlobalParams_s(node+2,3)],'Color','k','LineWidth',1);
% line([setstart(1) GlobalParams_s(1,1) ],[setstart(2) GlobalParams_s(1,2)],[setstart(3) GlobalParams_s(1,3)],'Color','k','LineWidth',1);
% scatter3(GlobalParams_s(:,1),GlobalParams_s(:,2),GlobalParams_s(:,3),'*','r');
% line(GlobalParams_s(:,1),GlobalParams_s(:,2),GlobalParams_s(:,3),'Color','k','LineWidth',1); 
%====================================================================================%
%hold on;
save all
CBI;%路径平滑绘图
%================================计算路径长度=========================================%
C=GlobalParams_s;
A=0;
for i=1:(size(C,1)-1)
A=A+sqrt(((C(i+1,:)-C(i,:)).^2)*ones(3,1));
end
A%输出规划航迹长度
min=sqrt(((setstart-setfinal).^2)*ones(3,1))%输出起始两点间的直线距离
 

三、运行结果

四、备注

完整代码或者代写添加QQ912100926
往期回顾>>>>>>
【路径规划】粒子群优化算法之三维无人机路径规划【Matlab 012期】
【路径规划】遗传算法之多物流中心的开放式车辆路径规划【Matlab 013期】
【路径规划】粒子群算法之机器人栅格路径规划【Matlab 014期】
【路径规划】蚁群算法之求解最短路径【Matlab 015期】
【路径规划】免疫算法之物流中心选址【Matlab 016期】