基于改進人工勢場法的無人機航跡規(guī)劃

韓 堯, 李少華

(電子科技大學(xué)航空航天學(xué)院, 四川 成都 611731)

0 引 言

至今為止,無人機已有約80多年的發(fā)展歷史,由于其隱蔽性好、造價相對低廉且不懼傷亡等優(yōu)點,受到了各國軍事單位的重視[1]。在現(xiàn)代戰(zhàn)場上已出現(xiàn)了許多基于無人機的智能化武器,利用這些武器也能實現(xiàn)遠距離攻擊,在未來軍事斗爭中無人機將扮演著十分重要的角色。除此之外,民用無人機發(fā)展迅猛且應(yīng)用十分廣泛,可用于農(nóng)業(yè)、電力、測繪等多個行業(yè)[2]。

航跡規(guī)劃是無人機任務(wù)規(guī)劃的關(guān)鍵問題之一[3],其根據(jù)環(huán)境數(shù)據(jù)、無人機機動約束和航程代價等信息,規(guī)劃出一條從起始位置到目標位置的可行航跡,同時要求飛行代價盡可能小。在軍用和民用領(lǐng)域,高質(zhì)量的規(guī)劃航跡是保證無人機安全完成任務(wù)的重要手段[4-5]。目前,路徑規(guī)劃算法主要可以分為傳統(tǒng)經(jīng)典算法和現(xiàn)代智能算法[6]。其中,傳統(tǒng)經(jīng)典算法主要有人工勢場法[7-10]、A*算法[11-12]、模擬退火算法[13]等;現(xiàn)代智能算法主要有蟻群優(yōu)化算法[14]、粒子群優(yōu)化算法[15-16]、遺傳算法[17-18]等。

為解決全局路徑規(guī)劃問題,Khatib于1986年提出人工勢場法[19],該算法簡明且涉及信息較少,具有規(guī)劃速度快、實時性較好等優(yōu)點，但也存在比較明顯的問題。首先,當(dāng)障礙物的斥力和目標點的引力在同一直線上且方向相反,無人機容易陷入局部極小點,從而出現(xiàn)震蕩或終止飛行[20-21]。其次,未引入無人機物理約束,無人機以固定步長飛行且無任何飛行角限制[22],因此規(guī)劃出的軌跡并不符合動力學(xué)模型,無人機無法按照規(guī)劃軌跡飛行。

國內(nèi)外學(xué)者對于傳統(tǒng)路徑規(guī)劃的局部極小點問題進行了比較深入的研究。文獻[23]在遇到局部極小點時通過虛設(shè)障礙物來改變勢場合力,進而使受控對象離開局部極小點;文獻[24]利用模糊人工勢場法對勢場參數(shù)做出模糊決策,但該決策并未考慮位置信息的變化,因此仍出現(xiàn)了劇烈震蕩現(xiàn)象;文獻[25]引入了最優(yōu)控制理論,將無人機路徑規(guī)劃問題重構(gòu)為約束優(yōu)化問題并最終將其改造為最優(yōu)控制問題,該方法的不足在于仍然存在部分陡峭的轉(zhuǎn)彎角;文獻[26]將人工勢場法和模擬退火法相結(jié)合,在無人機陷入威脅區(qū)時,引入懲罰勢函數(shù)代替?zhèn)鹘y(tǒng)斥力勢函數(shù),并采用模擬退火法來搜索路徑,有效避免了局部最小點和震蕩點,但引入太多信息導(dǎo)致計算量增加。文獻[27]為了克服動態(tài)環(huán)境路徑規(guī)劃的震蕩問題,使用快速探索的隨機樹(rapid-exploration random tree,RRT)來提供隨機干擾,以減少振蕩并在更短的時間內(nèi)達到目標。根據(jù)仿真結(jié)果,所提出的算法發(fā)現(xiàn)與人工勢場法方法相比,找到更優(yōu)路徑的時間成本減少了約27%,并且發(fā)生碰撞的情況有了大幅下降。

本文首先在傳統(tǒng)人工勢場(traditional artificial potential field, TAPF)算法的基礎(chǔ)上,引入角度與速度參量及相應(yīng)的參量調(diào)節(jié)算法,解決了規(guī)劃軌跡不可行的問題,同時由于加入角度限制,也避免了震蕩情況。其次，提出一種改進人工勢場(improved artificial potential field, IAPF)算法,施加輔助避障力,提前改變航向角,使無人機避開局部極小點的同時進一步平滑路徑。最后，通過Matlab仿真驗證算法可行性,并與其他算法對比，證明本算法的有效性。

1 人工勢場法

TAPF算法假設(shè)無人機在虛擬力場下運動;力場包括引力場和斥力場,引力場吸引無人機飛向目標點,斥力場使無人機避開障礙物,引力與斥力的合力引導(dǎo)無人機從起點飛向終點的同時避開航跡中的所有障礙物[28]。

1.1 引力場

常用的引力勢場函數(shù)如下:

(1)

式中:Ka是引力勢場尺度因子;d(p,pgoal)為無人機當(dāng)前位置與目標點之間的距離,假定無人機在特定高度飛行,令p=(x,y)為當(dāng)前位置坐標,pgoal=(xgoal,ygoal)為目標點坐標。引力為引力勢場的負梯度,可表示為

(2)

引力方向θatt是從無人機當(dāng)前位置指向目標點,如圖1所示。

圖1 TAPF算法方法的合力

1.2 斥力場

Khatib提出的常規(guī)斥力場函數(shù)如下:

(3)

式中:i表示第i個障礙物附近的斥力勢場;Kr是斥力尺度因子;do是障礙物的斥力有效范圍半徑;d(p,poi)是無人機與第i個障礙物之間的距離,poi=(xoi,yoi)為當(dāng)前障礙物坐標。為了解決目標不可達問題[29],常用的斥力場函數(shù)如下:

(4)

該斥力場函數(shù)基于常規(guī)的斥力場函數(shù)進行了改進,引入了因子dn(p,pgoal),在目標和障礙物距離較近的情況,可以避免斥力太大而繞開目標的情況,本文將采用改進的斥力場函數(shù)。同理,斥力函數(shù)如下:

(5)

(6)

(7)

當(dāng)計算出地圖中所有障礙物的斥力后,最終的斥力合力即為所有斥力矢量之和。

(8)

式中:n為地圖中障礙物數(shù)量。如圖1所示,無人機在整個勢場中受到障礙物斥力和目標引力組成的合力是Fres。

2 IAPF算法

為了便于實際航跡規(guī)劃,將地圖信息中的所有障礙物作簡化處理,將其統(tǒng)一建模為圓形,圓的半徑ro根據(jù)實際障礙物大小而定。要求建模后的圓形障礙物必須將實際障礙物覆蓋,為了航跡規(guī)劃的安全性,也可以將圓形障礙物的半徑適當(dāng)增大,同時保證障礙物的斥力有效范圍do必須大于ro。除此之外,假定無人機在特定高度飛行。

2.1 無人機物理約束

為解決TAPF算法規(guī)劃軌跡不符合實際飛行軌跡的問題,引入無人機物理約束。

首先引入無人機最大轉(zhuǎn)向角Δθmax,定義理論轉(zhuǎn)向角Δθtheory如下:

(9)

(10)

其次,在TAPF算法中,無人機是按固定速度勻速飛行,為結(jié)合無人機實際飛行情況,引入速度參量。直線飛行或轉(zhuǎn)向角很小時,無人機可以全力加速至最大速度;在正常轉(zhuǎn)向時,可以調(diào)整至常規(guī)轉(zhuǎn)向速度;當(dāng)轉(zhuǎn)向角很大時,無人機需要將速度減至最小飛行速度[30]。在此基礎(chǔ)上提出一種自適應(yīng)速度調(diào)節(jié)算法,以實現(xiàn)飛行速度的最優(yōu)控制。

如圖2所示,引入兩個角度閾值θ1和θ2(θ1≤θ2<Δθmax),當(dāng)無人機實際轉(zhuǎn)向角|Δθprac|小于θ1時,無人機可以全力加速至最大速度Vmax;當(dāng)|Δθprac|大于θ2時,無人機需要減速至最小速度;當(dāng)|Δθprac|介于θ1和θ2之間時,速度進行線性調(diào)節(jié)。

圖2 角度閾值

V=

(11)

2.2 輔助避障力

(12)

引入障礙物差值角Δθobs來判定探測到障礙物時轉(zhuǎn)向的方向,判定原則是更小的轉(zhuǎn)向角代價。令無人機指向障礙物圓心的角度為θobs,定義障礙物差值角Δθobs:

(13)

圖3 輔助避障力

(14)

(15)

(16)

(17)

3 航跡規(guī)劃策略

基于IAPF算法的航跡規(guī)劃流程如圖4所示,其基本步驟如下。

圖4 航跡規(guī)劃策略

步驟 1初始化算法參數(shù)。根據(jù)讀取的地圖信息設(shè)置起始位置pstart、目標位置pgoal、障礙物坐標Xobs等基本信息;除此之外,根據(jù)需求設(shè)定角度限制算法的最大轉(zhuǎn)向角Δθmax和速度調(diào)節(jié)算法的角度閾值θ1和θ2。

步驟 2計算當(dāng)前位置p處所受的目標點引力Fatt以及Xobs中所有障礙物的斥力Frep,并將二者組合得到合力Fres,得到理論轉(zhuǎn)向角Δθtheory。

步驟 5計算引入輔助避障力Faux后的最終合力F以及新的理論轉(zhuǎn)向角Δθtheory。

步驟 7計算無人機下一時刻的飛行位置,如果已到達終點pgoal則結(jié)束,否則返回步驟2。

4 仿真對比分析

以上述算法分析為理論基礎(chǔ),本文在處理器為IntelCore i3-7100T,主頻為3.40 GHz,內(nèi)存為8 GHz的計算機上通過Matlab進行仿真研究,驗證了算法的可行性,并比較了TAPF算法、文獻[31]的A-APF算法與引入物理約束調(diào)節(jié)算法和探測避障算法的IAPF算法的差異。

將無人機視為一個質(zhì)點,黑色實心圓代表區(qū)域中的障礙物,加號“+”代表起始點,倒三角“▽”代表目標點。主要參數(shù)設(shè)置如表1所示。由于TAPF算法通常假定無人機勻速飛行,故設(shè)TAPF算法中無人機飛行速度為常規(guī)速度Vc,IAPF算法針對TAPF算法的問題所作改進如圖5和圖6所示。圖5中由于障礙物恰好位于無人機與目標點的連線上,所以障礙物產(chǎn)生的斥力與目標點引力反向。TAPF算法中無人機在靠近障礙物附近陷入局部極小點產(chǎn)生震蕩,無法到達目標點;IAPF算法在探測距離ddet內(nèi)發(fā)現(xiàn)障礙物時,就會受到輔助避障力的作用開始轉(zhuǎn)向以提前避開障礙物,所以無人機可以避開震蕩點。同時,可以發(fā)現(xiàn)雖然A-APF算法和IAPF算法均能解決局部極小點問題,但IAPF算法的軌跡更為平滑可行。

表1 主要參數(shù)

圖5 局部極小點

圖6 改進算法與傳統(tǒng)算法的對比

在圖6(a)中可以觀察到,由于TAPF算法是無人機進入斥力勢場后再受到斥力作用開始避開障礙物,同時斥力隨距離變化較大,所以無人機飛行軌跡不夠平滑;此外,由圖6(b)可知,該軌跡上無人機最大轉(zhuǎn)向角為-303.585 4°,航跡總長度為55.2,飛行時間為69 s,顯然無人機的最大轉(zhuǎn)向角不符合實際飛行狀態(tài)。而在IAPF算法中,無人機受到的輔助避障力根據(jù)飛行情況動態(tài)調(diào)整,使無人機與斥力勢場相切飛過,避免無人機受到斥力勢場影響而導(dǎo)致無人機軌跡擺動變大,并且在繞過障礙物后,輔助避障力消失,無人機繼續(xù)朝著目標點飛行,在此軌跡上無人機的最大轉(zhuǎn)向角為18.482 4°,航跡總長度為49.2,飛行時間為50 s,除了使最大轉(zhuǎn)向角符合無人機實際飛行狀態(tài)之外,在航跡長度和飛行時間上都有一定的改善。

為了進一步對IAPF算法的效果進行分析,將建模區(qū)域提升到80×80,在區(qū)域內(nèi)隨機產(chǎn)生多個障礙物,將起點設(shè)置在(75,75),終點設(shè)置在(2,2),進行多次仿真實驗并記錄仿真數(shù)據(jù),相同地圖情況下3種算法的對比仿真結(jié)果以圖7為例。

圖7 多障礙物情況

從圖7可看出,TAPF算法和A-APF算法的最大轉(zhuǎn)向角均超過300°,IAPF算法的最大轉(zhuǎn)向角為23.23°,IAPF算法在多障礙物的情況下能夠成功避開障礙物且規(guī)劃出的軌跡仍較為平滑。此外,TAPF算法、A-APF算法和IAPF算法的軌跡長度分別為120.8、111.6和107.2,IAPF算法航程代價最小。TAPF算法、A-APF算法和IAPF算法的飛行時間分別為151 s、124 s、117 s。由于TAPF算法和A-APF算法未引入速度參量,故只能設(shè)定為常規(guī)飛行速度勻速飛行,但實際飛行時,在直線段內(nèi)無人機是可以按照最大飛行速度進行飛行,IAPF算法考慮了該點,故其時間代價更小。

多次實驗的仿真數(shù)據(jù)如表2所示。

表2 隨機多障礙物地圖下算法仿真結(jié)果比較

由于引入了角度限制,可以發(fā)現(xiàn)IAPF算法的最大轉(zhuǎn)向角都限制在了30°,而其他算法的最大轉(zhuǎn)向角都遠大于IAPF算法;在少數(shù)情況下,IAPF算法的飛行時間大于A-APF算法,但差距不大。除仿真實驗中TAPF算法的目標不可達情況,IAPF算法相對于TAPF算法規(guī)劃航跡長度減少9.63%,飛行時間減少19.1%,相對于A-APAF算法規(guī)劃軌跡長度減少2.52%,飛行時間減少1.86%。

5 結(jié) 論

TAPF算法進行航跡規(guī)劃存在航跡不符合實際飛行軌跡和局部極小值的問題,本文引入角度限制和速度自適應(yīng)調(diào)節(jié)方案,一定程度上平滑了軌跡,減輕了TAPF算法規(guī)劃航跡的擺動問題;提出輔助避障力來實現(xiàn)提前避障,使無人機可以在還未到斥力勢場范圍內(nèi)時便開始轉(zhuǎn)向,以避開局部極小點的同時進一步平滑軌跡。Matlab仿真實驗結(jié)果表明,IAPF算法成功地避開了局部極小點和震蕩點,并且避開了TAPF算法中的急轉(zhuǎn)彎處,使無人機以很小的轉(zhuǎn)向角代價避開障礙物;在多次相同地圖信息下進行仿真,IAPF算法相比于TAPF算法規(guī)劃航跡長度減少9.63%,飛行時間減少19.1%,相比于A-APF也有一定的改進作用,在最大轉(zhuǎn)向角上尤為突出。