基于蚁群算法的无人机三维航路规划（MATLAB实现）

算法原理与无人机约束

1. 核心数学模型

信息素更新规则：

τij(t+1)=(1−ρ)τij(t)+Δτijkτ_ij(t+1) = (1-ρ)τ_ij(t) + Δτ_ij^kτij(t+1)=(1−ρ)τij(t)+Δτijk
Δτijk=Q/LkΔτ_ij^k = Q / L_kΔτijk=Q/Lk

路径选择概率：

Pijk=[τij]α×[ηij]β/Σ[τis]α×[ηis]βP_ij^k = [τ_ij]^α × [η_ij]^β / Σ[τ_is]^α × [η_is]^βPijk=[τij]α×[ηij]β/Σ[τis]α×[ηis]β
ηij=1/(w1×dij+w2×Tij+w3×Hij)η_ij = 1 / (w1×d_ij + w2×T_ij + w3×H_ij)ηij=1/(w1×dij+w2×Tij+w3×Hij)

MATLAB实现

1. 主程序（main.m）

%% 基于蚁群算法的无人机三维航路规划
% 功能：在三维空间中规划无人机从起点到终点的最优路径
% 考虑：威胁区域、地形障碍、无人机性能约束

clear; clc; close all;
warning off;

%% 参数设置
fprintf('=== 基于蚁群算法的无人机航路规划 ===\n\n');

% 算法参数
params.ant_count = 50;          % 蚂蚁数量
params.max_iter = 100;          % 最大迭代次数
params.alpha = 1.0;            % 信息素重要程度因子
params.beta = 5.0;             % 启发函数重要程度因子
params.rho = 0.1;              % 信息素挥发系数
params.Q = 100;                % 信息素强度
params.pheromone_init = 0.1;   % 初始信息素浓度

% 无人机约束
params.max_turn_angle = 45;    % 最大转弯角度(度)
params.min_height = 50;        % 最低飞行高度(m)
params.max_height = 500;       % 最高飞行高度(m)
params.max_climb_angle = 30;   % 最大爬升角(度)
params.safe_distance = 20;     % 安全距离(m)

% 环境参数
params.map_size = [1000, 1000, 300];  % 地图尺寸 [x, y, z]
params.grid_resolution = 20;          % 网格分辨率(m)
params.start_point = [50, 50, 100];   % 起点 [x, y, z]
params.goal_point = [950, 950, 150];  % 终点 [x, y, z]

% 权重参数
params.weight_distance = 1.0;         % 距离权重
params.weight_threat = 2.0;           % 威胁权重
params.weight_height = 0.5;           % 高度权重
params.weight_smooth = 0.3;           % 平滑度权重

%% 环境初始化
fprintf('初始化环境...\n');
[environment, grid_coords] = initialize_environment(params);

% 显示环境
visualize_environment(environment, params);

%% 初始化蚂蚁算法
fprintf('初始化蚁群算法...\n');
[pheromone, heuristic, adjacency] = initialize_aco(environment, params, grid_coords);

%% 蚁群算法主循环
fprintf('开始蚁群算法优化...\n');

best_path = [];
best_cost = inf;
best_paths_history = cell(params.max_iter, 1);
best_costs_history = zeros(params.max_iter, 1);
convergence_history = zeros(params.max_iter, 1);

% 创建进度条
h = waitbar(0, '蚁群算法迭代中...');

for iter = 1:params.max_iter
    % 进度更新
    waitbar(iter/params.max_iter, h, sprintf('迭代进度: %d/%d', iter, params.max_iter));
    
    % 蚂蚁寻路
    [paths, costs] = ant_colony_optimization(environment, params, grid_coords, ...
                                            pheromone, heuristic, adjacency, iter);
    
    % 更新信息素
    pheromone = update_pheromone(pheromone, paths, costs, params, environment, adjacency);
    
    % 记录最佳路径
    [min_cost, min_idx] = min(costs);
    if min_cost < best_cost
        best_cost = min_cost;
        best_path = paths{min_idx};
        
        % 保存最佳路径的历史记录
        best_paths_history{iter} = best_path;
    end
    best_costs_history(iter) = best_cost;
    convergence_history(iter) = min(costs);
    
    % 显示当前迭代结果
    if mod(iter, 10) == 0
        fprintf('迭代 %d: 最佳代价 = %.4f\n', iter, best_cost);
    end
end

close(h);

%% 结果处理
fprintf('优化完成！\n');
fprintf('最佳路径代价: %.4f\n', best_cost);
fprintf('最佳路径长度: %.2f m\n', calculate_path_length(best_path, grid_coords));

% 平滑路径
smoothed_path = smooth_path(best_path, environment, params, grid_coords);

% 计算路径统计
path_stats = calculate_path_statistics(best_path, smoothed_path, environment, params, grid_coords);

%% 结果可视化
visualize_results(environment, params, best_path, smoothed_path, ...
                 best_costs_history, convergence_history, path_stats);

%% 保存结果
save_results(environment, params, best_path, smoothed_path, best_costs_history, path_stats);

fprintf('\n程序执行完成！\n');

2. 环境初始化（initialize_environment.m）

function [environment, grid_coords] = initialize_environment(params)
    % 初始化无人机飞行环境
    % 输入：params - 参数结构体
    % 输出：environment - 环境信息结构体
    %       grid_coords - 网格坐标
    
    % 创建网格
    x_coords = params.start_point(1):params.grid_resolution:params.map_size(1);
    y_coords = params.start_point(2):params.grid_resolution:params.map_size(2);
    z_coords = params.start_point(3):params.grid_resolution:params.map_size(3);
    
    [X, Y, Z] = meshgrid(x_coords, y_coords, z_coords);
    
    % 生成网格坐标列表
    nx = length(x_coords);
    ny = length(y_coords);
    nz = length(z_coords);
    total_nodes = nx * ny * nz;
    
    grid_coords = zeros(total_nodes, 3);
    idx = 1;
    
    for iz = 1:nz
        for iy = 1:ny
            for ix = 1:nx
                grid_coords(idx, :) = [x_coords(ix), y_coords(iy), z_coords(iz)];
                idx = idx + 1;
            end
        end
    end
    
    % 环境信息结构体
    environment = struct();
    
    % 地形生成（模拟山地地形）
    environment.terrain = generate_terrain(params.map_size, grid_coords);
    
    % 威胁区域生成
    environment.threats = generate_threat_zones(params.map_size, grid_coords);
    
    % 计算每个网格节点的威胁值
    environment.threat_values = calculate_threat_values(grid_coords, environment.threats);
    
    % 计算每个网格节点的地形高度
    environment.terrain_heights = interp_terrain_height(grid_coords, environment.terrain);
    
    % 可行区域标记（考虑最小飞行高度）
    environment.feasible = (grid_coords(:, 3) >= params.min_height) & ...
                          (grid_coords(:, 3) <= params.max_height) & ...
                          (grid_coords(:, 3) >= environment.terrain_heights + params.safe_distance);
    
    % 起点和终点索引
    [~, environment.start_idx] = min(sum((grid_coords - params.start_point).^2, 2));
    [~, environment.goal_idx] = min(sum((grid_coords - params.goal_point).^2, 2));
    
    % 更新起点和终点坐标
    params.start_point = grid_coords(environment.start_idx, :);
    params.goal_point = grid_coords(environment.goal_idx, :);
    
    fprintf('环境初始化完成:\n');
    fprintf('  网格节点数: %d\n', total_nodes);
    fprintf('  起点: [%.1f, %.1f, %.1f]\n', params.start_point);
    fprintf('  终点: [%.1f, %.1f, %.1f]\n', params.goal_point);
end

function terrain = generate_terrain(map_size, grid_coords)
    % 生成山地地形
    terrain = struct();
    
    % 使用多个正弦波叠加生成地形
    nx = length(unique(grid_coords(:, 1)));
    ny = length(unique(grid_coords(:, 2)));
    
    x = linspace(0, map_size(1), nx);
    y = linspace(0, map_size(2), ny);
    [X, Y] = meshgrid(x, y);
    
    % 生成多峰地形
    Z = zeros(size(X));
    
    % 添加几个山峰
    peaks = [
        300, 300, 100, 150;   % [x, y, 高度, 宽度]
        700, 700, 120, 200;
        500, 200, 80, 100;
        200, 600, 90, 120;
        800, 300, 110, 180;
    ];
    
    for i = 1:size(peaks, 1)
        peak_x = peaks(i, 1);
        peak_y = peaks(i, 2);
        peak_h = peaks(i, 3);
        peak_w = peaks(i, 4);
        
        dist = sqrt((X - peak_x).^2 + (Y - peak_y).^2);
        Z = Z + peak_h * exp(-(dist.^2) / (2 * peak_w^2));
    end
    
    % 添加一些随机噪声
    Z = Z + 5 * randn(size(Z));
    
    % 确保地形高度非负
    Z = max(Z, 0);
    
    terrain.X = X;
    terrain.Y = Y;
    terrain.Z = Z;
    
    % 创建插值函数
    terrain.interp_func = @(xq, yq) interp2(X, Y, Z, xq, yq, 'linear');
end

function threats = generate_threat_zones(map_size, grid_coords)
    % 生成威胁区域
    n_threats = 8;  % 威胁区域数量
    
    threats = cell(n_threats, 1);
    
    for i = 1:n_threats
        threat = struct();
        
        % 随机生成威胁中心
        threat.center = [
            rand * map_size(1) * 0.8 + map_size(1) * 0.1,  % 避免在边界
            rand * map_size(2) * 0.8 + map_size(2) * 0.1,
            rand * 100 + 50  % 高度在50-150m之间
        ];
        
        % 随机生成威胁半径
        threat.radius = rand * 100 + 50;  % 50-150m
        
        % 随机生成威胁等级
        threat.level = rand * 0.8 + 0.2;  % 0.2-1.0
        
        threats{i} = threat;
    end
    
    % 确保起点和终点附近没有威胁
    start_point = [50, 50, 100];
    goal_point = [950, 950, 150];
    
    for i = 1:n_threats
        dist_start = norm(threats{i}.center - start_point);
        dist_goal = norm(threats{i}.center - goal_point);
        
        if dist_start < 150
            threats{i}.center = start_point + [150, 150, 0] + rand(1,3) * 100;
        end
        
        if dist_goal < 150
            threats{i}.center = goal_point + [-150, -150, 0] + rand(1,3) * 100;
        end
    end
end

function threat_values = calculate_threat_values(grid_coords, threats)
    % 计算每个网格节点的威胁值
    n_nodes = size(grid_coords, 1);
    n_threats = length(threats);
    
    threat_values = zeros(n_nodes, 1);
    
    for i = 1:n_nodes
        point = grid_coords(i, :);
        total_threat = 0;
        
        for j = 1:n_threats
            threat = threats{j};
            dist = norm(point - threat.center);
            
            if dist <= threat.radius
                % 威胁值随距离衰减
                threat_value = threat.level * (1 - dist/threat.radius);
                total_threat = total_threat + threat_value;
            end
        end
        
        threat_values(i) = min(total_threat, 1.0);  % 归一化到[0,1]
    end
end

function terrain_heights = interp_terrain_height(grid_coords, terrain)
    % 插值计算网格节点的地形高度
    n_nodes = size(grid_coords, 1);
    terrain_heights = zeros(n_nodes, 1);
    
    for i = 1:n_nodes
        terrain_heights(i) = terrain.interp_func(grid_coords(i,1), grid_coords(i,2));
    end
end

3. 蚁群算法初始化（initialize_aco.m）

function [pheromone, heuristic, adjacency] = initialize_aco(environment, params, grid_coords)
    % 初始化蚁群算法
    % 输入：environment - 环境信息
    %       params - 参数
    %       grid_coords - 网格坐标
    % 输出：pheromone - 信息素矩阵
    %       heuristic - 启发信息矩阵
    %       adjacency - 邻接矩阵
    
    n_nodes = size(grid_coords, 1);
    
    % 1. 初始化信息素矩阵
    pheromone = ones(n_nodes, n_nodes) * params.pheromone_init;
    
    % 2. 构建邻接矩阵（考虑无人机约束）
    fprintf('构建邻接矩阵...\n');
    adjacency = build_adjacency_matrix(grid_coords, environment, params);
    
    % 3. 计算启发信息
    fprintf('计算启发信息...\n');
    heuristic = calculate_heuristic(grid_coords, environment, params, adjacency);
    
    % 4. 初始化起点和终点
    start_idx = environment.start_idx;
    goal_idx = environment.goal_idx;
    
    % 起点到所有节点的启发信息增强
    for i = 1:n_nodes
        if adjacency(start_idx, i) > 0
            dist = norm(grid_coords(i,:) - grid_coords(start_idx,:));
            heuristic(start_idx, i) = heuristic(start_idx, i) + 1/(dist + 1e-6);
        end
    end
    
    % 所有节点到终点的启发信息增强
    for i = 1:n_nodes
        if adjacency(i, goal_idx) > 0
            dist = norm(grid_coords(goal_idx,:) - grid_coords(i,:));
            heuristic(i, goal_idx) = heuristic(i, goal_idx) + 1/(dist + 1e-6);
        end
    end
    
    fprintf('蚁群算法初始化完成\n');
end

function adjacency = build_adjacency_matrix(grid_coords, environment, params)
    % 构建邻接矩阵，考虑无人机约束
    
    n_nodes = size(grid_coords, 1);
    adjacency = zeros(n_nodes, n_nodes);
    
    % 最大移动距离（考虑无人机速度和转向能力）
    max_move_distance = 100;  % 最大单步移动距离
    min_move_distance = 10;   % 最小单步移动距离
    
    for i = 1:n_nodes
        if ~environment.feasible(i)
            continue;  % 不可行节点
        end
        
        for j = 1:n_nodes
            if i == j || ~environment.feasible(j)
                continue;  % 跳过自身和不可行节点
            end
            
            % 计算距离
            dist = norm(grid_coords(j,:) - grid_coords(i,:));
            
            if dist < min_move_distance || dist > max_move_distance
                continue;  % 距离约束
            end
            
            % 计算高度变化
            dz = grid_coords(j,3) - grid_coords(i,3);
            climb_angle = atan2d(abs(dz), norm(grid_coords(j,1:2) - grid_coords(i,1:2)));
            
            if climb_angle > params.max_climb_angle
                continue;  % 爬升角约束
            end
            
            % 计算威胁值（路径中点）
            mid_point = (grid_coords(i,:) + grid_coords(j,:)) / 2;
            mid_threat = 0;
            
            for k = 1:length(environment.threats)
                threat = environment.threats{k};
                dist_to_threat = norm(mid_point - threat.center);
                
                if dist_to_threat <= threat.radius
                    threat_value = threat.level * (1 - dist_to_threat/threat.radius);
                    mid_threat = mid_threat + threat_value;
                end
            end
            
            mid_threat = min(mid_threat, 1.0);
            
            if mid_threat > 0.8
                continue;  % 威胁太大，不可连接
            end
            
            % 检查与地形的碰撞
            if check_collision(grid_coords(i,:), grid_coords(j,:), environment, params)
                continue;  % 有碰撞
            end
            
            % 如果通过所有检查，则建立连接
            adjacency(i, j) = 1;
        end
        
        % 显示进度
        if mod(i, 1000) == 0
            fprintf('  已处理 %d/%d 个节点\n', i, n_nodes);
        end
    end
    
    % 确保起点和终点有足够的连接
    start_idx = environment.start_idx;
    goal_idx = environment.goal_idx;
    
    % 如果起点连接太少，添加一些
    start_connections = find(adjacency(start_idx, :) > 0);
    if length(start_connections) < 5
        % 添加最近的可行节点
        for i = 1:n_nodes
            if i ~= start_idx && environment.feasible(i)
                dist = norm(grid_coords(i,:) - grid_coords(start_idx,:));
                if dist <= max_move_distance
                    adjacency(start_idx, i) = 1;
                end
            end
        end
    end
    
    % 如果终点连接太少，添加一些
    goal_connections = find(adjacency(:, goal_idx) > 0);
    if length(goal_connections) < 5
        for i = 1:n_nodes
            if i ~= goal_idx && environment.feasible(i)
                dist = norm(grid_coords(goal_idx,:) - grid_coords(i,:));
                if dist <= max_move_distance
                    adjacency(i, goal_idx) = 1;
                end
            end
        end
    end
end

function heuristic = calculate_heuristic(grid_coords, environment, params, adjacency)
    % 计算启发信息
    
    n_nodes = size(grid_coords, 1);
    heuristic = zeros(n_nodes, n_nodes);
    goal_idx = environment.goal_idx;
    
    for i = 1:n_nodes
        for j = 1:n_nodes
            if adjacency(i, j) > 0
                % 距离启发
                dist = norm(grid_coords(j,:) - grid_coords(i,:));
                dist_to_goal = norm(grid_coords(goal_idx,:) - grid_coords(j,:));
                
                % 威胁启发（路径中点威胁）
                mid_point = (grid_coords(i,:) + grid_coords(j,:)) / 2;
                mid_threat = 0;
                
                for k = 1:length(environment.threats)
                    threat = environment.threats{k};
                    dist_to_threat = norm(mid_point - threat.center);
                    
                    if dist_to_threat <= threat.radius
                        threat_value = threat.level * (1 - dist_to_threat/threat.radius);
                        mid_threat = mid_threat + threat_value;
                    end
                end
                
                mid_threat = min(mid_threat, 1.0);
                
                % 高度启发（鼓励在合适高度飞行）
                height_penalty = 0;
                ideal_height = 200;  % 理想飞行高度
                height_diff = abs(grid_coords(j,3) - ideal_height);
                height_penalty = height_diff / params.max_height;
                
                % 计算综合启发值
                heuristic(i, j) = 1 / (params.weight_distance * dist + ...
                                     params.weight_threat * mid_threat + ...
                                     params.weight_height * height_penalty + 1e-6);
            end
        end
        
        if mod(i, 1000) == 0
            fprintf('  已计算 %d/%d 个节点的启发信息\n', i, n_nodes);
        end
    end
end

function collision = check_collision(point1, point2, environment, params)
    % 检查两点间路径是否与地形碰撞
    
    collision = false;
    
    % 采样检查
    n_samples = 10;
    for t = linspace(0, 1, n_samples)
        point = point1 + t * (point2 - point1);
        
        % 检查地形高度
        terrain_height = environment.terrain.interp_func(point(1), point(2));
        
        if point(3) < terrain_height + params.safe_distance
            collision = true;
            return;
        end
        
        % 检查威胁区域
        for k = 1:length(environment.threats)
            threat = environment.threats{k};
            dist_to_threat = norm(point - threat.center);
            
            if dist_to_threat <= threat.radius
                collision = true;
                return;
            end
        end
    end
end

4. 蚁群优化主函数（ant_colony_optimization.m）

function [paths, costs] = ant_colony_optimization(environment, params, grid_coords, ...
                                                 pheromone, heuristic, adjacency, iter)
    % 蚁群优化主函数
    % 每只蚂蚁寻找路径
    
    n_ants = params.ant_count;
    n_nodes = size(grid_coords, 1);
    start_idx = environment.start_idx;
    goal_idx = environment.goal_idx;
    
    paths = cell(n_ants, 1);
    costs = zeros(n_ants, 1);
    
    % 自适应参数（随着迭代调整）
    alpha = params.alpha;
    beta = params.beta;
    
    if iter > params.max_iter * 0.7
        % 后期增加启发信息权重
        beta = beta * 1.5;
    end
    
    % 并行处理每只蚂蚁
    for ant = 1:n_ants
        current_node = start_idx;
        path = [current_node];
        visited = false(n_nodes, 1);
        visited(current_node) = true;
        
        % 最大步数限制
        max_steps = 500;
        steps = 0;
        
        while current_node ~= goal_idx && steps < max_steps
            % 获取可行邻居
            neighbors = find(adjacency(current_node, :) > 0);
            
            % 移除已访问的节点
            neighbors = neighbors(~visited(neighbors));
            
            if isempty(neighbors)
                break;  % 无路可走
            end
            
            % 计算选择概率
            probs = zeros(length(neighbors), 1);
            
            for i = 1:length(neighbors)
                j = neighbors(i);
                
                % 信息素和启发信息
                tau = pheromone(current_node, j)^alpha;
                eta = heuristic(current_node, j)^beta;
                
                probs(i) = tau * eta;
            end
            
            % 归一化概率
            probs = probs / sum(probs);
            
            % 轮盘赌选择
            r = rand;
            cum_prob = 0;
            selected = neighbors(1);  % 默认选择第一个
            
            for i = 1:length(neighbors)
                cum_prob = cum_prob + probs(i);
                if r <= cum_prob
                    selected = neighbors(i);
                    break;
                end
            end
            
            % 移动到选择的节点
            current_node = selected;
            path = [path, current_node];
            visited(current_node) = true;
            steps = steps + 1;
            
            % 如果到达终点
            if current_node == goal_idx
                break;
            end
        end
        
        % 存储路径
        paths{ant} = path;
        
        % 计算路径代价
        if current_node == goal_idx
            costs(ant) = calculate_path_cost(path, grid_coords, environment, params);
        else
            costs(ant) = inf;  % 未到达终点
        end
    end
    
    % 移除无效路径
    valid_idx = costs < inf;
    paths = paths(valid_idx);
    costs = costs(valid_idx);
    
    if isempty(paths)
        paths = {[start_idx, goal_idx]};
        costs = inf;
    end
end

function cost = calculate_path_cost(path, grid_coords, environment, params)
    % 计算路径代价
    
    n_segments = length(path) - 1;
    if n_segments < 1
        cost = inf;
        return;
    end
    
    total_cost = 0;
    
    for i = 1:n_segments
        node1 = path(i);
        node2 = path(i+1);
        
        point1 = grid_coords(node1, :);
        point2 = grid_coords(node2, :);
        
        % 距离代价
        dist = norm(point2 - point1);
        distance_cost = params.weight_distance * dist;
        
        % 威胁代价
        threat_cost = 0;
        mid_point = (point1 + point2) / 2;
        
        for k = 1:length(environment.threats)
            threat = environment.threats{k};
            dist_to_threat = norm(mid_point - threat.center);
            
            if dist_to_threat <= threat.radius
                threat_value = threat.level * (1 - dist_to_threat/threat.radius);
                threat_cost = threat_cost + threat_value;
            end
        end
        
        threat_cost = params.weight_threat * threat_cost;
        
        % 高度代价
        ideal_height = 200;
        height_diff = abs(point2(3) - ideal_height);
        height_cost = params.weight_height * height_diff / params.max_height;
        
        % 平滑度代价（转弯角）
        if i > 1
            node0 = path(i-1);
            point0 = grid_coords(node0, :);
            
            v1 = point1 - point0;
            v2 = point2 - point1;
            
            if norm(v1) > 0 && norm(v2) > 0
                cos_angle = dot(v1, v2) / (norm(v1) * norm(v2));
                cos_angle = min(max(cos_angle, -1), 1);
                turn_angle = acosd(cos_angle);
                
                if turn_angle > params.max_turn_angle
                    smooth_cost = params.weight_smooth * (turn_angle - params.max_turn_angle);
                else
                    smooth_cost = 0;
                end
            else
                smooth_cost = 0;
            end
        else
            smooth_cost = 0;
        end
        
        % 总段代价
        segment_cost = distance_cost + threat_cost + height_cost + smooth_cost;
        total_cost = total_cost + segment_cost;
    end
    
    cost = total_cost;
end

5. 信息素更新（update_pheromone.m）

function pheromone = update_pheromone(pheromone, paths, costs, params, environment, adjacency)
    % 更新信息素
    
    n_nodes = size(pheromone, 1);
    rho = params.rho;
    Q = params.Q;
    
    % 1. 信息素挥发
    pheromone = (1 - rho) * pheromone;
    
    % 2. 只对有效路径进行信息素增强
    valid_idx = costs < inf;
    if any(valid_idx)
        valid_paths = paths(valid_idx);
        valid_costs = costs(valid_idx);
        
        % 找到最佳路径
        [min_cost, min_idx] = min(valid_costs);
        best_path = valid_paths{min_idx};
        
        % 精英策略：只对最佳路径进行全局信息素增强
        delta_pheromone = Q / (min_cost + 1e-6);
        
        for i = 1:length(best_path)-1
            node1 = best_path(i);
            node2 = best_path(i+1);
            
            if adjacency(node1, node2) > 0
                pheromone(node1, node2) = pheromone(node1, node2) + delta_pheromone;
                
                % 对称更新
                if adjacency(node2, node1) > 0
                    pheromone(node2, node1) = pheromone(node2, node1) + delta_pheromone;
                end
            end
        end
        
        % 3. 信息素边界限制
        tau_max = 100;
        tau_min = 0.01;
        pheromone = min(max(pheromone, tau_min), tau_max);
    end
end

6. 路径后处理（smooth_path.m）

function smoothed_path = smooth_path(path, environment, params, grid_coords)
    % 路径平滑处理
    
    if length(path) < 3
        smoothed_path = path;
        return;
    end
    
    n_nodes = length(path);
    smoothed_path = [path(1)];  % 起点不变
    
    i = 1;
    while i < n_nodes
        % 尝试连接当前点到更远的点
        found = false;
        
        for j = n_nodes:-1:i+1
            node1 = path(i);
            node2 = path(j);
            
            point1 = grid_coords(node1, :);
            point2 = grid_coords(node2, :);
            
            % 检查直接连接是否可行
            if check_direct_connection(point1, point2, environment, params)
                smoothed_path = [smoothed_path, node2];
                i = j;
                found = true;
                break;
            end
        end
        
        if ~found
            % 如果不能跳过，就添加下一个节点
            if i + 1 <= n_nodes
                smoothed_path = [smoothed_path, path(i+1)];
                i = i + 1;
            else
                break;
            end
        end
    end
    
    % 二次B样条平滑
    if length(smoothed_path) >= 3
        smoothed_path = bspline_smooth(smoothed_path, grid_coords, environment, params);
    end
end

function feasible = check_direct_connection(point1, point2, environment, params)
    % 检查两点间直接连接是否可行
    
    feasible = true;
    
    % 距离检查
    dist = norm(point2 - point1);
    if dist > 200  % 最大直接连接距离
        feasible = false;
        return;
    end
    
    % 爬升角检查
    dz = point2(3) - point1(3);
    climb_angle = atan2d(abs(dz), norm(point2(1:2) - point1(1:2)));
    
    if climb_angle > params.max_climb_angle
        feasible = false;
        return;
    end
    
    % 碰撞检查
    n_samples = 20;
    for t = linspace(0, 1, n_samples)
        point = point1 + t * (point2 - point1);
        
        % 地形高度检查
        terrain_height = environment.terrain.interp_func(point(1), point(2));
        
        if point(3) < terrain_height + params.safe_distance
            feasible = false;
            return;
        end
        
        % 威胁检查
        for k = 1:length(environment.threats)
            threat = environment.threats{k};
            dist_to_threat = norm(point - threat.center);
            
            if dist_to_threat <= threat.radius
                feasible = false;
                return;
            end
        end
    end
end

function smoothed_path = bspline_smooth(path, grid_coords, environment, params)
    % B样条路径平滑
    
    n_points = length(path);
    points = grid_coords(path, :);
    
    % 生成B样条曲线
    t = linspace(0, 1, n_points);
    tt = linspace(0, 1, n_points * 5);
    
    % 三次B样条
    order = 3;
    
    % 控制点（使用原始路径点）
    ctrl_points = points;
    
    % 生成B样条
    smoothed_points = zeros(length(tt), 3);
    
    for i = 1:length(tt)
        u = tt(i);
        
        % 找到对应的区间
        span = find_span(n_points-1, order, u, t);
        
        % 计算基函数
        N = basis_functions(span, u, order, t);
        
        % 计算曲线点
        point = zeros(1, 3);
        for j = 0:order
            idx = span - order + j;
            if idx >= 1 && idx <= n_points
                point = point + N(j+1) * ctrl_points(idx, :);
            end
        end
        
        smoothed_points(i, :) = point;
    end
    
    % 将平滑后的点映射回最近的网格节点
    smoothed_path_nodes = zeros(size(smoothed_points, 1), 1);
    
    for i = 1:size(smoothed_points, 1)
        distances = sum((grid_coords - smoothed_points(i, :)).^2, 2);
        [~, nearest_idx] = min(distances);
        smoothed_path_nodes(i) = nearest_idx;
    end
    
    % 移除重复节点
    smoothed_path = unique(smoothed_path_nodes, 'stable')';
    
    % 确保起点和终点正确
    smoothed_path(1) = path(1);
    smoothed_path(end) = path(end);
end

function span = find_span(n, p, u, U)
    % 找到节点矢量U中u所在的区间
    if u == U(n+2)
        span = n;
        return;
    end
    
    low = p;
    high = n + 1;
    mid = floor((low + high) / 2);
    
    while u < U(mid) || u >= U(mid+1)
        if u < U(mid)
            high = mid;
        else
            low = mid;
        end
        mid = floor((low + high) / 2);
    end
    
    span = mid;
end

function N = basis_functions(i, u, p, U)
    % 计算B样条基函数
    N = zeros(1, p+1);
    N(1) = 1;
    
    for j = 1:p
        left = zeros(1, j+1);
        right = zeros(1, j+1);
        
        for k = 0:j-1
            left(k+1) = u - U(i+1-j+k);
            right(k+1) = U(i+1+k) - u;
            
            saved = 0;
            
            for r = 0:k
                temp = N(r+1) / (right(r+1) + left(k-r+1));
                N(r+1) = saved + right(r+1) * temp;
                saved = left(k-r+1) * temp;
            end
            
            N(j+1) = saved;
        end
    end
end

7. 可视化函数（visualize_*.m）

function visualize_environment(environment, params)
    % 可视化环境
    
    figure('Position', [100, 100, 1200, 900]);
    
    % 1. 地形3D图
    subplot(2, 3, 1);
    surf(environment.terrain.X, environment.terrain.Y, environment.terrain.Z, ...
         'FaceColor', [0.7, 0.5, 0.3], 'EdgeColor', 'none', 'FaceAlpha', 0.7);
    hold on;
    
    % 威胁区域
    for i = 1:length(environment.threats)
        threat = environment.threats{i};
        [x, y, z] = sphere(20);
        
        x = x * threat.radius + threat.center(1);
        y = y * threat.radius + threat.center(2);
        z = z * threat.radius + threat.center(3);
        
        surf(x, y, z, 'FaceColor', [1, 0, 0, 0.3], 'EdgeColor', 'none', 'FaceAlpha', 0.3);
    end
    
    % 起点和终点
    plot3(params.start_point(1), params.start_point(2), params.start_point(3), ...
          'g^', 'MarkerSize', 15, 'MarkerFaceColor', 'g', 'LineWidth', 2);
    plot3(params.goal_point(1), params.goal_point(2), params.goal_point(3), ...
          'r^', 'MarkerSize', 15, 'MarkerFaceColor', 'r', 'LineWidth', 2);
    
    xlabel('X (m)'); ylabel('Y (m)'); zlabel('高度 (m)');
    title('三维环境（地形+威胁区域）');
    grid on; axis equal; view(45, 30);
    legend('地形', '威胁区域', '起点', '终点');
    
    % 2. 地形等高线
    subplot(2, 3, 2);
    contourf(environment.terrain.X, environment.terrain.Y, environment.terrain.Z, 20);
    hold on;
    
    % 威胁区域投影
    for i = 1:length(environment.threats)
        threat = environment.threats{i};
        rectangle('Position', [threat.center(1)-threat.radius, threat.center(2)-threat.radius, ...
                  2*threat.radius, 2*threat.radius], ...
                  'Curvature', [1,1], 'EdgeColor', 'r', 'LineWidth', 2, 'FaceColor', [1,0,0,0.2]);
    end
    
    plot(params.start_point(1), params.start_point(2), 'g^', 'MarkerSize', 15, 'MarkerFaceColor', 'g');
    plot(params.goal_point(1), params.goal_point(2), 'r^', 'MarkerSize', 15, 'MarkerFaceColor', 'r');
    
    xlabel('X (m)'); ylabel('Y (m)');
    title('地形等高线（威胁区域投影）');
    colorbar; axis equal;
    
    % 3. 威胁值分布
    subplot(2, 3, 3);
    
    % 创建威胁值网格
    nx = size(environment.terrain.X, 2);
    ny = size(environment.terrain.Y, 1);
    threat_grid = zeros(ny, nx);
    
    for i = 1:ny
        for j = 1:nx
            point = [environment.terrain.X(i,j), environment.terrain.Y(i,j), 0];
            
            threat_value = 0;
            for k = 1:length(environment.threats)
                threat = environment.threats{k};
                dist = norm(point - threat.center);
                
                if dist <= threat.radius
                    threat_value = threat_value + threat.level * (1 - dist/threat.radius);
                end
            end
            
            threat_grid(i,j) = min(threat_value, 1.0);
        end
    end
    
    imagesc(threat_grid);
    colormap(jet);
    colorbar;
    
    % 标记起点终点
    [~, start_x] = min(abs(environment.terrain.X(1,:) - params.start_point(1)));
    [~, start_y] = min(abs(environment.terrain.Y(:,1) - params.start_point(2)));
    [~, goal_x] = min(abs(environment.terrain.X(1,:) - params.goal_point(1)));
    [~, goal_y] = min(abs(environment.terrain.Y(:,1) - params.goal_point(2)));
    
    hold on;
    plot(start_x, start_y, 'g^', 'MarkerSize', 15, 'MarkerFaceColor', 'g');
    plot(goal_x, goal_y, 'r^', 'MarkerSize', 15, 'MarkerFaceColor', 'r');
    
    title('威胁值分布图');
    xlabel('X索引'); ylabel('Y索引');
    
    % 4. 飞行高度剖面
    subplot(2, 3, 4);
    
    % 生成一条参考线
    x_line = linspace(params.start_point(1), params.goal_point(1), 100);
    y_line = linspace(params.start_point(2), params.goal_point(2), 100);
    
    terrain_height_line = zeros(size(x_line));
    for i = 1:length(x_line)
        terrain_height_line(i) = environment.terrain.interp_func(x_line(i), y_line(i));
    end
    
    plot(sqrt((x_line - x_line(1)).^2 + (y_line - y_line(1)).^2), terrain_height_line, ...
         'b-', 'LineWidth', 2);
    hold on;
    
    % 最小飞行高度线
    min_height_line = terrain_height_line + params.min_height;
    plot(sqrt((x_line - x_line(1)).^2 + (y_line - y_line(1)).^2), min_height_line, ...
         'r--', 'LineWidth', 1.5);
    
    % 最大飞行高度线
    max_height_line = ones(size(x_line)) * params.max_height;
    plot(sqrt((x_line - x_line(1)).^2 + (y_line - y_line(1)).^2), max_height_line, ...
         'g--', 'LineWidth', 1.5);
    
    xlabel('距离 (m)'); ylabel('高度 (m)');
    title('飞行高度剖面');
    legend('地形高度', '最低飞行高度', '最高飞行高度');
    grid on;
    
    % 5. 威胁区域统计
    subplot(2, 3, 5);
    
    threat_centers = zeros(length(environment.threats), 3);
    threat_radii = zeros(length(environment.threats), 1);
    threat_levels = zeros(length(environment.threats), 1);
    
    for i = 1:length(environment.threats)
        threat_centers(i, :) = environment.threats{i}.center;
        threat_radii(i) = environment.threats{i}.radius;
        threat_levels(i) = environment.threats{i}.level;
    end
    
    barh(threat_levels);
    xlabel('威胁等级');
    ylabel('威胁区域编号');
    title('威胁区域统计');
    grid on;
    
    % 6. 无人机约束
    subplot(2, 3, 6);
    
    constraints = {
        '最大转弯角', sprintf('%.1f°', params.max_turn_angle);
        '最低飞行高度', sprintf('%.1f m', params.min_height);
        '最高飞行高度', sprintf('%.1f m', params.max_height);
        '最大爬升角', sprintf('%.1f°', params.max_climb_angle);
        '安全距离', sprintf('%.1f m', params.safe_distance);
    };
    
    text(0.1, 0.9, '无人机约束条件:', 'FontSize', 12, 'FontWeight', 'bold');
    
    for i = 1:size(constraints, 1)
        text(0.1, 0.8 - i*0.1, sprintf('%s: %s', constraints{i,1}, constraints{i,2}), ...
             'FontSize', 10);
    end
    
    axis off;
    
    sgtitle('无人机航路规划环境', 'FontSize', 16, 'FontWeight', 'bold');
end

function visualize_results(environment, params, best_path, smoothed_path, ...
                          best_costs_history, convergence_history, path_stats)
    % 可视化结果
    
    figure('Position', [100, 100, 1400, 900]);
    
    % 1. 三维路径图
    subplot(2, 3, [1, 2, 4, 5]);
    
    % 绘制地形
    surf(environment.terrain.X, environment.terrain.Y, environment.terrain.Z, ...
         'FaceColor', [0.7, 0.5, 0.3], 'EdgeColor', 'none', 'FaceAlpha', 0.5);
    hold on;
    
    % 绘制威胁区域
    for i = 1:length(environment.threats)
        threat = environment.threats{i};
        [x, y, z] = sphere(20);
        
        x = x * threat.radius + threat.center(1);
        y = y * threat.radius + threat.center(2);
        z = z * threat.radius + threat.center(3);
        
        surf(x, y, z, 'FaceColor', [1, 0, 0, 0.2], 'EdgeColor', 'none', 'FaceAlpha', 0.2);
    end
    
    % 绘制最优路径
    if ~isempty(best_path)
        path_points = environment.grid_coords(best_path, :);
        plot3(path_points(:,1), path_points(:,2), path_points(:,3), ...
              'b-o', 'LineWidth', 2, 'MarkerSize', 6, 'MarkerFaceColor', 'b');
    end
    
    % 绘制平滑路径
    if ~isempty(smoothed_path)
        smoothed_points = environment.grid_coords(smoothed_path, :);
        plot3(smoothed_points(:,1), smoothed_points(:,2), smoothed_points(:,3), ...
              'g-s', 'LineWidth', 3, 'MarkerSize', 8, 'MarkerFaceColor', 'g');
    end
    
    % 起点和终点
    plot3(params.start_point(1), params.start_point(2), params.start_point(3), ...
          'g^', 'MarkerSize', 20, 'MarkerFaceColor', 'g', 'LineWidth', 2);
    plot3(params.goal_point(1), params.goal_point(2), params.goal_point(3), ...
          'r^', 'MarkerSize', 20, 'MarkerFaceColor', 'r', 'LineWidth', 2);
    
    xlabel('X (m)'); ylabel('Y (m)'); zlabel('高度 (m)');
    title('三维航路规划结果');
    grid on; axis equal; view(45, 30);
    
    if ~isempty(best_path) && ~isempty(smoothed_path)
        legend('地形', '威胁区域', '原始路径', '平滑路径', '起点', '终点');
    else
        legend('地形', '威胁区域', '起点', '终点');
    end
    
    % 2. 收敛曲线
    subplot(2, 3, 3);
    plot(1:length(best_costs_history), best_costs_history, 'b-', 'LineWidth', 2);
    xlabel('迭代次数');
    ylabel('最佳代价');
    title('算法收敛曲线');
    grid on;
    
    % 3. 路径统计
    subplot(2, 3, 6);
    
    stats_text = {
        sprintf('原始路径长度: %.2f m', path_stats.original_length);
        sprintf('平滑路径长度: %.2f m', path_stats.smoothed_length);
        sprintf('最大威胁值: %.3f', path_stats.max_threat);
        sprintf('平均威胁值: %.3f', path_stats.mean_threat);
        sprintf('最大转弯角: %.1f°', path_stats.max_turn_angle);
        sprintf('最大爬升角: %.1f°', path_stats.max_climb_angle);
        sprintf('路径节点数: %d -> %d', path_stats.original_nodes, path_stats.smoothed_nodes);
    };
    
    text(0.1, 0.9, '路径统计信息:', 'FontSize', 12, 'FontWeight', 'bold');
    
    for i = 1:length(stats_text)
        text(0.1, 0.8 - i*0.1, stats_text{i}, 'FontSize', 10);
    end
    
    axis off;
    
    % 4. 路径剖面
    figure('Position', [100, 100, 1000, 400]);
    
    % 高度剖面
    subplot(1, 2, 1);
    
    if ~isempty(smoothed_path)
        smoothed_points = environment.grid_coords(smoothed_path, :);
        
        % 计算沿路径距离
        distances = zeros(size(smoothed_points, 1), 1);
        for i = 2:size(smoothed_points, 1)
            distances(i) = distances(i-1) + norm(smoothed_points(i,:) - smoothed_points(i-1,:));
        end
        
        % 绘制高度剖面
        plot(distances, smoothed_points(:,3), 'b-', 'LineWidth', 2);
        hold on;
        
        % 绘制地形高度
        terrain_heights = zeros(size(smoothed_points, 1), 1);
        for i = 1:size(smoothed_points, 1)
            terrain_heights(i) = environment.terrain.interp_func(smoothed_points(i,1), smoothed_points(i,2));
        end
        
        plot(distances, terrain_heights, 'r-', 'LineWidth', 2);
        
        % 绘制安全边界
        plot(distances, terrain_heights + params.safe_distance, 'r--', 'LineWidth', 1);
        
        xlabel('沿路径距离 (m)');
        ylabel('高度 (m)');
        title('路径高度剖面');
        legend('飞行高度', '地形高度', '安全边界');
        grid on;
    end
    
    % 威胁值剖面
    subplot(1, 2, 2);
    
    if ~isempty(smoothed_path)
        smoothed_points = environment.grid_coords(smoothed_path, :);
        
        % 计算每个路径段的威胁值
        threat_values = zeros(size(smoothed_points, 1), 1);
        
        for i = 1:size(smoothed_points, 1)
            point = smoothed_points(i, :);
            threat_value = 0;
            
            for k = 1:length(environment.threats)
                threat = environment.threats{k};
                dist_to_threat = norm(point - threat.center);
                
                if dist_to_threat <= threat.radius
                    threat_value = threat_value + threat.level * (1 - dist_to_threat/threat.radius);
                end
            end
            
            threat_values(i) = min(threat_value, 1.0);
        end
        
        plot(distances, threat_values, 'r-', 'LineWidth', 2);
        xlabel('沿路径距离 (m)');
        ylabel('威胁值');
        title('路径威胁值剖面');
        grid on;
        
        % 添加威胁阈值线
        hold on;
        plot([distances(1), distances(end)], [0.5, 0.5], 'r--', 'LineWidth', 1);
        text(distances(end)*0.8, 0.55, '威胁阈值', 'Color', 'r');
    end
    
    sgtitle('路径分析', 'FontSize', 16, 'FontWeight', 'bold');
end

8. 辅助函数

function path_length = calculate_path_length(path, grid_coords)
    % 计算路径长度
    if length(path) < 2
        path_length = 0;
        return;
    end
    
    path_length = 0;
    for i = 1:length(path)-1
        point1 = grid_coords(path(i), :);
        point2 = grid_coords(path(i+1), :);
        path_length = path_length + norm(point2 - point1);
    end
end

function stats = calculate_path_statistics(original_path, smoothed_path, environment, params, grid_coords)
    % 计算路径统计信息
    
    stats = struct();
    
    % 原始路径统计
    if ~isempty(original_path)
        stats.original_length = calculate_path_length(original_path, grid_coords);
        stats.original_nodes = length(original_path);
        
        % 计算威胁值
        [stats.original_max_threat, stats.original_mean_threat] = ...
            calculate_path_threat(original_path, grid_coords, environment.threats);
    end
    
    % 平滑路径统计
    if ~isempty(smoothed_path)
        stats.smoothed_length = calculate_path_length(smoothed_path, grid_coords);
        stats.smoothed_nodes = length(smoothed_path);
        
        % 计算威胁值
        [stats.max_threat, stats.mean_threat] = ...
            calculate_path_threat(smoothed_path, grid_coords, environment.threats);
        
        % 计算转弯角
        stats.max_turn_angle = calculate_max_turn_angle(smoothed_path, grid_coords);
        
        % 计算爬升角
        stats.max_climb_angle = calculate_max_climb_angle(smoothed_path, grid_coords);
    end
end

function [max_threat, mean_threat] = calculate_path_threat(path, grid_coords, threats)
    % 计算路径威胁值
    n_points = length(path);
    threat_values = zeros(n_points, 1);
    
    for i = 1:n_points
        point = grid_coords(path(i), :);
        
        threat_value = 0;
        for k = 1:length(threats)
            threat = threats{k};
            dist_to_threat = norm(point - threat.center);
            
            if dist_to_threat <= threat.radius
                threat_value = threat_value + threat.level * (1 - dist_to_threat/threat.radius);
            end
        end
        
        threat_values(i) = min(threat_value, 1.0);
    end
    
    max_threat = max(threat_values);
    mean_threat = mean(threat_values);
end

function max_turn_angle = calculate_max_turn_angle(path, grid_coords)
    % 计算最大转弯角
    if length(path) < 3
        max_turn_angle = 0;
        return;
    end
    
    max_turn_angle = 0;
    
    for i = 2:length(path)-1
        p1 = grid_coords(path(i-1), :);
        p2 = grid_coords(path(i), :);
        p3 = grid_coords(path(i+1), :);
        
        v1 = p2 - p1;
        v2 = p3 - p2;
        
        if norm(v1) > 0 && norm(v2) > 0
            cos_angle = dot(v1, v2) / (norm(v1) * norm(v2));
            cos_angle = min(max(cos_angle, -1), 1);
            turn_angle = acosd(cos_angle);
            
            if turn_angle > max_turn_angle
                max_turn_angle = turn_angle;
            end
        end
    end
end

function max_climb_angle = calculate_max_climb_angle(path, grid_coords)
    % 计算最大爬升角
    if length(path) < 2
        max_climb_angle = 0;
        return;
    end
    
    max_climb_angle = 0;
    
    for i = 1:length(path)-1
        p1 = grid_coords(path(i), :);
        p2 = grid_coords(path(i+1), :);
        
        dz = p2(3) - p1(3);
        dx = norm(p2(1:2) - p1(1:2));
        
        if dx > 0
            climb_angle = atan2d(abs(dz), dx);
            
            if climb_angle > max_climb_angle
                max_climb_angle = climb_angle;
            end
        end
    end
end

function save_results(environment, params, best_path, smoothed_path, best_costs_history, path_stats)
    % 保存结果
    timestamp = datestr(now, 'yyyy-mm-dd_HH-MM-SS');
    filename = sprintf('uav_path_planning_%s.mat', timestamp);
    
    % 保存数据
    save(filename, 'environment', 'params', 'best_path', 'smoothed_path', ...
         'best_costs_history', 'path_stats');
    
    fprintf('结果已保存到文件: %s\n', filename);
    
    % 生成报告
    report_filename = sprintf('uav_path_report_%s.txt', timestamp);
    fid = fopen(report_filename, 'w');
    
    fprintf(fid, '无人机航路规划报告\n');
    fprintf(fid, '生成时间: %s\n\n', timestamp);
    fprintf(fid, '=== 参数设置 ===\n');
    fprintf(fid, '地图尺寸: [%.1f, %.1f, %.1f] m\n', params.map_size);
    fprintf(fid, '起点: [%.1f, %.1f, %.1f]\n', params.start_point);
    fprintf(fid, '终点: [%.1f, %.1f, %.1f]\n', params.goal_point);
    fprintf(fid, '蚂蚁数量: %d\n', params.ant_count);
    fprintf(fid, '最大迭代次数: %d\n', params.max_iter);
    fprintf(fid, '最大转弯角: %.1f°\n', params.max_turn_angle);
    fprintf(fid, '飞行高度范围: %.1f-%.1f m\n', params.min_height, params.max_height);
    fprintf(fid, '威胁区域数量: %d\n\n', length(environment.threats));
    
    fprintf(fid, '=== 路径统计 ===\n');
    if ~isempty(smoothed_path)
        fprintf(fid, '平滑路径长度: %.2f m\n', path_stats.smoothed_length);
        fprintf(fid, '路径节点数: %d\n', path_stats.smoothed_nodes);
        fprintf(fid, '最大威胁值: %.3f\n', path_stats.max_threat);
        fprintf(fid, '平均威胁值: %.3f\n', path_stats.mean_threat);
        fprintf(fid, '最大转弯角: %.1f°\n', path_stats.max_turn_angle);
        fprintf(fid, '最大爬升角: %.1f°\n', path_stats.max_climb_angle);
    end
    
    fclose(fid);
    fprintf('报告已保存到文件: %s\n', report_filename);
end

参考代码 基于蚁群算法的无人机航路规划 www.youwenfan.com/contentcst/45501.html

算法参数调优建议

参数	推荐范围	影响	调优建议
蚂蚁数量	20-100	探索能力 vs 计算成本	复杂环境用更多蚂蚁
信息素因子α	0.5-2.0	信息素的重要性	增大α增强正反馈
启发因子β	1.0-10.0	启发信息的重要性	增大β增强贪婪性
挥发系数ρ	0.05-0.3	信息素挥发速度	小ρ增强记忆，大ρ增强探索
信息素强度Q	10-1000	信息素增量	与路径长度成反比

应用场景扩展

1. 多无人机协同：扩展为多蚁群算法，考虑无人机间的避碰
2. 动态威胁：实时更新威胁信息，在线重规划
3. 能量优化：考虑电池消耗，优化爬升和下降策略
4. 通信中继：结合通信质量模型，优化中继节点位置
5. 传感器部署：优化航路以最大化传感器覆盖