基于漂角估計的無人船局部動態(tài)避障方法

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

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

0 引言

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

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

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

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

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

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

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

(1)

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

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

(2)

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

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

(3)

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

無人船的速度空間為

Vr=Vd∩Vs∩Va。

(4)

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

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

(5)

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

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

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

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

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

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

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

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

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

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

因此，在上述應(yīng)采取避碰行動的情況下，無人船需向其他船只右側(cè)行駛，則無人船的速度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)窗口法的速度空間。

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

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

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

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

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

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

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

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

(8)

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

t+1時刻橫截面寬度的預(yù)測值為

(9)

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

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

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

(10)

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

(11)

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

(12)

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

(13)

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

(14)

(15)

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

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

(16)

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

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

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

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

(17)

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

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

(18)

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

(19)

(20)

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

(21)

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

(22)

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

(23)

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

(24)

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

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

(25)

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

(26)

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

(27)

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

(28)

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

φ′=α+β

。

(29)

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

3 仿真驗證及結(jié)果分析

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

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

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

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

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

評價函數(shù)中的參數(shù)設(shè)置為：k1=0.2，k2=0.2，k3=0.1。基于上述仿真條件，實驗結(jié)果如圖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)為預(yù)測障礙物運動狀態(tài)前后無人船分別在20 s，40 s和70 s的避障軌跡，可以看出，無人船與障礙物的安全距離明顯增加。由圖9(d)可以看出，改進(jìn)后無人船與障礙物始終保持安全距離。

圖9 預(yù)測障礙物運動狀態(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 進(jìn)行漂角估計和補償?shù)木植縿討B(tài)避障仿真實驗

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

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

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

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

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

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

圖13為漂角的估計值和實際值的對比?？梢钥闯觯疚奶岢龅姆椒蓪ζ沁M(jìn)行有效的估計。圖14對比了漂角補償前后無人船與障礙物影響范圍的距離。可以發(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 結(jié)束語

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