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

榮少巍

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

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

榮少巍

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

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

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

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

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

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

1 A*算法基本原理

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

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

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

(1)

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

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

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

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

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

2 高速水下航行器運動建模

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

圖1 8方向分割圖

圖2 24方向分割圖

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

搜索區(qū)域為一個較大的區(qū)域,而水下航行器相對于搜索區(qū)域來說顯得較小,可認定為一個質(zhì)點。

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

θn+1=θn+θΔ

(2)

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

(3)

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

(4)

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

圖3 運動位置改變量示意圖

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

(5)

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

(6)

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

圖4 最大航向角改變量與最小轉向半徑計算示意

3 改進A*搜索算法

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

(7)

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

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

(8)

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

改進A*算法的基本的基本計算步驟為:

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

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

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

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

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

4 算法仿真

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

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

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

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

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

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

5 結束語

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

[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),高小光.一種無人機路徑規(guī)劃算法研究[J].系統(tǒng)仿真學報,2004,16(1):20-21.

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

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

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

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

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

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

[10]鄭睿;孟曉風;聶晶,等.基于VXI總線的QCM測試系統(tǒng)設計與實現(xiàn)[J].電子測量與儀器學報,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