復雜水域慎思型避碰軌跡規劃

秦操　楊榮武

摘要：為解決復雜水域的船舶自主避碰問題，提出一種基于A*算法的慎思型避碰軌跡規劃算法，旨在滿足船舶操縱性約束、靜態與動態障礙物約束和《國際海上避碰規則》（International Regulations for Preventing Collisions at Sea，COLREGs）約束下，規劃出一條最經濟的航行軌跡。通過無人三體船自主避碰試驗和模擬試驗，驗證算法的有效性，具有較高的參考價值。

關鍵詞：船舶; 自主避碰; 慎思型軌跡規劃; COLREGs; 操縱性約束

中圖分類號：? U675.73

文獻標志碼：A

Deliberative collision avoidance trajectory planning for complex waters

QIN Cao1， YANG Rongwu2

（1. Collaborative Innovation Center for Advanced Ship and Deep-Sea Exploration， Shanghai Jiao Tong University，

Shanghai 200240， China; 2. Seastel Marine System （Shanghai） Co.， Ltd.， Shanghai 200241， China）

Abstract：

To solve the problem of ship autonomous collision avoidance in complex waters， a deliberative collision avoidance trajectory planning algorithm based on A* algorithm is proposed， which aims to plan a most economical sailing trajectory with the ship maneuverability constraints， static and dynamic obstacle constraints as well as International Regulations for Preventing Collisions at Sea （COLREGs） constraints. The effectiveness of the algorithm is verified by the unmaned trimaran autonomous collision avoidance test and the simulation test， which has high reference value.

Key words：

ship; autonomous collision avoidance; deliberative trajectory planning; COLREGs; maneuverability constraint

0 引 言

近年來海上碰撞事故頻發，不僅造成了巨大的人員傷亡和經濟損失，還帶來了嚴重的環境污染。歐洲海事安全局發布的《2017海上事故年度回顧報告》顯示，海上事故的一半為船舶碰撞及擱淺事故，其中60%以上是人的因素造成的[1]。為避免人因事故，船舶自主避碰系統的研究受到越來越多的關注[2]。另外，隨著無人商船技術的發展，各國船級社陸續發布了有關規范，其中中國船級社《智能船舶規范》[3]第2.6節要求智能船舶應具有在狹窄水道自動避碰的能力，以及在復雜環境條件下自主航行的能力。《自主貨物運輸船舶指南》[4]第3.3.1節也提出了自主貨船應根據感知到的場景信息和船舶自身信息進行綜合分析決策，并遵守《國際海上避碰規則》（International Regulations for Preventing Collisions at Sea， COLREGs）。因此，對船舶自主避碰系統的研發和改進也越來越迫切。

軌跡規劃是船舶自主避碰系統的核心，其目標是給出一條無碰撞危險的、動力學可行的、滿足航行規則的、最經濟的軌跡來引導船舶安全航行。規劃算法需要滿足操縱性約束、障礙物約束、COLREGs約束和實時性約束，并達到最優軌跡規劃目標[2]。目前的計算能力無法同時保證實時性和軌跡最優性，于是產生了放棄全局最優而保證高重規劃率的反應型規劃算法，以及降低重規劃率來追求全局最優的慎思型規劃算法。

近年來，一些慎思型規劃算法被提出，主要包括圖基法和采樣法兩類[2]。SHAH等[5]提出的適應性風險應急響應規劃（adaptive-risk and contingency-aware planning，? ARCAP）算法為圖基法的代表，它采用A*算法[6]在五維狀態空間中搜索路徑，在 COLREGs 約束下最小化碰撞風險，并利用自適應采樣的方式加快搜索速度。YANG等[7]提出了一種基于采樣法的算法，它基于快速搜索隨機樹（rapidly-exploring random tree， RRT）算法[8]框架建立了機動自動化（maneuver automation， MA）運動基元庫來滿足操縱性約束，并且可以同時滿足COLREGs約束和實時性約束。

本文提出一種基于A*算法同時考慮操縱性約束、靜態與動態障礙物約束、COLREGs約束的慎思型避碰軌跡規劃算法。通過無人三體船自主避碰試驗和模擬試驗，驗證算法的有效性。

1 慎思型避碰軌跡規劃算法實現

1.1 問題的數學描述

為清晰地描述慎思型避碰軌跡規劃問題，現對一些重要的概念進行定義：連續的船舶狀態

s=（x，y，ψ，u，t）

構成連續的船舶狀態空間S，其中x和y為大地坐標，ψ為艏向角，u為船舶縱向速度，t為時間。軌跡是連續的船舶狀態s沿時間維度遞增構成的序列。連續的船舶狀態空間S離散后得到的最小單元稱為網格，一個狀態只對應一個網格。定義離散的控制基元空間Uc，d，其由控制基元uc，d=（ud，ψd，T）構成，其中ud和ψd分別為期望的縱向速度和期望的艏向角，T為執行時間。船舶執行Uc，d中的一個控制基元對應著其在S中從一個狀態變換到另一個狀態的一段連續的航行軌跡，稱為軌跡基元utra=（u0，ud，dψ，T），u0是軌跡基元初始狀態的縱向速度，dψ是軌跡基元的轉艏角度。一系列連續的軌跡基元串聯構成一條完整的軌跡。

算法的目標是在包含障礙物的狀態空間內找到一條從初始狀態s0到最終狀態sg的最經濟的軌跡，并滿足船舶操縱性約束、障礙物約束和COLREGs約束。

1.2 慎思型避碰軌跡規劃算法架構

算法主要包含3個模塊：全局軌跡搜索器、可行軌跡生成器和狀態代價評估器。全局軌跡搜索器采用A*算法框架，在船舶狀態空間內搜索代價最小的全局軌跡，并在搜索過程中不斷地調用可行軌跡生成器和狀態代價評估器。可行軌跡生成器采用控制基元理論，生成滿足操縱性約束的可行軌跡基元以完成完整的全局估計。狀態代價評估器用于計算狀態的代價，作為搜索該往哪個狀態延伸的評價指標。算法架構見圖1。

1.3 全局軌跡搜索器

全局軌跡搜索器在船舶狀態空間內搜索從初始狀態到最終狀態的代價最小的全局軌跡。全局軌跡搜索器基于A*算法，通過可行軌跡生成器生成的軌跡基元延伸到后續子狀態，直至搜索到最終狀態，然后通過最終狀態不斷地回溯前面的父狀態來獲得一條全局軌跡。本文在A*算法的基礎上進行如下改進：（1） 將搜索空間的維度由傳統A*算法的二維提高到四維，增加ψ維度以便確定后續子狀態的延伸方向，增加u維度以便有更多的速度選擇。（2） 狀態的延伸不再由當前網格狀態延伸到其鄰接網格狀態，而是采用可行軌跡生成器向外延伸，以滿足船舶操縱性要求，并提高搜索效率。

全局軌跡搜索器是慎思型避碰軌跡規劃算法的“大腦”，其搜索流程如下：

步驟1 生成優先隊列

Q，將初始狀態s0加入Q中。

步驟2 如果Q為空，則返回“不存在軌跡”;否則，將Q中代價最小的狀態s取出，并將s對應的網格關閉，如果s在終點狀態sg的一定范圍內，則遞歸地回溯s的父狀態來獲得一條軌跡并返回。

步驟3 調用可行軌跡生成器（見第1.4節），以s為起點生成若干軌跡基元，找到軌跡基元末端狀態s′所對應的網格。若該網格已關閉，則拋棄該段軌跡基元。若該網格未關閉，則調用狀態代價評估器（見第1.5節）計算s′的代價：若該網格中有狀態在Q中，則比較該狀態與狀態s′的代價，只將代價較小的狀態保留在Q中;若該網格內沒有其他的狀態，則將狀態s′加入Q中。重復調用步驟2。

以x、y坐標維度舉例說明，如圖2所示：灰色實心圓表示Q中的狀態，黑色實心圓表示已經從Q中取出的狀態;步驟2是從Q中取出代價最小的狀態s，并關閉對應的網格;步驟3是s延伸擴展出3個子狀態s′1、s′2和s′3，因為s′1對應的網格中有Q中的另一個狀態，所以保留代價較小的子狀態s′1，因為子狀態s′2、s′3對應的網格中沒有其他狀態，所以將其加入Q。

1.4 可行軌跡生成器

全局軌跡搜索器在搜索過程中需要由某一狀態擴展到后續子狀態，由于船舶操縱性限制，船舶從一個狀態出發，并不能到達任意的下一狀態。可行軌跡生成器采用控制基元理論，從某一狀態出發沿著軌跡基元延伸到下一狀態，滿足操縱性約束。控制基元理論是一種處理操縱性約束的方法，其基本思想是船舶執行相同的控制基元形成的軌跡段是一條固定不變的軌跡基元，不會隨位置、艏向和時刻的變化發生改變。傳統的生成軌跡基元的方法

是給定目標uc，d=（ud，ψd，T），分別用速度控制器和艏向控制器實現ud和ψd，執行時間T后生成的軌跡就是軌跡基元[5]。為實現船舶從靜止啟動、勻速航行到緊急制動，設置兩

擋速度（設計航速0.8 m/s和停止0 m/s），設置9擋轉艏角度（-60°、-45°、-30°、-15°、0°、15°、30°、45°、60°）和一擋執行時間（8 s），構成軌跡基元庫，見圖3，其中軌跡基元

utra=（u0，ud，dψ，T）。軌跡基元的初始狀態和最終狀態都可能對應兩種不同速度，因此全局軌跡搜索器增加u維度以便實現不同速度選擇。傳統A*算法以當前狀態的前、后、左、右4個方向進行后續狀態的延伸，而可行軌跡生成器只能向當前狀態的前方一定范圍進行延伸，以滿足操縱性約束，因此全局軌跡搜索器需增加ψ維度以便確定后續子狀態的延伸方向。

1.5 狀態代價評估器

1.5.1 狀態代價函數

在全局軌跡搜索器搜索的過程中，狀態代價評估器用來計算當前狀態s′的代價f（s′）：

式中：g（s′）是從初始狀態s0到當前狀態s′的實際代價;h（s′）是從當前狀態s′到最終狀態sg的預估代價;e是系數，用以調節g（s′）與h（s′）之間的權重;s為s′的父狀態，s′由s執行一個控制基元得到;pc，s′表示在執行這個控制基元過程中的碰撞概率，計算方法詳見第1.5.2節;g（s′）由父狀態s的實際代價g（s）、從s到s′的航行過程中可能發生碰撞的代價ce和非碰撞代價cs′三部分構成;ce為一個定值;cs′由耗時tss′、路程dss′和違反COLREGs的代價cCOLREGs三部分構成，cCOLREGs的計算詳見第1.5.3節;h（s′）由從s′到sg的估計時間ts′sg和估計路程ds′sg構成;ωc、tmax和dmax為系數。

第二個場景包含復雜的靜態障礙物和2艘來船，本船從地圖右上方向左下方航行，與第一艘來船形成對遇局面，與第二艘來船形成右舷交叉會遇局面。慎思型避碰軌跡規劃算法給出的軌跡見圖8a，不考慮COLREGs的規劃算法給出的軌跡見圖8b。如果不考慮COLREGs，算法在依次面對兩個會遇局面時都會選擇向左避讓，雖然全局軌跡會更短，但是違反了COLREGs。考慮COLREGs的慎思型避碰軌跡規劃算法雖然規劃出的軌跡較長，但在依次面對兩個會遇局面時均能遵守COLREGs避開來船，因此更合理。

4 結 論

本文提出一種慎思型避碰軌跡規劃算法，在A*算法的基礎上進行了大量的改進，將狀態由二維空間擴展到四維空間，利用控制基元方法延伸狀態并同時滿足操縱性要求，重新設計了代價函數來避開復雜障礙物和滿足COLREGs約束。通過三體船的自主避碰航行試驗和仿真試驗，驗證了算法的有效性，具有較高的參考價值。

算法仍存在問題和改進的空間：（1）動態障礙物的預測軌跡與其真實航行軌跡之間的差別是一個重要影響因素，如果他船也是具有自主規劃能力的智能船舶，則軌跡規劃會陷入一種博弈局面，未來的研究可以考慮預測他船航行意圖。（2）在環境干擾（風浪流）較大的情況下，軌跡跟隨會變得非常困難，如何設計更好的軌跡跟隨方法也是后續的研究方向。（3）減小網格大小、增加軌跡基元的數量都能進一步優化規劃軌跡，但是計算量會大大增加，給滿足實時性約束帶來挑戰，因此如何在軌跡最優性與實時性之間進行權衡也是今后研究的一個難點。（4）鑒于上述問題和不足，算法的實用性需要進一步研究與驗證。

參考文獻：

[1]EMSA. Annual overview of marine casualties and incidents 2017[R]. European Maritime Safety Agency， 2018： 15-41.

[2]CAMPBELL S， NAEEM W， IRWIN G W. A review on improving the autonomy of unmanned surface vehicles through intelligent collision avoidance manoeuvres[J]. Annual Reviews in Control， 2012， 36（2）： 267-283. DOI： 10.1016/j.arcontrol.2012.09.008.

[3]中國船級社（CCS）. 智能船舶規范[S]. 2015.

[4]中國船級社（CCS）. 自主貨物運輸船舶指南[S]. 2018.

[5]SHAH B C， VEC P， BERTASKA I R， et al. Resolution-adaptive risk-aware trajectory planning for surface vehicles operating in congested civilian traffic[J]. Autonomous Robots， 2016， 40（7）： 1139-1163. DOI： 10.1007/s10514-015-9529-x.

[6]HART P E， NILSSON N J， RAPHAEL B. A formal basis for the heuristic determination of minimum cost paths[J]. IEEE Transactions on Systems Science and Cybernetics， 1968， 4（2）： 100-107.

[7]YANG Rongwu， XU Jinsong， WANG Xin， et al. Parallel trajectory planning for shipborne autonomous collision avoidance system[J]. Applied Ocean Research， 2019， 91： 101875. DOI： 10.1016/j.apor.2019.101875.

[8]LAVALLE S M. Rapidly-exploring random trees： a new tool for path planning[R]. Iowa： Iowa State University， 1998.

[9]趙勁松， 王逢辰. 船舶避碰學原理[M]. 大連： 大連海事大學出版社， 1999.

[10]IMO. Convention on the international regulations for preventing collisions at sea[S]. http：//www.imo.org/conventions.

[11]KUWATA Y， WOLF M T， ZARZHITSKY D， et al. Safe maritime autonomous navigation with COLREGs using velocity obstacles[J]. IEEE Journal of Oceanic Engineering， 2013， 39（1）： 110-119. DOI： 10.1109/JOE.2013.2254214.

[12]COLITO J. Autonomous mission planning and execution for unmanned surface vehicles in compliance with the marine rules of the road[D]. Washington： University of Washington， 2007.

[13]SVEC P， SHAH B C， BERTASKA I R， et al. Dynamics-aware target following for an autonomous surface vehicle operating under COLREGs in civilian traffic[C]//2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE， 2013： 3871-3878.

（編輯 趙勉）

收稿日期： 2019-11-26

修回日期： 2020-02-24

作者簡介：

秦操（1994—），男，湖北荊州人，碩士研究生，研究方向為船舶自主避碰，（E-mail）403502830@qq.com;

楊榮武（1984—），男，福建漳州人，博士，研究方向為船舶自主避碰，（E-mail）yangrongwu@seastel.com