其实大多数优化算法都是“试错”的过程，不同的是如何利用在“试错”过程中得到的经验。蚁群算法在“试错”的过程中通过留下信息素对未来的试错过程加以提示，从而保证一定的收敛性。
我写了一份matlab蚁群算法三维避障规划的代码，能保证收敛，但不够优秀，可以作为基础加以改进。
无论是GA、SA、PSO、还是本文讲到的蚁群，应用在三维轨迹规划中都存在一个问题：如何将轨迹表述成一个解以及如何生成一个从起始点到终点的解。在我的A*算法三维规划博客中通过分解空间成类似点云的形式来表述解。而在蚁群算法三维规划中，我采用的是对空间切片的方式（可能不够好，但是能用）。
clc; clear; close all;
%% 参数读取与设置
obstacleMatrix = csvread("./data_csv/obstacleMatrix.csv");
RobstacleMatrix = csvread("./data_csv/RobstacleMatrix.csv")';
cylinderMatrix = csvread("./data_csv/cylinderMatrix.csv");
cylinderRMatrix = csvread("./data_csv/cylinderRMatrix.csv")';
cylinderHMatrix = csvread("./data_csv/cylinderHMatrix.csv")';
start = csvread("./data_csv/start.csv")';
goal = csvread("./data_csv/goal.csv")';
没有csv文件可以直接写这几个matrix，它们表述的是障碍物的坐标和半径等信息，obstacleMatrix为球障碍物的圆心坐标矩阵（n*3矩阵），RobstacleMatrix为对应半径向量（n*1向量），cylinderMatrix为圆柱体中心坐标（n*2矩阵，没有第三个维度，从z=0开始绘制圆柱体），cylinderRMatrix为对应圆柱体半径（n*1向量），cylinderHMatrix为对应圆柱体的高（n*1向量）。
popNumber = 50; % 蚁群个体数量
rou = 0.1; % 挥发因子
bestFitness = []; % 每一代的最佳适应值储存列表
bestfitness = inf;% 初始化最佳适应值（本案例中越小越好）
everyIterFitness = [];
deltaX = 0.2; deltaY = 0.2; deltaZ = 0.2;
gridXNumber = floor(abs(goal(1) - start(1)) / deltaX);
gridYNumber = 50; gridZNumber = 50;
ybegin = start(2) - 20*deltaY; zbegin = start(3) - 20*deltaZ;
pheromone = ones(gridXNumber, gridYNumber, gridZNumber);
ycMax = 3; zcMax = 3; % 蚂蚁沿y轴最大变动格子数和沿z轴最大变动格子数
bestPath = [];
iterMax = 150;
其中deltaX为对x轴的切片步长：
deltaY和deltaZ为每一个切片下对Y轴和Z轴的网格划分步长。ybegin和zbegin为y和z的起始参考值，在信息素矩阵pheromone中利用y和z分别减去ybegin和zbegin再分别处以deltaY和deltaZ就是索引值了。相当于对空间离散坐标进行编码。gridXNumber为横向切片的数量。
%% 绘制障碍环境
figure(1)
[n,~] = size(obstacleMatrix);
for i = 1:n %绘制静态球障碍物
[x,y,z] = sphere();
surfc(RobstacleMatrix(i)*x+obstacleMatrix(i,1),...
RobstacleMatrix(i)*y+obstacleMatrix(i,2),...
RobstacleMatrix(i)*z+obstacleMatrix(i,3));
hold on;
end
[n,~] = size(cylinderMatrix);
for i = 1:n %绘制圆柱体障碍物
[x,y,z] = cylinder(cylinderRMatrix(i));
z(2,:) = cylinderHMatrix(i);
surfc(x + cylinderMatrix(i,1),y + cylinderMatrix(i,2),...
z,'FaceColor','interp');
hold on;
end
bar1 = scatter3(start(1),start(2),start(3),80,"cyan",'filled','o','MarkerEdgeColor','k');hold on
bar2 = scatter3(goal(1),goal(2),goal(3),80,"magenta",'filled',"o",'MarkerEdgeColor','k');
text(start(1),start(2),start(3),' 起点'); text(goal(1),goal(2),goal(3),' 终点');
axis equal
set(gcf,'unit','centimeters','position',[30 10 20 15]);
for iter = 1:iterMax
fprintf("程序已运行：%.2f%%\n",iter/iterMax*100);
% 路径搜索
[path, pheromone] = searchPath(popNumber, pheromone, start, goal, ycMax, zcMax,...
deltaX, deltaY, deltaZ, obstacleMatrix,RobstacleMatrix,...
cylinderMatrix, cylinderRMatrix, cylinderHMatrix,...
ybegin, zbegin, gridYNumber, gridZNumber);
% 路径适应值计算
fitness = calFit(path, deltaX, start, goal);
[newBestFitness, bestIndex] = min(fitness);
everyIterFitness = [everyIterFitness, newBestFitness];
if newBestFitness < bestfitness
bestfitness = newBestFitness;
bestPath = path(bestIndex, :, :);
end
bestFitness = [bestFitness, bestfitness];
% 更新信息素
cfit = 100 / bestfitness;
iterNum = 0;
for x = start(1) + deltaX : deltaX : goal(1) - 0.001
iterNum = iterNum + 1;
pheromone(iterNum, round((bestPath(:,iterNum+1,1)-ybegin)/deltaY), round((bestPath(:,iterNum+1,2)-zbegin)/deltaZ))...
= (1 - rou) * pheromone(iterNum, round((bestPath(:,iterNum+1,1)-ybegin)/deltaY), round((bestPath(:,iterNum+1,2)-zbegin)/deltaZ)) + cfit;
end
end
这里的searchPath为每一个迭代过程中每一只蚂蚁搜索出一条从起点到终点的路径。之后对这些蚂蚁的路径计算适应值（长度衡量），选出适应值最低的和历史最低比较从而更新历史最优解。最后是更新信息素，这里我剑走偏锋只用每一代最优路径来更新信息素而没有将所有蚂蚁的路径用来更新，后者我试了下在这个例子上不太好使容易不收敛。
信息素更新方式我采用的是：
I
=
(
1
−
r
o
u
)
I
+
c
/
f
i
t
I = (1-rou)I + c/fit
I=(1−rou)I+c/fit
rou是衰减因子，fit是路径适应值,c是一个可调因子。由于我的fit是路径长度，路径越短，信息素增加得越多。
function [path, pheromone] = searchPath(popNumber, pheromone, start, goal, ycMax, zcMax,...
deltaX, deltaY, deltaZ, obstacleMatrix, RobstacleMatrix,...
cylinderMatrix, cylinderRMatrix, cylinderHMatrix,...
ybegin, zbegin, gridYNumber, gridZNumber)
% 获取从起点到终点的路径函数
path = []; % 用于记录所有蚂蚁的路径
for ant = 1:popNumber % 对于每一只蚂蚁
path(ant, 1, 1:2) = start(2:3); % 只记录y和z轴坐标，x轴每次加deltaX
nowPoint = start(2:3);
iterNum = 0;
for x = start(1) + deltaX : deltaX : goal(1) - 0.001 % 减去一个小数避免x直接取到goal(1)
iterNum = iterNum + 1;
nextPoint = [];
p = [];
for y = -ycMax * deltaY : deltaY : ycMax * deltaY
for z = -zcMax * deltaZ : deltaZ : zcMax * deltaZ
nextPoint = [nextPoint; nowPoint + [y, z]];
if nextPoint(end,1) > ybegin+0.01 && nextPoint(end,1) < ybegin + gridYNumber*deltaY && ...
nextPoint(end,2) > zbegin+0.01 && nextPoint(end,2) < zbegin + gridZNumber*deltaZ % 判断是否越界（信息素矩阵大小已经定了，避免超出）
hValue = calHeuristicValue(nowPoint, nextPoint(end,:), goal, x, deltaX, obstacleMatrix,...
RobstacleMatrix, cylinderMatrix, cylinderRMatrix, cylinderHMatrix);
% pher = pheromone(iterNum, round((nextPoint(end,1) - ybegin)/deltaY), round((nextPoint(end,2) - zbegin)/deltaZ));
try
pher = pheromone(iterNum, round((nextPoint(end,1) - ybegin)/deltaY), round((nextPoint(end,2) - zbegin)/deltaZ));
catch
round((nextPoint(end,1) - ybegin)/deltaY)
end
p = [p, pher * hValue];
else
p = [p,0]; %置零在轮盘赌中不可能被选中
end
end
end
% 轮盘赌选择下一坐标点
p1 = p / sum(p); % 归一化
pc = cumsum(p1);
targetIndex = find(pc >= rand);
targetNextPoint = nextPoint(targetIndex(1),:);
path(ant, iterNum + 1, 1:2) = targetNextPoint;
nowPoint = targetNextPoint;
end
path(ant, iterNum + 2, 1:2) = goal(2:3);
end
end
这里的大体思路为：已知切片i上一点的坐标，获取切片i+1上的坐标。而切片i+1上的横坐标已经定了即为第i切片的横坐标加上deltaX，纵坐标y和z坐标允许在第i切片的y和z的基础上做偏移，即正负ycMax*deltaY和正负zcMax*deltaZ。对每一个可行的坐标计算其启发值（使用calHeuristicValue函数），得到hValue并乘以该坐标对应的信息素从而得到一个值，值越大越容易被选中。最后使用轮盘赌的方式选取第i+1切片上的坐标点。这里需要注意的是避障检测在计算启发值函数calHeuristicValue函数中使用，如果坐标与障碍物碰撞则将启发值置为0，那么它乘以信息素也是0，在轮盘赌算法中0是不可能被选取的。因此与障碍物有交点的坐标是不可能被选中的。
对每一只蚂蚁采用上述思路都能找到一条路径，放入三维矩阵path中返回。path第一个维度表示是哪一只蚂蚁，第二个维度表示这只蚂蚁路径上的第几个切片，第三个维度表示这一切片上的y和z。
function h = calHeuristicValue(now, next, goal, x, deltaX, obstacleMatrix, RobstacleMatrix,...
cylinderMatrix, cylinderRMatrix, cylinderHMatrix)
% 判断下一个坐标点是否碰撞，若碰撞则将启发值置为0，在后续的轮盘赌点位选择时将不可能被选中
nextXYZ = [x, next];
flag = checkCol(nextXYZ, obstacleMatrix, RobstacleMatrix, cylinderMatrix, cylinderRMatrix, cylinderHMatrix);
% 计算启发值
d1 = getDist([x - deltaX, now], [x, next]);
d2 = getDist([x, next], goal);
D = 50 / (d1 + d2);
h = flag * D;
end
其中checkCol为碰撞检测，getDist为获取欧式几何距离函数：
function flag = checkCol(pos, circleCenter,circleR, cylinderCenter,cylinderR, cylinderH)
% 碰撞检测函数
[numberOfSphere, ~] = size(circleCenter);
[numberOfCylinder, ~] = size(cylinderCenter);
flag = true;
for i = 1:numberOfSphere
if getDist(pos, circleCenter(i,:)) <= circleR(i)
flag = false;
break;
end
end
for i = 1:numberOfCylinder
if getDist(pos(1:2), cylinderCenter(i,:)) <= cylinderR(i) && pos(3) <= cylinderH(i)
flag = false;
break;
end
end
if pos(3) <= 0, flag = false; end
end
function d = getDist(x,y)
d = sqrt(sum((x - y).^2));
end
function f = calFit(path, deltaX, start, goal)
% 计算适应值函数
[n,m,~] = size(path);
x = [start(1) : deltaX : goal(1) - 0.001, goal(1)];
for i = 1:n
f(i) = 0;
for j = 1:m-1
f(i) = f(i) + getDist([x(j), path(i,j,1), path(i,j,2)], [x(j+1), path(i,j+1,1), path(i,j+1,2)]);
end
end
end
采用的是以路径长度作为适应值，即适应值越小越好，返回每个蚂蚁路径的适应值，用于后续搜索最小适应值和最优路径以及更新信息素。
% 绘制路径
x = [start(1):deltaX:goal(1)-0.001,goal(1)];
[~,m] = size(x);
path_ = [];
for i = 1:m
path_ = [path_;bestPath(:,i,1),bestPath(:,i,2)];
end
bar3 = plot3(x, path_(:,1), path_(:,2),'LineWidth',2,'MarkerSize',7,'Color','r');
legend([bar1,bar2,bar3],["起始点","终止点","无人机航迹"])
% 绘制适应值变化图
figure(2)
plot(bestFitness,'LineWidth',2,'Color','r'); hold on;
plot(everyIterFitness,'LineWidth',2,'Color','b')
legend('历史最佳个体适应度','每代最佳个体适应度')
title('适应度变化趋势'); xlabel('迭代次数'); ylabel('适应度值'); grid on;
感觉我写的代码效率有点低，运行得比较慢，另外切片的这种方式让轨迹不够平滑。这个代码有很多超参数需要调…不想调了…