1 簡介

1 無人機路徑規(guī)劃環(huán)境建模

本文研究在已知環(huán)境下的無人機的全局路徑規(guī)劃，建立模擬城市環(huán)境的三維高程數(shù)字地圖模型?？紤]無人機飛行安全裕度后用圓柱體模擬建筑物，用半球體模擬其他樹木等障礙及禁飛區(qū)，其三維高程數(shù)學模型表示為[10,10]:

2 適應度函數(shù)

在采用粒子群算法進行路徑規(guī)劃時，適應度函數(shù)用以評價生成路徑的優(yōu)劣程度，也是算法種群迭代進化的依據(jù)，適應度函數(shù)的優(yōu)劣決定著算法執(zhí)行的效率與質(zhì)量。為了更好地進行路徑質(zhì)量判斷，本文綜合考慮路徑的長度代價、障礙危險代價以及路徑平滑度幾個方面來構(gòu)造適應度函數(shù)。假設有C條路徑，每條路徑由n個點組成，環(huán)境中共存在g個球形和柱形障礙。

2.1 路徑長度代價

路徑長度是評價路徑優(yōu)劣最重要的指標之一，路徑越短，其耗時和耗能都越少。引入路徑長度代價如下：

其中，Tm代表第m,m∈{1,2，…，M}條路徑中所有相鄰節(jié)點之間的距離總和，（xj,yj,zj）為路徑中第j個節(jié)點的坐標。

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 航跡高程代價

無人機的穩(wěn)定飛行高度也是無人機航跡規(guī)劃過程中的重要環(huán)節(jié)。對于大多數(shù)飛行器來說，飛行高度不應該發(fā)生太大的變化。穩(wěn)定飛行高度有助于減輕控制系統(tǒng)的負擔，節(jié)省更多的燃料。故引入航跡高程代價：

高程代價為航跡每個相鄰航跡點高度差之和，其中hj表示路徑節(jié)點j的高度。

綜合Tm、danm和Cm得到路徑適應度函數(shù)為：

其中，w1、w2和w3為[0,1]內(nèi)的權(quán)重系數(shù)，用來靈活配置Tm、danm和Cm之間的關系。

2 部分代碼

lear allclc% ? ? for o=1:4 ? ? ? tic% ? ? ? ? for u=1:50 ? ? ? ? %% 設置各參數(shù)值 ? ? ? ? ? ? ? startX=0;startY=0; ? ? ? ? ? ? ? ? ? ? ? ? ? ?%起開始坐標 ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ? endX=700;endY=700; ? ? ? ? ? ? ? ? ? ? ? ? ? ?%結(jié)束坐標 ? ? ? ? ? ? ? c1=2; ? ? ? ? ? ? ? c2=2; ? ? ? ? ? ? ? ? ? ? %學習因子 ? ? ? ? ? ? ? w=0.7; ?%慣性權(quán)數(shù) ? ? ? ? ? ? ? pop=20; ? ? ? ? ? ? ? %粒子數(shù) ? ? ? ? ? ? ? N_gen=500; ? ? ? ? ? ? ? popmax=700; ? ? ? ? ? ? ? popmin=0; ? ? ? ? ? ? ?%位置范圍，根據(jù)測試函數(shù)而定 ? ? ? ? ? ? ? Vmax=20; ? ? ? ? ? ? ? Vmin=-20; ? ? ? ? ? ? ? ? %速度范圍，根據(jù)測試函數(shù)而定 ? ? ? ? ? ? ? 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); ?%分布在速度范圍內(nèi) ? ? ? ? ? ? ? ? ? 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); ? ? ? ? ? ? ? %% 迭代取優(yōu) ? ? ? ? ? ? ? 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,:)); ?%根據(jù)公式更新速度 ? ? ? ? ? ? ? ? ? ? ? V(j,find(V(j,:)>Vmax))=Vmax; ?%限制速度大小 ? ? ? ? ? ? ? ? ? ? ? V(j,find(V(j,:)<Vmin))=Vmin; ? ? ? ? ? ? ? ? ? ? ? path(j,:)=path(j,:)+V(j,:); ?%根據(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; ?%更新個體最優(yōu)適應度 ? ? ? ? ? ? ? ? ? ? ? ? ? pbest(j,:)=path(j,:); ?%更新個體最優(yōu)值 ? ? ? ? ? ? ? ? ? ? ? end ? ? ? ? ? ? ? ? ? ? ? function [d,path,position]= verify(bestpath,threat,R,startX,startY,endX,endY,gridCount)%此函數(shù)主要是避開雷達的檢測以及計算航線的距離

3 仿真結(jié)果

4 參考文獻

[1]江冰, 郭彭. 基于粒子群算法的三維無人機路徑規(guī)劃方法及規(guī)劃系統(tǒng):, CN112230678A[P]. 2021.

博主簡介：擅長智能優(yōu)化算法、神經(jīng)網(wǎng)絡預測、信號處理、元胞自動機、圖像處理、路徑規(guī)劃、無人機等多種領域的Matlab仿真，相關matlab代碼問題可私信交流。

部分理論引用網(wǎng)絡文獻，若有侵權(quán)聯(lián)系博主刪除。