一种高动态自旋制导炮弹空中组合导航方法

申请号 CN201510657504.5 申请日 2015-10-13 公开(公告)号 CN105258698A 公开(公告)日 2016-01-20
申请人 北京航天控制仪器研究所; 发明人 赵龙; 范玉宝; 王盛; 郭涛; 郭琳; 段宇鹏;
摘要 一种高动态自旋制导炮弹空中组合导航方法,通过自旋制导炮弹空中粗对准方法得到初始时刻的 位置 、速度和 姿态 ,利用惯性 导航系统 进行导航解算得到每一时刻的导航结果;根据对应时间下GPS输出的导航信息得到对应时刻的航向 角 、 俯仰 角、三个速度以及三个速度误差值、俯仰角误差值和航向角误差值,并作为9维卡尔曼滤波估计的观测阵C,估算出对应时刻的三个姿态角修正值、三个速度修正值和三个 陀螺仪 零偏值,进而得到自旋制导炮弹空中对应时刻的姿态、速度和位置信息。本 发明 实现了高动态自旋制导炮弹空中失重情况下的组合导航,同时通过卡尔曼 滤波器 实现了对导航噪声和陀螺仪零偏的滤除,提高了自旋制导炮弹的落点 精度 ,增加了制导炮弹的可控性。
权利要求

1.一种高动态自旋制导炮弹空中组合导航方法,其特征在于步骤如下:
(1)根据惯性导航系统测得的加速度和速度信息和GPS测得的速度和位置信息,利用自旋制导炮弹空中粗对准方法计算得到自旋制导炮弹在空中飞行初始时刻实际的位置信息[la0 phia0 h0]、速度信息[Ve0 Vn0 Vu0]和姿态信息[θ0 γ0 ψ0],其中la0表示粗对准得到的初始经度,phia0表示粗对准得到的初始纬度,h0表示粗对准得到的初始高度,Ve0表示粗对准得到的初始东向速度,Vn0表示粗对准得到的初始北向速度,Vu0表示粗对准得到的初始天向速度,θ0表示粗对准得到的初始俯仰角,γ0表示粗对准得到的初始横滚角,ψ0表示粗对准得到的初始航向角;
(2)自旋制导炮弹在tn时刻采集惯性导航系统测得的加速度和角速度信息,并根据测得的加速度和角速度信息以及tn-1时刻自旋制导炮弹实际的姿态、位置和速度信息进行导航解算,获得tn时刻通过惯性导航系统解算出的自旋制导炮弹的姿态信息[θn γn ψn]、位置信息[lan phian hn]以及速度信息[Ven Vnn Vun],其中ψn为tn时刻自旋制导炮弹的航向角,θn为tn时刻自旋制导炮弹的俯仰角,γn为tn时刻自旋制导炮弹的横滚角,lan为tn时刻自旋制导炮弹的经度,phian为tn时刻自旋制导炮弹的纬度,hn为tn时刻自旋制导炮弹的高度,Ven为tn时刻自旋制导炮弹的东向速度,Vnn为tn时刻自旋制导炮弹的北向速度,Vun为tn时刻自旋制导炮弹的天向速度,进入步骤(3),其中n的初始值为1;
(3)在tn时刻,自旋制导炮弹判断是否接收到GPS输出的速度和位置信息,如果没有接收到,则将步骤(2)的导航解算结果作为tn时刻自旋制导炮弹实际的速度、位置和姿态信息,进入步骤(5);否则,进入步骤(4);
(4)自旋制导炮弹根据tn时刻接收到的GPS的速度信息[Vgen Vgnn Vgun]计算tn时刻自旋制导炮弹的俯仰角θgn和航向角ψgn,并依据tn时刻接收到的GPS的速度信息[Vgen Vgnn Vgun]和tn时刻惯性导航系统解算出的姿态信息[θn γn ψn]、位置信息[lan phian hn]以及速度信息[Ven Vnn Vun]进行组合导航计算,得到tn时刻自旋制导炮弹实际的速度、位置和姿态信息;其中,Vgen为tn时刻GPS输出的东向速度,Vgnn为tn时刻GPS输出的北向速度,Vgun为tn时刻GPS输出的天向速度;进入步骤(5);
(5)n的值加1后返回步骤(2),计算出自旋制导炮弹在空中飞行每一时刻的实际速度、位置和姿态信息,用于作为自旋制导炮弹导航和控制的测量信息,直到自旋制导炮弹命中目标为止;
上述各步骤中,tn与tn-1的时间间隔为惯性导航系统的测量周期。
2.根据权利要求1所述的高动态自旋制导炮弹空中组合导航方法,其特征在于:所述步骤(4)自旋制导炮弹根据tn时刻接收到的GPS的速度信息[Vgen Vgnn Vgun]计算tn时刻自旋制导炮弹的俯仰角θgn和航向角ψgn的方法为:


3.根据权利要求1所述的高动态自旋制导炮弹空中组合导航方法,其特征在于:所述步骤(4)中依据tn时刻接收到的GPS的速度信息[Vgen Vgnn Vgun]和tn时刻惯性导航系统解算出的姿态信息[θn γn ψn]、位置信息[lan phian hn]以及速度信息[Ven Vnn Vun]进行组合导航计算,得到tn时刻自旋制导炮弹实际的速度、位置和姿态信息的实现方式为:
(3.1)利用量测阵H和观测阵C,根据卡尔曼滤波算法解算出tn时刻的状态估计量其中
δVcn=Vnn-Vgnn,δVcu=Vun-Vgun,δθc=θn-θgn,δψc=ψn-ψgn其中δVen为tn时刻自旋制导炮弹的东向速度修正值,δVnn为tn时刻自旋制导炮弹的北向速度修正值,δVun为tn时刻自旋制导炮弹的天向速度修正值,δθn为tn时刻自旋制导炮弹的俯仰角修正值,δγn为tn时刻自旋制导炮弹的横滚角修正值,δψn为tn时刻自旋制导炮弹的航向角修正值, 为tn时刻自旋制导炮弹的三个陀螺仪零偏修正值;
(3.2)按照如下公式,利用tn时刻惯性导航系统解算出的速度信息[Ven Vnn Vun]计算tn时刻自旋制导炮弹实际的速度信息:
VKen=Ven-Xn (1)
VKnn=Vnn-Xn (2)
VKun=Vun-Xn (3)
其中VKen表示组合导航后tn时刻自旋制导炮弹实际的东向速度,VKnn表示组合导航后tn时刻自旋制导炮弹实际的北向速度,VKun表示组合导航后tn时刻自旋制导炮弹实际的天向速度,Xn(1)表示tn时刻自旋制导炮弹的东向速度修正值,Xn(2)表示tn时刻自旋制导炮弹的北向速度修正值,Xn(3)表示tn时刻自旋制导炮弹的天向速度修正值;
(3.3)按照如下公式,利用tn时刻惯性导航系统解算出的姿态信息[θn γn ψn]计算tn时刻自旋制导炮弹实际的姿态信息:
γKn=arcsin(Cbnn(3,2))
θKn=-arctan(Cbnn(3,1)/Cbnn(3,3))
ψKn=ψgn
其中,Cbnn=Cnn*Cbn,
Cbn=CnbT
其中Cnb表示tn时刻组合导航前从导航坐标系到载体坐标系的转换矩阵,Cbn表示tn时刻组合导航前从载体坐标系到导航坐标系的转换矩阵,Cnn表示tn时刻姿态修正矩阵,Cbnn表示tn时刻组合导航后从载体坐标系到导航坐标系的转换矩阵,γKn表示组合导航后tn时刻自旋制导炮弹实际的横滚角,θKn表示组合导航后tn时刻自旋制导炮弹实际的俯仰角,ψKn表示组合导航后tn时刻自旋制导炮弹实际的航向角;Xn(4)表示tn时刻自旋制导炮弹的俯仰角修正值,Xn(5)表示tn时刻自旋制导炮弹的横滚角修正值;
(3.4)tn时刻惯性导航系统解算出的位置信息[lan phian hn]即为tn时刻自旋制导炮弹实际的位置信息。

说明书全文

一种高动态自旋制导炮弹空中组合导航方法

技术领域

[0001] 本发明涉及一种空中组合导航方法,尤其涉及一种高动态自旋制导炮弹空中组合导航方法,属于制导控制技术和组合导航系统设计技术,可用于无人机、自旋制导炮弹等需要在空中组合导航的场合。

背景技术

[0002] 自旋制导炮弹是一种在空中发射,需要进行导航和控制的一种高精度武器,它包含了惯性导航和GPS等系统,通过GPS来修正惯性导航系统的误差,达到精确打击目标的能。组合导航即从惯导相对其他导航系统提供的导航参数(如速度等)的偏差中估计出惯导系统的失准并校正之。
[0003] 惯性导航系统是一个基于加速度二次积分的航程推算系统,它完全依靠机械设备和相应的算法自动、独立完成导航任务,和外界不发生任何光、电联系。由于其具有隐蔽性好、工作环境不受气象条件限制等优点,成为航天、航空、航海领域中一种广泛使用的主要导航系统。惯性导航系统的优点是,不需要任何外来信息也不向外辐射任何信息,可在任何介质和任何环境条件下实现导航,且能输出炮弹的位置、速度、方位和姿态等多种导航参数,系统地频带宽,能跟踪运载体的任何机动运动,导航输出数据平稳,短期稳定性好。但惯性系统具有固有的缺点:导航精度随时间而发散,且长期稳定性差。为此,需要引入GPS输出的导航信息进行制导炮弹空中对准,提高制导炮弹的打击精度。GPS导航系统导航精度高,且不随时间发散,但其频带窄,当运载体做较高机动运动时,接收机的码环和载波环极易失而丢失信号,从而完全丧失导航能力。因此,需要将惯性导航系统和GPS结合起来进行组合导航。传统组合导航中地面常用的Kalman滤波算法一般采用经纬度、平速度中的一个或者几个作为观测量,其他参数作为被观测量,进行组合。在空中的自旋制导炮弹与地面情况不同,因自旋制导炮弹处于高动态自旋转状态,且处于失重情况下,加速度计输出几乎为零。三个加速度计不仅包含了自身的误差,因为使用的是MEMS加速度计,故自身误差较大,而且包含了杆臂效应,导致测量得到的加速度计输出结果不准,从而导致真实的加速度信息被隐没在噪声中,不能作为真实结果应用。速度变化与姿态变化之间的耦合关系弱,不能有效的通过速度和位置的偏差观测出姿态角信息。常用的组合导航方法是采用速度和位置作为观测量的Kalman滤波算法实现的经典对准方案,由于不考虑惯导工作环境中的随机干扰因素和失重状态,如阵等引起的自旋制导炮弹的随机晃动、飞行过程中空气动力的随机改变引起的制导炸弹结构的饶曲变形等,导致导航精度不高且需要时间长。

发明内容

[0004] 本发明解决的技术问题是:克服现有技术的不足,提供一种高动态自旋制导炮弹空中组合导航方法,该方法能够快速、精确地得到制导炮弹在空中的位置、速度和姿态,有效的提高了自旋制导炮弹空中导航精度。
[0005] 本发明的技术解决方案是:一种高动态自旋制导炮弹空中组合导航方法,步骤如下:
[0006] (1)根据惯性导航系统测得的加速度和角速度信息和GPS测得的速度和位置信息,利用自旋制导炮弹空中粗对准方法计算得到自旋制导炮弹在空中飞行初始时刻实际的位置信息[la0 phia0 h0]、速度信息[Ve0 Vn0 Vu0]和姿态信息[θ0 γ0 ψ0],其中la0表示粗对准得到的初始经度,phia0表示粗对准得到的初始纬度,h0表示粗对准得到的初始高度,Ve0表示粗对准得到的初始东向速度,Vn0表示粗对准得到的初始北向速度,Vu0表示粗对准得到的初始天向速度,θ0表示粗对准得到的初始俯仰角,γ0表示粗对准得到的初始横滚角,ψ0表示粗对准得到的初始航向角;
[0007] (2)自旋制导炮弹在tn时刻采集惯性导航系统测得的加速度和角速度信息,并根据测得的加速度和角速度信息以及tn-1时刻自旋制导炮弹实际的姿态、位置和速度信息进行导航解算,获得tn时刻通过惯性导航系统解算出的自旋制导炮弹的姿态信息[θn γn ψn]、位置信息[lan phian hn]以及速度信息[Ven Vnn Vun],其中ψn为tn时刻自旋制导炮弹的航向角,θn为tn时刻自旋制导炮弹的俯仰角,γn为tn时刻自旋制导炮弹的横滚角,lan为tn时刻自旋制导炮弹的经度,phian为tn时刻自旋制导炮弹的纬度,hn为tn时刻自旋制导炮弹的高度,Ven为tn时刻自旋制导炮弹的东向速度,Vnn为tn时刻自旋制导炮弹的北向速度,Vun为tn时刻自旋制导炮弹的天向速度,进入步骤(3),其中n的初始值为1;
[0008] (3)在tn时刻,自旋制导炮弹判断是否接收到GPS输出的速度和位置信息,如果没有接收到,则将步骤(2)的导航解算结果作为tn时刻自旋制导炮弹实际的速度、位置和姿态信息,进入步骤(5);否则,进入步骤(4);
[0009] (4)自旋制导炮弹根据tn时刻接收到的GPS的速度信息[Vgen Vgnn Vgun]计算tn时刻自旋制导炮弹的俯仰角θgn和航向角ψgn,并依据tn时刻接收到的GPS的速度信息[Vgen Vgnn Vgun]和tn时刻惯性导航系统解算出的姿态信息[θnγnψn]、位置信息[lan phian hn]以及速度信息[Ven Vnn Vun]进行组合导航计算,得到tn时刻自旋制导炮弹实际的速度、位置和姿态信息;其中,Vgen为tn时刻GPS输出的东向速度,Vgnn为tn时刻GPS输出的北向速度,Vgun为tn时刻GPS输出的天向速度;进入步骤(5);
[0010] (5)n的值加1后返回步骤(2),计算出自旋制导炮弹在空中飞行每一时刻的实际速度、位置和姿态信息,用于作为自旋制导炮弹导航和控制的测量信息,直到自旋制导炮弹命中目标为止;
[0011] 上述各步骤中,tn与tn-1的时间间隔为惯性导航系统的测量周期。
[0012] 所述步骤(4)自旋制导炮弹根据tn时刻接收到的GPS的速度信息[Vgen Vgnn Vgun]计算tn时刻自旋制导炮弹的俯仰角θgn和航向角ψgn的方法为:
[0013]
[0014]
[0015] 所述步骤(4)中依据tn时刻接收到的GPS的速度信息[Vgen Vgnn Vgun]和tn时刻惯性导航系统解算出的姿态信息[θn γn ψn]、位置信息[lan phian hn]以及速度信息[Ven Vnn Vun]进行组合导航计算,得到tn时刻自旋制导炮弹实际的速度、位置和姿态信息的实现方式为:
[0016] (3.1)利用量测阵H和观测阵C,根据卡尔曼滤波算法解算出tn时刻的状态估计量Xn,
[0017] 其中 δVce=Ven-Vgen,
[0018] δVcn=Vnn-Vgnn,δVcu=Vun-Vgun,δθc=θn-θgn,δψc=ψn-ψgn[0019] 其中δVen为tn时刻自旋制导炮弹的东向速度修正值,δVnn为tn时刻自旋制导炮弹的北向速度修正值,δVun为tn时刻自旋制导炮弹的天向速度修正值,δθn为tn时刻自旋制导炮弹的俯仰角修正值,δγn为tn时刻自旋制导炮弹的横滚角修正值,δψn为tn时刻自旋制导炮弹的航向角修正值, 为tn时刻自旋制导炮弹的三个陀螺仪零偏修正值;
[0020] (3.2)按照如下公式,利用tn时刻惯性导航系统解算出的速度信息[Ven Vnn Vun]计算tn时刻自旋制导炮弹实际的速度信息:
[0021] VKen=Ven-Xn (1)
[0022] VKnn=Vnn-Xn (2)
[0023] VKun=Vun-Xn (3)
[0024] 其中VKen表示组合导航后tn时刻自旋制导炮弹实际的东向速度,VKnn表示组合导航后tn时刻自旋制导炮弹实际的北向速度,VKun表示组合导航后tn时刻自旋制导炮弹实际的天向速度,Xn(1)表示tn时刻自旋制导炮弹的东向速度修正值,Xn(2)表示tn时刻自旋制导炮弹的北向速度修正值,Xn(3)表示tn时刻自旋制导炮弹的天向速度修正值;
[0025] (3.3)按照如下公式,利用tn时刻惯性导航系统解算出的姿态信息[θn γn ψn]计算tn时刻自旋制导炮弹实际的姿态信息:
[0026] γKn=arcsin(Cbnn(3,2))
[0027] θKn=-arctan(Cbnn(3,1)/Cbnn(3,3))
[0028] ψKn=ψgn
[0029] 其中,Cbnn=Cnn*Cbn,
[0030]T
[0031] Cbn=Cnb
[0032]
[0033] 其中Cnb表示tn时刻组合导航前从导航坐标系到载体坐标系的转换矩阵,Cbn表示tn时刻组合导航前从载体坐标系到导航坐标系的转换矩阵,Cnn表示tn时刻姿态修正矩阵,Cbnn表示tn时刻组合导航后从载体坐标系到导航坐标系的转换矩阵,γKn表示组合导航后tn时刻自旋制导炮弹实际的横滚角,θKn表示组合导航后tn时刻自旋制导炮弹实际的俯仰角,ψKn表示组合导航后tn时刻自旋制导炮弹实际的航向角;Xn(4)表示tn时刻自旋制导炮弹的俯仰角修正值,Xn(5)表示tn时刻自旋制导炮弹的横滚角修正值;
[0034] (3.4)tn时刻惯性导航系统解算出的位置信息[lan phian hn]即为tn时刻自旋制导炮弹实际的位置信息。
[0035] 本发明与现有技术相比的优点如下:
[0036] (1)现有的卡尔曼滤波算法采用位置和速度作为观测量,但在空中炮弹处于失重情况下,位置和速度与炮弹姿态角之间的耦合关系弱,不能良好的估算出真实的姿态信息。本发明第一次将俯仰角和横滚角作为观测量,通过三个角度之间的耦合关系来估算真实的横滚角信息,收敛速度快,且精度高。
[0037] (2)现有的Kalman滤波采用姿态误差引起加速度计输出在导航坐标系下的误差,再通过速度作为观测量,观测出姿态误差,但制导炮弹在空中飞行过程中处于失重和高动态自旋状态下,故加速度计输出几乎为零,且因加速度计自身误差和杆臂效应引起的误差导致加表输出结果不可信,采用现有的Kalman滤波算法对加速度计性能和杆臂效应补偿要求高。本方法直接把航向角和俯仰角作为观测量,不再使用加速度计的信息去估计姿态信息,这样可以不用考虑加速度计性能和杆臂效应,降低了对惯性导航系统中加速度计的要求,从而降低了系统设计的难度。
[0038] (3)本发明通过采用GPS信息和惯性导航系统输出的陀螺加速度计信息进行组合导航,通过GPS速度信息计算航向角和俯仰角,并合理利用了两者信息,不仅修正了位置、速度和姿态等,而且减小了位置速度的噪声,同时能够估算出陀螺仪的零偏,从而进一步提高了自旋制导炮弹的导航精度。附图说明
[0039] 图1为本发明的自旋制导炮弹空中组合导航流程图
[0040] 图2为使用本发明方法设计的卡尔曼滤波速度误差估计效果图;
[0041] 图3为使用本发明方法设计的卡尔曼滤波姿态误差估计效果图;
[0042] 图4为使用本发明方法设计的卡尔曼滤波陀螺零偏估计效果图;
[0043] 图5为使用本发明方法得到的组合导航曲线和GPS的输出曲线示意图。

具体实施方式

[0044] 由于制导炮弹在空中时处于一种机动状态,不能通过惯性导航系统本身进行自对准,需要引入GPS辅助信息进行惯性导航系统初始状态的测量和解算,惯性导航系统本身存在仪表误差、初始对准误差、重力异常等,惯性导航系统长时间工作时导航误差随时间发散。为此,需要引入GPS输出的导航信息进行制导炮弹空中对准,提高制导炮弹的打击精度。空中因处于高机动状态,且不被遮挡,GPS信号好,三维速度可信度高,可通过三维速度解算航向角和俯仰角。
[0045] 如图1所示,本发明提出一种高动态自旋制导炮弹空中组合导航方法,步骤如下:
[0046] (1)根据惯性导航系统测得的加速度和角速度信息和GPS测得的速度和位置信息,利用自旋制导炮弹空中粗对准方法计算得到自旋制导炮弹在空中飞行初始时刻实际的位置信息[la0 phia0 h0]、速度信息[Ve0 Vn0 Vu0]和姿态信息[θ0 γ0 ψ0],其中la0表示粗对准得到的初始经度,phia0表示粗对准得到的初始纬度,h0表示粗对准得到的初始高度,Ve0表示粗对准得到的初始东向速度,Vn0表示粗对准得到的初始北向速度,Vu0表示粗对准得到的初始天向速度,θ0表示粗对准得到的初始俯仰角,γ0表示粗对准得到的初始横滚角,ψ0表示粗对准得到的初始航向角;
[0047] (2)根据在tn时刻采集惯性导航系统测得的加速度和角速度信息,并根据测得的加速度和角速度信息以及tn-1时刻自旋制导炮弹实际的姿态、位置和速度信息进行导航解算,获得tn时刻通过惯性导航系统解算出的自旋制导炮弹的姿态信息[θn γn ψn]、位置信息[lan phian hn]以及速度信息[Ven Vnn Vun],其中ψn为tn时刻自旋制导炮弹的航向角,θn为tn时刻自旋制导炮弹的俯仰角,γn为tn时刻自旋制导炮弹的横滚角,lan为tn时刻自旋制导炮弹的经度,phian为tn时刻自旋制导炮弹的纬度,hn为tn时刻自旋制导炮弹的高度,Ven为tn时刻自旋制导炮弹的东向速度,Vnn为tn时刻自旋制导炮弹的北向速度,Vun为tn时刻自旋制导炮弹的天向速度,进入步骤(3),其中n的初始值为1;
[0048] (3)在tn时刻,自旋制导炮弹判断是否接收到GPS输出的速度和位置信息,如果没有接收到,则将步骤(2)的导航解算结果作为tn时刻自旋制导炮弹实际的速度、位置和姿态信息,进入步骤(5);否则,进入步骤(4);
[0049] (4)自旋制导炮弹根据tn时刻接收到的GPS的速度信息[Vgen Vgnn Vgun]计算tn时刻自旋自转炮弹的俯仰角θgn和航向角ψgn,并通过和tn时刻惯性导航系统解算出的自旋制导炮弹的姿态信息[θn γn ψn]、位置信息[lan phian hn]以及速度信息[Ven Vnn Vun]进行组合导航计算,得到tn时刻自旋制导炮弹实际的速度、位置和姿态信息;其中,Vgen为tn时刻GPS输出的东向速度,Vgnn为tn时刻GPS输出的北向速度,Vgun为tn时刻GPS输出的天向速度,θgn为tn时刻GPS输出的速度计算得到的俯仰角,ψgn为tn时刻GPS输出的速度计算得到的航向角;进入步骤(5);
[0050] (5)n的值加1后返回步骤(2),依次类推,计算出每一时刻自旋制导炮弹空中的实际速度、位置和姿态信息,将组合导航解算后得到的三个姿态和三个速度反馈到导航解算中,作为下一时刻导航解算的初始值,进入解算,最终获得制导炮弹的导航轨迹和飞行控制参数。
[0051] 上述各步骤中,tn与tn-1的时间间隔为惯性导航系统的测量周期。
[0052] 所述步骤(4)计算得到tn时刻GPS输出的速度计算得到的俯仰角和航向角方法为:
[0053]
[0054]
[0055] 所述步骤(4)中进行组合导航计算方法为:
[0056] 本发明重新设计了卡尔曼滤波算法,
[0057] 利用量测阵H和观测阵C,根据卡尔曼滤波算法解算tn时刻的状态估计量δVce=Ven-Vgen,
[0058] δVcn=Vnn-Vgnn,δVcu=Vun-Vgun,δθc=θn-θgn,δψc=ψn-ψgn[0059] 其中δVen为tn时刻自旋制导炮弹的东向速度修正值,δVnn为tn时刻自旋制导炮弹的北向速度修正值,δVun为tn时刻自旋制导炮弹的天向速度修正值,δθn为tn时刻自旋制导炮弹的俯仰角修正值,δγn为tn时刻自旋制导炮弹的横滚角修正值,δψn为tn时刻自旋制导炮弹的航向角修正值,为tn时刻自旋制导炮弹的三个陀螺仪零偏值;根据Xn可以得到tn时刻东向速度修正值,北向速度修正值,天向速度修正值,俯仰角修正值,横滚角修正值,航向角修正值,三个陀螺仪零偏修正值。
[0060] 所述步骤(4)的组合导航计算三个速度解算方法为:
[0061] VKen=Ven-Xn (1)
[0062] VKnn=Vnn-Xn (2)
[0063] VKun=Vun-Xn (3)
[0064] 其中VKen表示组合导航后tn时刻自旋制导炮弹实际的东向速度,VKnn表示组合导航后tn时刻自旋制导炮弹实际的北向速度,VKun表示组合导航后tn时刻自旋制导炮弹实际的天向速度,Xn(1)表示tn时刻自旋制导炮弹的东向速度修正值,Xn(2)表示tn时刻自旋制导炮弹的北向速度修正值,Xn(3)表示tn时刻自旋制导炮弹的天向速度修正值。
[0065] 所述步骤(4)的组合导航计算三个姿态解算方法为:
[0066]
[0067] Cbn=CnbT
[0068]
[0069] Cbnn=Cnn*Cbn
[0070] γKn=arcsin(Cbnn(3,2))
[0071] θKn=-arctan(Cbnn(3,1)/Cbnn(3,3))
[0072] ψKn=ψgn
[0073] 其中Cnb表示tn时刻组合导航前从导航坐标系到载体坐标系的转换矩阵,Cbn表示tn时刻组合导航前从载体坐标系到导航坐标系的转换矩阵,Cnn表示tn时刻姿态修正矩阵,Cbnn表示tn时刻组合导航后从载体坐标系到导航坐标系的转换矩阵,γKn表示tn时刻组合导航后的横滚角,θKn表示tn时刻组合导航后的俯仰角,ψKn表示tn时刻组合导航后的航向角。Xn(4)表示tn时刻自旋制导炮弹的俯仰角修正值,Xn(5)表示tn时刻自旋制导炮弹的横滚角修正值。
[0074] tn时刻惯性导航系统解算出的位置信息[lan phian hn]即为tn时刻自旋制导炮弹实际的位置信息。
[0075] 通过上述方法即可完成高动态自旋制导炮弹空中组合导航。
[0076] 本发明通过自旋制导炮弹空中粗对准方法得到初始时刻的三个位置、三个速度和三个姿态,与惯性导航系统数据进行导航解算,得到对应的导航结果,包含三个位置、三个姿态(航向角、俯仰角、横滚角)和三个速度;通过解算对应时间下的GPS输出的导航信息得到自旋制导炮弹空中对应时刻的航向角和俯仰角,计算对应时刻的三个速度误差值、俯仰角误差值和航向角误差值,作为9维卡尔曼滤波估计的观测阵C,估算出对应时刻的三个姿态角修正值、三个速度修正值和三个陀螺仪零偏修正,对高动态自旋炮弹进行空中组合导航解算,得到了自旋制导炮弹空中对应时刻的三个姿态信息和三个速度信息,从而获得制导炮弹的导航轨迹和飞行控制参数。相比其它高动态自旋制导炮弹空中组合导航方法,本发明实现了高动态自旋制导炮弹空中失重情况下的组合导航问题,同时通过卡尔曼滤波器实现了对导航噪声和陀螺仪零偏的滤除,提高了自旋制导炮弹初始对准参数的精确性和修正算法的快速性,降低系统设计难度,提高了动态自旋制导炮弹的导航精度以及落点精度,增加了制导炮弹的可控性。
[0077] 图2为本发明方法设计的卡尔曼滤波速度误差估计效果图,图中第一行为东向速度修正曲线,第二行为北向速度修正曲线,第三行为天向速度修正曲线,可以从图中看出,速度误差收敛,且水平速度误差值在1m/s以内,高度速度误差值在2m/s,达到组合导航修正速度的效果。图3为本发明方法设计的卡尔曼滤波姿态误差估计效果图,图中第一行为俯仰角误差修正值,第二行为横滚角误差修正值,第三行为航向角误差修正值,可以从图中看出,姿态误差收敛,达到组合导航修正速度的效果。图4为本发明方法设计的卡尔曼滤波陀螺仪零偏估计效果图,图中第一行为X轴陀螺零偏估计值,第二行为Y轴陀螺零偏估计值,第三行为Z轴陀螺零偏估计值,可以从图中看出,零偏估计值收敛,且与所使用的陀螺仪相对应,估算结果准确。图5为使用本发明方法得到的组合导航曲线和GPS的输出曲线示意图,第一行依次分别为经度、纬度、高度,第二行依次分别为东向速度、北向速度、天向速度,第三行依次分别为俯仰角、横滚角和航向角。其中GPS输出结果用线表示,组合导航结果用点表示,从图中可以看出,组合导航结果与GPS输出相比,两者重合的较好,说明组合导航算法修正了惯性导航系统的输出,满足了制导炮弹空中导航控制的要求。
[0078] 本发明未详细描述内容为本领域技术人员公知技术。
QQ群二维码
意见反馈