AUV避障路徑動態(tài)快速規(guī)劃算法研究

蘇祥宇,何 燕

(青島科技大學(xué)機電學(xué)院,山東 青島 266061)

0 引言

近年來,自主式水下機器人(autonomous underwater vehicle,AUV)作為水下特種工作的智能裝備,因其在海洋地質(zhì)考察、海底石油勘探、海底打撈與海底管道修復(fù)等領(lǐng)域的應(yīng)用而引起了廣泛關(guān)注[1-4]。為克服復(fù)雜環(huán)境中不確定障礙物對AUV航行產(chǎn)生的威脅,AUV需要實時進行避障路徑規(guī)劃確保航行安全。

常見的避障路徑規(guī)劃方法有人工勢場法[5-6]、模糊邏輯法[7-8]和神經(jīng)網(wǎng)絡(luò)法[9-11]等。但人工勢場法多用于靜態(tài)環(huán)境;模糊邏輯法輸入量增多時,推理規(guī)則和模糊控制規(guī)則表會急劇膨脹,導(dǎo)致避障算法運算量劇增;人工神經(jīng)網(wǎng)絡(luò)法需要大量樣本數(shù)據(jù)進行訓(xùn)練,且容易陷入局部最優(yōu)。

為降低避障算法運算量,提高AUV動態(tài)避障實時性,本文提出一種基于最小安全會遇距離的避障路徑動態(tài)快速規(guī)劃算法。

1 避障路徑動態(tài)快速規(guī)劃

船舶避障過程通?？梢苑譃檎系K物探測、碰撞風險評估和避障路徑規(guī)劃3個階段。本文假定已通過聲吶等傳感器完成對障礙物的探測,重點研究碰撞風險評估和避障路徑規(guī)劃問題。碰撞風險評估包括碰撞危險判斷與避障時機預(yù)估。避障路徑規(guī)劃是根據(jù)會遇態(tài)勢,實時規(guī)劃避障路徑。

1.1 碰撞風險評估

1.1.1 碰撞要素

確定船舶是否存在碰撞危險的最根本的因素是兩船會遇時的最小會遇距離(dCPA)和兩船到達最小會遇距離時的時間(tCPA)。dCPA、tCPA定義如圖1所示,本船位于A點,船速為VA,目標船位于B點,船速為VB,目標船對于本船的相對速度為VAB,過B點作速度VAB的平行線BC,則BC即為目標船與本船的相對運動航跡線,過A點作AC⊥BC于C點,則有:

圖1 dCPA、tCPA定義

dCPA=|AC|

(1)

tCPA=|BC|/|VB-VA|

(2)

1.1.2 碰撞危險判斷

AUV搭載有聲吶和慣性導(dǎo)航設(shè)備等傳感器,借助AUV裝載的傳感器可以獲取AUV和目標船的位置和速度信息,當AUV檢測到目標船時,計算AUV與目標船的會遇參數(shù)。若檢測到多個具有碰撞風險的目標船時,對dCPA最小的目標船進行優(yōu)先避讓。

最近會遇點如圖2所示,用PA(xA,yA,zA)表示AUV的位置,PB(xB,yB,zB)表示目標船的位置,用VA(uA,vA,wA)和VB(uB,vB,wB)分別表示AUV和目標船在大地坐標系中的速度,tCPA和dCPA計算式為:

圖2 最近會遇點

(3)

(4)

如果|VB-VA|→0,則tCPA→+∞,表示兩船不可能發(fā)生碰撞;tCPA<0,表示兩船會遇結(jié)束;因此當發(fā)生碰撞危險時,tCPA和dCPA應(yīng)滿足

tCPA>0&dCPA≤dSPA

dSPA為設(shè)定的最小安全會遇距離。

1.2 避障安全區(qū)域設(shè)置

船域是AUV周圍必須維護的安全區(qū)域。本文設(shè)計的船域由2個以AUV為中心的球體組成,由外到內(nèi)分別為常規(guī)避障區(qū)和緊急避障區(qū),緊急避障區(qū)邊界是以最小安全會遇距離為半徑的球體,常規(guī)避障區(qū)邊界是以2.5倍最小安全會遇距離為半徑的球體。

當目標船進入AUV的常規(guī)避障區(qū)時,避障路徑動態(tài)快速規(guī)劃算法實時計算tCPA和dCPA,并進行碰撞風險評估。如果存在碰撞危險,則AUV將實時規(guī)劃避障路徑并啟動避障;如果沒有碰撞危險,AUV將繼續(xù)向目標航路點航行。

1.3 避障路徑規(guī)劃

當AUV與目標船有碰撞危險時,避障路徑動態(tài)快速規(guī)劃算法首先利用AUV、目標船及相對運動航跡線構(gòu)建輔助平面,然后基于最小安全會遇距離在輔助平面中計算避障航路點,以避障航路點為下一航路點生成避障航路,AUV將按照新規(guī)劃的避障航路航行,從而避免與目標船發(fā)生碰撞。

輔助平面內(nèi)避障航路點計算如圖3所示,用Pi(xi,yi,zi)表示圖3中A,B,…,F各點坐標,為實現(xiàn)安全避障AUV航行速度由VA(uA,vA,wA)調(diào)整至VAS(uAS,vAS,wAS),則AUV與目標船的相對速度由VAB(uAB,vAB,wAB)變化至VASB(uASB,vASB,wASB),選擇避障機動后的會遇點作為AUV的避障航路點,具體計算步驟如下:

圖3 輔助平面內(nèi)避障航路點計算

AUV與目標船的相對速度VASB的具體表達式為

VASB=VB-VAS

(5)

(6)

(7)

PD=PB+(VB-VA)·tCPA

(8)

|AF|=dSPA/cosθ3

(9)

θ1=arccos(dCPA/|AB|)

(10)

θ2=arccos(dSPA/|AB|)

(11)

θ3=θ1-θ2

(12)

(13)

AUV在避障過程中航速保持不變,則有

(14)

式(13)和式(14)聯(lián)立可得VAS,則AUV采取避障機動后的會遇時間tSPA為

(15)

避障航路點的坐標為

W=PA+VAS·tSPA

(16)

通過調(diào)整dSPA可實現(xiàn)對避障航路點W的調(diào)整,從而生成不同的避障航路。

避障路徑規(guī)劃流程如圖4所示。

圖4 動態(tài)避障流程

2 AUV制導(dǎo)與控制系統(tǒng)設(shè)計

2.1 AUV運動模型

AUV運動學(xué)模型為

(17)

x、y、z為AUV在大地坐標系中的位置;θ為縱傾角;ψ為航向角;u為縱蕩速度;v為橫蕩速度;w為垂蕩速度;q為縱搖角速度;r為艏搖角速度。

AUV的動力學(xué)模型為

(18)

m(·)為系統(tǒng)慣性系數(shù);d(·)為流體動力阻尼力;τu、τq和τr分別為螺旋槳和方向舵在縱蕩、縱搖和艏搖時產(chǎn)生的控制輸入。

2.2 AUV空間機動制導(dǎo)器設(shè)計

為了獲得空間制導(dǎo)算法,對二維路徑制導(dǎo)算法進行了擴展。通過2次坐標旋轉(zhuǎn),將慣性坐標系轉(zhuǎn)換為平行于軌跡軸的坐標系。新坐標系給出了AUV與規(guī)劃路徑的位置偏差。

AUV空間機動制導(dǎo)如圖5所示,繞慣性坐標系z軸旋轉(zhuǎn)α角度完成第1次旋轉(zhuǎn),繞第1次旋轉(zhuǎn)后的坐標系y軸旋轉(zhuǎn)β角度完成第2次旋轉(zhuǎn),α和β計算式為:

圖5 AUV空間機動制導(dǎo)

(19)

(20)

Wk(xk,yk,zk)為上一航路點坐標;Wk+1(xk+1,yk+1,zk+1)為下一航路點坐標。

定義旋轉(zhuǎn)后坐標系中位置偏差向量為e=[ex,ey,ez]T,其中,ex為縱向位置偏差,ey為橫向位置偏差,ez為垂直位置偏差,Wi(xi,yi,wi)為航路點坐標,偏差向量計算式為

e=Ry(β)Rz(α)(PA-Wi)T

(21)

其中,Rz(α)和Ry(β)為旋轉(zhuǎn)矩陣,即:

(22)

(23)

前視路徑制導(dǎo)算法通過引入虛擬目標,實時計算AUV與虛擬目標的位置偏差,并根據(jù)偏差量計算AUV指令位置以保證AUV沿期望軌跡航行。在該算法中,根據(jù)式(21)獲得偏差向量e,并應(yīng)用其分量ey和ez將AUV引導(dǎo)至虛擬目標。

基于前視路徑制導(dǎo)算法可得三維空間下指令縱傾θd和指令航向ψd計算式為:

θd=β+arctan(ez/Δz)

(24)

ψd=α+arctan(-ey/Δy)

(25)

Δy和Δz分別為新坐標系下的前視距離。通過比例增益kθ和kψ獲得期望角速度為:

qd=-kθ(θ-θd)

(26)

rd=-kψ(ψ-ψd)

(27)

2.3 AUV運動控制器設(shè)計

以某型AUV為研究對象設(shè)計控制器,分別控制水平面和垂直面運動。定義縱蕩線速度誤差z1、垂直面縱搖角速度誤差z2和水平面航向角速度誤差z3為

(28)

ud為期望縱蕩線速度;qd為期望縱搖角速度;rd為期望艏搖角速度。

由式(28)可得

(29)

式(18)與式(29)聯(lián)立可得AUV速度跟蹤誤差動力學(xué)為

(30)

為使速度跟蹤誤差收斂,設(shè)計AUV空間機動控制律為

(31)

其中,設(shè)計參數(shù)ku>0、kq>0和kr>0。

3 仿真分析

為了更好地說明避障路徑動態(tài)快速規(guī)劃算法的有效性,基于MATLAB/Simulink仿真平臺構(gòu)建仿真環(huán)境。AUV運動模型參數(shù)為:m11=25 kg,m22=17.5 kg,m33=30 kg,m55=22.5 kg·m2,m66=15 kg·m2,d11=30 kg·s-1,d22=30 kg·s-1,d33=30 kg·s-1,d55=20 kg·m2·s-1,d66=20 kg·m2·s-1,ρgGML=5。AUV運動控制器參數(shù)設(shè)置為:ku=1,kq=1,kr=1,kθ=1,kψ=1。設(shè)置AUV最小安全會遇距離為20 m,仿真過程中設(shè)置交叉、對遇和追越3種會遇態(tài)勢,對AUV復(fù)雜會遇場景下的避障路徑動態(tài)規(guī)劃和AUV避障效果分別進行仿真。

復(fù)雜會遇局面下AUV與目標船的運動參數(shù)如表1所示,不同仿真時間點各船的位置關(guān)系如圖6～圖15所示,AUV在設(shè)置的交叉、對遇和追越3種會遇態(tài)勢下均能實現(xiàn)安全避障。

表1 復(fù)雜會遇局面仿真參數(shù)

圖6 AUV與目標船B交叉會遇

圖7 AUV對目標船B執(zhí)行避障操作

圖8 AUV對目標船B完成避障操作并返航

圖9 AUV與目標船C對遇

圖10 AUV對目標船C執(zhí)行避障操作

圖11 AUV對目標船C完成避障操作并返航

圖12 AUV即將追越目標船D

圖13 AUV對目標船D執(zhí)行避障操作

圖14 AUV對目標船D完成避障操作并返航

圖15 AUV到達目標航路點

復(fù)雜會遇局面下避碰過程中dCPA變化曲線如圖16所示,AUV在T=38.6 s時開始對交叉會遇的目標船B執(zhí)行避障操作,避障機動后dCPA由14.1 m逐漸增大,在T=63.6 s時到達避障航路點,dCPA達到設(shè)定的最小安全會遇距離;在T=88.6 s時開始對對遇的目標船C執(zhí)行避障操作,避障機動后dCPA由3.2 m逐漸增大,在T=105.3 s時到達避障航路點,dCPA達到設(shè)定的最小安全會遇距離;在T=129.4 s時開始對要追越的目標船D執(zhí)行避障操作,避障機動后dCPA由4.4 m逐漸增大,在T=157.1 s時到達避障航路點,dCPA達到設(shè)定的最小安全會遇距離。仿真結(jié)果顯示,AUV遇到障礙物時通過調(diào)整dCPA至設(shè)定的最小安全會遇距離,從而保證以較短的避障航路完成規(guī)避動作。

圖16 復(fù)雜會遇局面下避碰過程中dCPA變化曲線

從圖6～圖16可以看出,當目標船進入AUV常規(guī)避障區(qū)時,AUV對碰撞風險進行評估,結(jié)果顯示AUV與目標船均存在不同類型的碰撞風險,避障路徑動態(tài)快速規(guī)劃算法根據(jù)預(yù)先設(shè)定的dSPA給出了相應(yīng)的避障航路點,仿真結(jié)果顯示即使在對遇、追越和交叉3種會遇同時存在的情況下算法依然能快速進行避障路徑計算,確保了復(fù)雜會遇態(tài)勢下的航行安全。

4 結(jié)束語

為提高復(fù)雜會遇態(tài)勢下AUV避障路徑動態(tài)規(guī)劃的實時性,本文提出一種基于最小安全會遇距離的避障路徑動態(tài)快速規(guī)劃算法。根據(jù)AUV和目標船的會遇態(tài)勢,利用會遇距離實時進行碰撞風險評估;設(shè)置避障安全區(qū)域,預(yù)估避障時機;構(gòu)建輔助平面求解避障航路點,通過執(zhí)行避障路徑以較小角度和較短路徑實現(xiàn)對目標船的有效避障。為驗證該算法的有效性,設(shè)計了AUV空間機動制導(dǎo)器和運動控制器,并基于MATLAB/Simulink仿真平臺對不同會遇態(tài)勢下AUV的避障能力進行仿真測試。仿真結(jié)果表明,本文算法具有較高的實時性,能夠有效應(yīng)對AUV所面臨的各種復(fù)雜會遇局面。