本發(fā)明涉及船舶控制工程與船舶自動化航行裝備應(yīng)用領(lǐng)域，尤其是涉及一種路徑跟蹤避障制導(dǎo)算法。

背景技術(shù)：

制導(dǎo)算法是解決船舶運動航跡保持問題的必不可少的技術(shù)，傳統(tǒng)的基于航路點的船舶路徑規(guī)劃，要求船舶沿著設(shè)定航線自動航行，歸結(jié)于路徑跟蹤的制導(dǎo)問題^[1]。然而，海洋環(huán)境具有多變性，船舶在航行過程中經(jīng)常會發(fā)生避障行為，尤其在穿越漁區(qū)和途徑狹窄或繁忙水道時，這就要求船舶制導(dǎo)算法具有避障功能。避障算法，起源于機(jī)器人控制的研究，常用的方法有勢場法，速度避障法和動態(tài)窗法，其中動態(tài)窗法將船舶前進(jìn)速度和艏搖速度在采樣周內(nèi)可達(dá)到的范圍以動態(tài)窗的形式進(jìn)行描述，尤其適合解決高速無人船的多目標(biāo)避障問題，下面對文獻(xiàn)[2]提出的基于動態(tài)窗的避障算法進(jìn)行技術(shù)特征的簡要介紹。

該算法在對設(shè)定航路點進(jìn)行追蹤的同時，對探測范圍內(nèi)的多個目標(biāo)進(jìn)行避障。要求已知船舶在采樣周期ts內(nèi)前進(jìn)速度的動態(tài)窗口vt和艏向角θusv的動態(tài)窗口vhead。按照式(8)的最大化原則選取最優(yōu)艏向角

fe(θ)＝ε·fhead(θ)+(1-ε)·fsafe(θ)(8)

其中，θgoal是航路點對于實船的真方位角，bobs是探測半徑dnear內(nèi)障礙物造成的不可行真方位角的集合，αi是探測半徑內(nèi)障礙物中心對于實船的真方位角，ε是可調(diào)整參數(shù)。根據(jù)式(11)插入當(dāng)前制導(dǎo)坐標(biāo)點pinsert，dinsert為一個小于探測半徑的距離。

進(jìn)而通過式(12)獲得當(dāng)前制導(dǎo)艏向角作為當(dāng)前艏向角的跟蹤目標(biāo)。

定義按照式(13),(14)定義當(dāng)前制導(dǎo)速度。

因為該避障算法僅考慮實船相對方位角范圍內(nèi)的障礙物，所以給出的定義如式(15)所示。

η1的定義如式(16)所示。是當(dāng)前轉(zhuǎn)艏角速度。

為了防止速度過大造成碰撞，實船速度應(yīng)當(dāng)滿足式(17)，如圖1所示。

化簡可得：

則vmax應(yīng)當(dāng)滿足式(19)，rmax是探測范圍內(nèi)最大障礙物的半徑。η2應(yīng)當(dāng)滿足式(20)。

通過以下流程實時獲取船舶目標(biāo)艏搖角和目標(biāo)速度對實船進(jìn)行避障制導(dǎo)。最終可以獲得如圖3所示的避障效果。

現(xiàn)有面向大型船舶制導(dǎo)算法的研究多是針對路徑跟蹤提出。其中，los制導(dǎo)算法^[3]發(fā)展較為成熟且被廣泛應(yīng)用，然而卻存在無法對轉(zhuǎn)向部分的曲線段進(jìn)行航跡保持，以及難以進(jìn)行路徑規(guī)劃等缺點。dvs制導(dǎo)算法^[4]通過導(dǎo)引虛擬小船實現(xiàn)直線段及曲線段的路徑規(guī)劃，從根本上克服了los制導(dǎo)算法的缺點，通過生成動態(tài)虛擬小船作為直接跟蹤目標(biāo)，避免了執(zhí)行器輸入過大。然而，基于動態(tài)虛擬小船的生成方式，傳統(tǒng)dvs制導(dǎo)算法難以同避障算法相結(jié)合。

在航海領(lǐng)域，避障算法的研究目前主要針對小型無人艇，常用的避障算法有動態(tài)窗算法^[2]，速度避障法^[5]，勢場避障法^[6]等。其中動態(tài)窗算法僅考慮了船舶的前進(jìn)和艏搖運動，沒有考慮橫蕩運動，故不適應(yīng)于考慮欠驅(qū)動特性的大型船舶。速度避障法有計算簡便的數(shù)學(xué)特性，卻僅需要假設(shè)船舶運動速度是線性的，且難以同路徑跟蹤制導(dǎo)算法結(jié)合。勢場法需要對環(huán)境提前預(yù)知，且在通過狹窄通道時極易失去穩(wěn)定，造成振蕩，故不適用于具有多變性的真實海況。

在過去的研究中，避障算法和路徑跟蹤制導(dǎo)算法的研究往往是分開的。從圖3可以看出，避障算法往往僅考慮對航路點的跟蹤，對于大型商船而言這是不切實際的。同時，大型商船在進(jìn)行路徑跟蹤時，經(jīng)常會遇到作業(yè)中的小型漁船群，島嶼礁石等一定程度上不可預(yù)測的靜止或慢時變目標(biāo)。一種能夠進(jìn)行路徑跟蹤同時考慮多目標(biāo)避障的制導(dǎo)算法成為亟待解決的研究點。文獻(xiàn)[7]和[8]分別基于los制導(dǎo)算法和serret-frenet框架提出了一種考慮單目標(biāo)避障的路徑跟蹤制導(dǎo)算法，該算法對多目標(biāo)障礙物無效且仍然沒有對曲線段進(jìn)行路徑規(guī)劃的能力。文獻(xiàn)[9]基于龍格庫塔法提出了一種具有路徑規(guī)劃和多目標(biāo)避障能力的制導(dǎo)算法，卻無法適用于具有欠驅(qū)動特性的大型商船。

技術(shù)實現(xiàn)要素：

鑒于已有技術(shù)存在的缺陷，本發(fā)明公開了一種路徑跟蹤避障制導(dǎo)方法，包括以下步驟：

一種路徑跟蹤避障制導(dǎo)方法，包括以下步驟：

(1)執(zhí)行路徑規(guī)劃并獲取導(dǎo)引虛擬小船的時間序列信號；

(2)根據(jù)動態(tài)虛擬小船和導(dǎo)引虛擬小船的位置信息，計算動態(tài)虛擬小船的路徑跟蹤艏向角ψdp和路徑跟蹤前進(jìn)速度udp；

(3)實時監(jiān)測動態(tài)虛擬小船到障礙物安全圓族邊界的最小距離lmin，并根據(jù)實時監(jiān)測的最小距離lmin確定動態(tài)虛擬小船的最終艏向角ψd；

(4)計算動態(tài)虛擬小船的最終前進(jìn)速度ud和轉(zhuǎn)艏角速度rd；

(5)判斷動態(tài)虛擬小船與導(dǎo)引虛擬小船間的距離ldr與設(shè)定的閾值距離ldrset的大小，如果動態(tài)虛擬小船與導(dǎo)引虛擬小船的距離ldr大于等于閾值距離ldrset，則導(dǎo)引虛擬小船下一時間點的運動狀態(tài)保持當(dāng)前狀態(tài)不變；如果動態(tài)虛擬小船與導(dǎo)引虛擬小船的距離ldr小于閾值距離ldrset，則導(dǎo)引虛擬小船下一時間點運動狀態(tài)信息按照其時間序列的排序下移；

(6)將制導(dǎo)信息輸入到控制回路中并更新動態(tài)虛擬小船下一時間點的位置信息；

(7)測量實船位置，判斷是否到達(dá)目的地，如果“是”，結(jié)束船舶航行；如果“否”，時間更新t＝t+1并進(jìn)入步驟(2)。

進(jìn)一步地，在步驟(2)中所述動態(tài)虛擬小船的路徑跟蹤艏向角ψdp采用公式(2)進(jìn)行計算：

其中，(xr，yr)，(xd，yd)分別為導(dǎo)引虛擬小船和動態(tài)虛擬小船的坐標(biāo)；

所述動態(tài)虛擬小船的路徑跟蹤前進(jìn)速度udp采用公式(3)進(jìn)行計算：

udp＝klldr+urcos(ψdp-ψr)(3)

其中l(wèi)dr為動態(tài)虛擬小船與導(dǎo)引虛擬小船間的距離，kl為預(yù)設(shè)參數(shù)，其數(shù)值大小是根據(jù)動態(tài)虛擬小船在路徑跟蹤情況下對導(dǎo)引虛擬小船位置的收斂速率進(jìn)行設(shè)置，其值越大，收斂越快，但考慮到執(zhí)行器的限制，應(yīng)當(dāng)適當(dāng)選取。

進(jìn)一步地，在步驟(3)中所述根據(jù)實時監(jiān)測的動態(tài)虛擬小船到障礙物安全圓族邊界的最小距離lmin確定動態(tài)虛擬小船的最終艏向角ψd包括：

1)當(dāng)動態(tài)虛擬小船到障礙物安全圓族邊界的最小距離lmin小于等于船舶障礙物探測設(shè)備的激活圈半徑ractive時，執(zhí)行避障算法以確定最終艏向角ψd：

避障算法原理為：根據(jù)船舶上障礙物探測設(shè)備(如導(dǎo)航雷達(dá)，攝像設(shè)備等)的作用半徑確定避障算法的探測圈半徑rtest和激活圈半徑ractive，當(dāng)障礙物進(jìn)入探測圈時，在探測到的障礙物的邊緣生成半徑為rs的安全圓族，此時若安全圓族部分或全部進(jìn)入激活圈，從路徑跟蹤制導(dǎo)算法給出的動態(tài)虛擬小船路徑跟蹤艏向角ψdp開始，向兩側(cè)搜索能夠避免與圓族相交的最近的艏向角和

按照評價函數(shù)(4)的最小化原則從兩個艏向角和中選取最優(yōu)值作為制導(dǎo)算法中動態(tài)虛擬小船的最終艏向角ψd：

其中i＝1,2，ε是自定義的權(quán)重值，ψd(j-1)代表上一個采樣時間點的最終艏向角；

2)當(dāng)動態(tài)虛擬小船到障礙物安全圓族邊界的最小距離lmin大于船舶中障礙物探測設(shè)備的激活圈半徑ractive時，動態(tài)虛擬小船的最終艏向角ψd＝ψdp。

進(jìn)一步地，在所述步驟(4)中，所述動態(tài)虛擬小船的最終前進(jìn)速度ud采用公式(5)(6)進(jìn)行計算：

其中，lmin為障礙物安全圓族邊界到動態(tài)虛擬小船的最小距離，rc∈[0,ractive/2]，為手動設(shè)置的一個閾值，udo為手動設(shè)置的避障過程的最小值，ldbset為手動設(shè)置的實船到動態(tài)虛擬小船間距離上界，ldb為實船距離動態(tài)虛擬小船的實際距離，u′dp值計算過程如下：首先設(shè)置動態(tài)虛擬小船路徑跟蹤速度最大值udpmax，按照公式(3)計算動態(tài)虛擬小船路徑跟蹤速度udp，若udp≥udpmax，則保持u′dp＝udpmax，否則令u′dp＝udp；

所述動態(tài)虛擬小船的轉(zhuǎn)艏角速度rd采用如公式(7)所示的1階濾波器進(jìn)行估計：

其中，βψd為動態(tài)虛擬小船艏向角ψd的估計值，τ為設(shè)定參數(shù)。

本發(fā)明具有以下有益效果：

1)本發(fā)明的制導(dǎo)算法解決了船舶在路徑跟蹤過程中對多目標(biāo)進(jìn)行避障的現(xiàn)實問題，提高了船舶的自動化程度?；诰徒瓌t的避障算法簡單易于實現(xiàn)，避免了航程的浪費且保證了航行安全。

2)本發(fā)明繼承了傳統(tǒng)dvs算法的優(yōu)點，解決了“l(fā)os算法無法進(jìn)行曲線段路徑規(guī)劃”的問題，適用于高精度航跡保持控制中，同時，易于同欠驅(qū)動問題的先進(jìn)控制算法結(jié)合?？紤]船舶執(zhí)行裝置的限制，本發(fā)明進(jìn)行了dvs的速度規(guī)劃，相比傳統(tǒng)dvs制導(dǎo)算法，具有異曲同工之妙。

附圖說明

圖1是現(xiàn)有技術(shù)中最小半徑限制原理圖；

圖2是現(xiàn)有技術(shù)中基于動態(tài)窗的避障算法執(zhí)行流程圖；

圖3是現(xiàn)有技術(shù)中基于動態(tài)窗的避障算法2維效果圖；

圖4是本發(fā)明的船舶路徑跟蹤控制邏輯結(jié)構(gòu)圖；

圖5是現(xiàn)有技術(shù)中導(dǎo)引虛擬小船的航路點處弧線路徑規(guī)劃示意圖；

圖6是本發(fā)明改進(jìn)的動態(tài)虛擬小船制導(dǎo)算法基本原理；

圖7是本發(fā)明的避障算法基本原理；

圖8是本發(fā)明中u′d速度規(guī)劃示意圖；

圖9是本發(fā)明跟蹤避障制導(dǎo)算法執(zhí)行流程圖；

圖10為本發(fā)明實施例的船舶示意圖；

圖11為實施例中實驗海浪模型干擾三維視圖；

圖12為實施例中模擬真實海洋環(huán)境下路徑跟蹤避障2-d效果圖；

圖13為實施例中船舶運動姿態(tài)變量u,v,ψ時間變化曲線示意圖；

圖14為實施例中執(zhí)行裝置控制輸入n,δ時間變化曲線示意圖。

具體實施方式

為使本發(fā)明的技術(shù)方案和優(yōu)點更加清楚，下面結(jié)合本發(fā)明實施例中的附圖，對本發(fā)明實施例中的技術(shù)方案進(jìn)行清楚完整的描述。

如圖4和圖9所示為本發(fā)明公開的路徑跟蹤避障制導(dǎo)算法的船舶路徑跟蹤控制邏輯結(jié)構(gòu)圖和跟蹤避障制導(dǎo)算法執(zhí)行流程圖(圖中g(shù)vs表示導(dǎo)引虛擬小船，dvs表示動態(tài)虛擬小船)。如圖4所示船舶的整個執(zhí)行模塊分為制導(dǎo)系統(tǒng)和控制系統(tǒng)兩部分，制導(dǎo)模塊采用本發(fā)明的提出的路徑跟蹤避障制導(dǎo)算法。

如圖9所示本發(fā)明提出的路徑跟蹤避障制導(dǎo)算法具體步驟如下：

(1)執(zhí)行路徑規(guī)劃并獲取導(dǎo)引虛擬小船的時間序列信號。

根據(jù)駕駛員設(shè)定的航路點位置信息計算出船舶的路徑跟蹤參考路徑，根據(jù)導(dǎo)引虛擬小船的運動學(xué)特性(1)，獲取導(dǎo)引虛擬小船運動狀態(tài)信息的時間序列信號(導(dǎo)引虛擬小船的艏向角ψr、前進(jìn)速度ur、位置坐標(biāo)(xr,yr)按照時間順序的排列)；

如圖6所示從左至右依次為實船、動態(tài)虛擬小船和導(dǎo)引虛擬小船。導(dǎo)引虛擬小船和動態(tài)虛擬小船均具有如公式(1)的運動學(xué)特性：

其中(xr，yr)、(xd，yd)分別為導(dǎo)引虛擬小船和動態(tài)虛擬小船的坐標(biāo)，ur、ud分別為導(dǎo)引虛擬小船的前進(jìn)速度和動態(tài)虛擬小船的最終前進(jìn)速度，ψr、ψd分別為導(dǎo)引虛擬小船的艏向角和動態(tài)虛擬小船的最終艏向角。導(dǎo)引虛擬小船的前進(jìn)速度ur和艏向角ψr與現(xiàn)有技術(shù)中動態(tài)虛擬小船路徑跟蹤制導(dǎo)算法的計算方法相同。

選取導(dǎo)引虛擬小船運動狀態(tài)信息時間序列的第一組數(shù)據(jù)作為導(dǎo)引虛擬小船初始狀態(tài)信息；實船所有運動狀態(tài)信息可以通過船用導(dǎo)航設(shè)備(gps、計程儀等)測得；對動態(tài)虛擬小船的位置信息進(jìn)行設(shè)定，其位置處于實船和導(dǎo)引虛擬小船的連線中點上，其全部運動狀態(tài)信息可通過下述步驟補(bǔ)全。

(2)根據(jù)動態(tài)虛擬小船和導(dǎo)引虛擬小船的當(dāng)前位置信息，計算動態(tài)虛擬小船的路徑跟蹤艏向角ψdp和路徑跟蹤前進(jìn)速度udp。動態(tài)虛擬小船的路徑跟蹤艏向角ψdp采用公式(2)進(jìn)行計算：

動態(tài)虛擬小船的路徑跟蹤前進(jìn)速度udp采用公式(3)進(jìn)行計算：

udp＝klldr+urcos(ψdp-ψr)(3)

其中l(wèi)dr為動態(tài)虛擬小船與導(dǎo)引虛擬小船間的距離，kl為預(yù)設(shè)參數(shù)，其數(shù)值大小是根據(jù)動態(tài)虛擬小船在路徑跟蹤情況下對導(dǎo)引虛擬小船位置的收斂速率進(jìn)行設(shè)置，其值越大，收斂越快，但考慮到執(zhí)行器的限制，應(yīng)當(dāng)適當(dāng)選取。

區(qū)別于傳統(tǒng)動態(tài)虛擬小船路徑跟蹤制導(dǎo)算法，實船相對導(dǎo)引虛擬小船位置不限制動態(tài)虛擬小船的生成。在該算法中，其本身按照(1)所示的運動學(xué)關(guān)系獨立航行，從公式(2)和(3)可知，動態(tài)虛擬小船的路徑跟蹤艏向角ψdp和路徑跟蹤速度udp通過其本身對于導(dǎo)引虛擬小船的關(guān)系生成，其運動過程是連續(xù)的，便于同后續(xù)避障策略結(jié)合。

(3)實時監(jiān)測動態(tài)虛擬小船到障礙物安全圓族邊界的最小距離lmin，并根據(jù)實時監(jiān)測的動態(tài)虛擬小船到障礙物安全圓族邊界的最小距離lmin確定動態(tài)虛擬小船的最終艏向角ψd；

根據(jù)實時監(jiān)測的動態(tài)虛擬小船到障礙物安全圓族邊界的最小距離lmin確定動態(tài)虛擬小船的最終艏向角ψd包括：

1)當(dāng)動態(tài)虛擬小船到障礙物安全圓族邊界的最小距離lmin小于等于船舶障礙物探測設(shè)備的激活圈半徑ractive時，執(zhí)行避障算法以確定最終艏向角ψd：

如圖7所示避障算法原理為：根據(jù)船舶上障礙物探測設(shè)備(如導(dǎo)航雷達(dá)，攝像設(shè)備等)的作用半徑確定避障算法的探測圈半徑rtest和激活圈半徑ractive。因為本制導(dǎo)算法要求通過控制算法保證實船對動態(tài)虛擬小船位置的快速收斂，所以假設(shè)探測圈和激活圈均是以動態(tài)虛擬小船的位置為圓心的。當(dāng)障礙物進(jìn)入動態(tài)虛擬小船探測圈時，在探測到的障礙物的邊緣生成半徑為rs的安全圓族，此時若安全圓族全部或部分進(jìn)入激活圈(lmin≤ractive)，則從路徑跟蹤制導(dǎo)算法給出的動態(tài)虛擬小船路徑跟蹤艏向角ψdp開始，向兩側(cè)搜索能夠避免與圓族相交的最近的艏向角和

按照評價函數(shù)(4)的最小化原則從兩個艏向角和中選取最優(yōu)值作為制導(dǎo)算法中動態(tài)虛擬小船的最終艏向角ψd：

其中i＝1,2，ε是自定義的權(quán)重值，ψd(j-1)代表上一個采樣時間點的最終艏向角；

該評價函數(shù)不僅保證了最終艏向角ψd距離ψdp偏差小，同時具有連續(xù)性，避免了最優(yōu)值來回切換引起的抖振。另外，因為障礙物邊界的圓族是逐漸進(jìn)入激活圈的，所以路徑跟蹤和避障算法的切換是光滑的。

2)當(dāng)動態(tài)虛擬小船到障礙物安全圓族邊界的最小距離lmin大于船舶中障礙物探測設(shè)備的激活圈半徑ractive時，動態(tài)虛擬小船的最終艏向角ψd＝ψdp。當(dāng)障礙物在動態(tài)虛擬小船探測圈外時，理論上lmin是無法測得的，此時定義lmin＝rtest。

(4)計算動態(tài)虛擬小船的最終前進(jìn)速度ud和轉(zhuǎn)艏角速度rd；

在該步驟中，出于安全性的考慮，船舶在遇到障礙物時需要降速，同時，考慮到執(zhí)行器有界性，動態(tài)虛擬小船在距離實船較遠(yuǎn)時，應(yīng)當(dāng)降速等待實船追蹤，本發(fā)明將采用公式(5)和(6)對動態(tài)虛擬小船的最終前進(jìn)速度ud進(jìn)行規(guī)劃：

其中，lmin為上述步驟定義的最小距離，rc∈[0,ractive/2]，為手動設(shè)置的一個閾值，其值的大小決定了過渡區(qū)間(如圖8所示，過渡區(qū)間表示u′dp過渡到udo對應(yīng)的lmin區(qū)間)的大小，為了保證避障的安全性，選取時應(yīng)在保證一定過渡區(qū)間的原則下盡量取大，udo為手動設(shè)置的避障過程的動態(tài)虛擬小船速度最小值，選取時應(yīng)滿足避碰過程中船舶安全性的要求，ldbset為手動設(shè)置的實船到動態(tài)虛擬小船間距離上界，其值的選取要滿足控制算法作用下實船對動態(tài)虛擬小船追蹤的要求，實船對動態(tài)虛擬小船追蹤越快，ldbset可選取越大。ldb為實船距離動態(tài)虛擬小船的實際距離。

公式(6)中的u′d采用公式(5)進(jìn)行規(guī)劃，圖8所示為采用公式(5)對u′d進(jìn)行規(guī)劃的示意圖。其中，公式(5)中的u′dp值計算過程如下：首先設(shè)置動態(tài)虛擬小船路徑跟蹤速度最大值udpmax，按照公式(3)計算動態(tài)虛擬小船路徑跟蹤速度udp，若udp≥udpmax，則保持u′dp＝udpmax，否則令u′dp＝udp。跟據(jù)公式(6)可知當(dāng)ldb等于ldbset時ud＝0，動態(tài)虛擬小船保持原地不動，又知控制回路會保證實船收斂到動態(tài)虛擬小船的位置上，由此保證了動態(tài)虛擬小船距離實船不會超過ldbset。

采用如公式(7)所示的1階濾波器進(jìn)行估計動態(tài)虛擬小船的轉(zhuǎn)艏角速度rd：

其中，βψd為動態(tài)虛擬小船艏向角ψd的估計值，τ為設(shè)定參數(shù)，其值的大小是根據(jù)濾波器的濾波性能和對rd估計的準(zhǔn)確性進(jìn)行設(shè)定，其值越大，濾波性能越好，但估計的準(zhǔn)確性越低，選取時要兩者兼顧。

(5)判斷動態(tài)虛擬小船與導(dǎo)引虛擬小船間的距離ldr與設(shè)定的閾值距離ldrset的大小，如果動態(tài)虛擬小船與導(dǎo)引虛擬小船的距離ldr大于等于閾值距離ldrset，則導(dǎo)引虛擬小船的下一時間點運動狀態(tài)保持當(dāng)前采樣時間點的運動狀態(tài)不變，如果動態(tài)虛擬小船與導(dǎo)引虛擬小船的距離ldr小于閾值距離ldrset，則導(dǎo)引虛擬小船下一時間點的運動狀態(tài)按照其時間序列的排序下移。

在動態(tài)虛擬小船執(zhí)行避障算法的過程中，前進(jìn)速度會下降，而導(dǎo)引虛擬小船的速度始終為ur，如果不對導(dǎo)引虛擬小船加以限制，則會造成動態(tài)虛擬小船距離導(dǎo)引虛擬小船過遠(yuǎn)，影響路徑跟蹤效果。因此，設(shè)置動態(tài)虛擬小船與導(dǎo)引虛擬小船間的閾值距離ldrset，通過限制動態(tài)虛擬小船與導(dǎo)引虛擬小船的距離ldr，可以實現(xiàn)精確路徑跟蹤，當(dāng)ldr≥ldrset時，導(dǎo)引虛擬小船的下一時間點運動狀態(tài)會保持當(dāng)前采樣時間點的狀態(tài)不變，這相當(dāng)于導(dǎo)引虛擬小船會對動態(tài)虛擬小船進(jìn)行等待，避免動態(tài)虛擬小船距離導(dǎo)引虛擬小船過遠(yuǎn)。

(6)將制導(dǎo)信息輸入到控制回路中并更新動態(tài)虛擬小船下一時間點的位置信息。

將動態(tài)虛擬小船的當(dāng)前狀態(tài)變量(最終艏向角ψd，轉(zhuǎn)艏角速度rd，最終速度ud，位置坐標(biāo)xd,yd)作為制導(dǎo)信息代入到控制器中，控制實船收斂到動態(tài)虛擬小船的位置上。根據(jù)動態(tài)虛擬小船的運動學(xué)公式(1)和當(dāng)前狀態(tài)變量信息計算下一時間點的動態(tài)虛擬小船的位置信息。

(7)測量實船位置判斷是否到達(dá)目的地，如果“是”，結(jié)束船舶航行；如果“否”，時間更新t＝t+1并進(jìn)入步驟(2)。

實施例：為了驗證本發(fā)明所提出制導(dǎo)算法的有效性，這部分將以大連海事大學(xué)科研實習(xí)船“育鯤”輪為被控對象(見圖10)，開展系統(tǒng)仿真實驗進(jìn)行算法測試。表1給出了“育鯤”輪主要參數(shù)，其數(shù)學(xué)模型采用式(21)所示的三自由度非線性機(jī)理模型，其水動力系數(shù)基于相關(guān)實船操縱性試驗數(shù)據(jù)進(jìn)行系統(tǒng)辨識獲取，關(guān)于模型辨識建模的理論成果已在國際期刊isatransactions公開發(fā)表，在此不做贅述。

表1.“育鯤”輪主要參數(shù)

其中，高階流體動力項表達(dá)式為

該實驗中，計劃航線由5個航路點w1(200,0),w2(200,600),w3(800,900),w4(800,1500),w5(1400,1800)確定。考慮“育鯤”輪穿過漁船作業(yè)區(qū)的情況，漁船在作業(yè)過程中可以視為靜止或慢時變的目標(biāo)，隨機(jī)在計劃航線附近生成多個障礙物。船舶初始狀態(tài)為[x,y,ψ,u,v,r]t＝0＝[0m,0m,90deg,0m/s,0m/s,0deg/s]，制導(dǎo)算法參數(shù)設(shè)置為：rtest＝130m,ractive＝100m,rs＝10m,ε＝0.6,rc＝30m,udpmax＝20m/s,udo＝4m/s,ldbset＝80m,ldrset＝100m,kl＝0.2,τ＝0.1。為了更加貼近真實環(huán)境，實驗中考慮了風(fēng)、浪、流對控制系統(tǒng)的擾動，相關(guān)干擾機(jī)理模型參見文獻(xiàn)[10]?？刂扑惴ú捎梦墨I(xiàn)[4]提出的魯棒神經(jīng)自適應(yīng)控制律，這一應(yīng)用體現(xiàn)了本發(fā)明與先進(jìn)控制算法的良好結(jié)合。

仿真實驗所使用環(huán)境干擾為：風(fēng)速(蒲福風(fēng)7級)vwind＝15.25m/s，風(fēng)向ψwind＝50deg；海浪干擾由風(fēng)干擾模型耦合產(chǎn)生，即為在蒲福風(fēng)7級情況下充分成長生成的海浪，圖11給出了試驗海浪干擾的三維視圖；海流vcurrent＝0.5m/s，流向βcurrent＝280deg。圖12-14給出了在上述實驗條件下，利用動態(tài)虛擬小船dvs路徑跟蹤避障制導(dǎo)算法得到的結(jié)果。從圖12可以看出，在導(dǎo)引虛擬小船gvs進(jìn)行直線和曲線路徑規(guī)劃的基礎(chǔ)上，動態(tài)虛擬小船dvs能夠有效跟蹤gvs軌跡并靈活地進(jìn)行多障礙物的避障，導(dǎo)引船舶實現(xiàn)有效的航跡保持及避障控制。圖13和圖14分別給出了船舶運動姿態(tài)變量u,v,ψ和控制輸入n,δ的時間變化曲線，可以看出，船舶姿態(tài)變化平穩(wěn)，基于該制導(dǎo)算法的控制輸入符合航海實踐中執(zhí)行裝置的要求，通過推進(jìn)裝置與舵機(jī)的配合，可實現(xiàn)精確的路徑跟蹤及避障控制。

文獻(xiàn)1：張顯庫.船舶運動簡捷魯棒自適應(yīng)控制.北京:科學(xué)出版社,2012.

文獻(xiàn)2：tangp,zhangr,liud,etal.localreactiveobstacleavoidanceapproachforhigh-speedunmannedsurfacevehicle[j].oceanengineering,2015,106:128-140.

文獻(xiàn)3：fossenti,breivikm,andskjetner.lineofsightpathfollowingofunderactuatedmarinecraft[c]//ifacconferenceonmanoeuveringandcontrolofmarinecraft.2003:244-249.

文獻(xiàn)4：guoqingzhang,xiankuzhang.anoveldvsguidanceprincipleandrobustadaptivepath-followingcontrolforunderactuatedshipsusinglowfrequencygain-learning[j].isatransactions,2015,56:75-85.

文獻(xiàn)5：kuwatay,wolfmt,zarzhitskyd,etal.safemaritimeautonomousnavigationwithcolregs,usingvelocityobstacles[j].ieeejournalofoceanicengineering,2014,39:110-119.

文獻(xiàn)6：koreny,borensteinj.potentialfieldmethodsandtheirinherentlimitationsformobilerobotnavigation[c]//ieeeinternationalconferenceonroboticsandautomation.1991:1398-1404.

文獻(xiàn)7：moes,pettersenky.set-basedline-of-sight(los)pathfollowingwithcollisionavoidanceforunderactuatedunmannedsurfacevessel[c]//mediterraneanconferenceoncontrolandautomation.2016.

文獻(xiàn)8：zereike,sorbaraa,bibulim,etal.prioritytaskapproachforusvs’pathfollowingmissionswithobstaclesavoidanceandspeedregulation[c]//ifacconferenceonmanouveringandcontrolofmarinecraft.2015,48:25-30.

文獻(xiàn)9：soltanra,ashrafiuonh,muskekr.ode-basedobstacleavoidanceandtrajectoryplanningforunmannedsurfacevessels[j].robotica,2011,29(5):691-703.

文獻(xiàn)10：賈欣樂,楊鹽生.船舶運動數(shù)學(xué)模型.大連:大連海事大學(xué)出版社,1999。