預測模型，然后求解優(yōu)化問題，若有可行解（丫1<，(^，￥1〇，根據下式計算控制量1 (t)和pk;否則增大 〇，直到其有解，然后根據下式計算控制量T(t)和pk;
[0155] Kk =1,0,1
[0156] r(/) = K,；v, (/),V/e. [/,,,,),k > 0
[0157] 將控制量作用于系統，然后k = k+l，返回刷新預測模型和重復滾動優(yōu)化過程，直至 實現與母船的對線控位過程，從而實現精確地對線控位，使得UUV坐落到母船回收平臺上。
[0158] 得到UUV與母船的位置誤差曲線如圖5(a)、圖5(b)和圖5(c)所示，從仿真結果可以 看出，魯棒約束模型預測控制在母船運動情況下能夠有效地對UUV進行安全、穩(wěn)定的回收。
【主權項】
1.基于魯棒約束模型預測控制的uuv對線控位回收方法，其特征在于包括w下步驟： 步驟1、通過對線控位方法得到UUV與母船的位置偏差值Δχ，Ay，Δζ、航向偏差值ΔΦ 和縱傾角偏差值ΔΘ;并將Δχ，Ay，Δζ，ΔΦ，ΔΘ作為初始化值； 步驟2、獲取當前時刻UUV狀態(tài)： 通過UUV自身的傳感器得到當前時刻狀態(tài):U、V、W分別為縱向、橫向和垂向速度，Ρ、q、r 分別為橫搖角、縱搖角、臘搖角速度，ξ、η、ζ分別為UUV相對于固定坐標系的位置，妒θ、φ為橫 搖角、縱搖角、臘搖角(rad); 步驟3、構建誤差預測模型，并計算局部時刻預測誤差動態(tài)方程的系統矩陣Qk和局部時 刻狀態(tài)預測誤差兩(0; 利用UUV六自由度動力學方程和運動學方程構造動態(tài)方程：y(t)=h(x(t) ,τ(?)) 式中，x(t)是UUV狀態(tài)向量，y(t)是輸出向量，T(t)是UUV控制輸入向量，ω (t)是干擾向 量；;-(/)為x(t)的一階導數;t表示時間；其中，XprDp，YprDp，ZprDp為UUV受到的縱向、橫向、垂向的作用力，Vdp，Mprcp，Nprcp為UUV受 到的縱向、橫向、垂向作用力的力矩;u(t)、v(t)、w(t)、p(t)、q(t)、;r(t)、C(t)、ri(t)、C(t)、 口(/)、目（t)、lKt)、Xprop(t)、Yprop(t)、Zpr叩（t)、Kprop(t)、Mpr叩（t)、Nprop(t)均表示對應參數為 時間t的函數； 定義UUV期望狀態(tài)其中，所有帶角標d的參數均表示對應參數的期望值； 則實際和期望之間誤差表示為Φ(?)-Φ??(?) = ΔΦ,θ(?)-θ(?(?) = ΔΘ, 因此建立誤差動態(tài)方程ye(t) =C(t)xe(t)+D(t)x(t) (1) 式中，4(〇、81(*)、82(*)、(：4)、0(*)均為系數矩陣； 方程(1)的系統矩陣為：在局部時刻[tk，tk+l)的預測誤差動態(tài)方程為：式中，所有帶角標k參數的均表示局部時刻的對應參數，tiO、巧和)表示Xe(t)、ye(t) 局部時刻的預測值； 方程(2)的系統矩陣步驟4、針對滾動優(yōu)化問題，在約束條件下進行滾動優(yōu)化，求解優(yōu)化 問題，求可行解（丫 k，Qk，Yk); 步驟5、針對優(yōu)化問題有可行解（丫 k，Qk，&)，根據下式計算控制輸入向量T(t)，Φ)=於在(0·々'/E II)，足 >() 將控制輸入向量作用于方程(2)的系統，然后令k = k+l，返回步驟2更新預測模型、重復 滾動優(yōu)化過程，直至實現UUV對線控位回收。2.根據權利要求1所述的基于魯棒約束模型預測控制的UUV對線控位回收方法，其特征 在于步驟4的具體步驟如下： 滾動優(yōu)化問題蘭足如下約束： 視線約束:Α^χα)^^，間個數； 干擾約束:Ηω <1，COER3; 其中，為常值矩陣，ler為常值列向量，η為線性約束個數；a < acd+σ)； 式中，e康示iT2空間的一個基函數，m2為空間維數，P0 = Xe ( to ) TPoXe ( to )表示to時刻的誤 差狀態(tài)向量Xe(to)與楠圓屯、Po的距離，Pk-l = Xe(tk-〇Tpk-lXe(tk-l)表示tk-l時刻的誤差狀態(tài)向 量Xe(tk-l)與楠圓屯、Pk-1的距離，Xe(tk)表示tk時刻UUV的誤差狀態(tài)向量，Tj,max表示在tk時刻 的j維的干擾力和力矩的最大值，丫功拉格朗日算子，表示性能指標，Qk為對稱矩陣，Kk為預 ii的狀態(tài)反饋增益，化= KkQk，qi、q2是優(yōu)化加權系數，a是要優(yōu)化的楠圓域的半徑，ac是楠圓 域半徑的給定值，σ為控制參數； 枉制輸入細足I Tj (t) I < Tj,max二Tmax, j二1,2 , . . . ,Π12表不全間維數； Tmax- [Xprop,max Yprop,max Zprop,max Kprop,max Mprop,max Nprop,max] 其中，帶有角標max的參數均表示對應參數的最大值； 針對W上約束條件對優(yōu)化問題求可行解（丫 k，Qk，&)。3. 根據權利要求2所述的基于魯棒約束模型預測控制的UUV對線控位回收方法，其特征 在于步驟4所述的視線約束ALCLX(l)<dL是通過W下步驟得到的： 視線約束屬于凸約束，采用多面體的形式進行表示，設其半空間表示為:曰護M+C店-dj <0，j = l，2,…，化，化個半空間相交形成多面體，因此視線約束可W采用如下形式表示?. Np. 其中Nf為多面體面數，將上式轉換為矩陣型式如下： AlClXQ) <山， 其中，X(l)表示轉換為矩陣 型式后的待求解矩陣。4. 根據權利要求2所述的基于魯棒約束模型預測控制的UUV對線控位回收方法，其特征 在于步驟4所述的避碰約束曼通過W下步驟得到的： 避碰約束屬于非凸約束，根據UUV和母船的外形結構及對接裝置，采用多個凸多面體的 方法進行近似數學表示，具體表示如下：將"or"約束通過采用大數定理和二進制變量進行處理將其轉化為"and"約束，引入大 數Η和二進制變量SQi(i = l，···，N。)得到轉換為矩陣形式如下其中'N。為半空 間個數。5. 根據權利要求1、2、3或4所述的基于魯棒約束模型預測控制的UUV對線控位回收方 法，其特征在于步驟4所述對優(yōu)化問題求可行解（丫 k，Qk，化)的過程中，若優(yōu)化問題無可行解 (丫 k，Qk，化），則增大0,再進行滾動優(yōu)化，直到其有解為止。6. 根據權利要求5所述的基于魯棒約束模型預測控制的UUV對線控位回收方法，其特征 在于步驟1所述的通過對線控位方法得到UUV與母船的位置偏差值Δχ，Ay，Δζ、航向偏差 值ΔΦ和縱傾角偏差值Δ Θ的具體步驟如下： 在基于母船的坐標系下，母船回收對接平臺上的臘艇導引燈的位置坐標分別表示為 (Χβ，Υβ，Zb )、（Xs，Ys，Zs)，則UUV腹部臘艇攝像頭設備的位置坐標分別表示為(X' Β，Υ' Β，Ζ' Β)、 (X's，Y's，Z's); UUV腹部臘艇攝像頭與母船回收平臺臘艇導引燈的位置之差，根據其各位置坐標偏差 值分別表示為（exB, eyB, ezB)、（exs, eys, ezs); 具體表達式為：exB = XB-X' B，eyB = YB_Y' B，ezB = ZB_Z' B，exs = Xs-X's，eys = Ys_Y' s，ezs = Zs-Z's; 進行變換處理：(巧Β-巧s)與航向偏差存在近似的比例關系：ΔΦ = σι (eyB-eys); 式中，ΔΦ表示UUV與母船的航向偏差值，〇1是其比例系數； 根據上述方法，將UUV與母船的縱傾角偏差表示為:Δ Θ = 02 (ezB-ezs) 式中，ΛΘ表示UUV與母船的縱傾角偏差值，02是比例系數； 并將Δχ，Ay，Δζ，ΔΦ，Δθ作為初始化值。
【專利摘要】基于魯棒約束模型預測控制的UUV對線控位回收方法，涉及一種水下母船背馱式搭載UUV的自主回收方法。為了解決現有水下回收UUV的方法主要是針對靜止的回收平臺對UUV進行回收的問題以及回收過程中由于超調過大、調節(jié)時間過長等不穩(wěn)定因素會使得UUV與回收平臺發(fā)生碰撞的問題；本發(fā)明首先獲得UUV與母船狀態(tài)信息，構建誤差預測模型，并計算局部時刻預測誤差動態(tài)方程的系統矩陣和局部時刻狀態(tài)預測誤差；在約束條件下進行滾動優(yōu)化，求解優(yōu)化問題；然后計算控制量并新預測模型、重復滾動優(yōu)化過程，直至實現與母船的對線控位過程。本發(fā)明適用于水下母船背馱式搭載UUV的自主回收。
【IPC分類】G05B13/04
【公開號】CN105700356
【申請?zhí)枴緾N201610104470
【發(fā)明人】張偉, 滕彥斌, 陳海田, 陳濤, 李娟 , 嚴浙平
【申請人】哈爾濱工程大學
【公開日】2016年6月22日
【申請日】2016年2月25日