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

摘要

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

主权项

1.一种卫星导航接收机的动态定位解算方法，其特征在于：包括以下几个步骤：步骤一：建立以机动加速度作为输入的载体运动方程，用该载体运动方程构成系统状态方程：采用以机动加速度作为输入的载体运动模型，同时利用导航接收机的时钟误差来建立动态定位解算的状态方程，如下：$\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{·}{\rho }}_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\times 3}\\ \begin{array}{ccc}--& --& --\end{array}\\ I_{3\times 3}\\ \begin{array}{ccc}--& --& --\end{array}\\ 0_{2\times 3}\end{array}\right],$I_3×3表示3×3的单位阵；为测量向量，表示m维的实空间；为测量输出向量,系统噪声w(t)是均值为零，方差为Q的高斯白噪声；测量噪声v(t)是均值为零、方差为R的高斯白噪声；以下标k表示t_k时刻，根据预测滤波方法得到[t_k,t_k+1]区间的模型误差为：${\hat{d}}_k=-{\left\{{[\Lambda \left(T\right)U\left({\hat{x}}_k\right)}^T{R}^{-1}\right[\Lambda \left(T\right)U\left({\hat{x}}_k\right)]+w\}}^{-1}{\left[\Lambda \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}_{{\rho }_1}\left({\hat{x}}_k\right)\\ ·\\ ·\\ ·\\ U_{{\rho }_n}\left({\hat{x}}_k\right)\end{array}}{\begin{array}{c}{U}_{{\stackrel{·}{\rho }}_1}\left({\hat{x}}_k\right)\\ ·\\ ·\\ ·\\ U_{{\stackrel{·}{\rho }}_n}\left({\hat{x}}_k\right)\end{array}}\end{array}\right],$其中分别与第i颗卫星的伪距、伪距率测量对应的行向量为:$U_{{\rho }_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{·}{\rho }}_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)为：$\Lambda \left(T\right)=\left[\begin{array}{cc}{\Lambda }_{\rho }& \\ & {\Lambda }_{\stackrel{·}{\rho }}\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}_{{\rho }_1}[{\hat{x}}_k,T]\\ ·\\ ·\\ ·\\ z_{{\rho }_n}[{\hat{x}}_k,T]\end{array}}{\begin{array}{c}{z}_{{\stackrel{·}{\rho }}_1}[{\hat{x}}_k,T]\\ ·\\ ·\\ ·\\ z_{{\stackrel{·}{\rho }}_n}[{\hat{x}}_k,T]\end{array}}\end{array}\right],$其中分别与第i颗卫星的伪距、伪距率测量对应的元素为：$z_{{\rho }_i}[{\hat{x}}_k,T]={\mathrm{TL}}_f^1{\rho }_i+\frac{T^2}{2}{L}_f^2{\rho }_i---\left(9a\right)$$z_{{\stackrel{·}{\rho }}_i}[{\hat{x}}_k,T]={\mathrm{TL}}_f^1{\stackrel{·}{\rho }}_i---\left(9b\right)$其中和分别为ρ_i相对于f的1阶和2阶李导数;为相对于f的1阶李导数，分别是：$L_f^1{\rho }_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{\rho }_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{·}{\rho }}_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{\partial f(x,t_k)}{\partial 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[\Lambda \left(T\right)U\left({\hat{x}}_k\right)\right]}^T{R}^{-1}\right[\Lambda \left(T\right)U\left({\hat{x}}_k\right)]+W\}}^{-1}{\left[\Lambda \left(T\right)U\left({\hat{x}}_k\right)\right]}^T{R}^{-1},$$A_k=I+T[\frac{\partial f(x,t_k)}{\partial x}{|}_{x={\hat{x}}_k}+\frac{{\partial g}_d·d}{\partial 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时刻的伪距、伪距率的观测量，为利用预测状态得到的预测测量向量。