基于魯棒約束模型預(yù)測(cè)控制的uuv對(duì)線控位回收方法
【技術(shù)領(lǐng)域】
[0001 ]本發(fā)明涉及一種水下母船背馱式搭載UUV的自主回收方法。
【背景技術(shù)】
[0002] UUV技術(shù)目前已經(jīng)作為探索海洋空間資源的最重要手段之一，UUV雖然具有隱蔽 性、深潛性能、安全性等優(yōu)點(diǎn)，但是由于UUV受到自身攜帶的能源限制，使其工作范圍和時(shí)間 受到嚴(yán)格的限制，因此要通過UUV水下回收實(shí)現(xiàn)在水下完成能源補(bǔ)給和上傳下載數(shù)據(jù)，這可 以大大提高UUV的工作范圍和時(shí)間。
[0003] 由于魯棒約束模型預(yù)測(cè)控制方法能夠有效的處理較復(fù)雜的狀態(tài)約束和控制約束 以及對(duì)參數(shù)不確定性系統(tǒng)有非常好的控制性能，因此本發(fā)明采用基于魯棒約束模型預(yù)測(cè)控 制算法的精確對(duì)線控位方法對(duì)UUV進(jìn)行水下回收。
[0004] 哈爾濱工程大學(xué)的張偉等人在《船舶工程》（2013年第05期)上發(fā)表了《基于對(duì)線控 位策略的UUV回收運(yùn)動(dòng)控制研究》，該文章對(duì)于水下塢艙回收UUV的過程提出了一種基于對(duì) 線控位策略的回收方法，并設(shè)計(jì)UUV回收的位置和姿態(tài)的灰色預(yù)測(cè)PID控制器，減小了 UUV回 收控制中的超調(diào)量和調(diào)整時(shí)間，但是該方法針對(duì)的是水下靜止平臺(tái)，而非移動(dòng)平臺(tái)，存在一 定的局限性。

【發(fā)明內(nèi)容】

[0005] 本發(fā)明是為了解決現(xiàn)有水下回收UUV的方法主要是針對(duì)靜止的回收平臺(tái)對(duì)UUV進(jìn) 行回收的問題以及回收過程中由于超調(diào)過大、調(diào)節(jié)時(shí)間過長(zhǎng)等不穩(wěn)定因素會(huì)使得UUV與回 收平臺(tái)發(fā)生碰撞的問題。
[0006] 基于魯棒約束模型預(yù)測(cè)控制的UUV對(duì)線控位回收方法，包括以下步驟：
[0007] 步驟1、通過對(duì)線控位方法得到UUV與母船的位置偏差值Λχ，Ay，Λζ、航向偏差值 ΛΦ和縱傾角偏差值Λθ;并將Λχ，Ay，Λζ，ΛΦ，Λθ作為初始化值；
[0008] 步驟2、獲取當(dāng)前時(shí)刻UUV狀態(tài)：
[0009] 通過UUV自身的一系列傳感器等得到當(dāng)前時(shí)刻狀態(tài):u、ν、w分別為縱向、橫向和垂 向速度(m/s)，p、q、r分別為橫搖角、縱搖角、艏搖角速度(rad/s)，ξ、η、ζ分別為UUV相對(duì)于固 定坐標(biāo)系的位置(πι)，％θ、φ為橫搖角、縱搖角、艏搖角( rad);
[0010] 步驟3、構(gòu)建誤差預(yù)測(cè)模型，并計(jì)算局部時(shí)刻預(yù)測(cè)誤差動(dòng)態(tài)方程的系統(tǒng)矩陣 局部時(shí)刻狀態(tài)預(yù)測(cè)誤差元
[0011] 利用UUV六自由度動(dòng)力學(xué)方程和運(yùn)動(dòng)學(xué)方程構(gòu)造動(dòng)態(tài)方程：
[0012] i(，).二 /(λ.(，), r(,，)，w⑴）
[0013] y(t)=h(x(t) ,τ(?))
[0014] 式中，x(t)是UUV狀態(tài)向量，y(t)是輸出向量，τ(〇是UUV控制輸入向量，co(t)是干 擾向量;為x(t)的一階導(dǎo)數(shù);t表示時(shí)間；
[0015] x(t) = [u(t) v(t) w(t) p(t) q(t) r(t)]T
[0016] n{t) C(〇 φ{(diào)() θ{?
[0017] T ( t) = [Xprop ( t) Yprop(t) Zprop(t) Kprop(t) Mprop(t) Nprop(t)]
[0018] 其中，Xprcip，Yprcip，Zprot^UUV受到的縱向、橫向、垂向的作用力，K prcip，Mprcip，Npr。!^ UUV受到的縱向、橫向、垂向作用力的力矩;u(t)、v(t)、w(t)、p(t)、q(t)、r(t)、C(t)、n(t)、G (t)、0(?)、.9(t)、iKt)、X prQp(t)、YprQp(t)、ZprQp(t)、KprQp(t)、M prQp(t)、NprQp(t)均表示對(duì)應(yīng)參 數(shù)為時(shí)間t的函數(shù)；
[0019] 定義UUV期望狀態(tài)
[0020] xd(t) = [ud(t) vd(t) wd(t) pa(t) qa(t) ra(t)]T
[0021] ν,(〇=[^(0 %(0 (0 φ,,{? ^ (0 ψλ (0]γ
[0022] 其中，所有帶角標(biāo)d的參數(shù)均表示對(duì)應(yīng)參數(shù)的期望值；
[0023] 則實(shí)際和期望之間誤差表示為
[0029] 建立誤差動(dòng)態(tài)方程
[0030] ,ν, (/) = A (/).v. (/) + /i, (/)〇.){ l j -l· B, (/) r (/)
[0031] ye(t) = C(t)xe(t)+D(t)x(t) (1)
[0032] 式中，△(〇、81(〇、82(〇、以〇、0(〇均為系數(shù)矩陣；
[0033]方程(1)的系統(tǒng)矩陣為：
[0036] 在局部時(shí)刻[tk，tk+1)的預(yù)測(cè)誤差動(dòng)態(tài)方程為：
[0037] t (〇 = A-K (〇 + Bu,^(0 + Byj(〇
[0038] 兌（?）= (? (/) + D, r(/) (2)
[0039] 式中，所有帶角標(biāo)k參數(shù)的均表示局部時(shí)刻的對(duì)應(yīng)參數(shù)，:^.(/)、；Γ:, (/)表示xjthye (t)局部時(shí)刻的預(yù)測(cè)值；
[0040] 方程(2)的系統(tǒng)矩陣
[0042] 由于預(yù)測(cè)模型采用了線性化誤差系統(tǒng)模型近似描述非線性系統(tǒng)的局部特征，因此 在每個(gè)采樣時(shí)刻需要更新系統(tǒng)矩陣Ω k，獲得新的預(yù)測(cè)模型完成預(yù)測(cè)過程；
[0043]
，在約束條件下進(jìn)行滾動(dòng)優(yōu)化，求解 優(yōu)化問題，求可行解（Tk，Qk，Yk);
[0044] 步驟5、針對(duì)優(yōu)化問題有可行解（丫15，(^，￥1〇，根據(jù)下式計(jì)算控制輸入向量1(〇，
[0045] Kk :YkQ-kl
[0046] r(/) = X, (/), V/ e [/,,,,:), A > 0
[0047] 將控制輸入向量作用于方程(2)的系統(tǒng)，然后令k = k+l，返回步驟2更新預(yù)測(cè)模型、 重復(fù)滾動(dòng)優(yōu)化過程，直至實(shí)現(xiàn)UUV對(duì)線控位回收。
[0048]本發(fā)明具有以下效果：
[0049] 本發(fā)明采用的魯棒約束模型預(yù)測(cè)控制方法不僅能夠針對(duì)靜止回收平臺(tái)對(duì)UUV進(jìn)行 回收，還能針對(duì)低速運(yùn)動(dòng)的回收平臺(tái)對(duì)UUV進(jìn)行回收，通過魯棒約束模型預(yù)測(cè)控制，使得回 收過程中超調(diào)量大大減小，調(diào)節(jié)時(shí)間加快，使得UUV能夠更加穩(wěn)定、安全的進(jìn)行回收。
【附圖說明】
[0050] 圖1為母船運(yùn)動(dòng)狀態(tài)下與UUV的相對(duì)初始位置示意圖；
[0051 ] 圖2為UUV對(duì)接示意圖；
[0052]圖3為魯棒約束模型預(yù)測(cè)控制結(jié)構(gòu)框圖；
[0053]圖4為魯棒模型預(yù)測(cè)控制實(shí)現(xiàn)流程；
[0054]圖5為仿真實(shí)驗(yàn)中UUV與母船的位置誤差曲線圖。
【具體實(shí)施方式】
[0055]【具體實(shí)施方式】一：
[0056]基于魯棒約束模型預(yù)測(cè)控制的UUV對(duì)線控位回收方法，包括以下步驟：
[0057]步驟1、通過對(duì)線控位方法得到UUV與母船的位置偏差值Λχ，Ay，Λζ、航向偏差值 ΛΦ和縱傾角偏差值Λθ;并將Λχ，Ay，Λζ，ΛΦ，Λθ作為初始化值；
[0058] 步驟2、獲取當(dāng)前時(shí)刻UUV狀態(tài)：
[0059]通過UUV自身的一系列傳感器等得到當(dāng)前時(shí)刻狀態(tài):u、ν、w分別為縱向、橫向和垂 向速度(m/s)，p、q、r分別為橫搖角、縱搖角、艏搖角速度(rad/s)，ξ、η、ζ分別為UUV相對(duì)于固 定坐標(biāo)系的位置(m)，Ρ、θ、φ為橫搖角、縱搖角、艏搖角(rad);
[0060]步驟3、構(gòu)建誤差預(yù)測(cè)模型，并計(jì)算局部時(shí)刻預(yù)測(cè)誤差動(dòng)態(tài)方程的系統(tǒng)矩陣 局部時(shí)刻狀態(tài)預(yù)測(cè)誤差t
[0061 ] 利用UUV六自由度動(dòng)力學(xué)方程和運(yùn)動(dòng)學(xué)方程構(gòu)造動(dòng)態(tài)方程：
[0062] f(/) = /(.'-(')，r(r)，iy(/))
[0063] y(t)=h(x(t) ,τ(?))
[0064] 式中，x(t)是UUV狀態(tài)向量，y(t)是輸出向量，τ(〇是UUV控制輸入向量，co(t)是干 擾向量Μ (/)為X (t)的一階導(dǎo)數(shù);t表示時(shí)間；
[0065] x(t) = [u(t) v(t) w(t) p(t) q(t) r(t)]T
[0066] v(r) = [^(r) η{? C[t) φ{(diào)?) θ{?) ^(r)]r
[0067] T ( t) = [Xprop ( t) Yprop(t) Zprop(t) Kprop(t) Mprop(t) Nprop(t)]
[0068] 其中，Xpr〇P，Ypr〇P，ZprciI^UUV受到的縱向、橫向、垂向的作用力，K pr〇P，M