[发明专利]一种卫星导航接收机的动态定位解算方法

权利要求书

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

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

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

P·=V+wvV·=ain+wat·d=td+wtbt·d=wtd---(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为：

Ri=(x-xsi)2+(y-ysi)2+(z-zsi)2---(3)]]>

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

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

ρ_i＝R_i+t_b+v_i      （4）

ρ·i=(x-xsi)(vx-vsxi)Ri+(y-ysi)(vy-vsyi)Ri+(z-zsi)(vz-vszi)Ri+td+ni---(5)]]>

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

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

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

x·(t)=f(x(t),t)+gd(x(t),t)d(t)+w(t)y(t)=h(x(t),t)+v(t)---(6)]]>

其中，为模型向量,表示n维实空间；t为连续系统的时间变量；对于动态导航定位，f＝[v_x v_y v_z 000t_d 0]^T，n＝8；为状态向量；为模型误差向量,表示p维实空间，d(t)＝a_in，p＝3；为模型误差扰动矩阵,表示n×p维的实空间，gd=03×3------I3×3------02×3,]]>I_3×3表示3×3的单位阵；为测量向量，表示m维的实空间；为测量输出向量,系统噪声w(t)是均值为零，方差为Q的高斯白噪声；测量噪声v(t)是均值为零、方差为R的高斯白噪声；

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

d^k=-{[Λ(T)U(x^k)TR-1[Λ(T)U(x^k)]+w}-1[Λ(T)U(x^k)]TR-1{y^k+z[x^k,T]-yk+1}---(7)]]>

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

灵敏度矩阵U(x^k)=Uρ1(x^k)···Uρn(x^k)Uρ·1(x^k)···Uρ·n(x^k),]]>其中分别与第i颗卫星的伪距、伪距率测量对应的行向量为:

Uρi(x^k)=x-xsiRiy-ysiRiz-zsiRi,]]>Uρ·i(x^k)=x-xsiRiy-ysiRiz-zsiRi,(i=1...num);]]>

对角阵Λ(T)为：

Λ(T)=ΛρΛρ·---(8)]]>

其中，Λ_ρ和为符号标识，T为滤波周期，I_num×num为num×num的单位阵；

列向量z[x^k,T]=zρ1[x^k,T]···zρn[x^k,T]zρ·1[x^k,T]···zρ·n[x^k,T],]]>其中分别与第i颗卫星的伪距、伪距率测量对应的元素为：

zρi[x^k,T]=TLf1ρi+T22Lf2ρi---(9a)]]>

zρ·i[x^k,T]=TLf1ρ·i---(9b)]]>

其中和分别为ρ_i相对于f的1阶和2阶李导数;为相对于f的1阶李导数，分别是：

Lf1ρi=x-xsiRivx+y-ysiRivy+z-zsiRivz+td]]>

Lf2ρi=[vx(Ri2-(x-xsi)2)Ri3-(y-ysi)vy(x-xsi)Ri3-(z-zsi)vz(x-xsi)Ri3]vx]]>

+[vy(Ri2-(y-ysi)2)Ri3-(x-xsi)vx(y-ysi)Ri3-(z-zsi)vz(y-ysi)Ri3]vy]]>

+[vz(Ri2-(z-zsi)2)Ri3-(x-xsi)vx(z-zsi)Ri3-(y-ysi)vy(z-(zsi))Ri3]vz]]>

Lf1ρ·1=[(vx-vsxi)(Ri2-(x-xsi)2)Ri3-(y-ysi)(vy-vsyi)(x-xsi)Ri3(z-zsi)(vz-vszi)(x-xsi)Ri3]vx]]>

+[(vy-vsyi)(Ri2-(y-ysi)2)Ri3-(x-xsi)(vx-vsxi)(y-ysi)Ri3-(z-zsi)(vz-vszi)(y-ysi)Ri3]vy]]>

+[(vz-vszi)(Ri2-(z-zsi)2)Ri3-(x-xsi)(vx-vsxi)(z-zsi)Ri3-(y-ysi)(vy-vsxi)(z-zsi)Ri3]vz]]>

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

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

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

x^k+1/k=x^k+Tf(x^k,tk)+T22∂f(x,tk)∂x|x=xkf(x^k,tk)+tgdd^k---(10)]]>

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为实现表达而出现的变量，它们是Mk={[Λ(T)U(x^k)]TR-1[Λ(T)U(x^k)]+W}-1[Λ(T)U(x^k)]TR-1,]]>Ak=I+T[∂f(x,tk)∂x|x=x^k+∂gd·d∂x|x=x^k,d=d^k],]]>I为单位阵，d为模型误差；H_k为基于状态得到的测量模型的雅可比矩阵，Q′_k为对称正定矩阵；P_k为t_k时刻状态误差方差阵；

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

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

Kk+1=Pk+1/kHk+1/kT(Hk+1/kPk+1/kHk+1/kT+Rk+1)-1---(12)]]>

H_k+1/k为基于预测状态得到的测量模型的雅可比矩阵，R_k+1为t_k+1时刻观测的噪声方差阵；基于测量残差对t_k+1时刻的预测状态进行修正得到t_k+1时刻的状态估计及其误差协方差阵P_k+1为：

x^k+1=x^k+1/k+Kk+1[yk+1-h(x^k+1/k)]---(13)]]>

Pk+1=(I-Kk+1Hk+1/k)Pk+1/k(I-Kk+1Hk+1/k)T+Kk+1Rk+1Kk+1T---(14)]]>

其中，I为单位阵，y_k+1为t_k+1时刻的伪距、伪距率的观测量，为利用预测状态得到的预测测量向量。