【无人机三维路径规划】基于蜘黑翅鸢算法BKA实现考虑路径、高度、威胁、转角成本的多无人机协同集群避障路径规划附Matlab代码

% 初始化无人机数量和位置
num_drones = 4;
start_positions = [0, 0; 10, 0; 20, 0; 30, 0];
goal_positions = [40, 40; 30, 40; 20, 40; 10, 40];

% 参数设置
max_iter = 100; % 最大迭代次数
pop_size = 50; % 种群规模
c1 = 2; % 个体学习因子
c2 = 2; % 社会学习因子
w = 0.6; % 惯性权重
max_speed = 1; % 最大速度
min_speed = 0.1; % 最小速度
max_turn_angle = pi/4; % 最大转角
min_turn_angle = -pi/4; % 最小转角

% 初始化种群
positions = zeros(pop_size, num_drones, 2);
speeds = zeros(pop_size, num_drones, 2);
pbest_positions = positions;
gbest_position = zeros(1, num_drones, 2);
pbest_fitness = inf(1, pop_size);
gbest_fitness = inf;

% 初始化路径规划参数
path_cost_weight = 1;
height_cost_weight = 1;
threat_cost_weight = 1;
turn_cost_weight = 1;

% 开始迭代
for iter = 1:max_iter
% 更新速度和位置
for i = 1:pop_size
for j = 1:num_drones
% 计算个体学习项
pbest_diff = pbest_positions(i, j, 😃 - positions(i, j, 😃;
pbest_component = c1 * rand() * pbest_diff;

        % 计算社会学习项
        gbest_diff = gbest_position(1, j, :) - positions(i, j, :);
        gbest_component = c2 * rand() * gbest_diff;
        
        % 计算惯性项
        inertia_component = w * speeds(i, j, :);
        
        % 更新速度
        speeds(i, j, :) = inertia_component + pbest_component + gbest_component;
        
        % 限制速度在最大和最小速度之间
        speed_norm = norm(squeeze(speeds(i, j, :)));
        if speed_norm > max_speed
            speeds(i, j, :) = max_speed * speeds(i, j, :) / speed_norm;
        elseif speed_norm < min_speed
            speeds(i, j, :) = min_speed * speeds(i, j, :) / speed_norm;
        end
        
        % 更新位置
        positions(i, j, :) = positions(i, j, :) + speeds(i, j, :);
        
        % 限制位置在边界内
        positions(i, j, 1) = max(0, min(positions(i, j, 1), 50));
        positions(i, j, 2) = max(0, min(positions(i, j, 2), 50));
    end
end

% 计算适应度值并更新个体最优和全局最优
for i = 1:pop_size
    fitness = calculate_fitness(positions(i, :, :), goal_positions, path_cost_weight, height_cost_weight, threat_cost_weight, turn_cost_weight);
    
    % 更新个体最优
    if fitness < pbest_fitness(i)
        pbest_fitness(i) = fitness;
        pbest_positions(i, :, :) = positions(i, :, :);
    end
    
    % 更新全局最优
    if fitness < gbest_fitness
        gbest_fitness = fitness;
        gbest_position(1, :, :) = positions(i, :, :);
    end
end

% 输出当前迭代的最优适应度值
disp(['Iteration: ', num2str(iter), ', Best Fitness: ', num2str(gbest_fitness)]);

end

% 计算适应度函数
function fitness = calculate_fitness(positions, goal_positions, path_cost_weight, height_cost_weight, threat_cost_weight, turn_cost_weight)
num_drones = size(positions, 2);

% 初始化适应度值
fitness = 0;

% 计算路径成本
for i = 1:num_drones
    fitness = fitness + path_cost_weight * norm(squeeze(positions(i, :, :) - goal_positions(i, :, :)));
end

% 计算高度成本（假设高度为第三维）
for i = 1:num_drones
    fitness = fitness + height_cost_weight * abs(positions(i, :, 3) - goal_positions(i, :, 3));
end

% 计算威胁成本（根据具体情况进行计算）
for i = 1:num_drones
    % 在这里添加计算威胁成本的代码
    % 可以考虑与障碍物的距离、避障路径长度等因素
end

% 计算转角成本
for i = 1:num_drones
    for j = 2:size(positions, 2)
        prev_direction = positions(i, j-1, :) - positions(i, j-2, :);
        current_direction = positions(i, j, :) - positions(i, j-1, :);

        angle_diff = acos(dot(prev_direction, current_direction) / (norm(prev_direction) * norm(current_direction)));
        fitness = fitness + turn_cost_weight * angle_diff;
    end
end

end