[發(fā)明專利]一種用于水下滑翔器的高精度組合導(dǎo)航定位方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201210585170.1
• 申請(qǐng)日: 2012-12-30
• 公開（公告）號(hào): CN103033186A
• 公開（公告）日: 2013-04-10
• 發(fā)明（設(shè)計(jì)）人: 陳熙源;黃浩乾;劉虎;周智愷;唐建;方琳
• 申請(qǐng)（專利權(quán)）人: 東南大學(xué)
• 主分類號(hào): G01C21/16
• 分類號(hào): G01C21/16;G01C21/20
• 代理公司: 南京天翼專利代理有限責(zé)任公司 32112
• 代理人: 黃明哲

1.一種用于水下滑翔器的高精度組合導(dǎo)航定位方法，其特征是以慣性測(cè)量單元IMU為導(dǎo)航原件，包括以下步驟：

1）將慣性測(cè)量單元IMU的輸出數(shù)據(jù)輸入無(wú)限脈沖響應(yīng)IIR低通濾波器，去除掉粗大噪聲，完成數(shù)據(jù)的粗處理；

2）對(duì)粗處理后的數(shù)據(jù)進(jìn)行細(xì)處理，所述細(xì)處理為計(jì)算粗處理后得到的數(shù)據(jù)的高斯分布均值和方差；

3）采用基于龍格－庫(kù)塔法Runge-Kutta的航位推算DR，推算每個(gè)更新周期內(nèi)滑翔器的位置、速度和姿態(tài)；

4）設(shè)置兩級(jí)濾波系統(tǒng)，第一級(jí)濾波為自適應(yīng)卡爾曼濾波AKF，第二級(jí)濾波是無(wú)跡卡爾曼濾波UKF，自適應(yīng)卡爾曼濾波AKF通過(guò)自適應(yīng)模糊控制調(diào)節(jié)濾波增益，通過(guò)自適應(yīng)強(qiáng)跟蹤的擴(kuò)展卡爾曼濾波EKF預(yù)測(cè)均方誤差，所述濾波增益與預(yù)測(cè)均方誤差共同調(diào)節(jié)得到估計(jì)均方誤差，將所述估計(jì)均方誤差代入無(wú)跡卡爾曼濾波UKF，得到兩級(jí)濾波系統(tǒng)，兩級(jí)濾波系統(tǒng)對(duì)輸入數(shù)據(jù)進(jìn)行UKF濾波；

5）將步驟2）得到的高斯分布均值和方差，以及步驟3）得到的推算數(shù)據(jù)融合進(jìn)行UKF濾波，由UKF估出的誤差反饋回慣性測(cè)量單元IMU，校正慣性測(cè)量單元IMU的累積誤差，維持慣性測(cè)量單元IMU的精度，輸出準(zhǔn)確的位置、速度和姿態(tài)信息，實(shí)現(xiàn)穩(wěn)定的導(dǎo)航定位。

2.根據(jù)權(quán)利要求1所述的一種用于水下滑翔器的高精度組合導(dǎo)航定位方法，其特征是步驟3）的基于龍格－庫(kù)塔法Runge-Kutta的航位推算DR具體為：設(shè)航位推算的采樣周期為T，在采樣周期內(nèi)進(jìn)行m次速度、位置和姿態(tài)的信息更新，每次更新的周期為采樣前速度為v₀，在采樣周期內(nèi)每次更新的速度為v_j，j=1,2，....，m，采樣后速度為v_m，則在時(shí)間t內(nèi)更新的速度由式(b)遞推：

vj+1=vj+t6(k1+2k2+2k3+k4)k1=f(t,vj)k2=f(t+12h,vj+12hk1)k3=f(t+12h,vj+12hk2)k4=f(t+h,vj+hk3)---(b)]]>

推導(dǎo)得到速度v_j，然后再代入式(c)中：

ve=vjcos(P)sin(H)vn=vjcos(P)cos(H)vu=vjsin(P)---(c)]]>

式中v_e,v_n,v_u分別表示推算后東北天三向速度，H,P,R分別表示航向角、俯仰角和橫滾角，然后根據(jù)三個(gè)方向的速度再推算位移，這樣在每個(gè)更新周期得到速度、位置和姿態(tài)信息。