劉淑霞， 李立剛， 金久才， 戴永壽

(1.中國石油大學(華東)，山東 青島 266000； 2.自然資源部第一海洋研究所，山東 青島 266000)

0 引言

無人船(Unmanned Surface Vhicles,USV)因無需人員操作，可以在海洋中承擔長時間、大范圍、低成本的海洋科研與工程任務，而具有廣闊的應用前景[1]。在執(zhí)行任務時，無人船不僅要跟蹤預定航線，而且要具備局部動態(tài)避障能力，同時還要考慮實時性約束、無人船運動學約束、國際海上避碰規(guī)則(COLREGS)約束和風浪流的干擾[2]。

動態(tài)窗口法(DWA)因考慮無人船運動學約束，被廣泛應用于無人船的局部動態(tài)避障[3]。王寧佳[4]提出將速度障礙法和動態(tài)窗口法相結合，通過劃定絕對避障區(qū)和在評價函數(shù)中引入目標區(qū)域影響這一指標，提高了避障的安全性;周曉周[5]提出在動態(tài)窗口法的目標函數(shù)中添加最小避碰時間、最短偏離距離、復航安全距離3個指標，用以確定最優(yōu)航向和避碰時間;SINGH等[6]在動態(tài)窗口法的目標函數(shù)中加入了考慮海況的權重系數(shù)，當海況發(fā)生變化時，通過增加距離權重和降低速度權重來保證無人船的安全。

但上述文獻都忽略了動態(tài)障礙物狀態(tài)的變化和風浪流干擾給無人船避障帶來的具體影響。因此，本文提出基于漂角估計和補償?shù)臒o人船局部動態(tài)避障方法。根據(jù)預測的障礙物運動狀態(tài)和國際海上避碰規(guī)則改進了動態(tài)窗口法，并提出利用橫向位置誤差估計并補償漂角，使該無人船局部動態(tài)避障方法更加適用于海洋擾動環(huán)境，更加具有實用性。

1 傳統(tǒng)動態(tài)窗口法及其基本原理

傳統(tǒng)動態(tài)窗口法的基本原理為：在速度空間中，采樣多組速度，并在前向周期內(nèi)模擬無人船在這些速度組合下的軌跡，按照一定評價標準選取最優(yōu)軌跡對應的速度和航向驅(qū)動無人船避障[7]。動態(tài)窗口法的基本流程如圖1所示。

圖1 傳統(tǒng)動態(tài)窗口法流程圖Fig.1 Flow chart of traditional DWA

首先，因受到加減速能力限制無人船能夠達到的速度范圍為

(1)

其次，由于受到最大、最小速度限制，無人船能夠達到的速度范圍為

Vs={v∈[vmin,vmax]∧ω∈[ωmin,ωmax]}

(2)

式中:vmin和vmax分別為最小和最大速度;ωmin和ωmax分別為最小和最大角速度。

最后，無人船也受到制動距離約束，以最大減速度停船而不能與障礙物發(fā)生碰撞。制動距離約束下,無人船能夠達到的速度范圍為

(3)

式中，d(v,ω)為無人船與障礙物的最近距離。

無人船的速度空間為

Vr=Vd∩Vs∩Va。

(4)

對上述速度空間進行采樣后，預測不同航向和航速組合下的軌跡，并按照預測軌跡的航向與目標點航向的夾角、預測軌跡對應的速度和預測軌跡與障礙物的最短距離3個指標評價出最優(yōu)軌跡，選擇最優(yōu)軌跡對應的航向和航速驅(qū)動無人船避障，評價函數(shù)為

G(v,ω)=k1·h(v,ω)+k2·d(v,ω)+k3·V(v,ω)

(5)

式中:h(v,ω)為預測軌跡的航向與目標點航向的夾角;d(v,ω)為預測軌跡與障礙物的最短距離;V(v,ω)為預測軌跡對應的速度;k1，k2和k3分別為3個指標的權重系數(shù)。

2 基于漂角估計和補償?shù)木植縿討B(tài)避障方法設計

首先，采用改進的動態(tài)窗口法規(guī)劃無人船的速度和航向;其次，利用橫向位置誤差估計并補償漂角。局部動態(tài)避障方法的流程如圖2所示。

圖2 局部動態(tài)避障方法流程圖Fig.2 Flow chart of local dynamic obstacle avoidance method

2.1 改進動態(tài)窗口法

考慮國際海上避碰規(guī)則和障礙物狀態(tài)變化對傳統(tǒng)動態(tài)窗口法進行了改進，提出根據(jù)COLREGS計算速度空間和根據(jù)預測的障礙物狀態(tài)選擇無碰預測軌跡，提高了無人船局部動態(tài)避障的規(guī)范性和安全性。

2.1.1 根據(jù)COLREGS計算速度空間

根據(jù)COLREGS無人船的會遇被劃分為追越、對遇、交叉相遇3種情況[8]，如圖3所示。但無人船只需在對遇、追越和右側交叉相遇情況下采取避碰行動[9]，如圖4所示。

圖3 會遇情況示意圖Fig.3 Diagram of the encounter situations

圖4 避碰行動示意圖Fig.4 Diagram of collision avoidance operations

因此，在上述應采取避碰行動的情況下，無人船需向其他船只右側行駛，則無人船的速度v和航向角α范圍為

Vg={v∈[vrmin,vrmax]∧α∈
[ωrmin·Δt,ωrmax·Δt]∧α∈(ψ,∞)}

(6)

式中:vrmin,vrmax，ωrmin和ωrmax分別為傳統(tǒng)動態(tài)窗口法速度空間中的最小、最大速度和最小、最大角速度;ψ為其他船只的方位角，即

(7)

式中，(xv,yv)和(xu,yu)分別為其他船只和無人船的位置。

如果下一時刻其他船只未遵守COLREGS，則按照傳統(tǒng)方法選取動態(tài)窗口法的速度空間。

綜上，上述方法有效避免了無人船過度自由地航行，進而降低了無人船與其他船只的碰撞風險。

2.1.2 根據(jù)預測的障礙物狀態(tài)選擇無碰預測軌跡

傳統(tǒng)動態(tài)窗口法根據(jù)障礙物在t時刻的橫截面寬度和位置,規(guī)劃t～t+1時刻內(nèi)無人船的速度和航向，忽略了兩者在t～t+1時刻內(nèi)的變化，可能導致避障失敗，無人船與障礙物的碰撞情況如圖5所示。

圖5 無人船與障礙物碰撞示意圖Fig.5 Diagram of collision between USV and obstacle

因此，有必要預測障礙物在t～t+1時刻的運動狀態(tài)，并作為規(guī)劃航向和航速的依據(jù)，以減少碰撞情況的發(fā)生，障礙物的橫截面寬度和位置預測如圖6所示。

圖6 預測障礙物運動狀態(tài)示意圖Fig.6 Schematic diagram of predicting state of obstacle

因為間隔時間較短，橫截面寬度的變化量不大，所以可以通過計算其變化率預測下一時刻的橫截面寬度值。

t-1～t時刻障礙物橫截面寬度的變化率為

(8)

式中，dt-1和dt分別表示t-1和t時刻障礙物的橫截面寬度。

t+1時刻橫截面寬度的預測值為

(9)

t+1時刻障礙物的影響范圍是以該時刻橫截面寬度為直徑的圓。式中:dt和dt+1分別表示t和t+1時刻障礙物的橫截面寬度。

通過預測下一時刻的位置，可以預測出障礙物在一段時間內(nèi)的運動軌跡。

假定t～t+1時刻內(nèi)障礙物的速度和航向保持不變，則t+1時刻障礙物的位置為

(10)

障礙物在t～t+1時刻內(nèi)的運動軌跡所在的直線為

(11)

根據(jù)障礙物的影響范圍和運動軌跡可計算出無人船預測軌跡到障礙物的最短距離。

(12)

式中:Q點為預測軌跡末端向障礙物運動軌跡所在線段作垂線的交點;rQ為障礙物在Q點時的影響范圍半徑，即

(13)

式中，tQ為障礙物運動到Q點(xQ,yQ)的時刻，即

(14)

(15)

式中，rt和rt+1分別為t和t+1時刻障礙物的影響范圍半徑。

根據(jù)預測軌跡末端到障礙物影響范圍的最短距離l,將速度空間中與障礙物碰撞的預測軌跡刪除，則無人船受減速距離限制能達到的速度范圍為

(16)

通過上述方法為無人船規(guī)劃出了更加安全的航向、航速進行避障。

2.2 利用橫向位置誤差估計漂角

風浪流干擾使無人船偏離實際航向，不利于避障的安全和航行的穩(wěn)定。因此，本文將風浪流干擾對無人船的影響等效為漂角[10]，提出利用橫向位置誤差對其進行估計和補償。風浪流干擾對無人船的影響如圖7所示。

圖7 風浪流干擾對無人船的影響原理圖Fig.7 Diagram of impact of wind,wave and current on USV

(17)

式中:αt為t時刻無人船的期望航向角；v和ω分別為無人船的縱向速度和角速度；α為無人船在t～t+1時刻內(nèi)的期望航向角。

在大地坐標系中，假設無人船當前實際位置為(xu,yu)，由圖7的幾何關系可得橫向位置誤差ye為

(18)

將式(18)對時間t求導，得

(19)

(20)

無人船三自由度運動學模型[11]為

(21)

式中:x，y和φ分別為在大地坐標系下的橫、縱坐標和航向角；μ為在船體坐標系下的橫向速度。

(22)

根據(jù)三角函數(shù)轉(zhuǎn)換式

(23)

以及μ,v,β和U之間的幾何關系，可將式(22)重寫為

(24)

由三角函數(shù)轉(zhuǎn)換式

sin(a+b)=sina·cosb+cosa·sinb

(25)

以及μ,v,β和U之間的幾何關系，將式(24)重寫為

(26)

當無人船控制系統(tǒng)能夠準確跟蹤期望航向時，期望航向角和實際航向角保持一致，即φ=α，則可由式(26)得漂角的大小為

(27)

當無人船的實際航向角與期望航向角存在誤差時，即φ≠α，由式(26)得漂角的大小為

(28)

通過無人船的橫向誤差，已經(jīng)估計出了無人船由于風浪流干擾產(chǎn)生的漂角。利用估計出的漂角可實現(xiàn)對動態(tài)窗口法規(guī)劃出的期望航向角α的補償，補償后無人船航向角為

φ′=α+β

。

(29)

綜上，本節(jié)所提方法能夠使無人船抵抗風浪流干擾且穩(wěn)定地航行。

3 仿真驗證及結果分析

為了驗證所提方法的有效性和優(yōu)越性，利用Matlab進行仿真實驗。根據(jù)路徑跟蹤控制實驗船實驗數(shù)據(jù)，計算得無人船的運動參數(shù)設置如表1所示。

表1 無人船運動參數(shù)

3.1 改進動態(tài)窗口法局部動態(tài)避障仿真實驗

為了驗證改進動態(tài)窗口法的有效性和優(yōu)越性，分別采用傳統(tǒng)動態(tài)窗口法和改進動態(tài)窗口法進行無人船局部動態(tài)避障仿真實驗。無人船與障礙物初始參數(shù)設置如表2所示。

表2 無人船與障礙物初始參數(shù)

評價函數(shù)中的參數(shù)設置為：k1=0.2，k2=0.2，k3=0.1?；谏鲜龇抡鏃l件，實驗結果如圖8和圖9所示。

對比圖8(a)～8(c)可以看出，本文所提方法能夠使無人船在右交叉相遇、對遇、追越3種情況下向右行駛，提高了無人船局部動態(tài)避障的規(guī)范性。

圖8 增加COLREGS約束前后的無人船避障軌跡Fig.8 Obstacle avoidance trajectory of USV before and after adding the restrictions of COLREGS

圖9(a)～9(c)為預測障礙物運動狀態(tài)前后無人船分別在20 s，40 s和70 s的避障軌跡，可以看出，無人船與障礙物的安全距離明顯增加。由圖9(d)可以看出，改進后無人船與障礙物始終保持安全距離。

圖9 預測障礙物運動狀態(tài)前后無人船避障軌跡和無人船與障礙物的距離Fig.9 Obstacle avoidance trajectory of USV before and after predicting the obstacle movement state,and the distance between USV and obstacle

3.2 進行漂角估計和補償?shù)木植縿討B(tài)避障仿真實驗

為驗證漂角估計和補償對于抵抗風浪流干擾效果，進行海洋環(huán)境擾動下的無人船局部動態(tài)避障仿真實驗。為模擬海洋環(huán)境，設置隨機漂角，如圖10所示。

圖10 漂角的大小Fig.10 Size of drift angle

圖11所示為漂角估計和補償對于無人船局部動態(tài)避障穩(wěn)定性的有效性驗證結果。圖11(a)～11(c)為有風浪流和無風浪流干擾下無人船分別在20 s，40 s和70 s的避障軌跡?？梢钥闯?，受到風浪流干擾時，無人船的實際航向與規(guī)劃的航向相比，發(fā)生了較大偏移。

圖11 風浪流干擾下無人船的避障軌跡Fig.11 Obstacle avoidance trajectory under wind,wave and current disturbances

圖12為進行漂角補償后無人船分別在20 s,40 s和70 s的避障軌跡。可以發(fā)現(xiàn)，漂角補償后，無人船的航向不再發(fā)生偏移，無人船能夠抵抗風浪流干擾穩(wěn)定地航行。

圖12 漂角補償后無人船的避障軌跡Fig.12 Obstacle avoidance trajectory after drift angle compensation

圖13為漂角的估計值和實際值的對比?？梢钥闯?，本文提出的方法可對漂角進行有效的估計。圖14對比了漂角補償前后無人船與障礙物影響范圍的距離?？梢园l(fā)現(xiàn)，漂角補償后無人船與障礙物影響范圍的距離均大于1.679 m，保證了無人船的航行安全。

圖13 漂角的估計值和實際值Fig.13 Estimated and actual drift angle

圖14 漂角補償前后無人船與障礙物的距離Fig.14 Distance between USV and obstacle before and after drift angle compensation

4 結束語

本文對無人船局部動態(tài)避障方法進行了深入研究，提出了基于改進動態(tài)窗口法結合漂角估計的方法?？紤]COLREGS選取速度空間，并根據(jù)預測障礙物的運動軌跡和影響范圍選擇無人船的無碰預測軌跡，保證了無人船避障行動的規(guī)范性，進一步提高了安全性。利用無人船實際位置與期望避障路徑的橫向誤差估計無人船由于風浪流干擾產(chǎn)生的漂角，并在規(guī)劃的航向中進行了補償，提高了無人船局部動態(tài)避障的穩(wěn)定性。通過仿真實驗，驗證了本文所提方法的有效性和優(yōu)越性。