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

算法原理与无人机约束

1. 核心数学模型

信息素更新规则：

\(τ_ij(t+1) = (1-ρ)τ_ij(t) + Δτ_ij^k\)
\(Δτ_ij^k = Q / L_k\)

路径选择概率：

\(P_ij^k = [τ_ij]^α × [η_ij]^β / Σ[τ_is]^α × [η_is]^β\)
\(η_ij = 1 / (w1×d_ij + w2×T_ij + w3×H_ij)\)

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_costbest_cost = min_cost;best_path = paths{min_idx};% 保存最佳路径的历史记录best_paths_history{iter} = best_path;endbest_costs_history(iter) = best_cost;convergence_history(iter) = min(costs);% 显示当前迭代结果if mod(iter, 10) == 0fprintf('迭代 %d: 最佳代价 = %.4f\n', iter, best_cost);end
endclose(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:nzfor iy = 1:nyfor ix = 1:nxgrid_coords(idx, :) = [x_coords(ix), y_coords(iy), z_coords(iz)];idx = idx + 1;endendend% 环境信息结构体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);
endfunction 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');
endfunction threats = generate_threat_zones(map_size, grid_coords)% 生成威胁区域n_threats = 8;  % 威胁区域数量threats = cell(n_threats, 1);for i = 1:n_threatsthreat = 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.0threats{i} = threat;end% 确保起点和终点附近没有威胁start_point = [50, 50, 100];goal_point = [950, 950, 150];for i = 1:n_threatsdist_start = norm(threats{i}.center - start_point);dist_goal = norm(threats{i}.center - goal_point);if dist_start < 150threats{i}.center = start_point + [150, 150, 0] + rand(1,3) * 100;endif dist_goal < 150threats{i}.center = goal_point + [-150, -150, 0] + rand(1,3) * 100;endend
endfunction 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_nodespoint = grid_coords(i, :);total_threat = 0;for j = 1:n_threatsthreat = 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;endendthreat_values(i) = min(total_threat, 1.0);  % 归一化到[0,1]end
endfunction terrain_heights = interp_terrain_height(grid_coords, terrain)% 插值计算网格节点的地形高度n_nodes = size(grid_coords, 1);terrain_heights = zeros(n_nodes, 1);for i = 1:n_nodesterrain_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_nodesif adjacency(start_idx, i) > 0dist = norm(grid_coords(i,:) - grid_coords(start_idx,:));heuristic(start_idx, i) = heuristic(start_idx, i) + 1/(dist + 1e-6);endend% 所有节点到终点的启发信息增强for i = 1:n_nodesif adjacency(i, goal_idx) > 0dist = norm(grid_coords(goal_idx,:) - grid_coords(i,:));heuristic(i, goal_idx) = heuristic(i, goal_idx) + 1/(dist + 1e-6);endendfprintf('蚁群算法初始化完成\n');
endfunction 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_nodesif ~environment.feasible(i)continue;  % 不可行节点endfor j = 1:n_nodesif i == j || ~environment.feasible(j)continue;  % 跳过自身和不可行节点end% 计算距离dist = norm(grid_coords(j,:) - grid_coords(i,:));if dist < min_move_distance || dist > max_move_distancecontinue;  % 距离约束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_anglecontinue;  % 爬升角约束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.radiusthreat_value = threat.level * (1 - dist_to_threat/threat.radius);mid_threat = mid_threat + threat_value;endendmid_threat = min(mid_threat, 1.0);if mid_threat > 0.8continue;  % 威胁太大，不可连接end% 检查与地形的碰撞if check_collision(grid_coords(i,:), grid_coords(j,:), environment, params)continue;  % 有碰撞end% 如果通过所有检查，则建立连接adjacency(i, j) = 1;end% 显示进度if mod(i, 1000) == 0fprintf('  已处理 %d/%d 个节点\n', i, n_nodes);endend% 确保起点和终点有足够的连接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_nodesif i ~= start_idx && environment.feasible(i)dist = norm(grid_coords(i,:) - grid_coords(start_idx,:));if dist <= max_move_distanceadjacency(start_idx, i) = 1;endendendend% 如果终点连接太少，添加一些goal_connections = find(adjacency(:, goal_idx) > 0);if length(goal_connections) < 5for i = 1:n_nodesif i ~= goal_idx && environment.feasible(i)dist = norm(grid_coords(goal_idx,:) - grid_coords(i,:));if dist <= max_move_distanceadjacency(i, goal_idx) = 1;endendendend
endfunction 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_nodesfor j = 1:n_nodesif 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.radiusthreat_value = threat.level * (1 - dist_to_threat/threat.radius);mid_threat = mid_threat + threat_value;endendmid_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);endendif mod(i, 1000) == 0fprintf('  已计算 %d/%d 个节点的启发信息\n', i, n_nodes);endend
endfunction 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_distancecollision = 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.radiuscollision = true;return;endendend
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_antscurrent_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_probselected = neighbors(i);break;endend% 移动到选择的节点current_node = selected;path = [path, current_node];visited(current_node) = true;steps = steps + 1;% 如果到达终点if current_node == goal_idxbreak;endend% 存储路径paths{ant} = path;% 计算路径代价if current_node == goal_idxcosts(ant) = calculate_path_cost(path, grid_coords, environment, params);elsecosts(ant) = inf;  % 未到达终点endend% 移除无效路径valid_idx = costs < inf;paths = paths(valid_idx);costs = costs(valid_idx);if isempty(paths)paths = {[start_idx, goal_idx]};costs = inf;end
endfunction cost = calculate_path_cost(path, grid_coords, environment, params)% 计算路径代价n_segments = length(path) - 1;if n_segments < 1cost = inf;return;endtotal_cost = 0;for i = 1:n_segmentsnode1 = 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.radiusthreat_value = threat.level * (1 - dist_to_threat/threat.radius);threat_cost = threat_cost + threat_value;endendthreat_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 > 1node0 = path(i-1);point0 = grid_coords(node0, :);v1 = point1 - point0;v2 = point2 - point1;if norm(v1) > 0 && norm(v2) > 0cos_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_anglesmooth_cost = params.weight_smooth * (turn_angle - params.max_turn_angle);elsesmooth_cost = 0;endelsesmooth_cost = 0;endelsesmooth_cost = 0;end% 总段代价segment_cost = distance_cost + threat_cost + height_cost + smooth_cost;total_cost = total_cost + segment_cost;endcost = 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)-1node1 = best_path(i);node2 = best_path(i+1);if adjacency(node1, node2) > 0pheromone(node1, node2) = pheromone(node1, node2) + delta_pheromone;% 对称更新if adjacency(node2, node1) > 0pheromone(node2, node1) = pheromone(node2, node1) + delta_pheromone;endendend% 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) < 3smoothed_path = path;return;endn_nodes = length(path);smoothed_path = [path(1)];  % 起点不变i = 1;while i < n_nodes% 尝试连接当前点到更远的点found = false;for j = n_nodes:-1:i+1node1 = 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;endendif ~found% 如果不能跳过，就添加下一个节点if i + 1 <= n_nodessmoothed_path = [smoothed_path, path(i+1)];i = i + 1;elsebreak;endendend% 二次B样条平滑if length(smoothed_path) >= 3smoothed_path = bspline_smooth(smoothed_path, grid_coords, environment, params);end
endfunction 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_anglefeasible = 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_distancefeasible = 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.radiusfeasible = false;return;endendend
endfunction 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:orderidx = span - order + j;if idx >= 1 && idx <= n_pointspoint = point + N(j+1) * ctrl_points(idx, :);endendsmoothed_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);
endfunction span = find_span(n, p, u, U)% 找到节点矢量U中u所在的区间if u == U(n+2)span = n;return;endlow = p;high = n + 1;mid = floor((low + high) / 2);while u < U(mid) || u >= U(mid+1)if u < U(mid)high = mid;elselow = mid;endmid = floor((low + high) / 2);endspan = mid;
endfunction N = basis_functions(i, u, p, U)% 计算B样条基函数N = zeros(1, p+1);N(1) = 1;for j = 1:pleft = zeros(1, j+1);right = zeros(1, j+1);for k = 0:j-1left(k+1) = u - U(i+1-j+k);right(k+1) = U(i+1+k) - u;saved = 0;for r = 0:ktemp = N(r+1) / (right(r+1) + left(k-r+1));N(r+1) = saved + right(r+1) * temp;saved = left(k-r+1) * temp;endN(j+1) = saved;endend
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]);endplot(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:nyfor j = 1:nxpoint = [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.radiusthreat_value = threat_value + threat.level * (1 - dist/threat.radius);endendthreat_grid(i,j) = min(threat_value, 1.0);endendimagesc(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));endplot(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;endbarh(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);endaxis off;sgtitle('无人机航路规划环境', 'FontSize', 16, 'FontWeight', 'bold');
endfunction 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('地形', '威胁区域', '原始路径', '平滑路径', '起点', '终点');elselegend('地形', '威胁区域', '起点', '终点');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);endaxis 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));endplot(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.radiusthreat_value = threat_value + threat.level * (1 - dist_to_threat/threat.radius);endendthreat_values(i) = min(threat_value, 1.0);endplot(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');endsgtitle('路径分析', 'FontSize', 16, 'FontWeight', 'bold');
end

8. 辅助函数

function path_length = calculate_path_length(path, grid_coords)% 计算路径长度if length(path) < 2path_length = 0;return;endpath_length = 0;for i = 1:length(path)-1point1 = grid_coords(path(i), :);point2 = grid_coords(path(i+1), :);path_length = path_length + norm(point2 - point1);end
endfunction 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
endfunction [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_pointspoint = 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.radiusthreat_value = threat_value + threat.level * (1 - dist_to_threat/threat.radius);endendthreat_values(i) = min(threat_value, 1.0);endmax_threat = max(threat_values);mean_threat = mean(threat_values);
endfunction max_turn_angle = calculate_max_turn_angle(path, grid_coords)% 计算最大转弯角if length(path) < 3max_turn_angle = 0;return;endmax_turn_angle = 0;for i = 2:length(path)-1p1 = 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) > 0cos_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_anglemax_turn_angle = turn_angle;endendend
endfunction max_climb_angle = calculate_max_climb_angle(path, grid_coords)% 计算最大爬升角if length(path) < 2max_climb_angle = 0;return;endmax_climb_angle = 0;for i = 1:length(path)-1p1 = 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 > 0climb_angle = atan2d(abs(dz), dx);if climb_angle > max_climb_anglemax_climb_angle = climb_angle;endendend
endfunction 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);endfclose(fid);fprintf('报告已保存到文件: %s\n', report_filename);
end

参考代码 基于蚁群算法的无人机航路规划 www.youwenfan.com/contentcnt/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. 传感器部署：优化航路以最大化传感器覆盖