本发明适用于自主导航领域，尤其适用于超高速飞行器自主导航系统。
背景技术：
超高速飞行器通常是指飞行马赫数大于2以上的飞行器，其自主导航是不依赖地面测控信息，仅靠机载传感器测控数据和自身运动模型确定飞行状态，完成轨迹实时确定的过程，对于飞行器实际工程应用具有极高价值。随着航天科技的不断发展，未来超高速飞行器呈现出超高音速、大升阻比、高机动等特点，超高速飞行器的这些飞行特点给自主导航带来严峻挑战：1)在超高速飞行器机动的过程中，受发动机推力等作用的影响，初始的动力学模型与实际运动情况不符，而模型误差会直接影响滤波精度；2)超高速飞行器在跨域飞行边界由于激波形成包围飞行器的高温等离子体壳层，会导致无线电导航信息间断，因此依赖于卫星导航的接收机会存在大范围、长弧段误差，严重影响超高速飞行器的导航精度，甚至导致发散。
上述第一个问题的主要难点源于飞行器发动机推力等产生的加速度的不确定性，这会造成滤波过程中基于动力学模型的预测精度下降。目前，利用加速度计可以获得较为准确的加速度信息，但是当飞行器高动态飞行时，可能导致加速度计信息不可靠或者无加速度计信息。此时，可以采用对带有动力学模型不确定性的系统进行状态估计的方法，目前已有的典型方法是鲁棒滤波。鲁棒滤波的设计指标能够使模型中的不确定因素对估计精度的影响降到最低程度。但是，鲁棒滤波需要不确定性模型误差的上界等信息，且其设计思想是抑制模型误差的影响，而不是估计并补偿模型误差的影响，因此估计精度受到一定限制。
针对第二个无线电导航信息间断问题，考虑到地磁传感器具有体积小、成本低、精度高，并且抗冲击能力和过载能力强的特点，可以采用gnss与地磁敏感器组合导航的方式，目前已有的工作主要集中在地磁/惯导/gnss组合导航上，并未针对gnss信号闪断、无加速度计信息的情况，考虑如何仅利用地磁测量进行导航的问题。
因此，目前针对超高速飞行器飞行特点给自主导航带来的上述两个问题，尚未见到合适的综合处理方法，而如何针对超高速飞行器的工程物理特点，提出适合超高速飞行器的实时在线导航算法具有重要的意义。
技术实现要素：
针对现有技术的不足，本发明提供一种能够补偿超高速飞行器的自主导航系统模型误差的方法，使得超高速飞行器在高动态飞行导致gnss信号闪断、无加速度计信息、动力学模型不准确等问题时，仍能保证自主导航系统稳定运行。
本发明的具体技术方案如下：
一种超高速飞行器自主导航系统的模型误差补偿方法，包括以下步骤：
s1、考虑地磁敏感器测量误差，建立自主导航系统测量模型；
进一步的，所述s1步骤具体如下：
s101、建立地磁敏感器测量模型；
地磁场自主导航系统通常使用的观测量有两种:一种是利用地磁场强度矢量,包括地磁场强的大小和方向；另一种仅仅利用地磁场强的长度幅值作为观测量；第一种利用矢量作为观测量研究的方法，观测维数多，约束较强，有利于估计精度的提高，且可同时求解姿态信息，但是模型比较复杂，计算量较大；第二种利用磁场强度的一维长度幅值作为观测量，不涉及坐标系变换，计算简单，但是观测维数少，约束弱，通常估计精度较低，在本发明中，考虑重点关注飞行器的位置速度信息，且采用组合导航的方式，故采用第二种观测量的方式进行快速计算，通过合适的组合导航方式及模型误差补偿方法，使得在采用第二种磁场强度的一维长度幅值作为观测量时，仍能保证导航系统的稳定运行；
具体的，地磁场是空间位置与时间的函数,由地球主磁场、地壳磁场和瞬时磁场三部分组成，其中,主磁场约占地球总磁场的95％,是实现地磁导航定向的基本依据,国际上已有的全球地磁模型或地磁图都是为了精确逼近或描述地球的主磁场部分。在地球物理学中,表示地球主磁场的国际标准叫做“国际参考地磁场”(igrf),它以球谐级数的形式表达：
式中，v表示磁位势，re为地球参考球赤道半径，r为空间一点的地心距，
对于近地空间，静年时的外源场强度不到内源场的千分之一，强扰动时也仅占内源场的百分之一，因此外磁场的影响可以忽略,则模型简化为:
对(2)式的磁位势函数求取负梯度函数则可以得到磁场强度，则地磁场强度矢量b(又称地磁场磁通密度矢量)由下式确定:
那么，为方便使用，采用地磁场矢量的长度(与坐标系无关)作为导航测量模型的形式如下：
式中，ε为磁场矢量长度测量噪声，磁强计该部分误差并不大，例如某型卫星用磁强计噪声为数nt，bb为磁场矢量长度测量的系统误差；
s102、地磁敏感器测量系统误差分析；
通常情况下，目标多由钢铁构件及电气线路组成，这些材料所形成的磁场，对地磁敏感器产生影响，导致测量得到的地磁场矢量与真实的地磁场矢量不一致，加上由于生产工艺水平、材料特性的制约使得地磁传感器本身存在各种误差，导致测量得到的地磁场矢量与真实的地磁场矢量不一致，甚至二者之间存在非常大的差别；通常的，对载体硬磁场补偿常采用固定磁棒或通电线圈，对载体软磁场感应场补偿常采用坡膜合金；但是，增加补偿元件不符合简易制导弹药对实际空间、成本的要求，目前对地磁敏感器误差的补偿可采用建立载体磁场数学模型的方法，计算出干扰大小达到补偿目的；
目前国内外关于地磁敏感器误差的研究普遍把地磁敏感器误差分为自身误差和环境干扰误差；自身误差是由于受到生产工艺水平和材料特性的限制，致使敏感器无法达到理想的状态而引起的误差，主要包括：灵敏度误差，通常用对角矩阵csi表示；偏置误差(也叫零位误差)，常用矢量形式bo表示；非正交误差，由地磁敏感器三个敏感轴的实际方向不完全正交的角度构成的矩阵cn表示；环境干扰误差是由于弹体自身的铁磁性材料致使地磁场畸变而引起的误差：根据其影响的表现形式不同，分为硬磁误差，其误差影响可用矢量形式bh表示；以及软磁误差，由误差系数矩阵cs表示；
综合考虑地磁敏感器的自身误差和环境干扰误差，可以得到完整的误差模型如下:
bm＝csicn(csb+bh)+bo+εm(5)
其中,bm是地磁敏感器的输出值，b是地磁敏感器没有误差影响条件下磁场矢量，其三个分量即为(4)式中的bx,by,bz，csi表示灵敏度误差矩阵，cn表示非正交误差矩阵，cs表示软磁误差矩阵，bh表示硬磁误差矢量，bo表示偏置误差矢量，εm表示地磁矢量测量噪声；
(5)式可以等价描述为如下地磁敏感器测量误差模型：
bm＝cb+b+εm＝b+(c-i)b+b+εm(6)
其中，c＝csicncs,b＝csicnbh+bo,i为单位矩阵；
本发明的地磁敏感器测量中，较为全面的考虑了多种测量系统误差的影响，后续设计的模型误差补偿的主要对象之一就是上述地磁敏感器的多种测量系统误差的影响；
s103、构建基于地磁敏感器的自主导航系统测量模型；
结合(5)和(4)式可知，地磁测量的系统误差包含磁强计灵敏度误差、非正交误差矩阵、软磁误差、硬磁误差、偏置误差等测量误差，由于这些误差项很难在地面实验室条件下精确的测量，且也并不是一成不变的，所以很难精确建模，但是这些误差会受在轨地磁场变化或者磁强计的工作时间而缓慢改变，从而影响地磁导航的精度；考虑到导航系统的敏感器采样频率较高(不低于1hz)，所以将采样间隔内的bb转变为确定性的系统误差扩充到状态方程中作为状态变量进行估计，且令bb＝εb；
综上，将测量模型记为：
hm＝h(x)+bb+ε(7)
其中
(7)式即为本发明建立的基于地磁敏感器的自主导航系统测量模型，其中的位置矢量x和地磁测量系统误差bb均为待估计的状态参数；基于gnss的自主导航测量模型直接是关于位置、速度的测量，可参见刘夫体的文章《gps与雷达测量数据融合处理方法在外弹道测量中的应用》；
s2、带模型误差补偿的自主导航系统动力学模型构建：
超高速飞行器的动力学模型主要由两部分构成，一部分是仅考虑地球与目标二者之间的引力模型；另一部分是飞行器在飞行过程中受到的除第一部分引力之外的动力学模型误差补偿项，因此，超高速飞行器的动力学模型的描述形式如下：
其中，(x,y,z)为位置参数，r为相应的距离值，μ为地球引力常数，δ为动力学模型误差补偿项，且δ＝εδ，v为噪声项；
此时，关于状态变量x＝[xyzvxvyvzδxδyδzbb]10×1的状态模型如下:
x＝fx+η＝f(x)+η(9)
式中，
上述(9)式即为本发明中建立的带模型误差补偿的自主导航系统动力学模型，待估计的状态变量包括位置、速度、动力学模型误差补偿矢量以及地磁强度幅值测量的系统误差；其中的动力学模型误差补偿矢量是状态模型误差补偿量，地磁强度幅值测量的系统误差是测量模型误差补偿量；
s3、采用基于无迹滤波(unscentedkalmanfilter,ukf)的自主导航算法进行导航：
根据构建的自主导航系统测量模型(7)可见，测量模型是复杂的非线性模型，因此，本发明中，实时的导航滤波算法采用适用于非线性系统模型的无迹滤波(ukf)；ukf算法的基本思想和主要过程可参见潘泉等的文章《一类非线性滤波器——ukf综述》，算法的主要步骤如下：
s301、初始点设置及相应的采样点选取：
初始状态的均值x0和协方差pxx，对称采样点为：
其中，
其中
s302、预测下一时刻点；
将状态模型(9)和测量模型(7)的非线性变换应用于k时刻采样的每个采样点，得到非线性转换后的点集χi(k+1|k)，进而利用转换后的点集计算其均值和协方差用于估计：
状态采样点下一时刻的状态预测:
χi(k+1|k)＝f[χi(k|k)](13)
相应的状态预测协方差矩阵计算:
状态采样点下一时刻测量预测:
hmi(k+1|k)＝h[χi(k+1|k)](16)
相应的测量预测协方差矩阵以及测量与状态之间的协方差矩阵计算:
s303、更新预测值：
利用测量更新状态预测(14)的增益矩阵计算：
状态更新：
状态更新之后相应的状态协方差矩阵计算：
pxx(k+1|k+1)＝p(k+1|k)-k(k+1)pyy(k+1|k)kt(k+1)(22)
本发明的有益效果是：
在超高速飞行器高动态飞行过程中，由于受发动机推力等作用的影响，初始的动力学模型与实际运动情况不符，同时，高动态飞行时可能出现无线电导航信息间断、加速度计信息不可靠或者无可用的加速度计信息时，仍能保证飞行器自主导航系统稳定运行，具体体现为：
1)考虑到自主导航系统采样频率较高(不低于1hz)，故将不确定的动力学模型误差补偿项转变为采样间隔内的确定待估项，并将其扩展至状态变量中一并估计，从而补偿了动力学模型误差，提高了s302中状态预测的精度；
2)结合地磁敏感器生产工艺水平、材料特性、环境干扰等方面因素，充分考虑地磁敏感器测量系统误差的影响构建了地磁敏感器测量模型，并将地磁敏感器的测量系统误差扩展至状态变量中一并估计，从而补偿了测量模型误差，提高s303中最终的状态更新精度；
3)从简化模型与计算角度出发，采用磁场强度的一维长度幅值作为地磁敏感器的观测值，通过基于模型误差补偿的组合导航方式较为准确的进行了状态估计并修正了其测量系统误差，使得在无加速度计以及gnss信号时，仍能保证超高速飞行器自主导航系统稳定运行。
附图说明
图1为实施例中在仿真条件下位置实时估计误差结果图
图2为实施例中在仿真条件下速度实时估计误差结果图
图3为实施例中在仿真条件下动力学模型误差补偿项估计结果图
图4为实施例中在仿真条件下地磁敏感器系统偏差的估计误差结果图
具体实施方式
下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。
给出仿真参数如下：
1)地磁敏感器的灵敏度误差矩阵为：
2)gnss定位误差为10米，速度误差为0.1米每秒；
3)超高速飞行的目标仿真为航程约1万公里，最大速度约6000m/s的超高速飞行器；
4)初始位置误差为：[1km,1km,1km]，初始速度误差为：[5m/s,5m/s,5m/s]；
5)敏感器采样频率1hz；
s1、考虑地磁敏感器测量误差，建立自主导航系统测量模型：
s101、建立地磁敏感器测量模型；
仿真计算时令公式(2)中n＝1，re＝6371000，根据仿真的飞行器各个时刻真实轨迹数据计算出相应的(r,θ,φ)，则地磁场强度矢量b的真实值由下式确定:
s102、地磁敏感器测量系统误差分析
根据仿真条件，地磁敏感器测量误灵敏度误差矩阵为：
s103、构建基于地磁敏感器的自主导航系统测量模型；
为方便计算，采用地磁场矢量的长度
s2、带动力学模型误差补偿估计的自主导航系统动力学模型构建：
超高速飞行器的动力学模型形式为公式(8)，设置待估计的状态变量为位置、速度、动力学模型误差补偿项以及地磁敏感器测量模型误差补偿项，那么，状态方程为公式(9)，其中的μ＝398600441500000；
s3、基于无迹滤波(unscentedkalmanfilter,ukf)的自主导航算法：
地磁敏感器系统误差的初始估计值取为：[0，0，0]；
初始位置误差为：[1km,1km,1km]，初始速度误差为：[5m/s,5m/s,5m/s]；
待估计的状态变量的维数nd＝10，κ＝3-nd，α＝1，β＝2，λ＝-9.97，γ＝0.173，进而按照ukf滤波计算流程进行各个时刻状态估计；
基于建立的系统模型(7)和(9)，利用地磁/惯导/gnss组合测量手段，当第100至160秒无gnss的位置速度测量数据以及加速度计信息时，仅利用地磁测量进行导航，利用ukf自主导航算法进行实时估计，实时的估计结果如图1、图2所示，动力学模型中的动力学模型误差补偿项的估计如图3所示，地磁敏感器的系统偏差估计如图4；
根据上述实验结果可见，对于超高速飞行的目标而言，其速度快、机动性强，高动态飞行可能导致gnss信息闪断以及加速度计信息不可靠或者无加速度计信息的情况，因此采用基于动力学修正模型进行状态预测，同时利用磁强计和gnss测量(有信息时使用，无信息则不使用)修正预测值，可以得到较好的估计效果。根据仿真结果可见，在初始状态误差较大时，导航算法可以快速收敛，导航定位精度优于2米，速度估计精度约0.05米每秒，导航精度优于单个敏感器的测量精度，同时能够较好地估计出地磁敏感器的系统误差，以及动力学修正模型中的动力学模型误差补偿项。并且根据动力学模型误差补偿的估计结果图3可见，从飞行器进入自由段(发动机关机之后)开始除引力之外的模型误差补偿项趋于零，符合实际情况，说明估计结果具有合理性。