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