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

摘要

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

主权项

1.一种SINS/GPS超紧致组合导航系统实现方法，其特征在于，具体包括以下步骤：步骤一：建立GPS接收机中载波环的跟踪误差模型和码环的跟踪误差模型；当GPS接收机处于独立工作模式时在载波环中，本地载波相位与参考载波相位θ的相位误差δθ由载波相位鉴别器确定，经过环路滤波器B处理后，得到频率跟踪误差δf，用于调整本地载波数控振荡器中的载波频率，从而使本地信号与输入信号的载波频率、相位保持一致；根据二阶载波环的基本结构，独立工作模式下载波环的跟踪误差方程：$\left\{\begin{array}{c}\delta \stackrel{·}{\theta }=2\mathrm{\pi \delta f}\\ \delta \stackrel{·}{f}=[2\pi \frac{t_2}{t_1}\delta \stackrel{·}{\theta }+\frac{\mathrm{\delta \theta }}{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}\delta \stackrel{·}{\theta }=2\pi ({\mathrm{\delta f}}_{\mathrm{TRK}}+{\text{δf}}_{\mathrm{aid}})\\ \delta {\stackrel{·}{f}}_{\mathrm{TRK}}=[2\pi \frac{T_2}{T_1}({\mathrm{\delta f}}_{\mathrm{TRK}}+{\mathrm{\delta f}}_{\mathrm{aid}})+\frac{\mathrm{\delta \theta }}{T_1}]·K_{\mathrm{PLL}}\end{array}\right.---\left(4\right)$其中，δθ为载波环中的相位误差，K_PLL为环路增益，T₁、T₂为环路滤波器C的参数，δf_TRK为环路滤波器C的输出量，δf_aid为辅助频率误差；当接收机处于组合工作模式时，采用一阶码环，则码环的误差方程为：$\delta {\stackrel{·}{\rho }}_{\mathrm{DLL}}=-K_{\mathrm{DLL}}{\mathrm{\delta \rho }}_{\mathrm{DLL}}+{\mathrm{\delta 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)·{\overrightarrow{L}}_i---\left(6\right)$其中，分别为第i颗卫星在协议地球坐标系中的位置、速度，为GPS接收机的位置、速度，为卫星和GPS接收机视线方向上的单位矢量；则多普勒频率估计值为：其中，f_L1为载波环中载波频率，c为光速；由SINS导航参数误差所导致的多普勒频率估计误差为：${\mathrm{\delta f}}_{\mathrm{dop}}=-\frac{f_{L1}}{c}·{\overrightarrow{L}}_i^T·(C_i^e·{\mathrm{\delta V}}^i-C_c^e·W_e·C_i^c·{\mathrm{\delta X}}^i)---\left(8\right)$其中，δXⁱ、δVⁱ分别为发射点惯性系中载体的位置、速度误差，分别为发射点惯性系到协议地球系、地心惯性坐标系到协议地球系、发射点惯性系到地心惯性坐标系的转换矩阵，W_e为地球自转角速度矢量在协议地球系中的叉乘矩阵；建立辅助频率误差模型时，忽略接收机钟频误差估计误差的影响，得：δf_aid≈δf_dop             (3)以跟踪通道内载波环的相位、频率误差作为状态变量，则第i个通道内载波环的跟踪误差模型为：${\left[\begin{array}{c}\delta \stackrel{·}{\theta }\\ \delta {\stackrel{·}{f}}_{\mathrm{TRK}}\end{array}\right]}_i=\left[\begin{array}{cc}0& 2\pi \\ \frac{K_{\mathrm{PLL}}}{T_1}& \frac{2\pi K_{\mathrm{PLL}}{T}_2}{T_1}\end{array}\right]{\left[\begin{array}{c}\mathrm{\delta \theta }\\ {\mathrm{\delta f}}_{\mathrm{TRK}}\end{array}\right]}_i+\left[\begin{array}{c}2\pi \\ \frac{2\pi K_{\mathrm{PLL}}{T}_2}{T_1}\end{array}\right]·{\mathrm{\delta f}}_{\mathrm{aid}}---\left(9\right)$其中，1≤i≤N，N为自然数且N≥4；以跟踪通道内码环输出的伪距测量误差作为状态变量，则第i个通道内码环的跟踪误差模型为：$\delta {\stackrel{·}{\text{ρ}}}_{\mathrm{DLLi}}=-K_{\mathrm{DLL}}\delta {\rho }_{\mathrm{DLLi}}+{\mathrm{\delta 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\times 3}& 0_{3\times 3}\\ I_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& {-C}_b^i\\ 0_{15\times 3}& 0_{15\times 3}\end{array}\right]}_{24\times 6}$在发射点惯性坐标系下，SINS系统状态矩阵F_I的形式如下：$F_I={\left[\begin{array}{c}\begin{array}{cccccccc}{0}_{3\times 3}& I_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ F_g& 0_{3\times 3}& B& C_1& C_2& C_b^i& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& C_3& {-C}_b^i\end{array}\\ 0_{15\times 24}\end{array}\right]}_{24\times 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}{\omega }_x^b& & \\ & {\omega }_y^b& \\ & & {\omega }_z^b\end{array}\right]$其中，F_g为引力加速度对位置坐标的雅克比矩阵，为本体坐标系到发射点惯性系的坐标转换阵，和分别为加速度计和陀螺仪输出的比力和角速度信息；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\times 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，δf_TRK1，δf_TRK2，...δf_TRKN]^T$F_P={\left[\begin{array}{cc}{0}_N& 2\pi ·I_N\\ K_{\mathrm{PLL}}/T_1·I_N& 2\pi K_{\mathrm{PLL}}{T}_2/T_1·I_N\end{array}\right]}_{2N\times 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}{c}\begin{array}{cccccccc}{0}_{3\times 3}& I_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ F_g& 0_{3\times 3}& B& C_1& C_2& C_b^i& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& C_3& {-C}_b^i\end{array}\\ 0_{15\times 24}\end{array}\right]}_{24\times 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}-{\overrightarrow{L}}_1^T·C_c^e·W_e·C_i^c& {\overrightarrow{L}}_1^T·C_i^e& 0_{1\times 18}\\ -{\overrightarrow{L}}_2^T·C_c^e·W_e·C_i^c& {\overrightarrow{L}}_2^T·C_i^e& 0_{1\times 18}\\ ···& ···& ···\\ -{\overrightarrow{L}}_N^T·C_c^e·W_e·C_i^c& {\overrightarrow{L}}_N^T·C_i^e& 0_{1\times 18}\end{array}\right]}_{N\times 24},$$F_D=-{\left[\begin{array}{cccc}{K}_{1\mathrm{DLL}}& & & \\ & K_{2\mathrm{DLL}}& & \\ & & ···& \\ & & & K_{\mathrm{NDLL}}\end{array}\right]}_{N\times N}$$F_{\mathrm{IP}}=-2\pi \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\times 24},$$F_P={\left[\begin{array}{cc}{0}_N& 2\pi ·I_N\\ K_{\mathrm{PLL}}/T_1·I_N& 2\pi K_{\mathrm{PLL}}{T}_2/T_1·I_N\end{array}\right]}_{2N\times 2N}$f.SINS/GPS超紧致组合系统量测方程；在SINS/GPS超紧致组合导航系统中，以伪距、伪距率为量测信息，一个跟踪通道内的伪距、伪距率量测方程为$\left\{\begin{array}{c}\mathrm{\delta \rho }={\rho }_I-{\rho }_G={\mathrm{\delta \rho }}_I-({\mathrm{\delta l}}_u+{\upsilon }_{\rho }+{\mathrm{\delta \rho }}_{\mathrm{DLL}})\\ \delta \stackrel{·}{\rho }={\stackrel{·}{\rho }}_I-{\stackrel{·}{\rho }}_G={\delta \stackrel{·}{\rho }}_I-({\mathrm{\delta l}}_{\mathrm{ru}}+{\upsilon }_{\stackrel{·}{\rho }}+{\delta \stackrel{·}{\rho }}_{\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{\delta \rho }}_1,{\mathrm{\delta \rho }}_2,···{\mathrm{\delta \rho }}_N,{\delta \stackrel{·}{\rho }}_1,\delta {\stackrel{·}{\rho }}_2,···\delta {\stackrel{·}{\rho }}_N]}^T,V={[{\upsilon }_{\rho 1},{\upsilon }_{\rho 2},···{\upsilon }_{\mathrm{\rho N}},{\upsilon }_{\stackrel{·}{\rho }1},{\upsilon }_{\stackrel{·}{\rho }2},···{\upsilon }_{\stackrel{·}{\rho }N}]}^T$$H=\left[\begin{array}{c}{H}_{\rho }\\ H_{\stackrel{·}{\rho }}\end{array}\right]={\left[\begin{array}{ccccccc}{\overrightarrow{L}}^T·C_i^e& 0_{N\times 21}& {-I}_{N\times 1}& 0_{N\times 1}& {-I}_{N\times N}& 0_{N\times N}& 0_{N\times N}\\ 0_{N\times 3}& 0_{N\times 21}& 0_{N\times 1}& {-I}_{N\times 1}& 0_{N\times N}& 0_{N\times N}& c/f_{L1}·I_N\end{array}\right]}_{2N\times (26+3N)}$其中，为四颗卫星对应的伪距、伪距率差，为第i颗卫星和接收机视线方向上的单位矢量；步骤三：组合导航信息融合；组合导航滤波器根据SINS与GPS输出的伪距、伪距率量测信息，对SINS、GPS以及码环、载波环跟踪误差状态进行估计；将滤波器估计的SINS误差状态反馈回SINS系统中，对导航参数及元件误差进行校正，将接收机钟频误差估计信息传递给辅助参数计算单元；步骤四：辅助参数计算；辅助参数计算单元根据校正后的SINS导航参数、卫星参数和接收机钟频误差估计信息计算卫星与载体之间的距离ρ_I和距离率转换为C/A码相位估计值和辅助频率估计值，并将C/A码相位和辅助频率估计值分别提供给GPS接收机码环中的C/A码数控振荡器和载波环中的载波数控振荡器，为GPS接收机的伪码、载波跟踪提供辅助；当SINS/GPS超紧致组合导航系统开始工作时，先使GPS接收机处于独立工作模式，利用载波环的输出信息来校正SINS误差；当多普勒频率估计误差δf_dop小于载波环的跟踪带宽时，启动组合工作模式，通过辅助参数计算单元为GPS接收机的码环和载波环提供辅助信息。