雙陀螺抗擾動尋北方法
【專利摘要】本發(fā)明公開了一種雙陀螺抗擾動尋北方法，其步驟包括：①初始化尋北系統(tǒng)；②陀螺/加速度計數(shù)據(jù)采集；③初始裝訂俯仰角、橫滾角，構(gòu)建天向虛擬信息，實現(xiàn)全姿態(tài)慣性器件信息的構(gòu)建；④利用慣性對準(zhǔn)技術(shù)，計算并輸出方位角的信息，結(jié)束尋北。本發(fā)明在尋北過程中出現(xiàn)外界擾動的情況下，仍能保證尋北系統(tǒng)工作正常，尋北結(jié)果有效，真正做到擾動下的尋北；省去了轉(zhuǎn)位機(jī)構(gòu)等機(jī)械結(jié)構(gòu)件，降低尋北系統(tǒng)內(nèi)部結(jié)構(gòu)復(fù)雜度、減輕尋北系統(tǒng)重量；還能實時給出載體的姿態(tài)和方位信息。
【專利說明】
雙陀螺抗擾動尋北方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明屬于慣性尋北技術(shù)領(lǐng)域，具體涉及一種雙陀螺抗擾動的尋北方法。
【背景技術(shù)】
[0002] 陀螺尋北系統(tǒng)是慣性技術(shù)應(yīng)用的重要成果之一，它通過敏感地球自轉(zhuǎn)來確定北 向，能全天候可靠地工作，不受外部環(huán)境如地球磁場、地形、氣候等的影響，可以在沒有任何 外部方位信息的情況下實現(xiàn)自主尋北功能，測出當(dāng)?shù)氐乩肀毕?。目前的尋北系統(tǒng)，絕大多數(shù) 采用單陀螺，依靠轉(zhuǎn)位機(jī)構(gòu)實現(xiàn)位置轉(zhuǎn)動后結(jié)算相應(yīng)的北向。按轉(zhuǎn)動位置可以分為二位置 法、四位置法、多位置法，以及連續(xù)旋轉(zhuǎn)調(diào)試。無論采用什么尋北方式，尋北系統(tǒng)在使用的過 程中總會受到各種擾動的影響，對于車載尋北系統(tǒng)擾動源主要有陣風(fēng)、發(fā)動機(jī)的振動、人員 的走動、地基的沉降等，基座的擾動會嚴(yán)重降低系統(tǒng)的尋北精度。為了減小各種擾動對尋北 精度的影響，目前采用的方法都局限于對慣性儀表即陀螺、加速度計輸出值的濾波及數(shù)字 處理，這些方法可以有效消除高頻擾動對尋北系統(tǒng)的影響。在實際應(yīng)用中，尤其是基座擾動 中包含較大低頻噪聲時，使用直接對慣性儀表輸出值濾波及數(shù)字處理的方法往往得不到比 較理想的效果。

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

[0003] 針對現(xiàn)有技術(shù)所存在的不足，本發(fā)明所解決的問題在于，怎樣提供一種在存在擾 動的情況下仍能正常工作，輸出有效結(jié)果，保證尋北精度的尋北方法。
[0004] 為實現(xiàn)上述目的，本發(fā)明采用如下方案：
[0005] 雙陀螺抗擾動尋北方法，具體步驟如下：
[0006] 步驟1:對尋北系統(tǒng)進(jìn)行初始化；
[0007] 步驟2:采集X軸、Y軸光纖陀螺與加速度計輸出信息，標(biāo)定慣性器件的參數(shù);具體來 說，1)在一個煒度已知，且標(biāo)定的轉(zhuǎn)臺下，通過采集均勻分布的12個水平位置的X軸、Y軸的 陀螺輸出數(shù)據(jù)，標(biāo)定出雙光纖陀螺誤差模型中的8個參數(shù);2)在轉(zhuǎn)臺下，通過敏感正負(fù)地球 重力加速度信息，采集6種條件下，X軸、Y軸加速度計輸出數(shù)據(jù)，標(biāo)定出雙加速度計誤差模型 中的8個參數(shù)；
[0008] 步驟3a:對俯仰角和橫滾角兩個參數(shù)是否已初始裝訂過進(jìn)行判斷，若判斷為否，則 初始計算裝訂俯仰角和橫滾角兩個參數(shù);若判斷為是，則采用更新后的俯仰角和橫滾角參 數(shù)；
[0009] 步驟3b:將步驟2中標(biāo)定的16個參數(shù)與步驟3a中所述的俯仰角、橫滾角兩個參數(shù)一 同構(gòu)建天向虛擬信息，即天向陀螺虛擬信息和天向加速度計虛擬信息，從而實現(xiàn)對全姿態(tài) 慣性器件信息的構(gòu)建；
[0010]步驟4:利用慣性系對準(zhǔn)技術(shù)，計算方位角、俯仰角、橫滾角的信息；用計算出的橫 滾角和俯仰角信息更新步驟3a中所述的橫滾角和俯仰角參數(shù);輸出計算出的方位角信息， 尋北過程結(jié)束。
[0011] 進(jìn)一步地，所述步驟4在尋北過程結(jié)束前，設(shè)置判斷條件來重復(fù)執(zhí)行步驟2~4以獲 得符合要求的尋北結(jié)果;若判斷為是，則尋北過程結(jié)束，若判斷為否，則返回執(zhí)行步驟2。
[0012] 另外，步驟2中對陀螺輸出數(shù)據(jù)的采集位置可不限于12個，也可以是4個、8個等等。 由于采用線性回歸來擬合參數(shù)，在0-360°的范圍內(nèi)，數(shù)據(jù)分布越均勻，則擬合的參數(shù)越準(zhǔn) 確。若增加采集位置，一是耗時，二是過多的采集位置對于擬合而言意義不大;若采集位置 過少，則擬合出的參數(shù)精度不高。綜合考慮以上因素，要實現(xiàn)本發(fā)明的目的，采集位置應(yīng)不 少于4個。
[0013] 進(jìn)一步地，所述步驟2中慣性器件的具體標(biāo)定方法如下：
[0014]相應(yīng)的姿態(tài)矩陣為： cos V cos y/ + sin γ sin (// sin Θ - cos γ sin ψ + sin y cosv/ sin Θ -sin ycosO
[0015] C1' - sin I；/cos (9 G〇sf// GQS ^ sin Θ (1) si n ;/ cos ψ - cos y si n i,;/ si η Θ -sin vsin;// -cos/cos^sin Θ cos γ cos Θ
[0016] 1)光纖陀螺參數(shù)標(biāo)定：
[0017] 雙光纖陀螺誤差模型：
[0018] X軸陀螺：
[0019] ~⑵
[0020] Υ軸陀螺：
[0021 ] ^hy=Ky+KK+K^+k:y4,s C3)
[0022] 上面兩式中，Wbx，Wby分別是X軸和Y軸陀螺輸出信息，W Qx，WQy分別是X軸和Y軸陀螺輸 出零位，< (? = X, >，, z)是陀螺對載體角速度的測量值，Kxx，Kyy分別是X軸和Y軸陀螺刻度系 數(shù)，kxy，kzx，kxy，kzy分別是陀螺交叉耦合系數(shù)；
[0023]在煒度已知，且水平靜止條件下，有如下三式： 〇)!.?χ = -ω.^ sin ψ cos L
[0024] = (〇u, cos ψ cos L (4) = K sin /-.
[0025] 上式中，coie3是地球自轉(zhuǎn)角速度，為常量;將(4)式帶入(2)、（3)式，有
[0026] Wbx=W〇x+Kxx(- ω iesinitcosL)+kyx( ω iecositcosL)+kzx( ω iesinL) (5)
[0027] Wby=W〇y+kxy(- ω iesinitcosL)+Kyy( ω iecositcosL)+kzy( ω iesinL) (6)
[0028] 標(biāo)定陀螺過程中，始終保持水平靜止，標(biāo)定方位點依次選取，相 應(yīng)的X軸陀螺和Y軸陀螺輸出為
~w w ….w ?
[0029] Wb= bx〇 ivl - ^vlIJt2x2
[0030] 構(gòu)建如下兩個矩陣，
[0033] X矩陣中的8個參數(shù)，即為雙光纖陀螺誤差模型(2)、（3)式的參數(shù)，所以在一個煒度 已知，且標(biāo)定的轉(zhuǎn)臺下，通過采集12水平位置的陀螺輸出數(shù)據(jù)，可標(biāo)定出以上8個參數(shù)； [0034] 2)加速度計標(biāo)定
[0035]雙加速度計誤差模型如下： n+KHdf?
[0036] , h (8) Vay - Kuyn + A + Kayyfv + ^azyfz
[0037] 上面兩式中，Vax，Vay分別是X軸和Y軸加速度計輸出信息，KaxQ，K axQ分別是X軸和Y軸 加速度計輸出零位，f 〇' = X、％ 2):是加速度計對載體比力的測量值，Kaxx，Kayy分別是X軸和 Y軸加速度計刻度系數(shù)，k axy，kazx，kaxy，kazy分別是加速度計交叉耦合系數(shù)；
[0038]加速度計標(biāo)定時候，靜止條件下，依次選取X軸、Y軸、Z軸加速度計敏感正負(fù)重力加 速，相應(yīng)的X軸加速度計和Y軸加速度計輸出為 ψ ψ γ γ γ "Γ
[0039] V(= m-?+客 ^_\-g in-r+g ai_r-g a\_z hg ax_z-g ^uv^vf? Kiv_r-g
[0040] 構(gòu)建如下兩個矩陣， -i i ο 0- 1-10 ο 10 1 ο κ
[0041 ] 4= Χ= "-10 Km,x Kayy 1001 Lu-L ―1 0 0 - 1 丄 κ4 'ΑΧ = V
[0042] I： f I νι r (9) Χ=(ΑΓΑ) ATVa
[0043] X矩陣中的8個參數(shù)，即為雙加速度計誤差模型（8)式的參數(shù)，所以在轉(zhuǎn)臺下，通過 敏感正負(fù)地球重力加速度信息，采集6種條件下，X軸Υ軸加速度計輸出數(shù)據(jù)，可標(biāo)定出以上8 個參數(shù)。
[0044] 進(jìn)一步地，所述步驟3b中天向虛擬信息的具體構(gòu)建方法如下：
[0045] 1)天向陀螺虛擬信息構(gòu)建
[0046] 根據(jù)歐拉方程有：
[0048] 考慮角速度動態(tài)約束條件，載體方位上角速度為零，即
[0049] ψ = 0 (11)
[0050] 帶入上式有，得到姿態(tài)角速度約束方程：
(.1:2)
[0052] 整理得到：
[0053] 〇4=1叫;咐二 （13)
[0054]考慮到有下式成立
[0055] e4 -+ ?4. = 4?, +tai1 ~dJ (W)
[0056] 根據(jù)(2)、（3)中的誤差模型知道，X軸陀螺和Y軸陀螺的輸出為 「71 「q卜?卜心乙f吟 m _7] kH、啦,.
[0058] 將（14)帶入（15)，得到： Γ ? Γ ? Γ ?「 Wbx - W0x = k'' k:x ,f - K. - kxv Kfl k:'' bC0J b
[0059] L .」l .」. .」L:6>"K + tan7(ft);M. -e^)」 (16) ^Kxx + kzx tan / kyx ? Γ ω? ? Γ kzx ? 1234567 其中 2 4 = cX=<K+<) <17) 3 由于尋北時系統(tǒng)在導(dǎo)航坐標(biāo)系中是靜止的，導(dǎo)航坐標(biāo)系相對地球坐標(biāo)系沒有轉(zhuǎn) 動，所以 為零，因此在東北天坐標(biāo)系下 "0 " 4 % cos! (18) caje sin L 5 因此有： KX\\wbx]\w0^\kzX b h ~ 6 +^,,. tanr [[whv\ L^0vJ [kzyp- dnr^JJ (19) ?L = ?L+tan r((0L - 4?-) 7 2)天向加速度計虛擬信息構(gòu)建
[0067] 根據(jù)比力方程有：
[0068] vB: - CJh - (2< + αξη) χ vH + g" (20)：
[0069] 考慮載體線運動的動態(tài)約束條件，垂直方向上的速度和加速度為零，以及水平方 向上速度為〇,即
[0070] v" =v =0 (,21)
[0071] 帶入上式有 χ χ χ. β 0
[0072] vj! = χ χ χ ,/'Λ - 0 (22) v'l -sin f cos (9 sin Θ cos γ cos Θ fb g
[0073] 整理得天向加速度信息為：
[0074] v11 = -sin γ cos Θ/^ + sin (9/^ + cos;/ cos Of1' - g = 0 .(2.3)
[0075] Z軸加速度計輸出為：
(24)
[0077] 根據(jù)(8)式中的誤差模型知道，X加速度計和Y加速度計的輸出為 _ 「f6 - γ ? [ ? Γ [ k k 1 ·*
[0078] U'.- U'. u= · u".廣 fl (25) _dv_ _?ν〇_ Ruyy Ai:y p
[0079] 將(24)帶入(25)，得到：
1234 因此有： 2
(27) 3 根據(jù)上面(19)和(27)構(gòu)造的數(shù)據(jù)，就構(gòu)成完整的三自由度慣性器件數(shù)據(jù)。 4 進(jìn)一步地，所述步驟4中利用慣性系對準(zhǔn)計算方位角的方法如下：
[0085]姿態(tài)矩陣的四元數(shù)形式如下
[0086] (L(t) = q：{t)?cf；-；'?ci：Jt) (28)
[0087] 其中
(29)
[0089] 初始時刻，<(〇) = <(0) = [1 0: 0 0f，尋北過程中，載體處于相對靜止，及沒有速度， 此時 <_ = β€=[0 .i%cosi： 為(19)式中的陀螺數(shù)據(jù)，即 對 (29)式采用四元數(shù)微分方程的畢卡求解法可以實時更新得到<(0和式。(0;
[0090] 由于有
[0091] q'：{t)fh{t)^：{t)^〇;' (30)
[0092] 將(28)式帶入(30)式得：
(31)
[0094] 整理得到：
(32)
[0096] 式中左核函數(shù)和右核函數(shù)分別為： β? ~β\ ~~β? ~β^ -~(Χχ ~(^2 ~^3 /? 爲(wèi).-:Α 爲(wèi) Af'i \ αι α? α? _α2
[0097] Μ.(β)= 〇 Μ(α)= ; β? β'3 Pq - β\ OC.Q ?γ -β, β' 爲(wèi)」 ?ο _
[0098] 為求解式6，轉(zhuǎn)換成求矩陣/C = 〃/Jf最小特征值對應(yīng)的特征矢量即可，將求解得 到后的帶入(28)式得到新的乂 (〇,就可得到方位、俯仰、橫滾信息；
[0099] 方位： （33)
[0100] 俯仰：
[0101] 橫滾： （35)
[0102] 其中ft 備 ％]。
[0103] 進(jìn)一步地，所述步驟3a中俯仰角與滾動角的初始裝訂的方法如下：
[0104] 對照(27)式，不考慮天向相關(guān)參數(shù)，有： Γ η 「/Π「u.'.TU「I。]"! %、
[0105] = . ..-. i36)
[f：\ [K,y ^,rJ LL^vJ L^，vaJJ
[0106] 此時利用水平加速計輸出信息計算俯仰角和橫滾角：
[0107] // (37) 〇 y =- a 簡(f ':' /g
[0108] 在已經(jīng)成熟的抗擾動初始對準(zhǔn)技術(shù)中，及慣性系下對準(zhǔn)技術(shù)，需要全姿態(tài)的三軸 陀螺輸出信息，三軸加速度計輸出信息，對準(zhǔn)完畢后，方位信息就是尋北系統(tǒng)最終的輸出結(jié) 果。在雙陀螺抗擾動尋北方法，利用一定的公式，構(gòu)造出天向角速率信息和加速度信息（均 為虛擬信息），這樣虛擬構(gòu)造出全姿態(tài)慣性信息，再利用慣性系下的對準(zhǔn)技術(shù)，就可以實現(xiàn) 抗擾動下尋北技術(shù)。
[0109] 相比于現(xiàn)有技術(shù)，本發(fā)明具有如下有益效果：
[0110] 1、本發(fā)明的優(yōu)點在于在尋北過程中出現(xiàn)陣風(fēng)、發(fā)動機(jī)振動、人員走動、地基沉降等 擾動的情況下，尤其是基座擾動中包含較大低頻噪聲時，仍能保證尋北系統(tǒng)工作正常，尋北 結(jié)果有效，真正做到擾動下的尋北；
[0111] 2、傳統(tǒng)尋北儀系統(tǒng)中，依靠轉(zhuǎn)位機(jī)構(gòu)，實現(xiàn)單陀螺在不同時間段內(nèi)，分別敏感相互 正交4個位置上的地球自轉(zhuǎn)角速度分量信息進(jìn)行尋北解算;本發(fā)明的優(yōu)點在于省去了轉(zhuǎn)位 機(jī)構(gòu)等機(jī)械結(jié)構(gòu)件，降低尋北系統(tǒng)內(nèi)部結(jié)構(gòu)復(fù)雜度、減輕尋北系統(tǒng)重量；
[0112] 3、傳統(tǒng)尋北儀系統(tǒng)中，只能在最后尋北結(jié)束完畢后給出方位信息;本發(fā)明的優(yōu)點 在于具備可以實時給出載體的姿態(tài)和方位信息。
【附圖說明】
[0113] 圖1為雙陀螺抗擾動尋北方法實施例的流程圖。
【具體實施方式】
[0114] 在本發(fā)明的標(biāo)定以及計算過程中，均采用東北天坐標(biāo)系，312轉(zhuǎn)序，即繞-Z軸轉(zhuǎn)動Φ (方位角），然后繞X軸轉(zhuǎn)動θ(俯仰角），最后繞Y軸轉(zhuǎn)動γ(橫滾角），慣性器件標(biāo)定位置的煒 度為L。下面結(jié)合附圖對本發(fā)明作進(jìn)一步詳細(xì)描述，但本發(fā)明的實施方式不限于此。
[0115] 實施例：
[0116] 雙陀螺抗擾動尋北方法，如圖1所示，具體步驟如下：
[0117] 步驟1:對尋北系統(tǒng)進(jìn)行初始化。
[0118] 步驟2:采集X軸、Υ軸光纖陀螺與加速度計輸出信息，標(biāo)定慣性器件的參數(shù);具體來 說，1)在一個煒度已知，且標(biāo)定的轉(zhuǎn)臺下，通過采集均勻分布的12個水平位置的X軸、Υ軸的 陀螺輸出數(shù)據(jù)，標(biāo)定出雙光纖陀螺誤差模型中的8個參數(shù);2)在轉(zhuǎn)臺下，通過敏感正負(fù)地球 重力加速度信息，采集6種條件下，X軸、Υ軸加速度計輸出數(shù)據(jù)，標(biāo)定出雙加速度計誤差模 型中的8個參數(shù)。
[0119]步驟3a:對俯仰角和橫滾角兩個參數(shù)是否已初始裝訂過進(jìn)行判斷，若判斷為否，則 初始計算裝訂俯仰角和橫滾角兩個參數(shù);若判斷為是，則采用更新后的俯仰角和橫滾角參 數(shù)。
[0120]步驟3b:將步驟2中標(biāo)定的16個參數(shù)與步驟3a中所述的俯仰角、橫滾角兩個參數(shù)一 同構(gòu)建天向虛擬信息，即天向陀螺虛擬信息和天向加速度計虛擬信息，從而實現(xiàn)對全姿態(tài) 慣性器件信息的構(gòu)建。
[0121] 步驟4:利用慣性對準(zhǔn)技術(shù)，計算方位角、俯仰角、橫滾角的信息；用計算出的橫滾 角和俯仰角信息更新步驟3a中所述的橫滾角和俯仰角參數(shù);輸出計算出的方位角信息，尋 北過程結(jié)束;在結(jié)束尋北之前，可通過設(shè)置判斷條件來重復(fù)執(zhí)行步驟2~4以獲得符合要求 的尋北結(jié)果，比如將"尋北時間<3min"作為判斷條件，若判斷為是，則尋北流程結(jié)束，若判斷 為否，則返回執(zhí)行步驟2。
[0122] 以下是各步驟中數(shù)據(jù)計算公式的具體推演以及與現(xiàn)有技術(shù)的試驗數(shù)據(jù)對比：
[0123] ( - )所述步驟2中慣性器件的具體標(biāo)定方法如下：
[0124] 相應(yīng)的姿態(tài)矩陣為： cos / cos ψ -?- si η;/ si η ?// si η θ - cos;/ si η y/ -\- sin;/ cos i// si η -sin;/cosi9
[0125] (J]二 sin ?//cos cos ?//cos (9 sin Θ (61) sin γ cos y/ - cos γ sin y/ sin Θ - sin;/ sin ψ - cos γ cos ψ sin Θ cos;/cos i9
[0126] 1)光纖陀螺參數(shù)標(biāo)定：
[0127] 雙光纖陀螺誤差模型：
[0128] X軸陀螺：
[0129] ⑵
[0130] Y軸陀螺：
[0131 ] (3)
[0132] 上面兩式中，Wbx，Wby分別是X軸和Y軸陀螺輸出信息，W Qx，WQy分別是X軸和Y軸陀螺輸 出零位，<,(? = z)是陀螺對載體角速度的測量值，Kxx，Kyy分別是X軸和Y軸陀螺刻度系 數(shù)，kxy，kzx，kxy，kzy分別是陀螺交叉耦合系數(shù)。
[0133] 在煒度已知，且水平靜止條件下，有如下三式： = ~ωη· S*n Ψ COS L
[0134] (?. = (〇h, cos ψ cos L .(4) oJ；.z = cow sin L
[0135] 上式中，coie3是地球自轉(zhuǎn)角速度，為常量。將(4)式帶入(2)、（3)式，有
[0136] Wbx=W〇x+Kxx(_ ω iesinitcosL)+kyx( ω iecositcosL)+kzx( ω iesinL) (5)
[0137] Wby=W〇y+kxy(-ω iesinitcosL)+Kyy( ω iecositcosL)+kzy( ω iesinL) (6)
[0138] 標(biāo)定陀螺過程中，始終保持水平靜止，標(biāo)定方位點依次選取蚵/ = 〇」.· 11) ?相 6 應(yīng)的X軸陀螺和Y軸陀螺輸出為 rm〇n1 w/ 「％'.? I …Inf
[0139] W^= W W -.· W _nby0 ￥Vbrl r^vllJI2x2
[0140] 構(gòu)建如下兩個矩陣，
[0143] X矩陣中的8個參數(shù)，即為雙光纖陀螺誤差模型(2)、（3)式的參數(shù)，所以在一個煒度 已知，且標(biāo)定的轉(zhuǎn)臺下，通過采集12水平位置的陀螺輸出數(shù)據(jù)，可標(biāo)定出以上8個參數(shù)。
[0144] 2)加速度計標(biāo)定
[0145] 雙加速度計誤差模型如下： Κχ ~ + + km-xfr +
[0146] h ' \ , (8) ν,γ=κιη,,+^：+κ^;：+^
[0147] 上面兩式中，Vax，Vay分別是X軸和Y軸加速度計輸出信息，K axQ，KaxQ分別是X軸和Y軸 加速度計輸出零位，廣(?_ = 1 /、Ζ).是加速度計對載體比力的測量值，Kaxx，Kayy分別是X軸和 Y軸加速度計刻度系數(shù)，k axy，kazx，kaxy，kazy分別是加速度計交叉耦合系數(shù)。
[0148] 加速度計標(biāo)定時候，靜止條件下，依次選取X軸、Y軸、Z軸加速度計敏感正負(fù)重力加 速，相應(yīng)的X軸加速度計和Y軸加速度計輸出為 V = Κν-*:一犮 ^a\-_Y+g Ktr_ v-g Kn_:+g ^ax z-g L J a v V V V V V av_x^-g av_x-g ar_ r+g· ^X_y-g av_~+g , 」6X?
[0150] 構(gòu)建如下兩個矩陣， '1 1 ο 0~ 1-10 0 「υ,: 10 10 κ Κ?η?
[0151] Α=: ανι 5 10-10 Ur lool LUv」4x2 _1 〇 Ο - ?χ = να
[0152] (9)
[0153] X矩陣中的8個參數(shù)，即為雙加速度計誤差模型（8)式的參數(shù)，所以在轉(zhuǎn)臺下，通過 敏感正負(fù)地球重力加速度信息，采集6種條件下，X軸Υ軸加速度計輸出數(shù)據(jù)，可標(biāo)定出以上8 個參數(shù)。
[0154] (二)所述步驟3b中天向虛擬信息具體構(gòu)建方法如下：
[0155] 1)天向陀螺虛擬信息構(gòu)建
[0156] 根據(jù)歐拉方程有： (10)
[0158] 考慮角速度動態(tài)約束條件，載體方位上角速度為零，即
[0159] ψ = 〇 (II)
[0160] 帶入上式有，得到姿態(tài)角速度約束方程：
(12)
[0162] 整理得到：
[0163] mhnb: = tan γω^ (13)
[0164] 考慮到有下式成立
[0165] e4 = e4 + e4： = αζ, + ^ Ο4)
[0166] 根據(jù)(2)、（3)中的誤差模型知道，X軸陀螺和Υ軸陀螺的輸出為 Γ .? Γ η Γ η Γ Whx W0r Ku. krx f
[0167] - °' =u ·,Λ col. (15) K. K'+ υ'γ k:v f ' . Ιω?,_
[0168] 將（14)帶入(15)，得到： 「? d u. b j "二、 『、
[0169] L?此+ tan,(?i&.-似如）」 （](>) _ X- + , tan r L P:r ] .....b .、
[0170] 其中
[0171] Gi=C^=CtiK+m；J (17)
[0172] 由于尋北時系統(tǒng)在導(dǎo)航坐標(biāo)系中是靜止的，導(dǎo)航坐標(biāo)系相對地球坐標(biāo)系沒有轉(zhuǎn) 動，所以為零，因此在東北天坐標(biāo)系下 '0 "
[0173] coiecosL (18) (〇iesinL_
[0174] 因此有： 地tan/ ' ft、
[0175] \\miy\[K+kzY^Y Kn,\ (1:9) 4,04 + ian，(< -<)
[0176] 2)天向加速度計虛擬信息構(gòu)建
[0177] 根據(jù)比力方程有：
[0178] νΒ=?7Λ-(2<+〇χνΒ+^ (20)
[0179] 考慮載體線運動的動態(tài)約束條件，垂直方向上的速度和加速度為零，以及水平方 向上速度為〇,即
[0180] 1?. (21)
[0181] 帶入上式有 X X X， /* 0
[0182] V；： = X X： X fy - 〇 (22) v[ - .sinf cos 沒 sin 61 cos f cos 6* g
[0183] 整理得天向加速度信息為：
[0184] v" =-sinycos(9f" -r^inff/^ -f cos;/cos6i//, -g = 0 (23)
[0185] Z軸加速度計輸出為：
(24)
[0187] 根據(jù)(8)式中的誤差模型知道，X加速度計和Y加速度計的輸出為 fb.
[0188] J；'.-=卜卜 （25) _ iiy _ _ny\)_ _ j-h
[0189] 將(24)帶入(25)，得到：
[0191]因此有：
(27)
[0193] 根據(jù)上面(19)和(27)構(gòu)造的數(shù)據(jù)，就構(gòu)成完整的三自由度慣性器件數(shù)據(jù)。
[0194] (三)所述步驟4中利用慣性系對準(zhǔn)計算方位角的方法如下：
[0195] 姿態(tài)矩陣的四元數(shù)形式如下
[0196] = (、
[0197] 其中
(29)
[0199] 初始時刻W:(〇)W,(〇Ml 〇 〇 Qf'，尋北過程中，載體處于相對靜止，及沒有速度， 此時< = <=[0 %cosi. 為(19)式中的陀螺數(shù)據(jù)，即c4=[r<4端.4.] *>對 (29)式采用四元數(shù)微分方程的畢卡求解法可以實時更新得到<(0和<。⑴。
[0200] 由于有
[0201] q!:(f)fh{t)q!;:{t) = g" (30)
[0202] 將(28)式帶入(30)式得：
Π1)
[0204] 整理得到：
(3:2)
[0206]式中左核函數(shù)和右核函數(shù)分別為： A -A -爲(wèi).-A aa ~ai ~ai ~a3 _7] M⑶=戽爲(wèi)-爲(wèi)A ， 爲(wèi)爲(wèi)爲(wèi)-A % -% ?〇 -爲(wèi) A ?2 -?1 ?〇 _
[0208] 為求解gf°，轉(zhuǎn)換成求矩陣最小特征值對應(yīng)的特征矢量即可。具體求解 υ Jfl 過程可以參考清華大學(xué)出版社《C常用算法程序集一第二版》中的第3.6節(jié)："求實對稱矩陣 特征值與特征向量的雅克比過關(guān)法"。將求解得到后的帶入(28)式得到新的乂⑴，就可 得到方位、俯仰、橫滾信息。
[0209] 方位： （，3)
[0210] 俯仰：
[0211] 橫滾： （35)
[0212] 其中 免沁 i/J.
[0213] (四)所述步驟3a中俯仰角與滾動角的初始裝訂的方法如下：
[0214] 對照(27)式，不考慮天向相關(guān)參數(shù)，有： ~fb~\ 「f L· Ι-ΤΓκ? Γ/γ ? γλ~jX ?Α-? tm αχ α.ν.0 r\
[0215] fb = k κ V ~ κ (36) J γ Λ//\τ ι^α.νν ' αν. ^ ανΟ'
[0216] 此時利用水平加速計輸出信息計算俯仰角和橫滾角： 0 = as\n(-/1 /?ζ)
[0217] .: m γ = i/sirK/.'/gcos^)
[0218] 在構(gòu)造數(shù)據(jù)之前，通過（19)式和(27)式可知，需要知道陀螺的8個標(biāo)定參數(shù)、加速 度計的8個標(biāo)定參數(shù)，以及俯仰角、橫滾角，共計18個參數(shù)。其中，前16個參數(shù)在慣性器件標(biāo) 定試驗中可以計算得到；后兩個參數(shù)(俯仰角、橫滾角）需要尋北系統(tǒng)初始計算裝訂，后續(xù)俯 仰角、橫滾角可以利用(34)式和(35)式計算結(jié)果參與后期運算。俯仰角和橫滾角初始計算 裝訂利用上述公式計算。
[0219] (五)本發(fā)明與傳統(tǒng)尋北方法的對比：
[0220]傳統(tǒng)方法尋北系統(tǒng)中，采用撓性陀螺2位置尋北技術(shù)，抗擾動尋北方法采用雙陀螺 雙加速計尋北，某次對比試驗結(jié)果見表1。
[0221 ] 表1數(shù)據(jù)對比
[0223] 上面試驗數(shù)據(jù)表明，采用本發(fā)明的雙陀螺抗擾動尋北方法不僅在靜態(tài)測試中與傳 統(tǒng)尋北系統(tǒng)尋北精度相當(dāng)；當(dāng)存在外界擾動情況下時，傳統(tǒng)尋北系統(tǒng)尋北精度大大降低，輸 出結(jié)果根本不能使用，而采用雙陀螺抗擾動尋北方法依舊可以保證尋北精度(1〇〈0.1°)。
[0224] 最后說明的是，以上實施例僅用以說明本發(fā)明的技術(shù)方案而非限制，盡管參照較 佳實施例對本發(fā)明進(jìn)行了詳細(xì)說明，本領(lǐng)域的普通技術(shù)人員應(yīng)當(dāng)理解，可以對本發(fā)明的技 術(shù)方案進(jìn)行修改或者等同替換，而不脫離發(fā)明技術(shù)方案的宗旨和范圍，其均應(yīng)涵蓋在本發(fā) 明的權(quán)利要求范圍當(dāng)中。
【主權(quán)項】
1. 雙陀螺抗擾動尋北方法，其特征在于具體步驟如下： 步驟1:對尋北系統(tǒng)進(jìn)行初始化； 步驟2:采集X軸、Y軸光纖陀螺與加速度計輸出信息，標(biāo)定慣性器件的參數(shù);具體來說， 1)在一個煒度已知，且標(biāo)定的轉(zhuǎn)臺下，通過采集均勻分布的12個水平位置的X軸、Y軸的陀螺 輸出數(shù)據(jù)，標(biāo)定出雙光纖陀螺誤差模型中的8個參數(shù);2)在轉(zhuǎn)臺下，通過敏感正負(fù)地球重力 加速度信息，采集6種條件下，X軸、Y軸加速度計輸出數(shù)據(jù)，標(biāo)定出雙加速度計誤差模型中的 8個參數(shù)； 步驟3a:對俯仰角和橫滾角兩個參數(shù)是否已初始裝訂過進(jìn)行判斷，若判斷為否，則初始 計算裝訂俯仰角和橫滾角兩個參數(shù);若判斷為是，則采用更新后的俯仰角和橫滾角參數(shù)； 步驟3b:將步驟2中標(biāo)定的16個參數(shù)與步驟3a中所述的俯仰角、橫滾角兩個參數(shù)一同構(gòu) 建天向虛擬信息，即天向陀螺虛擬信息和天向加速度計虛擬信息，從而實現(xiàn)對全姿態(tài)慣性 器件信息的構(gòu)建； 步驟4:利用慣性系對準(zhǔn)技術(shù)，計算方位角、俯仰角、橫滾角的信息；用計算出的橫滾角 和俯仰角信息更新步驟3a中所述的橫滾角和俯仰角參數(shù);輸出計算出的方位角信息，尋北 過程結(jié)束。2. 根據(jù)權(quán)利要求1所述的雙陀螺抗擾動尋北方法，其特征在于:所述步驟4在尋北過程 結(jié)束前，設(shè)置判斷條件來重復(fù)執(zhí)行步驟2~4以獲得符合要求的尋北結(jié)果;若判斷為是，則尋 北過程結(jié)束，若判斷為否，則返回執(zhí)行步驟2。3. 根據(jù)權(quán)利要求1所述的雙陀螺抗擾動尋北方法，其特征在于:所述步驟2中慣性器件 的具體標(biāo)定方法如下： 相應(yīng)的姿態(tài)矩陣為：1)光纖陀螺參數(shù)標(biāo)定： 雙光纖陀螺誤差模型： X軸陀螺：上面兩式中，Wbx，Wby分別是X軸和Y軸陀螺輸出信息，Wox，Wo y分別是X軸和Y軸陀螺輸出零 位，<_(? = Α.Γ,ζ)是陀螺對載體角速度的測量值，Kxx，Kyy分別是X軸和Y軸陀螺刻度系數(shù)， kXy，kzx，kXy，kZy分別是陀螺交叉親合系數(shù)； 在煒度已知，且水平靜止條件下，有如下三式：上式中，Wie3是地球自轉(zhuǎn)角速度，為常量;將(4)式帶入(2)、（3)式，有標(biāo)定陀螺過程中，始終保持水平靜止，標(biāo)定方位點依次選取ν=7/φ' = 〇、1...11)，相應(yīng)的X 6 軸陀螺和Y軸陀螺輸出為X矩陣中的8個參數(shù)，即為雙光纖陀螺誤差模型（2)、（3)式的參數(shù)，所以在一個煒度已 知，且標(biāo)定的轉(zhuǎn)臺下，通過采集12水平位置的陀螺輸出數(shù)據(jù)，可標(biāo)定出以上8個參數(shù)； 2)加速度計標(biāo)定 雙加速度計誤差模型如下：(B) 上面兩式中，Vax，Vay分別是X軸和Y軸加速度計輸出信息，KaxQ，K axQ分別是X軸和Y軸加速 度計輸出零位，.//'(/= /、4是加速度計對載體比力的測量值，Kaxx，K ayy分別是X軸和Y軸 加速度計刻度系數(shù)，kaxy，kazx，kaxy，kazy分別是加速度計交叉耦合系數(shù)； 加速度計標(biāo)定時候，靜止條件下，依次選取X軸、Y軸、Z軸加速度計敏感正負(fù)重力加速， 相疝的)(鈾加逋麼i+和γ鈾加逋麼i+輸m為(9) X矩陣中的8個參數(shù)，即為雙加速度計誤差模型（8)式的參數(shù)，所以在轉(zhuǎn)臺下，通過敏感 正負(fù)地球重力加速度信息，采集6種條件下，X軸Y軸加速度計輸出數(shù)據(jù)，可標(biāo)定出以上8個參 數(shù)。4.根據(jù)權(quán)利要求3所述的雙陀螺抗擾動尋北方法，其特征在于:所述步驟3b中天向虛擬 信息的具體構(gòu)建方法如下： 1)天向陀螺虛擬信息構(gòu)建 根據(jù)歐拉方程有：考慮角速度動態(tài)約束條件，載體方位上角速度為零，即(11) 帶入上式有，得到姿態(tài)角速度約束方程：根據(jù)(2)、（3)中的誤差模型知道，X軸陀螺和Y軸陀螺的輸出為由于尋北時系統(tǒng)在導(dǎo)航坐標(biāo)系中是靜止的，導(dǎo)航坐標(biāo)系相對地球坐標(biāo)系沒有轉(zhuǎn)動，所 以 為零，因此在東北天坐標(biāo)系下考慮載體線運動的動態(tài)約束條件，垂直方向上的速度和加速度為零，以及水平方向上 速度為O，即將(24)帶入(25)，得到：根據(jù)上面(19)和(27)構(gòu)造的數(shù)據(jù)，就構(gòu)成完整的三自由度慣性器件數(shù)據(jù)。5.根據(jù)權(quán)利要求4所述的雙陀螺抗擾動尋北方法，其特征在于:所述步驟4中利用慣性 系對準(zhǔn)計算方位角的方法如下： 姿態(tài)矩陣的四元數(shù)形式如下I ，尋北過程中，載體處于相對靜止，及沒有速度， 此時i: 3 (19)式中的陀螺數(shù)據(jù)，即;對(29)式采用四元數(shù)微分方程的畢卡求解法可以實時更新得 到 和<(，)；整理得到：為求解，轉(zhuǎn)換成求矩卩義小特征值對應(yīng)的特征矢量即可，將求解得到后 的帶入(28)式得到新的乂⑴，就可得到方位、俯仰、橫滾信息；(33) (3，）6.根據(jù)權(quán)利要求4所述的雙陀螺抗擾動尋北方法，其特征在于:所述步驟3a中俯仰角與 滾動角的初始裝訂的方法如下： 對照(27)式，不考慮天向相關(guān)參數(shù)，有：(36) 此時利用水平加速計輸出信息計算俯仰角和橫滾角：(37).
【文檔編號】G01C19/72GK106017452SQ201610652040
【公開日】2016年10月12日
【申請日】2016年8月10日
【發(fā)明人】劉程
【申請人】中國電子科技集團(tuán)公司第二十六研究所