CN102221363B - 一种水下潜器用捷联惯性组合导航系统容错组合方法 - Google Patents

Abstract

本发明提供的是一种水下潜器用捷联惯性组合导航系统容错组合方法。本发明由捷联惯性导航系统SINS、地形辅助导航系统TAN、多普勒测速仪DVL和磁航向仪MCP组成，采用分散滤波结构和智能容错方法完成组合导航。SINS作为参考导航系统与各辅助导航系统分别组成各子滤波器，从中提取出相关特征量进入由支持向量机构成的故障诊断模块，判断对应辅助导航系统是否出现故障，若出现故障则屏蔽该传感器的信息。故障诊断后进行系统重构，由主滤波器输出的误差量再反馈校正SINS。本发明能保证水下潜器用捷联惯性组合导航系统可靠性好、容错性高，尤其是在小样本情况下训练的支持向量机具有很强推广能力，为故障诊断提供了一种新的研究方法。

Description

一种水下潜器用捷联惯性组合导航系统容错组合方法

技术领域

本发明涉及一种水下潜器用捷联惯性组合导航系统容错组合方法，特别适用于水下潜器组合导航系统的故障诊断和系统重构。

背景技术

容错控制是伴随着基于解析冗余的故障诊断技术的发展而发展起来的，最早可以追溯到1971年，以Niederlinski提出完整性控制的新概念为标志。导航技术的发展使系统日益复杂化和大型化，也对导航系统的可靠性提出了更高的要求，可靠性的提高除了采用可靠的硬件之外，使系统具有故障检测、分离和重构功能及容错设计也是提高可靠性的重要途径。

水下潜器组合导航系统在恶劣的环境之中运行，增加了出现瞬时故障和永久故障的可能性，导致系统失效或运行中断，这就必须采用故障自诊断和容错技术来消除故障的影响，保证系统的正常运行，以提高系统的可靠性和生存能力，减少事故发生的因素，避免发生严重的经济损失和人员伤亡事故。

近年来人工智能理论迅速发展，使它应用于故障诊断和容错控制成为可能，并且由于它在解决不确定数学模型、非线性系统、分布参数的问题方面有着无法比拟的优点。所以将它用于组合导航控制系统的故障诊断和容错控制具有广泛的前景。支持向量机算法的诞生为这些问题的解决开辟了一条新的途径，但是目前国内外可以查找到的资料中很少将其应用于组合导航系统的故障诊断领域。支持向量机较好的解决了以往许多学习方法中小样本、非线性和高维数等实际难题，并克服了神经网络等学习算法中网络结构难以确定、收敛速度慢、局部极小点、过学习与欠学习以及训练时需要大量数据样本等不足，可以使在小样本情况下建立的分类器具有很强的推广能力，这对故障诊断而言具有很强的现实意义，为组合导航系统故障诊断提供了一种新的研究方法。

发明内容

技术问题：本发明的技术解决问题是：克服现有技术的不足，提供一种适合水下潜器用的捷联惯性组合导航系统的容错组合方法。

技术方案：本发明的一种水下潜器用捷联惯性组合导航系统容错组合方法技术解决方案为：由捷联惯性导航系统SINS、多普勒测速仪DVL和磁航向仪MCP和地形辅助导航系统TAN组成，采用分散滤波结构和智能容错方法完成组合导航，通过下列步骤实现：

1.)捷联惯性导航系统SINS作为参考导航系统和多普勒测速仪DVL组成第一子滤波器，捷联惯性导航系统SINS和磁航向仪MCP组成第二子滤波器，捷联惯性导航系统SINS和地形辅助导航系统TAN组成第三子滤波器；

2.)从所述第一子滤波器第二子滤波器和第三子滤波器中提取出相关特征量分别进入第一故障诊断模块、第二故障诊断模块和第三故障诊断模块；

3.)所述第一故障诊断模块判断多普勒测速仪DVL是否出现故障，若出现故障则屏蔽多普勒测速仪DVL的数据信息；所述第二故障诊断模块判断磁航向仪MCP是否出现故障，若出现故障则屏蔽磁航向仪MCP的数据信息；所述第三故障诊断模块判断地形辅助导航系统TAN是否出现故障，若出现故障则屏蔽地形辅助导航系统TAN的数据信息；

4.)故障诊断后进行系统重构，各子滤波器即：第一子滤波器、第二子滤波器和第三子滤波器的数据进入主滤波器进行信息融合，输出的误差量再反馈校正捷联惯性导航系统SINS。

各子滤波器分别是卡尔曼滤波器或者EKF滤波器，捷联惯性导航系统SINS作为参考导航系统提供姿态角、航向角、速度和位置信息；多普勒测速仪DVL提供水下潜器对地速度信息与捷联惯性导航系统SINS速度信息差值作为第一子滤波器的观测量，磁航向仪MCP提供水下潜器的航向角信息与捷联惯性导航系统SINS的航向角的差值作为第二子滤波器的观测量，地形辅助导航系统TAN提供水下潜器的经纬度信息与捷联惯性导航系统SINS经纬度的差值作为第三子滤波器的观测量。

所述的第一子滤波器、第二子滤波器和第三子滤波器中提取的相关特征量包括残差值γ(k)和状态法检测函数λ(k)，第一故障诊断模块根据以上的特征量判断多普勒测速仪DVL正常与否，第二故障诊断模块根据以上的特征量判断磁航向仪MCP正常与否，第三故障诊断模块根据以上的特征量判断地形辅助导航系统TAN正常与否。

所述的第一故障诊断模块、第二故障诊断模块和第三故障诊断模块由C型支持向量机C-SVM构成，选用软间隔支持向量机，为一个两类分类器，根据特征量将多普勒测速仪DVL和磁航向仪MCP和地形辅助导航系统TAN的状态分为2类，正常状态与故障状态。

所述系统重构为：当第一故障诊断模块诊断出多普勒测速仪DVL发生故障时，捷联惯性导航系统SINS、磁航向仪MCP和地形辅助导航系统TAN进行重构；当第二故障诊断模块诊断出磁航向仪MCP发生故障时，捷联惯性导航系统SINS、多普勒测速仪DVL和地形辅助导航系统TAN进行重构；当第三故障诊断模块诊断出地形辅助导航系统TAN发生故障时，捷联惯性导航系统SINS、多普勒测速仪DVL、磁航向仪MCP进行重构；当第一故障诊断模块和第二故障诊断模块诊断出多普勒测速仪DVL和磁航向仪MCP同时发生故障时，捷联惯性导航系统SINS与地形辅助导航系统TAN组成导航系统进行位置匹配；当第一故障诊断模块和第三故障诊断模块诊断出多普勒测速仪DVL和地形辅助导航系统TAN同时发生故障，，捷联惯性导航系统SINS与磁航向仪MCP组成导航系统进行航向匹配；当第二故障诊断模块和第三故障诊断模块诊断出磁航向仪MCP和地形辅助导航系统TAN同时发生故障时，捷联惯性导航系统SINS、多普勒测速仪DVL进行速度匹配；当发生故障的多普勒测速仪DVL、磁航向仪MCP和地形辅助导航系统TAN恢复正常状态时，随时加入组合导航系统进行信息融合。

所述信息融合采用分散卡尔曼滤波、神经网络、模糊控制技术中的一种。

所述的C-SVM的训练样本收集和模型建立方法如下：

1)原始样本收集

通过软件仿真得到了多普勒测速仪DVL、磁航向仪MCP和地形辅助导航系统TAN全部正常与DVL多普勒测速仪DVL、磁航向仪MCP和地形辅助导航系统TAN分别故障时的数据样本，从中选取一部分作为训练样本，选取另一部分作为测试样本验证模型；

2)特征量提取

设存在如下系统状态方程和量测方程为

X_k＝φ_k，k-1X_k-1+Γ_k-1W_k-1

Z_k＝H_kX_k+V_k

式中，X_k为k时刻的状态向量，也就是被估计矢量；Z_k为k时刻的测量序列；W_k-1为k-1时刻的系统噪声；V_k为k时刻的测量噪声序列；Φ_k，k-1为k-1时刻到k时刻的一步状态转移矩阵；Γ_k-1是系统噪声输入矩阵，H_k为k时刻的测量矩阵，离散卡尔曼滤波公式如下：

状态一步预测向量

X_k/k-1＝φ_k，k-1X_k-1

状态估值计算

X_k＝X_k/k-1+K_k(Z_k-H_kX_k/k-1)

滤波增益

K_k＝P_k/k-1H_k ^T(H_kP_k/k-1H_k ^T+R_k)^-1

一步预测均方误差矩阵

$P_{k/k-1}={\mathrm{\φ}}_{k,k-1}{P}_{k-1}{\mathrm{\φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T$

估计均方误差方程

$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T$

从以上模型中提取出残差值和状态法检测函数λ_k作为特征量，具体如下

残差：γ_k＝Z_k-H_kX_k|k-1

方差：

状态法检测函数：

其中Z_k为k时刻的测量序列，H_k为k时刻的测量矩阵，

为H_k的转置矩阵，X_k/k-1为状态一步预测向量，P_k/k-1为一步预测均方误差矩阵，R_k为k时刻的量测噪声的方差矩阵；

为残差向量γ_k的转置矩阵，

为方差阵A_k的逆矩阵；

3)C-SVM训练

选用RBF函数作为核函数，形式如下

$K(x,x_i)=\exp (-\frac{{|x-x_i|}^2}{{\mathrm{\δ}}^2})$

其中δ为核宽度，K(x，x_i)是样本向量x，x_i的内积核函数；在C-SVM中待确定的参数主要为惩罚参数C和RBF核函数里的核宽度δ，对于正常状态和故障状态设置不同的惩罚参数C，正常状态设置为C⁺，故障状态设置为C^-，且C⁺＞C^-；为了要找出最优的参数集，采用交叉验证法进行寻找，即针对两种参数各设定一定的数集，对各种组合用样本进行训练，然后用同样的测试样本考核SVM性能，最后经比较选取最佳的参数集。为C^-＝1.25，C⁺＝12.5，δ²＝0.2。

本发明的原理：所述组合导航系统由捷联惯性导航系统SINS、地形辅助导航系统TAN、多普勒测速仪DVL和磁航向仪MCP组成，由SINS作为参考导航系统，分别与DVL、MCP和TAN组成子滤波器，从子滤波器中提取残差和状态检测函数作为特征量，采用C型支持向量机构成故障诊断模块根据特征量判断DVL、MCP和TAN的工作状态，若出现故障则屏蔽该系统并进行系统重构，最后采用信息融合方法估计出状态误差并反馈至SINS校正。

有益效果：本发明与现有技术相比的优点在于：

(1)本发明根据水下潜器实际工作环境和要求，选择由SINS作为参考导航系统，DVL、MCP和TAN作为辅助导航系统的组合导航系统构成，该系统可弥补单一系统结构简单、可靠性差等不足，能够在辅助导航系统出现故障的情况下进行故障隔离、系统重构，进而通过信息融合确保为水下潜器提供导航任务，保证了整个组合导航系统的可靠性和容错性。

(2)本发明采用基于支持向量机的故障诊断方法，解决传统学习方法中小样本、非线性和高维数等实际难题，并克服了神经网络等学习算法中网络结构难以确定、收敛速度慢、局部极小点、过学习与欠学习以及训练时需要大量数据样本等不足，可以使在小样本情况下建立的分类器具有很强的推广能力，这对故障诊断而言具有很强的现实意义，为组合导航系统故障诊断提供了一种新的研究方法。

本发明采用支持向量机作为故障诊断模块对组合导航系统中的各辅助导航系统进行故障诊断，提高了系统的可靠性和容错性，下面仿真验证该发明的有益效果。

1.C-SVM故障诊断模型的建立

(1)数学描述

组合导航系统故障诊断技术的关键是实现从故障特征空间到故障空间的映射，从而实现对故障的识别和诊断。利用特征数据对组合导航系统进行故障诊断，实际上是完成了一个导航参数特征到故障的一个复杂的非线性映射。

设反映组合导航系统故障特征向量集合为X_P＝{x_i}(x_i∈Rⁿ，i＝1，2，...，Q)，即它为Q个n维向量的集合，对应故障模式向量的集合称为Y_P＝{y_i}(y_i∈R^m，i＝1，2，...，Q)，它是Q个m维向量的集合。如果它们组成样本对集合(x_i，y_i)，表达了故障特征与故障模式的内在关系。设F为故障特征向量到故障模式的映射，即：F：(Rⁿ)_P→(R^m)_PY＝F(X)。通常这种映射关系F是未知的，但SVM可以通过学习输入到输出的样本集，实现这种映射关系。

(2)样本的收集与模型的建立

(a)原始样本收集

通过软件仿真得到了传感器全部正常和DVL、MCP和TAN分别故障时的数据样本，故障数据生成方法按表1所示。从中选取一部分作为训练样本，选取另一部分作为测试样本验证模型。

表1故障数据生成表

(b)特征量提取

将残差值和状态法检测函数λ(k)作为输入向量，具体公式如下

残差：γ_k＝Z_k-H_kX_k|k-1

方差：

函数：

其中Z_k为k时刻的测量序列，H_k为k时刻的测量矩阵，

为H_k的转置矩阵，X_k/k-1为状态一步预测向量，P_k/k-1为一步预测均方误差矩阵，R_k为k时刻的量测噪声的方差矩阵；

为残差向量γ_k的转置矩阵，

为方差阵A_k的逆矩阵；

(c)C-SVM训练

选用RBF函数作为核函数，形式如下

$K(x,x_i)=\exp (-\frac{{|x-x_i|}^2}{{\mathrm{\δ}}^2})$

其中δ为核宽度，K(x，x_i)是样本向量x，x_i的内积核函数。在C-SVM中待确定的参数主要为惩罚参数C和RBF核函数里的核宽度δ。考虑到将故障状态误判为健康状态的危害性要远远大于将健康状态误判为故障状态的危害性，另外，训练样本集中健康样本的个数要远小于故障样本数，为克服此不平衡问题，对于两类数据设置不同的惩罚参数C，正常状态设置为C⁺，故障状态设置为C^-，且C⁺＞C^-。

为了要找出最优的参数组，采用交叉验证法进行寻找。即针对两种参数各设定一定的数集，对各种组合用样本进行训练，然后用同样的测试样本考核SVM性能，最后经比较选取最佳的参数组对为C^-＝1.25，C⁺＝12.5，δ²＝0.2。

(3)仿真结果与分析

由于三个子滤波器的故障判断原理相同，在此以MCP正常和故障时的数据为例，给出训练和测试结果。

(a)MCP正常和故障时的航向残差值和λ(k)值

图2为MCP正常时的λ值；

图3为MCP正常时的航向残差值；

图4为MCP故障时的λ值；

图5为MCP故障时的航向残差值。

(b)C-SVM训练结果

图6为C-SVM训练结果图。

抽取样本数量为90(正常状态样本数30，故障样本数60)，经训练后得到的最优分类面如图6中粗线所示，b^*＝-0.497734391782358，90个α中有55个大于0，所以共有55个支持向量。构造决策函数

用以上的方法分别对SINS/TAN和SINS/DVL构造决策函数，根据决策函数进行故障诊断：若f(x)＞0，则为健康；若f(x)＜0，则为故障。若为故障则屏蔽对应传感器的子滤波器，用其余的滤波器进行系统重构。

(c)C-SVM故障诊断性能验证

选取不同的训练样本数分别对神经网络和支持向量机进行训练，然后用相同的测试样本对模型的故障诊断正确率进行检验，检验结果如下。

表2模型分类正确率检验表

由表2可知，神经网络和支持向量机的模型精确度随着训练样本数的增加而提高，但当训练样本数较少时，神经网络明显表现出了推广能力不足的缺点，模型正确度较低；而支持向量机相对而言则能够较好的进行故障诊断，但是就自身而言还是比样本数多时弱些，毕竟样本数较多时能够提高模型的训练精度。随着训练样本数的不断增多，两种模型都显示出了较好的估计性能。

支持向量机是专门针对有限样本情况的，其目标是得到现有信息下的最优解而不仅仅是样本数趋于无穷大时的最优值，它用结构风险最小化代替经验风险最小化，较好地解决了小样本的学习问题。从理论上说，支持向量机算法得到的将是全局最优点，解决了神经网络方法始终无法避免的局部极值问题。故障发生本身是属小概率事件，样本一般较少，所以可以说支持向量机在故障诊断领域更能显示出优良特性。

附图说明

图1为容错组合导航系统结构图；

图2为MCP正常时的λ值；

图3为MCP正常时的航向残差值；

图4为MCP故障时的λ值；

图5为MCP故障时的航向残差值；

图6为C-SVM训练结果图。

图中有：捷联惯性导航系统SINS 1、多普勒测速仪DVL 2、磁航向仪MCP 3、地形辅助导航系统TAN 4、第一子滤波器5、第二子滤波器6、第三子滤波器(7)、第一故障诊断模块8、第二故障诊断模块9、第三故障诊断模块10。

具体实施方式

下面结合附图对本发明进一步详细说明。

如图1所示，本发明由捷联惯性导航系统SINS、地形辅助导航系统TAN、多普勒测速仪DVL和磁航向仪MCP组成，为了提高系统的可靠性和容错性，发明中采用了分散滤波和故障诊断技术，具体实现步骤如下：

1)捷联惯性导航系统SINS 1作为参考导航系统和多普勒测速仪DVL 2组成第一子滤波器5，捷联惯性导航系统SINS1和磁航向仪MCP 3组成第二子滤波器6，捷联惯性导航系统SIN1和地形辅助导航系统TAN 4组成第三子滤波器7。

捷联惯性导航系统SINS 1系统误差模型的建立：

根据捷联惯性导航系统长期工作时的误差特点，选择位置误差、速度误差、姿态误差、陀螺漂移和加速度计零偏作为状态量

式中，δV_E、δV_N分别是东向、北向速度误差；φ_E、φ_N、φ_U分别是东向、北向、天向失准角；δL、δλ分别是纬度、经度误差；

分别是x、y向的加速度计偏置；ε_bx、ε_by、ε_bz分别是x、y、z向的陀螺漂移。选取东北天坐标系作为导航坐标系，载体坐标系x轴沿水下潜器横轴指向右舷，y轴沿航行器纵轴指向前，z轴垂直于x与y轴所确定的平面构成右手坐标系。状态方程为

X^& _SINS＝F_SINSX_SINS+W_SINS

系统噪声矩阵

其中：

$F_{12}={2\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R}\tan L$ $F_{16}=({2\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R}{\sec }^{2}L)V_N$

$F_{21}=-2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R}\tan L)$ $F_{26}=-({2\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R}{\sec }^{2}L)V_E$

$F_{34}=-F_{43}={\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R}\tan L$ $F_{35}=-F_{53}=-({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R})$

F₄₆＝-ω_iesinL $F_{56}={\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R}{\sec }^{2}L$

C_ij为姿态转移矩阵的元素

●捷联惯性导航系统SINS 1、多普勒测速仪DVL 2组成第一子滤波器

DVL误差模型的建立：

多普勒测速仪的优点是能够完全自主导航，反应速度快，隐蔽性好，测得的平均速度精度高，并且可以测定浅水和低速时的航速，这对于远程水下潜器是非常重要的。在实际应用中多普勒测速仪需要外部的航向信息和垂直基准信息，所以利用多普勒测速仪与捷联惯导系统进行组合，可以构成精度高，可靠性好的高自主性水下组合导航系统。由于多普勒测速仪的天向测速精度相对较低，选取多普勒测速仪的水平方向测速误差和刻度系数误差作为状态变量

其中多普勒测速误差δV_Dx、δV_Dy可用一阶马尔可夫过程近似描述，刻度系数误差δK_Dx、δK_Dy为随机常数，状态方程为

X^& _DVL＝F_DVLX_DVL+W_DVI

其中

系统噪声矩阵

τ_dx、τ_dy、W_Dx、W_Dy分别是一阶马尔可夫过程的相关时间和驱动白噪声。

捷联惯性导航系统SINS 1、多普勒测速仪DVL 2状态方程和量测方程：

状态方程

量测方程：

多普勒计程仪分别测出两个速度V_x和V_y。而在本发明的应用中，需要测量航行器东向及北向的移动速度，多普勒测速仪直接测出的是航行器在载体坐标系的速度分量V_x、V_y，故必须先将多普勒输出的速度分量变换到导航坐标系，然后与捷联惯导的相应输出信息相减才能得到量测量。

多普勒速度输出为

$\left\{\begin{array}{c}{V}_{\mathrm{DE}}=V_E+{\mathrm{\δV}}_{\mathrm{DE}}\\ =V_E+\cos \mathrm{\ψ}\×{\mathrm{\δV}}_{\mathrm{dx}}-\sin \mathrm{\ψ}\×{\mathrm{\δV}}_{\mathrm{dy}}+V_x\cos \mathrm{\ψ}\×{\mathrm{\δK}}_x-V_y\sin \mathrm{\ψ}\×{\mathrm{\δK}}_y-(V_x\sin \mathrm{\ψ}+V_y\cos \mathrm{\ψ})\×{\mathrm{\φ}}_U+{\mathrm{\η}}_E\\ V_{\mathrm{DN}}=V_N+{\mathrm{\δV}}_{\mathrm{DN}}\\ =V_N+\sin \mathrm{\ψ}\×{\mathrm{\δV}}_{\mathrm{dx}}+\cos \mathrm{\ψ}\×{\mathrm{\δV}}_{\mathrm{dy}}+V_x\sin \mathrm{\ψ}\×{\mathrm{\δK}}_x+V_y\cos \mathrm{\ψ}\×{\mathrm{\δK}}_y+(V_x\cos \mathrm{\ψ}-V_y\sin \mathrm{\ψ})\×{\mathrm{\φ}}_U+{\mathrm{\η}}_N\end{array}\right.$

其中

V_E、V_N分别为东向、北向真实速度；η_E、η_N为多普勒测速白噪声，ψ是方位角的真值。

捷联惯导速度输出为

$\left\{\begin{array}{c}{V}_{\mathrm{IE}}=V_E+{\mathrm{\δV}}_E\\ V_{\mathrm{IN}}=V_N+{\mathrm{\δV}}_N\end{array}\right.$

SINS/DVL形成的速度量测向量为

$Z_{\mathrm{ID}}=\left[\begin{array}{c}{V}_{\mathrm{IE}}-V_{\mathrm{DE}}\\ V_{\mathrm{IN}}-V_{\mathrm{DIN}}\end{array}\right]=\left[\begin{array}{c}(V_E+{\mathrm{\δV}}_E)-(V_E+{\mathrm{\δV}}_{\mathrm{DE}})\\ (V_N+{\mathrm{\δV}}_N)-(V_N+{\mathrm{\δV}}_{\mathrm{DN}})\end{array}\right]=\left[\begin{array}{c}{\mathrm{\δV}}_E-{\mathrm{\δV}}_{\mathrm{DE}}\\ {\mathrm{\δV}}_N-{\mathrm{\δV}}_{\mathrm{DN}}\end{array}\right]$

$=\left[\begin{array}{cccccccccc}1& 0& 0& 0& -(V_x\sin \mathrm{\ψ}+V_y\cos \mathrm{\ψ})& 0_{1\×7}& \cos \mathrm{\ψ}& -\sin \mathrm{\ψ}& V_x\cos \mathrm{\ψ}& -V_y\sin \mathrm{\ψ}\\ 0& 1& 0& 0& V_x\cos \mathrm{\ψ}-V_y\sin \mathrm{\ψ}& 0_{1\×7}& \sin \mathrm{\ψ}& \cos \mathrm{\ψ}& V_x\sin \mathrm{\ψ}& V_y\cos \mathrm{\ψ}\end{array}\right]\left[\begin{array}{c}{X}_{\mathrm{SINS}}\\ X_{\mathrm{DVL}}\end{array}\right]+\left[\begin{array}{c}{\mathrm{\η}}_E\\ {\mathrm{\η}}_N\end{array}\right]$

$=H_{\mathrm{ID}}{X}_{\mathrm{ID}}+V_{\mathrm{ID}}$

●捷联惯性导航系统SINS 1、磁航向仪MCP 3组成第二子滤波器

磁航向仪MCP 3误差模型的建立：

磁航向仪是利用磁敏感元件感测地磁场的方向，即磁子午线方向，并给出航行器纵轴相对磁子午线在水平面内的夹角，即磁航向角。磁航向仪很容易受外界的影响，精度也不易提高，但由于其结构简单，仪表本身的可靠性高、成本低，所以仍被很多使用场合采用。磁航向仪的误差可用一阶马尔可夫过程表示

δψ_MCP为MCP测量误差，τ_MCP为相关时间参数，W_ψ为量测噪声。

捷联惯性导航系统SINS 1、磁航向仪MCP 3状态方程和量测方程

状态方程：

式中，状态X_MCP取为δψ_MCP，W_MCP为MCP系统噪声。

量测方程：

磁航向仪MCP 3航向输出以及捷联惯导航向信息分别为

ψ_MCP＝ψ_t+δψ_MCP，ψ_SINS＝ψ_t+δψ_SINS

ψ_t为真实航向值，δψ_SINS为SINS航向测量误差。

量测量为

$Z_{\mathrm{IG}}=[{\mathrm{\ψ}}_{\mathrm{SINS}}-{\mathrm{\ψ}}_{\mathrm{MCP}}]=[{\mathrm{\δ\ψ}}_{\mathrm{SINS}}-{\mathrm{\δ\ψ}}_{\mathrm{MCP}}]=\left[\begin{array}{cccc}{0}_{1\×4}& 1& 0_{1\×7}& -1\end{array}\right]\·\left[\begin{array}{c}{X}_{\mathrm{SINS}}\\ X_{\mathrm{MCP}}\end{array}\right]+\left[\begin{array}{c}{W}_{\mathrm{SINS}}\\ W_{\mathrm{MCP}}\end{array}\right]$

●捷联惯性导航系统SINS 1、地形辅助导航系统TAN 4组成第三子滤波器

地形辅助导航系统TAN，实质是由捷联惯导系统SINS与测深测潜仪以及数字地图构成的组合导航系统。地形辅助导航系统具有较好的隐蔽性和较强的抗干扰能力，可以全天候工作，具有很高的自主性和军用价值。目前已经研制的地形辅助导航系统有地形轮廓匹配(TERCOM)、桑迪亚惯性地形辅助导航(SITAN)等。地形辅助导航系统的状态方程就是捷联惯导系统的状态方程，即采用间接法估计捷联惯导系统状态的误差。

捷联惯性导航系统SINS 1、地形辅助导航系统TAN 4形成的量测量由地形辅助导航系统输出的位置信息纬度L_TAN、经度λ_TAN减去捷联惯导输出的位置信息纬度L_SINS、经度λ_SINS形成。TAN给出的位置信息为

$\left\{\begin{array}{c}{L}_{\mathrm{TAN}}=L_t+{\mathrm{\δL}}_{\mathrm{TAN}}+m_l\\ {\mathrm{\λ}}_{\mathrm{TAN}}={\mathrm{\λ}}_t+\mathrm{\δ}{\mathrm{\λ}}_{\mathrm{TAN}}+m_{\mathrm{\λ}}\end{array}\right.$

δL_TAN、δλ_TAN为TAN系统纬度误差、经度误差；m_l、m_λ为TAN系统的白色观测噪声。

捷联惯导位置输出为

$\left\{\begin{array}{c}{L}_{\mathrm{SINS}}=L_t+{\mathrm{\δL}}_{\mathrm{SINS}}\\ {\mathrm{\λ}}_{\mathrm{SINS}}={\mathrm{\λ}}_t+{\mathrm{\δ\λ}}_{\mathrm{SINS}}\end{array}\right.$

δL_SINS、δλ_SINS为SINS系统纬度误差、经度误差。

捷联惯性导航系统SINS 1、地形辅助导航系统TAN 4形成的位置量测量为

其中，X_TAN＝[δL_TAN δλ_TAN]。

(2)从所述第一子滤波器、第二子滤波器和第三子滤波器中提取出相关特征量分别进入第一故障诊断模块、第二故障诊断模块和第三故障诊断模块。

子滤波器中提取的相关特征量包括残差值γ(k)和状态法检测函数λ(k)，第一故障诊断模块根据以上的特征量判断多普勒测速仪DVL 2正常与否，第二故障诊断模块根据以上的特征量判断多普勒测速仪DVL 2正常与否，第三故障诊断模块根据以上的特征量判断多普勒测速仪DVL 2正常与否。

为了给出特征量的定义，先给出离散卡尔曼滤波的公式：

设存在如下系统状态方程和量测方程为

X_k＝φ_k，k-1X_k-1+Γ_k-1W_k-1

Z_k＝H_kX_k+V_k

式中，X_k为k时刻的状态向量，也就是被估计矢量；Z_k为k时刻的测量序列；W_k-1为k-1时刻的系统噪声；V_k为k时刻的测量噪声序列；Φ_k，k-1为k-1时刻到k时刻的一步状态转移矩阵；Γ_k-1是系统噪声输入矩阵，H_k为k时刻的测量矩阵，离散卡尔曼滤波公式如下：

状态一步预测向量

X_k/k-1＝φ_k，k-1X_k-1

状态估值计算

X_k＝X_k/k-1+K_k(Z_k-H_kX_k/k-1)

滤波增益

K_k＝P_k/k-1H_k ^T(H_kP_k/k-1H_k ^T+R_k)^-1

一步预测均方误差矩阵

$P_{k/k-1}={\mathrm{\φ}}_{k,k-1}{P}_{k-1}{\mathrm{\φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T$

估计均方误差方程

$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T$

从以上模型中提取出残差值和状态法检测函数λ_k作为特征量，具体如下

残差：γ_k＝Z_k-H_kX_k|k-1

方差：

状态法检测函数：

其中Z_k为k时刻的测量序列，H_k为k时刻的测量矩阵，

为H_k的转置矩阵，X_k/k-1为状态一步预测向量，P_k/k-1为一步预测均方误差矩阵，R_k为k时刻的量测噪声的方差矩阵；

为残差向量γ_k的转置矩阵，

为方差阵A_k的逆矩阵；

(3)第一故障诊断模块判断多普勒测速仪DVL 2是否出现故障，若出现故障则屏蔽多普勒测速仪DVL 2的数据信息；所述第二故障诊断模块判断磁航向仪MCP 3是否出现故障，若出现故障则屏蔽磁航向仪MCP 3的数据信息；所述第三故障诊断模块判断地形辅助导航系统TAN 4是否出现故障，若出现故障则屏蔽地形辅助导航系统TAN 4的数据信息；

故障诊断模块由C型支持向量机C-SVM构成，该C-SVM选用软间隔支持向量机，为一个两类分类器，根据特征量将多普勒测速仪DVL 2、磁航向仪MCP 3和地形辅助导航系统TAN 4状态分为2类，正常状态和故障状态。

下面介绍C-SVM理论及算法：

支持向量机(SVM)是Vapnik等人根据统计学习理论中结构风险最小化原则提出的。SVM能够尽量提高学习机的推广能力，即使由有限数据集得到的判别函数对独立的测试集仍能够得到较小误差。此外，支持向量机是一个凸二次优化问题，能够保证找到的极值解就是全局最优解。这些特点使支持向量机成为一种优秀的基于数据的机器学习算法。

C-SVM是一种非线性软间隔支持向量机。对于非线性分类问题，一方面通过引进松弛变量ξ放松约束，另一方面引进从输入空间Rⁿ到Hilbert空间H的变换φ把数据样本集(x_i，y_i)，i＝1，...，n，x_i∈R^d，y_i∈{+1，-1}映射为φ(x₁，y₁)，φ(x₂，y₂)，...，φ(x_i，y_i)，得到如下的原始问题

$\left\{\begin{array}{c}\underset{\mathrm{\ω}\⋐H,b\∈R,\mathrm{\ξ}\∈R^{\′}}{\min }\frac{1}{2}{\left|\right|\mathrm{\ω}\left|\right|}^2+C\underset{i=1}{\overset{1}{\mathrm{\Σ}}}{\mathrm{\ξ}}_i\\ s.t.y_i(\mathrm{\ω}\·x_i+b)\≥1-{\mathrm{\ξ}}_i,{\mathrm{\ξ}}_i\≥0,i=1,...,l\end{array}\right.---\left(1\right)$

其中分类面方程为ω·x+b＝0(ω∈R^d，b∈R)，C＞0为惩罚参数。若对应于变换φ的核函数为K(x_i，x_j)，则原始问题的对偶问题为

$\left\{\begin{array}{c}\underset{\mathrm{\α}}{\min }\frac{1}{2}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}{y}_i{y}_j{\mathrm{\α}}_i{\mathrm{\α}}_{j}K(x_i,x_j)-\underset{j=1}{\overset{l}{\mathrm{\Σ}}}{\mathrm{\α}}_j\\ s.t.\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{y}_i{\mathrm{\α}}_i=\mathrm{0,0}\≤{\mathrm{\α}}_i\≤C,i=1,...,l\end{array}\right.---\left(2\right)$

若K是正定核，则对偶问题必有解，且其解为使得

$\left\{\begin{array}{c}{\mathrm{\ω}}^{*}=\underset{i=1}{\overset{l}{\mathrm{\Σ}}}{\mathrm{\α}}_i^{*}{y}_i{x}_i\\ b^{*}=y_j-\underset{i=1}{\overset{l}{\mathrm{\Σ}}}{y}_i{\mathrm{\α}}_i^{*}K(x_i,x_j)\end{array}\right.---\left(3\right)$

其中，系数α_i只有当相应的数据样本点(x_i，y_i)刚好使约束式等号成立时才可能非零。(ω^*，b^*)为原始问题(1)关于(ω，b)的解。

由上述分析可建立C-支持向量机，其对应的数学描述如下：

(a)设已知数据样本集(x_i，y_i)，其中i＝1，2，...，n，x_i∈R^d，y_i∈{+1，-1}。

(b)选取适当的核函数K(x_i，x_j)和适当的参数C，按照式(2)构造并求解最优化问题，求得最优解

(c)选取α^*的一个正分量0≤α_i≤C，并计算阈值

(d)构造决策函数

C-SVM的训练样本收集和模型建立方法如下：

(a)原始样本收集

通过软件仿真得到了多普勒测速仪DVL 2、磁航向仪MCP 3、地形辅助导航系统TAN4全部正常与多普勒测速仪DVL 2、磁航向仪MCP 3、地形辅助导航系统TAN 4分别故障时的数据样本，从中选取一部分作为训练样本，选取另一部分作为测试样本验证模型；

(b)特征量提取

从各子滤波器中提取出残差值和状态法检测函数λ_k作为特征量，具体如下

残差：γ_k＝Z_k-H_kX_k|k-1

方差：

状态法检测函数：

其中Z_k为k时刻的测量序列，H_k为k时刻的测量矩阵，

为H_k的转置矩阵，X_k/k-1为状态一步预测向量，P_k/k-1为一步预测均方误差矩阵，R_k为k时刻的量测噪声的方差矩阵；

为残差向量γ_k的转置矩阵，

为方差阵A_k的逆矩阵；

(c)C-SVM训练

选用RBF函数作为核函数，形式如下

$K(x,x_i)=\exp (-\frac{{|x-x_i|}^2}{{\mathrm{\δ}}^2})$

其中

δ为核宽度，K(x，x_i)是样本向量x，x_i的内积核函数；在C-SVM中待确定的参数主要为惩罚参数C和RBF核函数里的核宽度δ。对于正常状态和故障状态设置不同的惩罚参数C，正常状态设置为C⁺，故障状态设置为C^-，且C⁺＞C^-；为了要找出最优的参数集，采用交叉验证法进行寻找，即针对两种参数各设定一定的数集，对各种组合用样本进行训练，然后用同样的测试样本考核SVM性能，最后经比较选取最佳的参数集。

(4)故障诊断后进行系统重构，子滤波器的数据(如图1所示，

P_i分别代表子滤波器的状态向量和方差矩阵)进入主滤波器进行信息融合，输出的误差量(

P_g分别代表全局状态向量和方差矩阵)再反馈校正捷联惯性导航系统SINS 1。

捷联惯性导航系统SINS 1、多普勒测速仪DVL 2、磁航向仪MCP 3、地形辅助导航系统TAN 4

当第一故障诊断模块诊断出多普勒测速仪DVL 2发生故障时，捷联惯性导航系统SINS 1、磁航向仪MCP 3和地形辅助导航系统TAN 4进行重构；当第二故障诊断模块诊断出磁航向仪MCP 3发生故障时，捷联惯性导航系统SINS 1、多普勒测速仪DVL 2、地形辅助导航系统TAN 4进行重构；当第三故障诊断模块诊断出地形辅助导航系统TAN 4发生故障时，捷联惯性导航系统SINS 1、多普勒测速仪DVL 2、磁航向仪MCP 3、进行重构；当第一故障诊断模块和第二故障诊断模块诊断出多普勒测速仪DVL 2和磁航向仪MCP 3同时发生故障时，捷联惯性导航系统SINS 1、地形辅助导航系统TAN 4组成导航系统进行位置匹配；当第一故障诊断模块和第三故障诊断模块诊断出多普勒测速仪DVL 2和地形辅助导航系统TAN 4同时发生故障，捷联惯性导航系统SINS 1和磁航向仪MCP 3组成导航系统进行航向匹配；当第二故障诊断模块和第三故障诊断模块诊断出磁航向仪MCP 3、地形辅助导航系统TAN4同时发生故障时，捷联惯性导航系统SINS 1、多普勒测速仪DVL 2进行速度匹配；当发生故障的多普勒测速仪DVL 2、磁航向仪MCP 3或地形辅助导航系统TAN 4恢复正常状态时，随时加入组合导航系统进行信息融合。

主滤波器中的信息融合采用分散卡尔曼滤波、神经网络、模糊控制技术中的一种。

Claims (6)

1.一种水下潜器用捷联惯性组合导航系统容错组合方法，其特征在于：由捷联惯性导航系统SINS（1）、多普勒测速仪DVL（2）和磁航向仪MCP（3）和地形辅助导航系统TAN（4）组成，采用分散滤波结构和智能容错方法完成组合导航，通过下列步骤实现：

1.）捷联惯性导航系统SINS（1）作为参考导航系统和多普勒测速仪DVL（2）组成第一子滤波器（5），捷联惯性导航系统SINS（1）和磁航向仪MCP（3）组成第二子滤波器（6），捷联惯性导航系统SINS（1）和地形辅助导航系统TAN（4）组成第三子滤波器（7）；

2.）从所述第一子滤波器（5）、第二子滤波器（6）和第三子滤波器（7）中提取出相关特征量分别进入第一故障诊断模块（8）、第二故障诊断模块（9）和第三故障诊断模块（10）；

3.）所述第一故障诊断模块（8）判断多普勒测速仪DVL（2）是否出现故障，若出现故障则屏蔽多普勒测速仪DVL（2）的数据信息；所述第二故障诊断模块（9）判断磁航向仪MCP（3）是否出现故障，若出现故障则屏蔽磁航向仪MCP（3）的数据信息；所述第三故障诊断模块（10）判断地形辅助导航系统TAN（4）是否出现故障，若出现故障则屏蔽地形辅助导航系统TAN（4）的数据信息；

4.）故障诊断后进行系统重构，各子滤波器即：第一子滤波器（5）、第二子滤波器（6）和第三子滤波器（7）的数据进入主滤波器（11）进行信息融合，输出的误差量再反馈校正捷联惯性导航系统SINS（1）；

所述的第一故障诊断模块（8）、第二故障诊断模块（9）和第三故障诊断模块（10）由C型支持向量机C-SVM构成，选用软间隔支持向量机，为一个两类分类器，根据特征量将多普勒测速仪DVL（2）和磁航向仪MCP（3）和地形辅助导航系统TAN（4）的状态分为2类，正常状态与故障状态。

2.根据权利要求1所述的一种水下潜器用捷联惯性组合导航系统容错组合方法，其特征是：各子滤波器分别是卡尔曼滤波器或者EKF滤波器，捷联惯性导航系统SINS（1）作为参考导航系统提供姿态角、航向角、速度和位置信息；多普勒测速仪DVL（2）提供水下潜器对地速度信息与捷联惯性导航系统SINS（1）速度信息差值作为第一子滤波器（5）的观测量，磁航向仪MCP（3）提供水下潜器的航向角信息与捷联惯性导航系统SINS（1）的航向角的差值作为第二子滤波器（6）的观测量，地形辅助导航系统TAN（4）提供水下潜器的经纬度信息与捷联惯性导航系统SINS（1）经纬度的差值作为第三子滤波器（7）的观测量。

3.根据权利要求1所述的一种水下潜器用捷联惯性组合导航系统容错组合方法，其特征是：所述的第一子滤波器（5）、第二子滤波器（6）和第三子滤波器（7）中提取的相关特征量包括残差值γ(k)和状态法检测函数λ(k)，第一故障诊断模块（8）根据以上的特征量判断多普勒测速仪DVL（2）正常与否，第二故障诊断模块（9）根据以上的特征量判断磁航向仪MCP（3）正常与否，第三故障诊断模块（10）根据以上的特征量判断地形辅助导航系统TAN（4）正常与否。

4.根据权利要求1所述的一种水下潜器用捷联惯性组合导航系统容错组合方法，其特征是：所述系统重构为：当第一故障诊断模块（8）诊断出多普勒测速仪DVL（2）发生故障时，捷联惯性导航系统SINS（1）、磁航向仪MCP（3）和地形辅助导航系统TAN（4）进行重构；当第二故障诊断模块（9）诊断出磁航向仪MCP（3）发生故障时，捷联惯性导航系统SINS（1）、多普勒测速仪DVL（2）和地形辅助导航系统TAN（4）进行重构；当第三故障诊断模块（10）诊断出地形辅助导航系统TAN（4）发生故障时，捷联惯性导航系统SINS（1）、多普勒测速仪DVL（2）、磁航向仪MCP（3）进行重构；当第一故障诊断模块（8）和第二故障诊断模块（9）诊断出多普勒测速仪DVL（2）和磁航向仪MCP（3）同时发生故障时，捷联惯性导航系统SINS（1）与地形辅助导航系统TAN（4）组成导航系统进行位置匹配；当第一故障诊断模块（8）和第三故障诊断模块（10）诊断出多普勒测速仪DVL（2）和地形辅助导航系统TAN（4）同时发生故障，捷联惯性导航系统SINS（1）与磁航向仪MCP（3）组成导航系统进行航向匹配；当第二故障诊断模块（9）和第三故障诊断模块（10）诊断出磁航向仪MCP（3）和地形辅助导航系统TAN（4）同时发生故障时，捷联惯性导航系统SINS（1）、多普勒测速仪DVL（2）进行速度匹配；当发生故障的多普勒测速仪DVL（2）、磁航向仪MCP（3）和地形辅助导航系统TAN（4）恢复正常状态时，随时加入组合导航系统进行信息融合。

5.根据权利要求1所述的一种水下潜器用捷联惯性组合导航系统容错组合方法，其特征是：所述信息融合采用分散卡尔曼滤波、神经网络、模糊控制技术中的一种。

6.根据权利要求4所述的一种水下潜器用捷联惯性组合导航系统容错组合方法，其特征是：所述的C-SVM的训练样本收集和模型建立方法如下：

1）原始样本收集

通过软件仿真得到了多普勒测速仪DVL（2）、磁航向仪MCP（3）和地形辅助导航系统TAN（4）全部正常与DVL多普勒测速仪DVL（2）、磁航向仪MCP（3）和地形辅助导航系统TAN（4）分别故障时的数据样本，从中选取一部分作为训练样本，选取另一部分作为测试样本验证模型；

2）特征量提取

设存在如下系统状态方程和量测方程为

X_k=φ_k，k-1X_k-1+Γ_k-1W_k-1

Z_k=H_kX_k+V_k    式中，

X_k为k时刻的状态向量，也就是被估计矢量；Z_k为k时刻的测量序列；W_k-1为k-1时刻的系统噪声；V_k为k时刻的测量噪声序列；Φ_k，k-1为k-1时刻到k时刻的一步状态转移矩阵；Γ_k-1是系统噪声输入矩阵，H_k为k时刻的测量矩阵，离散卡尔曼滤波公式如下：

状态一步预测向量

X_k/k-1=φ_k，k-1X_k-1

状态估值计算

X_k=X_k/k-1+K_k(Z_k-H_kX_k/k-1)

滤波增益

K_k=P_k/k-1H_k ^T(H_kP_k/k-1H_k ^T+R_k)^-1

一步预测均方误差矩阵

$P_{k/k-1}={\mathrm{\φ}}_{k,k-1}{P}_{k-1}{\mathrm{\φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T$

估计均方误差方程

$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T$

从以上模型中提取出残差值和状态法检测函数λ_k作为特征量，具体如下

残差：γ_k=Z_k-H_kX_k/k-1

方差： $A_k=H_k{P}_{k|k-1}{H}_k^T+R_k$

状态法检测函数：

其中Z_k为k时刻的测量序列，H_k为k时刻的测量矩阵，为H_k的转置矩阵，X_k/k-1为状态一步预测向量，P_k/k-1为一步预测均方误差矩阵，R_k为k时刻的量测噪声的方差矩阵；

为残差向量γ_k的转置矩阵，为方差阵A_k的逆矩阵；Q_k-1为k-1时刻的系统噪声方差矩阵；I为单位阵；

3）C-SVM训练

选用RBF函数作为核函数，形式如下

$K(x,x_i)=\exp (-\frac{{|x-x_i|}^2}{{\mathrm{\δ}}^2})$

其中δ为核宽度，K(x，x_i)是样本向量x，x_i的内积核函数；在C-SVM中待确定的参数主要为惩罚参数C和RBF核函数里的核宽度δ，对于正常状态和故障状态设置不同的惩罚参数C，正常状态设置为C⁺，故障状态设置为C^-，且C⁺>C^-；为了要找出最优的参数集，采用交叉验证法进行寻找，即针对两种参数各设定一定的数集，对各种组合用样本进行训练，然后用同样的测试样本考核SVM性能，最后经比较选取最佳的参数集。

Images