[發(fā)明專利]連續(xù)沖擊載荷下的水下潛器運(yùn)動(dòng)姿態(tài)估計(jì)及控制方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201710332816.8
• 申請(qǐng)日: 2017-05-12
• 公開（公告）號(hào): CN107037821B
• 公開（公告）日: 2019-10-29
• 發(fā)明（設(shè)計(jì)）人: 李智生;閻肖鵬;劉可;王紅萍
• 申請(qǐng)（專利權(quán)）人: 中國(guó)人民解放軍91550部隊(duì)
• 主分類號(hào): G05D1/08
• 分類號(hào): G05D1/08
• 代理公司: 中國(guó)人民解放軍海軍專利服務(wù)中心 11044
• 代理人: 陳昭男

本發(fā)明公開了一種連續(xù)沖擊載荷下的水下潛器運(yùn)動(dòng)姿態(tài)估計(jì)及控制方法，在本發(fā)明中通過分析水下潛器的運(yùn)動(dòng)響應(yīng)情況，基于其六自由度運(yùn)動(dòng)方程和動(dòng)力學(xué)方程，結(jié)合水下潛器水下狀態(tài)的平衡方程，建立水下潛器低速定深航行運(yùn)動(dòng)仿真模型，考慮連續(xù)發(fā)射水下航行體時(shí)產(chǎn)生的連續(xù)沖擊載荷，添加發(fā)射水下航行體的反沖力和水錘效應(yīng)模型，對(duì)連續(xù)沖擊載荷下的水下潛器運(yùn)動(dòng)姿態(tài)進(jìn)行估計(jì)，并建立相應(yīng)的水下潛器深度與縱傾的PID姿態(tài)控制方法。

技術(shù)領(lǐng)域

本發(fā)明屬于水下運(yùn)動(dòng)目標(biāo)的姿態(tài)估計(jì)及控制領(lǐng)域，尤其涉及一種連續(xù)沖擊載荷影響下的水下潛器運(yùn)動(dòng)姿態(tài)估計(jì)及控制技術(shù)。

背景技術(shù)

作為發(fā)射平臺(tái)的水下潛器必須滿足苛刻的平衡狀態(tài)才能為被發(fā)射的水下航行體提供良好的初始狀態(tài)。但是被發(fā)射的水下航行體一旦點(diǎn)火，水下潛器受到劇烈沖擊，引起升沉、縱傾、橫傾等一系列運(yùn)動(dòng)和姿態(tài)變化，使其平衡狀態(tài)遭到破壞，這直接影響所發(fā)射的水下航行體的發(fā)射狀態(tài)和精度。因此，如何對(duì)連續(xù)沖擊載荷環(huán)境下的水下潛器姿態(tài)進(jìn)行估計(jì)和控制，為其所發(fā)射的水下航行體提供良好的初始狀態(tài)，是一個(gè)值得研究的問題。

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

本發(fā)明通過分析水下潛器的運(yùn)動(dòng)響應(yīng)情況，基于其六自由度運(yùn)動(dòng)方程和動(dòng)力學(xué)方程，結(jié)合水下潛器水下狀態(tài)的平衡方程，建立水下潛器低速定深航行運(yùn)動(dòng)仿真模型，考慮連續(xù)發(fā)射水下航行體時(shí)產(chǎn)生的連續(xù)沖擊載荷，添加發(fā)射水下航行體的反沖力和水錘效應(yīng)模型，對(duì)連續(xù)沖擊載荷下的水下潛器運(yùn)動(dòng)姿態(tài)進(jìn)行估計(jì)，并建立相應(yīng)的水下潛器深度與縱傾的PID姿態(tài)控制方法。

建立固連于大地的地面坐標(biāo)系ox₀y₀z₀，原點(diǎn)o置于運(yùn)動(dòng)時(shí)刻潛器浮心在水面上的投影點(diǎn)，ox₀z₀坐標(biāo)平面與水面重合，y₀軸垂直向上。再建立原點(diǎn)位于浮心的潛器坐標(biāo)系oxyz，ox軸指向潛器頭部，oy軸在潛器縱對(duì)稱面內(nèi)與ox軸垂直，oz軸垂直于oxy平面，指向按右手系確定。速度坐標(biāo)系原點(diǎn)與潛器坐標(biāo)系原點(diǎn)重合，速度坐標(biāo)系ox'軸與原點(diǎn)處潛器的速度矢量方向重合，oy'軸位于潛器的縱對(duì)稱面內(nèi)，垂直于ox'軸并指向上方，oz'軸與ox'y'z'平面垂直，指向按右手系確定。地面坐標(biāo)系、速度坐標(biāo)系和潛器坐標(biāo)系示意圖如附圖1所示。

潛器在空間的位置由潛器浮心在地面坐標(biāo)系內(nèi)的坐標(biāo)給出，潛器在空間的姿態(tài)由三個(gè)歐拉角確定。這三個(gè)歐拉角的定義為：將潛器坐標(biāo)系平移至與其原點(diǎn)與地面坐標(biāo)系原點(diǎn)重合的位置，再從該位置開始，依次繞oy、oz、ox軸旋轉(zhuǎn)三個(gè)角度使得兩個(gè)坐標(biāo)系完全重合，我們將三個(gè)角度的組合稱為歐拉角，并將這三個(gè)角度用于表示潛器的空間姿態(tài)，分別稱為偏航角、俯仰角和橫滾角。

由地面系ox₀y₀z₀到潛器系oxyz的轉(zhuǎn)換矩陣為

反之，由潛器系oxyz到地面系ox₀y₀z₀的轉(zhuǎn)換矩陣為的轉(zhuǎn)置，即

潛器運(yùn)動(dòng)的攻角與側(cè)滑角由潛器坐標(biāo)系與速度坐標(biāo)系之間的相對(duì)位置定義。速度軸ox'在潛器縱對(duì)稱面oxy內(nèi)的投影與潛器縱軸ox之間的夾角，稱為攻角，記為α；速度軸ox'與潛器縱對(duì)稱面oxy平面的夾角稱為側(cè)滑角，記為β。

水下潛器漂浮于水面任一水線或者潛伏于水下保持平衡的充分必要條件為：

(1)水下潛器受到的重力等于所受到的浮力；

(2)重力與浮力的作用線在同一條鉛垂線上。