一種改進A*算法在無人船路徑規(guī)劃中的應(yīng)用

陳 新，袁宇浩，饒 丹

(南京工業(yè)大學(xué)電氣工程與控制科學(xué)學(xué)院，江蘇南京 211816)

1 引言

水面無人船，可以簡稱為(USV)，其主要用于軍事作戰(zhàn)、海上監(jiān)視巡航、海洋環(huán)境等領(lǐng)域。作為監(jiān)測海洋環(huán)境、維護海洋權(quán)益的現(xiàn)代化裝備，具有廣闊的應(yīng)用前景[1]。當(dāng)前，美國、以色列等國在這方面的研究已經(jīng)取得了一定成果，結(jié)合美國軍方出版的“美國國防部無人系統(tǒng)綜合路線圖”和美國海軍出版的“海軍無人船總體計劃”[2]相結(jié)合，USV研究所涉及的關(guān)鍵技術(shù)主要包括：自動路徑生成與路徑規(guī)劃技術(shù)、自主決策與避碰技術(shù)、水面目標(biāo)檢測與目標(biāo)自動識別技術(shù)以及通信技術(shù)[3]。

自動路徑生成和路徑規(guī)劃技術(shù)主要研究USV全局和局部路徑規(guī)劃，全局路徑規(guī)劃基于完全信息，而局部路徑規(guī)劃基于傳感器獲取的信息[4]。當(dāng)前路徑規(guī)劃的方法主要有A*算法、人工勢場法、遺傳算法和蟻群算法等；在局部路徑規(guī)劃方面最近也產(chǎn)生了一些新的方法，例如強化學(xué)習(xí)法、D*算法等[5]。目前，國內(nèi)外相關(guān)學(xué)者都在對USV的全局路徑規(guī)劃方法進行深入研究，并已取得一定的成果。盧艷爽采用A*算法結(jié)合海事規(guī)則實現(xiàn)全局路徑規(guī)劃[6]；饒森應(yīng)用激活值和遺傳算法，利用分層思想完成了全局路徑規(guī)劃[7]；莊家園提出了一種基于電子海圖的距離尋優(yōu)Dijkstra算法，該算法可以減少規(guī)劃時間，提高規(guī)劃精度并產(chǎn)生安全合理的路徑[8]。Hanguen Kim在考慮海洋環(huán)境和USV最大曲率的地圖中設(shè)計了基于A*算法的3-D曲率路徑規(guī)劃算法[9]；Song C.H在柵格環(huán)境模型中應(yīng)用改進的蟻群算法完成USV的全局路徑規(guī)劃，驗證算法的有效性[10]。

電子海圖是為適應(yīng)航海需要而繪制的地理信息和海事信息的數(shù)字地圖[11]。如果能夠充分運用這些信息自動獲取水面無人艇到目的地的所有可航區(qū)域和不可航區(qū)域，則將會為水面無人艇設(shè)計計劃航線提供重要參考。本文在完成對電子海圖數(shù)據(jù)讀取、建立環(huán)境模型的基礎(chǔ)上，提出了一種基于距離信息和角度變量的改進A*算法，將其運用在水面無人船全局路徑規(guī)劃。

2 海洋環(huán)境模型的建立

雷達、相機和其它傳感器無法提供完全的海洋環(huán)境信息，而USV卻需要在廣闊的海域中航行，因此擁有可以提供詳細而準(zhǔn)確的全局海洋環(huán)境信息的S-57電子海圖是必不可少的。對電子海圖中的海洋環(huán)境地理信息進行統(tǒng)一描述，并采用柵格法進行環(huán)境建模。柵格化的電子海圖一致性表述是提高路徑搜索算法效率的基礎(chǔ)。

2.1 電子海圖數(shù)據(jù)提取

電子海圖主要由海域要素組成，具體表現(xiàn)為海底地形、航行障礙、導(dǎo)航標(biāo)志、港口設(shè)施等要素[11]。S-57電子海圖是一種便于進行數(shù)據(jù)交換和傳輸?shù)捏{度壓縮的數(shù)據(jù)格式，其標(biāo)準(zhǔn)封裝格式為ISO/IEC8211國際標(biāo)準(zhǔn)，海圖文件包括數(shù)據(jù)集的描述信息、特征記錄和空間記錄[12]。將此電子海圖應(yīng)用到USV全局路徑規(guī)劃中，需先按其對數(shù)據(jù)解析、提取和存儲；因此本文采用ISO8211開源庫，根據(jù)S-57電子海圖的文件包結(jié)構(gòu)對所有電子海圖數(shù)據(jù)進行解析。S-57電子海圖解析和存儲的流程如圖1所示。

圖1 S-57電子海圖解析和存儲流程圖

讀取電子海圖文件中的每條記錄，特征記錄用矢量容器存儲，空間記錄用圖形容器存放。將包含陸地和島嶼等障礙物的海洋環(huán)境空間描述成USV能識別的海洋環(huán)境信息。

2.2 環(huán)境模型的建立

USV環(huán)境模型的建立可以簡化為：USV在海平面有限的區(qū)域內(nèi)移動，區(qū)域中存在著有限數(shù)量的靜態(tài)障礙物，障礙物的形狀和分布位置是不確定的。環(huán)境建模方法一般分為柵格法、幾何法和拓撲法3類。由于采用柵格法建立環(huán)境模型簡單有效，且便于維護和修改，因此本文采用柵格法來表示環(huán)境模型。

環(huán)境模型的柵格化是通過將區(qū)域轉(zhuǎn)化為一個矩形圖，用網(wǎng)格方法將矩形劃分為多個等尺寸網(wǎng)格單元，將陰影部分填充區(qū)作為障礙物區(qū)域。本文根據(jù)依次提取的信息判斷網(wǎng)格中是否存在陸地和島嶼等障礙，以便在網(wǎng)格單元中建立可通航區(qū)和不可航行區(qū)。

原始環(huán)境模型和可航行判斷后的環(huán)境模型如圖2(a)、(b)所示，其中陰影部分表明是不可航行區(qū)域。

圖2 環(huán)境模型

USV的仿真環(huán)境經(jīng)柵格化后成為柵格地圖，柵格法的一致性和規(guī)范性使環(huán)境空間變得簡單有效，從而將全局路徑規(guī)劃問題轉(zhuǎn)化為在柵格網(wǎng)中尋找2個柵格節(jié)點之間的最優(yōu)路徑問題。

3 改進A*算法的描述與實現(xiàn)

3.1 傳統(tǒng)A*搜索算法基本思想及缺點

在路徑規(guī)劃算法中，A*算法是一種經(jīng)典的啟發(fā)式搜索算法。其基本思想是使用預(yù)設(shè)的代價函數(shù)來計算當(dāng)前節(jié)點可能到達的每個相鄰子節(jié)點的值，并選擇最小成本節(jié)點加入搜索空間并展開，以此類推，直到到達目標(biāo)點為止[13]。A*算法綜合考慮了起始點到當(dāng)前節(jié)點的真實代價和當(dāng)前節(jié)點到終點的估計代價，因此可以引導(dǎo)搜索方向。

其中代價計算方式如式(1)所示

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

(1)

f(n)——經(jīng)過當(dāng)前節(jié)點n的全局評估代價值；g(n)——起始節(jié)點到當(dāng)前節(jié)點n的真實代價值；h(n)——當(dāng)前節(jié)點n到目標(biāo)節(jié)點的代價估計。

在評價函數(shù)中，啟發(fā)函數(shù)h(n)對A*算法起主要影響作用：h(n)估值越小，A*算法需要計算的節(jié)點就越多，此時的算法效率就會降低，逐步趨近Dijkstra算法；但如果h(n)估值很大，甚至遠大于g(n)，此時g(n)的作用便會失效，逐步趨于BFS算法，只追求速度無法獲取合理路徑。

傳統(tǒng)A*算法雖然能夠規(guī)劃出一條全局路徑，但是往往在特殊復(fù)雜環(huán)境下規(guī)劃效率低，且路徑線路不優(yōu)，具體問題表現(xiàn)在：無人船航行的環(huán)境里存在著許多不規(guī)則形狀的島嶼、礁石等障礙物，如果采用傳統(tǒng)A*算法規(guī)劃路徑，USV會在各個拐點或者障礙物處容易出現(xiàn)線路抖動甚至卡死現(xiàn)象，降低了規(guī)劃效率，增加了路徑拐點數(shù)和航行總距離。

3.2 改進A*算法

在上一小節(jié)提到過啟發(fā)函數(shù)h(n)對A*算法起主要影響作用，它表示的是當(dāng)前節(jié)點C到目標(biāo)節(jié)點的距離代價估計值。在傳統(tǒng)A*算法中距離估計一般采用曼哈頓距離，以X、Y方向上的距離差的絕對值之和作為估計距離，h(n)函數(shù)如式(2)所示

h(n)=|Cx-Gx|+|Cy-Gy|

(2)

式中：C為當(dāng)前計算節(jié)點，G為目標(biāo)節(jié)點，x，y分別是節(jié)點對應(yīng)的距離行列號。

在保證USV行進間與障礙物有一定安全距離的約束下，本文主要對A*算法的啟發(fā)函數(shù)h(n)做出改進，以分區(qū)加權(quán)方式來表達距離信息，以向量叉積來表達角度信息，新的啟發(fā)函數(shù)h(n)+如下式(3)所示

(3)

式中，S為起始節(jié)點，C為當(dāng)前計算節(jié)點，G為目標(biāo)節(jié)點，x，y分別是節(jié)點對應(yīng)的距離行列號，p，q，w是權(quán)值。

權(quán)值p，q的設(shè)置使得h(n)可以根據(jù)節(jié)點C所處區(qū)域的不同進行調(diào)整，更好地引導(dǎo)搜索方向；變量w使得h(n)可根據(jù)節(jié)點向量間角度變化作出調(diào)整，誘導(dǎo)分布在起止點連線附近的節(jié)點趨近于S-G直線上。總而言之，本研究是根據(jù)節(jié)點C所處位置的不同調(diào)整啟發(fā)函數(shù)h(n)，獲取更理想的h(n)值來引導(dǎo)路徑搜索，權(quán)值選取的詳細描述如下。

1)距離信息表示

常用的距離估算方式有曼哈頓距離、切比雪夫距離和歐幾里得距離，三種距離估算方式具體含義和作用如表1所示：

表1 三種距離估算方式的含義和作用

因為歐幾里得距離和切比雪夫距離在障礙復(fù)雜環(huán)境下與真實代價的偏差較大，所以本文選擇在多障礙環(huán)境下距離估計更準(zhǔn)的曼哈頓距離更為合適。

為避免路徑行進中遇到節(jié)點代價相同而造成的路徑不確定性和抖動現(xiàn)象，將坐標(biāo)差加權(quán)后再求和。通過設(shè)置十組權(quán)值比{1∶9，2∶8，…，9∶1}進行路徑搜索，最后確定權(quán)值比6∶10效率最高，即選取p=0.6，q=1。而分區(qū)加權(quán)是為了起到一定的方向引導(dǎo)作用。

分區(qū)加權(quán)對引導(dǎo)作用的體現(xiàn)如圖3所示，以終點為坐標(biāo)原點，以X=Y作為分割線，左下方區(qū)域中當(dāng)前點到終點在X方向距離差大于在Y方向距離差，此時定義X1的權(quán)重為小系數(shù)，由于在選擇計算點時優(yōu)先選擇代價小的節(jié)點，故將會引導(dǎo)搜索方向朝X1小的方向前進(箭頭標(biāo)識)，同理分割線右上方的區(qū)域則會朝Y1小的方向前進(箭頭標(biāo)識)。分析起止線左偏的情況同樣適用。

圖3 距離函數(shù)幾何意義分析

2)角度變量表示

有時規(guī)劃出的航行成本最小的路徑不止有一條，雖然最終結(jié)果只需要一條；或者規(guī)劃出的航線不合理，因為分布在起始點S到目標(biāo)點G連線附近的節(jié)點偏離起始點S和目標(biāo)點G之間的直線LSG過多。為此，在啟發(fā)函數(shù)中加入角度信息變量，來引導(dǎo)A*算法偏向拓展分布在起始點S到目標(biāo)點G連線附近的節(jié)點。

在式(3)中加入的角度信息變量是兩個向量的叉積結(jié)果的模。如圖4所示，將起始點設(shè)置為S，目標(biāo)點為G，當(dāng)前節(jié)點為C，通過式(4)計算向量叉積得到S-G向量與C-G向量之間的角度θ

(4)

式中，C為當(dāng)前計算節(jié)點，G為目標(biāo)節(jié)點，x，y分別是節(jié)點對應(yīng)的距離行列號。

圖4 角度函數(shù)幾何意義分析

通常情況下起止點距離|SG|是常量，而|CS|是隨著兩向量夾角變化而變化的量，當(dāng)兩者平行的時候，|CS|達最小值，當(dāng)兩者垂直的時候，|CS|達最大值，它很好地以距離的形式反映了兩者間的角度關(guān)系。因此，在啟發(fā)函數(shù)中加入角度信息變量可誘導(dǎo)A*算法偏向拓展分布在起始點到目標(biāo)點連線附近的節(jié)點。在本研究中將當(dāng)前點C角度權(quán)重w表示為式(5)所示

w=3/(4-sin(θc))

(5)

3.3 路徑優(yōu)化

如果把規(guī)劃出來的路徑點連接起來作為水面無人艇的航行路徑，折線會過多。路徑中的多余節(jié)點指的是那些去掉以后不會影響路徑的有效性和安全性的節(jié)點。出現(xiàn)的多余節(jié)點使得規(guī)劃出來的路徑有時會出現(xiàn)階梯和鋸齒狀的線段。將規(guī)劃的結(jié)果作為航行路徑要經(jīng)常改變航向，這樣對水面無人艇的控制提出了很高的機動性要求。為此需要對規(guī)劃的路徑進行優(yōu)化，以減少路徑中不必要的路徑點，增加路徑的平滑度。

路徑平滑的方法是遍歷規(guī)劃路徑中的所有節(jié)點，當(dāng)節(jié)點i和i+2之間的連接不存在障礙時，刪除剩余節(jié)點i+1。繼續(xù)上述的步驟直到i和j之間的連接線穿過障礙物為止，然后在節(jié)點i之后取出3個連續(xù)節(jié)點mj-1、mj和mj+1，繼續(xù)這些步驟，直到已經(jīng)遍歷了整個路徑中的所有節(jié)點。

4 仿真驗證

為了驗證所建環(huán)境模型和改進的A*算法的可行性和有效性，對上述算法進行了仿真。基于Map Objects開發(fā)的電子海圖顯示平臺界面，路徑規(guī)劃仿真環(huán)境利用IDLE(Python3.6)開發(fā)，運行于PC機?；陔娮雍D的USV全局路徑規(guī)劃流程如圖5所示。實驗中實現(xiàn)的主要功能有：設(shè)置USV和電子海圖參數(shù)；根據(jù)USV的參數(shù)信息獲取并顯示電子海圖中障礙物的空間幾何信息；完成海洋模型的建立，設(shè)置柵格大小，對電子海圖進行柵格化；啟動改進A*算法程序，完成全局路徑規(guī)劃。

圖5 全局路徑規(guī)劃流程圖

選取中國某海域(區(qū)域坐標(biāo)范圍為東經(jīng)112.50度至113.00度，北緯21.50度至21.98度)海圖環(huán)境，在實驗中，設(shè)置USV的起始位置均為(東經(jīng)112.663度，北緯21.829度)，終止位置均為(東經(jīng)112.753度，北緯21.665度)給出仿真驗證結(jié)果。

圖6 傳統(tǒng)A*算法路徑規(guī)劃圖

如下圖6所示是在柵格化電子海圖環(huán)境模型下傳統(tǒng)A*算法路徑規(guī)劃圖，；圖7所示為改進A*算法和傳統(tǒng)A*算法路徑規(guī)劃效果對比圖，實線為算法改進后的路徑；從圖中可以明顯看出實線路徑走向要比虛線路徑好得多。圖8所示為對改進算法路徑平滑優(yōu)化后的全局路徑和沒有平滑優(yōu)化的路徑對比效果圖，其中線a表示平滑優(yōu)化過的效果圖，線b是表示沒有平滑優(yōu)化過的；從圖中可以看出，經(jīng)過平滑優(yōu)化過的全局路徑轉(zhuǎn)折點更少，曲線更加平滑流暢。

圖7 改進A*算法和傳統(tǒng)A*算法路徑規(guī)劃對比圖

圖8 路徑平滑優(yōu)化前后效果對比圖

表2所示是仿真后的三種結(jié)果各個指標(biāo)的對比圖，在航行距離方面，使用傳統(tǒng)A*算法的距離最遠，而改進算法后USV的航行距離減少了0.58，在這基礎(chǔ)上加入路徑平滑優(yōu)化后航行距離進一步減少了0.49，降低了USV的航行成本。與此對應(yīng)的路徑節(jié)點數(shù)也發(fā)生了改變，傳統(tǒng)A*算法路徑節(jié)點數(shù)有38個，而路徑平滑后的改進后A*算法只有19個路徑節(jié)點；節(jié)點數(shù)減少使得USV航行時轉(zhuǎn)向次數(shù)減少，在保持安全距離的同時路徑更加平滑，線路抖動甚至卡死現(xiàn)象大大改善。在規(guī)劃計算時間方面，由于對啟發(fā)函數(shù)加入了權(quán)值和變量信息，導(dǎo)致計算時間相較于未改進前有所增多。

表2 仿真結(jié)果各指標(biāo)對比表

5 結(jié)論

針對USV復(fù)雜的工作環(huán)境下，傳統(tǒng)A*算法規(guī)劃全局路徑容易在各個拐點或者障礙物處出現(xiàn)線路抖動甚至卡死現(xiàn)象的缺點，本文基于柵格化后的S-57電子海圖仿真環(huán)境模型，提出了一種對啟發(fā)函數(shù)h(n)結(jié)合了分區(qū)距離信息和角度變量信息加權(quán)的改進A*算法并用于USV全局路徑規(guī)劃中；最后再使用路徑曲線平滑法進一步優(yōu)化。通過仿真實驗驗證得出該算法能準(zhǔn)確地在給定海域內(nèi)任意兩個起止點之間生成無碰撞全局路徑。相較于傳統(tǒng)A*算法，改進A*算法雖然犧牲了規(guī)劃計算時間效率，但是規(guī)劃航行距離縮短，USV航行時轉(zhuǎn)向次數(shù)減少，路徑更加平滑，線路抖動甚至卡死現(xiàn)象大大改善。