本發(fā)明屬于無人水下航行器自主控制領(lǐng)域，尤其涉及一種補償uuv運動學(xué)和動力學(xué)干擾的空間軌跡跟蹤控制方法。

背景技術(shù)：

水下無人航行器(unmannedunderwatervehicle，uuv)的出現(xiàn)，為進行海洋探索和開發(fā)提供了非常重要的手段，已經(jīng)成為目前公認的最有效的海洋開發(fā)工具。uuv是一種自帶能源、自主導(dǎo)航與控制、能夠不受監(jiān)控的自主的執(zhí)行眾多的海洋使命的水下無人航行器。

欠驅(qū)動自主水下航行器的反饋控制在近幾年吸引力了大批控制和海洋工程領(lǐng)域人員的關(guān)注。與全驅(qū)動uuv運動控制相比，欠驅(qū)動uuv控制器設(shè)計過程中主要考慮的問題是uuv獨立的執(zhí)行機構(gòu)數(shù)量少于自由度的個數(shù)。這種結(jié)構(gòu)增加了非線性控制器設(shè)計的難度。本發(fā)明就是針對欠驅(qū)動uuv進行軌跡跟蹤控制。

在對uuv進行軌跡跟蹤控制過程中，一般我們會先對軌跡進行規(guī)劃，當(dāng)uuv沿著期望軌跡航行時，由于外界及uuv自身條件的影響，使得uuv的實際運動軌跡與期望的運動軌跡存在著偏差，于是我們需要進行合理的控制，使得uuv能夠更好的沿著期望軌跡航行，完成回收對接。現(xiàn)有技術(shù)中曹永輝、石秀華《水下航行器軌跡跟蹤控制與仿真》針對uuv的水平面運動提出了一種基于滑模控制的將橫向軌跡誤差法和視線法組合的軌跡跟蹤控制方法。首先分別建立橫向軌跡誤差法的滑模控制器和視線法的滑模控制器，當(dāng)航向角偏差較大時采用視線法，當(dāng)航向偏差小于一個定值時采用橫向軌跡誤差法。高劍、徐德民、嚴衛(wèi)生等人《自主水下航行器回塢路徑規(guī)劃與跟蹤控制》同樣針對uuv的水平面運動，提出了一包含位置跟蹤和航向角跟蹤的級聯(lián)系統(tǒng)的軌跡跟蹤控制方法。根據(jù)反步法設(shè)計位置跟蹤控制器，并且保證了軌跡跟蹤誤差控制全局一致漸近穩(wěn)定性。但是現(xiàn)有技術(shù)中大多是研究uuv的水面軌跡跟蹤控制問題，對于三維空間的軌跡跟蹤問題一般也是基于反步法設(shè)計，并且數(shù)學(xué)復(fù)雜性高。

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

本發(fā)明的目的是提供一種控制精度高的，能夠補償uuv運動學(xué)和動力學(xué)干擾的空間軌跡跟蹤控制方法。

本發(fā)明是通過以下方案實現(xiàn)的：

一種補償uuv運動學(xué)和動力學(xué)干擾的空間軌跡跟蹤控制方法，包括以下幾個步驟，

步驟一：給定平滑有界的期望軌跡yd；

步驟二：通過uuv搭載的慣性導(dǎo)航儀、深度計、姿態(tài)傳感器和多普勒計程儀采集uuv當(dāng)前時刻的位姿信息和速度信息；

其中，位姿信息η＝[x,y,z,θ,ψ]^t，包括縱向位移x、橫向位移y、垂向位移z、縱搖角θ和艏搖角ψ；速度信息包括直接驅(qū)動速度矢量υ＝[u,q,r]^t和間接驅(qū)動速度矢量w＝[v,w]^t，包括縱向速度u、橫向速度v、垂向速度w、縱搖角速度q和艏搖角速度r；

步驟三：選取uuv前端的虛擬控制點的位置；

步驟四：建立軌跡跟蹤誤差e，對軌跡跟蹤誤差e進行濾波處理，得到濾波后的軌跡跟蹤誤差ef；

步驟五：利用具有l(wèi)個節(jié)點的兩層rbf神經(jīng)網(wǎng)絡(luò)估計uuv運動學(xué)和動力學(xué)干擾項f(α)，得到uuv的運動學(xué)和動力學(xué)干擾項估計值利用濾波后的軌跡跟蹤誤差ef得到神經(jīng)網(wǎng)絡(luò)自適應(yīng)控制律

步驟六：根據(jù)神經(jīng)網(wǎng)絡(luò)自適應(yīng)控制律得到軌跡跟蹤控制信號τan，進一步得到執(zhí)行機構(gòu)控制信號τa＝[τu,τq,τr]^t，其中τu是由uuv主推產(chǎn)生的縱向推力，τq為縱傾控制力矩，τr為轉(zhuǎn)艏控制力矩；

步驟七：判斷uuv前端的虛擬控制點的位置是否到達給定期望軌跡的終點，如果是，則結(jié)束運行；否則返回步驟二。

本發(fā)明一種補償uuv運動學(xué)和動力學(xué)干擾的空間軌跡跟蹤控制方法，還可以包括：

1、所述的uuv前端的虛擬控制點的位置為，

其中，是恒定正常數(shù)，表示虛擬控制點pl和uuv質(zhì)心com之間的距離。

2、所述的軌跡跟蹤誤差e為：

e＝y(tǒng)-yd，

對軌跡跟蹤誤差e進行濾波處理，得到濾波后的軌跡跟蹤誤差ef：

其中，q1為增益矩陣，k1和k2為可調(diào)系數(shù)。

3、所述的神經(jīng)網(wǎng)絡(luò)自適應(yīng)控制律的求取過程為，

(1)利用具有l(wèi)個節(jié)點的兩層rbf神經(jīng)網(wǎng)絡(luò)，得到估計后的uuv的運動學(xué)和動力學(xué)干擾項

其中，α＝[η,υ,w,τan]^t，w是神經(jīng)網(wǎng)絡(luò)的可調(diào)參數(shù)矩陣，ξ(α)＝[ξ1(α),，..,ξl(α)^t]是神經(jīng)網(wǎng)絡(luò)基函數(shù)向量，ξi(α)為高斯函數(shù)：

其中，μi＝[μi1,μi2,...μin]^t和βi分別是高斯函數(shù)的中心和寬度，向量α和w分別屬于緊集u和ω，其中，m1和m2是參數(shù)；

ρ^*＝ε(α)+ρ，ε(α)為神經(jīng)網(wǎng)絡(luò)的誤差，誤差||ε(α)||≤bε，bε是給定閾值；干擾矩陣ρ有界||ρ||≤bρ，bρ為給定閾值；τa＝[τu,τq,τr]^t為執(zhí)行機構(gòu)控制信號，慣性矩陣是慣性矩陣m1(η)的估計值，m11,m55,m66是uuv的質(zhì)量和慣性參數(shù)；

(2)利用濾波后的軌跡跟蹤誤差ef得到神經(jīng)網(wǎng)絡(luò)自適應(yīng)控制律

w和ρm的更新規(guī)則為：

其中，閾值ρm＝bε+bρ，γw和γρ為自適應(yīng)增益，σw和σρ為正常數(shù)，kp為增益。

4、所述的軌跡跟蹤控制信號τan為

其中，

其中，雅克比矩陣

5、所述的uuv運動學(xué)和動力學(xué)干擾包括：測量儀器不確定性干擾，模型參數(shù)不確定性干擾，海流與海浪干擾，載荷動力學(xué)干擾。

6、所述uuv前端的虛擬控制點的位置與執(zhí)行機構(gòu)控制信號τa＝[τu,τq,τr]^t的關(guān)系為，

式中，為運動學(xué)和動力學(xué)干擾項的估計值。

7、所述神經(jīng)網(wǎng)絡(luò)的可調(diào)參數(shù)矩陣w的最優(yōu)矩陣為：

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

本發(fā)明一種補償uuv運動學(xué)和動力學(xué)干擾的空間軌跡跟蹤控制方法，能夠成功地控制uuv跟蹤上期望的軌跡，跟蹤偏差是收斂到零點附近的一個鄰域內(nèi)的，而且所有的閉環(huán)信號是有界的。神經(jīng)網(wǎng)絡(luò)的突出優(yōu)點是表現(xiàn)出平滑的響應(yīng)。本發(fā)明考慮了uuv軌跡跟蹤控制過程中測量儀器不確定性干擾，模型參數(shù)不確定性干擾，海流與海浪干擾及載荷動力學(xué)干擾對uuv控制精度的影響，采用神經(jīng)網(wǎng)絡(luò)逼近uuv運動學(xué)與動力學(xué)擾動，能夠有效補償uuv的運動學(xué)與動力學(xué)干擾，提高軌跡跟蹤控制精度。本發(fā)明對軌跡跟蹤誤差進行有效濾波，采用加權(quán)動態(tài)濾波方式，有效效地減少了執(zhí)行機構(gòu)飽和的風(fēng)險。

說明書附圖

圖1為本發(fā)明uuv三維空間自適應(yīng)軌跡跟蹤控制方法流程圖；

圖2為uuv前端的虛擬控制點的位置示意圖；

圖3為uuv空間軌跡跟蹤結(jié)果：圖3(a)xyz跟蹤結(jié)果；圖3(b)xy跟蹤結(jié)果；圖3(c)yz跟蹤結(jié)果。

圖4為本發(fā)明濾波后的軌跡跟蹤誤差ef(t)濾波效果圖，圖4(a)為本發(fā)明濾波后信號和原信號的對比圖，圖4(b)為本發(fā)明濾波后軌跡跟蹤誤差ef(t)效果圖。

具體實施方式

下面將結(jié)合附圖對本發(fā)明做進一步詳細說明。

本發(fā)明一種uuv三維空間自適應(yīng)軌跡跟蹤控制方法，如圖1所示，包括以下幾個步驟：

步驟一：給定平滑有界的期望軌跡yd(t)；

本發(fā)明的控制目標是：對于存在運動學(xué)與動力學(xué)干擾的欠驅(qū)動uuv設(shè)計一個跟蹤控制律，并且使得跟蹤誤差在三維空間中是一致最終有界的。

期望軌跡yd(t)及其都是有界的，supt≥0||yd(t)||＜bdp，其中bdp、bdv和bda邊界常數(shù)。

步驟二：通過uuv搭載的傳感器采集當(dāng)前的位姿信息和速度信息，位姿信息η＝[x,y,z,θ，ψ,]^t包括大地坐標系下的縱蕩x、橫蕩y、垂蕩z、縱搖角θ和艏搖角ψ，速度信息包括船體坐標系下的縱蕩u、橫蕩v、垂蕩w、縱搖q和艏搖速度r，分別記為直接驅(qū)動速度矢量υ＝[u,q,r]^t和間接驅(qū)動速度矢量w＝[v,w]^t。

欠驅(qū)動uuv的5自由度數(shù)學(xué)模型如下：

其中，τu，τq，τr是由執(zhí)行機構(gòu)產(chǎn)生的信號，τwu(t)，τwv(t)，τww(t)，τwq(t)，是有界的時變擾動。mii是uuv的質(zhì)量和慣性參數(shù)，dii是阻尼系數(shù)，i＝1，2,3,5,6。ρ為水密度，g為重力加速度，▽為水的容積，gml為縱向穩(wěn)心高。

運動學(xué)模型(1)可以如下表示：

其中，υ＝[u,q,r]^t和w＝[v,w]^t是重新定義的速度矢量，前者是直接驅(qū)動的，后者是不能直接驅(qū)動的。s(η)和分別是運動學(xué)矩陣和運動學(xué)干擾矢量陣，如下所示：

航行器直接驅(qū)動部分的動力學(xué)模型：

其中τa＝[τu,τq,τr]^t為控制輸入向量。為慣性矩陣，是科氏向心力，是水動力阻尼矩陣，是重力向量，是由海浪、海流引起的擾動。

航行器不能直接驅(qū)動部分的動力學(xué)模型：

其中，

其中，是科氏向心力，是水動力阻尼矩陣，是由海浪、海流引起的擾動。

說明：1)航行器的橫蕩和垂蕩速度是被動有界的supt≥0||w(t)||≤bw，bw為邊界常數(shù)。

2)擾動向量和的界限是：||τw1(t)||≤λw11，其中λw11和是正常數(shù)。

3)為了避免穩(wěn)定性分析中出現(xiàn)奇點，定義縱傾角的界限是：|θ(t)|≤θmax＜π/2。

步驟三：選取uuv前端的虛擬控制點的位置；

因為本發(fā)明主要研究uuv三維點跟蹤控制，x,y,z方向的坐標應(yīng)該選大地坐標系下的。一個簡化的選擇是質(zhì)心的位置，記為com，如圖2所示。然而，這種選擇的優(yōu)點是：(1)基于前面所提出的uuv模型，該控制器不會呈現(xiàn)縱搖和艏搖方向的擾動。(2)質(zhì)心位置不會受縱搖和艏搖控制輸入的影響。因此，引入以下變量變換，包括在各方向結(jié)合了uuv動力的所有自由度和所有控制輸入。

選取uuv前端的虛擬控制點的位置：

其中，是恒定正常數(shù)，表示虛擬控制點pl和uuv質(zhì)心com之間的距離，如圖2所示。

根據(jù)uuv當(dāng)前的位姿信息和速度信息，構(gòu)建uuv前端虛擬控制點與執(zhí)行機構(gòu)控制信號τa＝[τu,τq,τr]^t的關(guān)系，即uuv的輸入輸出模型，具體過程為：

(1)uuv模型狀態(tài)空間表示

將uuv運動學(xué)模型式(3)和動力學(xué)模型式(5)結(jié)合得到狀態(tài)空間表示形式：

其中：

狀態(tài)變量將在下面進行簡化狀態(tài)空間模型(9)，進行控制器設(shè)計，穩(wěn)定性分析。狀態(tài)反饋控制是：

其中，τa是uuv的控制輸入，τan是一個新的控制輸入，是m1(η)的近似值。將公式(11)代入公式(9)中，uuv狀態(tài)空間模型改寫為：

其中，為了簡化，f(x)和g(x)表示系統(tǒng)的平滑向量場，q(x)表示運動學(xué)和動力學(xué)擾動。

(2)uuv的輸入輸出模型

通過uuv運動學(xué)模型和uuv輸出方程，可以得出：

其中，lfh(x)＝▽hf，lgh(x)＝▽hg，lqh(x)＝▽hq，表示h分別沿著矢量f,g,q方向的導(dǎo)數(shù)。▽h是h的梯度(導(dǎo)數(shù))，jδ(η，w)是輸入-輸出模型對應(yīng)運動學(xué)模型擾動的部分。

其中，雅克比矩陣與公式(4)中運動學(xué)矩陣s(η)相反，j(η)對于所有的θ，沒有奇異點。因為公式(13)并不是全部與執(zhí)行機構(gòu)控制輸入相關(guān)，因此再一次變形得：

其中，可得ρ≤bρ。

步驟四：根據(jù)給定的期望軌跡yd建立軌跡跟蹤誤差e＝y(tǒng)-yd。

建立軌跡跟蹤誤差e：

e＝y(tǒng)-yd，

對軌跡跟蹤誤差e進行濾波處理，得到濾波后的軌跡跟蹤誤差：

其中，

tanh(·)為雙曲正切函數(shù)，(xd,yd,zd)為期望軌跡yd的坐標,q1為增益矩陣，k1和k2為可調(diào)系數(shù)，

給出一個平滑有界的期望軌跡期望軌跡由開環(huán)的運動規(guī)劃器給出。

其中，期望軌跡狀態(tài)向量狀態(tài)變量ηd為期望軌跡位姿信息，υd為期望軌跡速度信息，τand為期望軌跡的控制輸入。

步驟五：利用具有l(wèi)個節(jié)點的兩層rbf神經(jīng)網(wǎng)絡(luò)估計uuv運動學(xué)和動力學(xué)干擾項f(α)，得到估計后的uuv的運動學(xué)和動力學(xué)干擾項利用濾波后的軌跡跟蹤誤差ef得到神經(jīng)網(wǎng)絡(luò)自適應(yīng)控制律能夠補償估計后的uuv運動學(xué)與動力學(xué)干擾

(1)利用具有l(wèi)個節(jié)點的兩層rbf神經(jīng)網(wǎng)絡(luò)，得到估計后的uuv的運動學(xué)和動力學(xué)干擾項

其中，α＝[η,υ,w,τan]^t，w是神經(jīng)網(wǎng)絡(luò)的可調(diào)參數(shù)矩陣，ξ(α)＝[ξ1(α),...,ξl(α)]^t是神經(jīng)網(wǎng)絡(luò)基函數(shù)向量，ξi(α)為高斯函數(shù)：

其中，μi＝[μi1,μi2,...μin]^t和βi分別是高斯函數(shù)的中心和寬度，向量α和w分別屬于緊集u和ω，其中，m1和m2是參數(shù)；

神經(jīng)網(wǎng)絡(luò)的可調(diào)參數(shù)矩陣w的最優(yōu)矩陣為：

ρ^*＝ε(α)+ρ，ε(α)為神經(jīng)網(wǎng)絡(luò)的誤差，誤差||ε(α)||≤bε，bε是給定閾值；干擾矩陣ρ有界||ρ||≤bρ，bρ為給定閾值；τa＝[τu,τq,τr]^t為執(zhí)行機構(gòu)控制信號，慣性矩陣是慣性矩陣m1(η)的估計值，m11,m55,m66是uuv的質(zhì)量和慣性參數(shù)；

考慮uuv實際系統(tǒng)，包括各種傳感器的數(shù)據(jù)采集，空間機動所處介質(zhì)物理屬性，同時結(jié)合uuv的五自由度數(shù)學(xué)模型，uuv運動過程中所受的運動學(xué)與動力學(xué)干擾包括：測量儀器不確定性干擾，模型參數(shù)不確定性干擾，海流與海浪干擾及載荷動力學(xué)干擾。

測量儀器不確定性干擾主要指儀器量測中所受噪聲干擾，而實際海洋環(huán)境復(fù)雜多變，加之本身元器件的工藝水平限制，量測系統(tǒng)不可避免將受到各種噪聲的污染。如多普勒計程儀利用多普勒效應(yīng)原理進行測量底跟蹤速度或者對流速度，如果測量過程受水中散射體的影響，將給測量得到的速度量引入隨機誤差。這種情況下，該速度信號屬于非平穩(wěn)信號，其頻率隨時間而變化。

模型參數(shù)不確定性干擾主要指建立uuv動力學(xué)模型時，認為水動力系數(shù)不變，為定值，實際中水動力系數(shù)會隨著運動狀態(tài)的變化產(chǎn)生微小的攝動，此時相關(guān)的水動力項在計算值上應(yīng)附加一個偏移量，同比例縮尺度模型試驗研究表明在節(jié)航速范圍內(nèi)該偏移量不占主導(dǎo)地位，可視為擾動。

uuv在航速不高的近海面航行時受海流、海浪影響較大，流體流速是空間和時間的一個復(fù)雜函數(shù)，隨水域、深度和時間的變化而改變，將控制器的抗流能力作為運動控制設(shè)計的一項指標。

uuv在航行中，所附在的載荷結(jié)構(gòu)或者形狀發(fā)生變化時，對uuv質(zhì)量分布會產(chǎn)生影響。

(2)利用濾波后的軌跡跟蹤誤差ef進行控制器設(shè)計

得到神經(jīng)網(wǎng)絡(luò)自適應(yīng)控制律

w和ρm的更新規(guī)則為：

其中，閾值ρm＝bε+bρ，γw和γρ為自適應(yīng)增益，σw和σρ為正常數(shù)，kp為增益；

步驟六：根據(jù)神經(jīng)網(wǎng)絡(luò)自適應(yīng)控制律得到軌跡跟蹤控制信號τan，

進一步得到執(zhí)行機構(gòu)控制信號τa＝[τu,τq,τr]^t，其中τu是有uuv主推產(chǎn)生的縱向推力，τq為縱傾控制力矩，τr為轉(zhuǎn)艏控制力矩；

得到下述閉環(huán)動力誤差方程：

其中，是寬度估計誤差。

步驟七：判斷uuv前端的虛擬控制點的位置是否到達給定期望軌跡的終點，如果是，則結(jié)束運行；否則返回步驟二。

在本發(fā)明中，λmax(·)(λmin(·))定義為矩陣最大的(最小的)特征值。定義為向量的歐幾里得范數(shù)。矩陣a的誘導(dǎo)范數(shù)為矩陣a的frobenius范數(shù)為：其中tr{·}表示求跡運算。矩陣in表示n維單位陣。還定義了以下符號tanh(x)＝[tanh(x1),...,tanh(xn)]^t，sech²(x)＝diag[sech²(x1),...,sech²(xn)]^t。其中diag[·]表示對角陣，tanh(·)為雙曲正切函數(shù)，sech(·)＝1/cosh(·)為雙曲正割函數(shù)，cosh(·)為雙曲余弦函數(shù)。

本發(fā)明中uuv運動學(xué)和動力學(xué)干擾項f(α)＝d(α)+ρ，利用神經(jīng)網(wǎng)絡(luò)的逼近性質(zhì)，可以近似未知函數(shù)d(α)：

神經(jīng)網(wǎng)絡(luò)的誤差：

此處，w^*是w的估計值，定義估計誤差非線性不確定性d改寫為：d(x)＝w^*ξ(x)+ε使得||ε||≤bε。因此，公式(16)可以改寫為：

其中，ρ^*(t)＝ε(t)+ρ(t)的界限其中

下面給出本發(fā)明的一個實驗驗證本發(fā)明方法的有效性：

利用randn(.)函數(shù)在uuv輸出的測量中加入了高斯白噪聲來建模位置測量系統(tǒng)。所有的仿真都利用時間步長是20ms的歐拉解算法來完成。uuv配置有螺旋槳來提供縱向力、縱傾和偏航力矩。對于采用的實際uuv的模型，用到的模型參數(shù)為：

m11＝25kg,m22＝17.5kg,m33＝30kg,m55＝22.5kgm²,m66＝15kgm²,d11＝30kgs^-1,d22＝30kgs^-1,d33＝30kgs^-1,d55＝20kgm²s^-1,d66＝20kgm²s^-1，ρg▽gml＝5。不過，在實際中來確定這些參數(shù)的實際值是非常困難的，因此uuv是具有參數(shù)不確定性的。另外，通過以下方式加入環(huán)境干擾：

τw1(t)＝0.5sgn(υ)+2[sin(0.1t),sin(0.1t),sin(0.1t)]^t

控制參數(shù)的選擇如下：kp＝10i3,q＝10i3,γp＝1,σp＝0.005,εt＝1。設(shè)定控制信號是限值為|τai|≤100nm,i＝1,2,，3來建模執(zhí)行機構(gòu)的飽和特性。實驗中uuv的初始位姿為x(0)＝5m,y(0)＝5m,z(0)＝0m,θ(0)＝0rad,ψ(0)＝0rad。uuv的參考軌跡yd(t)通過一個開環(huán)的運動規(guī)劃器產(chǎn)生。參考軌跡的初始位姿和控制信號設(shè)置為

x(0)＝5m,y(0)＝5m,z(0)＝0m,θd(0)＝0rad,ψd(0)＝0rad,τad＝[7.5,1.5,3]^tnm。

此外，采用一個具有6個隱含節(jié)點(l＝6)、三個輸出節(jié)點的rbf神經(jīng)網(wǎng)絡(luò)來建模和逼近uuv的運動學(xué)與動力學(xué)干擾。rbf神經(jīng)網(wǎng)絡(luò)的參數(shù)為γw＝10,σw＝0.04,μi＝[-3,-2,-1,1,2,3]^t,βi＝10。w是一個初始值為0的3×6的矩陣。圖3給出了跟蹤的結(jié)果，包括uuv和參考軌跡的xyz、xy和yz三個軌跡圖。從圖中可以看出，uuv成功的跟蹤上了期望的軌跡，跟蹤偏差是收斂到零點附近的一個鄰域內(nèi)的。而且所有的閉環(huán)信號是有界的。神經(jīng)網(wǎng)絡(luò)的突出優(yōu)點是表現(xiàn)出平滑的響應(yīng)。本發(fā)明產(chǎn)生的控制信號都位于可接受的飽和限值之內(nèi)。圖4(a)為本發(fā)明濾波后信號和原信號的對比圖，可見本發(fā)明濾波效果非常好，本發(fā)明濾波后軌跡跟蹤誤差ef信號，其中k1＝0.1,k2＝1。圖4(b)中對比濾波信號函數(shù)為時濾波效果很差，沒有本發(fā)明濾波效果好。