拥堵路径规划matlab

% 建立地图矩阵
map = zeros(10,10); 
map(3:5,7:10) = 1; % 设定交通拥堵区域为1

% 设定参数
ant_num = 10; % 蚂蚁数量
iteration = 100; % 迭代次数
alpha = 1; % α
beta = 0.1; % β
rho = 0.1; % evaporation rate
Q = 1; % 常量Q
eta = 1./(map+0.1); % 可见性矩阵

% 初始化信息素矩阵
pheromone = ones(size(map));

% 进行迭代
for iter = 1:iteration
    % 初始化蚂蚁
    ant_location = [randi(size(map,1),1,ant_num);randi(size(map,2),1,ant_num)];
    ant_route = ant_location(1,:); % 记录蚂蚁路径

    % 循环迭代所有蚂蚁
    for ant = 1:ant_num
        % 循环迭代到蚂蚁到达终点或无法继续前进
        while true
            cur_loc = ant_location(:,ant); % 当前蚂蚁位置
            available_pos = [cur_loc+[1;0], cur_loc+[-1;0], cur_loc+[0;1], cur_loc+[0;-1]];
            % 剔除超出地图和被阻挡的行列
            out_of_bound = available_pos(1,:)<1 | available_pos(1,:)>size(map,1) | available_pos(2,:)<1 | available_pos(2,:)>size(map,2);
            blocked = map(sub2ind(size(map),available_pos(1,:),available_pos(2,:))) == 1;
            available_pos(:,out_of_bound | blocked) = [];

            % 计算可选择路径的信息素和可见性之和
            prob = (pheromone(sub2ind(size(map),available_pos(1,:),available_pos(2,:))).^alpha) .* (eta(sub2ind(size(map),available_pos(1,:),available_pos(2,:))).^beta);
            if isempty(prob)
                break % 无可选路径
            end

            % 选择下一个位置
            prob = prob/sum(prob);
            next_pos = available_pos(:,randsrc(1,1, [1:length(prob); prob]));
            ant_location(:,ant) = next_pos; % 更新位置
            ant_route = [ant_route,next_pos]; % 记录路径

            % 到达终点
            if next_pos == [9;10]
                break
            end
        end
    end

    % 更新信息素
    delta_pheromone = zeros(size(map));
    for ant = 1:ant_num
        for i = 1:size(ant_route,2)-1
            delta_pheromone(ant_route(1,i), ant_route(2,i)) = delta_pheromone(ant_route(1,i), ant_route(2,i)) + Q/sum(map(sub2ind(size(map),ant_location(1,:),ant_location(2,:)))==i);
        end
    end
    pheromone = (1-rho)*pheromone + delta_pheromone;
end

% 找到最优路径
optimized_route = [];
for ant = 1:ant_num
    if ant_location(:,ant) == [9;10]
        optimized_route = ant_route(:,1:length(find(ant_location(1,:)==9))); % 找到终点的路径
    end
end

% 绘制地图和路径
figure;
imagesc(map); colormap(gray); hold on;
plot(optimized_route(2,:),optimized_route(1,:),'r');