留言板

尊敬的读者、作者、审稿人, 关于本刊的投稿、审稿、编辑和出版的任何问题, 您可以本页添加留言。我们将尽快给您答复。谢谢您的支持!

姓名
邮箱
手机号码
标题
留言内容
验证码

自适应渐消卡尔曼滤波及其在SINS初始对准中的应用

郭士荦 吴苗 许江宁 李京书

郭士荦, 吴苗, 许江宁, 李京书. 自适应渐消卡尔曼滤波及其在SINS初始对准中的应用[J]. 武汉大学学报 ● 信息科学版, 2018, 43(11): 1667-1672, 1680. doi: 10.13203/j.whugis20160548
引用本文: 郭士荦, 吴苗, 许江宁, 李京书. 自适应渐消卡尔曼滤波及其在SINS初始对准中的应用[J]. 武汉大学学报 ● 信息科学版, 2018, 43(11): 1667-1672, 1680. doi: 10.13203/j.whugis20160548
GUO Shiluo, WU Miao, XU Jiangning, LI Jingshu. Adaptive Fading Kalman Filter and Its Application in SINS Initial Alignment[J]. Geomatics and Information Science of Wuhan University, 2018, 43(11): 1667-1672, 1680. doi: 10.13203/j.whugis20160548
Citation: GUO Shiluo, WU Miao, XU Jiangning, LI Jingshu. Adaptive Fading Kalman Filter and Its Application in SINS Initial Alignment[J]. Geomatics and Information Science of Wuhan University, 2018, 43(11): 1667-1672, 1680. doi: 10.13203/j.whugis20160548

自适应渐消卡尔曼滤波及其在SINS初始对准中的应用

doi: 10.13203/j.whugis20160548
基金项目: 

国家自然科学基金 41574069

国家自然科学基金 41404002

国家自然科学基金 61503404

详细信息

Adaptive Fading Kalman Filter and Its Application in SINS Initial Alignment

Funds: 

The National Natural Science Foundation of China 41574069

The National Natural Science Foundation of China 41404002

The National Natural Science Foundation of China 61503404

More Information
图(3) / 表(2)
计量
  • 文章访问数:  1564
  • HTML全文浏览量:  106
  • PDF下载量:  322
  • 被引次数: 0
出版历程
  • 收稿日期:  2017-07-17
  • 刊出日期:  2018-11-05

自适应渐消卡尔曼滤波及其在SINS初始对准中的应用

doi: 10.13203/j.whugis20160548
    基金项目:

    国家自然科学基金 41574069

    国家自然科学基金 41404002

    国家自然科学基金 61503404

    作者简介:

    郭士荦, 博士, 主要从事惯性技术及应用研究。hg_guoshiluo@163.com

    通讯作者: 吴苗, 博士, 讲师。Wumiao_nav@163.com
  • 中图分类号: P228

摘要: 卡尔曼滤波常常被用于惯性导航系统初始对准算法,其使用前提是对系统状态进行建模,从而得到比较准确的系统噪声和观测噪声统计特性。在模型失配和观测噪声干扰的情况下,常规卡尔曼滤波会出现精度下降甚至发散,从而影响初始对准精度。针对这一问题,提出了一种新型渐消卡尔曼滤波算法,引入了多重渐消因子对预测误差协方差阵进行调整,设计了基于新息向量统计特性的滤波状态χ2检验条件,使渐消因子的引入时机更加合理,算法的自适应性得到增强。将改进的卡尔曼滤波算法应用到惯性导航系统的初始对准问题中,仿真试验和实测数据试验结果表明,与常规渐消因子滤波算法相比,新算法可以有效提高滤波精度及鲁棒性。

English Abstract

郭士荦, 吴苗, 许江宁, 李京书. 自适应渐消卡尔曼滤波及其在SINS初始对准中的应用[J]. 武汉大学学报 ● 信息科学版, 2018, 43(11): 1667-1672, 1680. doi: 10.13203/j.whugis20160548
引用本文: 郭士荦, 吴苗, 许江宁, 李京书. 自适应渐消卡尔曼滤波及其在SINS初始对准中的应用[J]. 武汉大学学报 ● 信息科学版, 2018, 43(11): 1667-1672, 1680. doi: 10.13203/j.whugis20160548
GUO Shiluo, WU Miao, XU Jiangning, LI Jingshu. Adaptive Fading Kalman Filter and Its Application in SINS Initial Alignment[J]. Geomatics and Information Science of Wuhan University, 2018, 43(11): 1667-1672, 1680. doi: 10.13203/j.whugis20160548
Citation: GUO Shiluo, WU Miao, XU Jiangning, LI Jingshu. Adaptive Fading Kalman Filter and Its Application in SINS Initial Alignment[J]. Geomatics and Information Science of Wuhan University, 2018, 43(11): 1667-1672, 1680. doi: 10.13203/j.whugis20160548
  • 捷联惯性导航系统(strapdown inertial navigation system,SINS)是一种自主式导航系统,其导航过程就是对惯性测量单元(inertial measurement unit,IMU)的输出进行导航解算,这实际上是一个航位推算过程。因此在SINS进入正常导航状态之前,需要给定初始值,即对其进行初始对准,初始对准的精度和速度决定了惯导系统的工作性能[1]

    卡尔曼滤波(Kalman filter,KF)是一种常用的惯导系统精对准算法[2-6],针对不同的对准条件,延伸出了扩展卡尔曼滤波(extended Kalman filter,EKF)[4]、无迹卡尔曼滤波(unscented Kalman filter,UKF)[2, 5]、自适应卡尔曼滤波(adaptive Kalman filter,AKF)[6]等多种形式。KF初始对准算法的对准精度依赖于惯导系统数学模型的准确性以及观测量的噪声水平。系统数学模型多采用惯导误差模型,观测量一般是通过外界参考系统(如GPS、多普勒计程仪等)获取参考基准后得到的。目前对惯导误差模型的研究已经较为深入,但在初始对准条件设置方面还存在不确定性;另外,观测量的噪声扰动也会影响滤波结果的收敛性。

    渐消因子卡尔曼滤波(fading Kalman filter,FKF)是抑制滤波发散的一种有效方法[7-9],通过引入渐消因子对预测误差协方差阵进行调整,增加当前观测值对滤波结果的修正作用。文献[10-11]提出了通过强迫新息序列正交来确定渐消因子的渐消滤波算法,使算法具有自适应性;针对单渐消因子对多变量跟踪能力较差的问题,文献[12-14]提出了基于多重渐消因子的渐消滤波算法,并通过强迫新息序列协方差估计值与理论值相等来确定渐消因子的数值,提高了不同滤波通道的调节能力;文献[15-17]将多重渐消因子滤波算法应用到SINS初始对准问题中,有效提高了复杂干扰环境下的对准精度。

    传统FKF算法通过强迫新息序列正交或强迫新息序列协方差估计值与其理论值相等的方式来确定渐消因子的取值,并以渐消因子大于1作为引入条件,这种引入条件具有一定的随机性,制约了FKF的工作性能。文献[18]引入影响函数来验证KF的鲁棒性,并利用M估计对过程噪声进行膨胀以提高KF算法质量。文献[19-20]基于新息向量统计特性构造χ2检验条件,判断观测噪声是否异常,通过标量因子调节量测噪声或先验协方差,以增强滤波器的鲁棒性[21-22]。受此启发,本文引入了基于新息向量统计特性的滤波状态χ2检验条件,以一定置信度条件下的检验异常作为渐消因子的引入门槛,使得渐消因子的引入时机更加合理,算法的自适应性得到增强。仿真和实测数据试验表明,新算法具有更好的滤波精度和鲁棒性。

    • 设线性离散系统的状态方程以及观测方程为:

      $$ \left\{ \begin{array}{l} {\mathit{\boldsymbol{x}}_{k + 1}} = {\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_{k/k + 1}}{\mathit{\boldsymbol{x}}_k} + {\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_k}{\mathit{\boldsymbol{w}}_k}\\ {\mathit{\boldsymbol{z}}_k} = {\mathit{\boldsymbol{H}}_k}{\mathit{\boldsymbol{x}}_k} + {\mathit{\boldsymbol{v}}_k} \end{array} \right. $$ (1)

      式中,xkRnk时刻的状态向量;zkRmk时刻的量测向量;Φk/k-1Rn×nk-1时刻至k时刻的状态一步转移矩阵;Γkk时刻的系统噪声阵;HkRm×nk时刻的观测阵;wkvk分别代表k时刻系统的噪声序列与量测噪声序列, 同时,wkvk满足:

      $$ \left\{ \begin{array}{l} E\left[ {{\mathit{\boldsymbol{w}}_k}} \right] = {\bf{0}},Cov\left[ {{\mathit{\boldsymbol{w}}_k},{\mathit{\boldsymbol{w}}_j}} \right] = E\left[ {{\mathit{\boldsymbol{w}}_k}\mathit{\boldsymbol{w}}_j^{\rm{T}}} \right] = {\mathit{\boldsymbol{Q}}_k}{\delta _{kj}}\\ E\left[ {{\mathit{\boldsymbol{v}}_k}} \right] = {\bf{0}},Cov\left[ {{\mathit{\boldsymbol{v}}_k},{\mathit{\boldsymbol{v}}_j}} \right] = E\left[ {{\mathit{\boldsymbol{v}}_k}\mathit{\boldsymbol{v}}_j^{\rm{T}}} \right] = {\mathit{\boldsymbol{R}}_k}{\delta _{kj}}\\ {\rm{Cov}}\left[ {{\mathit{\boldsymbol{w}}_k},{\mathit{\boldsymbol{v}}_j}} \right] = E\left[ {{\mathit{\boldsymbol{w}}_k}\mathit{\boldsymbol{v}}_j^{\rm{T}}} \right] = {\bf{0}} \end{array} \right. $$ (2)

      式中,Qk为非负的系统噪声序列方差阵;Rk为正定的量测噪声序列方差阵;δkj为Kronecker-δ函数。

      KF基本方程为:

      $$ {{\mathit{\boldsymbol{\hat x}}}_{k/k - 1}} = {\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_{k/k - 1}}{{\mathit{\boldsymbol{\hat x}}}_{k - 1}} $$ (3)
      $$ {\mathit{\boldsymbol{P}}_{k/k - 1}} = {\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_{k/k - 1}}{\mathit{\boldsymbol{P}}_{k - 1}}\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_{k/k - 1}^{\rm{T}} + {\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_{k - 1}}{\mathit{\boldsymbol{Q}}_{k - 1}}\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_{k - 1}^{\rm{T}} $$ (4)
      $$ {\mathit{\boldsymbol{K}}_k} = {\mathit{\boldsymbol{P}}_{k/k - 1}}\mathit{\boldsymbol{H}}_k^{\rm{T}}{\left( {{\mathit{\boldsymbol{H}}_k}{\mathit{\boldsymbol{P}}_{k/k - 1}}\mathit{\boldsymbol{H}}_k^{\rm{T}} + {\mathit{\boldsymbol{R}}_k}} \right)^{ - 1}} $$ (5)
      $$ {{\mathit{\boldsymbol{\hat x}}}_k} = {{\mathit{\boldsymbol{\hat x}}}_{k/k - 1}} + {\mathit{\boldsymbol{K}}_k}\left( {{\mathit{\boldsymbol{z}}_k} - {\mathit{\boldsymbol{H}}_k}{{\mathit{\boldsymbol{\hat x}}}_{k/k - 1}}} \right) $$ (6)
      $$ {\mathit{\boldsymbol{P}}_k} = \left( {\mathit{\boldsymbol{I}} - {\mathit{\boldsymbol{K}}_k}{\mathit{\boldsymbol{H}}_k}} \right) {\mathit{\boldsymbol{P}}_{k/k - 1}} $$ (7)

      式(3)~(7)中,$ {\mathit{\boldsymbol{\hat x}}_{k - 1}} $是滤波器对k-1时刻状态值的估计值;$ {\mathit{\boldsymbol{\hat x}}_{k/k - 1}} $是滤波器对k时刻状态值的一步预测;Pk-1k-1时刻的状态估计均方误差阵;Pk/k-1k-1时刻至k时刻的一步预测均方误差阵;Kkk时刻的滤波增益矩阵;Qk-1k-1时刻的系统噪声序列方差阵。

      典型的FKF与KF的区别是在滤波方程式(4)中引入渐消因子s,得到:

      $$ {\mathit{\boldsymbol{P}}_{k/k - 1}} = {s_k}{\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_{k/k - 1}}{\mathit{\boldsymbol{P}}_{k - 1}}\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_{k/k - 1}^{\rm{T}} + {\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_{k - 1}}{\mathit{\boldsymbol{Q}}_{k - 1}}\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_{k - 1}^{\rm{T}} $$ (8)

      sk > 1可以使Pk/k-1增大,由式(5)可知,Kk增大,意味着对当前量测值的利用权重比增大,同时由式(6)可知,对$ {\mathit{\boldsymbol{\hat x}}_{k/k - 1}}$利用的权重相对降低,即降低了陈旧量测值对估计值的影响。渐消因子sk是基于新息向量的统计特性来确定的,滤波器中每一步预测的新息向量为:

      $$ {\mathit{\boldsymbol{\varepsilon }}_k} = {\mathit{\boldsymbol{z}}_k} - {\mathit{\boldsymbol{H}}_k}{\mathit{\boldsymbol{x}}_{k/k - 1}} $$ (9)

      在滤波最优的情况下,其协方差理论值满足:

      $$ {\mathit{\boldsymbol{C}}_k} = E\left[ {{\mathit{\boldsymbol{\varepsilon }}_k}\mathit{\boldsymbol{\varepsilon }}_k^{\rm{T}}} \right] = {\mathit{\boldsymbol{H}}_k}{\mathit{\boldsymbol{P}}_{k/k - 1}}\mathit{\boldsymbol{H}}_k^{\rm{T}} + {\mathit{\boldsymbol{R}}_k} $$ (10)

      新息序列协方差的估计值${\mathit{\boldsymbol{\hat C}}_k} $可以通过开窗法[10]得到:

      $$ {{\mathit{\boldsymbol{\hat C}}}_k} = \frac{1}{{k - 1}}\sum\limits_{i = 1}^k {{\mathit{\boldsymbol{\varepsilon }}_i}\mathit{\boldsymbol{\varepsilon }}_i^{\rm{T}}} $$ (11)

      文献[14]提出了一种基于指数加权的新息向量协方差估计方法,加大了新近新息向量的权重,进一步提高了$ {\mathit{\boldsymbol{\hat C}}_k} $的估计精度,其计算方法为:

      $$ \left\{ \begin{array}{l} {{\mathit{\boldsymbol{\hat C}}}_k} = {d_k}{\mathit{\boldsymbol{\varepsilon }}_i}\mathit{\boldsymbol{\varepsilon }}_i^{\rm{T}} + \left( {1 - {d_k}} \right){{\mathit{\boldsymbol{\hat C}}}_{k - 1}}\\ {d_k} = \frac{{1 - b}}{{1 - {b^k}}} \end{array} \right. $$ (12)

      式中,$ {\mathit{\boldsymbol{\hat C}}_0} = 0 $;b为遗忘因子,一般取值为0.7~0.95之间。这种递推计算方式实现起来较为简便,文献[14]通过仿真试验和实测数据试验证明了使用这种估计方法的有效性。

      结合式(8)至式(12)可以得到渐消因子sk的确定方法为:

      $$ \left\{ \begin{array}{l} {s_k} = \max \left\{ {1,{\rm{tr}}\left( {{\mathit{\boldsymbol{N}}_k}} \right)/{\rm{tr}}\left( {{\mathit{\boldsymbol{M}}_k}} \right)} \right\}\\ {\mathit{\boldsymbol{N}}_k} = {{\mathit{\boldsymbol{\hat C}}}_k} - {\mathit{\boldsymbol{H}}_k}{\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_{k - 1}}{\mathit{\boldsymbol{Q}}_{k - 1}}\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_{k - 1}^{\rm{T}}\mathit{\boldsymbol{H}}_k^{\rm{T}} - {\mathit{\boldsymbol{R}}_k}\\ {\mathit{\boldsymbol{M}}_k} = {\mathit{\boldsymbol{H}}_k}{\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_{k/k - 1}}{\mathit{\boldsymbol{P}}_{k - 1}}\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_{k/k - 1}^{\rm{T}}\mathit{\boldsymbol{H}}_k^{\rm{T}} \end{array} \right. $$ (13)

      式中,tr(·)为求迹算子;$ {\mathit{\boldsymbol{\hat C}}_k}$为式(12)得到的k时刻的新息序列协方差估计值。

    • 单渐消因子对Pk/k-1各通道的调节能力是相同的,制约了其调节能力,引入多重渐消因子Sk可以有效解决这一问题。

      Sk=diag{s1  s2sn},si表示第i(i=1, 2…n)个状态变量对应的渐消因子,则可将式(8)写成:

      $$ {\mathit{\boldsymbol{P}}_{k/k - 1}} = {\mathit{\boldsymbol{S}}_k}{\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_{k/k - 1}}{\mathit{\boldsymbol{P}}_{k - 1}}\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_{k/k - 1}^{\rm{T}}\mathit{\boldsymbol{S}}_k^{\rm{T}} + {\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_{k - 1}}{\mathit{\boldsymbol{Q}}_{k - 1}}\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_{k - 1}^{\rm{T}} $$ (14)

      将式(14)代入式(10),可得:

      $$ \begin{array}{*{20}{c}} {{\mathit{\boldsymbol{C}}_k} = {\mathit{\boldsymbol{H}}_k}\left( {{\mathit{\boldsymbol{S}}_k}{\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_{k/k - 1}}{\mathit{\boldsymbol{P}}_{k - 1}}\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_{k/k - 1}^{\rm{T}}\mathit{\boldsymbol{S}}_k^{\rm{T}} + } \right.}\\ {\left. {{\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_{k - 1}}{\mathit{\boldsymbol{Q}}_{k - 1}}\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_{k - 1}^{\rm{T}}} \right)\mathit{\boldsymbol{H}}_k^{\rm{T}} + {\mathit{\boldsymbol{R}}_k}} \end{array} $$ (15)

      将新息序列协方差用式(12)计算得到的估计值${\mathit{\boldsymbol{\hat C}}_k} $代替,则有:

      $$ \left\{ \begin{array}{l} {\mathit{\boldsymbol{H}}_k}{\mathit{\boldsymbol{M}}_k}\mathit{\boldsymbol{H}}_k^{\rm{T}} = {\mathit{\boldsymbol{N}}_k}\\ {\mathit{\boldsymbol{N}}_k} = {{\mathit{\boldsymbol{\hat C}}}_k} - {\mathit{\boldsymbol{H}}_k}{\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_{k - 1}}{\mathit{\boldsymbol{Q}}_{k - 1}}\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_{k - 1}^{\rm{T}}\mathit{\boldsymbol{H}}_k^{\rm{T}} - {\mathit{\boldsymbol{R}}_k}\\ {\mathit{\boldsymbol{M}}_k} = {\mathit{\boldsymbol{S}}_k}{\mathit{\boldsymbol{J}}_k}\mathit{\boldsymbol{S}}_k^{\rm{T}}\\ {\mathit{\boldsymbol{J}}_k} = {\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_{k/k - 1}}{\mathit{\boldsymbol{P}}_{k - 1}}\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_{k/k - 1}^{\rm{T}} \end{array} \right. $$ (16)

      若量测矩阵Hk满足:

      $$ {\mathit{\boldsymbol{H}}_k} = \left[ {\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{\lambda }}_{m \times m}}}&{{{\bf{0}}_{m \times \left( {n - m} \right)}}} \end{array}} \right] $$ (17)

      式中,λm×m=diag{λ1  λ2λm};n为状态向量维数;m为量测向量维数。将矩阵Jk分为4个部分:

      $$ {\mathit{\boldsymbol{J}}_k} = \left[ {\begin{array}{*{20}{c}} \begin{array}{l} \mathit{\boldsymbol{J}}_{m \times m}^{11}\\ \mathit{\boldsymbol{J}}_{\left( {n - m} \right) \times m}^{21} \end{array}&\begin{array}{l} \mathit{\boldsymbol{J}}_{m \times \left( {n - m} \right)}^{12}\\ \mathit{\boldsymbol{J}}_{\left( {n - m} \right) \times \left( {n - m} \right)}^{22} \end{array} \end{array}} \right] $$ (18)

      定义矩阵L=[li, j]=λm×mJm×m11λm×m,将式(16)代入式(15)可以解得多重渐消因子的求法为:

      $$ {S_k}\left( i \right) = \left\{ \begin{array}{l} \max \left\{ {1,\sqrt {\frac{{{\mathit{\boldsymbol{N}}_k}\left( {i,i} \right)}}{{{l_{i,i}}}}} } \right\},i = 1,2 \cdots m\\ 1,i = m + 1,m + 2 \cdots n \end{array} \right. $$ (19)

      式中,li, i为矩阵L的第i个对角元素;Nk(i, i)为矩阵Nk的第i个对角元素。

      由于一般情况下滤波观测维数小于状态维数,因此FKF只对部分状态通道进行了直接渐消,式(19)也反映了这一实际情况。

    • 传统的FKF通过强制新息序列协方差估计值与理论值相等来确定渐消因子的数值,如果数值大于1则引入渐消因子。而实际上通过新息序列协方差的估计值计算出来的渐消因子是否大于1,并不代表滤波器当前是否处于稳定状态,这就有可能造成在并不需要引入渐消因子时,引入了渐消因子。

      针对传统渐消滤波中渐消因子的引入条件过于宽松的问题,本文提出了一种基于新息向量统计特性的检验条件。在滤波稳定的情况下,新息序列的统计特性满足[11]

      $$ \left\{ \begin{array}{l} {\mathit{\boldsymbol{\varepsilon }}_k} \sim N\left( {{\bf{0}},{\mathit{\boldsymbol{H}}_k}{\mathit{\boldsymbol{P}}_{k/k - 1}}\mathit{\boldsymbol{H}}_k^{\rm{T}} + {\mathit{\boldsymbol{R}}_k}} \right)\\ {\gamma _k} = \mathit{\boldsymbol{\varepsilon }}_k^{\rm{T}}{\left[ {{\mathit{\boldsymbol{H}}_k}{\mathit{\boldsymbol{P}}_{k/k - 1}}\mathit{\boldsymbol{H}}_k^{\rm{T}} + {\mathit{\boldsymbol{R}}_k}} \right]^{ - 1}}{\mathit{\boldsymbol{\varepsilon }}_k} \sim {\chi ^2}\left( m \right) \end{array} \right. $$ (20)

      由式(20)可知,εk服从均值为0、方差为HkPk/k-1HkT+Rk的正态分布;γk服从自由度为mχ2分布,m为量测的维度。因此可以利用χ2分布上侧分位点的性质来进行滤波稳定性的判断[19-20]。例如,当量测维度m=2时,选取分位点ζ=9.210,则有[23]

      $$ P\left\{ {{\chi ^2}\left( 2 \right) > \zeta } \right\} = 1\% $$ (21)

      也就是说,在滤波正常的情况下,γk大于ζ的概率只有1%。根据假设检验原理,若γk > ζ,则在99%的置信度下可以认为滤波异常。本文以此作为渐消因子引入的判断条件,即首先进行正常KF并实时计算γk,若满足γk > ζ,则将式(4)换为式(14),引入渐消因子。与§1.2中的多重渐消因子滤波相比,在渐消因子的引入条件上进行了改进。

    • 假设SINS处于静基座状态,以SINS水平速度输出作为速度误差的观测量, 并令载体真实速度为Vn=[0 0 0],加速度计输出在n系的分量fn=[0 0 g](g为地球重力加速度),SINS所处的位置(Lλh)可由GPS等外部参考基准得到。

      建立SINS误差状态模型为:

      $$ \mathit{\boldsymbol{\dot x}} = \mathit{\boldsymbol{Ax}} + \mathit{\boldsymbol{w}} $$ (22)

      式中,

      $$ \mathit{\boldsymbol{x}} = {\left[ {{\rm{ \mathit{ δ} }}{v_{\rm{E}}}{\rm{ \mathit{ δ} }}{v_{\rm{N}}}{\varphi _x}{\varphi _y}{\varphi _z}{\nabla _x}{\nabla _y}{\varepsilon _x}{\varepsilon _y}{\varepsilon _z}} \right]^{\rm{T}}} $$ (23)
      $$ \mathit{\boldsymbol{w}} = {\left[ {{w_{{\rm{ \mathit{ δ} }}{v_{\rm{E}}}}}{w_{{\rm{ \mathit{ δ} }}{v_{\rm{N}}}}}{w_{{\varphi _x}}}{w_{{\varphi _y}}}{w_{{\varphi _z}}}0\;0\;0\;0\;0} \right]^{\rm{T}}} $$ (24)
      $$ \mathit{\boldsymbol{A = }}\left[ {\begin{array}{*{20}{c}} \begin{array}{l} {\mathit{\boldsymbol{F}}_1}\\ {{\bf{0}}_{5 \times 5}} \end{array}&\begin{array}{l} {\mathit{\boldsymbol{F}}_2}\\ {{\bf{0}}_{5 \times 5}} \end{array} \end{array}} \right] $$ (25)
      $$ {\mathit{\boldsymbol{F}}_1} = \left[ {\begin{array}{*{20}{c}} 0&{2{\mathit{\Omega }_{\rm{U}}}}&0&{ - g}&0\\ { - 2{\mathit{\Omega }_{\rm{U}}}}&0&g&0&0\\ 0&{ - 1/R}&0&{{\mathit{\Omega }_{\rm{U}}}}&{ - {\mathit{\Omega }_{\rm{N}}}}\\ {1/R}&0&{ - {\mathit{\Omega }_{\rm{U}}}}&0&0\\ {\tan L/R}&0&{{\mathit{\Omega }_{\rm{N}}}}&0&0 \end{array}} \right] $$ (26)
      $$ {\mathit{\boldsymbol{F}}_2} = \left[ {\begin{array}{*{20}{c}} {{C_{11}}}&{{C_{12}}}&0&0&0\\ {{C_{21}}}&{{C_{22}}}&0&0&0\\ 0&0&{ - {C_{11}}}&{ - {C_{12}}}&{ - {C_{13}}}\\ 0&0&{ - {C_{21}}}&{ - {C_{22}}}&{ - {C_{23}}}\\ 0&0&{ - {C_{31}}}&{ - {C_{32}}}&{ - {C_{33}}} \end{array}} \right] $$ (27)

      式中,δvE、δvN分别为东向和北向速度误差;φxφyφz分别为俯仰、横滚、航向3个轴方向的姿态误差角;wδvEwδvN分别为东向和北向速度误差的系统噪声;wφxwφywφz分别为俯仰、横滚、航向3个轴方向的姿态误差角的系统噪声;$ \nabla $x、$ \nabla $y为加速度计的随机常值偏置;εxεyεz为陀螺仪的随机常值漂移;ΩN=wie cosLΩU=wie sinL分别为地球自转角速度在北向和天向的分量,wie为地球自传角速度值;L为当地地理纬度;R为地球半径;Cij(i=1, 2, 3;j=1,2,3)为姿态矩阵Cbn中的元素。

      以水平速度误差作为观测量,在静基座状态下,惯导输出的速度即为速度误差:

      $$ \mathit{\boldsymbol{z}} = \mathit{\boldsymbol{Hx}} + \mathit{\boldsymbol{v}} $$ (28)

      式中,观测量z=[δvE  δvN]Tv为量测高斯白噪声,其噪声序列方差阵为Rk;量测矩阵H为:

      $$ \mathit{\boldsymbol{H}} = \left[ {\begin{array}{*{20}{c}} 1&0&0&0&0&0&0&0&0&0\\ 0&1&0&0&0&0&0&0&0&0 \end{array}} \right] $$ (29)
    • 捷联惯导系统初始对准仿真条件为:初始位置为(114.24° E, 30.58° N);粗对准后的初始失准角为φ=[10′  10′  30′]。其他参数设置如下:陀螺漂移ε及白噪声wg为:

      $$ \mathit{\boldsymbol{\varepsilon }} = \left[ {\begin{array}{*{20}{c}} {{{0.02}^ \circ }/{\rm{h}}}&{{{0.02}^ \circ }/{\rm{h}}}&{{{0.02}^ \circ }/{\rm{h}}} \end{array}} \right] $$
      $$ {\mathit{\boldsymbol{w}}_g} = \left[ {\begin{array}{*{20}{c}} {{{0.002}^ \circ }/{\rm{h}}}&{{{0.002}^ \circ }/{\rm{h}}}&{{{0.002}^ \circ }/{\rm{h}}} \end{array}} \right] $$

      加速度计零偏$ \nabla $及白噪声wa为:

      $$ \nabla = \left[ {\begin{array}{*{20}{c}} {1 \times {{10}^{ - 4}}g}&{1 \times {{10}^{ - 4}}g} \end{array}} \right] $$
      $$ {\mathit{\boldsymbol{w}}_a} = \left[ {\begin{array}{*{20}{c}} {1 \times {{10}^{ - 5}}g}&{1 \times {{10}^{ - 5}}g} \end{array}} \right] $$

      速度观测误差v为:

      $$ \mathit{\boldsymbol{v}} = \left[ {\begin{array}{*{20}{c}} {0.1\;{\rm{m}}/{\rm{s}}}&{0.1\;{\rm{m}}/{\rm{s}}} \end{array}} \right] $$

      初始条件为:

      $$ {{\mathit{\boldsymbol{\hat x}}}_0} = {\rm{zeros}}\left( {10,1} \right) $$
      $$ {\mathit{\boldsymbol{P}}_0} = {\rm{diag}}\left( {\left[ {\begin{array}{*{20}{c}} \mathit{\boldsymbol{v}}&\mathit{\boldsymbol{\varphi }}&\nabla &\mathit{\boldsymbol{\varepsilon }} \end{array}} \right]{.^2}} \right) $$
      $$ {\mathit{\boldsymbol{R}}_k} = {\rm{diag}}\left( {\left[ {\begin{array}{*{20}{c}} {0.1{\rm{m}}/{\rm{s}}}&{0.1{\rm{m}}/{\rm{s}}} \end{array}} \right]{.^2}} \right) $$
      $$ \mathit{\boldsymbol{Q}} = {\rm{diag}}\left( {\left[ {\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{w}}_a}}&{{\mathit{\boldsymbol{w}}_g}}&0&0&0&0&0 \end{array}} \right]{.^2}} \right) \cdot T $$

      采样时间T=1/10 s。

      为验证算法的有效性,设计系统模型不准确及观测量受噪声干扰的初始对准环境。令P0=10P0Qk=100Qk,同时对速度误差观测量施加噪声干扰zk=zk+[vD  vD]T,且有:

      $$ {v_D} = \left\{ \begin{array}{l} 0.5 \times {\rm{rand}}\left( {2,1} \right),3000\;{\rm{s}} < t < 400\;{\rm{s}}\\ 1 + \sin \left( {\frac{{\rm{ \mathit{ π} }}}{5}t + \frac{{\rm{ \mathit{ π} }}}{6}} \right),600\;{\rm{s}} < t < 700\;{\rm{s}}\\ 0,其他 \end{array} \right. $$

      分别使用常规KF(算法1)、文献[14]提出的多重渐消因子滤波算法(算法2)以及本文提出的基于新息向量统计特性检验的改进算法(算法3)进行初始对准仿真。在进行新息序列协方差估计时,取b=0.85,ζ=9.210。

      3种滤波算法得到的航向角对准误差曲线如图 1所示。

      图  1  仿真试验

      Figure 1.  Simulation Experiment

      图 1可以看出,在系统建模不准确及观测量受噪声干扰的情况下,算法1出现严重发散,对准结果并不可靠;算法2与算法3都能较好地抑制滤波器的扰动,但算法2的滤波曲线并没有达到很好的收敛效果,在10 min后出现轻微发散的情况;算法3的滤波结果较为稳定,10 min后航向角估计误差稳定在1′以内。表 1为3种算法最终的姿态对准误差。

      表 1  仿真数据姿态对准误差/(′)

      Table 1.  Attitude Errors with Simulation Data/(′)

      算法 φx φy φz
      算法1 9.94 -2.03 -104.30
      算法2 0.31 -0.30 3.33
      算法3 0.30 -0.26 -0.69

      表 1中,φxφyφz分别表示惯导系统俯仰、横滚、航向3个方向上的姿态对准误差。可以看出,算法1的对准误差较大,算法2、算法3都表现出较好的鲁棒性,算法3的对准精度要高于算法2。

    • 为进一步验证本文算法的有效性,选用北京自动化控制设备研究所研制的FOSC型光纤陀螺IMU组件,在实验室环境下采集光纤陀螺惯导原始数据,试验环境如图 2所示。

      图  2  试验环境

      Figure 2.  Experimental Environment

      数据采集过程中,给光纤IMU接入GPS信号进行高精度的静基座对准,以此结果作为对比试验的姿态基准, 并设置初始失准角为[10′  10′  30′],同样对速度误差观测量施加干扰vD,3种滤波算法得到的航向角对准误差曲线如图 3所示。

      图  3  实测数据验证

      Figure 3.  Experiment on Real Data

      图 3可见,实测数据对准结果与仿真试验基本一致。在外界干扰下,算法1的对准精度较低,算法2有一定改善,算法3的收敛效果优于算法2,最终航向对准精度在2′以内。表 2为种算法最终的姿态对准误差。

      表 2  实测数据姿态对准误差/(′)

      Table 2.  Attitude Errors with Real Data/(′)

      算法 φx φy φz
      算法1 -1.02 -0.13 -14.50
      算法2 -0.43 -0.17 -4.88
      算法3 -0.28 0.10 -1.28

      表 2说明实测数据验证结果与仿真试验结果是一致的,算法3的最终航向对准精度约为1.3′,在噪声干扰情况下仍保持了较高的对准精度。

    • 受系统建模不准确及外界观测噪声的干扰,常规卡尔曼滤波会出现精度下降甚至发散的情况。针对这一问题,本文研究了多重渐消因子卡尔曼滤波算法,提出了基于新息向量统计特性的滤波器工作状态χ2检验方法,使渐消因子的引入时机更加合理,算法的自适应性得到提高,克服了常规多重渐消因子滤波算法中因过度使用渐消因子导致的滤波精度下降问题。仿真试验与实测数据试验表明,本文提出的算法在系统建模不准确及观测噪声扰动的影响下,可以有效抑制滤波发散,与常规多重渐消因子滤波算法相比,精度更高,鲁棒性更好。

参考文献 (23)

目录

    /

    返回文章
    返回