CN105911575A - 一种级联式惯性/卫星深组合导航滤波器观测量提取方法 - Google Patents

Abstract

本发明公开了一种级联式惯性/卫星深组合导航滤波器观测量提取方法，属于惯性/卫星组合导航技术领域。该方法包括以下步骤：首先选取惯性/卫星深组合跟踪环路的鉴别器模型；在传统预处理滤波器模型基础上，提出基于三维状态量的载波预处理、基于四维状态量的载波和码预处理滤波器模型；在基于子***参数做差式的基础上进行改进，采用基于环路参量估计式方法提取组合导航滤波器观测量。本方法能够提高惯性/卫星深组合跟踪环路的性能，有效降低了组合导航滤波器观测量误差，提高了惯性/卫星深组合***的导航解算精度，适合于工程应用。

Description

一种级联式惯性/卫星深组合导航滤波器观测量提取方法

技术领域

本发明属于导航技术领域，特别涉及一种级联式惯性/卫星深组合导航滤波器观测量提取方法。

背景技术

随着人们逐渐提高对于导航***性能和稳定性的要求，传统的惯性/卫星松、紧组合导航***已日渐不能满足。基于此前提，子***信息融合层次更高的深组合导航***应运而生。此模式下卫星导航***和惯导***的信息处理融为一体，两个子***之间进行深度耦合与辅助。

国内外的研究机构对惯性/卫星深组合尚未有统一明确定义，其跟踪环路有集中式和级联式两类，主要以组合导航滤波器的观测模型为判别依据。集中式环路模型中，直接将环路各通道的相关器输出作为组合导航滤波器观测量，再对惯性导航***状态量进行估计。此模型的环路估计误差最小，但是组合导航滤波器计算负荷大，实际应用中难以进行实时估计。

级联式模型环路中，相关器和组合导航滤波器之间添加鉴别器环节和预处理滤波环节，其输出最终转换为组合导航滤波器的输入量，完成状态误差估计并修正惯导信息，产生的控制量作为反馈形成完整的闭合环路。其中鉴别器模型、预处理滤波器模型、组合导航滤波器观测量提取方式的差异会决定了跟踪环路估计参量的误差，深组合***的导航解算精度会随之变化，进而影响深组合***的整体性能。

发明内容

本发明的目的是提供一种级联式惯性/卫星深组合导航滤波器观测量提取方法，以降低组合导航滤波器观测量误差，提高惯性/卫星深组合***的导航解算精度。

为实现上述目的，本发明采用的技术方案为：

一种级联式惯性/卫星深组合导航滤波器观测量提取方法，包括如下步骤：

步骤一、选取惯性/卫星深组合跟踪环路的鉴别器模型：

选取码跟踪环鉴相器模型：码跟踪环采用延迟锁定环路(DLL)，其鉴别方式是非相干超前减滞后幅值法，公式如下所示：

$\mathrm{\Δ}\mathrm{\φ}=\frac{1}{2}\·\frac{\sqrt{({I_E}^2+{Q_E}^2)}-\sqrt{({I_L}^2+{Q_L}^2)}}{\sqrt{({I_E}^2+{Q_E}^2)}+\sqrt{({I_L}^2+{Q_L}^2)}}$

上式中，Δφ为码相位误差，I_E和Q_E为超前支路相干积分结果，I_L和Q_L为滞后支路相干积分结果；

选取载波跟踪环鉴相器模型：载波跟踪环路中，锁频环(FLL)和锁相环(PLL)互相结合，采用锁频环辅助锁相环的方式保持相位一致并锁定频率，采用的科斯塔斯环鉴相方式为二象限反正切函数法，公式如下所示：

$\mathrm{\Δ}\mathrm{\θ}=\arctan \left(\frac{Q_P}{I_P}\right)$

上式中，Δθ为载波相位误差，I_P和Q_P为即时支路相干积分结果；

选取载波跟踪环鉴频器模型：锁频环鉴频器采用的是四象限反正切函数法，公式如下所示：

$\mathrm{\Δ}f=\frac{\arctan 2(P_{cross},P_{dot})}{2\mathrm{\π}T}$

上式中，Δf为载波频率误差，P_cross和P_dot分别为叉积和点积，即：

$\left\{\begin{array}{c}{P}_{corss}=I_P(k-1)Q_P\left(k\right)-Q_P(k-1)I_P\left(k\right)\\ P_{dot}=I_P(k-1)I_P\left(k\right)+Q_P(k-1)Q_P\left(k\right)\end{array}\right.;$

步骤二、在基于三维状态量的载波预处理滤波模型的基础上，建立基于四维状态量的载波和码预处理滤波器模型：

当码跟踪环鉴相结果和载波鉴相结果同时进行环路参量预处理，构建四位状态量的滤波器模型；在基于载波和码预处理的跟踪环路模型中，将鉴别器输出的码环估计的误差量Δφ，以及载波环估计的误差量Δθ、Δω和作为状态量X

$X={\left[\begin{array}{cccc}\mathrm{\Δ}\mathrm{\φ}& \mathrm{\Δ}\mathrm{\θ}& \mathrm{\Δ}\mathrm{\ω}& \mathrm{\Δ}\stackrel{\·}{\mathrm{\ω}}\end{array}\right]}^T$

基于此状态量的预处理滤波器状态方程为：

${\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\φ}\\ \mathrm{\Δ}\mathrm{\θ}\\ \mathrm{\Δ}\mathrm{\ω}\\ \mathrm{\Δ}\stackrel{\·}{\mathrm{\ω}}\end{array}\right]}_{k+1}=\left[\begin{array}{cccc}1& 0& \frac{{\mathrm{\λ}}_{carr}}{{\mathrm{\λ}}_{code}}T& 0\\ 0& 1& T& \frac{1}{2}{T}^2\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right]{\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\φ}\\ \mathrm{\Δ}\mathrm{\θ}\\ \mathrm{\Δ}\mathrm{\ω}\\ \mathrm{\Δ}\stackrel{\·}{\mathrm{\ω}}\end{array}\right]}_k+\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]W$

上式中，W表示状态噪声；

预处理滤波器的观测量选择码环和载波环鉴相器的输出，码环鉴相函数选择非相干超前减滞后幅值法，载波环鉴相函数选择二象限反正切函数法，结合公式构建的观测方程为：

${\left[\begin{array}{c}\mathrm{\Δ}{\mathrm{\φ}}_{code}\\ {\mathrm{\Δ\θ}}_{carr}\end{array}\right]}_k=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& \frac{1}{2}T& \frac{1}{6}{T}^2\end{array}\right]X_k+V_k$

上式中，V_k表示观测噪声；

步骤三、在基于子***参数做差式的基础上改进，采用基于环路参量估计式方法提取组合导航滤波器观测量；

分别获取惯性导航***(SINS)和卫星导航***(GNSS)解算的伪距、伪距率，则基于子***参数做差的组合导航滤波器观测量由两者直接做差得到，伪距差Δρ和伪距率差方程如下：

Δρ＝ρ_I-ρ_G

$\mathrm{\Δ}\stackrel{\·}{\mathrm{\ρ}}={\stackrel{\·}{\mathrm{\ρ}}}_I-{\stackrel{\·}{\mathrm{\ρ}}}_G$

以上两式中，ρ_I和ρ_G分别是惯性导航***和卫星导航***解算的伪距，和分别是惯性导航***和卫星导航***解算的伪距率；

在卫星与载体视线方向上，稳定跟踪环路估计得到的码相位差和两个子***推算的载***置误差存在对应关系；基于环路参量估计式的提取方法中，伪距差Δρ是通过码跟踪环鉴相器输出，并通过预处理滤波器估计的码相位误差线性转换得到，转换公式为：

Δρ＝λ_codeΔφ

上式中，λ_code表示伪码波长；

在卫星与载体视线方向上，稳定跟踪环路估计得到的载波频率差和两个子***推算的载体速度误差存在对应关系；基于环路参量估计式的提取方法中，伪距率差是通过载波跟踪环鉴相器输出，并通过预处理滤波器估计的载波相位误差线性转换得到，转换公式为：

$\mathrm{\Δ}\stackrel{\·}{\mathrm{\ρ}}={\mathrm{\λ}}_{carr}\frac{\mathrm{\Δ}\mathrm{\θ}}{2\mathrm{\π}T}$

上式中，λ_carr表示载波波长，T为环路周期。

所述步骤二中，基于三维状态量的载波预处理滤波模型通过如下方法建立：

选取载波预处理的三维状态量：

当载波鉴相结果进行环路参量预处理，而码环鉴相结果利用非相干累加求和平均时，构建三维状态量的滤波器模型；在基于载波预处理的跟踪环路模型中，将鉴别器载波环估计的误差量Δθ、Δω和作为状态量X

$X={\left[\begin{array}{ccc}\mathrm{\Δ}\mathrm{\θ}& \mathrm{\Δ}\mathrm{\ω}& \mathrm{\Δ}\stackrel{\·}{\mathrm{\ω}}\end{array}\right]}^T$

建立状态量参数之间的对应关系：

针对载波跟踪环路和码跟踪环路，在一个周期T前后，由k时刻的环路估计量推算k+1时刻的环路估计量：

${\mathrm{\Δ\θ}}_{k+1}={\mathrm{\Δ\θ}}_k+{\mathrm{\Δ\ω}}_{k}T+\frac{1}{2}\mathrm{\Δ}{\stackrel{\·}{\mathrm{\ω}}}_k{T}^2$

${\mathrm{\Δ\ω}}_{k+1}={\mathrm{\Δ\ω}}_k+\mathrm{\Δ}{\stackrel{\·}{\mathrm{\ω}}}_{k}T$

$\mathrm{\Δ}{\stackrel{\·}{\mathrm{\ω}}}_{k+1}=\mathrm{\Δ}{\stackrel{\·}{\mathrm{\ω}}}_k$

建立载波预处理滤波器状态方程

基于此状态量的预处理滤波器状态方程为：

${\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\θ}\\ \mathrm{\Δ}\mathrm{\ω}\\ \mathrm{\Δ}\stackrel{\·}{\mathrm{\ω}}\end{array}\right]}_{k+1}=\left[\begin{array}{ccc}1& T& T^2/2\\ 0& 1& T\\ 0& 0& 1\end{array}\right]{\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\θ}\\ \mathrm{\Δ}\mathrm{\ω}\\ \mathrm{\Δ}\stackrel{\·}{\mathrm{\ω}}\end{array}\right]}_k+\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]W$

上式中，W表示状态噪声。

本发明的有益效果是：

采用本发明的方法能够提高惯性/卫星深组合跟踪环路的性能，有效降低了组合导航滤波器观测量误差，提高了惯性/卫星深组合***的导航解算精度，适合于工程应用。本发明的优点在于选取跟踪环路鉴别器模型，使其在有效鉴别范围内的线性误差最小；在常见预处理滤波器模型的基础上，提出基于三维状态量的载波预处理、基于四维状态量的载波和码预处理的两种滤波模型，进一步降低了环路参量估计的误差；在传统的基于子***参数做差式提取方法的基础上进行改进，采用基于环路参量估计式提取方法，有效降低了深组合导航滤波器观测量的误差波动，可以提高惯性/卫星深组合***的导航解算精度。

附图说明

图1为惯性/卫星深组合导航***原理示意图；

图2为未采用预处理滤波器的环路码相位误差；

图3为未采用预处理滤波器的环路载波相位误差；

图4为基于载波预处理的环路码相位误差；

图5为基于载波预处理的环路载波相位误差；

图6为基于载波和码预处理的环路码相位误差；

图7为基于载波和码预处理的环路载波相位误差；

图8为基于子***参数做差式的伪距差观测量；

图9为基于子***参数做差式的伪距率差观测量；

图10为基于环路参量估计式的伪距差观测量；

图11为基于环路参量估计式的伪距差观测量。

具体实施方式

下面结合附图对本发明作更进一步的说明。

如图1所示，本文构建的惯性/卫星深组合导航***是级联式的基本结构。该***包括依次相连的信号解调模块、相关积分模块、鉴别器、预处理滤波器、组合导航滤波器、惯性导航***(INS)、本地信号产生模块，本地产生信号、卫星信号均输入信号调节模块。此时的接收机原环路断开，利用组合导航滤波器和惯性***构成深组合跟踪环路。相关器输出的I/Q信号值依次经过鉴相器环节、预处理滤波器环节，将载波环估计的相关误差转换为伪距率差，码环估计的相关误差转换成伪距差，两者构成组合滤波器的观测量，之后利用组合滤波器完成状态误差估计并修正惯导。

本发明的具体实施方如下

1.选取惯性/卫星深组合跟踪环路的鉴别器模型

(1.1)选取码跟踪环鉴相器模型

跟踪环路采用的鉴相器、鉴频器有多种，其输出的线性范围和误差特性各异。文中涉及的码跟踪环采用的是延迟锁定环路(DLL)，其鉴别方式是非相干超前减滞后幅值法，公式如下所示：

$\mathrm{\Δ}\mathrm{\φ}=\frac{1}{2}\·\frac{\sqrt{({I_E}^2+{Q_E}^2)}-\sqrt{({I_L}^2+{Q_L}^2)}}{\sqrt{({I_E}^2+{Q_E}^2)}+\sqrt{({I_L}^2+{Q_L}^2)}}---\left(1\right)$

式(1)中，Δφ为码相位误差，I_E和Q_E为超前支路相干积分结果，I_L和Q_L为滞后支路相干积分结果。

(1.2)选取载波跟踪环鉴相器模型

载波跟踪环路中，锁频环(FLL)和锁相环(PLL)互相结合，采用FLL辅助PLL的方式保持相位一致并锁定频率。本文采用的Costas环鉴相方式为二象限反正切函数法，公式如下所示：

$\mathrm{\Δ}\mathrm{\θ}=\arctan \left(\frac{Q_P}{I_P}\right)---\left(2\right)$

式(2)中，Δθ为载波相位误差，I_P和Q_P为即时支路相干积分结果。

(1.3)选取载波跟踪环鉴频器模型

针对FLL，它需要两个相邻历元时刻和时刻的环路相关信息共同构建，文中涉及的FLL鉴频器采用的是四象限反正切函数法，公式如下所示：

$\mathrm{\Δ}f=\frac{\arctan 2(P_{cross},P_{dot})}{2\mathrm{\π}T}---\left(3\right)$

式(3)中，Δf为载波频率误差，P_cross和P_dot分别为叉积和点积，即：

$\left\{\begin{array}{c}{P}_{corss}=I_P(k-1)Q_P\left(k\right)-Q_P(k-1)I_P\left(k\right)\\ P_{dot}=I_P(k-1)I_P\left(k\right)+Q_P(k-1)Q_P\left(k\right)\end{array}\right.---\left(4\right)$

2.在基于三维状态量的载波预处理滤波的基础上进行改进，采用基于四维状态量的载波和码预处理滤波模型：

(2.1)选取载波预处理的三维状态量

当载波鉴相结果进行环路参量预处理，而码环鉴相结果利用非相干累加求和平均时，可以构建三维状态量的滤波器模型。在基于载波预处理的跟踪环路模型中，将鉴别器载波环估计的误差量Δθ、Δω和作为状态量X

$X={\left[\begin{array}{ccc}\mathrm{\Δ}\mathrm{\θ}& \mathrm{\Δ}\mathrm{\ω}& \mathrm{\Δ}\stackrel{\·}{\mathrm{\ω}}\end{array}\right]}^T---\left(5\right)$

(2.2)建立状态量参数之间的对应关系

针对载波跟踪环路和码跟踪环路，在一个周期T前后，由k时刻的环路估计量推算k+1时刻的环路估计量：

$\begin{array}{c}{\mathrm{\Δ\θ}}_{k+1}={\mathrm{\Δ\θ}}_k+{\mathrm{\Δ\ω}}_{k}T+\frac{1}{2}\mathrm{\Δ}{\stackrel{\·}{\mathrm{\ω}}}_k{T}^2\\ {\mathrm{\Δ\ω}}_{k+1}={\mathrm{\Δ\ω}}_k+\mathrm{\Δ}{\stackrel{\·}{\mathrm{\ω}}}_{k}T\\ \mathrm{\Δ}{\stackrel{\·}{\mathrm{\ω}}}_{k+1}=\mathrm{\Δ}{\stackrel{\·}{\mathrm{\ω}}}_k\end{array}---\left(6\right)$

(2.3)建立载波预处理滤波器状态方程

基于此状态量的预处理滤波器状态方程为：

${\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\θ}\\ \mathrm{\Δ}\mathrm{\ω}\\ \mathrm{\Δ}\stackrel{\·}{\mathrm{\ω}}\end{array}\right]}_{k+1}=\left[\begin{array}{ccc}1& T& T^2/2\\ 0& 1& T\\ 0& 0& 1\end{array}\right]{\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\θ}\\ \mathrm{\Δ}\mathrm{\ω}\\ \mathrm{\Δ}\stackrel{\·}{\mathrm{\ω}}\end{array}\right]}_k+\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]W---\left(7\right)$

式(7)中，W表示状态噪声。

(2.4)建立载波预处理滤波器观测方程

观测模型的建立要避免一个周期相关时间滞后的影响。当存在多普勒频移时，k+1时刻环路的鉴相输出并不是真实的k+1时刻载波相位误差，而是反映了k时刻到k+1时刻的平均相位差，因此定义k+1时刻的载波相位误差Δθ_carr,k+1，则

${\mathrm{\Δ\θ}}_{carr,k+1}=\frac{1}{T}\underset{0}{\overset{T}{\∫}}({\mathrm{\Δ\θ}}_k+{\mathrm{\Δ\ω}}_{k}t+\frac{1}{2}\mathrm{\Δ}{\stackrel{\·}{\mathrm{\ω}}}_k{t}^2)dt---\left(8\right)$

可以求得

${\mathrm{\Δ\θ}}_{carr,k}={\mathrm{\Δ\θ}}_k+\frac{1}{2}{\mathrm{\Δ\ω}}_{k}T+\frac{1}{6}\mathrm{\Δ}{\stackrel{\·}{\mathrm{\ω}}}_k{T}^2---\left(9\right)$

选取载波环的相位误差Δθ_carr作为观测量，由式(9)可以构建观测方程：

${\[{\mathrm{\Δ\θ}}_{carr}\]}_k=\left[\begin{array}{ccc}1& \frac{1}{2}T& \frac{1}{6}{T}^2\end{array}\right]X_k+V_k---\left(10\right)$

式(10)中，V_k表示观测噪声。

(2.5)选取载波和码预处理的四维状态量

当码跟踪环鉴相结果和载波鉴相结果同时进行环路参量预处理，可以构建四位状态量的滤波器模型。在基于载波和码预处理的跟踪环路模型中，将鉴别器输出的码环估计的误差量Δφ，以及载波环估计的误差量Δθ、Δω和作为状态量X

$X={\left[\begin{array}{cccc}\mathrm{\Δ}\mathrm{\φ}& \mathrm{\Δ}\mathrm{\θ}& \mathrm{\Δ}\mathrm{\ω}& \mathrm{\Δ}\stackrel{\·}{\mathrm{\ω}}\end{array}\right]}^T---\left(11\right)$

(2.6)建立状态量参数之间的对应关系

针对载波跟踪环路和码跟踪环路，在一个周期T前后，由k时刻的环路估计量推算k+1时刻的环路估计量：

$\begin{array}{c}{\mathrm{\Δ\φ}}_{k+1}={\mathrm{\Δ\φ}}_k+{\mathrm{\Δ\ω}}_{code,k}T\\ {\mathrm{\Δ\θ}}_{k+1}={\mathrm{\Δ\θ}}_k+{\mathrm{\Δ\ω}}_{k}T+\frac{1}{2}\mathrm{\Δ}{\stackrel{\·}{\mathrm{\ω}}}_k{T}^2\\ {\mathrm{\Δ\ω}}_{k+1}={\mathrm{\Δ\ω}}_k+\mathrm{\Δ}{\stackrel{\·}{\mathrm{\ω}}}_{k}T\\ \mathrm{\Δ}{\stackrel{\·}{\mathrm{\ω}}}_{k+1}=\mathrm{\Δ}{\stackrel{\·}{\mathrm{\ω}}}_k\end{array}---\left(12\right)$

式(12)中，Δω_code,k表示k时刻的码频率误差，它与载波频率误差存在对应关系。

(2.7)建立载波和码预处理滤波器状态方程

基于此状态量的预处理滤波器状态方程为：

${\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\φ}\\ \mathrm{\Δ}\mathrm{\θ}\\ \mathrm{\Δ}\mathrm{\ω}\\ \mathrm{\Δ}\stackrel{\·}{\mathrm{\ω}}\end{array}\right]}_{k+1}=\left[\begin{array}{cccc}1& 0& \frac{{\mathrm{\λ}}_{carr}}{{\mathrm{\λ}}_{code}}T& 0\\ 0& 1& T& \frac{1}{2}{T}^2\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right]{\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\φ}\\ \mathrm{\Δ}\mathrm{\θ}\\ \mathrm{\Δ}\mathrm{\ω}\\ \mathrm{\Δ}\stackrel{\·}{\mathrm{\ω}}\end{array}\right]}_k+\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]W---\left(13\right)$

式(13)中，W表示状态噪声。

(2.8)建立载波和码预处理滤波器观测方程

预处理滤波器的观测量选择码环和载波环鉴相器的输出，文中码环鉴相函数选择非相干超前减滞后幅值法，载波环鉴相函数选择二象限反正切函数法。结合公式构建的观测方程为：

${\left[\begin{array}{c}\mathrm{\Δ}{\mathrm{\φ}}_{code}\\ {\mathrm{\Δ\θ}}_{carr}\end{array}\right]}_k=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& \frac{1}{2}T& \frac{1}{6}{T}^2\end{array}\right]X_k+V_k---\left(14\right)$

式(14)中，V_k表示观测噪声。

仿真验证中分别设定三种方案：不添加预处理环节，采用直接鉴相式的跟踪环路；采用基于载波预处理的跟踪环路；采用基于载波和码预处理的跟踪环路，图2和3、图4和5、图6和7分别描述了环路某个跟踪通道误差，可以看出采用四维状态量的载波和码预处理滤波的环路中，码相位误差波动幅值在正负0.05个码片之间，载波相位误差波动在正负0.03弧度之间，两者的误差波动均大幅变小，数据上的毛刺信号也得以抑制。因此可知环路采用载波和码预处理模型可以有效减小环路估计参量的误差。

3.在基于子***参数做差式的基础上改进，采用基于环路参量估计式方法提取组合导航滤波器观测量。

(3.1)基于子***参数做差式的观测量提取方案

分别获取惯性导航***(SINS)和卫星导航***(GNSS)解算的伪距、伪距率，则基于子***参数做差的组合导航滤波器观测量由两者直接做差得到，伪距差Δρ和伪距率差方程如下：

Δρ＝ρ_I-ρ_G (15)

$\mathrm{\Δ}\stackrel{\·}{\mathrm{\ρ}}={\stackrel{\·}{\mathrm{\ρ}}}_I-{\stackrel{\·}{\mathrm{\ρ}}}_G---\left(16\right)$

式(15)、(16)中ρ_I和ρ_G分别是SINS和GNSS解算的伪距，和分别是SINS和GNSS解算的伪距率。

(3.2)基于环路参量估计式的观测量提取方案

在卫星与载体视线方向上，稳定跟踪环路估计得到的码相位差和两个子***推算的载***置误差存在对应关系。基于环路参量估计式的提取方法中，伪距差Δρ是通过码跟踪环鉴相器输出，并通过预处理滤波器估计的码相位误差线性转换得到，转换公式为

Δρ＝λ_codeΔφ (17)

式(17)中λ_code表示伪码波长。

在卫星与载体视线方向上，稳定跟踪环路估计得到的载波频率差和两个子***推算的载体速度误差存在对应关系。基于环路参量估计式的提取方法中，伪距率差是通过载波跟踪环鉴相器输出，并通过预处理滤波器估计的载波相位误差线性转换得到，转换公式为

$\mathrm{\Δ}\stackrel{\·}{\mathrm{\ρ}}={\mathrm{\λ}}_{carr}\frac{\mathrm{\Δ}\mathrm{\θ}}{2\mathrm{\π}T}---\left(18\right)$

式(18)中λ_carr表示载波波长，T为环路周期。

由图8和9、图10和11对比结果可知：针对伪距差观测量，环路参量估计式相比于子***参数做差式得到的误差均值减小，降低约6.7m，而且误差波动的趋势有所减弱。对于伪距率差观测量，环路参量估计式相比于子***参数做差式得到的误差均值同样减小，降低约0.11m/s。由此可知，环路参量估计模式下的观测量提取方式具有较优的误差均值，可以降低组合导航滤波器输出状态误差。

Claims (2)

1.一种级联式惯性/卫星深组合导航滤波器观测量提取方法，其特征在于：包括如下步骤：

步骤一、选取惯性/卫星深组合跟踪环路的鉴别器模型：

选取码跟踪环鉴相器模型：码跟踪环采用延迟锁定环路，其鉴别方式是非相干超前减滞后幅值法，公式如下所示：

$\mathrm{\Δ}\mathrm{\φ}=\frac{1}{2}\·\frac{\sqrt{({I_E}^2+{Q_E}^2)}-\sqrt{({I_L}^2+{Q_L}^2)}}{\sqrt{({I_E}^2+{Q_E}^2)}+\sqrt{({I_L}^2+{Q_L}^2)}}$

上式中，Δφ为码相位误差，I_E和Q_E为超前支路相干积分结果，I_L和Q_L为滞后支路相干积分结果；

选取载波跟踪环鉴相器模型：载波跟踪环路中，锁频环和锁相环互相结合，采用锁频环辅助锁相环的方式保持相位一致并锁定频率，采用的科斯塔斯环鉴相方式为二象限反正切函数法，公式如下所示：

$\mathrm{\Δ}\mathrm{\θ}=\arctan \left(\frac{Q_P}{I_P}\right)$

上式中，Δθ为载波相位误差，I_P和Q_P为即时支路相干积分结果；

选取载波跟踪环鉴频器模型：锁频环鉴频器采用的是四象限反正切函数法，公式如下所示：

$\mathrm{\Δ}f=\frac{\arctan 2(P_{cross},P_{dot})}{2\mathrm{\π}T}$

上式中，Δf为载波频率误差，P_cross和P_dot分别为叉积和点积，即：

$\left\{\begin{array}{c}{P}_{corss}=I_P(k-1)Q_P\left(k\right)-Q_P(k-1)I_P\left(k\right)\\ P_{dot}=I_P(k-1)I_P\left(k\right)+Q_P(k-1)Q_P\left(k\right)\end{array}\right.;$

步骤二、在基于三维状态量的载波预处理滤波模型的基础上，建立基于四维状态量的载波和码预处理滤波器模型：

当码跟踪环鉴相结果和载波鉴相结果同时进行环路参量预处理，构建四位状态量的滤波器模型；在基于载波和码预处理的跟踪环路模型中，将鉴别器输出的码环估计的误差量Δφ，以及载波环估计的误差量Δθ、Δω和作为状态量X

$X={\left[\begin{array}{cccc}\mathrm{\Δ}\mathrm{\φ}& \mathrm{\Δ}\mathrm{\θ}& \mathrm{\Δ}\mathrm{\ω}& \mathrm{\Δ}\stackrel{\·}{\mathrm{\ω}}\end{array}\right]}^T$

基于此状态量的预处理滤波器状态方程为：

${\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\φ}\\ \mathrm{\Δ}\mathrm{\θ}\\ \mathrm{\Δ}\mathrm{\ω}\\ \mathrm{\Δ}\stackrel{\·}{\mathrm{\ω}}\end{array}\right]}_{k+1}=\left[\begin{array}{cccc}1& 0& \frac{{\mathrm{\λ}}_{carr}}{{\mathrm{\λ}}_{code}}T& 0\\ 0& 1& T& \frac{1}{2}{T}^2\\ 0& 0& 1& T\\ 0& 0& 0& 1\end{array}\right]{\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\φ}\\ \mathrm{\Δ}\mathrm{\θ}\\ \mathrm{\Δ}\mathrm{\ω}\\ \mathrm{\Δ}\stackrel{\·}{\mathrm{\ω}}\end{array}\right]}_k+\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]W$

上式中，W表示状态噪声；

预处理滤波器的观测量选择码环和载波环鉴相器的输出，码环鉴相函数选择非相干超前减滞后幅值法，载波环鉴相函数选择二象限反正切函数法，结合公式构建的观测方程为：

${\left[\begin{array}{c}\mathrm{\Δ}{\mathrm{\φ}}_{code}\\ {\mathrm{\Δ\θ}}_{carr}\end{array}\right]}_k=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& \frac{1}{2}T& \frac{1}{6}{T}^2\end{array}\right]X_k+V_k$

上式中，V_k表示观测噪声；

步骤三、在基于子***参数做差式的基础上改进，采用基于环路参量估计式方法提取组合导航滤波器观测量；

分别获取惯性导航***和卫星导航***解算的伪距、伪距率，则基于子***参数做差的组合导航滤波器观测量由两者直接做差得到，伪距差Δρ和伪距率差方程如下：

Δρ＝ρ_I-ρ_G

$\mathrm{\Δ}\stackrel{\·}{\mathrm{\ρ}}={\stackrel{\·}{\mathrm{\ρ}}}_I-{\stackrel{\·}{\mathrm{\ρ}}}_G$

以上两式中，ρ_I和ρ_G分别是惯性导航***和卫星导航***解算的伪距，和分别是惯性导航***和卫星导航***解算的伪距率；

在卫星与载体视线方向上，稳定跟踪环路估计得到的码相位差和两个子***推算的载***置误差存在对应关系；基于环路参量估计式的提取方法中，伪距差Δρ是通过码跟踪环鉴相器输出，并通过预处理滤波器估计的码相位误差线性转换得到，转换公式为：

Δρ＝λ_codeΔφ

上式中，λ_code表示伪码波长；

在卫星与载体视线方向上，稳定跟踪环路估计得到的载波频率差和两个子***推算的载体速度误差存在对应关系；基于环路参量估计式的提取方法中，伪距率差是通过载波跟踪环鉴相器输出，并通过预处理滤波器估计的载波相位误差线性转换得到，转换公式为：

$\mathrm{\Δ}\stackrel{\·}{\mathrm{\ρ}}={\mathrm{\λ}}_{carr}\frac{\mathrm{\Δ}\mathrm{\θ}}{2\mathrm{\π}T}$

上式中，λ_carr表示载波波长，T为环路周期。

2.根据权利要求1所述的级联式惯性/卫星深组合导航滤波器观测量提取方法，其特征在于：包括如下步骤：所述步骤二中，基于三维状态量的载波预处理滤波模型通过如下方法建立：

选取载波预处理的三维状态量：

当载波鉴相结果进行环路参量预处理，而码环鉴相结果利用非相干累加求和平均时，构建三维状态量的滤波器模型；在基于载波预处理的跟踪环路模型中，将鉴别器载波环估计的误差量Δθ、Δω和作为状态量X

$X={\left[\begin{array}{ccc}\mathrm{\Δ}\mathrm{\θ}& \mathrm{\Δ}\mathrm{\ω}& \mathrm{\Δ}\stackrel{\·}{\mathrm{\ω}}\end{array}\right]}^T$

建立状态量参数之间的对应关系：

针对载波跟踪环路和码跟踪环路，在一个周期T前后，由k时刻的环路估计量推算k+1时刻的环路估计量：

${\mathrm{\Δ\θ}}_{k+1}={\mathrm{\Δ\θ}}_k+{\mathrm{\Δ\ω}}_{k}T+\frac{1}{2}\mathrm{\Δ}{\stackrel{\·}{\mathrm{\ω}}}_k{T}^2$

${\mathrm{\Δ\ω}}_{k+1}={\mathrm{\Δ\ω}}_k+\mathrm{\Δ}{\stackrel{\·}{\mathrm{\ω}}}_{k}T$

$\mathrm{\Δ}{\stackrel{\·}{\mathrm{\ω}}}_{k+1}=\mathrm{\Δ}{\stackrel{\·}{\mathrm{\ω}}}_k$

建立载波预处理滤波器状态方程

基于此状态量的预处理滤波器状态方程为：

${\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\θ}\\ \mathrm{\Δ}\mathrm{\ω}\\ \mathrm{\Δ}\stackrel{\·}{\mathrm{\ω}}\end{array}\right]}_{k+1}=\left[\begin{array}{ccc}1& T& T^2/2\\ 0& 1& T\\ 0& 0& 1\end{array}\right]{\left[\begin{array}{c}\mathrm{\Δ}\mathrm{\θ}\\ \mathrm{\Δ}\mathrm{\ω}\\ \mathrm{\Δ}\stackrel{\·}{\mathrm{\ω}}\end{array}\right]}_k+\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]W$

上式中，W表示状态噪声。