復(fù)雜水域慎思型避碰軌跡規(guī)劃

秦操　楊榮武

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

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

中圖分類號(hào)：? U675.73

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

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

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

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

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

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

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

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

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

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

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

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

1.3 全局軌跡搜索器

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

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

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

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

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

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

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

1.4 可行軌跡生成器

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

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

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

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

1.5 狀態(tài)代價(jià)評(píng)估器

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

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

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

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

4 結(jié) 論

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

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

參考文獻(xiàn)：

[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]中國(guó)船級(jí)社（CCS）. 智能船舶規(guī)范[S]. 2015.

[4]中國(guó)船級(jí)社（CCS）. 自主貨物運(yùn)輸船舶指南[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]趙勁松， 王逢辰. 船舶避碰學(xué)原理[M]. 大連： 大連海事大學(xué)出版社， 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

作者簡(jiǎn)介：

秦操（1994—），男，湖北荊州人，碩士研究生，研究方向?yàn)榇白灾鞅芘觯‥-mail）403502830@qq.com;

楊榮武（1984—），男，福建漳州人，博士，研究方向?yàn)榇白灾鞅芘?，（E-mail）yangrongwu@seastel.com