榮少巍

(昆明船舶設(shè)備研究試驗(yàn)中心 第1研究室,云南 昆明 650051)

基于改進(jìn)A*算法的水下航行器自主搜索航跡規(guī)劃

榮少巍

(昆明船舶設(shè)備研究試驗(yàn)中心 第1研究室,云南 昆明 650051)

以水下航行器在水下路徑規(guī)劃為研究重點(diǎn),提出了基于改進(jìn)型A*算法的水下無人航行器自主搜索航跡規(guī)劃算法。一般航跡規(guī)劃可由多種算法完成,而在這些算法中以A*的計(jì)算流程最為簡單、算法易于實(shí)現(xiàn),并在理論上可保證全局最優(yōu)解的收斂性;且程序較為簡短,可在一些低功耗、低主頻的系統(tǒng)中應(yīng)用。由于傳統(tǒng)的A*算法不具備最小轉(zhuǎn)彎半徑等約束條件,因此,針對水下航行器高低速問題,對傳統(tǒng)的A*算法進(jìn)行改進(jìn),使得A*算法可實(shí)現(xiàn)高速與低速相結(jié)合的應(yīng)用。

水下航行器;A*算法;航跡規(guī)劃

無人飛行器已在航空領(lǐng)域大量應(yīng)用,在水下航行器領(lǐng)域,無人航行器也開始展開應(yīng)用,無人水下航行器得到了關(guān)注。而無人水下航行器在水下如何避障成為了研究水下航行器路徑規(guī)劃的重點(diǎn)。

目前在航跡規(guī)劃領(lǐng)域中,通常有包含A*搜索算法[1-3]、動態(tài)規(guī)劃Dijkstra算法、遺傳算法、粒子群算法、數(shù)學(xué)最優(yōu)規(guī)劃[8]等。在所有這些算法中,A*的計(jì)算流程最為簡單且最易實(shí)現(xiàn),并在理論上可保證全局最優(yōu)解的收斂性。此外,程序也較簡短,可在一些低功耗低主頻的系統(tǒng)中應(yīng)用。因此,A*算法得到了廣泛的應(yīng)用。

本文提出基于改進(jìn)型A*算法的水下無人航行器自主搜索航跡規(guī)劃算法。傳統(tǒng)的A*算法沒有最小轉(zhuǎn)彎半徑等約束等條件。然而水下航行器的運(yùn)動特點(diǎn)卻不同,既有低速、小轉(zhuǎn)向半徑或者零轉(zhuǎn)向半徑的航行器,也有高速大轉(zhuǎn)向半徑的航行器,這一點(diǎn)與傳統(tǒng)的基于無人機(jī)的A*搜索算法不同,因此需要對傳統(tǒng)的A*算法進(jìn)行改進(jìn),使得A*可以實(shí)現(xiàn)高速與低速結(jié)合的應(yīng)用。本算法正是針對小型水下航行器的特點(diǎn)進(jìn)行了改進(jìn)A*算法研究與仿真,該算法可以較好地規(guī)避障礙,并能以最優(yōu)路徑進(jìn)行目標(biāo)搜索,因此具有較好的應(yīng)用前景。

1 A*算法基本原理

A*算法的基本原理是一種啟發(fā)式的圖搜索算法,能夠滿足航跡規(guī)劃中不同約束條件的要求,在滿足這些條件下可計(jì)算得到一個最優(yōu)的路徑解。而且A*算法在理論上可得到一個完全收斂的結(jié)果,且只需最少的擴(kuò)展點(diǎn)數(shù)即可完成計(jì)算,這樣對硬件計(jì)算能力的要求也最低,可利用最少的資源進(jìn)行復(fù)雜的路徑規(guī)劃任務(wù)。

A*算法的代價(jià)函數(shù)表示為

f(n)=g(n)+h(n)

(1)

其中,f(n)是節(jié)點(diǎn)n從初始點(diǎn)到目標(biāo)點(diǎn)的估價(jià)函數(shù);g(n)是在狀態(tài)空間中從初始節(jié)點(diǎn)到n節(jié)點(diǎn)的實(shí)際代價(jià);h(n)是從n到目標(biāo)節(jié)點(diǎn)最佳路徑的估計(jì)代價(jià)。保證找到最短路徑條件,關(guān)鍵在于估價(jià)函數(shù)h(n)的選取:

(1)估價(jià)值h(n)≤n到目標(biāo)節(jié)點(diǎn)的距離實(shí)際值,這種情況下,搜索的點(diǎn)數(shù)多、搜索范圍大、效率低,但能得到最優(yōu)解。

(2)若估價(jià)值>實(shí)際值,搜索的點(diǎn)數(shù)少、搜索范圍小、效率高,但不能保證得到最優(yōu)解。

因此,h(n)也被稱為啟發(fā)函數(shù),故在實(shí)際搜索應(yīng)用中不能需要對啟發(fā)函數(shù)按實(shí)際操作進(jìn)行修正才可能得到一個較好地應(yīng)用。

針對這種啟發(fā)式的搜索方式,在算法中加入了類似于游戲地圖的模式,即close/open模式。這種模式的原理在于,將一個未知需要搜索的地域劃分為N個方格,探測器一次可探測一個方格的內(nèi)容,一旦被探測器探測過的區(qū)域,這個區(qū)域就被記錄于open的數(shù)據(jù)庫中,而未被探測的未知地域則被記錄在close數(shù)據(jù)庫中,通過不重復(fù)的搜索過程,最終通過最優(yōu)路徑到達(dá)目的地。

2 高速水下航行器運(yùn)動建模

傳統(tǒng)的A*算法將一個區(qū)域劃分為方塊區(qū)域,因此一個點(diǎn)就有8個區(qū)域方向或是擴(kuò)展為24個區(qū)域方向可選的路徑。如圖1和圖2所示。

圖1 8方向分割圖

圖2 24方向分割圖

8方向與24方向都是沒有考慮高速運(yùn)動物體的運(yùn)動特性而確定的理想方式,這種模式針對低速或零轉(zhuǎn)向半徑的航行器可適用,而針對高速水下航行器則不能較好地運(yùn)用。因此針對高速水下航行器,需要對其運(yùn)動進(jìn)行建模,然后確定其方向狀態(tài)模式。

搜索區(qū)域?yàn)橐粋€較大的區(qū)域,而水下航行器相對于搜索區(qū)域來說顯得較小,可認(rèn)定為一個質(zhì)點(diǎn)。

水下航行器的實(shí)際運(yùn)動方程較為復(fù)雜,在路徑規(guī)劃中無需如此復(fù)雜的運(yùn)動方程,因此進(jìn)行了簡化。設(shè)某一水下航行器在一固定深度航行,其運(yùn)動方程為

θn+1=θn+θΔ

(2)

xn+1=xn+Ccos(θΔ+1)

(3)

yn+1=yn+Ccos(θΔ+1)

(4)

式(2)中,θn為當(dāng)前水下航行器的航向角;θn+1為下一時刻的航向角;xn+1,yn+1為下一時刻的坐標(biāo),這個坐標(biāo)的變化量為前一時刻坐標(biāo)加上航向角的變量,如圖3所示。

圖3 運(yùn)動位置改變量示意圖

而高速的物體在水下進(jìn)行轉(zhuǎn)向則需要較大的轉(zhuǎn)向半徑,最小轉(zhuǎn)向半徑的近似計(jì)算公式為

(5)

θmax=arcsin(Co/(2Rmin))

(6)

式(6)表明了最大航向角改變量與最小轉(zhuǎn)向半徑成非線性反比關(guān)系。

圖4 最大航向角改變量與最小轉(zhuǎn)向半徑計(jì)算示意

3 改進(jìn)A*搜索算法

A*搜索算法的代價(jià)函數(shù)由式(1)給出,其中g(shù)(n)為水下航行器運(yùn)動的起點(diǎn)至目前n點(diǎn)位置的真實(shí)代價(jià),再加入一個最小的估計(jì)代價(jià)函數(shù)h(n),就可以得到完整的代價(jià)函數(shù)。其中真實(shí)代價(jià)函數(shù)表示為

(7)

式(7)中,l為當(dāng)前路徑的長度,真實(shí)的代價(jià)函數(shù)就表示為總的路徑長度。

而在一個區(qū)域中,如果進(jìn)行搜索前進(jìn),則在全局中的最優(yōu)解必然是兩點(diǎn)之間的直線為最優(yōu)解,表示為

(8)

采用式(8)的兩點(diǎn)間直線距離公式作為A*算法的啟發(fā)函數(shù),這樣可以保證航行器在理論上從起點(diǎn)至終點(diǎn)的航行距離最小,即代價(jià)函數(shù)的值最小。

改進(jìn)A*算法的基本的基本計(jì)算步驟為:

首先確定目標(biāo)位置,根據(jù)目標(biāo)位置,確定航行器的起點(diǎn)為起始節(jié)點(diǎn),目標(biāo)位置為終止節(jié)點(diǎn),每個節(jié)點(diǎn)的大小由航行器避碰的探測能力確定,航行器航行路徑為式(8)確定的最優(yōu)解。

再次,將航行路徑中的所有區(qū)域設(shè)定為未知區(qū)域節(jié)點(diǎn),放入close數(shù)據(jù)庫中;當(dāng)航行器經(jīng)過任意一個區(qū)域節(jié)點(diǎn),該節(jié)點(diǎn)就放置于open數(shù)據(jù)庫中。

如果在探測過程中發(fā)現(xiàn)前方有節(jié)點(diǎn)為障礙節(jié)點(diǎn)時,應(yīng)當(dāng)沿障礙節(jié)點(diǎn)的邊沿進(jìn)行規(guī)避,規(guī)避過程中需根據(jù)運(yùn)動方程所確定的最小轉(zhuǎn)向半徑進(jìn)行規(guī)避,如果是擁有零轉(zhuǎn)向半徑的水下航行器,則可沿直角或任意角度進(jìn)行轉(zhuǎn)向。

在進(jìn)行轉(zhuǎn)向前,再次對路徑進(jìn)行規(guī)劃,首先以當(dāng)前節(jié)點(diǎn)作為起點(diǎn),以最小代價(jià)函數(shù)為路徑,再次判斷至下一節(jié)點(diǎn)能否以直線前進(jìn),而不與障礙相交,若通路的直線視線沒有被遮擋,則可直接通過,無需進(jìn)行轉(zhuǎn)向,沿直向前行,確保路徑最短,路徑優(yōu)化原理如圖5所示。

圖5 路徑優(yōu)化原理

4 算法仿真

算法仿真采用Matlab進(jìn)行,圖6為整個仿真的區(qū)域圖,其中圓點(diǎn)區(qū)域?yàn)檎系K區(qū)域,+字符號為水下航行器前進(jìn)路徑,其余為無障礙水域。在仿真設(shè)置中,加入了橫向縱向的障礙,且障礙交替出現(xiàn),由起點(diǎn)至終點(diǎn)無法通過單一直線路徑到達(dá),需經(jīng)過多次轉(zhuǎn)向才能到達(dá),因此增加了路徑規(guī)劃的難度,可較好地對高速低速水下航行器路徑規(guī)劃進(jìn)行仿真。路徑規(guī)劃仿真結(jié)果如圖6所示。

圖6 路徑規(guī)劃仿真圖

在仿真過程中采用未優(yōu)化的傳統(tǒng)A*算法仿真的路徑如圖7所示,其中出現(xiàn)一些很大的直接轉(zhuǎn)向路徑,這也模擬了低速或零轉(zhuǎn)向半徑的水下航行器的運(yùn)動軌跡。從圖中可看出,轉(zhuǎn)向出現(xiàn)了許多折線部分,這在低速與零轉(zhuǎn)向半徑的航行器可使用,但高速物體的轉(zhuǎn)向不能出現(xiàn)角度較大的折線。

圖7 未優(yōu)化路徑結(jié)果輸出

優(yōu)化前后的高速水下航行器路徑規(guī)劃算法仿真如圖8所示,實(shí)線為傳統(tǒng)算法加入部分優(yōu)化后的路線,從中還是可看到較為大幅的轉(zhuǎn)向,這個在實(shí)際當(dāng)中不能較好地為高速航行器進(jìn)行路徑規(guī)劃,而虛線則利用改進(jìn)A*算法結(jié)合高速航行器運(yùn)動方程進(jìn)行優(yōu)化的路徑,這里面完全沒有了折線的出現(xiàn),且曲線也較實(shí)線平滑,更適合于高速水下航行器的運(yùn)動。

圖8 優(yōu)化前后高速航行器路徑結(jié)果輸出

5 結(jié)束語

本文敘述了基于改進(jìn)型A*算法的水下無人航行器自主搜索航跡規(guī)劃算法。克服了傳統(tǒng)A*算法沒有最小轉(zhuǎn)彎半徑等約束等條件的缺陷。結(jié)合水下航行器的運(yùn)動方程的特點(diǎn),對傳統(tǒng)的A*算法進(jìn)行改進(jìn),使得A*可實(shí)現(xiàn)高速與低速結(jié)合的應(yīng)用。本算法針對小型水下航行器的特點(diǎn)進(jìn)行了改進(jìn)A*算法的研究與仿真,該算法可較好地規(guī)避障礙,并能以最優(yōu)路徑進(jìn)行目標(biāo)搜索,具有較好的應(yīng)用前景。

[1]SenthilArumugamM,RaoMVC,AarthiChandramohan.Anewandimprovedversionofparticleswarmoptimizationalgorithmwithglobal-localbestparameters[C].KnowlInformationsystem,2008(16):331-357.

[2]SzczerbaRJ.Robustalgorithmforalgorithmforrealtimerouteplanning[J].IEEETransactiononAerospaceandElectronicSystem,2000,36(3):869-878.

[3] 符小衛(wèi),高小光.一種無人機(jī)路徑規(guī)劃算法研究[J].系統(tǒng)仿真學(xué)報(bào),2004,16(1):20-21.

[4] 諸靜.模糊控制原理及應(yīng)用[M].北京:機(jī)械工業(yè)出版社,1999.

[5] 徐德民.魚雷自動控制系統(tǒng)[M].2版.西安:西北工業(yè)大學(xué)出版社,2001.

[6]ChenLD,ToruS.Dataminingmethods,applications,andtools[J].InformationSystemManagement,2000,17(1):65-70.

[7] 杜萍,楊春.飛行器航跡規(guī)劃算法綜述[J].飛行力學(xué),2005,23(2):10-14.

[8] 張旭東,李文龍,胡良梅,等.基于PMD相機(jī)的特征跟蹤位姿測量方法[J].電子測量與儀器學(xué)報(bào),2013(7):26-30.

[9] 馬云紅,周德云.基于B樣條曲線的無人機(jī)航路規(guī)劃算法[J].飛行力學(xué),2004,22(2):74-77.

[10]鄭睿;孟曉風(fēng);聶晶,等.基于VXI總線的QCM測試系統(tǒng)設(shè)計(jì)與實(shí)現(xiàn)[J].電子測量與儀器學(xué)報(bào),2012(8):77-79.

[11]WilliamsWJ,JeongJ.Newtime-frequencydistribution:Theoryandapplications[C].ProceedingofIEEEICASSP-89,1989:1243-1247.

Research of Underwater Autonomous Search Path Planning Based on Improved A*Algorithm

RONG Shaowei

(First laboratory,Kunming Shipborne Equipment Research & Test Center,Kunming 650051,China)

This paper studies the underwater path planning of the underwater vehicle.It puts forward an underwater unmanned underwater vehicle autonomous search path planning algorithm based on the improved A*algorithm.The general path planning can be done by a variety of algorithms.Among them,the A*algorithm is simple and easy to realize,and can theoretically guarantee the convergence of the global optimal solution;and its program is simple,so that it can be used in the system of low power consumption and low frequency.Because the traditional A*algorithm does not have the minimum turning radius and other constraints,we improve the traditional A*algorithm since underwater vehicles run either at a high or at a low speed,so that it can be applied in both cases.

Underwater vehicle;A*algorithm;path planning

2014- 08- 28

榮少巍(1986—),男,碩士,助理工程師。研究方向:嵌入式系統(tǒng)。E-mail:formulaf11986@163.com

10.16180/j.cnki.issn1007-7820.2015.04.005

TP306.1

A

1007-7820(2015)04-017-04