CN102749633B - 一种卫星导航接收机的动态定位解算方法 - Google Patents

Abstract

本发明提出一种卫星导航接收机的动态定位解算方法，属于卫星导航技术领域，该动态定位解算方法包括以下几个步骤：建立以机动加速度作为未知输入的载体运动模型作为状态方程；建立导航接收机伪距和伪距率的测量方程；基于预测滤波估计[t_k,t_k+1]区间的未知机动加速度；实现载体导航定位参数的预测；利用测量残差对预测状态进行修正，实现载体导航定位参数的估计。本发明采用的改进的预测滤波可以在线实时估计机动加速度并修正载体运动模型，克服了传统动态定位解算过程中将系统误差处理为零均值白噪声的缺点；不仅可以在线估计载体机动加速度，而且可以获得较好的定位效果，特别适合于载体机动未知或较大机动的情况。

Description

一种卫星导航接收机的动态定位解算方法

技术领域

本发明涉及一种卫星导航接收机的动态定位解算方法，属于卫星导航技术领域。

背景技术

导航定位解算是卫星导航接收机的重要组成部分，利用伪距和伪距率测量来计算载体的位置、速度、加速度等导航定位参数。常用的定位方法有最小二乘法（LS）和扩展卡尔曼滤波法（EKF）。其中最小二乘法（LS）简单不考虑载体的运动状态，定位精度较低；而扩展卡尔曼滤波法（EKF）需要对载体运动进行建模，定位精度优于最小二乘法（LS）。测量模型的非线性和系统模型的不精确是制约卫星导航接收机动态导航定位精度的两个主要因素。由于伪距和伪距率测量模型的非线性强，无迹卡尔曼滤波（UKF）对非线性具有良好的逼近效果，因此基于无迹卡尔曼滤波（UKF）的定位解算精度优于扩展卡尔曼滤波法（EKF）。但是载体在实际运动中存在多种形式的机动，很难建立精确的模型，特别是对于机动性强的载体，即使采用Singer模型、“当前”统计模型也难以准确描述载体运动的动态过程。如果动态定位所用运动模型不准确，尽管可以通过调整系统噪声的方差阵来尽可能的减少模型误差的影响，但是由于机动带来的模型误差并不满足白噪声特性，必将导致基于EKF和UKF的动态定位精度下降，因此该方法难以满足任意机动形式下的导航解算的需要。非线性预测滤波（NPF）是可以在线估计和修正模型误差的非线性递推状态估计方法。但NPF收敛速度较慢，且对初值误差较敏感，若NPF对模型误差估计不准确，通过状态传播还会导致较大的状态误差，甚至造成滤波发散。

发明内容

本发明的目的是为了解决载体机动情况未知或机动较大时扩展卡尔曼滤波法（EKF）的卫星导航接收机动态定位解算精度降低的问题，提出一种基于改进的预测滤波（IPF）的卫星导航接收机动态定位解算方法。

本发明是一种卫星导航接收机的动态定位解算方法，包括以下几个步骤：

步骤一：建立以机动加速度作为输入的载体运动方程，用该载体运动方程构成系统状态方程：

采用以机动加速度作为输入的载体运动模型，同时利用导航接收机的时钟误差来建立动态定位解算的状态方程，如下：

$\left\{\begin{array}{c}\stackrel{\·}{P}=V+w_v\\ \stackrel{\·}{V}=a_{\mathrm{in}}+w_a\\ {\stackrel{\·}{t}}_d=t_d+w_{\mathrm{tb}}\\ {\stackrel{\·}{t}}_d=w_{\mathrm{td}}\end{array}\right.---\left(1\right)$

其中，P＝[x y z]^T和V＝[v_x v_y v_z]^T分别为接收机在导航参考坐标系下位置向量和速度向量；x、y、z和v_x、v_y、v_z分别表示接收机在导航参考坐标系中的沿x、y、z轴的位置分量和速度分量，a_in＝[a_x a_y a_z]^T为由载体机动引起的未建模加速度向量，其中a_x、a_y、a_z表示在导航参考坐标系的沿x、y、z轴的加速度分量；t_b为接收机的等效时钟误差；t_d为接收机的等效时钟频率误差；系统噪声向量由速度噪声w_v、加速度噪声w_a、时钟误差噪声w_tb和时钟频率误差w_td构成，均为零均值的高斯白噪声，方差分别为σ_v、σ_a、σ_tb、σ_td。

步骤二：建立卫星导航接收机伪距和伪距率的观测方程：

利用接收机测量得到的伪距和伪距率构建测量向量y如下:

其中，ρ_i、

分别为第i颗卫星的伪距、伪距率测量值，i＝1,…num；num为用于动态导航解算的可见卫星颗数；

设第i颗卫星与接收机的几何距离R_i为：

$R_i=\sqrt{{(x-x_{\mathrm{si}})}^2+{(y-y_{\mathrm{si}})}^2+{(z-z_{\mathrm{si}})}^2}---\left(3\right)$

其中，x_si、y_si、z_si为在导航参考坐标系下第i颗卫星沿x、y、z轴的位置分量；

利用接收机测量得到的伪距和伪距率作为观测量，建立由第i颗卫星数据得到的伪距ρ_i和伪距率

测量方程分别如下：

ρ_i＝R_i+t_b+v_i      （4）

${\stackrel{\·}{\mathrm{\ρ}}}_i=\frac{(x-x_{\mathrm{si}})(v_x-v_{\mathrm{sxi}})}{R_i}+\frac{(y-y_{\mathrm{si}})(v_y-v_{\mathrm{syi}})}{R_i}+\frac{(z-z_{\mathrm{si}})(v_z-v_{\mathrm{szi}})}{R_i}+t_d+n_i---\left(2\right)$

其中v_sxi、v_syi、v_szi为导航参考坐标系下第i颗卫星沿x、y、z轴的速度分量；v_i表示伪距测量噪声，是方差为

的零均值高斯白噪声；n_i表示伪距率测量噪声，是方差为的零均值高斯白噪声。

步骤三：利用t_k+1时刻的伪距、伪距率观测量,基于预测滤波获得[t_k,t_k+1]区间的未知机动加速度：

将动态导航定位的系统模型表示为如下的非线性模型：

$\left\{\begin{array}{c}\stackrel{\·}{x}\left(t\right)=f(x\left(t\right),t)+g_d(x\left(t\right),t)d\left(t\right)+w\left(t\right)\\ y\left(t\right)=h(x\left(t\right),t)+v\left(t\right)\end{array}\right.---\left(6\right)$

其中，

为模型向量,

表示n维实空间；t为连续系统的时间变量；对于动态导航定位，f＝[v_x v_y v_z 000t_d 0]^T，n＝8；

为状态向量；

为模型误差向量,表示p维实空间，d(t)＝a_in，p＝3；

为模型误差扰动矩阵,

表示n×p维的实空间， $g_d=\left[\begin{array}{c}{0}_{3\×3}\\ \begin{array}{ccc}--& --& --\end{array}\\ I_{3\×3}\\ \begin{array}{ccc}--& --& --\end{array}\\ 0_{2\×3}\end{array}\right],$ I_3×3表示3×3的单位阵；

为测量向量，

表示m维的实空间；

为测量输出向量,m＝2×num；系统噪声w(t)是均值为零，方差为Q的高斯白噪声；测量噪声v(t)是均值为零、方差为R的高斯白噪声。

以下标k表示t_k时刻，根据预测滤波方法得到[t_k,t_k+1]区间的模型误差

为：

${\hat{d}}_k=-{\left\{{[\mathrm{\Λ}\left(T\right)U\left({\hat{x}}_k\right)}^T{R}^{-1}\right[\mathrm{\Λ}\left(T\right)U\left({\hat{x}}_k\right)]+w\}}^{-1}{\left[\mathrm{\Λ}\left(T\right)U\left({\hat{x}}_k\right)\right]}^T{R}^{-1}\{{\hat{y}}_k+z[{\hat{x}}_k,T]-y_{k+1}\}---\left(7\right)$

其中为t_k时刻的系统状态估计；

为由

根据测量方程得到的t_k时刻的测量估计；R为观测噪声方差阵；y_k+1为t_k+1时刻的实际测量向量；T为滤波周期；为灵敏度矩阵；Λ(T)为对角矩阵；

为列向量；W为加权矩阵。

灵敏度矩阵 $U\left({\hat{x}}_k\right)=\left[\begin{array}{c}\frac{\begin{array}{c}{U}_{{\mathrm{\ρ}}_1}\left({\hat{x}}_k\right)\\ \·\\ \·\\ \·\\ U_{{\mathrm{\ρ}}_n}\left({\hat{x}}_k\right)\end{array}}{\begin{array}{c}{U}_{{\stackrel{\·}{\mathrm{\ρ}}}_1}\left({\hat{x}}_k\right)\\ \·\\ \·\\ \·\\ U_{{\stackrel{\·}{\mathrm{\ρ}}}_n}\left({\hat{x}}_k\right)\end{array}}\end{array}\right],$ 其中分别与第i颗卫星的伪距、伪距率测量对应的行向量为:

$U_{{\mathrm{\ρ}}_i}\left({\hat{x}}_k\right)=\left[\begin{array}{ccc}\frac{x-x_{\mathrm{si}}}{R_i}& \frac{y-y_{\mathrm{si}}}{R_i}& \frac{z-z_{\mathrm{si}}}{R_i}\end{array}\right],$ $U_{{\stackrel{\·}{\mathrm{\ρ}}}_i}\left({\hat{x}}_k\right)=\left[\begin{array}{ccc}\frac{x-x_{\mathrm{si}}}{R_i}& \frac{y-y_{\mathrm{si}}}{R_i}& \frac{z-z_{\mathrm{si}}}{R_i}\end{array}\right],(i=1...\mathrm{num});$

对角阵Λ(T)为：

$\mathrm{\Λ}\left(T\right)=\left[\begin{array}{cc}{\mathrm{\Λ}}_{\mathrm{\ρ}}& \\ & {\mathrm{\Λ}}_{\stackrel{\·}{\mathrm{\ρ}}}\end{array}\right]---\left(8\right)$

其中，

Λ_ρ和

为符号标识；T为滤波周期；I_num×num为num×num的单位阵。

列向量 $z[{\hat{x}}_k,T]=\left[\begin{array}{c}\frac{\begin{array}{c}{z}_{{\mathrm{\ρ}}_1}[{\hat{x}}_k,T]\\ \·\\ \·\\ \·\\ z_{{\mathrm{\ρ}}_n}[{\hat{x}}_k,T]\end{array}}{\begin{array}{c}{z}_{{\stackrel{\·}{\mathrm{\ρ}}}_1}[{\hat{x}}_k,T]\\ \·\\ \·\\ \·\\ z_{{\stackrel{\·}{\mathrm{\ρ}}}_n}[{\hat{x}}_k,T]\end{array}}\end{array}\right],$ 其中分别与第i颗卫星的伪距、伪距率测量对应的元素为：

$z_{{\mathrm{\ρ}}_i}[{\hat{x}}_k,T]={\mathrm{TL}}_f^1{\mathrm{\ρ}}_i+\frac{T^2}{2}{L}_f^2{\mathrm{\ρ}}_i---\left(9a\right)$

$z_{{\stackrel{\·}{\mathrm{\ρ}}}_i}[{\hat{x}}_k,T]={\mathrm{TL}}_f^1{\stackrel{\·}{\mathrm{\ρ}}}_i---\left(9b\right)$

其中

和分别为ρ_i相对于f的1阶和2阶李导数;为

相对于f的1阶李导数。它们分别是：

$L_f^1{\mathrm{\ρ}}_i=\frac{x-x_{\mathrm{si}}}{R_i}{v}_x+\frac{y-y_{\mathrm{si}}}{R_i}{v}_y+\frac{z-z_{\mathrm{si}}}{R_i}{v}_z+t_d$

$L_f^2{\mathrm{\ρ}}_i=[\frac{v_x({R_i}^2-{(x-x_{\mathrm{si}})}^2)}{{R_i}^3}-\frac{(y-y_{\mathrm{si}})v_y(x-x_{\mathrm{si}})}{{R_i}^3}-\frac{(z-z_{\mathrm{si}})v_z(x-x_{\mathrm{si}})}{{R_i}^3}]v_x$

$+[\frac{v_y({R_i}^2-{(y-y_{\mathrm{si}})}^2)}{{R_i}^3}-\frac{(x-x_{\mathrm{si}})v_x(y-y_{\mathrm{si}})}{{R_i}^3}-\frac{(z-z_{\mathrm{si}})v_z(y-y_{\mathrm{si}})}{{R_i}^3}]v_y$

$+[\frac{v_z({R_i}^2-{(z-z_{\mathrm{si}})}^2)}{{R_i}^3}-\frac{(x-x_{\mathrm{si}})v_x(z-z_{\mathrm{si}})}{{R_i}^3}-\frac{(y-y_{\mathrm{si}})v_y(z-\left(z_{\mathrm{si}}\right))}{{R_i}^3}]v_z$

$L_f^1{\stackrel{\·}{\mathrm{\ρ}}}_1=[\frac{(v_x-v_{\mathrm{sxi}})({R_i}^2-{(x-x_{\mathrm{si}})}^2)}{{R_i}^3}-\frac{(y-y_{\mathrm{si}})(v_y-v_{\mathrm{syi}})(x-x_{\mathrm{si}})}{{R_i}^3}\frac{(z-z_{\mathrm{si}})(v_z-v_{\mathrm{szi}})(x-x_{\mathrm{si}})}{{R_i}^3}]v_x$

$+[\frac{(v_y-v_{\mathrm{syi}})({R_i}^2-{(y-y_{\mathrm{si}})}^2)}{{R_i}^3}-\frac{(x-x_{\mathrm{si}})(v_x-v_{\mathrm{sxi}})(y-y_{\mathrm{si}})}{{R_i}^3}-\frac{(z-z_{\mathrm{si}})(v_z-v_{\mathrm{szi}})(y-y_{\mathrm{si}})}{{R_i}^3}]v_y$

$+[\frac{(v_z-v_{\mathrm{szi}})({R_i}^2-{(z-z_{\mathrm{si}})}^2)}{{R_i}^3}-\frac{(x-x_{\mathrm{si}})(v_x-v_{\mathrm{sxi}})(z-z_{\mathrm{si}})}{{R_i}^3}-\frac{(y-y_{\mathrm{si}})(v_y-v_{\mathrm{sxi}})(z-z_{\mathrm{si}})}{{R_i}^3}]v_z$

最终根据公式（6）得到[t_k,t_k+1]区间的机动加速度估计。

步骤四：利用[t_k,t_k+1]区间的机动加速度估计修正系统模型，并进行时间更新，预测载体导航定位参数：

将得到的机动加速度作为系统输入代入系统模型公式（1）中，并对状态进行时间更新，更新过程如公式（10）、（11）所示，状态及其误差协方差阵的一步预测为：

${\hat{x}}_{k+1/k}={\hat{x}}_k+\mathrm{Tf}({\hat{x}}_k,t_k)+\frac{T^2}{2}\frac{\∂f(x,t_k)}{\∂x}{|}_{x=x_k}f({\hat{x}}_k,t_k)+{\mathrm{tg}}_d{\hat{d}}_k---\left(10\right)$

P_k+1/k＝(A_k-B_kM_kH_k)P_k(A_k-B_kM_kH_k)^T+Q′_k    （11）

其中

为t_k+1时刻的状态一步预测；P_k+1/k为t_k+1时刻的状态误差协方差阵一步预测;

为t_k时刻模型向量的估计值；f(x,t_k)为t_k时刻的模型向量；

为[t_k,t_k+1]区间的模型误差估计；g_d为误差扰动矩阵；A_k、B_k和M_k为实现表达而出现的变量，它们是

$M_k={\left\{{\left[\mathrm{\Λ}\left(T\right)U\left({\hat{x}}_k\right)\right]}^T{R}^{-1}\right[\mathrm{\Λ}\left(T\right)U\left({\hat{x}}_k\right)]+W\}}^{-1}{\left[\mathrm{\Λ}\left(T\right)U\left({\hat{x}}_k\right)\right]}^T{R}^{-1},$ $A_k=I+T[\frac{\∂f(x,t_k)}{\∂x}{|}_{x={\hat{x}}_k}+\frac{{\∂g}_d\·d}{\∂x}{|}_{x={\hat{x}}_k,d={\hat{d}}_k}],$

I为单位阵，d为模型误差；H_k为基于状态

得到的测量模型的雅可比矩阵，

Q′_k为对称正定矩阵；P_k为t_k时刻状态误差方差阵。

步骤五：进行测量更新，利用测量残差修正预测状态，实现载体导航定位参数的估计：

如式（12）、（13）、（14）所示，t_k+1时刻的增益矩阵K_k+1为：

$K_{k+1}=P_{k+1/k}{H}_{k+1/k}^T{(H_{k+1/k}{P}_{k+1/k}{H}_{k+1/k}^T+R_{k+1})}^{-1}---\left(12\right)$

H_k+1/k为基于预测状态

得到的测量模型的雅可比矩阵，R_k+1为t_k+1时刻观测的噪声方差阵；基于测量残差对t_k+1时刻的预测状态

进行修正得到t_k+1时刻的状态估计

及其误差协方差阵P_k+1为：：

${\hat{x}}_{k+1}={\hat{x}}_{k+1/k}+K_{k+1}[y_{k+1}-h\left({\hat{x}}_{k+1/k}\right)]---\left(13\right)$

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

其中I为单位阵，y_k+1为t_k+1时刻的伪距、伪距率的观测量，

为基于预测状态

得到的预测测量向量。

本发明的优点在于：

（1）本发明提出一种卫星导航接收机的动态定位解算方法，采用的改进的预测滤波（IPF）可以在线实时估计机动加速度并修正载体运动模型，克服了传统动态定位解算过程中将系统误差处理为零均值白噪声的缺点；

（2）本发明提出一种卫星导航接收机的动态定位解算方法，采用了改进的预测滤波（IPF）不仅可以在线估计载体机动加速度，而且可以获得较好的定位效果，特别适合于载体机动未知或较大机动的情况，并可以推广用于以载波相位和载波相位变化率作为观测量的接收机动态定位的场合。

附图说明

图1是本发明提出的一种卫星导航接收机的动态定位解算方法的流程图；

图2a是无初始误差时IPF（CV）与EKF（CV）和EKF（Singer）方法仿真结果的位置误差曲线对比图；

图2b是无初始误差时IPF（CV）与EKF（CV）和EKF（Singer）方法仿真结果的速度误差曲线对比图；

图2c是无初始误差时IPF（CV）与EKF（CV）和EKF（Singer）方法仿真结果的加速度误差曲线对比图；

图3a是有初始误差时IPF（CV）与EKF（CV）和EKF（Singer）方法仿真结果的位置误差曲线对比图；

图3b是有初始误差时IPF（CV）与EKF（CV）和EKF（Singer）方法仿真结果的速度误差曲线对比图；

图3c是有初始误差时IPF（CV）与EKF（CV）和EKF（Singer）方法仿真结果的加速度误差曲线对比图；

图4是仿真时的载体飞行三维轨迹图；

图5是系统加速度输入（a_in）的三轴变化曲线图；

具体实施方式

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

本发明提出一种卫星导航接收机的动态定位解算方法，利用预测滤波来实时在线估计并补偿所建载体运动模型误差—机动加速度，再利用EKF进行时间更新和测量更新，得到卫星接收机的导航参数，如图1所示，包括以下几个步骤：

步骤一：建立以机动加速度作为输入的载体运动方程，用该载体运动方程构成系统状态方程。

卫星导航接收机导航方法中常用的动态定位模型有常速度模型（CV模型）和Singer模型。其中，CV模型假设载体速度变化满足随机游走模型，当载体的速度发生较大变化时，将会造成较大的模型误差。Singer模型假设加速度变化满足一阶马尔科夫随机过程模型，用机动频率α表示载体的机动程度。当载体以复杂的机动形式运动时，将导致所用载体运动模型存在模型误差。

本发明中采用以机动加速度作为输入的载体运动模型，同时考虑导航接收机的时钟误差来建立动态定位解算的状态方程，如下：

$\left\{\begin{array}{c}\stackrel{\·}{P}=V+w_v\\ \stackrel{\·}{V}=a_{\mathrm{in}}+w_a\\ {\stackrel{\·}{t}}_d=t_d+w_{\mathrm{tb}}\\ {\stackrel{\·}{t}}_d=w_{\mathrm{td}}\end{array}\right.---\left(1\right)$

其中，P＝[x y z]^T和V＝[v_x v_y v_z]^T分别为接收机在导航参考坐标系下位置向量和速度向量；x、y、z和v_x、v_y、v_z分别表示接收机在导航参考坐标系中的沿x、y、z轴的位置分量和速度分量，a_in＝[a_x a_y a_z]^T为由载体机动引起的未建模加速度向量，其中a_x、a_y、a_z表示在导航参考坐标系的沿x、y、z轴的加速度分量；t_b为接收机的等效时钟误差；t_d为接收机的等效时钟频率误差；系统噪声向量由速度噪声w_v、加速度噪声w_a、时钟误差噪声w_tb和时钟频率误差w_td构成，均为零均值的高斯白噪声，方差分别为σ_v、σ_a、σ_tb、σ_td。

步骤二：建立卫星导航接收机伪距和伪距率的观测方程。

利用接收机测量得到的伪距和伪距率构建测量向量y如下:

其中，ρ_i、分别为第i颗卫星的伪距、伪距率测量值（i＝1,…num）；m为用于动态导航解算的可见卫星颗数，如果采用几何精度因子(GDOP)最小的4颗卫星的测量用于动态定位，则num＝4。

设第i颗卫星与接收机的几何距离R_i为：

$R_i=\sqrt{{(x-x_{\mathrm{si}})}^2+{(y-y_{\mathrm{si}})}^2+{(z-z_{\mathrm{si}})}^2}---\left(3\right)$

其中，x_si、y_si、z_si为在导航参考坐标系下第i颗卫星沿x、y、z轴的位置分量。

利用接收机测量得到的伪距和伪距率作为观测量，由此建立由第i颗卫星数据得到的伪距ρ_i和伪距率测量方程分别如下：

ρ_i＝R_i+t_b+v_i    （4）

${\stackrel{\·}{\mathrm{\ρ}}}_i=\frac{(x-x_{\mathrm{si}})(v_x-v_{\mathrm{sxi}})}{R_i}+\frac{(y-y_{\mathrm{si}})(v_y-v_{\mathrm{syi}})}{R_i}+\frac{(z-z_{\mathrm{si}})(v_z-v_{\mathrm{szi}})}{R_i}+t_d+n_i---\left(5\right)$

式中，v_sxi、v_syi、v_szi为导航参考坐标系下第i颗卫星沿x、y、z轴的速度分量；v_i表示伪距测量噪声，是方差为

的零均值高斯白噪声；n_i表示伪距率测量噪声，是方差为

的零均值高斯白噪声。

步骤三：利用t_k+1时刻的伪距、伪距率观测量,基于预测滤波获得[t_k,t_k+1]区间的未知机动加速度估计。

将动态导航定位的系统模型表示为如下的非线性模型：

$\left\{\begin{array}{c}\stackrel{\·}{x}\left(t\right)=f(x\left(t\right),t)+g_d(x\left(t\right),t)d\left(t\right)+w\left(t\right)\\ y\left(t\right)=h(x\left(t\right),t)+v\left(t\right)\end{array}\right.---\left(6\right)$

其中，

为模型向量,

表示n维实空间；t为连续系统的时间变量；对于动态导航定位，f＝[v_x v_y v_z 000t_d 0]^T，n＝8；

为状态向量；

为模型误差向量,

表示p维实空间，d(t)＝a_in，p＝3；

为模型误差扰动矩阵,

表示n×p维的实空间， $g_d=\left[\begin{array}{c}{0}_{3\×3}\\ \begin{array}{ccc}--& --& --\end{array}\\ I_{3\×3}\\ \begin{array}{ccc}--& --& --\end{array}\\ 0_{2\×3}\end{array}\right],$ I_3×3表示3×3的单位阵；

为测量向量，

表示m维的实空间；

为测量输出向量,

m＝2×num；系统噪声w(t)是均值为零，方差为Q的高斯白噪声；测量噪声v(t)是均值为零、方差为R的高斯白噪声。

以下标k来表示t_k时刻，根据预测滤波方法可得[t_k,t_k+1]区间的模型误差为：

${\hat{d}}_k=-{\left\{{[\mathrm{\Λ}\left(T\right)U\left({\hat{x}}_k\right)}^T{R}^{-1}\right[\mathrm{\Λ}\left(T\right)U\left({\hat{x}}_k\right)]+w\}}^{-1}{\left[\mathrm{\Λ}\left(T\right)U\left({\hat{x}}_k\right)\right]}^T{R}^{-1}\{{\hat{y}}_k+z[{\hat{x}}_k,T]-y_{k+1}\}---\left(7\right)$

其中，

为t_k时刻的系统状态估计；

为由

根据测量方程得到的t_k时刻的测量估计；R为观测噪声方差阵；y_k+1为t_k+1时刻的实际测量向量；T为滤波周期；为灵敏度矩阵；Λ(T)为对角矩阵；为列向量；W为加权矩阵。

加权矩阵W可预先设定数值，其选取原则是首先确定滤波器稳定的W的取值范围，然后在这个范围内平衡滤波估计的噪声影响和估计偏差的大小，来选择W矩阵。本发明中考虑这些因素后设定 $W=\left[\begin{array}{ccc}10& 0& 0\\ 0& 10& 0\\ 0& 0& 10\end{array}\right].$

灵敏度矩阵 $U=\left[\begin{array}{c}\frac{\begin{array}{c}{U}_{{\mathrm{\ρ}}_1}\left({\hat{x}}_k\right)\\ \·\\ \·\\ \·\\ U_{{\mathrm{\ρ}}_n}\left({\hat{x}}_k\right)\end{array}}{\begin{array}{c}{U}_{{\stackrel{\·}{\mathrm{\ρ}}}_1}\left({\hat{x}}_k\right)\\ \·\\ \·\\ \·\\ U_{{\stackrel{\·}{\mathrm{\ρ}}}_n}\left({\hat{x}}_k\right)\end{array}}\end{array}\right],$ 其中分别与第i颗卫星的伪距、伪距率测量对应的行向量为:

$U_{{\mathrm{\ρ}}_i}\left({\hat{x}}_k\right)=\left[\begin{array}{ccc}\frac{x-x_{\mathrm{si}}}{R_i}& \frac{y-y_{\mathrm{si}}}{R_i}& \frac{z-z_{\mathrm{si}}}{R_i}\end{array}\right],$ $U_{{\stackrel{\·}{\mathrm{\ρ}}}_i}\left({\hat{x}}_k\right)=\left[\begin{array}{ccc}\frac{x-x_{\mathrm{si}}}{R_i}& \frac{y-y_{\mathrm{si}}}{R_i}& \frac{z-z_{\mathrm{si}}}{R_i}\end{array}\right],(i=1...\mathrm{num}).$

对角阵Λ(T)为：

$\mathrm{\Λ}\left(T\right)=\left[\begin{array}{cc}{\mathrm{\Λ}}_{\mathrm{\ρ}}& \\ & {\mathrm{\Λ}}_{\stackrel{\·}{\mathrm{\ρ}}}\end{array}\right]---\left(8\right)$

其中，

Λ_ρ和

只是符号标识；T为滤波周期；I_num×num为num×num的单位阵。

列向量 $z=[{\hat{x}}_k,T]\left[\begin{array}{c}\frac{\begin{array}{c}{z}_{{\mathrm{\ρ}}_1}[{\hat{x}}_k,T]\\ \·\\ \·\\ \·\\ z_{{\mathrm{\ρ}}_n}[{\hat{x}}_k,T]\end{array}}{\begin{array}{c}{z}_{{\stackrel{\·}{\mathrm{\ρ}}}_1}[{\hat{x}}_k,T]\\ \·\\ \·\\ \·\\ z_{{\stackrel{\·}{\mathrm{\ρ}}}_n}[{\hat{x}}_k,T]\end{array}}\end{array}\right],$ 其中分别与第i颗卫星的伪距、伪距率测量对应的元素为：

$z_{{\mathrm{\ρ}}_i}[{\hat{x}}_k,T]={\mathrm{TL}}_f^1{\mathrm{\ρ}}_i+\frac{T^2}{2}{L}_f^2{\mathrm{\ρ}}_i---\left(9a\right)$

$z_{{\stackrel{\·}{\mathrm{\ρ}}}_i}[{\hat{x}}_k,T]={\mathrm{TL}}_f^1{\stackrel{\·}{\mathrm{\ρ}}}_i---\left(9b\right)$

式中，和

分别为ρ_i相对于f的1阶和2阶李导数;

为

相对于f的1阶李导数。它们分别是：

$L_f^1{\mathrm{\ρ}}_i=\frac{x-x_{\mathrm{si}}}{R_i}{v}_x+\frac{y-y_{\mathrm{si}}}{R_i}{v}_y+\frac{z-z_{\mathrm{si}}}{R_i}{v}_z+t_d$

$L_f^2{\mathrm{\ρ}}_i=[\frac{v_x({R_i}^2-{(x-x_{\mathrm{si}})}^2)}{{R_i}^3}-\frac{(y-y_{\mathrm{si}})v_y(x-x_{\mathrm{si}})}{{R_i}^3}-\frac{(z-z_{\mathrm{si}})v_z(x-x_{\mathrm{si}})}{{R_i}^3}]v_x$

$+[\frac{v_y({R_i}^2-{(y-y_{\mathrm{si}})}^2)}{{R_i}^3}-\frac{(x-x_{\mathrm{si}})v_x(y-y_{\mathrm{si}})}{{R_i}^3}-\frac{(z-z_{\mathrm{si}})v_z(y-y_{\mathrm{si}})}{{R_i}^3}]v_y$

$+[\frac{v_z({R_i}^2-{(z-z_{\mathrm{si}})}^2)}{{R_i}^3}-\frac{(x-x_{\mathrm{si}})v_x(z-z_{\mathrm{si}})}{{R_i}^3}-\frac{(y-y_{\mathrm{si}})v_y(z-\left(z_{\mathrm{si}}\right))}{{R_i}^3}]v_z$

$L_f^1{\stackrel{\·}{\mathrm{\ρ}}}_1=[\frac{(v_x-v_{\mathrm{sxi}})({R_i}^2-{(x-x_{\mathrm{si}})}^2)}{{R_i}^3}-\frac{(y-y_{\mathrm{si}})(v_y-v_{\mathrm{syi}})(x-x_{\mathrm{si}})}{{R_i}^3}\frac{(z-z_{\mathrm{si}})(v_z-v_{\mathrm{szi}})(x-x_{\mathrm{si}})}{{R_i}^3}]v_x$

$+[\frac{(v_y-v_{\mathrm{syi}})({R_i}^2-{(y-y_{\mathrm{si}})}^2)}{{R_i}^3}-\frac{(x-x_{\mathrm{si}})(v_x-v_{\mathrm{sxi}})(y-y_{\mathrm{si}})}{{R_i}^3}-\frac{(z-z_{\mathrm{si}})(v_z-v_{\mathrm{szi}})(y-y_{\mathrm{si}})}{{R_i}^3}]v_y$

$+[\frac{(v_z-v_{\mathrm{szi}})({R_i}^2-{(z-z_{\mathrm{si}})}^2)}{{R_i}^3}-\frac{(x-x_{\mathrm{si}})(v_x-v_{\mathrm{sxi}})(z-z_{\mathrm{si}})}{{R_i}^3}-\frac{(y-y_{\mathrm{si}})(v_y-v_{\mathrm{sxi}})(z-z_{\mathrm{si}})}{{R_i}^3}]v_z$

最终根据公式（6）可以得到[t_k,t_k+1]区间的机动加速度估计。

步骤四：利用[t_k,t_k+1]区间的机动加速度估计修正系统模型，并进行时间更新，预测载体导航定位参数。

将得到的机动加速度作为系统输入代入系统模型即公式（1）中，并对状态进行时间更新，更新过程如公式（10）、（11）所示，状态及其误差协方差阵的一步预测为：

${\hat{x}}_{k+1/k}={\hat{x}}_k+\mathrm{Tf}({\hat{x}}_k,t_k)+\frac{T^2}{2}\frac{\∂f(x,t_k)}{\∂x}{|}_{x=x_k}f({\hat{x}}_k,t_k)+{\mathrm{tg}}_d{\hat{d}}_k---\left(10\right)$

P_k+1/k＝(A_k-B_kM_kH_k)P_k(A_k-B_kM_kH_k)^T+Q′_k    （11）

其中

为t_k+1时刻的状态一步预测；P_k+1/k为t_k+1时刻的状态误差协方差阵一步预测;

为t_k时刻模型向量的估计值；f(x,t_k)为t_k时刻的模型向量；

为[t_k,t_k+1]区间的模型误差估计；g_d为误差扰动矩阵；A_k、B_k和M_k为实现表达而出现的变量，它们是 $M_k={\left\{{\left[\mathrm{\Λ}\left(T\right)U\left({\hat{x}}_k\right)\right]}^T{R}^{-1}\right[\mathrm{\Λ}\left(T\right)U\left({\hat{x}}_k\right)]+W\}}^{-1}{\left[\mathrm{\Λ}\left(T\right)U\left({\hat{x}}_k\right)\right]}^T{R}^{-1},$ $A_k=I+T[\frac{\∂f(x,t_k)}{\∂x}{|}_{x={\hat{x}}_k}+\frac{{\∂g}_d\·d}{\∂x}{|}_{x={\hat{x}}_k,d={\hat{d}}_k}],$

I为单位阵，d为模型误差；H_k为基于状态

得到的测量模型的雅可比矩阵，

Q′_k为对称正定矩阵；P_k为t_k时刻状态误差方差阵。

式（11）中的Q′_k是一个特别引入的对称正定矩阵，可以看作经过估计模型误差补偿后的等效系统噪声的方差阵，由于一部分系统噪声的影响在模型误差估计中被等效估计并补偿，因此Q′_k的取值略小于系统噪声方差阵Q_k，Q′_k＝λQ_k，λ＝0.8~1，λ为修正系数。

步骤五：进行测量更新，利用测量残差修正预测状态，实现载体导航定位参数的估计。

该过程如式（12）、（13）、（14）所示，t_k+1时刻的增益矩阵K_k+1为：

$K_{k+1}=P_{k+1/k}{H}_{k+1/k}^T{(H_{k+1/k}{P}_{k+1/k}{H}_{k+1/k}^T+R_{k+1})}^{-1}---\left(12\right)$

H_k+1/k为基于预测状态

得到的测量模型的雅可比矩阵，

R_k+1为t_k+1时刻观测的噪声方差阵；基于测量残差

对t_k+1时刻的预测状态

进行修正得到t_k+1时刻的状态估计

及其误差协方差阵P_k+1为：

${\hat{x}}_{k+1}={\hat{x}}_{k+1/k}+K_{k+1}[y_{k+1}-h\left({\hat{x}}_{k+1/k}\right)]---\left(13\right)$

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

式中：为t_k+1时刻的状态估计，P_k+1为t_k+1时刻状态误差方差阵，I为单位阵，y_k+1为t_k+1时刻的伪距、伪距率的观测量，

为利用预测状态得到的预测观测量，其中

为t_k+1时刻的状态一步预测。

本发明提出的一种卫星导航接收机的动态定位解算方法，具有对载体机动加速度实时估计并利用估计的模型误差修正状态估计的能力，因此可以得到更高精度的导航参数估计。同时，本方法还可以适用于其他类似带有模型误差的系统。

在存在模型误差时，本发明提出的一种卫星导航接收机的动态定位解算方法为CV模型下基于IPF的定位方法IPF（CV），与CV模型下基于EKF的定位方法EKF（CV）、Singer模型下基于EKF的定位方法EKF（Singer）在系统无初始误差、有初始误差条件下的仿真结果误差曲线对比分别如图2和图3所示。图4和图5分别为仿真时载体轨迹的三维图和系统加速度输入（a_in）的三轴变化曲线图。

仿真参数设置如下：

（1）滤波周期T=1s。

（2） ${{\mathrm{\σ}}_v}_i=5m,{{\mathrm{\σ}}_n}_i=0.5/s.$

（3）σ_v=10m,σ_a=6m/s,σ_tb=0.8m,σ_td=0.05m/s。

（4）仿真次数e_n＝200。

当存在模型误差而无初始状态误差时，在同样的仿真条件下，本发明提出的CV模型下基于IPF的定位方法IPF（CV）、CV模型下基于EKF的定位方法EKF（CV），Singer模型下基于EKF的定位方法EKF（Singer）的位置、速度、加速度的误差曲线分别如图2a、图2b、图2c所示。当同时存在模型误差和初始误差时，在同样地仿真条件下，本发明提出的CV模型下基于IPF的定位方法IPF（CV）、CV模型下基于EKF的定位方法EKF（CV）Singer模型下基于EKF的定位方法EKF（Singer）的位置、速度、加速度的误差曲线分别如图3a、图3b、图3c所示。

图2a和图3a中横坐标均是仿真时间，纵坐标是位置估计误差曲线（真实值与估计值之差），从图中可以看出，EKF（CV）和Singer模型下基于EKF的定位方法EKF（Singer）的位置估计误差发散，而本发明提出的CV模型下基于IPF的定位方法IPF（CV）的位置估计误差在10m以内。

图2b和图3b中横坐标均是仿真时间，纵坐标是速度的估计误差曲线；图2c和图3c的横坐标均是仿真时间，纵坐标是加速度的估计误差曲线；从图中可以看出，CV模型下基于EKF的定位方法EKF（CV）和Singer模型下基于EKF的定位方法EKF（Singer）的速度估计误差震荡比较剧烈，而本发明CV模型下基于IPF的定位方法IPF（CV）估计误差因为能够实时估计模型误差并补偿，所以速度估计误差仍然在1m/s以内，且加速度估计误差在0.7m/s²以内；因此本发明提出CV模型下基于IPF的定位方法IPF（CV）精度有明显提高。三种方法的估计效果如表1、表2所示：

表1：无初始状态误差的滤波精度

表2：有初始状态误差的滤波精度

从表1和表2中可以看出，无论是否有初始误差，本发明提出的CV模型下基于IPF的定位方法IPF(CV)的定位精度明显优于CV模型下基于EKF的定位方法EKF(CV)和Singer模型下基于EKF的定位方法EKF(Singer)。

Claims (4)

1.一种卫星导航接收机的动态定位解算方法，其特征在于：包括以下几个步骤：

步骤一：建立以机动加速度作为输入的载体运动方程，用该载体运动方程构成系统状态方程：

采用以机动加速度作为输入的载体运动模型，同时利用导航接收机的时钟误差来建立动态定位解算的状态方程，如下：

其中，P＝[x y z]^T和V＝[v_x v_y v_z]^T分别为接收机在导航参考坐标系下位置向量和速度向量；x、y、z和v_x、v_y、v_z分别表示接收机在导航参考坐标系中的沿x、y、z轴的位置分量和速度分量，a_in＝[a_x a_y a_z]^T为由载体机动引起的未建模加速度向量，其中a_x、a_y、a_z表示在导航参考坐标系的沿x、y、z轴的加速度分量；t_b为接收机的等效时钟误差；t_d为接收机的等效时钟频率误差；系统噪声向量由速度噪声w_v、加速度噪声w_a、时钟误差噪声w_tb和时钟频率误差w_td构成，均为零均值的高斯白噪声，方差分别为σ_v、σ_a、σ_tb、σ_td；

步骤二：建立卫星导航接收机伪距和伪距率的观测方程：

利用接收机测量得到的伪距和伪距率构建测量向量y如下:

其中，ρ_i、 

分别为第i颗卫星的伪距、伪距率测量值，i＝1,…num；num为用于动态导航解算的可见卫星颗数；

设第i颗卫星与接收机的几何距离R_i为：

其中，x_si、y_si、z_si为在导航参考坐标系下第i颗卫星沿x、y、z轴的位置分量；

利用接收机测量得到的伪距和伪距率作为观测量，建立由第i颗卫星数据得到的伪距ρ_i和伪距率 

观测方程分别如下：

ρ_i＝R_i+t_b+v_i      （4）

其中v_sxi、v_syi、v_szi为导航参考坐标系下第i颗卫星沿x、y、z轴的速度分量；v_i表示伪距测量 噪声，是方差为 

的零均值高斯白噪声；n_i表示伪距率测量噪声，是方差为 的零均值高斯白噪声；

步骤三：利用t_k+1时刻的伪距、伪距率观测量,基于预测滤波获得[t_k,t_k+1]区间的未知机动加速度：

将动态导航定位的系统模型表示为如下的非线性模型：

其中， 

为模型向量, 

表示n维实空间；t为连续系统的时间变量；对于动态导航定位，f＝[v_x v_y v_z 000t_d 0]^T，n＝8； 为状态向量； 

为模型误差向量, 

表示p维实空间，d(t)＝a_in，p＝3； 

为模型误差扰动矩阵, 

表示n×p维的实空间， 

I_3×3表示3×3的单位阵； 

为测量向量， 

表示m维的实空间； 

为测量输出向量, 

系统噪声w(t)是均值为零，方差为Q的高斯白噪声；测量噪声v(t)是均值为零、方差为R的高斯白噪声；

以下标k表示t_k时刻，根据预测滤波方法得到[t_k,t_k+1]区间的模型误差 

为：

其中， 

为t_k时刻的系统状态估计； 为由 根据测量方程得到的t_k时刻的测量估计；R为观测噪声方差阵；y_k+1为t_k+1时刻的实际测量向量；T为滤波周期； 

为灵敏度矩阵；Λ(T)为对角矩阵； 

为列向量；W为加权矩阵；

灵敏度矩阵

其中分别与第i颗卫星的伪距、伪距率测量对应的行向量为:

对角阵Λ(T)为：

其中， 

Λ_ρ和 

为符号标识，T为滤波周期，I_num×num为num×num的单位阵；

列向量其中分别与第i颗卫星的伪距、伪距率测量对应的元素为：

其中 

和 

分别为ρ_i相对于f的1阶和2阶李导数; 

为 相对于f的1阶李导数，分别是：

最终根据公式（6）得到[t_k,t_k+1]区间的机动加速度估计；

步骤四：利用[t_k,t_k+1]区间的机动加速度估计修正系统模型，并进行时间更新，预测载体导航定位参数；

将得到的机动加速度作为系统输入代入系统模型公式（1）中，并对状态进行时间更新，更新过程如公式（10）、（11）所示，状态及其误差协方差阵的一步预测为：

P_k+1/k＝(A_k-B_kM_kH_k)P_k(A_k-B_kM_kH_k)^T+Q′_k    （11） 

其中 

为t_k+1时刻的状态一步预测；P_k+1/k为t_k+1时刻的状态误差协方差阵一步预测; 

为t_k时刻模型向量的估计值；f(x,t_k)为t_k时刻的模型向量； 

为[t_k,t_k+1]区间的模型误差估计；g_d为误差扰动矩阵；A_k、B_k和M_k为实现表达而出现的变量，它们是 

I为单位阵，d为模型误差；H_k为基于状态 

得到的测量模型的雅可比矩阵， 

Q′_k为对称正定矩阵；P_k为t_k时刻状态误差方差阵；

步骤五：进行测量更新，利用测量残差修正预测状态，实现载体导航定位参数的估计；

如式（12）、（13）、（14）所示，t_k+1时刻的增益矩阵K_k+1为：

H_k+1/k为基于预测状态 得到的测量模型的雅可比矩阵， 

R_k+1为t_k+1时刻观测的噪声方差阵；基于测量残差 

对t_k+1时刻的预测状态 

进行修正得到t_k+1时刻的状态估计 

及其误差协方差阵P_k+1为：

其中，I为单位阵，y_k+1为t_k+1时刻的伪距、伪距率的观测量， 

为利用预测状态 

得到的预测测量向量。

2.根据权利要求1所述的一种卫星导航接收机的动态定位解算方法，其特征在于：当采用几何精度因子最小的4颗卫星的测量用于动态定位时，可见卫星颗数num＝4。

3.根据权利要求1所述的一种卫星导航接收机的动态定位解算方法，其特征在于：所述的加权矩阵W为

4.根据权利要求1所述的一种卫星导航接收机的动态定位解算方法，其特征在于：Q_k′=λQ_k，对称正定矩阵Q_k为系统噪声方差阵，λ=0.8~1 ，λ为修正系数。

Images