分段優化RRT的無人機動態航跡規劃算法

李文廣, 孫世宇, 李建增, 胡永江, 張 巖

(陸軍工程大學石家莊校區無人機工程系, 河北 石家莊 050003)

0 引 言

目前,大多數的航跡規劃主要研究在靜態威脅下如何規劃航跡[1-3],對于突發威脅下的動態航跡規劃問題相關文獻較少。在實戰背景下,只考慮靜態威脅對無人機飛行安全的影響是不符合實際任務要求的。無人機動態航跡規劃就是解決無人機在發現突發威脅時,該如何重新規劃局部航跡的問題。這有利于提升無人機對于復雜戰場環境的適應能力和生存能力,同時更符合實戰背景，所以研究無人機動態航跡規劃意義重大。

在無人機動態航跡規劃方面,學者們做了大量的工作:文獻[4]提出了一種改進的快速擴展隨機樹(rapidly-exploring random tree, RRT)算法,提升了算法搜索路徑的效率和可行性,路徑的可跟蹤性更強。但該算法只考慮了靜態威脅下的航跡規劃問題,不符合實戰任務要求。文獻[5]提出了稀疏A*和人工勢場相結合的動態航跡規劃算法。該算法首先利用稀疏A*算法完成全局航跡規劃,然后在出現突發威脅的情況下結合人工勢場法完成局部航跡規劃。其充分利用了稀疏A*和人工勢場法的優點,同時避免了出現局部極小的缺陷。但稀疏A*算法需要前期對規劃空間進行預處理,算法效率不高,并且用稀疏A*算法的航跡代價函數去近似描述局部航跡的代價,誤差較大。文獻[6]提出了基于動態貝葉斯網絡(dynamic Bayesian network, DBN)威脅評估模型與模型預測控制(model predictive control, MPC)路徑規劃算法相結合的方法。該算法首先利用DBN模型評估得到突發威脅的威脅等級,然后結合航跡規劃算法規劃動態航跡。其能夠解決一般不確定的突發威脅以及尾隨無人機的突發威脅的動態航跡規劃問題。但考慮空中威脅(敵機)并不是實戰條件下的主要威脅來源,實用性不強。文獻[7]提出了一種基于動態步長的實時航跡規劃算法,即無人機在飛行過程中實時規劃飛行航跡,若出現突發威脅,則通過設置子目標點的方法,規劃局部航跡。該算法將規劃空間進行了簡化處理,降低了問題的復雜度,同時提升了算法在威脅區域搜索航跡的精度。但由于整個規劃過程都是實時的,對算法的實時性要求非常高,也對無人機的智能化程度要求非常高。

上述航跡規劃算法都針對突發威脅下的動態航跡規劃問題進行了不同算法的創新和改進,但仍存在以下問題:前期全局航跡規劃效率較低,導致整體規劃效率不高;DBN威脅評估模型對于突發威脅的等級評估結果可信度不高;算法對無人機智能化程度要求太高,實用性不強。

針對以上問題,本文提出了分段優化RRT的無人機動態航跡規劃算法(dynamic path planning of unmanned aerial vehicle based on segment optimization RRT, DPPUAVSORRT)。該算法首先利用分段優化RRT算法生成全局航跡。然后在規劃空間中若有對無人機飛行安全產生影響的突發威脅,則根據新的威脅信息確定局部航跡規劃的起點和終點,并使用分段優化RRT算法生成從新起點能夠繞過突發威脅并回到原航跡的局部航跡。若無,則繼續沿著全局航跡飛行直至目標點。最后通過實驗對所提出的方法進行了驗證。

1 算法流程

DPPUAVSORRT算法流程如圖1所示。

圖1 DPPUAVSORRT流程圖Fig.1 Flowchart of DPPUAVSORRT

步驟1規劃全局航跡。根據已知的戰場情報信息和任務要求,利用分段優化RRT算法規劃從起點到終點的全局航跡。

步驟2威脅實時檢測。無人機沿著全局航跡飛行時,利用各類傳感器實時探測戰場信息,同時不間斷接收地面的情報信息。判斷規劃空間中是否有未知突發威脅產生,且突發威脅的位置、類型和作用范圍是否對無人機的飛行安全有很大影響。

步驟3更新規劃空間。根據步驟2的結果,若有突發威脅,則更新整個規劃空間的威脅信息,包括威脅的位置、類型和作用范圍等(將威脅作用范圍放大1.2倍);若無,則無人機繼續沿著全局航跡飛行,并繼續探測戰場信息。

步驟4確定新的起點。動態航跡規劃算法首先根據突發威脅的相關威脅信息,判斷所有航程點中最先可能處在威脅作用范圍內的航程點pj和最后可能處在威脅作用范圍內的航程點pk(2

步驟5規劃避讓航跡。根據步驟4確定的起點和終點,利用分段優化RRT算法搜索得到避讓突發威脅的局部航跡,使得無人機能夠成功繞過突發威脅并回到原航跡。

相關說明:

(1) 假定全局航跡的前兩個航程點和最后兩個航程點之內不會有未知的突發威脅。

(2) 將威脅作用范圍擴大1.2倍,是為了在規劃局部航跡時,降低航跡的威脅代價以及無人機被擊落的概率。

(3) 利用分段優化RRT算法規劃得到的航跡經過B樣條曲線法平滑后是大量擬合航跡的點,通過對這些擬合點的處理,設置均勻分布的若干航程點,可確保設置的起點pj-2和終點pk+2不會距離突發威脅太近,提高規劃的成功率。

2 分段優化RRT算法

2.1 算法流程

分段優化RRT算法流程如圖2所示。

圖2 分段優化RRT算法流程圖Fig.2 Flowchart of segmented optimization RRT

步驟1搜索節點。在整個規劃空間中根據面向目標的采樣策略隨機選取節點。

步驟2節點篩選。根據步驟1選取的節點,判斷其是否滿足所有約束條件。若滿足,則執行步驟3;若不滿足,則執行步驟1。

步驟3擴展隨機樹。在隨機樹中找到離節點最近的葉節點,并根據步長要求擴展新的節點(該節點也必須滿足所有約束條件),將新的節點加入到隨機樹中。若不存在,則執行步驟1。

步驟4反向搜索。當隨機樹擴展到終點或距離終點在一個步長以內時,反向搜索隨機樹,找到起點和終點之間的最短航跡。

步驟5分段優化。根據分段優化的航跡策略對所有航程點進行處理,分段優化航跡。

2.2 面向目標的采樣策略

傳統RRT算法在擴展隨機樹時,是對整個規劃空間進行隨機采樣并得到隨機節點prand,這樣雖然有利于對未知的規劃空間進行探索,保證了樹節點在規劃空間中的均勻分布,但實際上在規劃空間中的部分區域擴展隨機樹對尋找路徑沒有實質性的幫助,反而導致了隨機樹擴展效率低,搜索路徑速度慢的缺點。

面向目標的采樣策略是通過設定引導因子來改變隨機節點的采樣規則。引導因子是將終點pend以一定的概率q(引導因子的大小就是q值的大小)作為隨機節點prand(prand=pend),導致隨機節點的選取并不完全是隨機的。這樣使得隨機樹擴展時向目標位置生長的趨勢更加明顯,減約了隨機樹在部分規劃空間的擴展過程,大大減少了隨機樹擴展過程中隨機節點的數目,不僅使搜索路徑更具有目的性,而且有利于隨機樹快速擴展至目標點。

2.3 分段優化的航跡策略

現有的大部分RRT改進算法都考慮了規劃后的航跡是否滿足無人機最小轉彎半徑等無人機自身性能的約束[1,4,8],于是對隨機節點prand的選取提出了不同的約束條件,確保規劃得到的航跡能夠滿足無人機的性能約束。但這一方面限制了算法對于各類無人機的通用性,另一方面增加了隨機樹擴展過程中的運算代價,對算法搜索路徑的效率產生一定影響。

分段優化的航跡策略是將數據結構中“二分查找法”[9]的思想引入到RRT算法中。首先利用基于面向目標的采樣策略RRT算法規劃得到一系列航程點(按飛行順序排列),接著利用二分法將所有航程點一分為二,并從后一半航程點中找到能與目標點pend直接相連且與所有障礙不相交的航程點,記為Pn-1(記pend=Pn),然后對起始點pstart和Pn-1之間的航程點再進行分段優化處理,得到Pn-2,重復以上步驟直到所有航程點處理完畢。

分段優化的航跡策略處理的航程點數量遠遠小于隨機節點的數量,所以該優化策略的計算量遠小于對隨機節點的選取考慮無人機性能時的計算量,同時由于該改進策略不需要結合無人機的具體性能約束,使得算法通用性不受限制,并且該優化策略能夠大大縮短路徑長度。

相關性說明:分段優化的航跡策略不僅僅采用“二分法”,也可根據算法效率和精度的要求,對基于面向目標的采樣策略RRT算法規劃得到一系列航程點(按飛行順序排列)多劃分幾段進行優化。

2.4 航跡平滑

采用分段優化RRT規劃得到的航跡是由幾條線段組成的,但部分線段相交的拐點部分不夠平滑,不利于無人機在拐點處的機動飛行,需進行平滑處理。

由于B樣條曲線法兼備了Bezier曲線法的幾何不變性、仿射不變性等優點,同時克服了由于整體表示帶來不具備局部性質的缺點[10],使其在航跡規劃中應用廣泛。本文利用B樣條曲線法對拐點部分路徑點進行擬合,生成平滑路徑。

B樣條曲線方程可寫為

(1)

式中,控制頂點為di(i=0,1,…,n);k次規范B樣條基函數為Ni,k(u)(i=0,1,…,n),最高次數為k。基函數是由一個稱為節點矢量的非遞減參數u的序列U:u0≤u1≤…≤un+k+1所決定的k次分段多項式。

B樣條的基函數采用Cox-deBoor遞推公式,即

(2)

式中,i為節點序號;k是基函數的次數,共有n+1個控制點。

2.5 算法的概率完備性及收斂性證明

傳統RRT算法的隨機節點選取是在整個規劃空間中隨機選取的,使其能夠對規劃空間的未知區域展開搜索。而待擴展節點的概率是收斂于均勻分布的[11-14],算法具備概率完備性。本文引入面向目標的采樣策略影響了算法隨機節點選取的規則,下面從理論上對引入面向目標的采樣策略RRT算法的概率完備性進行證明。

假設Dk(p*)表示節點p*與隨機樹上最近節點之間距離的隨機變量。dk表示該隨機變量的取值,k表示節點數,ε表示擴展步長。

定理1在n維有界規劃空間U中(包含威脅),起始點pstart和目標點pend在同一連通區域內,當引入面向目標的采樣策略RRT算法的隨機樹節點數趨向于無窮大時,擴展節點pi等價于目標點pend的概率為1。即

(3)

式中，Ufree表示安全子空間,即規劃空間中無威脅障礙的區域。

證明設p*為Ufree中的任意一點,O(p*)表示以p*為圓心,ε為半徑的圓形區域,則有

O(p*)={p‖p-p*‖≤ε}

(4)

O′(p*)表示O(p*)與Ufree的交集,則有O′(p*)=O(p*)∩Ufree,μ(O′(p*))>0,μ表示集合的測度。

初始i=1時,d1(p*)=ρ(pend,p*),其中ρ(pend,p*)表示p*與起始點的距離。當隨機樹不斷擴展,隨機節點prand位于O′(p*)內的概率滿足

P{prand∈O′(p*)}>0

(5)

假設所有節點均在O′(p*)的外部,算法進一步擴展。當prand=p*時,依據pnew的擴展方法可知,Dk+1(p*)的數學期望是

E[Dk+1(p*)|prand=p*]

(6)

當prand≠p*時,有

E[Dk+1(p*)|prand≠p*]≤E[Dk(p*)]

(7)

進一步可得到

E[Dk+1(p*)]=E[qr·(Dk+1(p*)|prand=p*)+

(1-qr)·(Dk+1(p*)|prand≠p*)]

(8)

式中,00,使得Dk+1的數學期望E(Dk+1)

由此可以得出,引入面向目標的采樣策略RRT算法在擴展過程中使得p*與隨機樹節點之間的距離在逐漸縮小,當且僅當隨機樹節點數趨于無窮大時,節點位于O′(p*)的概率為1,即

由此可知,引入面向目標的采樣策略實質上只是改變了Ufree區域的大小,并沒有改變傳統RRT算法有界連通性以及隨機采樣性,保證了算法的概率完備性。同時在擴展節點趨于無窮時,保證擴展節點仍然服從于隨機節點采樣分布。

證畢

下面對引入面向目標的采樣策略RRT算法的收斂性進行證明。

證明設X和Xi分別是傳統RRT算法和引入面向目標的采樣策略RRT算法的節點概率分布密度函數,且pstart和p*位于同一連通區域內,則存在一個序列p1,p2,p3,…,pn以及對應的O(p1),O(p2),…,O(pn)圓形區域,且滿足pstart∈O(p1),p*∈O(pn),則有

Oi(pi)∩Oi+1(pi+1)≠?,i=1,2,…,n-1

(9)

(10)

設第i次擴展時Ufree中未被引入面向目標的采樣策略RRT算法搜索到的區域為

Yi={p*∈Ufree|ρ(p*,q)>ε,?ζ∈Hi}

(11)

ρ(p*,q)為p*與隨機樹節點q之間的距離,Hi為此時的節點集合,μ(Yi)為Yi的測度,且有Yi+1?Yi,當i→∞時,μ(Yi)→0。根據概率函數的光滑性,可知Xi概率收斂于X。

由上述證明可知,引入面向目標的采樣策略RRT算法仍然是收斂的。

證畢

3 實驗驗證

工作站配置:Thinkstation D30,CPU:Intel Xeon E5-2620 (雙處理器),64G內存,64位Win7系統。

編程工具:Matlab 2015(64位)。

3.1 分段優化RRT算法參數設置實驗

3.1.1 數據集

實驗設置200×200的二維無量綱規劃空間,起點為(10, 10),終點為(190, 190),用紅色五角星表示。其中分別設有8個威脅區域,10個威脅區域和12個威脅區域,用黃色圓形區域表示。3種規劃空間如圖3所示。

圖3 3種規劃空間Fig.3 Three types of planning space

3.1.2 實驗對象及相關參數設置

對象1分段優化RRT算法是以一定的引導因子q選取目標點pend作為prand。實驗對q值在[0,1)(q=1時,無法成功規劃航跡)范圍內,每隔0.1取值,并進行仿真實驗(步長設置為ε=5)。記錄每個q值下的100次仿真數據。

對象2隨機樹擴展時,若隨機節點prand和pnear(隨機樹上離prand最近的節點)之間的距離大于一個步長,則在pnear沿著prand的方向上,擴展一個步長ε得到新的節點pnew;若不大于,則直接將prand作為pnew。實驗對步長ε值取1,3,5,8,10,12,15,20,25,30,35,40,45,50(引導因子q=0.5)。記錄每個ε值下的100次仿真數據。

3.1.3 評估準則

指標1隨機樹節點數。隨機樹節點數是指在完成路徑搜索時,擴展隨機樹的節點數。隨機樹節點數越少表征算法搜索路徑時采樣點越少,算法效率越高。

指標2路徑代價。路徑代價是指航跡規劃算法計算得到路徑的長度,這是航跡規劃所關心的一項重要指標。因為在無人機性能有限的條件下,路徑代價直接決定著規劃的航跡是否可行。

指標3算法運行時間。算法運行時間是指在相同的硬件條件下算法的運行速度。在完成相同目標時,算法運行時間和算法效率呈負相關。

3.1.4 實驗結果及分析

q值對算法性能影響的實驗對比結果如圖4所示。

圖4 q值對算法性能的影響Fig.4 Effect of q value on the performance of the algorithm

將實驗結果分析如下:

(1) 由圖4可知,整體上當q=0(等價于傳統RRT算法)時,隨機樹節點數和算法運行時間都遠大于其他情況,路徑代價也大于其他情況,表明引入面向目標的采樣策略RRT算法效率優于傳統的RRT算法。

(2) 在第一種規劃空間中,隨機樹節點數呈現遞減趨勢,但當q>0.5時,遞減趨勢很小,此時增大q值對隨機樹節點數的影響很小。路徑代價隨著q值的增大呈遞減趨勢。而算法運行時間在0.1

(3) 在第二種規劃空間中,隨機樹節點數和路徑代價的變化趨勢與第一種規劃空間相似。而算法運行時間在q>0.5時的增長趨勢相對于第一種規劃空間更加明顯。但算法運行效率均低于第一種規劃空間。

(4) 在第三種規劃空間中,隨機樹節點數和路徑代價的變化趨勢與前兩種規劃空間相似。特別的是算法運行時間的變化趨勢在q≥0.5時增長更快更明顯。算法運行效率均低于前兩種規劃空間。

以上結果表明,增大q值能夠有效降低算法的隨機樹節點數、路徑代價以及在一定范圍內降低算法運行時間。但較大的q值反而會降低算法運行效率,這是由于較大的q值使得隨機節點prand以較大的概率選取目標點,導致隨機節點的選取比較單一,不利于隨機樹的擴展。同時越復雜的規劃空間,搜索路徑的效率也比較低。

ε值對算法性能影響的實驗對比結果如圖5所示。

圖5 ε值對算法性能的影響Fig.5 Effect of ε value on the performance of the algorithm

圖5(a)步長為1時的隨機樹節點數(3種規劃空間)遠大于其他情況,為了實驗數據顯示更加客觀形象,所以未在折線圖中表示。圖5(c)步長為1時的算法運行時間(規劃空間2和規劃空間3)遠大于其他情況,也未在折線圖中表示。

將實驗結果分析如下:

(1) 由圖5可知,隨著ε的增大,隨機樹節點數呈現遞減趨勢。路徑代價開始在一定范圍內波動,之后隨著ε繼續增大,呈現遞增趨勢。而算法運行時間也在一定范圍內呈現遞減趨勢,過大的ε值反而會增大算法運行時間。整體上看在適當范圍內選擇ε的值能夠有效降低算法運行效率。

(2) 在第一種規劃空間中,隨機樹節點數呈現遞減趨勢,但當ε>25時,遞減趨勢很小,此時增大ε值對隨機樹節點數的影響很小。路徑代價開始在一定范圍內波動(ε為3時規劃空間1的實驗數據誤差較大),之后隨著ε增大,呈現遞增趨勢。而算法運行時間在5<ε≤35時變化很小,此時算法已經能夠很快搜索得到路徑。但當ε>35時,算法運行時間有增長的趨勢,此時增大ε值反而降低了搜索路徑的效率。

(3) 在第二種規劃空間中,隨機樹節點數、路徑代價以及算法運行時間的變化趨勢與第一種規劃空間相似。但算法運行效率均低于第一種規劃空間。

(4) 在第三種規劃空間中,3類性能指標的變化趨勢同前二種規劃空間。算法運行效率也均低于前兩種規劃空間。但路徑代價在ε>30時增長趨勢更快更明顯。算法運行效率均低于前兩種規劃空間。

以上結果表明,在一定范圍內增大ε值能夠有效降低算法的運行效率。但較大的ε值反而會提升算法路徑代價以及運行時間,大大降低算法運行效率。這是由于較大的步長雖然有利于減少隨機樹的擴展,簡約了隨機樹擴展的空間。但是隨著規劃空間的復雜度提升,較大的步長使得擴展節點的失敗率也在增大,只能通過更遠距離的繞行來避開威脅,反而增大了路徑代價和算法運行時間。

綜上所述,引導因子q和步長ε的值在一定范圍內能夠提高算法搜索路徑的效率,其取值不僅要考慮規劃空間的復雜度也應結合具體任務要求、威脅態勢和其他情報信息等條件。

3.2 分段優化RRT算法實驗

3.2.1 實驗對象及相關參數設置

實驗對象:在設置的規劃空間中,利用分段優化RRT算法計算得到起點和終點之間的路徑。

仿真參數設置如表1所示。

表1 仿真參數設置

3.2.2 實驗結果及分析

分段優化RRT算法實驗結果如圖6所示。

圖6 分段優化RRT算法實驗結果Fig.6 Simulation results of segmented optimization RRT

圖6(a)、圖6(b)中藍色表示擴展的隨機樹,紅色表示搜索得到的路徑。圖6(c)藍色表示采用分段優化的航跡策略優化后的路徑,紅色表示B樣條曲線法平滑后的路徑。表2是對航跡進行分段優化前后的路徑長度實驗對比數據。

表2 分段優化前后路程比較

將實驗結果分析如下:

(1) 圖6(a)的隨機樹擴展遍布整個規劃空間,圖6(b)的隨機樹擴展只分布在路徑沿線的局部區域,表明引入面向目標的采樣策略的RRT算法在擴展隨機樹的過程中大大節約了擴展空間,使得隨機樹向目標點擴展趨勢更加明顯,提升了算法搜索路徑的效率。

(2) 圖6(c)藍色路徑是分段優化RRT算法搜索得到的路徑,由幾條線段組成,相比于傳統RRT算法大大減少了無人機進行機動轉彎的部分。同時,由表2可知,分段優化RRT算法相對于傳統RRT算法縮短了11.53%左右的路徑長度。表明引入分段優化的航跡策略一方面能夠減少無人機的機動轉彎,算法的通用性更強;另一方面能夠縮短路程長度,降低路徑代價。

3.3 動態航跡規劃實驗

數據集以及相關參數設置均同第3.2節。動態航跡規劃實驗結果如圖7所示。

圖7 動態航跡規劃實驗結果Fig.7 Simulation results of dynamic path planning

圖7中綠色圓點表示航程點,紅色圓形表示突發威脅區域,綠色五角星表示動態規劃航跡的起點和終點,淡藍色表示局部航跡規劃搜索得到的路徑,紫色表示分段優化后的路徑。

將實驗結果分析如下:

(1) 圖7(a)是突發威脅所處的位置、范圍對無人機沿著全局航跡飛行沒有影響的情況下,無人機無需重新規劃局部航跡,仍沿著全局航跡飛行即可。

(2) 圖7(b)是突發威脅的作用范圍與全局航跡相交時,無人機需規劃局部航跡的情形。此時算法能夠生成避讓突發威脅的局部航跡,并重新回到原航跡。

(3) 圖7(c)與圖7(b)相似,區別在于圖7(c)在突發威脅的兩側存在兩個狹窄通道,只有通過上方的通道繞行才能避開突發威脅。而算法能夠根據威脅信息在可行的通道內規劃局部航跡,確保無人機能夠安全避讓突發威脅。

(4) 實驗表明算法能夠準確判斷突發威脅的位置和范圍是否會對無人機的飛行安全產生影響,若有影響,則算法能夠成功規劃得到新起點和終點間的路徑并回到原航跡。

4 結 論

本文提出分段優化RRT的無人機動態航跡規劃算法,并通過實驗驗證了算法的可行性與優勢,主要得到以下結論:

(1) 面向目標的采樣策略能大大簡約隨機樹在規劃空間的擴展,提升算法搜索路徑的效率。

(2) 分段優化的航跡策略使得算法的通用性更強,同時減小了路徑代價,使得整體航跡代價更小,實用性更強。

(3) DPPUAVSORRT能夠準確判斷突發威脅對于無人機的飛行安全是否有影響。若有,則規劃局部航跡,確保無人機能夠安全避讓突發威脅。若無,則無人機繼續沿著全局航跡飛行。