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

韓 堯, 李少華

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

0 引 言

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

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

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

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

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

1 人工勢場法

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

1.1 引力場

常用的引力勢場函數如下:

(1)

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

(2)

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

圖1 TAPF算法方法的合力

1.2 斥力場

Khatib提出的常規斥力場函數如下:

(3)

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

(4)

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

(5)

(6)

(7)

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

(8)

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

2 IAPF算法

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

2.1 無人機物理約束

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

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

(9)

(10)

其次,在TAPF算法中,無人機是按固定速度勻速飛行,為結合無人機實際飛行情況,引入速度參量。直線飛行或轉向角很小時,無人機可以全力加速至最大速度;在正常轉向時,可以調整至常規轉向速度;當轉向角很大時,無人機需要將速度減至最小飛行速度[30]。在此基礎上提出一種自適應速度調節算法,以實現飛行速度的最優控制。

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

圖2 角度閾值

V=

(11)

2.2 輔助避障力

(12)

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

(13)

圖3 輔助避障力

(14)

(15)

(16)

(17)

3 航跡規劃策略

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

圖4 航跡規劃策略

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

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

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

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

4 仿真對比分析

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

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

表1 主要參數

圖5 局部極小點

圖6 改進算法與傳統算法的對比

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

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

圖7 多障礙物情況

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

多次實驗的仿真數據如表2所示。

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

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

5 結 論

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