CN102508279B - 卫星导航系统gnss定姿测量值的处理方法及gnss定姿测量仪 - Google Patents

Abstract

本发明公开了一种卫星导航系统GNSS定姿测量值的处理方法及GNSS定姿测量仪。其中，该方法包括：读取目标载体的载体姿态信息的历元初始值；对历元初始值进行姿态量测处理，以获取载体姿态信息的历元量测值；对前一历元最终值进行估值处理，以获取载体姿态信息的历元预报值；对历元量测值和历元预报值进行抗差自适应融合滤波处理，以获取载体姿态信息的当前历元最终值。通过本发明，能够实现使得GNSS定姿测量方法的精度更高、连续更好、可靠性更强。

Description

卫星导航系统GNSS定姿测量值的处理方法及GNSS定姿测量仪

技术领域

本发明涉及姿态测量领域，具体而言，涉及一种卫星导航系统GNSS定姿测量值的处理方法及GNSS定姿测量仪。

背景技术

GNSS设备具有体积小、成本低、自动化程度高、能耗小等诸多优点，因此已被广泛的应用于各类运动载体的姿态测量中。但实际测量时，经常会遇到观测数据中断、观测数据跳跃以及必要观测数据不足等情况，从而导致GNSS的定姿结果精度较低、连续性较差、可靠性较弱等一系列问题。如何有效解决上述问题对进一步提高GNSS的定姿能力，扩大GNSS的定姿应用范围至关重要。对此本发明提出了一种基于自适应抗差融合滤波的GNSS定姿方法。利用该方法用户可在观测数据出现异常、中断以及不足时获取准确可靠连续的载体姿态信息；在观测数据充裕情况下，则能得到相对现有方法精度更高的定姿结果。

所谓载体姿态确定，就是先在目标载体上建立一个载体坐标系并与目标载体固连，称之为目标载体框架坐标系；然后根据目标所处环境，选择一个合适的坐标系作为参考坐标系，GNSS定姿中参考坐标系通常选用当地水平坐标系；最后确定出目标载体坐标系相对于参考坐标系的取向，即载体姿态信息，可以用欧拉角式、欧拉轴式、欧拉四元素式等方式表示。如：机载动态测量中，一般都采用航向角、俯仰角和翻滚角来描述载体的三维姿态信息。

GNSS定姿方法有许多，从天线布局上分，有立体布局法、平面布局法和长短基线结合布局法；从观测值使用形式上分，有基于单差或双差载波相位的方法，也有基于信号强度的粗估计方法；从计算方法上分，有直接计算法、九参数最小二乘法和三参数迭代最小二乘法。计算方法是GNSS定姿的核心问题，现有三种定姿方法的基本原理如下：

一、直接计算法

直接计算法是指通过使用代数的方法来求解载体的姿态角，即每次代入一条基线的已知坐标来逐次求解载体的三个姿态角。若某运动载体上装有三幅GNSS天线，它们构成如图1所示的载体坐标系。其中天线1为主天线，并作为载体坐标系的原点；天线1至天线2的方向作为载体坐标系的Y轴；X轴位于该平面内，垂直于Y轴并指向Y轴的右侧；Z轴则垂直于该平面并构成右手系。载体坐标系与当地水平坐标系依靠上述三个轴的旋转联系在一起：若当地水平坐标系绕载体坐标系的Z轴旋转，则为航向角；若绕Y轴旋转，则为俯仰角；绕X轴旋转，则为翻滚角。

直接计算法的具体计算方法如下：

1、每个历元用伪距单点定位确定出主天线（天线1）的地心坐标（WGS84），并利用载波相位相对定位确定出其它天线相对于主天线的基线解：

${\mathrm{dX}}_{\mathrm{EE}}=\left[\begin{array}{cc}{\mathrm{dx}}_{E,2}& {\mathrm{dx}}_{E,3}\\ {\mathrm{dy}}_{E,2}& {\mathrm{dy}}_{E,3}\\ {\mathrm{dz}}_{E,2}& {\mathrm{dz}}_{E,3}\end{array}\right]---\left(1\right)$

式中，E表示为地心坐标系。

2、若将天线1作为载体坐标系的原点，则可利用下式将（1）的地心坐标解转换成当地水平坐标系的解：

dX_LL=R₁₃dX_EE                      （2）

式中，L表示当地水平坐标系，λ和

分别表示天线1的经度和纬度。

3、建立载体坐标与当地水平坐标的函数关系：

dX_LL=R₃₁₂dX_BB                  （4）

$R_{312}^k=\left[\begin{array}{ccc}\cos \mathrm{\α}\cos \mathrm{\γ}+\sin \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}& \cos \mathrm{\β}\sin \mathrm{\γ}& \sin \mathrm{\α}\cos \mathrm{\γ}-\cos \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}\\ -\cos \mathrm{\α}\sin \mathrm{\γ}+\sin \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}& \cos \mathrm{\β}\cos \mathrm{\γ}& -\sin \mathrm{\α}\sin \mathrm{\γ}-\cos \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}\\ -\sin \mathrm{\α}\cos \mathrm{\β}& \sin \mathrm{\β}& \cos \mathrm{\α}\cos \mathrm{\β}\end{array}\right]---\left(5\right)$

式中，B表示载体坐标系，α，β，γ分别表示翻滚角、俯仰角和航向角。

接着上述步骤，将基线L₁₂的已知当地水平坐标系的解dX_LL=[dx_L,2dy_L,2dz_L,2]^T和载体坐标系的解dX_BB=[0L₁₂0]^T同时代入（4）式，得：

$\left[\begin{array}{c}{\mathrm{dx}}_{L,2}\\ {\mathrm{dy}}_{L,2}\\ {\mathrm{dz}}_{L,2}\end{array}\right]=L_{12}\left[\begin{array}{c}\cos \mathrm{\β}\sin \mathrm{\γ}\\ \cos \mathrm{\β}\cos \mathrm{\γ}\\ \sin \mathrm{\β}\end{array}\right]---\left(6\right)$

由（6）式可解得：

γ=arctan(dx_L,2/dy_L,2)               （7）

$\mathrm{\β}=\arctan ({\mathrm{dz}}_{L,2}/\sqrt{{\mathrm{dx}}_{L,2}^2+{\mathrm{dy}}_{L,2}^2})---\left(8\right)$

4、再将基线L₁₃的已知地心坐标dX_LL=[dx_L,3dy_L,3dz_L,3]^T和载体坐标dX_BB=[L₁₃sinθL₁₃cosθ0]^T，以及由（6）式求得的γ,β同时代入（4）式，则可得：

$\mathrm{\α}=\mathrm{ar}\sin \left(\frac{\sin \mathrm{\β}\cos \mathrm{\θ}{L}_{13}-{\mathrm{dz}}_{L,3}}{\cos \mathrm{\β}\sin \mathrm{\θ}{L}_{13}}\right)---\left(9\right)$

二、九参数最小二乘法

九参数最小二乘法的计算步骤可以分为以下六步：

1、在姿态测量开始前，利用GNSS静态相对定位模式测量出其他天线相对于主天线在载体坐标下的基线解dX_BB=[dX_B,2dX_B,3]，天线1为载体坐标系原点。

2、载体运动过程中，利用伪距单点定位方式求出主天线每个历元的地心坐标（如WGS84坐标）。并在此基础上，再利用载波相位相对定位模式求出其他天线相对于主天线在地心坐标系下的基线解dX_EE=[dX_E,2dX_E,3]。

3、利用（2）式将上述地心坐标系下的解转换为当地水平坐标下的解dX_LL=[dX_L,2dX_L,3]。

4、按（4）式建立载体坐标与当地水平坐标的函数关系dX_LL=R₃₁₂dX_BB。

5、利用最小二乘平差解得转换矩阵R₃₁₂的九个矩阵元素：

$R_{312}={\mathrm{dX}}_{\mathrm{LL}}^T{\mathrm{dX}}_{\mathrm{BB}}{\left({\mathrm{dX}}_{\mathrm{BB}}{\mathrm{dX}}_{\mathrm{BB}}^T\right)}^{-1}---\left(10\right)$ （10）

6、根据（10）式求得的矩阵元素，即可进一步求出三个姿态参数：

α=arctan(-R₃₁₂(3,1)/R₃₁₂(3,3))             （11）

β=arcsin(R₃₁₂(3,2))               （12）

γ=arctan(R₃₁₂(1,2)/R₃₁₂(2,2))            （13）

三、三参数迭代最小二乘

由于旋转矩阵R₃₁₂中实际只有三个独立参数，即α,β,γ。因此三参数迭代最小二乘法直接将这三个独立参数作为未知变量进行求解。具体方法如下：

1、利用近似解α₀，β₀，γ₀，对于某一历元，可建立如下量测方程：

$A\widehat{\mathrm{\δ}}-L=V,P---\left(14\right)$

式中，

$A=\left[\begin{array}{ccc}\frac{\∂R_{312}}{\∂\mathrm{\α}}{\mathrm{dX}}_{B,2}& \frac{\∂R_{312}}{\∂\mathrm{\β}}{\mathrm{dX}}_{B,2}& \frac{\∂R_{312}}{\∂\mathrm{\γ}}{\mathrm{dX}}_{B,2}\\ \frac{\∂R_{312}}{\∂\mathrm{\α}}{\mathrm{dX}}_{B,3}& \frac{\∂R_{312}}{\∂\mathrm{\β}}{\mathrm{dX}}_{B,3}& \frac{\∂R_{312}}{\∂\mathrm{\γ}}{\mathrm{dX}}_{B,3}\end{array}\right]---\left(15\right)$

$\widehat{\mathrm{\δ}}=\left[\begin{array}{c}\∂\mathrm{\α}\\ \∂\mathrm{\β}\\ \∂\mathrm{\γ}\end{array}\right]---\left(16\right)$

$L=\left[\begin{array}{c}{\mathrm{dX}}_{L,2}-B_0{\mathrm{dX}}_{B,2}\\ {\mathrm{dX}}_{L,3}-B_0{\mathrm{dX}}_{B,3}\end{array}\right]---\left(17\right)$

B₀=R₃₁₂(α₀,β₀,γ₀)           （18）

$P=\left[\begin{array}{cc}{\mathrm{\Σ}}_{{\mathrm{dX}}_{L_{12}}}& 0\\ 0& {\mathrm{\Σ}}_{{\mathrm{dX}}_{L_{13}}}\end{array}\right]---\left(19\right)$

式中V为残差项，∑表示协方差。

2、利用最小二乘平差求出待估参数的改正值：

$\widehat{\mathrm{\δ}}={\left[A^T\mathrm{PA}\right]}^{-1}\left[A^T\mathrm{PL}\right]---\left(20\right)$

3、判断改正值是否大于限定值，若大于限定值，则继续利用新的近似值重复计算；若小于则终止运算，得到最终解。

现有的三种GNSS定姿方法间既有区别又有联系：它们的基本原理是相同的，即都是利用当地水平坐标系与载体坐标系的转换矩阵R₃₁₂来求解载体的姿态信息（航向角、俯仰角、翻滚角）。但三种方法采用的解算策略各不相同：直接计算法是通过代数方法来求解载体的三个姿态角，即每次代入一条基线的已知坐标来逐次求解载体的姿态角。该方法的优点是计算过程简单，解算速度快；缺点是不能同时利用多条基线解的信息来估计姿态角，因此其估计值是次优的。而九参数最小二乘法是通过矩阵运算的方式来求解载体的姿态角，即先通过最小二乘求解出转换矩阵R₃₁₂的九个矩阵元素，然后利用R₃₁₂的已知矩阵元素来计算三个姿态角参数。该方法的优点是可以同时利用多条基线解的信息来估计姿态角，因此相对于直接计算法其解精度和可靠性都更高。但矩阵R₃₁₂中实际上只有三个真正独立的参数，即航向角、俯仰角和翻滚角，因此九参数最小二乘法的解并不是最优估值。三参数迭代最小二乘法则直接将三个姿态角参数作为待估值，通过迭代最小二乘来求解载体的姿态角，其估计值理论上是最优的。但该方法的主要缺陷是计算时需要较精确的初始值来进行方程线性化，并需迭代计算，解算过程较为复杂。

现有的三种GNSS定姿方法虽各有优缺点，但它们都存在一些共有的缺陷：

一是现有的GNSS定姿方法都不具有抵御异常历元量测值的能力。对于高速运动的载体而言，由于周围环境复杂多变导致粗差出现十分的频繁，具有较好的抗差能力对其定姿结果的可靠性至关重要；

二是上述方法都没有充分利用载体的运动状态模型信息，因此对于历元量测值的使用而言不够充分，造成有用信息的浪费；

三是当某些历元的实际量测数少于必要量测数时，上述方法都会出现无解的情况，这对保持定姿结果的连续性十分不利。

目前针对相关技术的GNSS定姿方法在目标载体处于恶劣监测环境下，监测的定姿结果的稳健性、抗差性、连续性以及精确性较差的问题，目前尚未提出有效的解决方案。

发明内容

针对相关技术的GNSS定姿方法在目标载体处于恶劣监测环境下，监测的定姿结果的稳健性、抗差性、连续性以及精确性较差的问题，目前尚未提出有效的问题而提出本发明，为此，本发明的主要目的在于提供一种卫星导航系统GNSS定姿测量值的处理方法及GNSS定姿测量仪，以解决上述问题。

为了实现上述目的，根据本发明的一个方面，提供了一种卫星导航系统GNSS定姿测量值的处理方法，该方法包括：读取目标载体的载体姿态信息的历元初始值；对历元初始值进行姿态量测处理，以获取载体姿态信息的历元量测值；对前一历元最终值进行估值处理，以获取载体姿态信息的历元预报值；对历元量测值和历元预报值进行抗差自适应融合滤波处理，以获取载体姿态信息的当前历元最终值。

进一步地，在读取目标载体的载体姿态信息的历元初始值之前，方法还包括：实时读取目标载体的载体姿态信息的历元观测值；对历元观测值进行估计处理，以获取载体姿态信息的历元初始值。

进一步地，对历元初始值进行姿态量测处理，以获取载体姿态信息的历元量测值包括：通过GNSS静态相对定位算法来获取目标载体的多条基线在载体坐标系下的第一基线向量组

k为历元号，BB为载体坐标系；通过载波相对定位算法来获取目标载体的多条基线在地心坐标系下的第二基线向量组

EE表示地心坐标系；通过旋转矩阵

将第二基线向量组

转换成在当地水平坐标系下的第三基线向量组

LL表示当地水平坐标系，第三基线向量组

根据旋转矩阵

来获取第一基线向量组

与第三基线向量组

的函数关系：

通过泰勒级数展开至一阶来将非线性函数 ${\mathrm{dX}}_{\mathrm{LL}}^k=R_{312}^k{\mathrm{dX}}_{\mathrm{BB}}^k$ 转换为线性函数 $A_k\mathrm{\δ}{\tilde{X}}_k-L_k=V_k,P_k,$ 其中，

$A_k=\left[\begin{array}{ccc}\frac{\∂R_{312}}{\∂\mathrm{\α}}{\mathrm{dX}}_{B,2}^k& \frac{\∂R_{312}}{\∂\mathrm{\β}}{\mathrm{dX}}_{B,2}^k& \frac{{\∂R}_{312}}{\∂\mathrm{\γ}}{\mathrm{dX}}_{B,2}^k\\ \·\·\·& \·\·\·& \·\·\·\\ \frac{\∂R_{312}}{\∂\mathrm{\α}}{\mathrm{dX}}_{B,i}^k& \frac{\∂R_{312}}{\∂\mathrm{\β}}{\mathrm{dX}}_{B,i}^k& \frac{\∂R_{312}}{\∂\mathrm{\γ}}{\mathrm{dX}}_{B,i}^k\\ \·\·\·& \·\·\·& \·\·\·\\ \frac{\∂R_{312}}{\∂\mathrm{\α}}{\mathrm{dX}}_{B,n}^k& \frac{\∂R_{312}}{\∂\mathrm{\β}}{\mathrm{dX}}_{B,n}^k& \frac{\∂R_{312}}{\∂\mathrm{\γ}}{\mathrm{dX}}_{B,n}^k\end{array}\right],$ $L_k=\left[\begin{array}{c}{\mathrm{dX}}_{L,2}^k-B_2^k{\mathrm{dX}}_{B,2}^k\\ \·\·\·\\ {\mathrm{dX}}_{L,i}^k-B_i^k{\mathrm{dX}}_{B,i}^k\\ \·\·\·\\ {\mathrm{dX}}_{L,n}^k-B_{n-1}^k{\mathrm{dX}}_{B,n}^k\end{array}\right],$ $P_k=\left[\begin{array}{ccc}{\mathrm{\Σ}}_{{\mathrm{dX}}_{L,2}^k}& 0& 0\\ \·\·\·& \·\·\·& \·\·\·\\ 0& {\mathrm{\Σ}}_{{\mathrm{dX}}_{L,i}^k}& 0\\ \·\·\·& \·\·\·& \·\·\·\\ 0& 0& {\mathrm{\Σ}}_{{\mathrm{dX}}_{L,n}^k}\end{array}\right],$

表示第i条基线k历元的解算精度，V_k为残差， $\mathrm{\δ}{\tilde{X}}_k={\left[A_k^T{P}_k{A}_k\right]}^{-1}\left[A_k^T{P}_k{L}_k\right],$ $B_i^k=R_{312}^k({\mathrm{\α}}_0,{\mathrm{\β}}_0,{\mathrm{\γ}}_0),$ (α₀,β₀,γ₀)为历元初始值，(α,β,γ)表征载体姿态信息，k为历元号，i为基线编号；通过最小二乘解算法对

进行求解，以获取历元量测值

进一步地，在获取载体姿态信息的历元量测值之后，方法还包括：获取历元量测值的第一精度值 ${\mathrm{\Σ}}_{{\tilde{X}}_k}={\left[A_k^T{P}_k{A}_k\right]}^{-1}.$

进一步地，对前一历元最终值进行估值处理，以获取载体姿态信息的历元预报值包括：通过状态方程

来获取历元预报值

其中，状态转移矩阵 ${\mathrm{\Φ}}_{k,k-1}=\left[\begin{array}{cccccc}1& 0& 0& \mathrm{\Δt}& 0& 0\\ 0& 1& 0& 0& \mathrm{\Δt}& 0\\ 0& 0& 1& 0& 0& \mathrm{\Δt}\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right],$ W_k为动态噪声，△t为历元间隔，且历元最终值的初始值为历元量测值。

进一步地，在获取载体姿态信息的历元预报值之后，方法还包括：获取历元预报值的第二精度值 ${\mathrm{\Σ}}_{{\stackrel{\‾}{X}}_k}={\mathrm{\Φ}}_{k,k-1}{\mathrm{\Σ}}_{{\hat{X}}_{k-1}}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Σ}}_{W_t}.$

进一步地，对历元量测值和历元预报值进行抗差自适应融合滤波处理，以获取载体姿态信息的当前历元最终值包括：通过状态方程

来获取历元最终值其中， ${\stackrel{\‾}{X}}_k={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}+W_k,K_k={\mathrm{\Σ}}_{{\stackrel{\‾}{X}}_k}{A}_k^T{(A_k{\mathrm{\Σ}}_{{\stackrel{\‾}{X}}_k}{A}_k^T+P_k^{-1})}^{-1},{\mathrm{\Σ}}_{{\hat{X}}_k}=[I-K_k{A}_k]{\mathrm{\Σ}}_{{\stackrel{\‾}{X}}_k}.$

进一步地，对历元量测值和历元预报值进行抗差自适应融合滤波处理，以获取载体姿态信息的当前历元最终值包括：通过抗差自适应融合滤波算法的状态方程： ${\hat{X}}_k={[A_k^T{\stackrel{\‾}{P}}_k{A}_k+\mathrm{\ϵ}{P}_{{\stackrel{\‾}{X}}_k}]}^{-1}[A_k^T{\stackrel{\‾}{P}}_k{L}_k+\mathrm{\ϵ}{P}_{{\stackrel{\‾}{X}}_k}{\stackrel{\‾}{X}}_k]$ 来获取历元最终值

且 ${\mathrm{\Σ}}_{{\hat{X}}_k}={(A_k^T{\stackrel{\‾}{P}}_k{A}_k+\mathrm{\ϵ}{P}_{{\stackrel{\‾}{X}}_k})}^{-1},$

其中，降权因子

自适应因子 $\mathrm{\&epsiv;}=\left\{\begin{array}{cc}1& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{X}}_t\right|\&le;c_0\\ \frac{c_0}{\left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{X}}_t\right|}{\left(\frac{c_1-\left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{X}}_t\right|}{c_1-c_0}\right)}^2,& c_0<\left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{X}}_t\right|\&le;c_1,\\ 0& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{X}}_t\right|>c_1\end{array}\right.$ c₀和c₁为常数，取值范围为c₀=1.0～1.5，c₁＝3.0～4.5， $\mathrm{\&Delta;}{\stackrel{\&OverBar;}{X}}_t=\left|\right|{\tilde{X}}_t-{\stackrel{\&OverBar;}{X}}_t\left|\right|/\sqrt{\mathrm{tr}\left\{{\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{X}}_t}\right\}},$

为标准化残差。

进一步地，在获取载体姿态信息的当前历元最终值之后，方法还包括：获取历元最终值的第三精度 ${\mathrm{\&Sigma;}}_{{\hat{X}}_k}={(A_k^T{P}_k{A}_k+P_{{\stackrel{\&OverBar;}{X}}_k})}^{-1},$ 其中， $P_{{\stackrel{\&OverBar;}{X}}_k}={\left({\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{X}}_k}\right)}^{-1}.$

为了实现上述目的，根据本发明的另一方面，提供了一种卫星导航系统GNSS定姿测量仪，该GNSS定姿测量仪包括：读取单元，用于读取目标载体的载体姿态信息的历元初始值；第一处理单元，用于对历元初始值进行姿态量测处理，以获取载体姿态信息的历元量测值；第二处理单元，用于对前一历元最终值进行估值处理，以获取载体姿态信息的历元预报值；第三处理单元，用于对历元量测值和历元预报值进行抗差自适应融合滤波处理，以获取载体姿态信息的当前历元最终值。

通过本发明，采用读取目标载体的载体姿态信息的历元初始值；对历元初始值进行姿态量测处理，以获取载体姿态信息的历元量测值；对前一历元最终值进行估值处理，以获取载体姿态信息的历元预报值；对历元量测值和历元预报值进行抗差自适应融合滤波处理，以获取载体姿态信息的当前历元最终值，解决了相关现有技术的GNSS定姿方法在目标载体处于恶劣监测环境下，监测的定姿结果的稳健性、抗差性、连续性以及精确性较差的问题，进而实现使得GNSS定姿测量方法的精度更高、连续更好、可靠性更强的效果。

附图说明

此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1是根据相关现有技术的目标载体中三条GNSS天线的载体坐标系示意图；

图2是根据本发明实施例的GNSS定姿测量仪的结构示意图；

图3是根据本发明实施例的GNSS定姿测量值的处理方法的流程图；

图4是根据本发明图2和3所示实施例在飞机上的位置示意图；

图5是根据本发明实施例在静态实验中三参数迭代最小二乘法的静态定姿结果示意图；

图6是根据本发明实施例在静态实验中抗差自适应融合滤波方法静态定姿的结果示意图；

图7是根据本发明实施例在动态实验中飞机飞行的平面轨迹和高度变化示意图；

图8是根据本发明实施例在动态实验中飞机航向角的变化示意图；

图9是根据本发明实施例在动态实验中飞机俯仰角的变化示意图。

具体实施方式

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本发明。

图2是根据本发明实施例的GNSS定姿测量仪的结构示意图。

如图2所示，该GNSS定姿测量仪包括：读取单元10，用于读取目标载体的载体姿态信息的历元初始值；第一处理单元30，用于对历元初始值进行姿态量测处理，以获取载体姿态信息的历元量测值；第二处理单元50，用于对前一历元最终值进行估值处理，以获取载体姿态信息的历元预报值；第三处理单元70，用于对历元量测值和历元预报值进行抗差自适应融合滤波处理，以获取载体姿态信息的当前历元最终值。

本申请上述实施例的处理单元利用抗差自适应融合滤波算法对目标载体的历元初始值的进行优化，从而获得一种精度更高、连续性更好、可靠性更强的GNSS定姿测量仪，对进一步完善GNSS定姿理论和提高GNSS的实际定姿能力具有十分重要的理论与实际意义。该GNSS定姿测量仪可以广泛应用于各类基于GNSS接收设备的载体姿态测量装置，如：车载导航仪、地理信息移动采集系统、飞行状态监测设备、卫星姿态控制器以及测量船姿态校正装置等等。

本发明的关键点和欲保护点就是如何利用抗差自适应融合滤波算法进行GNSS定姿，它包括以下三个方面：一是如何构建相应的姿态观测方程和姿态运动方程；二是定姿过程中如何合理选择自适应因子；三是定姿过程中如何有效的抵御异常观测值的影响。

图3是根据本发明实施例的GNSS定姿测量值的处理方法的流程图，如图3所示该方法包括如下步骤：

步骤S102，通过图2中的读取单元10来读取目标载体的载体姿态信息的历元初始值。

步骤S104，通过图2中的第一处理单元30来对历元初始值进行姿态量测处理，以获取载体姿态信息的历元量测值。

步骤S106，通过图2中的第二处理单元50来对前一历元最终值进行估值处理，以获取载体姿态信息的历元预报值。

步骤S108，通过图2中的第三处理单元70来对历元量测值和历元预报值进行抗差自适应融合滤波处理，以获取载体姿态信息的当前历元最终值。

本申请上述实施例利用了抗差自适应融合滤波进行GNSS定姿，通过处理单元来构建相应的姿态量测方程和姿态运动方程，实现对目标载体的历元初始值的进行优化，从而获得一种精度更高、连续性更好、可靠性更强的GNSS定姿测量仪，对进一步完善GNSS定姿理论和提高GNSS的实际定姿能力具有十分重要的理论与实际意义；二是定姿过程中如何合理选择自适应因子；三是定姿过程中如何有效的抵御异常历元量测值的影响。本申请通过(α,β,γ)来表征载体姿态信息。

优选地，在读取目标载体的载体姿态信息的历元初始值之前，方法还包括：实时读取目标载体的载体姿态信息的历元观测值；对历元观测值进行估计处理，以获取载体姿态信息的历元初始值。

本申请上述实施例中，对历元初始值进行姿态量测处理，以获取载体姿态信息的历元量测值的步骤的具体实施过程可以如下：

首先，假设某运动目标载体上装有n个GNSS天线用于姿态确定。若任意选取其中一个为主天线，并将其作为载体坐标系的原点，则其它天线与主天线可形成n-1条独立基线。在姿态测量开始前，通过GNSS静态相对定位方式确定出其它天线相对于主天线在载体坐标系下的基线向量组

${\mathrm{dX}}_{\mathrm{BB}}^k=\left[\begin{array}{ccccc}{\mathrm{dX}}_{B,2}^k& \&CenterDot;\&CenterDot;\&CenterDot;& {\mathrm{dX}}_{B,i}^k& \&CenterDot;\&CenterDot;\&CenterDot;& {\mathrm{dX}}_{B,n}^k\end{array}\right]---\left(21\right)$

式中k为历元号，i为基线编号。

目标载体运动过程中，每个历元可通过伪距单点定位方式确定出主天线的地心坐标（如WGS84坐标）。在此基础上，再利用载波相对定位方式确定其它天线相对主天线在地心坐标系下的基线向量组如公式（22）。相关文献已证明主天线位置误差为100米时，对定姿结果的影响仅为毫米级，因此利用伪距单点定位方式确定的主天线位置可以满足定姿精度的要求。

${\mathrm{dX}}_{\mathrm{EE}}^k=\left[\begin{array}{ccccc}{\mathrm{dX}}_{E,2}^k& \&CenterDot;\&CenterDot;\&CenterDot;& {\mathrm{dX}}_{E,i}^k& \&CenterDot;\&CenterDot;\&CenterDot;& {\mathrm{dX}}_{E,n}^k\end{array}\right]---\left(22\right)$

然后利用旋转矩阵

将上述地心坐标系下的解转换成当地水平坐标系下的解，如：

${\mathrm{dX}}_{\mathrm{LL}}^k=R_{13}^k{\mathrm{dX}}_{\mathrm{EE}}^k---\left(23\right)$

$R_{13}^k=\left[\begin{array}{ccc}-\sin \mathrm{\&lambda;}& \cos \mathrm{\&lambda;}& 0\\ -\cos \mathrm{\&lambda;}\sin \mathrm{\&phi;}& -\sin \mathrm{\&lambda;}\sin \mathrm{\&phi;}& \cos \mathrm{\&phi;}\\ \cos \mathrm{\&lambda;}\cos \mathrm{\&phi;}& \sin \mathrm{\&lambda;}\cos \mathrm{\&phi;}& \sin \mathrm{\&phi;}\end{array}\right]---\left(24\right)$

式中λ，φ分别为载体位置的经度和纬度，可由伪距单点动态定位方法获取。

再利用旋转矩阵得到每条基线当地水平坐标系下的解与载体坐标系下解的函数关系式：

${\mathrm{dX}}_{\mathrm{Li}}^k=R_{313}^k{\mathrm{dX}}_{B,i}^k,i=2,\&CenterDot;\&CenterDot;\&CenterDot;,n---\left(25\right)$

$R_{312}^k=\left[\begin{array}{ccc}\cos \mathrm{\&alpha;}\cos \mathrm{\&gamma;}+\sin \mathrm{\&alpha;}\sin \mathrm{\&beta;}\sin \mathrm{\&gamma;}& \cos \mathrm{\&beta;}\sin \mathrm{\&gamma;}& \sin \mathrm{\&alpha;}\cos \mathrm{\&gamma;}-\cos \mathrm{\&alpha;}\sin \mathrm{\&beta;}\sin \mathrm{\&gamma;}\\ -\cos \mathrm{\&alpha;}\sin \mathrm{\&gamma;}+\sin \mathrm{\&alpha;}\sin \mathrm{\&beta;}\cos \mathrm{\&gamma;}& \cos \mathrm{\&beta;}\cos \mathrm{\&gamma;}& -\sin \mathrm{\&alpha;}\sin \mathrm{\&gamma;}-\cos \mathrm{\&alpha;}\sin \mathrm{\&beta;}\cos \mathrm{\&gamma;}\\ -\sin \mathrm{\&alpha;}\cos \mathrm{\&beta;}& \sin \mathrm{\&beta;}& \cos \mathrm{\&alpha;}\cos \mathrm{\&beta;}\end{array}\right]---\left(26\right)$

式中α，β，γ分别表示翻滚角、俯仰角和航向角。优选地，本发明中可以通过最小二乘估计来获取旋转矩阵R₃₁₂的九个矩阵元素，

并通过旋转矩阵

的九个矩阵元素来获取所述载体姿态信息的该历元初始值（α₀，β₀，γ₀），其中，

$\mathrm{\&alpha;}=\arctan ({-R}_{312}^k\left(\mathrm{3,1}\right)/R_{312}^k\left(\mathrm{3,3}\right)),\mathrm{\&beta;}=\arcsin \left(R_{312}^k\left(\mathrm{3,2}\right)\right),$

$\mathrm{\&gamma;}=\arctan (R_{312}^k\left(\mathrm{1,2}\right)/R_{312}^k\left(\mathrm{2,2}\right)).$

显然（25）式是一个关于α，β，γ的非线性表达式，若要用抗差自适应融合滤波进行参数估计，则需先进行线性化处理。利用九参数最小二乘可得到较精确的近似解α₀，β₀，γ₀，通过泰勒级数展开至一阶项，可得到（25）式的线性化表达式，下述表达式表示任意一个基线的现象表达式：

$A_i^k\mathrm{\&delta;}{\tilde{X}}^k-L_i^k=V_i^k,P_i^K---\left(27\right)$

其中，R₃₁₂显性化系数矩阵 $A_i^k=\left[\begin{array}{ccc}\frac{{\&PartialD;R}_{312}}{\&PartialD;\mathrm{\&alpha;}}{\mathrm{dX}}_{B,i}^k& \frac{\&PartialD;R_{312}}{\&PartialD;\mathrm{\&beta;}}{\mathrm{dX}}_{B,i}^k& \frac{\&PartialD;R_{312}}{\&PartialD;\mathrm{\&gamma;}}{\mathrm{dX}}_{B,i}^k\end{array}\right]---\left(28\right)$ 改进矩阵 $\stackrel{}{\mathrm{\&delta;}{\tilde{X}}^k}=\left[\begin{array}{c}\&PartialD;{\mathrm{\&alpha;}}^k\\ \&PartialD;{\mathrm{\&beta;}}^k\\ \&PartialD;{\mathrm{\&gamma;}}^k\end{array}\right]---\left(29\right)$ 量测矩阵 $L_i^k={[\mathrm{dX}}_{L,i}^k-B_i^k{\mathrm{dX}}_{B,i}^k]---\left(30\right)$ 残差矩阵 $B_i^k=R_{312}^k({\mathrm{\&alpha;}}_0,{\mathrm{\&beta;}}_0,{\mathrm{\&gamma;}}_0)---\left(31\right)$ 权矩阵 $P_i^k={\left({\mathrm{\&Sigma;}}_{{\mathrm{dX}}_{L,i}^k}\right)}^{-1}---\left(32\right)$

上述（27）-（32）式中表示

的协方差矩阵，

为残差。

对于n-1条独立基线，则其最小二乘解及协方差为：

$A_k\mathrm{\&delta;}{\tilde{X}}_k-L_k=V_k,P_k---\left(33\right)$

历元量测值的第一精度值为： ${\mathrm{\&Sigma;}}_{{\tilde{X}}_k}={\left[A_k^T{P}_k{A}_k\right]}^{-1}---\left(34\right)$

通过最小二乘解算法将（33）式求解可得历元量测值：

上式（33）中， $A_k=\left[\begin{array}{ccc}\frac{\&PartialD;R_{312}}{\&PartialD;\mathrm{\&alpha;}}{\mathrm{dX}}_{B,2}^k& \frac{\&PartialD;R_{312}}{\&PartialD;\mathrm{\&beta;}}{\mathrm{dX}}_{B,2}^k& \frac{{\&PartialD;R}_{312}}{\&PartialD;\mathrm{\&gamma;}}{\mathrm{dX}}_{B,2}^k\\ \&CenterDot;\&CenterDot;\&CenterDot;& \&CenterDot;\&CenterDot;\&CenterDot;& \&CenterDot;\&CenterDot;\&CenterDot;\\ \frac{\&PartialD;R_{312}}{\&PartialD;\mathrm{\&alpha;}}{\mathrm{dX}}_{B,i}^k& \frac{\&PartialD;R_{312}}{\&PartialD;\mathrm{\&beta;}}{\mathrm{dX}}_{B,i}^k& \frac{\&PartialD;R_{312}}{\&PartialD;\mathrm{\&gamma;}}{\mathrm{dX}}_{B,i}^k\\ \&CenterDot;\&CenterDot;\&CenterDot;& \&CenterDot;\&CenterDot;\&CenterDot;& \&CenterDot;\&CenterDot;\&CenterDot;\\ \frac{\&PartialD;R_{312}}{\&PartialD;\mathrm{\&alpha;}}{\mathrm{dX}}_{B,n}^k& \frac{\&PartialD;R_{312}}{\&PartialD;\mathrm{\&beta;}}{\mathrm{dX}}_{B,n}^k& \frac{\&PartialD;R_{312}}{\&PartialD;\mathrm{\&gamma;}}{\mathrm{dX}}_{B,n}^k\end{array}\right]---\left(35\right)$

$L_k=\left[\begin{array}{c}{\mathrm{dX}}_{L,2}^k-B_2^k{\mathrm{dX}}_{B,2}^k\\ \&CenterDot;\&CenterDot;\&CenterDot;\\ {\mathrm{dX}}_{L,i}^k-B_i^k{\mathrm{dX}}_{B,i}^k\\ \&CenterDot;\&CenterDot;\&CenterDot;\\ {\mathrm{dX}}_{L,n}^k-B_{n-1}^k{\mathrm{dX}}_{B,n}^k\end{array}\right]---\left(36\right)$

$P_k=\left[\begin{array}{ccc}{\mathrm{\&Sigma;}}_{{\mathrm{dX}}_{L,2}^k}& 0& 0\\ \&CenterDot;\&CenterDot;\&CenterDot;& \&CenterDot;\&CenterDot;\&CenterDot;& \&CenterDot;\&CenterDot;\&CenterDot;\\ 0& {\mathrm{\&Sigma;}}_{{\mathrm{dX}}_{L,i}^k}& 0\\ \&CenterDot;\&CenterDot;\&CenterDot;& \&CenterDot;\&CenterDot;\&CenterDot;& \&CenterDot;\&CenterDot;\&CenterDot;\\ 0& 0& {\mathrm{\&Sigma;}}_{{\mathrm{dX}}_{L,n}^k}\end{array}\right]---\left(37\right)$

上述实施例中表示第i条基线k历元的解算精度，在静态相对定位解算时获取；通过最小二乘估计对改正量

进行迭代计算，

直至小于某限定值为止如0.00001，以获取所述该历元量测值

其中，

(α₀,β₀,γ₀)为所述历元初始值，(α,β,γ)表征载体姿态信息，k为历元号，i为基线编号。

本申请上述实施例中，对前一历元最终值进行估值处理，以获取载体姿态信息的历元预报值的步骤的具体实施过程可以如下：

执行动态测量，可以根据前一历元的信息，利用状态方程来获取当前历元待估参数的先验信息及其协方差阵。假设采用常速度模型，则有：

历元预报值为 ${\stackrel{\&OverBar;}{X}}_k={\mathrm{\&Phi;}}_{k,k-1}{\hat{X}}_{k-1}+W_k---\left(38\right)$

历元预报值的第一精度值为： ${\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{X}}_k}={\mathrm{\&Phi;}}_{k,k-1}{\mathrm{\&Sigma;}}_{{\hat{X}}_{t-1}}{\mathrm{\&Phi;}}_{k,k-1}^T+{\mathrm{\&Sigma;}}_{W_t}---\left(39\right)$

上式（38）中，

${\stackrel{\&OverBar;}{X}}_k={\left[\begin{array}{cccccc}{\stackrel{\&OverBar;}{\mathrm{\&alpha;}}}_k& {\stackrel{\&OverBar;}{\mathrm{\&beta;}}}_k& {\stackrel{\&OverBar;}{\mathrm{\&gamma;}}}_k& {\stackrel{\&OverBar;}{V}}_{{\mathrm{\&alpha;}}_k}& {\stackrel{\&OverBar;}{V}}_{{\mathrm{\&beta;}}_k}& {\stackrel{\&OverBar;}{V}}_{{\mathrm{\&gamma;}}_k}\end{array}\right]}^T---\left(40\right)$

${\hat{X}}_{k-1}={\left[\begin{array}{cccccc}{\widehat{\mathrm{\&alpha;}}}_{k-1}& {\widehat{\mathrm{\&beta;}}}_{k-1}& {\widehat{\mathrm{\&gamma;}}}_{k-1}& {\hat{V}}_{{\mathrm{\&alpha;}}_{k-1}}& {\hat{V}}_{{\mathrm{\&beta;}}_{k-1}}& {\hat{V}\mathrm{\&gamma;}}_{k-1}\end{array}\right]}^T---\left(41\right)$

${\mathrm{\&Phi;}}_{k,k-1}=\left[\begin{array}{cccccc}1& 0& 0& \mathrm{\&Delta;t}& 0& 0\\ 0& 1& 0& 0& \mathrm{\&Delta;t}& 0\\ 0& 0& 1& 0& 0& \mathrm{\&Delta;t}\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]---\left(42\right)$

${\mathrm{\&Sigma;}}_{W_k}=\left[\begin{array}{cccccc}\frac{1}{3}{\mathrm{Q\&Delta;t}}^3& \frac{1}{2}Q{\mathrm{\&Delta;t}}^2& \frac{1}{2}Q{\mathrm{\&Delta;t}}^2& \frac{1}{2}\mathrm{Q\&Delta;}{t}^2& \frac{1}{2}Q{t}^2& \frac{1}{2}Q{\mathrm{\&Delta;t}}^2\\ \frac{1}{2}Q{\mathrm{\&Delta;t}}^2& \frac{1}{3}\mathrm{Q\&Delta;}{t}^3& \frac{1}{2}\mathrm{Q\&Delta;}{t}^2& \frac{1}{2}Q{\mathrm{\&Delta;t}}^2& \frac{1}{2}Q{t}^2& \frac{1}{2}Q{\mathrm{\&Delta;t}}^2\\ \frac{1}{2}Q{\mathrm{\&Delta;t}}^2& \frac{1}{2}\mathrm{Q\&Delta;}{t}^2& \frac{1}{3}\mathrm{Q\&Delta;}{t}^3& \frac{1}{2}\mathrm{Q\&Delta;}{t}^2& \frac{1}{2}\mathrm{Q\&Delta;}{t}^2& \frac{1}{2}\mathrm{Q\&Delta;}{t}^2\\ \frac{1}{2}Q{\mathrm{\&Delta;t}}^2& \frac{1}{2}Q{\mathrm{\&Delta;t}}^2& \frac{1}{2}\mathrm{Q\&Delta;}{t}^2& \mathrm{Q\&Delta;t}& \frac{1}{2}\mathrm{Q\&Delta;}{t}^2& \frac{1}{2}\mathrm{Q\&Delta;}{t}^2\\ \frac{1}{2}\mathrm{Q\&Delta;}{t}^2& \frac{1}{2}\mathrm{Q\&Delta;}{t}^2& \frac{1}{2}\mathrm{Q\&Delta;}{t}^2& \frac{1}{2}\mathrm{Q\&Delta;}{t}^2& \mathrm{Q\&Delta;t}& \frac{1}{2}\mathrm{Q\&Delta;}{t}^2\\ \frac{1}{2}\mathrm{Q\&Delta;}{t}^2& \frac{1}{2}\mathrm{Q\&Delta;}{t}^2& \frac{1}{2}\mathrm{Q\&Delta;}{t}^2& \frac{1}{2}Q{t}^2& \frac{1}{2}\mathrm{Q\&Delta;}{t}^2& \mathrm{Q\&Delta;t}\end{array}\right]---\left(43\right)$

式中，

和

分别表示历元k的历元预报值和历元k-1的历元最终值，Φ_k,k-1为状态转移矩阵，W_k为动态噪声，△t为历元间隔，V为姿态角变化的速度，Q为姿态角变化的速度功率谱密度。其中，历元最终值得初始值可以是历元初始值或历元量测值

本申请上述实施例中，对历元量测值和历元预报值进行抗差自适应融合滤波处理，以获取载体姿态信息的当前历元最终值的步骤的具体实施过程可以如下：

首先，可以通过状态方程

来获取历元最终值

其中， ${\stackrel{\&OverBar;}{X}}_k={\mathrm{\&Phi;}}_{k,k-1}{\hat{X}}_{k-1}+W_k,K_k={\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{X}}_k}{A}_k^T{(A_k{\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{X}}_k}{A}_k^T+P_k^{-1})}^{-1},{\mathrm{\&Sigma;}}_{{\hat{X}}_k}=[I-K_k{A}_k]{\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{X}}_k}.$

具体的，该基于抗差自适应融合滤波的递推解为：

${\hat{X}}_k={\stackrel{\&OverBar;}{X}}_k+K_k(L_k-A_k{\stackrel{\&OverBar;}{X}}_k)---\left(44\right)$

其中， $K_k={\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{X}}_k}{A}_k^T{(A_k{\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{X}}_k}{A}_k^T+P_k^{-1})}^{-1}---\left(45\right)$

${\mathrm{\&Sigma;}}_{{\hat{X}}_k}=[I-K_k{A}_k]{\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{X}}_k}---\left(46\right)$

由于上式（44）在实际实现过程中不便于研发人员编写代码，且含义不够直观，因此，可以通过矩阵反演算将得到滤波解： ${\hat{X}}_k={[A_k^T{P}_k{A}_k+P_{{\stackrel{\&OverBar;}{X}}_k}]}^{-1}[A_k^T{P}_k{L}_k+P_{{\stackrel{\&OverBar;}{X}}_k}{\stackrel{\&OverBar;}{X}}_k],$ 并获取历元最终值的第三精度 ${\mathrm{\&Sigma;}}_{{\hat{X}}_k}={(A_k^T{P}_k{A}_k+P_{{\stackrel{\&OverBar;}{X}}_k})}^{-1},$ 其中， $P_{{\stackrel{\&OverBar;}{X}}_k}={\left({\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{X}}_k}\right)}^{-1}.$

上述实施例实现利用矩阵反演，可以得到滤波解的另一种表达形式：

${\hat{X}}_k={[A_k^T{P}_k{A}_k+P_{{\stackrel{\&OverBar;}{X}}_k}]}^{-1}[A_k^T{P}_k{L}_k+P_{{\stackrel{\&OverBar;}{X}}_k}{\stackrel{\&OverBar;}{X}}_k]---\left(47\right)$

其中， ${\mathrm{\&Sigma;}}_{{\hat{X}}_k}={(A_k^T{P}_k{A}_k+P_{{\stackrel{\&OverBar;}{X}}_k})}^{-1}---\left(48\right)$

$P_{{\stackrel{\&OverBar;}{X}}_k}={\left({\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{X}}_k}\right)}^{-1}---\left(49\right)$

进一步，本发明通过状态方程 ${\hat{X}}_k={[A_k^T{\stackrel{\&OverBar;}{P}}_k{A}_k+{\mathrm{\&lambda;P}}_{{\stackrel{\&OverBar;}{X}}_k}]}^{-1}[A_k^T{\stackrel{\&OverBar;}{P}}_k{L}_k+\mathrm{\&lambda;}{P}_{{\stackrel{\&OverBar;}{X}}_k}{\stackrel{\&OverBar;}{X}}_k]$ 获取基于抗差自适应融合滤波算法的的历元最终值

具体的：

${\hat{X}}_k={[A_k^T{\stackrel{\&OverBar;}{P}}_k{A}_k+{\mathrm{\&lambda;P}}_{{\stackrel{\&OverBar;}{X}}_k}]}^{-1}[A_k^T{\stackrel{\&OverBar;}{P}}_k{L}_k+\mathrm{\&lambda;}{P}_{{\stackrel{\&OverBar;}{X}}_k}{\stackrel{\&OverBar;}{X}}_k]---\left(50\right)$

其中， ${\mathrm{\&Sigma;}}_{{\hat{X}}_k}={(A_k^T{\stackrel{\&OverBar;}{P}}_k{A}_k+{\mathrm{\&lambda;P}}_{{\stackrel{\&OverBar;}{X}}_k})}^{-1}---\left(51\right)$

上式（50）中，为降权因子，ε为自适应因子，它们的计算公式如下：

$\mathrm{\&lambda;}=\left\{\begin{array}{cc}1& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{X}}_t\right|\&le;c_0\\ \frac{c_0}{\left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{X}}_t\right|}{\left(\frac{c_1-\left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{X}}_t\right|}{c_1-c_0}\right)}^2,& c_0<\left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{X}}_t\right|\&le;c_1\\ 0& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{X}}_t\right|>c_1\end{array}\right.---\left(54\right)$

其中c₀和c₁为常数，一般的取值范围为c₀=1.0～1.5，c₁=3.0～4.5。

按下式求取： $\mathrm{\&Delta;}{\stackrel{\&OverBar;}{X}}_t=\left|\right|{\tilde{X}}_t-{\stackrel{\&OverBar;}{X}}_t\left|\right|/\sqrt{\mathrm{tr}\left\{{\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{X}}_t}\right\}}---\left(55\right)$

上述实施例中，在读取目标载体的载体姿态信息的历元初始值之前，方法还包括：通过九参数最小二乘算法对目标载体上的GNSS天线采集到的位置数据进行处理，以获取载体姿态信息的历元初始值。

按照上述实施方法，即可以实现利用抗差自适应融合滤波算法进行GNSS定姿。值得注意的是：我们且通过大量实验发现，由于动态测量中粗差出现十分的频繁，而抗差估计需要迭代计算其等价权，当数据采用率较高时，计算速度会明显下降；并且当一些历元的粗差过大时，还将导致滤波发散。因此我们采用了抗差估计与粗差探测相结合的办法来抵御异常历元量测值的影响，即在数据预处理阶段采用中位数法和较为宽松的判别标准剔除大的粗差，在参数解算阶段采用抗差估计来抑制小的粗差。这样既能有效控制异常历元量测值，又能保持软件的解算速度。

需要说明的是，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机系统中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。

图5是根据本发明实施例在静态实验中三参数迭代最小二乘法的静态定姿结果示意图；图6是根据本发明实施例在静态实验中抗差自适应融合滤波方法的静态定姿结果示意图；图7是根据本发明实施例在动态实验中飞机飞行的平面轨迹和高度变化示意图；图8是根据本发明实施例在动态实验中飞机航向角的变化示意图；图9是根据本发明实施例在动态实验中飞机俯仰角的变化示意图。

根据图5-图9所示，使用本发明的飞机的工作过程详细描述如下：

为对所提方法的正确性和可行性进行验证，本申请利用实测的静态和动态GNSS数据进行了定姿实验，实验数据来源于某次航空重力测量实验的机载GNSS数据。数据采样间隔为1秒，共有8670个历元。如图4所示，由于飞机的头部和尾部各安有一台GNSS接收机，因此该实施例仅举例航向角和俯仰角的变化情况。

一、静态实验

其中前298个历元的数据为飞机起飞前停在跑道上的量测数据，由于飞机处于相对静止状态，其姿态角理论上应该为一不变的常数，因此可以用来检验定姿结果的精度。我们分别采用三参数迭代最小二乘定姿法和基于抗差自适应融合滤波的定姿方法对其进行了解算，计算结果和精度统计情况如图5和6所示。具体的，静态定姿实验的精度统计情况如同下表1的结果。

表1静态定姿实验的精度统计情况（单位：度）

二、动态实验

为进一步检验所提方法的实际定姿效果，本申请对实际飞行阶段的量测数据进行了解算，并与机载的高精度INS定姿结果进行了做差比较，结果如图7-9所示。具体的，动态定姿实验的精度的统计结果如下表2的结果。

表2动态定姿实验的精度统计情况（单位：度）

通过上述静态和动态的GNSS定姿实验，可以看出该方法的定姿精度要明显优于现有GNSS定姿方法，证明了该方法的可行性和正确性。实验结果显示：本申请的定姿精度优于0.01度，可以满足绝大多数运动载体的实际定姿需求。由于本次实验数据的质量较好，因此现有方法的定姿结果没有出现异常误差情况。但若在恶劣量测环境下进行定姿，如：高楼林立的市区、森林等，本发明所提方法的优势将会体现得更加明显。

从以上的描述中，可以看出，本发明实现了如下技术效果：一是该方法具有更好的抗差性，因此能更好的满足恶劣量测环境下的GNSS定姿需求；二是充分合理的利用了载体的运动状态模型信息，因此对有用信息的使用上更加充分。当载体的实际运动状态与假设模型相符合时，该方法能提供更加准确可靠的定姿结果；而当载体的实际运动状态与假设模型不符时，该方法可通过自适应因子来减少或屏蔽假设模型的影响，同样能得到准确可靠的结果；三是当一些历元的实际量测数少于必要量测数时，现有方法都会出现无解的情况，而该方法可为用户提供准确可靠的预报解，保持了解的连续性。

由此可见，该方法在稳健性、可靠性、连续性以及精确性上都要优于现有的GNSS定姿方法。

显然，本领域的技术人员应该明白，上述的本发明的各模块或各步骤可以用通用的计算装置来实现，它们可以集中在单个的计算装置上，或者分布在多个计算装置所组成的网络上，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。这样，本发明不限制于任何特定的硬件和软件结合。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种卫星导航系统GNSS定姿测量值的处理方法，其特征在于，包括：

读取目标载体的载体姿态信息的历元初始值；

对所述历元初始值进行姿态量测处理，以获取所述载体姿态信息的历元量测值；

对前一历元最终值进行估值处理，以获取所述载体姿态信息的历元预报值；

对所述历元量测值和所述历元预报值进行抗差自适应融合滤波处理，以获取所述载体姿态信息的当前历元最终值，

其中，对所述历元初始值进行姿态量测处理，以获取所述载体姿态信息的历元量测值包括：通过GNSS静态相对定位算法来获取所述目标载体的多条基线在载体坐标系下的第一基线向量组

k为历元号，BB为载体坐标系；通过载波相对定位算法来获取所述目标载体的多条基线在地心坐标系下的第二基线向量组

EE表示地心坐标系；通过旋转矩阵

将所述第二基线向量组

转换成在当地水平坐标系下的第三基线向量组

LL表示当地水平坐标系，所述第三基线向量组

根据旋转矩阵

来获取所述第一基线向量组

与所述第三基线向量组的函数关系：

通过泰勒级数展开至一阶来将非线性函数

转换为线性函数 $A_k\mathrm{\&delta;}{\tilde{X}}_k-L_k=V_k\text{,}{P}_k,$ 其中，

$A_k=\left[\begin{array}{ccc}\frac{{\&PartialD;R}_{312}}{\&PartialD;\mathrm{\&alpha;}}{\mathrm{dX}}_{B,2}^k& \frac{{\&PartialD;R}_{312}}{\&PartialD;\mathrm{\&beta;}}{\mathrm{dX}}_{B,2}^k& \frac{{\&PartialD;R}_{312}}{\&PartialD;\mathrm{\&gamma;}}{\mathrm{dX}}_{B,2}^k\\ ...& ...& ...\\ \frac{{\&PartialD;R}_{312}}{\&PartialD;\mathrm{\&alpha;}}{\mathrm{dX}}_{B,i}^k& \frac{{\&PartialD;R}_{312}}{\&PartialD;\mathrm{\&beta;}}{\mathrm{dX}}_{B,i}^k& \frac{{\&PartialD;R}_{312}}{\&PartialD;\mathrm{\&gamma;}}{\mathrm{dX}}_{B,i}^k\\ ...& ...& ...\\ \frac{{\&PartialD;R}_{312}}{\&PartialD;\mathrm{\&alpha;}}{\mathrm{dX}}_{B,n}^k& \frac{{\&PartialD;R}_{312}}{\&PartialD;\mathrm{\&beta;}}{\mathrm{dX}}_{B,n}^k& \frac{{\&PartialD;R}_{312}}{\&PartialD;\mathrm{\&gamma;}}{\mathrm{dX}}_{B,n}^k\end{array}\right],L_k=\left[\begin{array}{c}{\mathrm{dX}}_{L,2}^k-B_2^k{\mathrm{dX}}_{B,2}^k\\ ...\\ {\mathrm{dX}}_{L,i}^k-B_i^k{\mathrm{dX}}_{B,i}^k\\ ...\\ {\mathrm{dX}}_{L,n}^k-B_{n-1}^k{\mathrm{dX}}_{B,n}^k\end{array}\right],$ $P_k=\left[\begin{array}{ccc}{\mathrm{\&Sigma;}}_{{\mathrm{dX}}_{L,2}^k}& 0& 0\\ ...& ...& ...\\ 0& {\mathrm{\&Sigma;}}_{{\mathrm{dX}}_{L,i}^k}& 0\\ ...& ...& ...\\ 0& 0& {\mathrm{\&Sigma;}}_{{\mathrm{dX}}_{L,n}^k}\end{array}\right],$

表示第i条基线k历元的解算精度，V_k为残差， $\mathrm{\&delta;}{\tilde{X}}_k={\left[A_k^T{P}_k{A}_k\right]}^{-1}\left[A_k^T{P}_k{L}_k\right],B_i^k=R_{312}^k({\mathrm{\&alpha;}}_0,{\mathrm{\&beta;}}_0,{\mathrm{\&gamma;}}_0),$ (α₀，β₀，γ₀)为所述历元初始值，(α,β,γ)表征载体姿态信息，k为历元号，i为基线编号；通过最小二乘解算法对所述

进行求解，以获取所述历元量测值

其中，所述为所述历元量测值的初始值。

2.根据权利要求1所述的方法，其特征在于，在读取目标载体的载体姿态信息的历元初始值之前，所述方法还包括：

实时读取所述目标载体的载体姿态信息的历元观测值；

对所述历元观测值进行估计处理，以获取载体姿态信息的所述历元初始值。

3.根据权利要求1所述的方法，其特征在于，在获取所述载体姿态信息的历元量测值之后，所述方法还包括：获取所述历元量测值的第一精度值

4.根据权利要求1或3所述的方法，其特征在于，对前一历元最终值进行估值处理，以获取所述载体姿态信息的历元预报值包括：

通过状态方程

来获取所述历元预报值

其中，状态转移矩阵 ${\mathrm{\&Phi;}}_{k,k-1}=\left[\begin{array}{cccccc}1& 0& 0& \mathrm{\&Delta;t}& 0& 0\\ 0& 1& 0& 0& \mathrm{\&Delta;t}& 0\\ 0& 0& 1& 0& 0& \mathrm{\&Delta;t}\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right],$ W_k为动态噪声，△t为历元间隔，

表示历元k-1的历元最终值，且历元最终值的初始值为所述历元量测值。

5.根据权利要求4所述的方法，其特征在于，在获取所述载体姿态信息的历元预报值之后，所述方法还包括：获取所述历元预报值的第二精度值

6.根据权利要求4所述的方法，其特征在于，对所述历元量测值和所述历元预报值进行抗差自适应融合滤波处理，以获取所述载体姿态信息的当前历元最终值包括：

通过状态方程

来获取所述历元最终值

其中，

${\stackrel{\&OverBar;}{X}}_k={\mathrm{\&Phi;}}_{k,k-1}{\hat{X}}_{k-1}+W_k,K_k={\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{X}}_k}{A}_k^T{(A_k{\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{X}}_k}{A}_k^T+P_k^{-1})}^{-1},{\mathrm{\&Sigma;}}_{{\hat{X}}_k}=[I-K_k{A}_k]{\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{X}}_k}.$

7.根据权利要求4所述的方法，其特征在于，对所述历元量测值和所述历元预报值进行抗差自适应融合滤波处理，以获取所述载体姿态信息的当前历元最终值包括：

通过抗差自适应融合滤波算法的状态方程：

${\hat{X}}_k={[A_k^T{\stackrel{\&OverBar;}{P}}_k{A}_k+\mathrm{\&epsiv;}{P}_{{\stackrel{\&OverBar;}{X}}_k}]}^{-1}[A_k^T{\stackrel{\&OverBar;}{P}}_k{L}_k+\mathrm{\&epsiv;}{P}_{{\stackrel{\&OverBar;}{X}}_k}{\stackrel{\&OverBar;}{X}}_k]$ 来获取所述历元最终值且 ${\mathrm{\&Sigma;}}_{{\hat{X}}_k}={(A_k^T{\stackrel{\&OverBar;}{P}}_k{A}_k+\mathrm{\&epsiv;}{P}_{{\stackrel{\&OverBar;}{X}}_k})}^{-1},$

其中，降权因子自适应因子 $\mathrm{\&epsiv;}=\left\{\begin{array}{cc}1& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{X}}_t\right|\&le;c_0\\ \frac{c_0}{\left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{X}}_t\right|}{\left(\frac{c_1-\left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{X}}_t\right|}{c_1-c_0}\right)}^2,& c_0<\left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{X}}_t\right|\&le;c_1,\\ 0& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{X}}_t\right|>c_1\end{array}\right.$ c₀和c₁为常数，取值范围为c₀=1.0～1.5，c₁＝3.0～4.5， $\mathrm{\&Delta;}{\stackrel{\&OverBar;}{X}}_t=\left|\right|{\tilde{X}}_t-{\stackrel{\&OverBar;}{X}}_t\left|\right|/\sqrt{\mathrm{tr}\left\{{\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{X}}_t}\right\}},$

为标准化残差。

8.根据权利要求7所述的方法，其特征在于，在获取所述载体姿态信息的当前历元最终值之后，所述方法还包括：获取所述历元最终值的第三精度

其中， $P_{{\stackrel{\&OverBar;}{X}}_k}={\left({\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{X}}_k}\right)}^{-1}.$

9.一种卫星导航系统GNSS定姿测量仪，其特征在于，包括：

读取单元，用于读取目标载体的载体姿态信息的历元初始值；

第一处理单元，用于对所述历元初始值进行姿态量测处理，以获取所述载体姿态信息的历元量测值；

第二处理单元，用于对前一历元最终值进行估值处理，以获取所述载体姿态信息的历元预报值；

第三处理单元，用于对所述历元量测值和所述历元预报值进行抗差自适应融合滤波处理，以获取所述载体姿态信息的当前历元最终值，

其中，所述第一处理单元还用于通过GNSS静态相对定位算法来获取所述目标载体的多条基线在载体坐标系下的第一基线向量组

k为历元号，BB为载体坐标系；通过载波相对定位算法来获取所述目标载体的多条基线在地心坐标系下的第二基线向量组

EE表示地心坐标系；通过旋转矩阵将所述第二基线向量组

转换成在当地水平坐标系下的第三基线向量组

LL表示当地水平坐标系，所述第三基线向量组

根据旋转矩阵来获取所述第一基线向量组

与所述第三基线向量组

的函数关系：

通过泰勒级数展开至一阶来将非线性函数 ${\mathrm{dX}}_{\mathrm{LL}}^k=R_{312}^k{\mathrm{dX}}_{\mathrm{BB}}^k$ 转换为线性函数 $A_k\mathrm{\&delta;}{\tilde{X}}_k-L_k=V_k,P_k,$ 其中，

$A_k=\left[\begin{array}{ccc}\frac{{\&PartialD;R}_{312}}{\&PartialD;\mathrm{\&alpha;}}{\mathrm{dX}}_{B,2}^k& \frac{{\&PartialD;R}_{312}}{\&PartialD;\mathrm{\&beta;}}{\mathrm{dX}}_{B,2}^k& \frac{{\&PartialD;R}_{312}}{\&PartialD;\mathrm{\&gamma;}}{\mathrm{dX}}_{B,2}^k\\ ...& ...& ...\\ \frac{{\&PartialD;R}_{312}}{\&PartialD;\mathrm{\&alpha;}}{\mathrm{dX}}_{B,i}^k& \frac{{\&PartialD;R}_{312}}{\&PartialD;\mathrm{\&beta;}}{\mathrm{dX}}_{B,i}^k& \frac{{\&PartialD;R}_{312}}{\&PartialD;\mathrm{\&gamma;}}{\mathrm{dX}}_{B,i}^k\\ ...& ...& ...\\ \frac{{\&PartialD;R}_{312}}{\&PartialD;\mathrm{\&alpha;}}{\mathrm{dX}}_{B,n}^k& \frac{{\&PartialD;R}_{312}}{\&PartialD;\mathrm{\&beta;}}{\mathrm{dX}}_{B,n}^k& \frac{{\&PartialD;R}_{312}}{\&PartialD;\mathrm{\&gamma;}}{\mathrm{dX}}_{B,n}^k\end{array}\right],L_k=\left[\begin{array}{c}{\mathrm{dX}}_{L,2}^k-B_2^k{\mathrm{dX}}_{B,2}^k\\ ...\\ {\mathrm{dX}}_{L,i}^k-B_i^k{\mathrm{dX}}_{B,i}^k\\ ...\\ {\mathrm{dX}}_{L,n}^k-B_{n-1}^k{\mathrm{dX}}_{B,n}^k\end{array}\right],$ $P_k=\left[\begin{array}{ccc}{\mathrm{\&Sigma;}}_{{\mathrm{dX}}_{L,2}^k}& 0& 0\\ ...& ...& ...\\ 0& {\mathrm{\&Sigma;}}_{{\mathrm{dX}}_{L,i}^k}& 0\\ ...& ...& ...\\ 0& 0& {\mathrm{\&Sigma;}}_{{\mathrm{dX}}_{L,n}^k}\end{array}\right],$ 表示第i条基线k历元的解算精度，V_k为残差， $\mathrm{\&delta;}{\tilde{X}}_k={\left[A_k^T{P}_k{A}_k\right]}^{-1}\left[A_k^T{P}_k{L}_k\right],B_i^k=R_{312}^k({\mathrm{\&alpha;}}_0,{\mathrm{\&beta;}}_0,{\mathrm{\&gamma;}}_0),$ (α₀，β₀，γ₀)为所述历元初始值，(α,β,γ)表征载体姿态信息，k为历元号，i为基线编号；通过最小二乘解算法对所述

进行求解，以获取所述历元量测值

其中，所述

为所述历元量测值的初始值。

Images