[發(fā)明專利]基于模型預(yù)測(cè)控制的AUV回收對(duì)接動(dòng)力定位控制方法

有效

• 申請(qǐng)?zhí)枺?/td>: 202010985822.5
• 申請(qǐng)日: 2020-09-18
• 公開(kāi)（公告）號(hào): CN112068440B
• 公開(kāi)（公告）日: 2022-12-02
• 發(fā)明（設(shè)計(jì)）人: 曾慶軍;謝爭(zhēng)明;郭雨青;朱志宇;戴曉強(qiáng)
• 申請(qǐng)（專利權(quán)）人: 江蘇科技大學(xué)
• 主分類號(hào): G05B13/04
• 分類號(hào): G05B13/04
• 代理公司: 南京經(jīng)緯專利商標(biāo)代理有限公司 32200
• 代理人: 徐澍

1.一種基于模型預(yù)測(cè)控制的AUV回收對(duì)接動(dòng)力定位控制方法,其特征在于，該方法包含下列步驟：

1)初始化：建立大地坐標(biāo)系與運(yùn)動(dòng)坐標(biāo)系，構(gòu)建AUV六自由度空間運(yùn)動(dòng)方程，AUV根據(jù)當(dāng)前回收對(duì)接任務(wù)指令設(shè)定深度、姿態(tài)角、軌跡及速度期望值信息，進(jìn)而計(jì)算與實(shí)際深度、姿態(tài)角、軌跡及速度之間誤差；

2)狀態(tài)估計(jì)：利用AUV的傳感器所提供的深度、姿態(tài)、軌跡及速度信息并根據(jù)狀態(tài)估計(jì)算法對(duì)AUV的實(shí)際狀態(tài)進(jìn)行估計(jì)；

狀態(tài)估計(jì)包括以下步驟：

步驟(2.1)：狀態(tài)估計(jì)初始化：給定系統(tǒng)狀態(tài)X₀和方差的初始值P₀，并將AUV六自由度運(yùn)動(dòng)和傳感器測(cè)量方程簡(jiǎn)化：

式中：f(·)是AUV六自由度方程；h(·)是傳感器測(cè)量方程；x是AUV當(dāng)前的狀態(tài)；是x的一階導(dǎo)數(shù)；ang＝[γ,θ,ψ]^T是AUV姿態(tài)角；γ、θ、ψ分別為橫滾角、縱傾角、航向角；為估計(jì)的推進(jìn)器系統(tǒng)的推力；y為傳感器測(cè)量值；

將上式離散化為：

式中：F(·)＝1+Δt·f(·)為離散后的AUV六自由度近似方程；Δt為采樣時(shí)間；下標(biāo)k、k-1分別表示為k、k-1時(shí)刻的狀態(tài)；

步驟(2.2)：利用UKF無(wú)跡卡爾曼濾波算法估計(jì)當(dāng)前系統(tǒng)狀態(tài)，其中采樣點(diǎn)及其對(duì)應(yīng)權(quán)值的計(jì)算為：

其中，λ為尺度參數(shù)，χ_0,k-1表示初始采樣點(diǎn)，χ_i,k-1表示第i個(gè)采樣點(diǎn)；和分別為初始采樣點(diǎn)均值和方差的權(quán)值，和分別對(duì)應(yīng)第j個(gè)采樣點(diǎn)均值和方差的權(quán)值，α表示采樣點(diǎn)圍繞均值擴(kuò)散程度，為非負(fù)的權(quán)系數(shù)；β為運(yùn)動(dòng)變量；表示k-1時(shí)刻估計(jì)誤差協(xié)方差，P_k-1表示k-1時(shí)刻的協(xié)方差矩陣；

步驟(2.3):引入自適應(yīng)縮放因子Δ_k，針對(duì)在AUV面對(duì)不同環(huán)境自適應(yīng)調(diào)整其觀測(cè)權(quán)值，減少環(huán)境對(duì)狀態(tài)估計(jì)造成的影響,UKF先驗(yàn)均方差公式為：

其中為不包含過(guò)程噪聲的先驗(yàn)均方差；Q_k為過(guò)程噪聲的方差；P_k'為引入自適應(yīng)因子的先驗(yàn)均方差；通過(guò)引入Δ_k對(duì)于不同程度的噪聲取得不同的權(quán)值，提高自適應(yīng)性；

步驟(2.4):時(shí)間更新:將時(shí)間t更新為t＝t+1；

步驟(2.5):根據(jù)狀態(tài)是否滿足動(dòng)力定位要求判斷任務(wù)是否結(jié)束，若沒(méi)結(jié)束則根據(jù)當(dāng)前狀態(tài)，以模型預(yù)測(cè)控制器控制量輸出完成推力分配，并根據(jù)步驟(2.2)繼續(xù)執(zhí)行狀態(tài)估計(jì)；

3)線性處理：根據(jù)實(shí)際狀態(tài)估計(jì)值與設(shè)定狀態(tài)期望值的誤差，將其線性部分輸出至模型預(yù)測(cè)控制器；

4)設(shè)計(jì)RBF神經(jīng)網(wǎng)絡(luò)補(bǔ)償器：計(jì)算實(shí)際狀態(tài)估計(jì)值與設(shè)定狀態(tài)期望值誤差中的非線性部分并利用帶自調(diào)整功能的RBF神經(jīng)網(wǎng)絡(luò)補(bǔ)償器產(chǎn)生補(bǔ)償量；

5)設(shè)計(jì)模型預(yù)測(cè)控制器：以線性處理和RBF神經(jīng)網(wǎng)絡(luò)補(bǔ)償器的補(bǔ)償量作為輸入量利用模型預(yù)測(cè)控制器產(chǎn)生控制量，依據(jù)模型預(yù)測(cè)控制控制量輸出進(jìn)行推力分配完成AUV回收對(duì)接動(dòng)力定位控制。

2.如權(quán)利要求1所述的基于模型預(yù)測(cè)控制的AUV回收對(duì)接動(dòng)力定位控制方法,其特征在于，步驟1)所述初始化為:建立大地坐標(biāo)系E-ξηζ和運(yùn)動(dòng)坐標(biāo)系O-x'y'z'，大地坐標(biāo)系以水平面一點(diǎn)E為原點(diǎn)，ξ軸指向地理北向、η軸指向地理東向，ζ軸指向地心；運(yùn)動(dòng)坐標(biāo)系的原點(diǎn)O在AUV上，x'、y'、z'三軸分別指向AUV的艏向、右舷和底部；構(gòu)建AUV六自由度空間運(yùn)動(dòng)方程，AUV根據(jù)當(dāng)前回收對(duì)接任務(wù)指令設(shè)定深度、姿態(tài)角、軌跡及速度期望值信息，通過(guò)與實(shí)際狀態(tài)估計(jì)反饋信息進(jìn)行比較，進(jìn)而計(jì)算與實(shí)際深度、姿態(tài)角、軌跡及速度之間誤差。

3.如權(quán)利要求1所述的基于模型預(yù)測(cè)控制的AUV回收對(duì)接動(dòng)力定位控制方法,其特征在于，步驟3)針對(duì)AUV系統(tǒng)為復(fù)雜的非線性系統(tǒng)，將其非線性系統(tǒng)分解為非線性部分與線性部分疊加，其中線性部分作為單獨(dú)部分輸出至控制器。