遵循COLREGs的USV多階段自適應(yīng)ph-Radau偽譜避障軌跡規(guī)劃

戴懷志 蒲紅紅

障礙物規(guī)避策略對無人水面航行器(Unmanned Surface Vehicle,USV)順利完成自主航行與作業(yè)十分重要,而為實現(xiàn)有效且協(xié)調(diào)的避障,將USV作為洋流中航行船舶的一種,遵循國際海上避碰規(guī)則公約(International Regulations for Preventing Collisions at Sea,COLREGs)是必要的[1].

針對USV避障問題,多數(shù)學(xué)者均采用遵循COLREGs的速度避障法[2?4].文獻[2]針對未知動態(tài)環(huán)境,線性化處理相對速度,并將其作為約束條件整合到混合整數(shù)線性規(guī)劃器中,選擇多目標(biāo)函數(shù)作為優(yōu)化函數(shù),提出一種遵循COLREGs的在線路徑規(guī)劃算法.文獻[3]則利用動態(tài)障礙物與USV的相對運動狀態(tài),提出一種遵循COLREGs的粒子群優(yōu)化(Particle Swarm Optimization,PSO)障礙物規(guī)避策略,實時解算規(guī)避障礙物的最優(yōu)航行和航速改變量.而文獻[4]在遵循COLREGs的前提下,靜態(tài)和動態(tài)障礙物的規(guī)避問題,提出基于PSO的動、靜態(tài)障礙物規(guī)避分層路徑規(guī)劃方法.

然而,上述文獻中USV避障策略無法從USV運動模型出發(fā)且未充分考慮航行機動性能限制等約束條件,難以保證所規(guī)劃路徑的可行性,本質(zhì)上歸屬于路徑規(guī)劃范疇.而基于偽譜法的軌跡優(yōu)化策略是一類活躍在航天器領(lǐng)域的軌跡規(guī)劃數(shù)值解法,以最優(yōu)控制理論為通用框架,可方便地加入各種約束條件,且求解精度高,收斂速度快.此外,航天器軌跡優(yōu)化策略可分為偽譜法和多階段偽譜法[5?7]求解兩類軌跡優(yōu)化策略.文獻[8]從六自由度運動模型出發(fā),充分考慮飛行機動性能限制和三維空間靜態(tài)障礙物約束,建立連續(xù)時間避障最優(yōu)軌跡規(guī)劃模型,給出基于Gauss偽譜法的無人直升機避障軌跡規(guī)劃策略.文獻[9]則引入內(nèi)點約束將突防軌跡分段,并綜合考慮求解精度和速率,結(jié)合Radau偽譜法和自適應(yīng)hp節(jié)點配置法,求解含靜態(tài)禁飛區(qū)和必經(jīng)航路點約束的多階段再入飛行器軌跡快速優(yōu)化問題.值得注意的是,以上基于偽譜法的航天器軌跡優(yōu)化策略均為針對靜態(tài)障礙物和靜態(tài)必經(jīng)路徑點的處理.

因此,基于以上分析,本文針對USV對動、靜態(tài)障礙物的機動避障問題,在遵循COLREGs的前提下,提出一種基于自適應(yīng)ph-Radau偽譜法的USV多階段避障軌跡規(guī)劃算法.該算法充分考慮了機動性能、狀態(tài)方程以及障礙物(包括動態(tài)和靜態(tài))等約束條件,能夠保證所規(guī)劃軌跡的可行性;COLREGs的遵守和內(nèi)點約束的引入,使得USV能同時對動態(tài)和靜態(tài)障礙物有效且協(xié)調(diào)地機動避讓;相比Gauss偽譜法,所設(shè)計算法具備更高的求解精度和速率.

1 國際海上避碰規(guī)則公約與避讓策略

1.1 國際海上避碰規(guī)則公約

目前還未有相關(guān)法律對USV的航行行為作出明確規(guī)定,但較為合理的方案就是將USV作為船舶的一種,遵循國際海事組織制定的COLREGs.

該公約第13～15條規(guī)定了碰撞局面及相關(guān)避讓規(guī)則.以USV航向為基準(zhǔn),根據(jù)障礙物與USV的航向角度差值,定義圖1所示的碰撞局面和如下避讓規(guī)則[3]:

1)追越:若障礙物與USV的航向角度差值在［0?,45?)和［315?,360?)之間,當(dāng)接近危險距離時,要求USV從被超越障礙物左舷處安全通過.

2)正面相遇:若障礙物與USV的航向角度差值在(165?,195?]之間,當(dāng)接近危險距離時,要求USV從障礙物右舷處安全通過.

3)交叉相遇:若障礙物與USV的航向角度差值在[45?,165?]之間,障礙物與USV 右交叉相遇,當(dāng)接近危險距離時,要求USV從障礙物右舷處安全通過;若航向角度差值在(195?,315?)之間,障礙物與USV左交叉相遇,當(dāng)接近危險距離時,要求USV從障礙物左舷處安全通過.

1.2 避讓策略

當(dāng)USV在攜帶傳感器的探測范圍內(nèi)檢測到未知的動、靜態(tài)障礙物并存在碰撞危險時,依據(jù)COLREGs判定碰撞局面,在障礙物和USV保持當(dāng)前位姿不變的假定條件下估算相撞時刻,設(shè)定合適的避讓航路點和禁航區(qū)約束.

其中,避讓航路點可看作是為了提高海事交通安全性,即有效且協(xié)調(diào)地安全避障,讓USV遵循COLREGs而設(shè)定的必經(jīng)地理位置.而為避免避讓策略的復(fù)雜性,僅在動態(tài)障礙物規(guī)避時設(shè)置避讓航路點即可.此外,為保證USV在安全避讓障礙物后能“優(yōu)雅”且快速地返回原始機動航行路線,需限制避讓航路點處的其他條件(如時間、姿態(tài)),避讓航路點約束可設(shè)置如下:

式中,tl為要求USV到達避讓航路點處的估算時刻,即時間約束;(xl,yl,ψl)為第l個避讓航路點的位置和航向;ψlmin和ψlmax為航向ψl的邊界值;lN為避讓航路點數(shù)目.

為保障避讓策略的可靠性,將未知靜態(tài)障礙物看作是一類特殊的動態(tài)障礙物并考慮避讓過程中仍存在碰撞危險,要求USV始終與障礙物保持一定的安全距離,即設(shè)置禁航區(qū)約束.將障礙物近似為一個速度和航向均為常值,半徑為Rj的圓域,j=1,2,···jN,jN為障礙物數(shù)目.假設(shè)初始時刻t0=0圓心坐標(biāo)為(xj0,yj0),移動速度為Vj,航向為ψj,t時刻的障礙物可描述為

則禁航區(qū)約束為

2 避障最優(yōu)軌跡規(guī)劃模型

2.1 約束條件

1)狀態(tài)方程約束

水平面三自由度欠驅(qū)動USV運動模型,即狀態(tài)方程[10]約束如下:

式中,m11=m?X˙u,m22=m?Y˙v,m33=Iz?N˙r;C13=?m22v,C23=m11u;d11=?Xu,d22=?Xv,d33=?Nr;τ=[Tu0Tr]T為控制輸入向量,τu為前進推力,τr為偏航力矩.注意到,式(4)中僅存在作用在縱蕩和艏搖兩個運動自由度上的控制力/力矩τu和τr,橫蕩運動自由度上不存在控制力/力矩,系統(tǒng)的控制輸入少于運動自由度個數(shù),故該三自由度USV為典型的欠驅(qū)動系統(tǒng).

2)機動性能約束

為保證所使用運動模型的可靠性,根據(jù)航行器的機動性能對狀態(tài)變量作如下約束:

式中,umax、vmax和ψmax均為非負常值.此外,考慮到控制變量受其執(zhí)行機構(gòu)性能與姿態(tài)控制系統(tǒng)的限制,為避免USV在航行過程中失控,對其前進推力和偏航力矩作如下約束:

式中,Tumax和Trmax均為非負常值.

3)禁航區(qū)約束

遵循COLREGs設(shè)置禁航區(qū)和避讓航路點約束,其中禁航區(qū)約束為式(3).

4)邊界條件約束

初始時刻t0=0的USV位姿為x x x(t0),指定終端時刻tf的避障結(jié)束位置(xf,yf)為終端目標(biāo),即

為提高避障可靠性,引入終端目標(biāo)處的其他條件約束(如速度、姿態(tài)),即

式中,ufmin、ufmax、ψfmin和ψfmax均為常值.

2.2 性能指標(biāo)

考慮到大洋中自主航行的USV攜帶能量有限,且能量補給困難,可選擇已到達終端目標(biāo)處的能耗最少為性能指標(biāo),即

當(dāng)然,依據(jù)USV的具體作業(yè)與任務(wù)需求,該性能指標(biāo)也可選擇為航行時間最短、航程最短等.

2.3 多階段避障最優(yōu)軌跡規(guī)劃模型

將避障軌跡在避讓航路點處分段,并引入內(nèi)點約束,將避讓航路點轉(zhuǎn)化為內(nèi)點約束,進而可借鑒多階段偽譜法軌跡優(yōu)化策略,建立USV多階段跡規(guī)劃模型.

給定避讓航路點數(shù)目lN,將原始時間區(qū)域I=[t0,tf]劃分為P段,P=lN+1,階段Ip=[tp?1,tp],p=1,2,···,P滿足:且于是,含有避讓航路點約束的USV避障軌跡規(guī)劃問題可描述為多階段軌跡規(guī)劃問題,即尋找控制變量u u u(p)(t)∈Rnu,最小化Bolza型性能指標(biāo):

滿足約束條件:

本文避讓航路點約束式(1)轉(zhuǎn)化為如下內(nèi)點約束:

式中,ψl滿足ψlmin≤ψl≤ψlmax.

3 自適應(yīng)ph-Radau偽譜法

在階段Ip=[tp?1,tp],p=1,2,···,P內(nèi),以偽譜法,如Gauss、Radau偽譜法離散化求解策略,可將連續(xù)時間最優(yōu)控制式(11)～式(14)轉(zhuǎn)化為含有帶有代數(shù)方程約束的大規(guī)模非線性規(guī)劃(Nonlinear Porgram,NLP)問題,借助序列二次規(guī)劃等數(shù)值方法,可獲得原連續(xù)時間最優(yōu)控制問題的近似數(shù)值解[11?12].

然而,多數(shù)實際問題均為非光滑問題,固定網(wǎng)格個數(shù)和配點數(shù)配置方式的偽譜法無法捕捉非光滑最優(yōu)控制問題的狀態(tài)或控制的不連續(xù)性;且求解精度與全局插值多項式維數(shù)(即配點數(shù))有關(guān),配點數(shù)的增加在提高求解精度的同時也增加計算代價[13].h法和p法是兩典型的網(wǎng)格細化算法,可有效解決NLP維度高與求解速率低的矛盾問題.因此,本文在Radau偽譜法的基礎(chǔ)上,結(jié)合自適應(yīng)網(wǎng)格劃分與節(jié)點個數(shù)分配[14?15],即基于自適應(yīng)ph-Rddau偽譜法求解多階段避障軌跡規(guī)劃問題.

其中,自適應(yīng)ph配置方法為:當(dāng)任意網(wǎng)格區(qū)間的最大相對誤差,k=1,2,···,K大于設(shè)定的誤差門限值ε時,自適應(yīng)進行網(wǎng)格劃分或增加Legendre Gauss Radau(LGR)節(jié)點個數(shù).

1)誤差判定準(zhǔn)則

假設(shè)在階段Ip的第k個網(wǎng)格區(qū)間[tk?1,tk]內(nèi),Nk個 LGR 配點和末端點=1構(gòu)成了該網(wǎng)格區(qū)間的插值節(jié)點,而連續(xù)時間最優(yōu)控制式(11)～式(14)及其狀態(tài)變量、控制變量的Radau偽譜法離散化過程見文獻[12?14].考慮到增加配點個數(shù)可提高求解精度,因而設(shè)定為網(wǎng)格區(qū)間[tk?1,tk]內(nèi)的Mk個LGR配點,且Mk=Nk+1,=?1,=1.取Mk個點作為誤差判定準(zhǔn)則的采樣點,則近似狀態(tài)?變量·和近似控?制變·量在采樣點上可表示為、,i=1,2,···,Mk.以? 為插·值節(jié)點,?得到·近似狀態(tài)變量和控制變量和,則狀態(tài)方程在LGR配點=1,2,···,Mk上可近似為

式 中,=j=1,2,···,Mk為網(wǎng)格區(qū)間[tk?1,tk]的Mk×Mk維LGR積分權(quán)重矩陣.則第i個狀態(tài)在采樣點上的絕對誤差和相對誤差可表示為

網(wǎng)格區(qū)間[tk?1,tk]中最大相對誤差為

2)自適應(yīng)ph網(wǎng)格配置方法

令Nmin和Nmax分別為每個網(wǎng)格區(qū)間內(nèi)的最小和最大LGR配點個數(shù).假定網(wǎng)格區(qū)間[tk?1,tk]內(nèi)LGR配點個數(shù)為Nk,Nk≤Nmax,最大相對誤差大于誤差門限值ε,則令

增加配點個數(shù),并取整為

當(dāng)進行網(wǎng)格細化時,要求細化后所有網(wǎng)格區(qū)間的配點個數(shù)之和與一致,且每個網(wǎng)格區(qū)間的配點個數(shù)均為Nmin,即將網(wǎng)格區(qū)間[tk?1,tk]進行Bq等分,等分子區(qū)間配點個數(shù)之和為BqNmin.等分子區(qū)間的個數(shù)Bq取

值得注意的是,在該網(wǎng)格更新策略中,無論是增加配點個數(shù)還是進一步網(wǎng)格細化,配點總數(shù)均為?Nq.該網(wǎng)格更新策略可稱為自適應(yīng)ph網(wǎng)格更新策略,即先p后h的網(wǎng)格更新策略,當(dāng)區(qū)間的配點個數(shù)超過最大配點個數(shù)Nmax時,才對網(wǎng)格進行細化處理.而結(jié)合Radau偽譜法與自適應(yīng)ph網(wǎng)格更新策略的自適應(yīng)ph-Radau偽譜法具體迭代策略可參見文獻[15].

4 仿真實驗與分析

采用挪威科技大學(xué)試驗?zāi)Ｐ痛珻yberShipΠ[16]模型進行仿真實驗,驗證所設(shè)計算法的可行性與有效性,即是否能遵循COLREGs制定的避讓航路點和禁航區(qū)約束,成功規(guī)避動、靜態(tài)障礙物的同時,也能兼顧算法的求解精度與速率.

4.1 避讓航路點約束驗證實驗

選擇靜態(tài)和動態(tài)障礙物各一個進行避障實例驗證.為提高避障安全度,對靜、動態(tài)障礙物進行膨脹處理.障礙物運動參數(shù)如表1所示.

表1 障礙物運動參數(shù)

遵循COLREGs判定存在碰撞危險,且與動態(tài)障礙物的碰撞局面為正面相遇,避讓航路點數(shù)目lN=1,障礙物數(shù)目jN=2.其中,估算避讓時刻為tl=40 s,避讓航路點約束為

而帶入障礙物運動參數(shù)表1和式(2)～式(3),即可獲得禁航區(qū)約束.

此外,給定USV的邊界條件約束:

USV的機動性能約束:

給定自適應(yīng)ph-Radau偽譜法離散化求解策略中的參數(shù):

式中,Mmax為最大迭代次數(shù);K0和N0分別表示階段內(nèi)網(wǎng)格區(qū)間初始個數(shù)和網(wǎng)格區(qū)間內(nèi)LGR配點個數(shù);Nmin和Nmax分別為網(wǎng)格區(qū)間內(nèi)最小和最大配點個數(shù).

仿真結(jié)果如圖2～圖5所示.圖2為在最優(yōu)控制變量作用下USV最優(yōu)航行軌跡變化曲線,其中,黑色方框?qū)?yīng)初始時刻t0、避讓時刻tl和終端時刻tf對應(yīng)的USV具體位置;橙色實心圓對應(yīng)初始時刻t0的動態(tài)障礙物具體位置,灰色實心圓分別對應(yīng)避讓時刻tl和終端時刻tf的動態(tài)障礙物具體位置,黑色箭頭表示動態(tài)障礙物行進過程;黑色實心圓為靜態(tài)障礙物.圖3為禁航區(qū)變化曲線,即避障過程中USV與靜、動態(tài)障礙物之間距離的時間變化曲線.注意到,在禁航區(qū)約束中取膨脹2m后的障礙物半徑,而在實際繪制USV最優(yōu)航行軌跡變化曲線時取原障礙物半徑.圖4為最優(yōu)控制輸入變化曲線.圖5為USV最優(yōu)狀態(tài)變化曲線.可以看出:1)控制變量在整個避障過程中較為平滑,無較大跳變出現(xiàn),USV的初始狀態(tài)和終端狀態(tài)也均在約束范圍內(nèi),即機動性能約束條件得以滿足;2)USV準(zhǔn)確地經(jīng)過了避讓航路點,且禁航區(qū)約束的設(shè)置使得USV在整個航行過程中與動、靜態(tài)障礙物保持一定安全距離,即避讓航路點約束和禁航區(qū)約束條件得以滿足;3)USV的初始狀態(tài)和終端狀態(tài)均在約束范圍內(nèi),邊界條件約束得以滿足;4)由避讓航路點造成的兩階段避障軌跡和各狀態(tài)變量銜接處的變化均較為平滑,表明本章所設(shè)計的多階段避障軌跡規(guī)劃策略中內(nèi)點約束的有效性.

此外,該自適應(yīng)ph-Radau偽譜法多階段避障軌跡規(guī)劃算法得到的網(wǎng)格區(qū)間最大相對誤差為5.2344×10?4,滿足誤差門限值.文獻[4]中的避障優(yōu)化軌跡與本文的優(yōu)化軌跡差別較大,這是由于基于PSO算法優(yōu)化避障軌跡過程中未從USV無人運載平臺的狀態(tài)方程出發(fā),而在實際航行過程中各運動自由度存在一定的耦合關(guān)系.

4.2 禁航區(qū)約束驗證實驗

保持上述仿真參數(shù)不變,驗證本章所設(shè)計算法是否能夠有效處理制定的禁航區(qū)約束.圖6給出了同時包含避讓航路點約束、禁航區(qū)約束和僅包含避讓航路點約束的USV航行軌跡變化曲線.圖7對應(yīng)兩種情況的禁航區(qū)變化曲線.

其中,同時包含避讓航路點和禁航區(qū)約束的情況存在禁航區(qū)變化曲線,僅包含避讓航路點約束的情況不存在禁航區(qū)約束,但考慮到禁航區(qū)實質(zhì)即為USV與動、靜態(tài)障礙物之間的距離變化,故為了描述方便,仍用“禁航區(qū)變化曲線”描述僅包含避讓航路點約束情況下USV與障礙物之間距離的變化.此外,取原障礙物半徑進行USV最優(yōu)航行軌跡變化曲線的繪制,而在禁航區(qū)約束中取膨脹半徑為2 m后的障礙物半徑.

可以看出:1)兩種情況下,USV均精確通過避讓航路點;2)僅考慮避讓航路點約束,即忽略避讓過程中存在的碰撞危險,與同時考慮避讓航路點、禁航區(qū)約束的情況下,USV的最優(yōu)航行軌跡是不同的.對于存在禁航區(qū)的情況,USV需要做“S”形機動繞過禁航區(qū),以達到指定目標(biāo)位置.

因此,圖2～圖7的分析表明了將避讓航路點轉(zhuǎn)化為內(nèi)點,并考慮禁航區(qū)約束的多階段避障軌跡規(guī)劃策略的可行性.

4.3 求解精度和速度驗證實驗

為了更好地說明本章所設(shè)計算法的有效性,給出采用Gauss偽譜法離散化求解策略和本文求解策略的USV最優(yōu)航行軌跡對比,如圖8所示.表2對應(yīng)兩種情況下的部分仿真結(jié)果,其中LG點為Gauss偽譜法離散化求解策略所需的Lagendre-Gauss離散點.為便于比較兩種求解策略的求解精度,給出圖8中障礙物膨脹2 m后的USV最優(yōu)航跡局部放大圖,如圖9所示.其中,粉色區(qū)域?qū)?yīng)障礙物的膨脹部分.

從圖8、圖9和表2可明顯地看出:1)在離散點個數(shù)近乎相同時,Gauss偽譜離散化求解策略下的靜態(tài)障礙物禁航區(qū)約束未被滿足,得到的最優(yōu)航行軌跡穿過了禁航區(qū);而自適應(yīng)ph-Radau偽譜法則根據(jù)設(shè)定的解的誤差判定準(zhǔn)則,自動檢測到軌跡精度的滿足情況,自適應(yīng)增加離散點個數(shù)或者進一步網(wǎng)格細化.3)自適應(yīng)ph-Radau偽譜法的仿真運行時間遠小于Gauss偽譜法.

表2 部分仿真結(jié)果

因此,本章自適應(yīng)ph-Radau偽譜法多階段避障軌跡規(guī)劃算法能夠較好處理避讓航路點和禁航區(qū)約束的同時,也能兼顧解的求解精度和速度.

5 結(jié)論

本文提出一種基于自適應(yīng)ph-Radau偽譜法的USV多階段避障軌跡規(guī)劃算法.相比基于PSO等速度避障法,該算法從USV運動模型出發(fā),充分考慮航行機動性能限制等約束條件,保證了所規(guī)劃避障軌跡的可行性.而遵循COLREGs設(shè)定的必然航路點和禁航區(qū)約束,以及內(nèi)點約束的引入,保證了USV能同時有效且協(xié)調(diào)地規(guī)避動態(tài)和靜態(tài)障礙物.此外,相比Gausss偽譜法,該算法能兼顧求解精度和速率.

如何將該方法應(yīng)用于外界環(huán)境,如海風(fēng)、海浪、海流擾動下的USV避障軌跡規(guī)劃問題可作為進一步的研究方向.