[发明专利]一种应用于弹道导弹的惯导误差在线补偿方法

摘要

本发明公开了一种应用于弹道导弹的惯导误差在线补偿方法，包括步骤：S1、采集导弹进入高空后惯组输出的脉冲数，计算导弹视加速度；S2、计算视加速度表的零位误差、纵向加速度表比例项误差、初始调平误差以及瞄准误差；S3、采集当前时刻视加速度以及四元数，计算第一组导弹位置和速度；S4、利用零位误差、比例项误差修正当前时刻视加速度，利用初始调平误差、瞄准误差修正当前时刻四元数；利用修正后的视加速度、四元数，计算第二组导弹位置和速度；S5、计算导航位置误差和速度误差；S6、对导弹各时刻位置和速度进行修正，完成惯导误差在线补偿。实施本发明可在不改变导弹惯组指标的情况下，完成惯导误差在线补偿，提高导弹惯导精度。

主权项

1.一种应用于弹道导弹的惯导误差在线补偿方法，其特征在于，所述方法包括以下步骤：S1、采集导弹进入高空后惯组输出的脉冲数，进行导航计算得到导弹在所述高空飞行过程内的视加速度；S2、根据所述视加速度计算视加速度表在弹体坐标系下三个方向的零位误差、纵向加速度表比例项误差、初始调平误差以及瞄准误差；S3、采集当前时刻导弹加速度表输出的视加速度以及四元数信息，进行导航计算获得第一组导弹位置和速度；S4、利用所述视加速度表零位误差、比例项误差对所述当前时刻的视加速度进行修正，利用所述初始调平误差以及瞄准误差对当前时刻的四元数进行修正；利用修正后的视加速度、四元数进行第二次导航计算，获得第二组导弹位置和速度；S5、将第二组导弹位置和速度值分别减去第一组导弹位置和速度值，得到导航位置误差和导航速度误差；S6、利用所述导航位置误差和导航速度误差对导弹各时刻的位置和速度进行修正，完成惯导误差在线补偿；所述步骤S1的视加速度为： δ W x 2 , i δW y 2 , i δW z 2 , i = G i j · δ W x 1 , i ′ δW y 1 , i ′ δW z 1 , i ′ - W x 0 W y 0 W z 0 ]]> δ θ x 1 , i δθ y 1 , i δθ z 1 , i = T i j · δ θ x 1 , i ′ - θ x 0 δθ y 1 , i ′ - θ x 0 δθ z 1 , i ′ - θ x 0 - M i j · δ W x 2 , i δW y 2 , i δW z 2 , i ]]> δW x 3 , i = δW x 2 , i + 50 * ( l x * ( δθ y 1 , i 2 + δθ z 1 , i 2 ) - l y * δθ x 1 , i * δθ y 1 , i ) ]]> δW y 3 , i = δW y 2 , i + 50 * ( l y * ( δθ x 1 , i 2 + δθ z 1 , i 2 ) - l x * δθ x 1 , i * δθ y 1 , i ) ]]>δWz3,i＝δWz2,i-50*(lx*δθx1,i*δθz1,i+ly*δθy1,i*δθz1,i) W · x 1 , i W · y 1 , i W · z 1 , i = δ W x 3 , i δW y 3 , i δW z 3 , i / t 0 ]]>ΔWx1,i＝ΔWx1,i-1+δWx3iΔWy1,i＝ΔWy1,i-1+δWy3iΔWz1,i＝ΔWz1,i-1+δWz3ii＝1,2,…,M，M为采样周期的个数；其中：为第i个导航周期的视加速度；t0为导航周期；δW′x1,i、δW′y1,i、δW′z1,i分别为三个加速度表输出的脉冲增量；δθ′x1,i、δθ′y1,i、δθ′z1,i分别为三个陀螺输出的脉冲增量；δWx2,i、δWy2,i、δWz2,i分别为弹体坐标系三个方向上的视速度增量；δθx1,i、δθy1,i、δθz1,i分别为弹体坐标系三个方向上的角增量；Gij、Tij、Mij(i＝1～3,j＝1～3)为工具误差补偿系数；lx,ly,lz分别为相对于弹体坐标系下惯组到弹体质心三个方向的距离；Wx0、Wy0、Wz0、θx0、θy0、θz0分别为工具误差零位补偿系数；ΔWx1,M,ΔWy1,M,ΔWz1,M即为最终弹体坐标系下三个方向的视加速度；所述步骤S2的误差计算方法具体为：ΔK0x＝-ΔWx1,M/tM/g0ΔK0y＝-ΔWy1,M/tM/g0ΔK0z＝-ΔWz1,M/tM/g0ΔK1x＝-ΔK0xΔψ0＝-ΔK0zΔγ0＝-Δψ0tanα其中：tM为从开始采样到采样结束消耗的时长；g0为发射点重力加速度；α为瞄准时弹体仰角；ΔK0x,ΔK0y,ΔK0z为辨识出的惯组误差零位系数；ΔK1x为辨识出的惯组误差比例项系数；Δψ0为初始调平误差；Δγ0为瞄准误差；所述步骤S3第一组导弹位置和速度的计算方法具体为： a 11 = q 0 2 + q 1 2 - q 2 2 - q 3 2 ]]>a12＝2(q1·q2-q0·q3)a13＝2(q1·q3-q0·q2)a21＝2(q2·q1+q0·q3) a 22 = q 0 2 - q 1 2 + q 2 2 - q 3 2 ]]>a23＝2(q2·q3-q0·q1)a31＝2(q3·q1-q0·q2)a32＝2(q3·q2+q0·q1) a 33 = q 0 2 - q 1 2 - q 2 2 + q 3 2 ]]> D = a 11 a 12 a 13 a 21 a 22 a 23 a 31 a 32 a 33 ]]> V · 1 x V · 1 y V · 1 z = W · D · W · x 1 W · y 1 W · z 1 + g x g y g z - W e x W e y W e z - W c x W c y W c z X · 1 Y · 1 Z · 1 = V 1 x V 1 y V 1 z ]]>其中：a11、a12、a13、a21、a22、a23、a31、a32、a33、D为中间变量；q0、q1、q2、q3为第一次导航计算中当前时刻弹体坐标系与发射惯性坐标系之间的四元数；W为发射惯性坐标系到发射坐标系的转换矩阵；[gx,gy,gz]T为当前时刻发射坐标系下导弹质心位置的重力加速度；[Wex,Wey,Wez]T为当前时刻发射坐标系下牵连加速度；[Wcx,Wcy,Wcz]T为当前时刻发射坐标系下科氏加速度；[V1x,V1y,V1z]T即为发射坐标系下第一组速度值；[X1,Y1,Z1]T即为发射坐标系下第一组位置值；所述步骤S4第二组导弹位置和速度的具体计算方法为：对视加速度修正公式为： W · x 2 = ( W · x 2 + ΔK 0 x * g 0 ) ( 1 + ΔK 1 x ) ]]> W · y 2 = W · y 1 + ΔK 0 y * g 0 ]]> W · z 2 = W · z + ΔK 0 z * g 0 ]]>对四元数修正公式为： 0.5 * c o s ( γ 0 / 2 ) - sin ( γ 0 / 2 ) - sin ( γ 0 / 2 ) - c o s ( γ 0 / 2 ) sin ( γ 0 / 2 ) c o s ( γ 0 / 2 ) - c o s ( γ 0 / 2 ) sin ( γ 0 / 2 ) sin ( γ 0 / 2 ) c o s ( γ 0 / 2 ) c o s ( γ 0 / 2 ) - sin ( γ 0 / 2 ) c o s ( γ 0 / 2 ) - sin ( γ 0 / 2 ) sin ( γ 0 / 2 ) c o s ( γ 0 / 2 ) · d q 0 - d q 1 - d q 2 - d q 3 d q 1 d q 0 - d q 3 d q 2 d q 2 d q 3 d q 0 - d q 1 d q 3 - d q 2 d q 1 d q 0 · ]]> q 0 q 1 q 2 q 3 = c o s ( γ 0 / 2 ) sin ( γ 0 / 2 ) sin ( γ 0 / 2 ) c o s ( γ 0 / 2 ) - sin ( γ 0 / 2 ) c o s ( γ 0 / 2 ) c o s ( γ 0 / 2 ) - sin ( γ 0 / 2 ) - sin ( γ 0 / 2 ) - c o s ( γ 0 / 2 ) c o s ( γ 0 / 2 ) sin ( γ 0 / 2 ) - c o s ( γ 0 / 2 ) sin ( γ 0 / 2 ) - sin ( γ 0 / 2 ) c o s ( γ 0 / 2 ) · q 0 q 1 q 2 q 3 ]]>利用修正后的视加速度与修正后的四元数，进行第二次导航计算： a 11 ′ = q 0 ′ 2 + q 1 ′ 2 - q 2 ′ 2 - q 3 ′ 2 ]]> a 12 ′ = 2 ( q 1 ′ 2 · q 2 ′ 2 - q 0 ′ 2 · q 3 ′ 2 ) ]]> a 13 ′ = 2 ( q 1 ′ 2 · q 3 ′ 2 - q 0 ′ 2 · q 2 ′ 2 ) ]]> a 21 ′ = 2 ( q 2 ′ 2 · q 1 ′ 2 + q 0 ′ 2 · q 3 ′ 2 ) ]]> a 22 ′ = q 0 ′ 2 - q 1 ′ 2 + q 2 ′ 2 - q 3 ′ 2 ]]> a 23 ′ = 2 ( q 2 ′ 2 · q 3 ′ 2 - q 0 ′ 2 · q 1 ′ 2 ) ]]> a 31 ′ = 2 ( q 3 ′ 2 · q 1 ′ 2 - q 0 ′ 2 · q 2 ′ 2 ) ]]> a 32 ′ = 2 ( q 3 ′ 2 · q 2 ′ 2 + q 0 ′ 2 · q 1 ′ 2 ) ]]> a 33 ′ = q 0 ′ 2 - q 1 ′ 2 - q 2 ′ 2 + q 3 ′ 2 ]]> D ′ = a 11 ′ a 12 ′ a 13 ′ a 21 ′ a 22 ′ a 23 ′ a 31 ′ a 32 ′ a 33 ′ ]]> V · x 2 V · y 2 V · z 2 = W ′ · D ′ · W · x 2 W · y 2 W · z 2 + g ′ x g ′ y g ′ z - W ′ e x W ′ e y W ′ e z - W ′ c x W ′ c y W ′ c z X · 2 Y · 2 Z · 2 = V 2 x V 2 y V 2 z ]]>其中，分别为修正后的弹体坐标系下三个方向的视加速度；q′0、q′1、q′2、q′3分别为第二次导航计算中当前时刻弹体坐标系与发射惯性坐标系之间的四元数；γ0为初始滚转角；[g′x,g′y,g′z]T为当前时刻发射坐标系下导弹质心位置的重力加速度；[Wex,Wey,Wez]T为当前时刻发射坐标系下牵连加速度；[Wcx,Wcy,Wcz]T为当前时刻发射坐标系下科氏加速度；[V2x,V2y,V2z]T即为第二组速度值；[X2,Y2,Z2]T即为第二组位置值；所述步骤S5位置导航误差和速度导航误差的具体计算方法为： Δ X Δ Y Δ Z = X 2 Y 2 V 2 - X 1 Y 1 Z 1 ]]> Δ V x Δ V y Δ V z = V 2 x V 2 y V 2 z - V 1 x V 1 y V 1 z ]]>其中，[ΔVx ΔVy ΔVz]T即为分离的速度导航误差，[ΔX ΔY ΔZ]T即为分离的位置导航误差；所述步骤S6补偿后的导航值得具体计算方法为：Xfs＝Xfs0+ΔXYfs＝Yfs0+ΔyZfs＝Zfs0+ΔzVxfs＝Vxfs0+ΔVxVyfs＝Vyfs0+ΔVyVzfs＝Vzfs0+ΔVz其中，[Xfs0 Yfs0 Zfs0]T为补偿前的导弹位置导航值，[Vxfs0 Vyfs0 Vzfs0]T为补偿前的导弹速度导航值，[Xfs Yfs Zfs]T为补偿后的导弹位置导航值，[Vxfs Vyfs Vzfs]T为补偿后的导弹速度导航值。