CN102636798A - 基于环路状态自检测的sins/gps深组合导航方法 - Google Patents

Abstract

本发明公开了基于环路状态自检测的SINS/GPS深组合导航方法，利用跟踪环路中的六路相关积分值进行环路状态自检测，建立环路预处理扩展卡尔曼滤波器模型对状态变量进行更新得到的伪距偏差和伪距率偏差输入到组合滤波器中；建立组合滤波器模型根据获得的观测量信息对状态量进行更新得到导航误差参数；利用导航误差参数对SINS系统进行状态校正；利用校正后的SINS状态与同时刻的卫星状态信息进行环路中码环及载波环的频率控制量估算，并利用环路预处理滤波器得到的本地信号和输入信号载波相位差的最优估计值对本地载波相位进行补偿，以控制本地码及本地载波信号的生成，提高SINS/GPS组合导航系统在高动态环境下的导航性能。

Description

基于环路状态自检测的SINS/GPS深组合导航方法

技术领域

本发明属于惯性与卫星组合导航的技术领域，特别是一种基于环路状态自检测的SINS/GPS深组合导航方法。

背景技术

惯性/卫星深组合技术是一种使用卫星导航系统内部跟踪环路信息作为观测量，并利用组合导航系统的输出反馈辅助GPS跟踪环路，提高高动态环境下接收机环路跟踪适应性能及导航精度的组合导航技术。

捷联惯性导航系统和卫星导航系统的特性具有良好互补性，基于惯性和卫星系统互补特性而构建的惯性/卫星组合导航系统可实现全球范围内全天时、全天候的高精度连续导航，是导航技术近年来以及未来相当长一段时期内发展的主要方向。近年来，在高动态环境适应性和抗干扰、高精度等导航需求的牵引下，惯性导航与卫星导航的组合导航技术组合的层次和信息利用与处理的水平不断深化，深组合模式与当前的松组合和紧组合有着显著的不同，是惯性/卫星组合导航系统三种组合模式中的技术复杂度最高的组合模式。目前，深组合技术已成为国外组合导航领域的一个研究热点，其核心是将惯性系统与卫星接收机的导航信息实现深度耦合，通过断开卫星接收机中跟踪环路的反馈通道，利用卫星/惯性组合的导航结果直接控制卫星导航接收机中本地信号，实现惯性和卫星接收机信息的双向辅助。

因此，惯性/卫星深度组合技术在导航系统要求更高导航性能的需求下，具有重要的应用研究价值与现实意义。

发明内容

本发明所解决的技术问题在于提供一种高动态环境下具有稳定导航性能的基于环路状态自检测的SINS/GPS深组合导航方法。

实现本发明目的的技术解决方案为：一种基于环路状态自检测的SINS/GPS深组合导航方法，包括以下步骤：

步骤1、对捷联惯性导航系统进行初始对准和GPS软件接收机参数初始化设置；

步骤2、GPS软件接收机对接收到的数字中频信号进行捕获、码和载波跟踪、导航电文解码、伪距估算、位置和速度解算处理；

步骤3、在GPS软件接收机各通道跟踪环路中建立环路预处理扩展卡尔曼滤波器模型，该模型利用码和载波跟踪环路中相关运算后生成的六路相关积分值作为观测量，利用跟踪环路本地信号特征参量作为状态量；环路预处理扩展卡尔曼滤波器对状态变量进行更新，得到状态估计值后通过参数转换后得到GPS卫星和载体视线方向上的伪距偏差和伪距率偏差输入到组合滤波器中；

步骤4、建立捷联惯性导航系统和GPS导航系统深组合卡尔曼滤波器模型，该模型的状态量为捷联惯性导航系统在东、北、天地理坐标系下三维姿态误差、三维速度误差、三维位置误差、三维陀螺常值漂移误差、三维陀螺一阶马尔可夫漂移误差和三维加速度计零偏，以及GPS接收机在地心地固坐标系(ECEF)下的等效时钟误差对应的距离，等效时钟频率误差对应的距离率；该模型的观测量为步骤(3)中所述的GPS卫星和载体视线方向上的伪距偏差和伪距率偏差，根据获得的观测量信息，对状态量进行更新得到导航误差参数；

步骤5、利用步骤(4)中所述的导航误差参数对捷联惯性导航系统进行状态校正；

步骤6、利用校正后的捷联惯性导航系统输出的位置和速度结果结合同时刻卫星位置、速度状态信息进行跟踪环路中码环及载波环的频率控制量估算，以控制下一周期跟踪环路中本地信号的生成，并利用环路预处理扩展卡尔曼滤波器估计得到的载波相位差最优估计值Δθ_carr对本地载波相位进行补偿，完成对跟踪环路中本地信号的反馈控制。

进一步地，本发明基于环路状态自检测的SINS/GPS深组合导航方法中，所述步骤3具体包括：

(a)建立环路预处理扩展卡尔曼滤波器模型

观测量为：跟踪环路的六路相关积分值I_E，I_P，I_L，Q_E，Q_P，Q_L，其观测模型为：

$I_E=\frac{\mathrm{AR}(\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca}}-\mathrm{\τ})}{2\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}}[\sin (\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}\mathrm{\ΔT}+\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}})-\sin \left(\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}}\right)]$

$I_P=\frac{\mathrm{AR}\left(\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca}}\right)}{2\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}}[\sin (\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}\mathrm{\ΔT}+\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}})-\sin \left(\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}}\right)]$

$I_L=\frac{\mathrm{AR}(\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca}}+\mathrm{\τ})}{2\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}}[\sin (\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}\mathrm{\ΔT}+\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}})-\sin \left(\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}}\right)]$

$Q_E=-\frac{\mathrm{AR}(\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca}}-\mathrm{\τ})}{2\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}}[\cos (\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}\mathrm{\ΔT}+\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}})-\cos \left(\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}}\right)]$

$Q_P=-\frac{\mathrm{AR}\left(\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca}}\right)}{2\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}}[\cos (\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}\mathrm{\ΔT}+\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}})-\cos \left(\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}}\right)]$

$Q_L=-\frac{\mathrm{AR}(\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca}}+\mathrm{\τ})}{2\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}}[\cos (\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}\mathrm{\ΔT}+\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}})-\cos \left(\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}}\right)]$

其中，I_E为同相支路的超前累加输出、I_P为同相支路的即时累加输出、I_L为同相支路的滞后累加输出、Q_E为正交支路的超前累加输出、Q_P为正交支路的即时累加输出、Q_L为正交支路的滞后累加输出，A为卫星星历码在一个积分周期内的积分值，由信号强度决定，并且与时间无关，Δφ_ca为本地码与接收码之间的相位差，τ为相关器间距，Δω_carr为本地载波与接收载波之间的角频率差，Δθ_carr为本地载波与接收载波之间的相位差，ΔT为相关积分时间，R为最大值为1的码自相关函数，具体表示形式如下：

$R\left(x\right)=\left\{\begin{array}{c}1-\left|x\right|,\left|x\right|\≤1\\ 0,\left|x\right|>1\end{array}\right.;$

状态量为： $X_{\mathrm{loop}}=\left[\begin{array}{cccc}\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca}}& \mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}}& {\mathrm{\ω}}_{\mathrm{rec}}& {\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{rec}}\end{array}\right],$ 状态模型为：

$\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca},k+1}=\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca},k}+(\frac{f_{\mathrm{rec}}-f_{\mathrm{IF}}}{f_{\mathrm{rec}}}-\frac{f_{\mathrm{local}}-f_{\mathrm{IF}}}{f_{\mathrm{local}}})\×\mathrm{\ΔT}$

$\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr},k+1}=\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr},k}+{\mathrm{\ω}}_{\mathrm{rec},k}\×\mathrm{\ΔT}+{\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{rec},k}\×\frac{\mathrm{\Δ}{T}^2}{2}$

${\mathrm{\ω}}_{\mathrm{rec},k+1}={\mathrm{\ω}}_{\mathrm{rec},k}+{\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{rec},k}\×\mathrm{\ΔT}$

${\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{rec},k+1}={\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{rec},k+1}$

其中，ω_rec为接收载波角频率，

为接收载波角频率变化率，k为k时刻的下标，

为接收载波频率，f_local为本地载波频率，f_IF为载波中频频率；

(b)环路预处理扩展卡尔曼滤波器的输出信息

利用环路预处理滤波器估计值Δφ_ca及Δω_carr推算出的GPS卫星和载体视线方向上的伪距偏差Δρ和伪距率偏差计算方法如下：

$\mathrm{\Δ\ρ}=\frac{c}{f_{\mathrm{codeBasis}}}\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca}}$

$\mathrm{\Δ}\stackrel{\·}{\mathrm{\ρ}}=\frac{c}{f_L}\frac{\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}}{2\mathrm{\π}}$

其中，c为光速，f_codeBasis＝1.023MHz为基准码频率，f_L＝1575.42MHz为卫星发射信号的调制载波频率。

进一步地，本发明基于环路状态自检测的SINS/GPS深组合导航方法中，在步骤2与步骤3之间还包括利用码和载波跟踪环路中的六路相关积分值对码和载波跟踪环路进行状态自检测和载噪比信息计算的步骤，判断如果该通道未发生失锁则跟踪环路的跟踪状态正常并执行步骤3，否则返回步骤2。

进一步地，本发明基于环路状态自检测的SINS/GPS深组合导航方法中，利用码和载波跟踪环路中的六路相关积分值对码和载波跟踪环路进行锁定自检测及载噪比信息计算具体为：

按设定周期计算接收信号载噪比信息，并判断跟踪时刻前面相邻n个积分周期的相关积分结果I_P值符号，1≤n≤10且n为正整数，所述积分周期为1ms，如果在相邻n个积分周期内出现两次IP值符号变化且载噪比下降大于阈值(阈值为10～20)，则判定该通道本时刻的信号发生失锁现象，如仅有载噪比下降而符号变化小于两次情况，则判定该通道信号还未失锁；

载噪比信息计算具体为：

(1)计算每个时刻积分结果I_P、Q_P的平方和，构造序列Z，Z_k表示如下：

$Z_k=(I_{p,k}^2+Q_{p,k}^2)$

其中k为标示时刻的下标，I_P为同相支路的即时累加输出，Q_P为正交支路的即时累加输出；

(2)计算载波功率均值：

${\stackrel{\‾}{P}}_{\mathrm{carr}}=\sqrt{{\stackrel{\‾}{Z}}^2-{\mathrm{\σ}}_Z^2}$

其中

为序列Zk的均值，

为载波功率均值，

为序列Z_k的方差， ${\mathrm{\σ}}_Z^2=\left(\frac{1}{k-1}\right){\mathrm{\Σ}}_k{(Z_k-\stackrel{\‾}{Z})}^2;$

(3)计算I_P、Q_P噪声方差

${\mathrm{\σ}}_{\mathrm{IQ}}^2=\frac{1}{2}(\stackrel{\‾}{Z}-\sqrt{{\stackrel{\‾}{Z}}^2-{\mathrm{\σ}}_Z^2})$

(4)计算载噪比C/N₀：

$C/N_0=10{\log }_{10}\left[\frac{{\stackrel{\‾}{P}}_{\mathrm{carr}}}{2T{\mathrm{\σ}}_{\mathrm{IQ}}^2}\right]$

其中T为载噪比设定积分时间，载噪比单位为dB-Hz。

进一步地，本发明基于环路状态自检测的SINS/GPS深组合导航方法中，所述步骤4中状态量为：

$X_I={\left[\begin{array}{cccccccccccccccccc}{\mathrm{\φ}}_E& {\mathrm{\φ}}_N& {\mathrm{\φ}}_U& {\mathrm{\δv}}_E& {\mathrm{\δv}}_N& {\mathrm{\δv}}_U& \mathrm{\δL}& \mathrm{\δ\λ}& \mathrm{\δh}& {\mathrm{\ϵ}}_{\mathrm{bx}}& {\mathrm{\ϵ}}_{\mathrm{by}}& {\mathrm{\ϵ}}_{\mathrm{bz}}& {\mathrm{\δ}}_{\mathrm{rx}}& {\mathrm{\ϵ}}_{\mathrm{xy}}& {\mathrm{\ϵ}}_{\mathrm{rz}}& {\▿}_z& {\▿}_y& {\▿}_z\end{array}\right]}^T$

X_G＝[δt_u δt_ru]^T

其中，X_I为SINS系统中对应的状态量，分别为三维姿态误差(φ_E φ_N φ_U)、三维速度误差(δv_E δv_N δv_U)、三维位置误差(δL δλ δh)、三维陀螺常值漂移误差(ε_bx ε_by ε_bz)、三维陀螺一阶马尔可夫漂移误差(ε_rx ε_ry ε_rz)和三维加速度计零偏 $\left(\begin{array}{ccc}{\▿}_x& {\▿}_y& {\▿}_z\end{array}\right),$ T为转置运算；X_G为GPS系统中对应的状态量，分别为一维接收机等效时钟误差对应的距离δt_u和一维等效时钟频率误差对应的距离率δt_ru；

捷联惯性导航系统和GPS导航系统深度组合系统状态方程为：

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

其中，t为时间下标，F(t)为一步转移阵，G(t)为系统噪声系数阵，W(t)为系统噪声，X(t)为系统状态量，由X(t)＝[X_I X_G]表示。

进一步地，本发明基于环路状态自检测的SINS/GPS深组合导航方法中，所述步骤6中利用校正过的捷联惯性导航系统输出位置和速度结果结合同时刻卫星位置、速度状态信息进行跟踪环路中码环及载波环的频率控制量估算方法如下：

$v_{\mathrm{user}\_\mathrm{sate}}^{\mathrm{ECEF}}={({\stackrel{\→}{v}}_u^E-{\stackrel{\→}{v}}_s^E)}^T\·\frac{{\stackrel{\→}{r}}_s^E-{\stackrel{\→}{r}}_u^E}{|{\stackrel{\→}{r}}_s^E-{\stackrel{\→}{r}}_u^E|}$

$f_{\mathrm{code}}=f_{\mathrm{codeBasis}}+\frac{v_{\mathrm{user}\_\mathrm{sate}}^{\mathrm{ECEF}}}{c}\×f_{\mathrm{codeBasis}}$

$f_{\mathrm{carr}}=f_{\mathrm{IF}}+\frac{v_{\mathrm{user}\_\mathrm{sate}}^{\mathrm{ECEF}}}{c}\×f_L$

其中，为接收机和卫星相对速度在径向方向上的投影，

分别为接收机与卫星在地心地固直角坐标系中的速度，分别为接收机与卫星在地心地固直角坐标系中位置，f_code为码环频率控制量，f_carr为载波环频率控制量，f_IF为载波中频频率；

利用环路预处理扩展卡尔曼滤波器得到的本地信号和输入信号载波相位差的最优估计值Δθ_carr，k+1对本地载波相位进行补偿，具体补偿方法如下：

θ_carr，k+1＝θ_carr，k+Δθ_carr+1

其中，θ_carr，k+1为k+1时刻本地载波相位值。

本发明与现有技术相比具有显著的优点：

1、本发明具有跟踪环路状态自检测功能，可对软件接收机中可见星信号跟踪状态进行实时监测，并为环路信号预处理滤波器及深组合导航组合滤波器提供可靠观测信息。

2、本发明设计了环路预处理扩展卡尔曼滤波器，提高了对本地信号特征参量的估计精度，并为深组合系统中组合滤波器提供更加精确的观测量信息。

3、本发明直接利用环路预处理滤波器输出作为组合滤波器输入，提供了深组合滤波模型与设计方法。

4、本发明利用校正后的SINS状态信息与同时刻的卫星状态信息进行环路中码环及载波环的频率控制量估算，并利用环路预处理滤波器得到的本地信号和输入信号载波相位差的最优估计值对本地载波相位进行补偿，以控制本地码及本地载波信号的生成。

5、本发明提高SINS/GPS组合导航系统在高动态环境下的导航性能。

下面结合附图对本发明作进一步详细描述。

附图说明

图1为本发明基于环路状态自检测的SINS/GPS深组合导航方法的流程图

图2为SINS/GPS信息深组合中载波跟踪环路相位补偿前后效果对比图；其中图2(a)为载波相位补偿前跟踪结果；图2(b)为载波相位补偿后跟踪结果。

图3为高动态环境下SINS/GPS紧组合与深组合环路跟踪结果对比图，其中图3(a)为所设计的高动态航迹下的紧组合环路跟踪结果；图3(b)为图3(a)的局部放大图；图3(c)为所设计的高动态航迹下的深组合环路跟踪结果；图3(d)为图3(c)的局部放大图。

图4为高动态环境下SINS/GPS深组合导航结果图，其中图4(a)为深组合位置误差图，图4(b)为深组合速度误差图。

具体实施方式

如图1所示，本发明一种基于环路状态自检测的SINS/GPS深组合导航方法，包括以下步骤：

步骤1、对捷联惯性导航系统和GPS软件接收机进行初始化，其中捷联惯性导航系统初始化包括初始对准，GPS软件接收机系统初始化包括接收机系统参数设置；

步骤2、GPS软件接收机对接收到的数字中频信号进行捕获、码和载波跟踪、导航电文解码、伪距估算、位置和速度解算处理；

步骤3、利用码和载波跟踪环路中的六路相关积分值对码和载波跟踪环路进行状态自检测和载噪比信息计算，判断如果该通道未发生失锁则跟踪环路的跟踪状态正常并执行步骤3，否则返回步骤2；

具体为：利用跟踪环路的六路相关积分结果中的IP值对环路进行锁定自检测及载噪比信息计算，锁定自检测方法为判断跟踪时刻前面相邻5个积分周期(积分周期一般为1ms)的相关积分结果的IP值符号，并结合载噪比信息进行综合判定，如跟踪时刻前面相邻5个积分周期内出现两次IP值符号变化情况并且载噪比下降幅度大于一定阈值时(阈值一般可设为20)，则判定该通道本时刻的信号发生失锁现象，如仅有载噪比下降而符号变化小于两次情况，则判定该通道信号还未失锁，通过该检测方法对卫星环路的跟踪状态进行同步的监测，可在卫星信号进行处理的同时观测到卫星信号的跟踪状况；

载噪比计算具体为：

(1)计算每个时刻积分结果I_P、Q_P的平方和，构造序列Z，Z_k表示如下：

$Z_k=(I_{p,k}^2+Q_{p,k}^2)$

其中k为标示时刻的下标，I_P为同相支路的即时累加输出，Q_P为正交支路的即时累加输出；

(2)计算载波功率均值：

${\stackrel{\‾}{P}}_{\mathrm{carr}}=\sqrt{{\stackrel{\‾}{Z}}^2-{\mathrm{\σ}}_Z^2}$

其中

为序列Z_k的均值，

为序列Z_k的方差，

${\mathrm{\σ}}_Z^2=\left(\frac{1}{k-1}\right){\mathrm{\Σ}}_k{(Z_k-\stackrel{\‾}{Z})}^2;$

(3)计算I_P、Q_P噪声方差

${\mathrm{\σ}}_{\mathrm{IQ}}^2=\frac{1}{2}(\stackrel{\‾}{Z}-\sqrt{{\stackrel{\‾}{Z}}^2-{\mathrm{\σ}}_Z^2})$

(4)计算载噪比C/N₀：

$C/N_0=10{\log }_{10}\left[\frac{{\stackrel{\‾}{P}}_{\mathrm{carr}}}{2T{\mathrm{\σ}}_{\mathrm{IQ}}^2}\right]$

其中T为载噪比设定积分时间，载噪比单位为dB-Hz。

步骤4、在GPS软件接收机各通道跟踪环路中建立环路预处理扩展卡尔曼滤波器模型，该模型利用跟踪环路中相关运算后生成的六路相关积分值作为观测量，利用跟踪环路本地信号特征参量作为状态量；环路预处理扩展卡尔曼滤波器对状态变量进行更新，得到状态估计值后通过参数转换单元进行参数转换后得到GPS卫星和载体视线方向上的伪距偏差和伪距率偏差输入到组合滤波器中，具体为：

(a)建立环路预处理扩展卡尔曼滤波器模型

观测量为相关运算后生成六路相关积分值I_E，I_P，I_L，Q_E，Q_P，Q_L，具体计算过程为跟踪处理过程中输入的卫星中频信号分别同时与同相支路(I支路)及正交支路(Q支路)上的载波复制信号相乘，然后混频结果再分别同时与超前、即时和滞后复制本地码进行相关运算，相关运算由数字相关器完成；

其观测模型为：

$I_E=\frac{\mathrm{AR}(\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca}}-\mathrm{\τ})}{2\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}}[\sin (\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}\mathrm{\ΔT}+\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}})-\sin \left(\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}}\right)]$

$I_P=\frac{\mathrm{AR}\left(\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca}}\right)}{2\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}}[\sin (\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}\mathrm{\ΔT}+\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}})-\sin \left(\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}}\right)]$

$I_L=\frac{\mathrm{AR}(\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca}}+\mathrm{\τ})}{2\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}}[\sin (\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}\mathrm{\ΔT}+\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}})-\sin \left(\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}}\right)]$

$Q_E=-\frac{\mathrm{AR}(\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca}}-\mathrm{\τ})}{2\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}}[\cos (\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}\mathrm{\ΔT}+\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}})-\cos \left(\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}}\right)]$

$Q_P=-\frac{\mathrm{AR}\left(\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca}}\right)}{2\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}}[\cos (\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}\mathrm{\ΔT}+\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}})-\cos \left(\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}}\right)]$

$Q_L=-\frac{\mathrm{AR}(\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca}}+\mathrm{\τ})}{2\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}}[\cos (\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}\mathrm{\ΔT}+\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}})-\cos \left(\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}}\right)]$

其中，I_E为同相支路的超前累加输出、I_P为同相支路的即使累加输出、I_L为同相支路的滞后累加输出、Q_E为正交支路的超前累加输出、Q_P为正交支路的即使累加输出、Q_L为正交支路的滞后累加输出，A为卫星星历码在一个积分周期内的积分值，由信号强度决定，并且与时间无关，Δφ_ca为本地码与接收码之间的相位差，τ为相关器间距，Δω_carr为本地载波与接收载波之间的角频率差，Δθ_carr为本地载波与接收载波之间的相位差，ΔT为相关积分时间，R为最大值为1的码自相关函数，具体表示形式如下：

$R\left(x\right)=\left\{\begin{array}{c}1-\left|x\right|,\left|x\right|\≤1\\ 0,\left|x\right|>1\end{array}\right.;$

状态量为： $X_{\mathrm{loop}}=\left[\begin{array}{cccc}\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca}}& \mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}}& {\mathrm{\ω}}_{\mathrm{rec}}& {\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{rec}}\end{array}\right],$ 状态模型为：

$\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca},k+1}=\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca},k}+(\frac{f_{\mathrm{rec}}-f_{\mathrm{IF}}}{f_{\mathrm{rec}}}-\frac{f_{\mathrm{local}}-f_{\mathrm{IF}}}{f_{\mathrm{local}}})\×\mathrm{\ΔT}$

$\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr},k+1}=\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr},k}+{\mathrm{\ω}}_{\mathrm{rec},k}\×\mathrm{\ΔT}+{\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{rec},k}\×\frac{\mathrm{\Δ}{T}^2}{2}$

${\mathrm{\ω}}_{\mathrm{rec},k+1}={\mathrm{\ω}}_{\mathrm{rec},k}+{\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{rec},k}\×\mathrm{\ΔT}$

${\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{rec},k+1}={\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{rec},k+1}$

其中，ω_rec为接收载波角频率，为接收载波角频率变化率，k为k时刻的下标，为接收载波频率，f_local为本地载波频率，f_IF为载波中频频率；

(b)环路预处理扩展卡尔曼滤波器的输出信息

利用环路预处理滤波器估计值Δφ_ca及Δω_carr推算出的GPS卫星和载体视线方向上的伪距偏差Δρ和伪距率偏差

计算方法如下：

$\mathrm{\Δ\ρ}=\frac{c}{f_{\mathrm{codeBasis}}}\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca}}$

$\mathrm{\Δ}\stackrel{\·}{\mathrm{\ρ}}=\frac{c}{f_L}\frac{\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}}{2\mathrm{\π}}$

其中，c为光速，f_codeBasis＝1.023MHz为基准码频率，f_L＝1575.42MHz为卫星发射信号的调制载波频率，系统观测方程为：

$\left[\begin{array}{c}\mathrm{\Δ}{\mathrm{\ρ}}_1\\ \·\\ \·\\ \·\\ \mathrm{\Δ}{\mathrm{\ρ}}_i\\ \mathrm{\Δ}{\stackrel{\·}{\mathrm{\ρ}}}_i\\ \·\\ \·\\ \·\\ \mathrm{\Δ}{\stackrel{\·}{\mathrm{\ρ}}}_i\end{array}\right]=\left[\begin{array}{cccccccc}{a}_{11}& a_{12}& a_{13}& 0& 0& 0& 1& 0\\ & \·& & & \·& & & \\ & \·& & & \·& & & \\ & \·& & & \·& & & \\ a_{i1}& a_{i2}& a_{i3}& 0& 0& 0& 1& 0\\ 0& 0& 0& b_{11}& b_{12}& b_{13}& 0& 1\\ & \·& & & \·& & & \\ & \·& & & \·& & & \\ & \·& & & \·& & & \\ 0& 0& 0& b_{i1}& b_{i2}& b_{i3}& 0& 1\end{array}\right]\left[\begin{array}{c}\mathrm{\δL}\\ \mathrm{\δ\λ}\\ \mathrm{\δh}\\ {\mathrm{\δv}}_E\\ {\mathrm{\δv}}_N\\ {\mathrm{\δv}}_U\\ {\mathrm{\δt}}_u\\ {\mathrm{\δt}}_{\mathrm{ru}}\end{array}\right]+\left[\begin{array}{c}{v}_{\mathrm{\ρ}1}\\ \·\\ \·\\ \·\\ v_{\mathrm{\ρi}}\\ v_{\stackrel{\·}{\mathrm{\ρ}}1}\\ \·\\ \·\\ \·\\ v_{\stackrel{\·}{\mathrm{\ρ}}i}\end{array}\right]$

其中，v_ρi与

分别为Δ_ρi与

对应的观测噪声，δL δλ δh为SINS系统中三维位置误差，δv_E δv_N δv_U为SINS系统中三维速度误差，δt_u为一维接收机等效时钟误差对应的距离，δt_ru为一维接收机等效时钟频率误差对应的距离率，a_i1，a_i2，a_i3与b_i1，b_i2，b_i3的表示式分别如下：

a_i1＝(R_N+h)[-e_i1 sinLcosλ-e_i2 sinLsinλ]+[R_N(1-f)²+h]e_i3 cosL

a_i2＝(R_N+h)[e_i2 cosLcosλ-e_i1 cosLsinλ]

a_i3＝e_i1 cosLcosλ+e_i2 cosLsinλ+e_i3 sinL

b_i1＝-e_i1 sinλ+e_i2 cosλ

b_i2＝-e_i1 sinLcosλ-e_i2 sinLsinλ+e_i3 cosL

a_i3＝e_i1 cosLcosλ+e_i2 cosLsinλ+e_i3 sinL

上式中，R_N为地球垂直子午面的法线平面的曲率半径，通过R_N＝R_e(1+fsin²L)公式计算，其中R_e为地球赤道面半径，f为地球椭圆率，L、λ、h分别为当前时刻载体运动的三维位置纬度、经度和高度；

e_ij为由组合导航结果预测出第i颗GPS卫星在载体位置处的单位观测矢量，其计算方法如下：

$\left\{\begin{array}{c}{e}_{\mathrm{il}}=\frac{x_{\mathrm{SINS}}-x_s}{r_i}\\ e_{i2}=\frac{y_{\mathrm{SINS}}-y_s}{r_i},r_i={[{(x_{\mathrm{SINS}}-x_s)}^2+{(y_{\mathrm{SINS}}-y_s)}^2+{(z_{\mathrm{SINS}}-z_s)}^2]}^{\frac{1}{2}}\\ e_{i3}=\frac{z_{\mathrm{SINS}}-z_s}{r_i}\end{array}\right.$

其中x_SINS，y_SINS，z_SINS为SINS在ECEF坐标系下位置，x_s，y_s，z_s为第i颗GPS卫星在ECEF坐标系下位置，r_i为第i颗GPS卫星和载体之间的距离。

步骤5、建立捷联惯性导航系统和GPS导航系统深度组合滤波器模型，该模型的状态量为捷联惯性导航系统在东、北、天地理坐标系下三维姿态误差、三维速度误差、三维位置误差、三维陀螺常值漂移误差、三维陀螺一阶马尔可夫漂移误差和三维加速度计零偏，以及GPS接收机在地心地固坐标系(ECEF)下的等效时钟误差对应的距离，等效时钟频率误差对应的距离率，状态量表示为：

$X_I={\left[\begin{array}{cccccccccccccccccc}{\mathrm{\φ}}_E& {\mathrm{\φ}}_N& {\mathrm{\φ}}_U& {\mathrm{\δv}}_E& {\mathrm{\δv}}_N& {\mathrm{\δv}}_U& \mathrm{\δL}& \mathrm{\δ\λ}& \mathrm{\δh}& {\mathrm{\ϵ}}_{\mathrm{bx}}& {\mathrm{\ϵ}}_{\mathrm{by}}& {\mathrm{\ϵ}}_{\mathrm{bz}}& {\mathrm{\δ}}_{\mathrm{rx}}& {\mathrm{\ϵ}}_{\mathrm{xy}}& {\mathrm{\ϵ}}_{\mathrm{rz}}& {\▿}_z& {\▿}_y& {\▿}_z\end{array}\right]}^T$

X_G＝[δt_u δt_ru]^T

其中，X_I为SINS系统中对应的状态量，分别为东、北、天地理坐标系下三维姿态误差(φ_E φ_N φ_U)、三维速度误差(δv_E δv_N δv_U)、三维位置误差(δL δλ δh)、三维陀螺常值漂移误差(ε_bx ε_by ε_bz)、三维陀螺一阶马尔可夫漂移误差(ε_rx ε_ry ε_rz)和三维加速度计零偏 $\left(\begin{array}{ccc}{\▿}_x& {\▿}_y& {\▿}_z\end{array}\right),$ T为转置运算；X_G为GPS系统中对应的状态量，分别为一维接收机等效时钟误差对应的距离δt_u和一维等效时钟频率误差对应的距离率δt_ru；状态微分方程如以下形式：

$\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\φ}}}_E=(w_{\mathrm{ie}}\sin L+\frac{v_E}{R_N+h}\tan L){\mathrm{\φ}}_N-(w_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h}){\mathrm{\φ}}_U\\ -\frac{\mathrm{\δ}{v}_N}{R_M+h}+{\mathrm{\ϵ}}_E\\ {\stackrel{\·}{\mathrm{\φ}}}_N=-(w_{\mathrm{ie}}\sin L+\frac{v_E}{R_N+h}\tan L){\mathrm{\φ}}_E-\frac{v_N}{R_M+h}{\mathrm{\φ}}_U\\ +\frac{\mathrm{\δ}{v}_E}{R_N+h}-w_{\mathrm{ie}}\sin \mathrm{L\δL}+{\mathrm{\ϵ}}_N\\ {\stackrel{\·}{\mathrm{\φ}}}_U=(w_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h}){\mathrm{\φ}}_E+\frac{v_N}{R_M+h}{\mathrm{\φ}}_N\\ +\frac{\mathrm{\δ}{v}_E}{R_N+h}\tan L+(w_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h}{\sec }^{2}L)\mathrm{\δL}+{\mathrm{\ϵ}}_U\end{array}\right]$

$\left[\begin{array}{c}\mathrm{\δ}\stackrel{\·}{L}=\frac{\mathrm{\δ}{v}_N}{R_M+h}\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{\sec L}{R_N+h}\mathrm{\δ}{v}_E+\frac{v_E}{R_N+h}\sec L\tan \mathrm{L\δL}\\ \mathrm{\δ}\stackrel{\·}{h}=\mathrm{\δ}{v}_U\end{array}\right]$

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{v}}_E=-f_u{\mathrm{\φ}}_N+f_N{\mathrm{\φ}}_U+(\frac{v_N}{R_M+h}\tan L-\frac{v_U}{R_M+h})\mathrm{\δ}{v}_E\\ +(2w_{\mathrm{ie}}\sin L+\frac{v_E}{R_N+h}\tan L)\mathrm{\δ}{v}_N-(2w_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h})\mathrm{\δ}{v}_U\\ +(2w_{\mathrm{ie}}\cos L{v}_N+\frac{v_E{v}_N}{R_N+h}{\sec }^{2}L+2w_{\mathrm{ie}}\sin L{v}_U)\mathrm{\δL}+{\▿}_E\\ \mathrm{\δ}{\stackrel{\·}{v}}_N=f_U{\mathrm{\φ}}_E-f_E{\mathrm{\φ}}_U\\ -({2w}_{\mathrm{ie}}\sin L+\frac{v_E}{R_N+h}\tan L)\mathrm{\δ}{v}_E-\frac{v_U}{R_M+h}\mathrm{\δ}{v}_N-\frac{v_N}{R_M+h}\mathrm{\δ}{v}_U\\ -({2w}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h}{\sec }^{2}L)v_E\mathrm{\δL}+{\▿}_N\\ \mathrm{\δ}{\stackrel{\·}{v}}_U=-f_N{\mathrm{\φ}}_E+f_E{\mathrm{\φ}}_N\\ +({2w}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h})\mathrm{\δ}{v}_E+2\frac{v_N}{R_M+h}\mathrm{\δ}{v}_N\\ -{2w}_{\mathrm{ie}}\sin L{v}_E\mathrm{\δL}+{\▿}_U\end{array}\right.$

$\left\{\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{t}}_u=\mathrm{\δ}{t}_{\mathrm{ru}}\\ \mathrm{\δ}{\stackrel{\·}{t}}_{\mathrm{ru}}=0\end{array}\right.$

上式中，L、λ、h分别为当前时刻载体运动的三维位置纬度、经度、高度，v_E、v_E、v_E为当前时刻载体运动的东向、北向、天向三维速度，w_ie为地球相对于惯性参考系的转动角速率，具体值为w_ie＝7.292115147×10^-5rad/s，ε_E、ε_N、ε_U为三维陀螺误差，

为三维加速度计测量误差，f_E、f_N、f_U分别为当前时刻加速度计的比力(北向、东向、天向)，R_M为地球参考椭球子午面内的曲率半径，通过式R_M＝R_e(1-2f+3f sinL)sinL计算，R_e为地球赤道面半径，f为地球椭圆率。

捷联惯性导航系统和GPS导航系统深度组合系统状态方程为：

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

其中，F(t)为一步转移阵，G(t)为系统噪声系数阵，W(t)为系统噪声，X(t)为系统状态量，由X(t)＝[X_I X_G]表示。

所述观测量为步骤4中获得的GPS卫星和载体视线方向上的伪距偏差和伪距率偏差；根据捷联惯性导航系统和GPS导航系统输出的量测信息，对状态量进行更新得到导航误差参数；

步骤6、利用步骤5中所述的导航误差参数对捷联惯性导航系统进行状态校正；

步骤7、利用校正后的捷联惯性导航系统输出的位置和速度结果结合同时刻卫星位置、速度状态信息进行跟踪环路中码环及载波环的频率控制量估算，以控制下一周期跟踪环路中本地信号的生成，具体为：

$v_{\mathrm{user}\_\mathrm{sate}}^{\mathrm{ECEF}}={({\stackrel{\→}{v}}_u^E-{\stackrel{\→}{v}}_s^E)}^T\·\frac{{\stackrel{\→}{r}}_s^E-{\stackrel{\→}{r}}_u^E}{|{\stackrel{\→}{r}}_s^E-{\stackrel{\→}{r}}_u^E|}$

$f_{\mathrm{code}}=f_{\mathrm{codeBasis}}+\frac{v_{\mathrm{user}\_\mathrm{sate}}^{\mathrm{ECEF}}}{c}\×f_{\mathrm{codeBasis}}$

$f_{\mathrm{carr}}=f_{\mathrm{IF}}+\frac{v_{\mathrm{user}\_\mathrm{sate}}^{\mathrm{ECEF}}}{c}\×f_L$

其中，为接收机和卫星相对速度在径向方向上的投影，分别为接收机与卫星在地心地固直角坐标系中的速度，

分别为接收机与卫星在地心地固直角坐标系中位置，f_code为码环频率控制量，f_carr为载波环频率控制量，f_IF为载波中频频率；

并利用环路预处理扩展卡尔曼滤波器估计得到的载波相位差最优估计值Δθ_carr对本地载波相位进行补偿，完成对跟踪环路本地信号的反馈控制。

实施例

本实施例基于高动态环境下仿真的GPS中频数据及SINS数据，对基于环路状态自检测的SINS/GPS信息深度耦合组合导航方法进行实验，得出有益的结论。

首先设计一条高动态环境下的飞行航迹，仿真时间150秒，飞行动态主要包括了加速滑跑、抬头、加速爬升、匀速平飞等过程，并在平飞阶段130秒处加载30g的加速度，持续8s，随即恢复正常，接着利用此航迹进行GPS中频数据的仿真，最后利用GPS中频仿真数据及SINS仿真数据进行SINS/GPS信息深度耦合组合导航方法对高过载的适应性能及导航精度测试，部分时间的测试结果如图2-4所示，其中图2(a)为载波相位补偿前跟踪结果，由于未对载波环路进行相位补偿，因此出现了本地载波信号相位旋转及解调导航电文幅值周期性变化的问题，而图2(b)为载波跟踪环路相位补偿后的跟踪效果图，可发现解决了以上所述问题。

图3为高动态环境下SINS/GPS紧组合与深组合环路跟踪结果对比图，其中图3(a)为所设计的高动态航迹下的紧组合环路跟踪结果，由图中可见，当130秒开始加载30g加速度之后，紧组合环路逐渐开始出现失锁现象，图3(b)为图3(a)的局部放大图，而深组合环路跟踪结果图3(c)中环路仍可以保持跟踪，图3(d)为图3(c)的局部放大图，仅在30g加速度加载后的短暂时间内有较小的幅值波动，但仍处于跟踪状态，不影响后续的导航电文二值化处理，表明了SINS/GPS深组合导航方法对高过载的适应性能，图3(a)中失锁后环路跟踪状态的标示部分也表明了锁定状态综合判定方法的有效性。

图4为高动态环境下SINS/GPS深组合导航结果图，其中图4(a)为深组合位置误差图，图4(b)为速度误差图，从对比图中可见，当130秒开始加载30g加速度之后，深组合模式仍可以保持5米以内的定位精度与0.2m/s以内的定速精度，仅在高度方向上误差稍稍偏大一些，验证了SINS/GPS深组合导航方法在高动态环境下稳定的导航性能。

Claims (6)

1.一种基于环路状态自检测的SINS/GPS深组合导航方法，其特征在于，包括以下步骤：

步骤1、对捷联惯性导航系统进行初始对准和GPS软件接收机参数初始化设置；

步骤2、GPS软件接收机对接收到的数字中频信号进行捕获、码和载波跟踪、导航电文解码、伪距估算、位置和速度解算处理；

步骤3、在GPS软件接收机各通道跟踪环路中建立环路预处理扩展卡尔曼滤波器模型，该模型利用码和载波跟踪环路中相关运算后生成的六路相关积分值作为观测量，利用跟踪环路本地信号特征参量作为状态量；环路预处理扩展卡尔曼滤波器对状态变量进行更新，得到状态估计值后通过参数转换后得到GPS卫星和载体视线方向上的伪距偏差和伪距率偏差输入到组合滤波器中；

步骤4、建立捷联惯性导航系统和GPS导航系统深组合卡尔曼滤波器模型，该模型的状态量为捷联惯性导航系统在东、北、天地理坐标系下三维姿态误差、三维速度误差、三维位置误差、三维陀螺常值漂移误差、三维陀螺一阶马尔可夫漂移误差和三维加速度计零偏，以及GPS接收机在地心地固坐标系(ECEF)下的等效时钟误差对应的距离，等效时钟频率误差对应的距离率；该模型的观测量为步骤(3)中所述的GPS卫星和载体视线方向上的伪距偏差和伪距率偏差，根据获得的观测量信息，对状态量进行更新得到导航误差参数；

步骤5、利用步骤4中所述的导航误差参数对捷联惯性导航系统进行状态校正；

步骤6、利用校正后的捷联惯性导航系统输出的位置和速度结果结合同时刻卫星位置、速度状态信息进行跟踪环路中码环及载波环的频率控制量估算，以控制下一周期跟踪环路中本地信号的生成，并利用环路预处理扩展卡尔曼滤波器估计得到的载波相位差最优估计值Δθ_carr对本地载波相位进行补偿，完成对跟踪环路中本地信号的反馈控制。

2.根据权利要求1所述的基于环路状态自检测的SINS/GPS深组合导航方法，其特征在于，步骤3具体包括：

(a)建立环路预处理扩展卡尔曼滤波器模型

观测量为：跟踪环路的六路相关积分值I_E，I_P，I_L，Q_E，Q_P，Q_L，其观测模型为：

$I_E=\frac{\mathrm{AR}(\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca}}-\mathrm{\τ})}{2\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}}[\sin (\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}\mathrm{\ΔT}+\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}})-\sin \left(\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}}\right)]$

$I_P=\frac{\mathrm{AR}\left(\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca}}\right)}{2\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}}[\sin (\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}\mathrm{\ΔT}+\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}})-\sin \left(\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}}\right)]$

$I_L=\frac{\mathrm{AR}(\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca}}+\mathrm{\τ})}{2\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}}[\sin (\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}\mathrm{\ΔT}+\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}})-\sin \left(\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}}\right)]$

$Q_E=-\frac{\mathrm{AR}(\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca}}-\mathrm{\τ})}{2\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}}[\cos (\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}\mathrm{\ΔT}+\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}})-\cos \left(\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}}\right)]$

$Q_P=-\frac{\mathrm{AR}\left(\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca}}\right)}{2\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}}[\cos (\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}\mathrm{\ΔT}+\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}})-\cos \left(\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}}\right)]$

$Q_L=-\frac{\mathrm{AR}(\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca}}+\mathrm{\τ})}{2\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}}[\cos (\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}\mathrm{\ΔT}+\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}})-\cos \left(\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}}\right)]$

其中，I_E为同相支路的超前累加输出、I_P为同相支路的即时累加输出、I_L为同相支路的滞后累加输出、Q_E为正交支路的超前累加输出、Q_P为正交支路的即时累加输出、Q_L为正交支路的滞后累加输出，A为卫星星历码在一个积分周期内的积分值，由信号强度决定，并且与时间无关，Δφ_ca为本地码与接收码之间的相位差，τ为相关器间距，Δω_carr为本地载波与接收载波之间的角频率差，Δθ_carr为本地载波与接收载波之间的相位差，ΔT为相关积分时间，R为最大值为1的码自相关函数，具体表示形式如下：

$R\left(x\right)=\left\{\begin{array}{c}1-\left|x\right|,\left|x\right|\≤1\\ 0,\left|x\right|>1\end{array}\right.;$

状态量为： $X_{\mathrm{loop}}=\left[\begin{array}{cccc}\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca}}& \mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr}}& {\mathrm{\ω}}_{\mathrm{rec}}& {\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{rec}}\end{array}\right],$ 状态模型为：

$\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca},k+1}=\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca},k}+(\frac{f_{\mathrm{rec}}-f_{\mathrm{IF}}}{f_{\mathrm{rec}}}-\frac{f_{\mathrm{local}}-f_{\mathrm{IF}}}{f_{\mathrm{local}}})\×\mathrm{\ΔT}$

$\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr},k+1}=\mathrm{\Δ}{\mathrm{\θ}}_{\mathrm{carr},k}+{\mathrm{\ω}}_{\mathrm{rec},k}\×\mathrm{\ΔT}+{\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{rec},k}\×\frac{\mathrm{\Δ}{T}^2}{2}$

${\mathrm{\ω}}_{\mathrm{rec},k+1}={\mathrm{\ω}}_{\mathrm{rec},k}+{\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{rec},k}\×\mathrm{\ΔT}$

${\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{rec},k+1}={\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{rec},k+1}$

其中，ω_rec为接收载波角频率，

为接收载波角频率变化率，k为k时刻的下标，为接收载波频率，f_local为本地载波频率，f_IF为载波中频频率；

(b)环路预处理扩展卡尔曼滤波器的输出信息

利用环路预处理滤波器估计值Δφ_ca及Δω_carr推算出的GPS卫星和载体视线方向上的伪距偏差Δρ和伪距率偏差

公式如下：

$\mathrm{\Δ\ρ}=\frac{c}{f_{\mathrm{codeBasis}}}\mathrm{\Δ}{\mathrm{\φ}}_{\mathrm{ca}}$

$\mathrm{\Δ}\stackrel{\·}{\mathrm{\ρ}}=\frac{c}{f_L}\frac{\mathrm{\Δ}{\mathrm{\ω}}_{\mathrm{carr}}}{2\mathrm{\π}}$

其中，c为光速，f_codeBasis＝1.023MHz为基准码频率，f_L＝1575.42MHz为卫星发射信号的调制载波频率。

3.根据权利要求1所述的基于环路状态自检测的SINS/GPS深组合导航方法，其特征在于，在步骤2与步骤3之间还包括利用码和载波跟踪环路中的六路相关积分值对码和载波跟踪环路进行状态自检测和载噪比信息计算的步骤，判断如果该通道未发生失锁则跟踪环路的跟踪状态正常并执行步骤3，否则返回步骤2。

4.根据权利要求3所述的基于环路状态自检测的SINS/GPS深组合导航方法，其特征在于，利用码和载波跟踪环路中的六路相关积分值对码和载波跟踪环路进行锁定自检测及载噪比信息计算具体为：

按设定周期计算接收信号载噪比信息，并判断跟踪时刻前面相邻n个积分周期的相关积分结果I_P值符号，1≤n≤10且n为正整数，所述积分周期为1ms，如果在相邻n个积分周期内出现两次I_P值符号变化且载噪比下降大于阈值(阈值为10～20)，则判定该通道本时刻的信号发生失锁现象，如仅有载噪比下降而符号变化小于两次情况，则判定该通道信号还未失锁；

载噪比信息计算具体为：

(1)计算每个时刻积分结果I_P、Q_P的平方和，构造序列Z，Z_k表示如下：

$Z_k=(I_{p,k}^2+Q_{p,k}^2)$

其中k为标示时刻的下标，I_P为同相支路的即时累加输出，Q_P为正交支路的即时累加输出；

(2)计算载波功率均值：

${\stackrel{\‾}{P}}_{\mathrm{carr}}=\sqrt{{\stackrel{\‾}{Z}}^2-{\mathrm{\σ}}_Z^2}$

其中

为序列Z_k的均值，

为载波功率均值，为序列Z_k的方差， ${\mathrm{\σ}}_Z^2=\left(\frac{1}{k-1}\right){\mathrm{\Σ}}_k{(Z_k-\stackrel{\‾}{Z})}^2;$

(3)计算I_P、Q_P噪声方差

${\mathrm{\σ}}_{\mathrm{IQ}}^2=\frac{1}{2}(\stackrel{\‾}{Z}-\sqrt{{\stackrel{\‾}{Z}}^2-{\mathrm{\σ}}_Z^2})$

(4)计算载噪比C/N₀：

$C/N_0=10{\log }_{10}\left[\frac{{\stackrel{\‾}{P}}_{\mathrm{carr}}}{2T{\mathrm{\σ}}_{\mathrm{IQ}}^2}\right]$

其中T为载噪比设定积分时间，载噪比单位为dB-Hz。

5.根据权利要求1所述的基于环路状态自检测的SINS/GPS深组合导航方法，其特征在于，步骤4中状态量由两部分组成，可表示为X(t)＝[X_I X_G]，其中X_I和X_G分别为：

$X_I={\left[\begin{array}{cccccccccccccccccc}{\mathrm{\φ}}_E& {\mathrm{\φ}}_N& {\mathrm{\φ}}_U& {\mathrm{\δv}}_E& {\mathrm{\δv}}_N& {\mathrm{\δv}}_U& \mathrm{\δL}& \mathrm{\δ\λ}& \mathrm{\δh}& {\mathrm{\ϵ}}_{\mathrm{bx}}& {\mathrm{\ϵ}}_{\mathrm{by}}& {\mathrm{\ϵ}}_{\mathrm{bz}}& {\mathrm{\δ}}_{\mathrm{rx}}& {\mathrm{\ϵ}}_{\mathrm{xy}}& {\mathrm{\ϵ}}_{\mathrm{rz}}& {\▿}_z& {\▿}_y& {\▿}_z\end{array}\right]}^T$

X_G＝[δt_u δt_ru]^T

其中，X_I为SINS系统中对应的状态量，分别为三维姿态误差(φ_E φ_N φ_U)、三维速度误差(δv_E δv_N δv_U)、三维位置误差(δL δλ δh)、三维陀螺常值漂移误差(ε_bx ε_by ε_bz)、三维陀螺一阶马尔可夫漂移误差(ε_rx ε_ry ε_rz)和三维加速度计零偏T为转置运算；X_G为GPS系统中对应的状态量，分别为一维接收机等效时钟误差对应的距离δt_u和一维等效时钟频率误差对应的距离率δt_ru；

捷联惯性导航系统和GPS导航系统深度组合系统状态方程为：

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

其中，t为时间下标，F(t)为一步转移阵，G(t)为系统噪声系数阵，W(t)为系统噪声，X(t)为系统状态量。

6.根据权利要求1所述的基于环路状态自检测的SINS/GPS深组合导航方法，其特征在于，步骤6中利用校正过的捷联惯性导航系统输出位置和速度结果结合同时刻卫星位置、速度状态信息进行跟踪环路中码环及载波环的频率控制量估算方法如下：

$v_{\mathrm{user}\_\mathrm{sate}}^{\mathrm{ECEF}}={({\stackrel{\→}{v}}_u^E-{\stackrel{\→}{v}}_s^E)}^T\·\frac{{\stackrel{\→}{r}}_s^E-{\stackrel{\→}{r}}_u^E}{|{\stackrel{\→}{r}}_s^E-{\stackrel{\→}{r}}_u^E|}$

$f_{\mathrm{code}}=f_{\mathrm{codeBasis}}+\frac{v_{\mathrm{user}\_\mathrm{sate}}^{\mathrm{ECEF}}}{c}\×f_{\mathrm{codeBasis}}$

$f_{\mathrm{carr}}=f_{\mathrm{IF}}+\frac{v_{\mathrm{user}\_\mathrm{sate}}^{\mathrm{ECEF}}}{c}\×f_L$

其中，

为接收机和卫星相对速度在径向方向上的投影，

分别为接收机与卫星在地心地固直角坐标系中的速度，分别为接收机与卫星在地心地固直角坐标系中位置，f_code为码环频率控制量，f_carr为载波环频率控制量，f_IF为载波中频频率；

利用环路预处理扩展卡尔曼滤波器得到的本地信号和输入信号载波相位差的最优估计值Δθ_carr，k+1对本地载波相位进行补偿，具体补偿方法如下：

θ_carr，k+1＝θ_carr，k+Δθ_carr，k+1

其中，θ_carr，k+1为k+1时刻本地载波相位值。

Images