MATLAB 实现基于天牛须搜索算法（BAS）进行无人机三维路径规划
无人机（UAV, Unmanned Aerial Vehicle）技术在近年来迅猛发展，广泛应用于军事侦察、环境监测、物流配送、农业喷洒、灾害救援等多个领域。随着应用场景的复杂化和任务需求的多样化，无人机在三维空间中的路径规划变得尤为关键。路径规划不仅关系到任务的效率，更直接影响无人机的安全性和资源利用效率。传统路径规划算法如 A*、Dijkstra 算法，在二维平面内表现良好，但面对三维空间的复杂环境和多约束条件，计算复杂度剧增，且难以适应动态变化的环境。为此，智能优化算法被引入无人机路径规划领域，以提升规划的效率和鲁棒性。
天牛须搜索算法（Beetle Antennae Search, BAS）是一种新兴的群智能优化算法，受到天牛利用其触角探测环境的启发。BAS 算法结构简单，计算开销低，且在全局搜索和局部搜索间取得良好平衡，适合处理高维复杂优化问题。将 BAS 算法应用于无人机三维路径规划，旨在通过模拟天牛触角的探索机制，在三维环境中高效搜索最优或近优路径，避免传统搜索方法陷入局部最优，从而提升路径规划的智能化水平。
项目目标与意义
- 提升无人机自主路径规划能力：通过引入天牛须搜索算法，提升无人机在三维复杂环境中的路径规划能力，实现无人机自主识别障碍物并寻找最优飞行路径。
- 优化三维路径的安全性和效率：通过 BAS 算法的全局搜索能力，规划出避障合理且飞行距离最短的路径，降低碰撞风险和能耗。
- 适应动态复杂环境：设计能够适应动态环境变化的路径规划策略，确保无人机能在出现新障碍物时快速调整飞行路径。
- 降低计算资源消耗：优化 BAS 算法参数及计算流程，降低算法的计算复杂度和能耗，保证算法能在嵌入式系统中实时运行。
项目挑战及解决方案
- 三维环境建模复杂性：结合激光雷达、视觉传感器等多源数据，构建高精度三维环境模型。
- 高维搜索空间带来的计算压力：采用 BAS 算法，有效降低搜索空间复杂度，同时结合并行计算提升搜索效率。
- 避障与路径优化多目标冲突：通过设计多目标适应度函数，将安全距离、路径长度等因素加权融合。
- 动态障碍物影响路径稳定性：引入路径重规划机制，结合实时环境感知数据，利用 BAS 算法快速重新搜索替代路径。
项目模型架构
本项目模型架构主要包括环境建模模块、路径规划模块（基于 BAS 算法）、适应度函数设计模块、路径更新与重规划模块、约束条件处理模块和结果评估模块。
- 环境建模模块：利用传感器数据生成障碍物信息和空旷空间的三维网格地图。
- 路径规划模块（BAS 算法核心）：基于天牛须搜索算法实现路径优化，包含路径编码、采样点生成、左右'触角'位置计算等关键步骤。
- 适应度函数设计模块：设计多目标适应度函数，引导 BAS 算法在复杂目标间权衡。
- 路径更新与重规划模块：当环境发生变化时，触发重规划机制，调用 BAS 算法快速重新搜索最优路径。
- 约束条件处理模块：对飞行器动力学约束、高度限制等进行硬性限制。
- 结果评估模块：对规划路径进行多指标评估，提供优化性能反馈。
项目模型描述及代码示例
function [best_pos, best_fit] = BAS_3D_PathPlanning(current_pos, goal_pos, max_iter)
% 初始化参数
step_size = 0.5; % 每步搜索距离
antenna_dist = 1.0; % 天牛触角长度
dim = 3; % 三维空间维度
beta = 5.0; % 障碍物代价权重
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, beta);
fit_right = fitnessFunction(env_map, right_pos, goal_pos, beta);
% 估计梯度方向（适应度差异）
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, beta);
% 更新最优解
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, beta)
% 适应度函数融合路径距离和障碍物代价
dist_to_goal = norm(pos - goal);
obstacle_cost = obstacleDistanceCost(env_map, pos);
alpha = 1.0; % 距离权重
% 综合代价作为适应度，目标是最小化
fit = alpha * dist_to_goal + beta * obstacle_cost;
end
function cost = obstacleDistanceCost(env_map, pos)
% 计算当前位置距离最近障碍物的负距离代价
idx = round(pos);
search_radius = 5;
% 简单方法为搜索周围小范围网格
[X,Y,Z] = ndgrid((idx(1)-search_radius):(idx(1)+search_radius), ...
(idx(2)-search_radius):(idx(2)+search_radius), ...
(idx(3)-search_radius):(idx(3)+search_radius));
valid_idx = X>=1 & Y>=1 & Z>=1 & X<=size(env_map,1) & Y<=size(env_map,2) & Z<=size(env_map,3);
obstacle_points = env_map(sub2ind(size(env_map), X(valid_idx), Y(valid_idx), Z(valid_idx)));
distances = sqrt((X(valid_idx)-idx(1)).^2 + (Y(valid_idx)-idx(2)).^2 + (Z(valid_idx)-idx(3)).^2);
min_dist = inf;
if any(obstacle_points == 1)
obs_dists = distances(obstacle_points==1);
if ~isempty(obs_dists)
min_dist = min(obs_dists);
end
end
if min_dist == 0 || isinf(min_dist)
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
