基于速度障礙的多船自動避碰控制方法

熊 勇, 賀益雄， 黃立文(1. 武漢理工大學 航運學院， 武漢 430063； 2. 內河航運技術湖北省重點實驗室， 武漢 430063)

基于速度障礙的多船自動避碰控制方法

熊 勇1, 2, 賀益雄1, 2， 黃立文1, 2
(1. 武漢理工大學 航運學院， 武漢 430063； 2. 內河航運技術湖北省重點實驗室， 武漢 430063)

傳統避碰方法側重于根據船舶實時位置設計可行的避碰路徑，較少考慮結合船舶動力學和船舶運動控制來設計一個完整的自動避碰系統。對此，提出一種實用、基于相對速度方向的自動避碰方法。該方法同時考慮避碰路徑規劃和本船運動控制，主要包括3個部分：首先，基于運動方程計算本船和目標船的基本運動與位置參數；其次，利用速度障礙方法計算本船實時的可行避碰方向；最后，實現控制變量的變換和控制算法設計。該方法是一個靈活、容易實現的多船避碰控制算法，模擬結果顯示其具有較好的效果，并給出了進一步研究的建議。

水路運輸； 自動避碰； 船舶運動數學模型； 速度障礙； PID控制

當在水面上航行的船舶不斷增多時，船舶碰撞的危險性就會逐漸增加。相關學者[1-4]從避碰計算和避碰方法設計的角度對船舶避碰問題進行了詳細研究， 主要基于船舶的位置來設計可行的避碰方案，結果主要集中在船舶避碰路徑規劃和碰撞危險度評估上。這些研究隱含的假定條件是本船和目標船在碰撞發生前后一個較短的時間段內均為勻速直線運動，以此為基礎來設計基于位置的避碰方案，較少考慮船舶的動力學特性。該類方法傳統上稱為“幾何避碰方法”，是航海避碰實踐的總結和提升，具有較好的實用性，主要依據船舶的位置信息和幾何計算來做出避碰決策。在最近的10 a中，一些現代方法被逐漸引入到求解該問題的框架中。例如：遺傳算法被用于搜尋最優避碰路徑[5-7]，模糊專家系統被用于構建船舶碰撞的風險評估模型[8-10]?？偟膩碚f，現有方法主要用于避碰路徑規劃，最終避碰是否成功還依賴于船長的操船經驗和水平，其主要不足在于較少考慮在一個動態、持續變化、不確定的來船環境中，船舶如何自動、持續地完成避碰并恢復到原來的航向上。 此外，現有方法較少考慮船舶動力學，即使計算出了可行的避碰路徑，也無法確認船舶是否能被控制并按設定的避碰路徑行駛，因此對于自動導航而言，僅是完成了一部分功能，缺乏將避碰和船舶運動控制聯接起來的完整的系統設計?；谏鲜隹紤]，從船舶自動航行的角度出發，設計一種在船舶運動方程的基礎上，通過計算和控制速度而不是位置，能連續控制船舶避開障礙物并保持航向的算法。

速度障礙的概念和方法是由FIORINI等[11]首先提出的，主要指目標物和障礙物在保持當前運動狀態的情況下，所有可能導致目標物與障礙物發生碰撞的速度集合。這里從速度障礙的角度考慮，提出一種實用的多船避碰控制方法，其核心思想如下。

1) 根據實時的本船速度和所有目標船速度計算本船與所有目標船的相對速度。

2) 計算可行的本船相對速度方向，使得本船的相對速度方向不指向任何目標船。

3) 根據本船的動力學方程，控制本船按上述航向行駛。

上述過程是不斷進行的，具體計算的采樣時間可根據船速來適當設定，因此該采用方法可使本船避開移動或固定的障礙物。根據這些描述，該過程連續進行，直到本船恢復到計劃航線。若所有的參數已被調節好，則整個過程無需人為干預，可自動進行，計算機模擬的結果顯示該方法是有效的。

1 問題描述

所描述的船舶避碰問題見圖1，在t時刻， 本船O以速度Vo按照計劃航線OB運動, 目標船P1,P2,…，Pi隨機出現在水面上, 分別以速度VP1,VP2,…，VPi在水面做勻速直線運動。針對該情境，設計一種自動避碰方法，滿足以下要求。

圖1 船舶避碰問題

1) 自動分析來船和本船碰撞的可能性及風險，決定本船是否需要采取避碰措施。

2) 計算本船O和最危險目標船Pi可行的相對避碰方向，并由此反向推算在保持本船速度不變的情況下，本船可行的避碰速度方向。

3) 控制本船按可行的避碰速度方向行駛，并在完成避碰過程后使本船O恢復到計劃航向上。

4) 上述算法要隨時間連續不斷地進行，直到航行結束，且來船數目不影響算法的一般性。

該方法集中解決船舶避碰的主要問題，暫不考慮外部環境擾動力(如風、浪、流力)和國際避碰規則。問題的重點是在探索靜水中自動避開多個運動來船。在此基礎上，稍微改變約束條件，可以解決滿足上述更一般的約束和外部條件的問題。

2 船舶運動數學模型

用船舶運動數學模型中最常用的MMG(Manoeuvring Model Group)模型來計算本船的實時速度和位移。以下詳細描述一個三自由度，用于描述水面運動船舶縱蕩、橫蕩、艏搖運動的MMG模型。為方便計算，該模型采用一撇系統來實現變量的無量綱化，并將方程放在地球固定坐標系中統一處理。

(1)

螺旋槳的推進力XP的計算式為

(2)

式(2)中：ctp和tP0為相關的常數，由試驗測定；n為螺旋槳的轉速；DP為螺旋槳的半徑。

舵力XR,YR和舵力矩NR的計算式為

(3)

(4)

(5)

作用在船體上的外力XH,YH和外力矩NH用 Inoue 模型，然后根據式(6)將船體坐標系轉換為大地坐標系。

(6)

聯合式(1)和式(6)即可求出本船任意時刻的速度和位置，所有的目標船都按勻速直線運動。

考慮到簡單而不失一般性，目標船作為可能的碰撞目標都假定為圓形，并滿足式(7)。

(xc-xf)2+(yc-yf)2=R2

(7)

3 基于速度障礙的本船避碰方向計算

船舶自動導航的核心問題避碰也是一個目標航向不斷變化的航向跟蹤問題，而可行航向由不同的船舶會遇形勢來決定。這里通過引入速度障礙的概念，提出一種自然、簡單的避碰方法，將動態障礙物避碰問題轉化為靜態障礙物避碰問題，并對其求解。

圖2 速度障礙

由圖2可知，該方法的基本思想為:A和B分別為本船和目標船；VA(t)和VB(t) 分別為t時刻兩船的速度向量；VAB(t)為t時刻A相對于B的速度。若B是靜態目標, 則VB(t)=0, 顯然，若VA(t)的方向落入∠B1AB2的范圍內，則A將會與B發生碰撞；否則，若B是移動目標, 則VB(t)≠0, 若VAB(t)的方向落入∠B1AB2區域，則A將不可避免地與B發生碰撞。下面首先給出速度障礙的定義，然后根據不同的會遇形式設計避碰算法。

定義1A相對于B的速度障礙區CAB(t)為

CAB(t)={VAB(t)|LAB∩B≠φ}

(8)

式(8)中：CAB(t) 為圖2中的三角形區域B1AB2。

定義2速度障礙VO(t)為

VO(t)=CAB(t)?VB(t)

(9)

式(9)中：? 為閔科夫斯基向量加和。

若本船與n個不同的目標船會遇，且每個目標船均是勻速直線運動，則第i個目標船應滿足

(10)

式(10)中：xi0和yi0為第i個目標船的初始位置；ui和vi為第i個目標船沿x軸和y軸的初始速度。由此知,VOi(t) 為本船相對于第i個目標船的速度障礙， 總的速度障礙VO(t) 為本船相對于n個不同的目標船的速度障礙VOi(t)的并集。

(11)

若本船的速度不會導致碰撞，即VA(t)?VO(t)，則

(12)

式 (12) 為多船避碰的充分條件，但其只是一個形式化表達，如何在具體的會遇形勢下計算VOi(t)是問題的關鍵。圖3中假設O為本船，Pi為目標船，則基于速度障礙的船舶避碰方法可有以下表述。

圖3 多船避碰

1) 討論圓與直線相交的條件，若直線y=ax+b與圓 (x-c)2+(y-d)2=r2聯立有解，則二者相交，設判別式函數

delta(a,b,c,d,r)=a2(r2-c2)+(d-b)(2ac-

d+b)+r2

(13)

式(13)中：若delta≥0，則表明圓與直線相交。

2) 根據標準判斷某目標是位于本船的前方還是后方，“前”或“后”是一個與本船隨時間運動的方向有關的量。假定本船在t-1時刻位于A(a1,b1),t時刻位于B(a2,b2)，判斷點C(a3,b3)在t時刻是位于本船前方還是后方的依據如下。

作一條過A點垂直于AB的直線，其方程為

(14)

若B點和C點均位于該直線的同側，則C點在目標的前方，否則在后方，前提是采樣間隔時間很短，船舶在此時間段內可能運動的距離不會超過目前到此直線的距離。定義函數

(15)

式(15)中：若fd>0,則位于本船前方;否則fd<0,位于本船后方。

在上述2個判斷函數的基礎上，采用相對運動速度障礙設計自動避碰算法如下。

1) 根據式(1)和式(6)，采用龍格-庫塔方法計算本船O在t時刻的位置坐標(x(t),y(t)) ，根據其勻速直線運動方程計算第i個目標船Pi的位置坐標(xi(t),yi(t)) 。

2) 根據上述位置坐標，計算∠P1OC,∠P2OC,…,∠PnOC。

∠PiOC=

(16)

式(16)中：i=1,2，…，n；Ri為第i個目標障礙物的半徑。

3) 按升序排列∠PiOC，即 ∠Pi1OC<∠Pi2OC<…<∠PinOC，并因此重新排序下標為i1,…,ij,…,in，j=1,2,…,n。

5) 采用式(13)判斷相對速度線OPij是否與目標障礙物Pij所在的圓相交，若相交，則繼續由式(15)來確定目標是否位于本船前方。若既相交又位于前方，則可確定本船與目標船按目前狀態航行會相撞，否則返回式(4) ，j=j+1。

6) 若本船O與目標船Pi1可能碰撞(如圖3)，則考慮與其最相近的左右兩艘船，將會有以下4種會遇態勢。

A1. 左側速度障礙區重疊：∠Ai1OC>∠Bi1+1OC且∠Bi1OC>∠Ai1-1OC。

A2. 右側速度障礙區重疊：∠Ai1OC<∠Bi1+1OC且∠Bi1OC<∠Ai1-1OC。

A3. 兩側速度障礙區重疊： ∠Ai1OC>∠Bi1+1OC且∠Bi1OC<∠Ai1-1OC。

A4. 兩側都可以通過：∠Ai1OC<∠Bi1+1OC且∠Bi1OC>∠Ai1-1OC。

具體判斷方法為：以A1為例，根據式(14)判斷直線OAij是否與障礙物圓Pij+1相交，以此確定是否左側重疊。否則j=j+1, 直至j=n。

7) 計算避開速度障礙區本船的速度方向角，方法如下。

首先根據不同的會遇態勢確定能避開速度障礙區的最小變化的相對速度角∠VOPi1，用θd表示；然后根據相對速度角方程來求解可避開障礙區的本船速度角βd。

(17)

(18)

根據4種不同的會遇態勢，若要以最少的轉向實現避碰，則可設定相對速度角為：A1,θd=∠Bi1OC; A2,θd=∠Ai1OC; A3，速度角選擇為最接近當前航向角的那一側，具體為

A4, 若∠Bi1OPi1<∠Ai1OPi1， 則θd=∠Ai1OC，否則θd=∠Bi1OC。

可由固定速度值V不變求合適的速度角βd，也可由固定速度角βd求合適的速度值V。由于速度角更易控制，這里選擇前者，則解式(17), 設

Δ=[V2(1+tan2θd)+2uivitanθd-

(19)

若Δ>0, 則本船的速度角βd有2個解

(20)

則可取速度角為

(21)

4 航向控制和恢復

為最終實現避碰，還需控制本船的速度方向。當船轉向不大時，漂角較小，速度方向可近似等同于航向。若要直接控制速度方向，則將航向ψ換成速度方向即可。有許多方法可實現船舶航向控制，例如PID 控制、預測控制、自適應控制及神經網絡控制等等?？紤]到一般性和實用性，這里采用最常用的PID控制來求解該問題。首先考慮舵機的動力學和舵角的限制問題，一般來說，舵機滿足

(22)

式(22)中：δ(0)=δ0；δe為指令舵角；δ為實時舵角；Te為時間常數；Ke為舵機增益常數。通常，舵角的實際值限制在 -δmax≤δ≤δmax，這里

δmax=35°

(23)

為滿足上述條件，同時保持舵角的連續、光滑、可導，可用一個雙曲正切函數對指令舵角做以下變換

(24)

則用變量ξ?(-∞,+∞)替換了命令舵角變量δe?(-δmax,δmax), 然后用增量式PID控制算法使本船保持在航向βd上。

當本船O按上述算法駛過與目標船Pi的最近會遇點之后，開始恢復到計劃航線上，因此需不斷計算最近會遇距離，當兩船間距離最小時，設定航向變為計劃航向并送到該控制模塊。

5 仿真模擬及結果討論

利用經典的Mariner標準船做模擬，其主要船型的參數為：船長171.8 m，型寬23.17 m，設計型深8.23 m，設計排水量18 541 t，設計速度15 kn。其他參數詳見文獻[12]。

本船在大地坐標系下的初始位置為(0,0)，初始速度為15.44 kn。在以下模式試驗中，每個目標船被設定為半徑為200 m的圓，目標船的初始位置和速度是隨機的，依賴于不同的船舶會遇態勢，詳見表1～表2。

表1 會遇態勢 A1

表2 會遇形勢A2

本船的PID控制參數設定為：KP=6,KI=400,KD=6。為簡化，僅顯示A1和A2兩種會遇態勢下的模擬結果(見圖4和圖5)。

通過觀察上述仿真結果可知：本船可在上述模擬的會遇態勢下實現與目標船的避碰，但是實際避碰效果與以下3個因素有關。

圖4 A1下的模擬結果

圖5 A2下的模擬結果

1) 本船與目標船間的距離(越長則本船越安全)。若距離太短，則本船與目標船發生碰撞的風險會大大增加。在實際航行中，本船的避碰操作執行越早越安全，但執行得太頻繁或太早會浪費燃油，因此設定一個合適的避碰起始距離很重要，通常根據經驗把該距離設定為5 n mile，即小于此距離才開始啟動本系統。

2) 理論上該方法可根據會遇船舶的實時位置來判斷當前的會遇態勢，并隨著時間連續地進行判斷，由此持續計算可行的相對速度方向，然后求解相對速度方程得到本船當前可行的避碰速度，并將其送到PID控制模塊。但是在實際情況中，由于各種外部干擾的存在，來船和本船實際位置的判斷會出現偏差，因此也會出現判斷失誤的情況。

3) 本船PID航向控制的效果。 要實現船舶的避碰，不僅要計算出可行的避碰路徑，而且要控制船舶按該路徑運動。因此，若本船航向控制的效果很差，則即使算出了可行的避碰方向，避碰效果也不會好。本船的航向控制可用任意一種方法，不一定是PID控制，但是無論用哪種方法，都要仔細調節控制參數以達到滿意的控制效果。

前述算法的描述都是基于船舶運動模型及所有參數已知的前提下得到的。因此，若要設計實用的自動避碰系統，就必須事先測得船舶所有水動力參數并將其輸入系統，或安裝一套實時的船舶水動力參數估計系統，并將其與自動避碰系統聯合起來，只有如此，才能真正實現“自動航行”。

[1] 鄭中義, 吳兆麟. 轉向避讓的最后機會模型及其應用[J].大連海事大學學報,2001,27(2):6-10.

[2] 鄭中義, 吳兆麟. 船舶碰撞危險度的新模型[J].大連海事大學學報,2002,28(2):1-5.

[3] 李麗娜, 王俊玲, 陳國權. 船舶擬人智能避碰決策理論的集成機器學習策略[J].信息與控制,2011,40(3):359-368.

[4] 李麗娜, 熊振南, 高巖松, 等. 單船避碰智能決策的生成與優化方法[J].中國航海,2002(2):49-52.

[5] SMIERZCHALSKI R, MICHALEWICZ Z. Modeling of Ship Trajectory in Collision Situations by an Evolutionary Algorithm[J]. Evolutionary Computation, IEEE Transactions on, 2000, 4(3): 227-241.

[6] TSOU M C, KAO S L, SU C M. Decision Support from Genetic Algorithms for Ship Collision Avoidance Route Planning and Alerts[J]. Journal of Navigation, 2010, 63(1): 167-182.

[7] ITO M, ZHNNG F, YOSHIDA N. Collision Avoidance Control of Ship with Genetic Algorithm[C]//Control Applications, 1999. Proceedings of the 1999 IEEE International Conference on. IEEE, 1999.

[8] LEE H J, RHEE K P. Development of Collision Avoidance System by Using Expert System and Search Algorithm[J]. International Shipbuilding Progress, 2001, 48(3): 197-212.

[9] LIU Y H, YANG W L. The Structure Design of an Intelligent Decision Support System for Navigation Collision Avoidance[C]//Machine Learning and Cybernetics, 2004. Proceedings of 2004 International Conference on. IEEE, 2004.

[10] IIJIMA Y, HAGIWARA H, KASAI H. Results of Collision Avoidance Manoeuvre Experiments Using a Knowledge-Based Autonomous Piloting System[J]. Journal of Navigation, 1991, 44(02): 194-204.

[11] FIORINI P, SHILLER Z. Motion Planning in Dynamic Environments Using Velocity Obstacles[J]. The International Journal of Robotics Research, 1998, 17(7): 760-772.

[12] FOSSEN T I. Guidance and Control of Ocean Vehicles[M]. New York: Wiley, 1994：431-432.

Multi-ShipCollisionAvoidingControlConsideringVelocityObstacle

XIONGYong1,2,HEYixiong1,2,HUANGLiwen1,2
(1. School of Navigation，Wuhan University of Technology，Wuhan 430063，China;2. Hubei Inland Shipping Technology Key Laboratory，Wuhan 430063，China)

Ordinary collision avoidance calculation is not fit for automatic control operation because it designs feasible avoiding routes according to the relative positions and speeds of targets without considering the manoeuvreability of own ship. A practical multi-ship collision avoidance control strategy is proposed which plans collision avoiding route according to the relative velocity direction and own ship motion control simultaneously. This method contains three main steps: 1. calculating basic motion and position parameters of own ship and target ships; 2. calculating real time feasible velocity direction of own ship based on the velocity obstacle; 3. proceeding to do control variable transformation and control algorithm design. This method allows real time multi-ship collision avoidance. Simulating experiments show promising results and is flexible and easy to implement. Some suggestions are given for further study.

waterway transportation; automatic collision avoidance； ship motion mathematic model； velocity obstacle； PID control

2015-04-19

國家自然科學基金(51379170)；中央高?；究蒲袠I務費專項資金(133212003)；湖北省自然科學基金(2014CFB878)

熊 勇(1976—)，男，湖北黃岡人，副教授，博士，從事艦船運動控制研究。E-mail:bear_brave@163.com

1000-4653(2015)03-0046-06

U675.96

A