[發(fā)明專(zhuān)利]一種適用于水下MEMS航向陀螺航向角解算方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201711339294.0
• 申請(qǐng)日: 2017-12-14
• 公開(kāi)（公告）號(hào): CN108151736B
• 公開(kāi)（公告）日: 2021-11-19
• 發(fā)明（設(shè)計(jì)）人: 田新興;李海龍;王薇薇;楊斌
• 申請(qǐng)（專(zhuān)利權(quán)）人: 中船重工西安東儀科工集團(tuán)有限公司
• 主分類(lèi)號(hào): G01C21/16
• 分類(lèi)號(hào): G01C21/16;G01C21/20
• 代理公司: 西安吉盛專(zhuān)利代理有限責(zé)任公司 61108
• 代理人: 江琴賢

1.一種適用于水下MEMS航向陀螺航向角解算方法，其特征是：至少包括：

步驟1：MEMS航向陀螺啟動(dòng)；

步驟2：各傳感器測(cè)量數(shù)據(jù)在坐標(biāo)軸上矢量投影計(jì)算；

步驟3：第一時(shí)間延時(shí)判斷，小于6秒，執(zhí)行下一步；大于等于6秒，執(zhí)行步驟10；

步驟4：?jiǎn)?dòng)后6秒內(nèi)進(jìn)行航向角速率積分計(jì)算，積分結(jié)果作為輸出航向角數(shù)據(jù)，以便快速建立水下航行器的航向基準(zhǔn)；

步驟5：進(jìn)行角速率零位誤差值計(jì)算；

步驟6：第二時(shí)間延時(shí)判斷，小于5秒，執(zhí)行步驟9；大于等于5秒，執(zhí)行下一步；

步驟7：進(jìn)行各軸角速率零位誤差補(bǔ)償修正；

步驟8：后臺(tái)進(jìn)行航向角的四元數(shù)解算，解算結(jié)果不輸出；

步驟9：將準(zhǔn)備好的航向角數(shù)據(jù)輸出，重返步驟2；

步驟10：第三時(shí)間延時(shí)判斷，等于6秒，執(zhí)行下一步；大于6秒，執(zhí)行步驟12；

步驟11：根據(jù)步驟8計(jì)算的前5秒航向角進(jìn)行航向角零位誤差值估算；

步驟12：進(jìn)行正式航向角解算前對(duì)各軸角速率零位誤差補(bǔ)償修正；

步驟13：進(jìn)行航向角的四元數(shù)解算；

步驟14：進(jìn)行航向角4象限拓展計(jì)算；

步驟15：進(jìn)行航向角弧度轉(zhuǎn)角度整理計(jì)算，準(zhǔn)備輸出航向角數(shù)據(jù)，重返步驟9；

所述的步驟2包括：

采集角速率陀螺儀和加速度計(jì)信號(hào)，并按航行器坐標(biāo)軸系定義將各傳感器數(shù)據(jù)矢量折算到航向、橫滾和俯仰軸上，折算包括測(cè)量值極性轉(zhuǎn)換、標(biāo)度系數(shù)換算和軸間安裝耦合疊加，按如下公式組(1)計(jì)算航向、橫滾、俯仰角速率、航向、橫滾、俯仰加速度；

Wx＝S_xxω_x+S_yxω_y+S_zxω_z

Wy＝S_xyω_x+S_yyω_y+S_zyω_z

Wz＝S_xzω_x+S_yzω_y+S_zzω_z (1)

Ax＝g_xxa_x+g_yxa_y+g_zxa_z

Ay＝g_xya_x+g_yya_y+g_zya_z

Az＝g_xza_x+g_yza_y+g_zza_z

其中W_x、W_y、W_z為航向、橫滾、俯仰角速率，A_x、A_y、A_z為航向、橫滾、俯仰加速度，ω_x、ω_y、ω_z為三個(gè)角速率陀螺儀測(cè)量值，a_x、a_y、a_z為三個(gè)加速度計(jì)測(cè)量值，S_*x、S_*y、S_*z角速率陀螺在航向、橫滾、俯仰軸上的投影系數(shù)，g_*x、g_*y、g_*z加速度計(jì)在航向、橫滾、俯仰軸上的投影系數(shù)，各投影系數(shù)通過(guò)轉(zhuǎn)臺(tái)標(biāo)定測(cè)試獲得；

所述的步驟8包括：

設(shè)定當(dāng)前時(shí)刻的解算航向、橫滾、俯仰角為Ψ_i、Ф_i、θ_i，則前一時(shí)刻的解算航向角為Ψ_i-1，規(guī)定解算航向角初始值為0°，航向角0～360°范圍，橫滾角和俯仰角±80°范圍，當(dāng)前時(shí)刻的解算橫滾和俯仰角計(jì)算公式見(jiàn)(7)和(8)；

Ф_i＝arctg(A_z/A_x) (7)

θ_i＝arctg(A_y/A_x) (8)

航向角為Ψ_i解算采用四元數(shù)進(jìn)行，四元數(shù)解算首先按公式組(9)計(jì)算當(dāng)前時(shí)刻四元系數(shù)q₀、q₁、q₂、q₃，角度和角速率計(jì)算時(shí)采用單位為弧度；

按步驟7公式組(6)進(jìn)行各軸角速率零偏補(bǔ)償后，根據(jù)當(dāng)前四元數(shù)系數(shù)q₀、q₁、q₂、q₃值按公式組見(jiàn)(10)～(13)計(jì)算解算中間變量值k1、k2、k3、k4；

k1[0]＝(Wx*q₁+Wy*q₂+Wz*q₃)/(-200)

k1[1]＝(Wy*q₃-Wx*q₀-Wz*q₂)/(-200) (10)

k1[2]＝(Wz*q₁-Wy*q₀-Wx*q₃)/(-200)

k1[3]＝(Wx*q₂-Wy*q₁-Wz*q₀)/(-200)

根據(jù)解算中間變量值k1、k2、k3、k4按公式組(14)迭代計(jì)算當(dāng)前四元數(shù)系數(shù)q₀、q₁、q₂、q₃；

接上一步，根據(jù)當(dāng)前四元數(shù)系數(shù)q₀、q₁、q₂、q₃按公式組(15)計(jì)算當(dāng)前航向角為Ψ_i，并進(jìn)行航向角4個(gè)象限拓展計(jì)算；