張偉， 伍文華， 滕延斌， 宮鵬， 唐業(yè)競

(1.哈爾濱工程大學(xué) 水下機(jī)器人技術(shù)重點(diǎn)實(shí)驗(yàn)室，黑龍江 哈爾濱 150001； 2.中國船舶科學(xué)研究中心 水動力學(xué)國防重點(diǎn)實(shí)驗(yàn)室，江蘇 無錫 214082)

動基座水下無人航行器(unmanned underwater vehicles,UUV)回收是指UUV通過運(yùn)動控制使得其最終保持與運(yùn)動母船的位置和姿態(tài)一致[1-3]，在動態(tài)過程中完成與母船的回收。動基座UUV回收過程主要包括跟蹤控制階段和對接控制階段。在UUV的跟蹤控制中，Rezazadegan等[4]研究了六自由度水下機(jī)器人的軌跡跟蹤控制問題，基于李亞普諾夫直接法和反步技術(shù)，提出了一種自適應(yīng)控制器，保證了對參數(shù)不確定性的魯棒性。趙杰梅等[4]設(shè)計(jì)了神經(jīng)網(wǎng)絡(luò)自適應(yīng)輸出反饋控制器應(yīng)用于AUV水平面路徑跟蹤，使得系統(tǒng)跟蹤誤差最終一致有界。王宏健等[5]設(shè)計(jì)了濾波反步法跟蹤控制器應(yīng)用于欠驅(qū)動UUV的三維軌跡跟蹤控制，增強(qiáng)了系統(tǒng)對噪聲的魯棒性。張偉等[6]針對UUV在流體運(yùn)動中存在時(shí)變干擾問題，提出了自適應(yīng)RBF神經(jīng)網(wǎng)絡(luò)反步跟蹤控制，提高了UUV對環(huán)境的自適應(yīng)能力。王曉偉等[7]應(yīng)用李雅普諾夫理論和反步法設(shè)計(jì)了滑模控制器，實(shí)現(xiàn)了欠驅(qū)動AUV對三維直線的跟蹤控制，削弱了系統(tǒng)抖振。在UUV的對接控制中，張偉等[8]研究了UUV對水下塢艙的對接問題，提出了一種基于對線控位策略的灰色預(yù)測PID控制方法，取得了良好效果。鄭榮等[9]研究了AUV與移動塢站的對接技術(shù)，提出了一種基于路徑特征匹配的動態(tài)導(dǎo)引算法，通過試驗(yàn)驗(yàn)證了該導(dǎo)引算法的可靠性。Enrico等[10]將強(qiáng)化學(xué)習(xí)策略應(yīng)用于AUV的對接控制，以更低的計(jì)算成本實(shí)現(xiàn)了最優(yōu)控制的性能，對于不同系統(tǒng)具有較強(qiáng)的適應(yīng)性。上述回收控制方法雖然在一定程度上取得了良好的效果，但由于算法本身的局限性而難以處理各種復(fù)雜約束。水下無人潛航器(UUV)作為一種水下無人自主化裝備，被廣泛應(yīng)用于海洋調(diào)查、勘測、搜索等領(lǐng)域[1]。傳統(tǒng)的UUV回收主要通過水面船起吊完成，這種過多依靠人力的回收方式存在隱秘性差、安全性低、受環(huán)境影響大等缺點(diǎn)[2]。因此，UUV水下自主回收對接技術(shù)日益成為水下無人系統(tǒng)領(lǐng)域的研究熱點(diǎn)[3]。

由于模型預(yù)測控制對存在多種復(fù)雜約束，以及不確定性的系統(tǒng)具有良好的控制性能[11-12]，因此本文將基于MPC控制設(shè)計(jì)動基座UUV對接控制器。然而，傳統(tǒng)的MPC控制方法在對UUV數(shù)學(xué)模型進(jìn)行目標(biāo)函數(shù)優(yōu)化時(shí)，不僅計(jì)算代價(jià)較高，而且其在約束條件下解算的全局最優(yōu)解，可能存在以犧牲其他指標(biāo)為代價(jià)的問題。滿意控制卻可以很好地解決這一問題。滿意控制是把約束條件、代價(jià)函數(shù)當(dāng)作廣義上的要求，通過使用有限的操作變量去符合這些要求，它的解是根據(jù)這些要求的不同權(quán)衡得到的滿意解[13]?；谝陨嫌懻?，本文提出了約束條件下基于滿意MPC的動基座UUV對接控制方法，采用滿意控制思想通過MPC控制方法對UUV模型進(jìn)行分布式處理，將單一的UUV對接控制性能函數(shù)轉(zhuǎn)化為多自由度多目標(biāo)函數(shù)，從而對多個(gè)目標(biāo)函數(shù)分別求得最優(yōu)控制解，然后由這多個(gè)局部的最優(yōu)控制解組成滿足對接約束的全局次優(yōu)解，也就是滿意解，從而保證系統(tǒng)的魯棒性與穩(wěn)定性。

1 UUV回收對接系統(tǒng)與模型

1.1 UUV回收對接系統(tǒng)

本文所研究的回收對接系統(tǒng)是基于叉柱式的回收對接系統(tǒng)[14]，該系統(tǒng)主要包括對接UUV與回收母船兩大部分。如圖1所示，對接UUV主要由對接柱、超短基線(USBL)接收機(jī)及水下攝像機(jī)組成。回收母船主要由母UUV、聲學(xué)信標(biāo)、L型光學(xué)燈陣、對接導(dǎo)引叉組成。首先，UUV通過聲光組合的導(dǎo)引方式獲取與回收母船的相對位置和姿態(tài)信息。然后，UUV通過運(yùn)動控制器進(jìn)行運(yùn)動控制和姿態(tài)調(diào)整，從而不斷縮小UUV與運(yùn)動母船對接叉的位置和姿態(tài)誤差。最后，UUV的對接柱沿運(yùn)動母船上的導(dǎo)引叉滑動到中間的鎖緊裝置處，從而完成UUV與運(yùn)動回收母船完成對接。

圖1 UUV回收對接系統(tǒng)

1.2 對接UUV模型

為了研究上的方便,根據(jù)UUV的主要特征,現(xiàn)作出如下合理假設(shè)：

1)UUV重力與浮力大小基本相等；

2)UUV外形的斷面、切面和截面是3個(gè)嚴(yán)格對稱的對稱面；

3)UUV在運(yùn)動過程中橫傾角很小，可以基本忽略。

基于以上假設(shè)，可建立UUV五自由度的動力學(xué)與運(yùn)動學(xué)模型[15]：

(1)

(2)

其中：

式中：m為UUV質(zhì)量；τu、τv、τw和τr、τr分別表示UUV所受的控制力和力矩；ξ、η、ζ分別為UUV北向、東向、深度位置；φ、θ、Ψ分別為UUV的橫搖角、縱搖角和艏向角；u、v、w分別為UUV的縱向、橫向和垂向速度；p、q、r分別為UUV的橫搖角、縱搖角、艏向角速度。

1.3 回收母船模型

在本文研究的動基座UUV對接控制中，運(yùn)動回收母船采用的是另一條不同的UUV，因此運(yùn)動回收母船的模型與對接UUV的空間五自由度模型(1)、(2)一致，在此處不再贅述。

回收母船主要參數(shù)形式如下：

位置和姿態(tài)向量為[ξmηmζmφmθmψm]T;速度和角速度向量為[umvmwmpmqmrm]T;控制量為[τmuτmvτmwτmpτmqτmr]T。

2 多種約束問題分析

在動基座UUV對接過程中，存在著各種復(fù)雜約束。本節(jié)從UUV回收對接安全性、UUV自身物理限制、UUV所受環(huán)境干擾力等角度出發(fā)，將多種約束問題主要?dú)w為狀態(tài)約束、自身約束和干擾約束三大類。

2.1 狀態(tài)約束

狀態(tài)約束主要是對UUV的航向狀態(tài)所做的約束，主要分為終端對接約束和速度約束。

1)終端對接約束。

終端對接約束主要是根據(jù)回收母船上的位置劃定對接安全區(qū)域和禁止駛?cè)雲(yún)^(qū)域，使得UUV在終端對接過程中處于相對安全的狀態(tài)。動基座UUV終端對接安全區(qū)如圖2所示。

圖2 動基座UUV終端對接安全區(qū)示意

通過將動基座UUV終端對接約束轉(zhuǎn)化為位置約束，k時(shí)刻該約束條件可表示為：

(3)

式中：(x(k),y(k))為UUV的水平面位置坐標(biāo)；(xm(k),ym(k))為運(yùn)動回收母船的水平面位置坐標(biāo)；Φc為對接導(dǎo)引叉的開角；Rd為考慮到UUV轉(zhuǎn)向能力而設(shè)定的值。

在第k時(shí)刻，可將UUV運(yùn)動坐標(biāo)系下的約束(3)，轉(zhuǎn)化為大地坐標(biāo)系下的約束：

(4)

式中：ξmin(k)、ηmin(k)、ζmin(k)分別為當(dāng)前時(shí)刻3個(gè)輸出量的最小值；ξmax(k)、ηmax(k)、ηmax(k)分別為當(dāng)前時(shí)刻3個(gè)輸出量的最大值。

2)速度約束。

速度約束主要是對UUV的速度增量進(jìn)行約束，從而防止UUV因速度變化過快與運(yùn)動回收母船發(fā)生碰撞，速度變化量約束表示為：

(5)

式中：u(k)、v(k)、w(k)是第k時(shí)刻的速度量；u(k-1)、v(k-1)、w(k-1)是k-1時(shí)刻的速度量；d·是根據(jù)UUV的速度變化而分別設(shè)定的值。

根據(jù)UUV動力學(xué)模型，速度變化量約束(5)可轉(zhuǎn)化為控制力變化量約束：

(6)

式中：τ·(k)表示k時(shí)刻控制力；τ·(k-1)表示k-1時(shí)刻控制力；Δτ·表示控制量增量。

2.2 自身約束

自身約束是由于UUV本身硬件能力的限制，主要包括最大舵角約束和推進(jìn)器推力約束，可將這2種約束整合，表示為控制量約束：

umin≤u(k)≤umax

(7)

式中u(k)=[τu(k)τv(k)τw(k)τq(k)τr(k)]T。

2.3 干擾約束

干擾約束即為動基座UUV在回收過程中所受到的干擾，主要包括海流約束和近壁面約束。

1)海流約束。

海流約束是指UUV在水下航行過程中所受到的定常海流干擾約束。假設(shè)海流大小為τcd，以及在固定坐標(biāo)系下的方向?yàn)?φcd,θcd,ψcd)，將其轉(zhuǎn)換到運(yùn)動坐標(biāo)系下的干擾為：

(8)

其中Si(i=1,2,3)具體形式為：

2)近壁面約束。

近壁面約束是指UUV靠近運(yùn)動回收母船時(shí)所引起的流體的變化經(jīng)過母船的反彈而對UUV的影響。根據(jù)文獻(xiàn)[16],對接過程中UUV在近壁面干擾下所受的垂向力τjdw、縱傾力矩τjdq關(guān)系式為：

(9)

式中：ρ為流體密度；g為重力加速度；U為速度矢量；V為UUV排水體積；CZ、CM分別表示垂向力、縱傾力矩參數(shù)。

將式(8)、(9)整合得到對接過程中的控制干擾量約束為：

(10)

3 控制器設(shè)計(jì)

在UUV與運(yùn)動回收母船對接的過程中，存在各種復(fù)雜約束，要求UUV在多自由度控制中完成與母船在一定精度內(nèi)的對接。針對以上問題，本文提出了基于滿意MPC的動基座UUV對接算法。首先，將UUV模型按照滿意思想進(jìn)行分布式處理，根據(jù)MPC 理論，構(gòu)建UUV北東位置、深度、縱傾和航向預(yù)測模型；然后將動基座UUV對接過程存在的復(fù)雜約束分別應(yīng)用到對應(yīng)的預(yù)測模型中，設(shè)計(jì)相應(yīng)的滿意目標(biāo)函數(shù)，對目標(biāo)函數(shù)優(yōu)化求解；最后將解算出的下一時(shí)刻控制量作用于UUV系統(tǒng)，從而進(jìn)行反復(fù)解算，滾動優(yōu)化直至完成UUV對運(yùn)動回收母船的對接。

3.1 預(yù)測模型

1)構(gòu)建UUV北東位置預(yù)測模型。

首先根據(jù)UUV五自由度運(yùn)動學(xué)模型(1)、動力學(xué)模型(2)，引入相應(yīng)的干擾約束(10)，構(gòu)建UUV北東位置模型：

(11)

將式(11)表示成連續(xù)狀態(tài)空間模型：

(12)

將模型(12)采用一階差商的離散化方法離散化，采樣周期為1，得到離散化后的模型為：

式中：A1p=A1p+I4×4；B1p=B1c；C1p=C1c。

然后，根據(jù)北東位置離散化模型(13)構(gòu)建UUV北東位置誤差增強(qiáng)模型：

(14)

最后將模型(14)在預(yù)測時(shí)域Np1和控制時(shí)域Nc1內(nèi)展開，得到北東位置預(yù)測模型：

Y1=F1x1e(k)+Φ1(ΔU1+ΔDu1)

(15)

Y1=[y1e(k+1|k)y1e(k+2|k)…y1e(k+Np1|k)]T；

ΔDu1=[due1(k)due1(k+1) …due1(k+Nc1-1)]T;

ΔU1=[u1e(k)u1e(k+1)…u1e(k+Nc1-1)]T;

2)構(gòu)建UUV深度預(yù)測模型。

根據(jù)UUV五自由度空間模型(1)、(2)，引入干擾約束(10)，構(gòu)建UUV深度模型：

(16)

同樣地，可得到UUV深度誤差增強(qiáng)模型：

(17)

將模型(17)在預(yù)測時(shí)域Np2和控制時(shí)域Nc2內(nèi)展開，得到深度預(yù)測模型：

Y2=F2x2e(k)+Φ2(ΔU2+ΔDu2)

(18)

Y2=[y2e(k+1|k)y2e(k+2|k)…y2e(k+Np2|k)]T；

ΔDu2=[due2(k)due2(k+1)…due2(k+Nc2-1)]T；

ΔU2=[u2e(k)u2e(k+1)…u2e(k+Nc2-1)]T；

3)構(gòu)建UUV縱傾預(yù)測模型。

根據(jù)UUV五自由度空間模型(1)、(2)，干擾約束(10)，構(gòu)建UUV深度模型：

(19)

同樣地，可得到UUV深度誤差增強(qiáng)模型：

(20)

將模型(20)在預(yù)測時(shí)域Np3和控制時(shí)域Nc3內(nèi)展開，得到縱傾預(yù)測模型：

Y3=F3x3e(k)+Φ3(ΔU3+ΔDu3)

(21)

Y3=[y3e(k+1|k)y3e(k+2|k)…y3e(k+Np3|k)]T；

ΔDu3=[due3(k)due3(k+1)…due3(k+Nc3-1)]T；

ΔU3=[u3e(k)u3e(k+1)…u3e(k+Nc3-1)]T；

4)構(gòu)建UUV航向預(yù)測模型。

根據(jù)UUV五自由度空間模型(1)、(2)，構(gòu)建UUV航向模型：

(22)

同樣地，可得到UUV航向誤差增強(qiáng)模型：

(23)

將模型(23)在預(yù)測時(shí)域Np4和控制時(shí)域Nc4內(nèi)展開，得到航向預(yù)測模型：

Y4=F4x4e(k)+Φ4ΔU4

(24)

Y4=[y4e(k+1|k)y4e(k+2|k)…y4e(k+Np4|k)]T;

ΔU4=[u4e(k)u4e(k+1)…u4e(k+Nc4-1)]T;

3.2 約束處理與目標(biāo)函數(shù)優(yōu)化求解

1)北東位置預(yù)測模型約束處理與優(yōu)化求解。

根據(jù)終端對接約束(4)，提取相應(yīng)的大地坐標(biāo)系下北東位置約束，可得到預(yù)測時(shí)域內(nèi)的北東位置終端對接約束：

Y1min≤Y1≤Y1max

(25)

將北東預(yù)測模型(15)代入式(25)的約束條件中，得到：

Y1min≤F1x1e(k)+Φ1(ΔU1+ΔDu1)≤Y1max

(26)

根據(jù)速度約束(6)，提取相關(guān)速度約束，可得到控制時(shí)域內(nèi)的北東位置速度約束：

ΔU1min≤ΔU1≤ΔU1max

(27)

根據(jù)UUV自身約束式(7)，可得到控制時(shí)域內(nèi)的UUV北東位置自身約束：

(28)

基于以上分析，綜合約束條件式(26)、式(27)、式(28)，可以得到UUV對接北東位置約束條件：

(29)

設(shè)計(jì)北東位置目標(biāo)函數(shù)為：

minJ1=(Y1d-Y1)T(Y1d-Y1)+ΔU1TRw1ΔU1

(30)

求得的控制量變化量為：

那么，得到北東位置控制量的增量為:

(32)

2)深度預(yù)測模型約束處理與優(yōu)化求解。

根據(jù)UUV終端對接約束(4)，提取相應(yīng)的深度約束，可得到預(yù)測時(shí)域內(nèi)的深度終端對接約束：

Y2min≤Y2≤Y2max

(33)

式中：Y2min=ζmin；Y2max=ζmax。

將深度預(yù)測模型(18)代入式(33)的約束條件中，得到：

Y2min≤F2x2e(k)+Φ2(ΔU2+ΔDu2)≤Y2max

(34)

根據(jù)式(6)所示的碰撞約束，得到控制時(shí)域內(nèi)的縱傾碰撞約束：

ΔU2min≤ΔU2≤ΔU2max

(35)

根據(jù)UUV自身約束(7)，將其在控制時(shí)域內(nèi)展開，可得到控制時(shí)域內(nèi)的UUV深度自身約束：

(36)

式中：A21、A22與式(28)相同，僅維數(shù)不同，A21維數(shù)為Nc2×1，A22維數(shù)為Nc2×Nc2。

綜合約束條件式(26)、(27)、(28)，可得到UUV對接深度約束條件：

(37)

下一步，設(shè)計(jì)深度目標(biāo)函數(shù)為:

求得的控制量變化量為：

那么，得到深度控制量的增量為:

Δu2(k+1)=Δτw(k+1)=ΔU2(1)

(40)

3)縱傾預(yù)測模型約束處理與優(yōu)化求解。

根據(jù)UUV自身約束式(7)，將其在控制時(shí)域內(nèi)展開，得到控制時(shí)域內(nèi)的UUV縱傾自身約束：

(41)

式中：A31、A32與式(28)相同，僅維數(shù)不同，A31維數(shù)為Nc3×1，A33維數(shù)為Nc3×Nc3。

設(shè)計(jì)縱傾目標(biāo)函數(shù)為：

(42)

求得的控制量變化量為：

那么，得到深度控制量的增量為:

Δu3(k+1)=Δτq(k+1)=ΔU3(1)

(44)

4)航向預(yù)測模型約束處理與優(yōu)化求解。

根據(jù)UUV自身約束式(7)，將其在控制時(shí)域內(nèi)展開，得到控制時(shí)域內(nèi)的UUV航向自身約束：

(45)

式中：A41、A42與式(28)相同，僅維數(shù)不同，A41維數(shù)為Nc4×1，A42維數(shù)為Nc4×Nc4。

下一步，設(shè)計(jì)航向目標(biāo)函數(shù)為：

求得的控制量變化量為：

那么，得到航向控制量的增量為:

Δu4(k+1)=Δτr(k+1)=ΔU4(1)

(48)

3.3 滾動時(shí)域控制

基于上節(jié)討論，得出了不同預(yù)測模型、不同目標(biāo)函數(shù)下求解的控制量增量式(32)、(40)、(44)和(48)。由此，可得UUV下一時(shí)刻的控制量：

(49)

將u(k+1)作為實(shí)際的控制輸入量作用于UUV系統(tǒng)，得出UUV在k+1時(shí)刻的狀態(tài)量，然后通過約束條件下滿意MPC控制算法，繼續(xù)解算出第k+2時(shí)刻的控制量u(k+2)，如此循環(huán)迭代求解UUV輸入控制量，直至完成動基座UUV對接控制。

4 仿真結(jié)果與分析

在動基座UUV對接的仿真過程中，回收母船做定深圓周運(yùn)動。母船的初始速度為[umvmwmqmrm]=[0.3 0.05 0 0 -0.0157]，位姿狀態(tài)為[ξmηmζmθmψm]=[45.6 -23.8 12.49 0 -4.646]，初始控制力與力矩為[τmuτmvτmwτmqτmr]=[30 5 0 0 0]，對接UUV的初始速度為[uvwqr]=[0.27 0.155 0 0 -0.0157]，初始位姿狀態(tài)為[ξηζθψ]=[48-319.50-4.59]，UUV初始控制力與力矩為[τuτvτwτqτr]=[27.4 19.4 0 0 1.32]。海流干擾力大小為8 N，仿真時(shí)間為500 s。本次仿真對比了在約束條件下，UUV動基座對接過程中滿意MPC和MPC之間的差異，并給出了相應(yīng)的分析。

UUV三維運(yùn)動軌跡如圖3所示，UUV初始位置距離母船較遠(yuǎn)。約束條件下MPC控制的UUV經(jīng)過較大的波動后才收斂到達(dá)回收母船運(yùn)動軌跡；約束條件下滿意MPC控制的UUV軌跡平滑，并且UUV深度一直處于母船的上方，這就保證了UUV能夠穩(wěn)定獲取運(yùn)動回收母船上對接裝置的聲學(xué)信號。

圖3 UUV三維軌跡對比圖

從圖4(a)的縱向誤差對比圖中可以看出，滿意MPC比MPC能夠更快地達(dá)到穩(wěn)定狀態(tài)，并且誤差更小;從圖4(b)橫向誤差對比圖中可以看出，滿意MPC比MPC下UUV與母船的橫向誤差小，且波動范圍更?。粡膱D4(c)垂向誤差對比圖中可以看出，滿意MPC下UUV與母船的誤差變化比MPC下更為平滑，雖然穩(wěn)態(tài)誤差較前者偏大，但未發(fā)生超調(diào)，符合對接安全性的要求；從圖4(d)的縱傾角誤差可以看出，滿意MPC比MPC波動更小，穩(wěn)定時(shí)間更短，穩(wěn)態(tài)誤差小。從圖4(e)的航向角誤差可以看出，滿意MPC比MPC的航向控制更為穩(wěn)定，誤差收斂速度更快，控制精度高。因此，綜合圖4的仿真結(jié)果圖來看，在動基座UUV對接過程中，在約束條件下，滿意MPC相比于MPC具有較好的穩(wěn)定性與快速性。

圖4 UUV位置和姿態(tài)誤差對比

從圖5的UUV的速度對比圖可以看出，滿意MPC下UUV輸出的速度、角速度更為平滑，這表明在存在海流干擾的情況下，滿意MPC相比于MPC具有更強(qiáng)的抗干擾能力。除此之外，滿意MPC使得UUV的(角)速度能夠更快地穩(wěn)定達(dá)到母船的(角)速度，這說明滿意MPC在計(jì)算速度上要明顯優(yōu)于MPC。

圖5 UUV速度和角速度對比

為了更真實(shí)的體現(xiàn)UUV與回收母船的對接軌跡，現(xiàn)將運(yùn)動坐標(biāo)系下UUV的中心點(diǎn)o′移動到對接柱下端o，即UUV的中心點(diǎn)o′縱向移動0.1 m，橫向移動0 m，垂向移動0.3 m，UUV運(yùn)動坐標(biāo)系與大地坐標(biāo)系之間的轉(zhuǎn)換如圖6所示。

圖6 UUV運(yùn)動坐標(biāo)系轉(zhuǎn)換

圖7展示了UUV下端對接柱與回收母船的三維軌跡。從仿真結(jié)果來看，UUV下端的對接柱能夠平滑地收斂到回收母船的運(yùn)動軌跡，證明了所設(shè)計(jì)的約束滿意MPC對接控制方法的有效性。

圖7 對接柱與回收母船三維軌跡

5 結(jié)論

1)能夠靈活處理UUV與運(yùn)動母船對接過程中存在的各種復(fù)雜約束，與傳統(tǒng)的MPC控制算法相比更加符合工程應(yīng)用需求。

2)結(jié)合滿意控制思想與MPC理論，設(shè)計(jì)了一種在復(fù)雜約束條件下的多目標(biāo)多自由度滿意MPC動基座UUV對接控制算法，減少了系統(tǒng)計(jì)算量，滿足了多自由度同時(shí)最優(yōu)。

3) 通過對MPC對接、約束滿意MPC對接控制仿真結(jié)果分析，本文所設(shè)計(jì)的滿意MPC在海流干擾下仍能實(shí)現(xiàn)UUV對運(yùn)動回收母船的對接，達(dá)到了預(yù)期效果。