CN103968843A - 一种gps/sins超紧组合导航系统自适应混合滤波方法 - Google Patents

Abstract

本发明涉及一种GPS/SINS超紧组合导航系统自适应混合滤波方法，其特征在于:步骤1：建立GPS/SINS超紧组合导航系统误差模型；步骤2：采用卡尔曼滤波方法进行数据融合，得到系统状态的最优估计值和估计误差方程阵P；步骤3：利用步骤2中获得的系统状态的最优估计值对SINS输出的导航定位信息进行输出校正；步骤4：采用误差方差阵的特征值和特征向量的可观测性分析方法，利用步骤2中获得的估计误差方程阵P求取系统各个状态的可观测度信息；步骤5：将步骤4中获得的系统状态可观测度信息作为反馈因子，将其与步骤2中获得的系统状态的最优估计值的乘积作为反馈量，对GPS和SINS的参数进行反馈校正。

Description

一种GPS/SINS超紧组合导航系统自适应混合滤波方法

技术领域

本发明涉及一种GPS/SINS超紧组合导航系统自适应混合滤波方法。

背景技术

全球定位系统(GPS，Global Positioning System)与捷联惯性导航系统(SINS，Strapdown Inertial Navigation System)构成的组合导航系统，以其良好的优势互补特性，成为组合导航系统的一个重要分支。GPS/SINS组合导航系统按照组合方式可分为：松组合、紧组合、超紧组合。与松组合和紧组合相比，超紧组合是将GPS和SINS的信息进行更深层次的融合。融合的结果既可以校正惯性器件，抑制误差累积，又可以修正接收机参数，提高其对卫星信号的跟踪能力，因此GPS/SINS超紧组合导航系统必将在航空、航天、航海等领域得到广泛的应用。

GPS/SINS超紧组合导航系统在建模时，将导航参数的误差作为估计状态，称为间接法滤波。间接法滤波有两种，分别是输出校正和反馈校正。理论上，当模型能够准确反映系统本身时，输出校正和反馈校正的估计效果是一致的。工程实践中，输出校正实现简单，滤波器故障不影响SINS工作，但它只能改善输出的准确性，不能对SINS内部误差作修正，长时间工作时，SINS误差累积，最后使系统模型跟实际模型不匹配，导致滤波精度下降或发散；反馈校正估计的是经过校正的导航参数误差，保持为小量，没有模型误差，更接近于真实系统，但是反馈校正实现复杂，滤波器故障或估计精度不高会污染SINS输出，使系统可靠性降低。结合这两种结构的优缺点出现了混合校正滤波结构，该结构只有在所有状态估计精度都很高时才能得出较好的滤波结果，而状态的估计精度是由其可观测性决定的，因此这种结构只能提高完全可观系统估计精度，对于GPS/SINS超紧组合导航系统这种不完全可观测的系统是不适用的。

发明内容

本发明目的在于提供一种GPS/SINS超紧组合导航系统自适应混合滤波方法，可以有效提高系统状态不完全可观测的GPS/SINS超紧组合导航系统的导航定位精度。

实现本发明目的技术方案：

一种GPS/SINS超紧组合导航系统自适应混合滤波方法，其特征在于:

步骤1：建立GPS/SINS超紧组合导航系统误差模型；

步骤2：采用卡尔曼滤波方法进行数据融合，得到系统状态的最优估计值和估计误差方程阵P；

步骤3：利用步骤2中获得的系统状态的最优估计值对SINS输出的导航定位信息进行输出校正；

步骤4：采用误差方差阵的特征值和特征向量的可观测性分析方法，利用步骤2中获得的估计误差方程阵P求取系统各个状态的可观测度信息；

步骤5：将步骤4中获得的系统状态可观测度信息作为反馈因子，将其与步骤2中获得的系统状态的最优估计值的乘积作为反馈量，对GPS和SINS的参数进行反馈校正。

步骤4中,计算估计误差方程阵P的特征值和与之对应的特征向量分别为λ_i和η_i，i∈{1,2,…17}，则系统状态的可观测度信息为最小特征值λ_min对应的特征向量η_min，min∈{1,2,…17}。

步骤5中，包括如下步骤，

步骤5.1：将步骤4中的最小特征值λ_min对应的特征向量η_min作为反馈因子，将反馈因子记为α＝[α₁ α₂ … α₁₇]^T，即α＝η_min；

则反馈量为

$\mathrm{\τ}=\mathrm{\α}\·\hat{X}$

式中，为步骤2中获得的系统状态的最优估计值；

步骤5.2：利用步骤5.1获得的反馈量对惯性测量元件进行反馈校正，公式如下，

$f=\left[\begin{array}{c}{f^{\text{'}}}_x-{\mathrm{\α}}_{10}{\▿}_x\\ {f^{\text{'}}}_y-{\mathrm{\α}}_{11}{\▿}_y\\ {f^{\text{'}}}_z-{\mathrm{\α}}_{12}{\▿}_z\end{array}\right]$

$\mathrm{\ω}=\left[\begin{array}{c}{{\mathrm{\ω}}^{\text{'}}}_x-{\mathrm{\α}}_{13}{\mathrm{\ϵ}}_x\\ {{\mathrm{\ω}}^{\text{'}}}_y-{\mathrm{\α}}_{14}{\mathrm{\ϵ}}_y\\ {{\mathrm{\ω}}^{\text{'}}}_z-{\mathrm{\α}}_{15}{\mathrm{\ϵ}}_z\end{array}\right]$

t_u＝t_u'-α₁₆δt_u

t_ru＝t_ru'-α₁₇δt_ru

式中，校正后的加速度计测量参数为比力f，校正后的陀螺仪测量参数为角速度ω，校正后的GPS接收机参数为时钟t_u，时钟频率t_ru；前述参数的实际测量值分别为f'、ω'、t_u'和t_ru'。

本发明具有的有益效果：

本发明建立了系统状态的反馈量与可观测度的定量关系，将可观测度信息作为反馈因子，将反馈因子与状态估计值的乘积作为反馈量，使系统在进行输出校正的同时获得更加准确的反馈校正，从而避免了盲目反馈使系统导航精度降低的问题，进而提高了系统状态不完全可观测的GPS/SINS超紧组合导航系统的导航定位精度。

附图说明

图1是本发明方法流程框图；

图2是本发明自适应混合滤波器原理图；

图3是平台失准角估计精度对比图；

图4是速度误差估计精度对比图；

图5是位置误差估计精度对比图。

具体实施方式

如图1、图2所示，本发明方法包括以下步骤：

步骤1：建立GPS/SINS超紧组合导航系统误差模型；

具体实现步骤为：

步骤1.1：建立GPS/SINS超紧组合导航系统状态方程；

状态变量为： $X={\left[\begin{array}{cccccc}\mathrm{\δR}& \mathrm{\δv}& \mathrm{\φ}\▿& \mathrm{\ϵ}& \mathrm{\δ}{t}_u& \mathrm{\δ}{t}_{\mathrm{ru}}\end{array}\right]}^T,$ 其中δR＝[δL δλ δh]^T为位置误差；δv＝[δv_E δv_N δv_U]^T为速度误差；φ＝[φ_E φ_N φ_U]^T为平台失准角； $\▿={\left[\begin{array}{ccc}{\▿}_x& {\▿}_y& {\▿}_z\end{array}\right]}^T$ 为加速度计零位误差；ε＝[ε_x ε_y ε_z]^T为陀螺常值漂移；δt_u为接收机钟差；δt_ru为时钟频率漂移；那么，系统误差方程为

$\stackrel{\·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(1\right)$

其中状态转移矩阵 $A=\left[\begin{array}{cc}{A}_{\mathrm{SINS}}& 0_{15\×2}\\ 0_{2\×15}& A_{\mathrm{GPS}}\end{array}\right],$ $A_{\mathrm{SINS}}=\left[\begin{array}{ccccc}-\left({\mathrm{\ω}}_{\mathrm{en}}\right)\×& I& 0& 0& 0\\ 0& -({\mathrm{\ω}}_{\mathrm{in}}+{\mathrm{\ω}}_{\mathrm{ie}})\×& f\×& I& 0\\ 0& 0& -{\mathrm{\ω}}_{\mathrm{in}}\×& 0& I\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\end{array}\right],$ $A_{\mathrm{GPS}}=\left[\begin{array}{cc}-1& 0\\ -1/T& 0\end{array}\right],$ 系统噪声为 $W={\left[\begin{array}{cccc}{w}_{\▿}& w_{\mathrm{\ϵ}}& w_{\mathrm{tu}}& w_{\mathrm{tru}}\end{array}\right]}^T$ (包括加速度计噪声、陀螺仪噪声、接收机钟差驱动噪声和钟差漂移驱动噪声)，噪声驱动阵为 $G=\left[\begin{array}{cccc}0& 0& 0& 0\\ 0& C_b^n& 0& 0\\ C_b^n& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],$ ω_en为计算地理系相对于地球坐标系的旋转角速度矢量，ω_in为计算地理系相对于惯性坐标系的旋转角速度矢量，ω_ie为地球坐标系相对于惯性坐标系的旋转角速度矢量，f为加速度计测量的比力矢量，T为积分时间，I为适当维数的单位阵，0为适当维数的零矩阵，为从载体坐标系b到导航坐标系n的坐标变换矩阵，(·)×表示向量的反对称矩阵。

步骤1.2：建立GPS/SINS超紧组合导航系统量测方程；

Z(t)＝H(t)X(t)+V(t) (2)

量测Z＝[δI₁ … δI_m δQ₁ … δQ_m]^T，m为跟踪卫星数目，δI和δQ为接收机内部同相信号和正交信号的误差。量测矩阵为

$H=\left[\begin{array}{cccccccccc}{h}_{x1}& h_{y1}& h_{z1}& 0& 0& 0& \·\·\·& \·\·\·& 1& 0\\ \·& \·& \·& \·& \·& \·& \·& \·& \·& \·\\ \·& \·& \·& \·& \·& \·& \·& \·& \·& \·\\ \·& \·& \·& \·& \·& \·& \·& \·& \·& \·\\ h_{\mathrm{xn}}& h_{\mathrm{yn}}& h_{\mathrm{zn}}& 0& 0& 0& \·\·\·& \·\·\·& 1& 0\\ 0& 0& 0& {\stackrel{\·}{h}}_{x1}& {\stackrel{\·}{h}}_{y1}& {\stackrel{\·}{h}}_{z1}& \·\·\·& \·\·\·& 0& 1\\ \·& \·& \·& \·& \·& \·& \·& \·& \·& \·\\ \·& \·& \·& \·& \·& \·& \·& \·& \·& \·\\ \·& \·& \·& \·& \·& \·& \·& \·& \·& \·\\ 0& 0& 0& {\stackrel{\·}{h}}_{\mathrm{xn}}& {\stackrel{\·}{h}}_{\mathrm{yn}}& {\stackrel{\·}{h}}_{\mathrm{zn}}& \·\·\·& \·\·\·& 0& 1\end{array}\right],$ $h_{\mathrm{xi}}=\frac{\∂E\left[I_i\right]}{\∂{\mathrm{\φ}}_{\mathrm{ei}}}\frac{\∂{\mathrm{\φ}}_{\mathrm{ei}}}{\∂L_i}+\frac{\∂E\left[I_i\right]}{\∂{\mathrm{\ω}}_{\mathrm{ei}}}\frac{\∂{\mathrm{\ω}}_{\mathrm{ei}}}{\∂L_i},$ ${\stackrel{\·}{h}}_{\mathrm{xi}}=\frac{\∂E\left[Q_i\right]}{\∂{\mathrm{\φ}}_{\mathrm{ei}}}\frac{\∂{\mathrm{\φ}}_{\mathrm{ei}}}{\∂v_{\mathrm{Ei}}}+\frac{\∂E\left[Q_i\right]}{\∂{\mathrm{\ω}}_{\mathrm{ei}}}\frac{\∂{\mathrm{\ω}}_{\mathrm{ei}}}{\∂v_{\mathrm{Ei}}},h_{\mathrm{yi}}=\frac{\∂E\left[I_i\right]}{\∂{\mathrm{\φ}}_{\mathrm{ei}}}\frac{\∂{\mathrm{\φ}}_{\mathrm{ei}}}{\∂{\mathrm{\λ}}_i}+\frac{\∂E\left[I\right]}{\∂{\mathrm{\ω}}_{\mathrm{ei}}}\frac{\∂{\mathrm{\ω}}_{\mathrm{ei}}}{\∂{\mathrm{\λ}}_i},$ ${\stackrel{\·}{h}}_{\mathrm{yi}}=\frac{\∂E\left[Q_i\right]}{\∂{\mathrm{\φ}}_{\mathrm{ei}}}\frac{\∂{\mathrm{\φ}}_{\mathrm{ei}}}{\∂v_{\mathrm{Ni}}}+\frac{\∂E\left[Q_i\right]}{\∂{\mathrm{\ω}}_{\mathrm{ei}}}\frac{\∂{\mathrm{\ω}}_{\mathrm{ei}}}{\∂v_{\mathrm{Ni}}},h_{\mathrm{zi}}=\frac{\∂E\left[I_i\right]}{\∂{\mathrm{\φ}}_{\mathrm{ei}}}\frac{\∂{\mathrm{\φ}}_{\mathrm{ei}}}{\∂{\mathrm{h\λ}}_i}+\frac{\∂E\left[I_i\right]}{\∂{\mathrm{\ω}}_{\mathrm{ei}}}\frac{\∂{\mathrm{\ω}}_{\mathrm{ei}}}{\∂{\mathrm{h\λ}}_i},$ ${\stackrel{\·}{h}}_{\mathrm{zi}}=\frac{\∂E\left[Q_i\right]}{\∂{\mathrm{\φ}}_{\mathrm{ei}}}\frac{\∂{\mathrm{\φ}}_{\mathrm{ei}}}{\∂v_{\mathrm{Ui}}}+\frac{\∂E\left[Q_i\right]}{\∂{\mathrm{\ω}}_{\mathrm{ei}}}\frac{\∂{\mathrm{\ω}}_{\mathrm{ei}}}{\∂v_{\mathrm{Ui}}},$ E[I]和E[Q]为同相和正交信号的期望。

步骤2：采用卡尔曼滤波方法进行数据融合，得到系统状态的最优估计值和估计误差方程阵P；

具体实现方法为：

首先将步骤1中建立的线性连续系统方程离散化，得到线性离散的系统方程。在此基础上采用卡尔曼滤波算法，一方面得到系统状态的最优估计值另一方面得到估计误差方程阵P。

步骤3：利用步骤2中获得的系统状态的最优估计值对SINS输出的导航定位信息进行输出校正；

具体实现方法为：

捷联惯导解算的导航信息有位置p'、速度v'、姿态φ'。对其校正后分别为

p＝p'-δp (3)

v＝v'-δv (4)

步骤4：采用误差方差阵的特征值和特征向量的可观测性分析方法，利用步骤2中获得的估计误差方程阵P求取系统各个状态的可观测度信息；

具体实现方法为：

通过全等变换将P规范化为P^N+(k)，

$P^{N+}\left(k\right)=\frac{1}{\mathrm{Tr}\left[P^{\text{'}+}\left(k\right)\right]}{P}^{\text{'}+}\left(k\right)$

其中Tr[·]表示求矩阵的迹。全等变换使P的特征值大小发生变化，被限定在[0,1]之间，但它正特性和矩阵的秩不变。

计算P的特征值和与之对应的特征向量分别为λ_i和η_i，i∈{1,2,…17}。因此，P^N+(k)的最大特征值是可观测性最差的状态或状态线性组合的方差；最小的特征值是可观测性最好的状态或状态线性组合的方差。同时特征值的大小差别也反应了状态或状态线性组合的不同可观测程度。则系统状态的可观测度信息为最小特征值λ_min对应的特征向量η_min，min∈{1,2,…17}。

步骤5：将步骤4中获得的系统状态可观测度信息作为反馈因子，将其与步骤2中获得的系统状态的最优估计值的乘积作为反馈量，对GPS和SINS的参数进行反馈校正。

具体包括如下实现步骤：

步骤5.1：将步骤4中的最小特征值λ_min对应的特征向量η_min作为反馈因子，将反馈因子记为α＝[α₁ α₂ … α₁₇]^T，即α＝η_min。

反馈量为

$\mathrm{\τ}=\mathrm{\α}\·\hat{X}---\left(6\right)$

步骤5.2：利用反馈量对惯性测量元件进行反馈校正。加速度计测量的信息为比力f，陀螺仪测量的信息为角速度ω，GPS接收机时钟为t_u，时钟频率为t_ru。由于以上测量元器件存在误差，因此实际测量值分别为f'、ω'、t_u'和t_ru'，则校正后的比力和角速度分别为

$f=\left[\begin{array}{c}{f^{\text{'}}}_x-{\mathrm{\α}}_{10}{\▿}_x\\ {f^{\text{'}}}_y-{\mathrm{\α}}_{11}{\▿}_y\\ {f^{\text{'}}}_z-{\mathrm{\α}}_{12}{\▿}_z\end{array}\right]---\left(7\right)$

$\mathrm{\ω}=\left[\begin{array}{c}{{\mathrm{\ω}}^{\text{'}}}_x-{\mathrm{\α}}_{13}{\mathrm{\ϵ}}_x\\ {{\mathrm{\ω}}^{\text{'}}}_y-{\mathrm{\α}}_{14}{\mathrm{\ϵ}}_y\\ {{\mathrm{\ω}}^{\text{'}}}_z-{\mathrm{\α}}_{15}{\mathrm{\ϵ}}_z\end{array}\right]---\left(8\right)$

t_u＝t_u'-α₁₆δt_u (9)

t_ru＝t_ru'-α₁₇δt_ru (10)

为了进一步验证该导航方法的实用性和优越性，在仿真试验中将新导航方法与传统的混合校正方法进行了对比。仿真条件为：初始位置误差为[2m 2m 2m]，初始速度误差为[0.6m/s 0.6m/s 0.6m/s]，初始平台失准角为[300″ 300″ 300″]，陀螺常值漂移为10°/h，加速度计常值漂移为1mg。滤波周期1s，输出校正周期1s，从第51s开始加入反馈校正，反馈校正周期5s，仿真时间500s。

实验分为两组：一组为本发明给出的基于可观测性的自适应混合校正滤波器；另一组为传统的混合校正滤波器，即反馈因子为α_tra＝[1 1 … 1]^T，其他参数相同。自适应混合滤波器与传统的混合滤波器仿真结果对比如图3—图5所示。

在0s～50s，两个滤波器都没有进行反馈校正，误差有一定的累积，从51s开始进行每5s一次反馈校正。传统混合滤波器由于没有考虑系统状态可观测性影响，将状态的估计值无论优劣全部反馈给SINS，可观测性低和不可观测的状态估计盲目对SINS进行不完全正确的修正，容易导致校正过度，使估计误差出现较大范围波动。以陀螺仪常值漂移ε为例，在第400s时，传统混合校正滤波器针对ε的三个反馈因子分别为1，1，1；自适应混合校正滤波器中对ε的三个反馈因子分别为0.0641，0.3007，0.0432。而事实上，只有与外部观测量直接相关的δv和δR的可观测性最高，反馈因子接近于1，ε的可观测性相对较低，反馈因子小于1；则传统混合滤波器的反馈因子偏离于实际情况，相对于自适应混合校正滤波器来说，对SINS修正效果相对较差，对滤波精度的提高有限。

Claims (3)

1.一种GPS/SINS超紧组合导航系统自适应混合滤波方法，其特征在于:

步骤1：建立GPS/SINS超紧组合导航系统误差模型；

步骤2：采用卡尔曼滤波方法进行数据融合，得到系统状态的最优估计值和估计误差方程阵P；

步骤3：利用步骤2中获得的系统状态的最优估计值对SINS输出的导航定位信息进行输出校正；

步骤4：采用误差方差阵的特征值和特征向量的可观测性分析方法，利用步骤2中获得的估计误差方程阵P求取系统各个状态的可观测度信息；

步骤5：将步骤4中获得的系统状态可观测度信息作为反馈因子，将其与步骤2中获得的系统状态的最优估计值的乘积作为反馈量，对GPS和SINS的参数进行反馈校正。

2.根据权利要求1所述的GPS/SINS超紧组合导航系统自适应混合滤波方法，其特征在于：步骤4中,计算估计误差方程阵P的特征值和与之对应的特征向量分别为λ_i和η_i，i∈{1,2,…17}，则系统状态的可观测度信息为最小特征值λ_min对应的特征向量η_min，min∈{1,2,…17}。

3.根据权利要求2所述的GPS/SINS超紧组合导航系统自适应混合滤波方法，其特征在于：步骤5中，包括如下步骤，

步骤5.1：将步骤4中的最小特征值λ_min对应的特征向量η_min作为反馈因子，将反馈因子记为α＝[α₁ α₂…a₁₇]^T，即α＝η_min；

则反馈量为

$\mathrm{\τ}=\mathrm{\α}\·\hat{X}$

式中，为步骤2中获得的系统状态的最优估计值；

步骤5.2：利用步骤5.1获得的反馈量对惯性测量元件进行反馈校正，公式如下，

$f=\left[\begin{array}{c}{f^{\text{'}}}_x-{\mathrm{\α}}_{10}{\▿}_x\\ {f^{\text{'}}}_y-{\mathrm{\α}}_{11}{\▿}_y\\ {f^{\text{'}}}_z-{\mathrm{\α}}_{12}{\▿}_z\end{array}\right]$

$\mathrm{\ω}=\left[\begin{array}{c}{{\mathrm{\ω}}^{\text{'}}}_x-{\mathrm{\α}}_{13}{\mathrm{\ϵ}}_x\\ {{\mathrm{\ω}}^{\text{'}}}_y-{\mathrm{\α}}_{14}{\mathrm{\ϵ}}_y\\ {{\mathrm{\ω}}^{\text{'}}}_z-{\mathrm{\α}}_{15}{\mathrm{\ϵ}}_z\end{array}\right]$

t_u＝t_u'-α₁₆δt_u

t_ru＝t_ru'-α₁₇δt_ru

式中，校正后的加速度计测量参数为比力f，校正后的陀螺仪测量参数为角速度ω，校正后的GPS接收机参数为时钟t_u，时钟频率t_ru；前述参数的实际测量值分别为f'、ω'、t_u'和t_ru'。