CN101666650A - 一种sins/gps超紧致组合导航系统及实现方法 - Google Patents

Abstract

本发明公开了一种SINS/GPS超紧致组合导航系统及实现方法，利用捷联惯导系统的速度信息为GPS载波环提供多普勒频率辅助，增大了环路等效带宽，降低了载体动态对载波环的影响，并通过降低滤波器带宽，提高了噪声抑制能力；同时，为了消除伪距率误差与惯导误差之间的相关性，通过建立载波跟踪误差与惯导速度误差之间的关系，得到了载波环跟踪误差模型，并在量测方程中扣除载波跟踪误差的影响；另外，根据组合滤波器输出的误差估计信息调整载波频率，提高了载波环的跟踪精度。本发明能够有效地提高跟踪环路的噪声抑制能力和动态跟踪性能，可用于提高强干扰、高动态环境下GPS接收机的跟踪精度和组合系统的导航精度。

Description

一种SINS/GPS超紧致组合导航系统及实现方法

技术领域

本发明属于组合导航技术领域，具体涉及一种SINS/GPS超紧致组合导航系统及实现方法。

背景技术

捷联惯性导航系统(SINS)是一种完全自主的导航系统，可以连续、实时地提供位置、速度和姿态信息，其短时精度很高，且具有隐蔽性好，不受气候条件限制等优点，因而广泛应用于航空、航天、航海等领域。但是，SINS误差随时间增长，因此常与GPS全球卫星定位系统组合构成SINS/GPS组合导航系统。GPS和SINS在性能上具有很强的互补性，将两者组合不仅可以充分发挥各自的优势，而且随着组合程度的加深，SINS/GPS组合系统的总体性能要远远优于各独立系统。按照组合程度的不同，SINS和GPS的组合模式可分为松散组合、紧密组合和超紧致组合。松散组合和紧密组合主要是依靠GPS输出的位置、速度或伪距、伪距率等信息来辅助惯导系统，以提高组合系统的位置、速度和姿态精度，目前这两种组合模式应用较为广泛。然而，随着高机动性飞行器的发展和GPS应用领域的扩展，为满足高动态用户(歼击机、导弹等)及强噪声干扰条件下的应用需求，并进一步提高组合系统的可靠性，以紧密组合为基础发展起来的超紧致组合，成为SINS/GPS组合系统的新一代设计模式。

超紧致组合改变了传统意义上的GPS接收机跟踪环路，将组合的概念应用到了接收机内部结构中。超紧致组合的主要优点在于：一方面，利用组合滤波器提供的位置、速度估计信息，进一步计算得到伪码相位和多普勒频移的先验估计，可用于缩短由干扰或衰减导致的GPS信号失锁后的重捕获时间；另一方面，组合滤波器可以为GPS跟踪环路提供辅助信息，从而扩展了环路的跟踪能力，有效地消除了环路跟踪载体动态的功能需求，并能够降低环路带宽以增强噪声抑制能力。

早期超紧致组合方案的主要目标是利用惯导信息为伪码跟踪环路提供辅助，以在载波失锁的情况下维持伪码跟踪锁定。而载波跟踪环路是GPS接收机中比较脆弱的环节，它对载体动态和噪声干扰十分敏感。如果载体动态引起的载波多普勒频移过大或接收信号的载噪比过低，将导致载波跟踪失锁、导航电文无法提取，致使接收机不能独立完成导航任务，这对于高动态情况下的组合系统是致命的。因此提高载波跟踪环的动态性能和噪声抑制能力，是高动态、强干扰环境下SINS/GPS组合系统亟待解决的问题，也是SINS/GPS超紧致组合领域的研究重点。

发明内容

本发明的目的是为了解决现有技术的不足，提供一种基于SINS辅助GPS载波和伪码跟踪的SINS/GPS超紧致组合导航系统结构及其实现方法，该方法提高了GPS接收机的动态跟踪性能和组合系统的导航精度。

一种SINS/GPS超紧致组合导航系统实现方法，具体包括以下步骤：

步骤一：建立GPS接收机中载波环的跟踪误差模型和码环的跟踪误差模型；

步骤二：建立SINS/GPS超紧致组合系统状态方程和量测方程；

a.SINS系统的误差状态方程；

b.GPS误差状态方程；

c.码环跟踪误差方程；

d.载波环跟踪误差方程；

e.SINS/GPS超紧致组合系统状态方程；

f.SINS/GPS超紧致组合系统量测方程；

步骤三：组合导航信息融合；

步骤四：辅助参数计算。

一种SINS/GPS超紧致组合导航系统，包括捷联惯性导航系统、GPS接收机、组合导航滤波器和辅助参数计算单元；

捷联惯性导航系统包括惯性测量元件和导航解算单元；惯性测量元件测量载体的比力和角速率，将得到的比力和角速率信息传送给导航解算环节，导航解算单元根据惯性测量元件传输的信息得到载体的位置、速度和姿态，并转换为卫星与载体之间的距离ρ_I和距离率

输入到组合导航滤波器中，导航解算单元将SINS导航参数输入到辅助参数计算单元中，所述的SINS导航参数为载体的位置和速度；

GPS接收机包括天线、射频前端、码环和载波环；射频前端对天线接收到的GPS射频信号进行处理，得到GPS中频信号，GPS中频信号经过混频，得到混频后的基带信号；

码环包括码相位鉴别器、环路滤波器A和C/A码数控振荡器，C/A码数控振荡器产生本地C/A码，混频后的GPS基带信号与本地C/A码进行相关运算，并将相关结果输入到码相位鉴别器中，码相位鉴别器得到码相位差，将得到码相位差输入到环路滤波器A，相位差经过环路滤波器A滤波后，对C/A码数控振荡器输出控制信号，C/A码数控振荡器根据控制信号和辅助信息调整本地C/A码相位，使得本地C/A码相位与输入的GPS中频信号中的码相位对准；

所述的辅助信息为载波环提供的辅助信息和辅助参数计算单元提供的C/A码相位估计值；

载波环包括载波相位鉴别器、环路滤波器B、环路滤波器C、载波数控振荡器和比例转换单元，射频前端输出的GPS中频信号与载波数控振荡器生成的本地载波余弦、正弦信号进行混频，得到同相、正交两路混频后的GPS基带信号，混频后的GPS基带信号与本地C/A码进行相关运算，相关结果输入到载波相位鉴别器中，载波相位鉴别器得到输入GPS中频信号与本地载波之间的相位差，输出载波相位差信号，根据GPS接收机的工作模式选择环路滤波器B或环路滤波器C对载波相位差信号进行滤波处理；

载波数控振荡器根据不同模式下的控制信号调整本地载波频率和相位，使之与输入GPS中频信号中的载波频率、相位对准；同时，载波数控振荡器将调整后的本地载波频率传递到比例转换单元中，比例转换单元将本地载波频率转换为C/A码频率输入C/A码数控振荡器中，对码环进行辅助，C/A码频率即为载波环为C/A码数控振荡器提供的辅助信息；码环和载波环分别输出码相位和载波频率信息，将其转换为伪距ρ_G、伪距率

作为量测信息输入到组合导航滤波器中；

组合导航滤波器根据码环、载波环和导航解算单元提供的伪距ρ_G、伪距率

和距离ρ_I、距离率

对捷联惯性导航系统的导航参数和惯性器件的误差进行估计，并将其反馈回SINS导航解算单元中，对相应的误差进行校正和补偿，同时将接收机钟频误差估计信息传递给辅助参数计算单元；

辅助参数计算单元根据校正后的SINS导航参数、卫星参数和接收机钟频误差估计信息计算得到辅助参数，所述的辅助参数为C/A码相位估计值和辅助频率估计值，将C/A码相位估计值和辅助频率估计值分别提供给GPS接收机的码环中的C/A码数控振荡器和载波环中的载波数控振荡器。

本发明的优点在于：

(1)本发明利用SINS的位置、速度参数为载波跟踪环路提供频率辅助信息，增大了环路等效带宽，从而减小了载波环的动态跟踪范围；

(2)对载波、伪码跟踪环误差进行建模，并在伪距、伪距率量测方程中扣除跟踪误差的影响，从而消除了伪距、伪距率误差与惯导速度误差之间的相关性，并提高了GPS跟踪环路的测量精度；

(3)为保证高动态环境中码环的动态性能和跟踪精度，在GPS接收机内部采用载波辅助码环，以消除载体动态对码环的影响，而码环则通过降低相关间隔来提高伪码相位的跟踪精度；

(4)为防止恶劣环境中载波环性能下降对码环造成污染，将载波环对码环的辅助设置为可选的，即载波环正常工作时对码环进行辅助，而如果载波环发生异常，则利用SINS速度信息辅助码环，以保证码环的稳定性以及组合系统的可靠性。

附图说明

图1是本发明一种SINS/GPS超紧致组合导航系统的结构示意图；

图2是一种SINS/GPS超紧致组合导航系统实现方法的流程图；

图3是本发明的载波环误差模型示意图；

图中：

1-捷联惯生导航系统  2-GPS接收机          3-组合导航滤波器  4-辅助参数计算单元

101-惯性测量元件    102-导航解算单元     201-天线          202-射频前端

203-码环            204-载波环           205-码相位鉴别器  206-环路滤波器A

207-C/A码数控振荡   208-载波相位鉴别器   209-环路滤波器B   210-环路滤波器C

器

211-载波数控振荡器  212-比例转换单元

具体实施方式

下面将结合附图和实施例对本发明作进一步的详细说明。

本发明的一种SINS/GPS超紧致组合导航系统，如图1所示，包括捷联惯性导航系统1、GPS接收机2、组合导航滤波器3和辅助参数计算单元4；

捷联惯性导航系统(SINS)1包括惯性测量元件(IMU)101和导航解算单元102。惯性测量元件101测量载体的比力和角速率，将得到的比力和角速率信息传送给导航解算环节102，导航解算单元102根据惯性测量元件101传输的信息得到载体的位置、速度和姿态，并转换为卫星与载体之间的距离ρ_I和距离率

输入到组合导航滤波器3中，导航解算单元102将SINS导航参数输入到辅助参数计算单元4中，所述的SINS导航参数为载体的位置和速度；

GPS接收机2包括天线201、射频前端202、码环203和载波环204。射频前端202对天线201接收到的GPS射频信号进行处理，得到GPS中频信号，GPS中频信号经过混频，得到混频后的GPS基带信号；

码环203包括码相位鉴别器205、环路滤波器A206和C/A码数控振荡器207，C/A码数控振荡器207产生本地C/A码，混频后的GPS基带信号与本地C/A码进行相关运算，并将相关结果输入到码相位鉴别器205中，码相位鉴别器205得到码相位差，将得到码相位差输入到环路滤波器A206，相位差经过环路滤波器A206滤波后，对C/A码数控振荡器207输出控制信号，C/A码数控振荡器207根据控制信号和辅助信息调整本地C/A码相位，使得本地C/A码相位与输入的GPS中频信号中的码相位对准；

所述的辅助信息为载波环204提供的辅助信息或辅助参数计算单元4提供的C/A码相位估计值。

载波环204包括载波相位鉴别器208、环路滤波器B209、环路滤波器C210、载波数控振荡器211和比例转换单元212，射频前端202输出的GPS中频信号与载波数控振荡器211生成的本地载波余弦、正弦信号进行混频，得到同相、正交两路混频后的GPS基带信号，混频后的GPS基带信号与本地C/A码进行相关运算，相关结果输入到载波相位鉴别器208中，载波相位鉴别器208得到输入GPS中频信号与本地载波之间的相位差，输出载波相位差信号，根据GPS接收机2的工作模式选择环路滤波器B209或环路滤波器C210对载波相位差信号进行滤波处理，所述的GPS接收机2的工作模式包括：独立工作模式和组合工作模式。当GPS接收机2处于独立工作模式时，载波环204中a、c连通，环路滤波器B209工作，对载波数控振荡器211的控制信号为环路滤波器B209的输出信号；而当GPS接收机2与SINS组成组合系统，处于组合工作模式时，载波环中a、b连通，环路滤波器C210工作，对载波数控振荡器211的控制信号包括环路滤波器C210的输出信号和辅助参数计算单元4输出的辅助频移估计值；

载波数控振荡器211根据不同模式下的控制信号调整本地载波频率和相位，使之与输入GPS中频信号中的载波频率、相位对准；同时，载波数控振荡器211将调整后的本地载波频率传递到比例转换单元212中，比例转换单元212将本地载波频率转换为C/A码频率输入C/A码数控振荡器207中，对码环203进行辅助，C/A码频率即为载波环204为C/A码数控振荡器207提供的辅助信息；

码环203和载波环204分别输出码相位和载波频率信息，将其转换为伪距ρ_G、伪距率作为量测信息输入到组合导航滤波器3中；

组合导航滤波器3根据码环203、载波环204和导航解算单元102提供的伪距ρ_G、伪距率

和距离ρ_I、距离率

对捷联惯性导航系统1的导航参数和惯性器件的误差进行估计，并将其反馈回SINS导航解算单元102中，对相应的误差进行校正和补偿，同时将接收机钟频误差估计信息传递给辅助参数计算单元4；

辅助参数计算单元4根据校正后的SINS导航参数、卫星参数和接收机钟频误差估计信息计算得到辅助参数，所述的辅助参数为C/A码相位估计值和辅助频率估计值，将C/A码相位估计值和辅助频率估计值分别提供给GPS接收机2的码环203中的C/A码数控振荡器207和载波环204中的载波数控振荡器211，

为防止载波环204工作性能下降对码环造成污染，并保证码环203的稳定性以及SINS/GPS超紧致组合导航系统的可靠性，将载波环204对码环203的辅助设置为可选的，即载波环正常工作时，连接触点d、f，利用载波频率信息对码环203进行辅助，而如果载波环发生异常，则连接触点d、e，则捷联惯性导航系统1通过辅助参数计算单元4辅助码环203。

本发明的一种SINS/GPS超紧致组合导航系统实现方法，流程如图2所示，具体包括以下步骤：

步骤一：建立GPS接收机2中载波环204的跟踪误差模型和码环203的跟踪误差模型；

SINS辅助的载波环204误差模型如图3所示，虚线框内为GPS接收机2处于独立工作模式下载波环204的基本误差模型。当GPS接收机2处于独立工作模式在载波环204中，本地载波相位

与参考载波相位θ的相位误差δθ由载波相位鉴别器208确定，经过环路滤波器B209处理后，得到频率跟踪误差δf，用于调整本地载波数控振荡器211中的载波频率，从而使本地信号与输入信号的载波频率、相位保持一致。根据二阶载波环204的基本结构，因此可得独立工作模式下载波环204的跟踪误差方程：

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{\mathrm{\θ}}=2\mathrm{\π\δf}\\ \mathrm{\δ}\stackrel{\·}{f}=[2\mathrm{\π}\frac{t_2}{t_1}\mathrm{\δ}\stackrel{\·}{\mathrm{\θ}}+\frac{\mathrm{\δ\θ}}{t_1}]\·K_{\mathrm{PLL}}\end{array}\right.---\left(1\right)$

其中，δf、δθ分别为载波跟踪环路中的频率跟踪误差和相位误差，K_PLL为环路增益，t₁、t₂为环路滤波器B209的参数，环路滤波器B209的频域表达式为：

$F\left(s\right)=\frac{(t_{2}s+1)}{t_{1}s}---\left(2\right)$

当GPS接收机2处于独立工作模式时，载波环204中的频率误差就是环路滤波器B209的输出值δf；

当GPS接收机2处于组合工作模式时，载波环204中频率误差δf_PLL则为环路滤波器C210的输出量δf_TRK与辅助频率误差δf_aid之和：

δf_PLL＝δf_TRK+δf_aid            (2)

根据载波环跟踪误差与SINS误差状态之间的关系，在组合工作模式下，用频率误差δf_PLL代替独立工作模式下的频率跟踪误差δf，就可以得到组合工作模式下载波环204的跟踪误差方程：

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{\mathrm{\θ}}=2\mathrm{\π}({\mathrm{\δf}}_{\mathrm{TRK}}+{\mathrm{\δf}}_{\mathrm{aid}})\\ \mathrm{\δ}{\stackrel{\·}{f}}_{\mathrm{TRK}}=[2\mathrm{\π}\frac{T_2}{T_1}({\mathrm{\δf}}_{\mathrm{TRK}}+{\mathrm{\δf}}_{\mathrm{aid}})+\frac{\mathrm{\δ\θ}}{T_1}]\·K_{\mathrm{PLL}}\end{array}\right.---\left(4\right)$

其中，δθ为载波环中的相位误差，K_PLL为环路增益，T₁、T₂为环路滤波器C210的参数，δf_TRK为环路滤波器C210的输出量，δf_aid为辅助频率误差。

当接收机处于组合工作模式时，可以采用一阶码环203，则码环203的误差方程可以表示为：

$\mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}_{\mathrm{DLL}}=-K_{\mathrm{DLL}}{\mathrm{\δ\ρ}}_{\mathrm{DLL}}+{\mathrm{\δV}}_{\mathrm{aid}}+K_{\mathrm{DLL}}Q---\left(5\right)$

其中，δρ_DLL为伪距测量误差；K_DLL为码环增益；Q为由热噪声以及干扰引起的驱动噪声。

辅助参数计算单元4根据校正后的SINS导航参数、卫星参数和接收机钟频误差估计信息计算的辅助频率估计值f_aid为：

$f_{\mathrm{aid}}={\hat{f}}_{\mathrm{dop}}+{\hat{f}}_{\mathrm{clk}}---\left(6\right)$

其中，为根据校正后的SINS参数计算的多普勒频率估计值，为组合导航滤波器的接收机钟频误差估计值。

根据SINS位置、速度参数和卫星参数，可得惯导辅助速度V_aid：

$V_{\mathrm{aid}}=\frac{(X_s^e-X_r^e)(V_s^e-V_r^e)}{\left|\right|X_s^e-X_r^e\left|\right|}=(V_s^e-V_r^e)\·{\stackrel{\→}{L}}_i---\left(7\right)$

其中，X_s ^e、V_s ^e分别为第i颗卫星在协议地球坐标系中的位置、速度，X_r ^e、V_r ^e为GPS接收机2的位置、速度，

为卫星和GPS接收机2视线方向上的单位矢量。

则根据SINS导航参数得到的多普勒频率估计值为：

${\hat{f}}_{\mathrm{dop}}=-\frac{f_{L1}}{c}\·V_{\mathrm{aid}}=-\frac{f_{L1}}{c}\·(V_s^e-V_r^e)\·{\stackrel{\→}{L}}_i---\left(8\right)$

其中，f_L1为L1载波频率，c为光速。

由SINS导航参数误差所导致的多普勒频率估计误差为：

${\mathrm{\δf}}_{\mathrm{dop}}=-\frac{f_{L1}}{c}\·{\stackrel{\→}{L}}_i^T\·(C_i^e\·{\mathrm{\δV}}^i-C_c^e\·W_e\·C_i^c\·\mathrm{\δ}{X}^i)---\left(9\right)$

其中，δXⁱ、δVⁱ分别为发射点惯性系中载体的位置、速度误差，C_i ^e、C_c ^e、C_i ^c分别为发射点惯性系到协议地球系、地心惯性坐标系到协议地球系、发射点惯性系到协议地球系的转换矩阵，W_e为地球自转角速度矢量在协议地球系中的叉乘矩阵。

由于组合导航滤波器对接收机钟频误差的估计误差远小于由SINS导航参数误差导致的多普勒频率估计误差，因此在建立辅助频率误差模型时，可忽略接收机钟频误差估计误差的影响，可得：

δf_aid≈δf_dop                      (10)

以跟踪通道内载波环204的相位、频率误差作为状态变量，则第i个通道内载波环204的跟踪误差模型为

${\left[\begin{array}{c}\mathrm{\δ}\stackrel{\·}{\mathrm{\θ}}\\ \mathrm{\δ}{\stackrel{\·}{f}}_{\mathrm{TRK}}\end{array}\right]}_i=\left[\begin{array}{cc}0& 2\mathrm{\π}\\ \frac{K_{\mathrm{PLL}}}{T_1}& \frac{2\mathrm{\π}{K}_{\mathrm{PLL}}{T}_2}{T_1}\end{array}\right]{\left[\begin{array}{c}\mathrm{\δ\θ}\\ {\mathrm{\δf}}_{\mathrm{TRK}}\end{array}\right]}_i+\left[\begin{array}{c}2\mathrm{\π}\\ \frac{2\mathrm{\π}{K}_{\mathrm{PLL}}{T}_2}{T_1}\end{array}\right]\·\mathrm{\δ}{f}_{\mathrm{aid}}---\left(11\right)$

以跟踪通道内码环203输出的伪距测量误差作为状态变量，则通道i内码环203的跟踪误差模型为

$\mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}_{\mathrm{DLLi}}=-K_{\mathrm{DLL}}{\mathrm{\δ\ρ}}_{\mathrm{DLLi}}+{\mathrm{\δV}}_{\mathrm{aid}}+K_{\mathrm{DLL}}Q---\left(12\right)$

其中，δρ_DLLi为伪距测量误差；K_DLL为码环增益；Q为由热噪声以及干扰等引起的驱动噪声。

步骤二：建立SINS/GPS超紧致组合系统状态方程和量测方程；

由于辅助参数计算单元4需根据SINS导航参数计算辅助频率，因此SINS速度误差会导致辅助频率估计误差，继而引发载波频率、相位误差，从而导致载波环的伪距率测量误差与SINS速度误差相关。如果忽略了量测信息与状态变量之间的关系，可能导致系统不稳定。因此，将载波环204的频率和相位跟踪误差扩充为组合导航滤波器3的状态变量，利用组合导航滤波器3对其进行估计，并在伪距、伪距率量测方程中消除载波环跟踪误差的影响。

在SINS/GPS超紧致组合导航系统中，组合导航滤波器3的误差模型包括SINS、GPS误差模型、码环203、载波环204跟踪误差模型。

a.SINS系统的误差状态方程；

${\stackrel{\·}{X}}_I=F_I{X}_I+G_I{W}_I---\left(13\right)$

其中，X_I为SINS系统状态矢量，W_I为SINS系统噪声矢量，F_I为SINS系统状态矩阵，G_I为系SINS统噪声矩阵，SINS的误差状态包括位置误差(δx，δy，δz)、速度误差(δv_x，δv_y，δv_z)、姿态误差角(φ_x，φ_y，φ_z)、加速度计零偏加速度计系数误差(k_a1x，k_a1y，k_a1z，k_a2x，k_a2y，k_a2z)、陀螺仪系数误差(k_w1x，k_w1y，k_w1z)和陀螺仪常值漂移(ε_x，ε_y，ε_z)， $X_I={[\mathrm{\δx},\mathrm{\δy},\mathrm{\δz},\mathrm{\δ}{v}_x,\mathrm{\δ}{v}_y,{\mathrm{\δv}}_z,{\mathrm{\φ}}_x,{\mathrm{\φ}}_y,{\mathrm{\φ}}_z,k_{a1x},k_{a1y},k_{a1z},k_{a2x},k_{a2y},k_{a2z},{\▿}_x,{\▿}_y,{\▿}_z,k_{w1x},k_{w1y},k_{w1z},{\mathrm{\ϵ}}_x,{\mathrm{\ϵ}}_y,{\mathrm{\ϵ}}_z]}^T,$ 则：

W_I＝[w_ax，w_ay，w_az，w_gx，w_gy，w_gz]^T

$G_I={\left[\begin{array}{cc}{0}_{3\×3}& 0_{3\×3}\\ I_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& {-C}_b^i\\ 0_{15\×3}& 0_{15\×3}\end{array}\right]}_{24\×6}$

在发射点惯性坐标系下，SINS系统状态矩阵F_I的形式如下：

$F_I={\left[\begin{array}{c}\begin{array}{cccccccc}{0}_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ F_g& 0_{3\×3}& B& C_1& C_2& C_b^i& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& C_3& -C_b^i\end{array}\\ 0_{15\×24}\end{array}\right]}_{24\×24},$ $B=-C_b^i\left[\begin{array}{ccc}0& f_z^b& -f_y^b\\ -f_z^b& 0& f_x^b\\ f_y^b& {-f}_x^b& 0\end{array}\right]$

$C_1=C_b^i\left[\begin{array}{ccc}{f}_x^b& & \\ & f_y^b& \\ & & f_z^b\end{array}\right],$ $C_2=C_b^i\left[\begin{array}{ccc}{\left(f_x^b\right)}^2& & \\ & {\left(f_y^b\right)}^2& \\ & & {\left(f_z^b\right)}^2\end{array}\right],$ $C_3=-C_b^i\left[\begin{array}{ccc}{\mathrm{\ω}}_x^b& & \\ & {\mathrm{\ω}}_y^b& \\ & & {\mathrm{\ω}}_z^b\end{array}\right]$

其中，F_g为引力加速度对位置坐标的雅克比矩阵，C_b ⁱ为本体坐标系到发射点惯性系的坐标转换阵，[f_x ^b f_y ^b f_z ^b]^T和[ω_x ^b ω_y ^b ω_z ^b]^T分别为加速度计和陀螺仪输出的比力和角速度信息。

b.GPS误差状态方程；

${\stackrel{\·}{X}}_G=F_G{X}_G+G_G{W}_G---\left(14\right)$

其中，X_G为GPS误差状态变量，W_G为GPS系统噪声矢量，F_G为GPS系统状态矩阵，G_G为GPS系统噪声矩阵，GPS的误差状态包括两个与时间有关的误差：与时钟误差等效的距离误差δl_u，与时钟频率误差等效的距离率误差δl_ru，T_ru为相关时间，

X_G＝[δl_u，δl_ru]^T，W_G＝[w_u，w_ru]^T， $F_G=\left[\begin{array}{cc}0& 1\\ 0& -\frac{1}{T_{\mathrm{ru}}}\end{array}\right],$ $G_G=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$

c.码环203跟踪误差方程；

${\stackrel{\·}{X}}_D=F_D{X}_D+G_D{W}_D---\left(15\right)$

其中，X_D为码环误差状态变量，W_D为码环噪声矢量，F_D为码环203的系统状态矩阵，G_D为码环203的系统噪声矩阵。GPS接收机2选择N颗导航星座，N为自然数，N≥4，并启动N个跟踪通道工作，δρ_DLLi(i＝1，2，…N)为N颗卫星对应的伪距跟踪误差，

X_D＝[δρ_DLL1 δρ_DLL2…δρ_DLLN]^T，W_D＝[Q₁，Q₂，…Q_N]^T

$F_D=-{\left[\begin{array}{cccc}{K}_{1\mathrm{DLL}}& & & \\ & K_{2\mathrm{DLL}}& & \\ & & \·\·\·& \\ & & & K_{\mathrm{NDLL}}\end{array}\right]}_{N\×N},$ $G_D=\left[\begin{array}{cccc}{K}_{1\mathrm{DLL}}& & & \\ & K_{2\mathrm{DLL}}& & \\ & & \·\·\·& \\ & & & K_{\mathrm{NDLL}}\end{array}\right]$

d.载波环204跟踪误差方程；

GPS接收机2内N个通道的载波环跟踪误差方程：

${\stackrel{\·}{X}}_P=F_P{X}_P+G_P{W}_P---\left(16\right)$

其中，X_P为载波环的误差状态变量，W_P为载波环系统噪声序列，G_P为载波环系统噪声矩阵，F_P为载波环系统状态矩阵，δθ_i、δf_TRKi分别为第i个通道内载波跟踪环的相位误差和环路滤波器输出，K_PLL为环路增益，

X_P＝[δθ₁，δθ₂，…δθ_N，δf_TRK1，δf_TRK2，…δf_TRKN]^T

$F_P={\left[\begin{array}{cc}{0}_N& 2\mathrm{\π}\·I_N\\ K_{\mathrm{PLL}}/T_1\·I_N& 2\mathrm{\π}{K}_{\mathrm{PLL}}{T}_2/T_1\·I_N\end{array}\right]}_{2N\×2N}$

e.SINS/GPS超紧致组合系统状态方程；

将SINS、GPS误差方程以及GPS码环、载波环跟踪误差方程合并，得到超紧致组合系统的状态方程：

$\stackrel{\·}{X}=\mathrm{FX}+\mathrm{GW}---\left(17\right)$

其中，X为超紧致组合系统状态矢量，F为超紧致组合系统状态矩阵，W为超紧致组合系统噪声矢量，G为超紧致组合系统噪声矩阵，

X＝[X_I X_G X_D X_P]^T，W＝[W_I W_G W_D W_P]^T

$F=\left[\begin{array}{cccc}{F}_1& & & \\ & F_G& & \\ F_{\mathrm{ID}}& & F_D& \\ F_{\mathrm{IP}}& & & F_P\end{array}\right],$ $G=\left[\begin{array}{cccc}{G}_I& & & \\ & G_G& & \\ & & G_D& \\ & & & G_P\end{array}\right]$

$F_I={\left[\begin{array}{c}\begin{array}{cccccccc}{0}_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ F_g& 0_{3\×3}& B& C_1& C_2& C_b^i& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& C_3& -C_b^i\end{array}\\ 0_{15\×24}\end{array}\right]}_{24\×24},$ $F_G=\left[\begin{array}{cc}0& 1\\ 0& -\frac{1}{T_{\mathrm{ru}}}\end{array}\right]$

$F_{\mathrm{ID}}={\left[\begin{array}{ccc}-{\stackrel{\→}{L}}_1^T\·C_c^e{\·W}_e\·C_i^c& {\stackrel{\→}{L}}_1^T\·C_i^e& 0_{1\×18}\\ -{\stackrel{\→}{L}}_2^T\·C_c^e\·W_e\·C_i^c& {\stackrel{\→}{L}}_2^T\·C_i^e& 0_{1\×18}\\ \·\·\·& \·\·\·& \·\·\·\\ -{\stackrel{\→}{L}}_N^T\·C_c^e\·W_e\·C_i^c& {\stackrel{\→}{L}}_N^T\·C_i^e& 0_{1\×18}\end{array}\right]}_{N\×24},$ $F_D=-{\left[\begin{array}{cccc}{K}_{1\mathrm{DLL}}& & & \\ & K_{2\mathrm{DLL}}& & \\ & & \·\·\·& \\ & & & K_{\mathrm{NDLL}}\end{array}\right]}_{N\×N}$

$F_{\mathrm{IP}}=-2\mathrm{\π}\frac{f_{L1}}{c}\·{\left[\begin{array}{c}{F}_{\mathrm{ID}}\\ F_{\mathrm{ID}}\·\frac{T_2}{T_1}\·K_{\mathrm{PLL}}\end{array}\right]}_{2N\×24},$ $F_P={\left[\begin{array}{cc}{0}_N& 2\mathrm{\π}\·I_N\\ K_{\mathrm{PLL}}/T_1\·I_N& 2\mathrm{\π}{K}_{\mathrm{PLL}}{T}_2/T_1\·I_N\end{array}\right]}_{2N\×2N}$

f.SINS/GPS超紧致组合系统量测方程；

在SINS/GPS超紧致组合导航系统中，以伪距、伪距率为量测信息，一个跟踪通道内的伪距、伪距率量测方程为

$\left\{\begin{array}{c}\mathrm{\δ\ρ}={\mathrm{\ρ}}_I-{\mathrm{\ρ}}_G={\mathrm{\δ\ρ}}_I-({\mathrm{\δl}}_u+{\mathrm{\υ}}_{\mathrm{\ρ}}+{\mathrm{\δ\ρ}}_{\mathrm{DLL}})\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\ρ}}={\stackrel{\·}{\mathrm{\ρ}}}_I-{\stackrel{\·}{\mathrm{\ρ}}}_G=\mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}_I-({\mathrm{\δl}}_{\mathrm{ru}}+{\mathrm{\υ}}_{\stackrel{\·}{\mathrm{\ρ}}}+\mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}_{\mathrm{PLL}})\end{array}\right.---\left(18\right)$

其中，δρ和

分别为SINS和GPS的伪距差、伪距率差，δρ_I和为由SINS导航误差导致的卫星与载体间的距离、距离率误差，

为GPS接收机的量测噪声，δρ_DLL、

为由GPS接收机跟踪误差导致的伪距、伪距率测量误差。

将N个通道的伪距、伪距率量测方程合并，得到超紧致组合系统的量测方程：

Z＝HX+V                       (19)

其中，Z超紧致组合系统的为观测矢量，H为超紧致组合系统的观测矩阵，V为超紧致组合系统的量测噪声序列，

$Z={[{\mathrm{\δ\ρ}}_1,{\mathrm{\δ\ρ}}_2,\·\·\·{\mathrm{\δ\ρ}}_N,\mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}_1,\mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}_2,\·\·\·\mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}_N]}^T,$ $V={[{\mathrm{\υ}}_{\mathrm{\ρ}1},{\mathrm{\υ}}_{\mathrm{\ρ}2},\·\·\·{\mathrm{\υ}}_{\mathrm{\ρN}},{\mathrm{\υ}}_{\stackrel{\·}{\mathrm{\ρ}}1},{\mathrm{\υ}}_{\stackrel{\·}{\mathrm{\ρ}}2},\·\·\·{\mathrm{\υ}}_{\stackrel{\·}{\mathrm{\ρ}}N}]}^T$

$H=\left[\begin{array}{c}{H}_{\mathrm{\ρ}}\\ H_{\stackrel{\·}{\mathrm{\ρ}}}\end{array}\right]{=\left[\begin{array}{ccccccc}{\stackrel{\→}{L}}^T\·C_i^e& 0_{N\×21}& -I_{N\×1}& 0_{N\×1}& -I_{N\×N}& 0_{N\×N}& 0_{N\×N}\\ 0_{N\×3}& 0_{N\×21}& 0_{N\×1}& {-I}_{N\×1}& 0_{N\×N}& 0_{N\×N}& c/f_{L1}\·I_N\end{array}\right]}_{2N\×(26+3N)}$

其中，

为N颗卫星对应的伪距、伪距率差， $\stackrel{\→}{L}=[{\stackrel{\→}{L}}_1;{\stackrel{\→}{L}}_2;\·\·\·;{\stackrel{\→}{L}}_N],$

为第i颗卫星和接收机视线方向上的单位矢量。

步骤三：组合导航信息融合；

组合导航滤波器3根据SINS与GPS输出的伪距、伪距率量测信息，对SINS、GPS以及码环、载波环跟踪误差状态进行估计；将滤波器估计的SINS误差状态反馈回SINS系统中，对导航参数及元件误差进行校正，同时，将接收机钟频误差估计信息传递给辅助参数计算单元4；

步骤四：辅助参数计算；

辅助参数计算单元4根据校正后的SINS导航参数、卫星参数和接收机钟频误差估计信息计算卫星与载体之间的距离ρ_I和距离率

转换为C/A码相位估计值和辅助频率估计值，并将C/A码相位和辅助频率的估计值分别提供给GPS接收机码环203中的C/A码数控振荡器207和载波环204中的载波数控振荡器211，为GPS接收机的伪码、载波跟踪提供辅助。

在所述的SINS/GPS超紧致组合系统开始工作时，由于SINS系统具有一定的误差积累，需要先使GPS接收机处于独立工作模式，利用GPS的输出信息来校正SINS误差；当SINS的导航参数达到一定精度时，使得多普勒频率估计误差δf_dop小于载波环204的跟踪带宽时，才能启动组合工作模式，通过辅助参数计算单元4为GPS接收机2的码环203和载波环204提供辅助信息。

GPS接收机载波环和码环在SINS辅助下，能够工作在较低的环路带宽条件下，从而提高跟踪精度，并为组合系统提供更为精确的伪距、伪距率量测信息，进而提高组合系统的导航精度。

Claims (5)

1、一种SINS/GPS超紧致组合导航系统实现方法，其特征在于，具体包括以下步骤：

步骤一：建立GPS接收机中载波环的跟踪误差模型和码环的跟踪误差模型；

当GPS接收机处于独立工作模式时在载波环中，本地载波相位

与参考载波相位θ的相位误差δθ由载波相位鉴别器确定，经过环路滤波器B处理后，得到频率跟踪误差δf，用于调整本地载波数控振荡器中的载波频率，从而使本地信号与输入信号的载波频率、相位保持一致；根据二阶载波环的基本结构，独立工作模式下载波环的跟踪误差方程：

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{\mathrm{\θ}}=2\mathrm{\π\δf}\\ \mathrm{\δ}\stackrel{\·}{f}=[2\mathrm{\π}\frac{t_2}{t_1}\mathrm{\δ}\stackrel{\·}{\mathrm{\θ}}+\frac{\mathrm{\δ\θ}}{t_1}]\·K_{\mathrm{PLL}}\end{array}\right.---\left(1\right)$

其中，δf、δθ分别为载波跟踪环路中的频率跟踪误差和相位误差，K_PLL为环路增益，t₁、t₂为环路滤波器B的参数；

环路滤波器B的频域表达式为：

$F\left(s\right)=\frac{(t_{2}s+1)}{t_{1}s}---\left(2\right)$

当GPS接收机处于组合工作模式时，载波环中频率误差δf_PLL为环路滤波器C的输出量δf_TRK与辅助频率误差δf_aid之和：

δf_PLL＝δf_TRK+δf_aid              (3)

在组合工作模式下，用频率误差δf_PLL代替独立工作模式下的频率跟踪误差δ_f，得到组合工作模式下载波环的跟踪误差方程：

$\left\{\begin{array}{c}\mathrm{\δ}\stackrel{\·}{\mathrm{\θ}}=2\mathrm{\π}({\mathrm{\δf}}_{\mathrm{TRK}}+\mathrm{\δ}{f}_{\mathrm{aid}})\\ \mathrm{\δ}{\stackrel{\·}{f}}_{\mathrm{TRK}}=[2\mathrm{\π}\frac{T_2}{T_1}({\mathrm{\δf}}_{\mathrm{TRK}}+{\mathrm{\δf}}_{\mathrm{aid}})+\frac{\mathrm{\δ\θ}}{T_1}]\·K_{\mathrm{PLL}}\end{array}\right.---\left(4\right)$

其中，δθ为载波环中的相位误差，K_PLL为环路增益，T₁、T₂为环路滤波器C的参数，δf_TRK为环路滤波器C的输出量，δf_aid为辅助频率误差；

当接收机处于组合工作模式时，采用一阶码环，则码环的误差方程为：

$\mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}_{\mathrm{DLL}}=-K_{\mathrm{DLL}}{\mathrm{\δ\ρ}}_{\mathrm{DLL}}+{\mathrm{\δV}}_{\mathrm{aid}}+K_{\mathrm{DLL}}Q---\left(5\right)$

其中，δρ_DLL为伪距测量误差；K_DLL为码环增益；Q为由热噪声以及干扰引起的驱动噪声；

辅助参数计算单元根据校正后的SINS导航参数、卫星参数和接收机钟频误差估计信息计算的辅助频率估计值f_aid为：

$f_{\mathrm{aid}}={\hat{f}}_{\mathrm{dop}}+{\hat{f}}_{\mathrm{clk}}---\left(3\right)$

其中，为根据校正后的SINS参数计算的多普勒频率估计值，

为组合导航滤波器的接收机钟频误差估计值；

根据SINS位置、速度参数和卫星参数，得到惯导辅助速度V_aid：

$V_{\mathrm{aid}}=\frac{(X_s^e-X_r^e)(V_s^e-V_r^e)}{\left|\right|X_s^e-X_r^e\left|\right|}=(V_s^e-V_r^e)\·{\stackrel{\→}{L}}_i---\left(6\right)$

其中，X_s ^e、V_s ^e分别为第i颗卫星在协议地球坐标系中的位置、速度，X_r ^e、V_r ^e为GPS接收机的位置、速度，

为卫星和GPS接收机视线方向上的单位矢量；

则多普勒频率估计值为：

${\hat{f}}_{\mathrm{dop}}=-\frac{f_{L1}}{c}\·V_{\mathrm{aid}}=-\frac{f_{L1}}{c}\·(V_s^e-V_r^e)\·{\stackrel{\→}{L}}_i---\left(7\right)$

其中，f_L1为L1载波频率，c为光速；

由SINS导航参数误差所导致的多普勒频率估计误差为：

${\mathrm{\δf}}_{\mathrm{dop}}=-\frac{f_{L1}}{c}\·{\stackrel{\→}{L}}_i^T\·(C_i^e\·\mathrm{\δ}{V}^i-C_c^e\·W_e\·C_i^c\·{\mathrm{\δX}}^i)---\left(8\right)$

其中，δXⁱ、δVⁱ分别为发射点惯性系中载体的位置、速度误差，C_i ^e、C_c ^e、C_i ^c分别为发射点惯性系到协议地球系、地心惯性坐标系到协议地球系、发射点惯性系到协议地球系的转换矩阵，W_e为地球自转角速度矢量在协议地球系中的叉乘矩阵；

建立辅助频率误差模型时，忽略接收机钟频误差估计误差的影响，得：

δf_aid≈δf_dop                               (3)

以跟踪通道内载波环的相位、频率误差作为状态变量，则第i个通道内载波环的跟踪误差模型为：

${\left[\begin{array}{c}\mathrm{\δ}\stackrel{\·}{\mathrm{\θ}}\\ \mathrm{\δ}{\stackrel{\·}{f}}_{\mathrm{TRK}}\end{array}\right]}_i=\left[\begin{array}{cc}0& 2\mathrm{\π}\\ \frac{K_{\mathrm{PLL}}}{T_1}& \frac{2\mathrm{\π}{K}_{\mathrm{PLL}}{T}_2}{T_1}\end{array}\right]{\left[\begin{array}{c}\mathrm{\δ\θ}\\ \mathrm{\δ}{f}_{\mathrm{TRK}}\end{array}\right]}_i+\left[\begin{array}{c}2\mathrm{\π}\\ \frac{2\mathrm{\π}{K}_{\mathrm{PLL}}{T}_2}{T_1}\end{array}\right]\·\mathrm{\δ}{f}_{\mathrm{aid}}---\left(9\right)$

其中，1≤i≤N，N为自然数且N≥4；

以跟踪通道内码环输出的伪距测量误差作为状态变量，则第i个通道内码环的跟踪误差模型为：

$\mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}_{\mathrm{DLLi}}=-K_{\mathrm{DLL}}{\mathrm{\δ\ρ}}_{\mathrm{DLLi}}+{\mathrm{\δV}}_{\mathrm{aid}}+K_{\mathrm{DLL}}Q---\left(10\right)$

其中，δρ_DLLi为伪距测量误差；

步骤二：建立SINS/GPS超紧致组合系统状态方程和量测方程；

在SINS/GPS超紧致组合导航系统中，组合导航滤波器的误差模型包括SINS、GPS误差模型、码环、载波环跟踪误差模型；

a.SINS系统的误差状态方程；

${\stackrel{\·}{X}}_I=F_I{X}_I+G_I{W}_I---\left(11\right)$

其中，X_I为SINS系统状态矢量，W_I为SINS系统噪声矢量，F_I为SINS系统状态矩阵，G_I为系SINS统噪声矩阵，SINS的误差状态包括位置误差(δx，δy，δz)、速度误差(δv_x，δv_y，δv_z)、姿态误差角(φ_x，φ_y，φ_z)、加速度计零偏()、加速度计系数误差(k_a1x，k_a1y，k_a1z，k_a2x，k_a2y，k_a2z)、陀螺仪系数误差(k_w1x，k_w1y，k_w1z)和陀螺仪常值漂移(ε_x，ε_y，ε_z)，

则：

W_I＝[w_ax，w_ay，w_az，w_gx，w_gy，w_gz]^T

$G_i={\left[\begin{array}{cc}{0}_{3\×3}& 0_{3\×3}\\ I_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& -C_b^i\\ 0_{15\×3}& 0_{15\×3}\end{array}\right]}_{24\×6}$

在发射点惯性坐标系下，SINS系统状态矩阵F_I的形式如下：

$F_I={\left[\begin{array}{cccccccc}{0}_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ F_g& 0_{3\×3}& B& C_1& C_2& C_b^i& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& C_3& -C_b^i\\ & & & 0_{15\×24}& & & & \end{array}\right]}_{24\×24},$ $B=-C_b^i\left[\begin{array}{ccc}0& f_z^b& -f_y^b\\ -f_z^b& 0& f_x^b\\ f_y^b& -f_x^b& 0\end{array}\right]$

$C_1=C_b^i\left[\begin{array}{ccc}{f}_x^b& & \\ & f_y^b& \\ & & f_z^b\end{array}\right],$ $C_2=C_b^i\left[\begin{array}{ccc}{\left(f_x^b\right)}^2& & \\ & {\left(f_y^b\right)}^2& \\ & & {\left(f_z^b\right)}^2\end{array}\right],$ $C_3=-C_b^i\left[\begin{array}{ccc}{\mathrm{\ω}}_x^b& & \\ & {\mathrm{\ω}}_y^b& \\ & & {\mathrm{\ω}}_z^b\end{array}\right]$

其中，F_g为引力加速度对位置坐标的雅克比矩阵，C_b ⁱ为本体坐标系到发射点惯性系的坐标转换阵，[f_x ^b f_y ^b f_z ^b]^T和[ω_x ^b ω_y ^b ω_x ^b]^T分别为加速度计和陀螺仪输出的比力和角速度信息；

b.GPS误差状态方程；

${\stackrel{\·}{X}}_G=F_G{X}_G+G_G{W}_G---\left(12\right)$

其中，X_G为GPS误差状态变量，W_G为GPS系统噪声矢量，F_G为GPS系统状态矩阵，G_G为GPS系统噪声矩阵，GPS的误差状态包括两个与时间有关的误差：与时钟误差等效的距离误差δl_u，与时钟频率误差等效的距离率误差δl_ru，T_ru为相关时间，

X_G＝[δl_u，δl_ru]^T，W_G＝[w_u，w_ru]^T， $F_G=\left[\begin{array}{cc}0& 1\\ 0& -\frac{1}{T_{\mathrm{ru}}}\end{array}\right],$ $G_G=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]$

c.码环跟踪误差方程；

${\stackrel{\·}{X}}_D=F_D{X}_D+G_D{W}_D---\left(13\right)$

其中，X_D为码环误差状态变量，W_D为码环噪声矢量，F_D为码环的系统状态矩阵，G_D为码环的系统噪声矩阵；GPS接收机选择N颗导航星座，并启动N个跟踪通道工作，δρ_DLLi(i＝1，2，…N)为N颗卫星对应的伪距跟踪误差，

X_D＝[δρ_DLL1 δρ_DLL2…δρ_DLLN]^T，W_D＝[Q₁，Q₂，…Q_N]^T

$F_D=-{\left[\begin{array}{cccc}{K}_{1\mathrm{DLL}}& & & \\ & K_{2\mathrm{DLL}}& & \\ & & ...& \\ & & & K_{\mathrm{NDLL}}\end{array}\right]}_{N\×N},$ $G_D=\left[\begin{array}{cccc}{K}_{1\mathrm{DLL}}& & & \\ & K_{2\mathrm{DLL}}& & \\ & & ...& \\ & & & K_{\mathrm{NDLL}}\end{array}\right]$

d.载波环跟踪误差方程；

GPS接收机内N个通道的载波环跟踪误差方程：

${\stackrel{\·}{X}}_P=F_P{X}_P+G_P{W}_P---\left(14\right)$

其中，X_P为载波环的误差状态变量，W_P为载波环系统噪声序列，G_P为载波环系统噪声矩阵，F_P为载波环系统状态矩阵，δθ_i、δf_TRKi分别为第i个通道内载波跟踪环的相位误差和环路滤波器输出，K_PLL为环路增益，

X_P＝[δθ₁，δθ₂，…δθ_N，δ_fTRK1，δ_fTRK2，…δ_fTRKN]^T

$F_P={\left[\begin{array}{cc}{0}_N& 2\mathrm{\π}\·I_N\\ K_{\mathrm{PLL}}/T_1\·I_N& 2\mathrm{\π}{K}_{\mathrm{PLL}}{T}_2/T_1\·I_N\end{array}\right]}_{2N\×2N}$

e.SINS/GPS超紧致组合系统状态方程；

将SINS、GPS误差方程以及GPS码环、载波环跟踪误差方程合并，得到超紧致组合系统的状态方程：

$\stackrel{\·}{X}=\mathrm{FX}+\mathrm{GW}---\left(15\right)$

其中，X为超紧致组合系统状态矢量，F为超紧致组合系统状态矩阵，W为超紧致组合系统噪声矢量，G为超紧致组合系统噪声矩阵，

X＝[X_I X_G X_D X_P]^T，W＝[W_I W_G W_D W_P]^T

$F=\left[\begin{array}{cccc}{F}_I& & & \\ & F_G& & \\ F_{\mathrm{ID}}& & F_D& \\ F_{\mathrm{IP}}& & & F_P\end{array}\right],$ $G=\left[\begin{array}{cccc}{G}_I& & & \\ & G_G& & \\ & & G_D& \\ & & & G_P\end{array}\right]$

$F_I={\left[\begin{array}{cccccccc}{0}_{3\×3}& I_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ F_g& 0_{3\×3}& B& C_1& C_2& C_b^i& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& C_3& -C_b^i\\ & & & 0_{15\×24}& & & & \end{array}\right]}_{24\×24},$ $F_G=\left[\begin{array}{cc}0& 1\\ 0& -\frac{1}{T_{\mathrm{ru}}}\end{array}\right]$

$F_{\mathrm{ID}}={\left[\begin{array}{ccc}-{\stackrel{\→}{L}}_1^T\·C_c^e\·W_e\·C_i^c& {\stackrel{\→}{L}}_1^T\·C_i^e& 0_{1\×18}\\ -{\stackrel{\→}{L}}_2^T\·C_c^e\·W_e\·C_i^c& {\stackrel{\→}{L}}_2^T\·C_i^e& 0_{1\×18}\\ ...& ...& ...\\ -{\stackrel{\→}{L}}_N^T\·C_c^e\·W_e\·C_i^c& {\stackrel{\→}{L}}_N^T\·C_i^e& 0_{1\×18}\end{array}\right]}_{N\×24},$ $F_D={-\left[\begin{array}{cccc}{K}_{1\mathrm{DLL}}& & & \\ & K_{2\mathrm{DLL}}& & \\ & & ...& \\ & & & K_{\mathrm{NDLL}}\end{array}\right]}_{N\×N}$

$F_{\mathrm{IP}}=-2\mathrm{\π}\frac{f_{L1}}{c}\·{\left[\begin{array}{c}{F}_{\mathrm{ID}}\\ F_{\mathrm{ID}}\·\frac{T_2}{T_1}\·K_{\mathrm{PLL}}\end{array}\right]}_{2N\×24},$ $F_P={\left[\begin{array}{cc}{0}_N& 2\mathrm{\π}\·I_N\\ K_{\mathrm{PLL}}/T_1\·I_N& 2\mathrm{\π}{K}_{\mathrm{PLL}}{T}_2/T_1\·I_N\end{array}\right]}_{2N\×2N}$

f.SINS/GPS超紧致组合系统量测方程；

在SINS/GPS超紧致组合导航系统中，以伪距、伪距率为量测信息，一个跟踪通道内的伪距、伪距率量测方程为

$\left\{\begin{array}{c}\mathrm{\δ\ρ}={\mathrm{\ρ}}_I-{\mathrm{\ρ}}_G={\mathrm{\δ\ρ}}_I-({\mathrm{\δl}}_u+{\mathrm{\υ}}_{\mathrm{\ρ}}+{\mathrm{\δ\ρ}}_{\mathrm{DLL}})\\ \mathrm{\δ}\stackrel{\·}{\mathrm{\ρ}}={\stackrel{\·}{\mathrm{\ρ}}}_I-{\stackrel{\·}{\mathrm{\ρ}}}_G=\mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}_I-({\mathrm{\δl}}_{\mathrm{ru}}+{\mathrm{\υ}}_{\stackrel{\·}{\mathrm{\ρ}}}+\mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}_{\mathrm{PLL}})\end{array}\right.---\left(16\right)$

其中，δρ和

分别为SINS和GPS的伪距差、伪距率差，δρ_I和

为由SINS导航误差导致的卫星与载体间的距离、距离率误差，

为GPS接收机的量测噪声，δρ_DLL、

为由GPS接收机跟踪误差导致的伪距、伪距率测量误差；

将N个通道的伪距、伪距率量测方程合并，得到超紧致组合系统的量测方程：

Z＝HX+V                   (17)

其中，Z超紧致组合系统的为观测矢量，H为超紧致组合系统的观测矩阵，V为超紧致组合系统的量测噪声序列，

$Z={[{\mathrm{\δ\ρ}}_1,{\mathrm{\δ\ρ}}_2,...{\mathrm{\δ\ρ}}_N,\mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}_1,\mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}_2,...\mathrm{\δ}{\stackrel{\·}{\mathrm{\ρ}}}_N]}^T,$ $V={[{\mathrm{\υ}}_{\mathrm{\ρ}1},{\mathrm{\υ}}_{\mathrm{\ρ}2},...{\mathrm{\υ}}_{\mathrm{\ρN}},{\mathrm{\υ}}_{\stackrel{\·}{\mathrm{\ρ}}1},{\mathrm{\υ}}_{\stackrel{\·}{\mathrm{\ρ}}2},...{\mathrm{\υ}}_{\stackrel{\·}{\mathrm{\ρ}}N}]}^T$

$H=\left[\begin{array}{c}{H}_{\mathrm{\ρ}}\\ H_{\stackrel{\·}{\mathrm{\ρ}}}\end{array}\right]={\left[\begin{array}{ccccccc}{\stackrel{\→}{L}}^T\·C_i^e& 0_{N\×21}& -I_{N\×1}& 0_{N\×1}& -I_{N\×N}& 0_{N\×N}& 0_{N\×N}\\ 0_{N\×3}& 0_{N\×21}& 0_{N\×1}& -I_{N\×1}& 0_{N\×N}& 0_{N\×N}& c/f_{L1}\·I_N\end{array}\right]}_{2N\×(26+3N)}$

其中，δρ_i，

为四颗卫星对应的伪距、伪距率差，

为第i颗卫星和接收机视线方向上的单位矢量；

步骤三：组合导航信息融合；

组合导航滤波器根据SINS与GPS输出的伪距、伪距率量测信息，对SINS、GPS以及码环、载波环跟踪误差状态进行估计；将滤波器估计的SINS误差状态反馈回SINS系统中，对导航参数及元件误差进行校正，将接收机钟频误差估计信息传递给辅助参数计算单元；

步骤四：辅助参数计算；

辅助参数计算单元根据校正后的SINS导航参数、卫星参数和接收机钟频误差估计信息计算卫星与载体之间的距离ρ_i和距离率

转换为C/A码相位估计值和辅助频率估计值，并将C/A码相位和辅助频率的估计值分别提供给GPS接收机码环中的C/A码数控振荡器和载波环中的载波数控振荡器，为GPS接收机的伪码、载波跟踪提供辅助。

2、根据权利要求1所述的一种SINS/GPS超紧致组合导航系统实现方法，其特征在于，所述的步骤四中的辅助参数计算为GPS接收机处于组合工作模式，当SINS/GPS超紧致组合导航系统开始工作时，先使GPS接收机处于独立工作模式，利用载波环的输出信息来校正SINS误差；当多普勒频率估计误差δf_dop小于载波环的跟踪带宽时，启动组合工作模式，通过辅助参数计算单元为GPS接收机的码环和载波环提供辅助信息。

3、一种SINS/GPS超紧致组合导航系统，其特征在于：包括捷联惯性导航系统、GPS接收机、组合导航滤波器和辅助参数计算单元；

捷联惯性导航系统包括惯性测量元件和导航解算单元；惯性测量元件测量载体的比力和角速率，将得到的比力和角速率信息传送给导航解算环节，导航解算单元根据惯性测量元件传输的信息得到载体的位置、速度和姿态，并转换为卫星与载体之间的距离ρ_i和距离率

输入到组合导航滤波器中，导航解算单元将SINS导航参数输入到辅助参数计算单元中，所述的SINS导航参数为载体的位置和速度；

GPS接收机包括天线、射频前端、码环和载波环；射频前端对天线接收到的GPS射频信号进行处理，得到GPS中频信号，GPS中频信号经过混频，得到混频后的GPS基带信号；

码环包括码相位鉴别器、环路滤波器A和C/A码数控振荡器，C/A码数控振荡器产生本地C/A码，混频后的GPS基带信号与本地C/A码进行相关运算，并将相关结果输入到码相位鉴别器中，码相位鉴别器得到码相位差，将得到的码相位差输入到环路滤波器A，相位差经过环路滤波器A滤波后，对C/A码数控振荡器输出控制信号，C/A码数控振荡器根据控制信号和辅助信息调整本地C/A码相位，使得本地C/A码相位与输入的GPS中频信号中的码相位对准；

所述的辅助信息为载波环提供的辅助信息和辅助参数计算单元提供的C/A码相位估计值；

载波环包括载波相位鉴别器、环路滤波器B、环路滤波器C、载波数控振荡器和比例转换单元，射频前端输出的GPS中频信号与载波数控振荡器生成的本地载波余弦、正弦信号进行混频，得到同相、正交两路混频后的GPS基带信号，混频后的GPS基带信号与本地C/A码进行相关运算，相关结果输入到载波相位鉴别器中，载波相位鉴别器得到输入GPS中频信号与本地载波之间的相位差，输出载波相位差信号，根据GPS接收机的工作模式选择环路滤波器B或环路滤波器C对载波相位差信号进行滤波处理；

载波数控振荡器根据不同模式下的控制信号调整本地载波频率和相位，使之与输入GPS中频信号中的载波频率、相位对准；同时，载波数控振荡器将调整后的本地载波频率传递到比例转换单元中，比例转换单元将本地载波频率转换为C/A码频率输入C/A码数控振荡器中，对码环进行辅助，C/A码频率即为载波环为C/A码数控振荡器提供的辅助信息；

码环和载波环分别输出码相位和载波频率信息，将其转换为伪距ρ_G、伪距率

作为量测信息输入到组合导航滤波器中；

组合导航滤波器根据码环、载波环和导航解算单元提供的伪距ρ_G、伪距率

和距离ρ_I、距离率

对捷联惯性导航系统的导航参数和惯性器件的误差进行估计，并将其反馈回SINS导航解算单元中，对相应的误差进行校正和补偿，同时将接收机钟频误差估计信息传递给辅助参数计算单元；

辅助参数计算单元根据校正后的SINS导航参数、卫星参数和接收机钟频误差估计信息计算得到辅助参数，所述的辅助参数为C/A码相位估计值和辅助频率估计值，将C/A码相位估计值和辅助频率估计值分别提供给GPS接收机的码环中的C/A码数控振荡器和载波环中的载波数控振荡器。

4、根据权利要求3所述的一种SINS/GPS超紧致组合导航系统，其特征在于：所述的GPS接收机的工作模式包括独立工作模式和组合工作模式，当GPS接收机处于独立工作模式时，载波环中a、c连通，环路滤波器B工作，对载波数控振荡器的控制信号为环路滤波器B的输出信号；当GPS接收机与SINS组成组合系统，处于组合工作模式时，载波环中a、b连通，环路滤波器C工作，对载波数控振荡器的控制信号包括环路滤波器C的输出信号和辅助参数计算单元输出的辅助频移估计值。

5、根据权利要求3所述的一种SINS/GPS超紧致组合导航系统，其特征在于：所述的载波环对码环的辅助设置为可选，当载波环正常工作时，连接触点d、f，利用载波频率信息对码环进行辅助，如果载波环发生异常，则连接触点d、e，捷联惯性导航系统通过辅助参数计算单元辅助码环。

Images