基于后数据的旋转制导炮弹快速空中对准方法

摘要

本发明提供一种基于后数据的旋转制导炮弹快速空中对准方法，该方法中对准的位置和速度由卫星导航系统给出，并利用卫星导航输出的速度信息解算出对应时刻的航向角和俯仰角，然后根据设定时刻T到对准时刻T

说明书

技术领域

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

背景技术

自旋制导炮弹是一种在空中发射，需要进行自我对准的一种高精度武 器，它包含了惯性导航和GPS等系统，通过GPS来修正惯性导航系统的误 差，达到精确打击目标的能力。空中对准从惯导相对其他导航系统提供的导 航参数(如速度等)的偏差中估计出惯导系统的失准角并校正之。

惯性导航系统是一个基于加速度二次积分的航程推算系统，它完全依靠 机械设备和相应的算法自动、独立完成导航任务，和外界不发生任何光、电 联系。由于其具有隐蔽性好、工作环境不受气象条件限制等优点，成为航天、 航空、航海领域中一种广泛使用的主要导航系统。在惯性导航系统工作解算 前，需要给出初始状态，就是需要进行初始对准。常用的对准方法是采用 Kalman滤波算法实现，该算法需要建立系统的误差模型，算法稳定性严重依 赖于导航误差模型的正确性和精确程度，而且时间开销较大，滤波周期较长； 另外在Kalman滤波过程中未考虑惯导工作在失重环境中，加速度计输出几 乎为零，对横滚角的观测效果差，而且对准的精度不高且需要时间长。

发明内容

本发明解决的技术问题是：克服现有技术的不足，提供一种基于后数据的 旋转制导炮弹快速空中对准方法，该方法中对准的位置和速度由卫星导航系统 给出，并利用卫星导航输出的速度信息解算出对应时刻的航向角和俯仰角，然 后再通过最小二乘法求解横滚角观测方程，从而实现对惯导初始位置、速度和 姿态角的精确解算，即实现自旋制导炮弹的快速空中对准，大大提高了自旋制 导炮弹的落点精度。

本发明的上述目的通过以下方案实现：

基于后数据的旋转制导炮弹快速空中对准方法，其特征在于包括如下步骤：

(1)、发射后的制导炮弹接收卫星导航信号进行导航处理，其中实现卫星 导航信号捕获跟踪并输出导航结果的时刻为T₀；然后从所述时刻T₀到设定的对 准时刻T₁保存卫星导航系统输出的M组卫星导航结果和INS输出的N组INS数 据，其中：T_GPS为卫星导航结果输出周期，T_INS为INS 数据输出周期，且T_GPS＝Q×T_INS，即N＝Q×M，Q为正整数；

所述卫星导航结果包括制导炮弹的速度和位置；所述INS数据包括前向陀 螺、左向陀螺和上向陀螺输出的角速度，其中，前向陀螺敏感弹体横滚角角速 度，左向陀螺敏感俯仰角角速度，上向陀螺敏感航向角角速度；

(2)、根据M组卫星导航结果中的制导导弹的速度，计算得到相应的M组 航向角和俯仰角；

(3)、对步骤(2)计算得到的M组航向角和俯仰角的计算结果进行拟合 计算，得到航向角和俯仰角在时刻T₀到时刻T₁之间随时间变换的函数；然后对 所述函数进行求导运算，得到航向角变化率和俯仰角变化率的时间函数；

(4)、将输出N组INS数据的时刻值代入到步骤(3)确定的4个时间函数 中，计算得到输出N组INS数据时的制导炮弹的航向角、俯仰角、航向角变化 率和俯仰角变化率；

(5)、根据设定时刻T到时刻T₁的N_p′组俯仰角、航向角变化率、俯仰角变 化率和INS数据中的陀螺输出角速度，计算观测方程Z＝H×X中的观测矩阵H和 测量矩阵Z；其中X为两维的测量向量，X(1)为制导炮弹横滚角的正弦值，X(2) 为制导炮弹横滚角的余弦值；其中，设定时刻T的取值范围为T₀≤T<T₁，正整 数${N_p}^{\prime }=\frac{T_1-T}{T_{INS}};$

(6)、利用最小二乘法对观测方程Z＝H×X进行求解，得到观测向量 X＝(H^TH)^-1H^TZ；

(7)、根据观测向量X计算结果中的制导炮弹横滚角的正弦值和余弦值， 计算得到制导炮弹的横滚角；

(8)、将步骤(7)计算得到的横滚角，以及时刻T₁的卫星导航系统输出的 速度、位置，以及根据所述速度计算得到的航向角和俯仰角，作为空中对准结 果，输出到制导炮弹的导航系统，用于对所述制导炮弹进行导航和控制。

上述的基于后数据的旋转制导炮弹快速空中对准方法，在步骤(5)中， 根据时刻T～T₁的N_p′组俯仰角、航向角变化率和俯仰角变化率，以及INS数据 中的陀螺输出角速度，计算观测矩阵H和测量矩阵Z，具体计算过程如下：

(5a)、对观测矩阵H和测量矩阵Z进行初始化，得到初始观测矩阵H₀和 测量矩阵Z₀：

如果初始化H₀＝[a(T₁-T_INS)b(T₁-T_INS)]，则Z₀＝z(T₁-T_INS)；

如果初始化H₀＝[-b(T₁-T_INS)a(T₁-T_INS)]，则Z₀＝z′(T₁-T_INS)；

其中：

$a(T_1-T_{INS})=cos[\left({\omega }_x\right(T_1-T_{INS})+▿{\phi }_{gz}(T_1-T_{INS})sin({\phi }_{gx}\left(T_1-T_{INS}\right)\left)\right)\times T_{INS}];$

$b(T_1-T_{INS})=sin[\left({\omega }_x\right(T_1-T_{INS})+▿{\phi }_{gz}(T_1-T_{INS}\left)sin\right({\phi }_{gx}\left(T_1-T_{INS}\right)\left)\right)\times T_{INS}];$

$z(T_1-T_{INS})=\frac{{\omega }_y(T_1-T_{INS})▿{\phi }_{gz}(T_1-T_{INS})cos\left({\phi }_{gx}\right(T_1-T_{INS}\left)\right)-{\omega }_z(T_1-T_{INS})▿{\phi }_{gx}(T_1-T_{INS})}{▿{\phi }_{gx}{(T_1-T_{INS})}^2+{(▿{\phi }_{gz}(T_1-T_{INS}\left)cos\right({\phi }_{gx}\left(T_1-T_{INS}\right)\left)\right)}^2};$

$z^{\prime }(T_1-T_{INS})=\frac{{\omega }_z(T_1-T_{INS})▿{\phi }_{gz}(T_1-T_{INS})cos\left({\phi }_{gx}\right(T_1-T_{INS}\left)\right)+{\omega }_y(T_1-T_{INS})▿{\phi }_{gx}(T_1-T_{INS})}{▿{\phi }_{gx}{(T_1-T_{INS})}^2+{(▿{\phi }_{gz}(T_1-T_{INS}\left)cos\right({\phi }_{gx}\left(T_1-T_{INS}\right)\left)\right)}^2};$

其中：ω_x(T₁-T_INS)、ω_y(T₁-T_INS)和ω_z(T₁-T_INS)分别为时刻T₁-T_INS前向陀螺、 左向陀螺和上向陀螺输出的角速度测量值；φ_gx(T₁-T_INS)为时刻T₁-T_INS的俯仰角； 和分别为时刻T₁-T_INS的航向角变化率和俯仰角变化率；

(5b)、在时刻T_n′＝T₁-(n+1)×T_INS，n＝1、2、…N_p′-1，按照如下的迭代公式 对观测矩阵H和测量矩阵Z进行迭代更新，得到时刻T_n′的观测矩阵H_n和测量矩 阵Z_n；

如果H_n＝[H_n-1；a(T_n′),b(T_n′)]，则Z_n＝[Z_n-1；z(T_n′)]；

如果H_n＝[H_n-1；-b(T_n′),a(T_n′)]，则Z_n＝[Z_n-1；z′(T_n′)]；

其中：

$a\left(T_n^{\prime }\right)=cos[\underset{m=0}{\overset{n}{\Sigma }}({\omega }_x\left(T_m^{\prime }\right)+▿{\phi }_{gz}(T_m^{\prime }\left)sin\right({\phi }_{gx}\left(T_m^{\prime }\right)\left)\right)\times T_{INS}];$

$b\left(T_n^{\prime }\right)=sin[\underset{m=0}{\overset{n}{\Sigma }}({\omega }_x\left(T_m^{\prime }\right)+▿{\phi }_{gz}(T_m^{\prime }\left)sin\right({\phi }_{gx}\left(T_m^{\prime }\right)\left)\right)\times T_{INS}];$

$z\left(T_n^{\prime }\right)=\frac{{\omega }_y\left(T_n^{\prime }\right)▿{\phi }_{gz}\left(T_n^{\prime }\right)cos\left({\phi }_{gx}\right(T_n^{\prime }\left)\right)-{\omega }_z\left(T_n^{\prime }\right)▿{\phi }_{gx}\left(T_n^{\prime }\right)}{▿{\phi }_{gx}{\left(T_n^{\prime }\right)}^2+{(▿{\phi }_{gz}(T_n^{\prime }\left)cos\right({\phi }_{gx}\left(T_n^{\prime }\right)\left)\right)}^2};$

$z^{\prime }\left(T_n^{\prime }\right)=\frac{{\omega }_z\left(T_n^{\prime }\right)▿{\phi }_{gz}\left(T_n^{\prime }\right)cos\left({\phi }_{gx}\right(T_n^{\prime }\left)\right)+{\omega }_y\left(T_n^{\prime }\right)▿{\phi }_{gx}\left(T_n^{\prime }\right)}{▿{\phi }_{gx}{\left(T_n^{\prime }\right)}^2+{(▿{\phi }_{gz}(T_n^{\prime }\left)cos\right({\phi }_{gx}\left(T_n^{\prime }\right)\left)\right)}^2};$

其中：ω_x(T_m′)、ω_y(T_m′)和ω_z(T_m′)分别为时刻T_m′前向陀螺、左向陀螺和上向陀 螺输出的角速度测量值，φ_gx(T_m′)和分别为时刻T_m′的俯仰角和航向角变化 率，m＝0～n且T₀′＝T₁-T_INS；φ_gx(T_n′)为时刻T_n′的俯仰角；和分别为 时刻T_n′的航向角变化率和俯仰角变化率；ω_x(T_n′)、ω_y(T_n′)和ω_z(T_n′)分别为时刻T_n′前 向陀螺、左向陀螺和上向陀螺输出的角速度测量值；

(5c)、将步骤(5b)迭代得到的时刻T＝T₁-N_p′×T_INS的观测矩阵和测 量矩阵Z_N′p-1作为观测矩阵H和测量矩阵Z的最终计算结果，用于步骤(6)的最 小二乘计算。

上述的基于后数据的旋转制导炮弹快速空中对准方法，在步骤(7)中， 根据观测向量X计算结果中的制导炮弹横滚角的正弦值和余弦值，计算得到制 导炮弹的横滚角γ₀，具体计算方法如下：

当|X(1)|<1,|X(2)|<1时，

如果X(1)>0,X(2)>0，γ₀＝(arcsin(X(1))+arccos(X(2)))/2；

如果X(1)>0,X(2)<0，γ₀＝(180-arcsin(X(1))+arccos(X(2)))/2；

如果X(1)<0,X(2)>0，γ₀＝(arcsin(X(1))-arccos(X(2)))/2；

如果X(1)<0,X(2)<0，γ₀＝(-arcsin(X(1))-arccos(X(2)))/2。

当|X(1)|>1,|X(2)|<1时，

如果X(1)>0,X(2)>0，γ₀＝arccos(X(2))/2；

如果X(1)>0,X(2)<0，γ₀＝arccos(X(2))；

如果X(1)<0,X(2)>0，γ₀＝-arccos(X(2))；

如果X(1)<0,X(2)<0，γ₀＝-arccos(X(2))。

当|X(1)|<1,|X(2)|>1时，

如果X(1)>0,X(2)>0，γ₀＝arcsin(X(1))；

如果X(1)>0,X(2)<0，γ₀＝180-arcsin(X(1))；

如果X(1)<0,X(2)>0，γ₀＝arcsin(X(1))；

如果X(1)<0,X(2)<0，γ₀＝-arcsin(X(1))。

上述的基于后数据的旋转制导炮弹快速空中对准方法，在步骤(2)中，根 据制导炮弹速度计算航向角和俯仰角的公式如下：

${\phi }_{gz}=\arctan \left(\frac{V_{gn}}{V_{ge}}\right);{\phi }_{gx}=-\arctan \left(\frac{V_{gu}}{\sqrt{V_{ge}^2+V_{gn}^2}}\right);$

其中：φ_gz和φ_gx分别为制导炮弹的航向角和俯仰角；V_gn、V_ge和V_gu分别为制 导炮弹的北速、东速和天速。

上述的基于后数据的旋转制导炮弹快速空中对准方法，在步骤(3)中，采 用最小二乘4次曲线拟合方法对M组航向角和俯仰角的计算结果进行拟合计 算，得到航向角和俯仰角在时刻T₀到时刻T₁之间随时间变换的函数如下：

φ_gz(t)＝k_z4t⁴+k_z3t³+k_z2t²+k_z1t+k_z0；

φ_gx(t)＝k_x4t⁴+k_x3t³+k_x2t²+k_x1t+k_x0；

其中，φ_gz(t)和φ_gx(t)分别为拟合得到的航向角和俯仰角的时间函数；k_z0、k_z1、 k_z2、k_z3、k_z4分别为航向角时间函数拟合的常系数、一次系数、二次系数、三次 系数和四次系数；k_x0、k_x1、k_x2、k_x3、k_x4分别为俯仰角时间函数拟合的常系数、 一次系数、二次系数、三次系数和四次系数；时间变量t＝T₀～T₁。

上述的基于后数据的旋转制导炮弹快速空中对准方法，对航向角时间函数 φ_gz(t)和俯仰角时间函数φ_gx(t)进行求导运算，得到航向角变化率时间函数和俯仰角变化率时间函数其中：

$▿{\phi }_{gz}\left(t\right)=4k_{z4}{t}^3+3k_{z3}{t}^2+2k_{z2}t+k_{z1};$

$▿{\phi }_{gx}\left(t\right)=4k_{x4}{t}^3+3k_{x3}{t}^2+2k_{x2}t+k_{x1}.$

本发明与现有技术相比的优点如下：

(1)、本发明通过卫星导航的速度信息和陀螺仪输出的角速度信息，实现 对惯导系统初始横滚角的估计，不依赖于导航误差模型的正确性和精确程度， 因此得到的横滚角估计结果更精确；

(2)、本发明采用最小二乘法实现横滚角观测方程的求解，相对于现有技 术中采用的卡尔曼滤波算法而言，该本发明的对准方法可以大大降低时间开销， 提高运算速度和估计精度；

(3)、本发明采用陀螺仪输出的角速度信息进行横滚角计算，适用于失重 环境。

(4)采用后数据计算方法，有利于在实弹中利用，计算结果可以直接装填， 作为初始的状态量，跟前数据方法比更加快捷，有实时性，在实弹飞行中计算 量也会相对较小。

附图说明

图1为本发明的基于后数据的旋转制导炮弹快速空中对准方法流程图；

图2为使用本发明方法计算得到的俯仰角信息；

图3为使用本发明方法计算得到的航向角信息；

图4为使用本发明方法计算得到的俯仰角变化率信息；

图5为使用本发明方法计算得到的航向角变化率信息；

图6为弹上坐标系和陀螺输出轴；

图7为使用本发明方法计算得到的初始横滚角信息。

具体实施方式

下面结合附图和具体实施例对本发明作进一步详细的描述：

惯性导航系统是一个基于加速度二次积分的航程推算系统，它完全依靠机 械设备和相应的算法自动、独立完成导航任务，和外界不发生任何光、电联系。 由于其具有隐蔽性好、工作环境不受气象条件限制等优点，成为航天、航空、 航海领域中一种广泛使用的主要导航系统。在惯性导航系统工作解算前，需要 给出初始状态，就是需要进行初始对准。惯性导航系统在地面静止状态时，位 置可以由GPS系统给出，三个姿态角可以通过惯性系统自对准给出，因为是 静止状态，三个速度为零；惯性导航系统在空中飞行状态时，位置和速度仍然 可以由GPS系统给出，但姿态角无法由惯性导航系统自对准给出。进行空中 惯性导航系统自对准的有效途径是采用GPS导航信息解算和估计技术，即通 过GPS输出的导航信息解算出对应时刻的航向角和俯仰角，因为位置和速度 可以由GPS直接输出，所以初始状态中还剩横滚角一个参数需要进行估计解 算。

常用的对准方法是采用Kalman滤波算法实现，由于未考虑惯导工作在失 重环境中，加速度计输出几乎为零，对横滚角的观测效果差，而且对准的精度 不高且需要时间长。因此，为了提高自旋制导炮弹初始对准参数的精确性和修 正算法的快速性，降低系统设计难度，快速修正导航姿态，提高导航结果的精 确性，本发明提供了一种基于后数据的旋转制导炮弹快速空中对准方法。

本发明根据最小二乘算法计算横滚角，其原理阐述如下：

用GPS接收机提供的速度测量值在地面坐标系各轴上的分量V_x、V_y、V_z， 解算出制导炮弹的弹道倾角和弹道偏角，计算公式如下

$\left(\begin{array}{c}\theta =-\arctan (V_z/\sqrt{V_x^2+V_y^2})\\ \psi =\arctan (V_y/V_x)\end{array}\right)---\left(1\right)$

GPS接收机输出的速度测量数据是不连续，可根据(1)式可以计算得 到输出点处的弹道倾角和弹道偏角。输出点之间的数据可用曲线拟合方法获 得，弹道倾角的变化率和弹道倾角的变化率可通过计算拟合曲线在各点处的 斜率获得。

弹箭绕质心转动运动的运动学方程，即姿态微分方程为：

这里忽略了地球自转角速度对姿态的影响，ω_xm、ω_ym、ω_zm即为三个陀螺 仪的输出数据。

可以解算得到：

${\omega }_{ym}^2+{\omega }_{zm}^2={\stackrel{·}{\theta }}^2+{\left(\stackrel{·}{\psi }cos\theta \right)}^2---\left(3\right)$

结合式(2)和式(3)中可以解算出sinγ和cosγ，即：

逆推算法就是运用对准时刻之前的数据来估算导航初始点的横滚角信 息。可将横滚角γ写成角速率和初始横滚角的形式：

$\gamma ={\gamma }_0-\int \stackrel{·}{\gamma }dt---\left(5\right)$

对式(5)两边积分得

故式(6)两边可写为

$\{\begin{array}{c}\sin \gamma =\cos [\int ({\omega }_{xm}+\stackrel{·}{\psi }\sin \theta )dt]{\mathrm{sin\gamma }}_0-\sin [\int ({\omega }_{xm}+\stackrel{·}{\psi }\sin \theta )dt]{\mathrm{cos\gamma }}_0\\ \cos \gamma =\cos [\int ({\omega }_{xm}+\stackrel{·}{\psi }\sin \theta )dt]{\mathrm{cos\gamma }}_0+\sin [\int ({\omega }_{xm}+\stackrel{·}{\psi }\sin \theta )dt]{\mathrm{sin\gamma }}_0\end{array}---\left(7\right)$

比较式(4)和式(7)可得到

式(7)可写为：

$\left(\begin{array}{cc}a\left(t\right)& -b\left(t\right)\\ b\left(t\right)& a\left(t\right)\end{array}\right)·\left(\begin{array}{c}{\mathrm{sin\gamma }}_0\\ {\mathrm{cos\gamma }}_0\end{array}\right)=\left(\begin{array}{c}{z}_1\left(t\right)\\ z_2\left(t\right)\end{array}\right)---\left(10\right)$

运用最小二乘法估计，即可求得初始横滚角γ₀。

基于以上的理论分析，如图1所示的方法流程图，本发明提供的基于后数 据的旋转制导炮弹快速空中对准方法，具体实现步骤如下:

(1)、发射后的制导炮弹接收GPS导航信号进行导航处理，其中实现GPS 导航信号捕获跟踪并输出导航结果的时刻为T₀；然后从该时刻T₀到设定的对准 时刻T₁保存GPS导航系统输出的M组GPS导航结果和INS输出的N组INS数 据，其中：T_GPS为GPS导航结果输出周期，T_INS为 INS数据输出周期，且T_GPS＝Q×T_INS，即N＝Q×M，Q为正整数。

以上所述的GPS导航结果包括制导炮弹的速度和位置，INS数据包括前向 陀螺、左向陀螺和上向陀螺输出的角速度，其中，前向陀螺敏感弹体横滚角角 速度，左向陀螺敏感俯仰角角速度，上向陀螺敏感航向角角速度；

(2)、根据M组GPS导航结果中的制导导弹的速度，计算得到相应的M组 航向角和俯仰角，具体计算公式如下：

${\phi }_{gz}=\arctan \left(\frac{V_{gn}}{V_{ge}}\right);{\phi }_{gx}=-\arctan \left(\frac{V_{gu}}{\sqrt{V_{ge}^2+V_{gn}^2}}\right);$

其中：φ_gz和φ_gx分别为计算得到的制导炮弹的航向角和俯仰角；V_gn、V_ge和V_gu分别为GPS导航结果中的制导炮弹的北速、东速和天速。

(3)、由于GPS导航结果的输出周期太长，在INS数据的输出时刻不一定 有对应的GPS导航结果输出，因此需要对步骤(2)计算得到的M组航向角和 俯仰角的计算结果进行拟合计算，得到航向角和俯仰角在时刻T₀到时刻T₁之间 随时间变换的函数；然后对所述函数进行求导运算，得到航向角变化率和俯仰 角变化率的时间函数；

在本实施例中，采用最小二乘4次曲线拟合方法对M组航向角和俯仰角的 计算结果进行拟合计算，得到航向角和俯仰角在时刻T₀到时刻T₁之间随时间变 换的函数如下：

φ_gz(t)＝k_z4t⁴+k_z3t³+k_z2t²+k_z1t+k_z0；

φ_gx(t)＝k_x4t⁴+k_x3t³+k_x2t²+k_x1t+k_x0；

其中，φ_gz(t)和φ_gx(t)分别为拟合得到的航向角和俯仰角的时间函数；k_z0、k_z1、 k_z2、k_z3、k_z4分别为航向角时间函数拟合的常系数、一次系数、二次系数、三次 系数和四次系数；k_x0、k_x1、k_x2、k_x3、k_x4分别为俯仰角时间函数拟合的常系数、 一次系数、二次系数、三次系数和四次系数；时间变量t＝T₀～T₁。

然后对以上的航向角时间函数φ_gz(t)和俯仰角时间函数φ_gx(t)进行求导运算， 得到航向角变化率时间函数和俯仰角变化率时间函数其中：

$▿{\phi }_{gz}\left(t\right)=4k_{z4}{t}^3+3k_{z3}{t}^2+2k_{z2}t+k_{z1};$

$▿{\phi }_{gx}\left(t\right)=4k_{x4}{t}^3+3k_{x3}{t}^2+2k_{x2}t+k_{x1}.$

(4)、将输出N组INS数据的时刻值代入到步骤(3)确定的4个时间函数 中，计算得到输出N组INS数据时的制导炮弹的航向角、俯仰角、航向角变化 率和俯仰角变化率；

(5)、根据设定时刻T到时刻T₁的N_p′组俯仰角、航向角变化率、俯仰角变 化率和INS数据中的陀螺输出角速度，计算观测方程Z＝H×X中的观测矩阵H和 测量矩阵Z；其中X为两维的测量向量，X(1)为制导炮弹横滚角的正弦值，X(2) 为制导炮弹横滚角的余弦值；其中，设定时刻T的取值范围为T₀≤T<T₁，正整 数${N_p}^{\prime }=\frac{T_1-T}{T_{INS}};$

在该步骤中，观测矩阵H和测量矩阵Z的具体计算过程如下：

(5a)、对观测矩阵H和测量矩阵Z进行初始化，得到初始观测矩阵H₀和 测量矩阵Z₀：

如果初始化H₀＝[a(T₁-T_INS)b(T₁-T_INS)]，则Z₀＝z(T₁-T_INS)；

如果初始化H₀＝[-b(T₁-T_INS)a(T₁-T_INS)]，则Z₀＝z′(T₁-T_INS)；

其中：

$a(T_1-T_{INS})=cos[\left({\omega }_x\right(T_1-T_{INS})+▿{\phi }_{gz}(T_1-T_{INS})sin({\phi }_{gx}\left(T_1-T_{INS}\right)\left)\right)\times T_{INS}];$

$b(T_1-T_{INS})=\sin [\left({\omega }_x\right(T_1-T_{INS})+▿{\phi }_{gz}(T_1-T_{INS})sin({\phi }_{gx}\left(T_1-T_{INS}\right)\left)\right)\times T_{INS}];$

$z(T_1-T_{INS})=\frac{{\omega }_y(T_1-T_{INS})▿{\phi }_{gz}(T_1-T_{INS})cos\left({\phi }_{gx}\right(T_1-T_{INS}\left)\right)-{\omega }_z(T_1-T_{INS})▿{\phi }_{gx}(T_1-T_{INS})}{▿{\phi }_{gx}{(T_1-T_{INS})}^2+{(▿{\phi }_{gz}(T_1-T_{INS}\left)cos\right({\phi }_{gx}\left(T_1-T_{INS}\right)\left)\right)}^2};$

$z^{\prime }(T_1-T_{INS})=\frac{{\omega }_z(T_1-T_{INS})▿{\phi }_{gz}(T_1-T_{INS})cos\left({\phi }_{gx}\right(T_1-T_{INS}\left)\right)+{\omega }_y(T_1-T_{INS})▿{\phi }_{gx}(T_1-T_{INS})}{▿{\phi }_{gx}{(T_1-T_{INS})}^2+{(▿{\phi }_{gz}(T_1-T_{INS}\left)cos\right({\phi }_{gx}\left(T_1-T_{INS}\right)\left)\right)}^2};$

其中：ω_x(T₁-T_INS)、ω_y(T₁-T_INS)和ω_z(T₁-T_INS)分别为时刻T₁-T_INS前向陀螺、 左向陀螺和上向陀螺输出的角速度测量值；φ_gx(T₁-T_INS)为时刻T₁-T_INS的俯仰角； 和分别为时刻T₁-T_INS的航向角变化率和俯仰角变化率；

(5b)、在时刻T_n′＝T₁-(n+1)×T_INS，n＝1、2、…N_p′-1，按照如下的迭代公式 对观测矩阵H和测量矩阵Z进行迭代更新，得到时刻T_n′的观测矩阵H_n和测量矩 阵Z_n；

如果H_n＝[H_n-1；a(T_n′),b(T_n′)]，则Z_n＝[Z_n-1；z(T_n′)]；

如果H_n＝[H_n-1；-b(T_n′),a(T_n′)]，则Z_n＝[Z_n-1；z′(T_n′)]；

其中：

$a\left(T_n^{\prime }\right)=cos[\underset{m=0}{\overset{n}{\Sigma }}({\omega }_x\left(T_m^{\prime }\right)+▿{\phi }_{gz}(T_m^{\prime }\left)sin\right({\phi }_{gx}\left(T_m^{\prime }\right)\left)\right)\times T_{INS}];$

$b\left(T_n^{\prime }\right)=sin[\underset{m=0}{\overset{n}{\Sigma }}({\omega }_x\left(T_m^{\prime }\right)+▿{\phi }_{gz}(T_m^{\prime }\left)sin\right({\phi }_{gx}\left(T_m^{\prime }\right)\left)\right)\times T_{INS}];$

$z\left(T_n^{\prime }\right)=\frac{{\omega }_y\left(T_n^{\prime }\right)▿{\phi }_{gz}\left(T_n^{\prime }\right)cos\left({\phi }_{gx}\right(T_n^{\prime }\left)\right)-{\omega }_z\left(T_n^{\prime }\right)▿{\phi }_{gx}\left(T_n^{\prime }\right)}{▿{\phi }_{gx}{\left(T_n^{\prime }\right)}^2+{(▿{\phi }_{gz}(T_n^{\prime }\left)cos\right({\phi }_{gx}\left(T_n^{\prime }\right)\left)\right)}^2};$

$z^{\prime }\left(T_n^{\prime }\right)=\frac{{\omega }_z\left(T_n^{\prime }\right)▿{\phi }_{gz}\left(T_n^{\prime }\right)cos\left({\phi }_{gx}\right(T_n^{\prime }\left)\right)+{\omega }_y\left(T_n^{\prime }\right)▿{\phi }_{gx}\left(T_n^{\prime }\right)}{▿{\phi }_{gx}{\left(T_n^{\prime }\right)}^2+{(▿{\phi }_{gz}(T_n^{\prime }\left)cos\right({\phi }_{gx}\left(T_n^{\prime }\right)\left)\right)}^2};$

其中：ω_x(T_m′)、ω_y(T_m′)和ω_z(T_m′)分别为时刻T_m′前向陀螺、左向陀螺和上向陀 螺输出的角速度测量值，φ_gx(T_m′)和分别为时刻T_m′的俯仰角和航向角变化 率，m＝0～n且T₀′＝T₁-T_INS；φ_gx(T_n′)为时刻T_n′的俯仰角；和分别为 时刻T_n′的航向角变化率和俯仰角变化率；ω_x(T_n′)、ω_y(T_n′)和ω_z(T_n′)分别为时刻T_n′前 向陀螺、左向陀螺和上向陀螺输出的角速度测量值；

(5c)、将步骤(5b)迭代得到的时刻T＝T₁-N_p′×T_INS的观测矩阵和测 量矩阵Z_Np′-1作为观测矩阵H和测量矩阵Z的最终计算结果，用于步骤(6)的最 小二乘计算。

本发明提供的观测矩阵H和测量矩阵Z的计算方法，不易产生病态矩阵， 且能够解除因陀螺仪输出误差引起的结果不可估算性，从而能够在任何情况下 输出结果，不会引起算法发散或者无解，估算结果精度较高，因此适用于高速 旋转制导炮弹。

(6)、利用最小二乘法对观测方程Z＝H×X进行求解，得到观测向量 X＝(H^TH)^-1H^TZ；

(7)、根据观测向量X计算结果中的制导炮弹横滚角的正弦值和余弦值， 计算得到制导炮弹的横滚角，其中：

当|X(1)|<1,|X(2)|<1时，

如果X(1)>0,X(2)>0，γ₀＝(arcsin(X(1))+arccos(X(2)))/2；

如果X(1)>0,X(2)<0，γ₀＝(180-arcsin(X(1))+arccos(X(2)))/2；

如果X(1)<0,X(2)>0，γ₀＝(arcsin(X(1))-arccos(X(2)))/2；

如果X(1)<0,X(2)<0，γ₀＝(-arcsin(X(1))-arccos(X(2)))/2。

当|X(1)|>1,|X(2)|<1时，

如果X(1)>0,X(2)>0，γ₀＝arccos(X(2))/2；

如果X(1)>0,X(2)<0，γ₀＝arccos(X(2))；

如果X(1)<0,X(2)>0，γ₀＝-arccos(X(2))；

如果X(1)<0,X(2)<0，γ₀＝-arccos(X(2))。

当|X(1)|<1,|X(2)|>1时，

如果X(1)>0,X(2)>0，γ₀＝arcsin(X(1))；

如果X(1)>0,X(2)<0，γ₀＝180-arcsin(X(1))；

如果X(1)<0,X(2)>0，γ₀＝arcsin(X(1))；

如果X(1)<0,X(2)<0，γ₀＝-arcsin(X(1))。

(8)、将步骤(7)计算得到的横滚角，以及时刻TGPS导航结果中的速度、 位置和根据所述速度计算得到的航向角和俯仰角，作为空中对准结果，输出到 制导炮弹的导航系统，用于对所述制导炮弹进行导航和控制。

实施例：

在本实施例中，在制导炮弹发射后，对GPS信号进行重新捕捉，在实现捕 获跟踪并输出导航结果后，保存GPS导航结果和INS数据，并在到达设定对准 时刻时，开始进行初始对准。

其中，图2为根据GPS输出的速率计算得到的俯仰角，以及拟合得到的俯 仰角曲线。从图2可以看出，在该过程中俯仰角变化范围较大。图3为根据俯仰 角拟合曲线求导计算得到的俯仰角变化率曲线，从中可以看出俯仰角变化率的 噪声比较大，拟合后减少噪声。图4中表示了航向角计算曲线和拟合曲线，可以 从图中看出，航向角几乎不变。图5为航向角变化率曲线，可以从中看出航向角 变化率的噪声比较大，拟合后减少噪声。图6表示了制导炮弹上的陀螺仪安装方 式和输出信息的关系，三个陀螺的坐标系遵循右手准则，如图6放置时，前向陀 螺敏感弹体横滚角的变化，左向陀螺敏感俯仰角的变化，上向陀螺敏感航向角 的变化，它们的输出分别为ω_x、ω_y和ω_z。图7为计算得到的初始横滚角曲线， 可以从图中看出，无论多少个点进行计算初始横滚角，曲线结果收敛，收敛速 度较快达到真值。这样就可以得到精确的初始横滚角信息，初始俯仰角信息， 初始航向角信息，初始位置信息和初始速度信息，为后续的导航和控制提供了 完备的导航信息。

以上所述，仅为本发明一个具体实施方式，但本发明的保护范围并不局限 于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想 到的变化或替换，都应涵盖在本发明的保护范围之内。

本发明说明书中未作详细描述的内容属于本领域专业技术人员的公知技 术。