Matlab路径规划算法

该算法输入为三维点的坐标，输出为坐标的最短路径，见代码：

% 定义蚁群算法的参数
clear all

% 定义一组随机的三维空间点
numPoints = 100; % 例如，我们想要30个点
points = rand(numPoints, 3) * 100; % 创建一个10x3的矩阵，包含0到100之间的随机数

% 之后的代码片段，如计算距离矩阵和蚁群算法的实现，都将使用这个points变量

numAnts = 50; % 蚂蚁的数量
numPoints = size(points, 1); % 空间点的数量
evaporationRate = 0.5; % 信息素的蒸发率
alpha = 1; % 信息素重要程度的参数
beta = 5; % 启发函数重要程度的参数

% 初始化信息素矩阵
pheromoneMatrix = ones(numPoints, numPoints);

% 计算距离矩阵
distanceMatrix = zeros(numPoints, numPoints);
for i = 1:numPoints
    for j = i+1:numPoints
        distanceMatrix(i, j) = norm(points(i, :) - points(j, :));
        distanceMatrix(j, i) = distanceMatrix(i, j); % 距离矩阵是对称的
    end
end

%************************************************************************%
maxIter = 150; % 迭代次数
bestLength = inf; % 初始化最佳路径长度
bestPath = []; % 初始化最佳路径
figure;
hPlot = plot3(NaN, NaN, NaN, 'b-'); % 初始化路径绘图
hold on;
plot3(points(:, 1), points(:, 2), points(:, 3), 'ro'); % 绘制所有点
xlabel('X');
ylabel('Y');
zlabel('Z');
title('3D Path Optimization using Ant Colony Optimization');
grid on;
bestLengthText = text(0.7, 0.9, '', 'Units', 'normalized'); % 初始化最短路径长度的文本标注
%************************************************************************%
% 创建一个新的图表窗口用于绘制最短路径长度的变化
figure;
hLine = plot(NaN, NaN, 'b-', 'LineWidth', 2); % 初始化绘图对象，暂时没有数据
xlabel('Iteration');
ylabel('Best Path Length');
title('Real-time Best Path Length by Iteration');
grid on;
iterBestLengths = zeros(maxIter, 1); % 初始化存储每次迭代最佳长度的数组
%************************************************************************%
for iter = 1:maxIter
    pathLength = zeros(numAnts, 1);
    paths = zeros(numAnts, numPoints);
    
    for k = 1:numAnts
        start = randi(numPoints); % 随机选择起点
        path = start;
        visited = false(numPoints, 1);
        visited(start) = true;
        
        for step = 2:numPoints
            current = path(end);
            probabilities = zeros(numPoints, 1);
            
            for j = 1:numPoints
                if ~visited(j)
                    probabilities(j) = (pheromoneMatrix(current, j)^alpha) * ((1 / distanceMatrix(current, j))^beta);
                end
            end
            
            probabilities = probabilities / sum(probabilities);
            next = randsample(numPoints, 1, true, probabilities);
            path = [path, next];
            visited(next) = true;
        end
        
        pathLength(k) = sum(distanceMatrix(sub2ind(size(distanceMatrix), path(1:end-1), path(2:end))));
        paths(k, :) = path;
        
        if pathLength(k) < bestLength
            bestLength = pathLength(k);
            bestPath = path;
        end
    end
    
    % 更新信息素
    pheromoneMatrix = (1 - evaporationRate) * pheromoneMatrix;
    for k = 1:numAnts
        for step = 1:numPoints-1
            pheromoneMatrix(paths(k, step), paths(k, step+1)) = pheromoneMatrix(paths(k, step), paths(k, step+1)) + 1 / pathLength(k);
            pheromoneMatrix(paths(k, step+1), paths(k, step)) = pheromoneMatrix(paths(k, step), paths(k, step+1)); % 保持对称
        end
    end
     iterBestLengths(iter) = bestLength; % 存储当前迭代的最佳路径长度
     % 更新图表数据
    set(hLine, 'XData', 1:iter, 'YData', iterBestLengths(1:iter));
    
    drawnow; % 强制MATLAB立即绘制更新后的图表

   if ~isempty(bestPath)
        x = [points(bestPath, 1); points(bestPath(1), 1)];
        y = [points(bestPath, 2); points(bestPath(1), 2)];
        z = [points(bestPath, 3); points(bestPath(1), 3)];
        if isgraphics(hPlot)
            set(hPlot, 'XData', x, 'YData', y, 'ZData', z);

            % 更新最短路径长度的标注
            bestLengthStr = sprintf('Best Length: %.2f', bestLength);
            set(bestLengthText, 'String', bestLengthStr);

            drawnow; % 强制MATLAB立即绘制当前图表
            pause(0.5); % 暂停0.5秒
        end
    end
end
%************************************************************************%

代码运行环境为Matlab2018b，运行效果为输出一个路径规划展示效果图，以及一个迭代展示图

如下：