基于改進A*算法的水面無人艇路徑規劃

舒偉楠　趙建森　謝宗軒　張學生　馬欣

摘要：為解決水面無人艇（unmanned surface vessel， USV）在礙航物比較密集水域的全局路徑規劃問題，提出一種基于改進A*算法的路徑規劃方法。利用像素閾值法剔除船載雷達圖像中的冗余信息，并使用柵格法進行環境建模;提出節點礙航物檢測法，用于優化無人艇路徑規劃，同時改進評價函數來減少遍歷的節點總數;對規劃路徑上的冗余節點進行平滑處理。仿真結果表明：相較于A*算法，本文提出的改進A*算法能夠確保USV遠離礙航物，且算法遍歷節點總數和規劃路徑上的轉折點都較少。

關鍵詞：? 水面無人艇（USV）; 路徑規劃; A*算法; 礙航物檢測

中圖分類號：? U675.7; TP273文獻標志碼：? A

Path planning for unmanned surface vessels based on

improved A* algorithm

Abstract： To address the global path planning issue of unmanned surface vessels （USVs） in waters with dense obstacles， this paper proposes a path planning method based on the improved A* algorithm. The redundant information of shipborne radar images is removed by the pixel threshold method， and the grid method is used to carry out the environment modeling. The node obstacle detection method is proposed to optimize the path planning of USVs， and the evaluation function is improved to reduce the total number of traversal nodes. The redundant nodes on the planned path are smoothed. The simulation results show that， compared with A* algorithm， the proposed improved A* algorithm can ensure that USVs are away from obstacles， the total number of traversal nodes is fewer， and the turning nodes on the planned path are fewer.

Key words： unmanned surface vessel （USV）; path planning; A* algorithm; obstacle detection

引言

隨著人工智能技術的快速發展，路徑規劃在移動機器人領域的應用取得了很大的成就。水面無人艇（unmanned surface vessel， USV）是一種能夠實現自主避碰的水上艦艇。當前對USV的研究主要集中在路徑規劃和自主避障等方面。USV在復雜水域完成全局路徑規劃和在特殊情況下避障的能力越突出，則說明其智能化水平越高。[1]

路徑規劃是指在有障礙物的情況下，在起點與終點之間尋找出一條盡可能短的安全路徑[2-3]。按照對周圍環境感知情況的不同，路徑規劃可以分為環境信息全部掌握的全局路徑規劃和環境信息部分未知的局部路徑規劃。A*算法具有搜索效率高的優點，被廣泛應用于多種類型的搜索問題中。國內外學者已經對A*算法及其改進算法做了廣泛的研究，并且取得了一定的成果。薛雙飛[2]將人工勢場法與A*算法相結合，根據不同的障礙物類型建立不同的斥力勢場，并將該方法應用于東海大橋風機維護船舶的路徑規劃。王紅衛等[4]提出平滑A*算法，解決了規劃路徑轉折次數多的問題。陳豪等[5]基于A*算法，引入方向矢量以優化開放列表。WANG等[6]將船舶轉彎半徑融入A*算法，在估值函數中引入限制搜索方向的角度成本，生成與船舶實際轉彎角度更加契合的路徑。余必秀等[7]針對船舶避碰后無法快速回到預設航線的問題，在估值函數中增加一個代價值，讓無人航道測量船在估值函數的使用上有不同的選擇。王錦川[8]利用二叉堆對A*算法中的Open列表進行維護，加快了A*算法的搜索效率。楊瑤等[9]將車輛的最大轉向角約束和最大曲率約束加入啟發式函數中，同時提出車身輪廓代價改進拓展節點，使路徑更符合車輛的運動學特性。榮少巍[10]將最小轉彎半徑作為約束條件對傳統A*算法進行改進，同時使得A*算法可以實現高速與低速結合的應用。

USV具有慣性大、時滯大的特性，且在惡劣海況下易受風浪流的影響。USV只有與礙航物保持一定的安全距離，才能在碰撞危險出現時有足夠的反應時間和操縱空間。A*算法的搜索特點可能會使規劃路徑貼近礙航物，影響USV航行安全。鄧博文等[11]對障礙物做膨脹處理，王中玉等[12]在障礙物周圍設置禁止搜索區，雖然效果都較好，但都需要對環境中的障礙物進行處理。針對A*算法應用在USV上的缺陷，本文提出礙航物檢測方法：首先對A*算法遍歷的候選節點進行篩選，使路徑上的每個節點都能與礙航物保持一定的安全距離;然后對評價函數進行改進，使算法遍歷的節點數更少;最后對所規劃路徑上的冗余節點進行平滑處理。

1環境模型建立

USV路徑規劃的前提是搭建出合適的環境模型。目前，常用的環境建模方法有柵格法、幾何法和拓撲法[1]。由于采用柵格法規劃的環境能夠表現出一致性、規范性、簡單性，所以本文使用柵格法建立環境模型。柵格法將環境空間劃分為多個大小相同的柵格，每個柵格都用二進制數值表示。設整個環境空間S被劃分為a×b個大小相同的柵格，每個柵格都對應著一個環境狀態，用Nij表示，則S可以表示為（1）每個柵格的環境狀態信息如下：Nij=0，自由空間5694CAD6-5319-4D70-9CE8-1A0831F542B3

1，礙航物（2）本文基于船載雷達圖像進行路徑規劃，具體方法如下：（1）利用像素閾值法剔除雷達圖像中的冗余信息，如背景顏色、航向線和距標圈等，只保留海冰回波（礙航物）;（2）將雷達圖像轉換為二值圖像;（3）將二值圖像柵格化。雷達圖像處理前后對比示意圖見圖1。本文利用像素來描述環境空間中的圖像柵格。圖像柵格化可以把雷達圖像轉化成網格環境地圖，給USV利用網格進行路徑規劃提供所需的環境信息。

基于A*算法的移動機器人運動方向通常為四鄰域方向或八鄰域方向?？紤]到USV模型的適用性和復雜性，本文采取八鄰域方向，即USV有上、下、左、右、右上、右下、左上、左下等8個運動方向可選擇，見圖2。

2A*算法的基本原理

A*算法是一種典型的啟發式算法[13-14]，主要依據評價函數拓展節點。評價函數表示為（3）式中：f（n）表示節點n的評價函數;g（n）表示從起始點到節點n的實際代價值;h（n）表示從節點n到目標點的估計代價值。h（n）對評價函數的影響是最大的，本文采用歐幾里得距離衡量h（n），即（4）式中：xn、yn分別為節點n的橫、縱坐標;xg、yg分別為目標點的橫、縱坐標。

A*算法根據評價函數對所有候選節點進行計算，選出f（n）值最小的節點，再將該節點作為父節點繼續遍歷剩余節點，直至遍歷完所有節點或者成功找到目標點。具體實施步驟如下：

步驟1將起始點放入Open列表中，并對Open列表進行判斷。如果Open列表為空列表，則執行步驟5，否則執行步驟2。如果Open列表包含目標點，則執行步驟4，否則執行步驟2。

步驟2從Open列表中選出具有最小f（n）值的節點，放入Closed列表中，并將其標記為父節點。

步驟3考察父節點的8個鄰域節點。對于已經在Open列表中的節點，若其f（n）值小于原始值，則更新它。對于已經在Closed列表中的節點，跳過它，繼續搜索。如果節點不在兩個列表中，就將其放入Open列表中。跳到步驟1。

步驟4將目標點放入Closed列表中。在Closed列表中，復原從目標點到起始點的路徑。

步驟5結束。

3改進的A*算法

針對A*算法規劃出的路徑貼近礙航物、遍歷節點數較多等問題，采用礙航物檢測方法將距離礙航物太近的節點剔除，同時對評價函數進行改進，使算法遍歷的節點減少。最后對所規劃路徑上的冗余節點進行平滑處理。

3.1改進評價函數

將節點間的向量夾角值引入評價函數中，對其進行改進。如圖3所示，S（xs，ys）是起始點，G（xg，yg）是目標點，N（xN，yN）是當前節點，n（xn，yn）、m（xm，ym）是當前節點的兩個候選節點。假設起始點到目標點的向量為SG，起始點到兩個候選節點的向量分別為Sn、Sm，則α、β分別為向量Sn、Sm與向量SG的夾角。

任意節點n的評價函數可表示為

（5）

式中：f（n）、g（n）、h（n）與式（3）定義相同;θ（n）是起始點S到任一候選節點n的向量Sn與向量SG的夾角。在評價函數中增加θ（n）的目的是使算法趨向于拓展起始點與目標點連線附近的節點。α、β越大，表明候選節點距離起始點與目標點連線越遠，算法遍歷的節點數會越多;α、β越小，表明候選節點距離起始點與目標點連線越近，算法遍歷的節點數會越少。同時，改進評價函數可以使算法遍歷的節點更加集中在起始點與目標點連線附近。候選節點n的θ（n）的計算式為

（6）

其中：（7）

（8）

（9）

（10）θ（m）也根據式（6）計算。

3.2礙航物檢測

A*算法在應用于機器人路徑規劃時，將機器人視為一個質點，不考慮機器人的實際寬度和大小，這就使得按照規劃路徑行走的機器人會非常貼近礙航物。在水面航行的USV具有速度高、慣性大和不易操控的特點，如果規劃出的路徑太貼近礙航物，則極易導致USV與礙航物發生碰撞，因此需要讓路徑與礙航物保持一定的距離。

本文提出礙航物檢測方法，并將該方法的實施放在A*算法步驟3之前，用來確保規劃出的路徑與礙航物之間保持一定的安全距離。礙航物檢測就是判斷每個候選節點周圍一定范圍內是否有礙航物，如果有礙航物，就將該候選節點剔除。礙航物檢測方法的具體實施步驟如下：

步驟1確定待檢測的候選節點。圖4a中，紫色的點為當前節點，將8個候選節點放入序列N（={n1，n2，…，n8}）中。

步驟2依次選取序列N中的候選節點。圖4b中，n1為候選節點之一。為保證USV與礙航物之間有足夠的安全距離，本文以兩個柵格單位為安全距離，確定節點n1周圍的8個鄰域節點序列M（={m1，m2，…，m8}）。

步驟3礙航物檢測。判斷n1與節點序列M中每一個節點的連線是否經過礙航物。如果任何一條連線通過的區域都有礙航物存在，則將節點n1從序列N中剔除。

步驟4對沒有被剔除的候選節點繼續執行A*算法剩余程序。

礙航物檢測

在算法中增加了礙航物檢測之后，算法計算量會變大，因此對已經遍歷過的節點進行標記，不再對其進行篩選，以提高算法搜索效率。如果水域中礙航物比較多，在確保USV航行安全的情況下，可以將檢測范圍適當減小。比如，在本文仿真所選擇的北極冰區，礙航物比較多，如果節點檢測范圍設置得太大，就會出現找不到最優路徑的情況，因此可將檢測范圍適當調小。

3.3路徑平滑

用改進的A*算法規劃出的路徑會存在轉折次數多、冗余點多等問題。在實際應用中，這種頻繁的轉向會導致USV產生大幅度的航向改變，對USV的控制系統產生不利的影響。因此，需要對規劃出的路徑進行平滑處理，具體步驟如下：5694CAD6-5319-4D70-9CE8-1A0831F542B3

步驟1對節點進行共線判斷，如果任一節點與其前后兩個節點共線，就刪除該節點。比如，圖5a中的節點c與其前后節點b和d共線，則節點c會被刪除，結果見圖5b。

步驟2刪除多余的轉折點，處理方法是：遍歷航線中剩余的所有節點，判斷任一節點（起始點、目標點除外）前后兩個節點連線上有沒有礙航物。如果沒有礙航物，就將該中間節點直接刪除。比如，圖5b中節點b的前后節點a與d的連線上沒有礙航物，則節點b會被刪除，結果見圖5c。

3.4改進的A*算法路徑規劃流程

改進的A*算法路徑規劃流程見圖6。

4仿真實驗

為驗證改進A*算法的有效性和適用性，在不同的雷達圖像上進行實驗。經處理后每張雷達柵格圖像尺寸為400×300像素，即橫坐標和縱坐標范圍分別是[0，400]和[0，300]。每張雷達圖像上，黃色五角星為USV的起始點，藍色三角形為USV的目標點。

4.1改進評價函數的A*算法仿真

在兩種具有不同礙航物的雷達圖像中，給USV設置不同的起始點和目標點，用來驗證改進A*算法的適用性。圖7中藍色連線是用A*算法搜索得到的路徑，黑色連線是用改進評價函數的A*算法得到的路徑。由圖7可以很直觀地看出，改進了評價函數的A*算法趨向于選擇起始點與目標點連線附近的節點。

由表1可知，與A*算法相比，改進了評價函數的A*算法遍歷節點總數和節點向量夾角總和都是較小的。這表明，本文對A*算法的改進具有明顯的效果，能夠使得算法遍歷的節點更少，且具有目的性。

4.2進行礙航物檢測的A*算法仿真

為驗證礙航物檢測方法能夠使USV航線遠離礙航物，在兩種不同的雷達圖像上進行仿真實驗。從圖7b和7d可以看出，A*算法規劃出的路徑（藍色連線）容易出現距離礙航物較近的情況，而對候選節點進行礙航物檢測的改進A*算法規劃出的路徑（黑色連線）能夠遠離礙航物，保證了USV航行安全。表1改進評價函數的A*算法與A*算法數據對比圖7子圖

序號起始點，目標點A*算法遍歷

節點總數DA*算法遍歷

節點總數A*算法節點向量

夾角總和/（°）DA*算法節點向量

夾角總和/（°）a（350，100）， （200，150）1 8401 2162 121479b（320，200）， （120，100）1 6481 6003 7901 342c（350，100）， （200，150）2 9761 2402 1091 325d（380，120）， （80，250）5 5842 7205 0053 067注：改進評價函數的A*算法用DA*算法表示

4.3路徑平滑

用改進的A*算法進行USV路徑規劃，并對規劃出的路徑進行平滑處理。平滑處理前后的路徑見圖8。由圖8可以看出，平滑處理能夠將多余的節點刪除，最終剩余的轉折點也僅剩6個。本例中，改進A*算法生成路徑所用時間是103 ms，改進A*算法生成路徑后再進行平滑處理所用時間是112 ms。這說明改進A*算法可以在不增加太多時間的情況下，生成能夠與礙航物保持安全距離的路徑，而且遍歷節點更少，轉折點也大幅減少，更加適用于USV航行。

圖9是基于本文提出的改進A*算法得到的其他路徑規劃結果。由圖9可知，改進A*算法規劃出的路徑均能遠離礙航物，且效果較好。

5結論

針對A*算法規劃路徑存在較多冗余點、貼近礙航物等問題，提出采用礙航物檢測方法篩選出比較安全的候選節點，同時在評價函數中引入節點向量夾角值進行改進，并通過對規劃路徑的平滑處理去除冗余點，最終提出一種基于改進A*算法的水面無人艇（USV）路徑規劃方法。仿真實驗表明，本文的改進方法能夠生成遠離礙航物的USV路徑，改進后的A*算法趨向于選擇起始點與目標點連線附近的節點，遍歷的節點總數明顯減少，平滑處理后生成的路徑更符合USV實際航行特性，能夠解決USV在內河航道和沿海等礙航物密集水域的路徑規劃問題。

參考文獻：

[1]莊佳園， 萬磊， 廖煜雷， 等. 基于電子海圖的水面無人艇全局路徑規劃研究[J]. 計算機科學， 2011， 38（9）： 211214， 219. DOI： 10.3969/j.issn.1002137X.2011.09.049.

[2]薛雙飛. 基于改進A*算法的近海船舶路徑規劃[D]. 武漢： 武漢理工大學， 2018.

[3]ZUO L， GUO Q， XU X， et al. A hierarchical path planning approach based on A* and leastsquares policy iteration for mobile robots[J]. Neurocomputing， 2015， 170： 257266. DOI： 10.1016/j.neucom.2014.09.092.

[4]王紅衛， 馬勇， 謝勇， 等. 基于平滑A*算法的移動機器人路徑規劃[J]. 同濟大學學報（自然科學版）， 2010， 38（11）： 16471650， 1655. DOI： 10.3969/j.issn.0253374x.2010.11.016.

[5]陳豪， 李勇， 羅靖迪. 基于改進A*算法優化的移動機器人路徑規劃研究[J]. 自動化與儀器儀表， 2018（12）： 14. DOI： 10.14016/j.cnki.10019227.2018.12.001.

[6]WANG Z， XIANG X B. Improved A star algorithm for path planning of marine robot[C]//2018 37th Chinese Control Conference. IEEE， 2018： 54105414. DOI： 10.23919/ChiCC.2018.8483946.5694CAD6-5319-4D70-9CE8-1A0831F542B3

[7]余必秀， 初秀民， 柳晨光， 等. 基于改進A*算法的無人航道測量船路徑規劃方法[J]. 武漢大學學報（信息科學版）， 2019， 44（8）： 12581264. DOI： 10.13203/j.whugis20170239.

[8]王錦川. 自主式水面航行器導航與制導算法的研究[D]. 大連： 大連海事大學， 2014.

[9]楊瑤， 付克昌， 蔣濤， 等. 改進A*算法的智能車路徑規劃研究[J]. 計算機測量與控制， 2020， 28（10）： 170176. DOI： 10.16526/j.cnki.114762/tp.2020.10.035.

[10]榮少巍. 基于改進A*算法的水下航行器自主搜索航跡規劃[J]. 電子科技， 2015， 28（4）： 1719， 22. DOI： 10.16180/j.cnki.issn10077820.2015.04.005.

[11]鄧博文， 張春華， 李娟， 等. 局部路徑規劃在無人工程機械作業中的應用[J]. 兵工自動化， 2016， 35（10）： 7073， 79. DOI： 10.7690/bgzdh.2016.10.018.

[12]王中玉， 曾國輝， 黃勃， 等. 改進A*算法的機器人全局最優路徑規劃[J]. 計算機應用， 2019， 39（9）： 25172522. DOI： 10.11772/j.issn.10019081.2019020284.

[13]CHEN J C， LI M Y， YUAN Z Y， et al. An improved A* algorithm for UAV path planning problems[C]//2020 IEEE 4th Information Technology， Networking， Electronic and Automation Control Conference. IEEE， 2020： 958962. DOI： 10.1109/ITNEC48623.2020.9084806.

[14]陸皖麟， 雷景森， 邵炎. 基于改進A*算法的移動機器人路徑規劃[J]. 兵器裝備工程學報， 2019， 40（4）： 197201. DOI： 10.11808/bqzbgcxb2019.04.048.

（編輯賈裙平）

收稿日期： 20210429修回日期： 2021-06-18

基金項目： 國家自然科學基金（51709167）;國家重點研發計劃（2019YFB1600605）;上海市科技創新行動計劃（18DZ1206101）

作者簡介： 舒偉楠（1995—），男，安徽宿州人，碩士研究生，研究方向為船舶導航與海上通信，（Email）15755782120@163.com;

趙建森（1983—），男，黑龍江牡丹江人，副教授，博士，研究方向為智能通信、微波和天線，（Email）7230981@163.com05694CAD6-5319-4D70-9CE8-1A0831F542B3