基于火箭橇軌道坐標(biāo)系的慣性測(cè)量系統(tǒng)一維定位方法
【專利摘要】本發(fā)明公開了基于火箭橇軌道坐標(biāo)系的慣性測(cè)量系統(tǒng)一維定位方法，在火箭橇試驗(yàn)中的每一時(shí)刻，通過姿態(tài)角更新確定姿態(tài)變換矩陣，獲得運(yùn)動(dòng)方向加速度誤差后，通過公式計(jì)算得到姿態(tài)角、速度、位置誤差值，并進(jìn)行速度和位置的導(dǎo)航解算。在定位結(jié)束后，從導(dǎo)航結(jié)果中扣除誤差，得到真值。通過本發(fā)明進(jìn)行導(dǎo)航解算可以直接獲得在實(shí)際運(yùn)行方向上的導(dǎo)航距離值，并通過誤差補(bǔ)償，減少了軌道坐標(biāo)系Y、Z軸速度和位置導(dǎo)航誤差，進(jìn)而提高了導(dǎo)航精度。
【專利說明】基于火箭橇軌道坐標(biāo)系的慣性測(cè)量系統(tǒng)一維定位方法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種慣性測(cè)量系統(tǒng)基于火箭橇軌道坐標(biāo)系的一維定位方法，應(yīng)用于火 箭橇試驗(yàn)中的慣性導(dǎo)航中。

【背景技術(shù)】
[0002] 火箭橇試驗(yàn)具有產(chǎn)生大過載、高速度、強(qiáng)振動(dòng)和沖擊等綜合條件的能力，可以最逼 真地模擬導(dǎo)彈真實(shí)飛行環(huán)境。通過試驗(yàn)?zāi)軌蚩己藨T性測(cè)量系統(tǒng)在綜合環(huán)境條件下的各項(xiàng)性 能指標(biāo)和精度，驗(yàn)證慣性測(cè)量系統(tǒng)誤差模型在高動(dòng)態(tài)條件下的正確性，特別是在大過載情 況下，高次項(xiàng)放大作用，能夠確定慣性測(cè)量系統(tǒng)高次誤差項(xiàng)對(duì)導(dǎo)航性能的影響，是實(shí)現(xiàn)慣性 測(cè)量系統(tǒng)動(dòng)態(tài)性能驗(yàn)證的最佳途徑。
[0003] 捷聯(lián)式慣導(dǎo)系統(tǒng)的最大特點(diǎn)是依靠算法建立起導(dǎo)航坐標(biāo)系，即平臺(tái)坐標(biāo)系以數(shù)學(xué) 平臺(tái)形式存在，這樣就省略了復(fù)雜的物理實(shí)體平臺(tái)，一般情況下的導(dǎo)航坐標(biāo)系是在地理坐 標(biāo)系下進(jìn)行導(dǎo)航解算。
[0004] 在慣性測(cè)量系統(tǒng)火箭橇試驗(yàn)中，目前主要采用基于地理坐標(biāo)系的導(dǎo)航計(jì)算方法， 速度信息取東向速度北向速度￥"和天向速度V u，位置信息取緯度

【權(quán)利要求】
1. 基于火箭橇軌道坐標(biāo)系的慣性測(cè)量系統(tǒng)一維定位方法，其特征在于步驟如下： (1) 將慣性測(cè)量系統(tǒng)搭載在火箭橇上進(jìn)行火箭橇試驗(yàn)，并在火箭橇運(yùn)動(dòng)的每一時(shí)刻采 集并記錄慣性測(cè)量系統(tǒng)在捷聯(lián)本體坐標(biāo)系三個(gè)軸上的加速度和角速度；其中捷聯(lián)本體坐標(biāo) 系原點(diǎn)為慣性測(cè)量系統(tǒng)的中心，X、y、Z軸按照慣性測(cè)量系統(tǒng)規(guī)定方向定義； (2) 在火箭橇試驗(yàn)完成后，根據(jù)h時(shí)刻慣性測(cè)量系統(tǒng)在捷聯(lián)本體坐標(biāo)系三個(gè)軸上的加 速度和角速度以及慣性測(cè)量系統(tǒng)的對(duì)準(zhǔn)類型，計(jì)算h時(shí)刻慣性測(cè)量系統(tǒng)在軸、軸和 0?軸上的姿態(tài)角，并根據(jù)得到的姿態(tài)角計(jì)算h時(shí)刻火箭橇軌道坐標(biāo)系到捷聯(lián)本體坐標(biāo)系 的姿態(tài)變換矩陣；其中火箭橇軌道坐標(biāo)系0X&&以火箭橇軌道起始點(diǎn)為原點(diǎn)，0\軸指向火 箭橇橇體運(yùn)動(dòng)方向，OZi軸朝上垂直于軌道，軸在水平面內(nèi)垂直于軌道，且軸、〇1軸 和OZi軸滿足右手坐標(biāo)系； (3) η的初始值取0,執(zhí)行步驟（4)至步驟⑶； (4) 根據(jù)地球轉(zhuǎn)速，結(jié)合tn時(shí)刻慣性測(cè)量系統(tǒng)在捷聯(lián)本體坐標(biāo)系三個(gè)軸上的角速度以 及慣性測(cè)量系統(tǒng)在0?軸、〇1軸和0?軸上的姿態(tài)角，計(jì)算t n+1時(shí)刻慣性測(cè)量系統(tǒng)在火箭橇 軌道坐標(biāo)系0?軸、軸和軸方向上的姿態(tài)角； (5) 利用步驟⑷中得到的tn+1時(shí)刻慣性測(cè)量系統(tǒng)在火箭橇軌道坐標(biāo)系軸、軸 和0?軸方向上的姿態(tài)角計(jì)算tn+1時(shí)刻火箭橇軌道坐標(biāo)系到捷聯(lián)本體坐標(biāo)系的姿態(tài)變換矩 陣； (6) 根據(jù)tn+1時(shí)刻慣性測(cè)量系統(tǒng)在捷聯(lián)本體坐標(biāo)系三個(gè)軸上的加速度和角速度，以及tn 時(shí)刻慣性測(cè)量系統(tǒng)在軸和0?軸的速度以及位置信息，計(jì)算tn+1時(shí)刻的以下參數(shù)： (a) 慣性測(cè)量系統(tǒng)在軸、軸和軸上的姿態(tài)角誤差值； (b) 慣性測(cè)量系統(tǒng)在軸和軸上的速度誤差值； (c) 慣性測(cè)量系統(tǒng)在軸和0?軸上的位置誤差值； (d) 慣性測(cè)量系統(tǒng)在軸方向上的一維加速度誤差值； (7) 利用重力加速度在火箭橇軌道坐標(biāo)系下的分量，以及tn+1時(shí)刻火箭橇軌道坐標(biāo)系 到捷聯(lián)本體坐標(biāo)系的姿態(tài)變換矩陣、慣性測(cè)量系統(tǒng)在捷聯(lián)本體坐標(biāo)系三個(gè)軸上的加速度、 慣性測(cè)量系統(tǒng)在0?軸方向上的一維加速度誤差值，進(jìn)行導(dǎo)航解算得到t n+1時(shí)刻慣性測(cè)量系 統(tǒng)在0?軸、軸和軸上的速度和位置信息； (8) 判斷是否完成所有時(shí)刻的導(dǎo)航解算，如果是，執(zhí)行步驟（9)，否則，η的值加1，執(zhí)行 步驟⑷至步驟（8); (9) 在導(dǎo)航解算結(jié)束后，在每一時(shí)刻慣性測(cè)量系統(tǒng)在軸、軸和軸上的姿態(tài)角、 速度、位置導(dǎo)航值中扣除對(duì)應(yīng)的誤差值，得到每一時(shí)刻慣性測(cè)量系統(tǒng)在0?軸、軸和〇Ζι 軸上的姿態(tài)角、速度和位置的真值。
2. 根據(jù)權(quán)利要求1所述的基于火箭橇軌道坐標(biāo)系的慣性測(cè)量系統(tǒng)一維定位方法，其特 征在于：所述步驟（2)的實(shí)現(xiàn)方式為： (2. 1)當(dāng)慣性測(cè)量系統(tǒng)進(jìn)行自對(duì)準(zhǔn)時(shí)，利用如下公式得到、時(shí)刻慣性測(cè)量系統(tǒng)在ΟΧι 軸、OYjl^POZi軸上的姿態(tài)角Φχ、Φρ Φζ:
其中，ax、ay、az分別為h時(shí)刻慣性測(cè)量系統(tǒng)在捷聯(lián)本體坐標(biāo)系三個(gè)軸上測(cè)量得到的加 速度值，ωχ、coy、ωζ*?(ι時(shí)刻慣性測(cè)量系統(tǒng)在捷聯(lián)本體坐標(biāo)系三個(gè)軸上測(cè)量得到的角速 度值，為地球轉(zhuǎn)速，，為試驗(yàn)地點(diǎn)緯度； 當(dāng)進(jìn)行傳遞對(duì)準(zhǔn)時(shí)，tQ時(shí)刻慣性測(cè)量系統(tǒng)在軸、軸和軸上的姿態(tài)角Φχ、(ty 和Φζ由外部系統(tǒng)給出； (2. 2)根據(jù)得到的姿態(tài)角利用如下公式計(jì)算、時(shí)刻火箭橇軌道坐標(biāo)系到捷聯(lián)本體坐標(biāo) 系的姿態(tài)變換矩陣
3. 根據(jù)權(quán)利要求1所述的基于火箭橇軌道坐標(biāo)系的慣性測(cè)量系統(tǒng)一維定位方法，其特 征在于：所述步驟（4)中利用如下公式計(jì)算t n+1時(shí)刻慣性測(cè)量系統(tǒng)在火箭橇軌道坐標(biāo)系ΟΧι 軸、oYdi^noZi軸方向上的姿態(tài)角 Φχαη+1)、（ty(tn+1)和 Φζαη+1):
其中，ΛΤ為更新周期，AT = tn+1-tn;cK(tn)、Cty(t n)、Φζαη)分別為tn時(shí)刻慣性測(cè) 量系統(tǒng)在0?軸、軸和OZi軸上的姿態(tài)角；說(〇為tn時(shí)刻慣性測(cè)量系統(tǒng)在捷聯(lián)本體坐 標(biāo)系三個(gè)軸上的角速度，形式為X、Y、z軸向角速度組成的列向量；為地球轉(zhuǎn)速在火箭橇 軌道坐標(biāo)系下的分量，在導(dǎo)航計(jì)算中為固定值。
4. 根據(jù)權(quán)利要求1所述的基于火箭橇軌道坐標(biāo)系的慣性測(cè)量系統(tǒng)一維定位方法，其 特征在于：所述步驟（5)中tn+1時(shí)刻火箭橇軌道坐標(biāo)系到捷聯(lián)本體坐標(biāo)系的姿態(tài)變換矩陣
其中，Φχαη+1)、Φ>η+1)和Φζα η+1)分別為tn+1時(shí)刻慣性測(cè)量系統(tǒng)在火箭橇軌道坐標(biāo) 系軸、軸和軸方向上的姿態(tài)角。
5.根據(jù)權(quán)利要求1所述的基于火箭橇軌道坐標(biāo)系的慣性測(cè)量系統(tǒng)一維定位方法，其特 征在于：所述步驟（6)的實(shí)現(xiàn)方式為： (5. 1)在tn+1時(shí)刻，利用如下公式計(jì)算慣性測(cè)量系統(tǒng)在軸、軸和0?軸上的姿態(tài) 角誤差值、在軸和軸上的速度誤差值、在軸和軸上的位置誤差值： (1) x(tn| tn+1) = Φ(?η)χ(?η) (2) P (tn I tn+1) = Φ (tn) P (tn) Φ (tn) T+Q · Δ τ (3) K (tn) = P (tn I tn+1) HT [HP (tn I tn+1) HT+R] ―1 (4) X (tn+1) = X (tn I tn+1) +K (tn) [Y (tn) -HX (tn)] (5) P(tn+1) = [I7-K(tn)H]P(tn|tn+1)[I 7-K(tn)H]T+K(tn)RK(tn) T
量系統(tǒng)在0?軸、〇1軸和OZi軸上的姿態(tài)角誤差，δ Vy、δ Vz為慣性測(cè)量系統(tǒng)在軸和〇Zi 軸上的速度誤差，S ry、δ rz為慣性測(cè)量系統(tǒng)在軸和0?軸上的位置誤差值，在h時(shí)刻， x(t〇) = [0 0 0 0 0 0 0]τ ； €
Α12 = _ ωx (tn) sin Φ y (tn) + ωZ (tn) C0S 小 y (tn) 為3 = ?4, sin #:) - cos#:)
A22 = [ ωx (tn) COS Φ y (tn) + ω z (tn) Sin 小 y (tn) ] tan 小 X (tj Ai = [4., cos Φ, kn) + sin #, (iH)] sec φχ {t,t) 41 = {tn )s?n #r(/") - ?r (t" )cos#" (ta)]tan #x (tn )scc #x(/,,) -[?4. sin 4 (i") - r4、r eos 4 (i")] see2 #A. (/,,) A32 = - [ ω x (tn) cos Φ y (tn) + ω z (tn) sin Φ y (tn) ] sec Φ x (tn) 為3 = -[?L cos + c4r sk 4 (，")] lan 4 (/,,) A41 = ax (tn) sin Φ y (tn) cos Φ x (tn) cos Φ z (tn) _ay (tn) sin Φ x (tn) cos Φ z (tn) -az (tn) cos Φ y (tn) cos Φ x (tn) cos Φ z (tn) A42 = ax (tn) [-sin Φ y (tn) sin Φ z (tn) +cos Φ y (tn) sin Φ x (tn) cos Φ z (tn)] +az (tn) [cos Φ y (tn) sin Φ z (tn) +sin Φ y (tn) sin Φ x (tn) cos Φ z (tn)] A43 = ax (tn) [cos Φ y (tn) cos Φ z(tn) -sin Φ y (tn) sin Φ x(tn) sin Φ z(tn) ] -ay (tn) cos<i)x(tn)sin<i)z(tn) +az (tn) [sin Φ y (tn) cos Φ z (tn) +cos Φ y (tn) sin Φ x (tn) sin Φ z (tn)] A51 = ax (tn) sin Φ y (tn) sin Φ x (tn) +ay (tn) cos Φ x (tn) _az (tn) cos Φ y (tn) sin Φ x (tn) A52 = _ax (tn) C0S Φ y (tn) C0S 小 x (tn) _az (tn) sin Φ y (tn) C0S 小 x (tn) 其中，Λ T為更新周期，Λ T = tn+1_tn ; <A.、r<y、為 < 在火箭橇軌道坐標(biāo)系OXi 軸、軸和OZi軸上的分量；Φχαη)、（ty(tn)、Φ ζ(0分別為tn時(shí)刻慣性測(cè)量系統(tǒng)在ΟΧι 軸、軸和軸上的姿態(tài)角；ax(tn)、ay(tn)、a z(tn)分別為tn時(shí)刻慣性測(cè)量系統(tǒng)在捷聯(lián)本 體坐標(biāo)系三個(gè)軸上測(cè)量得到的加速度值；《 x(tn)、c〇y(tn)、c〇z(t n)為在捷聯(lián)本體坐標(biāo)系三 個(gè)軸上火箭橇橇體經(jīng)過補(bǔ)償?shù)慕撬俣?，?jì)算公式為：

坐標(biāo)系三個(gè)軸上測(cè)量到的角速度值，形式為X、Υ、Ζ軸向角速度組成的列向量；為地球 轉(zhuǎn)速在軌道坐標(biāo)系中的投影分量，在導(dǎo)航計(jì)算中為固定值；R$" ,；)為tn_i時(shí)刻火箭橇軌 道坐標(biāo)系到捷聯(lián)本體坐標(biāo)系的姿態(tài)變換矩陣；P(tn|tn+1)為一步預(yù)測(cè)均方誤差；P(t n)為 估計(jì)均方誤差；Q為噪聲序列方差陣，在導(dǎo)航解算過程中為固定值；K (tn)為濾波增益；

信息，ry(tn)、rz(tn)分別為t n時(shí)刻慣性導(dǎo)航系統(tǒng)在ΟΥ。0?軸的位置信息；17為7階單位 陣； (5. 2)在火箭橇運(yùn)動(dòng)的tn+1時(shí)刻，利用如下公式計(jì)算慣性測(cè)量系統(tǒng)在軸方向上的一 維加速度誤差值A(chǔ)ax(tn+1): Δ ax (tn+1) =[_ax (tn+1) sin Φ y (tn+1) cos Φ x (tn+1) sin Φ z (tn+1) +ay (tn+1) sin Φ x (tn+1) sin Φ z (tn+1) +az(tn+1)cos<i)y(tn+1)cos<i) x(tn+1)sin<i)z(tn+1)] δ φχ(?η+1) + [_ax (tn+1) (sin Φ y (tn+1) cos Φ z (tn+1) +cos Φ y (tn+1) sin Φ x (tn+1) sin Φ z (tn+1)) +az (tn+1) (cos Φ y (tn+1) cos Φ z (tn+1) -sin Φ y (tn+1) sin Φ x (tn+1) sin Φ z (tn+1)) ] δ φ y (tn+1) + [-ax (tn+1) (cos Φ y (tn+1) sin Φ z (tn+1) +sin Φ y (tn+1) sin Φ x (tn+1) cos Φ z (tn+1)) -ay (tn+1) cos Φ x (tn+1) cos Φ z (tn+1) +az (tn+1) (-sin Φ y (tn+1) sin Φ z (tn+1) +cos Φ y (tn+1) sin Φ x (tn+1) cos Φ z (tn+1)) ] δ φ z (tn+1) 其中，錢+Ρ,,,Ο、久H分別為tn+1時(shí)刻慣性測(cè)量系統(tǒng)在(?軸、軸和 0?軸上的姿態(tài)角；ax(tn+1)、ay(tn+1)、a z(tn+1)分別為tn+1時(shí)刻慣性測(cè)量系統(tǒng)在捷聯(lián)本體坐標(biāo) 系三個(gè)軸上測(cè)量得到的加速度值。
6.根據(jù)權(quán)利要求1所述的基于火箭橇軌道坐標(biāo)系的慣性測(cè)量系統(tǒng)一維定位方法，其特 征在于：所述步驟（7)中利用如下公式計(jì)算重力加速度在火箭橇軌道坐標(biāo)系下的分量：

a為地球長(zhǎng)半軸，e2為地球偏心率，為火箭橇橇體所在點(diǎn)的 緯度值；hp為橇體相對(duì)水準(zhǔn)面的高度；iPfl = (N+hp) β '，β '為矢量Ah和Of的夾角， 其中矢量〇〇Ρ〇為地球中心〇〇到軌道初始點(diǎn)P〇的矢量，〇〇Ρ為〇〇到P點(diǎn)的矢量，P為軌道與 地球表面切點(diǎn)，& +/V; +/Υ;2為火箭橇軌道重力加速度模型，其中rx為橇體沿軌道運(yùn)行的 距離；go為發(fā)射點(diǎn)位置的重力加速度，h和b2為常值。
7. 根據(jù)權(quán)利要求1所述的基于火箭橇軌道坐標(biāo)系的慣性測(cè)量系統(tǒng)一維定位方法，其特 征在于：所述步驟（7)中計(jì)算tn+1時(shí)刻慣性測(cè)量系統(tǒng)在軸、軸和0?軸上的速度和位 置信息的實(shí)現(xiàn)方式為： (7. 1)利用如下公式進(jìn)行導(dǎo)航解算，得到tn+1時(shí)刻慣性測(cè)量系統(tǒng)在軸、軸和ΟΖι 軸上的速度信息 Vx (tn+1)、vy (tn+1)和 vz (tn+1):
其中，vx (tn)、vy (tn)、vz (tn)為tn時(shí)刻慣性測(cè)量系統(tǒng)在(?軸、軸和OZi軸上的速度 信息；為tn+1時(shí)刻火箭橇軌道坐標(biāo)系到捷聯(lián)本體坐標(biāo)系的姿態(tài)變換矩陣；為地 球轉(zhuǎn)速在軌道坐標(biāo)系投影分量^的反對(duì)稱矩陣；fb(tn)為、時(shí)刻慣性測(cè)量系統(tǒng)在捷聯(lián)本體 坐標(biāo)系三個(gè)軸上的加速度，形式為X、Y、Z軸向加速度組成的列向量；g1為重力加速度在火 箭橇軌道坐標(biāo)系下的分量；Aax(tn)為tn時(shí)刻慣性測(cè)量系統(tǒng)在軸方向上的一維加速度 誤差值；Λ T為更新周期，Λ T = tn+1_tn ; (7. 2)利用如下公式進(jìn)行導(dǎo)航解算，得到tn+1時(shí)刻慣性測(cè)量系統(tǒng)在軸、軸和ΟΖι 軸上的位置信息 rx (tn+1)、ry (tn+1)和 rz (tn+1):
其中，rx (tn)、ry (tn)、rz (tn)為tn時(shí)刻慣性測(cè)量系統(tǒng)在(?軸、軸和OZi軸上的位置 信息。
8. 根據(jù)權(quán)利要求1所述的基于火箭橇軌道坐標(biāo)系的慣性測(cè)量系統(tǒng)一維定位方法，其特 征在于：所述步驟（9)的實(shí)現(xiàn)方式為： (8. 1)利用如下公式扣除慣性測(cè)量系統(tǒng)在軸、軸和0?軸上的對(duì)應(yīng)姿 態(tài)角誤差值，得到tn時(shí)刻慣性測(cè)量系統(tǒng)在軸、軸和OZi軸上的姿態(tài)角真值 Ψ.Λ1,,)- ψΛ?")ι
其中，φx(tn)、Φy(tn)、Φz(mì)(t n)分別為tn時(shí)亥lj慣性測(cè)量系統(tǒng)在軸、軸和軸 上的姿態(tài)角；S Φχαη)、δ cj5y(tn)、δ φζαη)分別為tn時(shí)刻性測(cè)量系統(tǒng)在軸、0\軸和 ?^軸上的姿態(tài)角誤差值； (8. 2)利用如下公式扣除慣性測(cè)量系統(tǒng)在軸和0?軸上的對(duì)應(yīng)速度誤差值，得到tn 時(shí)刻慣性測(cè)量系統(tǒng)在軸和0?軸上的速度真值vx'（tn)、vy'（t n)、vz'（tn):
其中，vx (tn)、vy (tn)、vz (tn)分別為tn時(shí)刻慣性測(cè)量系統(tǒng)在軸、OYi軸和OZi軸上的 速度；S Vy(tn)、δ Vz(tn)分別為tn時(shí)刻慣性測(cè)量系統(tǒng)在軸和0?軸上的速度誤差值； (8. 3)利用如下公式扣除tn時(shí)刻慣性測(cè)量系統(tǒng)在軸和0?軸上的對(duì)應(yīng)位置誤差值， 得到tn時(shí)刻慣性測(cè)量系統(tǒng)在軸和0?軸上的位置真值r x'（tn)、ry'（tn)、rz'（t n):
其中，rx (tn)、ry (tn)、rz (tn)分別為tn時(shí)刻慣性測(cè)量系統(tǒng)在(?軸、OYi軸和OZi軸上的 位置；S ry(tn)、δ rz(tn)分別為tn時(shí)刻慣性測(cè)量系統(tǒng)在軸和0?軸上的位置誤差值。
【文檔編號(hào)】G01C21/16GK104296747SQ201410584814
【公開日】2015年1月21日 申請(qǐng)日期:2014年10月27日 優(yōu)先權(quán)日:2014年10月27日
【發(fā)明者】魏宗康, 劉璠 申請(qǐng)人:北京航天控制儀器研究所