CN102508275B - 多天线gps/gf-ins深度组合定姿方法 - Google Patents

Abstract

本发明公开了一种多天线GPS/GF-INS深度组合定姿方法：多天线GPS/GF-INS系统首次运行时，首先由GPS独自工作，独立解算位置、速度以及姿态角；GPS工作稳定后，运用GPS输出的位置、速度和载体的姿态角对GF-INS进行位置、速度以及姿态角初始化；GF-INS初始化完成后，即正常工作开始导航计算，所述多天线GPS/GF-INS系统按预定方法输出位置、速度和姿态角。本发明结构简单，可以大大提高GPS双差整周模糊度的搜索成功率，进而提高GPS系统的姿态测量精度和整个组合导航系统的导航精度和抗干扰能力。

Description

多天线GPS/GF-INS深度组合定姿方法

技术领域

本发明涉及组合导航定位定姿领域，具体涉及一种多天线GPS/GF-INS深度组合定姿方法。

背景技术

随着小型无人机、灵巧炮弹、短程导弹等高动态的小型武器对高精度、抗干扰、高可靠性和低成本等导航性能的需求不断提高，而到目前为止，任何单一的导航系统都无法完全满足此需求，因此，能够实现优势互补，进而提高系统的精度、可靠性以及稳定性的组合导航系统成为目前导航领域的研究热点。

惯性导航系统(INS)是一种可独立工作的推航系统。传统的惯性导航系统采用陀螺仪来敏感载体的姿态角速度，然后通过对姿态角速度进行积分来计算姿态角。由于姿态角速度误差会随着积分不断累加，因此，单纯的惯性导航系统很难实现长期高精度的定姿。

无陀螺惯性导航系统(GF-INS)不包含陀螺仪，成本较传统惯性导航系统要低，但加速度计的安装较为复杂，且姿态角的计算也是通过积分的方法，因此同样具有误差随时间累积的缺点。

全球定位系统(GPS)是一种在全球范围内，全天候、实时、连续和高精度的导航系统，已在军事和民用领域获得了广泛应用。多天线(至少3个)双差技术是GPS进行姿态角估计的常用方法之一。

目前，单频、单历元姿态解算成为实时姿态测量系统的研究热点和难点。要实现GPS单历元实时定姿，关键是准确求解该历元的整周模糊度。

带约束的最小二乘降相关平差(CLAMDA)算法是目前最有效的整周模糊度估计算法，但其只能将高精度的浮点解映射到正确的整数解，在浮点解精度不高时，往往会出现错误。针对INS和GPS分别在姿态角解算中存在的以上问题，以及二者在定位定姿方面的优势互补，国内外不少相关的研究者在GPS/INS组合测姿方面开展了研究。

2001年，卡尔加里大学的Yunchun Yang等人提出一种IMU辅助的GPS单频单历元定姿算法，该算法核心思想是利用IMU输出的俯仰角和横滚角对整周模糊度的搜索空间进行约束，从而减少搜索空间的大小。但此算法中俯仰角和横滚角的解算完全依靠IMU，对IMU的精度要求很高，且双差整周模糊度直接根据载波相位残差的平方和最小来确定，很难保证搜索的成功率。

2003年，Yunchun Yang等人又提出一种两天线GPS/INS测姿方法，将由惯性导航系统和GPS系统分别计算出的载波相位、多普勒频率和短基线矢量做差，利用扩展卡尔曼滤波对惯导输出的位置、速度、姿态角以及陀螺和加速度计误差进行估计并补偿。当GPS信号较好时，此方法可显著提高整个系统的定位定姿精度；而当GPS信号较弱时，GPS双差载波相位整周数的估计成功率无法保证，此方法则黯然失色。

2004年，查月等人提出利用信息融合技术(具体应用为联合卡尔曼滤波技术)将GPS和SINS(捷联式惯性导航系统)进行信息融合，在GPS载波相位观测量精度较高的前提下，可以提高整个系统的姿态角测量精度，但是当GPS载波相位观测量精度不高时，姿态角的计算可能出现的错误结果，不仅会影响整个组合导航系统的姿态角测量精度，而且由于信息融合后对SINS进行了反馈矫正，因此，还会影响到SINS下一时刻的导航计算。

2006年，哈尔滨工业大学的卢鸿谦等人提出一种单天线/GPS组合测姿方法，此方法结构简单、成本低，但单天线测姿只能给出协调飞行(即侧滑角为零)情况下的载体姿态，不能实时提供在任意飞行条件下的姿态信息。因此，其应用范围不如多天线定姿系统广泛。

综上所述，目前已有GPS/INS组合测姿方法均着眼于对INS输出姿态角进行补偿，没有解决GPS信号较弱时双差整周模糊度搜索成功率较低的问题。成本较传统惯性导航系统更低的GF-INS系统，由于复杂配置方案下，加速度计的安装和校准难度大，简单配置方案不能全部给出载体的位置、速度和姿态角信息，使得有关GPS/GF-INS组合导航系统的研究和应用受到了限制。

发明内容

本发明的目的在于克服传统多天线GPS/INS定姿方法的不足，而提供一种多天线GPS/GF-INS定姿系统及深度组合定姿方法。

根据本发明的一个方面，提供一种多天线GPS/GF-INS深度组合定姿方法，多天线GPS/GF-INS系统首次运行时，首先由多天线GPS独自工作，利用单频、单历元姿态解算算法求解出载体的三个姿态角；在三天线GPS工作稳定后，应用主天线输出的经度、纬度和高度对GF-INS进行位置初始化，利用GPS输出的三个天线的速度对GF-INS进行速度初始化，利用GPS输出的航向角、俯仰角和横滚角对GF-INS进行姿态角初始化；GF-INS初始化完成后，即正常工作开始导航计算，所述多天线GPS/GF-INS系统按预定方法输出位置、速度和姿态角。

所述GPS天线为3个，包括主天线1，副天线2、3；所述GF-INS包括6个加速度计A1、A2、A3、A4、A5、A6；其中加速度计A1、A4、A6与主天线1安装在一起，A2和A5与副天线2安装在一起，A3与副天线3安装在一起；加速度计A1、A2、A3的敏感方向沿载体坐标系z轴正向，A4和A5的敏感方向沿载体坐标系x轴正向，A6的敏感方向沿载体坐标系y轴正向；所述三根天线分别固连在三个载体上；固连主天线1、副天线2的载体连成的基线与载体纵轴平行，称为主基线；固连主天线1、副天线3的载体连成的基线与载体横轴平行，称为副基线。

所述单频单历元姿态解算算法的步骤具体包括：

(1)GNSS建模：建立卫星的双差载波相位观测方程和码伪距观测方程；

(2)浮点解计算：利用最小二乘法计算基线矢量和载波整周模糊度的浮点解及其方差协方差矩阵；

(3)载波整周模糊度整数解搜索：利用基线约束的最小二乘降相关平差算法搜索载波整周模糊度的整数解；

(4)基线矢量求解：利用最小二乘法由求解出的载波整周模糊度的整数解，解算出基线矢量在地理坐标系下的表达式；

(5)姿态角求解：重复步骤(1)-(4)，依次求解主基线矢量和副基线矢量在地理坐标系下的表达式，再由多天线GPS定姿原理求解出载体的三个姿态角。

所述建立卫星的双差载波相位观测方程和码伪距观测方程的步骤包括：

建立选定的某一短基线对于每一颗可观测卫星的单差载波相位观测方程和码伪距观测方程；

选定仰角最大的卫星作为参考星，建立双差载波相位观测方程和码伪距观测方程。

所述GF-INS导航计算包括：

速度更新：通过GF-INS的加速度计输出GPS的天线安装点相应敏感方向的加速度，利用所述加速度对时间的积分值对GF-INS对应位置对应方向的速度进行更新；

位置更新：对更新后的主天线位置的速度进行积分，对GF-INS进行位置更新；

姿态角更新：通过更新后的主天线和副天线相应敏感方向的速度，对基线矢量在地理坐标系相应坐标轴上的投影的线段长度进行更新，进而对三个姿态角进行更新。

所述多天线GPS/GF-INS系统按预定方法输出位置、速度和姿态角包括：

当GPS可观测卫星个数少于2颗时，直接输出GF-INS的位置、速度和姿态角；当GPS可观测卫星个数大于或等于2颗时，GF-INS和GPS系统按预定深度组合方法进行定姿。

所述按预定深度组合方法进行定姿的步骤包括：

当可观测卫星小于4颗时，利用GF-INS输出的位置和速度给GPS的位置和速度赋值；

当可观测卫星大于或等于4颗时，首先利用GPS定位原理解算接收机三个天线的位置和速度，其次通过GF-INS辅助GPS定姿，输出定姿势后的位置、速度和姿态角。

所述通过GF-INS辅助GPS定姿的步骤包括：

建立GF-INS辅助的GPS双差载波相位观测方程；

利用最小二乘法计算基线矢量和载波整周模糊度浮点解及其方差协方差矩阵；

根据所述单频单历元姿态解算算法的步骤(3)-(5)进行载波整周模糊度整数解搜索、基线矢量求解和姿态角计算；

通过组合滤波对GF-INS输出的位置、速度和姿态角反馈矫正后输出。

所述建立GF-INS辅助的GPS双差载波相位观测方程步骤包括：

建立对于选定的某一个短基线的双差载波相位和双差码伪距方程；

由GF-INS输出的姿态角求解出姿态矩阵，结合已知两基线矢量在载体坐标系下的表达式，建立基线矢量从地理坐标系到载体坐标系的转换方程；

联立所述基线矢量转换方程和双差载波相位及双差码伪距方程，建立GF-INS辅助的GPS观测方程。

所述通过组合滤波对GF-INS输出的位置、速度和姿态角反馈矫正后输出的步骤包括：

将GPS和GF-INS输出的位置、速度和姿态角分别做差，作为观测量进行扩展卡尔曼滤波；

估计GF-INS输出的位置、速度和姿态角的误差，对GF-INS输出的位置、速度和姿态角进行反馈矫正；

输出矫正后的位置、速度和姿态角。

本发明与现有技术相比的有益效果体现在：

(1)与现有的INS辅助多天线GPS定姿方法相比，本发明采用不含陀螺仪的GF-INS系统对多天线GPS定姿进行辅助，进一步降低了成本。

(2)本发明提出了一种易于安装的GF-INS加速度计配置方案，将加速度计直接安装在GPS的三个天线上，并设计了针对此方案的姿态角解算方法，即克服了传统加速度计配置方案安装难度大的问题，也解决了简单配置方案下，姿态角的解算需要借助外界信息辅助的难题。

(3)本发明将由GF-INS建立的基线矢量转换方程和双差载波相位及码伪距观测方程联合在一起，增加了观测方程的个数，使得在GPS的可观测卫星个数极少(大于或等于2颗)的情况下，GPS就能够利用单频、单历元算法进行姿态角解算，提高了GPS系统的抗干扰能力；另外，由于增大了方程对未知数的冗余度，也可提高整周模糊度和基线矢量浮点解的精度，从而提高载波整周模糊度的搜索成功率。

(4)本发明采用扩展卡尔曼滤波进行信息融合，建立了不同于传统GPS/INS组合导航系统的系统方程，对GF-INS输出的位置、速度和姿态角进行了误差估计并补偿，一方面抑制了GF-INS系统的误差累积，同时也提高了整个系统的位置、速度和姿态角精度。

设计了一种易于安装的GF-INS加速度计配置方案和针对此配置方案的姿态角计算方法，即降低了成本和加速度计安装复杂度，又解决了简单配置方案的GF-INS需要外界信息辅助才能计算出载体姿态角的问题；利用输出频率较高的GF-INS系统输出的姿态角信息，增加GPS双差观测方程对未知数的冗余度，不仅可通过提高整周模糊度和基线矢量浮点解的精度来提高整周模糊度搜索成功率，而且克服了多天线GPS系统在卫星颗数少于4颗时无法定姿的缺点；利用GPS系统全球全天候的优点，对GF-INS系统进行实时校正，从而抑制了GF-INS系统误差的累积。

附图说明

图1是本发明的GF-INS加速度计和GPS多天线的安装示意图；

图2是本发明的多天线GPS/GF-INS深度组合定姿方法的流程图；

图3是本发明的基于GPS的单频单历元姿态解算算法的流程图；

图4是本发明的GF-INS辅助的GPS单频单历元姿态解算算法的流程图；

图5是本发明的载体航向角示意图。

具体实施方式

如图1所示，本发明实施例中，GF-INS采用一种易于安装的加速度计配置方案，其包括6个加速度计A1、A2、A3、A4、A5、A6；GPS天线为3个：主天线1，副天线2、3，；加速度计A1、A4、A6与主天线1安装在一起，A2和A5与副天线2安装在一起，A3与副天线3安装在一起；加速度计A1、A2、A3的敏感方向沿载体坐标系z轴正向，A4和A5的敏感方向沿载体坐标系x轴正向，A6的敏感方向沿载体坐标系y轴正向；所述三根天线分别固连在三载体上；固连主天线1、副天线2的载体连成的基线与载体纵轴平行，称为主基线；固连主天线1、副天线3的载体连成的基线与载体横轴平行，称为副基线，主基线和副基线长度均为L。

以下结合该GF-INS加速度计与GPS天线的安装方法，对本发明作说明如下：

参见图2所示，该图示出了本发明的多天线GPS/GF-INS深度组合定姿方法的详细流程示意图；

步骤一：GPS系统独自进行位置、速度和姿态角解算；

当多天线GPS/GF-INS系统首次开始工作时(k＝0)，首先由GPS系统独自进行位置、速度和姿态角的解算。其中，姿态角的解算利用单频单历元姿态解算算法。假设可观测的卫星个数为m，其中m＞＝4，参见图3，所述单频、单历元姿态解算算法的步骤如下：

1、GNSS建模：建立卫星的双差载波相位和码伪距观测方程；

其具体实施过程如下：

1.1建立每一颗卫星的单差载波相位和码伪距观测方程；

以1、2两个天线为端点的短基线为例，其对于第i颗卫星的单差载波相位观测方程和单差码伪距观测方程分别如式(1)和式(2)：

${\mathrm{\ρ}}_{12}^i=s^i\·b+{\mathrm{\β}}_{12}^{\mathrm{\ρ}}+{\mathrm{\μ}}_{12}^i---\left(2\right)$

其中，

是1、2两个天线到第i颗卫星的单差载波相位的小数部分，

是1、2两个天线到第i颗卫星的单差码伪距，

是待估计的单差整周模糊度，si是接收机到卫星i的单位矢量，

和

分别为两个接收机钟差引起的载波相位和码伪距观测误差，

和

是观测噪声，λ为载波波长。对于m颗卫星而言，则有2m个单差方程。

1.2建立双差载波相位和码伪距观测方程；

假设卫星1仰角最大，作为参考星，则可以得到参考星的单差方程：

${\mathrm{\ρ}}_{12}^1=s^1\·b+{\mathrm{\β}}_{12}^{\mathrm{\ρ}}+{\mathrm{\μ}}_{12}^1---\left(4\right)$

式(3)和式(1)以及式(4)和式(2)分别相减后，可得出2(m-1)个双差方程，如下：

${\mathrm{\ρ}}_{12}^{i1}=(s^i-s^1)\·b+v_{12}^{i1}---\left(6\right)$

将多颗卫星的载波相位观测方程和码伪距观测方程表示成矩阵形式为：

$y_D^{\mathrm{\φ}}=\left[\begin{array}{cc}H& -I\end{array}\right]\left[\begin{array}{c}b\\ N_{\mathrm{DD}}\end{array}\right]+v_D^{\mathrm{\φ}},$ $v_D^{\mathrm{\φ}}~N\left(\mathrm{0,2}{\mathrm{\σ}}_{\mathrm{\φ}}^{2}Q\right)---\left(7\right)$

$y_D^{\mathrm{\ρ}}=H\·b+{\mathrm{\μ}}_D^{\mathrm{\ρ}},$ ${\mathrm{\μ}}_D^{\mathrm{\ρ}}~N\left(\mathrm{0,2}{\mathrm{\σ}}_{\mathrm{\ρ}}^{2}Q\right)---\left(8\right)$

其中， $y_D^{\mathrm{\φ}}=\left[\begin{array}{c}{\mathrm{\φ}}_{\mathrm{AB}}^{21}\\ {\mathrm{\φ}}_{\mathrm{AB}}^{31}\\ \·\\ \·\\ \·\\ {\mathrm{\φ}}_{\mathrm{AB}}^{m1}\end{array}\right],$ $y_D^{\mathrm{\ρ}}=\left[\begin{array}{c}{\mathrm{\ρ}}_{12}^{21}\\ {\mathrm{\ρ}}_{12}^{31}\\ \·\\ \·\\ \·\\ {\mathrm{\ρ}}_{12}^{m1}\end{array}\right],$ ${\mathrm{\μ}}_D^{\mathrm{\ρ}}=\left[\begin{array}{c}{\mathrm{\μ}}_{12}^{21}\\ {\mathrm{\μ}}_{12}^{31}\\ \·\\ \·\\ \·\\ {\mathrm{\μ}}_{12}^{m1}\end{array}\right],$ $N_{\mathrm{DD}}=\left[\begin{array}{c}{N}_{12}^{21}\\ N_{12}^{31}\\ \·\\ \·\\ \·\\ N_{12}^{m1}\end{array}\right],$ $v_D^{\mathrm{\φ}}=\left[\begin{array}{c}{v}_{12}^{21}\\ v_{12}^{31}\\ \·\\ \·\\ \·\\ v_{12}^{m1}\end{array}\right],$ $H=\frac{1}{\mathrm{\λ}}\left[\begin{array}{c}{\left(s^2\right)}^T-{\left(s^1\right)}^T\\ {\left(s^3\right)}^T-{\left(s^1\right)}^T\\ \·\\ \·\\ \·\\ {\left(s^m\right)}^T-{\left(s^1\right)}^T\end{array}\right].$

Q＝E_m-1+I_m-1，E_m-1为m-1阶全一矩阵，I_m-1为m-1阶单位矩阵。

联合式(7)和式(8)，可得载波相位观测方程和码伪距观测方程的组合形式为：

$\left[\begin{array}{c}{y}_D^{\mathrm{\φ}}\\ y_D^{\mathrm{\ρ}}\end{array}\right]=\left[\begin{array}{cc}H& -I\\ H& 0\end{array}\right]\left[\begin{array}{c}b\\ N_{\mathrm{DD}}\end{array}\right]+\left[\begin{array}{c}{v}_D^{\mathrm{\φ}}\\ {\mathrm{\μ}}_D^{\mathrm{\ρ}}\end{array}\right]---\left(9\right)$

其中方程个数为2(m-1)个，未知数个数为(m+2)个，因此，只有m≥4，(9)式有解。

2、浮点解计算；利用最小二乘法计算基线矢量和载波整周模糊度的浮点解及其方差协方差矩阵；

其具体实施过程如下：

由于载波相位双差存在相关性，会增加求解的难度，因此在利用最小二乘方法前，先进行如下处理。方程(1)的矩阵数学模型如下：

其中，

单差模糊度向量 $N_{\mathrm{SD}}={\left(\begin{array}{ccc}\mathrm{\Δ}{N}_{12}^1& \·\·\·,& \mathrm{\Δ}{N}_{12}^m\end{array}\right)}^T,$ 观测噪声向量为

e＝(1，1，…，1)^T，接收机到卫星的设计矩阵如下：

$E=\frac{1}{\mathrm{\λ}}\left[\begin{array}{c}{\left(s^1\right)}^T\\ \·\\ \·\\ \·\\ {\left(s^m\right)}^T\end{array}\right]---\left(11\right)$

噪声的统计模型为：

为了削减钟差项

需进行Householder变换，具体如下：

$P_e=\sqrt{m}{e}_1,$ $P\≡I-\frac{{2\mathrm{uu}}^T}{u^{T}u},$ $u\≡e_1-\frac{1}{\sqrt{m}}e$

其中P∈R^m×m，e₁＝(1，0，…，0)^T，进一步整理有，

$P=\left[\begin{array}{cc}\frac{1}{\sqrt{m}}& \frac{e^T}{\sqrt{m}}\\ \frac{e}{\sqrt{m}}& I_{m-1}-\frac{{\mathrm{ee}}^T}{m-\sqrt{m}}\end{array}\right]\≡\left[\begin{array}{c}{p}^T\\ \stackrel{\‾}{p}\end{array}\right]---\left(13\right)$

其中， $\stackrel{\‾}{p}=\left[\begin{array}{cc}\frac{e}{\sqrt{m}}& I_{m-1}-\frac{{\mathrm{ee}}^T}{m-\sqrt{m}}\end{array}\right].$ (10)式两边左乘P矩阵，即有

将上式中不含钟差项的部分提取出来，则有

Householder变换并不改变噪声的统计特性，即

定义

J≡[-e，I_m-1]。根据如下关系：

$N_{\mathrm{DD}}={\left(\begin{array}{cccc}\mathrm{\Δ}{N}_{12}^2-\mathrm{\Δ}{N}_{12}^1,& \mathrm{\Δ}{N}_{12}^3-\mathrm{\Δ}{N}_{12}^1,& \·\·\·,& \mathrm{\Δ}{N}_{12}^m-\mathrm{\Δ}{N}_{12}^1\end{array}\right)}^T$

容易证明

$\stackrel{\‾}{p}=\mathrm{FJ},$ $\stackrel{\‾}{p}{N}_{\mathrm{SD}}={\mathrm{FJN}}_{\mathrm{SD}}={\mathrm{FN}}_{\mathrm{DD}}---\left(17\right)$

由(15)和(17)式可以得到

与(10)式类似，得到码伪距单差观测方程

$y^{\mathrm{\ρ}}=E\·b+e{\mathrm{\β}}_{12}^{\mathrm{\ρ}}+v_{12}^{\mathrm{\ρ}},v_{12}^{\mathrm{\ρ}}~N(0,{\mathrm{\σ}}_{\mathrm{\ρ}}^2{I}_{m-1})---\left(19\right)$

其中，单差码伪距观测向量为 $y^{\mathrm{\ρ}}={\left(\begin{array}{ccc}{\mathrm{\Δ\ρ}}_{12}^1& \·\·\·,& {\mathrm{\Δ\ρ}}_{12}^m\end{array}\right)}^T$

与(18)式类似，得到，

$\stackrel{\‾}{p}{y}^{\mathrm{\ρ}}=\stackrel{\‾}{p}\mathrm{Eb}+\stackrel{\‾}{p}{v}_{12}^{\mathrm{\ρ}}---\left(20\right)$

定义

通常该值取0.001，则有

其中，噪声的统计模型为

(21)式等价为如下形式：

(23)式可以看成如下形式：

Y＝Ax+w，w～Q_Y    (24)

其中

分别为接收机载波相位量测噪声。

其最小二乘解为：

$\hat{x}={\left(A^T{Q}_Y^{-1}A\right)}^{-1}{A}^T{Q}_Y^{-1}Y---\left(25\right)$

其方差协方差矩阵：

$Q_{\hat{x}}={\left(A^T{Q}_Y^{-1}A\right)}^{-1}---\left(26\right)$

3、载波整周数模糊度整数解搜索；

运用基线约束的最小二乘降相关平差算法CLAMDA搜索载波整周模糊度整数解。其中带有基线约束的GNSS数学模型是：

E(Y)＝AN+Bb，D(Y)＝Q_Y，N∈Zⁿ，b∈R³，||b||＝L    (27)

其中，L为精确测量的基线长度。

设 $\hat{X}=\left[\begin{array}{c}\hat{b}\\ \hat{N}\end{array}\right],$ $Q_{\hat{x}}=\left[\begin{array}{cc}{Q}_{\hat{b}}& Q_{\hat{b}\hat{N}}\\ Q_{\hat{N}\hat{b}}& Q_{\hat{N}}\end{array}\right],$ 则CLAMDA算法的目标函数是：

$\underset{N\∈Z^n}{\min }({\left|\right|\hat{N}-N\left|\right|}_{Q_{\hat{N}}}^2+\underset{b\∈R^3,\left|\right|b\left|\right|=L}{\min }{\left|\right|\hat{b}\left(N\right)-b\left|\right|}_{Q_{\hat{b}\left(N\right)}}^2)---\left(28\right)$

4、基线矢量求解；

利用最小二乘法由求解出的载波整周模糊度的整数解，解算出基线矢量在地理坐标系下的表达式；

将步骤3中求解出的N_DD带入(7)式可得：

可重新解出基线矢量在地理坐标系下的表达式的最小二乘解为：

5、求解姿态角；

重复步骤1-4，依次求解主基线矢量和副基线矢量在地理坐标系下的表达式，再由多天线GPS定姿原理求解出载体的三个姿态角。

具体实施步骤为：

重复步骤1～4，求解以天线1、3为端点的短基线矢量

为描述方便，分别记

和

则就可以通过如下方式直接计算姿态角：

${\mathrm{\ψ}}_G=-{\tan }^{-1}\left(\frac{{\hat{x}}_1^n}{{\hat{y}}_1^n}\right)---\left(31\right)$

${\mathrm{\θ}}_G={\tan }^{-1}\left(\frac{{\hat{z}}_1^n}{\sqrt{{\left({\hat{x}}_1^n\right)}^2+{\left({\hat{y}}_1^n\right)}^2}}\right)---\left(32\right)$

基线矢量坐标

绕地理坐标系Z轴旋转Ψ，再绕地理坐标系X轴方向旋转θ，得到坐标值

则可计算横滚角：

${\mathrm{\γ}}_G={-\tan }^{-1}\left(\frac{{\hat{z}}_2^{n^{\′\′}}}{{\hat{x}}_2^{n^{\′\′}}}\right)---\left(33\right)$

步骤二、GF-INS初始化；

当三天线GPS系统工作稳定之后，在k＝0时，应用三天线GPS系统输出的位置、速度和载体的姿态角对GF-INS系统进行初始化；

具体为：应用主天线输出的经度、纬度和高度对GF-INS进行位置初始化，利用GPS输出的三个天线的速度对GF-INS进行速度初始化，利用GPS输出的航向角、俯仰角和横滚角对GF-INS进行姿态角初始化；

初始化之后的GF-INS系统即可独立工作。所述GF-INS系统位置、速度、姿态角初始化过程如下：

λ_I1＝λ_G1，L_I1＝L_G1，h_I1＝h_G1

V_I1E＝V_G1E，V_I1N＝V_G1N，V_I1U＝V_G1U，V_I2N＝V_G2N，V_I2U＝V_G2U，V_I3U＝V_G3U    (34)

ψ_I＝ψ_G，θ_I＝θ_G，γ_I＝γ_G

其中，下标I和G分别指GF-INS和GPS，下标1，2，3代表三个天线，E、N、U代表东、北、天，λ、L、h、V分别指经度、纬度、高度和速度，ψ，θ，γ指航向角、俯仰角和横滚角。

GF-INS独立工作的流程如下：

1、速度更新：GF-INS是一个推航系统，其速度更新公式如下：

$V=V_0+{\∫}_0^t\mathrm{adt}---\left(35\right)$

其中，V₀指上一时刻速度，a指对应位置、对应方向的加速度。式(34)中的6个不同位置不同方向的速度均用式(35)进行更新。

2、位置更新：假设地球参考椭球体长半轴为Re，地球扁平系数为f，则卯酉圈上纬度为L的点的曲率半径为R_N＝R_e(1+fsin²L)，子午圈上纬度为L的点的曲率半径为R_M＝Re(1-2f+3fsin²L)。令

其中，h为地理高度(以下出现相同符号时，含义同上)。则，GF-INS的位置更新公式为：

${\mathrm{\λ}}_{I1}={\mathrm{\λ}}_0+{\∫}_0^t\frac{V_{1E}}{R_1\cos L_0}\mathrm{dt}$

$L_{I1}=L_0+{\∫}_0^t\frac{V_{1N}}{R_2}\mathrm{dt}---\left(36\right)$

$h_{I1}=h_0+{\∫}_0^t{V}_{1U}\mathrm{dt}$

其中，λ₀，L₀，h₀分别指上一时刻GF-INS的经度、纬度和高度。

3、姿态角更新：

载体的航向角是指载体纵轴在水平面的投影与地理子午线之间的夹角。若沿载体纵轴方向安装两个敏感方向均为地理北向的加速度计1、2，如图5所示，两加速度计1、2之间的线段长度已知，记为L。

若已知k时刻载体的航向角为ψ₀(0≤ψ₀≤π/2)，则可计算出k时刻两加速度计1、2之间的距离在地理北向的投影线段AB′的长度为Lcos(ψ₀)，若已知k+1时刻加速度计1和加速度计2安装点在地理北向的速度分别为V_1N，V_2N，则可计算出k+1时刻线段AB′的长度为：

|AB′|＝Lcos(ψ₀)+(V_1N-V_2N)T             (37)

其中，T为k+1时刻和k时刻的时间间隔。

再由反三角函数即可计算出k+1时刻的航向角。依此类推，可计算出任意时刻载体的航向角。

同样，已知载体的俯仰角和横滚角的初始值，也可用类似方法计算出载体在任意时刻的俯仰角和横滚角。

因此，可总结出GF-INS姿态角更新公式为：

${\mathrm{\ψ}}_I=\arccos \left(\frac{L\cos {\mathrm{\ψ}}_0+{\∫}_0^t(V_{I1N}-V_{I2N})\mathrm{dt}}{L}\right)$

${\mathrm{\θ}}_I=\arcsin \left(\frac{L\sin {\mathrm{\θ}}_0+{\∫}_0^t(V_{I1U}-V_{I2U})\mathrm{dt}}{L}\right)$

${\mathrm{\γ}}_I=\arcsin \left(\frac{L\sin {\mathrm{\γ}}_0+{\∫}_0^t(V_{I1U}-V_{I3U})\mathrm{dt}}{L}\right)---\left(38\right)$

其中，ψ₀，θ₀，γ₀分别指上一时刻载体的航向角、俯仰角和横滚角。

步骤三、位置、速度和姿态角输出；

GF-INS开始工作以后，当GPS可观测卫星个数少于2颗时，直接输出GF-INS的位置、速度和姿态角；

当GPS可观测卫星大于或等于2颗时，GF-INS和GPS系统将进行深度组合。

所述GF-INS/GPS深度组合方法的流程如下：

当可观测卫星小于4颗时，利用GF-INS输出的位置和速度给GPS系统的位置和速度赋值；

当可观测卫星大于或等于4颗时，先利用GPS定位原理分别解算接收机三个天线的位置和速度；其次通过GF-INS辅助GPS定姿(单频单历元算法解算姿态角)，输出定姿势后的位置、速度和载体的姿态角。

参见图4，所述通过GF-INS辅助GPS定姿的具体步骤如下：

1、建立GF-INS辅助的GPS双差载波相位观测方程；

其具体步骤为：

对于选定的某一短基线，首先，在t时刻由系统输出的GPS接收机位置、在t时刻k历元载波相位和码伪距观测值、以及t时刻GF-INS输出的姿态角建立基本的双差载波相位和双差码伪距方程；其次，由t时刻GF-INS输出的姿态角求解出姿态矩阵，再结合已知的两基线矢量在载体坐标系下的表达式，建立基线矢量从地理坐标系到载体坐标系的转换方程；最后，将所述基线矢量转换方程和双差载波相位及双差码伪距方程联立，建立GF-INS辅助的GPS观测方程，即扩大以后的GPS双差观测方程；

设天线1、2确定的基线矢量在地理坐标中的表达式为 $x_{12}^n={\left[\begin{array}{ccc}{x}^n& y^n& z^n\end{array}\right]}^T,$ 在载体坐标系中的表达式为 $x_{12}^b={\left[\begin{array}{ccc}{x}^b& y^b& z^b\end{array}\right]}^T.$ 对于短基线，以下关系式成立：

$x_{12}^b=c_n^b{x}_{12}^n---\left(39\right)$

其中， $C_n^b=\left[\begin{array}{ccc}\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}+\cos \mathrm{\ψ}\cos \mathrm{\γ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\cos \mathrm{\γ}& -\cos \mathrm{\θ}\sin \mathrm{\γ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& \sin \mathrm{\θ}\\ \cos \mathrm{\ψ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}& -\sin \mathrm{\ψ}\sin \mathrm{\θ}-\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right],$ 称为姿态矩阵。假设载体为刚体，则

固定不变，且可通过精确测量获得。将t时刻GF-INS输出的姿态角和通过精确测量得出的

代入式(25)，即可得基线矢量转换方程。为统一符号，且考虑观测噪声，将式(39)调整为：

$x_{12}^b=C_n^b\·b+\mathrm{\ϵ}---\left(40\right)$

其中，ε为载体坐标系下基线矢量的观测噪声。

将式(23)和式(9)联立可得GF-INS辅助的GPS单频单历元观测方程为：

2、利用最小二乘法计算基线矢量和载波整周模糊度浮点解及其方差协方差矩阵；

同(25)式，可计算出基线矢量和载波整周模糊度的浮点解为：

${\hat{x}}^{\′}={\left(A^{\′T}{Q}_Y^{\′-1}{A}^{\′}\right)}^{-1}{A}^{\′T}{Q}_Y^{\′-1}{Y}^{\′}---\left(42\right)$

其中， ${\hat{x}}^{\′}=\left[\begin{array}{c}b\\ N_{\mathrm{DD}}\end{array}\right],$ $A^{\′}=\left[\begin{array}{cc}\stackrel{\‾}{p}E& F\\ {\mathrm{\σ}}_1\stackrel{\‾}{p}E& 0\\ C_n^b& 0\end{array}\right],$

及其方差协方差矩阵：

$Q_{{\hat{x}}^{\′}}={\left(A^{\′T}{Q}_Y^{\′-1}{A}^{\′}\right)}^{-1}---\left(43\right)$

3、进行载波整周模糊度整数解搜索、基线矢量求解和姿态角计算；

所述载波整周模糊度整数解搜索、基线矢量求解和姿态角计算分别同步骤一中的步骤3、4、和5，不再描述。在搜索载波整周模糊度整数解后输出该整周模糊度整数解，并在k＝k+1历元时刻将该整数解输出作为GPS在t时刻k历元载波相位的码伪距观测值进行下一步的计算；

4、通过EKF组合滤波对GF-INS输出的位置、速度和姿态角反馈矫正后输出；

将上述GPS和GF-INS输出的位置、速度和姿态角分别做差，作为观测量进行扩展卡尔曼滤波，估计出GF-INS输出的位置、速度和姿态角的误差，对GF-INS进行反馈矫正，并输出矫正以后的位置、速度和姿态角；

采用扩展卡尔曼滤波(EKF)作为组合滤波器。组合滤波器的观测量为：

z＝[L_G-L_I λ_G-λ_I h_G-h_I θ_G-θ_I γ_G-γ_I ψ_G-ψ_I

V_E1G-V_E1I V_N1G-V_N1I V_N2G-V_N2I V_U1G-V_U1I V_U2G-V_U2I V_U3G-V_U3I]^T

其中，θ、γ、φ分别指载体的俯仰角、横滚角和航向角，V_e，V_n和V_u分别指东向、北向和天向速度。下标1，2，3指三个天线，G和I分别代表GPS和GF-INS。

组合滤波器的状态量为：

X＝[δL δλ δh δψ δθ δγ δV_1E δV_1N δV_2N δV_1U δV_2U δV_3U]^T

其中，δL，δλ，δh，δθ，δγ，δψ，δV_E，δV_N，δV_U分别指GF-INS的位置误差、姿态角误差和东北天三个方向的速度误差。

此组合滤波器的状态方程采用GF-INS的误差传播方程，具体的推导过程如下：

4.1位置和速度误差传播方程。

GF-INS的位置和速度误差传播方程的推导同传统INS，此处不再详述，直接给出位置和速度误差传播方程如下：

经度误差传播方程：

$\mathrm{\δ}\stackrel{\·}{\mathrm{\λ}}=\frac{\mathrm{\δ}{V}_{1E}\left(R_1\cos L\right)-\mathrm{\δ}{R}_1{V}_{1E}\cos L+\mathrm{\δL}{V}_{1E}{R}_1\sin L}{{\left(R_1\cos L\right)}^2}$ (44)

$=\mathrm{\δL}(\frac{V_{1E}}{R_1}\sec L\tan L-\frac{{2V}_{1E}\mathrm{Ref}\sin L}{R_1^2})+\mathrm{\δ}{V}_{1E}\frac{1}{R_1\cos L}-\mathrm{\δh}\frac{V_{1E}}{R_1^2\cos L}$

纬度误差传播方程：

$\mathrm{\δ}\stackrel{\·}{L}=\frac{\mathrm{\δ}{V}_{1N}{R}_2-\mathrm{\δ}{R}_2{V}_{1N}}{R_2^2}$ (45)

$=-\mathrm{\δL}\frac{3\mathrm{Ref}{V}_{1N}\sin 2L}{R_2^2}+\mathrm{\δ}{V}_{1N}\frac{1}{R_2}-\mathrm{\δh}\frac{V_{1N}}{R_2^2}$

高度误差传播方程：

$\mathrm{\δ}\stackrel{\·}{h}=\mathrm{\δ}{V}_{1U}---\left(46\right)$

东向速度误差传播方程：

$\mathrm{\δ}{\stackrel{\·}{V}}_{1E}=\mathrm{\δL}[2{\mathrm{\ω}}_{\mathrm{ie}}(V_{1N}\cos L+V_{1U}\sin L)+\frac{V_{1E}}{R_1}(\frac{V_{1N}}{{\cos }^{2}L}+\frac{2\mathrm{Ref}}{R_1}\sin L(V_{1U}\cos L-V_{1N}\sin L))]$

$+\mathrm{\δ}{V}_{1E}\left(\frac{V_{1N}\mathrm{tgL}-V_{1U}}{R_1}\right)+\mathrm{\δ}{V}_{1N}(2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_{1E}}{R_1}\mathrm{tgL})-\mathrm{\δ}{V}_{1U}(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_{1E}}{R_1})---\left(47\right)$

$+\mathrm{\δh}\frac{V_{1E}(V_{1U}-V_{1N}\mathrm{tgL})}{R_1^2}+a_{1N}\mathrm{\δ\ψ}-a_{1U}\mathrm{\δ\γ}$

北向速度误差传播方程：

$\mathrm{\δ}{\stackrel{\·}{V}}_{1N}=\mathrm{\δL}(\frac{3\mathrm{Ref}{V}_{1U}{V}_{1N}\sin 2L}{R_2^2}+\frac{2\mathrm{Ref}{V}_{1E}^2{\sin }^{2}L}{R_1^2}-\frac{V_{1E}^2{\sec }^{2}L}{R_1}-2{\mathrm{\ω}}_{\mathrm{ie}}{V}_{1E}\cos L)$

$+\mathrm{\δ}{V}_{1E}(-2{\mathrm{\ω}}_{\mathrm{ie}}\sin L-\frac{{2V}_{1E}}{R_1}\mathrm{tgL})+\mathrm{\δ}{V}_{1N}(-\frac{V_{1U}}{R_2})+\mathrm{\δ}{V}_{1U}(-\frac{V_{1N}}{R_2})+\mathrm{\δh}(\frac{V_{1E}^2}{R_1^2}\mathrm{tgL}+\frac{V_{1U}{V}_{1N}}{R_2^2})$

$+a_{1U}\mathrm{\δ\θ}-a_{1E}\mathrm{\δ\ψ}---\left(48\right)$

$\mathrm{\δ}{\stackrel{\·}{V}}_{2N}=\mathrm{\δL}(\frac{3\mathrm{Ref}{V}_{2U}{V}_{2N}\sin 2L}{R_2^2}+\frac{2\mathrm{Ref}{V}_{1E}^2{\sin }^{2}L}{R_1^2}-\frac{V_{1E}^2{\sec }^{2}L}{R_1}-2{\mathrm{\ω}}_{\mathrm{ie}}{V}_{1E}\cos L)$

$+\mathrm{\δ}{V}_{1E}(-2{\mathrm{\ω}}_{\mathrm{ie}}\sin L-\frac{{2V}_{1E}}{R_1}\mathrm{tgL})+\mathrm{\δ}{V}_{2N}(-\frac{V_{2U}}{R_2})+\mathrm{\δ}{V}_{2U}(-\frac{V_{2N}}{R_2})+\mathrm{\δh}(\frac{V_{1E}^2}{R_1^2}\mathrm{tgL}+\frac{V_{2U}{V}_{2N}}{R_2^2})$

$+a_{2U}\mathrm{\δ\θ}-a_{1E}\mathrm{\δ\ψ}---\left(49\right)$

天向速度误差传播方程：

$\mathrm{\δ}{\stackrel{\·}{V}}_{1U}=\mathrm{\δL}(-2{\mathrm{\ω}}_{\mathrm{ie}}{V}_E\sin L-\frac{\mathrm{Re}{\mathrm{fV}}_{1E}^2\sin 2L}{R_1^2}-\frac{3\mathrm{Re}{\mathrm{fV}}_{1N}^2\sin 2L}{R_2^2})$ (50)

$+\mathrm{\δ}{V}_{1E}(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{{2V}_{1E}}{R_1})+\mathrm{\δ}{V}_{1N}\left(\frac{{2V}_{2N}}{R_2}\right)+\mathrm{\δh}(-\frac{V_{1E}^2}{R_1^2}-\frac{V_{1N}^2}{R_2^2}+\frac{2g_0}{\mathrm{Re}})-a_{1N}\mathrm{\δ\θ}+a_{1E}\mathrm{\δ\γ}$

$\mathrm{\δ}{\stackrel{\·}{V}}_{2U}=\mathrm{\δL}(-2{\mathrm{\ω}}_{\mathrm{ie}}{V}_{1E}\sin L-\frac{\mathrm{Re}{\mathrm{fV}}_{1E}^2\sin 2L}{R_1^2}-\frac{3\mathrm{Re}{\mathrm{fV}}_{2N}^2\sin 2L}{R_2^2})$

$+\mathrm{\δ}{V}_{1E}(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{{2V}_{1E}}{R_1})+\mathrm{\δ}{V}_{2N}\left(\frac{{2V}_{2N}}{R_2}\right)+\mathrm{\δh}(-\frac{V_{1E}^2}{R_1^2}-\frac{V_{2N}^2}{R_2^2}+\frac{2g_0}{\mathrm{Re}})---\left(51\right)$

$-a_{2N}\mathrm{\δ\θ}+a_{1E}\mathrm{\δ\γ}$

$\mathrm{\δ}{\stackrel{\·}{V}}_{3U}=\mathrm{\δL}(-2{\mathrm{\ω}}_{\mathrm{ie}}{V}_{1E}\sin L-\frac{\mathrm{Re}{\mathrm{fV}}_{1E}^2\sin 2L}{R_1^2}-\frac{3\mathrm{Re}{\mathrm{fV}}_{3N}^2\sin 2L}{R_2^2})$

$+\mathrm{\δ}{V}_{1E}(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{{2V}_{1E}}{R_1})+\mathrm{\δ}{V}_{3N}\left(\frac{{2V}_{3N}}{R_2}\right)+\mathrm{\δh}(-\frac{V_{1E}^2}{R_1^2}-\frac{V_{3N}^2}{R_2^2}+\frac{2g_0}{\mathrm{Re}})---\left(52\right)$

$-a_{3N}\mathrm{\δ\θ}+a_{1E}\mathrm{\δ\γ}$

4.2姿态角误差传播方程。

此配置方案下，载体姿态角的递推方程如式(38)，(38)式两边对时间t进行微分得到：

$\stackrel{\·}{\mathrm{\ψ}}=\frac{(V_{2N}-V_{1N})}{L\sqrt{1-{(\cos \left({\mathrm{\ψ}}_0\right)+\frac{1}{L}{\∫}_0^t(V_{1N}-V_{2N})\mathrm{dt})}^2}}$

$\stackrel{\·}{\mathrm{\θ}}=\frac{(V_{1U}-V_{2U})}{L\sqrt{1-{(\sin \left({\mathrm{\θ}}_0\right)+\frac{1}{L}{\∫}_0^t(V_{1U}-V_{2U})\mathrm{dt})}^2}}---\left(53\right)$

$\stackrel{\·}{\mathrm{\γ}}=\frac{(V_{1U}-V_{3U})}{L\sqrt{1-{(\sin \left({\mathrm{\γ}}_0\right)+\frac{1}{L}{\∫}_0^t(V_{1U}-V_{3U})\mathrm{dt})}^2}}$

对式(53)两端取全微分得航向角误差传播方程为：

$\mathrm{\δ}\stackrel{\·}{\mathrm{\ψ}}=\frac{1-{[\cos {\mathrm{\ψ}}_0+\frac{1}{L}{\∫}_0^t(V_{1N}-V_{2N})\mathrm{dt}]}^2+\frac{1}{L}{(V_{1N}-V_{2N})}^2[\cos {\mathrm{\ψ}}_0+\frac{1}{L}{\∫}_0^t(V_{1N}-V_{2N})\mathrm{dt}]}{L{(1-{[\cos {\mathrm{\ψ}}_0+\frac{1}{L}{\∫}_0^t(V_{1N}-V_{2N})\mathrm{dt}]}^2)}^{3/2}}(\mathrm{\δ}{V}_{2N}-\mathrm{\δ}{V}_{1N})---\left(54\right)$

俯仰角误差传播方程为：

$\mathrm{\δ}\stackrel{\·}{\mathrm{\θ}}=\frac{1-{[\sin {\mathrm{\θ}}_0+\frac{1}{L}{\∫}_0^t(V_{1U}-V_{2U})\mathrm{dt}]}^2+\frac{1}{L}{(V_{1U}-V_{2U})}^2[\sin {\mathrm{\θ}}_0+\frac{1}{L}{\∫}_0^t(V_{1U}-V_{2U})\mathrm{dt}]}{L{(1-{[\sin {\mathrm{\θ}}_0+\frac{1}{L}{\∫}_0^t(V_{1U}-V_{2U})\mathrm{dt}]}^2)}^{3/2}}(\mathrm{\δ}{V}_{1U}-\mathrm{\δ}{V}_{2U})---\left(55\right)$

横滚角误差传播方程为：

$\mathrm{\δ}\stackrel{\·}{\mathrm{\γ}}=\frac{1-{[\sin {\mathrm{\γ}}_0+\frac{1}{L}{\∫}_0^t(V_{1U}-V_{3U})\mathrm{dt}]}^2+\frac{1}{L}{(V_{1U}-V_{3U})}^2[\sin {\mathrm{\γ}}_0+\frac{1}{L}{\∫}_0^t(V_{1U}-V_{3U})\mathrm{dt}]}{L{(1-{[\sin {\mathrm{\γ}}_0+\frac{1}{L}{\∫}_0^t(V_{1U}-V_{3U})\mathrm{dt}]}^2)}^{3/2}}(\mathrm{\δ}{V}_{1U}-\mathrm{\δ}{V}_{3U})---\left(56\right)$

本发明未详细阐述的部分属于本领域公知技术。

本发明提出的多天线GPS/GF-INS深度组合定姿方法，根据多天线GPS单频单历元姿态解算方法中对双差整周模糊度搜索成功率不高，且当GPS可观测卫星个数小于4颗时不能进行姿态角解算，现有的INS辅助多天线GPS定姿系统无法提高GPS定姿精度的问题提出的，其中GF-INS采用一种易于安装的加速度计配置方案和特殊的姿态角更新方法，即减少了加速度计安装的复杂度，提高了系统的灵活性，又不需要外界条件的辅助。将由GF-INS输出的姿态角建立的载体坐标系下的基线矢量到地理坐标系下的基线矢量的转换矩阵，与双差载波相位和码伪距方程联立，增大了方程对未知数的冗余度，一方面使得当GPS可观测卫星个数最少为2颗时即可以进行单频单历元解算，另一方面也提高了载波相位双差整周模糊度和基线矢量浮点解的精度，从而可间接提高整周模糊度的搜索成功率。利用扩展卡尔曼滤波，将GPS输出的位置、速度和姿态角与GF-INS输出的位置、速度和姿态角进行信息融合，即进一步提高了整个系统的定位、定姿精度，又抑制了GF-INS系统的误差累积。当GPS信号出现短暂丢失时，可观测卫星个数小于2，GF-INS系统可以独立进行位置、速度和姿态角解算，保证了系统的连续性。

综上所述，本发明提出了一种多天线GPS/GF-INS深度组合定姿方法。提出应用不含陀螺仪的全加速度计惯性导航系统对多天线GPS定姿进行辅助，进一步降低了组合定姿系统的成本；设计一种易于安装的加速度计配置方案，并针对此配置方案设计了一种姿态角更新方法，使GF-INS系统在此简单配置下就可以独立进行位置、速度和姿态角解算，扩大了简单加速度计配置方案的GF-INS系统的应用范围；将由GF-INS输出的姿态角建立的载体坐标系下的基线矢量到地理坐标系下的基线矢量的转换矩阵，与双差载波相位和码伪距方程联立，增大了方程对未知数的冗余度，一方面使得当GPS可观测卫星个数最少为2颗时即可以进行单频单历元解算，另一方面也提高了载波相位双差整周模糊度和基线矢量浮点解的精度，从而可间接提高整周模糊度的搜索成功率；在信息融合方面，建立了不同于传统GPS/INS组合导航系统的系统方程，利用扩展卡尔曼滤波将GF-INS系统的GPS系统的定位以及定姿信息充分融合，即可以抑制GF-INS系统误差随时间累积，又进一步提高了整个系统的位置、速度和姿态角精度。

以上仅是本发明的具体应用范例，对本发明的保护范围不构成任何限制。其可扩展应用于所有组合导航定位、定姿的应用领域，凡采用等同变换或者等效替换而形成的技术方案，均落在本发明权利保护范围之内。

Claims (9)

1.一种多天线GPS/GF-INS深度组合定姿方法，其特征在于：

多天线GPS/GF-INS系统首次运行时，首先由多天线GPS独自工作，利用单频、单历元姿态解算算法求解出载体的三个姿态角；

在三天线GPS工作稳定后，应用主天线输出的经度、纬度和高度对GF-INS进行位置初始化，利用GPS输出的三个天线的速度对GF-INS进行速度初始化，利用GPS输出的航向角、俯仰角和横滚角对GF-INS进行姿态角初始化；GF-INS初始化完成后，即正常工作开始GF-INS导航计算，所述多天线GPS/GF-INS系统按预定方法输出位置、速度和姿态角。

2.根据权利要求1所述的多天线GPS/GF-INS深度组合定姿方法，其特征在于，所述单频单历元姿态解算算法包括：

（1）GNSS建模：建立卫星的双差载波相位观测方程和码伪距观测方程；

（2）浮点解计算：利用最小二乘法计算基线矢量和载波整周模糊度的浮点解及其方差协方差矩阵；

（3）载波整周模糊度整数解搜索：利用基线约束的最小二乘降相关平差算法搜索载波整周模糊度的整数解；

（4）基线矢量求解：利用最小二乘法由求解出的载波整周模糊度的整数解，解算出基线矢量在地理坐标系下的表达式；

（5）姿态角求解：重复步骤（1）－（4），依次求解主基线矢量和副基线矢量在地理坐标系下的表达式，再由多天线GPS定姿原理求解出载体的三个姿态角。

3.根据权利要求2所述的多天线GPS/GF-INS深度组合定姿方法，其特征在于，所述建立卫星的双差载波相位观测方程和码伪距观测方程包括：

建立选定的某一短基线对于每一颗可观测卫星的单差载波相位观测方程和码伪距观测方程；

选定仰角最大的卫星作为参考星，建立双差载波相位观测方程和码伪距观测方程。

4.根据权利要求1所述的多天线GPS/GF-INS深度组合定姿方法，其特征在于，所述GF-INS导航计算包括：

速度更新：通过GF-INS的加速度计输出GPS的天线安装点相应敏感方向的加速度，利用所述加速度对时间的积分值对GF-INS对应位置对应方向的速度进行更新；

位置更新：对更新后的主天线位置的速度进行积分，对GF-INS进行位置更新；

姿态角更新：通过更新后的主天线和副天线相应敏感方向的速度，对基线矢量在地理坐标系相应坐标轴上的投影的线段长度进行更新，进而对三个姿态角进行更新。

5.根据权利要求1所述的多天线GPS/GF-INS深度组合定姿方法，其特征在于，所述多天线GPS/GF-INS系统按预定方法输出位置、速度和姿态角包括：

当GPS可观测卫星个数少于2颗时，直接输出GF-INS的位置、速度和姿态角；当GPS可观测卫星个数大于或等于2颗时，GF-INS和GPS系统按预定深度组合方法进行定姿。

6.根据权利要求5所述的多天线GPS/GF-INS深度组合定姿方法，其特征在于，所述按预定深度组合方法进行定姿的步骤包括：

当可观测卫星小于4颗时，利用GF-INS输出的位置和速度给GPS的位置和速度赋值；

当可观测卫星大于或等于4颗时，首先利用GPS定位原理解算接收机三个天线的位置和速度，其次通过GF-INS辅助GPS定姿，输出定姿势后的位置、速度和姿态角。

7.根据权利要求6所述的多天线GPS/GF-INS深度组合定姿方法，其特征在于，所述通过GF-INS辅助GPS定姿的步骤包括：

建立GF-INS辅助的GPS双差载波相位观测方程；

利用最小二乘法计算基线矢量和载波整周模糊度浮点解及其方差协方差矩阵；

根据权利要求3所述的步骤（3）－（5）进行载波整周模糊度整数解搜索、基线矢量求解和姿态角计算；

通过组合滤波对GF-INS输出的位置、速度和姿态角反馈矫正后输出。

8.根据权利要求7所述的多天线GPS/GF-INS深度组合定姿方法，其特征在于，所述建立GF-INS辅助的GPS双差载波相位观测方程包括：

建立对于选定的某一个短基线的双差载波相位和双差码伪距方程；

由GF-INS输出的姿态角求解出姿态矩阵，结合已知两基线矢量在载体坐标系下的表达式，建立基线矢量从地理坐标系到载体坐标系的转换方程；

联立所述基线矢量转换方程和双差载波相位及双差码伪距方程，建立GF-INS辅助的GPS观测方程。

9.根据权利要求7所述的多天线GPS/GF-INS深度组合定姿方法，其特征在于，所述通过组合滤波对GF-INS输出的位置、速度和姿态角反馈矫正后输出包括：

将GPS和GF-INS输出的位置、速度和姿态角分别做差，作为观测量进行扩展卡尔曼滤波；

估计GF-INS输出的位置、速度和姿态角的误差，对GF-INS输出的位置、速度和姿态角进行反馈矫正；

输出矫正后的位置、速度和姿态角。

Images