本文研究在已知环境下的无人机的全局路径规划,建立模拟城市环境的三维高程数字地图模型。考虑无人机飞行安全裕度后用圆柱体模拟建筑物,用半球体模拟其他树木等障碍及禁飞区,其三维高程数学模型表示为[10,10]:

在采用粒子群算法进行路径规划时,适应度函数用以评价生成路径的优劣程度,也是算法种群迭代进化的依据,适应度函数的优劣决定着算法执行的效率与质量。为了更好地进行路径质量判断,本文综合考虑路径的长度代价、障碍危险代价以及路径平滑度几个方面来构造适应度函数。假设有C条路径,每条路径由n个点组成,环境中共存在g个球形和柱形障碍。
路径长度是评价路径优劣最重要的指标之一,路径越短,其耗时和耗能都越少。引入路径长度代价如下:

其中,Tm代表第m,m∈{1,2,…,M}条路径中所有相邻节点之间的距离总和,(xj,yj,zj)为路径中第j个节点的坐标。
引入障碍危险代价,使得离障碍过近的路径适应度差,保证最终得到的路径和障碍物保持一定距离。对于第k(k=1,2,…,g)个障碍,若其为球形障碍,求球心到各个路径段的距离lik,则路径到障碍物的最小距离为Lk=min(l1k,…,lik,…,lkn-1),若为柱形障碍,将路径和障碍投影在xoy面上,求障碍物中心到各个路径段的距离lik,则路径到障碍物的最小距离为Lk=min(l1k,…,lik,…,lkn-1)。
则第m条路径的障碍威胁代价为:

其中,rk为球形障碍或柱形障碍的半径。
无人机的稳定飞行高度也是无人机航迹规划过程中的重要环节。对于大多数飞行器来说,飞行高度不应该发生太大的变化。稳定飞行高度有助于减轻控制系统的负担,节省更多的燃料。故引入航迹高程代价:

高程代价为航迹每个相邻航迹点高度差之和,其中hj表示路径节点j的高度。
综合Tm、danm和Cm得到路径适应度函数为:

其中,w1、w2和w3为[0,1]内的权重系数,用来灵活配置Tm、danm和Cm之间的关系。
lear allclc% 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)%此函数主要是避开雷达的检测以及计算航线的距离


[1]江冰, 郭彭. 基于粒子群算法的三维无人机路径规划方法及规划系统:, CN112230678A[P]. 2021.