«上一篇
文章快速检索     高级检索
下一篇»
  哈尔滨工程大学学报  2021, Vol. 42 Issue (2): 240-245  DOI: 10.11990/jheu.201907006
0

引用本文  

王伟, 丛宁, 邬佳. 一种鲁棒GPS/INS组合导航滤波算法设计[J]. 哈尔滨工程大学学报, 2021, 42(2): 240-245. DOI: 10.11990/jheu.201907006.
WANG Wei, CONG Ning, WU Jia. A robust GPS/INS-integrated navigation filtering algorithm design[J]. Journal of Harbin Engineering University, 2021, 42(2): 240-245. DOI: 10.11990/jheu.201907006.

基金项目

国家自然科学基金项目(61571148,61871143)

通信作者

王伟, E-mail: wangwei407@hrbeu.edu.cn

作者简介

王伟, 男, 教授, 博士生导师;
丛宁, 女, 硕士研究生

文章历史

收稿日期:2019-07-01
网络出版日期:2020-12-15
一种鲁棒GPS/INS组合导航滤波算法设计
王伟 , 丛宁 , 邬佳     
哈尔滨工程大学 智能科学与工程学院, 黑龙江 哈尔滨 150001
摘要:针对卡尔曼及其扩展算法在滤波中噪声矩阵与实际偏差过大时出现滤波发散的情况,本文提出利用无偏有限冲击响应滤波器(UFIR)实现该背景下的状态估计。但将UFIR滤波器应用于GPS/INS组合系统存在2个问题:1)在线估计最佳滤波窗长的方法还有待改善;2)导航精度较低。本文设计了一种级联式滤波算法,主滤波器对UFIR滤波算法进行改进,设计在线估计窗口大小方法的同时,改进了现有的UFIR算法;从滤波器引入GPS的航向信息,设计一种自适应卡尔曼滤波以提高导航精度。通过仿真和实测对所提滤波算法进行了验证,实验结果表明该算法可以有效提高导航精度和系统的鲁棒性。
关键词组合导航    卡尔曼滤波器    无偏有限冲击响应滤波器    窗口滤波    级联滤波器    噪声统计特性    自适应滤波    批处理过程    
A robust GPS/INS-integrated navigation filtering algorithm design
WANG Wei , CONG Ning , WU Jia     
College of Intelligent Science and Engineering, Harbin Engineering University, Harbin 150001, China
Abstract: In view of the filtering divergence of the Kalman filter and its extended filter when the noise matrix in the filtering is too large to adapt to the actual deviation, the unbiased finite impulse response filter (UFIR) is proposed to realize the state estimation under these conditions. However, there are two problems in the application of UFIR filter to the GPS/INS-integrated system: one is that the method of estimating the optimal filter window length online needs to be improved; the other is that the navigation accuracy is relatively low. In this paper, a cascade filtering algorithm is designed. The main filter improves the UFIR filtering algorithm. The method estimates window size online while improving the existing UFIR algorithm. An adaptive Kalman filter is designed to improve the navigation accuracy by introducing GPS course information from the filter. The proposed filtering algorithm is verified by simulation and actual measurement. The experimental results show that the proposed algorithm can effectively improve the navigation accuracy and robustness of the system.
Keywords: integrated navigation    Kalman filter    unbiased finite impulse response filter    window filtering    cascaded filter    noise statistical characteristics    adaptive filtering    batch process    

GPS/INS组合导航系统能够充分利用2种导航系统的优点进行互补,具有高精度、高可靠性的特点,在动态定位领域获得了极为广泛的应用。GPS/INS中常用的滤波方式主要是卡尔曼算法[1-3],但该算法会出现过于依赖噪声特性准确性的情况,鲁棒性较差,有诸多学者对其进行了研究和改进。文献[4]提出了基于协方差匹配技术的自适应UKF算法,解决了惯性器件和GPS的噪声分布不准确时UKF的精度下降的问题。文献[5]提出的自适应鲁棒CKF,应用马氏距离减弱了异常量测值对估计的影响,一定程度上增强了系统的鲁棒性。文献[6]提出一种改进的自适应扩展卡尔曼滤波算法,该算法可以基于残差实时估计系统噪声。但当系统噪声或量测噪声不确定,再或初始条件不具备时,上述Kalman衍生算法均不能实现精确估计。

为解决上述问题,Shmaliy等[7]提出了一种在滤波过程中能够无视噪声统计特性的无偏有限冲击响应滤波算法(unbiased finite impulse response filter, UFIR)。该算法在不明确系统噪声、量测噪声的统计特性且初始条件未知的情况下,仅利用最佳窗口长度1个参数就能对系统做出精确估计。最佳滤波窗长的大小会影响滤波精度和运算时间。为了提高确认最佳滤波窗长的效率,文献[8]提出一种在线估计方法,但该方法没有实现真正的在线估计,影响系统的实时性。且由于UFIR自身的滤波原理所限,其导航精度要略差于卡尔曼滤波及其衍生滤波。

针对上述问题,本文将UFIR应用于GPS/INS组合导航系统中,提出一种级联式导航滤波器,设计一种在线估计最佳窗口长度算法的同时,引入了级联自适应卡尔曼滤波器以提高滤波器的导航估计精度。

1 UFIR及其改进算法 1.1 UFIR算法

设系统状态方程和量测方程为:

$ \left\{ {\begin{array}{*{20}{l}} {\mathit{\boldsymbol{\dot X}}(t) = \mathit{\boldsymbol{F}}(t)\mathit{\boldsymbol{X}}(t) + \mathit{\boldsymbol{W}}(t)}\\ {\mathit{\boldsymbol{Z}}(t) = \mathit{\boldsymbol{H}}(t)\mathit{\boldsymbol{X}}(t) + \mathit{\boldsymbol{V}}(t)} \end{array}} \right. $ (1)

式中: X(t)为系统状态矢量;F(t)为状态转移矩阵;W(t)为系统噪声矢量;Z(t)为量测矢量;H(t)为量测矩阵;V(t)为量测噪声矢量。

离散后为:

$\left\{ {\begin{array}{*{20}{l}} {{{\mathit{\boldsymbol{\dot X}}}_k} = {\mathit{\boldsymbol{F}}_k}{\mathit{\boldsymbol{X}}_k} + {\mathit{\boldsymbol{W}}_k}}\\ {{\mathit{\boldsymbol{Z}}_k} = {\mathit{\boldsymbol{H}}_k}{\mathit{\boldsymbol{X}}_k} + {\mathit{\boldsymbol{V}}_k}} \end{array}} \right.$ (2)

式中: Xk表示第k时刻的系统状态矢量,即X(t),同理,ZkZ(t),WkW(t),VkV(t),FkF(t)。当已知第k时刻的量测时,利用从m=k-N+1时刻到k时刻的N组量测可以实现对k时刻的系统状态估计。

1.1.1 滤波过程

UFIR不需要设置噪声协方差矩阵和初始误差,它以最佳滤波窗长代替了上述初始条件。在最佳滤波窗长下获取的估计值具有最小的均方误差(the mean-square error, MSE)。UFIR算法从k-N+1时刻开始通过滤波获得估计值,且通过并行计算降低了窗口滤波的运算量,不需要为获得某一时刻的估计值而重复运行N遍。对上文中建立的状态空间模型进行扩展[9]

$\left\{ \begin{array}{l} {\mathit{\boldsymbol{X}}_{k,m}} = {\left[ {\begin{array}{*{20}{c}} {\mathit{\boldsymbol{X}}_k^{\rm{T}}}&{\mathit{\boldsymbol{X}}_{k - 1}^{\rm{T}}}& \cdots &{\mathit{\boldsymbol{X}}_m^{\rm{T}}} \end{array}} \right]^{\rm{T}}}\\ {\mathit{\boldsymbol{Z}}_{k,m}} = {\left[ {\begin{array}{*{20}{c}} {\mathit{\boldsymbol{Z}}_k^{\rm{T}}}&{\mathit{\boldsymbol{Z}}_{k - 1}^{\rm{T}}}& \cdots &{\mathit{\boldsymbol{Z}}_m^{\rm{T}}} \end{array}} \right]^{\rm{T}}}\\ {\mathit{\boldsymbol{W}}_{k,m}} = {\left[ {\begin{array}{*{20}{c}} {\mathit{\boldsymbol{W}}_k^{\rm{T}}}&{\mathit{\boldsymbol{W}}_{k - 1}^{\rm{T}}}& \cdots &{\mathit{\boldsymbol{W}}_m^{\rm{T}}} \end{array}} \right]^{\rm{T}}}\\ {\mathit{\boldsymbol{V}}_{k,m}} = {\left[ {\begin{array}{*{20}{c}} {\mathit{\boldsymbol{V}}_k^{\rm{T}}}&{\mathit{\boldsymbol{V}}_{k - 1}^{\rm{T}}}& \cdots &{\mathit{\boldsymbol{V}}_m^{\rm{T}}} \end{array}} \right]^{\rm{T}}} \end{array} \right.$ (3)

将状态转移矩阵作如下扩展:

$ {\mathit{\boldsymbol{F}}_{k,m}} = {\left[ {\begin{array}{*{20}{c}} {{{\left( {\mathit{\boldsymbol{F}}_{k,0}^{m + 1}} \right)}^{\rm{T}}}}&{{{\left( {\mathit{\boldsymbol{F}}_{k,1}^{m + 1}} \right)}^{\rm{T}}}}& \cdots &{\begin{array}{*{20}{c}} {\mathit{\boldsymbol{F}}_{m + 1}^{\rm{T}}}&\mathit{\boldsymbol{I}} \end{array}} \end{array}} \right]^{\rm{T}}} $ (4)

$\mathit{\boldsymbol{F}}_{k, 0}^{m + 1} = \prod\limits_{i = 0}^{k - m - 1} {{F_{k - i}} = {F_k} \cdot \cdot \cdot {F_{k - m - 1}}} $

本文中观测矩阵Hk为时不变矩阵,可简化表示为H, 故 H 的状态扩展可表示为:

$ {{\mathit{\boldsymbol{\bar H}}}_{k,m}} = {\rm{diag}}\left( {\underbrace {\begin{array}{*{20}{c}} H&H& \cdots &H \end{array}}_N} \right) $ (5)

${\mathit{\boldsymbol{H}}_{k, m}}{\rm{ = }}{\mathit{\boldsymbol{\bar H}}_{k, m}}{\mathit{\boldsymbol{F}}_{k, m}}$

与卡尔曼滤波原理相似,UFIR滤波也利用类似的思想,令UFIR增益为[10]:

$ {\mathit{\boldsymbol{G}}_k} = {\left[ {\mathit{\boldsymbol{H}}_k^{\rm{T}}{\mathit{\boldsymbol{H}}_k} + {{\left( {{\mathit{\boldsymbol{F}}_k}{\mathit{\boldsymbol{G}}_{k - 1}}\mathit{\boldsymbol{F}}_k^{\rm{T}}} \right)}^{ - 1}}} \right]^{ - 1}} $ (6)
$ {\mathit{\boldsymbol{K}}_k} = {\mathit{\boldsymbol{G}}_k}\mathit{\boldsymbol{H}}_k^{\rm{T}} $ (7)

则:

$ {{\mathit{\boldsymbol{\hat X}}}_k} = {\mathit{\boldsymbol{F}}_k}{{\mathit{\boldsymbol{\hat X}}}_{k - 1}} + {\mathit{\boldsymbol{K}}_k}\left( {{\mathit{\boldsymbol{Z}}_k} - {\mathit{\boldsymbol{H}}_k}{\mathit{\boldsymbol{F}}_k}{{\mathit{\boldsymbol{\hat X}}}_{k - 1}}} \right) $ (8)
1.1.2 批处理过程

UFIR滤波初始条件由批处理滤波获得:

$ {{\mathit{\boldsymbol{\hat X}}}_s} = \mathit{\boldsymbol{F}}_{s,0}^{m{\rm{ + }}1}{\left[ {{{\left( {{\mathit{\boldsymbol{H}}_{s,m}}\mathit{\boldsymbol{H}}_{s,m}^{\rm{T}}} \right)}^{ - 1}}{\mathit{\boldsymbol{H}}_{s,m}}} \right]^{\rm{T}}}{\mathit{\boldsymbol{Z}}_{s,m}} $ (9)
$ {\mathit{\boldsymbol{G}}_s} = \mathit{\boldsymbol{F}}_{s,0}^{m + 1}{\left( {\mathit{\boldsymbol{H}}_{s,m}^{\rm{T}}{\mathit{\boldsymbol{H}}_{s,m}}} \right)^{ - 1}}{\left( {\mathit{\boldsymbol{F}}_{s,0}^{m + 1}} \right)^{\rm{T}}} $ (10)

式中s=m+M-1, M为批处理窗长。

由上述推导可看出,滤波窗长N的大小决定了矩阵的维数,同时也决定了UFIR算法的运算速度,而找出算法的最佳滤波窗长是滤波的前提。

1.2 在线估计$\hat N$设计与实现

UFIR滤波器是一种窗口滤波器。虽然在使用中不需要提供初始状态向量、初始协方差矩阵、系统噪声和量测噪声的统计特性,但需要对其滤波窗口大小N进行估计。N越大,估计效果可能越好,但同时运算量急剧增大;N过小,达不到要求的滤波精度。所以最佳滤波窗长的在线估计是UFIR滤波算法的重点。

本文对文献[8]中提出的在线估计方法进行了改进,提出了一种适用于时变系统的在线估计最佳滤波窗长算法。为保证滤波性能只与滤波窗长N有关,将批处理窗长M设为定值(批处理窗长$M \ll N$)。

1.2.1 确定NminNmax

为了能够更快的确定最佳滤波窗长,首先需要确定窗长的变化范围。现有方法中窗长的范围一般为经验值,不能适应不同条件下的需求,且若给定的范围过大,则影响运算速度,范围小则不能包含最佳滤波窗长。

针对上述情况,本文设计了详细的运算步骤来计算范围NminNmax,主要包括下列2个阶段:

1) 获取不同N的均方误差: 取L组量测值,先利用式(9)、(10)获得滤波的初始条件,再将式(6)、(7)代入式(8)求取${\hat X_k}$:

$ {v_k} = {Z_k} - H{{\hat X}_k} $ (11)
$ {V_k} = {\rm{E}}\left[ {{\mathit{\boldsymbol{v}}_k}\mathit{\boldsymbol{v}}_k^{\rm{T}}} \right] $ (12)

在不同的N下对L组数据求取均方误差值,均方误差最小的N+1设为$\hat N$:

$ \hat N = {\rm{argmin}}{G_N} + 1 $ (13)
$ {G_k} = {\rm{tr}}\left( {{M_N}} \right) $ (14)
$ {M_N} = \frac{1}{L}\sum\limits_{i = 1}^L {{V_k}\left( i \right)} $ (15)

2) 获取χ值: 根据文献[11]可知,当最佳滤波窗长$\hat N$的估计误差在30%之内时,不会影响UFIR的估计。故若要确定$\hat N$的最佳值,则$\hat N$的取值范围系数$\chi > \hat N \times 30\% $,在该范围内进行的选取有意义的。故本文令$\chi {\rm{ = }}\hat N \times Q$,其中Q取60%。由此可确定${N_\rm {min}} = \hat N - \chi $${N_\rm {max}}{\rm{ = }}\hat N + \chi $

1.2.2 确定最佳滤波窗长和${\hat X_k}$的最优估计值

NminNmax之间的整数对剩下的数据进行处理。范围内的每个N都会对剩余n-L+1组(n>L)数据进行1次UFIR滤波处理,每σ组数据为一批次(σ为滤波窗口大小N与批处理窗口大小M的差值)。

每批次得出的${\mathit{\boldsymbol{\hat X}}_{m|N}}$,获得新息lN|m:

$ {\mathit{\boldsymbol{l}}_{N|m}} = {\mathit{\boldsymbol{Z}}_m} - \mathit{\boldsymbol{H}}{{\mathit{\boldsymbol{\hat X}}}_{m|N}} $ (16)

再利用lN|m可得:

$ {\mathit{\boldsymbol{\mu }}_N} = \frac{1}{{L - 1}}\sum\limits_{i = L + 1}^{2L} {{\mathit{\boldsymbol{l}}_{N|i}}} $ (17)
$ {\mathit{\boldsymbol{ \boldsymbol{\varDelta} }}_N}{\rm{ = }}\frac{1}{{L - 1}}\sum\limits_{k = L{\rm{ + }}1}^{2L} {{{\left( {{\mathit{\boldsymbol{l}}_{N|m}} - {\mathit{\boldsymbol{\mu }}_N}} \right)}^{\rm{T}}}\left( {{\mathit{\boldsymbol{l}}_{N|m}} - {\mathit{\boldsymbol{\mu }}_N}} \right)} $ (18)

整理后可得出最优估计值${\hat X_k}$[12]:

$ {\psi _N} = \frac{{{\mathit{\boldsymbol{ \boldsymbol{\varDelta} }}_N}}}{{\mathit{\boldsymbol{\mu }}_N^{\rm{T}}{\mathit{\boldsymbol{\mu }}_N}}} $ (19)
$ {{\mathit{\boldsymbol{\hat N}}}_{{\rm{opt}}}} = {\rm{argmax}}{\psi _N} + 1 $ (20)

每次计算出${\mathit{\boldsymbol{\hat N}}_{opt}}$所对应的${\mathit{\boldsymbol{\hat X}}_{m|N}}$即为最优估计。

2 级联滤波算法设计

与KF算法不同,UFIR不是最优估计,由于自身的滤波原理所限,在系统噪声特性变化不大时,其滤波精度略低于卡尔曼算法。为了兼顾系统的鲁棒性和精度,本文提出一种级联式GPS/INS导航滤波算法。图 1为INS/GPS组合导航系统的原理框图。GPS为系统提供速度、位置和航向信息: 速度位置与惯导解算出的速度位置之差作为主滤波器改进UFIR算法的量测,继而通过主滤波器对速度和位置信息进行校正;将通过加速度计和GPS航向信息获得的姿态与惯导解算出的姿态之差作为从滤波器的量测,再利用主滤波器的新息、UFIR增益等信息获得自适应因子,通过从滤波器完成对姿态信息的校正。

Download:
图 1 INS/GPS组合系统原理 Fig. 1 INS/GPS integrated system block diagram
2.1 主滤波器设计

采用间接法建立状态方程,INS选取东北天坐标系作为导航坐标系。

选取系统的状态量:

$ \mathit{\boldsymbol{\dot X}}(t) = \mathit{\boldsymbol{F}}(t)\mathit{\boldsymbol{X}}(t) + \mathit{\boldsymbol{W}}(t) $ (21)

式中: $\mathit{\boldsymbol{X}} = {\left[ {\begin{array}{*{20}{c}} {\delta \mathit{\boldsymbol{v}}}&{\delta \mathit{\boldsymbol{p}}}&\nabla \end{array}} \right]^{\rm{T}}}$,令加速度零偏为$\nabla = {\left[ {\begin{array}{*{20}{c}} {{\nabla _x}}&{{\nabla _y}}&{{\nabla _z}} \end{array}} \right]^{\rm{T}}}$,速度误差为$\delta \mathit{\boldsymbol{v}} = {\left[ {\begin{array}{*{20}{c}} {\delta {v_E}}&{\delta {v_N}}&{\delta {v_U}} \end{array}} \right]^{\rm{T}}}$,位置误差为$\delta \mathit{\boldsymbol{p}} = {\left[ {\begin{array}{*{20}{c}} {\delta L}&{\delta \lambda }&{\delta h} \end{array}} \right]^{\rm{T}}}$

取速度位置差值作为观测量:

$ \mathit{\boldsymbol{Z}} = \left[ {\begin{array}{*{20}{c}} {v_{{\rm{INS}}}^n - v_{{\rm{GPS}}}^n}\\ {P_{{\rm{INS}}}^n - P_{{\rm{GPS}}}^n} \end{array}} \right]{\rm{ = }}\mathit{\boldsymbol{H}}(t)\mathit{\boldsymbol{X}}(t) + \mathit{\boldsymbol{V}}(t) $ (22)

相关参数变量见文献[13],这里不再赘述。

2.2 从滤波器设计

从滤波器状态方程为四元数误差线性微分方程,将姿态角误差作为量测量,量测方程利用四元数误差与姿态角误差之间的转换关系得到。

2.2.1 滤波算法模型

通过加性四元数误差微分方程整理出的状态方程为:

$ {{\mathit{\boldsymbol{\dot X}}}_a}(t) = {\mathit{\boldsymbol{F}}_a}(t){\mathit{\boldsymbol{X}}_a}(t) + {\mathit{\boldsymbol{w}}_a}(t) $ (23)

式中${\mathit{\boldsymbol{X}}_a} = {[\begin{array}{*{20}{l}} {\delta {q_0}}&{\delta {q_1}}&{\delta {q_2}}&{\delta {q_3}}&{{\varepsilon _x}}&{{\varepsilon _y}}&{{\varepsilon _z}} \end{array}]^{\rm{T}}}$, wa是系统噪声,本文设为白噪声。相关参数变量见文献[14-15],这里不再赘述。

观测量设为加速度计与双天线GPS航向构成的姿态角与惯导系统解算出的姿态角之差:

$ \left[ {\begin{array}{*{20}{c}} {\delta \hat \theta }\\ {\delta \hat \gamma }\\ {\delta \hat \varphi } \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{{\hat \theta }_{{\rm{ins}}}}}\\ {{{\hat \gamma }_{{\rm{ins}}}}}\\ {{{\hat \varphi }_{{\rm{ins}}}}} \end{array}} \right] - \left[ {\begin{array}{*{20}{c}} {{{\hat \theta }_a}}\\ {{{\hat \gamma }_a}}\\ {{{\hat \varphi }_{{\rm{gps}}}}} \end{array}} \right] $ (24)

而上述姿态误差角不能直接当成观测量使用,需要利用变换矩阵将其转换为数学平台失准角才可用于导航滤波。

利用四元数与旋转角的关系可知:

$ \varPhi = \sqrt {\phi _x^2 + \phi _y^2 + \phi _z^2} $ (25)
$ \begin{array}{c} {\mathit{\boldsymbol{Q}}_e} = {\left[ {\begin{array}{*{20}{c}} {{q_{e0}}}&{{q_{e1}}}&{{q_{e2}}}&{{q_{e3}}} \end{array}} \right]^{\rm{T}}} = \\ {\left[ {\begin{array}{*{20}{c}} {\cos {{\frac{\varPhi }{2}}}}&{{{\frac{{{\phi _x}} }{\varPhi}}}\sin {{\frac{\varPhi }{2}}}}&{{\frac{{{\phi _y}} }{\varPhi}}\sin {{\frac{\varPhi }{2}}}}&{{\frac{{{\phi _z}} }{\varPhi}}\sin {{\frac{\varPhi }{2}}}} \end{array}} \right]^{\rm{T}}} \end{array} $ (26)

${\mathit{\boldsymbol{Q}}_e} - {\left[ {\begin{array}{*{20}{c}} 1&0&0&0 \end{array}} \right]^{\rm{T}}}$作为量测量,则量测方程为:

$ \begin{array}{c} {\mathit{\boldsymbol{Z}}_a}(t) = \left[ {\begin{array}{*{20}{c}} {{q_{e0}} - 1}\\ {{q_{e1}}}\\ {{q_{e2}}}\\ {{q_{e3}}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {\cos \frac{\varPhi }{2} - 1}\\ {\frac{{{\phi _x}}}{\varPhi }\sin \frac{\varPhi }{2}}\\ {\frac{{{\phi _y}}}{\varPhi }\sin \frac{\varPhi }{2}}\\ {\frac{{{\phi _z}}}{\varPhi }\sin \frac{\varPhi }{2}} \end{array}} \right] = {\mathit{\boldsymbol{H}}_a}(t){\mathit{\boldsymbol{X}}_a}(t) + \\ {\mathit{\boldsymbol{v}}_a}(t) = \left[ {\begin{array}{*{20}{c}} {\hat q{}_0}&{\hat q{}_1}&{\hat q{}_2}&{\hat q{}_3}\\ { - \hat q{}_1}&{\hat q{}_0}&{\hat q{}_3}&{ - \hat q{}_2}\\ { - \hat q{}_2}&{ - \hat q{}_3}&{\hat q{}_0}&{\hat q{}_1}\\ { - \hat q{}_3}&{\hat q{}_2}&{ - \hat q{}_1}&{\hat q{}_0} \end{array}} \right]{\mathit{\boldsymbol{X}}_a}(t) + {\mathit{\boldsymbol{v}}_a}(t) \end{array} $ (27)

式中va(t)为量测白噪声。

2.2.2 自适应性改进

本文将UFIR(主滤波器)的新息引入自适应渐消因子ck[16-17]中,对级联卡尔曼滤波器的从滤波器进行改进:

$ \begin{array}{l} {\mathit{\boldsymbol{N}}_k} = \left\{ {\begin{array}{*{20}{c}} {\left( {{\mathit{\boldsymbol{Z}}_k} - \mathit{\boldsymbol{H}}{{\mathit{\boldsymbol{\hat X}}}_{k|N}}} \right){{\left( {{\mathit{\boldsymbol{Z}}_k} - \mathit{\boldsymbol{H}}{{\mathit{\boldsymbol{\hat X}}}_{k|N}}} \right)}^{\rm{T}}},}&{k = 1}\\ {\frac{{\left( {{\mathit{\boldsymbol{Z}}_k} - \mathit{\boldsymbol{H}}{{\mathit{\boldsymbol{\hat X}}}_{k|N}}} \right){{\left( {{\mathit{\boldsymbol{Z}}_k} - \mathit{\boldsymbol{H}}{{\mathit{\boldsymbol{\hat X}}}_{k|N}}} \right)}^{\rm{T}}} + \rho \times {\mathit{\boldsymbol{N}}_k}}}{{1 + \rho }},}&{k > 1} \end{array}} \right.\\ {c_k} = \left\{ {\begin{array}{*{20}{c}} {\beta \frac{{{\rm{tr}}\left( {{\mathit{\boldsymbol{N}}_k}} \right)}}{{{\rm{tr}}\left( {{\mathit{\boldsymbol{G}}_k}} \right)}},}&{{c_k} \ge 1}\\ {1,}&{{c_k} < 1} \end{array}} \right. \end{array} $ (28)

式中β为调节因子。

利用式(28)对自适应卡尔曼滤波增益方程进行优化(具体卡尔曼算法流程见文献[13]):

$ {\mathit{\boldsymbol{K}}_k} = {c_k}{\mathit{\boldsymbol{P}}_{k|k - 1}}{\mathit{\boldsymbol{H}}^{\rm{T}}}{\left[ {\mathit{\boldsymbol{H}}{\mathit{\boldsymbol{P}}_{k|k - 1}}{\mathit{\boldsymbol{H}}^{\rm{T}}} + {\mathit{\boldsymbol{R}}_k}} \right]^{ - 1}} $ (29)
3 仿真评估与分析

通过仿真计算和实验验证对提出的导航滤波算法性能进行了评估,并与文献[6]中提出的改进型自适应扩展卡尔曼滤波算法和最佳滤波窗长下的标准UFIR算法进行了比较。

3.1 仿真计算

假设车辆进行机动行驶,仿真时间2 760 s,数据更新频率200 Hz,解算周期0.01 s。仿真轨迹如图 2所示。其中,改进型自适应扩展卡尔曼滤波[7](AEKF)的遗忘因子b取0.986,标准UFIR的$\hat N$取72(具体见图 3);级联式GPS/INS滤波算法(下文称CUFIR)的主滤波器的N的范围取(10, 110),L取5 000,Q取0.6。

Download:
图 2 仿真轨迹 Fig. 2 Simulation trajectory
Download:
图 3 最佳滤波窗长曲线 Fig. 3 The line of optimal filter window length

采用增大仿真轨迹的噪声方差的方式来验证级联式GPS/INS松组合导航滤波器在系统噪声和量测噪声统计不准确的情况下的滤波性能。在500~ 1 000 s,将系统噪声标准差增大10倍,在1 000~1 500 s,将系统噪声标准差增大20倍,在1 500~ 2 000 s,将系统噪声标准差增30倍,在2 000~2 500 s,将系统噪声标准差增大50倍,在2 500 s之后的时间段,将系统噪声的标准差增大100倍。

对所得仿真数据进行测试,N从10到110依次进行计算,确定最小均方误差所对应的N值。由曲线可知,最佳滤波窗长$\hat N{\rm{ = }}71 + 1 = 72$

速度和位置的滤波误差效果如图 45所示,当系统噪声特性发生变化后,1 500 s之前噪声特性的变化对滤波结果影响较小,1 500 s后由于系统噪声变化幅度增大,AEKF滤波精度开始降低,而另2种滤波的效果则不会被系统噪声特性的变化影响。与选取最佳滤波窗长的UFIR算法相比,CUFIR滤波效果与其大致相当,在速度方面甚至表现更好,此结果一方面证明在线估计$\hat N$方法的有效性,另一方面也表现设计的级联算法对系统的精度有所提升。

Download:
图 4 速度误差仿真曲线 Fig. 4 Velocity error simulation curve
Download:
图 5 位置误差仿真曲线 Fig. 5 Position error simulation curve
3.2 实验验证

通过跑车实验验证CUFIR的实用性,跑车轨迹见图 6

Download:
图 6 跑车轨迹 Fig. 6 The actual driving test route

车载INS/GPS组合导航系统包括1套型号为ADIS16488的MEMS和1台LEA-M8S GPS接收机,INS数据更新频率为205 Hz,GPS更新频率为5 Hz。跑车实验的真值由RTK提供。由于车载导航设备受外界环境的影响较大,故其系统噪声统计特性虽会发生变化,但变化幅度会小于仿真所设计的情况。

图 7图 8是处理跑车数据得到的速度和位置误差曲线,其中,标准UFIR滤波算法的滤波窗长为最佳滤波窗长,$\hat N$=65。由图可以看出,CUFIR的滤波效果明显好于另2种算法,在位置误差曲线上表现尤为明显。故和标准UFIR与AEKF相比,CUFIR在噪声特性变化幅度较小的实际系统中也具有优势,故该算法可以在一定程度上提高系统的鲁棒性和滤波精度。

Download:
图 7 实测速度误差曲线 Fig. 7 Measured velocity error curve
Download:
图 8 实测位置误差曲线 Fig. 8 Measured position error curve
4 结论

1) 提出的估计最佳滤波窗长的方法能有效地实现窗长的在线估计。

2) 通过仿真与实测,级联滤波算法CUFIR能有效提高滤波精度,增强系统的鲁棒性。

参考文献
[1]
韩斌子, 胡柏青. 基于时间序列建模的组合导航系统故障诊断[J]. 哈尔滨工程大学学报, 2018, 39(11): 1843-1847.
HAN Binzi, HU Baiqing. Fault diagnosis of integrated navigation system based on time series modeling[J]. Journal of Harbin Engineering University, 2018, 39(11): 1843-1847. (0)
[2]
踪华, 齐建宇, 熊攀, 等. 惯性/天文/卫星组合导航误差在线标定方法[J]. 哈尔滨工业大学学报, 2017, 49(4): 88-94.
ZONG Hua, QI Jianyu, XIONG Pan, et al. On-line errors calibration method for INS/CNS/GNSS integrated navigation[J]. Journal of Harbin Institute of Technology, 2017, 49(4): 88-94. (0)
[3]
周卫东, 蔡佳楠, 孙龙. GPS/SINS超紧组合导航系统自适应混合滤波算法[J]. 哈尔滨工业大学学报, 2014, 46(7): 47-52.
ZHOU Weidong, CAI Jianan, SUN Long. An adaptive revising filtering method for GPS/SINS ultra-tightly coupled system[J]. Journal of Harbin Institute of Technology, 2014, 46(7): 47-52. (0)
[4]
MENG Yang, GAO Shesheng, ZHONG Yongmin, et al. Covariance matching based adaptive unscented Kalman filter for direct filtering in INS/GNSS integration[J]. Acta astronautica, 2016, 120: 171-181. DOI:10.1016/j.actaastro.2015.12.014 (0)
[5]
JIANG Chen, ZHANG Shubi. A novel adaptively-robust strategy based on the mahalanobis distance for GPS/INS integrated navigation systems[J]. Sensors, 2018, 18(3): 695. DOI:10.3390/s18030695 (0)
[6]
孟秀云, 王语嫣. 一种SINS/GPS紧组合导航系统的改进自适应扩展卡尔曼滤波算法[J]. 北京理工大学学报, 2018, 38(6): 625-630, 636.
MENG Xiuyun, WANG Yuyan. An improved adaptive extended Kalman filtering algorithm of SINS/GPS tightly-coupled integrated navigation[J]. Transactions of Beijing Institute of Technology, 2018, 38(6): 625-630, 636. (0)
[7]
SHMALIY Y S. An iterative Kalman-like algorithm ignoring noise and initial conditions[J]. IEEE transactions on signal processing, 2011, 59(6): 2465-2473. DOI:10.1109/TSP.2011.2129516 (0)
[8]
XU Yuan, AHN C K, SHMALIY Y S, et al. Adaptive robust INS/UWB-integrated human tracking using UFIR filter bank[J]. Measurement, 2018, 123: 1-7. DOI:10.1016/j.measurement.2018.03.043 (0)
[9]
ZHAO Shunyi, SHMALIY Y S, AHN C K, et al. Adaptive-horizon iterative UFIR filtering algorithm with applications[J]. IEEE transactions on industrial electronics, 2018, 65(8): 6393-6402. DOI:10.1109/TIE.2017.2784405 (0)
[10]
刘飞, 范雪峰, 赵顺毅. 线性离散状态时滞系统UFIR滤波算法[J]. 控制与决策, 2017, 32(6): 1109-1114.
LIU Fei, FAN Xuefeng, ZHAO Shunyi. UFIR filtering algorithm for linear discrete system with state delay[J]. Control and decision, 2017, 32(6): 1109-1114. (0)
[11]
SHMALIY Y S, ZHAO Shunyi, AHN C K. Unbiased finite impluse response filtering: an iterative alternative to Kalman filtering ignoring noise and initial conditions[J]. IEEE control systems magazine, 2017, 37(5): 70-89. DOI:10.1109/MCS.2017.2718830 (0)
[12]
ZHAO Shunyi, SHMALIY Y S, LIU Fei. On the iterative computation of error matrix in unbiased FIR filtering[J]. IEEE signal processing letters, 2017, 24(5): 555-558. DOI:10.1109/LSP.2017.2682641 (0)
[13]
秦永元, 张洪钺, 汪叔华. 卡尔曼滤波与组合导航原理[M]. 3版. 西安: 西北工业大学出版社, 2015: 287-387.
QIN Yongyuan, ZHANG Hongyue, WANG Shuhua. Kalman filtering and integrated navigation principle[M]. 3rd ed. Xi'an: Northwestern Polytechnical University Press, 2015: 287-387. (0)
[14]
WEI Chunling, ZHANG Hongyue. SINS in-flight alignment using quaternion error models[J]. Chinese journal of aeronautics, 2001, 14(3): 166-170. (0)
[15]
李开龙, 胡柏青, 常路宾. 改进四元数无味卡尔曼滤波算法[J]. 系统工程与电子技术, 2016, 38(6): 1399-1404.
LI Kailong, HU Baiqing, CHANG Lubin. Modified quaternion unscented Kalman filter[J]. Systems engineering and electronics, 2016, 38(6): 1399-1404. DOI:10.3969/j.issn.1001-506X.2016.06.28 (0)
[16]
任家栋, 曾庆双, 朱虹. 自适应TSKF的空间电推进机动目标跟踪[J]. 哈尔滨工业大学学报, 2018, 50(4): 36-40.
REN Jiadong, ZENG Qingshuang, ZHU Hong. Tracking of space electric-propulsion maneuvering target based on adaptive two-stage Kalman filter[J]. Journal of Harbin Institute of Technology, 2018, 50(4): 36-40. (0)
[17]
付锦斌, 孙进平, 卢松涛, 等. 针对机动目标的改进UFIR跟踪算法[J]. 北京航空航天大学学报, 2015, 41(1): 77-82.
FU Jinbin, SUN Jinping, LU Songtao, et al. Maneuvering target tracking with modified unbiased FIR filter[J]. Journal of Beijing University of Aeronautics and Astronautics, 2015, 41(1): 77-82. DOI:10.3969/j.issn.1005-4561.2015.01.035 (0)