約束條件下基于滿意模型預測控制的動基座水下無人航行器對接控制

張偉， 伍文華， 滕延斌， 宮鵬， 唐業競

(1.哈爾濱工程大學 水下機器人技術重點實驗室，黑龍江 哈爾濱 150001； 2.中國船舶科學研究中心 水動力學國防重點實驗室，江蘇 無錫 214082)

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

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

1 UUV回收對接系統與模型

1.1 UUV回收對接系統

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

圖1 UUV回收對接系統

1.2 對接UUV模型

為了研究上的方便,根據UUV的主要特征,現作出如下合理假設：

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

2)UUV外形的斷面、切面和截面是3個嚴格對稱的對稱面；

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

基于以上假設，可建立UUV五自由度的動力學與運動學模型[15]：

(1)

(2)

其中：

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

1.3 回收母船模型

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

回收母船主要參數形式如下：

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

2 多種約束問題分析

在動基座UUV對接過程中，存在著各種復雜約束。本節從UUV回收對接安全性、UUV自身物理限制、UUV所受環境干擾力等角度出發，將多種約束問題主要歸為狀態約束、自身約束和干擾約束三大類。

2.1 狀態約束

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

1)終端對接約束。

終端對接約束主要是根據回收母船上的位置劃定對接安全區域和禁止駛入區域，使得UUV在終端對接過程中處于相對安全的狀態。動基座UUV終端對接安全區如圖2所示。

圖2 動基座UUV終端對接安全區示意

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

(3)

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

在第k時刻，可將UUV運動坐標系下的約束(3)，轉化為大地坐標系下的約束：

(4)

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

2)速度約束。

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

(5)

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

根據UUV動力學模型，速度變化量約束(5)可轉化為控制力變化量約束：

(6)

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

2.2 自身約束

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

umin≤u(k)≤umax

(7)

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

2.3 干擾約束

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

1)海流約束。

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

(8)

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

2)近壁面約束。

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

(9)

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

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

(10)

3 控制器設計

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

3.1 預測模型

1)構建UUV北東位置預測模型。

首先根據UUV五自由度運動學模型(1)、動力學模型(2)，引入相應的干擾約束(10)，構建UUV北東位置模型：

(11)

將式(11)表示成連續狀態空間模型：

(12)

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

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

然后，根據北東位置離散化模型(13)構建UUV北東位置誤差增強模型：

(14)

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

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)構建UUV深度預測模型。

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

(16)

同樣地，可得到UUV深度誤差增強模型：

(17)

將模型(17)在預測時域Np2和控制時域Nc2內展開，得到深度預測模型：

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)構建UUV縱傾預測模型。

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

(19)

同樣地，可得到UUV深度誤差增強模型：

(20)

將模型(20)在預測時域Np3和控制時域Nc3內展開，得到縱傾預測模型：

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)構建UUV航向預測模型。

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

(22)

同樣地，可得到UUV航向誤差增強模型：

(23)

將模型(23)在預測時域Np4和控制時域Nc4內展開，得到航向預測模型：

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 約束處理與目標函數優化求解

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

根據終端對接約束(4)，提取相應的大地坐標系下北東位置約束，可得到預測時域內的北東位置終端對接約束：

Y1min≤Y1≤Y1max

(25)

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

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

(26)

根據速度約束(6)，提取相關速度約束，可得到控制時域內的北東位置速度約束：

ΔU1min≤ΔU1≤ΔU1max

(27)

根據UUV自身約束式(7)，可得到控制時域內的UUV北東位置自身約束：

(28)

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

(29)

設計北東位置目標函數為：

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

(30)

求得的控制量變化量為：

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

(32)

2)深度預測模型約束處理與優化求解。

根據UUV終端對接約束(4)，提取相應的深度約束，可得到預測時域內的深度終端對接約束：

Y2min≤Y2≤Y2max

(33)

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

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

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

(34)

根據式(6)所示的碰撞約束，得到控制時域內的縱傾碰撞約束：

ΔU2min≤ΔU2≤ΔU2max

(35)

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

(36)

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

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

(37)

下一步，設計深度目標函數為:

求得的控制量變化量為：

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

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

(40)

3)縱傾預測模型約束處理與優化求解。

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

(41)

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

設計縱傾目標函數為：

(42)

求得的控制量變化量為：

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

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

(44)

4)航向預測模型約束處理與優化求解。

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

(45)

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

下一步，設計航向目標函數為：

求得的控制量變化量為：

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

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

(48)

3.3 滾動時域控制

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

(49)

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

4 仿真結果與分析

在動基座UUV對接的仿真過程中，回收母船做定深圓周運動。母船的初始速度為[umvmwmqmrm]=[0.3 0.05 0 0 -0.0157]，位姿狀態為[ξ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]，初始位姿狀態為[ξηζθψ]=[48-319.50-4.59]，UUV初始控制力與力矩為[τuτvτwτqτr]=[27.4 19.4 0 0 1.32]。海流干擾力大小為8 N，仿真時間為500 s。本次仿真對比了在約束條件下，UUV動基座對接過程中滿意MPC和MPC之間的差異，并給出了相應的分析。

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

圖3 UUV三維軌跡對比圖

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

圖4 UUV位置和姿態誤差對比

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

圖5 UUV速度和角速度對比

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

圖6 UUV運動坐標系轉換

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

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

5 結論

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

2)結合滿意控制思想與MPC理論，設計了一種在復雜約束條件下的多目標多自由度滿意MPC動基座UUV對接控制算法，減少了系統計算量，滿足了多自由度同時最優。

3) 通過對MPC對接、約束滿意MPC對接控制仿真結果分析，本文所設計的滿意MPC在海流干擾下仍能實現UUV對運動回收母船的對接，達到了預期效果。