楊榮武, 許勁松, 王 鑫

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

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

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

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

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

1 DTP算法

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

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

1.2 位形空間搜索指引器

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

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

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

1.3 可行軌跡生成器

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

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

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

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

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

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

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

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

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

(1)

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

(2)

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

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

2 限制水域自主避碰試驗

2.1 案例船避碰航行試驗方法

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

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

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

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

(3)

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

(4)

(5)

(6)

i=l,r

DTP算法采用MA框架構(gòu)建可行軌跡生成器，需要根據(jù)式(3)～(6)預(yù)先建立定常軌跡基元庫和機動軌跡基元庫.在定常軌跡基元庫中，將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)彎取為負值)，共獲得14個tr基元.根據(jù)每一個tr基元的u、r速度值，求解式(3)～(6)可以獲得一對螺旋槳的ni，并將其作為控制器的輸入.在機動軌跡基元庫中，每一個ma基元都是速度控制器從一個tr基元(ui,ri)變化到另一個tr基元(uj,rj)的閉環(huán)階躍響應(yīng)，需要調(diào)用速度控制器并經(jīng)過離線計算獲得.案例三體船采用經(jīng)典的比例-積分-微分(PID)控制器實現(xiàn)對于u、r的控制，控制變量為一對螺旋槳的ni，所有控制器參數(shù)經(jīng)調(diào)試過程加以確定.

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

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

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

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

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

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

3 限制水域自主避碰仿真

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

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

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

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

第2個應(yīng)用場景同樣包含復(fù)雜的靜態(tài)障礙物區(qū)域，采用常規(guī)RRT規(guī)劃算法獲得的全局軌跡如圖9所示；采用DTP算法獲得的全局最優(yōu)軌跡如圖10所示.通過對比可以看出，圖9中每隔30 s更新生成的全局規(guī)劃軌跡雖然可行，但存在非常明顯的跳躍現(xiàn)象.這會導(dǎo)致船舶實際航行時不得不隨規(guī)劃軌跡的更新而急劇轉(zhuǎn)向，實際軌跡不能完全跟隨規(guī)劃軌跡，對航行安全及航行效率造成不利的影響.而圖10同樣每隔30 s更新生成的全局規(guī)劃軌跡都接近最優(yōu)結(jié)果，相互之間變動較小，實際航行軌跡可以穩(wěn)定地跟隨所規(guī)劃的軌跡，具有非常好的穩(wěn)定性和安全性，充分地說明了耦合最優(yōu)軌跡搜索算法對于慎思型規(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)的重要功能之一.針對DP算法現(xiàn)存的缺陷，提出一種基于RRT的改進算法——DTP算法，耦合處理靜態(tài)障礙物約束、船舶操縱性約束、軌跡最優(yōu)性等限制條件，在兩個長程路徑點之間完成全局軌跡規(guī)劃，以保證規(guī)劃軌跡的可行性、完備性與最優(yōu)性.

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