蚁群算法可以用于路径规划,在本例中,地形矩阵用0表示无障碍物、用1表示有障碍物,机器人从1x1处走到10x10处,使用蚁群算法找最短路径。
步骤如下:
结果如下,其中黄色块为障碍物,红色线为路线:
主函数如下:
functionmain()
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("minlength: %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
每只蚂蚁的下一步候选点应该是这样的:
得到待选点函数如下:
functionnextlist = 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);
在得到待选点列表后,就能通过轮盘赌法得到下一点了:
functionnextpointindex = getNextPoint(p)
%使用轮盘赌法给出下一个点
%p input 概率列表
%nextpointindex output 下一个点
sump =sum(p);
p = p / sump;
cumsump =cumsum(p);
list =find(cumsump >= rand);
nextpointindex= list(1);
制作地形矩阵函数:
functionG = 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;
制作启发式因子矩阵:
functioneta = makeEta(G)
% 制作启发式因子矩阵(到终点距离倒数,障碍物为0)
% G input 地形矩阵
% eta output 启发式因子矩阵
eta= G;
[rn,cn] = size(G);
fori = 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