CN115031724A - Sins/dvl紧组合系统dvl波束故障处理方法 - Google Patents

Abstract

本发明公开了一种SINS/DVL紧组合系统DVL波束故障处理方法，步骤一、进行捷联惯性导航系统初始对准，然后将声学多普勒计程仪波束量测信息传送到捷联惯性导航系统；步骤二：构建捷联惯性导航系统状态参量X及状态方程；步骤三、选取捷联惯性导航系统量测量Z并构建量测方程，步骤四：利用基于新息的χ²检验方法对接收到的DVL波束量测信息进行故障检测，步骤五、根据不同波束故障情况，重构故障波束速度信息。本发明直接使用了DVL原始的波束量测信息，能够更加充分的获得可利用的有效信息，当部分波束量测出现故障时，本发明通过相应的波束信息故障处理方法使组合导航系统依旧维持较高的导航精度。

Description

SINS/DVL紧组合系统DVL波束故障处理方法

技术领域

本发明属于导航技术领域，涉及一种SINS/DVL紧组合系统DVL波束故障处理方法。

背景技术

近年来，水下航行器应用在国民经济、海洋开发和军事等领域都起到了至关重要的作用。然而复杂的水下环境给航行器长时间、高精度导航带来了巨大挑战，在某些特殊应用领域又要求水下导航系统具有隐蔽性和自主性。捷联惯性导航系统(SINS)因其具有可靠性高、自主性强、输出的导航信息全面等优势被广泛应用于水下导航领域，但因其特殊的导航原理决定了导航参数的误差会随时间累积，长航时的导航精度无法保证。由于水下环境电磁信号衰减严重，因此，SINS/全球卫星导航系统(GNSS)组合导航系统在水下导航难以实现。相对电磁信号，声波在水下环境中具有衰减小、传播范围广等优势，因此声学导航设备在水下环境得到大量应用。多普勒计程仪(DVL)可以利用发射信号与接收信号之间的多普勒频移测量载体的速度，具备相对稳定的误差特性，且不受到工作范围约束、自主性强、可靠性高。目前，SINS/DVL组合导航系统为水下导航的主要导航方式。当应用在复杂水下环境导航时，DVL可能会出现故障情况和测速异常等情况，现有的SINS/DVL组合导航研究多为通过完善模型提高导航精度，所提解决方案也无法对误差量精准的建模，同时会引入新的速度量测噪声等问题。长时间的先验信息与噪声统计特性不匹配将导致组合导航结果精度变低。

发明内容

针对上述现有技术，本发明要解决的技术问题是提供一种SINS/DVL紧组合系统DVL波束故障处理方法，当部分波束量测出现故障时，通过相应的波束信息故障处理方法使组合导航系统依旧维持较高的导航精度。

为解决上述技术问题，本发明的一种SINS/DVL紧组合系统DVL波束故障处理方法，包括：

步骤一、进行捷联惯性导航系统初始对准，然后将声学多普勒计程仪波束量测信息传送到捷联惯性导航系统；

步骤二：构建捷联惯性导航系统状态参量X及状态方程；

步骤三、选取捷联惯性导航系统量测量Z并构建量测方程，以四波束詹纳斯配置结构坐标系B下的DVL四波束方向的速度误差作为量测量，该量测量由捷联惯性导航系统输出的速度信息在DVL四波束方向上的投影与DVL四波束速度量测信息之差组成；

步骤四：利用基于新息的χ²检验方法对接收到的DVL波束量测信息进行故障检测，SINS/DVL紧组合导航系统离散化系统方程和量测方程表示如下：

X_k＝Φ_k/k-1X_k-1+Γ_k/k-1W_k-1

Z_k＝H_kX_k与V_k

其中，X_k，Z_k为状态量和量测量的离散化表示；Φ_k/k-1，Γ_k/k-1和H_k分别为状态一步转移矩阵、系统噪声分配矩阵及量测矩阵；W_k-1是系统噪声向量，V_k是量测噪声向量；SINS/DVL紧组合系统卡尔曼滤波残差为r_k＝Z_k-H_kX_k/k-1，式中，X_k/k-1为状态一步预测，残差服从零均值高斯分布，其方差为：

其中，P_k/k-1＝E[X_k/k-1X_k/k-1 ^T]为一步预测协方差矩阵，

为量测噪声协方差矩阵；各波束多维残差独立不相关，即r_k(i)～N(0，S_k(i，i))，i＝1，2，3，4，选取各波束样本数据为η_k(i)＝r_k(i)r_k(i)T/S_k(i，i)，(i＝1，2，3，4)，其总体服从卡方分布，选取阈值η_v，当样本数据小于阈值时认为对应波束速度信息正常，当样本值大于阈值时认为对应波束速度信息存在异常，存在故障情况，执行步骤五；

步骤五、根据不同波束故障情况，重构故障波束速度信息。

进一步的，步骤二构建捷联惯性导航系统状态参量及状态方程具体为：

选取状态参量为：

n为导航坐标系，与当地地理坐标系重合，x、y、z轴分别指向东、北、天；δP＝[δL δλδh]^T为位置误差矢量，δL为纬度位置误差量、δλ为经度位置误量、δh高度位置误差量，δvⁿ＝[δv_E δv_N δv_U]^T为速度误差矢量，δv_E为东向速度误差、δv_N为北向速度误差、δv_U天向速度误差，φ＝[φ_e φ_u φ_u]T为失准角矢量，φ_e、φ_n、φ_u分别为东、北、天三个方向的失准角；b系为载体坐标系，ε^b＝[ε_x ε_y ε_z]^T为载体坐标系下陀螺常值漂移矢量，ε_x、ε_y、ε_z分别为x、y、z轴向的陀螺常值漂移，

为载体坐标系下加速度计常值偏置矢量，

为别为x、y、z轴向的加速度计常值偏置；

根据选取状态参量得到捷联惯性导航系统的状态方程为：

其中，状态量上的点代表一阶导，

为导航系相对惯性系的旋转角速度在导航系的投影，包括两个矢量部分：

其中

为地球自转角速度在导航坐标系的投影矢量，

为载体在导航系相对地固坐标系的旋转角速度在导航坐标系的投影矢量，

ω_ie为地球自转角速率标量，L为当地纬度，R_h为载体与地球中心的距离，其中R_h＝R_e+h，R_e为地球半径，h为载体所在海拔高度，vⁿ＝[v_E v_N v_U]^T为载体速度矢量在导航坐标系的投影，v_E、v_N、v_U分别为东向速度、北向速度及天向速度，f^b是加速度计输出载体坐标系的比力矢量；

为捷联惯导的姿态矩阵，T₁₁、T₁₂、T₁₃、T₂₁、T₂₂、T₂₃、T₃₁、T₃₂、T₃₃是姿态矩阵的各元素、

为载体坐标系下陀螺量测的高斯白噪声矢量，

为载体坐标系下x、y、z轴向陀螺量测高斯白噪声、

为载体坐标系下加速度计量测高斯白噪声矢量，

为载体坐标系下x、y、z轴向加速度计量测高斯白噪声。

进一步的，步骤三具体为：

SINS输出导航坐标系下的速度，利用惯性测速信息构造DVL波束方向速度信息，根据SINS输出导航参数有

为SINS输出速度在载体坐标系的投影，

为载体真实速度在导航坐标系下的投影，

为SINS输出导航坐标系下的速度误差；SINS导航信息构造的B坐标系下DVL波束方向速度信息

为

因此，以DVL输出速度为匹配量的SINS/DVL紧组合量测方程可以表示为

其中

为DVL测量获得的波束方向速度信息，

为波束方向测速误差。

进一步的，步骤五中根据不同波束故障情况，重构故障波束速度信息具体为：

当单个波束发生故障时，重构故障波束速度信息V_res为：

V_res＝V_i+V_j-V_n

其中，i、j表示与故障波束相邻的波束，n为与故障波束构成对角线关系的波束，V_i，V_j，V_n分别表示对应DVL波束的量测速度信息，适用于四波束DVL的“+”和“×”型配置方式，当四波数Janus配置安装角β为0度时，是“+”型配置方式，当四波数Janus配置安装角β为45度时，是“×”型配置方式；

当两个相邻波束速度信息V_n、V_l故障时(n,l∈{[1,2],[2,3],[3,4],[4,1]})，构造故障波束速度信息为：

n、l分别为与i、j呈对角线关系的故障波束，α为由DVL结构决定的固定波束发射角度；

当n、l∈{[1,3],[2,4]}且DVL采用“×”配置时，构造故障波束速度信息为：

当n、l∈{[2,4]}且DVL采用“+”配置时，构造故障波束速度信息为：

当n、l∈{[1,3]}且DVL采用“+”配置时，构造故障波束速度信息为：

当一个波束方向速度信息有效时，引入

重构四波束DVL速度，在“+”型Janus配置结构波束2或4波束方向信息有效时，引入

进行波束速度信息重构。

本发明的有益效果：本发明是一种声学多普勒计程仪与捷联惯性导航系统紧组合在多普勒计程仪波束信息发生故障时的补偿方法。本发明以多普勒计程仪的波束量测信息为基础，考虑到复杂水下环境多普勒计程仪波束信息可能出现故障，通过卡方检测故障情况并提出相应的波束信息重构方法。与传统的SINS/DVL松组合导航系统相比，SINS/DVL紧组合导航系统直接使用了DVL原始的波束量测信息，与松组合对于信息的降维使用不同，能够更加充分的获得可利用的有效信息，当部分波束量测出现故障时，松组合导航系统DVL无法提供辅助导航作用，紧组合导航系统可通过相应的波束信息故障处理方法使组合导航系统依旧维持较高的导航精度。本发明在紧组合研究通常是以完善模型的方式提高导航精度的情况下，对DVL波束量测出现故障时提供新的解决方案，具有较高的工程应用价值。

附图说明

图1是本发明实例中的四波束Janus阵列配置结构图。

图2是本发明实例中的四波数Janus配置波束故障处理方案图。

图3(a)是本发明实例中的DVL故障情况合速度误差及统计情况图。

图3(b)是本发明实例中的DVL故障情况纯惯性合速度误差统计情况图。

图3(c)是本发明实例中的DVL故障情况case3合速度误差统计情况图。

图3(d)是本发明实例中的DVL故障情况case2合速度误差统计情况图。

图3(e)是本发明实例中的DVL故障情况case1合速度误差统计情况图。

图4是本发明实例中的DVL故障情况下径向定位误差图。

图5是本发明实例中的DVL故障情况下东向、北向定位误差图。

具体实施方式

下面结合说明书附图和实施例对本发明做进一步说明。

如图1所示，为本发明适用的DVL四波束Janus阵列配置结构图，其中b系为载体坐标系，y轴指向载体纵轴方向，x轴指向载体横轴方向且与y轴垂直，z轴与x轴、y轴构成右手坐标系。α为由DVL结构决定的固定波束发射角度，β是四波数Janus配置安装角，“+”配置时为0度，“×”配置时为45度。忽略刻度系数误差及安装偏差角影响，载体坐标系三维速度和每个波束方向速度的关系为

上式中

表示由波束方向测量速度构成的速度矢量信息，

上角标T表示向量转置，

为DVL四个波束方向的速度矢量信息；

表示DVL输出速度信息在载体坐标系下的投影；C为波束方向矩阵。本发明具有波束信息构造易实现，故障处理效果优良等优势。包括如下步骤：

步骤一：航行器上配置多普勒计程仪，进行安装偏差角的标定，这里认为安装偏差角标定准确。

步骤二：进行捷联惯性导航系统初始对准后，将声学多普勒计程仪波束量测信息传送到捷联惯性导航系统，这里认为捷联惯导提供的姿态矩阵满足精度要求。

步骤三：构建捷联惯性导航系统状态参量及状态方程。选取状态量为

n为导航坐标系，与当地地理坐标系重合，x、y、z轴分别指向东、北、天。δP＝[δL δλδh]^T为位置误差矢量，δL为纬度位置误差量、δλ为经度位置误量、δh高度位置误差量，δvⁿ＝[δv_E δv_N δv_U]^T为速度误差矢量，δv_E为东向速度误差、δv_N为北向速度误差、δv_U天向速度误差，φ＝[φ_e φ_n φ_u]^T为失准角矢量，φ_e、φ_n、φ_u分别为东、北、天三个方向的失准角，ε^b＝[ε_x ε_y ε_z]^T为陀螺常值漂移矢量，ε_x、ε_y、ε_z分别为x、y、z轴向的陀螺常值漂移，

为加速度计常值偏置矢量，

为别是x、y、z轴向的加速度计常值偏置。

进而根据所选各状态参量可得捷联惯性导航系统的状态方程为：

其中状态量上的点代表一阶导，

为导航系相对惯性系的旋转角速度在导航系的投影，它包括两个矢量部分：

其中

为地球自转角速度在导航坐标系的投影矢量，

为载体在地球表面运动引起的导航系相对地固坐标系的旋转角速度在导航坐标系的投影矢量，

ω_ie为地球自转角速率标量，L为当地纬度，R_h为载体与地球中心的距离，其中R_h＝R_e+h，R_e为地球半径，h为载体所在海拔高度，vⁿ＝[v_E v_N v_U]^T为载体速度矢量在导航坐标系的投影，v_E、v_N、v_U分别为东向速度、北向速度及天向速度，f^b是加速度计输出载体坐标系的比力矢量，

为捷联惯导的姿态矩阵，T₁₁、T₁₂、T₁₃、T₂₁、T₂₂、T₂₃、T₃₁、T₃₂、T₃₃是姿态矩阵的各元素、

为陀螺量测的高斯白噪声矢量，

为x、y、z轴向陀螺量测高斯白噪声、

为加速度计量测高斯白噪声矢量，

为x、y、z轴向加速度计量测高斯白噪声。

步骤四：选取捷联惯性导航系统量测参量及构建量测方程。传统的SINS/DVL松组合导航系统以载体坐标系三维速度误差量作为量测，本发明采取的紧组合方式以DVL四波束方向的速度误差作为量测量，该量测量由捷联惯性导航系统输出的速度信息在DVL四波束方向上的投影与DVL四波束速度量测信息之差组成，具体构造方法如下：

SINS输出导航坐标系下的速度，需要利用惯性测速信息构造DVL波束方向速度信息，根据SINS输出导航参数有

为SINS输出速度在载体坐标系的投影，

为载体真实速度在导航坐标系下的投影，

为SINS输出导航坐标系下的速度误差；SINS导航信息构造的DVL波束方向速度信息

为

因此，以DVL输出速度为匹配量的SINS/DVL紧组合量测方程可以表示为

其中

为DVL测量获得的波束方向速度信息，

为波束方向测速误差。

步骤五：利用基于新息的χ²检验方法对接收到的DVL波束量测信息进行检测，并对发生故障的波束信息进行重构。SINS/DVL紧组合导航系统离散化系统方程和量测方程表示如下：

X_k＝Φ_k/k-1X_k-1+Γ_k/k-1W_k-1

Z_k＝H_kX_k+V_k

其中，X_k，Z_k为上述状态参量和量测量的离散化表示；Φ_k/k-1，Γ_k/k-1和H_k分别称为状态一步转移矩阵、系统噪声分配矩阵及量测矩阵；W_k-1是系统噪声向量，V_k是量测噪声向量。SINS/DVL紧组合系统卡尔曼滤波残差为r_k＝Z_k-H_kX_k/k-1，式中，X_k/k-1为状态一步预测，在理想情况下，残差服从零均值高斯分布，其方差为：

其中，P_k/k-1＝E[X_k/k-1X_k/k-1 ^T]为一步预测协方差矩阵，

为量测噪声协方差矩阵。因多维残差独立不相关，即r_k(i)～N(0，S_k(i，i))，i＝1，2，3，4，选取样本数据为η_k(i)＝r_k(i)r_k(i)^T/S_k(i，i)，(i＝1，2，3，4)，其总体服从卡方分布，根据自由度和需求的虚警率参照卡方分布表来选取合适的阈值η_v，当样本小于阈值时认为对应波束速度信息正常，当样本值大于阈值时认为波束速度信息存在异常。进而判断出波束故障的情况并进行相应的故障处理，本发明提出的SINS/DVL紧组合导航DVL波束故障容错处理方案如下所示：

(1)单波束故障

在单个波束故障时，故障波束测速信息可以等价为其他三波束测速信息的线性组合，可以重构故障波束方向测速V_res为

V_res＝V_i+V_j-V_n

i、j表示与故障波束相邻的波束，n为与故障波束构成对角线关系的波束，即V_i，V_j，V_n分别表示对应DVL波束的量测速度信息。这一方法可以适用于四波束DVL的“+”和“×”型配置方式。

(2)多波束故障

当两个相邻波束速度信息V_n、V_l故障时(n，l∈{[1，2]，[2，3]，[3，4]，[4，1]})，C([n，l]，：)的秩等于2，DVL垂向速度信息

等于SINS输出的载体垂向信息

构造故障波束速度信息为：

n、l分别为与i、j呈对角线关系的故障波束。当n、l∈{[1,3],[2,4]}且DVL采用“×”配置时，此时考虑到载体侧向速度及SINS输出侧向速度误差足够小，即DVL侧向速度

等于SINS输出

则构造故障波束速度信息为：

当n、l∈{[2,4]}且DVL采用“+”配置时，与上述方案相同，引入SINS速度

结合四波束DVL结构特点，重构故障波束信息为：

当n、l∈{[1,3]}且DVL采用“+”配置时，则需设SINS前向速度误差较小，并利用

构造故障波束速度信息，方案如下：

当一个波束方向速度信息有效时，波束方向矩阵C(i,:),(i∈{1,2,3,4})的秩等于1，需要同时引入两个维度的SINS速度信息才能重构所有的波束速度，考虑载体在垂向及侧向速度较小，所造成侧向速度误差及垂向速度误差也较小，所以通常情况下仅采用引入

来重构四波束DVL速度，而在“+”型Janus配置结构波束2或4波束方向信息有效时，此时有效波束信息中不包含载体前向速度信息，故需引入

进行波束速度信息重构。综上所述，四波束Janus配置波束故障处理方案如图2所示，其中不同颜色的线型描述了不同重构方案及所用的SINS速度信息。

仿真验证

仿真条件：惯性导航设备初始位置误差设置为

惯性测量单元参数如下：陀螺常值漂移0.01°h，随机游走系数为

加速度计常值偏置3×10^-4g，随机游走系数为

采样时间间隔为10ms。多普勒计程仪测速误差为0.1％航速±0.3ms，在14000秒至17600秒设置一小时的波束故障情况。

图3(a)-3(e)为DVL故障情况合速度误差及统计情况图，利用所提出的故障检测及处理方案对故障进行处理，并与在此区间内利用纯惯性导航的方案进行对比。Case1表示DVL波束2发生故障，Case2表示DVL波束2、3发生故障，Case3表示DVL波束2、3、4发生故障，可以看出在故障数据段采用惯性导航方案的速度产生较大偏移，在分布概率图中，速度误差在-0.2m/s至-0.4m/s区间段内存在较大分布概率，即速度误差存在偏置，从误差分布图中可以看出，经过所提波束补偿方案的处理之后，组合系统的速度误差呈现出零均值的高斯分布，即估计速度近似为真实速度的无偏估计。图4为DVL故障情况下径向定位误差图，可以看出在波束故障时段内采用惯性导航方式，在短时间内定位误差呈正弦趋势增长，一个小时定位误差增长约550m。同时在速度信息恢复时，位置误差并没有减小。本文提出的故障处理方案在定位精度上相较于采用惯性导航方案有了较大提高，其中Case 1在故障时间段误差几乎没有增加，在Case 2中定位误差约增长55m，Case 3中定位误差增长约90m，约为纯惯性方案定位误差的6.96％、12.95％、19.95％。图5为东向、北向定位误差图，可以看出故障Case 2的应对方案是将天向速度认为是零，天向速度误差经俯仰角的投影转为载体前向的速度误差，由此造成前向定位误差，载体在东向位移较大，北向位移较小，所以定位误差主要表现在东向定位误差约为75m；Case 3的应对方案将载体侧向及天向速度认为是零，此时造成了侧向及前向速度的不准确，东向及北向定位误差分别接近100m及50m。

Claims (4)

1.一种SINS/DVL紧组合系统DVL波束故障处理方法，其特征在于，包括：

步骤一、进行捷联惯性导航系统初始对准，然后将声学多普勒计程仪波束量测信息传送到捷联惯性导航系统；

步骤二：构建捷联惯性导航系统状态参量X及状态方程；

步骤三、选取捷联惯性导航系统量测量Z并构建量测方程，以四波束詹纳斯配置结构坐标系B下的DVL四波束方向的速度误差作为量测量，该量测量由捷联惯性导航系统输出的速度信息在DVL四波束方向上的投影与DVL四波束速度量测信息之差组成；

步骤四：利用基于新息的χ²检验方法对接收到的DVL波束量测信息进行故障检测，SINS/DVL紧组合导航系统离散化系统方程和量测方程表示如下：

X_k＝Φ_k/k-1X_k-1+Γ_k/k-1W_k-l

Z_k＝H_kX_k+V_k

其中，X_k，Z_k为状态量和量测量的离散化表示；Φ_k/k-1，Γ_k/k-1和H_k分别为状态一步转移矩阵、系统噪声分配矩阵及量测矩阵；W_k-1是系统噪声向量，V_k是量测噪声向量；SINS/DVL紧组合系统卡尔曼滤波残差为r_k＝Z_k-H_kX_k/k-1，式中，X_k/k-1为状态一步预测，残差服从零均值高斯分布，其方差为：

其中，P_k/k-1＝E[X_k/k-1X_k/k-1 ^T]为一步预测协方差矩阵，

为量测噪声协方差矩阵；各波束多维残差独立不相关，即r_k(i)～N(0，S_k(i，i))，i＝1，2，3，4，选取各波束样本数据为η_k(i)＝r_k(i)r_k(i)^T/S_k(i，i)，(i＝1，2，3，4)，其总体服从卡方分布，选取阈值η_v，当样本数据小于阈值时认为对应波束速度信息正常，当样本值大于阈值时认为对应波束速度信息存在异常，存在故障情况，执行步骤五；

步骤五、根据不同波束故障情况，重构故障波束速度信息。

2.根据权利要求1所述的一种SINS/DVL紧组合系统DVL波束故障处理方法，其特征在于：步骤二所述构建捷联惯性导航系统状态参量及状态方程具体为：

选取状态参量为：

n为导航坐标系，与当地地理坐标系重合，x、y、z轴分别指向东、北、天；δP＝[δL δλ δh]^T为位置误差矢量，δL为纬度位置误差量、δλ为经度位置误量、δh高度位置误差量，δvⁿ＝[δv_Eδv_N δv_U]^T为速度误差矢量，δv_E为东向速度误差、δv_N为北向速度误差、δv_U天向速度误差，φ＝[φ_e φ_u φ_u]^T为失准角矢量，φ_e、φ_n、φ_u分别为东、北、天三个方向的失准角；b系为载体坐标系，ε^b＝[ε_x ε_y ε_z]^T为载体坐标系下陀螺常值漂移矢量，ε_x、ε_y、ε_z分别为x、y、z轴向的陀螺常值漂移，

为载体坐标系下加速度计常值偏置矢量，

为别为x、y、z轴向的加速度计常值偏置；

根据选取状态参量得到捷联惯性导航系统的状态方程为：

其中，状态量上的点代表一阶导，

为导航系相对惯性系的旋转角速度在导航系的投影，包括两个矢量部分：

其中

为地球自转角速度在导航坐标系的投影矢量，

为载体在导航系相对地固坐标系的旋转角速度在导航坐标系的投影矢量，

ω_ie为地球自转角速率标量，L为当地纬度，R_h为载体与地球中心的距离，其中R_h＝R_e+h，R_e为地球半径，h为载体所在海拔高度，vⁿ＝[v_E v_N v_U]^T为载体速度矢量在导航坐标系的投影，v_E、v_N、v_U分别为东向速度、北向速度及天向速度，f^b是加速度计输出载体坐标系的比力矢量；

为捷联惯导的姿态矩阵，T₁₁、T₁₂、T₁₃、T₂₁、T₂₂、T₂₃、T₃₁、T₃₂、T₃₃是姿态矩阵的各元素、

为载体坐标系下陀螺量测的高斯白噪声矢量，

为载体坐标系下x、y、z轴向陀螺量测高斯白噪声、

为载体坐标系下加速度计量测高斯白噪声矢量，

为载体坐标系下x、y、z轴向加速度计量测高斯白噪声。

3.根据权利要求1所述的一种SINS/DVL紧组合系统DVL波束故障处理方法，其特征在于：步骤三具体为：

SINS输出导航坐标系下的速度，利用惯性测速信息构造DVL波束方向速度信息，根据SINS输出导航参数有

为SINS输出速度在载体坐标系的投影，

为载体真实速度在导航坐标系下的投影，

为SINS输出导航坐标系下的速度误差；SINS导航信息构造的B坐标系下DVL波束方向速度信息

为

因此，以DVL输出速度为匹配量的SINS/DVL紧组合量测方程可以表示为

其中

为DVL测量获得的波束方向速度信息，

为波束方向测速误差。

4.根据权利要求1所述的一种SINS/DVL紧组合系统DVL波束故障处理方法，其特征在于：步骤五所述根据不同波束故障情况，重构故障波束速度信息具体为：

当单个波束发生故障时，重构故障波束速度信息V_res为：

V_res＝V_i+V_j-V_n

其中，i、j表示与故障波束相邻的波束，n为与故障波束构成对角线关系的波束，V_i，V_j，V_n分别表示对应DVL波束的量测速度信息，适用于四波束DVL的“+”和“×”型配置方式，当四波数Janus配置安装角β为0度时，是“+”型配置方式，当四波数Janus配置安装角β为45度时，是“×”型配置方式；

当两个相邻波束速度信息V_n、V_l故障时(n，l∈{[1，2]，[2，3]，[3，4]，[4，1]})，构造故障波束速度信息为：

n、l分别为与i、j呈对角线关系的故障波束，α为由DVL结构决定的固定波束发射角度；

当n、l∈{[1，3]，[2，4]}且DVL采用“×”配置时，构造故障波束速度信息为：

当n、l∈{[2，4]}且DVL采用“+”配置时，构造故障波束速度信息为：

当n、l∈{[1，3]}且DVL采用“+”配置时，构造故障波束速度信息为：

当一个波束方向速度信息有效时，引入

重构四波束DVL速度，在“+”型Janus配置结构波束2或4波束方向信息有效时，引入

进行波束速度信息重构。

Images