[发明专利]基于环路状态自检测的SINS/GPS深组合导航方法

权利要求书

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，其观测模型为：

IE=AR(Δφca-τ)2Δωcarr[sin(ΔωcarrΔT+Δθcarr)-sin(Δθcarr)]]]>

IP=AR(Δφca)2Δωcarr[sin(ΔωcarrΔT+Δθcarr)-sin(Δθcarr)]]]>

IL=AR(Δφca+τ)2Δωcarr[sin(ΔωcarrΔT+Δθcarr)-sin(Δθcarr)]]]>

QE=-AR(Δφca-τ)2Δωcarr[cos(ΔωcarrΔT+Δθcarr)-cos(Δθcarr)]]]>

QP=-AR(Δφca)2Δωcarr[cos(ΔωcarrΔT+Δθcarr)-cos(Δθcarr)]]]>

QL=-AR(Δφca+τ)2Δωcarr[cos(ΔωcarrΔT+Δθcarr)-cos(Δθcarr)]]]>

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

R(x)=1-|x|,|x|≤10,|x|>1;]]>

状态量为：Xloop=ΔφcaΔθcarrωrecω·rec,]]>状态模型为：

Δφca,k+1=Δφca,k+(frec-fIFfrec-flocal-fIFflocal)×ΔT]]>

Δθcarr,k+1=Δθcarr,k+ωrec,k×ΔT+ω·rec,k×ΔT22]]>

ωrec,k+1=ωrec,k+ω·rec,k×ΔT]]>

ω·rec,k+1=ω·rec,k+1]]>

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

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

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

Δρ=cfcodeBasisΔφca]]>

Δρ·=cfLΔωcarr2π]]>

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