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

榮少巍

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

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

榮少巍

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

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

水下航行器;A*算法;航跡規劃

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

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

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

1 A*算法基本原理

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

A*算法的代價函數表示為

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

(1)

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

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

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

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

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

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

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

圖1 8方向分割圖

圖2 24方向分割圖

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

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

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

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

(7)

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

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

(8)

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

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

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

再次,將航行路徑中的所有區域設定為未知區域節點,放入close數據庫中;當航行器經過任意一個區域節點,該節點就放置于open數據庫中。

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

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

圖5 路徑優化原理

4 算法仿真

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

圖6 路徑規劃仿真圖

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

圖7 未優化路徑結果輸出

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

圖8 優化前后高速航行器路徑結果輸出

5 結束語

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

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

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

[3] 符小衛,高小光.一種無人機路徑規劃算法研究[J].系統仿真學報,2004,16(1):20-21.

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

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

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

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

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

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

[10]鄭睿;孟曉風;聶晶,等.基于VXI總線的QCM測試系統設計與實現[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—),男,碩士,助理工程師。研究方向:嵌入式系統。E-mail:formulaf11986@163.com

10.16180/j.cnki.issn1007-7820.2015.04.005

TP306.1

A

1007-7820(2015)04-017-04