本发明涉及一种高精度实时飞行导航计算方法，属于控制导航计算和姿态测量技术领域。
背景技术：
gnss(全球卫星导航系统)可以实现位置、速度与时间的测量，但在飞行器工作电磁环境复杂或卫星信号受影响的应用场景，无法稳定提供正确的导航信息；ins(惯性导航系统)能够实现位置、速度与姿态的测量，但作为一种积分解算的导航方式，存在误差随时间积累的固有缺陷；且对于自旋飞行器的复杂姿态运动，会导致ins的传感器性能降低。
技术实现要素：
本发明的技术解决问题是：克服现有技术的不足，本发明为实时精确导航计算，提出了一种高精度实时飞行导航计算方法，给出利用gnss(全球卫星导航系统)输出的位置、速度信息和ins(惯性导航系统)输出的速度、加速度信息进行组合导航计算的方案。
本发明的技术方案是：
一种高精度实时飞行导航计算方法，包括步骤如下：
步骤1.1、获得初始时刻t0的再入飞行器姿态初值，包括：初始俯仰角
步骤1.2、确定飞行器位置初始值为装订数据，装订数据包括：初始纬度l0、初始经度λ0、初始高度值h0；速度初始值vn,0,vu,0,ve,0；
步骤1.3、由步骤1.1得到的姿态初值，确定姿态四元数q(t0)和弹体坐标系到导航坐标系的姿态变换矩阵姿态转换矩阵
步骤1.4、使用ins惯性导航系统输出的速度和加速度信息，及姿态初始值、速度初始值和位置初始值，按导航解算周期执行导航解算，得到再入飞行器的姿态四元数q(t1)＝[q1,0q1,1q1,2q1,3]t、速度vn,k,vu,k,ve,k和位置信息纬度lk、经度λk、高度值hk；
步骤1.5、使用惯导外推的方法，用接收机数据修正导航解算结果，获得与接收机一致的再入飞行器的姿态四元数、速度和位置信息纬度、经度、高度值，作为导航解算的结果(lk,λk,hk,vn,k,vu,k,ve,k,qk,0,qk,1,qk,2,qk,3)；
步骤1.6、结合pps脉冲同步信号状态判断gnss导航信号的可信度，选择使用gnss提供的位置、速度测量数据，并根据pps脉冲同步信号提供的gnss数据延迟时间对gnss接收机的位置、速度信息进行延时补偿，获得经延时补偿过的gnss接收机的位置补偿值、速度信息补偿值；
步骤1.7、利用步骤1.5导航解算的结果(lk,λk,hk,vn,k,vu,k,ve,k,qk,0,qk,1,qk,2,qk,3)与步骤1.6经延时补偿过的gnss接收机的位置(lgk、λgk、hgk)、速度信息(vgn,k、vgu,k、vge,k)补偿值进行组合导航滤波，得到滤波后飞行器的的位置、速度和姿态信息作为组合导航结果；
步骤1.8、将步骤1.5所述的惯导外推结果和步骤1.7的组合导航结果分别反馈至导航解算模块。
一种双cpu运行平台，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如权利要求1所述方法的步骤，具体为：
k时刻接收到gnss数据后，首先依据步骤1.5使用导航解算数据进行惯导外推修正gnss数据，并用gnss数据替换k-1时刻sins导航解算结果，进行k时刻导航解算；同时，将惯导外推结果传递给组合导航卡尔曼滤波；
对k-1时刻的sins导航解算结果做线性外推处理；
k时刻组合导航卡尔曼滤波接收到惯导外推数据后，使用k-1时刻sins导航解算结果对k时刻的线性外推结果，使用步骤1.7所述方法计算组合导航卡尔曼滤波结果，并将计算结果反馈给k 1时刻sins导航解算模块。
本发明与现有技术相比的有益效果是：
1)本发明使用惯导外推的方法，用gnss数据弥补ins(惯性导航系统)作为一种积分解算的导航方式，存在误差随时间积累的固有缺陷；
2)本发明结合pps信号状态判断gnss(全球卫星导航系统)信号的可信度，选择使用gnss的位置、速度测量数据，在飞行器飞行过程中，能够稳定提供正确的导航信息的情况下，尽量使用接收机位置、速度测量结果进行组合导航解算；
3)本发明通过对双cpu系统的使用及计算模块的合理分配，实现快速组合导航计算，满足系统性能要求。
附图说明
图1为gnss数据补偿方法。
图2导航计算流程框图。
图3双cpu导航计算方案。
图4双cpu导航高速计算方案。
具体实施方式
为实现飞行器的高精确、实时控制导航或姿态角测量，需要对gnss(全球卫星导航系统)输出的位置、速度信息和ins(惯性导航系统)输出的速度、加速度信息进行综合处理，通过组合导航计算，实现两系统的优势互补，实现连续的、全天候的、高精度的导航，进而在精度和可靠性方面可以获得比单独使用任何一种导航设备都优良的性能。
为实现实时组合导航计算，设计了一种高精度实时处理系统，充分利用双cpu资源，可实现严苛的实时导航计算性能要求，实现精确组合导航的目的。
下面结合附图和具体实施方式对本发明做进一步详细的描述。本发明一种高精度实时导航计算方法，包括如下步骤：
步骤1、组合导航计算方法设计
步骤1.1、获得初始时刻t0的再入飞行器姿态初值，初始俯仰角
已知，ins(惯性导航系统)每2ms输出一帧飞行器角速度(ωx，k、ωy，k、ωz，k)和加速度(nx，k、ny，k、nz，k)信息，本发明在1分钟内积累250帧ins数据，对其中的加速度信息求取平均值，得到加速度平均值
使用加速度平均值计算得到飞行器的初始姿态，计算方法如下：
ψ0＝0
步骤1.2、确定位置初始值为装订的初始纬度l0、初始经度λ0、初始高度值h0，速度初始值为vn,0＝0，vu,0＝0，ve,0＝0。
步骤1.3、由步骤1得到的姿态初值，确定初始姿态的其他表示形式，包括姿态四元数q(t0)和姿态转换矩阵
姿态转换矩阵与姿态初始值关系如下：
使用姿态转换阵表示姿态四元数如下：
q(t0)＝[q0,0q0,1q0,2q0,3]t，其中
步骤1.4、使用ins(惯性导航系统)输出的速度和加速度信息，及姿态、速度和位置初始值，每2ms执行一次导航解算，得到再入飞行器的姿态q(tk)＝[qk，0，qk，1，qk，2，qk，3]t、速度vn，k,vu，k,ve，k和位置信息纬度lk、经度λk、高度值hk。
步骤1.4.1、计算姿态四元数q(tk)
q(tk)＝[qk,0，qk,1，qk,2，qk,3]t；
式中，
而
其中
其中
re＝6378137.0m为地球半长轴，
以上所述计算方法中，k为本解算周期，k-1为上个计算周期，第一个解算周期使用初始值，ts为解算周期时长，在本发明系统中为2ms。
步骤1.4.2、速度计算
速度的微分方程解算为：
其中，
北向、天向、东向速度；
g0＝9.78049m/s2为地表重力加速度。
记速度累计增量
步骤1.4.3、位置计算
位置的微分方程解算为：
其中，lk-1、λk-1、hk-1为k-1时刻解算得到的纬度、精度、高度数据；vn,k、vu,k、ve,k为k时刻解算得到的地理坐标系下北向、天向、东向速度；ts为导航解算周期。
步骤1.5、使用惯导外推的方法，用ins信息对gnss接收机数据修正
由于接收机计算使得gnss数据存在延时，为实现高精度实时导航计算，需要对延时进行补偿。飞行器在飞行过程中没有较大的机动，采用简单的方法即可收到较好的效果，即直接利用当前最近时刻的ins信息对接收到的gnss信息直接进行外推延时补偿。如图1所示。
pps信号为硬件同步信号，可以通过对pps信号的采集得到gnss数据延时具体时间
gnss数据补偿如下：
其中：
步骤1.6、结合pps信号状态判断gnss(全球卫星导航系统)信号的可信度，选择使用gnss的位置、速度测量数据；
由于飞行器工作电磁环境复杂或卫星信号受影响的应用场景，可能无法稳定提供正确的导航信息，因此为了实现高精度导航计算，需要根据pps信号状态和gnss数据信息进行如下判断，根据gnss信号特征确定gdop门限值g0，及连续可靠工作周期gt：
·gnss定位且gdop<g0时，如果
①pps信号到来，进行惯导外推和组合导航处理；
②pps信号未到来
a)若距离上次pps信号到来时刻大于gt，不使用δvfk，δpfk进行延时补偿，直接使用gnss信息进行惯性导航外推解算、组合导航解算；
b)若距离上次pps信号到来时刻小于gt，则不使用gnss信息进行惯性导航外推解算、组合导航解算；
·gnss未定位、超限或无gnss数据时
不使用gnss信息进行惯性导航外推解算、组合导航解算。
步骤1.7、利用导航解算的结果(lk,λk,hk,vn,k,vu,k,ve,k,qk,0,qk,1,qk,2,qk,3)与经延时补偿过的gnss接收机的位置(lgk、λgk、hgk)、速度信息(vgn,k、vgu,k、vge,k)补偿值进行组合导航滤波，得到滤波后的位置、速度和姿态信息。
1.7.1、卡尔曼滤波初始化
组合导航采用卡尔曼滤波。组合导航选取15阶状态变量，分别是3个位置误差变量、3个速度误差变量、3个姿态误差变量、3个陀螺漂移变量、3个加速度计零偏变量，即：
观测量选取6阶变量，分别为3个位置误差、3个速度误差，即：
组合导航基本方程的状态方程如下：
量测方程为：
zk＝hk·xk vk
状态方程和量测方程中，wk为系统激励噪声序列，vk为量测噪声序列，同时wk和vk满足以下条件：e[wk]＝0，cov[wk,wj]＝e[wkwjt]＝qkδkj；e[vk]＝0，
cov[vk,vj]＝e[vkvjt]＝rkδkj；cov[wk,vj]＝e[wkvjt]＝0。qk为系统噪声序列的方差阵，为非负定阵；rk为量测噪声序列的方差阵，为正定阵。由于噪声驱动矩阵γk-1只跟姿态
②计算
根据k时刻导航解算结果计算。
其中，ωie＝7.292115×10-5rad/s为地球自转角速度，re＝6378137.0m为地球半长轴。
③初始值
h、l0来自于装订数据，计算rm、rn值使用的l也为装订值l0
g0＝9.80665
·系统状态变量初值：
x0＝015×1
·系统状态估计误差的均方误差初值：
diag代表对角矩阵，除对角线外其余元素为0。p0中参数的单位如下：
diag[mmmm/sm/sm/sradradradrad/srad/srad/sm/s2m/s2m/s2]2
·系统噪声序列的方差阵初值：
q0中参数的单位如下：
diag[mmmm/sm/sm/sradradradrad/srad/srad/sm/s2m/s2m/s2]2
·量测噪声序列的方差阵初值：
r0中参数的单位如下：diag[mmmm/sm/sm/s]2
注：p0、q0为15×15的对角矩阵；r0为6×6的对角矩阵。p0、q0、r0初值的选取与惯组、接收机的指标相关，不同的设备初值选取不同，需经过多次试验取经验值。
1.7.2、卡尔曼滤波处理
第k滤波周期时，状态量的选取见xk，量测量计算见zk。
用于求解误差微分方程的一步转移矩阵为ak,k-1，其中a为15*15维矩阵，非零项计算公式如下，其中vx，vy，vz分别为vnf,k，vuf,k，vef,k：
ak,k-1(3,5)＝1
ak,k-1(4,8)＝-fe
ak,k-1(4,9)＝fu
ak,k-1(5,1)＝-2ωievzsin(lk)
ak,k-1(5,7)＝fe
ak,k-1(5,9)＝-fn
ak,k-1(6,7)＝-fu
ak,k-1(6,8)＝fn
ak,k-1(7,1)＝-ωiesin(lk)
ak,k-1(8,7)＝-ak,k-1(7,8)
ak,k-1(8,9)＝-ak,k-1(9,8)
在滤波更新时，需要将ak,k-1进行离散化，得到fk,k-1；将wk离散化得到qk。离散化过程如下：
其中q0为状态误差初值，ts1为滤波周期(即gnss数据更新时间)100ms。
求解一步预测均方误差矩阵：
根据状态方程，系统一步预测均方误差方程如下：
滤波增益方程：
kk＝pk/k-1hkt(hkpk/k-1hkt rk)-1
状态一步预测方程：
状态最优估计方程：
估计均方误差方程：
利用xk对滤波结果进行修正：
步骤1.8、惯导外推结果和组合导航结果分别反馈至导航解算模块
经过k时刻ins导航解算、惯导外推和组合导航计算，需分别将惯导外推和组合导航结果反馈至ins导航解算，以弥补ins(惯性导航系统)作为一种积分解算的导航方式，误差随时间积累的固有缺陷。
结合步骤1.1～1.7的描述，高精度导航计算方法流程框图如图2所示。
步骤2、确定组合导航算法的实时计算方法
方法1：算法模块串行工作
利用高速处理器，在接收到ins导航数据和gnss信息后，依次执行sins导航计算、惯导外推以及sins导航计算、组合导航卡尔曼滤波。
在本发明工作的典型工作场景中，sins导航解算0.5ms，gnss对sins导航外推用时小于0.01ms，可忽略，组合导航卡尔曼滤波1.6ms，由于图2中两次导航解算输入不同，需独立运行。因此，共耗时约0.5*2 1.6＝2.6ms，考虑80％时间余量，可满足组合导航计算周期大于3.25ms的系统实时性能要求。
方法2：利用双cpu运行平台
在本发明工作的典型场景中，有双片cpu可独立同时运行，使用双片cpu同时运行，构造工作流程如图3所示。
这种情况下，导航计算用时主要集中在cpu1，总用时约0.5 1.6＝2.1ms，考虑80％时间余量，可满足组合导航计算周期大于2.625ms的系统实时性能要求。
方法3：并行工作 数据补偿
但在本发明的典型工作场景中，系统的实时性性能要求为2ms内完成所有组合导航的计算。
依据上述时间测量，组合导航卡尔曼滤波计算为主要用时单元，但结合处理资源分析，双cpu在2ms的自然计时时间中，共有4ms的处理资源，算上80％的时间余量，可用处理资源为4×0.8＝3.2ms，由于3.2ms>2.625ms，因此原理上，双cpu可支撑实时导航计算。
依据各模块处理时间，对双cpu计算分工及处理流程进行重新分配如图4所示。
步骤2.1、k时刻接收到gnss数据后，首先依据步骤1.5的说明使用导航解算数据进行惯导外推修正gnss数据，并用gnss数据替换k-1时刻sins导航解算结果，进行k时刻导航解算。同时，将惯导外推结果传递给组合导航卡尔曼滤波；
步骤2.2、对k-1时刻sins导航解算结果做线性外推处理。因2ms时间较短，导航计算变化不大，可使用线性外推获得与实际导航解算较为相近值，使得整个系统在满足实时性的基础上，最大程度保证高精度；
步骤2.3、k时刻组合导航卡尔曼滤波接收到惯导外推数据后，使用k-1时刻sins导航解算结果对k时刻的线性外推结果，使用步骤1.7所述方法计算组合导航卡尔曼滤波结果，并将计算结果反馈给k 1时刻sins导航解算模块。
为叙述简便，本发明设定ins数据更新周期和导航解算周期皆为2ms，如果目标处理器运算能力强，或系统性能要求较为宽松时，可使用本发明所述方法1、2实现高精度实时导航计算；如果目标处理器运算能力一般，系统性能要求较为苛刻，在资源运行情况下，可使用本发明所述方法3实现高精度实时导航计算。
本发明说明书中未作详细描述的内容属本领域专业技术人员的公知技术。
1.一种高精度实时飞行导航计算方法，其特征在于，包括步骤如下：
步骤1.1、获得初始时刻t0的再入飞行器姿态初值，包括：初始俯仰角
步骤1.2、确定飞行器位置初始值为装订数据，装订数据包括：初始纬度l0、初始经度λ0、初始高度值h0；速度初始值vn,0,vu,0,ve,0；
步骤1.3、由步骤1.1得到的姿态初值，确定姿态四元数q(t0)和弹体坐标系到导航坐标系的姿态变换矩阵姿态转换矩阵
步骤1.4、使用ins惯性导航系统输出的速度和加速度信息，及姿态初始值、速度初始值和位置初始值，按导航解算周期执行导航解算，得到再入飞行器的姿态四元数q(t1)＝[q1,0q1,1q1,2q1,3]t、速度vn,k,vu,k,ve,k和位置信息纬度lk、经度λk、高度值hk；
步骤1.5、使用惯导外推的方法，用接收机数据修正导航解算结果，获得与接收机一致的再入飞行器的姿态四元数、速度和位置信息纬度、经度、高度值，作为导航解算的结果(lk,λk,hk,vn,k,vu,k,ve,k,qk,0,qk,1,qk,2,qk,3)；
步骤1.6、结合pps脉冲同步信号状态判断gnss导航信号的可信度，选择使用gnss提供的位置、速度测量数据，并根据pps脉冲同步信号提供的gnss数据延迟时间对gnss接收机的位置、速度信息进行延时补偿，获得经延时补偿过的gnss接收机的位置补偿值、速度信息补偿值；
步骤1.7、利用步骤1.5导航解算的结果(lk,λk,hk,vn,k,vu,k,ve,k,qk,0,qk,1,qk,2,qk,3)与步骤1.6经延时补偿过的gnss接收机的位置(lgk、λgk、hgk)、速度信息(vgn,k、vgu,k、vge,k)补偿值进行组合导航滤波，得到滤波后飞行器的的位置、速度和姿态信息作为组合导航结果；
步骤1.8、将步骤1.5所述的惯导外推结果和步骤1.7的组合导航结果分别反馈至导航解算模块。
2.一种双cpu运行平台，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于：所述处理器执行所述计算机程序时实现如权利要求1所述方法的步骤，具体为：
k时刻接收到gnss数据后，首先依据步骤1.5使用导航解算数据进行惯导外推修正gnss数据，并用gnss数据替换k-1时刻sins导航解算结果，进行k时刻导航解算；同时，将惯导外推结果传递给组合导航卡尔曼滤波；
对k-1时刻的sins导航解算结果做线性外推处理；
k时刻组合导航卡尔曼滤波接收到惯导外推数据后，使用k-1时刻sins导航解算结果对k时刻的线性外推结果，使用步骤1.7所述方法计算组合导航卡尔曼滤波结果，并将计算结果反馈给k 1时刻sins导航解算模块。
技术总结