秦操　楊榮武

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

關鍵詞：船舶; 自主避碰; 慎思型軌跡規(guī)劃; 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 引 言

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

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

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

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

1 慎思型避碰軌跡規(guī)劃算法實現(xiàn)

1.1 問題的數(shù)學描述

為清晰地描述慎思型避碰軌跡規(guī)劃問題，現(xiàn)對一些重要的概念進行定義：連續(xù)的船舶狀態(tài)

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

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

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

1.2 慎思型避碰軌跡規(guī)劃算法架構(gòu)

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

1.3 全局軌跡搜索器

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

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

步驟1 生成優(yōu)先隊列

Q，將初始狀態(tài)s0加入Q中。

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

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

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

1.4 可行軌跡生成器

全局軌跡搜索器在搜索過程中需要由某一狀態(tài)擴展到后續(xù)子狀態(tài)，由于船舶操縱性限制，船舶從一個狀態(tài)出發(fā)，并不能到達任意的下一狀態(tài)。可行軌跡生成器采用控制基元理論，從某一狀態(tài)出發(fā)沿著軌跡基元延伸到下一狀態(tài)，滿足操縱性約束?？刂苹碚撌且环N處理操縱性約束的方法，其基本思想是船舶執(zhí)行相同的控制基元形成的軌跡段是一條固定不變的軌跡基元，不會隨位置、艏向和時刻的變化發(fā)生改變。傳統(tǒng)的生成軌跡基元的方法

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

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

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

1.5 狀態(tài)代價評估器

1.5.1 狀態(tài)代價函數(shù)

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

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

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

4 結(jié) 論

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

算法仍存在問題和改進的空間：（1）動態(tài)障礙物的預測軌跡與其真實航行軌跡之間的差別是一個重要影響因素，如果他船也是具有自主規(guī)劃能力的智能船舶，則軌跡規(guī)劃會陷入一種博弈局面，未來的研究可以考慮預測他船航行意圖。（2）在環(huán)境干擾（風浪流）較大的情況下，軌跡跟隨會變得非常困難，如何設計更好的軌跡跟隨方法也是后續(xù)的研究方向。（3）減小網(wǎng)格大小、增加軌跡基元的數(shù)量都能進一步優(yōu)化規(guī)劃軌跡，但是計算量會大大增加，給滿足實時性約束帶來挑戰(zhàn)，因此如何在軌跡最優(yōu)性與實時性之間進行權衡也是今后研究的一個難點。（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）. 智能船舶規(guī)范[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