基于前数据的旋转制导炮弹快速空中对准方法与流程

本发明涉及初始对准技术和组合导航系统技术领域，特别涉及一种基于前数据的旋转制导炮弹快速空中对准方法，可以用于无人机、自旋制导炮弹等需要在空中自对准的场合。

背景技术：
自旋制导炮弹是一种在空中发射，需要进行自我对准的一种高精度武器，它包含了惯性导航和GPS等系统，通过GPS来修正惯性导航系统的误差，达到精确打击目标的能力。空中对准从惯导相对其他导航系统提供的导航参数(如速度等)的偏差中估计出惯导系统的失准角并校正之。惯性导航系统是一个基于加速度二次积分的航程推算系统，它完全依靠机械设备和相应的算法自动、独立完成导航任务，和外界不发生任何光、电联系。由于其具有隐蔽性好、工作环境不受气象条件限制等优点，成为航天、航空、航海领域中一种广泛使用的主要导航系统。在惯性导航系统工作解算前，需要给出初始状态，就是需要进行初始对准。常用的对准方法是采用Kalman滤波算法实现，该算法需要建立系统的误差模型，算法稳定性严重依赖于导航误差模型的正确性和精确程度，而且时间开销较大，滤波周期较长；另外在Kalman滤波过程中未考虑惯导工作在失重环境中，加速度计输出几乎为零，对横滚角的观测效果差，而且对准的精度不高且需要时间长。运用加速度计方法因制导炮弹在空中处于失重情况下，加速度计输出结果噪声太大，甚至隐没真实输出，导致最终计算得到的横滚角结果不准确。运用陀螺仪输出结果进行计算，因旋转速度比较快，故采用的陀螺仪的量测比较大，精度相对就比较低，输出的扰动较大，导致结果不可估算。

技术实现要素：
本发明解决的技术问题是：克服现有技术的不足，提供一种基于前数据的旋转制导炮弹快速空中对准方法，该方法中对准的位置和速度由卫星导航系统给出，并利用卫星导航输出的速度信息解算出对应时刻的航向角和俯仰角，然后再通过最小二乘法求解横滚角观测方程，从而实现对惯导初始位置、速度和姿态角的精确解算，即实现自旋制导炮弹的快速空中对准，大大提高了自旋制导炮弹的落点精度。本发明的上述目的通过以下方案实现：基于前数据的旋转制导炮弹快速空中对准方法，包括如下步骤：(1)、发射后的制导炮弹接收卫星导航信号进行导航处理，其中实现卫星导航信号捕获跟踪并输出导航结果的时刻为T0；然后从所述时刻T0到设定的对准时刻T1保存卫星导航系统输出的M组卫星导航结果和INS输出的N组INS数据，其中：TGPS为卫星导航结果输出周期，TINS为INS数据输出周期，且TGPS＝Q×TINS，即N＝Q×M，Q为正整数；所述卫星导航结果包括制导炮弹的速度和位置；所述INS数据包括前向陀螺、左向陀螺和上向陀螺输出的角速度，其中，前向陀螺敏感弹体横滚角角速度，左向陀螺敏感俯仰角角速度，上向陀螺敏感航向角角速度；(2)、根据M组卫星导航结果中的制导导弹的速度，计算得到相应的M组航向角和俯仰角；(3)、对步骤(2)计算得到的M组航向角和俯仰角的计算结果进行拟合计算，得到航向角和俯仰角在时刻T0到时刻T1之间随时间变换的函数；然后对所述函数进行求导运算，得到航向角变化率和俯仰角变化率的时间函数；(4)、将输出N组INS数据的时刻值代入到步骤(3)确定的4个时间函数中，计算得到输出N组INS数据时的制导炮弹的航向角、俯仰角、航向角变化率和俯仰角变化率；(5)、根据时刻T0到设定时刻T的Np组俯仰角、航向角变化率、俯仰角变化率和INS数据中的陀螺输出角速度，计算观测方程Z＝H×X中的观测矩阵H和测量矩阵Z；其中X为两维的测量向量，X(1)为制导炮弹横滚角的正弦值，X(2)为制导炮弹横滚角的余弦值；其中，设定时刻T的取值范围为T0≤T<T1，正整数(6)、利用最小二乘法对观测方程Z＝H×X进行求解，得到观测向量X＝(HTH)-1HTZ；(7)、根据观测向量X计算结果中的制导炮弹横滚角的正弦值和余弦值，计算得到制导炮弹的横滚角；(8)、将步骤(7)计算得到的横滚角，以及时刻T0的卫星导航结果中的速度、位置和根据所述速度计算得到的航向角和俯仰角，作为空中对准结果，输出到制导炮弹的导航系统，用于对所述制导炮弹进行导航和控制。上述的基于前数据的旋转制导炮弹快速空中对准方法，在步骤(5)中，根据时刻T0～T的Np组俯仰角、航向角变化率和俯仰角变化率，以及INS数据中的陀螺输出角速度，计算观测矩阵H和测量矩阵Z，具体计算过程如下：(5a)、对观测矩阵H和测量矩阵Z进行初始化，得到初始观测矩阵H0和测量矩阵Z0：如果初始化H0＝[a(T0)b(T0)]，则Z0＝z(T0)；如果初始化H0＝[-b(T0)a(T0)]，则Z0＝z′(T0)；其中：其中：ωx(T0)、ωy(T0)和ωz(T0)分别为时刻T0前向陀螺、左向陀螺和上向陀螺输出的角速度测量值；φgx(T0)为时刻T0的俯仰角；和分别为时刻T0的航向角变化率和俯仰角变化率；(5b)、在时刻Tn′＝T0+n×TINS，n＝1、2、…Np-1，按照如下的迭代公式对观测矩阵H和测量矩阵Z进行迭代更新，得到时刻Tn′的观测矩阵Hn和测量矩阵Zn；如果Hn＝[Hn-1；a(Tn′),b(Tn′)]，则Zn＝[Zn-1；z(Tn′)]；如果Hn＝[Hn-1；-b(Tn′),a(Tn′)]，则Zn＝[Zn-1；z′(Tn′)]；其中：其中：ωx(Tm′)、ωy(Tm′)和ωz(Tm′)分别为时刻Tm′前向陀螺、左向陀螺和上向陀螺输出的角速度测量值，φgx(Tm′)和分别为时刻Tm′的俯仰角和航向角变化率，m＝0～n且T0′＝T0；φgx(Tn′)为时刻Tn′的俯仰角；和分别为时刻Tn′的航向角变化率和俯仰角变化率；ωx(Tn′)、ωy(Tn′)和ωz(Tn′)分别为时刻Tn′前向陀螺、左向陀螺和上向陀螺输出的角速度测量值；(5c)、将步骤(5b)迭代得到的时刻T＝T0+(Np-1)×TINS的观测矩阵和测量矩阵作为观测矩阵H和测量矩阵Z的最终计算结果，用于步骤(6)的最小二乘计算。上述的基于前数据的旋转制导炮弹快速空中对准方法，在步骤(7)中，根据观测向量X计算结果中的制导炮弹横滚角的正弦值和余弦值，计算得到制导炮弹的横滚角γ0，具体计算方法如下：当|X(1)|<1,|X(2)|<1时:如果X(1)>0,X(2)>0，γ0＝(arcsin(X(1))+arccos(X(2)))/2；如果X(1)>0,X(2)<0，γ0＝(180-arcsin(X(1))+arccos(X(2)))/2；如果X(1)<0,X(2)>0，γ0＝(arcsin(X(1))-arccos(X(2)))/2；如果X(1)<0,X(2)<0，γ0＝(-arcsin(X(1))-arccos(X(2)))/2。当|X(1)|>1,|X(2)|<1时:如果X(1)>0,X(2)>0，γ0＝arccos(X(2))/2；如果X(1)>0,X(2)<0，γ0＝arccos(X(2))；如果X(1)<0,X(2)>0，γ0＝-arccos(X(2))；如果X(1)<0,X(2)<0，γ0＝-arccos(X(2))。当|X(1)|<1,|X(2)|>1时:如果X(1)>0,X(2)>0，γ0＝arcsin(X(1))；如果X(1)>0,X(2)<0，γ0＝180-arcsin(X(1))；如果X(1)<0,X(2)>0，γ0＝arcsin(X(1))；如果X(1)<0,X(2)<0，γ0＝-arcsin(X(1))。上述的基于前数据的旋转制导炮弹快速空中对准方法，在步骤(2)中，根据制导炮弹速度计算航向角和俯仰角的公式如下：其中：φgz和φgx分别为制导炮弹的航向角和俯仰角；Vgn、Vge和Vgu分别为制导炮弹的北速、东速和天速。上述的基于前数据的旋转制导炮弹快速空中对准方法，在步骤(3)中，采用最小二乘4次曲线拟合方法对M组航向角和俯仰角的计算结果进行拟合计算，得到航向角和俯仰角在时刻T0到时刻T1之间随时间变换的函数如下：φgz(t)＝kz4t4+kz3t3+kz2t2+kz1t+kz0；φgx(t)＝kx4t4+kx3t3+kx2t2+kx1t+kx0；其中，φgz(t)和φgx(t)分别为拟合得到的航向角和俯仰角的时间函数；kz0、kz1、kz2、kz3、kz4分别为航向角时间函数拟合的常系数、一次系数、二次系数、三次系数和四次系数；kx0、kx1、kx2、kx3、kx4分别为俯仰角时间函数拟合的常系数、一次系数、二次系数、三次系数和四次系数；时间变量t＝T0～T1。上述的基于前数据的旋转制导炮弹快速空中对准方法，对航向角时间函数φgz(t)和俯仰角时间函数φgx(t)进行求导运算，得到航向角变化率时间函数和俯仰角变化率时间函数其中：本发明与现有技术相比的优点如下：(1)、本发明通过卫星导航的速度信息和陀螺仪输出的角速度信息，实现对惯导系统初始横滚角的估计，不依赖于导航误差模型的正确性和精确程度，因此得到的横滚角估计结果更精确；(2)、本发明采用最小二乘法实现横滚角观测方程的求解，相对于现有技术中采用的卡尔曼滤波算法而言，该本发明的对准方法可以大大降低时间开销，提高运算速度和估计精度；(3)、本发明采用陀螺仪输出的角速度信息进行横滚角计算，适用于失重环境。(4)、本发明采用的前数据进行计算，可计算完整的飞行轨迹，在后续的组合导航中，后续数据多加了导航数据，相比较而言在时间上更早收敛，可控制时间更长。附图说明图1为本发明的基于前数据的旋转制导炮弹快速空中对准流程图；图2为使用本发明方法计算得到的俯仰角信息；图3为使用本发明方法计算得到的航向角信息；图4为使用本发明方法计算得到的俯仰角变化率信息；图5为使用本发明方法计算得到的航向角变化率信息；图6为弹上坐标系和陀螺输出轴；图7为使用本发明方法计算得到的初始横滚角信息。具体实施方式下面结合附图和具体实施例对本发明作进一步详细的描述：惯性导航系统是一个基于加速度二次积分的航程推算系统，它完全依靠机械设备和相应的算法自动、独立完成导航任务，和外界不发生任何光、电联系。由于其具有隐蔽性好、工作环境不受气象条件限制等优点，成为航天、航空、航海领域中一种广泛使用的主要导航系统。在惯性导航系统工作解算前，需要给出初始状态，就是需要进行初始对准。惯性导航系统在地面静止状态时，位置可以由GPS系统给出，三个姿态角可以通过惯性系统自对准给出，因为是静止状态，三个速度为零；惯性导航系统在空中飞行状态时，位置和速度仍然可以由GPS系统给出，但姿态角无法由惯性导航系统自对准给出。进行空中惯性导航系统自对准的有效途径是采用GPS导航信息解算和估计技术，即通过GPS输出的导航信息解算出对应时刻的航向角和俯仰角，因为位置和速度可以由GPS直接输出，所以初始状态中还剩横滚角一个参数需要进行估计解算。常用的对准方法是采用Kalman滤波算法实现，由于未考虑惯导工作在失重环境中，加速度计输出几乎为零，对横滚角的观测效果差，而且对准的精度不高且需要时间长。因此，为了提高自旋制导炮弹初始对准参数的精确性和修正算法的快速性，降低系统设计难度，快速修正导航姿态，提高导航结果的精确性，本发明提供了一种基于前数据的旋转制导炮弹快速空中对准方法。本发明根据最小二乘算法计算横滚角，其原理阐述如下：用GPS接收机提供的速度测量值在地面坐标系各轴上的分量Vx、Vy、Vz，解算出制导炮弹的弹道倾角和弹道偏角，计算公式如下GPS接收机输出的速度测量数据是不连续，可根据(1)式可以计算得到输出点处的弹道倾角和弹道偏角。输出点之间的数据可用曲线拟合方法获得，弹道倾角的变化率和弹道倾角的变化率可通过计算拟合曲线在各点处的斜率获得。弹箭绕质心转动运动的运动学方程，即姿态微分方程为：这里忽略了地球自转角速度对姿态的影响，ωxm、ωym、ωzm即为三个陀螺仪的输出数据。可以解算得到：结合式(2)和式(3)中可以解算出sinγ和cosγ，即：顺推算法就是运用导航初始点到设定时刻的数据来估算导航初始点的横滚角信息。可将横滚角γ写成角速率和初始横滚角的形式：由式(2)中可以解得对式(6)两边积分得故式(7)两边可写为比较式(4)和式(8)可得到式(8)可写为：运用最小二乘法估计，即可求得初始横滚角γ0。基于以上的理论分析，如图1所示的方法流程图，本发明提供的基于前数据的旋转制导炮弹快速空中对准方法，具体实现步骤如下:(1)、发射后的制导炮弹接收GPS导航信号进行导航处理，其中实现GPS导航信号捕获跟踪并输出导航结果的时刻为T0；然后从该时刻T0到设定的对准时刻T1保存GPS导航系统输出的M组GPS导航结果和INS输出的N组INS数据，其中：TGPS为GPS导航结果输出周期，TINS为INS数据输出周期，且TGPS＝Q×TINS，即N＝Q×M，Q为正整数。以上所述的GPS导航结果包括制导炮弹的速度和位置，INS数据包括前向陀螺、左向陀螺和上向陀螺输出的角速度，其中，前向陀螺敏感弹体横滚角角速度，左向陀螺敏感俯仰角角速度，上向陀螺敏感航向角角速度；(2)、根据M组GPS导航结果中的制导导弹的速度，计算得到相应的M组航向角和俯仰角，具体计算公式如下：其中：φgz和φgx分别为计算得到的制导炮弹的航向角和俯仰角；Vgn、Vge和Vgu分别为GPS导航结果中的制导炮弹的北速、东速和天速。(3)、由于GPS导航结果的输出周期太长，在INS数据的输出时刻不一定有对应的GPS导航结果输出，因此需要对步骤(2)计算得到的M组航向角和俯仰角的计算结果进行拟合计算，得到航向角和俯仰角在时刻T0到时刻T1之间随时间变换的函数；然后对所述函数进行求导运算，得到航向角变化率和俯仰角变化率的时间函数；在本实施例中，采用最小二乘4次曲线拟合方法对M组航向角和俯仰角的计算结果进行拟合计算，得到航向角和俯仰角在时刻T0到时刻T1之间随时间变换的函数如下：φgz(t)＝kz4t4+kz3t3+kz2t2+kz1t+kz0；φgx(t)＝kx4t4+kx3t3+kx2t2+kx1t+kx0；其中，φgz(t)和φgx(t)分别为拟合得到的航向角和俯仰角的时间函数；kz0、kz1、kz2、kz3、kz4分别为航向角时间函数拟合的常系数、一次系数、二次系数、三次系数和四次系数；kx0、kx1、kx2、kx3、kx4分别为俯仰角时间函数拟合的常系数、一次系数、二次系数、三次系数和四次系数；时间变量t＝T0～T1。然后对以上的航向角时间函数φgz(t)和俯仰角时间函数φgx(t)进行求导运算，得到航向角变化率时间函数和俯仰角变化率时间函数其中：(4)、将输出N组INS数据的时刻值代入到步骤(3)确定的4个时间函数中，计算得到输出N组INS数据时的制导炮弹的航向角、俯仰角、航向角变化率和俯仰角变化率；(5)、根据时刻T0到设定时刻T的Np组俯仰角、航向角变化率、俯仰角变化率和INS数据中的陀螺输出角速度，计算观测方程Z＝H×X中的观测矩阵H和测量矩阵Z；其中X为两维的测量向量，X(1)为制导炮弹横滚角的正弦值，X(2)为制导炮弹横滚角的余弦值；其中，设定时刻T的取值范围为T0≤T<T1，正整数在该步骤中，观测矩阵H和测量矩阵Z的具体计算过程如下：(5a)、对观测矩阵H和测量矩阵Z进行初始化，得到初始观测矩阵H0和测量矩阵Z0：如果初始化H0＝[a(T0)b(T0)]，则Z0＝z(T0)；如果初始化H0＝[-b(T0)a(T0)]，则Z0＝z′(T0)；其中：其中：ωx(T0)、ωy(T0)和ωz(T0)分别为时刻T0前向陀螺、左向陀螺和上向陀螺输出的角速度测量值；φgx(T0)为时刻T0的俯仰角；和分别为时刻T0的航向角变化率和俯仰角变化率；(5b)、在时刻Tn′＝T0+n×TINS，n＝1、2、…Np-1，按照如下的迭代公式对观测矩阵H和测量矩阵Z进行迭代更新，得到时刻Tn′的观测矩阵Hn和测量矩阵Zn；如果Hn＝[Hn-1；a(Tn′),b(Tn′)]，则Zn＝[Zn-1；z(Tn′)]；如果Hn＝[Hn-1；-b(Tn′),a(Tn′)]，则Zn＝[Zn-1；z′(Tn′)]；其中：其中：ωx(Tm′)、ωy(Tm′)和ωz(Tm′)分别为时刻Tm′前向陀螺、左向陀螺和上向陀螺输出的角速度测量值，φgx(Tm′)和分别为时刻Tm′的俯仰角和航向角变化率，m＝0～n且T0′＝T0；φgx(Tn′)为时刻Tn′的俯仰角；和分别为时刻Tn′的航向角变化率和俯仰角变化率；ωx(Tn′)、ωy(Tn′)和ωz(Tn′)分别为时刻Tn′前向陀螺、左向陀螺和上向陀螺输出的角速度测量值；(5c)、将步骤(5b)迭代得到的时刻T＝T0+(Np-1)×TINS的观测矩阵和测量矩阵作为观测矩阵H和测量矩阵Z的最终计算结果，用于步骤(6)的最小二乘计算。本发明提供的观测矩阵H和测量矩阵Z的计算方法，不易产生病态矩阵，且能够解除因陀螺仪输出误差引起的结果不可估算性，从而能够在任何情况下输出结果，不会引起算法发散或者无解，估算结果精度较高，因此适用于高速旋转制导炮弹。(6)、利用最小二乘法对观测方程Z＝H×X进行求解，得到观测向量X＝(HTH)-1HTZ；(7)、根据观测向量X计算结果中的制导炮弹横滚角的正弦值和余弦值，计算得到制导炮弹的横滚角，其中：当|X(1)|<1,|X(2)|<1时:如果X(1)>0,X(2)>0，γ0＝(arcsin(X(1))+arccos(X(2)))/2；如果X(1)>0,X(2)<0，γ0＝(180-arcsin(X(1))+arccos(X(2)))/2；如果X(1)<0,X(2)>0，γ0＝(arcsin(X(1))-arccos(X(2)))/2；如果X(1)<0,X(2)<0，γ0＝(-arcsin(X(1))-arccos(X(2)))/2。当|X(1)|>1,|X(2)|<1时:如果X(1)>0,X(2)>0，γ0＝arccos(X(2))/2；如果X(1)>0,X(2)<0，γ0＝arccos(X(2))；如果X(1)<0,X(2)>0，γ0＝-arccos(X(2))；如果X(1)<0,X(2)<0，γ0＝-arccos(X(2))。当|X(1)|<1,|X(2)|>1时:如果X(1)>0,X(2)>0，γ0＝arcsin(X(1))；如果X(1)>0,X(2)<0，γ0＝180-arcsin(X(1))；如果X(1)<0,X(2)>0，γ0＝arcsin(X(1))；如果X(1)<0,X(2)<0，γ0＝-arcsin(X(1))。(8)、将步骤(7)计算得到的横滚角，以及时刻T0GPS导航结果中的速度、位置和根据所述速度计算得到的航向角和俯仰角，作为空中对准结果，输出到制导炮弹的导航系统，用于对所述制导炮弹进行导航和控制。实施例：在本实施例中，在制导炮弹发射后，对GPS信号进行重新捕捉，在实现捕获跟踪并输出导航结果后，保存GPS导航结果和INS数据，并在到达设定对准时刻时，开始进行初始对准。其中，图2为根据GPS输出的速率计算得到的俯仰角，以及拟合得到的俯仰角曲线。从图2可以看出，在该过程中俯仰角变化范围较大。图3为根据俯仰角拟合曲线求导计算得到的俯仰角变化率曲线，从中可以看出俯仰角变化率的噪声比较大，拟合后减少噪声。图4中表示了航向角计算曲线和拟合曲线，可以从图中看出，航向角几乎不变。图5为航向角变化率曲线，可以从中看出航向角变化率的噪声比较大，拟合后减少噪声。图6表示了制导炮弹上的陀螺仪安装方式和输出信息的关系，三个陀螺的坐标系遵循右手准则，如图6放置时，前向陀螺敏感弹体横滚角的变化，左向陀螺敏感俯仰角的变化，上向陀螺敏感航向角的变化，它们的输出分别为ωx、ωy和ωz。图7为计算得到的初始横滚角曲线，可以从图中看出，无论多少个点进行计算初始横滚角，曲线结果收敛，收敛速度较快达到真值。这样就可以得到精确的初始横滚角信息，初始俯仰角信息，初始航向角信息，初始位置信息和初始速度信息，为后续的导航和控制提供了完备的导航信息。以上所述，仅为本发明一个具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。本发明说明书中未作详细描述的内容属于本领域专业技术人员的公知技术。