CN102998690B - 一种基于gps载波双差方程的姿态角直接求解方法 - Google Patents

Abstract

本发明提供一种基于GPS载波双差方程的姿态角直接求解方法。主要包括载波双差方程建立、将载体姿态信息引入载波双差方程中及利用非线性最小二乘解算姿态角。本发明本提供一种能够有效减少或消除短基线公共误差，实现高精度解算；将姿态角信息引入双差方程中，能够实现姿态角的直接解算，极大的减小了中间估计误差；运用非线性最小二乘的方法估计姿态角，提高***解算速度，完成姿态信息的实时性解算，使本发明更加适合实时性载体姿态测量。

Description

一种基于GPS载波双差方程的姿态角直接求解方法

技术领域

本发明涉及大型运载体的一种实时姿态测量方法，特别是涉及的是一种基于GPS载波双差方程的姿态角直接求解方法。

背景技术

为了满足不同应用平台对姿态信息的需求，目前已有大量的姿态测量设备问世，例如用于空间载体姿态测量的星敏感器、地平***、太阳敏感器、地磁仪等；用于为陆上或水下载体提供航向的磁罗盘、电磁罗盘；用于为各种飞行器、陆上运输设备、船舶、潜器、空间载体提供姿态信息的惯性器件等等。而这其中，星敏感器易受天气、地形或者其它客观因素的影响，不能实时地给出姿态或方位值；惯性导航设备结构复杂、价格昂贵，当工作时间较长时惯性器件误差会随时间积累而引起测姿精度的降低。

运用GPS定位信号进行载体姿态测量，只需要利用低成本的接收机即可以提供较高精度的姿态信息，以此来取代造价昂贵的传统测姿设备，同时还可以完成运载体的定位和授时，并且受环境的影响小，能够长时间进行高精度测姿任务。

GPS载体姿态测量是利用安装在载体固定位置上接收天线间的相对位置，以及坐标转换关系来确定姿态角。具体实现方法是，利用载体上安装的高精度定位***作为基准站，并在载体固定位置安装三条（或多条）接收天线，运用GPS信号测量天线与基站间的相对位置求出基线在WGS-84（World Geodetic System-84）系下的坐标，根据已知的基线载体系下坐标确定姿态旋转矩阵最终求出姿态角。

目前，GPS姿态解算算法主要是基于对定位结果的处理来获取姿态角，将常用的数学估计方法应用到姿态解算中，按解算结果分为求姿态矩阵和求欧拉角两类。

求解姿态矩阵的方法确定姿态角，是利用定位方程建立多颗卫星以及多个历元的定位方程组，通过求解超定方程完成基线坐标的估计，利用基线在不同坐标系中的坐标转换关系来估计出姿态矩阵，从而确定出姿态角。这一类方法中需要运用数学估计手段来实现基线坐标解算以及姿态角的确定。常用的方法包括：

（1）根据基线在地理系与载体系的坐标关系，利用姿态矩阵的正交性进行最优估计的TRIAD算法；

（2）基于Wahba问题的解算方法：如QUEST（QUaternion ESTimator）算法、FOAM（FastOptimal Attitude Matrix）算法、Euler-q算法、SVD（Singular Value Decomposition）算法等；

（3）利用多历元基线坐标解算的最小二乘直接估计法；

（4）利用伪距或载波观测量以及载体系下基线坐标，建立观测方程估计姿态矩阵的方法。

姿态角的直接估计方法一般是利用已知的天线位置与旋转姿态角的关系，分步求解姿态角。包括：使一条基线设置在载体的主轴方向，先求出偏航角和俯仰角，再利用第二条基线的旋转关系求出横滚角的两天线测姿法；利用两天线测姿公式确定偏航角和俯仰角，将另外天线经两次转动得到横滚角的多天线测姿法。

总的来讲，GPS测姿中存在如下问题：

1.运用数学估计方法对姿态矩阵进行估计中，并未直接得到姿态角结果，需要进行多历元的信息采集来完成最终姿态角计算，这一点一方面引入了进一步估计误差，另一方面也影响了姿态测量的实时性。

2.而运用直接测量方法进行姿态角测量，不需要对基线载体系坐标，也不用计算姿态矩阵，但所有天线基线的载体坐标z轴分量为零，坐标构成的矩阵不满秩，会导致姿态结果不可靠，精度远不及数学估计方法。

发明内容

本发明的目的在于提供一种能够提高估计过程精度，并实现实时测姿要求的一种基于GPS载波双差方程的姿态角直接求解方法。

本发明的目的是这样实现的：

一种基于GPS载波双差方程的姿态角直接求解方法包括以下步骤：

（1）采用三个GPS接收机测量天线采集GPS卫星信号获取载波观测信息

以直角处0号天线的载波观测信息作为基站信号；

（2）选取公共的可视卫星i，i＝1,2,…,M，并以可视卫星中仰角最大的卫星作为参考卫星（将其编号为1），接收机获取三天线载波观测信息后，以基站天线观测信息为基准，分别将各天线相对同一可见卫星的载波信号向基站信号作差获取单差载波相位观测量

n=0,1,2，然后对应不同天线，将各卫星单差载波信息向参考卫星的单差载波观测量作差得到双差载波观测量

（3）利用公式（1）所示载波观测量与姿态角的关系式，建立姿态角解算方程：

其中，

表示n号天线的双差载波观测量；λ表示载波波长，单位：m；

表示双差整周模糊度；

表示卫星i到参考的视线向量；R表示地理系到载体系的姿态矩阵；

表示地理系到地心系的姿态转换矩阵；y，r，p表示载体的三个姿态角，分别为航向角、横滚角、俯仰角，即方程未知数；b_n表示载体坐标系下基线向量，为已知量，单位：m。

（4）姿态测量***实时获取单历元载波信号进行双差处理，按步骤（1）-（4）实时计算载体的姿态信息。

本发明还可以包括：

所述的基于GPS载波双差方程的姿态角直接求解方法，其特征是：所述的天线采用直角正交式布局，即构成直角三角形平面，且安装距离在10m以上。

所述的基于GPS载波双差方程的姿态角直接求解方法，其特征是：所述的姿态角解算方程计算流程为：

1）由于式（1）等号右边整周模糊度已知，因此将方程（1）表示为：

y_t＝f(x)

其中，

表示整周模糊度求解后历元时刻t的双差载波观测量； $f\left(x\right)={\mathrm{\λ}}^{-1}\left[\begin{array}{c}-{(I_0^2-I_0^1)}^T\\ -{(I_0^3-I_0^1)}^T\\ ...\\ -{(I_0^M-I_0^1)}^T\end{array}\right]A_R^E{R}^T\left(x{b}_n\right)$ 表示以x为自变量的函数，x＝[y r p]^T。

2）给出初始姿态角

在初始姿态角处对方程f(x)进行一阶泰勒级数展开，令 $\mathrm{\Δx}={\left[\begin{array}{ccc}\mathrm{\Δy}& \mathrm{\Δr}& \mathrm{\Δp}\end{array}\right]}^T=\begin{array}{}\end{array}{\left[\begin{array}{ccc}y-\hat{y}& r-\hat{r}& p-\hat{p}\end{array}\right]}^T$ 为状态变量，方程简化为如下形式：

H·Δx＝Δy_t    （2）

式中，即第k次迭代中状态向量的估计结果x_k相对测量值

的偏差；观测矩阵H为：

$H=\left[\begin{array}{ccc}& .& \\ & .& \\ & .& \\ ...& -{(I_0^i-I_0^1)}^T{A}_R^E\left(\frac{{\∂R}^T}{\∂x}\right)b_n& ...\\ & .& \\ & .& \\ & .& \end{array}\right]$

其中，

表示姿态矩阵对各姿态角的偏导数。

3）采用最小二乘估计解超定方程（2），其解为：

Δx＝(H^TH)^-1H^TΔy_t            （3）

4）设定迭代截止门限，估计出姿态角终值x：

$x=\begin{array}{c}\left[\begin{array}{c}y\\ r\\ p\end{array}\right]\end{array}=\left[\begin{array}{c}\hat{y}+\mathrm{\Δy}\\ \hat{r}+\mathrm{\Δr}\\ \hat{p}+\mathrm{\Δp}\end{array}\right]---\left(4\right)$

本发明的方法的主要特点如下：

（1）GPS载波观测量能够提供高精度的定位信息，利用双差的方式建立载波方程更能够有效消除测姿基线两端的公共误差，大大提高姿态测量精度；

（2）将姿态角作为未知量引入双差测量方程中，减少了传统测姿方法的中间过程，能够有效降低估计误差，并且能够通过单历元信息解算，直接获取姿态角信息；

（3）非线性最小二乘估计算法的引入能够实现非线性超定方程的估计，使单历元快速解算得到实现，能大大提高载体姿态信息的实时性估计。

本发明的有益效果可以通过如下仿真加以验证：

1．姿态角解算仿真验证模型建立

姿态仿真验证模型的设计利用已知的卫星位置，通过设定主天线大地坐标、基线载体系下坐标以及预设姿态角即可进行姿态算法的验证。

验证***的设计包括以下几部分内容：

（1）卫星坐标获取

利用navcen.uscg.gov网站提供的GPS卫星实际播发过的历书文件，计算所有在轨卫星实时坐标，得到粗精度的卫星轨迹。根据设定用户位置完成所有卫星的仰角计算，得到可见卫星位置信息。

（2）载波信号模拟

设定主天线和参考天线的位置后，可以根据预定的姿态角得出所有天线的地心系坐标，即卫星至天线距离已知。通过加入一定的测量噪声即可实现各天线的载波信号模拟，完成载波观测方程的建立（***假设整周模糊度已知）。

（3）载体姿态结果验证

由模拟参数作为已知信息，根据不同的姿态解算方法建立方程，求解姿态角信息，再与预设姿态角进行对比，从而实现算法的验证。

在载体坐标系下建立三个天线的测姿阵列，分布情况如附图3所示。

设参考天线在载体旋转中心，卫星截至仰角为5°，其他仿真条件设定如表1所示。

表1仿真参数设置

2．姿态角解算方法实时性验证

设初始GPS时为0，采样100个历元，观察算法在载体静止、按固定角速率旋转以及随时间正弦旋转三种状态下的实时性解算效果。其中某个单历元迭代情况如附图4所示，发现经过4次循环迭代过程姿态结果达到稳定。100个解算历元验证结果如附图5所示，解算误差情况如附图6所示。

分析图6中姿态结果曲线，静态条件下，基于载波双差方程的直接姿态角解算方法结果精度可以达到10^-4度的数量级，且在仿真时间内均能有效解算；当载体以固定角速率旋转航向角随时间变化时，动态航向角精度同样达到10^-4度数量级；当载体俯仰角随时间按正弦规律变化时，解算精度达到了10^-2度数量级。

附图说明

图1为本发明的方法解算流程图；

图2为载波信号传播特性示图；

图3为本发明的姿态解算仿真天线分布情况；

图4为本发明单历元最小二乘迭代估计过程曲线；

图5a为本发明载体在静止状态姿态角解算结果曲线图；

图5b为本发明载体在固定角速率旋转状态姿态角解算结果曲线图；

图5c本发明载体正弦旋转状态姿态角解算结果曲线图；

图6a为本发明载体在静止状态姿态角解算误差曲线图；

图6b为本发明载体在固定角速率旋转状态姿态角解算误差曲线图；

图6c本发明载体在正弦旋转状态姿态角解算误差曲线图。

具体实施方式

下面结合附图举例对本发明做详细的描述：

1．姿态角直接求解算法实施流程

（1）建立基线向量与载波观测量关系

GPS载体三维姿态测量一般采用三根接收天线构成两条基线向量，因此本专利涉及的姿态测量***由三根天线构成，其中0号天线设为参考天线，1、2号天线为从天线。设三根天线同时跟踪M颗卫星，对于其中任意一颗卫星i，可以建立接收机对卫星的载波观测方程：

式中，表示天线n接收到第i颗星的载波观测量；λ表示载波波长，单位：m；

表示天线至卫星的实际距离，单位：m；表示电离层延迟,单位：m；

表示对流层延迟,单位：m；δt_n表示接收机钟差，单位：s；δtⁱ表示卫星钟差，单位：s；表示整周模糊度；

表示载波测量噪声。

对于每条基线建立单差方程以消除电离层延时、对流层延时，以及卫星钟差等误差项。由于基线长度远小于卫星，认为基线两端点的卫星视线向量相同，卫星信号传播如附图2所示。

根据单个接收机对卫星的载波观测方程建立两站对同一颗卫星i的单差观测方程：

进一步消除接收机钟差，建立三天线同时观测的另一颗卫星j的单差方程，联立两个单差方程作差得到双差载波观测方程：

把基线向量b_n0与双差载波观测方程联系起来。卫星到三个天线的视线向量为

则主从天线到卫星i的单差几何距离

（单位：m），等于基线向量在主天线对卫星i观测方向上投影长度的相反数，即：

$r_{n0}^i=-b_{n0}\·I_0^i---\left(4\right)$

因此，双差方程中双差几何距离与基线的关系可以表示成：

$r_{n0}^{\mathrm{ij}}=-b_{n0}\·I_0^i+b_{n0}\·I_0^i=-(I_0^i-I_0^j)\·b_{n0}---\left(5\right)$

则载波双差方程转变为：

式（6）给出了双差与基线向量b_n0之间的关系。式中，

是双差载波相位测量值，为已知量；b_n0是待求的三维基线向量，单位：m；

是双差整周模糊度，为未知整数。（2）建立姿态角解算模型

为了简化测姿算法解算步骤，避免中间过程引入的估计误差，提高解算精度，下面直接立姿态角与双差观测量的关系，方法如下：

设GPS姿态测量***天线与载体固联，即天线在载体系下坐标不变，且已知，分别为r_0，B，r_1,B，r_2,B。构成两个基线向量为b_n＝r_n,B-r_0,B，其中n=1,2。在地理坐标系下，

利用坐标系的转换关系，通过姿态矩阵将基线向量表示在载体系下，则式（6）转换为式（7）：

式中，R是地理系到载体系的姿态矩阵，

是地理系到地心系的姿态转换矩阵。

当双差整周模糊度确定后，方程中只有三个姿态角为未知量，利用（7）式中载波双差观测量与欧拉角的关系式，建立观测方程矩阵即可直接估计出姿态角。

对每条基线可以建立M-1个双差方程，方程矩阵形式为：

式（8）中，设1号卫星为参考卫星。根据姿态角与方程的关系将方程线性化，再通过最小二乘的方法对姿态角进行估计，最终得出稳定结果。

（3）最小二乘估计姿态角解算过程

三个旋转姿态角作为未知数，包含在姿态矩阵R中，姿态矩阵具有如下形式：

$R=\left(\begin{array}{ccc}\mathrm{CrCy}-\mathrm{SrSpSy}& \mathrm{CrSy}+\mathrm{SrSpCy}& -\mathrm{SrCp}\\ -\mathrm{CpSy}& \mathrm{CpCy}& \mathrm{Sp}\\ \mathrm{SrCy}+\mathrm{CrSpSy}& \mathrm{SrSy}-\mathrm{CrSpCy}& \mathrm{CrCp}\end{array}\right)---\left(9\right)$

式中，S表示sin；C表示cos；y、r、p分别为载体绕当地水平坐标系z轴转动的偏航角（Yaw）、绕y轴转动的横滚角（Roll）、绕x轴转动的俯仰角（Pitch）。由于模型对于姿态角有非线性关系，因此采用非线性最小二乘进行估计来确定姿态角。

认为式（8）等号右边整周模糊度已知，将方程表示为：

y_t＝f(x)

其中，表示整周模糊度求解后历元时刻t的双差载波观测量； $f\left(x\right)={\mathrm{\λ}}^{-1}\left[\begin{array}{c}-{(I_0^2-I_0^1)}^T\\ -{(I_0^3-I_0^1)}^T\\ ...\\ -{(I_0^M-I_0^1)}^T\end{array}\right]A_R^E{R}^T\left(x\right)b_n$ 表示以x为自变量的函数，x＝[y r p]^T。

设初始姿态角

在初始姿态角处对方程f(x)进行一阶泰勒级数展开，忽略高阶项。令 $\mathrm{\Δx}={\left[\begin{array}{ccc}\mathrm{\Δy}& \mathrm{\Δr}& \mathrm{\Δp}\end{array}\right]}^T=\begin{array}{}\end{array}{\left[\begin{array}{ccc}y-\hat{y}& r-\hat{r}& p-\hat{p}\end{array}\right]}^T$ 为状态变量，则非线性方程组可近似转化为以下用矩阵形式表达的线性方程组：

H·Δx＝Δy_t                （10）

式中，

即第k次迭代中状态向量的估计结果x_k相对测量值

的偏差。观测矩阵H表示为：

$H=\left[\begin{array}{ccc}& .& \\ & .& \\ & .& \\ ...& -{(I_0^i-I_0^1)}^T{A}_R^E\left(\frac{{\∂R}^T}{\∂x}\right)b_n& ...\\ & .& \\ & .& \\ & .& \end{array}\right]$

式中，

表示姿态矩阵对各姿态角的偏导数，形式如下：

$\frac{{\∂R}^T}{\∂y}=\left(\begin{array}{ccc}-\mathrm{CrSy}-\mathrm{CySpSr}& -\mathrm{CyCp}& -\mathrm{SrSy}+\mathrm{CySpCr}\\ \mathrm{CyCr}-\mathrm{SySpSr}& -\mathrm{SyCp}& \mathrm{CySr}+\mathrm{SySpCr}\\ 0& 0& 0\end{array}\right)$

$\frac{{\∂R}^T}{\∂r}=\left(\begin{array}{ccc}-\mathrm{CySr}-\mathrm{SySpCr}& 0& \mathrm{CyCr}-\mathrm{SySpSr}\\ -\mathrm{SySr}+\mathrm{CySpCr}& 0& \mathrm{SyCr}+\mathrm{CySpSr}\\ -\mathrm{CpCr}& 0& -\mathrm{CpSr}\end{array}\right)$

$\frac{{\∂R}^T}{\∂p}=\left(\begin{array}{ccc}-\mathrm{SyCpSr}& \mathrm{SySp}& \mathrm{SyCpCr}\\ \mathrm{CyCpSr}& -\mathrm{CySp}& -\mathrm{CyCpCr}\\ \mathrm{SpSr}& \mathrm{Cp}& -\mathrm{SpCr}\end{array}\right)$

则最小二乘解为：

Δx＝(H^TH)^-1H^TΔy_t                （11）

通过建立以上非线性最小二乘估计方程，在给定初始姿态角的条件下即可完成未知参数的估计。即，实现了利用载波双差观测方程直接估计得到姿态角。

2．姿态角直接求解方法误差情况分析

设所有卫星载波观测误差相同为且相互独立，均值为0，则双差测量值误差为

联立卫星载波双差观测方程，通过最小二乘方法进行超定方程求解。超定方程的一般形式为：

Ax＝b                （12）

则，存在唯一最小二乘解：

$A^H\mathrm{Ax}=A^{H}b\⇒x={\left(A^{H}A\right)}^{-1}{A}^{H}b$

式（12）中，b的误差为δb，A的误差为δA，它们对方程解的影响均与A的条件数的平方成正比，即超定方程的条件数将呈平方关系增大：

cond(A^HA)＝[cond(A)]²            （13）

根据误差传播规律，最小二乘解的误差即为

由于基于载波双差方程直接求解姿态角方法，只进行一步最小二乘估计即完成了姿态角的求取。由前面分析可知，向量x的估计误差为：

cov[Δx]＝E[ΔxΔx^T]＝(H^TH)^-1H^TE[ΔyΔy^T]H(H^TH)^-1    （14）

测量误差Δy的方差为

均值为0，且相互独立。因此，其中I为单位阵。

定义无量纲矩阵则

因此，姿态角估计误差与测量误差以及测量矩阵、基线长度有关。其中，在不考虑其他误差因素的情况下基线越长精度越高；而由式（15）可知，观测矩阵只与卫星的几何位置有关，当几何分布越好，测量误差对姿态角的估计影响越小。

Claims (1)

1.一种基于GPS载波双差方程的姿态角直接求解方法，其特征在于包括以下步骤：

（1）采用三个GPS接收机测量天线采集GPS卫星信号获取载波观测信息

以直角处0号天线的载波观测信息作为基站信号；

（2）选取公共的可视卫星i，i=1,2,…,M，并以可视卫星中仰角最大的卫星作为参考卫星，接收机获取三天线载波观测信息后，以基站天线观测信息为基准，分别将各天线相对同一可见卫星的载波信号向基站信号作差获取单差载波相位观测量

n=0,1,2，然后对应不同天线，将各卫星单差载波信息向参考卫星的单差载波观测量作差得到双差载波观测量

（3）利用公式

所示载波观测量与姿态角的关系式，建立姿态角解算方程，其中，

表示n号天线的双差载波观测量；λ表示载波波长，单位：m；

表示双差整周模糊度；

表示卫星i到参考天线的视线向量；R表示地理系到载体系的姿态矩阵；

表示地理系到地心系的姿态转换矩阵；y，r，p表示载体的三个姿态角，分别为航向角、横滚角、俯仰角，即方程未知数；b_n表示载体坐标系下的基线向量，为已知量，单位：m；

（4）姿态测量***实时获取单历元载波信号进行双差处理，按步骤（1）-（4）实时计算载体的姿态信息；

所述的天线采用直角正交式布局，即构成直角三角形平面，且安装距离在10m以上；

所述的姿态角解算方程计算流程为：

1）由于公式

等号右边整周模糊度已知，因此将

表示为：

y_t=f(x)

其中，

表示整周模糊度求解后历元时刻t的双差载波观测量； $f\left(x\right)={\mathrm{\λ}}^{-1}\left[\begin{array}{c}{-(I_0^2-I_0^1)}^T\\ {-(I_0^3-I_0^1)}^T\\ ...\\ {-(I_0^M-I_0^1)}^T\end{array}\right]A_R^E{R}^T\left(x\right)b_n$ 表示以x为自变量的函数，x=[y r p]^T；

2）给出初始姿态角 $x_0={\left[\begin{array}{ccc}\hat{y}& \hat{r}& \hat{p}\end{array}\right]}^T,$ 在初始姿态角处对方程f(x)进行一阶泰勒级数展开，令 $\mathrm{\Δx}=\begin{array}{c}{\left[\begin{array}{ccc}\mathrm{\Δy}& \mathrm{\Δr}& \mathrm{\Δp}\end{array}\right]}^T={\left[\begin{array}{ccc}y-\hat{y}& r-\hat{r}& p-\hat{p}\end{array}\right]}^T\end{array}$ 为状态变量，方程简化为如下形式：

H·Δx=Δy_t

式中，

即第k次迭代中状态向量的估计结果x_k相对测量值

的偏差；观测矩阵H为：

$H=\left(\begin{array}{ccc}& \·& \\ & \·& \\ & \·& \\ ...& -{(I_0^i-I_0^1)}^T{A}_R^E\left(\frac{{\∂R}^T}{\∂x}\right)b_n& ...\\ & \·& \\ & \·& \\ & \·& \end{array}\right)$

其中，

表示姿态矩阵对各姿态角的偏导数；

3）采用最小二乘估计解超定方程H·Δx=Δy_t，其解为：

Δx=(H^TH)^-1H^TΔy_t

4）设定迭代截止门限，估计出姿态角终值x：

$x=\left[\begin{array}{c}y\\ r\\ p\end{array}\right]=\begin{array}{c}\left[\begin{array}{c}\hat{y}+\mathrm{\Δy}\\ \hat{r}+\mathrm{\Δr}\\ \hat{p}+\mathrm{\Δp}\end{array}\right].\end{array}$

Images