基于PSO的UAV三维路径规划（Matlab代码实现）

news2025/7/5 2:39:36

🍒🍒🍒欢迎关注🌈🌈🌈

📝个人主页：我爱Matlab

👍点赞➕评论➕收藏 == 养成习惯（一键三连）🌻🌻🌻

🍌希望大家多多支持🍓~一起加油 🤗

💬语录：将来的我一定会感谢现在奋斗的自己！

🍁🥬🕒摘要🕒🥬🍁

对于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