基于火箭橇軌道坐標系的慣性測量系統實時一維定位方法

基于火箭橇軌道坐標系的慣性測量系統實時一維定位方法
【專利摘要】本發明公開了基于火箭橇軌道坐標系的慣性測量系統實時一維定位方法，該方法基于火箭橇軌道坐標系，能夠實時計算得到每一時刻慣性測量系統的速度和位置信息。通過本發明方法進行導航解算可以直接獲得在實際運行方向上的導航距離值，并通過誤差補償，減少了軌道坐標系Y、Z軸速度和位置導航誤差，進而提高了導航精度。
【專利說明】基于火箭橇軌道坐標系的慣性測量系統實時一維定位方法

【技術領域】
[0001] 本發明涉及一種慣性測量系統基于火箭橇軌道坐標系的實時一維定位方法，可用 于火箭橇試驗的慣性導航中。

【背景技術】
[0002] 火箭橇試驗具有產生大過載、高速度、強振動和沖擊等綜合條件的能力，可以最逼 真地模擬導彈真實飛行環境。通過試驗能夠考核慣性測量系統在綜合環境條件下的各項性 能指標和精度，驗證慣性測量系統誤差模型在高動態條件下的正確性，特別是在大過載情 況下，高次項放大作用，能夠確定慣性測量系統高次誤差項對導航性能的影響，是實現慣性 測量系統動態性能驗證的最佳途徑。
[0003] 在慣性測量系統火箭橇試驗中，目前主要采用基于地理坐標系的導航計算方法。 一般的導航結果包括慣性測量系統的姿態信息（以姿態轉移陣

【權利要求】
1. 基于火箭橇軌道坐標系的慣性測量系統實時一維定位方法，其特征在于步驟如下： (1) 將慣性測量系統搭載在火箭橇上，以火箭橇軌道起始點為原點建立火箭橇軌道坐 標系OXJA，其中軸指向火箭橇橇體運動方向，OZi軸朝上垂直于軌道，軸在水平面 內垂直于軌道，且OXi軸、OYi軸和OZi軸滿足右手坐標系； (2) 慣性測量系統進行自對準或傳遞對準，得到h時刻慣性測量系統在軸、軸 和0?軸上的姿態角，并根據得到的姿態角計算h時刻火箭橇軌道坐標系到捷聯本體坐標 系的姿態變換矩陣；其中捷聯本體坐標系原點為慣性測量系統的中心，X、y、Z軸按照慣性 測量系統規定方向定義； (3) 火箭橇點火啟動后，在火箭橇運動的tn+1時刻，根據慣性測量系統測得的加速度和 角速度，以及tn時刻慣性測量系統在軸和0?軸的速度以及位置信息，計算以下參數： (a) 慣性測量系統在軸、軸和軸上的姿態角誤差值； (b) 慣性測量系統在軸和軸上的速度誤差值； (c) 慣性測量系統在軸和0?軸上的位置誤差值； (d) 慣性測量系統在軸方向上的一維加速度誤差值； (4) 根據步驟（3)得到的tn+1時刻慣性測量系統在軸、軸和0?軸上的姿態角 誤差值，得到tn+1時刻慣性測量系統在軸、軸和軸方向上的姿態角； (5) 利用tn+1時刻慣性測量系統在軸、軸和0?軸方向上的姿態角計算得到tn+1 時刻火箭橇軌道坐標系到捷聯本體坐標系的姿態變換矩陣； (6) 利用重力加速度在火箭橇軌道坐標系下的分量，以及tn+1時刻火箭橇軌道坐標系 到捷聯本體坐標系的姿態變換矩陣、慣性測量系統的加速度、慣性測量系統在0?軸方向上 的一維加速度誤差值、慣性測量系統在軸和0?軸上的速度誤差值和位置誤差值，進行 導航解算得到t n+1時刻慣性測量系統在軸、軸和0?軸上的速度和位置信息，從而實 現慣性測量系統的實時一維定位； 其中η = 0，1，2，……Ν，Ν為自然數。
2. 根據權利要求1所述的基于火箭橇軌道坐標系的慣性測量系統實時一維定位方法， 其特征在于：所述步驟（2)的實現方式為： (2. 1)當慣性測量系統進行自對準時，利用如下公式得到、時刻慣性測量系統在ΟΧι 軸、OYjl^POZi軸上的姿態角Φχ、Φρ Φζ:
其中，ax、ay、az分別為、時刻在捷聯本體坐標系三個軸上慣性測量系統測量得到的加 速度值，ωχ、coy、ωζ*?(ι時刻在捷聯本體坐標系三個軸上慣性測量系統測量得到的角速 度值，為地球轉速，Ρ為試驗地點緯度； 當進行傳遞對準時，tQ時刻慣性測量系統在軸、軸和軸上的姿態角Φχ、(ty 和φζ由外部系統給出； (2. 2)根據得到的姿態角利用如下公式計算h時刻火箭橇軌道坐標系到捷聯本體坐標 系的姿態變換矩陣Rf:
3.根據權利要求1所述的基于火箭橇軌道坐標系的慣性測量系統實時一維定位方法， 其特征在于：所述步驟（3)的實現方式為： (3. 1)在火箭橇運動的tn+1時刻，利用如下公式計算慣性測量系統在軸、軸和 OZi軸上的姿態角誤差值、在軸和軸上的速度誤差值、在軸和軸上的位置誤 差值： (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 其中，X(/")
% tn時刻各誤差系數組成的向量，δ φχ、δ φ" δ φζ為慣性測 量系統在0?軸、〇1軸和軸上的姿態角誤差，δ Vy、δ Vz為慣性測量系統在軸和〇Ζι 軸上的速度誤差，S ry、δ rz為慣性測量系統在軸和0?軸上的位置誤差值，在h時刻， x(t〇) = [0 0 0 0 0 0 0]τ ； xk u"+1):
，為tn時刻至tn+1時刻各誤差系數一步預測值組成的向量； Φ(/,,) = Ι7
ΔΓ，為狀態轉移陣，且 Α12 = _ ωx (tn) sin Φ y (tn) + ωZ (tn) C0S 小 y (tn) An = (0L sin φ_ (/") - ωΙ ν cos φ (/") Αχ = [^v {t,, )sin φν {t")- ω {t" )cosφν (tn)]see2 φλ {t") + \(〇l r sin φ (tn) - a/, r cos φ {tn)] tan φχ (/,, )scc i/") A22 = [ ωx (tn) C0S Φ y (tn) + ω z (tn) Sin 小 y (O ] tan 小 X (tj A3 = [<,v c〇s φ_ (/") + sin φ (/(i)]see φχ (/,;) Ai = -[^v )sin φγ [t")- )cosφγ {tn)] tan φχ (/,, jscc φχ {t") -\(〇l x sin φ. {tl:) - (〇'ic v cos φζ {tl:)]see2 φλ (；") A32 = - [ ω x (tn) cos Φ y (tn) + ω z (tn) sin Φ y (tn) ] sec Φ x (tn) 為3 = _[<x cos 么(i") + (/")] tan 久(i") 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) ] -a y (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 ; 、<,,,、<z為(?在火箭橇軌道坐標系 軸、0L軸和OZi軸上的分量；Φx(tn)、Φy(tn)、Φ z(tn)分別為tn時刻慣性測量系統在 軸、軸和OZi軸上的姿態角；ax (tn)、ay (tn)、az(tn)分別為tn時刻慣性測量系統在捷 聯本體坐標系三個軸上測量得到的加速度值 ；《x(tn)、c〇y(tn)、c〇 z(tn)為在捷聯本體坐標 系三個軸上火箭橇橇體經過補償的角速度，計算公式為：
對于h時刻，
為tn時刻慣性測量系統陀螺儀在捷聯本體 坐標系三個軸上測量到的角速度值，形式為X、Y、z軸向角速度組成的列向量；成為地球 轉速在軌道坐標系中的投影分量，在導航計算中為固定值；為tn_i時刻火箭橇軌 道坐標系到捷聯本體坐標系的姿態變換矩陣；p(tn|tn+1)為一步預測均方誤差；P(t n)為 估計均方誤差；Q為噪聲序列方差陣，在導航解算過程中為固定值；K (tn)為濾波增益； H=
，為量測陣；R為量測噪聲序列的方差陣，在導航解算中為固定 值；):
，其中，vy (tn)、vz(tn)分別為tn時刻慣性導航系統在〇1、軸的速度 信息，ry(tn)、rz(tn)分別為t n時刻慣性導航系統在ΟΥ。0?軸的位置信息；17為7階單位 陣。 (3. 2)在火箭橇運動的tn+1時刻，利用如下公式計算慣性測量系統在軸方向上的一 維加速度誤差值Aax(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) 其中，朽汰+1)、A(~+1)分別為tn+1時刻慣性測量系統在(?軸、軸和OZi 軸上的姿態角；ax(tn+1)、ay(tn+1)、a z(tn+1)分別為tn+1時刻慣性測量系統在捷聯本體坐標系 三個軸上測量得到的加速度值。
4.根據權利要求1所述的基于火箭橇軌道坐標系的慣性測量系統實時一維定位方法， 其特征在于：所述步驟（4)中tn+1時刻慣性測量系統在軸、軸和軸上的姿態角 Φχαη+1)、Φ>η+1)和Φζα η+1)利用下述公式計算得到：
其中，φx(tn)、Φy(tn)、Φz(t n)分別為tn時亥|J慣性測量系統在軸、軸和OZi軸 上的姿態角，成^)為tn時刻慣性測量系統陀螺儀在捷聯本體坐標系三個軸上測量到的 角速度值，形式為X、Y、Z軸向角速度組成的列向量；成為地球轉速在軌道坐標系中的投 影分量，在導航計算中為固定值；λτ為更新周期，Λτ = ?η+1-?η;δφχαη+1)、δ(^α η+1)、 δ φζαη+1)為tn+1時刻慣性測量系統在軸、οι軸和軸上的姿態角誤差值。
5. 根據權利要求1所述的基于火箭橇軌道坐標系的慣性測量系統實時一維定位方法， 其特征在于：所述步驟￠)中重力加速度在火箭橇軌道坐標系下的分量如下：

a為地球長半軸,e2為地球偏心率，p為火箭橇橇體所在點的 緯度值；hp為橇體相對水準面的高度；iPfl = (N+hp) β '，β '為矢量Ah和Of的夾角， 其中矢量〇〇Ρ〇為地球中心〇〇到軌道初始點P〇的矢量，〇〇Ρ為〇〇到P點的矢量，P為軌道與 地球表面切點，私+iy;2為火箭橇軌道重力加速度模型，其中rx為橇體沿軌道運行的 距離；go為發射點位置的重力加速度，h和b2為常值。
6. 根據權利要求1所述的基于火箭橇軌道坐標系的慣性測量系統實時一維定位方法， 其特征在于：所述步驟（6)的實現方式為： (6. 1)利用如下公式進行導航解算，得到tn+1時刻慣性測量系統在軸、軸和ΟΖι 軸上的速度信息 Vx (tn+1)、vy (tn+1)和 vz (tn+1):
其中，vx(tn)、vy(tn)、vz(t n)為tn時刻慣性測量系統在(?軸、軸和OZi軸上的速 度信息；RU&+上為tn+1時刻火箭橇軌道坐標系到捷聯本體坐標系的姿態變換矩陣；Ω?Ρ為 地球轉速在軌道坐標系投影分量藏的反對稱矩陣；fb(tn)為慣性測量系統加速度計的測量 值，形式為X、Y、Z軸向加速度組成的列向量；g1為重力加速度在火箭橇軌道坐標系下的分 量；Λ ax (tn+1)為tn+1時刻慣性測量系統在(?軸方向上的一維加速度誤差值；Λ T為更新周 期，ΛΤ = tn+1_tn ; δ Vy(tn+1)、δ Vz(tn+1)為tn+1時刻慣性測量系統在軸和軸上的速 度誤差值； (6. 2)利用如下公式進行導航解算，得到tn+1時刻慣性測量系統在軸、軸和ΟΖι 軸上的位置信息 rx (tn+1)、ry (tn+1)和 rz (tn+1):
其中，rx (tn)、ry (tn)、rz (tn)為tn時刻慣性測量系統在(?軸、軸和OZi軸上的位置 信息；S ry(tn+1)、δ rz(tn+1)為tn+1時刻慣性測量系統在軸和0?軸上的位置誤差值。
【文檔編號】G01C25/00GK104296781SQ201410584789
【公開日】2015年1月21日 申請日期:2014年10月27日 優先權日:2014年10月27日
【發明者】魏宗康, 劉璠 申請人:北京航天控制儀器研究所