CN104181572B - 一种弹载惯性/卫星紧组合导航方法 - Google Patents

Abstract

本发明公开了一种弹载惯性/卫星紧组合导航方法。该方法利用GNSS输出的伪距、伪距率信息与惯导解算相对卫星的伪距、伪距率差为量测量，进行滤波并根据滤波结果对当前系统校正，主要包含如下步骤：SINS初始化；SINS导航解算；卫星的高度角，方位角解算；导航卫星选择；导航星的伪距测量误差补偿；载体相对每颗导航星的伪距、伪距率解算；系统状态判别与导航策略的选择；系统状态方程的构建，系统量测方程的构建；进行滤波解算，并根据滤波结果，对由通讯延时引起的滞后误差，通过基于状态转移的误差补偿方法对系统校正。本发明方法可实现基于惯性/卫星的伪距、伪距率无缝组合导航，提高了导航精度和对复杂环境的适应性，应用前景广阔。

Description

一种弹载惯性/卫星紧组合导航方法

技术领域

本发明涉及组合导航技术领域，特别是一种弹载惯性/卫星紧组合导航方法。

背景技术

卫星/惯性组合导航系统结合卫星导航、惯性导航的优点，具有定位精度高，稳定性强等特点，因此在军事领域及民用领域都被广泛应用。组合导航的模式有很多种，主要分为松组合、紧组合、深组合三类：松组合方式直接利用全球卫星导航系统(GNSS,GlobalNavigation Satellite System)和捷联惯性导航系统(SINS,Strapdown InertialNavigation System)接收机输出的位置和速度信息进行组合；深组合方式的核心是利用卫星/惯性组合的导航结果辅助接收机的环路进行跟踪与捕获；紧组合方式利用GNSS接收机输出的伪距、伪距率信息和由SINS输出的位置与速度信息解算得到的伪距、伪距率信息进行组合。

目前我国在主战飞机上仍以松组合实验为主，但是载体在诸如高动态飞行、接收机信号遮挡等情况下，GNSS接收机接收到卫星数目很容易少于四颗，此时松组合系统将工作在纯惯导状态，导航精度随时间下降。

发明内容

本发明的目的在于提供一种高精度的弹载惯性/卫星紧组合导航方法，基于惯性/卫星的伪距、伪距率无缝组合导航，从而有效抑制导航精度的发散。

实现本发明目的的技术解决方案为：一种弹载惯性/卫星紧组合导航方法，包括以下步骤：

步骤1，SINS初始对准，初始化速度、位置；

步骤2，导航计算机分别接收GNSS数据和SINS数据；

步骤3，导航计算机进行SINS导航解算，得到载体的速度、位置、姿态信息；导航计算机判断GNSS是否发送完所有通道信息，若发送完则计算与各通道对应的卫星高度角、方位角信息；

步骤4，判断可见卫星个数，若可见卫星大于4颗，则通过分布式最佳精度因子选星法选出4颗可见卫星作为导航星；若可见卫星少于4颗，则将所有可见卫星选为导航星；

步骤5，对导航卫星的伪距测量误差进行补偿；根据导航卫星的速度、位置信息，以及SINS的速度、位置信息，确定载体相对每颗导航卫星的伪距、伪距率信息；

步骤6，对组合导航的系统状态进行判别，并根据GNSS、IMU的工作状态选择匹配的导航策略，构建系统状态方程，并根据可见卫星的个数构建系统量测方程；

步骤7，根据系统状态方程和系统量测方程，采用卡尔曼滤波信息融合法进行滤波，并根据滤波结果，对由通讯延时引起的滞后误差，通过基于状态转移的误差补偿方法对系统进行校正得到最终的导航结果。

本发明与现有技术相比，其显著优点是：(1)由于采用GNSS输出的原始伪距、伪距率信息，不存在滤波器串联，从而消除了量测输出的时间相关性；(2)紧组合滤波器在收星数少于四颗的情况下动态调整系统相关变量维数，使系统工作在组合导航状态，实现无缝导航；(3)采用分布式最佳精度因子选星算法，既避免了选星对捷联解算的影响，又提供了最佳导航卫星组合；(4)采用容错组合，在组合前判别系统状态并进行导航策略的选择，可使组合导航系统具备无人干预下的自主运行状态判别、自主故障诊断、自主误差修正能力，从而大大提高了组合导航系统的容错性能；(5)基于状态转移矩阵，将通讯延时滞后误差递推至当前时刻，对当前系统进行补偿，大大的提高了组合导航系统的导航精度。

附图说明

图1是本发明弹载惯性/卫星紧组合导航方法的流程图。

图2是本发明弹载惯性/卫星紧组合导航方法的选星运行时序示意图。

图3是本发明弹载惯性/卫星紧组合导航方法的容错组合导航流程图。

图4是本发明弹载惯性/卫星紧组合导航方法的系统维数变化示意图。

图5是本发明弹载惯性/卫星紧组合导航方法的滞后误差补偿方法原理图。

具体实施方式

下面结合附图及具体实施例对本发明作进一步详细说明。

结合图1，本发明弹载惯性/卫星紧组合导航方法，步骤如下：

步骤1，SINS初始对准，初始化速度、位置，具体如下：

采用动基座传递对准技术，实现姿态、位置、速度的快速，精确初始化。

步骤2，导航计算机分别接收GNSS数据和SINS数据，具体如下：

(2.1)导航计算机接收GNSS数据

在紧组合导航系统中，GNSS接收机输出多个卫星的信息，每颗卫星信息通过一个通道输出，每个通道的信息包含：通道号，卫星编号，卫星工作状态，世界标准时间，接收机地心地固直角坐标系下的X、Y、Z轴位置和速度，伪距、伪距率量测值，卫星在地心地固直角坐标系下的X、Y、Z轴位置和速度；每个通道信息更新频率为1HZ，导航计算机依次接收、存储每个卫星对应的通道信息；

(2.2)导航计算机接收SINS数据

惯性测量单元(IMU，Inertial measurement unit)输出载体的加速度、角速度信息，IMU输出信息更新频率200HZ，导航计算机接收IMU输出信息，进行导航解算。

步骤3，导航计算机进行SINS导航解算，得到载体的速度、位置、姿态信息；导航计算机判断GNSS是否发送完所有通道信息，若发送完则计算与各通道对应的卫星高度角、方位角信息，具体为：

(3.1)采用传统四元数法进行捷联惯导系统姿态更新解算，其中四元数微分方程表达式为：

$\stackrel{\·}{Q}=\frac{1}{2}\mathrm{\ΩQ}$

其中，Ω为载体坐标系相对导航系下的角速率构成的反对称矩阵，Q为四元数；

通过龙格-库塔求解四元数微分方程，然后由四元数求得姿态矩阵，由姿态矩阵求解载体的三个姿态角；

(3.2)进行捷联惯导系统速度解算，速度微分方程如下：

${\stackrel{\·}{V}}^n=C_b^n{f}^b-(2{\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×V^n+g^n$

其中，Vⁿ、分别为导航系下载体的速度矢量、速度矢量变化率，为载体坐标系到导航坐标系的姿态转换矩阵，f^b为加速度计在载体坐标系下的输出值，为地球自转角速率在导航系下的投影，为导航系相对地球系的旋转角速率，gⁿ为当地重力加速度矢量；

(3.3)进行捷联惯导系统位置解算，载体的位置微分方程如下：

$\left\{\begin{array}{c}\stackrel{\·}{L}=\frac{V_N}{R_M+h}\\ \stackrel{\·}{\mathrm{\λ}}=\frac{V_E\sec L}{R_N+h}\\ \stackrel{\·}{h}=V_U\end{array}\right.$

其中，分别为导航系下载体的纬度、经度和高度的变化率，V_E,V_N,V_U分别为导航系下载体的东向、北向和天向速度，R_M为椭球子午圈上各点的曲率半径，R_N为卯酉圈上各点的曲率半径，L,λ,h分别为导航系下载体的纬度、经度和高度；

(3.4)导航计算机通过通道标志，判断GNSS是否发送完所有通道信息：若没有接收完，则继续接收；若接收完则计算与各通道对应的卫星高度角、方位角信息；计算方法如下：

$\left[\begin{array}{c}\mathrm{\Δe}\\ \mathrm{\Δn}\\ \mathrm{\Δu}\end{array}\right]=S\·\left[\begin{array}{c}\mathrm{\Δx}\\ \mathrm{\Δy}\\ \mathrm{\Δz}\end{array}\right]$

其中，[Δe Δn Δu]^T为导航坐标系中载体到卫星的观测向量，[Δx Δy Δz]^T为地心地固直角坐标系中载体到该卫星的观测向量，

$S=\left[\begin{array}{ccc}-\sin \mathrm{\λ}& \cos \mathrm{\λ}& 0\\ -\sin L\cos \mathrm{\λ}& -\sin L\sin \mathrm{\λ}& \cos L\\ \cos L\cos \mathrm{\λ}& \cos L\sin \mathrm{\λ}& \sin L\end{array}\right],\left[\begin{array}{c}\mathrm{\Δx}\\ \mathrm{\Δy}\\ \mathrm{\Δz}\end{array}\right]=\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]-\left[\begin{array}{c}x\\ y\\ z\end{array}\right]$

其中，[X Y Z]^T为卫星在地心地固直角坐标系中的位置，[x y z]^T为载体在地心地固直角坐标系中位置，则卫星的高度角θ、方位角α分别如下：

$\mathrm{\θ}=\arcsin (\mathrm{\Δu}/\sqrt{{\left(\mathrm{\Δe}\right)}^2+{\left(\mathrm{\Δn}\right)}^2+{\left(\mathrm{\Δu}\right)}^2})$

α＝arctan(Δe/Δn)

其中，0≤θ≤π/2、0≤α≤2π。

步骤4，判断可见卫星个数，若可见卫星大于4颗，则通过分布式最佳精度因子选星法选出4颗可见卫星作为导航星；若可见卫星少于4颗，则将所有可见卫星选为导航星，具体如下：

(4.1)判断可见星个数。对计算出来的每颗星的高度角进行判断，若大于最小高度角阈值，则判断为可见卫星；否则，判断为不可见卫星；

(4.2)导航卫星选择。若可见卫星少于4颗，则不需要选星，将所有可见卫星选为导航星，若可见星个数多于4颗，则需通过选星算法，选出导航星；

分布式最佳精度因子选星算法：例如在200MHZ的弹载计算机(TMS320C6713)上，每计算一种卫星组合情况，耗时0.1ms。以12颗可见星为例，需选星495次，耗时49.5ms，若在一个惯导解算周期内选星495次，必将影响惯导解算，所以应将选星算法拆分到几个惯导解算节拍内完成。

通过分布式最佳精度因子选星法选出4颗可见卫星作为导航星，将选星算法拆分到多个惯导解算周期内完成，拆分方式如图2所示，首先构建一个包含所有4颗可见卫星组合情况的表格，在GNSS数据接收完的下一个惯导解算周期开始选星，每个惯导解算周期内通过查表选择不同的4颗可见卫星组合情况进行几何精度因子GDOP计算，直到表格中所有组合情况的几何精度因子GDOP计算完，选择几何精度因子GDOP最小的一组4颗可见卫星作为导航星；其中几何精度因子GDOP的求取方法如下：

$\mathrm{CDOP}=\sqrt{\mathrm{tra}{\left(G^{T}G\right)}^{-1}}$

$G=\left[\begin{array}{cccc}-\cos {\mathrm{\θ}}^{\left(1\right)}\sin {\mathrm{\α}}^{\left(1\right)}& -\cos {\mathrm{\θ}}^{\left(1\right)}\cos {\mathrm{\α}}^{\left(1\right)}& -\sin {\mathrm{\θ}}^{\left(1\right)}& 1\\ -\cos {\mathrm{\θ}}^{\left(2\right)}\sin {\mathrm{\α}}^{\left(2\right)}& -\cos {\mathrm{\θ}}^{\left(2\right)}\cos {\mathrm{\α}}^{\left(2\right)}& -\sin {\mathrm{\θ}}^{\left(2\right)}& 1\\ -\cos {\mathrm{\θ}}^{\left(3\right)}\sin {\mathrm{\α}}^{\left(3\right)}& -\cos {\mathrm{\θ}}^{\left(3\right)}\cos {\mathrm{\α}}^{\left(3\right)}& -\sin {\mathrm{\θ}}^{\left(3\right)}& 1\\ -\cos {\mathrm{\θ}}^{\left(4\right)}\sin {\mathrm{\α}}^{\left(4\right)}& -\cos {\mathrm{\θ}}^{\left(4\right)}\cos {\mathrm{\α}}^{\left(4\right)}& -\sin {\mathrm{\θ}}^{\left(4\right)}& 1\end{array}\right]$

式中，θ^(σ)、α^(σ)分别为一组可见卫星中第σ颗卫星的高度角、方位角，σ＝1,2,3,4。

步骤5，对导航卫星的伪距测量误差进行补偿；根据导航卫星的速度、位置信息，以及SINS的速度、位置信息，确定载体相对每颗导航卫星的伪距、伪距率信息，具体如下：

(5.1)对导航卫星的伪距测量误差进行补偿，补偿地球自转误差、对流层误差；

地球自转误差：

$\mathrm{\δ}{t}_1=(\frac{P_x-p_x}{P_y}-\frac{P_y-p_y}{P_x})*\mathrm{we}*\mathrm{light}\_\mathrm{speed}$

其中，P_x,P_y分别为卫星在地心地固直角坐标系下x轴、y轴位置，p_x,p_y分别为载体在地心地固直角坐标系下x轴、y轴位置，we为地球自转角速率，light_speed为光速；

对流层误差：

$\mathrm{\δ}{t}_2=\frac{2.47}{\sin \mathrm{\θ}+0.0121}*\mathrm{light}\_\mathrm{speed}$

其中，θ为卫星的高度角，light_speed为光速；

通过校正，得到导航卫星伪距ρ_Gj、伪距率信息，伪距ρ_Gj为：

ρ_Gj＝r_j-δt_u-v_ρj

δt_u＝δt₁+δt₂

其中，δt_u为伪距测量误差，v_ρj为伪距测量白噪声，r_j为载体到第j颗卫星S^j的真实无差距离；

导航卫星的伪距率公式如下：

${\stackrel{\·}{\mathrm{\ρ}}}_{\mathrm{Gj}}={\stackrel{\·}{r}}_j-{\mathrm{\δt}}_{\mathrm{ru}}-v_{\stackrel{\·}{p}j}$

其中，δt_ru为钟漂引起的距离率误差，为伪距率测量白噪声，为载体到第j颗卫星S^j的真实无差距离变化率；

(5.2)根据导航卫星的速度、位置信息，以及SINS的速度、位置信息，确定载体相对每颗导航卫星的伪距、伪距率信息；

载体到第j颗卫星的伪距ρ_Ij为：

ρ_Ij＝r_j+e_j1δx+e_j2δy+e_j3δz

其中，δx、δy、δz分别为载体在地球坐标系中的位置误差在x轴、y轴、z轴分量，e_j1、e_j2、e_j3分别为载体和第j颗卫星的x轴、y轴、z轴方向余弦；

载体到第j颗卫星的伪距率为：

${\stackrel{\·}{\mathrm{\ρ}}}_{\mathrm{Ij}}={\stackrel{\·}{r}}_j+e_{j1}\mathrm{\δ}\stackrel{\·}{x}+e_{j2}\mathrm{\δ}\stackrel{\·}{y}+e_{j3}\mathrm{\δ}\stackrel{\·}{z}$

其中分别为载体在地球坐标系中的速度误差在x轴、y轴、z轴分量。

步骤6，对组合导航的系统状态进行判别，并根据GNSS、IMU的工作状态选择匹配的导航策略，构建系统状态方程，并根据可见卫星的个数构建系统量测方程，容错组合导航流程如图3所示，具体如下：

(6.1)对组合导航的系统状态进行判别

(a)根据IMU的陀螺仪采样值和加速度计采样值判断IMU的工作状态，设A_(axis)max为加速度阈值、ω_(axis)max为角速度阈值，判断加速度计采样值A_axis和陀螺采样值ω_axis是否满足以下条件：

|A_axis|＜A_(axis)max

|ω_axis|＜ω_(axis)max

当满足上式时，则IMU工作状态正常，否则IMU的工作状态异常；

(b)根据GNSS输出值判断GNSS的工作状态，先后进行外层判断和内层判断：

①外层判断为收星条件判别，设dop为精度因子门限，判断收星数N_sats和几何精度因子GDOP是否满足以下条件：

$\left\{\begin{array}{c}{N}_{\mathrm{sats}}\≥4\\ \mathrm{GDOP}\≤\mathrm{dop}\end{array}\right.$ 或1≤N_sats＜4

当上式满足其中一个时，继续内层判别，否则认为GNSS的工作状态异常；

②内层判断对GNSS量测粗大误差进行判别，设δ_ρ、分别为伪距差阈值、伪距率差阈值，ρ_Gj、分别为第j颗卫星当前时刻量测伪距、伪距率值，ρ_Ij、分别为载体相对第j颗卫星的伪距、伪距率值，则判断下式是否成立：

|ρ_Gj-ρ_Ij|＜δ_ρ

$|{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}_{\mathrm{Gj}}-{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}_{\mathrm{Ij}}|<{\mathrm{\&delta;}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}$

上式成立时，则认为GNSS的工作状态正常，否则GNSS的工作状态异常；

(6.2)根据GNSS、IMU的工作状态选择匹配的导航策略，具体方法如下：

(a)当IMU、GNSS的工作状态均正常时，采用紧组合导航：将IMU和GNSS进行位置速度误差组合得到误差方程，经卡尔曼滤波估计出载体的位置、速度和姿态误差，对IMU的位置、速度、横滚角和俯仰角进行反馈校正；

(b)当IMU工作状态异常、GNSS工作状态正常时，放弃当前时刻IMU中陀螺仪和加速度计的量测值，用前一时刻的量测值替代：

ω(k)_axis＝ω(k-1)_axis

A(k)_axis＝A(k-1)_axis

其中，ω(k)_axis为k时刻的角速度，ω(k-1)_axis为k-1时刻的角速度，A(k)_axis为k时刻的加速度，A(k-1)_axis为k-1时刻的加速度；

(c)当IMU工作状态正常、GNSS工作状态异常时，采用丢星算法处理；从丢星状态恢复收星时，利用状态误差转移矩阵F估计导航误差并修正导航输出；

(d)当IMU、GNSS工作状态均异常时，采用机动目标的轨迹预测方法对载体运动状态进行估计；

(6.3)导航系统的姿态、速度、位置、伪距、伪距率误差方程如下：

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{\mathrm{\&phi;}}}_E=(w_{\mathrm{ie}}\sin L+V_E\tan L/(R_N+h)){\mathrm{\&phi;}}_N-\mathrm{\&delta;}{V}_N/(R_M+h)\\ -(V_E/(R_N+h)+w_{\mathrm{ie}}\cos L){\mathrm{\&phi;}}_U+{\mathrm{\&epsiv;}}_E\\ {\stackrel{\&CenterDot;}{\mathrm{\&phi;}}}_N=-(w_{\mathrm{ie}}\sin L+V_E\tan L/(R_N+h)){\mathrm{\&phi;}}_E-w_{\mathrm{ie}}\sin \mathrm{L\&delta;L}\\ -{\mathrm{\&phi;}}_U{V}_N/(R_M+h)+\mathrm{\&delta;}{V}_E/(R_N+h)+{\mathrm{\&epsiv;}}_N\\ {\stackrel{\&CenterDot;}{\mathrm{\&phi;}}}_U=(w_{\mathrm{ie}}\cos L+V_E/(R_N+h)){\mathrm{\&phi;}}_E+\mathrm{\&delta;}{V}_E\tan L/(R_N+h)\\ +{\mathrm{\&phi;}}_N{V}_N/(R_M+h)+(w_{\mathrm{ie}}\cos L+V_E{\sec }^{2}L/(R_N+h))\mathrm{\&delta;L}+{\mathrm{\&epsiv;}}_U\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{V}}_E={\mathrm{\&phi;}}_U{f}_N-{\mathrm{\&phi;}}_N{f}_U+(V_N\tan L/(R_M+h)-V_U/(R_M+h))\mathrm{\&delta;}{V}_E\\ +(2w_{\mathrm{ie}}\sin L+V_E\tan L/(R_N+h))\mathrm{\&delta;}{V}_N-(2w_{\mathrm{ie}}\cos L+V_E/(R_N+h))\mathrm{\&delta;}{V}_U\\ +(2w_{\mathrm{ie}}\cos L{V}_N+V_E{V}_N{\sec }^{2}L/(R_N+h)+2w_{\mathrm{ie}}\sin L{V}_U)\mathrm{\&delta;L}+{\&dtri;}_E\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{V}}_N={\mathrm{\&phi;}}_E{f}_U-{\mathrm{\&phi;}}_U{f}_E-2(w_{\mathrm{ie}}\sin L+V_E\tan L/(R_N+h))\mathrm{\&delta;}{V}_E-\mathrm{\&delta;}{V}_N{V}_U/(R_M+h)\\ -\mathrm{\&delta;}{V}_U{V}_N/(R_M+h)+(2w_{\mathrm{ie}}\cos L+V_E{\sec }^{2}L/(R_N+h))V_E\mathrm{\&delta;L}+{\&dtri;}_N\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{V}}_U={\mathrm{\&phi;}}_N{f}_E-{\mathrm{\&phi;}}_E{f}_N+2(w_{\mathrm{ie}}\cos L+V_E/(R_N+h))\mathrm{\&delta;}{V}_E\\ +2\mathrm{\&delta;}{V}_N{V}_N/(R_M+h)-2w_{\mathrm{ie}}\sin L{V}_E\mathrm{\&delta;L}+{\&dtri;}_U\\ \mathrm{\&delta;}\stackrel{\&CenterDot;}{L}=\mathrm{\&delta;}{V}_N/(R_N+h)\\ \mathrm{\&delta;}\stackrel{\&CenterDot;}{\mathrm{\&lambda;}}=\mathrm{\&delta;}{V}_E\sec L/(R_N+h)+V_E\sec L\tan \mathrm{L\&delta;L}/(R_N+h)\\ \mathrm{\&delta;}\stackrel{\&CenterDot;}{h}=\mathrm{\&delta;}{V}_U\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{t}}_u=\mathrm{\&delta;}{t}_{\mathrm{ru}}\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{t}}_{\mathrm{ru}}=-{\mathrm{\&beta;}}_{\mathrm{tru}}\mathrm{\&delta;}{t}_{\mathrm{ru}}\end{array}\right.$

式中，φ_E、φ_N、φ_U分别为东、北、天方向平台失准角，δV_E、δV_N、δV_U分别为载体东、北、天方向速度误差，δL、δλ、δh分别为载体纬度、经度、高度误差，δt_u为与时钟等效的距离误差，δt_ru为与时钟频率等效的距离率误差，R_M为椭球子午圈上各点的曲率半径，R_N为卯酉圈上各点的曲率半径，w_ie为地球转动角速率，f_E、f_N、f_U分别是惯导系统的比力在导航系下东、北、天方向上的分量，ε_E、ε_N、ε_U、分别为地理坐标系内陀螺的等效漂移在东、北、天方向的分量，▽_E、▽_N、▽_U分别为地理坐标系内加速计的等效漂移在东、北、天方向的分量，β_tru为反相关时间；

(6.4)以导航系统的姿态误差、速度误差、位置误差及伪距、伪距率误差为状态变量，建立惯性/卫星组合导航系统的状态方程：

$\stackrel{\&CenterDot;}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

其中，X为系统状态矢量，表示系统状态矢量的导数，F为系统状态转移矩阵，G为系统噪声驱动矩阵，W为系统噪声矢量，具体如下：

系统状态矢量：

$X={[{\mathrm{\&phi;}}_E,{\mathrm{\&phi;}}_N,{\mathrm{\&phi;}}_U,{\mathrm{\&delta;V}}_E,{\mathrm{\&delta;V}}_N,{\mathrm{\&delta;V}}_U,\mathrm{\&delta;L},\mathrm{\&delta;\&lambda;},\mathrm{\&delta;h},{\mathrm{\&epsiv;}}_x,{\mathrm{\&epsiv;}}_y,{\mathrm{\&epsiv;}}_z,{\&dtri;}_x,{\&dtri;}_y,{\&dtri;}_z,{\mathrm{\&delta;t}}_u,{\mathrm{\&delta;t}}_{\mathrm{ru}}]}_{17\&times;1}^T$

其中，φ_E、φ_N、φ_U分别为东、北、天方向平台失准角，δV_E、δV_N、δV_U分别为载体东、北、天方向速度误差，δL、δλ、δh分别为载体纬度、经度、高度误差，ε_x、ε_y、ε_z分别为载体系下陀螺随机常值漂移在x、y、z轴上的分量，▽_x、▽_y、▽_z分别为载体系下加速度计偏置在x、y、z轴上的分量，δt_u为与时钟等效的距离误差，δt_ru为与时钟频率等效的距离率误差；

状态转移矩阵：

$F={\left[\begin{array}{cccc}{\left(F_{\mathrm{ins}}\right)}_{9\&times;9}& {\left(F_{\mathrm{sg}}\right)}_{9\&times;6}& 0& 0\\ 0_{6\&times;9}& 0_{6\&times;6}& 0& 0\\ 0& 0& 0& 1\\ 0& 0& 0& -{\mathrm{\&beta;}}_{\mathrm{tru}}\end{array}\right]}_{17\&times;17},F_{\mathrm{sg}}={\left[\begin{array}{cc}{C}_b^n& 0_{3\&times;3}\\ 0_{3\&times;3}& C_b^n\\ 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right]}_{9\&times;6}$

其中，F_ins由步骤(6.3)中误差方程构成，为载体坐标系到导航坐标系的姿态转换矩阵，β_tru为反相关时间；

系统噪声驱动矩阵为G，且：

$G={\left[\begin{array}{cccc}{C}_b^n& 0_{3\&times;3}& 0& 0\\ 0_{3\&times;3}& C_b^n& 0& 0\\ 0_{9\&times;3}& 0_{9\&times;3}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]}_{17\&times;8}$

系统噪声矢量为W，且：

$W={\left[\begin{array}{cccccccc}{w}_{\mathrm{gx}}& w_{\mathrm{gy}}& w_{\mathrm{gz}}& w_{\mathrm{ax}}& w_{\mathrm{ay}}& w_{\mathrm{az}}& w_{\mathrm{tu}}& w_{\mathrm{tru}}\end{array}\right]}_{8\&times;1}^T$

其中，w_gx、w_gy、w_gz分别为陀螺仪在x轴、y轴、z轴方向的随机白噪声，w_ax、w_ay、w_az分别为加速度计在x轴、y轴、z轴方向的随机白噪声，w_tu、w_tru分别为伪距随机白噪声和伪距率随机白噪声；

(6.5)构建系统状态方程，根据可见卫星个数，系统动态调整观测方程及组合滤波器相关矩阵的维数，图4给出了维数变化示意图，构建系统量测方程，如下：

Z(t)＝H(t)X(t)+V(t)

其中，Z(t)为系统观测矢量，H(t)为系统观测矩阵，V(t)为系统观测噪声阵，X为系统状态矢量；量测方程的维数及组合滤波器的维数根据可见卫星数量变化，变化关系如下：

$n=\left\{\begin{array}{cc}N& (0<N<4)\\ 4& (N\&GreaterEqual;4)\end{array}\right.$

其中，N为可见星数量，观测向量Z的维数为：2n×1；系统观测矩阵H为：2n×17；系统观测噪声方差R阵为：2n×2n；卡尔曼滤波增益阵为：17×2n；

伪距观测方程如下：

${\tilde{Z}}_{\mathrm{\&rho;}}={\tilde{H}}_{\mathrm{\&rho;}}\tilde{X}+{\tilde{V}}_{\mathrm{\&rho;}}=\left[\begin{array}{cccc}{0}_{n\&times;6}& {\tilde{H}}_{\mathrm{\&rho;n}1}& 0_{n\&times;6}& {\tilde{H}}_{\mathrm{\&rho;n}2}\end{array}\right]\tilde{X}+{\tilde{V}}_{\mathrm{\&rho;}}$

式中，为观测矢量，为观测矩阵，为观测噪声阵，为状态矢量，分别为：

$\begin{array}{cc}{\tilde{Z}}_{\mathrm{\&rho;}}={\left[\begin{array}{ccc}\mathrm{\&delta;}{\mathrm{\&rho;}}_1& \&CenterDot;\&CenterDot;\&CenterDot;& \mathrm{\&delta;}{\mathrm{\&rho;}}_n\end{array}\right]}_{1\&times;n}^T& {\tilde{V}}_{\mathrm{\&rho;}}={\left[\begin{array}{ccc}{v}_{\mathrm{\&rho;}}^1& \&CenterDot;\&CenterDot;\&CenterDot;& v_{\mathrm{\&rho;}}^1\end{array}\right]}_{1\&times;n}^T\end{array}$

δρ_j＝ρ_Ij-ρ_Gj＝e_j1δx+e_j2δy+e_j3δz+δt_u+ν_ρj

${\tilde{H}}_{\mathrm{\&rho;n}1}={\left[\begin{array}{ccc}{\tilde{a}}_{11}& {\tilde{a}}_{12}& {\tilde{a}}_{13}\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ {\tilde{a}}_{n1}& {\tilde{a}}_{n2}& {\tilde{a}}_{n3}\end{array}\right]}_{n\&times;3},{\tilde{H}}_{\mathrm{\&rho;n}2}={\left[\begin{array}{cc}1& 0\\ \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;\\ 1& 0\end{array}\right]}_{n\&times;2}$

其中，δρ_j为卫星伪距和载体相对卫星伪距之差，j＝1…n，…为各通道伪距测量白噪声，ρ_Ij为载体相对每颗导航卫星的伪距，ν_ρj为伪距量测白噪声，δt_u为钟差引起的距离误差，ρ_Gj为导航卫星的伪距，展开如下：

$\left\{\begin{array}{c}{\tilde{a}}_{j1}=(R_N+h)[-e_{j1}\sin L\cos \mathrm{\&lambda;}-e_{j2}\sin L\sin \mathrm{\&lambda;}]\\ +[R_N{(1-f)}^2+h]e_{j3}\cos L\\ {\tilde{a}}_{j2}=(R_N+h)[e_{j2}\cos L\cos \mathrm{\&lambda;}-e_{j1}\cos L\sin \mathrm{\&lambda;}]\\ {\tilde{a}}_{j3}=e_{j1}\cos L\cos \mathrm{\&lambda;}+e_{j2}\cos L\sin \mathrm{\&lambda;}+e_{j3}\sin L\end{array}\right.$

其中，e_j1、e_j2、e_j3分别为载体和第j颗卫星的x轴、y轴、z轴方向余弦，f为地球椭圆度；

伪距率观测方程如下所示：

${\tilde{Z}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}={\tilde{H}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}\tilde{X}+{\tilde{V}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}=\left[\begin{array}{cccc}{0}_{n\&times;3}& {\tilde{H}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}n1}& 0_{n\&times;9}& {\tilde{H}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}n2}\end{array}\right]\tilde{X}+{\tilde{V}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}$

式中，为观测矢量，为观测矩阵，为观测噪声阵，为状态矢量，分别为：

$\begin{array}{cc}{\tilde{Z}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}={\left[\begin{array}{ccc}\mathrm{\&delta;}{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}_1& \&CenterDot;\&CenterDot;\&CenterDot;& \mathrm{\&delta;}{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}_n\end{array}\right]}_{1\&times;n}^T& {\tilde{V}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}={\left[\begin{array}{ccc}{v}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}^1& \&CenterDot;\&CenterDot;\&CenterDot;& v_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}^1\end{array}\right]}_{1\&times;n}^T\end{array}$

$\mathrm{\&delta;}{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}_j={\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}_{\mathrm{Ij}}-{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}_{\mathrm{Gj}}=e_{j1}\mathrm{\&delta;}\stackrel{\&CenterDot;}{x}+e_{j2}\mathrm{\&delta;}\stackrel{\&CenterDot;}{y}+e_{j3}\mathrm{\&delta;}\stackrel{\&CenterDot;}{z}+\mathrm{\&delta;}{t}_{\mathrm{ru}}+v_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}j}$

${\tilde{H}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}n1}={\left[\begin{array}{ccc}{\tilde{b}}_{11}& {\tilde{b}}_{12}& {\tilde{b}}_{13}\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ {\tilde{b}}_{n1}& {\tilde{b}}_{n2}& {\tilde{b}}_{n3}\end{array}\right]}_{n\&times;3},{\tilde{H}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}n2}={\left[\begin{array}{cc}0& 1\\ \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;\\ 0& 1\end{array}\right]}_{n\&times;2}$

其中，为卫星伪距率和载体相对卫星伪距率之差，j＝1…n，…为各通道伪距率测量白噪声，为载体相对每颗导航卫星伪距率，为伪距率量测白噪声，δt_ru为钟漂引起的距离率误差，为导航卫星伪距率，展开如下：

$\left\{\begin{array}{c}{\tilde{b}}_{j1}=-e_{j1}\sin \mathrm{\&lambda;}+e_{j2}\cos \mathrm{\&lambda;}\\ {\tilde{b}}_{j2}=-e_{j1}\sin L\cos \mathrm{\&lambda;}-e_{j2}\sin L\sin \mathrm{\&lambda;}+e_{j3}\cos L\\ {\tilde{b}}_{j3}=e_{j1}\cos L\cos \mathrm{\&lambda;}+e_{j2}\cos L\sin \mathrm{\&lambda;}+e_{j3}\sin L\end{array}\right.$

其中，e_j1、e_j2、e_j3分别为载体和第j颗卫星的x轴、y轴、z轴方向余弦；

综合伪距、伪距率观测方程，得到惯性/卫星组合导航系统观测方程如下：

$\tilde{Z}=\left[\begin{array}{c}{\tilde{Z}}_{\mathrm{\&rho;}}\\ {\tilde{Z}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}\end{array}\right]=\left[\begin{array}{c}{\tilde{H}}_{\mathrm{\&rho;}}\\ {\tilde{H}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}\end{array}\right]\tilde{X}+\left[\begin{array}{c}{\tilde{V}}_{\mathrm{\&rho;}}\\ {\tilde{V}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}\end{array}\right]=\tilde{H}\tilde{X}+\tilde{V}$

步骤7，根据系统状态方程和系统量测方程，采用卡尔曼滤波信息融合法进行滤波，并根据滤波结果，对由通讯延时引起的滞后误差，通过基于状态转移的误差补偿方法对系统进行校正得到最终的导航结果，具体如下：

(7.1)根据系统状态方程和系统量测方程，采用卡尔曼滤波信息融合算法；

(7.2)根据滤波结果，对由通讯延时引起的滞后误差，通过基于状态转移的误差补偿方法对系统进行校正。在紧组合导航系统中，由GNSS接收机输出各通道卫星信息，数据传输量相较松组合系统大幅增加。以分布式导航系统为例，带来的串口通讯时延大幅增加，若12通道接收机，数据传输波特率115200bit/s，传输时延为420ms。因此需要高精度的误差补偿方法；在现有时间同步方法的基础上，提出一种基于状态转移的误差补偿方法，具体思路为：利用秒脉冲时刻SINS和GNSS输出进行组合滤波以获取导航状态误差量的最优估计，并利用状态转移矩阵将其递推到当前时刻，对当前时刻系统进行校正，其原理如图5所示，误差补偿方法具体为：

(7.1)设GNSS接收机秒脉冲时刻为t_k，GNSS数据传输完毕时刻为t_k+t_d，t_d为GNSS通讯延时；

(7.2)利用秒脉冲时刻SINS和GNSS输出进行组合卡尔曼滤波，求取t_k时刻导航状态误差量的最优估计

(7.3)采用GNSS传输完毕时刻的SINS解算输出求取连续系统下的状态转移矩阵F(t_k+t_d)，采用直接法求取t_k至t_k+t_d时刻的系统状态误差转移矩阵

${\mathrm{\&Phi;}}_{t_k+t_d|t_k}=I+\underset{m=1}{\overset{\&infin;}{\mathrm{\&Sigma;}}}\frac{{[F\&CenterDot;t]}^m}{m!}$

其中，I为单位阵；

(7.4)利用系统状态误差转移矩阵的性质，将t_k时刻导航状态误差量的最优估计递推至当前时刻，并进行反馈修正，得t_k+t_d时刻导航状态误差量的最优估计

$X_{t_k+t_d}={\mathrm{\&Phi;}}_{t_k+t_d|t_k}\&CenterDot;X_{t_k}$

综上所述，本发明弹载惯性/卫星紧组合导航方法可实现基于惯性/卫星的伪距、伪距率无缝组合导航，提高了导航精度和对复杂环境的适应性，应用前景广阔。

Claims (6)

1.一种弹载惯性/卫星紧组合导航方法，其特征在于，包括以下步骤：

步骤1，SINS初始对准，初始化速度、位置；

步骤2，导航计算机分别接收GNSS数据和SINS数据；

步骤3，导航计算机进行SINS导航解算，得到载体的速度、位置、姿态信息；导航计算机判断GNSS是否发送完所有通道信息，若发送完则计算与各通道对应的卫星高度角、方位角信息；

步骤4，判断可见卫星个数，若可见卫星大于4颗，则通过分布式最佳精度因子选星法选出4颗可见卫星作为导航星；若可见卫星少于4颗，则将所有可见卫星选为导航星；

步骤5，对导航卫星的伪距测量误差进行补偿；根据导航卫星的速度、位置信息，以及SINS的速度、位置信息，确定载体相对每颗导航卫星的伪距、伪距率信息；

步骤6，对组合导航的系统状态进行判别，并根据GNSS、IMU的工作状态选择匹配的导航策略，构建系统状态方程，并根据可见卫星的个数构建系统量测方程；

步骤7，根据系统状态方程和系统量测方程，采用卡尔曼滤波信息融合法进行滤波，并根据滤波结果，对由通讯延时引起的滞后误差，通过基于状态转移的误差补偿方法对系统进行校正得到最终的导航结果，具体如下：

(7.1)设GNSS接收机秒脉冲时刻为t_k，GNSS数据传输完毕时刻为t_k+t_d，t_d为GNSS通讯延时；

(7.2)利用秒脉冲时刻SINS和GNSS输出进行组合卡尔曼滤波，求取t_k时刻导航状态误差量的最优估计

(7.3)采用GNSS传输完毕时刻的SINS解算输出求取连续系统下的状态转移矩阵F(t_k+t_d)，采用直接法求取t_k至t_k+t_d时刻的系统状态误差转移矩阵

${\mathrm{\&Phi;}}_{t_k+t_d|t_k}=I+\underset{m=1}{\overset{\mathrm{\&infin;}}{\&Sigma;}}\frac{{\&lsqb;F\&CenterDot;t\&rsqb;}^m}{m!}$

其中，I为单位阵；

(7.4)利用系统状态误差转移矩阵的性质，将t_k时刻导航状态误差量的最优估计递推至当前时刻，并进行反馈修正，得t_k+t_d时刻导航状态误差量的最优估计

$X_{t_k+t_d}={\mathrm{\&Phi;}}_{t_k+t_d|t_k}\&CenterDot;X_{t_k}.$

2.根据权利要求1所述的弹载惯性/卫星紧组合导航方法，其特征在于，步骤2中所述导航计算机分别接收GNSS数据和SINS数据，具体如下：

(2.1)导航计算机接收GNSS数据

在紧组合导航系统中，GNSS接收机输出多个卫星的信息，每颗卫星信息通过一个通道输出，每个通道的信息包含：通道号，卫星编号，卫星工作状态，世界标准时间，接收机地心地固直角坐标系下的X、Y、Z轴位置和速度，伪距、伪距率量测值，卫星在地心地固直角坐标系下的X、Y、Z轴位置和速度；导航计算机依次接收、存储每个卫星对应的通道信息；

(2.2)导航计算机接收SINS数据

IMU输出载体加速度、角速度信息，导航计算机接收IMU输出信息，进行导航解算。

3.根据权利要求1所述的弹载惯性/卫星紧组合导航方法，其特征在于，步骤3中所述导航计算机进行SINS导航解算，得到载体的速度、位置、姿态信息；导航计算机判断GNSS是否发送完所有通道信息，若发送完则计算与各通道对应的卫星高度角、方位角信息，具体如下：

(3.1)采用传统四元数法进行捷联惯导系统姿态更新解算，其中四元数微分方程表达式为：

$\stackrel{\&CenterDot;}{Q}=\frac{1}{2}\mathrm{\&Omega;}Q$

其中，Ω为载体坐标系相对导航系下的角速率构成的反对称矩阵，Q为四元数；

通过龙格-库塔求解四元数微分方程，然后由四元数求得姿态矩阵，由姿态矩阵求解载体的三个姿态角；

(3.2)进行捷联惯导系统速度解算，速度微分方程如下：

${\stackrel{\&CenterDot;}{V}}^n=C_b^n{f}^b-(2{\mathrm{\&omega;}}_{ie}^n+{\mathrm{\&omega;}}_{en}^n)\&times;V^n+g^n$

其中，Vⁿ、分别为导航系下载体的速度矢量、速度矢量变化率，为载体坐标系到导航坐标系的姿态转换矩阵，f^b为加速度计在载体坐标系下的输出值，为地球自转角速率在导航系下的投影，为导航系相对地球系的旋转角速率，gⁿ为当地重力加速度矢量；

(3.3)进行捷联惯导系统位置解算，载体的位置微分方程如下：

$\left\{\begin{array}{c}\stackrel{\&CenterDot;}{L}=\frac{V_N}{R_M+h}\\ \stackrel{\&CenterDot;}{\mathrm{\&lambda;}}=\frac{V_E\sec L}{R_N+h}\\ \stackrel{\&CenterDot;}{h}=V_U\end{array}\right.$

分别为导航系下载体的纬度、经度和高度的变化率，V_E,V_N,V_U分别为导航系下载体的东向、北向和天向速度，R_M为椭球子午圈上各点的曲率半径，R_N为卯酉圈上各点的曲率半径，L,λ,h分别为导航系下载体的纬度、经度和高度；

(3.4)导航计算机通过通道标志，判断GNSS是否发送完所有通道信息：若没有接收完，则继续接收；若接收完则计算与各通道对应的卫星高度角、方位角信息；计算方法如下：

$\left[\begin{array}{c}\mathrm{\&Delta;}e\\ \mathrm{\&Delta;}n\\ \mathrm{\&Delta;}u\end{array}\right]=S\&CenterDot;\left[\begin{array}{c}\mathrm{\&Delta;}x\\ \mathrm{\&Delta;}y\\ \mathrm{\&Delta;}z\end{array}\right]$

其中，[Δe Δn Δu]^T为导航坐标系中载体到卫星的观测向量，[Δx Δy Δz]^T为地心地固直角坐标系中载体到该卫星的观测向量，

$S=\left[\begin{array}{ccc}-sin\mathrm{\&lambda;}& cos\mathrm{\&lambda;}& 0\\ -\sin L\cos \mathrm{\&lambda;}& -\sin Lsin\mathrm{\&lambda;}& \cos L\\ \cos Lcos\mathrm{\&lambda;}& \cos L\sin \mathrm{\&lambda;}& \sin L\end{array}\right],\left[\begin{array}{c}\mathrm{\&Delta;}x\\ \mathrm{\&Delta;}y\\ \mathrm{\&Delta;}z\end{array}\right]=\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]-\left[\begin{array}{c}x\\ y\\ z\end{array}\right]$

其中，[X Y Z]^T为卫星在地心地固直角坐标系中的位置，[x y z]^T为载体在地心地固直角坐标系中位置，则卫星的高度角θ、方位角α分别如下：

$\mathrm{\&theta;}=\arcsin (\mathrm{\&Delta;}u/\sqrt{{\left(\mathrm{\&Delta;}e\right)}^2+{\left(\mathrm{\&Delta;}n\right)}^2+{\left(\mathrm{\&Delta;}u\right)}^2})$

α＝arctan(Δe/Δn)

其中，0≤θ≤π/2、0≤α≤2π。

4.根据权利要求1所述的弹载惯性/卫星紧组合导航方法，其特征在于，步骤4中所述若可见卫星大于4颗，则通过分布式最佳精度因子选星法选出4颗可见卫星作为导航星，具体如下：

将选星算法拆分到多个惯导解算周期内完成，拆分方式为：首先构建一个包含所有4颗可见卫星组合情况的表格，在GNSS数据接收完的下一个惯导解算周期开始选星，每个惯导解算周期内通过查表选择不同的4颗可见卫星组合情况进行几何精度因子GDOP计算，直到表格中所有组合情况的几何精度因子GDOP计算完，选择几何精度因子GDOP最小的一组4颗可见卫星作为导航星；其中几何精度因子GDOP的求取方法如下：

$GDOP=\sqrt{tra{\left(G^{T}G\right)}^{-1}}$

$G=\left[\begin{array}{cccc}-{\mathrm{cos\&theta;}}^{\left(1\right)}{\mathrm{sin\&alpha;}}^{\left(1\right)}& -{\mathrm{cos\&theta;}}^{\left(1\right)}{\mathrm{cos\&alpha;}}^{\left(1\right)}& -{\mathrm{sin\&theta;}}^{\left(1\right)}& 1\\ -{\mathrm{cos\&theta;}}^{\left(2\right)}{\mathrm{sin\&alpha;}}^{\left(2\right)}& -{\mathrm{cos\&theta;}}^{\left(2\right)}{\mathrm{cos\&alpha;}}^{\left(2\right)}& -{\mathrm{sin\&theta;}}^{\left(2\right)}& 1\\ -{\mathrm{cos\&theta;}}^{\left(3\right)}{\mathrm{sin\&alpha;}}^{\left(3\right)}& -{\mathrm{cos\&theta;}}^{\left(3\right)}{\mathrm{cos\&alpha;}}^{\left(3\right)}& -{\mathrm{sin\&theta;}}^{\left(3\right)}& 1\\ -{\mathrm{cos\&theta;}}^{\left(4\right)}{\mathrm{sin\&alpha;}}^{\left(4\right)}& -{\mathrm{cos\&theta;}}^{\left(4\right)}{\mathrm{cos\&alpha;}}^{\left(4\right)}& -{\mathrm{sin\&theta;}}^{\left(4\right)}& 1\end{array}\right]$

式中，θ^(σ)、α^(σ)分别为一组可见卫星中第σ颗卫星的高度角、方位角，σ＝1,2,3,4。

5.根据权利要求1所述的弹载惯性/卫星紧组合导航方法，其特征在于，步骤5中所述对导航卫星的伪距测量误差进行补偿，然后根据导航卫星的速度、位置信息，以及SINS的速度、位置信息，确定载体相对每颗导航卫星的伪距、伪距率信息，具体为：

(5.1)对导航卫星的伪距测量误差进行补偿，补偿地球自转误差、对流层误差；

地球自转误差：

${\mathrm{\&delta;t}}_1=(\frac{P_x-p_x}{P_y}-\frac{P_y-p_y}{P_x})*we*light\_speed$

其中，P_x,P_y分别为卫星在地心地固直角坐标系下x轴、y轴位置，p_x,p_y分别为载体在地心地固直角坐标系下x轴、y轴位置，we为地球自转角速率，light_speed为光速；

对流层误差：

${\mathrm{\&delta;t}}_2=\frac{2.47}{sin\mathrm{\&theta;}+0.0121}*light\_speed$

其中，θ为卫星的高度角，light_speed为光速；

通过校正，得到导航卫星伪距ρ_Gj、伪距率信息，伪距ρ_Gj为：

ρ_Gj＝r_j-δt_u-v_ρj

δt_u＝δt₁+δt₂

其中，δt_u为伪距测量误差，v_ρj为伪距测量白噪声，r_j为载体到第j颗卫星S^j的真实无差距离；

导航卫星的伪距率公式如下：

${\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}_{Gj}={\stackrel{\&CenterDot;}{r}}_j-{\mathrm{\&delta;t}}_{ru}-v_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}j}$

其中，δt_ru为钟漂引起的距离率误差，为伪距率测量白噪声，为载体到第j颗卫星S^j的真实无差距离变化率；

(5.2)根据导航卫星的速度、位置信息，以及SINS的速度、位置信息，确定载体相对每颗导航卫星的伪距、伪距率信息；

载体到第j颗卫星的伪距ρ_Ij为：

ρ_Ij＝r_j+e_j1δx+e_j2δy+e_j3δz

其中，δx、δy、δz分别为载体在地球坐标系中的位置误差在x轴、y轴、z轴分量，e_j1、e_j2、e_j3分别为载体和第j颗卫星的x轴、y轴、z轴方向余弦；

载体到第j颗卫星的伪距率为：

${\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}_{Ij}={\stackrel{\&CenterDot;}{r}}_j+e_{j1}\mathrm{\&delta;}\stackrel{\&CenterDot;}{x}+e_{j2}\mathrm{\&delta;}\stackrel{\&CenterDot;}{y}+e_{j3}\mathrm{\&delta;}\stackrel{\&CenterDot;}{z}$

其中分别为载体在地球坐标系中的速度误差在x轴、y轴、z轴分量。

6.根据权利要求1所述的弹载惯性/卫星紧组合导航方法，其特征在于，步骤6中所述对组合导航的系统状态进行判别，并根据GNSS、IMU的工作状态选择匹配的导航策略，构建系统状态方程，并根据可见卫星的个数构建系统量测方程，具体如下：

(6.1)对组合导航的系统状态进行判别

(a)根据IMU的陀螺仪采样值和加速度计采样值判断IMU的工作状态，设A_(axis)max为加速度阈值、ω_(axis)max为角速度阈值，判断加速度计采样值A_axis和陀螺采样值ω_axis是否满足以下条件：

|A_axis|＜A_(axis)max

|ω_axis|＜ω_(axis)max

当满足上式时，则IMU工作状态正常，否则IMU的工作状态异常；

(b)根据GNSS输出值判断GNSS的工作状态，先后进行外层判断和内层判断：

①外层判断为收星条件判别，设dop为精度因子门限，判断收星数N_sats和几何精度因子GDOP是否满足以下条件：

或1≤N_sats＜4

当上式满足其中一个时，继续内层判别，否则认为GNSS的工作状态异常；

②内层判断对GNSS量测粗大误差进行判别，设δ_ρ、分别为伪距差阈值、伪距率差阈值，ρ_Gj、分别为第j颗卫星当前时刻量测伪距、伪距率值，ρ_Ij、分别为载体相对第j颗卫星的伪距、伪距率值，则判断下式是否成立：

|ρ_Gj-ρ_Ij|＜δ_ρ

$|{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}_{Gj}-{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}_{Ij}|<{\mathrm{\&delta;}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}$

上式成立时，则认为GNSS的工作状态正常，否则GNSS的工作状态异常；

(6.2)根据GNSS、IMU的工作状态选择匹配的导航策略，具体方法如下：

(a)当IMU、GNSS的工作状态均正常时，采用紧组合导航：将IMU和GNSS进行位置速度误差组合得到误差方程，经卡尔曼滤波估计出载体的位置、速度和姿态误差，对IMU的位置、速度、横滚角和俯仰角进行反馈校正；

(b)当IMU工作状态异常、GNSS工作状态正常时，放弃当前时刻IMU中陀螺仪和加速度计的量测值，用前一时刻的量测值替代：

ω(k)_axis＝ω(k-1)_axis

A(k)_axis＝A(k-1)_axis

其中，ω(k)_axis为k时刻的角速度，ω(k-1)_axis为k-1时刻的角速度，A(k)_axis为k时刻的加速度，A(k-1)_axis为k-1时刻的加速度；

(c)当IMU工作状态正常、GNSS工作状态异常时，采用丢星算法处理；从丢星状态恢复收星时，利用状态误差转移矩阵F估计导航误差并对导航输出进行修正；

(d)当IMU、GNSS工作状态均异常时，采用机动目标的轨迹预测方法对载体运动状态进行估计；

(6.3)导航系统的姿态、速度、位置、伪距、伪距率误差方程如下：

$\left\{\begin{array}{c}{\stackrel{\&CenterDot;}{\mathrm{\&phi;}}}_E=\left(w_{ie}\sin L+V_E\tan L/\left(R_N+h\right)\right){\mathrm{\&phi;}}_N-{\mathrm{\&delta;V}}_N/\left(R_M+h\right)\\ -\left(V_E/\left(R_N+h\right)+w_{ie}\cos L\right){\mathrm{\&phi;}}_U+{\mathrm{\&epsiv;}}_E\\ {\stackrel{\&CenterDot;}{\mathrm{\&phi;}}}_N=-\left(w_{ie}\sin L+V_E\tan L/\left(R_N+h\right)\right){\mathrm{\&phi;}}_E-w_{ie}\sin L\mathrm{\&delta;}L\\ -{\mathrm{\&phi;}}_U{V}_N/\left(R_M+h\right)+{\mathrm{\&delta;V}}_E/\left(R_N+h\right)+{\mathrm{\&epsiv;}}_N\\ {\stackrel{\&CenterDot;}{\mathrm{\&phi;}}}_U=\left(w_{ie}\cos L+V_E/\left(R_N+h\right)\right){\mathrm{\&phi;}}_E+{\mathrm{\&delta;V}}_E\tan L/\left(R_N+h\right)\\ +{\mathrm{\&phi;}}_N{V}_N/\left(R_M+h\right)+\left(w_{ie}\cos L+V_E{\sec }^{2}L/\left(R_N+h\right)\right)\mathrm{\&delta;}L+{\mathrm{\&epsiv;}}_U\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{V}}_E={\mathrm{\&phi;}}_U{f}_N-{\mathrm{\&phi;}}_N{f}_U+\left(V_N\tan L/\left(R_M+h\right)-V_U/\left(R_M+h\right)\right){\mathrm{\&delta;V}}_E\\ +\left(2w_{ie}\sin L+V_E\tan L/\left(R_N+h\right)\right){\mathrm{\&delta;V}}_N-\left(2w_{ie}\cos L+V_E/\left(R_N+h\right)\right){\mathrm{\&delta;V}}_U\\ +\left(2w_{ie}\cos {\mathrm{LV}}_N+V_E{V}_N{\sec }^{2}L/\left(R_N+h\right)+2w_{ie}\sin {\mathrm{LV}}_U\right)\mathrm{\&delta;}L+{\&dtri;}_E\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{V}}_N={\mathrm{\&phi;}}_E{f}_U-{\mathrm{\&phi;}}_U{f}_E-2\left(w_{ie}\sin L+V_E\tan L/\left(R_N+h\right)\right){\mathrm{\&delta;V}}_E-{\mathrm{\&delta;V}}_N{V}_U/\left(R_M+h\right)\\ -{\mathrm{\&delta;V}}_U{V}_N/\left(R_M+h\right)+\left(2w_{ie}\cos L+V_E{\sec }^{2}L/\left(R_N+h\right)\right)V_E\mathrm{\&delta;}L+{\&dtri;}_N\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{V}}_U={\mathrm{\&phi;}}_N{f}_E-{\mathrm{\&phi;}}_E{f}_N+2\left(w_{ie}\cos L+V_E/\left(R_N+h\right)\right){\mathrm{\&delta;V}}_E\\ +2{\mathrm{\&delta;V}}_N{V}_N/\left(R_M+h\right)-2w_{ie}\sin {\mathrm{LV}}_E\mathrm{\&delta;}L+{\&dtri;}_U\\ \mathrm{\&delta;}\stackrel{\&CenterDot;}{L}={\mathrm{\&delta;V}}_N/\left(R_M+h\right)\\ \mathrm{\&delta;}\stackrel{\&CenterDot;}{\mathrm{\&lambda;}}={\mathrm{\&delta;V}}_E\sec L/\left(R_N+h\right)+V_E\sec L\tan L\mathrm{\&delta;}L/\left(R_N+h\right)\\ \mathrm{\&delta;}\stackrel{\&CenterDot;}{h}={\mathrm{\&delta;V}}_U\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{t}}_u={\mathrm{\&delta;t}}_{ru}\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{t}}_{ru}=-{\mathrm{\&beta;}}_{tru}{\mathrm{\&delta;t}}_{ru}\end{array}\right.$

式中，φ_E、φ_N、φ_U分别为东、北、天方向平台失准角，δV_E、δV_N、δV_U分别为载体东、北、天方向速度误差，δL、δλ、δh分别为载体纬度、经度、高度误差，δt_u为与时钟等效的距离误差，δt_ru为与时钟频率等效的距离率误差，R_M为椭球子午圈上各点的曲率半径，R_N为卯酉圈上各点的曲率半径，w_ie为地球转动角速率，f_E、f_N、f_U分别是惯导系统的比力在导航系下东、北、天方向上的分量，ε_E、ε_N、ε_U、分别为地理坐标系内陀螺的等效漂移在东、北、天方向的分量，分别为地理坐标系内加速计的等效漂移在东、北、天方向的分量，β_tru为反相关时间；

(6.4)以导航系统的姿态误差、速度误差、位置误差及伪距、伪距率误差为状态变量，建立惯性/卫星组合导航系统的状态方程：

$\stackrel{\&CenterDot;}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)$

其中，X为系统状态矢量，表示系统状态矢量的导数，F为系统状态转移矩阵，G为系统噪声驱动矩阵，W为系统噪声矢量，具体如下：

系统状态矢量：

$X={\&lsqb;{\mathrm{\&phi;}}_E,{\mathrm{\&phi;}}_N,{\mathrm{\&phi;}}_U,{\mathrm{\&delta;V}}_E,{\mathrm{\&delta;V}}_N,{\mathrm{\&delta;V}}_U,\mathrm{\&delta;}L,\mathrm{\&delta;}\mathrm{\&lambda;},\mathrm{\&delta;}h,{\mathrm{\&epsiv;}}_x,{\mathrm{\&epsiv;}}_y,{\mathrm{\&epsiv;}}_z,{\&dtri;}_x,{\&dtri;}_y,{\&dtri;}_z,{\mathrm{\&delta;t}}_u,{\mathrm{\&delta;t}}_{ru}\&rsqb;}_{17\&times;1}^T$

其中，φ_E、φ_N、φ_U分别为东、北、天方向平台失准角，δV_E、δV_N、δV_U分别为载体东、北、天方向速度误差，δL、δλ、δh分别为载体纬度、经度、高度误差，ε_x、ε_y、ε_z分别为载体系下陀螺随机常值漂移在x、y、z轴上的分量，▽_x、▽_y、▽_z分别为载体系下加速度计偏置在x、y、z轴上的分量，δt_u为与时钟等效的距离误差，δt_ru为与时钟频率等效的距离率误差；

状态转移矩阵：

$F={\left[\begin{array}{cccc}{\left(F_{ins}\right)}_{9\&times;9}& {\left(F_{sg}\right)}_{9\&times;6}& 0& 0\\ 0_{6\&times;9}& 0_{6\&times;6}& 0& 0\\ 0& 0& 0& 1\\ 0& 0& 0& -{\mathrm{\&beta;}}_{tru}\end{array}\right]}_{17\&times;17},F_{sg}={\left[\begin{array}{cc}{C}_b^n& 0_{3\&times;3}\\ 0_{3\&times;3}& C_b^n\\ 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right]}_{9\&times;6}$

其中，F_ins由步骤(6.3)中误差方程构成，为载体坐标系到导航坐标系的姿态转换矩阵，β_tru为反相关时间；

系统噪声驱动矩阵为G，且：

$G={\left[\begin{array}{cccc}{C}_b^n& 0_{3\&times;3}& 0& 0\\ 0_{3\&times;3}& C_b^n& 0& 0\\ 0_{9\&times;3}& 0_{9\&times;3}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]}_{17\&times;8}$

系统噪声矢量为W，且：

$W={\left[\begin{array}{cccccccc}{w}_{gx}& w_{gy}& w_{gz}& w_{ax}& w_{ay}& w_{az}& w_{tu}& w_{tru}\end{array}\right]}_{8\&times;1}^T$

其中，w_gx、w_gy、w_gz分别为陀螺仪在x轴、y轴、z轴方向的随机白噪声，w_ax、w_ay、w_az分别为加速度计在x轴、y轴、z轴方向的随机白噪声，w_tu、w_tru分别为伪距随机白噪声和伪距率随机白噪声；

(6.5)构建系统状态方程，并根据可见卫星的个数构建系统量测方程，如下：

Z(t)＝H(t)X(t)+V(t)

其中，Z(t)为系统观测矢量，H(t)为系统观测矩阵，V(t)为系统观测噪声阵，X为系统状态矢量；量测方程的维数及组合滤波器的维数根据可见卫星数量变化，变化关系如下：

$n=\left\{\begin{array}{cc}N& (0<N<4)\\ 4& (N\&GreaterEqual;4)\end{array}\right.$

其中，N为可见星数量，观测向量Z的维数为：2n×1；系统观测矩阵H为：2n×17；系统观测噪声方差R阵为：2n×2n；卡尔曼滤波增益阵为：17×2n；

伪距观测方程如下：

${\tilde{Z}}_{\mathrm{\&rho;}}={\tilde{H}}_{\mathrm{\&rho;}}\tilde{X}+{\tilde{V}}_{\mathrm{\&rho;}}=\left[\begin{array}{cccc}{0}_{n\&times;6}& {\tilde{H}}_{\mathrm{\&rho;}n1}& 0_{n\&times;6}& {\tilde{H}}_{\mathrm{\&rho;}n2}\end{array}\right]\tilde{X}+{\tilde{V}}_{\mathrm{\&rho;}}$

式中，为观测矢量，为观测矩阵，为观测噪声阵，为状态矢量，分别为：

$\begin{array}{cc}{\tilde{Z}}_{\mathrm{\&rho;}}={\left[\begin{array}{ccc}{\mathrm{\&delta;\&rho;}}_1& ...& {\mathrm{\&delta;\&rho;}}_n\end{array}\right]}_{1\&times;n}^T& {\tilde{V}}_{\mathrm{\&rho;}}={\left[\begin{array}{ccc}{v}_{\mathrm{\&rho;}}^1& ...& v_{\mathrm{\&rho;}}^1\end{array}\right]}_{1\&times;n}^T\end{array}$

δρ_j＝ρ_Ij-ρ_Gj＝e_j1δx+e_j2δy+e_j3δz+δt_u+ν_ρj

${\tilde{H}}_{\mathrm{\&rho;}n1}={\left[\begin{array}{ccc}{\tilde{a}}_{11}& {\tilde{a}}_{12}& {\tilde{a}}_{13}\\ \begin{array}{c}.\\ .\\ .\end{array}& \begin{array}{c}.\\ .\\ .\end{array}& \begin{array}{c}.\\ .\\ .\end{array}\\ {\tilde{a}}_{n1}& {\tilde{a}}_{n2}& {\tilde{a}}_{n3}\end{array}\right]}_{n\&times;3},{\tilde{H}}_{\mathrm{\&rho;}n2}={\left[\begin{array}{cc}1& 0\\ \begin{array}{c}.\\ .\\ .\end{array}& \begin{array}{c}.\\ .\\ .\end{array}\\ 1& 0\end{array}\right]}_{n\&times;2}$

其中，δρ_j为卫星伪距和载体相对卫星伪距之差，j＝1…n，为各通道伪距测量白噪声，ρ_Ij为载体相对每颗导航卫星的伪距，ν_ρj为伪距量测白噪声，δt_u为钟差引起的距离误差，ρ_Gj为导航卫星的伪距，展开如下，j＝1…n,i＝1,2,3则：

$\left\{\begin{array}{c}{\tilde{a}}_{j1}=\left(R_N+h\right)\&lsqb;-e_{j1}\sin L\cos \mathrm{\&lambda;}-e_{j2}\sin L\sin \mathrm{\&lambda;}\&rsqb;\\ +\&lsqb;R_N{\left(1-f\right)}^2+h\&rsqb;e_{j3}\cos L\\ {\tilde{a}}_{j2}=\left(R_N+h\right)\&lsqb;e_{j2}\cos L\cos \mathrm{\&lambda;}-e_{j1}\cos L\sin \mathrm{\&lambda;}\&rsqb;\\ {\tilde{a}}_{j3}=e_{j1}\cos L\cos \mathrm{\&lambda;}+e_{j2}\cos L\sin \mathrm{\&lambda;}+e_{j3}\sin L\end{array}\right.$

其中，e_j1、e_j2、e_j3分别为载体和第j颗卫星的x轴、y轴、z轴方向余弦，f为地球椭圆度；

伪距率观测方程如下所示：

${\tilde{Z}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}={\tilde{H}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}\tilde{X}+{\tilde{V}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}=\left[\begin{array}{cccc}{0}_{n\&times;3}& {\tilde{H}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}n1}& 0_{n\&times;9}& {\tilde{H}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}n2}\end{array}\right]\tilde{X}+{\tilde{V}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}$

式中，为观测矢量，为观测矩阵，为观测噪声阵，为状态矢量，分别为：

$\begin{array}{cc}{\tilde{Z}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}={\left[\begin{array}{ccc}\mathrm{\&delta;}{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}_1& ...& \mathrm{\&delta;}{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}_n\end{array}\right]}_{1\&times;n}^T& {\tilde{V}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}={\left[\begin{array}{ccc}{\mathrm{\&nu;}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}^1& ...& {\mathrm{\&nu;}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}^1\end{array}\right]}_{1\&times;n}^T\end{array}$

$\mathrm{\&delta;}{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}_j={\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}_{Ij}-{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}_{Gj}=e_{j1}\mathrm{\&delta;}\stackrel{\&CenterDot;}{x}+e_{j2}\mathrm{\&delta;}\stackrel{\&CenterDot;}{y}+e_{j3}\mathrm{\&delta;}\stackrel{\&CenterDot;}{z}+{\mathrm{\&delta;t}}_{ru}+{\mathrm{\&nu;}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}j}$

${\tilde{H}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}n1}={\left[\begin{array}{ccc}{\tilde{b}}_{11}& {\tilde{b}}_{12}& {\tilde{b}}_{13}\\ \begin{array}{c}.\\ .\\ .\end{array}& \begin{array}{c}.\\ .\\ .\end{array}& \begin{array}{c}.\\ .\\ .\end{array}\\ {\tilde{b}}_{n1}& {\tilde{b}}_{n2}& {\tilde{b}}_{n3}\end{array}\right]}_{n\&times;3},{\tilde{H}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}n2}={\left[\begin{array}{cc}0& 1\\ \begin{array}{c}.\\ .\\ .\end{array}& \begin{array}{c}.\\ .\\ .\end{array}\\ 0& 1\end{array}\right]}_{n\&times;2}$

其中，为卫星伪距率和载体相对卫星伪距率之差，j＝1…n，为各通道伪距率测量白噪声，为载体相对每颗导航卫星伪距率，为伪距率量测白噪声，δt_ru为钟漂引起的距离率误差，为导航卫星伪距率，展开如下，j＝1…n,i＝1,2,3则：

$\left\{\begin{array}{c}{\tilde{b}}_{j1}=-e_{j1}\sin \mathrm{\&lambda;}+e_{j2}\cos \mathrm{\&lambda;}\\ {\tilde{b}}_{j2}=-e_{j1}\sin L\cos \mathrm{\&lambda;}-e_{j2}\sin L\sin \mathrm{\&lambda;}+e_{j3}\cos L\\ {\tilde{b}}_{j3}=e_{j1}\cos L\cos \mathrm{\&lambda;}+e_{j2}\cos L\sin \mathrm{\&lambda;}+e_{j3}\sin L\end{array}\right.$

其中，e_j1、e_j2、e_j3分别为载体和第j颗卫星的x轴、y轴、z轴方向余弦；

综合伪距、伪距率观测方程，得到惯性/卫星组合导航系统观测方程如下：

$\tilde{Z}=\left[\begin{array}{c}{\tilde{Z}}_{\mathrm{\&rho;}}\\ {\tilde{Z}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}\end{array}\right]=\left[\begin{array}{c}{\tilde{H}}_{\mathrm{\&rho;}}\\ {\tilde{H}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}\end{array}\right]\tilde{X}+\left[\begin{array}{c}{\tilde{V}}_{\mathrm{\&rho;}}\\ {\tilde{V}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}\end{array}\right]=\tilde{H}\tilde{X}+\tilde{V}.$