蚁群算法规划路径

蚁群算法可以用于路径规划，在本例中，地形矩阵用0表示无障碍物、用1表示有障碍物，机器人从1x1处走到10x10处，使用蚁群算法找最短路径。

步骤如下：

1. 初始化参数、地形矩阵、信息素矩阵和启发式因子矩阵。启发式因子矩阵中一点的值为该点到终点距离的倒数，距离越短，启发式因子越大，障碍物处的启发式因子为0。信息素矩阵被初始化为一个统一的值。
2. 在本例中，将一条路径表示如下：[路径长度 点1 点2 ……]，例如[2 1 2 0 0]表示该路径长度为2，路径为[1 2]。
3. 对每次迭代中的每只蚂蚁，进行如下3步，直至到达终点或者陷入死胡同：
4. 创建一个禁忌矩阵，禁忌矩阵中已经访问过的点为0，其余点与启发式因子矩阵中相应点的值相同。
5. 设置初始点，根据信息素、启发式因子、禁忌表，通过轮盘赌方法，选择下一个城市。
6. 更新路径和禁忌矩阵。
7. 每次迭代后，更新信息素，只对最优路径中的点进行增加信息素操作。
8. 迭代， 直至结束。

结果如下，其中黄色块为障碍物，红色线为路线：

蚁群算法路径规划结果

主函数

主函数如下：

function main()
rn = 10; cn = 10;
G       = makeG(rn, cn);                % 地形图
tau     = 8 .* ones(rn, cn);            % 初始化信息素
MaxGen  = 100;                          % 迭代次数
N       = 50;                           % 蚂蚁个数
S       = 1;            % 路径起始点
E       = rn * cn;      % 路径终点
Alpha   = 1;            % 信息素重要程度
Beta    = 30;           % 启发式因子重要程度
Rho     = 0.3;          % 信息素挥发系数
Q       = 5;            % 信息素增加系数

Eta     = makeEta(G);   % 距离倒数矩阵

gpath = zeros(MaxGen, rn*cn+1);     % 每代最优路径 [地点个数 地点……]

for g = 1:MaxGen
    npath = zeros(N, rn*cn+1);          % 每个路径 [地点个数 地点……]
    for n = 1:N
        D = Eta;                        % 禁忌矩阵
        path = zeros(1, rn*cn+1);       % 路径
        
        % 更新点、路径和禁忌矩阵
        point = S;
        path(1, 1) = path(1, 1) + 1;
        path(1, path(1,1)+1) = point;
        D(point) = 0;
        
        % 搜索下一个点的坐标范围
        nextlist = getNextList(point, rn, cn, D);
        % 一直前进，直到到达食物或者陷入死胡同
        while point ~= E && ~isempty(nextlist)
            % 轮盘赌算法取下一点
            p = zeros(1, length(nextlist));
            for i = 1:length(nextlist)
                p(1, i) = (tau(nextlist(i))^Alpha) * (Eta(nextlist(i))^Beta);
            end
            nextpoint = nextlist(getNextPoint(p));

            % 更新点、路径和禁忌矩阵
            point = nextpoint;
            path(1, 1) = path(1, 1) + 1;
            path(1, path(1,1)+1) = point;
            D(point) = 0;
            nextlist = getNextList(point, rn, cn, D);
        end
        % 记录成功成功到达终点的蚂蚁的路径
        if (path(1, 1+path(1,1)) == E)
            npath(n, :) = path;
        end
    end
    npath = npath(find(sum(npath,2)), :);       % 保留到达终点的路径
    lk = calLk(npath, rn, cn);                  % 计算lk距离
    % 更新信息素
    tau = (1 - Rho) .* tau;
    for i = 1:size(npath, 1)
        for j = 2:npath(i,1)+1
            tau(npath(i,j)) = tau(npath(i,j)) + Q / lk(i);
        end
    end
    [~, minindex] = min(lk);
    if size(npath, 1) > 0
        gpath(g, :) = npath(minindex, :); 
    end
end
lk = calLk(npath, rn, cn);
[minvalue, minindex] = min(lk);
fprintf("min length: %f\n", minvalue);
bestpath = gpath(minindex,:);
bestpath = bestpath(2:1+bestpath(1,1));
figure;
imagesc(G);
hold on;
for i = 2:length(bestpath)
    [x1, y1] = ind2sub([rn, cn], bestpath(i-1));
    [x2, y2] = ind2sub([rn, cn], bestpath(i));
    plot([y1, y2], [x1, x2], 'r');
    hold on;
end
end

得到下一点函数

每只蚂蚁的下一步候选点应该是这样的：

1. 没有障碍物
2. 该蚂蚁之前没有经过
3. 紧邻蚂蚁（蚂蚁不能飞）

得到待选点函数如下：

function nextlist = getNextList(point, rn, cn, D)
% 给出待选点列表
% point         input  当前点
% rn            input  地图行数
% cn            input  地图列数
% D             input  禁忌地图
% nextlist      output 待选点列表
list = find(D);
nextlist = zeros(1, length(list)+1);
[pointx, pointy] = ind2sub([rn, cn], point);
for i = 1:length(list)
    [indexx, indexy] = ind2sub([rn, cn], list(i));
    if (indexx >= pointx-1 && indexx <= pointx+1 ...
            && indexy >= pointy-1 && indexy <= pointy+1)
        nextlist(1, 1) = nextlist(1, 1) + 1;
        nextlist(1, nextlist(1,1)+1) = list(i);
    end
end
a  = nextlist(1,1);
nextlist = nextlist(1, 2:1+a);

在得到待选点列表后，就能通过轮盘赌法得到下一点了：

function nextpointindex = getNextPoint(p)
% 使用轮盘赌法给出下一个点
% p                 input  概率列表
% nextpointindex    output 下一个点
sump = sum(p);
p = p / sump;
cumsump = cumsum(p);
list = find(cumsump >= rand);
nextpointindex = list(1);

一些其他函数

制作地形矩阵函数：

function G = makeG(rn, cn)
% 制作地形矩阵
% rn        input  地形矩阵函数
% cn        input  地形矩阵函数
% G         output 地形矩阵
G = zeros(rn, cn);
G(1:3, 2) = 1;
G(7:10, 1:5) = 1;
G(5, 3) = 1;
G(1, 4) = 1;
G(1:5, 5) = 1;
G(5:7, 7:9) = 1;

制作启发式因子矩阵：

function eta = makeEta(G)
% 制作启发式因子矩阵（到终点距离倒数，障碍物为0）
% G         input  地形矩阵
% eta       output 启发式因子矩阵
eta = G;
[rn, cn] = size(G);
for i = 1:rn
    for j = 1:cn
        if G(i, j) == 1
            eta(i, j) = 0;
        elseif (i == rn && j == cn)
            eta(i, j) = 1;
        else
            eta(i, j) = 1 / ( (rn-i)^2+(cn-j)^2 )^0.5;
        end
    end
end

计算路径长度矩阵

function lk = calLk(npath, rn, cn)
% 计算路径长度
% npath         input  路径
% rn            input  地图行数
% cn            input  地图列数
% lk            output 路径长度 rnx1
[nr, ~] = size(npath);
lk = zeros(nr, 1);
for i = 1:nr
    path = npath(i, 2:1+npath(1,1));
    for j = 2:length(path)
        [x1, y1] = ind2sub([rn, cn], path(j-1));
        [x2, y2] = ind2sub([rn, cn], path(j));
        lk(i) = lk(i) + ((x2-x1)^2+(y2-y1)^2)^0.5;
    end
end
end