【PSO三维路径规划】粒子群算法无人机三维路径规划【含Matlab源码 192期】

✅博主简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，Matlab项目合作可私信。
🍎个人主页：海神之光
🏆代码获取方式：
海神之光Matlab王者学习之路—代码获取方式
⛳️座右铭：行百里者，半于九十。

更多Matlab仿真内容点击👇
Matlab图像处理（进阶版）
路径规划（Matlab）
神经网络预测与分类（Matlab）
优化求解（Matlab）
语音处理（Matlab）
信号处理（Matlab）
车间调度（Matlab）

⛄一、无人机简介

无人机的航迹规划是指在综合考虑无人机飞行油耗、威胁、飞行区域以及自身物理条件限制等因素的前提下, 为飞行器在飞行区域内规划出从初始点到目标点最优或者满意的飞行航迹, 其本质是一个多约束的目标优化问题。航迹规划算法是航迹规划的核心。国内外相继开展了相关研究, 提出了许多航迹规划算法, 如模拟退火算法、人工势场法、遗传算法、蚁群算法等。但由于无人机面临的规划空间异常复杂、规划约束条件多且模糊性大, 航迹搜索算法存在寻优能力差、计算量过大、效率不高等问题, 在航迹规划的最优性和实时性方面有待进一步提高。

粒子群优化算法 (particle swarm optimization, PSO)是Kennedy和Eberhart于1995年提出的一种群体智能仿生算法, 在解决一些典型的函数优化问题时, 能够取得比较好的优化结果。

1 无人机航迹规划模型
1.1 航迹表示方法
一般地, 无人机航迹规划的空间可以表示为某三维坐标系下所有点的集合{ (x, y, z) |xmin≤x≤xmax, ymin≤y≤ymax, zmin≤z≤zmax}, 其中x, y可以表示为该节点在飞行水平面下的坐标, 也可以表示为该点的经纬度, z为高程数据或海拔高度。航迹规划的目的是获得无人机在该空间中的飞行轨迹, 生成的航迹可表示为三维空间的一系列的点{PS, P1, P2, …, Pn-2, PG}, 相邻航迹点之间用直线段连接。

1.2 航迹代价函数
在航迹规划中, 常采用经过适当简化的航迹代价计算公式

式中, s表示航迹段数, Li表示第i段航迹长度, 该项代表距离代价。Hi表示第i段航迹的平均海拔高度, 该项代表高度代价。Ti为第i段航迹的威胁指数, 该项代表威胁代价。k1、k2、k3分别是距离代价、高度代价和威胁代价的权重值, 权重的选取与飞行任务要求相关。

2 基本粒子群算法
粒子群算法初始化为一群数量为N的随机粒子 (随机解) , 在D维空间中通过重复迭代、更新自身的位置以搜索适应度值最优解。粒子的位置代表被优化问题在搜索空间中的潜在解。在每次迭代中, 粒子通过跟踪2个“极值”来更新自己的速度和位置:一个是粒子自身目前所找到的最优解, 即个体极值;另一个是整个粒子群目前找到的最优解, 即全局极值。粒子i (i=1, 2, …, N) 在第j (j=1, 2, …, D) 维的速度vij和位置xij按如下格式更新:

式中, ω为非负数, 称为惯性权值 (惯性因子) , 描述了粒子对之前速度的“继承”, 即体现出粒子的“惯性”;c1和c2为非负常数, 称为学习因子 (加速因子) , 体现了粒子的社会性, 即粒子向全局最优粒子学习的特性;r1和r2为 (0, 1) 之间的随机数;pi= (pi1, pi2, …, pi D) 表示粒子i的个体极值所在位置;pg= (pg1, pg2, …, pg D) 表示所有粒子的全局极值所在位置。

速度更新公式的第一项, 反映粒子当前速度的影响, 每一个粒子按照惯性权值的比重沿着自身速度的方向搜索, 起到了平衡全局的作用, 同时避免算法陷入局部最优;第二项体现了个体最优值对粒子速度的影响, 即粒子本身的记忆和认识, 使得粒子具有全局搜索能力。第三项则反映群体对个体的影响, 即群体间的信息共享起到加速收敛的作用。

⛄二、部分源代码

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);

%% 循环
for iter = 1:N
for i = 1:M
% 更新速度
particles(i).v.x = wparticles(i).v.x …
+ c1rand([1,pointNum]).(particles(i).Best.pos.x-particles(i).pos.x) …
+ c2rand([1,pointNum]).(GlobalBest.pos.x-particles(i).pos.x);
particles(i).v.y = wparticles(i).v.y …
+ c1rand([1,pointNum]).(particles(i).Best.pos.y-particles(i).pos.y) …
+ c2rand([1,pointNum]).(GlobalBest.pos.y-particles(i).pos.y);
particles(i).v.z = wparticles(i).v.z …
+ c1rand([1,pointNum]).(particles(i).Best.pos.z-particles(i).pos.z) …
+ c2rand([1,pointNum]).*(GlobalBest.pos.z-particles(i).pos.z);

    % 判断是否位于速度界限以内
    particles(i).v.x = min(particles(i).v.x, velBound(1,2));
    particles(i).v.x = max(particles(i).v.x, velBound(1,1));
    particles(i).v.y = min(particles(i).v.y, velBound(2,2));
    particles(i).v.y = max(particles(i).v.y, velBound(2,1));
    particles(i).v.z = min(particles(i).v.z, velBound(3,2));
    particles(i).v.z = max(particles(i).v.z, velBound(3,1));
    
    % 更新粒子位置
   
        % 否则，表明可以选择此路径
        particles(i).fitness = fitness;
        particles(i).path = path;
    end
    
    % 更新个体粒子最优
    if (particles(i).fitness(1) < particles(i).Best.fitness(1))&(particles(i).fitness(2) < particles(i).Best.fitness(2))
        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
end


% 把每一代的最优粒子赋值给fitness_beat_iters
fitness_beat_iters(iter,1) = GlobalBest.fitness(1);
fitness_beat_iters(iter,2) = GlobalBest.fitness(2);

% 在命令行窗口显示每一代的信息
disp(['第' num2str(iter) '代:' '最优适应度1 = ' num2str(fitness_beat_iters(iter,1))  '最优适应度2 = ' num2str(fitness_beat_iters(iter,2))]);

% 画图
plotFigure(startPos,goalPos,X,Y,Z,GlobalBest);
pause(0.001);

end

%% 结果展示
% 理论最小适应度：直线距离
fitness_best = norm(startPos - goalPos);
disp([ '理论最优适应度 = ’ num2str(fitness_best)]);

% 画适应度迭代图
figure
plot(fitness_beat_iters(:,1),‘LineWidth’,2);
xlabel(‘迭代次数’);
ylabel(‘最优适应度1’);

figure
plot(fitness_beat_iters(:,2),‘LineWidth’,2);
xlabel(‘迭代次数’);
ylabel(‘最优适应度2’);

⛄三、运行结果

⛄四、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1]方群,徐青.基于改进粒子群算法的无人机三维航迹规划[J].西北工业大学学报. 2017,35(01)

3 备注
简介此部分摘自互联网，仅供参考，若侵权，联系删除

🍅 仿真咨询
1 各类智能优化算法改进及应用
生产调度、经济调度、装配线调度、充电优化、车间调度、发车优化、水库调度、三维装箱、物流选址、货位优化、公交排班优化、充电桩布局优化、车间布局优化、集装箱船配载优化、水泵组合优化、解医疗资源分配优化、设施布局优化、可视域基站和无人机选址优化

2 机器学习和深度学习方面
卷积神经网络（CNN）、LSTM、支持向量机（SVM）、最小二乘支持向量机（LSSVM）、极限学习机（ELM）、核极限学习机（KELM）、BP、RBF、宽度学习、DBN、RF、RBF、DELM、XGBOOST、TCN实现风电预测、光伏预测、电池寿命预测、辐射源识别、交通流预测、负荷预测、股价预测、PM2.5浓度预测、电池健康状态预测、水体光学参数反演、NLOS信号识别、地铁停车精准预测、变压器故障诊断

3 图像处理方面
图像识别、图像分割、图像检测、图像隐藏、图像配准、图像拼接、图像融合、图像增强、图像压缩感知

4 路径规划方面
旅行商问题（TSP）、车辆路径问题（VRP、MVRP、CVRP、VRPTW等）、无人机三维路径规划、无人机协同、无人机编队、机器人路径规划、栅格地图路径规划、多式联运运输问题、车辆协同无人机路径规划、天线线性阵列分布优化、车间布局优化

5 无人机应用方面
无人机路径规划、无人机控制、无人机编队、无人机协同、无人机任务分配

6 无线传感器定位及布局方面
传感器部署优化、通信协议优化、路由优化、目标定位优化、Dv-Hop定位优化、Leach协议优化、WSN覆盖优化、组播优化、RSSI定位优化

7 信号处理方面
信号识别、信号加密、信号去噪、信号增强、雷达信号处理、信号水印嵌入提取、肌电信号、脑电信号、信号配时优化

8 电力系统方面
微电网优化、无功优化、配电网重构、储能配置

9 元胞自动机方面
交通流 人群疏散 病毒扩散 晶体生长

10 雷达方面
卡尔曼滤波跟踪、航迹关联、航迹融合