CN105115508B - 基于后数据的旋转制导炮弹快速空中对准方法 - Google Patents
基于后数据的旋转制导炮弹快速空中对准方法 Download PDFInfo
- Publication number
- CN105115508B CN105115508B CN201510537052.7A CN201510537052A CN105115508B CN 105115508 B CN105115508 B CN 105115508B CN 201510537052 A CN201510537052 A CN 201510537052A CN 105115508 B CN105115508 B CN 105115508B
- Authority
- CN
- China
- Prior art keywords
- time
- ins
- angle
- course
- pitch
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- F—MECHANICAL ENGINEERING; LIGHTING; HEATING; WEAPONS; BLASTING
- F42—AMMUNITION; BLASTING
- F42B—EXPLOSIVE CHARGES, e.g. FOR BLASTING, FIREWORKS, AMMUNITION
- F42B15/00—Self-propelled projectiles or missiles, e.g. rockets; Guided missiles
- F42B15/01—Arrangements thereon for guidance or control
-
- G—PHYSICS
- G16—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR SPECIFIC APPLICATION FIELDS
- G16Z—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR SPECIFIC APPLICATION FIELDS, NOT OTHERWISE PROVIDED FOR
- G16Z99/00—Subject matter not provided for in other main groups of this subclass
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Chemical & Material Sciences (AREA)
- Aviation & Aerospace Engineering (AREA)
- Combustion & Propulsion (AREA)
- Navigation (AREA)
- Pharmaceuticals Containing Other Organic And Inorganic Compounds (AREA)
Abstract
本发明提供一种基于后数据的旋转制导炮弹快速空中对准方法,该方法中对准的位置和速度由卫星导航系统给出,并利用卫星导航输出的速度信息解算出对应时刻的航向角和俯仰角,然后根据设定时刻T到对准时刻T1的俯仰角、航向角变化率、俯仰角变化率和INS数据中的陀螺输出角速度,确定横滚角观测方程的系数矩阵,并通过最小二乘法求解横滚角观测方程,从而实现对惯导初始位置、速度和姿态角的精确解算,即实现自旋制导炮弹的快速空中对准,大大提高了自旋制导炮弹的落点精度;该对准算法简单、精度高,而且对准时间短,对准速度快,为提高自旋制导炮弹的落点精度和缩短打击时间做出了重要铺垫。
Description
技术领域
本发明涉及初始对准技术和组合导航系统技术领域,特别涉及一种基于后数据的旋转制导炮弹快速空中对准方法,可以用于无人机、自旋制导炮弹等需要在空中自对准的场合。
背景技术
自旋制导炮弹是一种在空中发射,需要进行自我对准的一种高精度武器,它包含了惯性导航和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)、根据设定时刻T到时刻T1的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)计算得到的横滚角,以及时刻T1的卫星导航系统输出的速度、位置,以及根据所述速度计算得到的航向角和俯仰角,作为空中对准结果,输出到制导炮弹的导航系统,用于对所述制导炮弹进行导航和控制。
上述的基于后数据的旋转制导炮弹快速空中对准方法,在步骤(5)中,根据时刻T~T1的Np′组俯仰角、航向角变化率和俯仰角变化率,以及INS数据中的陀螺输出角速度,计算观测矩阵H和测量矩阵Z,具体计算过程如下:
(5a)、对观测矩阵H和测量矩阵Z进行初始化,得到初始观测矩阵H0和测量矩阵Z0:
如果初始化H0=[a(T1-TINS)b(T1-TINS)],则Z0=z(T1-TINS);
如果初始化H0=[-b(T1-TINS)a(T1-TINS)],则Z0=z′(T1-TINS);
其中:
其中:ωx(T1-TINS)、ωy(T1-TINS)和ωz(T1-TINS)分别为时刻T1-TINS前向陀螺、左向陀螺和上向陀螺输出的角速度测量值;φgx(T1-TINS)为时刻T1-TINS的俯仰角;和分别为时刻T1-TINS的航向角变化率和俯仰角变化率;
(5b)、在时刻T′n=T1-(n+1)×TINS,n=1、2、…Np′-1,按照如下的迭代公式对观测矩阵H和测量矩阵Z进行迭代更新,得到时刻T′n的观测矩阵Hn和测量矩阵Zn;
如果Hn=[Hn-1;a(T′n),b(T′n)],则Zn=[Zn-1;z(T′n)];
如果Hn=[Hn-1;-b(T′n),a(T′n)],则Zn=[Zn-1;z′(T′n)];
其中:
其中:ωx(T′m)、ωy(T′m)和ωz(T′m)分别为时刻T′m前向陀螺、左向陀螺和上向陀螺输出的角速度测量值,φgx(T′m)和分别为时刻T′m的俯仰角和航向角变化率,m=0~n且T′0=T1-TINS;φgx(Tn′)为时刻T′n的俯仰角;和分别为时刻T′n的航向角变化率和俯仰角变化率;ωx(T′n)、ωy(T′n)和ωz(T′n)分别为时刻T′n前向陀螺、左向陀螺和上向陀螺输出的角速度测量值;
(5c)、将步骤(5b)迭代得到的时刻T=T1-Np′×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γ,即:
逆推算法就是运用对准时刻之前的数据来估算导航初始点的横滚角信息。可将横滚角γ写成角速率和初始横滚角的形式:
对式(5)两边积分得
故式(6)两边可写为
比较式(4)和式(7)可得到
式(7)可写为:
运用最小二乘法估计,即可求得初始横滚角γ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)、根据设定时刻T到时刻T1的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(T1-TINS)b(T1-TINS)],则Z0=z(T1-TINS);
如果初始化H0=[-b(T1-TINS)a(T1-TINS)],则Z0=z′(T1-TINS);
其中:
其中:ωx(T1-TINS)、ωy(T1-TINS)和ωz(T1-TINS)分别为时刻T1-TINS前向陀螺、左向陀螺和上向陀螺输出的角速度测量值;φgx(T1-TINS)为时刻T1-TINS的俯仰角;和分别为时刻T1-TINS的航向角变化率和俯仰角变化率;
(5b)、在时刻T′n=T1-(n+1)×TINS,n=1、2、…Np′-1,按照如下的迭代公式对观测矩阵H和测量矩阵Z进行迭代更新,得到时刻T′n的观测矩阵Hn和测量矩阵Zn;
如果Hn=[Hn-1;a(T′n),b(T′n)],则Zn=[Zn-1;z(T′n)];
如果Hn=[Hn-1;-b(T′n),a(T′n)],则Zn=[Zn-1;z′(T′n)];
其中:
其中:ωx(T′m)、ωy(T′m)和ωz(T′m)分别为时刻T′m前向陀螺、左向陀螺和上向陀螺输出的角速度测量值,φgx(T′m)和分别为时刻T′m的俯仰角和航向角变化率,m=0~n且T′0=T1-TINS;φgx(Tn′)为时刻T′n的俯仰角;和分别为时刻T′n的航向角变化率和俯仰角变化率;ωx(T′n)、ωy(T′n)和ωz(T′n)分别为时刻T′n前向陀螺、左向陀螺和上向陀螺输出的角速度测量值;
(5c)、将步骤(5b)迭代得到的时刻T=T1-Np′×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)计算得到的横滚角,以及时刻TGPS导航结果中的速度、位置和根据所述速度计算得到的航向角和俯仰角,作为空中对准结果,输出到制导炮弹的导航系统,用于对所述制导炮弹进行导航和控制。
实施例:
在本实施例中,在制导炮弹发射后,对GPS信号进行重新捕捉,在实现捕获跟踪并输出导航结果后,保存GPS导航结果和INS数据,并在到达设定对准时刻时,开始进行初始对准。
其中,图2为根据GPS输出的速率计算得到的俯仰角,以及拟合得到的俯仰角曲线。从图2可以看出,在该过程中俯仰角变化范围较大。图3为根据俯仰角拟合曲线求导计算得到的俯仰角变化率曲线,从中可以看出俯仰角变化率的噪声比较大,拟合后减少噪声。图4中表示了航向角计算曲线和拟合曲线,可以从图中看出,航向角几乎不变。图5为航向角变化率曲线,可以从中看出航向角变化率的噪声比较大,拟合后减少噪声。图6表示了制导炮弹上的陀螺仪安装方式和输出信息的关系,三个陀螺的坐标系遵循右手准则,如图6放置时,前向陀螺敏感弹体横滚角的变化,左向陀螺敏感俯仰角的变化,上向陀螺敏感航向角的变化,它们的输出分别为ωx、ωy和ωz。图7为计算得到的初始横滚角曲线,可以从图中看出,无论多少个点进行计算初始横滚角,曲线结果收敛,收敛速度较快达到真值。这样就可以得到精确的初始横滚角信息,初始俯仰角信息,初始航向角信息,初始位置信息和初始速度信息,为后续的导航和控制提供了完备的导航信息。
以上所述,仅为本发明一个具体实施方式,但本发明的保护范围并不局限于此,任何熟悉本技术领域的技术人员在本发明揭露的技术范围内,可轻易想到的变化或替换,都应涵盖在本发明的保护范围之内。
本发明说明书中未作详细描述的内容属于本领域专业技术人员的公知技术。
Claims (5)
1.基于后数据的旋转制导炮弹快速空中对准方法,其特征在于包括如下步骤:
(1)、发射后的制导炮弹接收卫星导航信号进行导航处理,其中实现卫星导航信号捕获跟踪并输出导航结果的时刻为T0;然后从所述时刻T0到设定的对准时刻T1保存卫星导航系统输出的M组卫星导航结果和INS输出的N组INS数据,其中:TGPS为卫星导航结果输出周期,TINS为INS数据输出周期,且TGPS=Q×TINS,Q为正整数;
所述卫星导航结果包括制导炮弹的速度和位置;所述INS数据包括前向陀螺、左向陀螺和上向陀螺输出的角速度,其中,前向陀螺敏感弹体横滚角角速度,左向陀螺敏感俯仰角角速度,上向陀螺敏感航向角角速度;
(2)、根据M组卫星导航结果中的制导炮弹的速度,计算得到相应的M组航向角和俯仰角;
(3)、对步骤(2)计算得到的M组航向角和俯仰角的计算结果进行拟合计算,得到航向角和俯仰角在时刻T0到时刻T1之间随时间变换的函数;然后对所述函数进行求导运算,得到航向角变化率和俯仰角变化率的时间函数;
(4)、将输出N组INS数据的时刻值代入到步骤(3)确定的4个时间函数中,计算得到输出N组INS数据时的制导炮弹的航向角、俯仰角、航向角变化率和俯仰角变化率;
(5)、根据设定时刻T到时刻T1的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)计算得到的横滚角,以及时刻T1的卫星导航系统输出的速度、位置,以及根据所述速度计算得到的航向角和俯仰角,作为空中对准结果,输出到制导炮弹的导航系统,用于对所述制导炮弹进行导航和控制;
在步骤(5)中,根据时刻T~T1的Np′组俯仰角、航向角变化率和俯仰角变化率,以及INS数据中的陀螺输出角速度,计算观测矩阵H和测量矩阵Z,具体计算过程如下:
(5a)、对观测矩阵H和测量矩阵Z进行初始化,得到初始观测矩阵H0和测量矩阵Z0:
如果初始化H0=[a(T1-TINS)b(T1-TINS)],则Z0=z(T1-TINS);
如果初始化H0=[-b(T1-TINS)a(T1-TINS)],则Z0=z′(T1-TINS);
其中:
<mrow>
<mi>a</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>T</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mo>&lsqb;</mo>
<mrow>
<mo>(</mo>
<msub>
<mi>&omega;</mi>
<mi>x</mi>
</msub>
<mo>(</mo>
<mrow>
<msub>
<mi>T</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
</mrow>
<mo>)</mo>
<mo>+</mo>
<mo>&dtri;</mo>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>z</mi>
</mrow>
</msub>
<mo>(</mo>
<mrow>
<msub>
<mi>T</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
</mrow>
<mo>)</mo>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mo>(</mo>
<mrow>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>x</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>T</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
<mo>)</mo>
<mo>)</mo>
</mrow>
<mo>&times;</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
<mo>&rsqb;</mo>
<mo>;</mo>
</mrow>
<mrow>
<mi>b</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>T</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mi>sin</mi>
<mo>&lsqb;</mo>
<mrow>
<mo>(</mo>
<msub>
<mi>&omega;</mi>
<mi>x</mi>
</msub>
<mo>(</mo>
<mrow>
<msub>
<mi>T</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
</mrow>
<mo>)</mo>
<mo>+</mo>
<mo>&dtri;</mo>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>z</mi>
</mrow>
</msub>
<mo>(</mo>
<mrow>
<msub>
<mi>T</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
</mrow>
<mo>)</mo>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mo>(</mo>
<mrow>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>x</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>T</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
<mo>)</mo>
<mo>)</mo>
</mrow>
<mo>&times;</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
<mo>&rsqb;</mo>
<mo>;</mo>
</mrow>
<mrow>
<mi>z</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>T</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfrac>
<mrow>
<msub>
<mi>&omega;</mi>
<mi>y</mi>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mi>T</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mo>&dtri;</mo>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>z</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mi>T</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>x</mi>
</mrow>
</msub>
<mo>(</mo>
<mrow>
<msub>
<mi>T</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
</mrow>
<mo>)</mo>
<mo>)</mo>
</mrow>
<mo>-</mo>
<msub>
<mi>&omega;</mi>
<mi>z</mi>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mi>T</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mo>&dtri;</mo>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>x</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mi>T</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mo>&dtri;</mo>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>x</mi>
</mrow>
</msub>
<msup>
<mrow>
<mo>(</mo>
<msub>
<mi>T</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>+</mo>
<msup>
<mrow>
<mo>(</mo>
<mo>&dtri;</mo>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>z</mi>
</mrow>
</msub>
<mo>(</mo>
<mrow>
<msub>
<mi>T</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
</mrow>
<mo>)</mo>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mo>(</mo>
<mrow>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>x</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>T</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
<mo>)</mo>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
</mrow>
</mfrac>
<mo>;</mo>
</mrow>
<mrow>
<msup>
<mi>z</mi>
<mo>&prime;</mo>
</msup>
<mrow>
<mo>(</mo>
<msub>
<mi>T</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfrac>
<mrow>
<msub>
<mi>&omega;</mi>
<mi>z</mi>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mi>T</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mo>&dtri;</mo>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>z</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mi>T</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>x</mi>
</mrow>
</msub>
<mo>(</mo>
<mrow>
<msub>
<mi>T</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
</mrow>
<mo>)</mo>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>&omega;</mi>
<mi>y</mi>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mi>T</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mo>&dtri;</mo>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>x</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mi>T</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mo>&dtri;</mo>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>x</mi>
</mrow>
</msub>
<msup>
<mrow>
<mo>(</mo>
<msub>
<mi>T</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>+</mo>
<msup>
<mrow>
<mo>(</mo>
<mo>&dtri;</mo>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>z</mi>
</mrow>
</msub>
<mo>(</mo>
<mrow>
<msub>
<mi>T</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
</mrow>
<mo>)</mo>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mo>(</mo>
<mrow>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>x</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>T</mi>
<mn>1</mn>
</msub>
<mo>-</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
<mo>)</mo>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
</mrow>
</mfrac>
<mo>;</mo>
</mrow>
其中:ωx(T1-TINS)、ωy(T1-TINS)和ωz(T1-TINS)分别为时刻T1-TINS前向陀螺、左向陀螺和上向陀螺输出的角速度测量值;φgx(T1-TINS)为时刻T1-TINS的俯仰角;和分别为时刻T1-TINS的航向角变化率和俯仰角变化率;
(5b)、在时刻Tn′=T1-(n+1)×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′)];
其中:
<mrow>
<mi>a</mi>
<mrow>
<mo>(</mo>
<msubsup>
<mi>T</mi>
<mi>n</mi>
<mo>&prime;</mo>
</msubsup>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mi>cos</mi>
<mo>&lsqb;</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>m</mi>
<mo>=</mo>
<mn>0</mn>
</mrow>
<mi>n</mi>
</munderover>
<mrow>
<mo>(</mo>
<msub>
<mi>&omega;</mi>
<mi>x</mi>
</msub>
<mo>(</mo>
<msubsup>
<mi>T</mi>
<mi>m</mi>
<mo>&prime;</mo>
</msubsup>
<mo>)</mo>
<mo>+</mo>
<mo>&dtri;</mo>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>z</mi>
</mrow>
</msub>
<mo>(</mo>
<msubsup>
<mi>T</mi>
<mi>m</mi>
<mo>&prime;</mo>
</msubsup>
<mo>)</mo>
<mi>sin</mi>
<mo>(</mo>
<mrow>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>x</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<msubsup>
<mi>T</mi>
<mi>m</mi>
<mo>&prime;</mo>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
<mo>)</mo>
<mo>)</mo>
</mrow>
<mo>&times;</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
<mo>&rsqb;</mo>
<mo>;</mo>
</mrow>
<mrow>
<mi>b</mi>
<mrow>
<mo>(</mo>
<msubsup>
<mi>T</mi>
<mi>n</mi>
<mo>&prime;</mo>
</msubsup>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mi>sin</mi>
<mo>&lsqb;</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>m</mi>
<mo>=</mo>
<mn>0</mn>
</mrow>
<mi>n</mi>
</munderover>
<mrow>
<mo>(</mo>
<msub>
<mi>&omega;</mi>
<mi>x</mi>
</msub>
<mo>(</mo>
<msubsup>
<mi>T</mi>
<mi>m</mi>
<mo>&prime;</mo>
</msubsup>
<mo>)</mo>
<mo>+</mo>
<mo>&dtri;</mo>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>z</mi>
</mrow>
</msub>
<mo>(</mo>
<msubsup>
<mi>T</mi>
<mi>m</mi>
<mo>&prime;</mo>
</msubsup>
<mo>)</mo>
<mi>sin</mi>
<mo>(</mo>
<mrow>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>x</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<msubsup>
<mi>T</mi>
<mi>m</mi>
<mo>&prime;</mo>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
<mo>)</mo>
<mo>)</mo>
</mrow>
<mo>&times;</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>I</mi>
<mi>N</mi>
<mi>S</mi>
</mrow>
</msub>
<mo>&rsqb;</mo>
<mo>;</mo>
</mrow>
<mrow>
<mi>z</mi>
<mrow>
<mo>(</mo>
<msubsup>
<mi>T</mi>
<mi>n</mi>
<mo>&prime;</mo>
</msubsup>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfrac>
<mrow>
<msub>
<mi>&omega;</mi>
<mi>y</mi>
</msub>
<mrow>
<mo>(</mo>
<msubsup>
<mi>T</mi>
<mi>n</mi>
<mo>&prime;</mo>
</msubsup>
<mo>)</mo>
</mrow>
<mo>&dtri;</mo>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>z</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<msubsup>
<mi>T</mi>
<mi>n</mi>
<mo>&prime;</mo>
</msubsup>
<mo>)</mo>
</mrow>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>x</mi>
</mrow>
</msub>
<mo>(</mo>
<msubsup>
<mi>T</mi>
<mi>n</mi>
<mo>&prime;</mo>
</msubsup>
<mo>)</mo>
<mo>)</mo>
</mrow>
<mo>-</mo>
<msub>
<mi>&omega;</mi>
<mi>z</mi>
</msub>
<mrow>
<mo>(</mo>
<msubsup>
<mi>T</mi>
<mi>n</mi>
<mo>&prime;</mo>
</msubsup>
<mo>)</mo>
</mrow>
<mo>&dtri;</mo>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>x</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<msubsup>
<mi>T</mi>
<mi>n</mi>
<mo>&prime;</mo>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mo>&dtri;</mo>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>x</mi>
</mrow>
</msub>
<msup>
<mrow>
<mo>(</mo>
<msubsup>
<mi>T</mi>
<mi>n</mi>
<mo>&prime;</mo>
</msubsup>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>+</mo>
<msup>
<mrow>
<mo>(</mo>
<mo>&dtri;</mo>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>z</mi>
</mrow>
</msub>
<mo>(</mo>
<msubsup>
<mi>T</mi>
<mi>n</mi>
<mo>&prime;</mo>
</msubsup>
<mo>)</mo>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mo>(</mo>
<mrow>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>x</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<msubsup>
<mi>T</mi>
<mi>n</mi>
<mo>&prime;</mo>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
<mo>)</mo>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
</mrow>
</mfrac>
<mo>;</mo>
</mrow>
<mrow>
<msup>
<mi>z</mi>
<mo>&prime;</mo>
</msup>
<mrow>
<mo>(</mo>
<msubsup>
<mi>T</mi>
<mi>n</mi>
<mo>&prime;</mo>
</msubsup>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfrac>
<mrow>
<msub>
<mi>&omega;</mi>
<mi>z</mi>
</msub>
<mrow>
<mo>(</mo>
<msubsup>
<mi>T</mi>
<mi>n</mi>
<mo>&prime;</mo>
</msubsup>
<mo>)</mo>
</mrow>
<mo>&dtri;</mo>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>z</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<msubsup>
<mi>T</mi>
<mi>n</mi>
<mo>&prime;</mo>
</msubsup>
<mo>)</mo>
</mrow>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>x</mi>
</mrow>
</msub>
<mo>(</mo>
<msubsup>
<mi>T</mi>
<mi>n</mi>
<mo>&prime;</mo>
</msubsup>
<mo>)</mo>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>&omega;</mi>
<mi>y</mi>
</msub>
<mrow>
<mo>(</mo>
<msubsup>
<mi>T</mi>
<mi>n</mi>
<mo>&prime;</mo>
</msubsup>
<mo>)</mo>
</mrow>
<mo>&dtri;</mo>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>x</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<msubsup>
<mi>T</mi>
<mi>n</mi>
<mo>&prime;</mo>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mo>&dtri;</mo>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>x</mi>
</mrow>
</msub>
<msup>
<mrow>
<mo>(</mo>
<msubsup>
<mi>T</mi>
<mi>n</mi>
<mo>&prime;</mo>
</msubsup>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>+</mo>
<msup>
<mrow>
<mo>(</mo>
<mo>&dtri;</mo>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>z</mi>
</mrow>
</msub>
<mo>(</mo>
<msubsup>
<mi>T</mi>
<mi>n</mi>
<mo>&prime;</mo>
</msubsup>
<mo>)</mo>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mo>(</mo>
<mrow>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>x</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<msubsup>
<mi>T</mi>
<mi>n</mi>
<mo>&prime;</mo>
</msubsup>
<mo>)</mo>
</mrow>
</mrow>
<mo>)</mo>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
</mrow>
</mfrac>
<mo>;</mo>
</mrow>
其中:ωx(Tm′)、ωy(Tm′)和ωz(Tm′)分别为时刻Tm′前向陀螺、左向陀螺和上向陀螺输出的角速度测量值,φgx(Tm′)和分别为时刻Tm′的俯仰角和航向角变化率,m=0~n且T0′=T1-TINS;φgx(Tn′)为时刻Tn′的俯仰角;和分别为时刻Tn′的航向角变化率和俯仰角变化率;ωx(Tn′)、ωy(Tn′)和ωz(Tn′)分别为时刻Tn′前向陀螺、左向陀螺和上向陀螺输出的角速度测量值;
(5c)、将步骤(5b)迭代得到的时刻T=T1-Np′×TINS的观测矩阵和测量矩阵作为观测矩阵H和测量矩阵Z的最终计算结果,用于步骤(6)的最小二乘计算。
2.根据权利要求1所述的基于后数据的旋转制导炮弹快速空中对准方法,其特征在于:在步骤(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))。
3.根据权利要求1所述的基于后数据的旋转制导炮弹快速空中对准方法,其特征在于:在步骤(2)中,根据制导炮弹速度计算航向角和俯仰角的公式如下:
<mrow>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>z</mi>
</mrow>
</msub>
<mo>=</mo>
<mi>arctan</mi>
<mrow>
<mo>(</mo>
<mfrac>
<msub>
<mi>V</mi>
<mrow>
<mi>g</mi>
<mi>n</mi>
</mrow>
</msub>
<msub>
<mi>V</mi>
<mrow>
<mi>g</mi>
<mi>e</mi>
</mrow>
</msub>
</mfrac>
<mo>)</mo>
</mrow>
<mo>;</mo>
<msub>
<mi>&phi;</mi>
<mrow>
<mi>g</mi>
<mi>x</mi>
</mrow>
</msub>
<mo>=</mo>
<mo>-</mo>
<mi>arctan</mi>
<mrow>
<mo>(</mo>
<mfrac>
<msub>
<mi>V</mi>
<mrow>
<mi>g</mi>
<mi>u</mi>
</mrow>
</msub>
<msqrt>
<mrow>
<msubsup>
<mi>V</mi>
<mrow>
<mi>g</mi>
<mi>e</mi>
</mrow>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>V</mi>
<mrow>
<mi>g</mi>
<mi>n</mi>
</mrow>
<mn>2</mn>
</msubsup>
</mrow>
</msqrt>
</mfrac>
<mo>)</mo>
</mrow>
<mo>;</mo>
</mrow>
其中:φgz和φgx分别为制导炮弹的航向角和俯仰角;Vgn、Vge和Vgu分别为制导炮弹的北速、东速和天速。
4.根据权利要求1所述的基于后数据的旋转制导炮弹快速空中对准方法,其特征在于:在步骤(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。
5.根据权利要求4所述的基于后数据的旋转制导炮弹快速空中对准方法,其特征在于:对航向角时间函数φgz(t)和俯仰角时间函数φgx(t)进行求导运算,得到航向角变化率时间函数和俯仰角变化率时间函数其中:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510537052.7A CN105115508B (zh) | 2015-08-27 | 2015-08-27 | 基于后数据的旋转制导炮弹快速空中对准方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510537052.7A CN105115508B (zh) | 2015-08-27 | 2015-08-27 | 基于后数据的旋转制导炮弹快速空中对准方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN105115508A CN105115508A (zh) | 2015-12-02 |
CN105115508B true CN105115508B (zh) | 2017-12-22 |
Family
ID=54663553
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201510537052.7A Active CN105115508B (zh) | 2015-08-27 | 2015-08-27 | 基于后数据的旋转制导炮弹快速空中对准方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN105115508B (zh) |
Families Citing this family (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105785415B (zh) * | 2016-03-03 | 2018-01-05 | 北京航天控制仪器研究所 | 一种制导炮弹的空中轨迹预测方法 |
CN107870563A (zh) * | 2017-08-17 | 2018-04-03 | 北京理工大学 | 一种旋转弹全阶反馈控制器的插值增益调度方法 |
CN109211232B (zh) * | 2018-09-07 | 2021-07-27 | 东南大学 | 一种基于最小二乘滤波的炮弹姿态估计方法 |
CN109059914B (zh) * | 2018-09-07 | 2021-11-02 | 东南大学 | 一种基于gps和最小二乘滤波的炮弹滚转角估计方法 |
CN111504256A (zh) * | 2020-04-29 | 2020-08-07 | 中国北方工业有限公司 | 一种基于最小二乘法的滚转角实时估计方法 |
CN111337056B (zh) * | 2020-05-19 | 2020-08-25 | 北京数字绿土科技有限公司 | 基于优化的LiDAR运动补偿位置姿态系统对准方法 |
CN112363195B (zh) * | 2020-09-30 | 2022-08-09 | 东南大学 | 基于运动学方程的旋转弹空中快速粗对准方法 |
CN114383603B (zh) * | 2022-03-23 | 2022-06-28 | 西北工业大学 | 一种基于旋转调制法的制导炮弹空中姿态辨识方法 |
CN115031763B (zh) * | 2022-04-25 | 2023-06-13 | 北京自动化控制设备研究所 | 一种基于角速率信息的旋转弹快速对准方法 |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101256080A (zh) * | 2008-04-09 | 2008-09-03 | 南京航空航天大学 | 卫星/惯性组合导航系统的空中对准方法 |
CN104457446A (zh) * | 2014-11-28 | 2015-03-25 | 北京航天控制仪器研究所 | 一种自旋制导炮弹的空中自对准方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8319164B2 (en) * | 2009-10-26 | 2012-11-27 | Nostromo, Llc | Rolling projectile with extending and retracting canards |
-
2015
- 2015-08-27 CN CN201510537052.7A patent/CN105115508B/zh active Active
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101256080A (zh) * | 2008-04-09 | 2008-09-03 | 南京航空航天大学 | 卫星/惯性组合导航系统的空中对准方法 |
CN104457446A (zh) * | 2014-11-28 | 2015-03-25 | 北京航天控制仪器研究所 | 一种自旋制导炮弹的空中自对准方法 |
Non-Patent Citations (2)
Title |
---|
"GPS/INS组合制导弹药空中对准的初始滚转角估计新算法";佘浩平 等,;《兵工学报》;20111031;第32卷(第10期);1265-1270页 * |
"一种灵巧弹药的滚转估计方法";文云 等,;《兵工自动化》;20130731;第32卷(第7期);31-33页 * |
Also Published As
Publication number | Publication date |
---|---|
CN105115508A (zh) | 2015-12-02 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN105115508B (zh) | 基于后数据的旋转制导炮弹快速空中对准方法 | |
CN107314718B (zh) | 基于磁测滚转角速率信息的高速旋转弹姿态估计方法 | |
CN105180728B (zh) | 基于前数据的旋转制导炮弹快速空中对准方法 | |
US8519313B2 (en) | Projectile navigation enhancement method | |
CN109373833B (zh) | 适用于旋转弹初始姿态和速度联合测量方法 | |
CN109373832B (zh) | 基于磁测滚转的旋转弹炮口初始参数测量方法 | |
CN107478110B (zh) | 一种基于状态观测器的旋转弹姿态角计算方法 | |
CN109724624B (zh) | 一种适用于机翼挠曲变形的机载自适应传递对准方法 | |
CN110398242B (zh) | 一种高旋高过载条件飞行器的姿态角确定方法 | |
CN105258698A (zh) | 一种高动态自旋制导炮弹空中组合导航方法 | |
CN110672128B (zh) | 一种星光/惯性组合导航及误差在线标定方法 | |
CN109059914A (zh) | 一种基于gps和最小二乘滤波的炮弹滚转角估计方法 | |
CN111189442A (zh) | 基于cepf的无人机多源导航信息状态预测方法 | |
CN108225323B (zh) | 基于偏差影响方向组合确定落区边界的方法、介质和设备 | |
CN109211232A (zh) | 一种基于最小二乘滤波的炮弹姿态估计方法 | |
CN110986926B (zh) | 一种基于地磁要素的飞行弹体旋转姿态测量方法 | |
CN114578857A (zh) | 基于全弹道信息的制导飞行器自主控制方法、装置及系统 | |
CN111089588B (zh) | 一种适用于筒装斜发射导弹的动基座传递对准方法 | |
CN109813302A (zh) | 最佳可用导航星快速确定方法 | |
SANTOSO | Modification of strapdown inertial navigation system algorithm for rocket flight test | |
Zhang et al. | Deep data fusion method for missile-borne inertial/celestial system | |
Kim et al. | Stability analysis of strapdown seeker scale factor error and los rate | |
Wang et al. | Roll estimation for smart munitions using a 3D magnetometer only | |
Fairfax et al. | Cost-efficient state estimation for precision projectiles | |
CN114383614B (zh) | 一种弹道环境下的多矢量空中对准方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |