CN103792561A - 一种基于gnss通道差分的紧组合降维滤波方法 - Google Patents

Abstract

本发明公开了一种基于GNSS通道差分的紧组合降维滤波方法。该方法步骤如下：根据惯性导航***的姿态误差、速度误差和位置误差，建立惯性/卫星组合导航***的状态方程；确定每个通道的伪距差分信息和伪距率差分信息；选择第一个通道作为基准通道，将基准通道的差分信息分别与其余通道的差分信息进行差分得到通道间差分信息，将该通道间差分信息作为观测值，以此建立观测方程；根据状态方程和观测方程，进行卡尔曼滤波迭代解算，得到载体的速度、位置、姿态的误差信息并对惯性导航***进行反馈校正，完成惯性/卫星***组合导航。本发明方法抵消了钟差、钟漂的影响且降低了状态方程和观测方程的维数，提高了滤波实时性和导航精确性，应用前景广阔。

Description

一种基于GNSS通道差分的紧组合降维滤波方法

一、技术领域

本发明涉及组合导航技术领域，特别是一种基于GNSS（全球导航卫星***）通道差分的紧组合降维滤波方法。

二、背景技术

卫星/惯性组合导航***结合卫星导航、惯性导航的优点，具有定位精度高，稳定性强等特点，因此在军事领域被广泛应用。载体在诸如高动态飞行、接收机信号遮挡等情况下，GNSS接收机接收到卫星数目很容易少于四颗，此时松组合***将工作在纯惯导状态，导航精度随时间下降。基于伪距、伪距率的紧组合导航***则可以在上述情况下进行组合，有效抑制导航精度的发散，因此紧组合***被广泛应用。

常规紧组合***的状态变量是在松组合***基础之上，添加伪距、伪距率误差两项，以卫星的伪距、伪距率和载体相对导航卫星的伪距、伪距率之差作为观测变量。若出现GNSS时钟异常或组合滤波相关时钟误差跳变等情况，则伪距、伪距率误差出现异常，常规紧组合***将异常的误差量引入到回路中，从而导致导航精度下降；同时，在有效可见卫星大于四颗的情况下，常规紧组合导航***一般采用17维状态变量，8维观测变量的卡尔曼滤波模型，滤波耗时和17³+8×17²成正比，耗时较大。

三、发明内容

本发明的目的在于提供一种实时性好、精度高的基于GNSS通道差分的紧组合降维滤波方法，以削弱时钟异常对***的影响并提高组合滤波的实时性。

实现本发明目的的技术解决方案为：一种基于GNSS通道差分的紧组合降维滤波方法，包括以下步骤：

步骤1，根据惯性导航***的姿态误差、速度误差和位置误差，建立惯性/卫星组合导航***的状态方程；

步骤2，确定每个GNSS通道的伪距差分信息和伪距率差分信息；

步骤3，选择第一个GNSS通道作为基准通道，将基准通道的差分信息分别与其余通道的差分信息进行差分得到通道间差分信息，将该通道间差分信息作为观测值，以此建立惯性/卫星组合导航***观测方程；

步骤4，根据步骤1所得的状态方程和步骤3所得的观测方程，进行卡尔曼滤波迭代解算，得到载体的速度、位置、姿态的误差信息；

步骤5，利用载体的速度、位置、姿态误差信息对惯性导航***进行反馈校正，完成惯性/卫星***组合导航。

本发明与现有技术相比，其显著优点是：（1）双通道降维滤波器未将伪距、伪距率误差考虑在状态变量中，同时在量测方程中将双通道的伪距、伪距率差作为量测量，抵消了钟差、钟漂等的影响，精度高；（2）双通道降维滤波器降低了状态方程和观测方程的维数，降低了组合滤波器的运算复杂度，因此具有更高的滤波实时性。

四、附图说明

图1是本发明基于GNSS通道差分的紧组合降维滤波方法的流程图。

图2是实施例1中降维滤波仿真试验载体飞行轨迹图。

图3是实施例1中降维滤波仿真试验伪距、伪距率误差。

图4是实施例1中降维滤波仿真试验和传统紧组合***导航结果对比图。

五、具体实施方式

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

结合图1，本发明基于GNSS通道差分的紧组合降维滤波方法，步骤如下：

步骤1，根据惯性导航***的姿态误差、速度误差和位置误差，建立惯性/卫星组合导航***的状态方程，具体如下：

（1.1）惯性导航***的姿态、速度、位置误差方程如下：

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

（1.2）以惯性导航***的姿态误差、速度误差、位置误差为状态变量，建立惯性/卫星组合导航***的状态方程：

$\stackrel{\·}{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{\φ}}_E,{\mathrm{\φ}}_N,{\mathrm{\φ}}_U,\mathrm{\δ}{V}_E,\mathrm{\δ}{V}_N,\mathrm{\δ}{V}_U,\mathrm{\δL},\mathrm{\δ\λ},\mathrm{\δh},{\mathrm{\ϵ}}_x,{\mathrm{\ϵ}}_y,{\mathrm{\ϵ}}_z,{\▿}_x,{\▿}_y,{\▿}_z{]}_{15\×1}^T$

状态转移矩阵：

$F={\left[\begin{array}{cc}{\left(F_{\mathrm{ins}}\right)}_{9\×9}& {\left(F_{\mathrm{sg}}\right)}_{9\×6}\\ 0_{6\×9}& 0_{6\×6}\end{array}\right]}_{15\×15}$

其中F_ins由（1.1）中误差方程构成，F_sg形式如下：

$F_{\mathrm{sg}}={\left[\begin{array}{cc}{C}_b^n& 0_{3\×3}\\ 0_{3\×3}& C_b^n\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]}_{9\×6}$

其中，

为载体坐标系到导航坐标系的姿态转换矩阵，

***噪声驱动矩阵：

$G={\left[\begin{array}{cc}{C}_b^n& 0_{3\×3}\\ 0_{3\×3}& C_b^n\\ 0_{9\×3}& 0_{9\×3}\end{array}\right]}_{15\×6}$

***噪声矢量：

$W={\left[\begin{array}{cccccc}{w}_{\mathrm{gx}}& w_{\mathrm{gy}}& w_{\mathrm{gz}}& w_{\mathrm{ax}}& w_{\mathrm{ay}}& w_{\mathrm{az}}\end{array}\right]}_{6\×1}^T$

假设***噪声为零均值高斯白噪声，协方差阵为E(WW^T)＝Q，Q为***过程噪声方差阵；w_gx、w_gy、w_gz分别为陀螺仪x轴、y轴、z轴方向的随机白噪声，w_ax、w_ay、w_az分别为加速度计x轴、y轴、z轴方向的随机白噪声。

步骤2，确定每个GNSS通道的伪距差分信息和伪距率差分信息；具体步骤如下：

（2.1）根据GNSS通道星历信息，选择导航卫星并确定导航卫星的伪距ρ_Gj、伪距率

信息，导航卫星的伪距ρ_Gj公式如下：

ρG_j＝r_j-δt_u-v_ρj

其中，δt_u为钟差引起的距离误差。v_ρj为伪距测量白噪声，它代表了所有未直接体现的各种误差总和，例如，由卫星星历参数得到的卫星位置、卫星时钟校正模型和大气延时估计值等存在着不可避免的误差。r_j为载体到第j颗卫星S^j的真实无差距离，公式如下：

r_j＝[(x-x_sj)²+(y-y_sj)²+(z-z_sj)²]^1/2

式中，(x,y,z)为载体的真实无差位置，(x_sj,y_sj,z_sj)为由卫星星历确定的第j颗卫星的位置；

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

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

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

${\stackrel{\·}{r}}_j=(\stackrel{\·}{x}-{\stackrel{\·}{x}}_{\mathrm{sj}})e_{j1}+(\stackrel{\·}{y}-{\stackrel{\·}{y}}_{\mathrm{sj}})e_{j2}+(\stackrel{\·}{z}-{\stackrel{\·}{z}}_{\mathrm{sj}})e_{j3}$

上式中，

为载体的真实无差速度，

为由卫星星历确定的第j颗卫星的速度；

（2.2）利用惯性导航***解算出的载体速度、位置信息，确定载体相对每颗导航卫星的伪距、伪距率信息，其中载体到第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轴方向余弦，分别如下：

$e_{j1}=\frac{x-x_{\mathrm{sj}}}{r_j},e_{j2}=\frac{y-y_{\mathrm{sj}}}{r_j},e_{j3}=\frac{z-z_{\mathrm{sj}}}{r_j}$

载体到第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轴的分量；

（2.3）将载体相对每颗导航卫星的伪距均减去对应导航卫星的伪距、载体相对每颗导航卫星的伪距率均减去对应导航卫星的伪距率，得到相应每个GNSS通道的伪距差分信息δρ_j和伪距率差分信息

公式如下：

$\left\{\begin{array}{c}\mathrm{\δ}{\mathrm{\ρ}}_j={\mathrm{\ρ}}_{\mathrm{Ij}}-{\mathrm{\ρ}}_{\mathrm{Gj}}=e_{j1}\mathrm{\δx}+e_{j2}\mathrm{\δy}+e_{j3}\mathrm{\δz}+\mathrm{\δ}{t}_u+v_{\mathrm{\ρj}}\\ \mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}_j={\stackrel{\·}{\mathrm{\ρ}}}_{\mathrm{Ij}}-{\stackrel{\·}{\mathrm{\ρ}}}_{\mathrm{Gj}}=e_{j1}\mathrm{\δ}\stackrel{\·}{x}+e_{j2}\mathrm{\δ}\stackrel{\·}{y}+e_{j3}\mathrm{\δ}\stackrel{\·}{z}+\mathrm{\δ}{t}_{\mathrm{ru}}+v_{\stackrel{\·}{\mathrm{\ρ}}j}\end{array}\right.$

步骤3，选择第一个GNSS通道作为基准通道，将基准通道的差分信息分别与其余通道的差分信息进行差分得到通道间差分信息，将该通道间差分信息作为观测值，以此建立惯性/卫星组合导航***观测方程，具体步骤如下：

（3.1）导航***观测方程如下：

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

其中Z(t)为***观测矢量，H(t)为***观测矩阵，V(t)为***观测噪声阵，X为***状态矢量；

将i通道的差分信息与j通道的差分信息进行差分得到通道间差分信息δρ^ij和

如下式所示：

$\left\{\begin{array}{c}\mathrm{\δ}{\mathrm{\ρ}}^{\mathrm{ij}}=\mathrm{\δ}{\mathrm{\ρ}}^i-\mathrm{\δ}{\mathrm{\ρ}}^j=(e_{i1}-e_{j1})\mathrm{\δx}+(e_{i2}-e_{j2})\mathrm{\δy}+(e_{i3}-e_{j3})\mathrm{\δz}+v_{\mathrm{\ρ}}^{\mathrm{ij}}\\ \mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}^{\mathrm{ij}}=\mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}^j-{\mathrm{\δ}\stackrel{\·}{\mathrm{\ρ}}}^j=(e_{i1}-e_{j1})\mathrm{\δ}\stackrel{\·}{x}+(e_{i2}-e_{j2})\mathrm{\δ}\stackrel{\·}{y}+(e_{i3}-e_{j3})\mathrm{\δ}\stackrel{\·}{z}+v_{\stackrel{\·}{\mathrm{\ρ}}}^{\mathrm{ij}}\end{array}\right.$

式中i＝1…n，j＝1…n且i≠j，n与接收到的卫星数目N有关：

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

（3.2）选择第一个GNSS通道作为基准通道，将基准通道的差分信息分别与其余通道的差分信息进行差分得到通道间差分信息，通道间伪距差分信息依次为δρ²¹…δρⁿ¹，通道间伪距率差分信息依次为

（3.3）将所得通道间差分信息作为观测值，分别建立伪距差分观测方程和伪距率差分观测方程，其中伪距差分观测方程如下式所示：

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

式中，

为观测矢量，

为观测矩阵，为观测噪声阵，

为状态矢量，分别如下：

${\tilde{Z}}_{\mathrm{\&rho;}}={\left[\begin{array}{ccc}\mathrm{\&delta;}{\mathrm{\&rho;}}^{21}& \&CenterDot;\&CenterDot;\&CenterDot;& \mathrm{\&delta;}{\mathrm{\&rho;}}^{n1}\end{array}\right]}_{1\&times;(n-1)}^T{\tilde{V}}_{\mathrm{\&rho;}}={\left[\begin{array}{ccc}{v}_{\mathrm{\&rho;}}^{21}& \&CenterDot;\&CenterDot;\&CenterDot;& v_{\mathrm{\&rho;}}^{n1}\end{array}\right]}_{1\&times;(n-1)}^T$

${\tilde{H}}_{\mathrm{\&rho;n}}={\left[\begin{array}{ccc}{\tilde{a}}_{21}& {\tilde{a}}_{22}& {\tilde{a}}_{23}\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ {\tilde{a}}_{n1}& {\tilde{a}}_{n2}& {\tilde{a}}_{n3}\end{array}\right]}_{(n-1)\&times;3}$

其中，

为各通道伪距测量白噪声和基准通道伪距测量白噪声之差， ${\tilde{a}}_{\mathrm{ij}}=a_{\mathrm{ij}}-a_{1j}(i=\mathrm{2,3}\&CenterDot;\&CenterDot;\&CenterDot;n,j=\mathrm{1,2,3}),$

展开如下：

$\left\{\begin{array}{c}{\tilde{a}}_{i1}=(R_N-h)[-(e_{i1}-e_{11})\sin L\cos \mathrm{\&lambda;}-(e_{i2}-e_{12})\sin L\sin \mathrm{\&lambda;}]\\ +[R_N{(1-f)}^2+h](e_{i3}-e_{13})\cos L\\ {\tilde{a}}_{i2}=(R_N+h)[(e_{i2}-e_{12})\cos L\cos \mathrm{\&lambda;}-(e_{i1}-e_{11})\cos L\sin \mathrm{\&lambda;}\\ {\tilde{a}}_{i3}=(e_{i1}-e_{11})\cos L\cos \mathrm{\&lambda;}+(e_{i2}-e_{12})\cos L\sin \mathrm{\&lambda;}+(e_{i3}-e_{13})\sin L\end{array}\right.$

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

${\tilde{Z}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}={\tilde{H}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}\tilde{X}+{\tilde{V}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}=\left[\begin{array}{ccc}{0}_{(n-1)\&times;3}& {\tilde{H}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}n}& 0_{(n-1)\&times;9}\end{array}\right]\tilde{X}+{\tilde{V}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}$

式中，

为观测矢量，

为观测矩阵，

为观测噪声阵，

为状态矢量，分别如下：

${\tilde{Z}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}={\left[\begin{array}{ccc}\mathrm{\&delta;}{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}^{21}& \&CenterDot;\&CenterDot;\&CenterDot;& \mathrm{\&delta;}{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}^{n1}\end{array}\right]}_{1\&times;(n-1)}^T{\tilde{V}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}={\left[\begin{array}{ccc}{v}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}^{21}& \&CenterDot;\&CenterDot;\&CenterDot;& v_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}}^{n1}\end{array}\right]}_{1\&times;(n-1)}^T$

${\tilde{H}}_{\stackrel{\&CenterDot;}{\mathrm{\&rho;}}n}={\left[\begin{array}{ccc}{\tilde{b}}_{21}& {\tilde{b}}_{22}& {\tilde{b}}_{23}\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ \&CenterDot;& \&CenterDot;& \&CenterDot;\\ {\tilde{b}}_{n1}& {\tilde{b}}_{n2}& {\tilde{b}}_{n3}\end{array}\right]}_{(n-1)\&times;3}$

其中，为各通道伪距率测量白噪声和基准通道伪距率测量白噪声之差， ${\tilde{b}}_{\mathrm{ij}}=b_{\mathrm{ij}}-b_{1j}(i=\mathrm{2,3}\&CenterDot;\&CenterDot;\&CenterDot;n,j=\mathrm{1,2,3}),$

展开如下：

$\left\{\begin{array}{c}{\tilde{b}}_{i1}=-(e_{i1}-e_{11})\sin \mathrm{\&lambda;}+(e_{i2}-e_{12})\cos \mathrm{\&lambda;}\\ {\tilde{b}}_{i2}=-(e_{i1}-e_{11})\sin L\cos \mathrm{\&lambda;}-(e_{i2}-e_{12})\sin L\sin \mathrm{\&lambda;}+(e_{i3}-e_{13})\cos L\\ {\tilde{b}}_{i3}=(e_{i1}-e_{11})\cos L\cos \mathrm{\&lambda;}+(e_{i2}-e_{12})\cos L\sin \mathrm{\&lambda;}+(e_{i3}-e_{13})\sin L\end{array}\right.$

（3.4）综合伪距差分观测方程和伪距率差分观测方程，得到惯性/卫星组合导航***观测方程如下：

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

步骤4，根据步骤1所得的状态方程和步骤3所得的观测方程，进行卡尔曼滤波迭代解算，得到载体的速度、位置、姿态的误差信息；首先将步骤1所得的状态方程和步骤3所得的观测方程离散化表示为：

X_k＝Φ_k,k-1X_k-1+Γ_k-1W_k-1

Z_k＝H_kX_k+V_k

其中，X_k表示被估计状态，Φ_k,k-1为t_k-1时刻的一步转移矩阵，Γ_k-1为***噪声驱动阵，W_k为***激励噪声序列，Z_k为量测矩阵，H_k为量测阵，V_k为量测噪声序列，根据***状态方程和***量测方程，采用卡尔曼滤波信息融合算法，公式编排具体如下：

①状态一步预测方程：

X_k/k-1＝φ_k,k-1X_k-1

其中，X_k/k-1为k时刻***状态一步预测值，X_k-1为k-1时刻***状态估计值，φ_k,k-1为k-1时刻到k时刻的***状态转移矩阵；

②一步预测均方误差方程：

$P_{k/k-1}={\mathrm{\&phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&phi;}}_{k,k-1}^T+{\mathrm{\&Gamma;}}_{k-1}{Q}_{k-1}{\mathrm{\&Gamma;}}_{k-1}^T$

其中，P_k/k-1为k-1时刻到k时刻的***状态协方差阵，P_k-1为k-1时刻的***状态协方差阵，Q_k-1为k-1时刻***噪声矩阵，Γ_k-1为k-1时刻***噪声驱动矩阵；

③最优滤波增益方程：

$K_k=P_{k|k-1}{H}_k^T{[H_k{P}_{k|k-1}{H}_k^T+R_k]}^{-1}$

其中，K_k为k时刻***增益矩阵，H_k为k时刻***量测矩阵，R_k为k时刻***量测噪声矩阵；

④状态估计方程：

X_k＝X_k/k-1+K_k(Z_k-H_kX_k/k-1)

其中，X_k为k时刻***状态估计值，K_k为k时刻***增益矩阵，Z_k为k时刻量测向量；

⑤估计均方误差方程：

$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_{k-1}^T$

其中，P_k为k时刻的***状态协方差阵，I是单位阵。

步骤5，利用载体的速度、位置、姿态误差信息对惯性导航***进行反馈校正，完成惯性/卫星***组合导航。

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

实施例1

为了对本发明方法进行说明，充分展现出该方法的可靠性与精确性，仿真试验如下：

（1）仿真初始条件及参数设置

载体初始静止，初始位置：北纬32.03°、东经118.46°、高程5m；初始姿态：俯仰角0°、横滚角0°、航向角45°。伪距率误差跳变时刻及跳变值参见表1。

表1伪距、伪距率误差跳变时刻

在双通降维滤波器中初始X₀，估计方差P₀，***噪声方差Q，***观测噪声方差R，分别设置如下：

X₀＝0

P₀＝diag{(0.1°)²,(0.1°)²,(0.2°)²,(0.1m/s)²,(0.1m/s)²,(0.1m/s)²,(10m)²,

(10m)²,(15m)²,(2°/h)²,(2°/h)²,(2°/h)²,(1mg)²，(1mg)²,(1mg)²}

Q＝diag{(3°/h)²,(3°/h)²,(3°/h)²,(1mg)2，(1mg)2,(1mg)2}

R＝diag{(15m)²,(15m)²,(15m)²，(0.1m/s)²,(0.1m/s)²,(0.1m/s)²}

设置载体的运行轨迹如图2；模拟GNSS时钟或组合滤波相关时钟误差出现跳变情况，***的伪距、伪距率误差如图3。

（2）仿真结果分析

图4给出了传统紧组合滤波和本发明降维滤波的仿真对比曲线，其中图4（a）为位置误差对比、图4（b）为速度误差对比、图4（c）为姿态误差对比。从图中可以看出双通道降维滤波器水平位置误差小于5m、高度误差小于10m，速度误差小于0.3m/s，姿态角误差小于0.2°，满足导航精度要求。在GNSS时钟存在异常的情况下，若采用传统滤波则将异常的伪距、伪距率误差反馈到***回路中，必将导致导航精度的下降。双通道降维滤波器未将伪距、伪距率误差考虑在状态变量中，同时在量测方程中将双通道的伪距、伪距率差作为量测量，抵消了钟差、钟漂等的影响，因此降维滤波的导航精度优于传统滤波。双通道降维滤波器降低了状态方程和观测方程的维数，降低了组合滤波器的运算复杂度，因此相较传统单通道滤波器具有更高的滤波实时性。

Claims (5)

1.一种基于GNSS通道差分的紧组合降维滤波方法，其特征在于，包括以下步骤： 

步骤1，根据惯性导航***的姿态误差、速度误差和位置误差，建立惯性/卫星组合导航***的状态方程； 

步骤2，确定每个GNSS通道的伪距差分信息和伪距率差分信息； 

步骤3，选择第一个GNSS通道作为基准通道，将基准通道的差分信息分别与其余通道的差分信息进行差分得到通道间差分信息，将该通道间差分信息作为观测值，以此建立惯性/卫星组合导航***观测方程； 

步骤4，根据步骤1所得的状态方程和步骤3所得的观测方程，进行卡尔曼滤波迭代解算，得到载体的速度、位置、姿态的误差信息； 

步骤5，利用载体的速度、位置、姿态误差信息对惯性导航***进行反馈校正，完成惯性/卫星***组合导航。 

2.根据权利要求1所述的基于GNSS通道差分的紧组合降维滤波方法，其特征在于，步骤1中所述建立惯性/卫星组合导航***的状态方程，具体如下： 

（1.1）惯性导航***的姿态、速度、位置误差方程如下： 

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

（1.2）以惯性导航***的姿态误差、速度误差、位置误差为状态变量，建立惯性/卫星组合导航***的状态方程： 

其中，X为***状态矢量，

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

***状态矢量： 

状态转移矩阵： 

其中F_ins由（1.1）中误差方程构成，F_sg形式如下： 

其中，

为载体坐标系到导航坐标系的姿态转换矩阵， 

***噪声驱动矩阵： 

***噪声矢量： 

假设***噪声为零均值高斯白噪声，协方差阵为E(WW^T)＝Q，Q为***过程噪声方差阵；w_gx、w_gy、w_gz分别为陀螺仪x轴、y轴、z轴方向的随机白噪声，w_ax、w_ay、w_az分别为加速度计x轴、y轴、z轴方向的随机白噪声。 

3.根据权利要求1所述的基于GNSS通道差分的紧组合降维滤波方法，其特征在于，步骤2中所述确定每个GNSS通道的伪距差分信息和伪距率差分信息，具体步骤如下： 

（2.1）根据GNSS通道星历信息，选择导航卫星并确定导航卫星的伪距ρ_Gj、伪距率信息，导航卫星的伪距ρ_Gj公式如下： 

ρ_Gj＝r_j-δt_u-v_ρj

其中，δt_u为钟差引起的距离误差，v_ρj为伪距测量白噪声，r_j为载体到第j颗卫星S^j的真实无差距离，公式如下： 

r_j＝[(x-x_sj)²+(y-y_sj)²+(z-z_sj)²]^1/2

式中，(x,y,z)为载体的真实无差位置，(x_sj,y_sj,z_sj)为由卫星星历确定的第j颗卫星的位置； 

导航卫星的伪距率

公式如下： 

其中，δt_ru为钟漂引起的距离率误差，

为伪距率测量白噪声，

为载体到第j颗卫星S^j的真实无差距离变化率，公式如下： 

上式中，

为载体的真实无差速度，

为由卫星星历确定的第j颗卫星的速度，e_j1、e_j2、e_j3分别为载体和第j颗卫星的x轴、y轴、z轴方向余弦，分别如下： 

（2.2）利用惯性导航***解算出的载体速度、位置信息，确定载体相对每颗导航卫星的伪距、伪距率信息，其中载体到第j颗卫星的伪距ρ_Ij为： 

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

其中δx、δy、δz分别为载体在地球坐标系中的位置误差在x轴、y轴、z轴的分量； 

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

为： 

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

（2.3）将载体相对每颗导航卫星的伪距均减去对应导航卫星的伪距、载体相对每颗导航卫星的伪距率均减去对应导航卫星的伪距率，得到相应每个GNSS通道的伪距差分信息δρ_j和伪距率差分信息

公式如下： 

。

4.根据权利要求1所述的基于GNSS通道差分的紧组合降维滤波方法，其特征在于，步骤3中所述建立惯性/卫星组合导航***观测方程的具体步骤如下： 

（3.1）导航***观测方程如下： 

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

其中Z(t)为***观测矢量，H(t)为***观测矩阵，V(t)为***观测噪声阵，X为***状态矢量； 

将i通道的差分信息与j通道的差分信息进行差分得到通道间差分信息δρ^ij和如下式所示： 

式中i＝1…n，j＝1…n且i≠j，n与接收到的卫星数目N有关： 

（3.2）选择第一个GNSS通道作为基准通道，将基准通道的差分信息分别与其余通道的差分信息进行差分得到通道间差分信息，通道间伪距差分信息依次为δρ²¹…δρⁿ¹，通道间伪距率差分信息依次为

（3.3）将所得通道间差分信息作为观测值，分别建立伪距差分观测方程和伪距率差分观测方程，其中伪距差分观测方程如下式所示： 

式中，为观测矢量，

为观测矩阵，

为观测噪声阵，

为状态矢量，分别如下： 

其中，

为各通道伪距测量白噪声和基准通道伪距测量白噪声之差， 

展开如下： 

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

式中，

为观测矢量，为观测矩阵，

为观测噪声阵，

为状态矢量，分别如下： 

其中，为各通道伪距率测量白噪声和基准通道伪距率测量白噪声之差， 

展开如下： 

（3.4）综合伪距差分观测方程和伪距率差分观测方程，得到惯性/卫星组合导航***观测方程如下： 

。

5.根据权利要求1所述的基于GNSS通道差分的紧组合降维滤波方法，其特征在于，步骤4中所述进行卡尔曼滤波迭代解算，首先将步骤1所得的状态方程和步骤3所得的观测方程离散化表示为： 

X_k＝Φ_k,k-1X_k-1+Γ_k-1W_k-1

Z_k＝H_kX_k+V_k

其中，X_k表示被估计状态，Φ_k,k-1为t_k-1时刻的一步转移矩阵，Γ_k-1为***噪声驱动阵，W_k为***激励噪声序列，Z_k为量测矩阵，H_k为量测阵，V_k为量测噪声序列，根据***状态方程和***量测方程，采用卡尔曼滤波信息融合算法，公式编排具体如下： 

①状态一步预测方程： 

X_k/k-1＝φ_k,k-1X_k-1

其中，X_k/k-1为k时刻***状态一步预测值，X_k-1为k-1时刻***状态估计值，φ_k,k-1为k-1时刻到k时刻的***状态转移矩阵； 

②一步预测均方误差方程： 

其中，P_k/k-1为k-1时刻到k时刻的***状态协方差阵，P_k-1为k-1时刻的***状态协方差阵，Q_k-1为k-1时刻***噪声矩阵，Γ_k-1为k-1时刻***噪声驱动矩阵； 

③最优滤波增益方程： 

其中，K_k为k时刻***增益矩阵，H_k为k时刻***量测矩阵，R_k为k时刻***量测噪声矩阵； 

④状态估计方程： 

X_k＝X_k/k-1+K_k(Z_k-H_kX_k/k-1) 

其中，X_k为k时刻***状态估计值，K_k为k时刻***增益矩阵，Z_k为k时刻量测向量； 

⑤估计均方误差方程： 

其中，P_k为k时刻的***状态协方差阵，I是单位阵。 

Images