基于稀疏A*算法的AUV全局路徑規(guī)劃

陳 實(shí), 劉純武, 黃芝平, 蔡郭汕

?

基于稀疏A*算法的AUV全局路徑規(guī)劃

陳 實(shí), 劉純武, 黃芝平, 蔡郭汕

(國(guó)防科技大學(xué) 機(jī)電工程與自動(dòng)化學(xué)院, 湖南 長(zhǎng)沙, 410073)

路徑規(guī)劃是自主式水下航行器(AUV)研究領(lǐng)域的重要課題之一。傳統(tǒng)的AUV路徑規(guī)劃算法, 如人工勢(shì)場(chǎng)法、圖搜索法等, 容易出現(xiàn)陷入局部最優(yōu)解、計(jì)算速度慢等問題, 為克服上述缺陷, 本文基于稀疏A*算法, 提出了一種新的用于構(gòu)造搜索空間的隨機(jī)布點(diǎn)方法, 在路徑規(guī)劃區(qū)域內(nèi), 利用隨機(jī)函數(shù)均勻地布撒足夠多的搜索節(jié)點(diǎn), 從而構(gòu)成搜索空間, 可顯著降低計(jì)算量, 提高搜索效率; 并進(jìn)一步對(duì)所得路徑進(jìn)行通視性檢查, 有效地減少路徑點(diǎn)個(gè)數(shù)和折點(diǎn)數(shù), 獲得更優(yōu)路徑。仿真試驗(yàn)結(jié)果驗(yàn)證了該算法的正確性和有效性, 表明該算法具有全局優(yōu)化能力強(qiáng)、計(jì)算速度快的優(yōu)點(diǎn), 具有一定的工程應(yīng)用價(jià)值。

自主式水下航行器; 路徑規(guī)劃; 稀疏A*算法; 路徑優(yōu)化

0 引言

自主式水下航行器(autonomous underwater vehicle, AUV)作為一種高技術(shù)手段, 在海洋這塊未來極具價(jià)值的發(fā)展空間中起著至關(guān)重要的作用, 而路徑規(guī)劃是自主式水下航行器的關(guān)鍵技術(shù)之一, 是自主式水下航行器能在所處的環(huán)境中安全、有效地行駛, 順利完成給定任務(wù)的重要保證[1-2]。AUV全局路徑規(guī)劃本質(zhì)是在規(guī)劃區(qū)域內(nèi), 在給定的障礙物和約束條件下, 尋找一條從起始點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)或可行的路徑。傳統(tǒng)的路徑規(guī)劃算法, 如人工勢(shì)場(chǎng)法、圖搜索法等, 大多有一些不足之處, 如容易陷入局部極小及計(jì)算量大等。目前, 基于智能控制理論的方法, 如啟發(fā)式搜索算法、粒子群算法[3]及遺傳算法[2]等, 由于具有優(yōu)良的魯棒性, 得到了廣泛的研究與應(yīng)用。

Szczerba等人在2000年提出了一種改進(jìn)的A*算法, 稱為稀疏A* (sparse A* search, SAS)算法[4]。該算法通過把約束條件結(jié)合到基本A*算法中去, 可以有效地修剪搜索空間中的無用節(jié)點(diǎn), 從而大大縮短搜索時(shí)間[5]。近幾年, 國(guó)內(nèi)學(xué)者將SAS算法主要應(yīng)用于飛行器的航跡規(guī)劃上。文獻(xiàn)[6]利用改進(jìn)的啟發(fā)式SAS算法獲得的飛行器航跡具有良好的地形跟隨、規(guī)避和威脅規(guī)避能力。文獻(xiàn)[7]提出了改進(jìn)A*算法并應(yīng)用于飛行器航跡規(guī)劃, 該算法把地形平滑技術(shù)融合到路徑搜索的過程中, 使得到的最優(yōu)航跡更加貼近地形。文獻(xiàn)[8]將SAS算法和人工勢(shì)場(chǎng)法結(jié)合起來, 運(yùn)用到無人機(jī)動(dòng)態(tài)航跡規(guī)劃中, 能夠規(guī)劃出給定威脅指標(biāo)下的全局最優(yōu)路徑并達(dá)到良好的動(dòng)態(tài)規(guī)避性能。

本文在以隨機(jī)布點(diǎn)的方式構(gòu)造搜索空間的基礎(chǔ)上, 采用SAS算法進(jìn)行AUV全局路徑規(guī)劃, 充分利用約束條件來進(jìn)一步減小搜索空間, 并結(jié)合通視性檢查方法使路徑最優(yōu), 可以有效地解決傳統(tǒng)AUV路徑規(guī)劃算法容易陷入局部最優(yōu)點(diǎn)、計(jì)算量大等問題。

1 環(huán)境模型

針對(duì)所研究問題的特點(diǎn), 本文在環(huán)境模型上進(jìn)行如下假設(shè)。

1) 僅在2D空間下進(jìn)行規(guī)劃, 不考慮障礙物的高度信息;

2) 將AUV視為質(zhì)點(diǎn), 相應(yīng)的障礙物作了一定的拓展;

3) 所有障礙物均為靜止的。

傳統(tǒng)A*算法用網(wǎng)格單元進(jìn)行路徑節(jié)點(diǎn)擴(kuò)展, 如果采用的網(wǎng)格圖比較大, 那么整個(gè)搜索過程需要計(jì)算的節(jié)點(diǎn)數(shù)就非常多, 要收斂到最優(yōu)解需要很長(zhǎng)的時(shí)間。

為了減少需計(jì)算的節(jié)點(diǎn)數(shù), 縮短收斂時(shí)間, 本文提出一種隨機(jī)布點(diǎn)的方法產(chǎn)生所需的搜索節(jié)點(diǎn)。如圖1所示, 10個(gè)圓形障礙物周圍的小點(diǎn)為隨機(jī)布置的搜索節(jié)點(diǎn)。在進(jìn)行A*算法搜索之前, 利用隨機(jī)函數(shù)

在×大小的規(guī)劃圖上產(chǎn)生個(gè)節(jié)點(diǎn), 并且產(chǎn)生的節(jié)點(diǎn)不在障礙物內(nèi), 所產(chǎn)生的節(jié)點(diǎn)數(shù)可根據(jù)情況設(shè)定。該方法的優(yōu)點(diǎn)就是可以在規(guī)劃時(shí)間和路徑的質(zhì)量之間進(jìn)行權(quán)衡, 節(jié)點(diǎn)數(shù)越多, 得到最優(yōu)路徑的可能性越大, 但計(jì)算時(shí)間也越長(zhǎng)。

圖1 隨機(jī)布置搜索節(jié)點(diǎn)示意圖

Fig. 1 Schematic of randomly distributing search nodes

2 基于SAS算法的路徑規(guī)劃

2.1 SAS算法的約束條件

A*算法[9]是一種啟發(fā)式搜索算法, 它在搜索過程中引入啟發(fā)信息, 對(duì)當(dāng)前節(jié)點(diǎn)的每一個(gè)可能到達(dá)的節(jié)點(diǎn)計(jì)算代價(jià), 然后選擇最低代價(jià)的節(jié)點(diǎn)加入搜索空間來探索。加入搜索空間的這一新節(jié)點(diǎn)又被用來產(chǎn)生更多的可能路徑。SAS算法是標(biāo)準(zhǔn)啟發(fā)式搜索A*算法的一種改進(jìn)形式。該算法通過把約束條件加入到搜索中, 能有效地裁剪掉搜索空間中的無效節(jié)點(diǎn), 從而大大縮短搜索時(shí)間。本文主要考慮2種約束, 即最大轉(zhuǎn)彎角約束和最大路徑長(zhǎng)度約束。

1) 最大轉(zhuǎn)彎角約束

假定給定了起始點(diǎn)和目標(biāo)點(diǎn), 一條從起始點(diǎn)到當(dāng)前節(jié)點(diǎn)的路徑和最大轉(zhuǎn)彎角, 那么從當(dāng)前節(jié)點(diǎn)在最大轉(zhuǎn)彎角()約束之內(nèi)能夠到達(dá)的就只有有限數(shù)量的節(jié)點(diǎn)單元, 如圖2所示。當(dāng)前節(jié)點(diǎn)的搜索空間張角為2。

圖2 可行搜索空間示意圖

2) 最大路徑長(zhǎng)度約束

最大路徑長(zhǎng)度約束[8]指的是AUV完成某一特定任務(wù)所能行駛的最長(zhǎng)距離。在搜索過程中長(zhǎng)度大于這個(gè)距離(記為max)的路徑被認(rèn)為無效。

如圖3所示,是一個(gè)當(dāng)前節(jié)點(diǎn),()是從起始點(diǎn)到經(jīng)過的真實(shí)距離,()是從到目標(biāo)點(diǎn)的直線距離, 這個(gè)直線距離小于航跡實(shí)際要經(jīng)過的路徑長(zhǎng)度, 滿足可接納性條件, 因此可以加入到當(dāng)前路徑長(zhǎng)度。則約束條件如式(2)所示。

圖3 路徑總長(zhǎng)度示意圖

式中,max的使用可以產(chǎn)生更直的路徑, 因?yàn)閙ax限制了路徑中可能的轉(zhuǎn)彎次數(shù)。而且, 一個(gè)較小的max值可以顯著提高搜索速度, 因?yàn)樗阉鬟^程中需要考慮的節(jié)點(diǎn)更少了。

2.2 路徑規(guī)劃代價(jià)函數(shù)

A*算法采用的代價(jià)函數(shù)()對(duì)節(jié)點(diǎn)進(jìn)行評(píng)估, 其形式如式(3)所示

其中,()為從起始位置到當(dāng)前節(jié)點(diǎn)的真實(shí)代價(jià),()為啟發(fā)式函數(shù), 表示從當(dāng)前節(jié)點(diǎn)到目標(biāo)位置代價(jià)的估計(jì)。在A*擴(kuò)展的每一步都將選擇具有最小()值的節(jié)點(diǎn)插入到可能路徑的鏈表中。根據(jù)AUV的實(shí)際情況, 本文采用如式(4)的真實(shí)代價(jià)函數(shù)

式中,l為第段路徑的長(zhǎng)度。表示AUV從起始點(diǎn)到當(dāng)前節(jié)點(diǎn)經(jīng)過的實(shí)際距離。

啟發(fā)函數(shù)需要滿足可接納性和一致性的條件才能使A* 算法收斂到既滿足全局最優(yōu)又滿足局部最優(yōu)的解[10]。本文采用由當(dāng)前節(jié)點(diǎn)到目標(biāo)點(diǎn)之間的直線距離作為啟發(fā)函數(shù), 即

這一啟發(fā)函數(shù)是從擴(kuò)展節(jié)點(diǎn)到目標(biāo)點(diǎn)實(shí)際代價(jià)的下限, 既滿足可接納性, 又滿足一致性。

2.3 算法描述

采用A*算法需要設(shè)置2個(gè)數(shù)據(jù)結(jié)構(gòu): OPEN表和CLOSED表。OPEN表用于存放正在等待擴(kuò)展的節(jié)點(diǎn), CLOSED表用于存放已被擴(kuò)展的節(jié)點(diǎn)。

AUV路徑規(guī)劃算法的具體步驟如下。

Step 1: 將起始點(diǎn)插入OPEN表中, 將CLOSED表置空;

Step 2: 如果OPEN表為空, 算法以搜索失敗結(jié)束。調(diào)整算法參數(shù), 然后重新運(yùn)行規(guī)劃算法;

Step 3: 從OPEN表中移出代價(jià)最小的節(jié)點(diǎn)作為當(dāng)前節(jié)點(diǎn), 將它放入CLOSED表中;

Step 4: 若當(dāng)前節(jié)點(diǎn)是目標(biāo)點(diǎn), 則路徑搜索過程結(jié)束。從目標(biāo)點(diǎn)開始向上回溯直到起始點(diǎn), 得到從起始點(diǎn)到目標(biāo)點(diǎn)的最小代價(jià)路徑;

Step 5: 對(duì)當(dāng)前節(jié)點(diǎn)周圍的節(jié)點(diǎn)進(jìn)行判斷, 如滿足最大轉(zhuǎn)彎角約束和最大路徑長(zhǎng)度約束, 則將其作為一個(gè)可擴(kuò)展節(jié)點(diǎn)計(jì)算其代價(jià)函數(shù), 否則舍棄。對(duì)于選擇的可擴(kuò)展節(jié)點(diǎn)作如下處理。

1) 如果該節(jié)點(diǎn)不在OPEN表和CLOSED表中, 則添加到OPEN表中, 并將它的父節(jié)點(diǎn)指針指向當(dāng)前節(jié)點(diǎn);

2) 如果該節(jié)點(diǎn)已在OPEN表中, 則比較其代價(jià)函數(shù)值與該節(jié)點(diǎn)在OPEN表中的原代價(jià)函數(shù)值, 如果較小, 則記錄新的代價(jià)函數(shù)值并將它的父節(jié)點(diǎn)指針指向當(dāng)前節(jié)點(diǎn);

3) 如果該節(jié)點(diǎn)在CLOSED表中, 則跳過它繼續(xù)擴(kuò)展其節(jié)點(diǎn)。

Step 6: 轉(zhuǎn)Step 3, 繼續(xù)循環(huán), 直到找到解或無解退出。

2.4 路徑優(yōu)化

由于隨機(jī)布點(diǎn)方法的使用, 使得搜索節(jié)點(diǎn)相互之間的距離大小不一, 有些節(jié)點(diǎn)靠得比較近, 有些則離得比較遠(yuǎn), 所以采用A*算法搜索到的路徑一般比實(shí)際的路徑要長(zhǎng), 有必要對(duì)路徑進(jìn)一步優(yōu)化, 從而獲得實(shí)際的路徑。

本文提出一種通視性檢查方法: 從已產(chǎn)生路徑的起始點(diǎn)開始, 將起始點(diǎn)作為當(dāng)前節(jié)點(diǎn), 依次檢查當(dāng)前節(jié)點(diǎn)是否能夠與路徑中的其他節(jié)點(diǎn)通視(即兩點(diǎn)之間的線段不與障礙物相交)。若能夠通視, 則保留該節(jié)點(diǎn), 并刪掉2個(gè)通視節(jié)點(diǎn)之間的其他節(jié)點(diǎn); 若不能, 則將下一節(jié)點(diǎn)作為當(dāng)前節(jié)點(diǎn), 繼續(xù)進(jìn)行檢查, 依次類推。圖4中的虛線顯示了路徑優(yōu)化后的效果, 顯示了該方法能夠有效地減少路徑節(jié)點(diǎn), 縮短路徑的長(zhǎng)度。

圖4 通視性檢查示意圖

3 路徑規(guī)劃仿真試驗(yàn)

仿真采用Visual C++ 6.0進(jìn)行編程, 運(yùn)行系統(tǒng)為Windows 2003, 主頻為2.20 GHz。假設(shè)AUV在4 km2的水域執(zhí)行任務(wù), 即規(guī)劃區(qū)域的大小為2 000 m×2 000 m, 出發(fā)點(diǎn)坐標(biāo)為(0,0), 目標(biāo)點(diǎn)坐標(biāo)為(2 000, 2 000)。在試驗(yàn)中, 最大轉(zhuǎn)彎角=60°,max取值為起始點(diǎn)和目標(biāo)點(diǎn)之間直線距離的1.3倍。

圖5和圖6顯示了仿真試驗(yàn)的結(jié)果(隨機(jī)布置的點(diǎn)未顯示, 其中=2 000), 圖中圓形表示障礙物, 曲線為規(guī)劃得到的路徑。其中, 圖5是未采用通視性檢查方法所獲得的路徑, 圖6是采用通視性檢查方法優(yōu)化后的路徑。

通過圖5和圖6的對(duì)比可以看出, 在采用隨機(jī)布點(diǎn)減少搜索節(jié)點(diǎn)的前提下, 只采用SAS算法(如圖5所示)進(jìn)行規(guī)劃, 出現(xiàn)了路徑比較曲折的問題, 若AUV采用此種路徑規(guī)劃, 將會(huì)增加路徑的規(guī)劃點(diǎn)數(shù), 從而增加了AUV的航程; 而采用通視性檢查方法進(jìn)行優(yōu)化后(如圖6所示), 就有效避免了該問題的發(fā)生, 減少了AUV的規(guī)劃路徑點(diǎn), 提高了AUV路徑規(guī)劃的效果。

圖5 優(yōu)化前的路徑圖

圖6 優(yōu)化后的路徑圖

表1是利用2 000 m×2 000 m大小的規(guī)劃區(qū)域?qū)Φ湫偷幕跂鸥穹?gòu)造搜索空間的SAS和本文所采用的基于隨機(jī)布點(diǎn)構(gòu)造搜索空間的SAS進(jìn)行比較仿真的結(jié)果。其中柵格法的柵格數(shù)設(shè)為200×200, 而隨機(jī)布點(diǎn)法的節(jié)點(diǎn)數(shù)分別取=500,=1 000,=1 500,=2 000,=2 500。從表中可以看出, 與典型柵格法的SAS相比, 本文所采用的方法在保證所得的路徑趨于最優(yōu)的情況下, 計(jì)算速度得到了顯著的提高。

表1 2種方法的比較結(jié)果

Table 1 Comparison between two methods

4 結(jié)論

本文在采用隨機(jī)布撒搜索節(jié)點(diǎn)的基礎(chǔ)上, 采用SAS算法進(jìn)行AUV全局路徑規(guī)劃, 為了獲得更優(yōu)的實(shí)際幾何路徑, 提出了一種通視性檢查方法, 該方法減少了AUV的規(guī)劃路徑點(diǎn), 提高了AUV路徑規(guī)劃的效果。仿真試驗(yàn)結(jié)果表明, 本文提出的方法能夠達(dá)到良好的全局路徑規(guī)劃效果, 并且顯著提高了計(jì)算速度。

[1] 趙濤, 劉明雍, 周良榮. 自主水下航行器的研究現(xiàn)狀與挑戰(zhàn)[J]. 火力與指揮控制, 2010, 35(6): 1-6. Zhao Tao, Liu Ming-yong, Zhou Liang-rong. A Survey of Autonomous Underwater Vehicle Recent Advances and Future Challenges[J]. Fire Control & Command Control, 2010, 35(6): 1-6.

[2] 王軍. 基于改進(jìn)雙種群遺傳算法的AUV路徑規(guī)劃方法研究[J]. 控制理論與應(yīng)用, 2010, 29(6): 13-16. Wang Jun. Improved Double Populations Genetic Algorithm Based on Path Planning for AUV[J]. Control Theory and Applications, 2010, 29(6): 13-16.

[3] 初磊, 紀(jì)金耀, 羅笛. 基于一種改進(jìn)粒子群算法的水下遠(yuǎn)程武器航路規(guī)劃[J]. 魚雷技術(shù), 2011, 19(3): 201-204. Chu Lei, Ji Jin-yao, Luo Di. Route Planning of Underwater Long-range Weapon Based on an Improved PSO Algorithm [J]. Torpedo Thchnology, 2011, 19(3): 201-204.

[4] Szczerba R J. Robust Algorithm for Algorithm for Real time Route Planning[J]. IEEE Transaction on Aerospace and Electronic System, 2000, 36 (3): 869-878.

[5] 李春華, 鄭昌文, 周成平, 等. 一種三維航跡快速搜索方法[J]. 宇航學(xué)報(bào), 2002, 23(3): 13-17. Li Chun-hua, Zheng Chang-wen, Zhou Cheng-ping, et al. Fast Search Algorithm for 3D-routh Planning[J]. Journal of Astronautics, 2002, 23(3): 13-17.

[6] 李銳, 劉占辰, 荊獻(xiàn)勇. 基于啟發(fā)式算法的無人機(jī)三維航跡規(guī)劃仿真研究[J]. 電光與控制, 2009, 16(8): 27-31. Li Rui, Liu Zhan-chen, Jing Xian-yong. On Simulation of 3-D Routh Planning for UCAV Based on Heuristic Algorithm[J]. Electronics Optics & Control, 2009, 16(8): 27-31.

[7] 任波, 周燾, 于雷. 基于改進(jìn) A*算法的飛行器三維航跡規(guī)劃算法[J]. 系統(tǒng)工程與電子技術(shù), 2008, 30(2): 324-326. Ren Bo, Zhou Tao, Yu Lei. Study on Path Planning for Aircraft Based on Improved A*algorithm[J]. Systems Engineering and Electronics, 2008, 30(2): 324-326.

[8] 姚遠(yuǎn), 周興社, 張凱龍, 等. 基于稀疏A*搜索和改進(jìn)人工勢(shì)場(chǎng)的無人機(jī)動(dòng)態(tài)航跡規(guī)劃[J]. 控制理論與應(yīng)用, 2010, 27(7): 953-959. Yao Yuan, Zhou Xing-she, Zhang Kai-long, et al. Dynamic Trajectory Planning for Unmanned Aerial Vehicle Based on Sparse A* Search and Improved Artificial Potential Field[J]. Control Theory and Applications, 2010, 27(7): 953-959.

[9] Mitchell J S B, Keirney D M. Planning Strategic Path Through Variable Terrain Data[C]//Proceedings of the Applications of Arti?cial Intelligence. USA: SPIE Press, 1984: 172-179.

[10] Nils J N. Artificial Intelligence a New Synthesis[M]. San Mateo: Morgan Kaufmann Publisher, Inc, 1998.

Global Path Planning for AUV Based on Sparse A* Search Algorithm

CHEN Shi, LIU Chun-wu, HUANG Zhi-ping, CAI Guo-shan

(College of Mechatronics and Automation, National University of Defense Technology College, Changsha 410073, China )

Classical autonomous underwater vehicle (AUV) path planning algorithms, such as artificial potential field method and graph search algorithm, often result in the problems of easily converging on local optimum and low calculation speed. To solve the problems, a new method for distributing random points is proposed based on the sparse A* search algorithm for constructing search space, where a random function is used to evenly distribute enough search nodes in the path planning area. This method can obviously reduce the calculation work and increase the search efficiency. After the original path is deduced, an intervisibility test is conducted to reduce the path turning points and get an optimal path. Simulation result shows that the proposed method is feasible and valid, and it features better global optimization and higher calculation speed.

autonomous underwater vehicle(AUV); path planning; sparse A* search algorithm; path optimization.

TJ630.33; TP301.6

A

1673-1948(2012)04-0271-05

2012-01-07;

2012-03-07.

陳 實(shí)(1986-), 男, 在讀碩士, 研究方向?yàn)閿?shù)字化測(cè)試技術(shù).

(責(zé)任編輯: 楊力軍)