CN112507281A - 一种基于双状态多因子抗差估计的sins/dvl紧组合系统 - Google Patents

一种基于双状态多因子抗差估计的sins/dvl紧组合系统 Download PDF

Info

Publication number
CN112507281A
CN112507281A CN202011302705.0A CN202011302705A CN112507281A CN 112507281 A CN112507281 A CN 112507281A CN 202011302705 A CN202011302705 A CN 202011302705A CN 112507281 A CN112507281 A CN 112507281A
Authority
CN
China
Prior art keywords
dvl
representing
state
sins
measurement
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202011302705.0A
Other languages
English (en)
Other versions
CN112507281B (zh
Inventor
张涛
刘射德
刘咏怡
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Southeast University
Original Assignee
Southeast University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Southeast University filed Critical Southeast University
Priority to CN202011302705.0A priority Critical patent/CN112507281B/zh
Publication of CN112507281A publication Critical patent/CN112507281A/zh
Application granted granted Critical
Publication of CN112507281B publication Critical patent/CN112507281B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • Mathematical Analysis (AREA)
  • Data Mining & Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • Computational Mathematics (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Computing Systems (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)

Abstract

一种基于双状态多因子抗差估计的SINS/DVL紧组合系统。通过下列步骤实现：步骤1：获取载体当前时刻SINS和DVL的采集数据；步骤2：根据步骤1的采集数据建立状态方程和量测方程；步骤3：对状态量和量测量进行卡方检测；步骤4：计算模型误差和量测误差的自适应因子；步骤5：根据步骤4的多因子调节卡尔曼滤波的增益矩阵；步骤6：根据步骤2的系统方程和步骤5的增益矩阵，利用卡尔曼进行最优估计，将递推结果反馈到SINS的解算结果，得到最终速度和位置信息。该方法利用DVL四波束的多普勒频移测量信息，在四波束测量值缺失非完整情况下依旧保证连续的测量信息。该方法能够有效抑制导航状态中的不确定噪声，提高导航精度。

Description

一种基于双状态多因子抗差估计的SINS/DVL紧组合系统
技术领域
本发明属于水下航行器SINS/DVL组合导航技术，涉及一种基于双状态多因子抗差估计的SINS/DVL紧组合系统。
背景技术
在水下导航中，捷联惯导系统(SINS)通常被视为导航系统的核心，可以提供所需的所有数据：姿态、速度和位置。由于误差的积累，多普勒速度(DVL)一直是全球定位系统(GPS)无法使用时最常用的组合方式之一。作为抑制捷联惯导系统误差的有效方法，DVL提供的基于多普勒频移的三维速度是独立的，误差不会随时间累积。多普勒测速误差是影响SINS/DVL组合导航系统精度的重要因素。DVL测量通常受到标度因子误差、安装误差、束宽误差和流速等因素的影响。
目前，学者们对提高SINS/DVL组合导航系统的精度做了大量的研究。为了校正SINS/DVL之间的较大偏差，提出了一种基于SINS/GPS/DVL系统的四元数标定算法来完成SINS的对准和系统标定；建立了基于捷联惯导系统与捷联惯导系统(SINS)与捷联惯导系统(DVL)之间的标度因子和失准角的误差模型，并将可观测性分析方法引入到失调角和标度因子的估计中。为了提高组合导航系统的精度，卡尔曼滤波是常用的滤波器。事实上，标准卡尔曼滤波器只适用于线性系统。对于非线性系统，一些学者提出了一种模糊自适应UKF滤波器，它利用模糊推理系统(FIS)自适应修正测量噪声，并用UKF估计非线性系统误差。
在SINS/DVL紧组合方面，目前比较成熟的方法是利用DVL的四通道的速度测量值作为信息量测，还未有利用原始四通道多普勒频移的原始信息做系统模型的研究。由于水下环境的复杂性，多普勒频移经常受到环境影响引入的较大测量噪声和系统的状态模型不服会引起滤波精度的下降，如何在复杂环境下保证DVL部分波束测量情况下的稳定导航和系统的抗差能力是SINS/DVL的研究方向之一。
发明内容
针对以上问题，在针对SINS/DVL组合系统中DVL速度测量值易受到复杂环境的干扰引起较大噪声和测量值缺失的问题，提出一种基于双状态多因子抗差估计的SINS/DVL紧组合系统。该方法利用DVL四波束的多普，本发明提供一种基于双状态多因子抗差估计的SINS/DVL紧组合系统，该方法利用DVL四波束的多普勒频移测量信息，在四波叔测量值缺失非完整情况下依旧保证连续的测量信息。该方法能够有效抑制导航状态中的不确定噪声，提高导航精度。
本专利提供一种基于双状态多因子抗差估计的SINS/DVL紧组合系统，包括：
步骤1：获取载体当前时刻SINS和DVL的采集数据；
步骤2：根据步骤1的采集数据建立状态方程和量测方程；
步骤3：对状态量和量测量进行卡方检测；
步骤4：计算模型误差和量测误差的自适应因子；
步骤5：根据步骤4的多因子调节卡尔曼滤波的增益矩阵；
步骤6：根据步骤2的系统方程和步骤5的增益矩阵，利用卡尔曼进行最优估计，将递推结果反馈到SINS的解算结果，得到最终速度和位置信息。
作为本发明进一步改进，步骤1的SINS和DVL采集数据包括：IMU采集的陀螺数据：
Figure BDA0002787294670000021
其中，
Figure BDA0002787294670000022
表示载体坐标系下X轴的角速度，
Figure BDA0002787294670000023
表示载体坐标系下Y轴角速度，
Figure BDA0002787294670000024
表示载体坐标系下Z轴角速度；IMU采集的加速度数据：
Figure BDA0002787294670000025
其中，
Figure BDA0002787294670000026
表示载体坐标系下X轴的加速度，
Figure BDA0002787294670000027
表示载体坐标系下Y轴的及速度，
Figure BDA0002787294670000028
表示载体坐标系下Z轴的加速度；DVL采集的四通道的多普勒频移
Figure BDA0002787294670000029
其中，
Figure BDA00027872946700000210
表示DVL通道1的多普勒频移，
Figure BDA00027872946700000211
表示DVL通道2的多普勒频移，
Figure BDA00027872946700000212
表示DVL通道3的多普勒频移，
Figure BDA00027872946700000213
表示DVL通道4的多普勒频移。
作为本发明进一步改进，步骤2建立系统状态方程，具体步骤如下：
Figure BDA00027872946700000214
其中，F表示系统状态转移矩阵，X表示状态向量，G表示系统噪声矩阵，W表示系统噪声矢量。其中，状态向量表示为：
Figure BDA00027872946700000215
其中，[φx φy φz]T表示载体俯仰角、横滚角、航向角误差；
Figure BDA00027872946700000216
表示载体东向、北向、天向速度误差；[δλδLδh]T表示载体地理经度、纬度、高度误差；
Figure BDA00027872946700000217
表示SINS的加速度零偏；[εx εy εz]T表示SINS的陀螺零偏；[ρ1 ρ2 ρ3 ρ4]T表示DVL四通道多普勒频移常值偏移；kD表示DVL刻度因子误差；bps表示深度计零偏；系统状态转移矩阵F、系统噪声矩阵G、系统噪声矢量W表示为：
Figure BDA00027872946700000218
Figure BDA00027872946700000219
其中，
Figure BDA0002787294670000031
Figure BDA0002787294670000032
Figure BDA0002787294670000033
Figure BDA0002787294670000034
Figure BDA0002787294670000035
Figure BDA0002787294670000036
Figure BDA0002787294670000037
Figure BDA0002787294670000041
其中，
Figure BDA0002787294670000042
表示b系到n系的姿态转移矩阵，
Figure BDA0002787294670000043
表示加速度计噪声，
Figure BDA0002787294670000044
表示陀螺噪声，ωie表示地球自转角速率，VE表示东向速度，VN表示北向速度，VU表示天向速度，Re表示地球长半轴半径，e表示地球偏心率，ωie表示地球自转角速率，ge表示赤道重力，β表示地球重力扁率，β2＝3.08×10-6S-2，β1＝(1/8)(2βf+f2)，f表示地球扁率。
作为本发明进一步改进，步骤2建立系统量测方程，具体步骤如下：
Z＝HX+V
其中，Z表示系统量测值，H表示量测方程转移矩阵，V表示量测噪声；
定义DVL所在坐标系为d系，四通道所在坐标系为beam系，波束与水下航行器的夹角为a，DVL在d系下速度为
Figure BDA0002787294670000045
则坐标系转移矩阵可表示为：
Figure BDA0002787294670000046
定义DVL发射声波信号时四通道的速度为
Figure BDA0002787294670000047
接收脉冲时四通道的速度为
Figure BDA0002787294670000048
声波信号在水中的速度为c，声波信号的发射频率为f0，则四波束的多普勒频移值为：
Figure BDA0002787294670000049
若忽略发射接收时DVL的速度变化量以及水声速度和DVL速度之间的较小量，则多普勒频移可简化为：
Figure BDA00027872946700000410
导航坐标系下SINS的速度为
Figure BDA00027872946700000411
载体坐标系下SINS的速度为
Figure BDA00027872946700000412
标定后载体坐标系与DVLd系的坐标系转移矩阵为
Figure BDA00027872946700000413
标定后的安装偏差角
Figure BDA00027872946700000414
其中，
Figure BDA00027872946700000415
表示绕载体横轴的安装偏角，
Figure BDA00027872946700000416
表示绕载体纵轴的安装偏角，
Figure BDA00027872946700000417
表示绕载体竖轴的安装偏角，则利用导航坐标系的SINS速度计算四通道多普勒频移值为：
Figure BDA0002787294670000051
DVL的多普勒频移测量误差模型为：
Figure BDA0002787294670000052
定义深度计的测量误差模型为：
Figure BDA0002787294670000053
则多普勒频移误差方程为：
Figure BDA0002787294670000054
Figure BDA0002787294670000055
则系统量测值Z、量测方程转移矩阵H、量测噪声V分别为：
Figure BDA0002787294670000056
Figure BDA0002787294670000057
V＝[WD Wps]T。
作为本发明进一步改进，根据步骤3对量测不符值和状态不符值进行卡方检测，步骤如下：
(1)量测不符值检测：
①k时刻，预测量测残差向量ek为：
ek＝Zk-Zk,k-1＝Zk-HXk,k-1＝Zk-HFXk-1
其中，Zk表示k时刻的量测值，Zk,k-1表示预测的量测值序列，Xk,k-1表示状态预测值，Xk-1表示k-1时刻的状态值。
②状态一步预测均方误差Pk,k-1为：
Figure BDA0002787294670000058
③相应的协方差阵为：Pe,k＝HPk,k-1HT+Rk
④故障检测函数为：
Figure BDA0002787294670000059
(2)状态不符值检测：
①k时刻，状态预测值Xk,k-1为：Xk,k-1＝FkXk-1
②k时刻，根据量测方程，最小二乘无偏估计结果
Figure BDA00027872946700000510
为：
Figure BDA00027872946700000511
③预测状态残差向量dk为：
Figure BDA00027872946700000512
④故障检测函数为：
Figure BDA0002787294670000061
(3)卡方检测：
定义量测不符值预设门限为Tλ，状态不符值预设门限为Tσ，则故障判定准则为：
Figure BDA0002787294670000062
作为本发明进一步改进，根据步骤4对模型误差和量测误差的自适应因子，步骤如下：
(1)根据最小二乘原则构造损失函数Jk：
Figure BDA0002787294670000063
其中，
Figure BDA0002787294670000064
为k时刻观测自适应因子，βk为k时刻动力学模型信息的自适应因子，Pk为状态估计向量的权矩阵，
Figure BDA0002787294670000065
为状态预测向量的权矩阵，
Figure BDA0002787294670000066
为待估计值；
对Jk求导并令导数为0，则待估计值
Figure BDA0002787294670000067
为：
Figure BDA0002787294670000068
(2)结合步骤3卡方判定结果，自适应因子的选取准则为：
①
Figure BDA0002787294670000069
观测自适应因子观测自适应选取：
定义误差判别统计量
Figure BDA00027872946700000610
Figure BDA00027872946700000611
则
Figure BDA00027872946700000612
其中，c为常量，最优值为1.0，范围取1.0～1.5；
②βk动力学模型信息的自适应因子选取：
定义误差判别统计量
Figure BDA00027872946700000613
Figure BDA00027872946700000614
则
Figure BDA00027872946700000615
其中，c为常量，最优值为1.0，范围取1.0～1.5。
作为本发明进一步改进，根据步骤5根据多因子调节卡尔曼滤波的增益矩阵，步骤如下：利用，步骤4计算的待估计值
Figure BDA00027872946700000616
由矩阵变换可得：
Figure BDA00027872946700000617
其中，
Figure BDA00027872946700000618
作为本发明进一步改进，步骤6中的最终速度信息：
Figure BDA00027872946700000622
其中，
Figure BDA00027872946700000620
表示导航坐标系下载体X轴速度，
Figure BDA00027872946700000621
表示导航坐标系下载体Y轴速度，
Figure BDA00027872946700000623
表示导航坐标系下载体Z轴速度；最终位置信息：λ，L，h，其中，λ表示地理经度值，L表示地理纬度值，h表示高度值。
作为本发明进一步改进，根据步骤6卡尔曼估计，步骤如下：
Xk,k-1＝FXk-1
Pk,k-1＝FPk-1FT+GWGT
Figure BDA0002787294670000071
Xk＝Xk,k-1+Kk(Zk-HXk,k-1)
Pk＝(I-Kk)Pk,k-1
本发明提供一种基于双状态多因子抗差估计的SINS/DVL紧组合系统。通过下列步骤实现：步骤1：获取载体当前时刻SINS和DVL的采集数据；步骤2：根据步骤1的采集数据建立状态方程和量测方程；步骤3：对状态量和量测量进行卡方检测；步骤4：计算模型误差和量测误差的自适应因子；步骤5：根据步骤4的多因子调节卡尔曼滤波的增益矩阵；步骤6：根据步骤2的系统方程和步骤5的增益矩阵，利用卡尔曼进行最优估计，将递推结果反馈到SINS的解算结果，得到最终速度和位置信息。该方法利用DVL四波束的多普勒频移测量信息，在四波叔测量值缺失非完整情况下依旧保证连续的测量信息。该方法能够有效抑制导航状态中的不确定噪声，提高导航精度。
附图说明
图1为本发明的具体流程图；
图2为本发明描述的基于双状态多因子抗差估计的SINS/DVL紧组合系统原理图。
具体实施方式
下面结合附图与具体实施方式对本发明作进一步详细描述：
本发明提供一种基于双状态多因子抗差估计的SINS/DVL紧组合系统，该方法利用DVL四波束的多普勒频移测量信息，在四波束测量值缺失非完整情况下依旧保证连续的测量信息。该方法能够有效抑制导航状态中的不确定噪声，提高导航精度。
作为本发明一种具体实施例，本发明提供流程图如图1所示方法，原理图如图2所示，一种基于双状态多因子抗差估计的SINS/DVL紧组合系统，其特征为：
采集量：IMU采集的陀螺数据：
Figure BDA0002787294670000072
IMU采集的加速度数据：
Figure BDA0002787294670000073
DVL采集的四通道的多普勒频移
Figure BDA0002787294670000074
输出量：速度信息：
Figure BDA0002787294670000077
最终位置信息：λ，L，h。
步骤1：建立系统状态方程：
Figure BDA0002787294670000076
其中，F表示系统状态转移矩阵，X表示状态向量，G表示系统噪声矩阵，W表示系统噪声矢量。其中，状态向量表示为：
Figure BDA0002787294670000081
其中，[φx φy φz]T表示载体俯仰角、横滚角、航向角误差；
Figure BDA0002787294670000082
表示载体东向、北向、天向速度误差；[δλδLδh]T表示载体地理经度、纬度、高度误差；
Figure BDA0002787294670000083
表示SINS的加速度零偏；[εx εy εz]T表示SINS的陀螺零偏；[ρ1 ρ2 ρ3 ρ4]T表示DVL四通道多普勒频移常值偏移；kD表示DVL刻度因子误差；bps表示深度计零偏；系统状态转移矩阵F、系统噪声矩阵G、系统噪声矢量W表示为：
Figure BDA0002787294670000084
其中，
Figure BDA0002787294670000085
Figure BDA0002787294670000086
Figure BDA0002787294670000087
Figure BDA0002787294670000088
Figure BDA0002787294670000091
Figure BDA0002787294670000092
Figure BDA0002787294670000093
Figure BDA0002787294670000094
其中，
Figure BDA0002787294670000095
表示b系到n系的姿态转移矩阵，
Figure BDA0002787294670000096
表示加速度计噪声，
Figure BDA0002787294670000097
表示陀螺噪声，ωie表示地球自转角速率，VE表示东向速度，VN表示北向速度，VU表示天向速度，Re表示地球长半轴半径，e表示地球偏心率，ωie表示地球自转角速率，ge表示赤道重力，β表示地球重力扁率，β2＝3.08×10-6S-2，β1＝(1/8)(2βf+f2)，f表示地球扁率。
步骤2：建立系统量测方程，具体步骤如下：
Z＝HX+V
其中，Z表示系统量测值，H表示量测方程转移矩阵，V表示量测噪声。
定义DVL所在坐标系为d系，四通道所在坐标系为beam系，波束与水下航行器的夹角为a，DVL在d系下速度为
Figure BDA0002787294670000098
则坐标系转移矩阵可表示为：
Figure BDA0002787294670000099
定义DVL发射声波信号时四通道的速度为
Figure BDA00027872946700000910
接收脉冲时四通道的速度为
Figure BDA00027872946700000911
声波信号在水中的速度为c，声波信号的发射频率为f0，则四波束的多普勒频移值为：
Figure BDA0002787294670000101
若忽略发射接收时DVL的速度变化量以及水声速度和DVL速度之间的较小量，则多普勒频移可简化为：
Figure BDA0002787294670000102
导航坐标系下SINS的速度为
Figure BDA0002787294670000103
载体坐标系下SINS的速度为
Figure BDA0002787294670000104
标定后载体坐标系与DVLd系的坐标系转移矩阵为
Figure BDA0002787294670000105
标定后的安装偏差角
Figure BDA0002787294670000106
其中，
Figure BDA0002787294670000107
表示绕载体横轴的安装偏角，
Figure BDA0002787294670000108
表示绕载体纵轴的安装偏角，
Figure BDA0002787294670000109
表示绕载体竖轴的安装偏角，则利用导航坐标系的SINS速度计算四通道多普勒频移值为：
Figure BDA00027872946700001010
DVL的多普勒频移测量误差模型为：
Figure BDA00027872946700001011
定义深度计的测量误差模型为：
Figure BDA00027872946700001012
则多普勒频移误差方程为：
Figure BDA00027872946700001013
Figure BDA00027872946700001014
则系统量测值Z、量测方程转移矩阵H、量测噪声V分别为：
Figure BDA00027872946700001015
Figure BDA00027872946700001016
V＝[WD Wps]T
步骤3：
量测不符值检测：
①k时刻，预测量测残差向量ek为：
ek＝Zk-Zk,k-1＝Zk-HXk,k-1＝Zk-HFXk-1
其中，Zk表示k时刻的量测值，Zk,k-1表示预测的量测值序列，Xk,k-1表示状态预测值，Xk-1表示k-1时刻的状态值。
②状态一步预测均方误差Pk,k-1为：
Figure BDA0002787294670000111
③相应的协方差阵为：Pe,k＝HPk,k-1HT+Rk
④故障检测函数为：
Figure BDA0002787294670000112
状态不符值检测：
①k时刻，状态预测值Xk,k-1为：Xk,k-1＝FkXk-1
②k时刻，根据量测方程，最小二乘无偏估计结果
Figure BDA0002787294670000113
为：
Figure BDA0002787294670000114
③预测状态残差向量dk为：
Figure BDA0002787294670000115
④故障检测函数为：
Figure BDA0002787294670000116
卡方检测：
定义量测不符值预设门限为Tλ，状态不符值预设门限为Tσ，则故障判定准则为：
Figure BDA0002787294670000117
步骤4：构造模型误差和量测误差的自适应因子：
根据最小二乘原则构造损失函数Jk：
Figure BDA0002787294670000118
其中，
Figure BDA0002787294670000119
为k时刻观测自适应因子，βk为k时刻动力学模型信息的自适应因子，Pk为状态估计向量的权矩阵，
Figure BDA00027872946700001110
为状态预测向量的权矩阵，
Figure BDA00027872946700001111
为待估计值。
对Jk求导并令导数为0，则待估计值
Figure BDA00027872946700001112
为：
Figure BDA00027872946700001113
结合步骤3卡方判定结果，自适应因子的选取准则为：
①
Figure BDA00027872946700001114
观测自适应因子观测自适应选取：
定义误差判别统计量
Figure BDA00027872946700001115
Figure BDA00027872946700001116
则
Figure BDA0002787294670000121
②βk动力学模型信息的自适应因子选取：
定义误差判别统计量
Figure BDA0002787294670000122
Figure BDA0002787294670000123
则
Figure BDA0002787294670000124
步骤5：根据多因子调节卡尔曼滤波的增益矩阵：
利用，步骤4计算的待估计值
Figure BDA0002787294670000125
由矩阵变换可得：
Figure BDA0002787294670000126
其中，
Figure BDA0002787294670000127
步骤6：基于卡尔曼滤波的SINS/DVL数据融合：
Xk,k-1＝FXk-1
Pk,k-1＝FPk-1FT+GWGT
Figure BDA0002787294670000128
Xk＝Xk,k-1+Kk(Zk-HXk,k-1)
Pk＝(I-Kk)Pk,k-1
最终，将递推结果反馈到SINS的解算结果，得到最终速度和位置信息。
以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。

Claims (9)

1.一种基于双状态多因子抗差估计的SINS/DVL紧组合系统，，包括：
步骤1：获取载体当前时刻SINS和DVL的采集数据；
步骤2：根据步骤1的采集数据建立状态方程和量测方程；
步骤3：对状态量和量测量进行卡方检测；
步骤4：计算模型误差和量测误差的自适应因子；
步骤5：根据步骤4的多因子调节卡尔曼滤波的增益矩阵；
步骤6：根据步骤2的系统方程和步骤5的增益矩阵，利用卡尔曼进行最优估计，将递推结果反馈到SINS的解算结果，得到最终速度和位置信息。
2.根据权利要求1所述的一种基于双状态多因子抗差估计的SINS/DVL紧组合系统，其特征在于：步骤1的SINS和DVL采集数据包括：IMU采集的陀螺数据：
Figure FDA0002787294660000011
其中，
Figure FDA0002787294660000012
表示载体坐标系下X轴的角速度，
Figure FDA0002787294660000013
表示载体坐标系下Y轴角速度，
Figure FDA0002787294660000014
表示载体坐标系下Z轴角速度；IMU采集的加速度数据：
Figure FDA0002787294660000015
其中，
Figure FDA0002787294660000016
表示载体坐标系下X轴的加速度，
Figure FDA0002787294660000017
表示载体坐标系下Y轴的及速度，
Figure FDA0002787294660000018
表示载体坐标系下Z轴的加速度；DVL采集的四通道的多普勒频移
Figure FDA0002787294660000019
其中，
Figure FDA00027872946600000110
表示DVL通道1的多普勒频移，
Figure FDA00027872946600000111
表示DVL通道2的多普勒频移，
Figure FDA00027872946600000112
表示DVL通道3的多普勒频移，
Figure FDA00027872946600000113
表示DVL通道4的多普勒频移。
3.根据权利要求1所述的一种基于双状态多因子抗差估计的SINS/DVL紧组合系统，其特征在于：步骤2建立系统状态方程，具体步骤如下：
Figure FDA00027872946600000114
其中，F表示系统状态转移矩阵，X表示状态向量，G表示系统噪声矩阵，W表示系统噪声矢量，其中，状态向量表示为：
Figure FDA00027872946600000115
其中，[φx φy φz]T表示载体俯仰角、横滚角、航向角误差；
Figure FDA00027872946600000116
表示载体东向、北向、天向速度误差；[δλ δL δh]T表示载体地理经度、纬度、高度误差；
Figure FDA00027872946600000117
表示SINS的加速度零偏；[εx εy εz]T表示SINS的陀螺零偏；[ρ1 ρ2 ρ3 ρ4]T表示DVL四通道多普勒频移常值偏移；kD表示DVL刻度因子误差；bps表示深度计零偏；系统状态转移矩阵F、系统噪声矩阵G、系统噪声矢量W表示为：
Figure FDA0002787294660000021
其中，
Figure FDA0002787294660000022
Figure FDA0002787294660000023
Figure FDA0002787294660000024
Figure FDA0002787294660000025
Figure FDA0002787294660000026
Figure FDA0002787294660000027
Figure FDA0002787294660000031
Figure FDA0002787294660000032
其中，
Figure FDA0002787294660000033
表示b系到n系的姿态转移矩阵，
Figure FDA0002787294660000034
表示加速度计噪声，
Figure FDA0002787294660000035
表示陀螺噪声，ωie表示地球自转角速率，VE表示东向速度，VN表示北向速度，VU表示天向速度，Re表示地球长半轴半径，e表示地球偏心率，ωie表示地球自转角速率，ge表示赤道重力，β表示地球重力扁率，β2＝3.08×10-6S-2，β1＝(1/8)(2βf+f2)，f表示地球扁率。
4.根据权利要求1所述的一种基于双状态多因子抗差估计的SINS/DVL紧组合系统，其特征在于：步骤2建立系统量测方程，具体步骤如下：
Z＝HX+V
其中，Z表示系统量测值，H表示量测方程转移矩阵，V表示量测噪声；
定义DVL所在坐标系为d系，四通道所在坐标系为beam系，波束与水下航行器的夹角为a，DVL在d系下速度为
Figure FDA0002787294660000036
则坐标系转移矩阵可表示为：
Figure FDA0002787294660000037
定义DVL发射声波信号时四通道的速度为
Figure FDA0002787294660000038
接收脉冲时四通道的速度为
Figure FDA0002787294660000039
声波信号在水中的速度为c，声波信号的发射频率为f0，则四波束的多普勒频移值为：
Figure FDA00027872946600000310
若忽略发射接收时DVL的速度变化量以及水声速度和DVL速度之间的较小量，则多普勒频移可简化为：
Figure FDA0002787294660000041
导航坐标系下SINS的速度为
Figure FDA0002787294660000042
载体坐标系下SINS的速度为
Figure FDA0002787294660000043
标定后载体坐标系与DVLd系的坐标系转移矩阵为
Figure FDA0002787294660000044
标定后的安装偏差角
Figure FDA0002787294660000045
其中，
Figure FDA0002787294660000046
表示绕载体横轴的安装偏角，
Figure FDA0002787294660000047
表示绕载体纵轴的安装偏角，
Figure FDA0002787294660000048
表示绕载体竖轴的安装偏角，则利用导航坐标系的SINS速度计算四通道多普勒频移值为：
Figure FDA0002787294660000049
DVL的多普勒频移测量误差模型为：
Figure FDA00027872946600000410
定义深度计的测量误差模型为：
Figure FDA00027872946600000411
则多普勒频移误差方程为：
Figure FDA00027872946600000412
Figure FDA00027872946600000413
则系统量测值Z、量测方程转移矩阵H、量测噪声V分别为：
Figure FDA00027872946600000414
Figure FDA00027872946600000415
V＝[WD Wps]T。
5.根据权利要求1所述的一种基于双状态多因子抗差估计的SINS/DVL紧组合系统，其特征在于：根据步骤3对量测不符值和状态不符值进行卡方检测，步骤如下：
(1)量测不符值检测：
①k时刻，预测量测残差向量ek为：
ek＝Zk-Zk,k-1＝Zk-HXk,k-1＝Zk-HFXk-1
其中，Zk表示k时刻的量测值，Zk,k-1表示预测的量测值序列，Xk,k-1表示状态预测值，Xk-1表示k-1时刻的状态值：
②状态一步预测均方误差Pk,k-1为：
Figure FDA0002787294660000051
③相应的协方差阵为：Pe,k＝HPk,k-1HT+Rk：
④故障检测函数为：
Figure FDA0002787294660000052
(2)状态不符值检测：
①k时刻，状态预测值Xk,k-1为：Xk,k-1＝FkXk-1：
②k时刻，根据量测方程，最小二乘无偏估计结果
Figure FDA0002787294660000053
为：
Figure FDA0002787294660000054
③预测状态残差向量dk为：
Figure FDA0002787294660000055
④故障检测函数为：
Figure FDA0002787294660000056
(3)卡方检测：
定义量测不符值预设门限为Tλ，状态不符值预设门限为Tσ，则故障判定准则为：
Figure FDA0002787294660000057
6.根据权利要求1所述的一种基于双状态多因子抗差估计的SINS/DVL紧组合系统，其特征在于：根据步骤4对模型误差和量测误差的自适应因子，步骤如下：
(1)根据最小二乘原则构造损失函数Jk：
Figure FDA0002787294660000058
其中，
Figure FDA0002787294660000059
为k时刻观测自适应因子，βk为k时刻动力学模型信息的自适应因子，Pk为状态估计向量的权矩阵，
Figure FDA00027872946600000510
为状态预测向量的权矩阵，
Figure FDA00027872946600000511
为待估计值：
对Jk求导并令导数为0，则待估计值
Figure FDA00027872946600000512
为：
Figure FDA00027872946600000513
(2)结合步骤3卡方判定结果，自适应因子的选取准则为：
①
Figure FDA00027872946600000514
观测自适应因子观测自适应选取：
定义误差判别统计量
Figure FDA00027872946600000515
Figure FDA0002787294660000061
则
Figure FDA0002787294660000062
其中，c为常量，最优值为1.0，范围取1.0～1.5.
②βk动力学模型信息的自适应因子选取：
定义误差判别统计量
Figure FDA0002787294660000063
Figure FDA0002787294660000064
则
Figure FDA0002787294660000065
其中，c为常量，最优值为1.0，范围取1.0～1.5。
7.根据权利要求1所述的一种基于双状态多因子抗差估计的SINS/DVL紧组合系统，其特征在于：根据步骤5根据多因子调节卡尔曼滤波的增益矩阵，步骤如下：利用，步骤4计算的待估计值
Figure FDA0002787294660000066
由矩阵变换可得：
Figure FDA0002787294660000067
其中，
Figure FDA0002787294660000068
8.根据权利要求1所述的一种基于双状态多因子抗差估计的SINS/DVL紧组合系统，其特征在于：步骤6中的最终速度信息：
Figure FDA0002787294660000069
其中，
Figure FDA00027872946600000610
表示导航坐标系下载体X轴速度，
Figure FDA00027872946600000611
表示导航坐标系下载体Y轴速度，
Figure FDA00027872946600000612
表示导航坐标系下载体Z轴速度；最终位置信息：λ，L，h，其中，λ表示地理经度值，L表示地理纬度值，h表示高度值。
9.据权利要求1所述的一种基于双状态多因子抗差估计的SINS/DVL紧组合系统，其特征在于：根据步骤6卡尔曼估计，步骤如下：
Xk,k-1＝FXk-1
Pk,k-1＝FPk-1FT+GWGT
Figure FDA00027872946600000613
Xk＝Xk,k-1+Kk(Zk-HXk,k-1)
Pk＝(I-Kk)Pk,k-1。
CN202011302705.0A 2020-11-19 2020-11-19 一种基于双状态多因子抗差估计sins/dvl紧组合系统的方法 Active CN112507281B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011302705.0A CN112507281B (zh) 2020-11-19 2020-11-19 一种基于双状态多因子抗差估计sins/dvl紧组合系统的方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011302705.0A CN112507281B (zh) 2020-11-19 2020-11-19 一种基于双状态多因子抗差估计sins/dvl紧组合系统的方法

Publications (2)

Publication Number Publication Date
CN112507281A true CN112507281A (zh) 2021-03-16
CN112507281B CN112507281B (zh) 2024-01-02

Family

ID=74958774

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011302705.0A Active CN112507281B (zh) 2020-11-19 2020-11-19 一种基于双状态多因子抗差估计sins/dvl紧组合系统的方法

Country Status (1)

Country Link
CN (1) CN112507281B (zh)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113847915A (zh) * 2021-09-24 2021-12-28 中国人民解放军战略支援部队信息工程大学 一种捷联惯导/多普勒组合导航系统的导航方法
CN114111840A (zh) * 2021-11-12 2022-03-01 哈尔滨工业大学 一种基于组合导航的dvl误差参数在线标定方法
CN114279443A (zh) * 2021-10-29 2022-04-05 东南大学 一种基于最大熵自适应抗差估计的usbl斜距修正方法
CN115031729A (zh) * 2022-05-07 2022-09-09 江苏集萃清联智控科技有限公司 Sins/dvl/usbl水下紧组合导航方法及装置、水下载体控制设备
CN115031724A (zh) * 2022-03-21 2022-09-09 哈尔滨工程大学 Sins/dvl紧组合系统dvl波束故障处理方法
WO2023045357A1 (zh) * 2021-09-23 2023-03-30 哈尔滨工程大学 一种基于统计相似度量的组合导航鲁棒滤波方法
CN116886569A (zh) * 2023-08-04 2023-10-13 湖南科技大学 一种基于自主机器人的通信延迟补偿方法及系统

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109324330A (zh) * 2018-09-18 2019-02-12 东南大学 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
CN109443379A (zh) * 2018-09-28 2019-03-08 东南大学 一种深海潜航器的sins/dvl水下抗晃动对准方法
CN110567454A (zh) * 2019-09-08 2019-12-13 东南大学 一种复杂环境下sins/dvl紧组合导航方法
CN111024064A (zh) * 2019-11-25 2020-04-17 东南大学 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109324330A (zh) * 2018-09-18 2019-02-12 东南大学 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
CN109443379A (zh) * 2018-09-28 2019-03-08 东南大学 一种深海潜航器的sins/dvl水下抗晃动对准方法
CN110567454A (zh) * 2019-09-08 2019-12-13 东南大学 一种复杂环境下sins/dvl紧组合导航方法
CN111024064A (zh) * 2019-11-25 2020-04-17 东南大学 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2023045357A1 (zh) * 2021-09-23 2023-03-30 哈尔滨工程大学 一种基于统计相似度量的组合导航鲁棒滤波方法
CN113847915A (zh) * 2021-09-24 2021-12-28 中国人民解放军战略支援部队信息工程大学 一种捷联惯导/多普勒组合导航系统的导航方法
CN113847915B (zh) * 2021-09-24 2023-12-19 中国人民解放军战略支援部队信息工程大学 一种捷联惯导/多普勒组合导航系统的导航方法
CN114279443A (zh) * 2021-10-29 2022-04-05 东南大学 一种基于最大熵自适应抗差估计的usbl斜距修正方法
CN114279443B (zh) * 2021-10-29 2024-01-02 东南大学 一种基于最大熵自适应抗差估计的usbl斜距修正方法
CN114111840A (zh) * 2021-11-12 2022-03-01 哈尔滨工业大学 一种基于组合导航的dvl误差参数在线标定方法
CN114111840B (zh) * 2021-11-12 2022-08-26 哈尔滨工业大学 一种基于组合导航的dvl误差参数在线标定方法
CN115031724A (zh) * 2022-03-21 2022-09-09 哈尔滨工程大学 Sins/dvl紧组合系统dvl波束故障处理方法
CN115031729A (zh) * 2022-05-07 2022-09-09 江苏集萃清联智控科技有限公司 Sins/dvl/usbl水下紧组合导航方法及装置、水下载体控制设备
CN116886569A (zh) * 2023-08-04 2023-10-13 湖南科技大学 一种基于自主机器人的通信延迟补偿方法及系统
CN116886569B (zh) * 2023-08-04 2024-05-03 湖南科技大学 一种基于自主机器人的通信延迟补偿方法及系统

Also Published As

Publication number Publication date
CN112507281B (zh) 2024-01-02

Similar Documents

Publication Publication Date Title
CN112507281B (zh) 一种基于双状态多因子抗差估计sins/dvl紧组合系统的方法
CN110221332B (zh) 一种车载gnss/ins组合导航的动态杆臂误差估计和补偿方法
CN110823217B (zh) 一种基于自适应联邦强跟踪滤波的组合导航容错方法
CN109324330B (zh) 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
US6496778B1 (en) Real-time integrated vehicle positioning method and system with differential GPS
CN110567454B (zh) 一种复杂环境下sins/dvl紧组合导航方法
CN111156994B (zh) 一种基于mems惯性组件的ins/dr&gnss松组合导航方法
AU687215B2 (en) Assured-integrity monitored-extrapolation navigation apparatus
CN111024064B (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN110779521A (zh) 一种多源融合的高精度定位方法与装置
CN109724599A (zh) 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法
CN112505737B (zh) 一种gnss/ins组合导航方法
CN113203418B (zh) 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统
CN102252677A (zh) 一种基于时间序列分析的变比例自适应联邦滤波方法
CA2733032C (en) Method and apparatus for improved navigation of a moving platform
US20120022820A1 (en) Method for inertial navigation under water
CN110954132A (zh) Grnn辅助自适应卡尔曼滤波进行导航故障识别的方法
Hide et al. GPS and low cost INS integration for positioning in the urban environment
CN113984054A (zh) 基于信息异常检测的改进Sage-Husa自适应融合滤波方法及多源信息融合设备
CN114415224B (zh) 一种复杂受限环境中车辆融合定位系统及方法
CN115657097A (zh) 一种基于轨道约束的leo几何法定轨模糊度快速重收敛方法
CN114526731A (zh) 一种基于助力车的惯性组合导航方向定位方法
CN113252041B (zh) 一种适用于小型水下机器人的组合导航方法
CN112697154B (zh) 一种基于矢量分配的自适应多源融合导航方法
CN114659496B (zh) 一种用于船载北斗一体机倾斜监测的方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant