帶側(cè)滑角補(bǔ)償?shù)臍鈮|船路徑跟蹤LOS導(dǎo)引算法

王元慧， 佟海艷

(1.哈爾濱工程大學(xué) 智能科學(xué)與工程學(xué)院，黑龍江 哈爾濱 150001；2.哈爾濱理工大學(xué) 自動化學(xué)院 黑龍江， 哈爾濱 150080)

氣墊船作為一種特殊的高速水面船，具有獨(dú)特的兩棲性，能夠在水面、地面以及沼澤等地帶航行，因此被廣泛應(yīng)用于軍事和民事中。全墊升氣墊船是氣墊船的一種，其工作原理是：依靠船體底部與航行表面之間的靜升壓力支撐船體脫離航行表面，同時(shí)利用安裝于艉部的空氣螺旋槳旋轉(zhuǎn)產(chǎn)生推力，通過空氣舵等裝置進(jìn)行控制操作，從而實(shí)現(xiàn)高速航行于航行表面的目的。全墊升氣墊船墊態(tài)航行過程中，由于不具有橫向推力，極易受到外界環(huán)境的干擾，從而極易產(chǎn)生較大的側(cè)滑。而全高速回轉(zhuǎn)與側(cè)滑會導(dǎo)致全墊升氣墊船處于危險(xiǎn)的航行狀態(tài)。如果側(cè)滑與回轉(zhuǎn)超過船的安全限界，可能造成翻船事故[1]。

導(dǎo)引系統(tǒng)是海洋運(yùn)載器完成諸多控制任務(wù)的前提，例如：路徑跟蹤[2]、軌跡跟蹤[3]、目標(biāo)跟蹤[4]、避障控制[5]、編隊(duì)控制[6]等，都需要導(dǎo)引子系統(tǒng)給出精確的導(dǎo)引參考角度，作為控制子系統(tǒng)的參考輸入，進(jìn)而由控制子系統(tǒng)根據(jù)控制算法跟蹤導(dǎo)引參考角度。因此，導(dǎo)引技術(shù)近年來成為了廣大學(xué)者關(guān)注的熱點(diǎn)[7-9]。然而，傳統(tǒng)的導(dǎo)引方法中并沒有考慮側(cè)滑角的影響，難以實(shí)現(xiàn)高精度的路徑跟蹤效果。因此，提高氣墊船的路徑跟蹤精度、側(cè)滑角的估計(jì)和補(bǔ)償技術(shù)成為設(shè)計(jì)導(dǎo)引方法的關(guān)鍵技術(shù)之一。

目前，對于導(dǎo)引算法的研究已經(jīng)取得的一定的研究成果，主要集中于傳統(tǒng)排水水面船以及水下無人潛器上的LOS導(dǎo)引算法研究上，例如無人水面艇[10-11]、水下無人潛器[12]等。值得說明的是，在現(xiàn)有的導(dǎo)引算法中，LOS導(dǎo)引算法、ILOS導(dǎo)引算法和ALOS導(dǎo)引算法是較為經(jīng)典且實(shí)際運(yùn)用效果較好的導(dǎo)引算法。Fossen等[13]將Line-of-sight(LOS)導(dǎo)引方法應(yīng)用于自主水下潛器的路徑導(dǎo)引上，取得了良好的三維路徑跟蹤效果。其側(cè)滑角的補(bǔ)償是假設(shè)側(cè)滑角已知，并對導(dǎo)引進(jìn)行了補(bǔ)償。隨后，F(xiàn)ossen等[14]又將LOS應(yīng)用于三自由度水面船的路徑跟蹤上，并證明了LOS導(dǎo)引律的全局指數(shù)漸進(jìn)穩(wěn)定性。Borhaug等[15]針對欠驅(qū)動水面船在海流干擾下的路徑跟蹤問題，設(shè)計(jì)了積分LOS導(dǎo)引方法，以補(bǔ)償海流產(chǎn)生的側(cè)漂力對其路徑跟蹤效果以及精度的影響，但其僅對直線路徑進(jìn)行了仿真驗(yàn)證。Lekkas等[16]基于積分LOS導(dǎo)引方法提出了直接側(cè)滑補(bǔ)償和積分側(cè)滑補(bǔ)償2種方法，由于實(shí)際船舶上測量設(shè)備的準(zhǔn)確度和造價(jià)高等問題，直接測量側(cè)滑角不易實(shí)現(xiàn)，比較而知，側(cè)滑補(bǔ)償算法研究具有實(shí)際意義。而后，F(xiàn)ossen等[17]又針對Dubins路徑，提出了基于自適應(yīng)側(cè)滑角補(bǔ)償?shù)腖OS導(dǎo)引策略，這是通過對側(cè)滑角的估計(jì)設(shè)計(jì)LOS導(dǎo)引策略，對于恒定小側(cè)滑角的補(bǔ)償，其效果較好。Liu等[18]提出一種基于預(yù)測器的側(cè)滑角估計(jì)方法，并對恒定小側(cè)滑角的估計(jì)進(jìn)行了仿真驗(yàn)證。國內(nèi)，對側(cè)滑的研究比較少，王浩鐸等[19]提出一種變增益導(dǎo)引策略，能夠補(bǔ)償外部環(huán)境干擾的引起的側(cè)滑，但其只適用于直線路徑，且并沒有對側(cè)滑角進(jìn)行直接估計(jì)補(bǔ)償。

以上研究都是基于側(cè)滑角是小范圍常量的假設(shè)基礎(chǔ)上進(jìn)行的，而針對全墊升氣墊船極易產(chǎn)生大側(cè)滑的特點(diǎn)，本文提出了一種基于自適應(yīng)時(shí)變大側(cè)滑角補(bǔ)償?shù)腖OS導(dǎo)引策略。首先，采用三次樣條插值法生成一般曲線路徑，其優(yōu)點(diǎn)是能夠保證曲線在經(jīng)過特定航路點(diǎn)時(shí)保證其二階導(dǎo)數(shù)連續(xù)。其次，設(shè)計(jì)了一種基于預(yù)測器的側(cè)滑角自適應(yīng)估計(jì)方法，通過引入2個(gè)側(cè)滑角參數(shù)，取締了現(xiàn)有側(cè)滑角的恒定小角度假設(shè)，實(shí)現(xiàn)時(shí)變大側(cè)滑角的估計(jì)與補(bǔ)償目的。最后，在設(shè)計(jì)的側(cè)滑角參數(shù)估計(jì)的模型基礎(chǔ)上設(shè)計(jì)基于時(shí)變大側(cè)滑角估計(jì)和補(bǔ)償自適應(yīng)LOS導(dǎo)引方法，實(shí)現(xiàn)了氣墊船在未知側(cè)漂力下的路徑跟蹤收斂目標(biāo)。

1 氣墊船運(yùn)動模型及LOS導(dǎo)引算法

1.1 氣墊船運(yùn)動數(shù)學(xué)模型

為描述氣墊船運(yùn)動過程，分別引入北東坐標(biāo)系(north east down, NED)，船體坐標(biāo)系以及Serret-Frenet (SF)坐標(biāo)系，并建立氣墊船三自由度運(yùn)動學(xué)和動力學(xué)數(shù)學(xué)模型為[20]：

(1)

式中：x、y、ψ分別表示氣墊船在北東坐標(biāo)系下的北向位置，東向位置和艏向角度；u、v、r分別表示氣墊船在船體坐標(biāo)系下的縱蕩速度、橫蕩速度和艏搖角速度；m、Iz分別表示氣墊船的質(zhì)量和轉(zhuǎn)動慣量；Fx、Fy、Mz分別表示氣墊船在x、y、z方向上受到的合力及力矩。

由于全墊升氣墊船的物理墊升結(jié)構(gòu)，其船體是脫離航行表面的，因而與傳統(tǒng)水面船不同，空氣阻力是氣墊船的主要阻力，因而建模過程中首要考慮空氣動力/力矩。

1.2 LOS導(dǎo)引

基于LOS導(dǎo)引方法的氣墊船路徑跟蹤原理圖，如圖1所示。其中，β為側(cè)滑角，是合速度U與船體坐標(biāo)系xb軸的夾角；Δ為前視距離；?代表大于零的路徑參數(shù)化變量，任意?都對應(yīng)一個(gè)參考點(diǎn)位置(xp(?),yp(?))。為到達(dá)由參考點(diǎn)的切向向量和法向向量構(gòu)成的SF坐標(biāo)系、NED坐標(biāo)系需要旋轉(zhuǎn)角度：

圖1 LOS曲線路徑跟蹤原理Fig.1 The geometrical illustration of LOS guidance

rp(?)=arctan(y′p(?),x′p(?))

(2)

式中：x′p(?)=?xp/??，y′p(?)=?yp/??。

由式(2)可得：

(3)

對于位于NED坐標(biāo)下(x,y)位置的氣墊船，由模型 (1)可得其路徑跟蹤的切向誤差xe以及法向誤差ye模型：

(4)

本文設(shè)計(jì)的路徑跟蹤控制系統(tǒng)采用基于側(cè)滑識別補(bǔ)償?shù)腖OS導(dǎo)引策略與PID航向控制器相結(jié)合。導(dǎo)引系統(tǒng)根據(jù)實(shí)際位置和參考路徑位置計(jì)算出航向角度；控制系統(tǒng)根據(jù)導(dǎo)引系統(tǒng)給出的期望航向角以及當(dāng)前氣墊船的航向角度進(jìn)行航向控制，本文假設(shè)速度可以控制到期望速度，對速度不進(jìn)行控制設(shè)計(jì)。

1.3 參數(shù)化路徑生成

在許多領(lǐng)域中，多項(xiàng)式插值法是一種解決給定航路點(diǎn)的曲線路徑生成問題的通用方法。然而，三次多項(xiàng)式插值法并不能保證曲線的二階導(dǎo)數(shù)連續(xù)，鑒于本文的路徑跟蹤更新涉及到路徑的二階導(dǎo)數(shù)，本文設(shè)計(jì)了基于航路點(diǎn)式的三次樣條插值曲線路徑生成方法。有效地保證了曲線路徑的二階導(dǎo)數(shù)的在航路點(diǎn)處的連續(xù)，進(jìn)而進(jìn)一步提高了曲線路徑的光滑度。

預(yù)設(shè)航路點(diǎn)集N，參考點(diǎn)(xp(?),yp(?))可采用三次多項(xiàng)式模型計(jì)算：

(5)

式中參數(shù)aj、bj(j=0,1,2,3)為位置參數(shù)。參考點(diǎn)xp(?)和yp(?)對參數(shù)?的導(dǎo)數(shù)為：

(6)

所以，全墊升氣墊船的參考速度up可計(jì)算為：

(7)

式中：

(8)

對于式(6)中的未知參數(shù)，可采用三次樣條法計(jì)算[20]。

在航路點(diǎn)集N中的任意2個(gè)相鄰航路點(diǎn)(xk-1,yk-1)與(xk,yk)須滿足:

(9)

式中k=1,2，…,n。

且為保證曲線的光滑性還須滿足：

(10)

為了滿足上述約束，添加速度和加速度的上界條件，那么：

(11)

(12)

多項(xiàng)式xp(?k)通過ak=[a3ka2ka1ka0k]T計(jì)算，將n個(gè)航路點(diǎn)的未知參數(shù)寫成向量形式：

(13)

從而，三次插值問題轉(zhuǎn)化為線性等式：

y=A(?k-1,…,?k)x,k=1,2,…,n

(14)

式中y=[xstartx0x100x2x200 …xnxfinal]T；起始點(diǎn)可通過速度與加速度限制來指定：xstart∈{x′0,x″0}和xfinal∈{x′n,x″n}；得到:

(15)

根據(jù)x=A-1y可解得所有未知參數(shù)值。參數(shù)bk=[b3kb2kb1kb0k]T采用同樣的方法計(jì)算。

2 基于預(yù)測模型的自適應(yīng)大側(cè)滑角估計(jì)器設(shè)計(jì)

本節(jié)針對未知側(cè)滑角參數(shù),設(shè)計(jì)基于預(yù)測器的自適應(yīng)大側(cè)滑角的估計(jì)。Fossen等[17]將側(cè)滑角的范圍假設(shè)為β∈(0°，5°)，而氣墊船的極易產(chǎn)生側(cè)滑的特點(diǎn)導(dǎo)致側(cè)滑角的范圍有所擴(kuò)大β∈(0°，15°)，導(dǎo)致小側(cè)滑的假設(shè)不再適用。本文設(shè)計(jì)的側(cè)滑角估計(jì)有別于Fossen等小側(cè)滑角估計(jì)，取締其小側(cè)滑角的假設(shè)，引入2個(gè)參數(shù)θ1與θ2，對大側(cè)滑角進(jìn)行了自適應(yīng)估計(jì)。

設(shè)參數(shù)θ1=cosβ,θ2=sinβ，模型 (4)可轉(zhuǎn)化為：

(16)

由于參數(shù)θi(i=1,2)為未知參數(shù)，設(shè)計(jì)預(yù)測器為：

(17)

設(shè)計(jì)基于預(yù)測誤差的參數(shù)自適應(yīng)估計(jì)律為：

(18)

得到預(yù)測誤差動態(tài)系統(tǒng)為：

(19)

證明：取李雅普諾夫函數(shù)：

(20)

對其進(jìn)行求導(dǎo)并根據(jù)假設(shè)，得：

(21)

根據(jù)李雅普諾夫穩(wěn)定性定理，可知預(yù)測誤差和跟蹤誤差是全局一致漸近穩(wěn)定的。

3 基于側(cè)滑角補(bǔ)償?shù)腖OS導(dǎo)引

本節(jié)基于前述的自適應(yīng)側(cè)滑角參數(shù)估計(jì)值，對LOS導(dǎo)引進(jìn)行實(shí)時(shí)側(cè)滑補(bǔ)償，以求更好的全墊升氣墊船路徑跟蹤效果。為穩(wěn)定切向誤差xe，設(shè)計(jì)控制輸入up為：

(22)

由式 (8)可得路徑參數(shù)更新律：

(23)

式中κ1∈R,κ1>0為導(dǎo)引律的參數(shù)增益。

根據(jù)觀測模型 (17)和路徑參數(shù)更新率 (23)，可得：

(24)

由于路徑當(dāng)前參考點(diǎn)的旋轉(zhuǎn)角度γp(?)為已知量，并且路徑跟蹤誤差可測，設(shè)計(jì)導(dǎo)引角度為：

(25)

式中αy為待設(shè)計(jì)控制輸入。

由于

(26)

那么，式(17)中的法向誤差估計(jì)模型可化為：

(27)

(28)

式中:

(29)

綜上，模型 (17)轉(zhuǎn)化為：

(30)

導(dǎo)引律 (25)化為：

(31)

證明：取李雅普諾夫函數(shù)：

(32)

對上述李雅普諾夫函數(shù)求導(dǎo)：

(33)

式中：h=min{κ1-kx/2ε1,κ2-ky/2ε2}>0；ε1、ε2>0，通過設(shè)計(jì)適當(dāng)?shù)膮?shù)ε1、ε2可使：

(34)

那么：

(35)

因此，系統(tǒng) (30)是輸入狀態(tài)穩(wěn)定的。

定理3在更新律 (18)與導(dǎo)引律 (31)下，系統(tǒng) (30)與系統(tǒng) (19)構(gòu)成的級聯(lián)系統(tǒng)是全局一致漸近穩(wěn)定的。

4 仿真研究

為驗(yàn)證設(shè)計(jì)方法的有效性，以某型全墊升氣墊船為例，在外界風(fēng)干擾下對氣墊船的直線和曲線路徑分別進(jìn)行了路徑跟蹤仿真實(shí)驗(yàn)。仿真環(huán)境設(shè)置為：風(fēng)速為10 m/s，風(fēng)向?yàn)?0°，其相對風(fēng)速為：

(36)

因此，相對于船體來說隨著航向和速度的變化，風(fēng)干擾是非線性時(shí)變的。

仿真案例1：直線路徑跟蹤，其航路點(diǎn)信息如表1所示。初始位置及艏向?yàn)?0 m,150 m，0°)；直線路徑的起點(diǎn)(0 m,0 m)，終點(diǎn)(4 000 m,4 000 m)，初始航速為0 kn，目標(biāo)航速20 kn，仿真參數(shù)設(shè)置為κ1=0.5；kx=1；ky=1；γ1=γ2=0.2；仿真結(jié)果如圖2～7所示。

表1 直線路徑航路點(diǎn)信息Table 1 The way points data for line path

圖2 氣墊船直線路徑跟蹤結(jié)果Fig.2 The curve of linear path tracking of hovercraft

圖2和圖3表明：帶有自適應(yīng)側(cè)滑補(bǔ)償?shù)腁LOS導(dǎo)引的全墊升氣墊船直線路徑跟蹤效果優(yōu)于LOS導(dǎo)引的路徑跟蹤結(jié)果。圖3給出了直線路徑跟蹤的法向誤差比較曲線，說明無側(cè)滑補(bǔ)償?shù)穆窂礁櫈橛胁罡櫍€(wěn)態(tài)誤差保持在22 m左右，而帶有側(cè)滑補(bǔ)償?shù)穆窂礁櫿`差收斂于零。圖4表明：艏向控制系統(tǒng)能夠跟蹤導(dǎo)引系統(tǒng)給出的參考艏向。

圖3 氣墊船直線路徑跟蹤實(shí)際法向誤差比較Fig.3 The comparison of cross-error of path following of hovercraft

圖4 氣墊船艏向角度的參考值與實(shí)際值Fig.4 The reference heading and true heading of hovercraft

圖5和圖6表明：本文提出的方法能夠估計(jì)到較大的時(shí)變側(cè)滑角度，本方法將Fossen等[17]的小側(cè)滑角度假設(shè)取締，能夠估計(jì)到較大的側(cè)滑角度范圍。圖7表明：本文設(shè)計(jì)的預(yù)測器能夠保證路徑跟蹤過程中的跟蹤誤差的預(yù)測值趨于跟蹤誤差實(shí)際值，并且趨于0，從而實(shí)現(xiàn)路徑跟蹤誤差收斂的目的。

圖5 氣墊船直線路徑跟蹤過程中側(cè)滑角的實(shí)際值與估計(jì)值Fig.5 The estimated value and the true value of hovercraft during line path following

圖6 氣墊船直線路徑跟蹤的參數(shù)估計(jì)值Fig.6 The estimated parameters for line path following of hovercraft

圖7 氣墊船直線路徑跟蹤誤差的實(shí)際值與預(yù)測值Fig.7 The true and predicted value of following errors of hovercraft

仿真案例2：曲線路徑跟蹤，其航路點(diǎn)信息如表2所示。初始位置及艏向?yàn)?0 m,150 m,0°)；初始航速為0 kn，目標(biāo)航速20 kn，仿真參數(shù)設(shè)置為κ1=0.5；kx=1；ky=1;γ1=γ2=0.2；仿真結(jié)果如圖8～圖13所示。

表2 曲線路徑航路點(diǎn)信息Table 2 The way points data for cure path

圖8 氣墊船曲線路徑跟蹤結(jié)果Fig.8 The curve of path tracking of hovercraft

圖8和圖9表明無側(cè)滑補(bǔ)償?shù)穆窂礁櫈橛胁罡櫍谇€路徑的后半段穩(wěn)態(tài)誤差約為25 m，而帶有側(cè)滑補(bǔ)償?shù)腁LOS路徑跟蹤穩(wěn)態(tài)誤差收斂于0。圖10表明：艏向控制系統(tǒng)能夠控制艏向到由導(dǎo)引系統(tǒng)給出的艏向參考值。

圖9 氣墊船曲線路徑跟蹤實(shí)際法向誤差比較Fig.9 The comparison of cross-error for curve path following of hovercraft

圖10 氣墊船艏向角度的參考值與實(shí)際值Fig.10 The reference heading and true heading of hovercraft

圖11和圖12表明：氣墊船在曲線路徑跟蹤過程中，其側(cè)滑角為時(shí)變大側(cè)滑角，本文提出的基于預(yù)測器的側(cè)滑角估計(jì)方法能夠估計(jì)到氣墊船的時(shí)變大側(cè)滑角值，并且能夠在LOS導(dǎo)引方法中對時(shí)變大側(cè)滑角進(jìn)行補(bǔ)償。圖13表明：本文設(shè)計(jì)的預(yù)測器能夠保證氣墊船曲線路徑跟蹤過程中的跟蹤誤差估計(jì)值趨于跟蹤誤差實(shí)際值，并且趨于零，實(shí)現(xiàn)路徑跟蹤的目的。

圖11 氣墊船曲線路徑跟蹤過程中側(cè)滑角的實(shí)際值與估計(jì)值Fig.11 The estimated value and the true value of hovercraft during curve path following

圖12 氣墊船曲線路徑跟蹤的參數(shù)的估計(jì)值Fig.12 The estimated parameters for curve path following of hovercraft

圖13 氣墊船曲線路徑跟蹤誤差的實(shí)際值與預(yù)測值Fig.13 The actual and predicted value of following errors of hovercraft

綜上，本文提出的基于預(yù)測模型的時(shí)變大側(cè)滑角補(bǔ)償自適應(yīng)LOS導(dǎo)引方法能夠估計(jì)到時(shí)變大側(cè)滑角值，并在LOS導(dǎo)引系統(tǒng)中對側(cè)滑角進(jìn)行補(bǔ)償，進(jìn)而實(shí)現(xiàn)氣墊船在未知側(cè)漂力下的路徑跟蹤收斂目標(biāo)。

5 結(jié)論

1)提出的基于預(yù)測器的側(cè)滑角自適應(yīng)估計(jì)律能夠準(zhǔn)確地估計(jì)到未知時(shí)變大側(cè)滑角度值。

2)設(shè)計(jì)的預(yù)測器是基于位置跟蹤誤差對側(cè)滑角進(jìn)行估計(jì)和預(yù)測，不依賴于模型精確度。

3)提出的基于時(shí)變大側(cè)滑角估計(jì)補(bǔ)償?shù)淖赃m應(yīng)LOS導(dǎo)引策略能夠有效地提高氣墊船在未知側(cè)漂力存在情況下路徑跟蹤精度。