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

Abstract

本发明提供的是一种SINS/GPS自适应组合导航方法，其特征是将捷联惯导系统与GPS全球定位系统进行信息融合，并按组合频率，对由串口接收到的GPS信号进行噪声实时计算。本发明以量测残差为判断基准，通过对滑动窗大小的自动调节，有效地将新息引入到量测信号噪声的实时计算中来，可在GPS量测信号噪声增大时，减小滤波增益，降低量测信号对组合导航系统定位的作用；在量测信号噪声减小时，加大滤波增益，增大新息对组合导航的影响，自动延长的滑动窗口又保持了系统的平稳特性，有效地解决了由时变噪声导致的传统卡尔曼滤波精度下降的问题，提高了组合导航系统的定位精度。

Description

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

(一)技术领域

本发明涉及的是一种SINS/GPS组合导航系统自适应滤波方法，可有效地抑制由时变噪声引起的传统卡尔曼滤波精度下降的情况。

(二)背景技术

SINS/GPS组合导航系统因其结合了惯导系统与GPS全球定位系统的优势，一直吸引大量科研人员进行深入的理论研究，并在车载、舰船、飞行器中有着广泛的应用。卡尔曼滤波作为通用的SINS/GPS组合导航系统信息融合算法，要求精确获知量测噪声的统计信息；当噪声模型不准确或无法预知的情况下，传统卡尔曼滤波精度会大大降低。而GPS全球定位系统的定位精度不仅取决于器件误差，同时还依赖于星可见度及载体运行环境，仅按经验值预置GPS观测噪声无法适应GPS的工作特性，如何有效地解决时变GPS噪声对卡尔曼滤波算法的影响，对提高SINS/GPS组合导航系统精度具有重要意义。

近年来，许多学者对自适应滤波进行了深入的研究，如协方差及系统噪声缩放算法，多模自适应估计算法(MMAE)，引入模糊逻辑的自适应算法，基于新息序列的自适应算法，等等。多模自适应估计算法采用多个并行卡尔曼滤波器实现对模型及噪声的估计，但由于其计算量大，并没有被广泛的应用于实际工程中。常用的Sage-Husa噪声统计估值器采用新息序列均值法对噪声进行估计， ${\hat{C}}_k=\frac{1}{k}\underset{i=1}{\overset{k}{\mathrm{\Σ}}}{v}_i{v}_i^T=\frac{k-1}{k}{\hat{C}}_{k-1}+\frac{1}{k}{v}_i{v}_i^T,$ 滑动窗长度为k(k为当前时刻)，易知，随着时间的增加，当前新息对估值的作用逐渐降低，k→∞， $\frac{1}{k}{v}_i{v}_i^T\→0.$ 系统长时间运行后，当由于环境或载体机动引起噪声变化时，Saga-Husa算法无法解决该情况的噪声估算问题。本发明基于新息序列的自适应算法，提出了根据基于当前量测残差自动调节滑动窗长度的方法，进行在线噪声估计，将新息值与噪声估计有效地结合起来，既保证了系统的原有平稳特性，又充分利用新息序列对时变噪声进行有效的在线估计，具有实际的工程应用意义。

(三)发明内容

本发明的目的在于提供一种能够有效地提高GPS噪声未知的情况下组合导航系统的定位精度的一种SINS/GPS组合导航系统自适应滤波方法。

本发明的目的是这样实现的：

步骤1：用捷联惯导系统实时采集陀螺加速度信息，对实时采集的陀螺加速度信息进行位置速度姿态解算；

步骤2：通过串口实时采集GPS信号，按照数据格式从缓冲区中读取GPS提供的位置速度信息；

步骤3：根据SINS动态误差模型建立组合导航系统的状态方程，利用SINS与GPS的位置速度误差建立量测方程；

步骤4：依组合导航解算频率，根据解算出的SINS位置、速度信息与GPS信号计算新息大小，及量测残差值，量测残差大小反映了GPS信号噪声大小；

步骤5：利用量测残差值，对滑动窗口长度进行自动调节；

步骤6：依滑动窗大小计算新息均值，对GPS量测信号噪声协方差进行实时计算；

步骤7：将噪声协方差代入到滤波预测过程及量测更新过程，计算滤波增益，进行组合导航解算。

本发明根据当前新息自动调节滑动窗长度，对时变噪声进行计算的自适应组合导航方法，具体步骤为：

1、根据SINS与GPS观测量及观测方程求取当前新息及量测残差值(第k时刻)。

$v_k=Z_k-{H\hat{X}}_{k,k-1}$

$C_k=M\left[v_k{v}_k^T\right]={\mathrm{HP}}_{k|k-1}{H}^T+R_k$

$d=v_k^T{C}_k^{-1}{v}_k={(Z_k-{H\hat{X}}_{k,k-1})}^T{(H_k{P}_{k+1/k}{H}_k^T+R_k)}^{-1}(Z_k-{H\hat{X}}_{k,k-1})$

其中，v_k为新息值，C_k为新息均方差，d为量测残差值。

2、根据当前量测残差值求取滑动窗长度。

N＝1，d＞α_max

N＝k，d＜α_min

$N=\mathrm{Integer}\left({k\×\mathrm{\λ}}^{d-{\mathrm{\α}}_{\min }}\right),$ λ＜1，N≥1，α_min＜d＜α_max

易知，滑动窗最小长度为1，即N＝1，C_k只取决于当前新息，与历史新息序列无关。此时，当前新息比重对噪声估值起了最大作用，残差值大于设置的最大阈值，即d＞α_max。

残差值小于设置的最小阈值时，滑动窗取最大长度N＝k，此时与Sage-Husa极大后验噪声统计估值器一致，即 ${\hat{C}}_k=\frac{1}{k}\underset{i=1}{\overset{k}{\mathrm{\Σ}}}{v}_i{v}_i^T=\frac{k-1}{k}{\hat{C}}_{k-1}+\frac{1}{k}{v}_i{v}_i^T.$

当判断出残差值介于两者之间时，即α_min＜d＜α_max，滑动窗长度为 $N=\mathrm{Integer}\left({k\×\mathrm{\λ}}^{d-{\mathrm{\α}}_{\min }}\right),$ λ为调节因子，它的大小决定了滑动窗口随量测残差大小的变化趋势，λ＜1，N≥1。

3、依滑动窗大小(滤波窗口大小)计算新息均值，计算时变信号噪声协方差的方法为；

${\hat{C}}_k=\frac{1}{N}\underset{i=k-N+1}{\overset{k}{\mathrm{\Σ}}}{v}_i{v}_i^T$

根据滑动窗大小计算K，K-1，K-2，…，K-N+1时刻新息方差的均值，作为进一步估算观测噪声协方差阵的依据。

$R_k={\hat{C}}_k-{\mathrm{HP}}_{k|k-1}{H}^T$

为了避免矩阵奇异，当R_k＜0时，进行规一化处理，令R_k＝0

4、将噪声协方差代入到滤波预测过程及量测更新过程，根据噪声方差调节滤波增益，进行组合导航解算：

$P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T$

$K_k=P_{k+1/k}{H}_k^T{[H_k{P}_{k+1/k}{H}_k^T+R_k]}^{-1}$

${\hat{x}}_k={\hat{x}}_{k/k-1}+K_k{v}_k$

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

其中，

为状态变量预测值，对于非线性系统，可通过一阶泰勒展开获得；v_k为新息；

为新息协方差；

为自适应滤波中的系统噪声协方差；R_k为量测噪声协方差；P_k/k-1为一步预测误差均方差；K_k为滤波增益阵。

本发明利用了通用的导航设备，通过对噪声的实时计算，调节滤波增益，有效地解决了由时变噪声导致的传统卡尔曼滤波精度下降的情况。常用的Sage-Husa噪声统计估值器作为一种极大后验估值方法，适用于对未知常值噪声进行估计，其缺点和局限性是不能处理时变噪声。本发明通过对滑动窗大小的自动调节，有效地将新息引入到噪声估值中来，可在量测信号噪声增大时，减小滤波增益，降低量测信号对组合导航系统定位的作用；在量测信号噪声减小时，加大滤波增益，增大新息对组合导航的影响，自动延长的滑动窗口又保持了系统的平稳特性，有效的解决了由时变噪声导致的传统卡尔曼滤波精度下降的问题，提高了组合导航系统的定位精度。

(四)附图说明

图1为本发明中，SINS/GPS自适应组合导航系统结构图。

图2为本发明中，量测噪声未知情况下自适应扩展卡尔曼滤波流程图。

(五)具体实施方式

下面结合附图举例对本发明做更详细地描述：

步骤1、根据捷联惯导系统的误差模型建立系统方程。

捷联惯导误差方程为：

其中，X_k为被估计状态；W_k为噪声序列；Φ_k，k-1为t_k-1至t_k时刻一步转移阵；Γ_k-1为系统噪声驱动阵；H_k为量测阵；V_k为量测噪声序列；δλ表示纬度、经度误差；δV_x，δV_y表示东向、北向速度误差；V_x，V_y表示东向、北向速度；φ_x，φ_y表示北向、东向水平失准角；φ_z表示方位失准角；  ε_x，ε_y，ε_z表示陀螺漂移。

连续系统的状态方程为：

$\stackrel{\·}{X}=f\left(X\left(t\right)\right)+\mathrm{\Γ}\left(t\right)w_t$ $E\left(w_t{w}_{t^{\′}}^T\right)=Q\left(t\right)\mathrm{\δ}(t-t^{\′})$

一阶泰勒展开，

$X_{k+1}=X_k+f\left(X_k\right)T+A\left(X_k\right)f\left(X_k\right)\frac{T^2}{2}+{\mathrm{\Γ}}_k{q}_k$

其中， $A\left(X\right)=\frac{\∂f\left(X\right)}{\∂X},$ ${\mathrm{\Γ}}_k{q}_k=\underset{k}{\overset{k+1}{\∫}}\mathrm{\Γ}\left(t\right)\mathrm{wdt},$ Eqq^T＝Q_k，X_k＝X(t_k)，Γ_k＝Γ(t_k)，Q_k＝Q(t_k)

Φ_k＝I+A(X_k|k)^T

取SINS与GPS系统的位置速度误差为观测量，因而可以直接建立离散时间量测方程：

$H_{{(4\×10)}^k}(i,j)=\left\{\begin{array}{c}1,i=j\\ 0,i\≠j\end{array}\right.$

步骤2、计算当前新息值及量测残差大小，根据残差值求取滑动窗长度

$v_k=Z_k-H{\hat{X}}_{k,k-1}$

$d=v_k^T{C}_k^{-1}{v}_k={(Z_k-H{\hat{X}}_{k,k-1})}^T{(H_k{P}_{k+1/k}{H}_k^T+R_k)}^{-1}(Z_k-H{\hat{X}}_{k,k-1})$

N＝1，d＞α_max

N＝k，d＜α_min

$N=\mathrm{Integer}(k\×{\mathrm{\λ}}^{d-{\mathrm{\α}}_{\min }}),$ λ＜1，N≥1，α_min＜d＜α_max

步骤3、量测噪声未知时，本发明中涉及到的自适应扩展卡尔曼滤波预测方程与更新过程为

${\hat{C}}_k=\frac{1}{N}\underset{i=k-N+1}{\overset{k}{\mathrm{\Σ}}}{v}_i{v}_i^T$

$R_k={\hat{C}}_k-H{P}_{k|k-1}{H}^T$

$P_{k+1|k}={\mathrm{\Φ}}_k{P}_{k|k}{\mathrm{\Φ}}_k^T+{\mathrm{\Γ}}_k{Q}_k{\mathrm{\Γ}}_k^T$

$K_k=P_{k+1/k}{H}_k^T{[H_k{P}_{k+1/k}{H}_k^T+R_k]}^{-1}$

${\hat{x}}_k={\hat{x}}_{k/k-1}+K_k{v}_k$

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

Claims (1)

1.一种SINS/GPS组合导航系统自适应滤波方法，其特征是：

步骤1：用捷联惯导系统实时采集陀螺加速度信息，对实时采集的陀螺加速度信息进行位置速度姿态解算；

步骤2：通过串口实时采集GPS信号，按照数据格式从缓冲区中读取GPS提供的位置速度信息；

步骤3：根据SINS动态误差模型建立组合导航系统的状态方程，利用SINS与GPS的位置速度误差建立量测方程；

步骤4：依组合导航解算频率，根据解算出的SINS位置、速度信息与GPS信号计算新息大小，及量测残差值，量测残差大小反映了GPS信号噪声大小；所述的计算新息及量测残差的具体步骤为：

$v_k=Z_k-H{\hat{X}}_{k,k-1}$

$C_k=M\left[v_k{v}_k^T\right]={\mathrm{HP}}_{k|k-1}{H}^T+R_k$

$d=v_k^T{C}_k^{-1}{v}_k={(Z_k-H{\hat{X}}_{k,k-1})}^T{(H_k{P}_{k+1/k}{H}_k^T+R_k)}^{-1}(Z_k-H{\hat{X}}_{k,k-1})$

其中，v_k为新息值，C_k为新息均方差，d为量测残差值，Z_k、

为系统的量测向量与状态向量，H为系统量测矩阵，R_k为量测噪声协方差，P_k|k-1为一步预测误差协方差；

步骤5：利用量测残差值，对滑动窗口长度进行自动调节；利用量测残差值，对系统k时刻滑动窗口长度进行自动调节，具体步骤为：

N＝1，d＞α_max

N＝k，d＜α_min

$N=\mathrm{Integer}(k\×{\mathrm{\λ}}^{d-{\mathrm{\α}}_{\min }}),$ λ＜1，N≥1

其中，N为滑动窗长度；α_max与α_min分别为预先设置的最大及最小阈值，当量测残差大于α_max时，滑动窗口取最小值1；当量测残差小于α_min时，滑动窗口取最大值k，k为当前时刻；λ为调节因子，它的大小决定了滑动窗口随量测残差大小的变化趋势，取整计算滑动窗大小；

步骤6：依滑动窗大小计算新息均值，对GPS量测信号噪声协方差进行实时计算；依滑动窗大小计算新息均值，实时计算量测信号噪声协方差，其具体步骤为：

依据滑动窗大小计算相关新息序列均值，计算量测噪声；

${\hat{C}}_k=\frac{1}{N}\underset{i=k-N+1}{\overset{k}{\mathrm{\Σ}}}{v}_i{v}_i^T$

$R_k={\hat{C}}_k-H{P}_{k|k-1}{H}^T$

为了避免矩阵奇异，当R_k＜0时，进行规一化处理，令R_k＝0；

步骤7：将噪声协方差代入到滤波预测过程及量测更新过程，计算滤波增益，进行组合导航解算。

Images