实现多无人机航迹规划附Matlab代码)
✅作者简介：热爱科研的Matlab仿真开发者，擅长毕业设计辅导、数学建模、数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。
🍎 往期回顾关注个人主页：Matlab科研工作室
👇 关注我领取海量matlab电子书和数学建模资料
🍊个人信条：格物致知,完整Matlab代码获取及仿真咨询内容私信。
🔥 内容介绍
随着多无人机集群在灾害应急测绘、山区输电线路巡检、农林大范围植保等场景的大规模落地应用，三维空间下的多无人机协同航迹规划已经成为集群任务执行的核心技术支撑。传统人工规划二维航迹的方法，无法适配福建漳州山地、近海等复杂三维地形环境，很容易出现无人机撞山、集群碰撞、航迹重叠冲突等安全问题，严重制约多无人机集群的任务执行效率。灰狼优化算法（Grey Wolf Optimizer, GWO）作为2014年提出的新型元启发式智能优化算法，凭借参数少、收敛稳定性强、全局搜索能力均衡的核心优势，完美适配多无人机三维航迹规划这类高维多约束优化场景，能够在复杂障碍环境下快速生成满足协同约束的全局最优多机航迹，大幅提升多无人机集群的任务安全性与执行效率。
一、 多无人机三维航迹规划的场景需求与技术痛点
多无人机三维航迹规划的核心目标，是在给定的三维任务空间内，为每一架无人机生成一条从起点到目标点的可飞行航迹，同时满足地形障碍规避、无人机之间最小安全距离约束、最大航迹长度限制、最大转弯角与爬升角约束等多重约束条件，最终实现多机集群的综合任务收益最大化。在福建漳州的沿海山地巡检场景中，任务区域内同时存在海拔超过1000米的山地障碍物、高压输电塔等人工障碍物、民航禁飞区等空域约束，传统的A*算法、人工势场法等传统路径规划方法，在三维多机场景下很容易出现搜索维数爆炸、陷入局部最优陷阱的问题，无法同时兼顾多机的航迹最优性与协同安全性。
传统的多无人机航迹规划方案大多采用“先单机规划、后协同校验”的两步策略，先为每一架无人机单独规划最优航迹，再通过后续调整消除航迹之间的冲突，这种方法生成的航迹全局最优性差，调整过程中往往会导致单架无人机的航迹长度大幅增加，甚至出现无法规避障碍的无效航迹。而基于GWO算法的多无人机三维协同航迹规划框架，将所有无人机的航迹作为整体进行统一编码优化，在迭代寻优的过程中同步完成障碍规避与多机协同约束校验，直接输出全局最优的多机协同航迹，从根源上解决了传统两步规划方法的性能缺陷。
灰狼优化算法的仿生逻辑来源于灰狼种群的分层狩猎行为，将灰狼种群严格划分为α、β、δ、ω四个社会等级，α狼是种群的最高领导者，对应优化问题的全局最优解，β狼是次级领导者，对应次优解，δ狼服从α与β的指挥，对应第三级优质解，剩余的普通ω狼围绕前三类优质狼的位置进行更新，模拟灰狼种群的围捕狩猎过程。这种基于三层精英狼引导的更新机制，相比传统粒子群算法的单精英引导机制，拥有更强的全局勘探能力，迭代过程中种群多样性衰减速度更慢，非常适配多无人机三维航迹规划这类高维多约束的复杂优化场景。
⛳️ 运行结果
📣 部分代码
function IMG_Plot(solution, UAV)
%IMG_PLOT 绘图函数(需手动添加无人机）
close all;
% 解
Tracks = solution.Tracks; % 航迹们
Data = solution.Alpha_Data; % 最优航迹信息
Fitness_list = solution.Fitness_list; % 适应度曲线
Alpha_no = solution.Alpha_no; % α解序号
Beta_no = solution.Beta_no; % β解序号
Delta_no = solution.Delta_no; % γ解序号
agent_no = Alpha_no; % 要绘制的解的序号
% 航迹图
if UAV.PointDim<3
%%%%%%%%% ———— 2D仿真 ———— %%%%%%%%%
x1 = [UAV.S(1,1),Tracks{agent_no, 1}.P{1, 1}(1,:),UAV.G(1,1)];
y1 = [UAV.S(1,2),Tracks{agent_no, 1}.P{1, 1}(2,:),UAV.G(1,2)];
x2 = [UAV.S(2,1),Tracks{agent_no, 1}.P{2, 1}(1,:),UAV.G(2,1)];
y2 = [UAV.S(2,2),Tracks{agent_no, 1}.P{2, 1}(2,:),UAV.G(2,2)];
x3 = [UAV.S(3,1),Tracks{agent_no, 1}.P{3, 1}(1,:),UAV.G(3,1)];
y3 = [UAV.S(3,2),Tracks{agent_no, 1}.P{3, 1}(2,:),UAV.G(3,2)];
% add more
figure(1)
plot(x1,y1,'k',x2,y2,'k-.',x3,y3,'k--',LineWidth=1) % 修改这里
hold on
for i = 1:UAV.num
plot(UAV.S(i,1),UAV.S(i,2),'ko',LineWidth=1,MarkerSize=9)
hold on
plot(UAV.G(i,1),UAV.G(i,2),'p',color='k',LineWidth=1,MarkerSize=10)
hold on
end
for i = 1:size(UAV.Menace.radar,1)
rectangle('Position',[UAV.Menace.radar(i,1)-UAV.Menace.radar(i,3),UAV.Menace.radar(i,2)-UAV.Menace.radar(i,3),2*UAV.Menace.radar(i,3),2*UAV.Menace.radar(i,3)],'Curvature',[1,1],'EdgeColor','k','FaceColor','g')
hold on
end
for i = 1:size(UAV.Menace.other,1)
rectangle('Position',[UAV.Menace.other(i,1)-UAV.Menace.other(i,3),UAV.Menace.other(i,2)-UAV.Menace.other(i,3),2*UAV.Menace.other(i,3),2*UAV.Menace.other(i,3)],'Curvature',[1,1],'EdgeColor','k','FaceColor','c')
hold on
end
for i = 1:UAV.num
leg_str{i} = ['Track',num2str(i)];
end
leg_str{UAV.num+1} = 'Start';
leg_str{UAV.num+2} = 'End';
legend(leg_str)
grid on
axis equal
xlim([-25,900]) % 修改这里
ylim([-25,900]) % 修改这里
xlabel('x(km)')
ylabel('y(km)')
title('路径规划图')
else
%%%%%%%%% ———— 3D仿真 ———— %%%%%%%%%
x1 = [UAV.S(1,1),Tracks{agent_no, 1}.P{1, 1}(1,:),UAV.G(1,1)];
y1 = [UAV.S(1,2),Tracks{agent_no, 1}.P{1, 1}(2,:),UAV.G(1,2)];
z1 = [UAV.S(1,3),Tracks{agent_no, 1}.P{1, 1}(3,:),UAV.G(1,3)];
x2 = [UAV.S(2,1),Tracks{agent_no, 1}.P{2, 1}(1,:),UAV.G(2,1)];
y2 = [UAV.S(2,2),Tracks{agent_no, 1}.P{2, 1}(2,:),UAV.G(2,2)];
z2 = [UAV.S(2,3),Tracks{agent_no, 1}.P{2, 1}(3,:),UAV.G(2,3)];
x3 = [UAV.S(3,1),Tracks{agent_no, 1}.P{3, 1}(1,:),UAV.G(3,1)];
y3 = [UAV.S(3,2),Tracks{agent_no, 1}.P{3, 1}(2,:),UAV.G(3,2)];
z3 = [UAV.S(3,3),Tracks{agent_no, 1}.P{3, 1}(3,:),UAV.G(3,3)];
% add more
figure(1)
plot3(x1,y1,z1,'g',LineWidth=2) % 修改这里
hold on
plot3(x2,y2,z2,'r-.',LineWidth=2)
hold on
plot3(x3,y3,z3,'b:',LineWidth=2)
hold on
% add more
for i = 1:UAV.num
plot3(UAV.S(i,1),UAV.S(i,2),UAV.S(i,3),'ko',LineWidth=1.3,MarkerSize=12)
hold on
plot3(UAV.G(i,1),UAV.G(i,2),UAV.G(i,3),'p',color='k',LineWidth=1.3,MarkerSize=13)
hold on
end
for i = 1:size(UAV.Menace.radar,1)
drawsphere(UAV.Menace.radar(i,1),UAV.Menace.radar(i,2),UAV.Menace.radar(i,3),UAV.Menace.radar(i,4),true)
hold on
end
for i = 1:size(UAV.Menace.other,1)
drawsphere(UAV.Menace.other(i,1),UAV.Menace.other(i,2),UAV.Menace.other(i,3),UAV.Menace.other(i,4))
hold on
end
for i = 1:UAV.num
leg_str{i} = ['Track',num2str(i)];
end
leg_str{UAV.num+1} = 'Start';
leg_str{UAV.num+2} = 'End';
legend(leg_str)
grid on
axis square
%axis equal
xlim([-25,900]) % 修改这里
ylim([-25,900]) % 修改这里
zlim([0,25]) % 修改这里
xlabel('x(km)')
ylabel('y(km)')
zlabel('z(km)')
title('路径规划图')
end
% 适应度
figure(2)
plot(Fitness_list,'k',LineWidth=1)
grid on
xlabel('iter')
ylabel('fitness')
title('适应度曲线')
% 屏幕输出信息
fprintf('\n无人机数量：%d', UAV.num)
fprintf('\n无人机导航点个数：')
fprintf('%d, ', UAV.PointNum)
fprintf('\n无人机飞行距离：')
fprintf('%.2fkm, ', Data.L)
fprintf('\n无人机飞行时间：')
fprintf('%.2fs, ', Data.t)
fprintf('\n无人机飞行速度：')
fprintf('%.2fm/s, ', Data.L./Data.t*1e3)
fprintf('\n无人机总碰撞次数：%d', Data.c)
fprintf('\n目标函数收敛值：%.2f', Fitness_list(end))
% fprintf('\nα、β、δ 解编号：%d, %d, %d', Alpha_no, Beta_no, Delta_no)
fprintf('\n\n')
end
%% 绘制球面
function drawsphere(a,b,c,R,useSurf)
% 以(a,b,c)为球心，R为半径
if (nargin<5)
useSurf = false;
end
% 生成数据
[x,y,z] = sphere(20);
% 调整半径
x = R*x;
y = R*y;
z = R*z;
% 调整球心
x = x+a;
y = y+b;
z = z+c;
if useSurf
% 使用surf绘制
axis equal;
surf(x,y,z);
hold on
else
% 使用mesh绘制
axis equal;
mesh(x,y,z);
hold on
end
end