[發(fā)明專利]一種基于CCKF的水下無(wú)人潛器姿態(tài)測(cè)量方法

在審

• 申請(qǐng)?zhí)枺?/td>: 202011290382.8
• 申請(qǐng)日: 2020-11-17
• 公開(kāi)（公告）號(hào): CN112729279A
• 公開(kāi)（公告）日: 2021-04-30
• 發(fā)明（設(shè)計(jì)）人: 王亞波;楊建;羅濤;魏世樂(lè);王春;袁克非;楊宗元;黃蔚;張士濤;燕熊
• 申請(qǐng)（專利權(quán)）人: 武漢第二船舶設(shè)計(jì)研究所（中國(guó)船舶重工集團(tuán)公司第七一九研究所）
• 主分類號(hào): G01C21/16
• 分類號(hào): G01C21/16;G01C21/20
• 代理公司: 北京理工大學(xué)專利中心 11120
• 代理人: 劉芳

本發(fā)明提供一種基于CCKF的水下無(wú)人潛器姿態(tài)測(cè)量方法，具體過(guò)程為：在UUV上固定安裝捷聯(lián)慣性導(dǎo)航設(shè)備、磁強(qiáng)計(jì)，進(jìn)行設(shè)備調(diào)試及時(shí)間校準(zhǔn)；設(shè)定UUV組合測(cè)姿系統(tǒng)的狀態(tài)方程；設(shè)定UUV組合測(cè)姿系統(tǒng)的量測(cè)方程；根據(jù)約束容積卡爾曼濾波的時(shí)間更新步驟，基于狀態(tài)方程及捷聯(lián)慣性導(dǎo)航設(shè)備測(cè)量的三維角速度，完成姿態(tài)測(cè)量濾波算法的時(shí)間更新；在時(shí)間更新的基礎(chǔ)上，根據(jù)系統(tǒng)量測(cè)方及捷聯(lián)慣導(dǎo)設(shè)備、磁強(qiáng)計(jì)輸出的三維姿態(tài)信息，完成姿態(tài)測(cè)量濾波算法的量測(cè)更新；根據(jù)得到的狀態(tài)估計(jì)，計(jì)算UUV的三個(gè)姿態(tài)角，實(shí)現(xiàn)基于捷聯(lián)慣性導(dǎo)航設(shè)備/磁強(qiáng)計(jì)組合測(cè)姿的UUV姿態(tài)測(cè)量。

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

本發(fā)明涉及的是一種水下無(wú)人潛器(UUV)姿態(tài)測(cè)量方法，特別是涉及運(yùn) 動(dòng)狀態(tài)受約束的UUV姿態(tài)測(cè)量方法，尤其是涉及一種基于CCKF(約束容積卡爾 曼濾波)的UUV姿態(tài)測(cè)量方法。

背景技術(shù)

隨著海洋開(kāi)發(fā)需求的拓展以及海軍裝備技術(shù)的進(jìn)步，水下無(wú)人潛器(UUV) 以其體積靈活，操作便捷，功能強(qiáng)大的優(yōu)勢(shì)受到越來(lái)越多的關(guān)注。姿態(tài)測(cè)量 作為UUV導(dǎo)航、控制、制導(dǎo)能力的基礎(chǔ)技術(shù)受到越來(lái)越多的關(guān)注，相對(duì)于 飛行器而言，UUV的運(yùn)動(dòng)變化相對(duì)緩慢，但是測(cè)量手段又相對(duì)單一，主要依 靠慣性以及磁場(chǎng)傳感器。特別是由于水下環(huán)境復(fù)雜，在很多情況下，要求UUV 的姿態(tài)只能在一定范圍內(nèi)變化，因此考慮運(yùn)動(dòng)狀態(tài)受約束的UUV姿態(tài)測(cè)量 方法顯得尤為重要。

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

有鑒于此，本發(fā)明的目的在于提供一種基于CCKF的水下無(wú)人潛器姿態(tài)測(cè) 量方法，能夠?qū)崿F(xiàn)在UUV姿態(tài)受約束情況下的狀態(tài)估計(jì)，提高UUV的導(dǎo)航能 力。

本發(fā)明通過(guò)以下步驟實(shí)現(xiàn)：

一種基于CCKF的水下無(wú)人潛器姿態(tài)測(cè)量方法，具體過(guò)程為：

(1)在UUV上固定安裝捷聯(lián)慣性導(dǎo)航設(shè)備、磁強(qiáng)計(jì)，進(jìn)行設(shè)備調(diào)試及時(shí) 間校準(zhǔn)；

(2)設(shè)定UUV組合測(cè)姿系統(tǒng)的狀態(tài)方程，所述狀態(tài)方程通過(guò)捷聯(lián)慣性導(dǎo) 航設(shè)備輸出的姿態(tài)角速度信息可進(jìn)行更新；

(3)設(shè)定UUV組合測(cè)姿系統(tǒng)的量測(cè)方程，所述量測(cè)方程利用捷聯(lián)慣性導(dǎo) 航設(shè)備、磁強(qiáng)計(jì)輸出的載體姿態(tài)信息進(jìn)行更新；

(4)根據(jù)約束容積卡爾曼濾波的時(shí)間更新步驟，基于步驟(2)定義的狀 態(tài)方程及捷聯(lián)慣性導(dǎo)航設(shè)備測(cè)量的三維角速度，完成姿態(tài)測(cè)量濾波算法的時(shí)間 更新；

(5)在時(shí)間更新的基礎(chǔ)上，根據(jù)步驟(3)設(shè)定的系統(tǒng)量測(cè)方及捷聯(lián)慣導(dǎo) 設(shè)備、磁強(qiáng)計(jì)輸出的三維姿態(tài)信息，完成姿態(tài)測(cè)量濾波算法的量測(cè)更新；

(6)根據(jù)步驟(5)量測(cè)更新得到的狀態(tài)估計(jì)，計(jì)算UUV的三個(gè)姿態(tài)角， 實(shí)現(xiàn)基于捷聯(lián)慣性導(dǎo)航設(shè)備/磁強(qiáng)計(jì)組合測(cè)姿的UUV姿態(tài)測(cè)量。

具體過(guò)程為：

第一步，將捷聯(lián)慣性導(dǎo)航設(shè)備、磁強(qiáng)計(jì)固定安裝在UUV共用平臺(tái)上，保證 兩設(shè)備輸出信息的時(shí)間軸同步，捷聯(lián)慣性導(dǎo)航設(shè)備輸出地理坐標(biāo)系下UUV三維 姿態(tài)r、三維角速度ω，磁強(qiáng)計(jì)輸出載體坐標(biāo)系下UUV三維姿態(tài)b；

第二步，選取UUV載體坐標(biāo)系與地理坐標(biāo)系間的轉(zhuǎn)動(dòng)四元數(shù) q＝[q₁ q₂ q₃ q₄]^T作為組合測(cè)姿系統(tǒng)狀態(tài)量，即：

X＝q

X為UUV組合測(cè)姿系統(tǒng)狀態(tài)量，根據(jù)四元數(shù)運(yùn)動(dòng)方程，狀態(tài)方程定義為：

其中，q_k+1、q_k分別代表UUV在T_k+1、T_k時(shí)刻的四元數(shù)值，為系統(tǒng)狀態(tài)矩陣， ω_k為T_k時(shí)刻慣導(dǎo)設(shè)備測(cè)量的三維角速度；且有：