🍁🥬🕒摘要🕒🥬🍁
对于UAV三维路径规划这一优化问题而言，需要考虑多方面的因素。首先，UAV要在地图限制的区域内飞行。其次，为了令UAV安全飞行，UAV不能与地面和障碍物发生碰撞，并且与地面保持一定的安全距离。最后，UAV的飞行距离要尽可能短，以节省燃料并尽快到达目的地。
本代码是基于粒子群算法对UAV进行三维路径规划。首先需要构建三维地图。
其次，设置粒子群算法的相关参数，迭代次数为100，粒子数为50，惯性权重为1.2，学习因子c1=c2=1.5，路径节点为4个（可自行增加或减少），其中每个节点为三维坐标（x，y，z），为了使UAV轨迹光滑，利用三次样条拟合散点对路径节点进行处理。
最后，需要判断路径是否与障碍相交，若相交，则设置一个比较大的惩罚项，反之保存当前适应度和路径节点，本代码的目标函数包括路径的总长度和路径节点距离地面的高度，均为越小越好，下图为优化的路径结果图。
✨🔎⚡运行结果⚡🔎✨
💂♨️👨🎓Matlab代码👨🎓♨️💂
clc clear close all %% 三维路径规划模型定义 startPos = [40, 129, 5]; goalPos = [951, 833, 10]; % 随机定义山峰地图 mapRange = [1000,1000,120]; % 地图长、宽、高范围 [X,Y,Z] = defMap4(mapRange); %% 初始参数设置 N = 100; % 迭代次数 M = 50; % 粒子数量 pointNum = 4; % 每一个粒子包含三个位置点 w = 1.2; % 惯性权重 c1 = 1.5; % 社会权重 c2 = 1.5; % 认知权重 % 粒子位置界限 posBound = [[0,0,10]',[1000,1000,60]']; % 粒子速度界限 alpha = 0.1; velBound(:,2) = alpha*(posBound(:,2) - posBound(:,1)); velBound(:,1) = -velBound(:,2); % velBound(3,1)=-4; % velBound(3,2)=4; %% 种群初始化 % 初始化一个空的粒子结构体 particles.pos= []; particles.v = []; particles.fitness = []; particles.path = []; particles.Best.pos = []; particles.Best.fitness = []; particles.Best.path = []; % 定义M个粒子的结构体 particles = repmat(particles,M,1); % 初始化每一代的最优粒子 GlobalBest.fitness = [inf,inf]; % 第一代的个体粒子初始化 for i = 1:M % 粒子按照正态分布随机生成 particles(i).pos.x = unifrnd(posBound(1,1),posBound(1,2),1,pointNum); particles(i).pos.x=sort(particles(i).pos.x); particles(i).pos.y = unifrnd(posBound(2,1),posBound(2,2),1,pointNum); particles(i).pos.y=sort(particles(i).pos.y); particles(i).pos.z = unifrnd(posBound(3,1),posBound(3,2),1,pointNum); %particles(i).pos.z=sort(particles(i).pos.z); % 初始化速度 % particles(i).v.x = zeros(1, pointNum); % particles(i).v.y = zeros(1, pointNum); % particles(i).v.z = zeros(1, pointNum); particles(i).v.x=unifrnd(velBound(1,1),velBound(1,2),1,pointNum); particles(i).v.y=unifrnd(velBound(2,1),velBound(2,2),1,pointNum); particles(i).v.z=unifrnd(velBound(3,1),velBound(3,2),1,pointNum); % 适应度 [flag,fitness,path] = calFitness(startPos, goalPos,X,Y,Z, particles(i).pos); % 碰撞检测判断 if flag == 1 % 若flag=1，表明此路径将与障碍物相交，则增大适应度值 particles(i).fitness = 1000*fitness; particles(i).path = path; else % 否则，表明可以选择此路径 particles(i).fitness = fitness; particles(i).path = path; end % 更新个体粒子的最优 particles(i).Best.pos = particles(i).pos; particles(i).Best.fitness = particles(i).fitness; particles(i).Best.path = particles(i).path; % 更新全局最优 if (particles(i).Best.fitness(1) < GlobalBest.fitness(1))&(particles(i).Best.fitness(2) < GlobalBest.fitness(2)) GlobalBest = particles(i).Best; end end % 初始化每一代的最优适应度，用于画适应度迭代图 fitness_beat_iters = zeros(N,2);
📜📢🌈参考文献🌈📢📜
[1]孙雪莹,易军凯.无人机三维路径规划的粒子群混合算法[J/OL].电讯技术:1-12[2022-11-15].http://kns.cnki.net/kcms/detail/51.1267.TN.20220307.1837.004.html
