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

隨博文，黃志堅

(上海海事大學 商船學院，上海 201306)

0 引 言

無人艇是一種無人操作的水面艦艇，常用于危險水域搜救或人員無法到達的水域執行探測，偵察等任務。無人艇的應用極大節省了人力物力成本，并且可以高效快速執行任務。為使無人艇執行任務時可以達到較高的避碰性能以及選擇最優的路徑規劃，本文對無人艇的航跡規劃、自主避障做進一步的研究。

當前智能航行體的路徑規劃算法主要有遺傳算法、Dijkstra 算法、A*算法、人工勢場法、蟻群算法等。遺傳算法最早是由美國J.Holland 教授提出的，遺傳算法是模擬自然界中按“優勝劣汰”法則進行進化過程而設計的算法，在動態路徑規劃中得到廣泛應用。謝玉龍等[1]針對傳統遺傳算法解決船舶路徑規劃問題的不足，提出了一種改進的遺傳算法。改進算法改變了種群的編碼方式，由二維編碼變為基于坐標軸的一維編碼；在傳統遺傳算法的基礎上增加3 個新的遺傳操作：復原操作、重構操作和錄優操作，使算法盡早收斂于全局最優解，錄優操作保證種群朝著最優解方向進化。Dijkstra 算法是一種常見的最短路徑算法，由Edsger W.Dijkstra 在1959 年提出。傳統的Dijkstra 算法直接搜索全局空間而不考慮目標信息，導致路徑求解時花費的時間較長，難以滿足快速路徑規劃的需求。陳家[2]提出一種將蟻群算法和Dijkstra 算法相結合的復合算法，應用于無人駕駛救助船路徑規劃中。該復合算法首先通過Dijkstra 算法尋找出最短路徑，然后利用改進的蟻群算法對所得到的最短路徑進行優化，使最后得到的路徑更符合實際情況。陳超等[3]提出一種改進的人工勢場法，通過建立新的引力和斥力場函數來避免局部最小點問題，并應用于水面無人艇的路徑規劃中。余必秀等[4]提出一種改進的A*算法，在原始代價函數的基礎上，新增一個與當前點到預設航線的垂直距離相關的代價值，并將改進后的算法應用于無人航道測量船的路徑規劃中，使無人航道測量船在避開障礙物之后更快地回到預設航線。陳立家等[5]將改進的蟻群算法應用于船舶航行路徑搜索中，提出一種多約束條件下航行綜合成本最低的最優航線生成算法，可以在多約束條件下規劃出最優航線。王紅衛等[6]提出一種平滑A*算法，能處理不同柵格規模下、障礙物隨機分布的復雜環境下移動機器人的路徑規劃問題。

A*算法是一種啟發式的搜索算法，是求解最短路徑最有效的直接搜索方法，同時也適用于路徑的二次規劃。由于A*算法中為了處理復雜流程需要大量數學計算和理論推導，從而導致A*算法規劃的路徑存在折線、轉折多的問題，這使得在仿真實驗中，規劃的路徑與障礙物距離過近，極易引發碰撞，存在極大的安全隱患。針對此問題，本文以柵格法環境建模為基礎，當航行體行至障礙物附近時，對障礙物尖角檢測，然后進行路徑平滑處理，使得航行體與障礙物始終處于安全距離的范圍內。對比結果表明，改進后的A*算法規劃路徑優于傳統的A*算法。

1 基于A*算法進行路徑規劃的基本原理

1.1 A*算法原理

A*算法是對估價函數加上一些限制后得到的一種啟發式搜索算法[7-8]，啟發式搜索可以有效地避免無效的搜索路徑，提高搜索效率。A*算法路徑搜索步驟如下：

新建Closed 表和Open 表，并進行初始化。將初始節點s 添加到新建的Open 表。如果Open 表為空則表示失敗并且退出，否則取最小節點F 作為當前考察點x。從Open 表中將x 移入Closed 表。如果x 是目標節點，那么確定找到最優解并且退出，否則擴展x 并生成繼節點n。考察x 的所有的繼節點n。

1）所有的繼節點n 都有g(n) = g(x) + g(x, n)；

2）創建一個新的指針，將n 返回s；

3）如果n 是Open 表的舊節點，則把舊節點標記成o，并且把n 加入到x 的字節點表里。若f(n) ＜ f(o)，則f(o) = f(n)。如果n 沒在Open 表，那么將判斷它是否在Closed 表。

4）如果n 是Closed 表的舊節點，則把它標記為o，把節點n 加入到x 的子節點表中。若f(n) ＜ f(o)，則f(o) = f(n)。否則將其加入到Open 表和x 的后繼節點；第5 步：算出F 值，并返回到第3 步，繼續執行。

5）算出F 值，并返回到第3 步，繼續執行。

采用A*算法無人艇的路徑規劃，首先需要建立A*優化函數[9]： f( n)=g(n)+h(n)，在A*算法中給出如下定義：

O 為存放等待擴展節點集合；C 為存放已擴展過節點集合；Ss為起點；Te為目標點；Oi為障礙物柵格；Oobs為障礙柵格的集合；g(n)為初始節點Ss到n 的實際移動距離；

利用柵格地圖和八鄰域節點擴展法，將無人艇從當前節點k 到目標點Te的Euclidean 距離作為啟發式函數：

1.2 A*算法流程

圖1 為A*算法的流程圖，通過下列程序的執行，得到所規劃的初始路徑。

圖 1 傳統A*算法流程圖Fig.1 Flowchart of traditional A* algorithm

2 改進A*算法原理與實現

2.1 改進A*算法

為了處理傳統算法規劃路徑的折線多，且易穿越障礙物之間的接觸地帶等問題，采用改進后的算法對路徑做出判斷并進行平滑處理。改進算法具體步驟如下：

1）定義無人艇的原始路徑為Pi，節點數為Pin，改進平滑處理后的路徑為Pp。

2）判斷原始路徑的節點是否等于2。若大于2，則執行下一步，否則將原始路徑Pi的值賦給Pp。

3）判斷Pi的節點是否小于等于Pin。若是則執行下一步，否則將Pi上非N 節點賦值給Pp。

4）調用線段lc（）所獲得節點i 和i+2 所在的線段。

5）調用障礙物集合Ob。

7）將Pi上節點i+1 置為N。

8）執行i=i+1。

9）最終獲得改進算法處理后所得的優化路徑Pp。

在改進后的算法中，新定義2 個函數Ps（）和W（）函數，Ps（）是需要處理的路徑節點，W（）函數用來判斷待連接節點的線段是否存在障礙物，有障礙物，則W（）失敗，即無通路；否則可聯通。函數Ps（）具體執行步驟如下：

1）定義無人艇的原始路徑Pi，連接點為Pinit，改進算法處理后的路徑Prp為空。

2）將連接點的下一個節點賦值給當前節點。

3）判斷當前節點是否為空。若是則執行下一步，否則連接所有節點獲得Prp。

4）判斷連接點同當前節點的下一節點之間是否能夠走通。若是則執行下一步，否則返回第2 步。

5）刪除當前節點，連接點的下一個節點賦值給當前節點。

函數W（）具體執行步驟如下：

1）定義連接點和當前節點的下一個節點；

2）將連接點的下一個節點賦值給當前節點；

3）用線段連接連接點與當前節點的下一個節點；

4）判斷當前線段上是否存在障礙物，若不存在執行下一步，若存在則不存在通路退出；

5）存在通路可行。

經過改進的A*算法用于無人艇路徑規劃的具體流程圖，如圖2 所示。

2.2 基于改進的A*算法路徑規劃模型

圖 2 改進A*算法水面無人艇航跡規劃流程圖Fig.2 Improved A* algorithm flowchart of route planning for surface unmanned boat

圖 3 改進A*算法平滑處理前后的路徑對比圖Fig.3 Path comparison before and after smoothing with improved A* algorithm

由圖3 可以看出，基于傳統A*算法的規劃的路徑為曲折的折線，這并不滿足無人艇的實際航行需求。而經過改進后的算法所規劃出的路徑更符合實際要求。

3 仿真實驗

模型仿真實驗在Matlab R2016b 環境中進行，搭建20×20 的仿真地圖，通過建立直角坐標系，定義仿真環境圖的左下角坐標為（1，1）；右上角坐標為（20，20）。其中黑色部分表示障礙物，白色部分表示可通行區域，障礙物模擬水閘、鉆井平臺，拋錨船只等靜態障礙物，本文的目的就是找到一條從起點到終點的無碰最優航跡。實驗分別在圖4 中簡單環境（a）和復雜環境（b）進行，分別采用傳統A*算法和改進后的算法做出最優路徑規劃，并進行分組對比，仿真結果如圖5 和圖6 所示。

圖 4 仿真環境圖Fig.4 Diagram of simulation environment

圖 5 簡單環境下的路徑規劃圖Fig.5 Path planning in simple environment

圖 6 復雜環境下的路徑規劃圖Fig.6 Path planning in complex environment

仿真環境中無人艇起點和終點分別用三角形和圓形圖標表示。分別將無人艇在仿真環境中的起點和終點設為（1，1），（20，20）。分別使用傳統A*算法和改進算法對水面無人艇在簡單水域仿真環境下進行仿真。由圖5（a）可以看出，利用傳統A*算法規劃出的路徑，很容易穿過障礙物的接觸位置以及緊挨著障礙物邊緣航行，并且在航行于障礙物水域中，規劃路線折線多，這將會導致無人艇在航行期間存在極大安全隱患，很容易與障礙物的邊緣發生碰撞。基于改進的A*算法所規劃出的安全路徑卻能很好地避免上述問題，從圖5（b）可以看出，在同樣的仿真環境中，基于改進的A*算法所規劃出的安全路徑完全避開了障礙物的接觸點，并且在路線拐角處進行圓弧平滑處理，使得無人艇和障礙物始終保持足夠的安全距離，以保證無人艇的航行安全。

為了更好驗證改進的A*算法在無人艇的路徑規劃中的有效性，通過建立更加復雜的水域模擬仿真環境（見圖4），再次對基于傳統A*算法和改進A*在無人艇的路徑規劃應用性能進行仿真驗證。仿真結果如圖6 所示。結果表明，基于傳統A*算法的所規劃出的無人艇航行路線更加顯示了其不能有效通過障礙物之間接觸點和障礙物邊緣的缺陷，而基于改進的A*算法在復雜模擬水域中規劃路徑依然高效且安全。

改變無人艇在復雜模擬水域中的起止點位置，將起點和終點改為（1，20），（20，3），仿真結果如圖7 所示。基于改進的A*算法在無人艇的路徑規劃中依然有著比傳統的A*算法所規劃出的路線更加科學、安全，能夠更好的滿足水面無人艇航行的實際需求。

4 結 語

針對基于傳統A*算法無人艇路徑規劃不能安全有效避開障礙物的問題，本文提出一種基于平滑A*算法的無人艇路徑規劃方法。仿真實驗表明，本文提出的算法可以較好地避開障礙物，并且與障礙物保持有足夠的安全距離。通過在拐點前適當位置沿著預定半徑轉向使得水面無人艇的規劃路徑更加平滑，有效避免了無人艇與障礙物接觸點及邊緣相碰撞的危險，最終使無人艇規劃出最短路徑并安全抵達目的地。

目前基于A*算法在無人艇路徑規劃的應用仍存在一定限制，其只適用于靜態環境中的路徑規劃，而在水面無人艇的實際航行中，往往會遇到各種動態的障礙物，如過往船只、浮漂，水域中的大型水生物等，此時應用A*算法進行路徑規劃將會受到一定限制。通過繼續改進A*算法使其能夠針對動態障礙物實現自主避障及安全路徑規劃將是以后的研究方向。

圖 7 復雜環境下的路徑規劃圖Fig.7 Path planning in complex environment