路径规划 | 基于蜣螂优化算法的栅格地图机器人路径规划（Matlab）

news2024/10/7 20:37:08

效果一览

在这里插入图片描述

基本介绍

路径规划 | 基于蜣螂优化算法的栅格地图机器人路径规划（Matlab）
1.利用蜣螂算法DBO优化栅格地图机器人路径规划，效果如图所示，包括迭代曲线图、栅格地图等等～
2.注释清晰，适合新手小白[火]
3.附赠示例地形数据，直接运行runme即可一键出图.

程序设计

完整源码和数据私信博主回复基于蜣螂优化算法的栅格地图机器人路径规划（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