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

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采集的陀螺数据：其中，/>表示载体坐标系下X轴的角速度，/>表示载体坐标系下Y轴角速度，/>表示载体坐标系下Z轴角速度；IMU采集的加速度数据：/>其中，/>表示载体坐标系下X轴的加速度，/>表示载体坐标系下Y轴的及速度，/>表示载体坐标系下Z轴的加速度；DVL采集的四通道的多普勒频移/>其中，/>表示DVL通道1的多普勒频移，/>表示DVL通道2的多普勒频移，/>表示DVL通道3的多普勒频移，/>表示DVL通道4的多普勒频移。

作为本发明进一步改进，步骤2建立系统状态方程，具体步骤如下：

其中，F表示系统状态转移矩阵，X表示状态向量，G表示系统噪声矩阵，W表示系统噪声矢量。其中，状态向量表示为：

其中，[φ_x φ_y φ_z]^T表示载体俯仰角、横滚角、航向角误差；表示载体东向、北向、天向速度误差；[δλ δL δh]^T表示载体地理经度、纬度、高度误差；表示SINS的加速度零偏；[ε_x ε_y ε_z]^T表示SINS的陀螺零偏；[ρ₁ ρ₂ ρ₃ ρ₄]^T表示DVL四通道多普勒频移常值偏移；k_D表示DVL刻度因子误差；b_ps表示深度计零偏；系统状态转移矩阵F、系统噪声矩阵G、系统噪声矢量W表示为：

其中，

其中，表示b系到n系的姿态转移矩阵，/>表示加速度计噪声，/>表示陀螺噪声，ω_ie表示地球自转角速率，V_E表示东向速度，V_N表示北向速度，V_U表示天向速度，R_e表示地球长半轴半径，e表示地球偏心率，ω_ie表示地球自转角速率，g_e表示赤道重力，β表示地球重力扁率，β₂＝3.08×10^-6S^-2，β₁＝(1/8)(2βf+f²)，f表示地球扁率。

作为本发明进一步改进，步骤2建立系统量测方程，具体步骤如下：

Z＝HX+V

其中，Z表示系统量测值，H表示量测方程转移矩阵，V表示量测噪声；

定义DVL所在坐标系为d系，四通道所在坐标系为beam系，波束与水下航行器的夹角为a，DVL在d系下速度为则坐标系转移矩阵可表示为：

定义DVL发射声波信号时四通道的速度为接收脉冲时四通道的速度为/>声波信号在水中的速度为c，声波信号的发射频率为f₀，则四波束的多普勒频移值为：

若忽略发射接收时DVL的速度变化量以及水声速度和DVL速度之间的较小量，则多普勒频移可简化为：

导航坐标系下SINS的速度为载体坐标系下SINS的速度为标定后载体坐标系与DVLd系的坐标系转移矩阵为/>标定后的安装偏差角/>其中，/>表示绕载体横轴的安装偏角，/>表示绕载体纵轴的安装偏角，/>表示绕载体竖轴的安装偏角，则利用导航坐标系的SINS速度计算四通道多普勒频移值为：

DVL的多普勒频移测量误差模型为：

定义深度计的测量误差模型为：

则多普勒频移误差方程为：

则系统量测值Z、量测方程转移矩阵H、量测噪声V分别为：

V＝[W_D W_ps]^T。

作为本发明进一步改进，根据步骤3对量测不符值和状态不符值进行卡方检测，步骤如下：

(1)量测不符值检测：

①k时刻，预测量测残差向量e_k为：

e_k＝Z_k-Z_k,k-1＝Z_k-HX_k,k-1＝Z_k-HFX_k-1

其中，Z_k表示k时刻的量测值，Z_k,k-1表示预测的量测值序列，X_k,k-1表示状态预测值，X_k-1表示k-1时刻的状态值。

②状态一步预测均方误差P_k,k-1为：

③相应的协方差阵为：P_e,k＝HP_k,k-1H^T+R_k

④故障检测函数为：

(2)状态不符值检测：

①k时刻，状态预测值X_k,k-1为：X_k,k-1＝F_kX_k-1

②k时刻，根据量测方程，最小二乘无偏估计结果为：

③预测状态残差向量d_k为：

④故障检测函数为：

(3)卡方检测：

定义量测不符值预设门限为T_λ，状态不符值预设门限为T_σ，则故障判定准则为：

作为本发明进一步改进，根据步骤4对模型误差和量测误差的自适应因子，步骤如下：

(1)根据最小二乘原则构造损失函数J_k：

其中，为k时刻观测自适应因子，β_k为k时刻动力学模型信息的自适应因子，P_k为状态估计向量的权矩阵，/>为状态预测向量的权矩阵，/>为待估计值；

对J_k求导并令导数为0，则待估计值为：

(2)结合步骤3卡方判定结果，自适应因子的选取准则为：

①观测自适应因子观测自适应选取：

定义误差判别统计量

则

其中，_c为常量，最优值为1.0，范围取1.0～1.5；

②β_k动力学模型信息的自适应因子选取：

定义误差判别统计量

则/>

其中，_c为常量，最优值为1.0，范围取1.0～1.5。

作为本发明进一步改进，根据步骤5根据多因子调节卡尔曼滤波的增益矩阵，步骤如下：利用，步骤4计算的待估计值由矩阵变换可得：

其中，/>

作为本发明进一步改进，步骤6中的最终速度信息：其中，/>表示导航坐标系下载体X轴速度，/>表示导航坐标系下载体Y轴速度，/>表示导航坐标系下载体Z轴速度；最终位置信息：λ，L，h，其中，λ表示地理经度值，L表示地理纬度值，h表示高度值。

作为本发明进一步改进，根据步骤6卡尔曼估计，步骤如下：

X_k,k-1＝FX_k-1

P_k,k-1＝FP_k-1F^T+GWG^T

X_k＝X_k,k-1+K_k(Z_k-HX_k,k-1)

P_k＝(I-K_k)P_k,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采集的陀螺数据：IMU采集的加速度数据：DVL采集的四通道的多普勒频移/>

输出量：速度信息：最终位置信息：λ，L，h。

步骤1：建立系统状态方程：

其中，F表示系统状态转移矩阵，X表示状态向量，G表示系统噪声矩阵，W表示系统噪声矢量。其中，状态向量表示为：

其中，[φ_x φ_y φ_z]^T表示载体俯仰角、横滚角、航向角误差；表示载体东向、北向、天向速度误差；[δλ δL δh]^T表示载体地理经度、纬度、高度误差；表示SINS的加速度零偏；[ε_x ε_y ε_z]^T表示SINS的陀螺零偏；[ρ₁ ρ₂ ρ₃ ρ₄]^T表示DVL四通道多普勒频移常值偏移；k_D表示DVL刻度因子误差；b_ps表示深度计零偏；系统状态转移矩阵F、系统噪声矩阵G、系统噪声矢量W表示为：

其中，

其中，表示b系到n系的姿态转移矩阵，/>表示加速度计噪声，/>表示陀螺噪声，ω_ie表示地球自转角速率，V_E表示东向速度，V_N表示北向速度，V_U表示天向速度，R_e表示地球长半轴半径，e表示地球偏心率，ω_ie表示地球自转角速率，g_e表示赤道重力，β表示地球重力扁率，β₂＝3.08×10^-6S^-2，β₁＝(1/8)(2βf+f²)，f表示地球扁率。

步骤2：建立系统量测方程，具体步骤如下：

Z＝HX+V

其中，Z表示系统量测值，H表示量测方程转移矩阵，V表示量测噪声。

定义DVL所在坐标系为d系，四通道所在坐标系为beam系，波束与水下航行器的夹角为a，DVL在d系下速度为则坐标系转移矩阵可表示为：

定义DVL发射声波信号时四通道的速度为接收脉冲时四通道的速度为/>声波信号在水中的速度为c，声波信号的发射频率为f₀，则四波束的多普勒频移值为：

若忽略发射接收时DVL的速度变化量以及水声速度和DVL速度之间的较小量，则多普勒频移可简化为：

导航坐标系下SINS的速度为载体坐标系下SINS的速度为标定后载体坐标系与DVLd系的坐标系转移矩阵为/>标定后的安装偏差角/>其中，/>表示绕载体横轴的安装偏角，/>表示绕载体纵轴的安装偏角，/>表示绕载体竖轴的安装偏角，则利用导航坐标系的SINS速度计算四通道多普勒频移值为：

DVL的多普勒频移测量误差模型为：

定义深度计的测量误差模型为：

则多普勒频移误差方程为：

则系统量测值Z、量测方程转移矩阵H、量测噪声V分别为：

V＝[W_D W_ps]^T

步骤3：

量测不符值检测：

①k时刻，预测量测残差向量e_k为：

e_k＝Z_k-Z_k,k-1＝Z_k-HX_k,k-1＝Z_k-HFX_k-1

其中，Z_k表示k时刻的量测值，Z_k,k-1表示预测的量测值序列，X_k,k-1表示状态预测值，X_k-1表示k-1时刻的状态值。

②状态一步预测均方误差P_k,k-1为：

③相应的协方差阵为：P_e,k＝HP_k,k-1H^T+R_k

④故障检测函数为：

状态不符值检测：

①k时刻，状态预测值X_k,k-1为：X_k,k-1＝F_kX_k-1

②k时刻，根据量测方程，最小二乘无偏估计结果为：

③预测状态残差向量d_k为：

④故障检测函数为：卡方检测：

定义量测不符值预设门限为T_λ，状态不符值预设门限为T_σ，则故障判定准则为：

步骤4：构造模型误差和量测误差的自适应因子：

根据最小二乘原则构造损失函数J_k：

其中，为k时刻观测自适应因子，β_k为k时刻动力学模型信息的自适应因子，P_k为状态估计向量的权矩阵，/>为状态预测向量的权矩阵，/>为待估计值。

对J_k求导并令导数为0，则待估计值为：

结合步骤3卡方判定结果，自适应因子的选取准则为：

①观测自适应因子观测自适应选取：

定义误差判别统计量

则

②β_k动力学模型信息的自适应因子选取：

定义误差判别统计量

则/>

步骤5：根据多因子调节卡尔曼滤波的增益矩阵：

利用，步骤4计算的待估计值由矩阵变换可得：

其中，/>

步骤6：基于卡尔曼滤波的SINS/DVL数据融合：

X_k,k-1＝FX_k-1

P_k,k-1＝FP_k-1F^T+GWG^T

X_k＝X_k,k-1+K_k(Z_k-HX_k,k-1)

P_k＝(I-K_k)P_k,k-1

最终，将递推结果反馈到SINS的解算结果，得到最终速度和位置信息。

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。

Claims (6)

1.一种基于双状态多因子抗差估计SINS/DVL紧组合系统的方法，包括，其特征在于：

步骤1：获取载体当前时刻SINS和DVL的采集数据；

步骤2：根据步骤1的采集数据建立状态方程和量测方程；

步骤2建立系统状态方程，具体步骤如下：

其中，F表示系统状态转移矩阵，X表示状态向量，G表示系统噪声矩阵，W表示系统噪声矢量，其中，状态向量表示为：

其中，[φ_x φ_y φ_z]^T表示载体俯仰角、横滚角、航向角误差；表示载体东向、北向、天向速度误差；[δλ δL δh]^T表示载体地理经度、纬度、高度误差；/>表示SINS的加速度零偏；[ε_x ε_y ε_z]^T表示SINS的陀螺零偏；[ρ₁ ρ₂ ρ₃ ρ₄]^T表示DVL四通道多普勒频移常值偏移；k_D表示DVL刻度因子误差；b_ps表示深度计零偏；系统状态转移矩阵F、系统噪声矩阵G、系统噪声矢量W表示为：

其中，

其中，表示b系到n系的姿态转移矩阵，/>表示加速度计噪声，/>表示陀螺噪声，ω_ie表示地球自转角速率，V_E表示东向速度，V_N表示北向速度，V_U表示天向速度，R_e表示地球长半轴半径，e表示地球偏心率，ω_ie表示地球自转角速率，g_e表示赤道重力，β表示地球重力扁率，β₂＝3.08×10^-6S^-2，β₁＝(1/8)(2βf+f²)，f表示地球扁率；

步骤2建立系统量测方程，具体步骤如下：

Z＝HX+V

其中，Z表示系统量测值，H表示量测方程转移矩阵，V表示量测噪声；

定义DVL所在坐标系为d系，四通道所在坐标系为beam系，波束与水下航行器的夹角为a，DVL在d系下速度为则坐标系转移矩阵可表示为：

定义DVL发射声波信号时四通道的速度为接收脉冲时四通道的速度为/>声波信号在水中的速度为c，声波信号的发射频率为f₀，则四波束的多普勒频移值为：

若忽略发射接收时DVL的速度变化量以及水声速度和DVL速度之间的较小量，则多普勒频移可简化为：

导航坐标系下SINS的速度为载体坐标系下SINS的速度为标定后载体坐标系与DVLd系的坐标系转移矩阵为/>标定后的安装偏差角/>其中，/>表示绕载体横轴的安装偏角，/>表示绕载体纵轴的安装偏角，/>表示绕载体竖轴的安装偏角，则利用导航坐标系的SINS速度计算四通道多普勒频移值为：

DVL的多普勒频移测量误差模型为：

定义深度计的测量误差模型为：

则多普勒频移误差方程为：

则系统量测值Z、量测方程转移矩阵H、量测噪声V分别为：

V＝[W_D W_ps]^T；

步骤3：对状态量和量测量进行卡方检测；

步骤4：计算模型误差和量测误差的自适应因子；

根据步骤4对模型误差和量测误差的自适应因子，步骤如下：

(1)根据最小二乘原则构造损失函数J_k：

其中，为k时刻观测自适应因子，β_k为k时刻动力学模型信息的自适应因子，P_k为状态估计向量的权矩阵，/>为状态预测向量的权矩阵，/>为待估计值：

对J_k求导并令导数为0，则待估计值为：

(2)结合步骤3卡方判定结果，自适应因子的选取准则为：

①观测自适应因子观测自适应选取：

定义误差判别统计量

则

其中，_c为常量，最优值为1.0，范围取1.0～1.5.

②β_k动力学模型信息的自适应因子选取：

定义误差判别统计量

则/>

其中，_c为常量，最优值为1.0，范围取1.0～1.5；

步骤5：根据步骤4的多因子调节卡尔曼滤波的增益矩阵；

步骤6：根据步骤2的系统方程和步骤5的增益矩阵，利用卡尔曼进行最优估计，将递推结果反馈到SINS的解算结果，得到最终速度和位置信息。

2.根据权利要求1所述的一种基于双状态多因子抗差估计SINS/DVL紧组合系统的方法，其特征在于：步骤1的SINS和DVL采集数据包括：IMU采集的陀螺数据：其中，/>表示载体坐标系下X轴的角速度，/>表示载体坐标系下Y轴角速度，/>表示载体坐标系下Z轴角速度；IMU采集的加速度数据：/>其中，/>表示载体坐标系下X轴的加速度，/>表示载体坐标系下Y轴的及速度，/>表示载体坐标系下Z轴的加速度；DVL采集的四通道的多普勒频移/>其中，/>表示DVL通道1的多普勒频移，/>表示DVL通道2的多普勒频移，/>表示DVL通道3的多普勒频移，/>表示DVL通道4的多普勒频移。

3.根据权利要求1所述的一种基于双状态多因子抗差估计SINS/DVL紧组合系统的方法，其特征在于：根据步骤3对量测不符值和状态不符值进行卡方检测，步骤如下：

(1)量测不符值检测：

①k时刻，预测量测残差向量e_k为：

e_k＝Z_k-Z_k,k-1＝Z_k-HX_k,k-1＝Z_k-HFX_k-1

其中，Z_k表示k时刻的量测值，Z_k,k-1表示预测的量测值序列，X_k,k-1表示状态预测值，X_k-1表示k-1时刻的状态值：

②状态一步预测均方误差P_k,k-1为：

③相应的协方差阵为：P_e,k＝HP_k,k-1H^T+R_k：

④故障检测函数为：

(2)状态不符值检测：

①k时刻，状态预测值X_k,k-1为：X_k,k-1＝F_kX_k-1：

②k时刻，根据量测方程，最小二乘无偏估计结果为：/>

③预测状态残差向量d_k为：

④故障检测函数为：

(3)卡方检测：

定义量测不符值预设门限为T_λ，状态不符值预设门限为T_σ，则故障判定准则为：

4.根据权利要求1所述的一种基于双状态多因子抗差估计SINS/DVL紧组合系统的方法，其特征在于：根据步骤5根据多因子调节卡尔曼滤波的增益矩阵，步骤如下：利用，步骤4计算的待估计值由矩阵变换可得：

其中，/>

5.根据权利要求1所述的一种基于双状态多因子抗差估计SINS/DVL紧组合系统的方法，其特征在于：步骤6中的最终速度信息：其中，/>表示导航坐标系下载体X轴速度，/>表示导航坐标系下载体Y轴速度，/>表示导航坐标系下载体Z轴速度；最终位置信息：λ，L，h，其中，λ表示地理经度值，L表示地理纬度值，h表示高度值。

6.据权利要求1所述的一种基于双状态多因子抗差估计SINS/DVL紧组合系统的方法，其特征在于：根据步骤6卡尔曼估计，步骤如下：

X_k,k-1＝FX_k-1

P_k,k-1＝FP_k-1F^T+GWG^T

X_k＝X_k,k-1+K_k(Z_k-HX_k,k-1)

P_k＝(I-K_k)P_k,k-1。