[發(fā)明專利]一種用于水下滑翔器的導(dǎo)航系統(tǒng)及姿態(tài)角校正和回溯解耦方法

有效

• 申請?zhí)枺?/td>: 201410712437.8
• 申請日: 2014-11-28
• 公開（公告）號: CN104406592A
• 公開（公告）日: 2015-03-11
• 發(fā)明（設(shè)計）人: 陳熙源;黃浩乾;周智愷;呂才平
• 申請（專利權(quán)）人: 東南大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 南京瑞弘專利商標事務(wù)所(普通合伙) 32249
• 代理人: 楊曉玲

1.一種用于水下滑翔器的導(dǎo)航系統(tǒng)姿態(tài)角校正方法，其特征在于，包括以下步驟：

步驟A，根據(jù)陀螺儀輸出的角速度、加速度計輸出的加速度、磁力計輸出的磁場強度進行導(dǎo)航信息解算,經(jīng)導(dǎo)航解算后得到姿態(tài)角、速度、位置信息的導(dǎo)航數(shù)據(jù)；

步驟B，根據(jù)導(dǎo)航解算出的導(dǎo)航數(shù)據(jù)，判斷因俯仰或橫滾運動，使得姿態(tài)角交叉耦合明顯增大而造成的姿態(tài)角解算錯誤的節(jié)點，用姿態(tài)角交叉耦合造成解算錯誤的前一步解算數(shù)據(jù)重新計算，得出新的姿態(tài)角、速度、位置信息；

步驟C，步驟B中回溯解耦后的姿態(tài)角、速度及位置信息經(jīng)前置濾波器去噪后，一方面利用誤差方程得到狀態(tài)量，一方面和步驟A中經(jīng)導(dǎo)航解算后得到姿態(tài)角、速度、位置信息分別作差得到觀測量；將該狀態(tài)量和觀測量進入基于卡爾曼的自適應(yīng)濾波算法，進行姿態(tài)角、速度、位置誤差的最優(yōu)估計，所得誤差估計值校正慣導(dǎo)所得的導(dǎo)航信息，最終得到校正后準確的姿態(tài)角、速度和位置。

2.根據(jù)權(quán)利要求1所述的一種用于水下滑翔器的導(dǎo)航系統(tǒng)姿態(tài)角校正方法，其特征在于：所述步驟B中的回溯解耦方法包括以下步驟：

步驟B1，根據(jù)導(dǎo)航解算得出的導(dǎo)航數(shù)據(jù)，判斷因俯仰或橫滾運動，使得姿態(tài)角交叉耦合明顯增大而造成的姿態(tài)角解算錯誤的節(jié)點；

步驟B2，用因俯仰或橫滾運動，使得姿態(tài)角交叉耦合明顯增大而造成解算錯誤的前一步解算數(shù)據(jù)，計算載體坐標系相對導(dǎo)航坐標系的轉(zhuǎn)動角速率在載體坐標系上的投影；

步驟B3，用載體坐標系相對導(dǎo)航坐標系的轉(zhuǎn)動角速率在載體坐標系上的投影通過解算得到新的姿態(tài)角、速度、位置信息。

3.根據(jù)權(quán)利要求2所述的一種用于水下滑翔器的導(dǎo)航系統(tǒng)姿態(tài)角校正方法，其特征在于：步驟B1中判斷因俯仰或橫滾運動，使得姿態(tài)角交叉耦合明顯增大而造成的姿態(tài)角解算錯誤的節(jié)點的方法，包括以下步驟：

步驟B11，因俯仰或橫滾運動，使得姿態(tài)角交叉耦合而造成三個失準角φ_x,φ_y,φ_z錯誤，其中φ_x,φ_y,φ_z分別為載體坐標系的三軸偏離導(dǎo)航坐標系對應(yīng)的三軸的偏離角，也即失準角；

步驟B12，將三個失準角φ_x,φ_y,φ_z代入姿態(tài)角校正方程其中是錯誤解算節(jié)點前一步正確的導(dǎo)航坐標系到載體坐標系的姿態(tài)角矩陣，

CT=1φz-φy-φz1φxφy-φx1]]>為姿態(tài)校正矩陣；

步驟B13，根據(jù)姿態(tài)角校正方程可得出四元數(shù)方程：

|q0|=0.5*1+Cncob(0,0)+Cncob(1,1)+Cncob(2,2)]]>

|q1|=0.5*1+Cncob(0,0)-Cncob(1,1)-Cncob(2,2)---(11)]]>

|q2|=0.5*1-Cncob(0,0)+Cncob(1,1)-Cncob(2,2)]]>

|q3|=0.5*1-Cncob(0,0)-Cncob(1,1)+Cncob(2,2);]]>

步驟B14，判斷式(11)中的根號下的計算結(jié)果是否是負數(shù)，若是負數(shù)，則四元數(shù)q₀、q₁、q₂、q₃錯誤，后續(xù)的導(dǎo)航解算會依次錯誤；從而得出上述式(11)中根號下計算結(jié)果出現(xiàn)負數(shù)是因姿態(tài)角交叉耦合造成導(dǎo)航解算錯誤的節(jié)點。