【无人机三维路径规划】基于粒子群算法无人机山地三维路径规划含Matlab源码
【无人机三维路径规划】基于粒子群算法无人机山地三维路径规划含Matlab源码
TT_Matlab
博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，完整matlab代码或者程序定制加qq1575304183。
1 简介
1 无人机路径规划环境建模
本文研究在已知环境下的无人机的全局路径规划，建立模拟城市环境的三维高程数字地图模型。考虑无人机飞行安全裕度后用圆柱体模拟建筑物，用半球体模拟其他树木等障碍及禁飞区，其三维高程数学模型表示为[ 10 , 10 ]:
2 适应度函数
在采用粒子群算法进行路径规划时，适应度函数用以评价生成路径的优劣程度，也是算法种群迭代进化的依据，适应度函数的优劣决定着算法执行的效率与质量。为了更好地进行路径质量判断，本文综合考虑路径的长度代价、障碍危险代价以及路径平滑度几个方面来构造适应度函数。假设有C条路径，每条路径由n个点组成，环境中共存在g个球形和柱形障碍。
2.1 路径长度代价
路径长度是评价路径优劣最重要的指标之一，路径越短，其耗时和耗能都越少。引入路径长度代价如下：
其中，Tm代表第m,m∈{1,2，…，M}条路径中所有相邻节点之间的距离总和，（xj,yj,zj）为路径中第j个节点的坐标。
2.2 障碍危险代价
引入障碍危险代价，使得离障碍过近的路径适应度差，保证最终得到的路径和障碍物保持一定距离。对于第k(k=1,2，…，g）个障碍，若其为球形障碍，求球心到各个路径段的距离lik，则路径到障碍物的最小距离为Lk=min(l1k，…，lik，…，lkn-1），若为柱形障碍，将路径和障碍投影在xoy面上，求障碍物中心到各个路径段的距离lik，则路径到障碍物的最小距离为Lk=min(l1k，…，lik，…，lkn-1）。
则第m条路径的障碍威胁代价为：
其中，rk为球形障碍或柱形障碍的半径。
2.3 航迹高程代价
无人机的稳定飞行高度也是无人机航迹规划过程中的重要环节。对于大多数飞行器来说，飞行高度不应该发生太大的变化。稳定飞行高度有助于减轻控制系统的负担，节省更多的燃料。故引入航迹高程代价：
高程代价为航迹每个相邻航迹点高度差之和，其中hj表示路径节点j的高度。
综合Tm、danm和Cm得到路径适应度函数为：
其中，w1、w2和w3为[0,1]内的权重系数，用来灵活配置Tm、danm和Cm之间的关系。
2 部分代码
lear
all
clc
%
for o=1:4
tic
%
for u=1:50
%%
设置各参数值
startX
=
0;startY=0; %起开始坐标
endX
=
700;endY=700; %结束坐标
c1
=
2;
c2
=
2; %学习因子
w
=
0.7; %惯性权数
pop
=
20; %粒子数
N_gen
=
500;
popmax
=
700;
popmin
=
0; %位置范围，根据测试函数而定
Vmax
=
20;
Vmin
=
-20; %速度范围，根据测试函数而定
gridCount
=
30;
%%
生成山峰
threat
=
[304 400 0;404 320 0;440 500 0;279 310 0;560 220 0;172 527 0;....
194
220 0;272 522 0;350 200 0;....
650
400 0;740 250 0;540 375 0;510 600 0];
r
=
[45 50 55 10 70 65 55 25 50 30 40 40 35];
for
i=1:length(r)
figure(1)
[x,y,z]
=
sphere;
mesh(threat(i,1)+r(i)*x,threat(i,2)+r(i)*y,abs(threat(i,3)+r(i)*z));
hold
on
end
view([-30,-30,70])
%%
初始化粒子
for
i=1:pop
for
j=1:gridCount
X(i,j)
=
startX+j*(endX-startX)/(gridCount+1);
Y(i,j)
=
startY+rand()*(endY-startY);
path(i,2*j-1)
=
X(i,j);
path(i,2*j)
=
Y(i,j);
end
end
for
i=1:pop
[distance,pathpoint,positionPoint]
=
verify(path(i,:),threat,....
r,startX,startY,endX,endY,gridCount);
fitness(i)
=
distance;
V(i,
:
)=5*rands(1,gridCount*2); %分布在速度范围内
end
[bestFitness,bestindex]
=
min(fitness);
bestpath
=
path(bestindex,:);
pbest
=
path;
T
=
std(fitness);
BestFitness
=
Inf;
globalFitness
=
Inf;
pathRecord
=
zeros(1,gridCount+1); bestRecord=zeros(1,gridCount+1);
position
=
zeros(gridCount+1,2);
%%
迭代取优
for
i=1:N_gen
for
j=1:pop
V(j,
:
)=w*V(j,:)+c1*rand*(pbest(j,:)-path(j,:))+c2*rand*(bestpath-path(j,:)); %根据公式更新速度
V(j,find(V(j,
:
)>Vmax))=Vmax; %限制速度大小
V(j,find(V(j,
:
)<Vmin))=Vmin;
path(j,
:
)=path(j,:)+V(j,:); %根据公式更新位置
path(j,find(path(j,
:
)>popmax))=popmax; %限制位置大小
path(j,find(path(j,
:
)<popmin))=popmin;
[distance,pathpoint,positionPoint]
=
verify(path(j,:),threat,....
r,startX,startY,endX,endY,gridCount);
fmin
=
distance;
if
fmin<fitness(j)
fitness(j)
=
fmin; %更新个体最优适应度
pbest(j,
:
)=path(j,:); %更新个体最优值
end
function
[d,path,position]= verify(bestpath,threat,R,startX,startY,endX,endY,gridCount)
%此函数主要是避开雷达的检测以及计算航线的距离
3 仿真结果
4 参考文献
[1]江冰, 郭彭. 基于粒子群算法的三维无人机路径规划方法及规划系统:, CN112230678A[P]. 2021.
博主简介：擅长智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，相关matlab代码问题可私信交流。
部分理论引用网络文献，若有侵权联系博主删除。