[发明专利]一种多卫星系统与捷联惯性导航系统紧组合导航方法

摘要

本发明公开了一种多卫星系统与捷联惯性导航系统紧组合导航方法，首先对IMU输出数据捷联解算获得SINS当前姿态、速度、位置信息。对卫星星历报文进行星历解算得到各卫星位置、速度，各卫星系统时空统一，进行卫星选星。其次，利用选取的卫星位置、速度信息与捷联解算得到的SINS位置、速度信息计算得到SINS伪距、伪距率。再次，利用GNSS输出的原始测量数据，即测码伪距和多普勒频移，与SINS伪距、伪距率进行比较，将差值作为滤波器的观测量，经过滤波器的最优估计，给出校正SINS的补偿量，闭环校正，得到SINS姿态、速度、位置最优解。本发明解决了惯导与单卫星组合导航在高楼遮挡、树木掩盖等复杂环境下卫星数目过少影响紧组合导航精度的问题。

主权项

一种多卫星系统与捷联惯性导航系统紧组合导航方法，其特征在于，包括以下步骤：(1)对惯性测量元件(IMU)输出数据通过捷联解算获得捷联惯导当前姿态、速度、位置，对卫星系统接收机输出的星历报文进行卫星数据处理，获得卫星的速度、位置信息；(2)利用卫星速度、位置与SINS位置、速度信息通过SINS伪距、伪距率计算得到SINS伪距、伪距率；(3)从卫星系统接收机输出的伪距报文获得测码伪距和多普勒频移，与SINS伪距、伪距率进行比较，将差值作为滤波器的观测量，建立紧组合状态模型及量测模型，经过最优估计与滤波，输出系统的误差校正量；对系统误差进行校正，输出SINS姿态、速度、位置的最优组合解；所述的紧组合状态模型及量测模型的建立方法如下：(3.1)多系统紧组合状态方程建立将捷联误差方程和卫星系统建模转为状态方程描述$\left[\begin{array}{c}{\stackrel{\·}{X}}_I\left(t\right)\\ {\stackrel{\·}{X}}_{GPS}\left(t\right)\\ {\stackrel{\·}{X}}_{GLO}\left(t\right)\\ {\stackrel{\·}{X}}_{Gal}\left(t\right)\\ {\stackrel{\·}{X}}_{BD}\left(t\right)\end{array}\right]=\left[\begin{array}{ccccc}{F}_I\left(t\right)& 0& 0& 0& 0\\ 0& F_{GPS}\left(t\right)& 0& 0& 0\\ 0& 0& F_{GLO}\left(t\right)& 0& 0\\ 0& 0& 0& F_{Gal}\left(t\right)& 0\\ 0& 0& 0& 0& F_{BD}\left(t\right)\end{array}\right]\left[\begin{array}{c}{X}_I\left(t\right)\\ X_{GPS}\left(t\right)\\ X_{GLO}\left(t\right)\\ X_{Gal}\left(t\right)\\ X_{BD}\left(t\right)\end{array}\right]+\left[\begin{array}{ccccc}{G}_I\left(t\right)& 0& 0& 0& 0\\ 0& G_{GPS}\left(t\right)& 0& 0& 0\\ 0& 0& G_{GLO}\left(t\right)& 0& 0\\ 0& 0& 0& G_{Gal}\left(t\right)& 0\\ 0& 0& 0& 0& F_{BD}\left(t\right)\end{array}\right]\left[\begin{array}{c}{W}_I\left(t\right)\\ W_{GPS}\left(t\right)\\ W_{GLO}\left(t\right)\\ W_{Gal}\left(t\right)\\ W_{BD}\left(t\right)\end{array}\right]$其中，X∈R²³是状态变量，由SINS的15个状态量、美国全球定位系统(GPS)、格洛纳斯卫星系统(GLONASS)、伽利略卫星系统(Galileo)、北斗卫星系统(BD)各2个状态量组成；$\begin{array}{c}X=\\ \left[\begin{array}{ccccccccccccccccccccccc}{\mathrm{\δV}}_E& {\mathrm{\δV}}_N& {\mathrm{\δV}}_U& {\mathrm{\φ}}_E& {\mathrm{\φ}}_N& {\mathrm{\φ}}_U& \mathrm{\δ}L& \mathrm{\δ}\mathrm{\λ}& \mathrm{\δ}h& {\▿}_{bx}& {\▿}_{by}& {\▿}_{bz}& {\mathrm{\ϵ}}_{bx}& {\mathrm{\ϵ}}_{by}& {\mathrm{\ϵ}}_{bz}& {\mathrm{\δt}}_{GPS}& {\mathrm{\δt}}_{GLO}& {\mathrm{\δt}}_{Gal}& {\mathrm{\δt}}_{BD}& {\mathrm{\δt}}_{rGPS}& {\mathrm{\δt}}_{rGLO}& {\mathrm{\δt}}_{rGal}& {\mathrm{\δt}}_{rBD}\end{array}\right]\end{array}$式中，δV_E,δV_N,δV_U是在东北天三个方向上的速度误差，捷联的三个平台姿态角，δL,δλ,δh是捷联的三个位置误差由地球坐标系描述，是加表三个轴向的偏置误差，ε_bx,ε_by,ε_bz是陀螺的三个轴向漂移，δt_GPS、δt_GLO、δt_Gal、δt_BD分别是GPS、GLONASS、Galileo、BD卫星时钟误差等效的距离，δt_rGPS、δt_rGLO、δt_rGal、δt_rBD分别是GPS、GLONASS、Galileo、BD卫星时钟频率误差等效的距离变化率，共23维；F_I(t)是惯导系统误差状态方程状态矩阵，而F_GPS(t)、F_GLO(t)、F_Gal(t)、F_BD(t)分别是对GPS、GLONASS、Galileo、BD卫星相关的两个状态量建模后得到的状态矩阵，G_I(t)和G_GPS(t)、G_GLO(t)、G_Gal(t)、G_BD(t)是噪声的输入矩阵；δt_*表示建模后其激励为白噪声，建模为一阶马尔可夫过程，*代表各系统GPS、GLONASS、Galileo、BD$F_{*}=\left[\begin{array}{cc}0& 1\\ 0& -{\mathrm{\β}}_{tr*}\end{array}\right],G_{*}=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right],W_{*}\left(t\right)=\left[\begin{array}{c}{\mathrm{\ω}}_{t*}\\ {\mathrm{\ω}}_{tr*}\end{array}\right];$(3.2)多系统紧组合量测方程建立(a)伪距量测方程建立δt_*是站钟误差，表示站钟钟差等效距离误差，v_ρ*j表示其他公共误差和独有误差，如电离层误差、对流层误差和多路径误差等测量误差；在紧组合仿真中，v_ρ*j认为是一个白噪声形式的误差，钟差误差则多个通道统一设置；在紧组合样机实验中，则必须对电离层延迟等诸多误差做补偿，补偿的方法在下面进行讨论；取卫星，写成矩阵形式有$\left[\begin{array}{c}{\mathrm{\δ\ρ}}_{GPS}\\ {\mathrm{\δ\ρ}}_{GLO}\\ {\mathrm{\δ\ρ}}_{Gal}\\ {\mathrm{\δ\ρ}}_{BD}\end{array}\right]=\left[\begin{array}{ccccc}{H}_{GPS}& 1_{GPS}& 0& 0& 0\\ H_{GLO}& 0& 1_{GLO}& 0& 0\\ H_{Gal}& 0& 0& 1_{Gal}& 0\\ H_{BD}& 0& 0& 0& 1_{BD}\end{array}\right]\left[\begin{array}{c}\mathrm{\δ}p\\ {\mathrm{\δt}}_{GPS}\\ {\mathrm{\δt}}_{GLO}\\ {\mathrm{\δt}}_{Gal}\\ {\mathrm{\δt}}_{BD}\end{array}\right]+\left[\begin{array}{c}{\mathrm{\υ}}_{\mathrm{\ρ}GPS}\\ {\mathrm{\υ}}_{\mathrm{\ρ}GLO}\\ {\mathrm{\υ}}_{\mathrm{\ρ}Gal}\\ {\mathrm{\υ}}_{\mathrm{\ρ}BD}\end{array}\right]$其中，δp＝[δx δy δz]^T为用户状态量，H_*为几何观测矩阵；其维数与所选系统卫星数目(n)有关即为2；H_*＝[e₁ e₂ e₃]_2*3，e_k为2*1维；伪距观测量的量测方程Z_ρ(t)＝H_ρ(t)X(t)+V_ρ(t)其中H_ρ(t)＝[0_N*3 0_N*3 H_ρ1 0_N*6 H_ρ2 0_N*3]_N*23，N为8，即卫星的总数；H_ρ＝(a_ji)_N*3，且$H_{\mathrm{\ρ}GPS}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 1& 0& 0& 0\end{array}\right],H_{\mathrm{\ρ}GLO}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 1& 0& 0& 0\end{array}\right],H_{\mathrm{\ρ}Gal}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 1& 0& 0& 0\end{array}\right],H_{\mathrm{\ρ}BD}=\left[\begin{array}{cccc}1& 0& 0& 0\\ 1& 0& 0& 0\end{array}\right]$a_j1＝(R_n+h)[‑e_j1sinLcosλ‑e_j2sinLsinλ]＋[R_n*(1‑e²)＋h]e_j3cosLa_j2＝(R_n＋h)[e_j2cosLcosλ‑e_j1cosLsinλ]a_j3＝e_j1cosLcosλ+e_j2cosLsinλ+e_j3sinL(b)伪距率量测方程建立伪距率观测量的量测方程$Z_{\stackrel{\·}{\mathrm{\ρ}}}\left(t\right)=H_{\stackrel{\·}{\mathrm{\ρ}}}\left(t\right)X\left(t\right)+V_{\stackrel{\·}{\mathrm{\ρ}}}\left(t\right)$式中，其中，b_j1＝‑e_*j1sinλ+e_*j2cosλb_j2＝‑e_*j2sinLcosλ‑e_*j1sinLsinλ+e_*j3cosLb_j3＝e_*j1cosLcosλ+e_*j2cosLsinλ+e_*j3sinL$e_{j1}=(R_n+h)\[-{\stackrel{\·}{e}}_{*j1}\sin Lcos\mathrm{\λ}-{\stackrel{\·}{e}}_{*j2}\sin Lsin\mathrm{\λ}\]+\[R_n*(1-e^2)+h\]{\stackrel{\·}{e}}_{*j3}\cos L$$c_{j2}=(R_n+h)\[{\stackrel{\·}{e}}_{*j2}\cos Lcos\mathrm{\λ}-{\stackrel{\·}{e}}_{*j1}\cos Lsin\mathrm{\λ}\]$$c_{j3}={\stackrel{\·}{e}}_{*j1}\cos Lcos\mathrm{\λ}+{\stackrel{\·}{e}}_{*j2}\cos Lsin\mathrm{\λ}+{\stackrel{\·}{e}}_{*j3}\sin L$综(a)、(b)得量测方程：$Z\left(t\right)=\left[\begin{array}{c}{H}_{\mathrm{\ρ}}\\ H_{\stackrel{\·}{\mathrm{\ρ}}}\end{array}\right]X\left(t\right)+\left[\begin{array}{c}{V}_{\mathrm{\ρ}}\left(t\right)\\ V_{\stackrel{\·}{\mathrm{\ρ}}}\left(t\right)\end{array}\right].$