1.本发明属于精确制导技术领域，尤其涉及一种弹体姿态误差估计方法及系统。
背景技术：
2.随着卫星导航技术的飞速发展，其正以低价高效的形象出现在多领域的精确制导武器装备中。随之而来的是对于卫星导航的反制手段也在逐渐普及，从干扰到诱骗，层出不穷，伴随着卫星导航的不稳定因素增多，对于弹上纯惯性导航系统也出现了更多更高的需求，特别是对于纯惯性长航时条件下的导航精度要求，正在造成整弹成本的快速增长。针对纯惯性导航精度提升的方法主要是提升初始姿态精度，目前要实现对姿态误差的估计和补偿多会受到载机平台机动能力低、机体结构精度不高、机翼飞行中变形等问题的困扰，如何利用框架激光导引头的框架角信息对弹体姿态误差进行估计和补偿是一个值得研究的方向。弹载框架激光导引头可以实现对目标点在方位和俯仰两个通道的跟踪，根据导引头和目标点之间的位置关系，实时输出对目标点的框架方位角和高低角信息，具备精度高，延时小的特点，已经应用于国内外多型弹药的制导化改造中。
技术实现要素：
3.本发明提出一种弹体姿态误差估计方法及系统，当弹体进入跟踪模式后，利用弹上激光框架导引头的方位角和俯仰角信息对弹体姿态误差进行补偿。如此可以更好地实现对弹药姿态信息的精准测量，解决了现有框架激光制导武器的导航姿态精度低的问题。
4.本发明提出一种弹体姿态误差估计方法，包括：
5.根据载机俯仰角信息和航向角信息以及在挂载于载机上的弹体的俯仰角和航向角信息，计算弹体的地面航向角偏差yawerrgn和地面俯仰角偏差piterrgn；
6.根据弹体挂飞中实时的位置信息(lonm,latm,altm)，弹体俯仰角p、弹体航向角y、弹体滚转角r，导引头的实际框架方位角seekeryaw和实际框架俯仰角seekerpit信息以及地面校准点位置信息(lon
t
,lat
t
,alt
t
)，计算导引头实际框架方位角seekeryaw与导引头理论框架方位角seekeryawnav之间的挂飞方位角偏差yawerrreal以及导引头实际框架俯仰角seekerpit与导引头理论框架俯仰角seekerpitnav之间的挂飞俯仰角偏差piterrreal；
7.将所述弹体的地面航向角偏差yawerrgnd和所述地面俯仰角偏差piterrgnd作为卡尔曼滤波的状态方程输出，将所述挂飞方位角偏差yawerrreal和所述挂飞俯仰角偏差piterrreal进行卡尔曼滤波作为卡尔曼滤波方程的观测信息，得到弹体方位角误差的估计值yawerrcom和弹体俯仰角偏差的估计值piterrcom。
8.进一步的，所述载机俯仰角信息和航向角信息是由载机的导航系统提供的，挂载于载机上的弹体的俯仰角和航向角信息是使用基于gnss的双天线测向装置测量得到的。
9.进一步的，所述导引头的实际框架方位角seekeryaw和实际框架俯仰角seekerpit信息的获取方法是：在地面校准点的指定距离范围内设置激光照射器，激光照射器-校准点连线与所述载机航线的夹角在指定范围之内，当弹体与所述地面校准点的水平距离
offsetdis小于等于指定阈值后，所述激光照射器对所述地面校准点进行照射，导引头进入跟踪模式，所述导引头实时输出实际框架方位角seekeryaw和实际框架俯仰角seekerpit信息。
10.进一步的，当导引头进入跟踪模式后，计算导引头实际框架方位角seekeryaw与导引头理论框架方位角seekeryawnav之间的挂飞方位角偏差yawerrreal以及导引头实际框架俯仰角seekerpit与导引头理论框架俯仰角seekerpitnav之间的挂飞俯仰角偏差piterrreal，具体包括：
11.将导弹—地面校准点的三方向距离[xn,yn,zn]
t
从地理系转换到弹体系[xb,yb,zb]
t
：
[0012]
计算导弹—地面校准点的三方向距离：
[0013][0014]
其中，lonm、latm、altm为弹体组合导航的位置信息，lon
t
、lat
t
、alt
t
为校准点位置信息，rm为地球子午圈半径，rn为地球卯酉圈半径；然后计算[xb,yb,zb]
t
，如下：
[0015][0016]
其中是地理系到弹体系的姿态转移矩阵，形式如下：
[0017][0018]
其中，p代表弹体俯仰角，y代表弹体航向角，r代表弹体滚转角；
[0019]
计算导引头理论框架方位角seekeryawnav和导引头理论框架俯仰角seekerpitnav，公式如下：
[0020][0021]
seekerpitnav＝π/2-arctan2(xb/zb)
[0022]
计算挂飞方位角偏差yawerrreal和挂飞俯仰角偏差piterrreal如下：
[0023]
yawerrreal＝seekeryaw-seekeryawnav
[0024]
piterr real＝seekerpit-seekerpitnav。
[0025]
进一步的，所述方法进一步包括，将弹体方位角误差的估计值yawerrcom和弹体俯仰角偏差的估计值piterrcom补偿到对应的导弹弹体姿态信息中。
[0026]
本发明提出一种弹体姿态误差估计系统，包括：
[0027]
地面姿态偏差计算模块，根据载机俯仰角信息和航向角信息以及在挂载于载机上的弹体的俯仰角和航向角信息，计算弹体的地面航向角偏差yawerrgnd和地面俯仰角偏差piterrgnd；
[0028]
挂飞姿态偏差计算模块，根据弹体挂飞中实时的位置信息(lonm,latm,altm)，弹体
俯仰角p、弹体航向角y、弹体滚转角r，导引头的实际框架方位角seekeryaw和实际框架俯仰角seekerpit信息以及地面校准点位置信息(lon
t
,lat
t
,alt
t
)，计算导引头实际框架方位角seekeryaw与导引头理论框架方位角seekeryawnav之间的挂飞方位角偏差yawerrreal以及导引头实际框架俯仰角seekerpit与导引头理论框架俯仰角seekerpitnav之间的挂飞俯仰角偏差piterrreal；
[0029]
姿态偏差估计模块，将所述弹体的地面航向角偏差yawerrgnd和所述地面俯仰角偏差piterrgnd作为卡尔曼滤波的状态方程输出，将所述挂飞方位角偏差yawerrreal和所述挂飞俯仰角偏差piterrreal作为卡尔曼滤波方程的观测信息，进行卡尔曼滤波，得到弹体方位角偏差的估计值yawerrcom和弹体俯仰角偏差的估计值piterrcom。
[0030]
进一步的，所述载机俯仰角信息和航向角信息是由载机的导航系统提供的，挂载于载机上的弹体的俯仰角和航向角信息是使用基于gnss的双天线测向装置测量得到的。
[0031]
进一步的，所述导引头的实际框架方位角seekeryaw和实际框架俯仰角seekerpit信息的获取方法是：在地面校准点的指定距离范围内设置激光照射器，激光照射器-校准点连线与所述载机航线的夹角在指定范围之内，当弹体与所述地面校准点的水平距离offsetdis小于等于指定阈值后，所述激光照射器对所述地面校准点进行照射，导引头进入跟踪模式，所述导引头实时输出实际框架方位角seekeryaw和实际框架俯仰角seekerpit信息。
[0032]
进一步的，当导引头进入跟踪模式后，计算导引头实际框架方位角seekeryaw与导引头理论框架方位角seekeryawnav之间的挂飞方位角偏差yawerrreal以及导引头实际框架俯仰角seekerpit与导引头理论框架俯仰角seekerpitnav之间的挂飞俯仰角偏差piterrreal，具体包括：
[0033]
将导弹—地面校准点的三方向距离[xn,yn,zn]
t
从地理系转换到弹体系[xb,yb,zb]
t
：
[0034]
计算导弹—地面校准点的三方向距离：
[0035][0036]
其中，lonm、latm、altm为弹体组合导航的位置信息，lon
t
、lat
t
、alt
t
为校准点位置信息，rm为地球子午圈半径，rn为地球卯酉圈半径；然后计算[xb,yb,zb]
t
，如下：
[0037][0038]
其中是地理系到弹体系的姿态转移矩阵，形式如下：
[0039][0040]
其中，p代表弹体俯仰角，y代表弹体航向角，r代表弹体滚转角；
[0041]
计算导引头理论框架方位角seekeryawnav和导引头理论框架俯仰角
seekerpitnav，公式如下：
[0042][0043]
seekerpitnav＝π/2-arctan2(xb/zb)
[0044]
计算挂飞方位角偏差yawerrreal和挂飞俯仰角偏差piterrreal如下：
[0045]
yawerrreal＝seekeryaw-seekeryawnav
[0046]
piterr real＝seekerpit-seekerpitnav。
[0047]
进一步的，所述系统进一步包括误差补偿模块，将弹体方位角误差的估计值yawerrcom和弹体俯仰角偏差的估计值piterrcom补偿到导弹弹体对应的姿态信息中。
[0048]
本发明旨在利用弹载框架激光导引头的框架角信息减小弹体的导航姿态误差，提高弹体的导航精度，有利于纯惯性导航激光末制导武器快速准确捕获目标，提升激光制导武器弹药的使用效能。
附图说明
[0049]
图1是本发明实施例中一种基于弹载框架激光导引头的弹体姿态误差估计方法的步骤流程图；
[0050]
图2是本发明实施例中导弹和校准点的位置、角度关系信息；
[0051]
图3是本发明实施例中方案飞行航线概况图示；
[0052]
图4是本发明量测偏差和滤波偏差信息。
具体实施方式
[0053]
以下结合附图对具体实施实施方式做出详细说明。本发明提出一种弹体姿态误差估计方法及系统。
[0054]
本发明一种弹体姿态误差估计方法的总体步骤包括：
[0055]
s1、初始化步骤：根据导引头性能参数预先设定地面校准点和校准航线，参见图3，便于导引头在挂飞过程中快速、准确地捕获校准点处的激光信号。
[0056]
在地面采集弹体的航向角和俯仰角姿态误差信息，采取多次采集取均值的方式，获取弹体地面姿态偏差信息yawerrgnd和piterrgnd；
[0057]
在基于弹载框架激光导引头的弹体姿态误差估计方法中，地面校准点的坐标需要提前获取。校准航线的选取需要依据导引头的框架方位角范围[lyawlim,ryawlim]和框架高低角范围[lpitlim,hpitlim]以及导引头有效识别距离effdis来计算。
[0058]
地面校准点的wgs-84坐标(lon
t
,lat
t
,alt
t
)，可地面事先采集或使用高精度机载吊舱进行采集；
[0059]
校准航线一般要求在校准点正上方，航线高度hairline应满足下述公式，
[0060]
hairline≤effdis
×
sin(lpitlim)
···
(1)
[0061]
航线起点与校准点水平距离stardis满足下述公式，
[0062]
stardis≥effdis
×
cos(lpitlim)
···
(2)
[0063]
航线水平偏移offsetdis满足下述公式，
[0064]
offsetdis≤effdis
×
atan(min[lyawlim,ryawlim])
···
(3)
[0065]
s2、地面弹体姿态偏差计算步骤：根据载机俯仰角信息和航向角信息以及在挂载于载机上的弹体的俯仰角和航向角信息，计算弹体的地面航向角偏差yawerrgnd和地面俯仰角偏差piterrgnd。在具体实施时，可以使用基于gnss的双天线侧向装置分别测量弹体在载机上挂载时的俯仰角信息和航向角信息，并获取载机的在同时刻的俯仰角信息和航向角信息，在利用二者算出弹体的地面航向角偏差yawerrgnd和地面俯仰角偏差piterrgnd，可将上述角度信息作为卡尔曼滤波器的状态方程输出；
[0066]
s3、挂飞弹体姿态偏差计算步骤：当导引头进入跟踪模式后，根据弹体实时的位置信息(lonm,latm,altm)，弹体俯仰角p、弹体航向角y、弹体滚转角r以及地面校准点位置信息(lon
t
,lat
t
,alt
t
)，反算导引头理论框架方位角seekeryawnav和理论框架俯仰角seekerpitnav信息，并将上述角度与导引头实际输出的框架方位角seekeryaw和框架高低角seekerpit作差，计算挂飞方位角偏差yawerrreal以及挂飞俯仰角偏差piterrreal，并将上述角度信息作为卡尔曼滤波的观测信息；
[0067]
具体的，在地面预先假设激光照射器，激光照射器-地面校准点距离不大于2km，与航线夹角在30～60
°
之间，当导弹-地面校准点水平距离offsetdis满足设计要求后，持续照射地面校准点，导引头捕获目标进入跟踪模式后，会实时输出框架方位角seekeryaw和框架俯仰角seekerpit。
[0068]
当弹体与所述地面校准点的水平距离offsetdis大于指定阈值时，导弹处于巡航模式。当导引头进入跟踪模式后，利用弹体组合导航的位置信息(lonm,latm,altm)、校准点位置信息(lon
t
,lat
t
,alt
t
)、弹体姿态信息(航向角y、俯仰角p、滚转角r)和导引头框架角信息(seekeryaw和seekerpit)实时计算弹体姿态误差信息yawerrreal和piterrreal。
[0069]
将弹-校准点的三方向距离[xn,yn,zn]
t
从地理系转换到弹体系[xb,yb,zb]
t
，计算公式如下：
[0070]
计算弹-校准点三方向距离：
[0071][0072]
其中，rm为地球子午圈半径，rn为地球卯酉圈半径。然后计算[xb,yb,zb]
t
，如下：
[0073][0074]
其中是地理系到弹体系的姿态转移矩阵，形式如下：
[0075][0076]
其中，p代表弹体俯仰角，y代表弹体航向角，r代表弹体滚转角。
[0077]
此时可根据弹体系下位置关系，反算出导引头理论框架方位角seekeryawnav和导引头理论框架俯仰角seekerpitnav，公式如下：
[0078][0079]
seekerpitnav＝pi/2-atan2(xb/zb)
···
(8)
[0080]
反算出的导引头理论框架方位角和俯仰角与导引头实际框架方位角和框架俯仰角存在偏差，就是我们要求的yawerrreal和piterrreal，计算公式如下：
[0081]
yawerrreal＝seekeryaw-seekeryawnav
···
(9)
[0082]
piterr real＝seekerpit-seekerpitnav
···
(10)。
[0083]
s4、误差估计步骤：对所述弹体的地面航向角偏差yawerrgnd和地面俯仰角偏差piterrgn，以及挂飞方位角偏差yawerrreal和挂飞俯仰角偏差piterrreal进行卡尔曼滤波，得到弹体方位角误差的估计值yawerrcom和弹体俯仰角误差的估计值piterrcom。进一步的，可以使用弹体姿态偏差的估计值对弹体姿态信息进行实时补偿。
[0084]
本发明还提出一种弹体姿态误差估计系统，包括：地面弹体姿态偏差计算模块、挂飞弹体姿态偏差计算模块、弹体姿态误差估计模块。
[0085]
地面弹体姿态偏差计算模块，根据载机俯仰角信息和航向角信息以及在挂载于载机上的弹体的俯仰角和航向角信息，计算弹体的地面航向角偏差yawerrgnd和地面俯仰角偏差piterrgnd；其中，所述载机俯仰角信息和航向角信息以及在挂载于载机上的弹体的俯仰角和航向角信息是使用基于gnss的双天线测向装置分别测量得到的。
[0086]
挂飞弹体姿态偏差计算模块，根据弹体实时的位置信息(lonm,latm,altm)，弹体俯仰角p、弹体航向角y、弹体滚转角r，导引头的框架方位角seekeryaw和俯仰角seekerpit信息以及地面校准点位置信息(lon
t
,lat
t
,alt
t
)，计算导引头理论框架方位角与导引头实际框架方位角之间的挂飞方位角偏差yawerrreal以及导引头理论俯仰角与导引头实际框架俯仰角之间的挂飞俯仰角偏差piterrreal。
[0087]
所述框架激光导引头的方位角seekeryaw和俯仰角seekerpit信息的获取方法是：在地面校准点的指定距离范围内设置激光照射器，所述“激光照射器-校准点”连线与所述载机航线的夹角在指定范围之内，当弹体与所述地面校准点的水平距离offsetdis小于指定阈值后，所述激光照射器对所述地面校准点进行照射，导引头进入跟踪模式，所述导引头实时输出对校准点的框架方位角seekeryaw和俯仰角seekerpit信息。
[0088]
将“导弹-校准点”的三方向距离[xn,yn,zn]
t
从地理系转换到弹体系[xb,yb,zb]
t
：
[0089]
计算“导弹-校准点”的三方向距离：
[0090][0091]
其中，lonm、latm、altm为弹体组合导航的位置信息，lon
t
、lat
t
、alt
t
为校准点位置信息，rm为地球子午圈半径，rn为地球卯酉圈半径；然后计算[xb,yb,zb]
t
，如下：
[0092][0093]
其中是地理系到弹体系的姿态转移矩阵，形式如下：
[0094][0095]
其中，p代表弹体俯仰角，y代表弹体航向角，r代表弹体滚转角；
[0096]
计算导引头框架方位角seekeryawnav和导引头框架俯仰角seekerpitnav，公式如下：
[0097][0098]
seekerpitnav＝π/2-arctan2(xb/zb)
[0099]
计算挂飞方位角偏差yawerrreal和挂飞俯仰角偏差piterrreal如下：
[0100]
yawerrreal＝seekeryaw-seekeryawnav
[0101]
piterrreal＝seekerpit-seekerpitnav。
[0102]
误差估计模块，对所述弹体的地面航向角偏差yawerrgnd和地面俯仰角偏差piterrgnd，以及挂飞方位角偏差yawerrreal和挂飞俯仰角偏差piterrreal进行卡尔曼滤波，得到弹体方位角误差的估计值yawerrcom和弹体俯仰角偏差的估计值piterrcom。
[0103]
本发明弹体姿态误差估计系统进一步包括误差补偿模块，其将所述弹体方位角误差的估计值yawerrcom和弹体俯仰角偏差的估计值piterrcom补偿到导弹弹体姿态信息中。
[0104]
实施例
[0105]
为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明公开的实施方式作进一步详细描述。
[0106]
如图1，在本实施例中，基于弹载框架激光导引头的弹体姿态误差估计方法，包括：
[0107]
步骤101，根据导引头性能参数预先设定地面校准点和校准航线。
[0108]
在本实施例中，地面校准点的位置坐标可由地面采集获取，通过数据链实时上传给弹上导航系统。校准点坐标和载机位置信息均采用wgs-84坐标系下的位置信息。也可以采用其它手段获取，需要保证校准点坐标精度综合误差小于50米。
[0109]
航线高度一般以不大于1/2hairline为宜；航线水平偏移距离以不大于2/3offsetdis为宜。
[0110]
步骤102，地面采集得到弹体姿态偏差信息yawerrgnd和piterrgnd。
[0111]
在本实施例中，地面使用基于gnss的双天线侧向装置分别测量弹体在载机上挂载时的俯仰角信息和航向角信息，并获取载机的在同时刻的俯仰角信息和航向角信息，利用二者算出弹体的航向角偏差yawerrgnd和俯仰角偏差piterrgnd，一般可以采集不少于7次的数据，统计测量结果的均值和标准差。
[0112]
步骤103，地面照射校准点，根据弹体挂飞过程中的导航信息和导引头框架角信息解算弹体姿态误差信息yawerrreal和piterrreal。
[0113]
在本实施例中，应合理规划激光照射点位置，地面激光照射点的位置应综合考虑实际天气和照射器功率，一般的照射器-校准点距离不大于2km，照射器-校准点连线与校准航线夹角在30～60
°
之间。采用指令控制的方式，控制激光照射器对校准点进行照射。为利于弹载框架激光导引头捕获目标，可采取在校准点架设特定反射系数靶标的措施。挂飞过程中，间隔固定周期ticom获取导引头框架角信息和导航信息，一般的，本实施例中ticom取
200ms。导航位置和角度信息采用弧度作为单位。
[0114]
步骤104使用卡尔曼滤波对上述姿态误差信息进行滤波处理。滤波步长采用1秒，实时对上述步骤中的姿态角误差信息进行滤波处理，获取弹体姿态误差的估计值yawerrcom和piterrcom。
[0115]
步骤105获取实时获取滤波处理后的姿态误差信息yawerrcom和piterrcom。
[0116]
步骤106将滤波更新后的yawerrcom、piterrcom补偿到弹体姿态信息。具体补偿的策略需要因时因地综合考虑，一般的对姿态误差信息yawerrcom、piterrcom进行动态限制处理。从图4中可以看出经过滤波处理后的误差值更准确，变化更平稳。
[0117]
对于系统实施例而言，由于其与方法实施例相对应，所以描述的比较简单，相关之处参见方法实施例部分的说明即可。
[0118]
本发明虽然已以较佳实施例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不脱离本发明的精神和范围内，都可以利用上述揭示的方法和技术内容对本发明技术方案做出可能的变动和修改，因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化及修饰，均属于本发明技术方案的保护范围。
[0119]
本发明说明书中未作详细描述的内容属于本领域专业技术人员的公知技术。