CN103675880A - 一种卫星信号阻塞情况下的持续导航方法 - Google Patents

Abstract

一种卫星信号阻塞情况下的持续导航方法，所述卫星信号阻塞是指可见卫星数会不足4颗，实现步骤（1）建立导航解算状态方程，选择用户三轴方向的位置、速度和加速度作为状态量并建立用户运动模型，同时将用户运动模型和时钟模型组合在一起并离散化作为状态方程。本发明在少于4颗星的情况下分别针对高动态和低动态用户提出不同的持续导航方法解决方法。

Description

一种卫星信号阻塞情况下的持续导航方法

技术领域

本发明提出的一种卫星信号阻塞情况下的持续导航方法，主要应用于对卫星信号阻塞情况下的连续定位。

背景技术

卫星导航定位算法中，最常见的算法是最小二乘或者加权最小二乘法，这两种算法在信号阻塞情况下无法定位，而且非阻塞情况下定位结果通常是粗糙且杂乱的。当卫星导航设备在民航、无人机及导弹上使用时，这些不定位点和跳动点可能会对用户带来难以预计的影响。现有的卫星导航定位算法中，LS和WLS方法能在含有误差和噪声的各个观测量之间寻求一个最优点，使得观测量的残余平方和最小，但是由于不同时刻的测量误差和噪声解算后转化为不同的定位误差影响导航精度。LS和WLS在卫星大于等于4颗算法才会收敛，在卫星阻塞情况下无法持续定位。

发明内容

本发明的技术解决问题：克服现有技术定位解算算法少于4颗星不能定位或者定位持续时间不长的缺点，提供一种卫星信号阻塞情况下的持续导航方法，在少于4颗星的情况下分别针对高动态和低动态用户提出不同的持续导航方法解决方法。

本发明技术解决方案：一种卫星信号阻塞情况下的持续导航方法，所述卫星信号阻塞是指可见卫星数会不足4颗，实现步骤如下：

（1）建立导航解算状态方程，选择用户三轴方向的位置、速度和加速度作为状态量并建立用户运动模型，同时将用户运动模型和时钟模型组合在一起并离散化作为状态方程；

X_k=Φ_k,k-1X_k-1+W_k-1                   (1)

式中

${\mathrm{\Φ}}_{k,k-1}=\left[\begin{array}{cc}{\mathrm{\Φ}}_u(k,k-1)& 0\\ 0& {\mathrm{\Φ}}_c(k,k-1)\end{array}\right]---\left(2\right)$

$W_{k-1}=E{\left[\begin{array}{cc}{W}_u(k-1)& W_c(k-1)\end{array}\right]}^T---\left(3\right)$

$Q_{k-1}=E\left[\begin{array}{cc}{W}_{k-1}& W_{k-1}^T\end{array}\right]=\left[\begin{array}{cc}{Q}_u(k-1)& 0\\ 0& Q_c(k-1)\end{array}\right]---\left(4\right)$

上式中各量的物理含义如下：

$\text{X=}{[x_u,v_{\mathrm{ux}},a_{\mathrm{ux}},y_u,v_{\mathrm{uy}},a_{\mathrm{uy}},z_u,v_{\mathrm{uz}},a_{\mathrm{uz}},{\mathrm{ct}}_u,c{\stackrel{\·}{t}}_u]}^T$ ——系统的状态向量，包括载体在ECEF坐标系中三个方向上的位置、速度、加速度，以及系统的钟差、钟漂；

x_u——载体在ECEF坐标系中X轴方向的位置；

v_ux——载体在ECEF坐标系中X轴方向的速度；

a_ux——载体在ECEF坐标系中X轴方向的加速度；

y_u——载体在ECEF坐标系中Y轴方向的位置；

v_uy——载体在ECEF坐标系中Y轴方向的速度；

a_uy——载体在ECEF坐标系中Y轴方向的加速度；

z_u——载体在ECEF坐标系中Z轴方向的位置；

v_uz——载体在ECEF坐标系中Z轴方向的速度；

a_uz——载体在ECEF坐标系中Z轴方向的加速度；

ct_u——系统钟差；

——系统钟漂；

Φ_k,k-1——状态转移矩阵，表征系统状态由k-1时刻向k时刻状态转移的情况。状态转移矩阵Φ_k,k-1由运动模型状态转移矩阵Φ_u(k,k-1)和时钟模型转移矩阵Φ_c(k,k-1)组成；

Φ_u(k,k-1)——运动模型状态转移矩阵，表征在状态转移过程中与系统运动模型相关的分量；

Φ_c(k,k-1)——时钟模型转移矩阵，表征在状态转移过程中与系统时钟模型相关的分量；

W_k-1——白噪声序列，表征在k-1时刻系统的过程噪声。白噪声序列W_k-1由位置白噪声序列W_u(k-1)和时钟白噪声序列W_c(k-1)组成；

W_u(k-1)——位置白噪声序列；

W_c(k-1)——时钟白噪声序列；

Q_k-1——白噪声序列的均方差；

（2）选择伪距和伪距率作为观测量，建立伪距方程和多普勒方程作为导航解算的量测方程；

伪距方程和多普勒方程分别表示如下：

${\mathrm{\ρ}}_j=\sqrt{{(x_j-x_u)}^2+{(y_j-y_u)}^2+{(z_j+z_u)}^2}+{\mathrm{ct}}_u---\left(5\right)$

$D_j=[(x_u-x_j)(v_{\mathrm{ux}}-v_{\mathrm{xj}})+(y_u-y_j)(v_{\mathrm{uy}}-v_{\mathrm{yj}})+(z_u-z_j)(v_{\mathrm{uz}}-v_{\mathrm{zj}})]/r_j+c{\stackrel{\·}{t}}_u---\left(6\right)$

上式中各量的物理含义如下：

ρ_j——接收机测得的第j颗卫星的伪距观测量；

D_j——接收机测得的第j颗卫星的多普勒观测量；

x_j——第j颗卫星在ECEF坐标系中X轴方向的位置；

y_j——第j颗卫星在ECEF坐标系中Y轴方向的位置；

z_j——第j颗卫星在ECEF坐标系中Z轴方向的位置；

v_xj——第j颗卫星在ECEF坐标系中X轴方向的速度；

v_yj——第j颗卫星在ECEF坐标系中Y轴方向的速度；

v_zj——第j颗卫星在ECEF坐标系中Z轴方向的速度；

$r_j=\sqrt{{(x_j-x_u)}^2+{(y_j-y_u)}^2+{(z_j-z_u)}^2}$ ——第j颗卫星相对于接收机的向径；

x_u——接收机在ECEF坐标系中X轴方向的位置；

y_u——接收机在ECEF坐标系中Y轴方向的位置；

z_u——接收机在ECEF坐标系中Z轴方向的位置；

v_ux——接收机在ECEF坐标系中X轴方向的速度；

v_uy——接收机在ECEF坐标系中Y轴方向的速度；

v_uz——接收机在ECEF坐标系中Z轴方向的速度；

ct_u——接收机的钟差；

——接收机的钟漂；

然后，根据公式（5）、（6）建立量测方程：

Z_k=H_kX_k+V_k                            (7)

上式中各量的物理含义如下：

X_k——状态向量；

Z_k——观测向量；

H_k——观测矩阵；

V_k——观测噪声协方差矩阵；

设m为接收机观测到的卫星数目，则有：

Z_k=[ρ₁,ρ₂,…ρ_m,D₁,D₂,…,D_m]^T                 (8)

$H_k=\left[\begin{array}{c}{H}_1\\ H_2\end{array}\right]---\left(9\right)$

$H_1=\left[\begin{array}{ccccccccccc}{h}_{{1x}_u}^1& 0& 0& h_{{1y}_u}^1& 0& 0& h_{{1z}_u}^1& 0& 0& 1& 0\\ h_{{1x}_u}^2& 0& 0& h_{{1y}_u}^2& 0& 0& h_{{1z}_u}^2& 0& 0& 1& 0\\ \·& \·& \·& \·& \·& \·& \·& \·& \·& \·& \·\\ \·& \·& \·& \·& \·& \·& \·& \·& \·& \·& \·\\ \·& \·& \·& \·& \·& \·& \·& \·& \·& \·& \·\\ h_{{1x}_u}^m& 0& 0& h_{{1y}_u}^m& 0& 0& h_{{1z}_u}^m& 0& 0& 1& 0\end{array}\right]---\left(10\right)$

$h_{{1x}_u}^j=\frac{{\∂\mathrm{\ρ}}_j}{{\∂x}_u}=\frac{x_u-x_j}{r_j}$

$h_{{1y}_u}^j=\frac{{\∂\mathrm{\ρ}}_j}{{\∂y}_u}=\frac{y_u-y_j}{r_j}---\left(11\right)$

$h_{{1z}_u}^j=\frac{{\∂\mathrm{\ρ}}_j}{{\∂z}_u}=\frac{z_u-z_j}{r_j}$

$H_2=\left[\begin{array}{ccccccccccc}{h}_{{2x}_u}^1& h_{{2v}_{\mathrm{ux}}}^1& 0& h_{{2y}_u}^1& h_{{2v}_{\mathrm{uy}}}^1& 0& h_{{2z}_u}^1& h_{{2v}_{\mathrm{uz}}}^1& 0& 0& 1\\ h_{{2x}_u}^2& h_{{2v}_{\mathrm{ux}}}^2& 0& h_{2y_u}^2& h_{{2v}_{\mathrm{uy}}}^2& 0& h_{2z_u}^2& h_{{2v}_{\mathrm{uz}}}^2& 0& 0& 1\\ \·& \·& \·& \·& \·& \·& \·& \·& \·& \·& \·\\ \·& \·& \·& \·& \·& \·& \·& \·& \·& \·& \·\\ \·& \·& \·& \·& \·& \·& \·& \·& \·& \·& \·\\ h_{{2x}_u}^m& h_{{2v}_{\mathrm{ux}}}^m& 0& h_{{2y}_u}^m& h_{{2v}_{\mathrm{uy}}}^m& 0& h_{{2z}_u}^m& h_{{2v}_{\mathrm{uz}}}^m& 0& 0& 1\end{array}\right]---\left(12\right)$

$h_{{2x}_u}^j=\frac{{\∂D}_j}{{\∂x}_u}=\frac{(v_{\mathrm{ux}}-v_{\mathrm{xj}})r_j^2-(x_u-x_j)J_j}{r_j^3}$

$h_{{2y}_u}^j=\frac{{\∂D}_j}{{\∂y}_u}=\frac{(v_{\mathrm{uy}}-v_{\mathrm{yj}})r_j^2-(y_u-y_j)J_j}{r_j^3}$

$h_{{2z}_u}^j=\frac{{\∂D}_j}{{\∂z}_u}=\frac{(v_{\mathrm{uz}}-v_{\mathrm{zj}})r_j^2-(z_u-z_j)J_j}{r_j^3}---\left(13\right)$

$h_{{2v}_{\mathrm{ux}}}^j=\frac{{\∂D}_j}{{\∂v}_{\mathrm{ux}}}=\frac{x_u-x_j}{r_j}$

$h_{{2v}_{\mathrm{uy}}}^j=\frac{{\∂D}_j}{{\∂v}_{\mathrm{uy}}}=\frac{y_u-y_j}{r_j}$

$h_{{2v}_{\mathrm{uz}}}^j=\frac{{\∂D}_j}{{\∂v}_{\mathrm{uz}}}=\frac{z_u-z_j}{r_j}$

$J_j=(x_u-x_j)(v_{\mathrm{ux}}-v_{\mathrm{xj}})+(y_u-y_j)(v_{\mathrm{uy}}-v_{\mathrm{yj}})+(z_u-z_j)(v_{\mathrm{uz}}-v_{\mathrm{zj}})---\left(14\right)$

$r_j=\sqrt{{(x_j-x_u)}^2+{(y_j-y_u)}^2+{(z_j-z_u)}^2}---\left(15\right)$

上述各式中各量的物理含义如下：

Z_k——观测向量；

H_k——观测矩阵；

H₁——与伪距观测量有关的观测矩阵分量；

H₂——与多普勒观测量有关的观测矩阵分量；

——观测量ρ_j对状态变量x_u的偏导数，反映了系统测量值ρ_j和系统状态x_u之间的关系；

——观测量ρ_j对状态变量y_u的偏导数，反映了系统测量值ρ_j和系统状态y_u之间的关系；

——观测量ρ_j对状态变量z_u的偏导数，反映了系统测量值ρ_j和系统状态z_u之间的关系；

——观测量D_j对状态变量x_u的偏导数，反映了系统测量值D_j和系统状态x_u之间的关系；

——观测量D_j对状态变量y_u的偏导数，反映了系统测量值D_j和系统状态y_u之间的关系；

——观测量D_j对状态变量z_u的偏导数，反映了系统测量值D_j和系统状态z_u之间的关系；

——观测量D_j对状态变量v_ux的偏导数，反映了系统测量值D_j和系统状态v_ux之间的关系；

——观测量D_j对状态变量v_uy的偏导数，反映了系统测量值D_j和系统状态v_uy之间的关系；

——观测量D_j对状态变量v_uz的偏导数，反映了系统测量值D_j和系统状态v_uz之间的关系；

ρ_j——接收机测得的第j颗卫星的伪距观测量；

D_j——接收机测得的第j颗卫星的多普勒观测量；

J_j——卫星与接收机的相对速度与相对位置向量的点积；

r_j——第j颗卫星相对于接收机的向径；

（3）实现滤波计算回路和增益计算回路

根据步骤（1）和步骤（2）中建立的状态方程和量测方程，将状态初始量输入状态方程实现状态一步预测，通过均方误差阵一步预测和滤波增益方程实现增益矩阵K的更新，增益矩阵K代入量测方程得到状态增量；状态的一步预测加上状态增量作为下一次状态一步预测的输入即实现了滤波计算回路；根据增益矩阵K计算新的均方误差矩阵P作为下一次均方误差一步预测的输入即实现了增益计算回路；

（4）根据步骤（3）中滤波计算回路和增益计算回路中的状态方程选取运动模型，在低动态用户时，选取位置和速度模型作为运动模型，所述低动态用户是指汽车、轮船及地面手持设备；在高动态用户时，选取位置、速度和加速度模型，所述高动态用户是指导弹、飞机及卫星，所述加速度模型中的加速度信息使用惯性导航提供；

（5）将上述步骤中建立的状态方程、量测方程、滤波计算回路执行流程、增益计算回路执行流程以及不同动态用户模型的选取，得到完整的扩展卡尔曼滤波方程，用于实现卫星阻塞情况下用户的持续导航算法；其中扩展卡尔曼滤波方程如下：

${\hat{X}}_{k/k-1}=f({\hat{X}}_{k-1},k-1)---\left(9\right)$

$P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(10\right)$

$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}---\left(11\right)$

Z_k=H_kX_k+V_k+y_k                 (12)

${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-h({\hat{X}}_{k/k-1},k))---\left(13\right)$

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

上式中各量的物理含义如下：

——k-1时刻系统状态变量的最优估计值（后验估计值）；

——基于

推出的k时刻系统状态变量的先验估计值；

Φ_k,k-1——系统由k-1时刻到k时刻的状态转移矩阵；

P_k/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时刻的观测噪声协方差矩阵；

Z_k——k时刻的观测向量；

X_k——k时刻的状态向量；

V_k——k时刻的观测噪声协方差矩阵；

——k时刻系统状态变量的最优估计值（后验估计值）；

P_k——k时刻系统状态变量后验估计误差的均方差矩阵；

I——单位矩阵；

所述步骤（4）中低运动模型的状态量的选取ECEF坐标系下的三个方向的位置、速度、钟差和钟漂：

$\text{X=}{[x_u,v_{\mathrm{ux}},y_u,v_{\mathrm{uy}},z_u,v_{\mathrm{uz}},{\mathrm{ct}}_u,c{\stackrel{\·}{t}}_u]}^T$

上式中：

X——系统的状态向量；

x_u——载体在ECEF坐标系中X轴方向的位置；

v_ux——载体在ECEF坐标系中X轴方向的速度；

y_u——载体在ECEF坐标系中Y轴方向的位置；

v_uy——载体在ECEF坐标系中Y轴方向的速度；

z_u——载体在ECEF坐标系中Z轴方向的位置；

v_uz——载体在ECEF坐标系中Z轴方向的速度；

ct_u——系统钟差；

——系统钟漂；

状态噪声方差和量测噪声方差选用固定值，该固定值的选取方法是在用户设备定位后根据伪距误差量和伪距率的误差量统计后得到，将统计得到的量测噪声扩大至统计数值的3-4倍，防止量测噪声大导致算法发散。

所述步骤（4）中高动态运动模型的状态量的选取三个方向的位置、速度、加速度、钟差和钟漂：

$\text{X=}{[x_u,v_{\mathrm{ux}},a_{\mathrm{ux}},y_u,v_{\mathrm{uy}},a_{\mathrm{uy}},z_u,v_{\mathrm{uz}},a_{\mathrm{uz}},{\mathrm{ct}}_u,c{\stackrel{\·}{t}}_u]}^T$

上式中：

X——系统的状态向量；

x_u——载体在ECEF坐标系中X轴方向的位置；

v_ux——载体在ECEF坐标系中X轴方向的速度；

a_ux——载体在ECEF坐标系中X轴方向的加速度；

y_u——载体在ECEF坐标系中Y轴方向的位置；

v_uy——载体在ECEF坐标系中Y轴方向的速度；

a_uy——载体在ECEF坐标系中Y轴方向的加速度；

z_u——载体在ECEF坐标系中Z轴方向的位置；

v_uz——载体在ECEF坐标系中Z轴方向的速度；

a_uz——载体在ECEF坐标系中Z轴方向的加速度；

ct_u——系统钟差；

——系统钟漂；

状态噪声方差和量测噪声方差进行实时的调整时，要使用惯性导航的辅助位置，根据惯性导航的位置和当前卫星位置算出该卫星的伪距，与当前接收机观测的伪距作差再开方后扩大2-3倍，得到量测噪声方差中位置分量，速度分量与位置类似，使用惯性导航的辅助速度，根据惯性导航的速度和当前卫星速度算出该卫星的伪距，与当前接收机观测的伪距率作差再开方后扩大2-3倍，得到量测噪声方差中速度分量。

本发明与现有技术相比的优点在于：

（1）本发明采用针对低动态（汽车、轮船等）用户和高动态（动车、飞机、导弹）用户建立不同的状态方程模型和量测方程。优化状态方程中的用户模型和量测方程噪声系数，使EKF算法克服最小二乘在不同时刻之间定位不互相关联这一缺点，并且在卫星信号阻塞情况下的实现导航解算。该持续导航方法针对低动态用户和高动态用户分别使用不同方法来实时调整状态方程噪声方差和量测方程噪声方差的大小，低动态通过减少状态方程噪声方差阵W_u(k-1)会使滤波结果中状态方程的更新量所占权重增加，有效地减少卫星信号阻塞情况下卫星量测数据不足导致的量测误差相对增加的而导致定位精度变差的影响，提供持续的定位。高动态用户采用惯导辅助的加速度信息后，可以降低V_k分量，从而使得量测方程的更新量所占权重增加，有效地减少卫星信号阻塞情况下状态方程误差估计不准导致的定位精度变差的影响，提供持续的定位。在卫星信号阻塞情况下，实时的调整状态方程噪声方差（给出符号）和量测方程噪声方差的大小，来改变状态增量的大小，从而充分利用运动模型的优点实现持续的导航定位；

（2）本发明的一种卫星信号阻塞情况下的持续导航算法即是使用EKF技术实现了信号阻塞情况下的持续定位，将用户在相邻时刻的位置状态联系起来，加上惯导辅助数据的使用，使定位结果更加平滑、准确，并且能适应高动态场景，提高了导航设备的稳定性和可靠性。

附图说明

图1为本发明卡尔曼滤波工作流程图；

图2为卫星信号阻塞情况下算法执行流程图。

具体实施方式

一种卫星信号阻塞情况下的持续导航方法，所述卫星信号阻塞是指可见卫星数会不足4颗，如图1所示，实现步骤如下：

（1）建立导航解算状态方程，选择用户三轴方向的位置、速度和加速度作为状态量并建立用户运动模型，同时将用户运动模型和时钟模型组合在一起并离散化作为状态方程；具体如下

①载体运动模型

利用Singer模型作为载体运动模型，设采样周期为T，离散化的载体运动模型为：

X_u(k)=Φ_u(k,k-1)X_u(k-1)+W_u(k-1)    （公式1）

式中

${\mathrm{\Φ}}_u(k,k-1)=\left[\begin{array}{ccc}{\mathrm{\Φ}}_x(k,k-1)& \mathrm{}& \mathrm{}\\ \mathrm{}& {\mathrm{\Φ}}_y(k,k-1)& \mathrm{}\\ \mathrm{}& \mathrm{}& {\mathrm{\Φ}}_z(k,k-1)\end{array}\right]$     （公式2）

${\mathrm{\Φ}}_x(k,k-1)=\left[\begin{array}{ccc}1& T& \frac{1}{{\mathrm{\α}}_x^2}(-1+{\mathrm{\α}}_{x}T+e^{-{\mathrm{\α}}_{x}T})\\ 0& 1& \frac{1}{{\mathrm{\α}}_x}(1-e^{-{\mathrm{\α}}_{x}T})\\ 0& 0& e^{-{\mathrm{\α}}_{x}T}\end{array}\right]$        （公式3）

又有：

$Q_u(k-1)=E\left[W_u(k-1)W_u^T(k-1)\right]=\left[\begin{array}{ccc}{2\mathrm{\α}}_x{\mathrm{\σ}}_x^2{Q}_x(k-1)& 0& 0\\ 0& {2\mathrm{\α}}_y{\mathrm{\σ}}_y^2{Q}_y(k-1)& 0\\ 0& 0& {2\mathrm{\α}}_z{\mathrm{\σ}}_z^2{Q}_z(k-1)\end{array}\right]$

                                                         （公式4）

$Q_x(k-1)=\left[\begin{array}{ccc}{q}_{11}& q_{12}& q_{13}\\ q_{21}& q_{22}& q_{23}\\ q_{31}& q_{32}& q_{33}\end{array}\right]$          （公式5）

$q_{11}=\frac{1}{2{\mathrm{\α}}_x^5}[1-e^{-2{\mathrm{\α}}_{x}T}+2{\mathrm{\α}}_{x}T+\frac{2}{3}{\mathrm{\α}}_x^3{T}^3-2{\mathrm{\α}}_x^2{T}^2-4{\mathrm{\α}}_x{\mathrm{Te}}^{-{\mathrm{\α}}_{x}T}]$   （公式6）

$q_{12}=q_{21}=\frac{2}{2{\mathrm{\α}}_x^4}[e^{-2{\mathrm{\α}}_{x}T}+1-2e^{-{\mathrm{\α}}_{x}T}+2{\mathrm{\α}}_{x}T{e}^{-{\mathrm{\α}}_{x}T}-2{\mathrm{\α}}_{x}T+{\mathrm{\α}}_x^2{T}^2]$   （公式7）

$q_{13}=q_{31}=\frac{1}{2{\mathrm{\α}}_x^3}[1-e^{-2{\mathrm{\α}}_{x}T}-2{\mathrm{\α}}_{x}T{e}^{-{\mathrm{\α}}_{x}T}]$         （公式8）

$q_{22}=\frac{1}{2{\mathrm{\α}}_x^3}[4e^{-{\mathrm{\α}}_{x}T}-3-e^{-2{\mathrm{\α}}_{x}T}+2{\mathrm{\α}}_{x}T]$            （公式9）

$q_{23}=q_{32}=\frac{1}{2{\mathrm{\α}}_x^2}[e^{-2{\mathrm{\α}}_{x}T}+1-2e^{-{\mathrm{\α}}_{x}T}]$            （公式10）

$q_{33}=\frac{1}{2{\mathrm{\α}}_x}[1-e^{-2{\mathrm{\α}}_{x}T}]$ （公式11）

以上公式中各量的物理意义如下：

X_u(k)——k时刻载体运动模型的状态向量，其分量包括载体三轴的位置、速度和加速度；

Φ_u(k,k-1)——载体运动模型从k-1时刻到k时刻的状态转移矩阵；

W_u(k-1)——k-1时刻的离散时间白噪声序列；

Φ_x(k,k-1)——载体运动模型从k-1时刻到k时刻X轴分量的状态转移矩阵；

Φ_y(k,k-1)——载体运动模型从k-1时刻到k时刻Y轴分量的状态转移矩阵；

Φ_z(k,k-1)——载体运动模型从k-1时刻到k时刻Z轴分量的状态转移矩阵；

Q_u(k-1)——k-1时刻白噪声序列的均方差；

α_x——载体在ECEF坐标系X轴方向的机动频率；

α_y——载体在ECEF坐标系Y轴方向的机动频率；

α_z——载体在ECEF坐标系Z轴方向的机动频率；

——载体在ECEF坐标系X轴方向的机动加速度方差；

——载体在ECEF坐标系Y轴方向的机动加速度方差；

——载体在ECEF坐标系Z轴方向的机动加速度方差；

Q_x(k-1)——k-1时刻白噪声序列均方差与X轴状态有关的分量；

Q_y(k-1)——k-1时刻白噪声序列均方差与Y轴状态有关的分量；

Q_z(k-1)——k-1时刻白噪声序列均方差与Z轴状态有关的分量；

T——采样周期；

q_ij——矩阵Q_x(k-1)第i行第j列的元素；

以上各式中，Φ_y(k,k-1)和Φ_z(k,k-1)与Φ_x(k,k-1)原理相同，在（公式3）中分别以α_y和α_z代替α_x，即可得到Φ_y(k,k-1)和Φ_z(k,k-1)的计算公式。Q_y(k-1)和Q_z(k-1)与Q_x(k-1)原理相同，在（公式5）～（公式11）中以α_y、

和α_z、

代替α_x、

即可得到Q_y(k-1)、Q_z(k-1)的计算公式。

②接收机时钟模型

接收机时钟模型建立如下：

X_c(k)=Φ_c(k,k-1)X_c(k-1)+W_c(k-1)     （公式12）

式中

${\mathrm{\Φ}}_c(k,k-1)=\left[\begin{array}{cc}1& T\\ 0& 1\end{array}\right]$          （公式13）

又有：

$Q_c=(k-1)=E\left[W_c(k-1)W_c^T(k-1)\right]=\left[\begin{array}{cc}{Q}_{11}& Q_{12}\\ Q_{21}& Q_{22}\end{array}\right]$      （公式14）

$Q_{11}=\frac{h_0}{2}T+{2h}_{-1}{T}^2+\frac{2}{3}{\mathrm{\π}}^1{h}_{-2}{T}^3$        （公式15）

Q₁₂=2h_-1T+π²h_-2T³          （公式16）

$Q_{22}=\frac{h_0}{2T}+2h_{-1}+\frac{8}{3}{\mathrm{\π}}^2{h}_{-2}T$              （公式17）

以上公式中各量的物理意义如下：

X_c(k)——k时刻接收机时钟模型的状态向量，其分量包括接收机的钟差和钟漂；

Φ_c(k,k-1)——接收机时钟模型从k-1时刻到k时刻的状态转移矩阵；

W_c(k-1)——k-1时刻的离散时间白噪声序列；

Q_c(k-1)——k-1时刻接收机时钟模型的白噪声序列均方差；

h_i——时钟模型参数，i取0、-1、-2；

Q_ij——矩阵Q_c(k-1)第i行第j列的元素；

T——采样周期；

根据典型的石英钟标准，取h₀=9.4×10^-20，h_-1=1.8×10^-19，h_-2=3.8×10^-21。

（2）首先选择伪距和伪距率作为观测量，建立伪距方程和多普勒方程作为导航解算的量测方程；

建立扩展卡尔曼滤波观测方程：

Z_k=H_kX_k+V_k                （公式18）

上式中各量的物理含义如下：

X_k——状态向量；

Z_k——观测向量；

H_k——观测矩阵；

V_k——观测噪声协方差矩阵；

设m为接收机观测到的卫星数目，（公式18）中：

Z_k=[ρ₁，ρ₂，...ρ_m，D1，D2，...，D_m]^T    （公式19）

$H_k=\left[\begin{array}{c}{H}_1\\ H_2\end{array}\right]$              （公式20）

$H_1=\left[\begin{array}{ccccccccccc}{h}_{{1x}_u}^1& 0& 0& h_{{1y}_u}^1& 0& 0& h_{{1z}_u}^1& 0& 0& 1& 0\\ h_{{1x}_u}^2& 0& 0& h_{{1y}_u}^2& 0& 0& h_{{1z}_u}^2& 0& 0& 1& 0\\ \·& \·& \·& \·& \·& \·& \·& \·& \·& \·& \·\\ \·& \·& \·& \·& \·& \·& \·& \·& \·& \·& \·\\ \·& \·& \·& \·& \·& \·& \·& \·& \·& \·& \·\\ h_{{1x}_u}^m& 0& 0& h_{{1y}_u}^m& 0& 0& h_{{1z}_u}^m& 0& 0& 1& 0\end{array}\right]$     （公式21）

$h_{{1x}_u}^j=\frac{{\∂\mathrm{\ρ}}_j}{{\∂x}_u}=\frac{x_u-x_j}{r_j}$

$h_{{1y}_u}^j=\frac{{\∂\mathrm{\ρ}}_j}{{\∂y}_u}=\frac{y_u-y_j}{r_j}$                    （公式22）

$h_{{1z}_u}^j=\frac{{\∂\mathrm{\ρ}}_j}{{\∂z}_u}=\frac{z_u-z_j}{r_j}$

$H_2=\left[\begin{array}{ccccccccccc}{h}_{{2x}_u}^1& h_{{2v}_{\mathrm{ux}}}^1& 0& h_{{2y}_u}^1& h_{{2v}_{\mathrm{uy}}}^1& 0& h_{{2z}_u}^1& h_{{2v}_{\mathrm{uz}}}^1& 0& 0& 1\\ h_{{2x}_u}^2& h_{{2v}_{\mathrm{ux}}}^2& 0& h_{2y_u}^2& h_{{2v}_{\mathrm{uy}}}^2& 0& h_{2z_u}^2& h_{{2v}_{\mathrm{uz}}}^2& 0& 0& 1\\ \·& \·& \·& \·& \·& \·& \·& \·& \·& \·& \·\\ \·& \·& \·& \·& \·& \·& \·& \·& \·& \·& \·\\ \·& \·& \·& \·& \·& \·& \·& \·& \·& \·& \·\\ h_{{2x}_u}^m& h_{{2v}_{\mathrm{ux}}}^m& 0& h_{{2y}_u}^m& h_{{2v}_{\mathrm{uy}}}^m& 0& h_{{2z}_u}^m& h_{{2v}_{\mathrm{uz}}}^m& 0& 0& 1\end{array}\right]$ （公式23）

$h_{{2x}_u}^j=\frac{{\∂D}_j}{{\∂x}_u}=\frac{(v_{\mathrm{ux}}-v_{\mathrm{xj}})r_j^2-(x_u-x_j)J_j}{r_j^3}$

$h_{{2y}_u}^j=\frac{{\∂D}_j}{{\∂y}_u}=\frac{(v_{\mathrm{uy}}-v_{\mathrm{yj}})r_j^2-(y_u-y_j)J_j}{r_j^3}$

$h_{{2z}_u}^j=\frac{{\∂D}_j}{{\∂z}_u}=\frac{(v_{\mathrm{uz}}-v_{\mathrm{zj}})r_j^2-(z_u-z_j)J_j}{r_j^3}$       （公式24）

$h_{{2v}_{\mathrm{ux}}}^j=\frac{{\∂D}_j}{{\∂v}_{\mathrm{ux}}}=\frac{x_u-x_j}{r_j}$

$h_{{2v}_{\mathrm{uy}}}^j=\frac{{\∂D}_j}{{\∂v}_{\mathrm{uy}}}=\frac{y_u-y_j}{r_j}$

$h_{{2v}_{\mathrm{uz}}}^j=\frac{{\∂D}_j}{{\∂v}_{\mathrm{uz}}}=\frac{z_u-z_j}{r_j}$

J_j=(x_u-x_j)(v_ux-v_xj)+(y_u-y_j)(v_uy-v_yj)+(z_u-z_j)(v_uz-v_zj)  （公式25）

上述各式中各量的物理含义如下：

Z_k——观测向量；

H_k——观测矩阵；

H₁——与伪距观测量有关的观测矩阵分量；

H₂——与多普勒观测量有关的观测矩阵分量；

——观测量ρ_j对状态变量x_u的偏导数，反映了系统测量值ρ_j和系统状态x_u之间的关系；

——观测量ρ_j对状态变量y_u的偏导数，反映了系统测量值ρ_j和系统状态y_u之间的关系；

——观测量ρ_j对状态变量z_u的偏导数，反映了系统测量值ρ_j和系统状态z_u之间的关系；

——观测量D_j对状态变量x_u的偏导数，反映了系统测量值D_j和系统状态x_u之间的关系；

——观测量D_j对状态变量y_u的偏导数，反映了系统测量值D_j和系统状态y_u之间的关系；

——观测量D_j对状态变量z_u的偏导数，反映了系统测量值D_j和系统状态z_u之间的关系；

——观测量D_j对状态变量v_ux的偏导数，反映了系统测量值D_j和系统状态v_ux之间的关系；

——观测量D_j对状态变量v_uy的偏导数，反映了系统测量值D_j和系统状态v_uy之间的关系；

——观测量D_j对状态变量v_uz的偏导数，反映了系统测量值D_j和系统状态v_uz之间的关系；

ρ_j——接收机测得的第j颗卫星的伪距观测量；

D_j——接收机测得的第j颗卫星的多普勒观测量；

J_j——卫星与接收机的相对速度与相对位置向量的点积；

r_j——第j颗卫星相对于接收机的向径；

需要注意的是在计算观测矩阵H_k的时候，使用的是状态向量的一步预测值

为表述简洁，（公式21）～（公式25）中使用了状态变量的基本形式，没有加入时间标记。

V_k为观测噪声序列，R_k为观测噪声协方差矩阵，

      （公式26）

上式中各量的物理含义如下：

R_k——噪声协方差矩阵；

V_k——观测噪声矩阵；

——伪距观测量的观测防炸；

——多普勒观测量的观测防炸；

（3）扩展卡尔曼滤波定位方程

根据上述的系统状态方程和观测方程，建立扩展卡尔曼滤波定位方程如下：

${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{X}_{k-1}+W_{k-1}$       （公式27）

${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-h({\hat{X}}_{k/k-1},k))$       （公式28）

$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}$       （公式29）

$P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{{\mathrm{\Φ}}^T}_{k,k-1}+Q_{k-1}$       （公式30）

$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T$       （公式31）

以上各式中各量的物理意义如下：

——k-1时刻系统状态变量的最优估计值（后验估计值）；

——基于

推出的k时刻系统状态变量的先验估计值；

Φ_k,k-1——系统由k-1时刻到k时刻的状态转移矩阵；

P_k/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时刻的观测噪声协方差矩阵；

Z_k——k时刻的观测向量；

X_k——k时刻的状态向量；

V_k——k时刻的观测噪声协方差矩阵；

——k时刻系统状态变量的最优估计值（后验估计值）；

P_k——k时刻系统状态变量后验估计误差的均方差矩阵；

I——单位矩阵；

本发明首先是建立状态方程和量测方程用于EKF滤波，针对不同用户建立不同的用户模型和相对应的状态方程系数和量测方程噪声系数，针对高动态用户，使用惯导加速度信息辅助EKF滤波提高定位精度。

（1）卫星信号阻塞情况下处理

本发明在卫星阻塞情况下针对不同动态的用户使用不同的策略如图2提高定位精度。

低动态场景下：由于动态很小，机动频率和加速度信息都很小，用户运动模型使用位置速度（PV）模型，由于EKF算法中状态方程中的W_u(k-1)分量是随着为用户在ECEF坐标系三个坐标轴方向上的机动频率和机动加速度方差量变化的，由于低动态用户该变化量很小，减少状态方程噪声方差阵W_u(k-1)会使滤波结果中状态方程的更新量所占权重增加，有效地减少卫星信号阻塞情况下卫星量测数据不足导致的量测误差相对增加的而导致定位精度变差的影响，提供持续的定位。

高动态场景下：用户处于高动态场景下，用户运动模型使用位置、速度、加速度（PVA）模型，ECEF坐标系三个坐标轴方向上的机动频率和机动加速度方差量变化剧烈，导致状态方程中的误差增大影响定位精度甚至不能持续定位，此时采用惯导辅助的加速度信息后，可以降低V_k分量，从而使得量测方程的更新量所占权重增加，有效地减少卫星信号阻塞情况下状态方程误差估计不准导致的定位精度变差的影响，提供持续的定位。

本发明未详细阐述部分属于本领域公知技术。

Claims (3)

1.一种卫星信号阻塞情况下的持续导航方法，所述卫星信号阻塞是指可见卫星数会不足4颗，其特征在于实现步骤 

如下： 

（1）建立导航解算状态方程，选择用户三轴方向的位置、速度和加速度作为状态量并建立用户运动模型，同时将用户运动模型和时钟模型组合在一起并离散化作为状态方程； 

X_k=Φ_k,k-1X_k-1+W_k-1                 (1) 

式中 

上式中各量的物理含义如下： 

——系统的状态向量，包括载体在ECEF坐标系中三个方向上的位置、速度、加速度，以及系统的钟差、钟漂； 

x_u——载体在ECEF坐标系中X轴方向的位置； 

v_ux——载体在ECEF坐标系中X轴方向的速度； 

a_ux——载体在ECEF坐标系中X轴方向的加速度； 

y_u——载体在ECEF坐标系中Y轴方向的位置； 

v_uy——载体在ECEF坐标系中Y轴方向的速度； 

a_uy——载体在ECEF坐标系中Y轴方向的加速度； 

z_u——载体在ECEF坐标系中Z轴方向的位置； 

v_uz——载体在ECEF坐标系中Z轴方向的速度； 

a_uz——载体在ECEF坐标系中Z轴方向的加速度； 

ct_u——系统钟差； 

——系统钟漂； 

Φ_k,k-1——状态转移矩阵，表征系统状态由k-1时刻向k时刻状态转移的情况。状态转移矩阵Φ_k,k-1由运动模型状态转移矩阵Φ_u(k,k-1)和时钟模型转移矩阵Φ_c(k,k-1)组成； 

Φ_u(k,k-1)——运动模型状态转移矩阵，表征在状态转移过程中与系统运动模型相关的分量； 

Φ_c(k,k-1)——时钟模型转移矩阵，表征在状态转移过程中与系统时钟模型相关的分量； 

W_k-1——白噪声序列，表征在k-1时刻系统的过程噪声。白噪声序列W_k-1由位置白噪声序列W_u(k-1)和时钟白噪声序列W_c(k-1)组成； 

W_u(k-1)——位置白噪声序列； 

W_c(k-1)——时钟白噪声序列； 

Q_k-1——白噪声序列的均方差； 

（2）选择伪距和伪距率作为观测量，建立伪距方程和多普勒方程作为导航解算的量测方程； 

伪距方程和多普勒方程分别表示如下： 

上式中各量的物理含义如下： 

ρ_j——接收机测得的第j颗卫星的伪距观测量； 

D_j——接收机测得的第j颗卫星的多普勒观测量； 

x_j——第j颗卫星在ECEF坐标系中X轴方向的位置； 

y_j——第j颗卫星在ECEF坐标系中Y轴方向的位置； 

z_j——第j颗卫星在ECEF坐标系中Z轴方向的位置； 

v_xj——第j颗卫星在ECEF坐标系中X轴方向的速度； 

v_yj——第j颗卫星在ECEF坐标系中Y轴方向的速度； 

v_zj——第j颗卫星在ECEF坐标系中Z轴方向的速度； 

——第j颗卫星相对于接收机的向径； 

x_u——接收机在ECEF坐标系中X轴方向的位置； 

y_u——接收机在ECEF坐标系中Y轴方向的位置； 

z_u——接收机在ECEF坐标系中Z轴方向的位置； 

v_ux——接收机在ECEF坐标系中X轴方向的速度； 

v_uy——接收机在ECEF坐标系中Y轴方向的速度； 

v_uz——接收机在ECEF坐标系中Z轴方向的速度； 

ct_u——接收机的钟差； 

——接收机的钟漂； 

然后，根据公式（5）、（6）建立量测方程： 

Z_k=H_kX_k+V_k                                (7) 

上式中各量的物理含义如下： 

X_k——状态向量； 

Z_k——观测向量； 

H_k——观测矩阵； 

V_k——观测噪声协方差矩阵； 

设m为接收机观测到的卫星数目，则有： 

Z_k=[ρ₁,ρ₂,…ρ_m,D₁,D₂,…,D_m]^T                    (8) 

上述各式中各量的物理含义如下： 

Z_k——观测向量； 

H_k——观测矩阵； 

H₁——与伪距观测量有关的观测矩阵分量； 

H₂——与多普勒观测量有关的观测矩阵分量； 

——观测量ρ_j对状态变量x_u的偏导数，反映了系统测量值ρ_j和系统状态x_u之间的关系； 

——观测量ρ_j对状态变量y_u的偏导数，反映了系统测量值ρ_j和系统状态y_u之间的关系； 

——观测量ρ_j对状态变量z_u的偏导数，反映了系统测量值ρ_j和系统状态z_u之间的关系； 

——观测量D_j对状态变量x_u的偏导数，反映了系统测量值D_j和系统状态x_u之间的关系； 

——观测量D_j对状态变量y_u的偏导数，反映了系统测量值D_j和系统状态y_u之间的关系； 

——观测量D_j对状态变量z_u的偏导数，反映了系统测量值D_j和系统状态z_u之间的关系； 

——观测量D_j对状态变量v_ux的偏导数，反映了系统测量值D_j和系统状态v_ux之间的关系； 

——观测量D_j对状态变量v_uy的偏导数，反映了系统测量值D_j和系统状态v_uy之间的关系； 

——观测量D_j对状态变量v_uz的偏导数，反映了系统测量值D_j和系统状态v_uz之间的关系； 

ρ_j——接收机测得的第j颗卫星的伪距观测量； 

D_j——接收机测得的第j颗卫星的多普勒观测量； 

J_j——卫星与接收机的相对速度与相对位置向量的点积； 

r_j——第j颗卫星相对于接收机的向径； 

（3）实现滤波计算回路和增益计算回路 

根据步骤（1）和步骤（2）中建立的状态方程和量测方程，将状态初始量输入状态方程实现状态一步预测，通过均方误差阵一步预测和滤波增益方程实现增益矩阵K的更新，增益矩阵K代入量测方程得到状态增量；状态的一步预测加上状态增量作为下一次状态一步预测的输入即实现了滤波计算回路；根据增益矩阵K计算新的均方误差矩阵P作为下一次均方误差一步预测的输入即实现了增益计算回路； 

（4）根据步骤（3）中滤波计算回路和增益计算回路中的状态方程选取运动模型，在低动态用户时，选取位置和速度模型作为运动模型，所述低动态用户是指汽车、轮船及地面手持设备；在高动态用户时，选取位置、速度和加速度模型，所述高动态用户是指导弹、飞机及卫星，所述加速度模型中的加速度信息使用惯性导航提供； 

（5）将上述步骤中建立的状态方程、量测方程、滤波计算回路执行流程、增益计算回路执行流程以及不同动态用户模型的选取，得到完整的扩展卡尔曼滤波方程，用于实现卫星阻塞情况下用户的持续导航算法；其中扩展卡尔曼滤波方程如下： 

Z_k=H_kX_k+V_k+y_k            (12) 

上式中各量的物理含义如下： 

——k-1时刻系统状态变量的最优估计值（后验估计值）； 

——基于

推出的k时刻系统状态变量的先验估计值； 

Φ_k,k-1——系统由k-1时刻到k时刻的状态转移矩阵； 

P_k/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时刻的观测噪声协方差矩阵； 

Z_k——k时刻的观测向量； 

X_k——k时刻的状态向量； 

V_k——k时刻的观测噪声协方差矩阵； 

——k时刻系统状态变量的最优估计值（后验估计值）； 

P_k——k时刻系统状态变量后验估计误差的均方差矩阵； 

I——单位矩阵。 

2.根据权利要求1所述的一种卫星信号阻塞情况下的持续导航方法，其特征在于：所述步骤（4）中低动态时的运动模型的状态量的选取ECEF坐标系下的三个方向的位置、速度、钟差和钟漂： 

上式中： 

X——系统的状态向量； 

x_u——载体在ECEF坐标系中X轴方向的位置； 

v_ux——载体在ECEF坐标系中X轴方向的速度； 

y_u——载体在ECEF坐标系中Y轴方向的位置； 

v_uy——载体在ECEF坐标系中Y轴方向的速度； 

z_u——载体在ECEF坐标系中Z轴方向的位置； 

v_uz——载体在ECEF坐标系中Z轴方向的速度； 

ct_u——系统钟差； 

——系统钟漂； 

状态噪声方差和量测噪声方差选用固定值，该固定值的选取方法是在用户设备定位后根据伪距误差量和伪距率的误差量统计后得到，将统计得到的量测噪声扩大至统计数值的3-4倍，防止量测噪声大导致算法发散。 

3.根据权利要求1所述的一种卫星信号阻塞情况下的持续导航方法，其特征在于：所述步骤（4）中高动态运动模型的状态量的选取三个方向的位置、速度、加速度、钟差和钟漂： 

上式中： 

X——系统的状态向量； 

x_u——载体在ECEF坐标系中X轴方向的位置； 

v_ux——载体在ECEF坐标系中X轴方向的速度； 

a_ux——载体在ECEF坐标系中X轴方向的加速度； 

y_u——载体在ECEF坐标系中Y轴方向的位置； 

v_uy——载体在ECEF坐标系中Y轴方向的速度； 

a_uy——载体在ECEF坐标系中Y轴方向的加速度； 

z_u——载体在ECEF坐标系中Z轴方向的位置； 

v_uz——载体在ECEF坐标系中Z轴方向的速度； 

a_uz——载体在ECEF坐标系中Z轴方向的加速度； 

ct_u——系统钟差； 

——系统钟漂； 

状态噪声方差和量测噪声方差进行实时的调整时，要使用惯性导航的辅助位置，根据惯性导航的位置和当前卫星位置算出该卫星的伪距，与当前接收机观测的伪距作差再开方后扩大2-3倍，得到量测噪声方差中位置分量。速度分量与位置类似，使用惯性导航的辅助速度，根据惯性导航的速度和当前卫星速度算出该卫星的伪距率，与当前接收机观测的伪距率作差再开方后扩大2-3倍，得到量测噪声方差中速度分量。 

Images