AntColonyOptimization蚁群算法（基于matlab解决机器人路径规划)

1、什么是蚁群算法
1.1、蚁群算法的来源
  同遗传算法相似，都来自于大自然的启迪。蚁群算法就来自于蚂蚁寻找食物过程中发现路径的行为。
  蚂蚁并没有视觉却可以寻找到食物，这得益于蚂蚁分泌的信息素，蚂蚁之间相互独立，彼此之间通过信息素进行交流，从而实现群体行为。
1.2、蚁群算法的基本原理
  基本原理的过程就是蚂蚁觅食的过程。首先，蚂蚁在觅食的过程中会在路径上留下信息素的物质，并在寻找食物的过程中感知这种物质的强度，并指导自己的行为方向，他们总会朝着浓度高的方向前进。因此可以看得出来，蚂蚁觅食的过程是一个正反馈的过程，该路段经过的蚂蚁越多，信息素留下的就越多，浓度越高，更多的蚂蚁都会选择这个路段。

2 仿真源码

clear;
clc;
G=[0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1; 
   1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1; 
   1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1; 
   1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1; 
   1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1; 
   1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1; 
   1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1; 
   1 0 0 0 0 1 1 1 1 1 1 1 1 0 0 0 0 0 0 1; 
   1 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1; 
   1 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1; 
   1 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1; 
   1 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1; 
   1 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1; 
   1 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1; 
   1 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1; 
   1 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1; 
   1 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 1; 
   1 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 1; 
   1 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 1; 
   1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1;];
mm = size(G, 1);      % G 地形图为01矩阵，如果为1表示障碍物
Tau = 8. * ones(mm^2, mm^2);  % Tau 初始信息素矩阵
epochs = 100;  % 迭代次数
ants = 50;  % 蚂蚁数量
start = 1;    %最短路径的起始点
stop =15 * 20+15 ; %最短路径的目的点
alpha = 1;  % Alpha 表征信息素重要程度的参数
beta = 7;   % Beta 表征启发式因子重要程度的参数
rho = 0.3;   % Rho 信息素蒸发系数
q = 1;  % 信息素增强系数
minkl = inf;
mink = 0;
minl = 0;
D = G2D(G);
n = size(D, 1);  %N表示问题的规模(象素个数)
stop_x = mod(stop, mm) - 0.5; %终止点横坐标
if stop_x == - 0.5
    stop_x = mm - 0.5;
end
%终止点纵坐标
stop_y = mm + 0.5 - ceil(stop / mm);
% 启发式信息 遍历所有节点
Eta = zeros(n);
%以下启发式信息矩阵
for i = 1: n
   ix =  mod(i, mm) - 0.5;
   if ix == - 0.5
       ix = mm - 0.5;
   end
   iy = mm + 0.5 - ceil(i / mm);
   if i ~= stop
       Eta(i) = 1/((ix - stop_x)^2 + (iy - stop_y)^2)^0.5;
   else
       Eta(i) = 100;
   end
end
ROUTES = cell(epochs, ants); %用细胞结构存储每一代的每一只蚂蚁的爬行路线
Distance = zeros(epochs, ants); %用矩阵存储每一代的每一只蚂蚁的爬行路线长度
%%  启动epochs轮蚂蚁觅食活动，每轮派出ants只蚂蚁
for epoch = 1: epochs
    for ant = 1: ants
        current = start;  %当前节点初始化为起始点
        Path = start;  %爬行路线初始化
        DisKm = 0;  %爬行路线长度初始化
        TABUkm = ones(n);  %禁忌表初始化
        TABUkm(start) = 0;  %已经在初始点了，因此要排除
        DD = D;     %邻接矩阵初始化
        %下一步可以前往的节点
        DW = DD(current, :);
        DW1 = find(DW);
        for j = 1: length(DW1)
            if TABUkm(DW1(j)) == 0
                DW(DW1(j)) = 0;
            end
        end
        LJD = find(DW);
        Len_LJD = length(LJD);%可选节点的个数
        %蚂蚁未遇到食物或者陷入死胡同或者觅食停止
        while current ~= stop && Len_LJD >= 1
            %转轮赌法选择下一步怎么走
            PP = zeros(Len_LJD);
            for i = 1: Len_LJD
                PP(i) = (Tau(current, LJD(i))^alpha) * (Eta(LJD(i))^beta);
            end
            sumpp = sum(PP);%建立概率分布
            PP = PP / sumpp;
            Pcum = cumsum(PP);
            select = find(Pcum > rand);
            to_visit = LJD(select(1));
            %状态更新和记录
            Path = [Path, to_visit]; %路径增加
            DisKm = DisKm + DD(current, to_visit); %路径长度增加
            current = to_visit;%蚂蚁移到下一个节点
            for kk = 1: n
                if TABUkm(kk) == 0
                    DD(current, kk) = 0;
                    DD(kk, current) = 0;
                end
            end
            TABUkm(current) = 0;%已访问过的节点从禁忌表中删除
            DW = DD(current, :);
            DW1 = find(DW);
            for j = 1: length(DW1)
                if TABUkm(DW1(j)) == 0
                    DW(DW(j)) = 0;
                end
            end
            LJD = find(DW);
            Len_LJD = length(LJD);%可选节点的个数
        end
        %记下每一代每一只蚂蚁的觅食路线和路线长度
        ROUTES{epoch, ant} = Path;
        if Path(end) == stop
            Distance(epoch, ant) = DisKm;
            if DisKm < minkl
                minkl = DisKm;
                mink = epoch;
                minl = ant;
            end
        else
            Distance(epoch, ant) = 0;
        end
    end
    %%更新信息素
    Delta_Tau = zeros(n, n);%更新量初始化
    for ant = 1: ants
        if Distance(epoch, ant)
            ROUT = ROUTES{epoch, ant};
            TS = length(ROUT) - 1;%跳数
            Dis_km = Distance(epoch, ant);
            for s = 1: TS
                x = ROUT(s);
                y = ROUT(s + 1);
                Delta_Tau(x, y) = Delta_Tau(x, y) + q / Dis_km;
                Delta_Tau(y, x) = Delta_Tau(y, x) + q / Dis_km;
            end
        end
    end
    Tau = (1 - rho) * Tau + Delta_Tau;
end
%% 绘图
plotif = 1;
if plotif == 1
    minDis = zeros(epochs, 1);
    for i = 1: epochs
        Dis = Distance(i, :);
        Nonzero = find(Dis);
        PLK = Dis(Nonzero);
        minDis(i) = min(PLK);
    end
    figure(1);
    plot(minDis);
    hold on;
    grid on;
    title('收敛曲线变化趋势'); 
    xlabel('迭代次数'); 
    ylabel('最小路径长度');
    figure(2) 
    axis([0,mm,0,mm]);
    for i = 1: mm
        for j = 1:mm
            if G(i, j) == 1
                x1 = j - 1; y1 = mm - i;
                x2 = j;y2 = mm - i;
                x3 = j;y3 = mm - i + 1;
                x4 = j - 1;y4 = mm - i + 1;
                fill([x1, x2, x3, x4], [y1, y2, y3, y4],[0.2, 0.2, 0.2]);
                hold on;
            else
                x1 = j - 1; y1 = mm - i;
                x2 = j;y2 = mm - i;
                x3 = j;y3 = mm - i + 1;
                x4 = j - 1;y4 = mm - i + 1;
                fill([x1, x2, x3, x4], [y1, y2, y3, y4],[1, 1, 1]);
                hold on;
            end
        end
    end
    hold on;
    title('蚁群算法'); 
    xlabel('坐标x'); 
    ylabel('坐标y');
    ROUT = ROUTES{mink, minl};
    LENROUT = length(ROUT);
    Rx = ROUT;
    Ry = ROUT;
    for ii = 1: LENROUT
        Rx(ii) = mod(ROUT(ii),mm)-0.5;
        if Rx(ii) == -0.5 
            Rx(ii) = mm - 0.5; 
        end
        Ry(ii) = mm + 0.5 - ceil(ROUT(ii) / mm); 
    end
    plot(Rx, Ry);
end

3.参考：AntColonyOptimization蚁群算法 | 曲怪曲怪 (quguai.cn)