戴懷志 蒲紅紅

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

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

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

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

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

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

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

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

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

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

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

1.2 避讓策略

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

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

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

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

則禁航區(qū)約束為

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

2.1 約束條件

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

水平面三自由度欠驅(qū)動(dòng)USV運(yùn)動(dòng)模型,即狀態(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為前進(jìn)推力,τr為偏航力矩.注意到,式(4)中僅存在作用在縱蕩和艏搖兩個(gè)運(yùn)動(dòng)自由度上的控制力/力矩τu和τr,橫蕩運(yùn)動(dòng)自由度上不存在控制力/力矩,系統(tǒng)的控制輸入少于運(yùn)動(dòng)自由度個(gè)數(shù),故該三自由度USV為典型的欠驅(qū)動(dòng)系統(tǒng).

2)機(jī)動(dòng)性能約束

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

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

式中,Tumax和Trmax均為非負(fù)常值.

3)禁航區(qū)約束

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

4)邊界條件約束

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

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

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

2.2 性能指標(biāo)

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

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

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

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

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

滿足約束條件:

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

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

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

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

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

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

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

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

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

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

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

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

增加配點(diǎn)個(gè)數(shù),并取整為

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

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

4 仿真實(shí)驗(yàn)與分析

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

4.1 避讓航路點(diǎn)約束驗(yàn)證實(shí)驗(yàn)

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

表1 障礙物運(yùn)動(dòng)參數(shù)

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

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

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

USV的機(jī)動(dòng)性能約束:

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

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

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

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

4.2 禁航區(qū)約束驗(yàn)證實(shí)驗(yàn)

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

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

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

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

4.3 求解精度和速度驗(yàn)證實(shí)驗(yàn)

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

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

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

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

5 結(jié)論

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

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