蚁群算法可以用于路径规划，在本例中，地形矩阵用0表示无障碍物、用1表示有障碍物，机器人从1x1处走到10x10处，使用蚁群算法找最短路径。
步骤如下：
初始化参数、地形矩阵、信息素矩阵和启发式因子矩阵。启发式因子矩阵中一点的值为该点到终点距离的倒数，距离越短，启发式因子越大，障碍物处的启发式因子为0。信息素矩阵被初始化为一个统一的值。
在本例中，将一条路径表示如下：[路径长度 点1 点2 ……]，例如[2 1 2 0 0]表示该路径长度为2，路径为[1 2]。
对每次迭代中的每只蚂蚁，进行如下3步，直至到达终点或者陷入死胡同：
创建一个禁忌矩阵，禁忌矩阵中已经访问过的点为0，其余点与启发式因子矩阵中相应点的值相同。
设置初始点，根据信息素、启发式因子、禁忌表，通过轮盘赌方法，选择下一个城市。
更新路径和禁忌矩阵。
每次迭代后，更新信息素，只对最优路径中的点进行增加信息素操作。
迭代， 直至结束。
结果如下，其中黄色块为障碍物，红色线为路线：
蚁群算法路径规划结果
主函数
主函数如下：
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, :);
endendlk = 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;endend得到下一点函数
每只蚂蚁的下一步候选点应该是这样的：
没有障碍物
该蚂蚁之前没有经过
紧邻蚂蚁（蚂蚁不能飞）
得到待选点函数如下：
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); endenda = 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 endend
计算路径长度矩阵
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; endendend
作者：mwangjs
链接：https://www.jianshu.com/p/622bf89ba88e
随时随地看视频
