基于啟發式搜索算法的無人機航跡快速規劃

吳玉文， 牛智越， 韓倩倩

(北京物資學院信息學院,北京 101149)

隨著社會的不斷發展,無人機的應用范圍越來越廣[1]。無人機導航是按照要求的精度,沿著預定的航線,把無人機從出發地引導至目的地完成任務。目前無人機主要采用的導航技術有慣性導航、衛星導航、地磁導航和地形輔助導航等[2]。由于現有導航技術無法完全保證對無人機的精準定位,故存在部分精度誤差。定位誤差是一種常見的精度誤差,在無人機常采用的慣性導航技術中就存在該誤差[2],它是由于無人機在飛行時無法對自身精準定位而隨時間累積的累積誤差,包括垂直誤差和水平誤差。目前,為了提高導航性能進而降低導航誤差,常見的解決方法是把慣性導航技術與其他導航技術組合使用。但是在惡劣的條件下,只能采用慣性導航技術,通過在飛行區域設置的校正點實時校正定位誤差,使得無人機校正誤差后按照預定航線到達目的地[3]。因此,要解決的航跡規劃問題是在只能使用慣性導航技術的條件下,在已知出發地、目的地的位置和校正點的類型、位置信息的基礎上,從起始點出發,確定無人機所經過的校正點類型以及校正點連接順序,使其在滿足校正定位誤差約束的前提下成功到達目的地,同時使得規劃的無人機航跡長度最短。

近年來學者們對無人機航跡規劃問題的研究主要集中在威脅規避、任務要求與性能限制等方面,對于定位誤差與航跡規劃兩者結合考慮的研究較少。尹高揚等[4]在航跡規劃中同時考慮了無人機的航跡生成約束和地形信息,應用快速擴展隨機樹進行了航跡規劃。張亞蘭等[5]提出離線規劃和在線避障兩者結合的航跡規劃方法,分別應用雙向A*算法和向量場直方圖算法來實現。李世曉等[6]研究了帶有威脅、任務戰術等多約束條件下的航跡規劃問題,應用改進的A*算法進行求解。方群等[7]、Goez等[8]通過考慮威脅區域和任務的側重程度,應用改進粒子群算法來提高航跡規劃的求解質量。許江波等[9]通過考慮威脅區域、無人機性能要求以及轉彎代價等方面,改進了人工魚群算法進行問題求解。李文廣等[10]考慮了無人機最小轉彎半徑,通過證明得出了最優的轉彎策略,并將其結果應用到航跡規劃中。劉世一等[3]首次將定位誤差與航跡規劃兩者結合考慮,建立了機載慣導系統誤差模型和光電偵察載荷校正模型來減小航跡的誤差,并提出在規劃航線上設置校正點進行誤差校正。

在已知校正點的類型、位置坐標以及可校正的誤差范圍的情況下,考慮飛行過程中無人機的累積誤差對有效航跡長度的影響,建立了考慮定位誤差的無人機航跡規劃模型,設計了啟發式深度優先搜索+回溯算法對模型進行快速求解,最后對解的結果做了進一步的優化改進。

1 考慮定位誤差的航跡快速規劃問題

1.1 問題描述

已知某無人機需要從出發點到達目的地快速規劃出一條航跡,A為航跡起始節點,B為航跡終止節點。從A點出發時,無人機的水平誤差和垂直誤差均為0。無人機每飛行1 m,垂直誤差和水平誤差將各增加δ個單位。因此飛行區域中有若干垂直校正點和水平校正點對其定位誤差進行校正,各節點(包括起點、終點與校正點)的位置與類型均已知。

當無人機到達某垂直校正點時,若垂直誤差不大于α1個單位,水平誤差不大于α2個單位,則對其進行垂直誤差校正。校正后,其垂直誤差變為0,水平誤差保持不變。當無人機到達某水平校正點時,若垂直誤差不大于β1個單位,水平誤差不大于β2個單位,則對其進行水平誤差校正。校正后,其水平誤差變為0,垂直誤差保持不變。若無人機到達B點時其垂直誤差和水平誤差均應小于θ個單位,則認為航跡規劃成功。如何規劃航跡才能使無人機從起始點A出發,以最短的距離準確地飛到目的地B？

1.2 模型建立

1.2.1 符號說明

H:垂直校正點集合。

L:水平校正點集合。

S:所有節點集合,S=H∪L∪{A}∪{B}。

(ai,bi,ci):節點i的空間坐標。

決策變量:

hi:無人機在節點i處的垂直誤差,其中hA=0。

li:無人機在節點i處的水平誤差,其中lA=0。

1.2.2 約束條件

根據不同節點約束條件的不同,校正約束與航跡約束可分為4類。

第1類從A點進入校正點j的誤差約束。式(1)表示從起點A飛入校正點j的垂直誤差約束,式(2)表示從起點A飛入校正點j的水平誤差約束。具體含義為:若節點j為垂直校正點,無人機到達j點時垂直誤差不大于α1,水平誤差不大于α2。若節點j為水平校正點,垂直與水平誤差分別不大于β1與β2。

xAjdAjδ≤(1-rj)α1+rjβ1,j∈H∪L

(1)

xAjdAjδ≤(1-rj)α2+rjβ2,j∈H∪L

(2)

第2類從校正點i飛入校正點j的誤差約束。式(3)表示飛入校正點j的垂直誤差約束,式(4)表示飛入校正點j的水平誤差約束。式(5)表示飛入校正點j后垂直誤差的值,式(6)表示飛入校正點j后水平誤差的值。

hiri+xijdijδ≤(1-rj)α1+rjβ1,i,j∈H∪L

(3)

li(1-ri)+xijdijδ≤(1-rj)α2+rjβ2,

i,j∈H∪L

(4)

hjxij=(hi+dijδ)rjxij,i∈H∪L∪{A},

j∈H∪L

(5)

ljxij=(li+dijδ)(1-rj)xij,i∈H∪L∪{A},

j∈H∪L

(6)

第3類從校正點i到達B點的誤差約束。式(7)表示到達B點的垂直誤差約束,式(8)表示到達B點的水平誤差約束。式(9)表示從A點到達B點的累計誤差約束。

xiB(hiri+diBδ)≤θ,i∈H∪L

(7)

xiB[li(1-ri)+diBδ]≤θ,i∈H∪L

(8)

xABdABδ≤θ

(9)

第4類航跡約束。無人機所經過的節點是連續的,即被選擇的節點之間的連線是一條路。

(10)

(11)

(12)

1.2.3 數學模型

目標函數為最小化無人機航跡規劃的長度:

(13)

因此,式(1)～式(13)給出了考慮定位誤差的無人機航跡規劃問題的混合整數規劃模型。

2 算法設計

由于模型中對于誤差校正的約束條件較多,且下一節點的選取是由上一節點的定位誤差與校正點類型決定的。因此該問題的解空間屬于未知深度的解空間樹,很難在短時間內利用求解器對模型進行求解。根據該模型的特點,針對每個節點的約束條件,逐步進行節點探索。

深度優先搜索(depth-first search,DFS)算法[11-12]是沿狀態樹的深度方向進行搜索,直到找到解或者無法繼續搜索,是一種盲目的搜索算法,適用于一個問題出現多個分枝以及如何選擇分枝可以盡快到達目標點的情況。回溯算法[13]在包含所有解的解空間樹中,從根結點出發搜索解空間樹。算法搜索至解空間樹的任一節點時,先判斷該節點是否滿足問題的約束。如果不滿足,則跳過對以該節點為根的子樹的搜索,逐層向其祖先節點回溯；否則,進入該子樹繼續搜索,從而減少解空間樹中無效節點的搜索。

結合以上兩種算法的優點,采用啟發式DFS+回溯算法求解考慮定位誤差的無人機航跡快速規劃問題。當解空間很大時,將DFS算法與啟發式算法相結合,避免DFS算法進行盲目搜索,從而更快地到達目標點。而在對節點進行分枝的過程中,根據約束條件,DFS算法可能會在某些節點處無法繼續搜索,找不到符合約束條件的下一節點。回溯算法能很好地解決DFS算法在搜索時找不到可行節點的問題,從而避免搜索失敗。因此,在無人機航跡規劃問題中應用啟發式DFS+回溯算法可以快速找到近似最優解,規劃出無人機航跡。

啟發式DFS+回溯算法使用3個列表,開放列表Open、節點列表Jiedian與刪除列表Shanchu,分別儲存當前待分析節點、已規劃的飛行節點和無效飛行節點。應用該算法時的操作步驟如下,算法流程如圖1所示。

圖1 算法流程Fig.1 Flowchart of algorithm

步驟1開始將起始點A放入Jiedian表,令Shanchu表為空。

步驟2令Jiedian表中最后一個點為節點i,判斷當前節點i是否在Shanchu表中。若節點i不在Shanchu表中,轉步驟3；否則在Open表中刪除節點i后,選擇節點i+1,轉步驟5。

步驟4以L為搜索步長,L=max{α1,α2,β1,β2},以當前節點i與目標點B向量方向為軸,以ω偏向角繞軸旋轉進行搜索,搜索區域如圖2所示。在圖2中,無人機每次的搜索區域是一個類錐體區域,類錐體區域內每一個滿足約束條件的節點j組成集合J。計算集合內每個節點的評價函數值fj,其中fj=cos{∠jiB},j∈J。將集合J中所有元素加入Open表,清空Shanchu表。

圖2 無人機搜索區域Fig.2 Search area of the UAV

步驟5判斷Open表是否為空。若為空,則執行回溯算法,即刪除Jiedian表中的節點i并記錄到Shanchu表,轉步驟2；否則轉步驟6。

步驟6對Open表中節點的評價函數值進行排序,選擇值最大的點j作為下一個航跡節點。把該節點j加入Jiedian表,轉步驟2。

3 算例分析

3.1 算例數據

已知某飛行區域內共規劃部署了611個校正點,其中各校正點的空間坐標、校正類型以及起點與終點的空間坐標均為已知的,飛行區域節點信息如表1所示,由于節點較多,表1只列舉了部分節點的信息。設定α1=25,α2=15,β1=20,β2=25,θ=30,δ=0.001,ω=90°。

表1 飛行區域節點信息Table 1 Node information of flight area

3.2 算例結果

根據本文的算法,利用MATLAB 2014b編程,在Intel(R)Core i5-6200U @ 2.30 GHz 2.30 GHz處理器上運行程序,經過0.059 s得出近似最優解,無人機從A點出發,經過8個校正點,飛行106 350.061 m后,到達B點。經過的校正點編號和類型以及校正前的誤差如表2所示,規劃出的路徑圖如圖3所示。

表2 航跡規劃結果Table 2 Path planning results

為驗證本文算法在性能上的優勢,利用表1的數據,對比A*算法,在相同處理器配置的計算機上運行算法的驗證,兩種算法結果對比分析如表3所示。

圖3 航跡規劃圖Fig.3 Path planning map

表3 算法結果對比Table 3 Results comparison of algorithms

由表3可見,啟發式DFS+回溯算法可以得到更好的解,其飛行距離比A*算法減少了4 936.437 m,經過的校正節點少1個,且求解時間更短。

3.3 求解結果優化

由于啟發式DFS+回溯算法可以快速求得一個較好的可行解,卻不能保證該解的全局最優性。為了提高解的質量,引入模擬退火的思想,即在由啟發式DFS算法搜索得到的每一步的領域結構解空間中,以一定概率的方式接受評價函數值變“差”的解。隨著啟發式DFS算法探索深度的增加,無人機與目的地距離的減少,接受差解的概率隨之逐漸減小。每一步搜索后選擇的解xq由以下方法獲得。

(14)

I：U(1,M)

(15)

xq=arg[Open(q)(I)]

(16)

式中：q是搜索步數；nq是第q步所產生的Open表中的元素個數；xq是第q步選擇的解；c為參數,在區間[0.1,0.9]內取值；經多次實驗,取0.4作為c的取值。式(14)表示第q步在Open表中排序后選擇解空間的范圍值；式(15)表示在服從均勻分布的區間[1,M]中隨機產生一個索引I；式(16)表示在排序后的Open表中選擇第I個評價函數值f對應的解xq。

根據式(14)～式(16)的優化操作,將表1中的數據按照啟發式DFS+回溯算法進行優化迭代計算,其優化迭代結果與航跡規劃結果如圖4和圖5所示。由圖4可知,隨著優化迭代次數的增加,算法能不斷地找到最優解,最后結果逐漸趨于穩定。

圖4 優化迭代結果Fig.4 Results of optimization iteration

圖5 優化后的航跡規劃圖Fig.5 Optimized path planning map

從圖5的航跡規劃結果來看,無人機的航跡更加趨于平滑,與之前的航跡規劃結果相比,沒有出現較大的飛行爬升和俯沖。這是因為在初始階段的解空間中,解的評價函數值相差并不大,選擇非最優評價函數的值可以跳出局部最優解,使得航跡距離最短,航跡更平滑。

對3種算法的求解結果進行比較分析,結果如表4所示。通過對比發現,在航跡距離方面,加入模擬退火機制的啟發式DFS+回溯算法求解出的航跡距離最短,其長度為103 887.759 m,比啟發式DFS+回溯算法節省了2 462.302 m,比A*算法節省了7 398.739 m。在求解時間方面,由于加入模擬退火機制,優化后的啟發式DFS+回溯算法求解時間相對較長,這是因為該算法的求解時間與迭代次數有關。可以根據實際要求對該算法的迭代次數進行調整,在求解的時間和求解精度之間取得較滿意的求解效果。

表4 算法比較分析Table 4 Comparison and analysis of algorithms

4 結論

建立了考慮定位誤差的無人機航跡規劃的數學模型,設計了啟發式DFS+回溯算法,解決了通過校正點不斷校正定位誤差的航跡規劃問題,并在此算法的基礎上進行了優化改進。實驗結果表明,與A*算法相比,本文的啟發式DFS+回溯算法在求解時間上具有一定優勢,航跡距離結果更優。而加入模擬退火機制的啟發式DFS+回溯算法雖然求解時間變長,但是求解質量更高。本文的模型與算法為無人機航跡快速規劃問題的研究及應用提供了一定的理論依據。