本发明涉及一种基于动力学模型辅助的卫星完好性监测方法，属于组合导航、故障诊断领域。
背景技术：
卫星导航系统具有全天候高精度测速、授时及定位能力，目前已被广泛应用与解决空中、陆地和海上运载体的定位与导航问题中，并成为一种主要导航设备，但由于受到卫星星历、星钟误差、电离层和对流层延迟、多路径和接收机热噪声等因素的影响，卫星导航系统的定位误差可能会超出性能需求所容许的范围，载体对该误差超限又存在盲视问题，即定位完好性问题。
目前主要通过完好性监测来保障卫星导航系统的完好性，常见的完好性监测方法分为：外部监测方法和内部监测方法。外部监测方法是在地面或空间中的地球同步卫星上设置完好性监测站，将卫星的完好性信息通过通信链路发送给用户，另外还可以通过差分定位的方法实现完好性的监测，但此类方法需要建立地面站或者同步卫星辅助加以实现，使用成本较高；内部监测方法是指不需要外部设备提供信息，仅通过载体内部设备实现卫星故障的检测与隔离。它包括接收机自主完好性监测和机载自主完好性监测两类方法，接收机自主完好性监测方法利用卫星冗余信息对测量值进行一致性检验，从而实现完好性监测；机载自主完好性监测方法借助飞机上的其它辅助信息(如气压高度表、惯导等)，实现卫星故障检测和故障识别。
技术实现要素：
本发明所要解决的技术问题是：卫星导航系统在使用过程中存在完好性不足的问题，采用外部监测方法成本较高，且通信已受干扰或存在信号未覆盖区域；内部监测方法中接收机自主完好性监测方法可用性上受可见卫星数目限制。针对上述技术不足之处，提出了一种基于动力学模型辅助的卫星完好性监测方法，在惯性/卫星组合导航系统基础之上，通过构建载体的动力学模型，将动力学模型作为一个虚拟的传感器，采用多模型滤波框架实现惯性/动力学模型/卫星导航系统信息的融合和状态估计，并采用多解分离方法实现卫星故障的检测与隔离，从而提高卫星完好性监测性能。
本发明为解决其技术问题采用如下技术方案：
一种基于动力学模型辅助的卫星完好性监测方法，包括以下步骤：
第一步：构建运动载体的动力学模型，实现载体的姿态、速度和位置求解；
第二步：采集惯性器件数据，根据惯性导航系统解算模型，求解得到惯性导航系统输出的姿态、速度和位置；
第三步：根据第一步和第二步描述的不同系统模型，选取载体的姿态、速度和位置误差以及惯性器件误差作为状态量，分别获取基于动力学模型和惯性导航系统的状态方程，并根据卫星导航系统定位原理建立量测方程，采用多模型滤波框架实现惯性/动力学模型/卫星导航系统的信息融合与状态估计；
第四步：在第三步的基础上，采用多解分离方法通过构建N+1个，N为可见卫星数，不同的滤波器实现卫星故障的检测与隔离，并计算保护级别指标，完成对卫星导航系统的完好性监测。
本发明的有益效果如下：
(1)本发明通过建立载体的动力学模型，采用多模型滤波框架实现惯性/动力学模型/卫星导航系统的信息融合和状态估计，基于多解分离法对卫星故障进行检测和隔离，有效综合利用载体内部信息，提升了卫星完好性监测性能；
(2)本发明中不需要增加额外设备，具有使用成本低、自主性强等特点，是一种较好的内部辅助完好性监测方法。
附图说明
图1为一种基于动力学模型辅助的卫星完好性监测方法结构框图。
图2是机体系下载体角速度及力矩组成示意图。
具体实施方式
下面结合附图对本发明创造做进一步详细说明。
本发明方法的结构框图如图1所示，一种基于动力学模型辅助的卫星完好性监测方法，其通过建立运动载体的动力学模型，并综合利用搭载的其他导航传感器，采用多模型滤波框架和多解分离法，实现惯性/动力学模型/卫星导航系统信息的最优融合及卫星导航系统的故障检测与隔离，从而保障卫星导航系统完好性，并提高组合导航系统精度和可靠性。其具体步骤如下：
第一步：首先建立载体的动力学模型，载体的动力学模型包含一系列非线性、耦合、常微分方程，可以用于估计作用于载体的空气动力与力矩。方程中包含空气动力系数、可量测的控制变量(副翼、方向舵和升降舵)及载体状态信息。根据空气动力学原理，可以获取载体的加速度和角速度。图2为机体系下载体角速度及力矩组成示意图，图中oxbybzb为载体的机体坐标系，原点为载体重心，p,q,r分别为载体滚转角速度、俯仰角速度和偏航角速度，u,v,w为载体速度在机体坐标轴上的分量，X,Y,Z为载体所受空气动力，L,M,N分别为气动滚转力矩、气动俯仰力矩和气动偏航力矩。
载体的力方程组如下：
载体的力矩方程组如下：
载体姿态角方程如下：
载体的位置方程如下：
其中，φ,θ,ψ分别为载体的横滚角、俯仰角和航向角，g为重力加速度，m为载体的重量，c1～c9为惯性系数，xg,yg,h为载体在地理系中的位置信息。根据上述方程可以获取载体的位置、速度和姿态。
第二步：采集惯性器件数据，解算惯性导航系统输出。陀螺和加速度计的输出分别为根据陀螺的输出采用欧拉角法求解载体姿态，是机体系相对于惯性空间的角速率在机体系上的投影。将其转换为即机体系相对地理系的角速率在机体系上的投影。根据空间几何关系有：
上式中，是地球角速率在地理坐标系上的投影，是由于载体在地球圆表面运动后的角速率，是姿态转移矩阵，为地理系相对于惯性空间的角速率在机体系上的投影。
欧拉角微分方程如下
根据上式可求得载体的横滚角φ、俯仰角θ和航向角ψ，其中分别为φ,θ,ψ的导数；为机体系相对地理系的角速率在机体系x轴上的投影；为机体系相对地理系的角速率在机体系y轴上的投影；为机体系相对地理系的角速率在机体系z轴上的投影。求解姿态角后，获得更新后的姿态转移矩阵比力的转换关系为：
其中：是姿态转移矩阵，为机体系相对于惯性空间的比力在地理系上的投影。
由比力方程可得：
其中：为地理系相对于地球坐标系的速度在地理系上的投影；为的导数；gn为重力加速度矢量在地理系上的投影。
可得载体在地理系中的速度微分方程为：
式中，L是当地的纬度，Rn为地球子午面内的曲率半径，Rm为垂直于子午面的法线平面内的曲率半径，Ve为载体地理系下的东向速度；Vn为载体地理系下的北向速度；Vd为载体地理系下的地向速度；分别为Ve,Vn,Vd的导数；为机体系相对于惯性空间的比力在地理系x轴上的投影；为机体系相对于惯性空间的比力在地理系y轴上的投影；为机体系相对于惯性空间的比力在地理系z轴上的投影；wie为地球自转角速率；由上式可求得载体在地理坐标系中的东向、北向和地向速度Ve,Vn,Vd。
由于载体在地球表面运动，因此定位计算时必须考虑地球曲率的影响，以经纬度和高度作为定位的物理量，由以下微分方程可求得载体的实时位置：
式中，L,λ,h分别为载体的经度、纬度和高度；分别为L,λ,h的导数。根据上述公式可求解惯性导航系统输出的位置、速度和姿态信息。
第三步：选取动力学模型及惯性导航系统的状态变量，采用多模型滤波框架实现惯性/动力学模型/卫星导航系统的信息融合及状态估计。
惯性导航系统采用19维误差状态变量，定义如下：
式中，XI为惯性导航系统的状态量；φe,φn,φu为平台误差角；δve,δvn,δvu为速度误差；δL,δλ,δh为纬度、经度、高度误差；εbx,εby,εbz,εrx,εry,εrz分别为陀螺随机常值漂移和一阶马尔可夫过程；为加速度计零偏的一阶马尔可夫过程，Δt为卫星接收机钟差。
同理，动力系模型的状态变量定义如下：
XD＝[φe,φn,φu,δve,δvn,δvu,δL,δλ,δh,Δt](12)
其中：XD为动力系模型的状态量；惯性/卫星导航系统及动力学模型/卫星导航系统均采用伪距组合模式，卫星导航系统的观测伪距为ρG，根据惯性导航系统及动力学模型解算的载体位置信息与卫星位置计算得到的近似伪距分别为ρI,ρD，选取近似伪距与观测伪距之间的差值作为观测量，如下：
ZI＝ρI-ρG(13)
ZD＝ρD-ρG(14)
其中：ZI为惯性导航系统的观测量；ZD为动力系模型的观测量；根据选定的状态变量和观测量，建立组合导航系统的状态方程和量测方程，采用多模型滤波框架中仅对惯性/卫星组合导航系统以及动力学模型/卫星组合导航系统的公共状态进行融合和估计。其步骤如下：
(1)输入交互
通过输入交互计算得到初始的状态估计和协方差估计，作为各自滤波器的输入值，包括状态交互和状态协方差交互。
其中，分别表示k时刻第i(i＝1,2)个模型的状态估计和状态协方差估计，其中i＝1表示为惯性/卫星组合导航系统，i＝2表示为动力学模型/卫星组合导航系统；表示k+1时刻第j(j＝1,2)个模型的滤波初值，表示模型i转移到模型j的预测模型转移概率，如式(17)所示：
其中，表示k时刻第j(j＝1,2)个模型的模型概率。πij表示模型i转移到模型j的转移概率。
(2)状态滤波
采用卡尔曼滤波分别对惯性/卫星组合导航系统以及动力学模型/卫星组合导航系统进行数据融合，得到各自的状态估计及协方差估计，其新息和新息协方差矩阵如下：
其中，表示观测量，表示一步预测状态估计，表示一步预测方差阵，表示量测矩阵，表示量测噪声方差阵。
(3)模型概率更新
通过状态滤波后，计算得到第j个滤波器的新息和新息协方差信息，结合预测模型转移概率得到当前时刻模型概率的估计。模型的似然概率如下：
其中：为第j个滤波器的新息；为新息协方差；N表示正态分布；
由上一时刻模型概率、似然概率以及概率转移矩阵得到当前时刻各模型的概率为：
其中：表示k时刻第i(i＝1,2)个模型的模型概率；
(4)状态输出
以当前时刻的模型概率为加权值将各个模型的状态估计和协方差估计进行融合得到当前时刻的状态估计和协方差估计的输出结果。
其中，表示k+1时刻全局状态估计和状态协方差估计，表示k+1时刻第j(j＝1,2)个滤波器的状态估计；表示k+1时刻第j(j＝1,2)个滤波器的状态估计协方差阵；表示k+1时刻第j(j＝1,2)个模型的模型概率；通过上述多模型滤波过程，可以有效融合惯性/多模型/卫星导航系统信息，获取精度更高的载体误差状态估计。
第四步：在第三步基础上，构建多个并行滤波器，包括一个主滤波器和N个子滤波器，其中N为k时刻可见卫星数，主滤波器中使用所有卫星观测信息，子滤波中对应剔除一个可见卫星量测，使用N-1个卫星观测伪距，采用多解分离法进行卫星故障检测和隔离。
由主滤波器得到的载体状态估计定义为X0，其协方差为P0，由子滤波器得到的载体状态估计定义为Xi(i＝1,2,...,N)，其协方差为Pi(i＝1,2,...,N)，
主滤波器与子滤波器状态估计之间差值的协方差矩阵dPi的定义和计算公式如下(假设主子滤波器之间估计相互独立)：
其中：E表示数学期望；X0表示主滤波输出的状态估计，P0为其对应的协方差阵；Xi表示第i(i＝1,2,...,N)个子滤波输出的状态估计，；Pi为其对应的协方差阵；
构建统计检验量di,(i＝1,2...,N)如下：
di＝max(|X0(7)-Xi(7)|,|X0(8)-Xi(8)|),(i＝1,2...,N)
(25)
其中：X0(7)表示X0中的第7个元素，Xi(7)表示Xi中的第7个元素，对应经度误差；X0(8)表示X0中的第8个元素，Xi(8)表示Xi中的第8个元素，对应纬度误差；
给定误警率Pfa，对应每个子滤波器的统计检验量的检测阈值Ti,(i＝1,2...,N)为：
式中，为对应dPi水平位置方向上的最大特征值，可由dPi(7,7),dPi(7,8),dPi(8,8)计算得到，F-1是余误差函数的反函数，其表达式如下：
则根据N组检验量进行故障检测，其故障判据为：
1)无故障H0：所有检验量均满足di≤Ti；
2)有故障H1：至少存在一组检验量为di＞Ti。
在检测出故障后，对于故障定位识别，需要子滤波器及其次级滤波器(即在子滤波器观测信息中再排除一个卫星观测)，采用上述相同方法进行计算，判定第i颗星故障的方法是：根据排除第i颗可见星后的子滤波器观测信息，采用多解分离方法计算得到的统计检验量均小于检测阈值。在识别故障卫星后，对故障卫星观测进行隔离，采用健康的观测量进行滤波融合。为了完成载体的完好性监测，需要对载体的水平保护级别(HorizontalProtectionLevel，HPL)和垂直保护级别(VerticalProtectionLevel，VPL)进行计算。
对应每个子估计的HPLi由两部分组成：一是子滤波器估计Xi与主滤波器估计X0差值的门限，即由误警率Pfa计算得到的检测门限Ti,(i＝1,2...,N)；二是子滤波器估计自身的水平位置误差门限ai,(i＝1,2...,N)，即：
HPLi＝Ti+ai,(i＝1,2...,N)(28)
给定漏警率Pmd，则水平位置误差门限ai,(i＝1,2...,N)的计算公式如下：
式中，为对应Pi水平位置方向上的最大特征值，可由Pi(7,7),Pi(7,8),Pi(8,8)计算得到。
综上所述，多解分离法的水平保护门限计算方法为：
HPL＝max(HPLi)＝max(Ti+ai),(i＝1,2...,N)(30)
同理，可以得到垂直保护级别的计算方法如下：
VPL＝max(VPLi)＝max(Di+bi),(i＝1,2...,N)(21)
其中，Di,(i＝1,2...,N)为垂直方向检测阈值，bi,(i＝1,2...,N)为垂直位置误差门限：
综上所述，即可实现基于动力学模型辅助的卫星完好性监测。