CN103292812A - 一种微惯性sins/gps组合导航系统的自适应滤波方法 - Google Patents

Abstract

本发明公开了一种微惯性SINS/GPS组合导航系统的自适应滤波方法，其目的是改善由短时GPS信号丢失或受到干扰导致常规卡尔曼滤波发散的情况，提高MEMS-SINS/GPS组合导航系统的定位精度。该方法借助新息序列和滤波估计值在线估计量测噪声统计信息，建立关于新息序列的限定窗口平滑器对新息序列进行平滑处理，并利用GPS提供的量测数据和滤波信息修正滤波估计均方误差，完成自适应卡尔曼滤波递推运算。本发明设计的方法能够克服短时间内量测数据不准引起的滤波发散问题，实时修正滤波估计均方误差，提高了MEMS-SINS/GPS组合导航系统的鲁棒性和导航定位精度。

Description

一种微惯性SINS/GPS组合导航系统的自适应滤波方法

技术领域

本发明属于导航系统技术领域，涉及一种微惯性SINS/GPS组合导航系统的自适应滤波方法，可用于提高在GPS信号短时丢失或受到干扰情况下MEMS-SINS/GPS组合导航系统的定位精度。 

背景技术

随着MEMS技术的不断进步，MEMS惯性传感器凭借体积小、重量轻、功耗低以及成本低的特点，目前已开始应用于中低精度的导航领域。但由于MEMS陀螺和加速度计的性能受外界环境影响大，精度不高且误差随时间快速增长等问题，限制了其进一步的应用；GPS是一种全天候、高精度的全球定位系统，具有定位精度高、误差不随时间积累的优点，但是自主性差、易受外界干扰。因此，能够充分利用两者优势的MEMS-SINS/GPS组合导航系统在中低精度的导航领域有着广阔的应用前景。 

MEMS-SINS/GPS组合导航系统普遍采用卡尔曼滤波技术进行信息融合，但是由于微惯性传感器误差和GPS量测噪声统计信息具有不确定性或时变性，容易导致常规卡尔曼滤波算法发散，使得导航定位精度降低。目前，很多学者对此进行了大量的研究工作。其中，文献“基于Kalman滤波的MEMS陀螺仪滤波算法，哈尔滨工程大学学报，2010，第31卷第9期”提出的利用带渐消因子的自适应滤波算法来处理动态情况下的滤波问题，但是渐消因子的选取缺乏理论依据，且需要依赖于先验知识，缺乏实用性；文献“基于MIMU/GPS组合导航定位系统的一种新型卡尔曼滤波算法的研究，弹箭与制导学报，2007，第27卷第4期”基于极大后验估计理论得到了量测噪声统计估值器，但是存在滤波发散，对初值敏感以及存在常值误差等问题。本专利设计的方法在加入量测噪声统计估值器的基础上，对系统的新息进行固定窗口平滑，并通过引入时变渐消衰减因子修正状态预测协方差阵，实时自适应调节滤波增益矩阵，从而提高MEMS-SINS/GPS组合导航系统的鲁棒性和导航定位精度。 

发明内容

为了克服现有技术中的缺陷，解决上述技术问题，本发明提供一种微惯性SINS/GPS组合导航系统的自适应滤波方法，提出一种带有量测噪声统计估值器和新息窗口平滑器，并通过引入渐消因子在线修正状态预测协方差，使得滤波器能够根据新息自适应的调节增益矩阵，从而提高MEMS-SINS/GPS组合导航系统鲁棒性和定位精度的自适应滤波方法。该方法可以明显改善短时 GPS信号丢失或受到干扰导致常规卡尔曼滤波发散的情况。其技术方案如下： 

一种微惯性SINS/GPS组合导航系统的自适应滤波方法，包括以下步骤： 

步骤一：建立MEMS-SINS的误差方程，利用GPS与SINS提供的速度差值作为量测信息，建立MEMS-SINS/GPS组合导航系统的状态方程和量测方程； 

步骤二：利用由步骤一得到的k时刻量测值Z_k和滤波器的一步预测值

计算当前时刻新息序列ε_k； 

步骤三：利用k时刻的新息序列ε_k和k-1时刻的滤波信息，对k时刻的量测噪声协方差阵

进行在线估计； 

步骤四：确定平滑窗口的宽度N，并借助步骤二得到新息序列新息ε_k，建立关于量测新息的协方差阵的限定窗口平滑器； 

步骤五：根据步骤三和步骤四得到的量测噪声估值器和窗口平滑器，借助k时刻的量测信息对滤波器的一步预测估计均方误差进行修正； 

步骤六：在步骤二、三、五的基础上，将k时刻的量测噪声估计值、新息序列值以及一步预测均方误差的修正值带入滤波过程中，建立自适应卡尔曼滤波器，并依次进行组合导航的递推解算。 

进一步优选，步骤四借助由步骤二得到新息序列，建立关于量测新息的协方差阵的限定窗口平滑器，通过对组合导航系统的新息序列进行固定窗口的平滑运算，借助以前时刻新息序列的调节作用，使得新息序列变化更加平稳，可以有效解决短时间内GPS提供的量测信息丢失或者受干扰情况下引起的滤波发散现象，其递推形式： 

$C_k=C_{k-1}+\frac{1}{N}[{\mathrm{\ϵ}}_k^T{\mathrm{\ϵ}}_k-{\mathrm{\ϵ}}_{k-1-N}^T{\mathrm{\ϵ}}_{k-1-N}]$

其中，N为固定平滑窗口的宽度，C_k和C_k-1分别是k和k-1时刻的量测噪声新息的协方差，ε_k和ε_k-1-N分别表示k和k-1-N时刻的新息序列。 

进一步优选，步骤五利用由步骤三和步骤四得到的量测噪声估值器和新息序列窗口平滑器，并借助k时刻的量测信息对滤波器的一步预测估计均方误差进行修正： 

修正因子 ${\mathrm{\μ}}_k=\frac{C_k-\mathrm{tr}\left[N_k\right]}{\mathrm{tr}\left[N_k\right]}$

一步预测均方误差P_k，k-1＝(1+μ_k)[Φ_k-1P_k-1Φ^T _k-1+Γ_k-1QΓ^T _k-1] 

其中，N_k＝H[Φ_k-1P_k-1Φ^T _k-1+Γ_k-1QΓ^T _k-1]H^T+R_k，P_k-1是滤波估计均方误差，Γ_k-1是系统噪声系数阵，Q是系统噪声协方差，R_k是量测噪声协方差；μ_k是一步预测均方误差的修正因子，P_k，k-1是一步预测估计均方误差。 

本发明的有益效果： 

1.本发明通过对实际测量的新息进行固定窗口平滑运算，来修正卡尔曼滤波增益，提高了组合导航系统在GPS量测值发生较大变化时鲁棒性。 

2.本发明通过引入量测噪声统计估值器，在线对量测噪声的统计协方差阵进行估计，为滤波器通过更加准确的噪声统计噪声，实时修正滤波增益矩阵。 

3.本发明通过引入渐消因子在线修正状态预测协方差，一方面降低了滤波器对噪声统计初值的敏感性，另一方面提高了对由系统模型不匹配和系统噪声统计信息不准确引起的误差的调节能力，使得自适应滤波器具有更强的鲁棒性和更高的滤波精度。 

附图说明

图1为本发明的工作流程图； 

图2为MEMS-SINS/GPS组合导航系统方框图。 

具体实施方式

下面结合附图和具体实施例对本发明的技术方案作进一步详细地说明。 

参照图1-图2，步骤一：建立MEMS-SINS的误差方程，利用GPS与SINS提供的速度差值作为量测信息，建立SINS/GPS组合导航系统状态空间方程。在SINS/GPS组合导航模式下，SINS作为主导航系统建立系统的状态方程，选取经纬度误差、速度误差以及失准角作为状态变量。 

状态变量为

状态方程：X_k＝Φ_kX_k-1+Γ_kW_k

量测方程：Z_k＝H_kX_k+V_k

其中，状态一步转移矩阵 

${\mathrm{\Φ}}_k=$

$=\left[\begin{array}{ccccccc}1& 0& \frac{T}{R}& 0& 0& 0& 0\\ \frac{v_E}{R}T\sec L\tan L& 1& \frac{\sec L}{R}T& 0& 0& 0& 0\\ {2w}_{\mathrm{ie}}T\cos L{v}_N+\frac{v_E{v}_N}{R}{T\sec }^{2}L& 0& 1+\frac{v_N}{R}T\tan L& {2w}_{\mathrm{ie}}\sin L+\frac{v_E}{R}\tan L& 0& -\mathrm{gT}& f_{N}T\\ -({2w}_{\mathrm{ie}}T\cos L{v}_E+\frac{v_E^2}{R}{T\sec }^{2}L)& 0& -({2w}_{\mathrm{ie}}T\sin L+\frac{v_E}{R}T\tan L)& 1& \mathrm{gT}& 0& -f_{E}T\\ 0& 0& 0& -\frac{T}{R}& 1& w_{\mathrm{ie}}\sin L+\frac{v_E}{R}\tan L& -(w_{\mathrm{ie}}T\cos L+\frac{v_{E}T}{R})\\ {-w}_{\mathrm{ie}}T\sin L& 0& \frac{T}{R}& 0& -(w_{\mathrm{ie}}T\sin L+\frac{v_E}{R}T\tan L)& 1& -\frac{v_N}{R}T\\ w_{\mathrm{ie}}T\cos L+\frac{v_E}{R}{T\sec }^{2}L& 0& \frac{T}{R}\tan L& 0& w_{\mathrm{ie}}T\cos L+\frac{v_E}{R}T& \frac{v_N}{R}T& 1\end{array}\right]$

系统噪声系数阵 

${\mathrm{\Γ}}_k=$

$=T\left[\begin{array}{ccccccc}0& 0& 0& 0& 0& 0& 0\\ 0& 0& & 0& 0& 0& 0\\ 0& 0& C_b^n\left(\mathrm{1,1}\right)& C_b^n\left(\mathrm{1,2}\right)& 0& 0& 0\\ 0& 0& C_b^n\left(\mathrm{2,1}\right)& C_b^n\left(\mathrm{2,2}\right)& 0& 0& 0\\ 0& 0& 0& 0& C_b^n\left(\mathrm{1,1}\right)& C_b^n(1,2)& C_b^n\left(\mathrm{1,3}\right)\\ 0& 0& 0& 0& C_b^n\left(\mathrm{2,1}\right)& C_b^n\left(\mathrm{2,2}\right)& C_b^n\left(\mathrm{2,3}\right)\\ 0& 0& 0& 0& C_b^n\left(\mathrm{3,1}\right)& C_b^n\left(\mathrm{3,2}\right)& C_b^n\left(\mathrm{3,3}\right)\end{array}\right]$

量测系数阵 

$H_k=\left[\begin{array}{ccccccc}0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0\end{array}\right]$

步骤二：利用由步骤一得到的k时刻量测值Z_k和滤波器的一步预测值

计算当前时刻新息序列ε_k； 

${\mathrm{\ϵ}}_k=Z_k-H_k{\hat{X}}_{k,k-1}=Z_k-H_k{\mathrm{\Φ}}_{k-1}{\hat{X}}_{k-1}$

其中，ε_k是k时刻的新息序列，Z_k是k时刻的量测信息，

是k-1时刻的滤波估计值。 

步骤三：利用k时刻的新息序列ε_k和k-1时刻的滤波信息，对量测噪声协方差阵

进行在线估计： 

${\hat{R}}_k=\left({1-d}_k\right){\hat{R}}_{k-1}+d_k[{\mathrm{\ϵ}}_k{\mathrm{\ϵ}}_k^T-{\mathrm{HP}}_{k,k-1}{H}^T]$

其中，d_k＝(1-b)/(1-b^k+1)，0＜b＜1是衰减因子；和

分别是k和k-1时刻的量测噪声协方差；P_k，k-1是k时刻的一步预测估计均方误差。 

步骤四：确定平滑窗口的宽度N，并借助步骤二得到新息序列新息，建立关于量测新息的协 方差阵的限定窗口平滑器，其递推形式： 

$C_k=C_{k-1}+\frac{1}{N}\left[{\mathrm{\ϵ}}_k^T{\mathrm{\ϵ}}_k{-\mathrm{\ϵ}}_{k-1-n}^T{\mathrm{\ϵ}}_{k-1-N}\right]$

其中，N为固定平滑窗口的宽度，C_k和C_k-1分别是k和k-1时刻的量测噪声新息的协方差，ε_k和ε_k-1-N分别表示k和k-1-N时刻的新息序列。 

步骤五：根据步骤三和步骤四得到的量测噪声估值器和窗口平滑器，借助k时刻的量测信息Z_k对滤波器的一步预测估计均方误差进行修正： 

修正因子 ${\mathrm{\μ}}_k=\frac{C_k-\mathrm{tr}\left[N_k\right]}{\mathrm{tr}\left[N_k\right]}$

一步预测均方误差P_k，k-1＝(1+μ_k)[Φ_k-1P_k-1Φ^T _k-1+Γ_k-1QΓ^T _k-1] 

其中，N_k＝H[Φ_k-1P_k-1Φ^T _k-1+Γ_k-1QΓ^T _k-1]H^T+R_k，P_k-1是滤波估计均方误差，Γ_k-1是系统噪声系数阵，Q是系统噪声协方差，R_k是量测噪声协方差；μ_k是一步预测均方误差的修正因子，P_k，k-1是一步预测估计均方误差。 

步骤六：在步骤二、三、五的基础上，将k时刻的量测噪声估计值、新息序列值以及一步预测均方误差的修正值带入滤波过程中，建立自适应卡尔曼滤波器，并依次进行组合导航的递推解算： 

$K_k=P_{k,k-1}{H}_k^T{({\mathrm{HP}}_{k,k-1}{H}^T+R_k)}^{-1}$

${\hat{X}}_k={\hat{X}}_{k,k-1}+K_k{\mathrm{\ϵ}}_k$

P_k＝(I-K_kH_k)P_k，k-1

其中，

K_k是k时刻滤波增益矩阵，

是k时刻的滤波估计值，P_k是k时刻的滤波估计误差方差。 

以上所述，仅为本发明较佳的具体实施方式，本发明的保护范围不限于此，任何熟悉本技术领域的技术人员在本发明披露的技术范围内，可显而易见地得到的技术方案的简单变化或等效替换均落入本发明的保护范围内。 

Claims (3)

1.一种微惯性SINS/GPS组合导航系统的自适应滤波方法，其特征在于，包括以下步骤：

步骤一：建立MEMS-SINS的误差方程，利用GPS与SINS提供的速度差值作为量测信息，建立MEMS-SINS/GPS组合导航系统的状态方程和量测方程；

步骤二：利用由步骤一得到的k时刻量测值Z_k和滤波器的一步预测值

计算当前时刻新息序列ε_k；

步骤三：利用k时刻的新息序列ε_k和k-1时刻的滤波信息，对k时刻的量测噪声协方差阵

进行在线估计；

步骤四：确定平滑窗口的宽度N，并借助步骤二得到新息序列新息ε_k，建立关于量测新息的协方差阵的限定窗口平滑器；

步骤五：根据步骤三和步骤四得到的量测噪声估值器和窗口平滑器，借助k时刻的量测信息对滤波器的一步预测估计均方误差进行修正；

步骤六：在步骤二、三、五的基础上，将k时刻的量测噪声估计值、新息序列值以及一步预测均方误差的修正值带入滤波过程中，建立自适应卡尔曼滤波器，并依次进行组合导航的递推解算。

2.根据权利要求1所述的微惯性SINS/GPS组合导航系统的自适应滤波方法，其特征在于：步骤四借助由步骤二得到新息序列，建立关于量测新息的协方差阵的限定窗口平滑器，通过对组合导航系统的新息序列进行固定窗口的平滑运算，借助以前时刻新息序列的调节作用，使得新息序列变化更加平稳，可以有效解决短时间内GPS提供的量测信息丢失或者受干扰情况下引起的滤波发散现象，其递推形式：

$C_k=C_{k-1}+\frac{1}{N}[{\mathrm{\ϵ}}_k^T{\mathrm{\ϵ}}_k-{\mathrm{\ϵ}}_{k-1-N}^T{\mathrm{\ϵ}}_{k-1-N}]$

其中，N为固定平滑窗口的宽度，C_k和C_k-1分别是k和k-1时刻的量测噪声新息的协方差，ε_k和ε_k-1-N分别表示k和k-1-N时刻的新息序列。

3.根据权利要求1所述的微惯性SINS/GPS组合导航系统的自适应滤波方法，其特征在于：

步骤五利用由步骤三和步骤四得到的量测噪声估值器和新息序列窗口平滑器，并借助k时刻的量测信息对滤波器的一步预测估计均方误差进行修正：

修正因子 ${\mathrm{\μ}}_k=\frac{C_k-\mathrm{tr}\left[N_k\right]}{\mathrm{tr}\left[N_k\right]}$

一步预测均方误差P_k，k-1＝(1+μ_k)[Φ_k-1P_k-1Φ^T _k-1+Γ_k-1QΓ^T _k-1]

其中，N_k＝H[Φ_k-1P_k-1Φ^T _k-1+Γ_k-1QΓ^T _k-1]HT+P_k，P_k-1是滤波估计均方误差，Γ_k-1是系统噪声系数阵，Q是系统噪声协方差，R_k是量测噪声协方差；μ_k是一步预测均方误差的修正因子，P_k，k-1是一步预测估计均方误差。

Images