一种基于双状态多因子抗差估计的SINS/DVL紧组合系统

摘要

一种基于双状态多因子抗差估计的SINS/DVL紧组合系统。通过下列步骤实现：步骤1：获取载体当前时刻SINS和DVL的采集数据；步骤2：根据步骤1的采集数据建立状态方程和量测方程；步骤3：对状态量和量测量进行卡方检测；步骤4：计算模型误差和量测误差的自适应因子；步骤5：根据步骤4的多因子调节卡尔曼滤波的增益矩阵；步骤6：根据步骤2的系统方程和步骤5的增益矩阵，利用卡尔曼进行最优估计，将递推结果反馈到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采集的陀螺数据：

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

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

其中，[φ

其中，

其中，

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

Z＝HX+V

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

定义DVL所在坐标系为d系，四通道所在坐标系为beam系，波束与水下航行器的夹角为a，DVL在d系下速度为

定义DVL发射声波信号时四通道的速度为

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

导航坐标系下SINS的速度为

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

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

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

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

V＝[W

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

(1)量测不符值检测：

①k时刻，预测量测残差向量e

e

其中，Z

②状态一步预测均方误差P

③相应的协方差阵为：P

④故障检测函数为：

(2)状态不符值检测：

①k时刻，状态预测值X

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

③预测状态残差向量d

④故障检测函数为：

(3)卡方检测：

定义量测不符值预设门限为T

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

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

其中，

对J

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

①

定义误差判别统计量

则

其中，

②β

定义误差判别统计量

其中，

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

作为本发明进一步改进，步骤6中的最终速度信息：

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

X

P

X

P

本发明提供一种基于双状态多因子抗差估计的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采集的陀螺数据：

输出量：速度信息：

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

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

其中，[φ

其中，

其中，

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

Z＝HX+V

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

定义DVL所在坐标系为d系，四通道所在坐标系为beam系，波束与水下航行器的夹角为a，DVL在d系下速度为

定义DVL发射声波信号时四通道的速度为

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

导航坐标系下SINS的速度为

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

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

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

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

V＝[W

步骤3：

量测不符值检测：

①k时刻，预测量测残差向量e

e

其中，Z

②状态一步预测均方误差P

③相应的协方差阵为：P

④故障检测函数为：

①k时刻，状态预测值X

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

③预测状态残差向量d

④故障检测函数为：

定义量测不符值预设门限为T

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

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

其中，

对J

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

①

定义误差判别统计量

则

②β

定义误差判别统计量

利用，步骤4计算的待估计值

X

P

X

P

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

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