基于火箭橇轨道坐标系的惯性测量系统一维定位方法
【专利摘要】本发明公开了基于火箭橇轨道坐标系的惯性测量系统一维定位方法，在火箭橇试验中的每一时刻，通过姿态角更新确定姿态变换矩阵，获得运动方向加速度误差后，通过公式计算得到姿态角、速度、位置误差值，并进行速度和位置的导航解算。在定位结束后，从导航结果中扣除误差，得到真值。通过本发明进行导航解算可以直接获得在实际运行方向上的导航距离值，并通过误差补偿，减少了轨道坐标系Y、Z轴速度和位置导航误差，进而提高了导航精度。
【专利说明】基于火箭橇轨道坐标系的惯性测量系统一维定位方法
【技术领域】
[0001] 本发明涉及一种惯性测量系统基于火箭橇轨道坐标系的一维定位方法，应用于火 箭橇试验中的惯性导航中。
【背景技术】
[0002] 火箭橇试验具有产生大过载、高速度、强振动和冲击等综合条件的能力，可以最逼 真地模拟导弹真实飞行环境。通过试验能够考核惯性测量系统在综合环境条件下的各项性 能指标和精度，验证惯性测量系统误差模型在高动态条件下的正确性，特别是在大过载情 况下，高次项放大作用，能够确定惯性测量系统高次误差项对导航性能的影响，是实现惯性 测量系统动态性能验证的最佳途径。
[0003] 捷联式惯导系统的最大特点是依靠算法建立起导航坐标系，即平台坐标系以数学 平台形式存在，这样就省略了复杂的物理实体平台，一般情况下的导航坐标系是在地理坐 标系下进行导航解算。
[0004] 在惯性测量系统火箭橇试验中，目前主要采用基于地理坐标系的导航计算方法， 速度信息取东向速度北向速度￥"和天向速度V u，位置信息取纬度
【权利要求】
1. 基于火箭橇轨道坐标系的惯性测量系统一维定位方法，其特征在于步骤如下： (1) 将惯性测量系统搭载在火箭橇上进行火箭橇试验，并在火箭橇运动的每一时刻采 集并记录惯性测量系统在捷联本体坐标系三个轴上的加速度和角速度；其中捷联本体坐标 系原点为惯性测量系统的中心，X、y、Z轴按照惯性测量系统规定方向定义； (2) 在火箭橇试验完成后，根据h时刻惯性测量系统在捷联本体坐标系三个轴上的加 速度和角速度以及惯性测量系统的对准类型，计算h时刻惯性测量系统在轴、轴和 0?轴上的姿态角，并根据得到的姿态角计算h时刻火箭橇轨道坐标系到捷联本体坐标系 的姿态变换矩阵；其中火箭橇轨道坐标系0X&&以火箭橇轨道起始点为原点，0\轴指向火 箭橇橇体运动方向，OZi轴朝上垂直于轨道，轴在水平面内垂直于轨道，且轴、〇1轴 和OZi轴满足右手坐标系； (3) η的初始值取0,执行步骤（4)至步骤⑶； (4) 根据地球转速，结合tn时刻惯性测量系统在捷联本体坐标系三个轴上的角速度以 及惯性测量系统在0?轴、〇1轴和0?轴上的姿态角，计算t n+1时刻惯性测量系统在火箭橇 轨道坐标系0?轴、轴和轴方向上的姿态角； (5) 利用步骤⑷中得到的tn+1时刻惯性测量系统在火箭橇轨道坐标系轴、轴 和0?轴方向上的姿态角计算tn+1时刻火箭橇轨道坐标系到捷联本体坐标系的姿态变换矩 阵； (6) 根据tn+1时刻惯性测量系统在捷联本体坐标系三个轴上的加速度和角速度，以及tn 时刻惯性测量系统在轴和0?轴的速度以及位置信息，计算tn+1时刻的以下参数： (a) 惯性测量系统在轴、轴和轴上的姿态角误差值； (b) 惯性测量系统在轴和轴上的速度误差值； (c) 惯性测量系统在轴和0?轴上的位置误差值； (d) 惯性测量系统在轴方向上的一维加速度误差值； (7) 利用重力加速度在火箭橇轨道坐标系下的分量，以及tn+1时刻火箭橇轨道坐标系 到捷联本体坐标系的姿态变换矩阵、惯性测量系统在捷联本体坐标系三个轴上的加速度、 惯性测量系统在0?轴方向上的一维加速度误差值，进行导航解算得到t n+1时刻惯性测量系 统在0?轴、轴和轴上的速度和位置信息； (8) 判断是否完成所有时刻的导航解算，如果是，执行步骤（9)，否则，η的值加1，执行 步骤⑷至步骤（8); (9) 在导航解算结束后，在每一时刻惯性测量系统在轴、轴和轴上的姿态角、 速度、位置导航值中扣除对应的误差值，得到每一时刻惯性测量系统在0?轴、轴和〇Ζι 轴上的姿态角、速度和位置的真值。
2. 根据权利要求1所述的基于火箭橇轨道坐标系的惯性测量系统一维定位方法，其特 征在于：所述步骤（2)的实现方式为： (2. 1)当惯性测量系统进行自对准时，利用如下公式得到、时刻惯性测量系统在ΟΧι 轴、OYjl^POZi轴上的姿态角Φχ、Φρ Φζ:
其中，ax、ay、az分别为h时刻惯性测量系统在捷联本体坐标系三个轴上测量得到的加 速度值，ωχ、coy、ωζ*?(ι时刻惯性测量系统在捷联本体坐标系三个轴上测量得到的角速 度值，为地球转速，，为试验地点纬度； 当进行传递对准时，tQ时刻惯性测量系统在轴、轴和轴上的姿态角Φχ、(ty 和Φζ由外部系统给出； (2. 2)根据得到的姿态角利用如下公式计算、时刻火箭橇轨道坐标系到捷联本体坐标 系的姿态变换矩阵
3. 根据权利要求1所述的基于火箭橇轨道坐标系的惯性测量系统一维定位方法，其特 征在于：所述步骤（4)中利用如下公式计算t n+1时刻惯性测量系统在火箭橇轨道坐标系ΟΧι 轴、oYdi^noZi轴方向上的姿态角 Φχαη+1)、（ty(tn+1)和 Φζαη+1):
其中，ΛΤ为更新周期，AT = tn+1-tn;cK(tn)、Cty(t n)、Φζαη)分别为tn时刻惯性测 量系统在0?轴、轴和OZi轴上的姿态角；说(〇为tn时刻惯性测量系统在捷联本体坐 标系三个轴上的角速度，形式为X、Y、z轴向角速度组成的列向量；为地球转速在火箭橇 轨道坐标系下的分量，在导航计算中为固定值。
4. 根据权利要求1所述的基于火箭橇轨道坐标系的惯性测量系统一维定位方法，其 特征在于：所述步骤（5)中tn+1时刻火箭橇轨道坐标系到捷联本体坐标系的姿态变换矩阵
其中，Φχαη+1)、Φ>η+1)和Φζα η+1)分别为tn+1时刻惯性测量系统在火箭橇轨道坐标 系轴、轴和轴方向上的姿态角。
5.根据权利要求1所述的基于火箭橇轨道坐标系的惯性测量系统一维定位方法，其特 征在于：所述步骤（6)的实现方式为： (5. 1)在tn+1时刻，利用如下公式计算惯性测量系统在轴、轴和0?轴上的姿态 角误差值、在轴和轴上的速度误差值、在轴和轴上的位置误差值： (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
量系统在0?轴、〇1轴和OZi轴上的姿态角误差，δ Vy、δ Vz为惯性测量系统在轴和〇Zi 轴上的速度误差，S ry、δ rz为惯性测量系统在轴和0?轴上的位置误差值，在h时刻， 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、为 < 在火箭橇轨道坐标系OXi 轴、轴和OZi轴上的分量；Φχαη)、（ty(tn)、Φ ζ(0分别为tn时刻惯性测量系统在ΟΧι 轴、轴和轴上的姿态角；ax(tn)、ay(tn)、a z(tn)分别为tn时刻惯性测量系统在捷联本 体坐标系三个轴上测量得到的加速度值；《 x(tn)、c〇y(tn)、c〇z(t n)为在捷联本体坐标系三 个轴上火箭橇橇体经过补偿的角速度，计算公式为：
坐标系三个轴上测量到的角速度值，形式为X、Υ、Ζ轴向角速度组成的列向量；为地球 转速在轨道坐标系中的投影分量，在导航计算中为固定值；R$" ,；)为tn_i时刻火箭橇轨 道坐标系到捷联本体坐标系的姿态变换矩阵；P(tn|tn+1)为一步预测均方误差；P(t n)为 估计均方误差；Q为噪声序列方差阵，在导航解算过程中为固定值；K (tn)为滤波增益；
信息，ry(tn)、rz(tn)分别为t n时刻惯性导航系统在ΟΥ。0?轴的位置信息；17为7阶单位 阵； (5. 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) 其中，钱+Ρ,,,Ο、久H分别为tn+1时刻惯性测量系统在(?轴、轴和 0?轴上的姿态角；ax(tn+1)、ay(tn+1)、a z(tn+1)分别为tn+1时刻惯性测量系统在捷联本体坐标 系三个轴上测量得到的加速度值。
6.根据权利要求1所述的基于火箭橇轨道坐标系的惯性测量系统一维定位方法，其特 征在于：所述步骤（7)中利用如下公式计算重力加速度在火箭橇轨道坐标系下的分量：
a为地球长半轴，e2为地球偏心率，为火箭橇橇体所在点的 纬度值；hp为橇体相对水准面的高度；iPfl = (N+hp) β '，β '为矢量Ah和Of的夹角， 其中矢量〇〇Ρ〇为地球中心〇〇到轨道初始点P〇的矢量，〇〇Ρ为〇〇到P点的矢量，P为轨道与 地球表面切点，& +/V; +/Υ;2为火箭橇轨道重力加速度模型，其中rx为橇体沿轨道运行的 距离；go为发射点位置的重力加速度，h和b2为常值。
7. 根据权利要求1所述的基于火箭橇轨道坐标系的惯性测量系统一维定位方法，其特 征在于：所述步骤（7)中计算tn+1时刻惯性测量系统在轴、轴和0?轴上的速度和位 置信息的实现方式为： (7. 1)利用如下公式进行导航解算，得到tn+1时刻惯性测量系统在轴、轴和ΟΖι 轴上的速度信息 Vx (tn+1)、vy (tn+1)和 vz (tn+1):
其中，vx (tn)、vy (tn)、vz (tn)为tn时刻惯性测量系统在(?轴、轴和OZi轴上的速度 信息；为tn+1时刻火箭橇轨道坐标系到捷联本体坐标系的姿态变换矩阵；为地 球转速在轨道坐标系投影分量^的反对称矩阵；fb(tn)为、时刻惯性测量系统在捷联本体 坐标系三个轴上的加速度，形式为X、Y、Z轴向加速度组成的列向量；g1为重力加速度在火 箭橇轨道坐标系下的分量；Aax(tn)为tn时刻惯性测量系统在轴方向上的一维加速度 误差值；Λ T为更新周期，Λ T = tn+1_tn ; (7. 2)利用如下公式进行导航解算，得到tn+1时刻惯性测量系统在轴、轴和ΟΖι 轴上的位置信息 rx (tn+1)、ry (tn+1)和 rz (tn+1):
其中，rx (tn)、ry (tn)、rz (tn)为tn时刻惯性测量系统在(?轴、轴和OZi轴上的位置 信息。
8. 根据权利要求1所述的基于火箭橇轨道坐标系的惯性测量系统一维定位方法，其特 征在于：所述步骤（9)的实现方式为： (8. 1)利用如下公式扣除惯性测量系统在轴、轴和0?轴上的对应姿 态角误差值，得到tn时刻惯性测量系统在轴、轴和OZi轴上的姿态角真值 Ψ.Λ1,,)- ψΛ?")ι
其中，φx(tn)、Φy(tn)、Φz(t n)分别为tn时亥lj惯性测量系统在轴、轴和轴 上的姿态角；S Φχαη)、δ cj5y(tn)、δ φζαη)分别为tn时刻性测量系统在轴、0\轴和 ?^轴上的姿态角误差值； (8. 2)利用如下公式扣除惯性测量系统在轴和0?轴上的对应速度误差值，得到tn 时刻惯性测量系统在轴和0?轴上的速度真值vx'（tn)、vy'（t n)、vz'（tn):
其中，vx (tn)、vy (tn)、vz (tn)分别为tn时刻惯性测量系统在轴、OYi轴和OZi轴上的 速度；S Vy(tn)、δ Vz(tn)分别为tn时刻惯性测量系统在轴和0?轴上的速度误差值； (8. 3)利用如下公式扣除tn时刻惯性测量系统在轴和0?轴上的对应位置误差值， 得到tn时刻惯性测量系统在轴和0?轴上的位置真值r x'（tn)、ry'（tn)、rz'（t n):
其中，rx (tn)、ry (tn)、rz (tn)分别为tn时刻惯性测量系统在(?轴、OYi轴和OZi轴上的 位置；S ry(tn)、δ rz(tn)分别为tn时刻惯性测量系统在轴和0?轴上的位置误差值。
【文档编号】G01C21/16GK104296747SQ201410584814
【公开日】2015年1月21日 申请日期:2014年10月27日 优先权日:2014年10月27日
【发明者】魏宗康, 刘璠 申请人:北京航天控制仪器研究所