楊榮武, 許勁松, 王 鑫

(1. 上海交通大學(xué) 高新船舶與深海開(kāi)發(fā)裝備協(xié)同創(chuàng)新中心，上海 200240；2. Marine Technology Department, Newcastle University, Newcastle upon Tyne, NE1 7RU, UK)

海上碰撞一直是影響船舶航行的重大安全問(wèn)題，尤其是大型船舶的碰撞事故，不但會(huì)造成人類(lèi)生命及經(jīng)濟(jì)財(cái)產(chǎn)上的巨大損失，更會(huì)給海上的生態(tài)環(huán)境帶來(lái)災(zāi)難性的后果(https:∥en.wikipedia.org/wiki/Sanchi_oil_tanker_collision).國(guó)際海事組織(IMO)在1972年發(fā)布了《國(guó)際海上避碰(COLREGs)規(guī)則》[1]，針對(duì)船舶航行中可能會(huì)遇場(chǎng)景給出了操船規(guī)范以及一般性的避讓原則，旨在最大程度地降低船舶的碰撞危險(xiǎn).然而，由于海上交通的日益繁忙，人工操船難以完全遵守 COLREGs 規(guī)則，以致于惡性的碰撞事故仍然頻繁發(fā)生.歐洲海事安全局(EMSA)最新發(fā)布的《2017海上事故年度回顧報(bào)告》[2]顯示，在2016年僅歐盟成員國(guó)所報(bào)告的海上事故就多達(dá) 3 145 起，其中50%的事故為船舶碰撞以及擱淺，而60%以上的事故是由人為原因造成的.

隨著近年來(lái)自主航行技術(shù)的持續(xù)發(fā)展及逐步應(yīng)用，船舶自主避碰系統(tǒng)的研究越來(lái)越受到各方面的關(guān)注[3-6].該類(lèi)系統(tǒng)必須具備以下3個(gè)功能：① 通過(guò)與外界環(huán)境交互的傳感器完成環(huán)境信息的采集與融合；② 根據(jù)數(shù)據(jù)處理獲得的環(huán)境信息完成軌跡規(guī)劃以及航行決策；③ 根據(jù)規(guī)劃軌跡完成對(duì)底層執(zhí)行機(jī)構(gòu)的自動(dòng)控制以實(shí)現(xiàn)精準(zhǔn)的循跡航行.其中，軌跡規(guī)劃的目標(biāo)是在障礙物環(huán)境中完成最優(yōu)軌跡的搜索計(jì)算，并且滿足靜態(tài)障礙物約束、船舶操縱性約束、COLREGs 規(guī)則約束、軌跡最優(yōu)性及規(guī)劃實(shí)時(shí)性等限制條件.為了解決上述復(fù)雜的耦合問(wèn)題，軌跡規(guī)劃一般采用分層解耦架構(gòu)(HDA)[4-6]，將必須滿足的限制條件分解在全局規(guī)劃以及局部規(guī)劃兩個(gè)層面進(jìn)行解耦處理.全局規(guī)劃采用慎思型規(guī)劃(DP)算法，即在兩個(gè)長(zhǎng)程路徑點(diǎn)之間考慮靜態(tài)障礙物約束以及路徑最優(yōu)性，生成初始全局路徑，以保證規(guī)劃的完備性；而局部規(guī)劃則采用反應(yīng)型規(guī)劃(RP)算法，跟隨全局路徑實(shí)時(shí)生成規(guī)避動(dòng)態(tài)障礙物的局部軌跡，以滿足操縱性約束以及COLREGs規(guī)則約束.這種解耦處理方式在大多數(shù)情況下可以獲得較快的、響應(yīng)性較好的規(guī)劃結(jié)果，但在復(fù)雜限制水域場(chǎng)景下，DP算法會(huì)因?yàn)椴豢紤]操縱性約束而生成不合理的全局路徑，使得在局部規(guī)劃階段無(wú)法生成可行的軌跡[7].

慎思型規(guī)劃的兩類(lèi)常用算法是圖基法(graph-based search)和采樣法(sampling-based search)[8].前者以A*算法為代表，后者以快速擴(kuò)展隨機(jī)樹(shù)(RRT)算法為代表.A*算法是一種非常適用于機(jī)器人最優(yōu)路徑搜索的啟發(fā)式搜索算法，但其難以處理復(fù)雜的動(dòng)力學(xué)約束問(wèn)題，在搜索網(wǎng)格數(shù)目過(guò)大時(shí)無(wú)法滿足計(jì)算實(shí)時(shí)性的要求.因此，在船舶軌跡規(guī)劃的應(yīng)用中不得不盡量降低搜索網(wǎng)格的精度，在復(fù)雜限制水域場(chǎng)景下存在安全隱患[6].而RRT算法可以比較方便地處理復(fù)雜的動(dòng)力學(xué)約束，算法的實(shí)時(shí)性較好，能快速收斂到一條可行路徑，但無(wú)法保證路徑的最優(yōu)性，對(duì)航行的經(jīng)濟(jì)性及安全性都有一定的影響[5].

針對(duì)上述DP算法的缺點(diǎn)，本文提出一種改進(jìn)的慎思型軌跡規(guī)劃(DTP)算法，通過(guò)位形空間搜索指引器、可行軌跡生成器以及全局最優(yōu)軌跡搜索器3個(gè)模塊來(lái)滿足靜態(tài)障礙物約束、船舶操縱性約束以及最優(yōu)性要求，在兩個(gè)長(zhǎng)程路徑點(diǎn)之間完成全局軌跡規(guī)劃，以保證規(guī)劃軌跡的可行性、完備性和最優(yōu)性.通過(guò)一條案例三體船的自主避碰仿真模擬和航行試驗(yàn)，從多方面驗(yàn)證了所提DTP算法的有效性及優(yōu)越性.

1 DTP算法

1.1 DTP算法的架構(gòu)

DTP算法由多個(gè)模塊構(gòu)成.首先，基于RRT改進(jìn)算法建立位形空間搜索指引器，保證滿足靜態(tài)障礙物約束；再以船舶機(jī)動(dòng)自動(dòng)機(jī)(MA)軌跡基元庫(kù)為基礎(chǔ)，建立可行軌跡生成器，結(jié)合搜索指引器生成動(dòng)力學(xué)可行的所有全局軌跡方案，以保證滿足船舶操縱性約束；最后，基于可行軌跡的代價(jià)函數(shù)建立全局最優(yōu)軌跡搜索器，通過(guò)迭代優(yōu)選搜索出全局最優(yōu)軌跡，以保證滿足最優(yōu)性要求.

1.2 位形空間搜索指引器

位形空間搜索指引器用以在兩個(gè)長(zhǎng)程路徑點(diǎn)之間搜索出一組能避開(kāi)所有靜態(tài)障礙物的安全路徑，構(gòu)成后續(xù)可行軌跡的基礎(chǔ).路徑搜索算法是在RRT算法的基礎(chǔ)上加以改進(jìn)獲得的.

RRT算法不需要預(yù)先對(duì)搜索環(huán)境進(jìn)行建模，而是在路徑搜索過(guò)程中同步建立環(huán)境模型，具有快速搜索非凸高維空間的能力[8].以當(dāng)前狀態(tài)為根節(jié)點(diǎn)，采用Halton偽隨機(jī)方法在搜索空間隨機(jī)采樣獲得一個(gè)采樣點(diǎn)i；以i為目標(biāo)點(diǎn)，就近選擇RRT上已有的節(jié)點(diǎn)j，用直線連接點(diǎn)i和j生成邊e；使用碰撞檢測(cè)技術(shù)檢查e是否與障礙物相交；若e不與障礙物相交，將候選采樣點(diǎn)i和e添加進(jìn)RRT，形成新節(jié)點(diǎn)i′和分支e′；按照上述過(guò)程逐次擴(kuò)展RRT的節(jié)點(diǎn)和分支，直至某個(gè)新節(jié)點(diǎn)到達(dá)目標(biāo)區(qū)域，并形成由多段分支構(gòu)成的從初始狀態(tài)到達(dá)目標(biāo)區(qū)域的可行路徑.RRT算法中所采用的Halton隨機(jī)采樣策略可以保證整個(gè)搜索空間都被采樣搜索，即保證了搜索的完備性.而通過(guò)添加樹(shù)節(jié)點(diǎn)的方式跟蹤標(biāo)記已訪問(wèn)過(guò)的采樣點(diǎn)，可以避免在同一區(qū)域的循環(huán)搜索，即保證了搜索的系統(tǒng)性.隨著采樣點(diǎn)的增加，RRT算法能快速搜索到一條可行路徑，但并不能保證該路徑的最優(yōu)性.

在上述RRT算法的基礎(chǔ)上做出如下兩點(diǎn)改進(jìn)：① 每一個(gè)新的采樣點(diǎn)不但連接最近樹(shù)節(jié)點(diǎn)，還同樣連接所有其他樹(shù)節(jié)點(diǎn)形成多條邊，每條通過(guò)碰撞檢測(cè)的邊e都被加入RRT，構(gòu)成一個(gè)新節(jié)點(diǎn)和多條新分支.當(dāng)某個(gè)新節(jié)點(diǎn)到達(dá)目標(biāo)區(qū)域時(shí)，將形成從初始狀態(tài)到達(dá)目標(biāo)區(qū)域的多條可行路徑.這種遍歷樹(shù)節(jié)點(diǎn)連接的方式可以獲得多條可行路徑，并最大程度地保留可能的最優(yōu)路徑.② 每一個(gè)新采樣點(diǎn)不但連接所有樹(shù)節(jié)點(diǎn)，還直接與目標(biāo)點(diǎn)相連.如果該連線通過(guò)了碰撞檢測(cè)，則同樣被加入RRT，快速形成新的可行路徑.這種貪婪算法的引入可以極大地提高可行路徑的搜索效率.

1.3 可行軌跡生成器

位形空間指引器所生成的路徑由樹(shù)節(jié)點(diǎn)之間的直線分支串聯(lián)構(gòu)成，適用于動(dòng)作靈活的移動(dòng)機(jī)器人.然而，船舶無(wú)法實(shí)現(xiàn)緊急制動(dòng)和隨意轉(zhuǎn)彎等靈活的操縱動(dòng)作，存在較強(qiáng)的動(dòng)力學(xué)限制，需要采用可行軌跡段替代樹(shù)節(jié)點(diǎn)之間的直線分支，并串聯(lián)形成兩個(gè)長(zhǎng)程路徑點(diǎn)之間完整的可行軌跡，以保證動(dòng)力學(xué)方面的可行性[9].

采用MA模型[10-11]，定義兩類(lèi)滿足運(yùn)載器動(dòng)力學(xué)約束的軌跡基元(trajectory primitive).其中：定常軌跡(tr)基元對(duì)應(yīng)保持恒定速度的定常巡航狀態(tài)，可通過(guò)直接求解操縱性方程獲得；而機(jī)動(dòng)軌跡(ma)基元對(duì)應(yīng)任意兩段tr基元之間的過(guò)渡軌跡，可由操縱性方程結(jié)合控制器生成.通過(guò)串聯(lián)組合兩類(lèi)軌跡基元，可以在任意兩個(gè)相鄰RRT節(jié)點(diǎn)之間構(gòu)建所有可行軌跡段.

對(duì)于只能控制縱向速度和艏向的欠驅(qū)動(dòng)船舶，tr基元由定常縱向速度u和定常轉(zhuǎn)艏角速度r所確定，而所有(u,r)組合生成的tr基元構(gòu)成定常軌跡基元庫(kù)T.從任意一個(gè)tr基元(ui,ri)過(guò)渡到另一個(gè)tr基元(uj,rj)的可行軌跡即為一個(gè)ma基元，也是速度控制器從初始狀態(tài)(ui,ri)變化到目標(biāo)狀態(tài)(uj,rj)的閉環(huán)階躍響應(yīng)，所有ma基元構(gòu)成機(jī)動(dòng)軌跡基元庫(kù)M.庫(kù)中的軌跡基元越豐富，組合出來(lái)的操控方式就越多，離散的MA模型就越趨近于船舶操縱性方程的完整解.

從初始狀態(tài)矢量M0前往目標(biāo)節(jié)點(diǎn)Mf的軌跡可以表達(dá)為一個(gè)由兩類(lèi)軌跡基元串聯(lián)組合成的基元序列.若設(shè)定船舶總是采用定常直航的方式接近目標(biāo)節(jié)點(diǎn)Mf，則基元序列的最后一段必然是定常直航的tr基元(u,r)|r=0,u≠0；在基元序列中還需要一段定?；剞D(zhuǎn)的tr基元(u,r)|r≠0以完成航向的調(diào)整；而為了保證軌跡上任何時(shí)刻位置、航向、速度、加速度的連續(xù)性，在基元序列中還需要最多兩段ma基元以實(shí)現(xiàn)不同定常巡航狀態(tài)的平滑切換，如圖1所示.由圖1可知，船舶從初始狀態(tài)M0到達(dá)目標(biāo)節(jié)點(diǎn)Mf的可行軌跡可以通過(guò)串聯(lián)最多4段軌跡基元——機(jī)動(dòng)軌跡ma1基元(ma1)、定?；剞D(zhuǎn)tr2基元(tr2)、機(jī)動(dòng)軌跡ma3基元(ma3)、定常直航tr4基元(tr4)實(shí)現(xiàn)，其中兩段tr基元的時(shí)長(zhǎng)屬于可調(diào)參數(shù).

圖1 由MA軌跡基元生成的可行軌跡段[9]Fig.1 Generation of trajectory segment from MA primitives[9]

在MA框架的軌跡基元庫(kù)中選擇不同的tr基元和ma基元進(jìn)行組合，可以在RRT樹(shù)節(jié)點(diǎn)之間生成多種不同的可行軌跡段，串聯(lián)這些可行軌跡段最終能夠生成多條全局可行軌跡，而這其中必然存在一條全局最優(yōu)軌跡.

1.4 全局最優(yōu)軌跡搜索器

為了實(shí)現(xiàn)全局最優(yōu)軌跡的搜索，首先構(gòu)建用以度量軌跡質(zhì)量的總代價(jià)函數(shù)C(x),

C(x)=αt1(x)+βt2(x)

(1)

式中：t(x)為經(jīng)濟(jì)性代價(jià)函數(shù)；s(x)為安全性代價(jià)函數(shù)；α為耗費(fèi)時(shí)間權(quán)重；β為安全性權(quán)重；x為當(dāng)前位置.通過(guò)權(quán)重α與β可以調(diào)整總代價(jià)函數(shù)中經(jīng)濟(jì)性與安全性的權(quán)重關(guān)系，兩者的取值范圍均為0～1，可以根據(jù)航行任務(wù)的特點(diǎn)來(lái)確定具體的取值.經(jīng)濟(jì)性代價(jià)函數(shù)t1(x)為軌跡從起始位置到當(dāng)前位置x的總耗時(shí).安全性代價(jià)函數(shù)t2(x)值等于該段軌跡通過(guò)靜態(tài)障礙物周?chē)踩彌_區(qū)的時(shí)長(zhǎng)，若與靜態(tài)障礙物發(fā)生碰撞則增加至∞.安全緩沖區(qū)是環(huán)繞在障礙物外圍的一個(gè)環(huán)形區(qū)域，其寬度d可由船舶的最大速度vmax及最大倒車(chē)加速度amax確定，表達(dá)式如下[5]：

(2)

在所有全局可行軌跡中，總代價(jià)函數(shù)C(x)最小的軌跡為全局最優(yōu)軌跡，其優(yōu)選過(guò)程如圖2所示.新節(jié)點(diǎn)w與根節(jié)點(diǎn)p間有多條可行軌跡段，從中選出代價(jià)最小的軌跡段pw(見(jiàn)圖2(a))；遍歷RRT已有節(jié)點(diǎn)p、q、s，可以獲得通向新節(jié)點(diǎn)w的3條可行軌跡段pw、qw、sw(見(jiàn)圖2(b))；通過(guò)比較軌跡代價(jià)，在上述3條可行軌跡段中優(yōu)選出軌跡段pw，并在新節(jié)點(diǎn)w與終點(diǎn)g間也生成可行軌跡段，獲得1條全局可行軌跡pwg(見(jiàn)圖2(c))；重復(fù)上述過(guò)程，得到新的節(jié)點(diǎn)h、c，可以獲得3條全局可行軌跡pwg、pwhg、pqcg，通過(guò)對(duì)比軌跡代價(jià)獲得全局最優(yōu)軌跡pwg(見(jiàn)圖2(d)).

圖2 全局最優(yōu)軌跡的搜索過(guò)程示意Fig.2 Process illustration of optimal global trajectory search

2 限制水域自主避碰試驗(yàn)

2.1 案例船避碰航行試驗(yàn)方法

針對(duì)案例三體船構(gòu)建如圖3所示的自主避碰系統(tǒng)，通過(guò)航行試驗(yàn)驗(yàn)證DTP算法的有效性及可靠性.

案例三體船采用3個(gè)S標(biāo)準(zhǔn)型船體，主船體總長(zhǎng)為1.2 m，附船體總長(zhǎng)為1.0 m，三體總寬度為0.9 m，船體線型如圖4所示，三體船的主要參數(shù)如表1所示.推進(jìn)機(jī)構(gòu)為一對(duì)外懸于船中兩側(cè)的、直徑為67 mm的四葉螺旋槳，由兩個(gè)螺旋槳轉(zhuǎn)速不同產(chǎn)生的差分推力控制航速和轉(zhuǎn)向.GPS信號(hào)接收器、姿態(tài)參考系統(tǒng)傳感器、無(wú)線路由器、一對(duì)電動(dòng)機(jī)驅(qū)動(dòng)器、船載主機(jī)以及船載電池全部集成安裝于控制箱內(nèi)，并固定安裝在甲板上，如圖5所示.試驗(yàn)時(shí)在地圖模塊中直接構(gòu)建靜態(tài)障礙物區(qū)域，三體船根據(jù)傳感器實(shí)測(cè)數(shù)據(jù)以及地圖模塊進(jìn)行軌跡規(guī)劃及循跡控制以完成自主避碰航行試驗(yàn).試驗(yàn)中設(shè)定DTP算法的計(jì)算更新頻率為30 s-1.

圖3 案例三體船的自主避碰系統(tǒng)框架Fig.3 Collision avoidance system framework for trimaran

圖4 三體船主體型線圖Fig.4 Lines plan of the trimaran main hull

表1 三體船主要參數(shù)Tab.1 Main parameters of the trimaran

圖5 案例三體船實(shí)物圖Fig.5 Autonomous trimaran

2.2 DTP算法的參數(shù)設(shè)置

案例三體船的3自由度操縱性方程采用如下分離式(MMG)模型[12]：

(3)

三體船的質(zhì)量和轉(zhuǎn)動(dòng)慣量可以通過(guò)測(cè)量確定，附加質(zhì)量和附加轉(zhuǎn)動(dòng)慣量可以根據(jù)經(jīng)驗(yàn)公式估算[13]，而其他水動(dòng)力項(xiàng)的表達(dá)式則需要以質(zhì)量項(xiàng)為基準(zhǔn)采用辨識(shí)方法獲得[14]，控制變量為一對(duì)螺旋槳的轉(zhuǎn)速ni(i=l,r)，

(4)

(5)

(6)

i=l,r

DTP算法采用MA框架構(gòu)建可行軌跡生成器，需要根據(jù)式(3)～(6)預(yù)先建立定常軌跡基元庫(kù)和機(jī)動(dòng)軌跡基元庫(kù).在定常軌跡基元庫(kù)中，將u按照高速(1 m/s)、低速(0.6 m/s)分為2組；將r按照高速轉(zhuǎn)彎(±0.3 rad/s)、中速轉(zhuǎn)彎(±0.2 rad/s)、低速轉(zhuǎn)彎(±0.1 rad/s)及直航(0 rad/s)分為7組(左轉(zhuǎn)彎取為負(fù)值)，共獲得14個(gè)tr基元.根據(jù)每一個(gè)tr基元的u、r速度值，求解式(3)～(6)可以獲得一對(duì)螺旋槳的ni，并將其作為控制器的輸入.在機(jī)動(dòng)軌跡基元庫(kù)中，每一個(gè)ma基元都是速度控制器從一個(gè)tr基元(ui,ri)變化到另一個(gè)tr基元(uj,rj)的閉環(huán)階躍響應(yīng)，需要調(diào)用速度控制器并經(jīng)過(guò)離線計(jì)算獲得.案例三體船采用經(jīng)典的比例-積分-微分(PID)控制器實(shí)現(xiàn)對(duì)于u、r的控制，控制變量為一對(duì)螺旋槳的ni，所有控制器參數(shù)經(jīng)調(diào)試過(guò)程加以確定.

2.3 自主避碰試驗(yàn)結(jié)果

試驗(yàn)場(chǎng)景包含較為復(fù)雜的靜態(tài)障礙物區(qū)域及直接在地圖模塊中構(gòu)建的虛擬障礙物.案例三體船在該障礙物區(qū)域內(nèi)的避碰規(guī)劃軌跡和實(shí)際航行軌跡如圖6所示.其中，綠色區(qū)域?yàn)殪o態(tài)障礙物；輪廓線外部區(qū)域?yàn)榘踩彌_區(qū)；箭頭指向?yàn)檐壽E方向；紅色三角為初始位置；紅色圓點(diǎn)為終點(diǎn)；t為航行時(shí)間.航行過(guò)程中的各項(xiàng)實(shí)測(cè)數(shù)據(jù)如圖7所示.其中，ψ為艏向角；ud為目標(biāo)縱向速度；rd為目標(biāo)轉(zhuǎn)艏角速度.全局地圖是一張網(wǎng)格地圖，地圖分辨率為 0.5 m×0.5 m.

圖6 案例船在限制水域的避碰規(guī)劃軌跡和實(shí)際軌跡Fig.6 The planning trajectory and real trajectory of example ship for collision avoidance in restricted waters

圖7 案例船在限制水域航行中的各項(xiàng)實(shí)測(cè)數(shù)據(jù)Fig.7 The measured data of example ship during the collision avoidance tests in restricted waters

由圖6和7可知，DTP算法在航行開(kāi)始后t=50，80，110 s時(shí)刻更新生成的全局最優(yōu)軌跡變動(dòng)較小，而實(shí)際航行軌跡可以很好地跟隨規(guī)劃軌跡，安全地避開(kāi)所有障礙物順利到達(dá)終點(diǎn)，沒(méi)有出現(xiàn)急劇轉(zhuǎn)向等不穩(wěn)定狀態(tài).在t=60～70 s以及t=90～100 s兩個(gè)時(shí)間段，軌跡出現(xiàn)兩次較大幅度的艏向角變化(對(duì)應(yīng)兩次轉(zhuǎn)彎過(guò)程)為合理的控制響應(yīng).在軌跡代價(jià)函數(shù)中，將安全性和經(jīng)濟(jì)性的權(quán)重系數(shù)均設(shè)定為0.5，規(guī)劃軌跡優(yōu)選u=1.0 m/s的高速tr基元.因此，案例三體船在啟動(dòng)后快速趨近并將速度保持在1.0 m/s，試驗(yàn)結(jié)果證明了DTP算法的可行性、實(shí)時(shí)性與有效性.

圖8 慎思型軌跡規(guī)劃和A*路徑規(guī)劃的結(jié)果比較Fig.8 Comparison of deliberative trajectory planning and A* path planning

3 限制水域自主避碰仿真

3.1 案例船避碰航行仿真方法

為驗(yàn)證DTP算法在不同限制水域中的應(yīng)用效果，進(jìn)一步采用仿真試驗(yàn)法實(shí)現(xiàn)案例三體船在限制水域中的自主避碰航行.在MATLAB/Simulink軟件平臺(tái)上構(gòu)建的自主避碰仿真框架圖與圖3一致.其中，大部分模塊都直接由模型試驗(yàn)中的對(duì)應(yīng)模塊移植而來(lái)，而案例船的操縱物理模型采用式(3)代替，以生成運(yùn)動(dòng)響應(yīng)信號(hào)并輸入傳感器模塊.試驗(yàn)場(chǎng)景中需要包含的靜態(tài)障礙物區(qū)域可以在地圖模塊中直接構(gòu)建.

3.2 自主避碰仿真結(jié)果

第1個(gè)應(yīng)用場(chǎng)景包含復(fù)雜的靜態(tài)障礙物，由DTP算法獲得的全局軌跡如圖8(a)所示，沒(méi)有考慮操縱性約束的A*算法的規(guī)劃路徑如圖8(b)所示.由圖8可知，A*算法雖然是一種最優(yōu)路徑規(guī)劃算法，但因?yàn)闆](méi)有考慮操縱性約束，使得圖8(b)中的全局路徑進(jìn)入了狹窄航道.由于實(shí)際航行軌跡無(wú)法跟隨所規(guī)劃的路徑完成急轉(zhuǎn)彎，船舶最終將會(huì)撞上障礙物.而由DTP算法規(guī)劃的軌跡雖然不是最短航線，但其規(guī)劃軌跡滿足操縱性約束，船舶的實(shí)際航行軌跡可以完全跟隨所規(guī)劃的軌跡，有效地避開(kāi)障礙物并到達(dá)目標(biāo)點(diǎn).

第2個(gè)應(yīng)用場(chǎng)景同樣包含復(fù)雜的靜態(tài)障礙物區(qū)域，采用常規(guī)RRT規(guī)劃算法獲得的全局軌跡如圖9所示；采用DTP算法獲得的全局最優(yōu)軌跡如圖10所示.通過(guò)對(duì)比可以看出，圖9中每隔30 s更新生成的全局規(guī)劃軌跡雖然可行，但存在非常明顯的跳躍現(xiàn)象.這會(huì)導(dǎo)致船舶實(shí)際航行時(shí)不得不隨規(guī)劃軌跡的更新而急劇轉(zhuǎn)向，實(shí)際軌跡不能完全跟隨規(guī)劃軌跡，對(duì)航行安全及航行效率造成不利的影響.而圖10同樣每隔30 s更新生成的全局規(guī)劃軌跡都接近最優(yōu)結(jié)果，相互之間變動(dòng)較小，實(shí)際航行軌跡可以穩(wěn)定地跟隨所規(guī)劃的軌跡，具有非常好的穩(wěn)定性和安全性，充分地說(shuō)明了耦合最優(yōu)軌跡搜索算法對(duì)于慎思型規(guī)劃的重要意義.

圖9 常規(guī)RRT算法的規(guī)劃結(jié)果Fig.9 Trajectories of conventional RRT planning

圖10 DTP算法的規(guī)劃結(jié)果Fig.10 Trajectories of DTP Planning

4 結(jié)論

軌跡規(guī)劃是船舶自主避碰系統(tǒng)的重要功能之一.針對(duì)DP算法現(xiàn)存的缺陷，提出一種基于RRT的改進(jìn)算法——DTP算法，耦合處理靜態(tài)障礙物約束、船舶操縱性約束、軌跡最優(yōu)性等限制條件，在兩個(gè)長(zhǎng)程路徑點(diǎn)之間完成全局軌跡規(guī)劃，以保證規(guī)劃軌跡的可行性、完備性與最優(yōu)性.

針對(duì)一條案例三體船構(gòu)建完整的自主避碰系統(tǒng)，成功地實(shí)現(xiàn)了案例船自主避碰的航行及仿真試驗(yàn).在限制水域靜態(tài)障礙物的場(chǎng)景下，該船可以有效地完成自主避碰航行，從多方面驗(yàn)證了所提DTP算法的可行性與有效性，對(duì)后續(xù)船舶自主避碰軌跡規(guī)劃研究及未來(lái)應(yīng)用具有重要的意義.