CN102033236B - 一种卫星导航位置速度联合估计方法 - Google Patents

Abstract

本发明公开了一种卫星导航位置速度联合估计方法。它利用多普勒测量值与接收机位置的相关性，通过对接收机复现的多普勒测量值进行建模，并与伪距测量模型联立求解，解决了无法获取准确载波相位测量值时的接收机位置速度实时估计的问题。该方法在每一新历元时刻根据当前的伪距和多普勒测量值进行定位测速联合估计，以非线性方程组加权最小二乘求解迭代过程中的估计位置代替伪距定位结果作为接收机速度估计的参考位置，使速度估计值随位置估计逐次收敛。同时，估计误差中包含了速度误差，使得算法收敛判决更为严格，算法在实际位置周围的搜索步进减小，以获得更高的估计精度。本发明能够可靠的实现实时定位测速，适用于GPS、GLONASS等多种系统卫星导航系统。

Description

一种卫星导航位置速度联合估计方法

技术领域

本发明涉及一种卫星导航位置速度联合估计方法。

背景技术

随着卫星导航技术在航天、航空、遥感、通信、测绘等领域的广泛应用，全球卫星导航系统迅速的深入到人么你生活的各个领域，极大的提高了声场效率。GNSS系统是具有全球性、全时性、全天候性的导航、定位、授时、测速系统。近年来，随着个人应用的普及，市场对GNSS定位、测速性能和稳定性的要求迅速上升。

卫星导航定位的方法主要包括载波相位定位和伪距定位两种方式。载波相位定位利用对卫星信号实时相位的准确最终，能够获得厘米级的距离测量精度，从而得到亚米级乃至毫米级的定位精度；伪距定位采用速率相对较慢的伪随机码相位测量值作为卫星与接收机间距离的测量工具，其测量值的可用性高于载波相位测量，但测量精度较低，定位精度通常在米级至10米级。

在城市环境下，直射信号收到建筑物和树木等遮挡，GNSS接收机收到的直射信号强度很低且包含大量多径信号，锁相环的输入信噪比极低，无法完成对信号相位的跟踪。在接收机载体具有很大瞬时动态的条件下，由于锁频环对多普勒的跟踪存在延时，瞬时载波相位旋转可能大于2π，此时锁相环路会发生短暂失锁，载波相位整周数变化测量发生跳变。在上述条件下，接收机无法利用载波相位完成高精度定位。

利用卫星导航系统测速方法主要包括位置差分和利用多普勒计算两种方式。位置差分法将前后两个连续历元间的位置估计结果求差后相对历元间隔平均，该方法极易受到位置估计结果中的噪声影响，无法提供具有可靠精度的速度测量值；利用多普勒计算接收机速度主要基于多普勒效应的表达式

$f_R=f_T(1-\frac{v_r\·a}{c})---\left(1\right)$

其中，f_T是卫星发射信号的频率；

v_r是卫星与接收机的相对速度矢量；

a是从接收机指向卫星方向的单位矢量，其取值受卫星位置和接收机位置影响；

c为光速。

该方法同样受限于前一阶段位置估计的精度。当位置估计偏差小于20m时，速度估计偏差约在1m/s至2m/s，无法适用于对测速精度要求较高的应用环境。

卫星导航位置速度联合估计方法的最大优势在于利用了多普勒测量值测量误差较小的特性，利用多普勒值与接收机位置的相关性，直接在测量值建模的过程中引入多普勒测量值，增强算法迭代收敛时的判决条件，从而提高了算法的估计精度。同时，由于锁频环的跟踪能力强于锁相环，该方法在上述恶劣条件下提供准确位置速度估计的可靠性要强于载波相位定位方式。

发明内容

本发明的目的是克服现有技术的不足，提供一种卫星导航位置速度联合估计方法。

卫星导航位置速度联合估计方法的步骤如下：

1)位置速度联合估计单元在历元到达时刻从接收机获取当前伪随机码相位、多普勒和载噪比测量值；

2)初始化阶段，对当前伪距测量值与接收机位置关系，以及当前多普勒测量值与接收机速度关系分别建模，利用加权最小二乘方法分别计算接收机位置和速度作为当前历元到达时刻的位置速度估计值；

3)初始化阶段完成后，对当前伪距测量值与接收机位置速度关系，以及当前多普勒测量值与接收机位置速度关系进行建模，联立伪距测量描述方程组和多普勒测量描述方程组构成测量方程组；以上一历元到达时刻得到的接收机位置速度估计值作为当前历元到达时刻测量方程组求解的初始值，利用加权最小二乘方法对上述测量方程组求解；

4)加权最小二乘权重系数中，不同通道信号的伪距根据当前载噪比估计值进行加权，不同通道信号的多普勒同样根据当前载噪比估计值进行加权，伪距和多普勒测量方程组根据接收机跟踪通道输出的伪随机码相位误差和多普勒误差的方差比值进行加权。

5)若当前历元到达时刻的位置速度估计失败，位置速度估计值失效，位置速度联合估计单元进入初始化阶段，重新对估计流程进行初始化。

所述的在历元到达时刻从接收机获取当前伪随机码相位和多普勒测量值步骤为：

1)卫星导航接收机捕获GNSS卫星信号，并实时跟踪信号，测量信号的伪随机码相位、多普勒和载噪比状态；

2)接收机根据其定时设置，在历元到达时刻对当前伪随机码相位、多普勒和载噪比测量值进行采样，并发送给位置速度联合估计单元；

3)位置速度联合估计单元获取接收机发来的信息并以信息到达时刻作为历元到达时刻。

所述的初始化阶段，对当前伪距测量值与接收机位置关系，以及当前多普勒测量值与接收机速度关系分别建模，利用加权最小二乘方法分别计算接收机位置和速度作为当前历元到达时刻的位置速度估计值步骤为：

1)位置速度联合估计单元中的权重计算模块获105取当前各通道信号载噪比测量值，将其按最大值归一化后作为不同通道信号伪距权重系数和不同通道信号的多普勒权重系数。

2)位置速度联合估计单元中的伪距测量建模模块102将伪随机码相位对应的信号发射时间和从计时器获得的当前时间求差，得到伪距时间，伪距时间乘以光速c得到伪距测量值矢量ρ＝{ρ₁，ρ₂，…，ρ_n}。其中n为当前可用于位置速度估计的GNSS卫星信号数量。伪距测量建模模块102对伪距值建模如下：

假定接收机的三维位置为p_u＝(x_u，y_u，z_u)，计时器时间偏差量为t_u，伪距测量值满足方程组

${\mathrm{\ρ}}_j=\sqrt{{(x_j-x_u)}^2+{(y_j-y_u)}^2+{(z_j-z_u)}^2}+{\mathrm{ct}}_u+{\mathrm{\ω}}_{\mathrm{\ρj}},j=\mathrm{1,2},\·\·\·,n---\left(1\right)$

其中，p_j＝(x_j，y_j，z_j)为第j颗卫星的三维位置，ω_ρj为第j颗卫星对应的伪距测量值中的噪声分量。

3)位置速度联合估计单元中的加权最小二乘位置估计模块106以当前的接收机位置估计值为

估计值与中心值误差为(Δx_u，Δy_u，Δz_u，Δt_u)，将方程组(1)线性化为

${\mathrm{\ρ}}_j={\widehat{\mathrm{\ρ}}}_j-\frac{x_j-{\hat{x}}_u}{{\widehat{\mathrm{\ρ}}}_j-c{\hat{t}}_u}\mathrm{\Δ}{x}_u-\frac{y_j-{\hat{y}}_u}{{\widehat{\mathrm{\ρ}}}_j-c{\hat{t}}_u}\mathrm{\Δ}{y}_u-\frac{z_j-{\hat{z}}_u}{{\widehat{\mathrm{\ρ}}}_j-c{\hat{t}}_u}\mathrm{\Δ}{z}_u+c{\mathrm{\Δt}}_u+{\mathrm{\ω}}_{\mathrm{\ρj}}---\left(2\right)$

加权最小二乘位置估计模块106由权重计算模块105获取权重系数，利用加权最小二乘方法进行迭代求解，得到接收机位置首次估计值p_u1。

4)位置速度联合估计单元中的多普勒测量建模模块103得到多普勒测量值矢量f_d＝{f_d1，f_d2，…，f_dn}。其中n为当前可用于位置速度估计的GNSS卫星信号数量。多普勒测量建模模块103对多普勒值建模如下：

假定接收机的三维速度为v_u＝(v_xu，v_yu，v_zu)，多普勒测量值满足方程组

$f_{\mathrm{di}}=-\left(\frac{(v_j-v_u)\·(p_j-p_u)}{c}\right)f_T+{\mathrm{\ω}}_{\mathrm{fj}},j=\mathrm{1,2},\·\·\·,n---\left(3\right)$

其中，p_j＝(x_j，y_j，z_j)为第j颗卫星的三维位置，v_j＝(v_xj，v_xj，v_zj)为第j颗卫星的三维速度，ω_fj为第j颗卫星对应的多普勒测量值中的噪声分量。

5)位置速度联合估计单元中的加权最小二乘速度估计模块107由权重计算模块105获取权重系数，利用加权最小二乘方法对线性方程组(3)进行求解，得到接收机位置首次估计值v_u1。

所述的初始化阶段完成后，对当前伪距测量值与接收机位置速度关系，以及当前多普勒测量值与接收机位置速度关系进行建模，联立伪距测量描述方程组和多普勒测量描述方程组构成测量方程组步骤为：

位置速度联合估计单元中的联合估计建模模块104将伪随机码相位对应的信号发射时间和从计时器获得的当前时间求差，得到伪距时间，伪距时间乘以光速c得到伪距测量值矢量ρ＝{ρ₁，ρ₂，…，ρ_n}；得到多普勒测量值矢量f_d＝{f_d1，f_d2，…，f_dn}，其中n为当前可用于位置速度估计的GNSS卫星信号数量；联合估计建模模块对测量值建模如下：

假定接收机的三维位置为p_u＝(x_u，y_u，z_u)，接收机的三维速度为v_u＝(v_xu，v_yu，v_zu)，计时器时间偏差量为t_u，测量值满足方程组

$\left\{\begin{array}{c}{\mathrm{\ρ}}_j=\sqrt{{(x_j-x_u)}^2+{(y_j-y_u)}^2+{(z_j-z_u)}^2}+{\mathrm{ct}}_u+{\mathrm{\ω}}_{\mathrm{\ρj}}\\ f_{\mathrm{dj}}=f_T\·\frac{\sqrt{(v_{\mathrm{xj}}-v_{\mathrm{xu}})(x_j-x_u)+(v_{\mathrm{yj}}-v_{\mathrm{yu}})(y_j-y_u)+(v_{\mathrm{zj}}-v_{\mathrm{zu}})(z_j-z_u)}}{c\sqrt{{(x_j-x_u)}^2+{(y_j-y_u)}^2+{(z_j-z_u)}^2}}+{\mathrm{\ω}}_{\mathrm{fj}}\end{array}\right.j=\mathrm{1,2},\·\·\·,n---\left(4\right)$

其中，p_j＝(x_j，y_j，z_j)为第j颗卫星的三维位置，v_j＝(v_xj，v_xj，v_zj)为第j颗卫星的三维速度，ω_ρj为第j颗卫星对应的伪距测量值中的噪声分量，ω_fj为第j颗卫星对应的多普勒测量值中的噪声分量。

所述的以上一历元到达时刻得到的接收机位置速度估计值作为当前历元到达时刻测量方程组求解的初始值，利用加权最小二乘方法对上述测量方程组求解步骤为：

1)位置速度联合估计单元中的权重计算模块105获取当前各通道信号载噪比测量值，将其按最大值归一化后作为不同通道信号伪距权重系数和不同通道信号的多普勒权重系数；将接收机跟踪通道伪随机码相位误差和多普勒误差的方差比值作为伪距和多普勒测量方程组权重系数。

2)位置速度联合估计单元中的加权最小二乘位置速度联合估计模块108以当前的接收机位置速度估计值为

估计值与中心值误差为(Δx_u，Δy_u，Δz_u，Δv_xu，Δv_yu，Δv_zu，Δt_u)，将方程组(4)线性化为

$\left\{\begin{array}{c}{\mathrm{\ρ}}_j={\widehat{\mathrm{\ρ}}}_j-\frac{x_j-{\hat{x}}_u}{{\widehat{\mathrm{\ρ}}}_j-c{\hat{t}}_u}\mathrm{\Δ}{x}_u-\frac{y_j-{\hat{y}}_u}{{\widehat{\mathrm{\ρ}}}_j-c{\hat{t}}_u}\mathrm{\Δ}{y}_u-\frac{z_j-{\hat{z}}_u}{{\widehat{\mathrm{\ρ}}}_j-c{\hat{t}}_u}\mathrm{\Δ}{z}_u+c{\mathrm{\Δt}}_u+{\mathrm{\ω}}_{\mathrm{\ρj}}\\ f_j={\hat{f}}_j-\frac{f_T}{c}\left\{\frac{1}{{\widehat{\mathrm{\ρ}}}_j}\right[(v_{\mathrm{xj}}-{\hat{v}}_{\mathrm{xu}}+\frac{x_j-{\hat{x}}_u}{{\widehat{\mathrm{\ρ}}}_j^2})\mathrm{\Δ}{x}_u+(v_{\mathrm{yj}}-{\hat{v}}_{\mathrm{yu}}+\frac{y_j-{\hat{y}}_u}{{\widehat{\mathrm{\ρ}}}_j^2})\\ -\frac{1}{{\widehat{\mathrm{\ρ}}}_j}[(x_j-{\hat{x}}_u){\mathrm{\Δv}}_{\mathrm{xu}}+(y_j-{\hat{y}}_u)\mathrm{\Δ}{v}_{\mathrm{yu}}+(z_j-{\hat{z}}_u)\mathrm{\Δ}{v}_{\mathrm{zu}}]\}{\mathrm{\ω}}_{\mathrm{fj}}\end{array}\mathrm{\Δ}{y}_u+(v_{\mathrm{zj}}-{\hat{v}}_{\mathrm{zu}}+\frac{z_j-{{\hat{z}}_u}_{}}{{\widehat{\mathrm{\ρ}}}_j^2}){\mathrm{\Δz}}_u]\right.---\left(5\right)$

假设当前时刻为历元(m+1)到达时刻，其中m为自然数。加权最小二乘位置速度联合估计模块108由权重计算模块105获取权重系数，利用加权最小二乘方法进行迭代求解，得到当前历元(m+1)到达时刻的接收机位置估计值p_u(m+1)。

本发明在每一新历元时刻根据当前的伪距和多普勒测量值进行定位测速联合估计，以非线性方程组加权最小二乘求解迭代过程中的估计位置代替伪距定位结果作为接收机速度估计的参考位置，使速度估计值随位置估计逐次收敛。同时，估计误差中包含了速度误差，使得算法收敛判决更为严格，算法在实际位置周围的搜索步进减小，以获得更高的估计精度。本发明能够可靠的实现实时定位测速，适用于GPS、GLONASS等多种系统卫星导航系统。

附图说明

图1是本发明的模块关系框图；

图2是本发明实施例的结构框图；

图3是本发明实施例的位置速度联合估计单元工作流程逻辑框图。

具体实施方式

卫星导航位置速度联合估计方法的步骤如下：

1)位置速度联合估计单元在历元到达时刻从接收机获取当前伪随机码相位、多普勒和载噪比测量值；

2)初始化阶段，对当前伪距测量值与接收机位置关系，以及当前多普勒测量值与接收机速度关系分别建模，利用加权最小二乘方法分别计算接收机位置和速度作为当前历元到达时刻的位置速度估计值；

3)初始化阶段完成后，对当前伪距测量值与接收机位置速度关系，以及当前多普勒测量值与接收机位置速度关系进行建模，联立伪距测量描述方程组和多普勒测量描述方程组构成测量方程组；以上一历元到达时刻得到的接收机位置速度估计值作为当前历元到达时刻测量方程组求解的初始值，利用加权最小二乘方法对上述测量方程组求解；

4)加权最小二乘权重系数中，不同通道信号的伪距根据当前载噪比估计值进行加权，不同通道信号的多普勒同样根据当前载噪比估计值进行加权，伪距和多普勒测量方程组根据接收机跟踪通道输出的伪随机码相位误差和多普勒误差的方差比值进行加权。

5)若当前历元到达时刻的位置速度估计失败，位置速度估计值失效，位置速度联合估计单元进入初始化阶段，重新对估计流程进行初始化。

所述的在历元到达时刻从接收机获取当前伪随机码相位和多普勒测量值步骤为：

1)卫星导航接收机捕获GNSS卫星信号，并实时跟踪信号，测量信号的伪随机码相位、多普勒和载噪比状态；

2)接收机根据其定时设置，在历元到达时刻对当前伪随机码相位、多普勒和载噪比测量值进行采样，并发送给位置速度联合估计单元；

3)位置速度联合估计单元获取接收机发来的信息并以信息到达时刻作为历元到达时刻。

所述的初始化阶段，对当前伪距测量值与接收机位置关系，以及当前多普勒测量值与接收机速度关系分别建模，利用加权最小二乘方法分别计算接收机位置和速度作为当前历元到达时刻的位置速度估计值步骤为：

1)位置速度联合估计单元中的权重计算模块获105取当前各通道信号载噪比测量值，将其按最大值归一化后作为不同通道信号伪距权重系数和不同通道信号的多普勒权重系数。

2)位置速度联合估计单元中的伪距测量建模模块102将伪随机码相位对应的信号发射时间和从计时器获得的当前时间求差，得到伪距时间，伪距时间乘以光速c得到伪距测量值矢量ρ＝{ρ₁，ρ₂，…，ρ_n}。其中n为当前可用于位置速度估计的GNSS卫星信号数量。伪距测量建模模块102对伪距值建模如下：

假定接收机的三维位置为p_u＝(x_u，y_u，z_u)，计时器时间偏差量为t_u，伪距测量值满足方程组

${\mathrm{\ρ}}_j=\sqrt{{(x_j-x_u)}^2+{(y_j-y_u)}^2+{(z_j-z_u)}^2}+{\mathrm{ct}}_u+{\mathrm{\ω}}_{\mathrm{\ρj}},j=\mathrm{1,2},\·\·\·,n---\left(1\right)$

其中，p_j＝(x_j，y_j，z_j)为第j颗卫星的三维位置，ω_ρj为第j颗卫星对应的伪距测量值中的噪声分量。

3)位置速度联合估计单元中的加权最小二乘位置估计模块106以当前的接收机位置估计值为

估计值与中心值误差为(Δx_u，Δy_u，Δz_u，Δt_u)，将方程组(1)线性化为

${\mathrm{\ρ}}_j={\widehat{\mathrm{\ρ}}}_j-\frac{x_j-{\hat{x}}_u}{{\widehat{\mathrm{\ρ}}}_j-c{\hat{t}}_u}\mathrm{\Δ}{x}_u-\frac{y_j-{\hat{y}}_u}{{\widehat{\mathrm{\ρ}}}_j-c{\hat{t}}_u}\mathrm{\Δ}{y}_u-\frac{z_j-{\hat{z}}_u}{{\widehat{\mathrm{\ρ}}}_j-c{\hat{t}}_u}\mathrm{\Δ}{z}_u+c{\mathrm{\Δt}}_u+{\mathrm{\ω}}_{\mathrm{\ρj}}---\left(2\right)$

加权最小二乘位置估计模块106由权重计算模块105获取权重系数，利用加权最小二乘方法进行迭代求解，得到接收机位置首次估计值p_u1。

4)位置速度联合估计单元中的多普勒测量建模模块103得到多普勒测量值矢量f_d＝{f_d1，f_d2，…，f_dn}。其中n为当前可用于位置速度估计的GNSS卫星信号数量。多普勒测量建模模块103对多普勒值建模如下：

假定接收机的三维速度为v_u＝(v_xu，v_yu，v_zu)，多普勒测量值满足方程组

$f_{\mathrm{di}}=-\left(\frac{(v_j-v_u)\·(p_j-p_u)}{c}\right)f_T+{\mathrm{\ω}}_{\mathrm{fj}},j=\mathrm{1,2},\·\·\·,n---\left(3\right)$

其中，p_j＝(x_j，y_j，z_j)为第j颗卫星的三维位置，v_j＝(v_xj，v_xj，v_zj)为第j颗卫星的三维速度，ω_fj为第j颗卫星对应的多普勒测量值中的噪声分量。

5)位置速度联合估计单元中的加权最小二乘速度估计模块107由权重计算模块105获取权重系数，利用加权最小二乘方法对线性方程组(3)进行求解，得到接收机位置首次估计值v_u1。

所述的初始化阶段完成后，对当前伪距测量值与接收机位置速度关系，以及当前多普勒测量值与接收机位置速度关系进行建模，联立伪距测量描述方程组和多普勒测量描述方程组构成测量方程组步骤为：

位置速度联合估计单元中的联合估计建模模块104将伪随机码相位对应的信号发射时间和从计时器获得的当前时间求差，得到伪距时间，伪距时间乘以光速c得到伪距测量值矢量ρ＝{ρ₁，ρ₂，…，ρ_n}；得到多普勒测量值矢量f_d＝{f_d1，f_d2，…，f_dn}，其中n为当前可用于位置速度估计的GNSS卫星信号数量；联合估计建模模块对测量值建模如下：

假定接收机的三维位置为p_u＝(x_u，y_u，z_u)，接收机的三维速度为v_u＝(v_xu，v_yu，v_zu)，计时器时间偏差量为t_u，测量值满足方程组

$\left\{\begin{array}{c}{\mathrm{\ρ}}_j=\sqrt{{(x_j-x_u)}^2+{(y_j-y_u)}^2+{(z_j-z_u)}^2}+{\mathrm{ct}}_u+{\mathrm{\ω}}_{\mathrm{\ρj}}\\ f_{\mathrm{dj}}=f_T\·\frac{\sqrt{(v_{\mathrm{xj}}-v_{\mathrm{xu}})(x_j-x_u)+(v_{\mathrm{yj}}-v_{\mathrm{yu}})(y_j-y_u)+(v_{\mathrm{zj}}-v_{\mathrm{zu}})(z_j-z_u)}}{c\sqrt{{(x_j-x_u)}^2+{(y_j-y_u)}^2+{(z_j-z_u)}^2}}+{\mathrm{\ω}}_{\mathrm{fj}}\end{array}\right.j=\mathrm{1,2},\·\·\·,n---\left(4\right)$

其中，p_j＝(x_j，y_j，z_j)为第j颗卫星的三维位置，v_j＝(v_xj，v_xj，v_zj)为第j颗卫星的三维速度，ω_ρj为第j颗卫星对应的伪距测量值中的噪声分量，ω_fj为第j颗卫星对应的多普勒测量值中的噪声分量。

所述的以上一历元到达时刻得到的接收机位置速度估计值作为当前历元到达时刻测量方程组求解的初始值，利用加权最小二乘方法对上述测量方程组求解步骤为：

1)位置速度联合估计单元中的权重计算模块105获取当前各通道信号载噪比测量值，将其按最大值归一化后作为不同通道信号伪距权重系数和不同通道信号的多普勒权重系数；将接收机跟踪通道伪随机码相位误差和多普勒误差的方差比值作为伪距和多普勒测量方程组权重系数。

2)位置速度联合估计单元中的加权最小二乘位置速度联合估计模块108以当前的接收机位置速度估计值为估计值与中心值误差为(Δx_u，Δy_u，Δz_u，Δv_xu，Δv_yu，Δv_zu，Δt_u)，将方程组(4)线性化为

$\left\{\begin{array}{c}{\mathrm{\ρ}}_j={\widehat{\mathrm{\ρ}}}_j-\frac{x_j-{\hat{x}}_u}{{\widehat{\mathrm{\ρ}}}_j-c{\hat{t}}_u}\mathrm{\Δ}{x}_u-\frac{y_j-{\hat{y}}_u}{{\widehat{\mathrm{\ρ}}}_j-c{\hat{t}}_u}\mathrm{\Δ}{y}_u-\frac{z_j-{\hat{z}}_u}{{\widehat{\mathrm{\ρ}}}_j-c{\hat{t}}_u}\mathrm{\Δ}{z}_u+c{\mathrm{\Δt}}_u+{\mathrm{\ω}}_{\mathrm{\ρj}}\\ f_j={\hat{f}}_j-\frac{f_T}{c}\left\{\frac{1}{{\widehat{\mathrm{\ρ}}}_j}\right[(v_{\mathrm{xj}}-{\hat{v}}_{\mathrm{xu}}+\frac{x_j-{\hat{x}}_u}{{\widehat{\mathrm{\ρ}}}_j^2})\mathrm{\Δ}{x}_u+(v_{\mathrm{yj}}-{\hat{v}}_{\mathrm{yu}}+\frac{y_j-{\hat{y}}_u}{{\widehat{\mathrm{\ρ}}}_j^2})\\ -\frac{1}{{\widehat{\mathrm{\σ}}}_j}[(x_j-{\hat{x}}_u){\mathrm{\Δv}}_{\mathrm{xu}}+(y_j-{\hat{y}}_u)\mathrm{\Δ}{v}_{\mathrm{yu}}+(z_j-{\hat{z}}_u)\mathrm{\Δ}{v}_{\mathrm{zu}}]\}{\mathrm{\ω}}_{\mathrm{fj}}\end{array}\mathrm{\Δ}{y}_u+(v_{\mathrm{zj}}-{\hat{v}}_{\mathrm{zu}}+\frac{z_j-{{\hat{z}}_u}_{}}{{\widehat{\mathrm{\ρ}}}_j^2}){\mathrm{\Δz}}_u]\right.---\left(5\right)$

假设当前时刻为历元(m+1)到达时刻，其中m为自然数。加权最小二乘位置速度联合估计模块108由权重计算模块105获取权重系数，利用加权最小二乘方法进行迭代求解，得到当前历元(m+1)到达时刻的接收机位置估计值p_u(m+1)。

实施例

本发明提供了一种卫星导航位置速度联合估计方法。实施例结构框图如图2所示，包括GNSS接收机201、位置速度估计单元202和计时器203三部分。其中，GNSS接收机包括射频模块204和基带信号处理模块205两个部分。GNSS卫星信号经由天线送入射频模块204的输入端，信号经过该模块下变频后形成模拟中频信号。射频模块204输出的模拟中频信号由AD按照48MHz时钟采样，形成数字中频信号，输入基带信号处理模块205。基带信号处理模块205由FPGA实现，完成GNSS卫星信号的捕获、跟踪、解调和载噪比估计等工作。

本实施例中，历元长度定位10ms。在每一历元到达时刻，基带信号处理模块205对当前伪随机码相位、多普勒和载噪比测量值进行采样，形成测量数据送至位置速度估计单元202和计时器203。计时器203将每次数据传输完成时刻作为参考时钟进行计时，并将计时结果送至位置速度估计单元202。位置速度估计单元202完成卫星导航位置速度联合估计方法实现，并输出位置速度估计结果。

本实施例中位置速度估计单元202工作流程如图3所示。位置速度估计单元开启后，其工作步骤如下：

1.在历元到达时刻1，位置速度估计单元中不存在前一次接收机位置速度估计数据，重置接收机位置估计值为坐标原点(0，0，0)，接收机速度估计值为(0，0，0)。伪随机码相位和多普勒测量值被状态判决模块101分别送入伪距测量建模模块102和多普勒测量建模模块103。

2.权重计算模块105获取当前各通道信号载噪比测量值，将载噪比测量值按最大值归一化后作为不同通道信号伪距权重系数和不同通道信号的多普勒权重系数；将接收机跟踪通道伪随机码相位误差和多普勒误差的方差比值作为伪距和多普勒测量方程组权重系数。

3.伪距测量建模模块102将伪随机码相位对应的信号发射时间和从计时器获得的当前时间求差，得到伪距时间，伪距时间乘以光速c得到伪距测量值矢量，并完成对伪距值的建模。

4.加权最小二乘位置估计模块106由权重计算模块105获取权重系数，利用加权最小二乘方法对伪距测量建模模块102给出的方程组进行迭代求解，得到接收机位置首次估计值p_u1。

5.多普勒测量建模模块103得到多普勒测量值矢量并对多普勒值建模。

6.加权最小二乘速度估计模块107由权重计算模块105获取权重系数，利用加权最小二乘方法对对多普勒测量建模模块103给出的方程组进行求解，得到接收机位置首次估计值v_u1。

7.假设在第m+1历元到达时刻，位置速度估计单元中存在有效接收机位置速度估计数据p_um和v_um，其中m为自然数。伪随机码相位和多普勒测量值被状态判决模块101送入联合估计建模模块104。

8.联合估计建模模块104将伪随机码相位对应的信号发射时间和从计时器获得的当前时间求差，得到伪距时间，伪距时间乘以光速c得到伪距测量值矢量，同时，得到多普勒测量值矢量。联合估计建模模块104获得测量值后对测量值进行建模。

9.加权最小二乘位置速度联合估计模块108由权重计算模块105获取权重系数，利用加权最小二乘方法进行迭代求解，得到当前历元到达时刻接收机位置估计值p_u(m+1)。

10.设第m历元到达时刻接收机位置速度估计失败，则重置接收机位置估计值为坐标原点(0，0，0)，接收机速度估计值为(0，0，0)，重新转入步骤1开始的流程。

Claims (5)

1.一种卫星导航位置速度联合估计方法，其特征在于它的步骤如下：

1)位置速度联合估计单元在历元到达时刻从接收机获取当前伪随机码相位、多普勒和载噪比测量值；

2)初始化阶段，对当前伪距测量值与接收机位置关系，以及当前多普勒测量值与接收机速度关系分别建模，利用加权最小二乘方法分别计算接收机位置和速度作为当前历元到达时刻的位置速度估计值；

3)初始化阶段完成后，对当前伪距测量值与接收机位置速度关系，以及当前多普勒测量值与接收机位置速度关系进行建模，联立伪距测量描述方程组和多普勒测量描述方程组构成测量方程组；以上一历元到达时刻得到的接收机位置速度估计值作为当前历元到达时刻测量方程组求解的初始值，利用加权最小二乘方法对上述测量方程组求解；

4)加权最小二乘权重系数中，不同通道信号的伪距根据当前载噪比估计值进行加权，不同通道信号的多普勒同样根据当前载噪比估计值进行加权，伪距和多普勒测量方程组根据接收机跟踪通道输出的伪随机码相位误差和多普勒误差的方差比值进行加权；

5)若当前历元到达时刻的位置速度估计失败，位置速度估计值失效，位置速度联合估计单元进入初始化阶段，重新对估计流程进行初始化。

2.根据权利要求1所述的一种卫星导航位置速度联合估计方法，其特征在于：所述的在历元到达时刻从接收机获取当前伪随机码相位和多普勒测量值步骤为：

1)卫星导航接收机捕获GNSS卫星信号，并实时跟踪信号，测量信号的伪随机码相位、多普勒和载噪比状态；

2)接收机根据其定时设置，在历元到达时刻对当前伪随机码相位、多普勒和载噪比测量值进行采样，并发送给位置速度联合估计单元；

3)位置速度联合估计单元获取接收机发来的信息并以信息到达时刻作为历元到达时刻。

3.根据权利要求1所述的一种卫星导航位置速度联合估计方法，其特征在于：所述的初始化阶段，对当前伪距测量值与接收机位置关系，以及当前多普勒测量值与接收机速度关系分别建模，利用加权最小二乘方法分别计算接收机位置和速度作为当前历元到达时刻的位置速度估计值步骤为：

1)位置速度联合估计单元中的权重计算模块(105)获取当前各通道信号载噪比测量值，将其按最大值归一化后作为不同通道信号伪距权重系数和不同通道信号的多普勒权重系数；

2)位置速度联合估计单元中的伪距测量建模模块(102)将伪随机码相位对应的信号发射时间和从计时器获得的当前时间求差，得到伪距时间，伪距时间乘以光速c得到伪距测量值矢量ρ＝{ρ₁，ρ₂，…，ρ_n}，其中n为当前可用于位置速度估计的GNSS卫星信号数量，伪距测量建模模块(102)对伪距值建模如下：

假定接收机的三维位置为p_u＝(x_u，y_u，z_u)，计时器时间偏差量为t_u，伪距测量值满足方程组

${\mathrm{\ρ}}_j=\sqrt{{(x_j-x_u)}^2+{(y_j-y_u)}^2+{(z_j-z_u)}^2}+{\mathrm{ct}}_u+{\mathrm{\ω}}_{\mathrm{\ρj}}j=\mathrm{1,2},...,n---\left(1\right)$

其中，p_j＝(x_j，y_j，z_j)为第j颗卫星的三维位置，ω_ρj为第j颗卫星对应的伪距测量值中的噪声分量；

3)位置速度联合估计单元中的加权最小二乘位置估计模块(106)以当前的接收机位置估计值为

估计值与中心值误差为(Δx_u，Δy_u，Δz_u，Δt_u)，将方程组(1)线性化为

${\mathrm{\ρ}}_j={\widehat{\mathrm{\ρ}}}_j-\frac{x_j-{\hat{x}}_u}{{\widehat{\mathrm{\ρ}}}_j-{\widehat{\mathrm{ct}}}_u}{\mathrm{\Δx}}_u-\frac{y_j-{\hat{y}}_u}{{\widehat{\mathrm{\ρ}}}_j-{\widehat{\mathrm{ct}}}_u}{\mathrm{\Δy}}_u-\frac{z_j-{\hat{z}}_u}{{\widehat{\mathrm{\ρ}}}_j-{\widehat{\mathrm{ct}}}_u}{\mathrm{\Δz}}_u+{\mathrm{c\Δt}}_u+{\mathrm{\ω}}_{\mathrm{\ρj}}---\left(2\right)$

加权最小二乘位置估计模块(106)由权重计算模块(105)获取权重系数，利用加权最小二乘方法进行迭代求解，得到接收机位置首次估计值p_u1；

4)位置速度联合估计单元中的多普勒测量建模模块(103)得到多普勒测量值矢量f_d＝{f_d1，f_d2，…，f_dn}，其中n为当前可用于位置速度估计的GNSS卫星信号数量，多普勒测量建模模块(103)对多普勒值建模如下：

假定接收机的三维速度为v_u＝(v_xu，v_yu，v_zu)，多普勒测量值满足方程组

$f_{\mathrm{di}}=-\frac{(v_j-v_u)\·(p_j-p_u)}{c}{f}_T+{\mathrm{\ω}}_{\mathrm{fj}}j=\mathrm{1,2},...,n---\left(3\right)$

其中，p_j＝(x_j，y_j，z_j)为第j颗卫星的三维位置，v_j＝(v_xj，v_xj，v_zj)为第j颗卫星的三维速度，ω_fj为第j颗卫星对应的多普勒测量值中的噪声分量；

5)位置速度联合估计单元中的加权最小二乘速度估计模块(107)由权重计算模块(105)获取权重系数，利用加权最小二乘方法对线性方程组(3)进行求解，得到接收机位置首次估计值vu1。

4.根据权利要求1所述的一种卫星导航位置速度联合估计方法，其特征在于：所述的初始化阶段完成后，对当前伪距测量值与接收机位置速度关系，以及当前多普勒测量值与接收机位置速度关系进行建模，联立伪距测量描述方程组和多普勒测量描述方程组构成测量方程组步骤为：

位置速度联合估计单元中的联合估计建模模块(104)将伪随机码相位对应的信号发射时间和从计时器获得的当前时间求差，得到伪距时间，伪距时间乘以光速c得到伪距测量值矢量ρ＝{ρ₁，ρ₂，…，ρ_n}；得到多普勒测量值矢量f_d＝{f_d1，f_d2，…，f_dn}，其中n为当前可用于位置速度估计的GNSS卫星信号数量；联合估计建模模块对测量值建模如下：

假定接收机的三维位置为p_u＝(x_u，y_u，z_u)，接收机的三维速度为v_u＝(v_xu，v_yu，v_zu)，计时器时间偏差量为t_u，测量值满足方程组

$\left\{\begin{array}{c}{\mathrm{\ρ}}_j=\sqrt{{(x_j-x_u)}^2+{(y_j-y_u)}^2+{(z_j-z_u)}^2}+{\mathrm{ct}}_u+{\mathrm{\ω}}_{\mathrm{\ρj}}\\ f_{\mathrm{dj}}=f_T\·\frac{\sqrt{(v_{\mathrm{xj}}-v_{\mathrm{xu}})(x_j-x_u)+(v_{\mathrm{yj}}-v_{\mathrm{yu}})(y_j-y_u)+(v_{\mathrm{zj}}-v_{\mathrm{zu}})(z_j-z_u)}}{c\sqrt{{(x_j-x_u)}^2+{(x_j-y_u)}^2+{(z_j-z_u)}^2}}+{\mathrm{\ω}}_{\mathrm{fj}}\end{array}\right.j=\mathrm{1,2},...,n---\left(4\right)$

其中，p_j＝(x_j，y_j，z_j)为第j颗卫星的三维位置，v_j＝(v_xj，v_xj，v_zj)为第j颗卫星的三维速度，ω_ρj为第j颗卫星对应的伪距测量值中的噪声分量，ω_fj为第j颗卫星对应的多普勒测量值中的噪声分量。

5.根据权利要求4所述的一种卫星导航位置速度联合估计方法，其特征在于：所述的以上一历元到达时刻得到的接收机位置速度估计值作为当前历元到达时刻测量方程组求解的初始值，利用加权最小二乘方法对上述测量方程组求解步骤为：

1)位置速度联合估计单元中的权重计算模块(105)获取当前各通道信号载噪比测量值，将其按最大值归一化后作为不同通道信号伪距权重系数和不同通道信号的多普勒权重系数；将接收机跟踪通道伪随机码相位误差和多普勒误差的方差比值作为伪距和多普勒测量方程组权重系数；

2)位置速度联合估计单元中的加权最小二乘位置速度联合估计模块(108)以当前的接收机位置速度估计值为

估计值与中心值误差为(Δx_u，Δy_u，Δz_u，Δv_xu，Δv_yu，Δv_zu，Δt_u)，将方程组(4)线性化为

$\left\{\begin{array}{c}{\mathrm{\ρ}}_j={\widehat{\mathrm{\ρ}}}_j-\frac{x_j-{\hat{x}}_u}{{\widehat{\mathrm{\ρ}}}_j-{\widehat{\mathrm{ct}}}_u}{\mathrm{\Δx}}_u-\frac{y_j-{\hat{y}}_u}{{\widehat{\mathrm{\ρ}}}_j-{\widehat{\mathrm{ct}}}_u}{\mathrm{\Δy}}_u-\frac{z_j-{\hat{z}}_u}{{\widehat{\mathrm{\ρ}}}_j-{\widehat{\mathrm{ct}}}_u}{\mathrm{\Δz}}_u+{\mathrm{c\Δt}}_u+{\mathrm{\ω}}_{\mathrm{\ρj}}\\ f_j={\hat{f}}_j-\frac{f_T}{c}\left\{\frac{1}{{\widehat{\mathrm{\ρ}}}_j}\right[(v_{\mathrm{xj}}-{\hat{v}}_{\mathrm{xu}}+\frac{x_j-{\hat{x}}_u}{{{\widehat{\mathrm{\ρ}}}_j}^2}){\mathrm{\Δx}}_u+(v_{\mathrm{yj}}-{\hat{v}}_{\mathrm{yu}}+\frac{y_j-{\hat{y}}_u}{{{\widehat{\mathrm{\ρ}}}_j}^2}){\mathrm{\Δy}}_u+(v_{\mathrm{zj}}-{\hat{v}}_{\mathrm{zu}}+\frac{z_j-{\hat{z}}_u}{{{\widehat{\mathrm{\ρ}}}_j}^2}){\mathrm{\Δz}}_u]\\ -\frac{1}{{\widehat{\mathrm{\ρ}}}_j}[(x_j-{\hat{x}}_u){\mathrm{\Δv}}_{\mathrm{xu}}+(y_j-{\hat{y}}_u){\mathrm{\Δv}}_{\mathrm{yu}}+(z_j-{\hat{z}}_u){\mathrm{\Δv}}_{\mathrm{zu}}]\}{\mathrm{\ω}}_{\mathrm{fj}}\end{array}\right.---\left(5\right)$

假设当前时刻为历元m+1到达时刻，其中m为自然数，加权最小二乘位置速度联合估计模块(108)由权重计算模块(105)获取权重系数，利用加权最小二乘方法进行迭代求解，得到当前历元m+1到达时刻的接收机位置估计值p_u(m+1)。

Images