一種關于航跡校正點選擇的優化方法

何 兵，朱曉宇，林浩申，李青勇，陳 琛

(火箭軍工程大學 核工程學院，西安 710025)

無人飛行器(Unmanned Aircraft Vehicle,UAV)具有使用靈活、飛行自主等突出特點，在軍事和民用領域應用廣泛[1]。航跡規劃是實現UAV自主飛行的重要環節，是指根據環境的已知信息和任務需求，考慮UAV飛行過程的相關約束，生成從起點到目標點的飛行航路。由于在對抗、遮擋等異常情況下衛星導航系統存在風險，通常UAV航路上設置一系列航路校正點降低慣性導航系統誤差，以實現高精度導航避免碰撞風險。因此，航跡規劃，不僅要考慮航路上的威脅、障礙、人文禁避飛區等規避約束，也要考慮UAV航跡校正點規劃[2]。本文主要研究UAV的航跡校正點選擇和優化問題。

當前航跡校正點選擇和優化常用搜索算法有A*算法[3-5]，快速搜索隨機樹(Rapidly-exploring Random Tree,RRT)[8-9]算法，群智能優化算法等。A*算法通過選取估價函數對每個航跡搜索點進行評價[5]，啟發式搜索在規避無效搜索和提高搜索效率上發揮了重要作用；群智能算法對目標函數和約束的表達比較寬松，可通過并行計算提高規劃效率[6]，但容易陷入局部最優。作為典型的隨機搜索算法，RRT算法在收斂速度上和總體的規劃效率具有明顯優勢[10-14]。本文針對導航精度受限的UAV航跡校正點選擇和優化問題，綜合航跡長度和校正點約束，對RRT算法進行改進，實現航跡校正點的選擇優化。與傳統A*算法、RRT算法的對比仿真試驗結果表明改進RRT算法的航跡校正點選擇優化上具有很好的快速性和有效性。

1 問題描述

1.1 問題背景

本文的應用場景可以描述為：UAV在給定任務空間中執行任務，由于飛行過程中導航誤差的存在，使UAV需要在航跡校正點集上進行誤差校正，在規定的任務約束下規劃出一條從起點A到目標點B的完整航跡。這類航跡規劃問題可以轉化為航跡校正點選擇優化(Optimization of Path Correction points,OPC)研究，通過優化航跡校正點的搜索選取，綜合考慮航跡長度代價和經過校正點個數，生成全局最優航跡。

不難發現，上述問題的尋優是在有向圖g=(U,L,Δ)搜索出一條滿足約束巡航路徑。有向圖中U表示任務空間內的校正點集，具體來說，空間內的垂直誤差校正點集U⊥和水平誤差校正點集U‖和任務的起點A、終點B共同構成有向圖g的各個節點。L表示UAV在有效校正點間的飛行航跡的集合，校正點間的有效航跡上的誤差增量不超過UAV正常工作導航精度的上限。Δ表示有效航跡上誤差增量的集合，L和Δ之間存在對應關系。

U作為航跡校正點選擇優化問題的輸入，可以表示為

U={Ni|Ni,i=0,1,2,…,n+1}

(1)

其中，N0和Nn+1分別表示起點A和終點B。

(2)

(3)

1.2 約束條件和優化目標

航跡校正點的選擇優化過程需要考慮到相鄰校正點在當前導航精度約束下的可達性，即在當前應用場景中，UAV按照規劃路徑飛行指定目標點的前提是飛行誤差的累積應始終維持在一個合理范圍。本文設飛行過程中的垂直誤差和水平誤差始終小于θ個單位，才能按照規劃路徑飛行直至終點，則對于校正節點Nj校正前的導航誤差為

此外還應考慮進行飛行誤差校正需要達到相應的誤差閾值條件，因此，對于航跡校正節點Nj在校正前的導航誤差需滿足：

(5)

綜合式(4)和式(5)可得校正誤差約束為

(6)

航跡校正點的選擇優化過程選擇的目標函數為校正次數代價和航跡長度代價，前者指的是確保飛行器經過的誤差校正點總數l最小，后者指的是航跡(N0,Ni1),(Ni1,Ni2),…,(Nil,Nn+1)的總長度最小,則：

(7)

其中，dik,ik+1表示航跡節點ik到ik+1的距離。

同時考慮航跡不能同時滿足兩個優化目標，在指標函數(見式(8))中引入權重系數來衡量優化目標的重要程度：

(8)

其中，J1和J2為歸一化系數，權重系數ω可根據飛行任務性質的不同人工調整。指標函數J越小說明航跡越優。

2 基于有效鄰域的改進RRT算法

對于航跡校正點規模較大的OPC問題，全局搜索是最優解決方案，但近似O(n!)時間復雜度使其在實際應用中不可能采用，因此需要考慮采用隨機搜索或啟發式搜索等策略進行選擇優化。同時傳統航跡規劃的解空間通常是無限且連續的，在搜索更新過程中，對搜索步長和搜索方向的限制較小，可以在任意步長和方向上進行搜索，而航跡校正點選擇優化問題的解空間是有限且離散的，其搜索步長會受到當前節點累積誤差信息影響，搜索方向受到先驗節點位置信息的限制，因此，針對經典RRT算法進行改進，通過構建有效鄰域在解空間中進行搜索，有效降低了獲得可行解的復雜度。

2.1 經典RRT算法

設當前的解空間區域為R，起點和終點分別為A和B，二維空間內經典RRT算法[15]擴展過程如圖1所示，其中，qnow為當前隨機樹的葉節點的集合，qrand表示在解空間子集R∩qnow內按一定的規則隨機采樣得到的葉節點，qnear表示在當前擴展樹中與qrand最近的葉節點，ε為當前樹向qrand方向的生長距離，qnew表示滿足相關約束后隨機樹的新增節點。

圖1 經典RRT算法擴展過程示意圖

經典RRT[11-13]偽代碼的描述，如下：

1Initialization:xA,xB,N,eps2for i=1:N3 qrand←random()4 qnear←min(qknow,qrand),k=1:K5 qtemp←ε·qrandqnear→qrandqnear→6 ifqtempNOTsatisfy{condition1,…} continue7 qnew←qtemp8 qnow←{qnow,qnew}9 ifdistance(qnew,xB)

其中，N,K,eps為邊界條件，N表示RRT構造的最大嘗試次數,K表示當前RRT節點的規模,eps表示與終點接近程度。

2.2 基于有效鄰域的改進RRT算法

2.2.1 有效鄰域的構建

(9)

(10)

2.2.2 改進的RRT校正點選擇算法

建立節點的鄰域可達集后，迭代過程中最近鄰節點從當前節點的鄰域可達集中產生，新節點可以采用一定策略從近鄰節點的Unear中選擇，搜索域規模大大減小。進一步，葉節點狀態矢量S用于存儲節點狀態信息，對相鄰節點狀態矢量建立鏈接關系，可以在反向溯源過程中快速給出可行航跡，具體步驟如下：

1) 葉節點狀態表構建及其初始化。定義每個葉節點的狀態矢量S為

S=(Norig,δ⊥,δ‖,Lpre,Lnow,dtotal,Ntotal)T

(11)

其中，Norig對應當前葉節點在U中的原始編號；δ⊥、δ‖為當前葉節點校正后的垂直誤差和水平誤差；Lpre為當前葉節點的前向葉節點的編號，Lnow為當前葉節點的編號，葉節點的編號由葉節點生長過程的先后順序唯一確定；dtotal、Ntotal分別為截至當前節點的航程代價和累計校正次數。

同時可以通過改善葉子狀態列表數據結構(圖2)，提高計算更新速度。

圖2 葉節點狀態示意圖

2) 選取目標點Lrand。設當前葉節點集合L={L0,L1,L2,…}，從當前隨機樹外的校正點集U-L中按如下策略選取一目標點Lrand：命中終點B的概率為p，命中其他葉節點的概率為1-p。其中，p為超參數，用于保證搜索的導向性，避免算法陷入局部最優解。

3) 搜索最近鄰節點Lnear。在當前葉節點集合L中，搜索出同時滿足Near法則和式(6)約束的節點Lnow作為最近鄰節點Lnear。Near法則存在以下兩種定義：

① 距離最近法則(LawA)：確保節點Lnear與節點Lrand的歐氏距離最小，如圖3中的Lnear。

(12)

(13)

圖3 最近鄰節點選擇法則示意圖

基于有效鄰域的快速擴展隨機樹(Rapidly Rapidly-exploring Random Tree based on effective Neighborhood,NRRT)算法流程如圖4所示。

圖4 NRRT算法流程框圖

3 仿真算例與分析

3.1 仿真環境與參數配置

本文的仿真采用MATLAB R2018a (64-bit) 進行編程計算，硬件環境為：i7-8700 CPU，8G RAM，Win 10操作系統。隨機樹的最近鄰產生采用距離最近法則，單位航程垂直和水平誤差累積按照線性處理，其他參數設置如表1所示。

表1 仿真過程參數設置

其中，m為初始葉節點個數，p為隨機選擇B作為目標點的概率，[α1,α2],[β1,β2]為進行垂直誤差和水平誤差校正需要滿足的誤差閾值。

3.2 仿真結果

采用本文的基于有效鄰域的RRT校正點選擇優化算法(NRRT)對所給的數據集1[2]進行處理，綜合考慮航跡長度約束和校正次數約束，分別規劃了航程最優航跡和校正最優航跡，經過104次蒙特卡洛仿真規劃結果如圖5、圖6所示，圖中離散點是任務區域內的校正點集，最優航跡經過的校正點采用三角形標注。其航程最優航跡(圖5)共經過9個校正點，航跡總長度為 104 279. 425 9 m，而校正最優航跡(圖6)共經過 8個校正點，航跡總長度為 104 898.374 9 m。

圖5 NRRT航程最優航跡

圖6 NRRT校正最優航跡

算法復雜度方面，104次蒙特卡洛仿真總耗時540.509 3 s，最大計算耗時僅0.999 8 s，最小計算耗時0.011 9 s，94.24%的搜索耗時集中在[0,0.2 s]的區間內。

針對OPC問題，目前常用的方法是A*及其改進算法[15-16]，A*算法作為最有效的直接搜索算法，本文采用其與NRRT作橫向比較，選取A*算法的代價函數為f(n)=g(n)+h(n)，航程最優航跡(圖7)共經過8個校正點，航跡總長度為 105 057.363 6 m。算法復雜度方面，經過104次蒙特卡洛仿真，平均耗時穩定在1.021 4 s左右。

圖7 A*航程最優航跡

NRRT算法的隨機采樣特性，極易通過單次搜索獲得可行的次優解，但單次解的質量上要弱于 A*算法。而NRRT的搜索快速性，可以使其通過少量的蒙特卡洛仿真逼近最優解，達到提升解的質量的效果。NRRT算法和A*算法求解過程經過的校正點集、獲得的最優航跡以及算法的平均耗時如表2所示。總體來說，面對復雜規劃問題，NRRT要比A*算法的規劃效率高，在處理OPC問題具有更大的優勢。

表2 仿真結果

4 結論

針對航跡校正點的選擇優化問題，設計了一種NRRT算法，一方面通過構建有效鄰域大幅減小了傳統RRT算法的搜索空間，間接提高其搜索的搜索效率，降低了算法的計算復雜度。另一方面設計了有效的葉節點列表存儲結構，以查表代替運算，有效降低搜索和回溯的復雜度。兩方面保證了算法的有效性，使其在實際操作過程中算法收斂速度很快，規劃效率高，極易獲得可行解，最后的仿真結果證明，在航跡校正點的選擇優化中，NRRT在規劃效率方面具有明顯優勢。