路径规划 | 基于蚁群算法的三维无人机航迹规划（Matlab）

效果一览

基本介绍

基于蚁群算法的三维无人机航迹规划（Matlab）。

蚁群算法（Ant Colony Optimization，ACO）是一种模拟蚂蚁觅食行为的启发式算法。该算法通过模拟蚂蚁在寻找食物时的行为，来解决各种优化问题，尤其是在图论和组合优化方面应用较广。

程序设计

• 完整源码和数据私信博主回复基于蚁群算法的三维无人机航迹规划（Matlab）

clc
clear
close all

%% 输入数据
G=[ 0 0 1 1 1 0 0 0 0 1
    1 0 0 0 0 0 0 0 0 0
    0 0 0 0 0 0 0 0 0 0
    0 1 1 0 1 0 0 0 0 0
    0 1 1 0 1 0 0 0 1 0
    0 0 0 0 0 0 1 0 0 0
    0 0 0 1 0 0 0 0 0 1
    0 0 0 0 0 0 1 1 0 0
    0 1 0 0 0 0 1 0 0 0
    0 1 0 0 1 0 0 0 0 0];
% G=[ 0 1 1 1 0 
%     1 0 0 0 0 
%     0 0 0 0 1 
%     0 0 0 0 1 
%     0 1 1 0 1];


G = zeros(10,10);
d = randperm(95,21)+1;
d=sort(d);
G(d) = 1;

%% 栅格绘制
drawShanGe(G,0)
title('栅格地图')
%%
S = [1 1];    % 起点
E = [10 10];  % 终点
G0 = G;
G = G0(S(1):E(1),S(2):E(2)); % 该方式是为了方便更改起点与终点
[Xmax,dim] = size(G);        % 栅格地图列数为粒子维数，行数为粒子的变化范围
dim = dim - 2;               % 减2是去掉起点与终点

%% 参数设置
maxgen = 50;    % 最大迭代次数
NP = 30;         % 种群数量

%%%%%%%%%%%%%%%%%%%%%%%%%%%
 rPercent = 0.2;    

%%%%%%%%%%%%%%%%%%%%%%%%%%%
pNum = round( NP * rPercent );    % %发现者



Xmin = 1;   % 变量下界

%% 初始化
X = zeros(NP,dim);
for i = 1:NP
    for j = 1:dim
       col = G(:,j+1);      % 地图的一列
       id = find(col == 0); % 该列自由栅格的位置
       X(i,j) =  id(randi(length(id))); % 随机选择一个自由栅格
       id = [];
    end 
    fit( i ) = fitness(X( i, : ),G);
end
fpbest = fit;   % 个体最优适应度
pX = X;      % 个体最优位置
 XX=pX;
[fgbest, bestIndex] = min( fit );        % 全局最优适应度
bestX = X(bestIndex, : );    % 全局最优位置
[fmax,B]=max(fit);
worse= X(B,:);  
%%
for gen = 1 : maxgen
    gen

       [ ans, sortIndex ] = sort( fit );% Sort.
     
      [fmax,B]=max( fit );
       worse= X(B,:);  
        
        [~, Idx] = sort( fpbest );
  r2=rand(1);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for i = 1 : pNum
 if(r2<0.9)
            r1=rand(1);
          a=rand(1,1);
          if (a>0.1)
           a=1;
          else
           a=-1;
          end
    X( i , : ) =  pX(  i , :)+0.3*abs(pX(i , : )-worse)+a*0.1*(XX( i , :)); % Equation (1)
       else
            
           aaa= randperm(180,1);
           if ( aaa==0 ||aaa==90 ||aaa==180 )
            X(  i , : ) = pX(  i , :);   
           end
         theta= aaa*pi/180;   
       
       X(  i , : ) = pX(  i , :)+tan(theta).*abs(pX(i , : )-XX( i , :));    % Equation (2)      

      end
          X( i , :) = Bounds(X(i , : ), Xmin, Xmax );
         fit(  i  ) = fitness( X(  i , : ),G );
    end 
 [ fMMin, bestII ] = min( fit );      
  bestXX = X( bestII, : );  
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 R=1-gen/maxgen;                           %
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 Xnew1 = bestXX.*(1-R); 
     Xnew2 =bestXX.*(1+R);                    %%% Equation (3)
   Xnew1= Bounds(Xnew1, Xmin, Xmax );
   Xnew2 = Bounds(Xnew2, Xmin, Xmax );
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
     Xnew11 = bestX.*(1-R); 
     Xnew22 =bestX.*(1+R);                     %%% Equation (5)
   Xnew11= Bounds(Xnew11, Xmin, Xmax );
   Xnew22 = Bounds(Xnew22, Xmin, Xmax );
   
   
     for i = ( pNum + 1 ) :12                  % Equation (4)
     X( i, : )=bestXX+((rand(1,dim)).*(pX( i , : )-Xnew1)+(rand(1,dim)).*(pX( i , : )-Xnew2));
    X( i , :) = Bounds(X(i , : ),  min(Xnew1), max(Xnew2) );
   fit(  i  ) = fitness( X(  i , : ),G );
   end
   
  for i = 13: 19                  % Equation (6)

   
        X( i, : )=pX( i , : )+((randn(1)).*(pX( i , : )-Xnew11)+((rand(1,dim)).*(pX( i , : )-Xnew22)));
       X( i , :) = Bounds(X(i , : ), Xmin, Xmax );
         fit(  i  ) = fitness( X(  i , : ),G );
  
  end
  
  for j = 20 : NP                 % Equation (7)
       X( j,: )=bestX+randn(1,dim).*((abs(( pX(j,:  )-bestXX)))+(abs(( pX(j,:  )-bestX))))./2;
      X( j , :) = Bounds(X(j , : ), Xmin, Xmax );
         fit(  j  ) = fitness( X(  j , : ),G );
  end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
     XX=pX;
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % 更新个体最优值和全局最优值
    for i = 1 : NP
        if (fit(i) < fpbest(i))
            fpbest(i) = fit(i);
            pX(i, :) = X(i, :);
        end
        if(fpbest(i) < fgbest)
            fgbest = fpbest(i);
            bestX = pX(i, :);
        end
    end
    bestX = LocalSearch(bestX,Xmax,G);
    fgbest = fitness(bestX,G);
    FG(gen,1)=fgbest;
end

参考文献

[1] 基于人工势场结合快速搜索树APF+RRT实现机器人避障规划附matlab代码
[2] 基于蚁群算法求解栅格地图路径规划问题matlab源码含GUI