MATLAB 实现基于天牛须搜索算法（BAS）的无人机三维路径规划

MATLAB 实现基于天牛须搜索算法（BAS）的无人机三维路径规划

无人机（UAV）技术在军事侦察、物流配送等领域应用广泛。随着场景复杂化，三维路径规划成为关键。传统算法如 A* 在二维表现良好，但在三维动态环境中计算复杂度剧增。天牛须搜索算法（BAS）结构简单，全局与局部搜索平衡性好，适合此类高维优化问题。

项目目标

本项目旨在提升无人机自主路径规划能力，优化三维路径的安全性和效率，适应动态复杂环境，降低计算资源消耗，并提供通用的三维路径规划框架。

挑战与方案

三维环境建模需结合激光雷达等多源数据；高维搜索空间采用 BAS 算法降低复杂度；多目标冲突通过加权适应度函数解决；动态障碍物引入重规划机制。

模型架构

系统包含环境建模、路径规划（BAS 核心）、适应度函数设计、路径更新与重规划、约束条件处理及结果评估模块。

代码实现

以下是核心 MATLAB 代码逻辑，展示了参数初始化、迭代搜索及适应度评估过程。

% 主函数：BAS_3D_PathPlanning
function [best_pos, best_fit] = BAS_3D_PathPlanning(env_map, goal_pos, max_iter)
    % 初始化参数
    step_size = 0.5;          % 每步搜索距离
    antenna_dist = 1.0;       % 天牛触角长度
    dim = 3;                  % 三维空间维度
    current_pos = env_map.start_pos; % 起始位置
    best_pos = current_pos;   % 记录当前最优位置
    best_fit = inf;           % 最优适应度初始化为无穷大
    path = current_pos;       % 记录路径轨迹

    for iter = 1:max_iter
        % 计算随机搜索方向（单位向量）
        dir_vec = rand(1, dim);
        dir_vec = dir_vec / norm(dir_vec);

        % 计算左右触角采样点位置
        left_pos = current_pos + antenna_dist * dir_vec;
        right_pos = current_pos - antenna_dist * dir_vec;

        % 计算适应度函数值
        fit_left = fitnessFunction(env_map, left_pos, goal_pos);
        fit_right = fitnessFunction(env_map, right_pos, goal_pos);

        % 估计梯度方向
        grad_estimate = (fit_left - fit_right) / (2 * antenna_dist);

        % 根据梯度调整当前位置
        current_pos = current_pos - step_size * grad_estimate;
        current_pos = boundPosition(current_pos, env_map);

        % 计算当前位置适应度
        current_fit = fitnessFunction(env_map, current_pos, goal_pos);

        % 更新最优解
        if current_fit < best_fit
            best_fit = current_fit;
            best_pos = current_pos;
        end
        path = [path; current_pos];
    end
end

% 适应度函数：融合路径距离和障碍物代价
function fit = fitnessFunction(env_map, pos, goal)
    dist_to_goal = norm(pos - goal);
    obstacle_cost = obstacleDistanceCost(env_map, pos);
    alpha = 1.0;      % 距离权重
    beta = 5.0;       % 障碍物代价权重
    fit = alpha * dist_to_goal + beta * obstacle_cost;
end

% 障碍物距离代价计算
function cost = obstacleDistanceCost(env_map, pos)
    idx = round(pos);
    search_radius = 5;
    % 简化版：检查周围网格是否存在障碍物
    min_dist = inf;
    if min_dist == 0
        cost = 100;
    else
        cost = 1/min_dist;
    end
end

% 边界限制
function pos_bounded = boundPosition(pos, env_map)
    pos_bounded = pos;
    for i = 1:length(pos)
        if pos(i) < 1
            pos_bounded(i) = 1;
        elseif pos(i) > size(env_map, i)
            pos_bounded(i) = size(env_map, i);
        end
    end
end

这段代码展示了基于天牛须搜索算法（BAS）实现无人机三维路径规划的核心模型。BAS_3D_PathPlanning 函数为主优化流程，包含参数初始化、随机方向采样、左右触角位置计算、适应度函数评估、位置更新和路径记录。 设计了结合路径距离和障碍物代价的综合适应度评价，确保路径既靠近目标又远离障碍。 计算当前位置与周围障碍物的距离代价，采用距离的倒数作为代价指标，越靠近障碍物代价越高。 用于限制搜索点始终在环境地图边界内，保证搜索合法性。整个模型逐步引导无人机在三维空间中通过模拟天牛触角的左右探测，寻找满足安全和效率要求的最优路径。