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}.$