m基于蚁群优化模糊控制的机器人路线规划和避障算法

1.算法描述 蚁群算法是受到对真实蚂蚁群觅食行为研究的启发而提出。生物学研究表明：一群相互协作的蚂蚁能够找到食物和巢穴之间的最短路径,而单只蚂蚁则不能。生物学家经过大量细致观察研究发现,蚂蚁个体之间的行为是相互作用相互影响的。蚂蚁在运动过程中,能够在它所经过的路径上留下一种称之为信息素的物质,而此物质恰恰是蚂蚁个体之间信息传递交流的载体。蚂蚁在运动时能够感知这种物质,并且习惯于追踪此物质爬行,当然爬行过程中还会释放信息素。一条路上的信息素踪迹越浓,其它蚂蚁将以越高的概率跟随爬行此路径,从而该路径上的信息素踪迹会被加强,因此,由大量蚂蚁组成的蚁群的集体行为便表现出一种信息正反馈现象。某一路径上走过的蚂蚁越多,则后来者选择该路径的可能性就越大。蚂蚁个体之间就是通过这种间接的通信机制实现协同搜索最短路径的目标的。

   蚁群算法是对自然界蚂蚁的寻径方式进行模拟而得出的一种仿生算法。蚂蚁在运动过程中，能够在它所经过的路径上留下信息素进行信息传递，而且蚂蚁在运动过程中能够感知这种物质，并以此来指导自己的运动方向。因此，由大量蚂蚁组成的蚁群的集体行为便表现出一种信息正反馈现象：某一路径上走过的蚂蚁越多，则后来者选择该路径的概率就越大。

2.仿真效果预览 matlab2022a仿真结果如下：

3.MATLAB核心程序

%产生障碍物模型
[Obstacle1,Obstacle2] = func_obstacle();
 
%%
%蚁群算法初始化
Initial_matrix = size(Obstacle1,1);      
%保留觅食活动中有残留的信息素
Res_pher       = 8*ones(Initial_matrix*Initial_matrix,Initial_matrix*Initial_matrix);
%迭代次数
Max_iter       = 100;    
%蚂蚁个数
Number         = 60;    
%起点
Start_point    = 1 ; 
%终点
End_point      = Initial_matrix*Initial_matrix;
%信息素重要性参数
Importance     = 1;      
%启发式因子重要性参数
Importance2    = 4;     
%蚂蚁信息素挥发系数
Rho            = 0.2;     
%信息素增加强度系数
Enhance_Q      = 1;           
minkl          = 1e50;
mink           = 0;
minl           = 0;
%障碍物搜搜
D              = func_search(Obstacle1);
%障碍物大小
N              = size(D,1); 
%小方格象素的边长
Lens           = 1;
 
%终点坐标
Ex             = Lens*(mod(End_point,Initial_matrix)-0.5);
if Ex == -0.5
   Ex = Initial_matrix-0.5;
end
Ey = Lens*(Initial_matrix+0.5-ceil(End_point/Initial_matrix));
 
%蚁群算法构造初始化信息
Eta    = func_Heuristic_matrix(Initial_matrix,Ex,Ey,End_point,N,Lens);
%爬行路线
Routes = cell(Max_iter,Number);
%每只蚂蚁的爬行路线长度
RLen   = zeros(Max_iter,Number);
 
 
%%
%开始算法
for i1=1:Max_iter
    i1
    for i2=1:Number
        %初始化模糊控制器
        %模糊控制器的输入为蚁群算法的迭代误差
        if i1==1
           error = 1; 
        else
           error = mean(mean(Res_pher)); 
        end
        %将误差输入到模糊控制器
        controller = func_fuzzy(error);
 
        %状态初始化
        %初始为起始点
        Wpoint   = Start_point;
        %路线初始化
        Wpath    = Start_point;
        %长度初始化
        Wlen    = 0;
        %禁忌表初始化
        Wtl     = ones(N);
        %已经在初始点了，因此要排除
        Wtl(Start_point) = 0;
        %邻接矩阵初始化
        Nmatrix = D;
        %蚂蚁搜索行走 
        dworks   = Nmatrix(Wpoint,:);
        dworks1  = find(dworks);
        for j=1:length(dworks1)
            if Wtl(dworks1(j))==0
               dworks(dworks1(j))=0;
            end
        end
        %蚂蚁行走过的节点
        LJD     = find(dworks);
        Len_LJD = length(LJD);
 
        %觅食条件
        while(Wpoint ~= End_point && Len_LJD >= 1)
            %转轮赌法
            PP=zeros(Len_LJD);
            for i=1:Len_LJD
                PP(i) = (Res_pher(Wpoint,LJD(i))^Importance)*((Eta(LJD(i)))^(Importance2+0.01/controller));
            end
            %概率分布
            sumpp   = sum(PP);
            PP      = PP/sumpp;
            Pcum(1) = PP(1);
            for i=2:Len_LJD
                Pcum(i) = Pcum(i-1) + PP(i);
            end
            %产生模拟超声波
            %设备发送超声波出去,产生超声波脉冲
            op_seq  = pnseq(6,[1 0 1 1 0 0],[1 0 1 0 0 1]);
            %发送超声波脉冲
            op_seq2 = 2*[rand(size(op_seq)),rand(size(op_seq)),rand(size(op_seq)),rand(size(op_seq)),rand(size(op_seq)),op_seq,rand(size(op_seq)),rand(size(op_seq))]-1;
            %如果感知前方没有障碍物，则进行下一个点运动，Pcum用来模拟传感器的接收信号      
            %根据接收到的信号分析是否前方有障碍物
            op_seq3 = awgn(op_seq2,10+20*randn,'measured');  
            %根据接收到的超声波反射信号计算峰值
            dout    = func_peak(op_seq3,op_seq);
            %根据接收到的信号获得方向，即select。
            %判断是否有回收信号峰值
            flag    = 0;
            [Vmax,Imax] = max(dout);
            if Vmax > mean(abs(dout));
               flag = 1;%说明检测到回声波，则说明有障碍物，则调整角度
            end
            if flag == 1
               Select   = find(Pcum>=rand);
               %需要调整角度
               to_visit = LJD(Select(1));
            else
               to_visit = LJD(1);
            end
            
            %状态更新和记录
            %路径增加
            Wpath    =[Wpath,to_visit];
            %路径长度增加
            Wlen     = Wlen+Nmatrix(Wpoint,to_visit);
            
            %蚂蚁移到下一个节点
            Wpoint   = to_visit;
            for kk=1:N
               if Wtl(kk)==0
                  Nmatrix(Wpoint,kk) = 0;
                  Nmatrix(kk,Wpoint) = 0;
               end
            end
            %已访问过的节点从禁忌表中删除
            Wtl(Wpoint) = 0;
            dworks      = Nmatrix(Wpoint,:);
            dworks1     = find(dworks);
            for j=1:length(dworks1)
                if Wtl(dworks1(j))==0
                   dworks(j)=0;
                end
            end
            LJD     = find(dworks);
            %可选节点的个数
            Len_LJD = length(LJD);
        end
        %记下每代每只蚂蚁的觅食路线和路线长度
        Routes{i1,i2} = Wpath;
        if Wpath(end)==End_point
           RLen(i1,i2)=Wlen;
           if Wlen < minkl
              mink = i1;
              minl = i2;
              minkl= Wlen;
           end
        else
          RLen(i1,i2)=0;
        end
    end
    %第六步：更新信息素
    Delta_Tau = zeros(N,N);%更新量初始化
    for i2 = 1:Number
        if RLen(i1,i2)
           ROUT  = Routes{i1,i2};
           TS    = length(ROUT)-1;%跳数
           PL_km = RLen(i1,i2);
           for s=1:TS
               x              = ROUT(s);
               y              = ROUT(s+1);
               Delta_Tau(x,y) = Delta_Tau(x,y)+Enhance_Q/PL_km;
               Delta_Tau(y,x) = Delta_Tau(y,x)+Enhance_Q/PL_km;
           end
        end
    end
    
    %改进算法，Delta_Tau计算方法的改进。
    if sum(sum(Delta_Tau)) > 10;
       Res_pher =(1-Rho).*Res_pher+Rho*Delta_Tau;%信息素挥发一部分，新增加一部分
    else
       Res_pher =(1-Rho).*Res_pher;%信息素挥发一部分，新增加一部分 
    end
    
    PLK      = RLen(i1,:);
    minPL(i1)= mean(PLK(find(PLK)))-1;
     
end
 
 
figure
plot(minPL);
grid on
title('收敛曲线');
xlabel('迭代次数');
ylabel('路径长度');
 
 
%绘爬行图
figure
func_lines(Obstacle2,Routes,Initial_matrix,mink,minl,Lens);
axis([0,20,0,20]); 
%显示模糊规则表
fuzzy; 
02_030m