基于啟發(fā)式搜索算法的無(wú)人機(jī)航跡快速規(guī)劃

吳玉文， 牛智越， 韓倩倩

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

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

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

在已知校正點(diǎn)的類(lèi)型、位置坐標(biāo)以及可校正的誤差范圍的情況下,考慮飛行過(guò)程中無(wú)人機(jī)的累積誤差對(duì)有效航跡長(zhǎng)度的影響,建立了考慮定位誤差的無(wú)人機(jī)航跡規(guī)劃模型,設(shè)計(jì)了啟發(fā)式深度優(yōu)先搜索+回溯算法對(duì)模型進(jìn)行快速求解,最后對(duì)解的結(jié)果做了進(jìn)一步的優(yōu)化改進(jìn)。

1 考慮定位誤差的航跡快速規(guī)劃問(wèn)題

1.1 問(wèn)題描述

已知某無(wú)人機(jī)需要從出發(fā)點(diǎn)到達(dá)目的地快速規(guī)劃出一條航跡,A為航跡起始節(jié)點(diǎn),B為航跡終止節(jié)點(diǎn)。從A點(diǎn)出發(fā)時(shí),無(wú)人機(jī)的水平誤差和垂直誤差均為0。無(wú)人機(jī)每飛行1 m,垂直誤差和水平誤差將各增加δ個(gè)單位。因此飛行區(qū)域中有若干垂直校正點(diǎn)和水平校正點(diǎn)對(duì)其定位誤差進(jìn)行校正,各節(jié)點(diǎn)(包括起點(diǎn)、終點(diǎn)與校正點(diǎn))的位置與類(lèi)型均已知。

當(dāng)無(wú)人機(jī)到達(dá)某垂直校正點(diǎn)時(shí),若垂直誤差不大于α1個(gè)單位,水平誤差不大于α2個(gè)單位,則對(duì)其進(jìn)行垂直誤差校正。校正后,其垂直誤差變?yōu)?,水平誤差保持不變。當(dāng)無(wú)人機(jī)到達(dá)某水平校正點(diǎn)時(shí),若垂直誤差不大于β1個(gè)單位,水平誤差不大于β2個(gè)單位,則對(duì)其進(jìn)行水平誤差校正。校正后,其水平誤差變?yōu)?,垂直誤差保持不變。若無(wú)人機(jī)到達(dá)B點(diǎn)時(shí)其垂直誤差和水平誤差均應(yīng)小于θ個(gè)單位,則認(rèn)為航跡規(guī)劃成功。如何規(guī)劃航跡才能使無(wú)人機(jī)從起始點(diǎn)A出發(fā),以最短的距離準(zhǔn)確地飛到目的地B？

1.2 模型建立

1.2.1 符號(hào)說(shuō)明

H:垂直校正點(diǎn)集合。

L:水平校正點(diǎn)集合。

S:所有節(jié)點(diǎn)集合,S=H∪L∪{A}∪{B}。

(ai,bi,ci):節(jié)點(diǎn)i的空間坐標(biāo)。

決策變量:

hi:無(wú)人機(jī)在節(jié)點(diǎn)i處的垂直誤差,其中hA=0。

li:無(wú)人機(jī)在節(jié)點(diǎn)i處的水平誤差,其中l(wèi)A=0。

1.2.2 約束條件

根據(jù)不同節(jié)點(diǎn)約束條件的不同,校正約束與航跡約束可分為4類(lèi)。

第1類(lèi)從A點(diǎn)進(jìn)入校正點(diǎn)j的誤差約束。式(1)表示從起點(diǎn)A飛入校正點(diǎn)j的垂直誤差約束,式(2)表示從起點(diǎn)A飛入校正點(diǎn)j的水平誤差約束。具體含義為:若節(jié)點(diǎn)j為垂直校正點(diǎn),無(wú)人機(jī)到達(dá)j點(diǎn)時(shí)垂直誤差不大于α1,水平誤差不大于α2。若節(jié)點(diǎn)j為水平校正點(diǎn),垂直與水平誤差分別不大于β1與β2。

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

(1)

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

(2)

第2類(lèi)從校正點(diǎn)i飛入校正點(diǎn)j的誤差約束。式(3)表示飛入校正點(diǎn)j的垂直誤差約束,式(4)表示飛入校正點(diǎn)j的水平誤差約束。式(5)表示飛入校正點(diǎn)j后垂直誤差的值,式(6)表示飛入校正點(diǎn)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類(lèi)從校正點(diǎn)i到達(dá)B點(diǎn)的誤差約束。式(7)表示到達(dá)B點(diǎn)的垂直誤差約束,式(8)表示到達(dá)B點(diǎn)的水平誤差約束。式(9)表示從A點(diǎn)到達(dá)B點(diǎn)的累計(jì)誤差約束。

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

(7)

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

(8)

xABdABδ≤θ

(9)

第4類(lèi)航跡約束。無(wú)人機(jī)所經(jīng)過(guò)的節(jié)點(diǎn)是連續(xù)的,即被選擇的節(jié)點(diǎn)之間的連線是一條路。

(10)

(11)

(12)

1.2.3 數(shù)學(xué)模型

目標(biāo)函數(shù)為最小化無(wú)人機(jī)航跡規(guī)劃的長(zhǎng)度:

(13)

因此,式(1)～式(13)給出了考慮定位誤差的無(wú)人機(jī)航跡規(guī)劃問(wèn)題的混合整數(shù)規(guī)劃模型。

2 算法設(shè)計(jì)

由于模型中對(duì)于誤差校正的約束條件較多,且下一節(jié)點(diǎn)的選取是由上一節(jié)點(diǎn)的定位誤差與校正點(diǎn)類(lèi)型決定的。因此該問(wèn)題的解空間屬于未知深度的解空間樹(shù),很難在短時(shí)間內(nèi)利用求解器對(duì)模型進(jìn)行求解。根據(jù)該模型的特點(diǎn),針對(duì)每個(gè)節(jié)點(diǎn)的約束條件,逐步進(jìn)行節(jié)點(diǎn)探索。

深度優(yōu)先搜索(depth-first search,DFS)算法[11-12]是沿狀態(tài)樹(shù)的深度方向進(jìn)行搜索,直到找到解或者無(wú)法繼續(xù)搜索,是一種盲目的搜索算法,適用于一個(gè)問(wèn)題出現(xiàn)多個(gè)分枝以及如何選擇分枝可以盡快到達(dá)目標(biāo)點(diǎn)的情況?；厮菟惴╗13]在包含所有解的解空間樹(shù)中,從根結(jié)點(diǎn)出發(fā)搜索解空間樹(shù)。算法搜索至解空間樹(shù)的任一節(jié)點(diǎn)時(shí),先判斷該節(jié)點(diǎn)是否滿足問(wèn)題的約束。如果不滿足,則跳過(guò)對(duì)以該節(jié)點(diǎn)為根的子樹(shù)的搜索,逐層向其祖先節(jié)點(diǎn)回溯；否則,進(jìn)入該子樹(shù)繼續(xù)搜索,從而減少解空間樹(shù)中無(wú)效節(jié)點(diǎn)的搜索。

結(jié)合以上兩種算法的優(yōu)點(diǎn),采用啟發(fā)式DFS+回溯算法求解考慮定位誤差的無(wú)人機(jī)航跡快速規(guī)劃問(wèn)題。當(dāng)解空間很大時(shí),將DFS算法與啟發(fā)式算法相結(jié)合,避免DFS算法進(jìn)行盲目搜索,從而更快地到達(dá)目標(biāo)點(diǎn)。而在對(duì)節(jié)點(diǎn)進(jìn)行分枝的過(guò)程中,根據(jù)約束條件,DFS算法可能會(huì)在某些節(jié)點(diǎn)處無(wú)法繼續(xù)搜索,找不到符合約束條件的下一節(jié)點(diǎn)?；厮菟惴芎芎玫亟鉀QDFS算法在搜索時(shí)找不到可行節(jié)點(diǎn)的問(wèn)題,從而避免搜索失敗。因此,在無(wú)人機(jī)航跡規(guī)劃問(wèn)題中應(yīng)用啟發(fā)式DFS+回溯算法可以快速找到近似最優(yōu)解,規(guī)劃出無(wú)人機(jī)航跡。

啟發(fā)式DFS+回溯算法使用3個(gè)列表,開(kāi)放列表Open、節(jié)點(diǎn)列表Jiedian與刪除列表Shanchu,分別儲(chǔ)存當(dāng)前待分析節(jié)點(diǎn)、已規(guī)劃的飛行節(jié)點(diǎn)和無(wú)效飛行節(jié)點(diǎn)。應(yīng)用該算法時(shí)的操作步驟如下,算法流程如圖1所示。

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

步驟1開(kāi)始將起始點(diǎn)A放入Jiedian表,令Shanchu表為空。

步驟2令Jiedian表中最后一個(gè)點(diǎn)為節(jié)點(diǎn)i,判斷當(dāng)前節(jié)點(diǎn)i是否在Shanchu表中。若節(jié)點(diǎn)i不在Shanchu表中,轉(zhuǎn)步驟3；否則在Open表中刪除節(jié)點(diǎn)i后,選擇節(jié)點(diǎn)i+1,轉(zhuǎn)步驟5。

步驟4以L為搜索步長(zhǎng),L=max{α1,α2,β1,β2},以當(dāng)前節(jié)點(diǎn)i與目標(biāo)點(diǎn)B向量方向?yàn)檩S,以ω偏向角繞軸旋轉(zhuǎn)進(jìn)行搜索,搜索區(qū)域如圖2所示。在圖2中,無(wú)人機(jī)每次的搜索區(qū)域是一個(gè)類(lèi)錐體區(qū)域,類(lèi)錐體區(qū)域內(nèi)每一個(gè)滿足約束條件的節(jié)點(diǎn)j組成集合J。計(jì)算集合內(nèi)每個(gè)節(jié)點(diǎn)的評(píng)價(jià)函數(shù)值fj,其中fj=cos{∠jiB},j∈J。將集合J中所有元素加入Open表,清空Shanchu表。

圖2 無(wú)人機(jī)搜索區(qū)域Fig.2 Search area of the UAV

步驟5判斷Open表是否為空。若為空,則執(zhí)行回溯算法,即刪除Jiedian表中的節(jié)點(diǎn)i并記錄到Shanchu表,轉(zhuǎn)步驟2；否則轉(zhuǎn)步驟6。

步驟6對(duì)Open表中節(jié)點(diǎn)的評(píng)價(jià)函數(shù)值進(jìn)行排序,選擇值最大的點(diǎn)j作為下一個(gè)航跡節(jié)點(diǎn)。把該節(jié)點(diǎn)j加入Jiedian表,轉(zhuǎn)步驟2。

3 算例分析

3.1 算例數(shù)據(jù)

已知某飛行區(qū)域內(nèi)共規(guī)劃部署了611個(gè)校正點(diǎn),其中各校正點(diǎn)的空間坐標(biāo)、校正類(lèi)型以及起點(diǎn)與終點(diǎn)的空間坐標(biāo)均為已知的,飛行區(qū)域節(jié)點(diǎn)信息如表1所示,由于節(jié)點(diǎn)較多,表1只列舉了部分節(jié)點(diǎn)的信息。設(shè)定α1=25,α2=15,β1=20,β2=25,θ=30,δ=0.001,ω=90°。

表1 飛行區(qū)域節(jié)點(diǎn)信息Table 1 Node information of flight area

3.2 算例結(jié)果

根據(jù)本文的算法,利用MATLAB 2014b編程,在Intel(R)Core i5-6200U @ 2.30 GHz 2.30 GHz處理器上運(yùn)行程序,經(jīng)過(guò)0.059 s得出近似最優(yōu)解,無(wú)人機(jī)從A點(diǎn)出發(fā),經(jīng)過(guò)8個(gè)校正點(diǎn),飛行106 350.061 m后,到達(dá)B點(diǎn)。經(jīng)過(guò)的校正點(diǎn)編號(hào)和類(lèi)型以及校正前的誤差如表2所示,規(guī)劃出的路徑圖如圖3所示。

表2 航跡規(guī)劃結(jié)果Table 2 Path planning results

為驗(yàn)證本文算法在性能上的優(yōu)勢(shì),利用表1的數(shù)據(jù),對(duì)比A*算法,在相同處理器配置的計(jì)算機(jī)上運(yùn)行算法的驗(yàn)證,兩種算法結(jié)果對(duì)比分析如表3所示。

圖3 航跡規(guī)劃圖Fig.3 Path planning map

表3 算法結(jié)果對(duì)比Table 3 Results comparison of algorithms

由表3可見(jiàn),啟發(fā)式DFS+回溯算法可以得到更好的解,其飛行距離比A*算法減少了4 936.437 m,經(jīng)過(guò)的校正節(jié)點(diǎn)少1個(gè),且求解時(shí)間更短。

3.3 求解結(jié)果優(yōu)化

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

(14)

I：U(1,M)

(15)

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

(16)

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

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

圖4 優(yōu)化迭代結(jié)果Fig.4 Results of optimization iteration

圖5 優(yōu)化后的航跡規(guī)劃圖Fig.5 Optimized path planning map

從圖5的航跡規(guī)劃結(jié)果來(lái)看,無(wú)人機(jī)的航跡更加趨于平滑,與之前的航跡規(guī)劃結(jié)果相比,沒(méi)有出現(xiàn)較大的飛行爬升和俯沖。這是因?yàn)樵诔跏茧A段的解空間中,解的評(píng)價(jià)函數(shù)值相差并不大,選擇非最優(yōu)評(píng)價(jià)函數(shù)的值可以跳出局部最優(yōu)解,使得航跡距離最短,航跡更平滑。

對(duì)3種算法的求解結(jié)果進(jìn)行比較分析,結(jié)果如表4所示。通過(guò)對(duì)比發(fā)現(xiàn),在航跡距離方面,加入模擬退火機(jī)制的啟發(fā)式DFS+回溯算法求解出的航跡距離最短,其長(zhǎng)度為103 887.759 m,比啟發(fā)式DFS+回溯算法節(jié)省了2 462.302 m,比A*算法節(jié)省了7 398.739 m。在求解時(shí)間方面,由于加入模擬退火機(jī)制,優(yōu)化后的啟發(fā)式DFS+回溯算法求解時(shí)間相對(duì)較長(zhǎng),這是因?yàn)樵撍惴ǖ那蠼鈺r(shí)間與迭代次數(shù)有關(guān)?？梢愿鶕?jù)實(shí)際要求對(duì)該算法的迭代次數(shù)進(jìn)行調(diào)整,在求解的時(shí)間和求解精度之間取得較滿意的求解效果。

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

4 結(jié)論

建立了考慮定位誤差的無(wú)人機(jī)航跡規(guī)劃的數(shù)學(xué)模型,設(shè)計(jì)了啟發(fā)式DFS+回溯算法,解決了通過(guò)校正點(diǎn)不斷校正定位誤差的航跡規(guī)劃問(wèn)題,并在此算法的基礎(chǔ)上進(jìn)行了優(yōu)化改進(jìn)。實(shí)驗(yàn)結(jié)果表明,與A*算法相比,本文的啟發(fā)式DFS+回溯算法在求解時(shí)間上具有一定優(yōu)勢(shì),航跡距離結(jié)果更優(yōu)。而加入模擬退火機(jī)制的啟發(fā)式DFS+回溯算法雖然求解時(shí)間變長(zhǎng),但是求解質(zhì)量更高。本文的模型與算法為無(wú)人機(jī)航跡快速規(guī)劃問(wèn)題的研究及應(yīng)用提供了一定的理論依據(jù)。