一種改進A*算法在無人船路徑規劃中的應用

陳 新，袁宇浩，饒 丹

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

1 引言

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

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

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

2 海洋環境模型的建立

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

2.1 電子海圖數據提取

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

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

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

2.2 環境模型的建立

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

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

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

圖2 環境模型

USV的仿真環境經柵格化后成為柵格地圖，柵格法的一致性和規范性使環境空間變得簡單有效，從而將全局路徑規劃問題轉化為在柵格網中尋找2個柵格節點之間的最優路徑問題。

3 改進A*算法的描述與實現

3.1 傳統A*搜索算法基本思想及缺點

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

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

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

(1)

f(n)——經過當前節點n的全局評估代價值；g(n)——起始節點到當前節點n的真實代價值；h(n)——當前節點n到目標節點的代價估計。

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

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

3.2 改進A*算法

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

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

(2)

式中：C為當前計算節點，G為目標節點，x，y分別是節點對應的距離行列號。

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

(3)

式中，S為起始節點，C為當前計算節點，G為目標節點，x，y分別是節點對應的距離行列號，p，q，w是權值。

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

1)距離信息表示

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

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

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

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

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

圖3 距離函數幾何意義分析

2)角度變量表示

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

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

(4)

式中，C為當前計算節點，G為目標節點，x，y分別是節點對應的距離行列號。

圖4 角度函數幾何意義分析

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

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

(5)

3.3 路徑優化

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

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

4 仿真驗證

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

圖5 全局路徑規劃流程圖

選取中國某海域(區域坐標范圍為東經112.50度至113.00度，北緯21.50度至21.98度)海圖環境，在實驗中，設置USV的起始位置均為(東經112.663度，北緯21.829度)，終止位置均為(東經112.753度，北緯21.665度)給出仿真驗證結果。

圖6 傳統A*算法路徑規劃圖

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

圖7 改進A*算法和傳統A*算法路徑規劃對比圖

圖8 路徑平滑優化前后效果對比圖

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

表2 仿真結果各指標對比表

5 結論

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