本發(fā)明涉及船舶控制工程與船舶自動(dòng)化航行領(lǐng)域，更具體地，涉及一種考慮混合多目標(biāo)避障的水面無人艇路徑跟蹤制導(dǎo)方法。
背景技術(shù)：
：：通過制導(dǎo)策略實(shí)現(xiàn)船舶的航跡保持及避碰/避障是船舶運(yùn)動(dòng)控制的關(guān)鍵技術(shù)。船舶路徑跟蹤的制導(dǎo)算法目前應(yīng)用較廣泛的主要有可視距離(los)制導(dǎo)算法和動(dòng)態(tài)虛擬小船(dvs)制導(dǎo)算法。避障算法可以借鑒移動(dòng)機(jī)器人和陸上小車的控制，采用基于非線性控制理論穩(wěn)定極限環(huán)(stablelimitcycles)概念的障礙物規(guī)避算法，即極限環(huán)避障算法，該避障算法具有抖振小、安全性能高、避障軌跡平滑的優(yōu)點(diǎn)。將上述船舶路徑跟蹤的制導(dǎo)算法同避障算法相結(jié)合的制導(dǎo)策略成為近年來研究的熱點(diǎn)。下面就現(xiàn)有的路徑跟蹤的船舶制導(dǎo)算法同極限環(huán)避障算法相結(jié)合的制導(dǎo)策略進(jìn)行簡要介紹。文獻(xiàn)[1]中給出了los制導(dǎo)算法結(jié)合避障算法的制導(dǎo)策略。los制導(dǎo)算法的基本原理如圖1所示：在前向距離固定的前提下，los制導(dǎo)算法根據(jù)實(shí)船位置與直線參考路徑上的點(diǎn)af之間的幾何關(guān)系獲得每一采樣時(shí)間點(diǎn)的路徑跟蹤期望艏搖角ψlos，前進(jìn)速度up保持不變，進(jìn)而引導(dǎo)船舶實(shí)現(xiàn)對(duì)直線參考路徑的跟蹤。該制導(dǎo)的策略能夠使實(shí)船指數(shù)收斂到參考直線路徑上。圖1中，pi-1pi為實(shí)船當(dāng)前跟蹤的直線路徑，d為實(shí)船到路徑的垂線距離，(x,y)為實(shí)船的當(dāng)前位置坐標(biāo)，則可以得到：考慮實(shí)船的欠驅(qū)動(dòng)特性，在期望艏向角中引入漂角的補(bǔ)償，則可以得到路徑跟蹤期望艏向角：當(dāng)船舶進(jìn)入航路點(diǎn)pi的轉(zhuǎn)向邊界環(huán)(circleofacceptance)時(shí)，船舶的跟蹤路徑切換為pipi+1。從上述推導(dǎo)可以看出：los制導(dǎo)算法不具備轉(zhuǎn)向點(diǎn)處的路徑規(guī)劃功能，如果在轉(zhuǎn)向點(diǎn)處遇到障礙物會(huì)出現(xiàn)制導(dǎo)算法的失效。同時(shí)，該算法還不滿足“一切參考路徑均可以由虛擬小船產(chǎn)生的假設(shè)”，難以同路徑跟蹤控制的相關(guān)研究結(jié)果結(jié)合。當(dāng)單船會(huì)遇時(shí)，通過虛擬定義在會(huì)遇船舶上的探測(cè)環(huán)感知其相對(duì)于實(shí)船的位置。在路徑跟蹤模式下，若避障操縱條件符合，則實(shí)船由路徑跟蹤模式切換到避障操縱模式，前進(jìn)運(yùn)動(dòng)由up加速到避障速度uoa，且按照避障期望艏向角ψo(hù)a的導(dǎo)引航行。避障期望艏向角可以導(dǎo)引實(shí)船收斂到會(huì)遇船舶半徑小于探測(cè)半徑的穩(wěn)定安全極限環(huán)上，確保了實(shí)船和會(huì)遇船舶之間的安全距離。該避障策略的參數(shù)說明如圖2所示。如圖2所示，探測(cè)環(huán)半徑為rm，安全極限環(huán)半徑為ro，當(dāng)實(shí)船進(jìn)入到探測(cè)環(huán)內(nèi)部時(shí)，若實(shí)船到來船距離σ滿足如下避障操縱條件則啟動(dòng)避障操縱模式。由圖2所示的避障策略幾何關(guān)系可知：v0＝-uccos(φ-θ)避障操縱模式下，避障期望艏向角ψo(hù)a選取如下：式(5)中，誤差e＝σ-ro；δ為避障操縱模式下手動(dòng)設(shè)定的前向距離(lookingaheaddistance)；λ根據(jù)colregs的要求選取為±1，+1表示以順時(shí)針的方向環(huán)繞安全極限環(huán)，-1表示以逆時(shí)針的方向環(huán)繞；k用于補(bǔ)償會(huì)遇船舶運(yùn)動(dòng)對(duì)艏向角ψo(hù)a造成的影響，定義為：其中，該避障策略為了保證避障過程的快速性，避障速度一定滿足uoa≥uc≥v0。在控制算法能夠保證實(shí)船實(shí)際艏向角和實(shí)際前進(jìn)速度對(duì)期望艏向角和前進(jìn)速度的有效收斂前提條件下，本制導(dǎo)策略根據(jù)當(dāng)前障礙物的位置及避障操縱條件，在路徑跟蹤模式和避障操縱模式之間來回切換。當(dāng)兩船會(huì)遇時(shí)，首先執(zhí)行避碰任務(wù)，避障期望艏搖角的選取策略會(huì)保證實(shí)船收斂到來船的安全極限環(huán)上，當(dāng)避障操縱條件不滿足時(shí)，實(shí)船從新回到直線參考路徑的路徑跟蹤任務(wù)上。與之對(duì)應(yīng)，實(shí)船的期望前進(jìn)速度也會(huì)根據(jù)任務(wù)模式的不同而進(jìn)行切換。該制導(dǎo)策略的執(zhí)行過程如流程圖3所示。具備曲線段路徑規(guī)劃能力非常重要。對(duì)于路徑跟蹤控制部分，不僅需要對(duì)直線參考路徑進(jìn)行規(guī)劃，而且還需要在轉(zhuǎn)向點(diǎn)附件進(jìn)行曲線段路徑規(guī)劃；對(duì)于避障控制部分，由于障礙物出現(xiàn)的位置不確定，所以，曲線段路徑規(guī)劃對(duì)避障的有效性尤為重要。另外，船舶避障/避碰行為主要發(fā)生在狹窄繁忙水道或漁船作業(yè)區(qū)等復(fù)雜海洋環(huán)境下，避障策略應(yīng)盡可能應(yīng)對(duì)多種避碰/避障條件并具有多適應(yīng)性，以體現(xiàn)船舶高度智能化航行的需求。同時(shí)，船舶路徑跟蹤及避障制導(dǎo)的有效性只有通過控制策略得以保證，制導(dǎo)算法應(yīng)便于同控制算法相結(jié)合。因此，文獻(xiàn)[1]中所闡述的los制導(dǎo)算法綜合避障控制的制導(dǎo)策略存在的缺陷，總結(jié)如下：(1)該制導(dǎo)算法基于los路徑跟蹤制導(dǎo)算法建立，因此也沿襲了los制導(dǎo)算法的缺陷。該算法不具備轉(zhuǎn)向點(diǎn)處的路徑規(guī)劃功能，如果在轉(zhuǎn)向點(diǎn)處遇到障礙物會(huì)出現(xiàn)制導(dǎo)算法的失效；該算法不滿足“一切參考路徑均可以由虛擬小船產(chǎn)生的假設(shè)”，難以同欠驅(qū)動(dòng)船舶路徑跟蹤控制相關(guān)研究結(jié)果結(jié)合。(2)該制導(dǎo)算法不具備對(duì)多障礙物避障的功能，僅僅適用于單船會(huì)遇避碰的情況，不適用于復(fù)雜海洋環(huán)境下及高速船的避障控制。另外，該制導(dǎo)算法路徑跟蹤模式和避障操縱模式的切換過程存在階躍，切換過程不平滑。參考文獻(xiàn)[1]s.moe,k.y.pettersen.set-basedline-of-sight(los)pathfollowingwithcollisionavoidanceforunderactuatedunmannedsurfacevessel[c].24thmediterreanconferenceoncontrolandautomation,2016:402-409.技術(shù)實(shí)現(xiàn)要素：本發(fā)明的目的在于克服現(xiàn)有技術(shù)存在的上述缺陷，提供一種便于同控制算法相結(jié)合且具有曲線段路徑規(guī)劃能力的dvs制導(dǎo)算法，該制導(dǎo)算法不僅克服了los制導(dǎo)算法的缺陷，還在一定程度上滿足了路徑跟蹤的時(shí)間要求。另外，本發(fā)明設(shè)置了避碰優(yōu)先級(jí)及相應(yīng)的艏向角及速度規(guī)劃策略，實(shí)現(xiàn)了船舶對(duì)多靜止障礙物、多動(dòng)態(tài)障礙物及混合障礙物的有效避障。為實(shí)現(xiàn)上述目的，本發(fā)明的技術(shù)方案如下：一種考慮混合多目標(biāo)避障的水面無人艇路徑跟蹤制導(dǎo)方法，包括以下步驟：s1：設(shè)定航路點(diǎn)信息w1,w2,…,wn，分別建立導(dǎo)引虛擬小船和動(dòng)態(tài)虛擬小船的運(yùn)動(dòng)數(shù)學(xué)模型；s2：導(dǎo)引虛擬小船根據(jù)制導(dǎo)算法規(guī)劃出參考路徑，并始終沿參考路徑行駛，啟動(dòng)路徑跟蹤制導(dǎo)模式，動(dòng)態(tài)虛擬小船的艏向角時(shí)刻等于導(dǎo)引虛擬小船相對(duì)于動(dòng)態(tài)虛擬小船的真方位角，并向?qū)嵈l(fā)出制導(dǎo)命令，以引導(dǎo)實(shí)船進(jìn)行路徑跟蹤，實(shí)船不斷獲取動(dòng)態(tài)虛擬小船的導(dǎo)引變量；s3：在采樣時(shí)間點(diǎn)進(jìn)行采樣，判斷動(dòng)態(tài)虛擬小船是否進(jìn)入障礙物探測(cè)環(huán)，如進(jìn)入障礙物探測(cè)環(huán)，進(jìn)入障礙物排序過程，執(zhí)行s4；如未進(jìn)入障礙物探測(cè)環(huán)，保持路徑跟蹤制導(dǎo)模式，執(zhí)行s6；s4：進(jìn)入障礙物排序過程：對(duì)避障物的優(yōu)先級(jí)進(jìn)行排序，得到障礙物的優(yōu)先級(jí)序列，選取最高優(yōu)先級(jí)障礙物作為當(dāng)前避障目標(biāo)，判斷當(dāng)前避障目標(biāo)是否滿足避障操縱條件，若當(dāng)前避障目標(biāo)滿足避障操縱條件，則進(jìn)入對(duì)當(dāng)前障礙物的避障制導(dǎo)模式，即執(zhí)行s5；若當(dāng)前避障目標(biāo)不滿足避障操縱條件，選取次優(yōu)先級(jí)的障礙物作為當(dāng)前避障目標(biāo)；若所有障礙物都不滿足避障操縱條件，則進(jìn)入路徑跟蹤模式，執(zhí)行s6；s5：進(jìn)入避障制導(dǎo)模式：在每一采樣時(shí)間點(diǎn)，判斷當(dāng)前障礙物的避障操縱條件是否滿足，如滿足避障操縱條件，則啟動(dòng)對(duì)當(dāng)前障礙物的避障制導(dǎo)模式，獲取動(dòng)態(tài)虛擬小船的導(dǎo)引變量，引導(dǎo)實(shí)船；如不滿足避障操縱條件，則從障礙物優(yōu)先級(jí)序列中去除當(dāng)前障礙物，并執(zhí)行s4；s6：根據(jù)動(dòng)態(tài)虛擬小船的導(dǎo)引變量和控制策略，實(shí)時(shí)調(diào)整實(shí)船的執(zhí)行器輸入，控制實(shí)船跟蹤動(dòng)態(tài)虛擬小船；s7：測(cè)量實(shí)船位置，判斷是否到達(dá)終點(diǎn)，如果是，結(jié)束船舶航行，如果否，則執(zhí)行s2。進(jìn)一步地，s1中，所述導(dǎo)引虛擬小船和動(dòng)態(tài)虛擬小船的運(yùn)動(dòng)數(shù)學(xué)模型的表達(dá)式為：其中，d代表動(dòng)態(tài)虛擬小船，g代表導(dǎo)引虛擬小船，(x,y)為位置坐標(biāo)，分別為距離的一階導(dǎo)數(shù)，ψ為艏向角，u為前進(jìn)速度，r為轉(zhuǎn)首角速度。進(jìn)一步地，s4中，所述的障礙物優(yōu)先級(jí)序列的生成方法為：s41：將障礙物分為靜態(tài)障礙物和動(dòng)態(tài)障礙物，靜態(tài)障礙物的優(yōu)先級(jí)高于動(dòng)態(tài)障礙物的優(yōu)先級(jí)；s42：靜態(tài)障礙物的優(yōu)先級(jí)與動(dòng)態(tài)虛擬小船到該靜態(tài)障礙物安全極限環(huán)的距離e大小成反比，e最小的障礙物擁有最高的優(yōu)先級(jí)；s43：動(dòng)態(tài)障礙物的優(yōu)先級(jí)由下列公式確定其中，f為評(píng)價(jià)函數(shù)，rm為障礙物探測(cè)環(huán)的半徑，ro為安全極限環(huán)的半徑，e為動(dòng)態(tài)虛擬小船到該動(dòng)態(tài)障礙物的安全極限環(huán)的距離，為動(dòng)態(tài)虛擬小船在路徑跟蹤模式下其與障礙物距離的導(dǎo)數(shù)，udo表示動(dòng)態(tài)虛擬小船的避障速度，為一個(gè)常數(shù)，其值要大于所有動(dòng)態(tài)障礙物的速度，為權(quán)重參數(shù)。進(jìn)一步地，s4中，所述避障操縱條件分為對(duì)靜態(tài)障礙物的避障操縱條件和對(duì)動(dòng)態(tài)障礙物的避障操縱條件，所述對(duì)靜態(tài)障礙物的避障操縱條件為：動(dòng)態(tài)虛擬小船的艏向在動(dòng)態(tài)虛擬小船到靜態(tài)障礙物的安全極限環(huán)的兩條切線之間；所述對(duì)動(dòng)態(tài)障礙物的避障操縱條件為下列公式：其中，σ為動(dòng)態(tài)虛擬小船到動(dòng)態(tài)障礙物的距離，ro為動(dòng)態(tài)障礙物的極限安全環(huán)的半徑，rm為動(dòng)態(tài)障礙物的探測(cè)環(huán)的半徑，為路徑跟蹤模式下動(dòng)態(tài)虛擬小船到動(dòng)態(tài)障礙物距離的導(dǎo)數(shù)。進(jìn)一步地，路徑跟蹤制導(dǎo)模式和避障制導(dǎo)模式切換時(shí)以及不同障礙物的避障制導(dǎo)模式切換時(shí)，采用如下時(shí)間過渡函數(shù)：其中，α(t)為時(shí)間過渡函數(shù)，tc為起始切換時(shí)間點(diǎn)，ts為手動(dòng)設(shè)定的過渡時(shí)間。進(jìn)一步地，s5所述的避障制導(dǎo)模式中，動(dòng)態(tài)虛擬小船的速度ud調(diào)整為避障速度，艏向角按照下列方法選?。簊51：如當(dāng)前避障目標(biāo)為靜態(tài)障礙物，則其中，ψdo為動(dòng)態(tài)虛擬小船在避障制導(dǎo)模式中的艏向角，φ為靜態(tài)障礙物相對(duì)動(dòng)態(tài)虛擬小船的真方位角，δ為設(shè)定的前向距離，λ＝±1決定動(dòng)態(tài)虛擬小船避障時(shí)環(huán)繞靜態(tài)障礙物的方向，+1為順時(shí)針方向環(huán)繞靜態(tài)障礙物的安全極限環(huán)，-1為逆時(shí)針方向環(huán)繞靜態(tài)障礙物的安全極限環(huán)；s52：如當(dāng)前避障目標(biāo)為動(dòng)態(tài)障礙物，則其中，φ為動(dòng)態(tài)障礙物相對(duì)動(dòng)態(tài)虛擬小船的真方位角，δ為設(shè)定的前向距離，k為補(bǔ)償參數(shù)，λ＝±1，+1為順時(shí)針環(huán)繞動(dòng)態(tài)障礙物的安全極限環(huán)，-1為逆時(shí)針環(huán)繞動(dòng)態(tài)障礙物的安全極限環(huán)。進(jìn)一步地，s2中，所述動(dòng)態(tài)虛擬小船的導(dǎo)引變量包括艏向角ψd和前進(jìn)速度ud，艏向角ψd的計(jì)算公式為：其中，g代表gvs，d代表dvs，(x,y)為位置坐標(biāo)；前進(jìn)速度ud的計(jì)算公式為：其中，kd為用于調(diào)整收斂速度快慢的設(shè)定參數(shù)，ldg為動(dòng)態(tài)虛擬小船到導(dǎo)引虛擬小船的距離，ug為導(dǎo)引虛擬小船的前進(jìn)速度，ψg為導(dǎo)引虛擬小船的艏向角，ψd為動(dòng)態(tài)虛擬小船的艏向角，ldbset為實(shí)船到動(dòng)態(tài)虛擬小船的距離的上限設(shè)定，ldb為實(shí)船到動(dòng)態(tài)虛擬小船的距離。進(jìn)一步地，s3中，所述障礙物探測(cè)環(huán)的半徑rm根據(jù)障礙物的大小確定，障礙物越大，其值越大。從上述技術(shù)方案可以看出，本發(fā)明通過將制導(dǎo)過程分為路徑跟蹤和避障操縱制導(dǎo)兩種模式，以改進(jìn)dvs制導(dǎo)算法為基本構(gòu)架，由gvs動(dòng)態(tài)規(guī)劃出由直線和曲線組成的光滑參考路徑，路徑跟蹤模式和避障操縱模式分別對(duì)應(yīng)不同的dvs的引導(dǎo)變量。對(duì)于多個(gè)或混合的障礙物的避障制導(dǎo)，首先將根據(jù)優(yōu)先級(jí)排序及避障操縱條件評(píng)判確定當(dāng)前的避障目標(biāo)，啟動(dòng)避障操縱模式，過渡函數(shù)保證dvs引導(dǎo)變量在不同模式切換之間的光滑性。本發(fā)明對(duì)多種控制策略具有適用性，便于同現(xiàn)有先進(jìn)控制算法結(jié)合，控制算法的作用在于保證實(shí)船對(duì)dvs的收斂，保證制導(dǎo)策略的有效性。附圖說明圖1是現(xiàn)有技術(shù)中l(wèi)os制導(dǎo)算法的基本原理圖；圖2是現(xiàn)有技術(shù)中單船會(huì)遇的避障策略的示意圖；圖3是現(xiàn)有技術(shù)中l(wèi)os路徑跟蹤及避障的制導(dǎo)算法的流程圖；圖4是本發(fā)明的改進(jìn)的dvs制導(dǎo)算法的基本原理圖；圖5是一種多靜態(tài)目標(biāo)避障態(tài)勢(shì)的示意圖；圖6是一種多動(dòng)態(tài)目標(biāo)避障態(tài)勢(shì)的示意圖；圖7是一種混合多目標(biāo)避障態(tài)勢(shì)的示意圖；圖8是本發(fā)明的混合多目標(biāo)避障態(tài)勢(shì)中的參數(shù)示意圖；圖9是本發(fā)明的考慮混合多目標(biāo)避障的水面無人艇路徑跟蹤制導(dǎo)方法的流程圖；圖10是大連海事大學(xué)教學(xué)實(shí)習(xí)船“育鯤”輪的示意圖；圖11是蒲福風(fēng)6級(jí)海況下的三維波面示意圖；圖12～圖15分別是t＝240s,290s,345s,600s時(shí)的實(shí)船軌跡圖；圖16是一具體實(shí)施例中的dvs引導(dǎo)變量的時(shí)間變化曲線；圖17是一具體實(shí)施例中的控制輸入一螺距p和舵角δ的時(shí)間變化曲線。具體實(shí)施方式下面結(jié)合附圖，對(duì)本發(fā)明的具體實(shí)施方式作進(jìn)一步的詳細(xì)說明。需要說明的是，在下述的具體實(shí)施方式中，在詳述本發(fā)明的實(shí)施方式時(shí)，為了清楚地表示本發(fā)明的結(jié)構(gòu)以便于說明，特對(duì)附圖中的結(jié)構(gòu)不依照一般比例繪圖，并進(jìn)行了局部放大、變形及簡化處理，因此，應(yīng)避免以此作為對(duì)本發(fā)明的限定來加以理解。在以下本發(fā)明的具體實(shí)施方式中，請(qǐng)參閱圖9，圖9是本發(fā)明的考慮混合多目標(biāo)避障的水面無人艇路徑跟蹤制導(dǎo)方法的流程圖,結(jié)合參考圖4～圖8，對(duì)本發(fā)明進(jìn)一步理解。如圖9所示，本發(fā)明的考慮混合多目標(biāo)避障的水面無人艇路徑跟蹤制導(dǎo)方法,包括以下步驟：s1：設(shè)定航路點(diǎn)信息w1,w2,…,wn，分別建立導(dǎo)引虛擬小船和動(dòng)態(tài)虛擬小船的運(yùn)動(dòng)數(shù)學(xué)模型。s2：導(dǎo)引虛擬小船根據(jù)制導(dǎo)算法規(guī)劃出參考路徑，并始終沿參考路徑行駛，啟動(dòng)路徑跟蹤制導(dǎo)模式，動(dòng)態(tài)虛擬小船的艏向角時(shí)刻等于導(dǎo)引虛擬小船相對(duì)于動(dòng)態(tài)虛擬小船的真方位角，并向?qū)嵈l(fā)出制導(dǎo)命令，以引導(dǎo)實(shí)船進(jìn)行路徑跟蹤，實(shí)船不斷獲取動(dòng)態(tài)虛擬小船的導(dǎo)引變量。在上述步驟中，請(qǐng)參閱圖4，圖4是改進(jìn)的動(dòng)態(tài)虛擬小船(dvs)的制導(dǎo)算法的原理圖。在本制導(dǎo)算法中，導(dǎo)引虛擬小船(gvs)和dvs均為不考慮慣性力和阻尼力的理想小船，僅考慮平面運(yùn)動(dòng)，它們分別采取獨(dú)立的運(yùn)動(dòng)數(shù)學(xué)方程為：其中，d代表動(dòng)態(tài)虛擬小船，g代表導(dǎo)引虛擬小船，(x,y)為位置坐標(biāo)，分別為距離的一階導(dǎo)數(shù)，ψ為艏向角，u為前進(jìn)速度，r為轉(zhuǎn)首角速度。gvs根據(jù)設(shè)定航路點(diǎn)wi-1,wi,wi+1的信息，依據(jù)傳統(tǒng)dvs制導(dǎo)算法規(guī)劃出參考路徑，即gvs的位置和姿態(tài)時(shí)間序列。參考路徑分為直線參考路徑部分和航路點(diǎn)處的曲線參考路徑部分，曲線段參考路徑通過圓弧的內(nèi)插法產(chǎn)生，該曲線段參考路徑解決了現(xiàn)有技術(shù)中在航路點(diǎn)附件不能引導(dǎo)的問題。設(shè)定gvs的縱向速度ur和轉(zhuǎn)艏角速度rr，滿足實(shí)船執(zhí)行器性能，按照公式(7)的運(yùn)動(dòng)學(xué)關(guān)系沿著直線及曲線段參考路徑航行。在傳統(tǒng)dvs制導(dǎo)算法中，在實(shí)船與gvs之間生成一個(gè)dvs作為實(shí)船的直接追蹤目標(biāo)，其位置固定在實(shí)船和gvs的連線上，dvs的位置受到約束，不適應(yīng)多目標(biāo)避障/避碰的制導(dǎo)。為了進(jìn)一步滿足避障/避碰制導(dǎo)的要求，本發(fā)明對(duì)dvs制導(dǎo)算法進(jìn)行了改進(jìn)，即dvs的生成不再受gvs和實(shí)船位置的限制，將其視為按照公式(7)所示運(yùn)動(dòng)學(xué)關(guān)系獨(dú)立航行的目標(biāo)。在路徑跟蹤模式下，dvs艏向角時(shí)刻ψd選取為gvs相對(duì)于dvs的真方位角ψdp，即式(8)：其中，g代表gvs，d代表dvs，(x,y)為位置坐標(biāo)。為了保證路徑跟蹤模式下dvs對(duì)gvs位置的指數(shù)收斂，將路徑跟蹤dvs速度定義為式(9)，令ud＝udp其中，kd為用于調(diào)整收斂速度快慢的設(shè)定參數(shù)，其值越大，udp越大，dvs對(duì)gvs收斂越快，由此可能造成實(shí)船執(zhí)行器輸入越大，所以應(yīng)根據(jù)執(zhí)行器能力折中選取。ldbset為實(shí)船到dvs距離的上限設(shè)定，其值的選取也由船舶的執(zhí)行器能力確定，ldg為動(dòng)態(tài)虛擬小船到導(dǎo)引虛擬小船的距離，ug為導(dǎo)引虛擬小船的前進(jìn)速度，ψg為導(dǎo)引虛擬小船的艏向角，ψd為動(dòng)態(tài)虛擬小船的艏向角，ldb為實(shí)船到動(dòng)態(tài)虛擬小船的距離。從式(9)可以看出，dvs路徑跟蹤速度和實(shí)船到dvs的距離ldb成線性關(guān)系。當(dāng)ldb＝ldbset時(shí)，速度udp＝0，這說明在路徑跟蹤模式下，dvs不會(huì)超出以實(shí)船為中心ldbset為半徑的圓范圍內(nèi)。該設(shè)定保證了在路徑跟蹤模式下，不會(huì)出現(xiàn)因?qū)嵈嚯xdvs位置過遠(yuǎn)而產(chǎn)生的執(zhí)行器輸入過大，該設(shè)計(jì)考慮了執(zhí)行器的飽和特性。由以上論述可知，改進(jìn)dvs制導(dǎo)算法中的dvs是獨(dú)立的，可根據(jù)其相對(duì)于障礙目標(biāo)的位置實(shí)時(shí)調(diào)整dvs姿態(tài)命令以實(shí)現(xiàn)避碰/避障引導(dǎo)。s3：在采樣時(shí)間點(diǎn)進(jìn)行采樣，判斷動(dòng)態(tài)虛擬小船是否進(jìn)入障礙物探測(cè)環(huán)，如進(jìn)入障礙物探測(cè)環(huán)，進(jìn)入障礙物排序過程，執(zhí)行s4；如未進(jìn)入障礙物探測(cè)環(huán)，保持路徑跟蹤制導(dǎo)模式，執(zhí)行s6。s4：進(jìn)入障礙物排序過程：對(duì)避障物的優(yōu)先級(jí)進(jìn)行排序，得到障礙物的優(yōu)先級(jí)序列，選取最高優(yōu)先級(jí)障礙物作為當(dāng)前避障目標(biāo)，判斷當(dāng)前避障目標(biāo)是否滿足避障操縱條件，若當(dāng)前避障目標(biāo)滿足避障操縱條件，則進(jìn)入對(duì)當(dāng)前障礙物的避障制導(dǎo)模式，即執(zhí)行s5；若當(dāng)前避障目標(biāo)不滿足避障操縱條件，選取次優(yōu)先級(jí)的障礙物作為當(dāng)前避障目標(biāo)；若所有障礙物都不滿足避障操縱條件，則進(jìn)入路徑跟蹤模式，執(zhí)行s6。s5：進(jìn)入避障制導(dǎo)模式：在每一采樣時(shí)間點(diǎn)，判斷當(dāng)前障礙物的避障操縱條件是否滿足，如滿足避障操縱條件，則啟動(dòng)對(duì)當(dāng)前障礙物的避障制導(dǎo)模式，獲取動(dòng)態(tài)虛擬小船的導(dǎo)引變量，引導(dǎo)實(shí)船；如不滿足避障操縱條件，則從障礙物優(yōu)先級(jí)序列中去除當(dāng)前障礙物，并執(zhí)行s4。s6：根據(jù)動(dòng)態(tài)虛擬小船的導(dǎo)引變量和控制策略，實(shí)時(shí)調(diào)整實(shí)船的執(zhí)行器輸入，控制實(shí)船跟蹤動(dòng)態(tài)虛擬小船。在上述過程中，首先對(duì)dvs是否進(jìn)入障礙物探測(cè)環(huán)做一個(gè)判斷。如果進(jìn)入，則需由路徑跟蹤制導(dǎo)模式改為避障制導(dǎo)模式，在避障制導(dǎo)模式中，根據(jù)障礙物的種類和個(gè)數(shù)對(duì)障礙物進(jìn)行避障優(yōu)先級(jí)的排序，依次完成避障任務(wù)。為便于避障優(yōu)先級(jí)的排序，本發(fā)明將避障任務(wù)分成如圖5～圖7所示的三種避障態(tài)勢(shì)：分別為多靜態(tài)目標(biāo)避障態(tài)勢(shì)、多動(dòng)態(tài)目標(biāo)避障態(tài)勢(shì)和混合多目標(biāo)避障態(tài)勢(shì)。所謂多目標(biāo)避障態(tài)勢(shì)是指dvs處于多個(gè)障礙物的探測(cè)環(huán)內(nèi)。因?yàn)榭刂扑惴〞?huì)保證實(shí)船快速收斂到dvs上，所以假設(shè)dvs擁有障礙物的探測(cè)能力是合理的。本發(fā)明通過分配障礙物的優(yōu)先級(jí)逐一對(duì)多個(gè)障礙物進(jìn)行避障，當(dāng)對(duì)優(yōu)先級(jí)最高的障礙物進(jìn)行避障時(shí)，不考慮其他障礙物。因?yàn)楸局茖?dǎo)策略可以保證dvs快速收斂到障礙物的安全極限環(huán)(如圖8所示具有半徑ro的實(shí)線圓)上，而障礙物之間(諸如多艘船舶之間，船舶與島礁之間)也存在安全距離，即障礙物的安全極限環(huán)之間不會(huì)相交，所以當(dāng)dvs對(duì)某個(gè)障礙物進(jìn)行避障時(shí)，不會(huì)受到其他障礙物的干擾。圖8中所示的障礙物探測(cè)環(huán)的半徑rm和安全極限環(huán)的半徑ro根據(jù)障礙物的大小確定，障礙物越大，其值越大。實(shí)際上，障礙物探測(cè)環(huán)和安全極限環(huán)是實(shí)船探測(cè)能力的一種體現(xiàn)。本發(fā)明規(guī)定多靜態(tài)避障根據(jù)圖8中所示dvs距離安全極限環(huán)的距離e的大小確定靜態(tài)障礙物的優(yōu)先級(jí)，e最小的障礙物擁有最高的優(yōu)先級(jí)，依次類推；對(duì)于多動(dòng)態(tài)避障，則根據(jù)評(píng)價(jià)函數(shù)的最大化原則決定優(yōu)先級(jí)，評(píng)價(jià)函數(shù)的公式如下：其中，f為評(píng)價(jià)函數(shù)，rm為障礙物探測(cè)環(huán)的半徑，ro為安全極限環(huán)的半徑，e為動(dòng)態(tài)虛擬小船到該動(dòng)態(tài)障礙物的安全極限環(huán)的距離，為動(dòng)態(tài)虛擬小船在路徑跟蹤模式下其與障礙物距離的導(dǎo)數(shù)，udo表示動(dòng)態(tài)虛擬小船的避障速度，為一個(gè)常數(shù)，其值要大于所有動(dòng)態(tài)障礙物的速度，以保證避障的有效性，同時(shí)滿足實(shí)船執(zhí)行器性能的要求；為權(quán)重參數(shù)，其值越大，則表示在評(píng)價(jià)函數(shù)中dvs與障礙物的距離越重要，dvs對(duì)障礙物的相對(duì)速度越不重要，反之亦然。對(duì)于同時(shí)存在靜態(tài)和動(dòng)態(tài)障礙物的情況，該制導(dǎo)策略規(guī)定靜態(tài)障礙物相比動(dòng)態(tài)具有更高的優(yōu)先級(jí)，在對(duì)全部靜態(tài)障礙物的優(yōu)先級(jí)進(jìn)行排序后，再對(duì)動(dòng)態(tài)障礙物的優(yōu)先級(jí)進(jìn)行排序。在確定障礙物優(yōu)先級(jí)順序之后，則根據(jù)避障操縱條件是否滿足決定是否啟動(dòng)避障操縱模式。如圖8所示，對(duì)于靜態(tài)障礙物，其避障操縱條件為dvs的艏向在切線l1和l2之間；對(duì)于動(dòng)態(tài)障礙物，其避障操縱條件如下式所示：其中，σ為動(dòng)態(tài)虛擬小船到動(dòng)態(tài)障礙物的距離，ro為動(dòng)態(tài)障礙物的極限安全環(huán)的半徑，rm為動(dòng)態(tài)障礙物的探測(cè)環(huán)的半徑，為路徑跟蹤模式下動(dòng)態(tài)虛擬小船到動(dòng)態(tài)障礙物距離的導(dǎo)數(shù)。即，當(dāng)動(dòng)態(tài)虛擬小船進(jìn)入動(dòng)態(tài)障礙物的安全極限環(huán)內(nèi)時(shí)，說明馬上需要避障，否則會(huì)發(fā)生碰撞；當(dāng)動(dòng)態(tài)虛擬小船進(jìn)入探測(cè)環(huán)內(nèi)并且路徑跟蹤模式下動(dòng)態(tài)虛擬小船到動(dòng)態(tài)障礙物距離的導(dǎo)數(shù)小于0時(shí)，說明有碰撞的危險(xiǎn)，需要采取措施避免碰撞。如果較高優(yōu)先級(jí)的障礙物不滿足避障操縱條件，則算法轉(zhuǎn)而尋找下一優(yōu)先級(jí)的避障操縱條件；如果所有障礙物都不滿足避障操縱條件，則算法保持路徑跟蹤模式不變；如果當(dāng)前優(yōu)先級(jí)障礙物滿足避障操縱條件，則啟動(dòng)避障操縱模式。啟動(dòng)避障操縱模式后的每一采樣時(shí)間點(diǎn)，制導(dǎo)算法不再對(duì)障礙物優(yōu)先級(jí)進(jìn)行排序，只判斷當(dāng)前障礙物的避障操縱條件是否依舊滿足，若不滿足，則退出避障操縱模式，判斷dvs與障礙物的相對(duì)位置，若dvs處于障礙物探測(cè)環(huán)內(nèi)，則重新開始優(yōu)先級(jí)排序，否則啟動(dòng)路徑跟蹤模式。在避障操縱模式下，首先dvs速度ud會(huì)調(diào)整為目標(biāo)避障速度udo。對(duì)靜態(tài)障礙目標(biāo)的避障dvs艏向角選取為下式：其中，ψdo為動(dòng)態(tài)虛擬小船在避障制導(dǎo)模式中的艏向角，φ為靜態(tài)障礙物相對(duì)動(dòng)態(tài)虛擬小船的真方位角，δ為設(shè)定的前向距離，其值越小，dvs對(duì)極限環(huán)的收斂速度越快，由此可能造成實(shí)船轉(zhuǎn)向幅度較大，所以應(yīng)當(dāng)根據(jù)實(shí)船轉(zhuǎn)向能力靈活選取。λ＝±1決定環(huán)繞靜態(tài)障礙物的方向，+1為順時(shí)針環(huán)繞靜態(tài)障礙物的安全極限環(huán)，-1為逆時(shí)針環(huán)繞靜態(tài)障礙物的安全極限環(huán)。對(duì)動(dòng)態(tài)障礙目標(biāo)的避障dvs艏向角選取為：其中，φ為動(dòng)態(tài)障礙物相對(duì)動(dòng)態(tài)虛擬小船的真方位角，δ為設(shè)定的前向距離，k為補(bǔ)償參數(shù)，其定義同公式(6)，v0＝uccos(π-φ+θ)，uc為當(dāng)前動(dòng)態(tài)障礙物的速度，b＝-2ev02，c＝-(δ2+e2)v02。λ＝±1，+1為順時(shí)針環(huán)繞動(dòng)態(tài)障礙物的安全極限環(huán)，-1為逆時(shí)針環(huán)繞動(dòng)態(tài)障礙物的安全極限環(huán)。當(dāng)動(dòng)態(tài)障礙物為單船時(shí)，λ的選取應(yīng)滿足《國際海上避碰規(guī)則》(colregs)的要求，當(dāng)動(dòng)態(tài)障礙物為其它，例如漂浮物等，按照以下原則選?。浩渲?，ψdp為導(dǎo)引虛擬小船相對(duì)于動(dòng)態(tài)虛擬小船的真方位角，為動(dòng)態(tài)障礙物相對(duì)動(dòng)態(tài)虛擬小船的真方位角。即當(dāng)導(dǎo)引虛擬小船相對(duì)于動(dòng)態(tài)虛擬小船的真方位角小于等于動(dòng)態(tài)障礙物相對(duì)動(dòng)態(tài)虛擬小船的真方位角時(shí)，則λ＝+1，順時(shí)針環(huán)繞動(dòng)態(tài)障礙物；當(dāng)導(dǎo)引虛擬小船相對(duì)于動(dòng)態(tài)虛擬小船的真方位角小于等于動(dòng)態(tài)障礙物相對(duì)動(dòng)態(tài)虛擬小船的真方位角時(shí)，則λ＝-1，逆時(shí)針環(huán)繞動(dòng)態(tài)障礙物。式(14)的障礙物環(huán)繞方向選取原則保證了對(duì)障礙物的最小環(huán)繞長度，減小了航程浪費(fèi)。為了保證dvs制導(dǎo)變量(ud,ψd)在避障操縱和路徑跟蹤兩種模式切換時(shí)以及不同障礙物避障之間的切換時(shí)的光滑性，引入如式(15)所示的時(shí)間過渡函數(shù)：其中，α(t)為時(shí)間過渡函數(shù)，tc為起始切換時(shí)間點(diǎn)，ts為手動(dòng)設(shè)定的過渡時(shí)間，其值的選取應(yīng)在不影響避障安全性能的前提下盡可能地保證過渡平緩。以ψd為例，其切換過程如式(16)所示：ψd(t)＝(1-α)ψd_start+αψd_end(16)其中，ψd將在ts時(shí)間內(nèi)完成從切換起始艏向角ψd_start到目標(biāo)模式艏向角ψd_end的過渡，其過渡過程是光滑的。對(duì)于ud而言，因?yàn)槟繕?biāo)避障速度udo是人為設(shè)定的，且對(duì)于所用障礙物都不變，所以u(píng)d的過渡僅有udp→udo,udo→udp兩種情況。s7：測(cè)量實(shí)船位置，判斷是否到達(dá)終點(diǎn)，如果是，結(jié)束船舶航行，如果否，則執(zhí)行s2。綜上所述，本發(fā)明將制導(dǎo)過程分為路徑跟蹤和避障操縱制導(dǎo)兩種模式，以改進(jìn)dvs制導(dǎo)算法為基本構(gòu)架，由gvs動(dòng)態(tài)規(guī)劃出由直線和曲線組成的光滑參考路徑，路徑跟蹤模式和避障操縱模式分別對(duì)應(yīng)不同的dvs的引導(dǎo)變量。對(duì)于多個(gè)或混合的障礙物的避障制導(dǎo)，首先將根據(jù)優(yōu)先級(jí)排序及避障操縱條件評(píng)判確定當(dāng)前的避障目標(biāo)，啟動(dòng)避障操縱模式，過渡函數(shù)保證dvs引導(dǎo)變量在不同模式切換之間的光滑性。本發(fā)明還可以對(duì)多種控制策略具有適用性，便于同現(xiàn)有先進(jìn)控制算法結(jié)合，控制算法的作用在于保證實(shí)船對(duì)dvs的收斂，保證制導(dǎo)策略的有效性。為了驗(yàn)證本發(fā)明所提出制導(dǎo)算法的有效性，這部分將以大連海事大學(xué)教學(xué)實(shí)習(xí)船“育鯤”(如圖10)為被控對(duì)象，利用matlab開展計(jì)算機(jī)仿真實(shí)驗(yàn)。表1給出了“育鯤”輪的主要尺度參數(shù)。這里采用三自由度欠驅(qū)動(dòng)數(shù)學(xué)模型(17)，其相關(guān)水動(dòng)力系數(shù)基于2013年9月“育鯤”輪開展的系列實(shí)船操縱性試驗(yàn)，利用先進(jìn)系統(tǒng)辨識(shí)算法獲得。表1.“育鯤”輪主要參數(shù)其中高階流體動(dòng)力項(xiàng)表達(dá)式為其中，p為螺距，δ為舵角，作為控制系統(tǒng)的輸入。該實(shí)例中，計(jì)劃航線由4個(gè)航路點(diǎn)w1(200,0)，w2(200,1000)，w3(1200,1500)，w4(1200,2500)確定。在計(jì)劃航線附近設(shè)置多靜態(tài)障礙物、多動(dòng)態(tài)障礙物以及動(dòng)靜混合障礙物，模擬船舶在穿越多島礁水域和航運(yùn)繁忙水域的情況。船舶初始狀態(tài)為[x,y,ψ,u,v,r]t＝0＝[0m,0m,90deg,0m/s,0m/s,0deg/s]，制導(dǎo)算法參數(shù)設(shè)置為：ug＝10kn，udo＝12kn，ldbset＝200m，kd＝0.05，δ＝20m，ts＝2s為了更加貼近真實(shí)環(huán)境，仿真中考慮了風(fēng)、浪、流的海洋環(huán)境干擾?？刂扑惴敯羯窠?jīng)阻尼控制律，這一應(yīng)用體現(xiàn)了本發(fā)明與先進(jìn)控制算法的良好結(jié)合。仿真實(shí)驗(yàn)所使用環(huán)境干擾為：風(fēng)速(蒲福風(fēng)6級(jí))vwind＝15.25m/s，風(fēng)向ψwind＝50deg；海浪干擾由風(fēng)干擾模型耦合產(chǎn)生，即為在蒲福風(fēng)7級(jí)情況下充分成長生成的海浪，圖11給出了試驗(yàn)海浪干擾的三維視圖；海流vcurrent＝0.5m/s，流向βcurrent＝280deg。圖12～圖15給出了在上述實(shí)驗(yàn)條件下，4個(gè)時(shí)間點(diǎn)采用本發(fā)明的制導(dǎo)策略得到的實(shí)船軌跡圖?？梢钥闯鲈撝茖?dǎo)和控制策略保證了實(shí)船在避障時(shí)有效地收斂到障礙物的安全極限環(huán)上，避障結(jié)束后快速回歸到路徑跟蹤模式上，單船避障滿足colregs的要求，多船避障按照優(yōu)先級(jí)順序執(zhí)行避障任務(wù)。圖16給出了制導(dǎo)策略得到的引導(dǎo)變量在整個(gè)實(shí)驗(yàn)過程中隨時(shí)間的變化，可以看出制導(dǎo)策略中過渡函數(shù)的引入保證了模式切換之間引導(dǎo)變量光滑的過渡。圖17給出了實(shí)驗(yàn)過程中的控制輸入—螺距p和舵角δ，控制輸入滿足執(zhí)行器有界的要求，抖振較小，可以直接作為執(zhí)行器的輸入，貼近工程實(shí)際。通過上述仿真實(shí)驗(yàn)，并同已有研究進(jìn)行對(duì)比，現(xiàn)將本發(fā)明帶來的有益效果總結(jié)為以下3點(diǎn)：1)本發(fā)明提出了一種適用于存在多靜態(tài)目標(biāo)、多動(dòng)態(tài)目標(biāo)和混合多目標(biāo)障礙物的復(fù)雜航行態(tài)勢(shì)的改進(jìn)dvs制導(dǎo)技術(shù)，適用范圍更廣。所提出的制導(dǎo)算法創(chuàng)造性地將船舶路徑跟蹤制導(dǎo)同混合避障制導(dǎo)結(jié)合。本策略下的混合避障制導(dǎo)機(jī)制具有多靜態(tài)障礙物避障、多動(dòng)態(tài)障礙物避障和混合多目標(biāo)障礙物避障的能力，在結(jié)束避障任務(wù)后，船舶可以迅速回歸到路徑跟蹤任務(wù)上，其適用范圍更廣。2)基于極限環(huán)的避障策略可以確保船舶快速地收斂到障礙物的安全極限環(huán)上，安全性能較高。該算法中，通過合理設(shè)計(jì)避障目標(biāo)的安全極限環(huán)和探測(cè)環(huán)(由船載設(shè)備的測(cè)量能力決定，如雷達(dá)、攝像機(jī)等)可以靈活調(diào)整船舶避障操縱的安全域度和避障操縱后恢復(fù)航線的收斂速度。實(shí)際工程中，避障目標(biāo)的安全極限環(huán)和探測(cè)環(huán)是實(shí)船探測(cè)能力的一種體現(xiàn)，實(shí)船進(jìn)入障礙物探測(cè)環(huán)的過程等效于障礙物進(jìn)入實(shí)船探測(cè)范圍的過程，故本制導(dǎo)算法便于工程實(shí)現(xiàn)。3)本發(fā)明提出的制導(dǎo)策略繼承了dvs制導(dǎo)策略的優(yōu)點(diǎn)，能夠在直線段、曲線段航線以及多種船舶航行態(tài)勢(shì)下提供合理有效的制導(dǎo)機(jī)制，引導(dǎo)船舶完成智能航行任務(wù)；該策略具有控制理論的通用性，為先進(jìn)控制算法和工程實(shí)踐的結(jié)合搭建了橋梁；該策略考慮了船舶執(zhí)行裝置的限制，符合“綠色、節(jié)能”的主題。以上所述，僅為本發(fā)明較佳的具體實(shí)施方式，但本發(fā)明的保護(hù)范圍并不局限于此，任何熟悉本
技術(shù)領(lǐng)域：
：的技術(shù)人員在本發(fā)明揭露的技術(shù)范圍內(nèi)，根據(jù)本發(fā)明的技術(shù)方案及其發(fā)明構(gòu)思加以等同替換或改變，都應(yīng)涵蓋在本發(fā)明的保護(hù)范圍之內(nèi)。當(dāng)前第1頁12當(dāng)前第1頁12