留言板

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

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

顾及系统噪声和观测噪声的分级自适应信息滤波算法

王振杰 刘慧敏 单瑞 贺凯飞 董凌宇

王振杰, 刘慧敏, 单瑞, 贺凯飞, 董凌宇. 顾及系统噪声和观测噪声的分级自适应信息滤波算法[J]. 武汉大学学报 ● 信息科学版, 2021, 46(1): 88-95. doi: 10.13203/j.whugis20190248
引用本文: 王振杰, 刘慧敏, 单瑞, 贺凯飞, 董凌宇. 顾及系统噪声和观测噪声的分级自适应信息滤波算法[J]. 武汉大学学报 ● 信息科学版, 2021, 46(1): 88-95. doi: 10.13203/j.whugis20190248
WANG Zhenjie, LIU Huimin, SHAN Rui, HE Kaifei, DONG Lingyu. Hierarchical Adaptive Information Filtering Algorithm Considering System Noise and Observation Noise[J]. Geomatics and Information Science of Wuhan University, 2021, 46(1): 88-95. doi: 10.13203/j.whugis20190248
Citation: WANG Zhenjie, LIU Huimin, SHAN Rui, HE Kaifei, DONG Lingyu. Hierarchical Adaptive Information Filtering Algorithm Considering System Noise and Observation Noise[J]. Geomatics and Information Science of Wuhan University, 2021, 46(1): 88-95. doi: 10.13203/j.whugis20190248

顾及系统噪声和观测噪声的分级自适应信息滤波算法

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

青岛海洋科学与技术国家实验室开放基金 QNLM2016ORP0401

国家重点研发计划 2016YFB0501700

国家重点研发计划 2016YFB0501705

中国地质调查局项目 DD20191003

国家自然科学基金青年项目 41604027

国家自然科学基金青年项目 41406115

中央高校基本科研业务费专项资金 18CX02054A

详细信息
    作者简介:

    王振杰,博士,教授,主要从事测量数据处理理论与方法研究。sdwzj@upc.edu.cn

    通讯作者: 单瑞,博士生,助理研究员。shanrui416@163.com
  • 中图分类号: P229.5

Hierarchical Adaptive Information Filtering Algorithm Considering System Noise and Observation Noise

Funds: 

The Qingdao National Laboratory for Marine Science and Technology Foundation QNLM2016ORP0401

the National Key Research and Development Program of China 2016YFB0501700

the National Key Research and Development Program of China 2016YFB0501705

the Geological Survey Projects of China Geological Survey DD20191003

the National Natural Science Foundation Youth Project 41604027

the National Natural Science Foundation Youth Project 41406115

the Specialized Fund for the Foundamental Research Funds for the Central Universities 18CX02054A

More Information
    Author Bio:

    WANG Zhenjie, PhD, professor, specializes in the theories and methods of geodetic data processing. E-mail: sdwzj@upc.edu.cn

    Corresponding author: SHAN Rui, PhD candidate, assistant researcher. E-mail: shanrui416@163.com
  • 摘要: 高精度的载体动态导航与定位不仅需要对载体异常扰动和观测异常有良好控制,还需要对状态方程系统噪声及观测噪声的时变特性有准确认识和处理。首先针对包含系统噪声的动力学模型和包含时变观测噪声的导航系统,提出一种基于信息滤波形式的分级自适应滤波算法。然后针对系统噪声的渐变性和突变性,增加了遗忘因子和二段自适应因子,提高了对突变噪声估计的稳定性;顾及观测噪声的时变特性,采用传感器间差分和观测数据历元差分法估计观测噪声协方差。最后进行了仿真实验和深海拖体实验,结果表明,该算法不仅可以有效地估计系统噪声,还能准确地估计时变观测噪声的协方差阵,提高水下载体动态参数的估计精度。
  • 图  1  水下运载体轨迹仿真

    Figure  1.  Trajectory of the Underwater Vehicle

    图  2  水下载体的姿态角和位移仿真

    Figure  2.  Simulation of Attitude and Position of Underwater Vehicle

    图  3  本文算法估计的洋流速度

    Figure  3.  Estimation for Current Velocity of Our Proposed Algorithm

    图  4  超短基线量测噪声矩阵的自适应估计结果

    Figure  4.  Adaptive Estimation Results of Observation Noise Matrix of USBL

    图  5  深海拖体和GAPS超短基线定位系统

    Figure  5.  Deep-Sea Towing Vehicle and GAPS USBL Positioning System

    图  6  导航传感器时间戳及水下拖体的运动轨迹

    Figure  6.  Time Stamp of Navigation Sensors and Motion Track of Underwater Towed Vehicle

    图  7  水平位置滤波处理结果

    Figure  7.  Processing Results of Horizontal Positioning Filtering

    图  8  水平速度滤波处理结果

    Figure  8.  Processing Results of Horizontal Velocity Filtering

    表  1  导航传感器的测量误差仿真参数

    Table  1.   Simulation Parameters of Measurement Errors of Navigation Sensor

    噪声类型 USBL/m DVL/(m·s-1) 航向角/(°) 姿态角/(°)
    随机噪声 0.005r 0.03 0.10 0.03
    系统噪声 cos(t/3 600) 0.05V 0.05 0.01
    注:r表示USBL基阵到信标的斜距;t表示仿真时间;V表示水下载体速度
    下载: 导出CSV

    表  2  4种算法的定位精度/m

    Table  2.   Positioning Accuracies of Four Algorithms/m

    定位精度 USBL+Depth组合 KF算法 增广KF算法 本文算法
    水平方向精度 1.94 2.46 1.06 0.94
    垂直方向精度 1.68 1.03 0.81 0.76
    下载: 导出CSV
  • [1] 杨元喜, 何海波, 徐天河.论动态自适应滤波[J].测绘学报, 2001, 30(4):293-298 doi:  10.3321/j.issn:1001-1595.2001.04.004

    Yang Yuanxi, He Haibo, Xu Tianhe.Adaptive Robust Filtering for Kinematic GPS Positioning[J].Acta Geodaetica et Cartographica Sinica, 2001, 30(4):293-298 doi:  10.3321/j.issn:1001-1595.2001.04.004
    [2] 欧吉坤, 柴艳菊, 袁运斌.自适应选权滤波[C].重力学与固体潮学术研讨会, 武汉, 2014

    Ou Jikun, Chai Yanju, Yuan Yunbin.Adaptive Filtering for Kinematic Positioning by Selection of the Parameter Weights[C].Symposium on Gravity and Solid Tide, Wuhan, China, 2014
    [3] 崔先强, 杨元喜, 高为广.多种有色噪声自适应滤波算法的比较[J].武汉大学学报·信息科学版, 2006, 31(8):731-735 http://ch.whu.edu.cn/article/id/2533

    Cui Xianqiang, Yang Yuanxi, Gao Weiguang.Comparison of Adaptive Filter Algrithmetics in Controlling Influence of Colored Noises[J].Geomatics and Information Science of Wuhan University, 2006, 31(8):731-735 http://ch.whu.edu.cn/article/id/2533
    [4] 赵长胜, 陶本藻.有色噪声作用下的卡尔曼滤波[J].武汉大学学报·信息科学版, 2008, 33(2):180-182 http://ch.whu.edu.cn/article/id/1550

    Zhao Changsheng, Tao Benzao.Kalman Filtering of Linear System with Colored Noises[J].Geomatics and Information Science of Wuhan University, 2008, 33(2):180-182 http://ch.whu.edu.cn/article/id/1550
    [5] Yang Y, Gao W.An Optimal Adaptive Kalman Filter[J].Journal of Geodesy, 2006, 80(4):177-183 doi:  10.1007/s00190-006-0041-0
    [6] Zhou Q, Zhang H, Li Y, et al.An Adaptive Low-Cost GNSS/MEMS-IMU Tightly-Coupled Integration System with Aiding Measurement in a GNSS Signal-Challenged Environment[J].Sensors, 2015, 15(9):23 953-23 982 doi:  10.3390/s150923953
    [7] Jiang L, Zhang H.Redundant Measurement-Based Second Order Mutual Difference Adaptive Kalman Filter[J].Automatica, 2019, 100:396-402 doi:  10.1016/j.automatica.2018.11.037
    [8] 隋立芬, 刘雁雨, 王威.自适应序贯平差及其应用[J].武汉大学学报·信息科学版, 2007, 32(1):51-54 http://ch.whu.edu.cn/article/id/1797

    Sui Lifen, Liu Yanyu, Wang Wei.Adaptive Sequential Adjustment and Its Application[J].Geomatics and Information Science of Wuhan University, 2007, 32(1):51-54 http://ch.whu.edu.cn/article/id/1797
    [9] Zhang Qinghua.Adaptive Kalman Filter for Actuator Fault Diagnosis[J].Automatica, 2018, 93:333-342 doi:  10.1016/j.automatica.2018.03.075
    [10] Tsai S H, Lin M H, Zheng C H, et al.Actuator Fault Detection and Performance Recovery with Kalman Filter-Based Adaptive Observer[J].International Journal of General Systems, 2007, 36(4):375-398 doi:  10.1080/03081070600928963
    [11] 杨元喜, 徐天河.基于移动开窗法协方差估计和方差分量估计的自适应滤波[J].武汉大学学报·信息科学版, 2003, 28(6):714-718 http://ch.whu.edu.cn/article/id/4832

    Yang Yuanxi, Xu Tianhe.An Adaptive Kalman Filter Combining Variance Component Estimation with Covariance Matrix Estimation Based on Moving Window[J].Geomatics and Information Science of Wuhan University, 2003, 28(6):714-718 http://ch.whu.edu.cn/article/id/4832
    [12] 徐韩, 曾超, 黄清华.一种基于渐消因子的区间卡尔曼滤波器方法[J].武汉大学学报·信息科学版, 2017, 42(12):1 826-1 833 doi:  10.13203/j.whugis20150567

    Xu Han, Zeng Chao, Huang Qinghua.Kalman Filtering for Ultra-Tight Coupled from Stable State to Catastrophe State in Noise Condition[J].Geomatics and Information Science of Wuhan University, 2017, 42(12):1 826-1 833 doi:  10.13203/j.whugis20150567
  • [1] 胡思奇, 欧阳永忠, 刘会杰, 朱野, 赵灵峰, 董超.  机载海洋激光测深系统的自适应匹配滤波算法 . 武汉大学学报 ● 信息科学版, 2021, 46(9): 1395-1403. doi: 10.13203/j.whugis20200402
    [2] 种洋, 柴洪洲, 苏明晓, 郭云飞, 陈洁.  基于量测残差估计残差协方差的RAE-PEKF匹配算法 . 武汉大学学报 ● 信息科学版, 2020, 45(2): 179-188. doi: 10.13203/j.whugis20180264
    [3] 张勇, 王庆, 黄永江.  非差增强信息与差分虚拟观测数据的等价变换理论 . 武汉大学学报 ● 信息科学版, 2019, 44(2): 221-227. doi: 10.13203/j.whugis20170087
    [4] 龚学文, 王甫红.  星载GPS伪距多路径误差与观测噪声对自主定轨的影响分析 . 武汉大学学报 ● 信息科学版, 2018, 43(7): 1048-1055. doi: 10.13203/j.whugis20160223
    [5] 王伟, 李姗姗, 邢志斌, 李中全, 黄韵颖.  RAE并行滤波的重力异常匹配算法 . 武汉大学学报 ● 信息科学版, 2016, 41(12): 1664-1670. doi: 10.13203/j.whugis20140652
    [6] 郭际明, 黄长军, 喻小东, 聂智平.  利用BEMD-自适应滤波去除SAR干涉图噪声 . 武汉大学学报 ● 信息科学版, 2014, 39(4): 422-427. doi: 10.13203/j.whugis20120013
    [7] 李征航, 龚晓颖, 刘万科.  误差与先验信息对导航卫星自主定轨的影响研究 . 武汉大学学报 ● 信息科学版, 2011, 36(7): 797-801.
    [8] 何正斌, 吴富梅, 聂建亮.  先验协方差阵误差对动态Kalman滤波解的影响 . 武汉大学学报 ● 信息科学版, 2011, 36(1): 34-38.
    [9] 钟波, 汪海洪, 罗志才, 崔立鲁.  顾及观测噪声和数据分辨率的线性估计方法研究 . 武汉大学学报 ● 信息科学版, 2009, 34(3): 273-276.
    [10] 黄贤源, 隋立芬, 范澎湃.  有色观测噪声的随机模型级数表示及其补偿法 . 武汉大学学报 ● 信息科学版, 2008, 33(6): 644-647.
    [11] 吴江飞, 黄珹.  非线性自适应抗差滤波定轨算法 . 武汉大学学报 ● 信息科学版, 2008, 33(2): 187-190.
    [12] 何典, 袁运斌, 柴艳菊.  GPS/INS组合中观测噪声方差阵的自适应估计方法研究 . 武汉大学学报 ● 信息科学版, 2008, 33(8): 838-841.
    [13] 赵长胜, 陶本藻.  有色噪声作用下的抗差卡尔曼滤波 . 武汉大学学报 ● 信息科学版, 2007, 32(10): 880-882.
    [14] 高为广, 杨元喜, 崔先强, 张双成.  IMU/GPS组合导航系统自适应Kalman滤波算法 . 武汉大学学报 ● 信息科学版, 2006, 31(5): 466-469.
    [15] 杨元喜, 高为广.  两种渐消滤波与自适应抗差滤波的综合比较分析 . 武汉大学学报 ● 信息科学版, 2006, 31(11): 980-982.
    [16] 崔先强, 杨元喜, 高为广.  多种有色噪声自适应滤波算法的比较 . 武汉大学学报 ● 信息科学版, 2006, 31(8): 731-735.
    [17] 伊廷华, 李宏男, 伊晓东, 王国新.  基于自适应噪声抵消与小波滤波的GPS监测误差分析 . 武汉大学学报 ● 信息科学版, 2006, 31(11): 995-998.
    [18] 杨元喜, 高为广.  基于多传感器观测信息抗差估计的自适应融合导航 . 武汉大学学报 ● 信息科学版, 2004, 29(10): 885-888.
    [19] 卜方玲, 徐新.  一种基于小波分析的SAR图像斑点噪声滤波算法 . 武汉大学学报 ● 信息科学版, 2001, 26(4): 315-319,330.
    [20] 李军, 林宗坚.  小波变换的多分辨率支集用于噪声图像低通滤波 . 武汉大学学报 ● 信息科学版, 1996, 21(4): 366-370.
  • 加载中
图(8) / 表(2)
计量
  • 文章访问数:  513
  • HTML全文浏览量:  185
  • PDF下载量:  71
  • 被引次数: 0
出版历程
  • 收稿日期:  2019-08-26
  • 刊出日期:  2021-01-05

顾及系统噪声和观测噪声的分级自适应信息滤波算法

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

    青岛海洋科学与技术国家实验室开放基金 QNLM2016ORP0401

    国家重点研发计划 2016YFB0501700

    国家重点研发计划 2016YFB0501705

    中国地质调查局项目 DD20191003

    国家自然科学基金青年项目 41604027

    国家自然科学基金青年项目 41406115

    中央高校基本科研业务费专项资金 18CX02054A

    作者简介:

    王振杰,博士,教授,主要从事测量数据处理理论与方法研究。sdwzj@upc.edu.cn

    通讯作者: 单瑞,博士生,助理研究员。shanrui416@163.com
  • 中图分类号: P229.5

摘要: 高精度的载体动态导航与定位不仅需要对载体异常扰动和观测异常有良好控制,还需要对状态方程系统噪声及观测噪声的时变特性有准确认识和处理。首先针对包含系统噪声的动力学模型和包含时变观测噪声的导航系统,提出一种基于信息滤波形式的分级自适应滤波算法。然后针对系统噪声的渐变性和突变性,增加了遗忘因子和二段自适应因子,提高了对突变噪声估计的稳定性;顾及观测噪声的时变特性,采用传感器间差分和观测数据历元差分法估计观测噪声协方差。最后进行了仿真实验和深海拖体实验,结果表明,该算法不仅可以有效地估计系统噪声,还能准确地估计时变观测噪声的协方差阵,提高水下载体动态参数的估计精度。

English Abstract

王振杰, 刘慧敏, 单瑞, 贺凯飞, 董凌宇. 顾及系统噪声和观测噪声的分级自适应信息滤波算法[J]. 武汉大学学报 ● 信息科学版, 2021, 46(1): 88-95. doi: 10.13203/j.whugis20190248
引用本文: 王振杰, 刘慧敏, 单瑞, 贺凯飞, 董凌宇. 顾及系统噪声和观测噪声的分级自适应信息滤波算法[J]. 武汉大学学报 ● 信息科学版, 2021, 46(1): 88-95. doi: 10.13203/j.whugis20190248
WANG Zhenjie, LIU Huimin, SHAN Rui, HE Kaifei, DONG Lingyu. Hierarchical Adaptive Information Filtering Algorithm Considering System Noise and Observation Noise[J]. Geomatics and Information Science of Wuhan University, 2021, 46(1): 88-95. doi: 10.13203/j.whugis20190248
Citation: WANG Zhenjie, LIU Huimin, SHAN Rui, HE Kaifei, DONG Lingyu. Hierarchical Adaptive Information Filtering Algorithm Considering System Noise and Observation Noise[J]. Geomatics and Information Science of Wuhan University, 2021, 46(1): 88-95. doi: 10.13203/j.whugis20190248
  • 动态导航与定位通常应用卡尔曼滤波(Kalman filter,KF)算法实现[1-2],为了提高载体动态导航与定位的精度和可靠性,许多学者结合实际问题不断优化和发展Kalman滤波理论。构建可靠的函数模型、随机模型和合理的估计方法是保证Kalman滤波精度和可靠性的关键,然而载体在复杂环境下一般很难建立准确的状态方程。此外,基于验前统计信息才能获取随机模型的统计信息,对于随时空变化会发生较大变化的观测信息,例如声学信号等,难以精确表达其随机模型。因此面对实际导航问题,设计合理可靠的自适应滤波算法十分必要。

    导航系统中的系统噪声通常指一种不变或保持规律性变化的噪声,这类噪声的数量级一般高于随机噪声[3]。系统误差的处理是Kalman滤波领域中长期存在的棘手问题之一,文献[1-6]对系统误差补偿问题进行了深入研究,目前常用的系统误差补偿方法主要有附加系统误差参数方法、附加系统权方法、附加系统误差参数和系统权方法3类[3]。上述方法都能一定程度上减弱系统误差的影响,但面对系统误差的渐变性、突变性等复杂特点仍有不足,且很难保证滤波系统的可观测性和可控性。标准的Kalman滤波要求观测噪声模型和状态模型准确已知,这在实际应用中很难实现。基于新息序列或残差序列开窗平滑的Sage滤波是常用的噪声协方差阵的自适应估计方法,一般分为新息自适应估计(innovation based adaptive estimation,IAE)方法和残差自适应估计(residual based adaptive estimation,RAE)方法[5]。无论是IAE方法还是RAE方法,对当前状态的估计都需基于历史估计,因此当前状态估计的精度受历史估计误差的影响,且估计观测噪声要求多个历元的观测残差同类、同分布、同维等,因此上述方法应用在组合导航有一定局限性。

    本文首先结合系统噪声的特性,通过对系统噪声模型进行简单假设,基于信息滤波理论和抗差自适应滤波理论,提出了对系统噪声进行参数分离和估计的自适应滤波算法。该算法根据已有经验设计阈值,不需要构造不同的增广形式就能对系统噪声进行估计,更有利于滤波的实时性和稳定性。然后针对观测噪声模型,基于观测值差分技术,采用两种观测噪声矩阵的自适应估计方法,即基于传感器间差分的估计方法和基于观测数据历元差分的观测噪声协方差估计方法[6-7],提高了对时变观测噪声矩阵的自适应估计精度。最后结合水下声学导航定位实验和深海拖体实验,验证了本文提出的自适应滤波算法的有效性。

    • 设含有状态系统误差的状态方程和观测方程如下:

      $$\left\{ {\begin{array}{*{20}{l}} {{\mathit{\boldsymbol{x}}_k} = {\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_k}{\mathit{\boldsymbol{x}}_{k - 1}} + {\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_k}{\mathit{\boldsymbol{\theta }}_{\mathit{\boldsymbol{k}} - 1}} + {\mathit{\boldsymbol{w}}_k}}\\ {{\mathit{\boldsymbol{y}}_k} = {\mathit{\boldsymbol{C}}_k}{\mathit{\boldsymbol{x}}_k} + {\mathit{\boldsymbol{v}}_k}} \end{array}} \right. $$ (1)

      式中,xkykθk分别为k时刻的状态向量、观测向量以及系统噪声;wkvk分别为k时刻的状态噪声向量、观测噪声向量,且满足wkN(0, Qk)、vkN(0, Rk);ΦΓC分别为状态转移矩阵、系统误差线性缩放矩阵和观测方程系数阵。若假设系统噪声为固定常数或者分段固定常数,则式(1)可写为下面的增广形式[3-4]

      $$\left\{ {\begin{array}{*{20}{l}} {\left[ {\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{x}}_k}}\\ {{\mathit{\boldsymbol{\theta }}_k}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_k}}&{{\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_k}}\\ 0&I \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{x}}_{k - 1}}}\\ {{\mathit{\boldsymbol{\theta }}_{k - 1}}} \end{array}} \right] + \left[ {\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{w}}_k}}\\ 0 \end{array}} \right]}\\ {{\mathit{\boldsymbol{y}}_k} = \left[ {\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{C}}_k}}&0 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{x}}_k}}\\ {{\mathit{\boldsymbol{\theta }}_k}} \end{array}} \right] + {\mathit{\boldsymbol{v}}_k}\begin{array}{*{20}{c}} {}&{} \end{array}} \end{array}} \right. $$ (2)

      式中,I为单位矩阵。

      经过增广后的状态方程和观测方程可使用经典Kalman滤波对系统噪声向量和状态向量直接进行估计,但式(2)的增广形式简化了系统噪声的物理模型,忽略了系统噪声存在的较强时变特性。本文将系统误差从一般的滤波方程中分离,分析系统误差对式(1)的影响,并用分级控制的方法对系统噪声进行处理。若不考虑噪声项,式(2)的广义最小二乘解为[8]

      $$\begin{array}{l} \mathit{\boldsymbol{x}}_k^{} = {(\mathit{\boldsymbol{C}}_k^{\rm{T}}\mathit{\boldsymbol{R}}_k^{ - 1}{\mathit{\boldsymbol{C}}_k} + {(\mathit{\boldsymbol{P}}_k^p)^{ - 1}})^{ - 1}}(\mathit{\boldsymbol{C}}_k^{\rm{T}}\mathit{\boldsymbol{R}}_k^{ - 1}{\mathit{\boldsymbol{y}}_k} + \\ \left( {\mathit{\boldsymbol{P}}_k^p{)^{ - 1}}\mathit{\boldsymbol{x}}_k^p} \right) = \mathit{\boldsymbol{N}}_k^{ - 1}\mathit{\boldsymbol{C}}_k^{\rm{T}}\mathit{\boldsymbol{R}}_k^{ - 1}{\mathit{\boldsymbol{y}}_k} + {(\mathit{\boldsymbol{P}}_k^p)^{ - 1}}\mathit{\boldsymbol{x}}_k^p \end{array} $$ (3)
      $$\left\{ {\begin{array}{*{20}{l}} {\mathit{\boldsymbol{x}}_k^p = {\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_k}\mathit{\boldsymbol{x}}_{k - 1}^{} + {\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_k}{\mathit{\boldsymbol{\theta }}_{k - 1}}}\\ {{\mathit{\boldsymbol{N}}_k} = \mathit{\boldsymbol{C}}_k^{\rm{T}}\mathit{\boldsymbol{R}}_k^{ - 1}{\mathit{\boldsymbol{C}}_k} + {{(\mathit{\boldsymbol{P}}_k^p)}^{ - 1}}}\\ {\mathit{\boldsymbol{P}}_k^p = {\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_k}\mathit{\boldsymbol{P}}_{k - 1}^{}\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_k^{\rm{T}} + {\mathit{\boldsymbol{Q}}_k}} \end{array}} \right. $$ (4)

      式中,Pkp表示预测状态协方差阵;xkp表示xk-1θk-1经过一步预测后的线性组合;xk同样由xk-1θk-1共同确定[9-10]。将xk分解为xk=xku+xkθ,两个参数满足:

      $$\left\{ {\begin{array}{*{20}{l}} {\mathit{\boldsymbol{x}}_k^u = \mathit{\boldsymbol{N}}_k^{ - 1}(\mathit{\boldsymbol{C}}_k^{\rm{T}}\mathit{\boldsymbol{R}}_k^{ - 1}{\mathit{\boldsymbol{y}}_k} + \left( {\mathit{\boldsymbol{P}}_k^p{)^{ - 1}}{\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_k}\mathit{\boldsymbol{x}}_{k - 1}^u} \right)}\\ {\mathit{\boldsymbol{x}}_k^\theta = \mathit{\boldsymbol{N}}_k^{ - 1}({{(\mathit{\boldsymbol{P}}_k^p)}^{ - 1}}{\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_k}\mathit{\boldsymbol{x}}_{k - 1}^\theta + \left( {\mathit{\boldsymbol{P}}_k^p{)^{ - 1}}{\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_k}{\mathit{\boldsymbol{\theta }}_{\mathit{\boldsymbol{k}} - 1}}} \right)} \end{array}} \right. $$ (5)

      式(5)是在不考虑系统噪声Γkθk-1时的信息滤波估计,可采用标准的信息滤波形式递归计算。若考虑高斯白噪声影响,式(5)的信息滤波估计变为:

      $$\mathit{\boldsymbol{\hat x}}_k^u = \mathit{\boldsymbol{N}}_k^{ - 1}(\mathit{\boldsymbol{C}}_k^{\rm{T}}\mathit{\boldsymbol{R}}_k^{ - 1}{\mathit{\boldsymbol{y}}_k} + \left( {\mathit{\boldsymbol{P}}_k^p{)^{ - 1}}{\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_k}\mathit{\boldsymbol{\hat x}}_{k - 1}^u} \right) $$ (6)
      $$\mathit{\boldsymbol{\hat x}}_k^\theta = \mathit{\boldsymbol{N}}_k^{ - 1}({(\mathit{\boldsymbol{P}}_k^p)^{ - 1}}{\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_k}\mathit{\boldsymbol{\hat x}}_{k - 1}^\theta + \left( {\mathit{\boldsymbol{P}}_k^p{)^{ - 1}}{\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_k}{{\mathit{\boldsymbol{\hat \theta }}}_{k - 1}}} \right) + {\mathit{\boldsymbol{ \boldsymbol{\varDelta} }}_{k - 1}} $$ (7)

      式中,Δk-1为系统噪声估值${\mathit{\boldsymbol{\hat \theta }}_{k - 1}}$对$\mathit{\boldsymbol{\hat x}}_k^\theta $估值的补偿项,反映了系统噪声的时变。考虑线性空间的基本线性运算,可以建立系统噪声${\mathit{\boldsymbol{\hat \theta }}_{k}}$与$\mathit{\boldsymbol{\hat x}}_k^\theta $的线性关系,假设在任意时刻都存在系数阵Τk使线性关系${\boldsymbol{\hat{x}}}_{k}^{\theta}=\boldsymbol{T}_{k} {\boldsymbol{\hat{\theta}}}_{k}$成立,则有:

      $${\mathit{\boldsymbol{{T}}}_k}{\mathit{\boldsymbol{\hat \theta }}_k} = \mathit{\boldsymbol{N}}_k^{ - 1}({(\mathit{\boldsymbol{P}}_k^p)^{ - 1}}{\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_k}{\mathit{\boldsymbol{{T}}}_{k - 1}}{\mathit{\boldsymbol{\hat \theta }}_{k - 1}} + \left( {\mathit{\boldsymbol{P}}_k^p{)^{ - 1}}{\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_k}{{\mathit{\boldsymbol{\hat \theta }}}_{\mathit{\boldsymbol{k}} - 1}}} \right) + {\mathit{\boldsymbol{ \boldsymbol{\varDelta} }}_{k - 1}} $$ (8)

      式(8)成立的意义是建立系统噪声估计值对状态估计值的线性表达式。假设系统噪声的估值${\mathit{\boldsymbol{\hat \theta }}_k}$满足${\mathit{\boldsymbol{\hat \theta }}_k} = {\mathit{\boldsymbol{\hat \theta }}_{k - 1}} + {\mathit{\boldsymbol{\varepsilon }}_{k - 1}}$,$\mathit{\boldsymbol{\varepsilon }}$为系统噪声的时变项,此时式(8)变为:

      $${\mathit{\boldsymbol{{T}}}_k}\left( {{{\mathit{\boldsymbol{\hat \theta }}}_{k - 1}} + {\mathit{\boldsymbol{\varepsilon }}_{k - 1}}} \right) = \mathit{\boldsymbol{N}}_k^{ - 1}({(\mathit{\boldsymbol{P}}_k^p)^{ - 1}}{\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_k}{\mathit{\boldsymbol{{T}}}_{k - 1}}{\mathit{\boldsymbol{\hat \theta }}_{k - 1}} + \left( {\mathit{\boldsymbol{P}}_k^p{)^{ - 1}}{\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_k}{{\mathit{\boldsymbol{\hat \theta }}}_{\mathit{\boldsymbol{k}} - 1}}} \right) + {\mathit{\boldsymbol{ \boldsymbol{\varDelta} }}_{k - 1}} $$ (9)

      若令Δk-1=Τkεk-1,则式(9)可简化为:

      $${\mathit{\boldsymbol{{T}}}_k}{\mathit{\boldsymbol{\hat \theta }}_{k - 1}} = \mathit{\boldsymbol{N}}_k^{ - 1}({(\mathit{\boldsymbol{P}}_k^p)^{ - 1}}{\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_k}{\mathit{\boldsymbol{{T}}}_{k - 1}}{\mathit{\boldsymbol{\hat \theta }}_{k - 1}} + \left( {\mathit{\boldsymbol{P}}_k^p{)^{ - 1}}{\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_k}{{\mathit{\boldsymbol{\hat \theta }}}_{k - 1}}} \right) $$ (10)

      进而得到Τk的递归表达形式:

      $${\mathit{\boldsymbol{{T}}}_k} = \mathit{\boldsymbol{N}}_k^{ - 1}({(\mathit{\boldsymbol{P}}_k^p)^{ - 1}}{\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_k}{\mathit{\boldsymbol{{T}}}_{k - 1}} + \left( {\mathit{\boldsymbol{P}}_k^p{)^{ - 1}}{\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_k}} \right) $$ (11)

      此时式(7)可重新表示为:

      $$\mathit{\boldsymbol{\hat x}}_k^\theta = \mathit{\boldsymbol{N}}_k^{ - 1}({(\mathit{\boldsymbol{P}}_k^p)^{ - 1}}{\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_k}\mathit{\boldsymbol{\hat x}}_{k - 1}^\theta + \left( {\mathit{\boldsymbol{P}}_k^p{)^{ - 1}}{\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_k}{{\mathit{\boldsymbol{\hat \theta }}}_{\mathit{\boldsymbol{k}} - 1}}} \right) + {\mathit{\boldsymbol{{T}}}_k}\left( {{{\mathit{\boldsymbol{\hat \theta }}}_k} - {{\mathit{\boldsymbol{\hat \theta }}}_{k - 1}}} \right) $$ (12)

      对式(6)、式(12)等号两端进行求和可得:

      $$\mathit{\boldsymbol{\hat x}}_k^{} = \mathit{\boldsymbol{N}}_k^{ - 1}(\mathit{\boldsymbol{C}}_k^{{T}}\mathit{\boldsymbol{R}}_k^{ - 1}{\mathit{\boldsymbol{y}}_k} + {(\mathit{\boldsymbol{P}}_k^p)^{ - 1}}{\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_k}\mathit{\boldsymbol{\hat x}}_{k - 1}^{} + \left( {\mathit{\boldsymbol{P}}_k^p{)^{ - 1}}{\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_k}{{\mathit{\boldsymbol{\hat \theta }}}_{\mathit{\boldsymbol{k}} - 1}}} \right) + {\mathit{\boldsymbol{{T}}}_k}\left( {{{\mathit{\boldsymbol{\hat \theta }}}_k} - {{\mathit{\boldsymbol{\hat \theta }}}_{k - 1}}} \right) $$ (13)

      式(13)为自适应滤波的表达形式,此时只需要考虑${\mathit{\boldsymbol{\hat \theta }}_k}$的递归计算问题。

    • 本文考虑两种常见的系统噪声变化情况:(1)系统噪声随时间变化,但在一定时间内稳定或呈稳定趋势变化;(2)系统噪声出现突然的跳变,跳变后保持一段时间内稳定或稳定变化。针对以上两种情况分别设计对应的自适应滤波算法,推导式(13)中${\mathit{\boldsymbol{\hat \theta }}_k}$的自适应估计方法。

      首先定义Vk为式(1)中不考虑系统噪声项的标准Kalman滤波新息向量,计算如下:

      $${\mathit{\boldsymbol{V}}_k} = {\mathit{\boldsymbol{y}}_k} - {\mathit{\boldsymbol{C}}_k}\left( {{\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_k}\mathit{\boldsymbol{\hat x}}_{k - 1}^u} \right) $$ (14)

      Vk应满足VkN(0, Σk),Σk=Rk+CkPkpCkT。当模型正确时,Vk的期望应该为0,结合$\mathit{\boldsymbol{\hat x}}_{k - 1}^\theta = \boldsymbol{T}_{k-1} \hat{\boldsymbol{\theta}}_{k-1}$,Vk同样满足:

      $${\mathit{\boldsymbol{V}}_k} = {\mathit{\boldsymbol{C}}_k}{\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_k}{\mathit{\boldsymbol{{T}}}_{k - 1}}{\mathit{\boldsymbol{\hat \theta }}_{k - 1}} + {\mathit{\boldsymbol{C}}_k}{\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_k}{\mathit{\boldsymbol{\hat \theta }}_{k - 1}} = \left( {{\mathit{\boldsymbol{C}}_k}{\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_k}{\mathit{\boldsymbol{{T}}}_{k - 1}} + {\mathit{\boldsymbol{C}}_k}{\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_k}} \right){\mathit{\boldsymbol{\hat \theta }}_{k - 1}} $$ (15)

      令${\mathit{\boldsymbol{D}}_k} = {\mathit{\boldsymbol{C}}_k}{\mathit{\boldsymbol{ \boldsymbol{\varPhi} }}_k}{\mathit{\boldsymbol{{T}}}_{k - 1}} + {\mathit{\boldsymbol{C}}_k}{\mathit{\boldsymbol{ \boldsymbol{\varGamma} }}_k}$,则式(15)可表示为:

      $${\mathit{\boldsymbol{V}}_k} = {\mathit{\boldsymbol{D}}_k}{\mathit{\boldsymbol{\hat \theta }}_{k - 1}} $$ (16)

      式(16)可采用序贯最小二乘(recursive least squares,RLS)算法进行递归计算得到[8]

      $$\left\{ {\begin{array}{*{20}{l}} {{\mathit{\boldsymbol{ \boldsymbol{\varPsi} }}_k} = {{({\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_k} + {\mathit{\boldsymbol{D}}_k}\mathit{\boldsymbol{ \boldsymbol{\varLambda} }}_{k - 1}^{}\mathit{\boldsymbol{D}}_k^{\rm{T}})}^{ - 1}}}\\ {{\mathit{\boldsymbol{G}}_k} = \mathit{\boldsymbol{ \boldsymbol{\varLambda} }}_{k - 1}^{}\mathit{\boldsymbol{D}}_k^{\rm{T}}{\mathit{\boldsymbol{ \boldsymbol{\varPsi} }}_k}}\\ {\mathit{\boldsymbol{ \boldsymbol{\varLambda} }}_k^{} = \mathit{\boldsymbol{ \boldsymbol{\varLambda} }}_{k - 1}^{} - \mathit{\boldsymbol{ \boldsymbol{\varLambda} }}_{k - 1}^{}\mathit{\boldsymbol{D}}_k^{\rm{T}}{\mathit{\boldsymbol{ \boldsymbol{\varPsi} }}_k}\mathit{\boldsymbol{D}}_k^{}\mathit{\boldsymbol{ \boldsymbol{\varLambda} }}_{k - 1}^{}}\\ {\mathit{\boldsymbol{\hat \theta }}_k^{{\rm{RLS}}} = \mathit{\boldsymbol{\hat \theta }}_{k - 1}^{{\rm{RLS}}} + {\mathit{\boldsymbol{G}}_k}\left( {{\mathit{\boldsymbol{V}}_k} - {\mathit{\boldsymbol{D}}_k}\mathit{\boldsymbol{\hat \theta }}_{k - 1}^{{\rm{RLS}}}} \right)} \end{array}} \right. $$ (17)

      式中,$\mathit{\boldsymbol{\hat \theta }}_k^{{\rm{RLS}}}$表示系统误差的序贯最小二乘估计。

      式(17)考虑了系统噪声的定常特性,利用RLS的序贯解对系统噪声进行估计。结合式(13),可得到一种新的无偏的信息滤波形式。若考虑系统噪声随时间的趋势性变化,可在式(17)中引入遗忘因子λλ∈[0, 1],增大当前新息向量对系统噪声估计的影响。此时的自适应RLS算法表示为:

      $$\left\{ {\begin{array}{*{20}{l}} {{{\mathit{\boldsymbol{ \boldsymbol{\bar \varPsi} }}}_k} = {{(\lambda {\mathit{\boldsymbol{ \boldsymbol{\varSigma} }}_k} + {\mathit{\boldsymbol{D}}_k}\mathit{\boldsymbol{ \boldsymbol{\bar \varLambda} }}_{k - 1}^{}\mathit{\boldsymbol{D}}_k^{\rm{T}})}^{ - 1}}}\\ {\mathit{\boldsymbol{ \boldsymbol{\bar \varLambda} }}_k^{} = {\lambda ^{ - 1}}\mathit{\boldsymbol{ \boldsymbol{\bar \varLambda} }}_{k - 1}^{} - {\lambda ^{ - 1}}\mathit{\boldsymbol{ \boldsymbol{\bar \varLambda} }}_{k - 1}^{}\mathit{\boldsymbol{D}}_k^{\rm{T}}{{\mathit{\boldsymbol{ \boldsymbol{\bar \varPsi} }}}_k}\mathit{\boldsymbol{D}}_k^{}\mathit{\boldsymbol{ \boldsymbol{\bar \varLambda} }}_{k - 1}^{}} \end{array}} \right. $$ (18)

      考虑部分系统噪声的突变特性,利用式(16)的观测残差向量${\mathit{\boldsymbol{\tilde v}}_k} = {\mathit{\boldsymbol{V}}_k} - {\mathit{\boldsymbol{D}}_k}\mathit{\boldsymbol{\hat \theta }}_{k - 1}^{{\rm{RLS}}}$和二段自适应因子[11-12],此时的自适应RLS算法表示为:

      $$\left\{ {\begin{array}{*{20}{l}} {{{\mathit{\boldsymbol{ \boldsymbol{\widetilde \varPsi} }}}_k} = {{\left( {{\alpha _k}{{\bf{\Sigma }}_k} + {D_k}{{\mathit{\boldsymbol{ \boldsymbol{\widetilde \varLambda} }}}_{k - 1}}D_k^{\rm{T}}} \right)}^{ - 1}}}\\ {{{\mathit{\boldsymbol{ \boldsymbol{\widetilde \varLambda} }}}_k} = \alpha _k^{ - 1}{{\mathit{\boldsymbol{ \boldsymbol{\widetilde \varLambda} }}}_{k - 1}} - \alpha _k^{ - 1}{{\mathit{\boldsymbol{ \boldsymbol{\widetilde \varLambda} }}}_{k - 1}}D_k^{\rm{T}}{{\mathit{\boldsymbol{ \boldsymbol{\widetilde \varPsi} }}}_k}{D_k}{{\mathit{\boldsymbol{ \boldsymbol{\widetilde \varLambda} }}}_{k - 1}}}\\ {{\alpha _k} \approx \left\{ {\begin{array}{*{20}{l}} {1, \Delta {{\tilde v}_k} \le c}\\ {\frac{c}{{\Delta {{\tilde v}_k}}}, \Delta {{\tilde v}_k} > c} \end{array}} \right.} \end{array}} \right. $$ (19)

      式中,${\rm{\Delta }}{\tilde v_k} = \frac{{\mathop \sum \limits_{i = 1}^s \tilde v_{ki}^2}}{{\mathop \sum \limits_{i = 1}^s \sigma _{ki}^2}}$,其中s为观测值个数,σki2表示Σk对角线元素,$\tilde v_{ki}^2$表示$\tilde v_{k}$中的元素;c为经验常数,通常按经验设为c∈[1.5, 2]。式(19)的意义在于减弱之前系统噪声估计对当前系统噪声的估计影响,提高突变后的系统噪声的收敛速度。对于可观测性较强的滤波系统,该方法可以得到较好的滤波结果,而对于弱观测系统应谨慎使用。

    • 测量噪声协方差阵的自适应估计多采用开窗法,应用较为广泛的有IAE开窗法和RAE开窗法,计算方法如下[12]

      $$\mathit{\boldsymbol{\hat R}}_k^{{\rm{IAE}}} = E[\mathit{\boldsymbol{e}}_k^I\left( {\mathit{\boldsymbol{e}}_k^I{)^{\rm{T}}}} \right] - {\mathit{\boldsymbol{C}}_k}\mathit{\boldsymbol{P}}_k^p\mathit{\boldsymbol{C}}_k^{\rm{T}} $$ (20)
      $$\mathit{\boldsymbol{\hat R}}_k^{{\rm{RAE}}} = E[{\rm{}}\mathit{\boldsymbol{e}}_k^R\left( {\mathit{\boldsymbol{e}}_k^R{)^{\rm{T}}}} \right] + {\mathit{\boldsymbol{C}}_k}\mathit{\boldsymbol{P}}_k^{}\mathit{\boldsymbol{C}}_k^{\rm{T}} $$ (21)

      式中,E[ ]表示求期望;$\mathit{\boldsymbol{e}}_k^I = {\mathit{\boldsymbol{y}}_k} - {\mathit{\boldsymbol{C}}_k}\mathit{\boldsymbol{\hat x}}_k^p$,为滤波新息向量;$\mathit{\boldsymbol{e}}_k^R = {\mathit{\boldsymbol{y}}_k} - {\mathit{\boldsymbol{C}}_k}\mathit{\boldsymbol{\hat x}}_k$,为残差向量。利用式(20)或式(21)结合开窗法可计算测量噪声的协方差矩阵。

      无论是IAE法还是RAE法,当前状态的估计都需要基于历史估计,因此当前状态估计的精度受历史估计误差的影响。针对此问题,本文利用差分思想进行观测噪声协方差阵的自适应估计。

    • 组合导航是目前导航领域研究的重点。在观测精度存在明显差异的传感器进行组合观测时,例如全球卫星导航定位系统(global navigation satellite system,GNSS)和捷联惯性导航系统(strap-down inertial navigation system,SINS)组合,或超短基线(ultra short baseline,USBL)声学定位系统和多普勒测速仪(Doppler velocity log,DVL)组合,当其中一类传感器出现观测协方差矩阵随时空变化较大且观测精度较差时,可利用另一类高精度传感器从原始观测数据中对低精度观测噪声矩阵进行求解。

      传感器r和传感器m对于同类观测值在同一时刻进行观测,得到观测向量ykrykm,此时的观测噪声协方差阵可由差分形式的近似式直接计算[8]

      $$\mathit{\boldsymbol{\hat R}}_k^{} \approx F\left( {\left( {\mathit{\boldsymbol{y}}_k^r - \mathit{\boldsymbol{y}}_{k - 1}^r} \right) - \left( {\mathit{\boldsymbol{y}}_k^m - \mathit{\boldsymbol{y}}_{k - 1}^m} \right)} \right)/2 $$ (22)

      式中,F( )表示协方差阵运算,其相关的证明参考文献[11]。

      利用高精度传感器的观测值,可以稳定地计算出低精度传感器存在时变特性的观测噪声协方差阵,即使是较短时间的观测数据,通过差分运算就能求出较准确的观测噪声协方差阵的近似估计。但这种方法需要两类精度差异较大的传感器的组合观测,并且仅应用于同时刻、同类的观测值。

    • 当仅存在单一的高频、高采样率传感器观测时,假设系统状态方程为时不变的,并忽略系统误差强时变因素,可对观测噪声阵进行如下估计[7]

      $$\begin{array}{*{20}{c}} {\mathit{\boldsymbol{\hat R}}_k^{} \approx [V\left( {\left( {{\mathit{\boldsymbol{y}}_k} - \mathit{\boldsymbol{y}}_{k - 1}^{}} \right) - \left( {\mathit{\boldsymbol{y}}_k^p - \mathit{\boldsymbol{y}}_{k - 1}^p} \right)} \right) - }\\ {{\mathit{\boldsymbol{C}}_k}{\mathit{\boldsymbol{Q}}_{k - 1}}\mathit{\boldsymbol{C}}_k^{\rm{T}}]/2} \end{array} $$ (23)

      式中,V( )表示求协方差运算;$\mathit{\boldsymbol{y}}_k^p = {\mathit{\boldsymbol{C}}_k}\mathit{\boldsymbol{\hat x}}_{k\left| {k - 2} \right.}^p$,$\mathit{\boldsymbol{y}}_{k - 1}^p = {\mathit{\boldsymbol{C}}_{k - 1}}\mathit{\boldsymbol{\hat x}}_{k - 1\left| {k - 2} \right.}^p$。

      式(23)可用于时不变系统或高采样率系统。利用时不变状态方程预测值组成虚拟观测方程,并与观测数据进行历元间差分估计观测噪声协方差。该方法可消除历元间变化不大的观测系统误差,得到较稳定的观测噪声协方差阵,有利于测量系统误差的参数化和分离。

    • 首先对水下载体的声学组合导航进行仿真实验。水下载体装有可测量载体相对于海流的相对速度的DVL和USBL声学定位系统。由于不同深度存在不同速度的未知海流,将载体相对海流的速度作为状态参数,并将未知海流的速度归入系统误差。仿真水深为2 300 m,拖体以1 m/s的常速度航行。导航传感器的测量误差仿真参数见表 1

      表 1  导航传感器的测量误差仿真参数

      Table 1.  Simulation Parameters of Measurement Errors of Navigation Sensor

      噪声类型 USBL/m DVL/(m·s-1) 航向角/(°) 姿态角/(°)
      随机噪声 0.005r 0.03 0.10 0.03
      系统噪声 cos(t/3 600) 0.05V 0.05 0.01
      注:r表示USBL基阵到信标的斜距;t表示仿真时间;V表示水下载体速度

      水下运载体的轨迹如图 1所示,红色菱形为USBL换能器基阵位置,黑色轨迹为水下载体轨迹。水下载体的姿态角仿真和位移仿真分别如图 2(a)2(b)所示。考虑海流的情况下,模拟上下两层都存在海流的情况。其中洋流方向为南北方向,上下两层间海流速度随时间变化存在突变可能性。将未知的洋流作为系统误差,并采用本文提出的分级自适应滤波算法进行估计,得到洋流的估计值结果如图 3所示。

      图  1  水下运载体轨迹仿真

      Figure 1.  Trajectory of the Underwater Vehicle

      图  2  水下载体的姿态角和位移仿真

      Figure 2.  Simulation of Attitude and Position of Underwater Vehicle

      图  3  本文算法估计的洋流速度

      Figure 3.  Estimation for Current Velocity of Our Proposed Algorithm

      为了验证本文算法对定位精度的改善,重复10次上述仿真实验,并统计了定位结果的均方根误差(root mean square error,RMSE)。表 2统计了4种算法的定位精度。其中USBL+Depth表示USBL(提供水平位置和斜距)和深度计(提供垂直深度)的组合定位;增广KF表示传统增广形式的KF算法。

      表 2  4种算法的定位精度/m

      Table 2.  Positioning Accuracies of Four Algorithms/m

      定位精度 USBL+Depth组合 KF算法 增广KF算法 本文算法
      水平方向精度 1.94 2.46 1.06 0.94
      垂直方向精度 1.68 1.03 0.81 0.76

      表 2可知,USBL+Depth组合在中水层水域的平均定位精度约为2 m,垂直方向精度约为1.7 m;仿真中KF算法未考虑洋流的影响,因而在水平方向定位结果较差;考虑洋流影响的增广KF算法与USBL+Depth组合相比,水平方向和垂直方向精度提高一倍以上;本文算法不仅对系统噪声进行了有效分离,同时可对超短基线强时变测量噪声矩阵进行实时估计;当系统误差的跳变模型未知,或跳变频繁发生时,本文算法可取得良好的滤波效果。

      仿真实验测试了本文算法对超短基线量测噪声矩阵的自适应估计情况。考虑水下载体运动较为缓慢及水下声学环境噪声的复杂性,采用50历元开窗估计,并计算其水平方向的测量噪声协方差R,结果如图 4所示。图 4中蓝点表示超短基线噪声真值,绿点表示RAE方法估计结果,红点表示本文历元差分的观测噪声协方差估计结果。由图 4可知,受系统状态噪声影响,RAE估计方法不能较好地反映超短基线定位精度的变化情况,而本文算法可以准确估计观测噪声的量级。尤其在其精度变化幅度较大时,本文算法仍较准确地算出噪声协方差阵。

      图  4  超短基线量测噪声矩阵的自适应估计结果

      Figure 4.  Adaptive Estimation Results of Observation Noise Matrix of USBL

    • 实验采用的深海拖体和IxBlue公司的全球声学定位系统(global acoustic positioning system,GAPS)型USBL系统,如图 5所示。深海拖体搭载着SINS定位系统、深度计、VDL和USBL定位信标。本实验水深约为1 100 m,其中深度计、USBL定位系统作为主导航系统,DVL作为辅助导航系统。垂直通道上以深度计测量的深度数据为参考,本文主要考虑拖体深水区水平方向上的导航定位。该实验采用的DVL是底跟踪测速模式,不考虑洋流影响,当DVL距离海底小于200 m时,可测量拖体相对于海底的速度。

      图  5  深海拖体和GAPS超短基线定位系统

      Figure 5.  Deep-Sea Towing Vehicle and GAPS USBL Positioning System

      图 6所示为导航传感器时间戳及水下拖体的运动轨迹。由图 6(a)6(b)可知,USBL在深水区(入水后6 500 s时)出现了观测数据缺失的情况,此时DVL测速信息可以作为有效的补充,一定程度上抑制了SINS系统导航发散的现象。由于很难获得水下动态载体的绝对定位结果,本实验同时将RAE方法滤波结果、本文算法与商业后处理软件得到的导航结果进行比较分析。作为主要比对对象,后处理软件可以结合SINS数据和前向双向滤波进行导航数据的后处理,得到更高精度的导航结果。

      图  6  导航传感器时间戳及水下拖体的运动轨迹

      Figure 6.  Time Stamp of Navigation Sensors and Motion Track of Underwater Towed Vehicle

      分别将USBL、KF+RAE方法估计USBL协方差的滤波位置及本文提出的分级自适应滤波算法同商业后处理软件的位置结果作差,误差结果如图 7所示。由图 7得到以下结论:(1)本文算法与其他算法相比,滤波位置误差稍有改善;(2)USBL系统在深水区的定位精度明显下降,此时USBL的噪声变化和斜距存在一定的正相关关系,但不严格对应;(3)实时估计USBL随机模型对组合导航有重要的意义,本文算法较传统的RAE方法得到的结果更加稳定。

      图  7  水平位置滤波处理结果

      Figure 7.  Processing Results of Horizontal Positioning Filtering

      分别将DVL、KF+RAE方法及本文提出的分级自适应滤波速度同商业后处理软件的速度结果作差,误差结果如图 8所示。从图 8中可得出以下结论:(1)本文算法在处理随机模型方面有明显的优势,DVL速度滤波结果明显改善;(2)在DVL底跟踪测速时,受声速和其他噪声的影响,DVL系统精度存在明显的变化;(3)传统的RAE方法需要假设状态模型的协方差阵比较准确,但在实际中很难获得。借助历元间差分观测数据及传感器间的差分观测数据对观测噪声进行估计,得到的滤波结果更接近后处理结果,验证了本文算法的有效性。

      图  8  水平速度滤波处理结果

      Figure 8.  Processing Results of Horizontal Velocity Filtering

    • 本文分析了系统噪声和状态参数之间的函数传递关系,提出了一种新的分级自适应滤波算法。该算法顾及系统误差的时变性和突变性,经测试能有效地应用在水下声学导航滤波案例中。基于高采样传感器原始观测值和较稳定的观测结构,本文借助历元间差分观测数据及传感器间的差分观测数据对观测噪声进行估计,提出了一种新的观测噪声协方差矩阵估计算法。该算法适用于含时变观测噪声的深海声学导航观测模型,为水下组合导航研究提供了一定的参考。

参考文献 (12)

目录

    /

    返回文章
    返回