船舶自主避碰的慎思型軌跡規劃

楊榮武, 許勁松, 王 鑫

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

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

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

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

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

1 DTP算法

1.1 DTP算法的架構

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

1.2 位形空間搜索指引器

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

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

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

1.3 可行軌跡生成器

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

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

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

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

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

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

1.4 全局最優軌跡搜索器

為了實現全局最優軌跡的搜索，首先構建用以度量軌跡質量的總代價函數C(x),

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

(1)

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

(2)

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

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

2 限制水域自主避碰試驗

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

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

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

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

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

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

圖5 案例三體船實物圖Fig.5 Autonomous trimaran

2.2 DTP算法的參數設置

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

(3)

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

(4)

(5)

(6)

i=l,r

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

2.3 自主避碰試驗結果

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

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

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

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

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

3 限制水域自主避碰仿真

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

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

3.2 自主避碰仿真結果

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

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

圖9 常規RRT算法的規劃結果Fig.9 Trajectories of conventional RRT planning

圖10 DTP算法的規劃結果Fig.10 Trajectories of DTP Planning

4 結論

軌跡規劃是船舶自主避碰系統的重要功能之一.針對DP算法現存的缺陷，提出一種基于RRT的改進算法——DTP算法，耦合處理靜態障礙物約束、船舶操縱性約束、軌跡最優性等限制條件，在兩個長程路徑點之間完成全局軌跡規劃，以保證規劃軌跡的可行性、完備性與最優性.

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