蚁群算法实现三维避障轨迹规划（Matlab）

关于蚁群算法

其实大多数优化算法都是“试错”的过程，不同的是如何利用在“试错”过程中得到的经验。蚁群算法在“试错”的过程中通过留下信息素对未来的试错过程加以提示，从而保证一定的收敛性。

代码分析

我写了一份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-rou)I+c/fit$$
rou是衰减因子，fit是路径适应值,c是一个可调因子。由于我的fit是路径长度，路径越短，信息素增加得越多。

路径搜索函数searchPath

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。

计算启发值函数calHeuristicValue

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

计算适应值函数calFit

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;

结果

the future

感觉我写的代码效率有点低，运行得比较慢，另外切片的这种方式让轨迹不够平滑。这个代码有很多超参数需要调…不想调了…