CN110873813B - 一种水流速度估算方法、组合导航方法及装置 - Google Patents

Abstract

本发明涉及一种水流速度估算方法、组合导航方法及装置，属于水下导航定位领域。其中水流速度估算方法包括以下步骤：1)获取组合导航系统输出的导航系下的速度

和多普勒测速仪在水跟踪模式下输出的速度v_d2；2)根据获取的速度

和速度v_d2，建立水流速度x_w的系统方程和观测方程，利用卡尔曼滤波算法计算水流速度x_w；所述系统方程为：

所述观测方程为：z_w＝x_w(t)+η_w；其中，z_w为观测量，

为导航坐标系n到载体坐标系b的转换矩阵，η_w为高斯白噪声。本发明提高了多普勒测速仪在水跟踪模式下时水流估算结果的准确性，基于此也提高了多普勒测速仪在水跟踪模式下时导航结果的准确性。

Description

一种水流速度估算方法、组合导航方法及装置

技术领域

本发明涉及一种水流速度估算方法、组合导航方法及装置，属于水下导航领域。

背景技术

当前，水下导航技术仍然是水下航行器应用的最大制约因素。惯性导航系统(Inertial Navigation System，INS)与声学多普勒(Doppler Velocity Log,DVL)组合是是实现水下自主导航的主要方式。通过多普勒提供的速度观测值，可以有效地抑制惯性导航系统误差的增长，从而使得水下航行器能完成水下、远程、长航时高精度自主导航。

多普勒是利用安装在载体上的超声换能器向海底发射超声波，并根据多普勒效应原理测量载体速度的仪器。多普勒通常有两种工作模式：底跟踪模式和水跟踪模式。当载体相对于海底的距离在多普勒测速的有效作用范围之内时，多普勒可以工作在底跟踪模式和水跟踪模式。在底跟踪模式下，多普勒可以提供精确的载体系下的对地速度；当载体对海底的距离超过了有效作用范围，多普勒只能工作在水跟踪模式，测量得到是载体的对水速度，需要知道水流的速度才能得到载体的对地速度。

目前，国内外对于底跟踪模式下的组合导航方法研究已经比较成熟，但是对于水跟踪模式下的组合导航方法研究仍然十分有限。对于水跟踪模式下的组合导航，需要计算水流速度，常用的计算水流速度的方式是把当前的水流速度作为INS/DVL组合导航系统的状态进行在线估计。然而，在进行在线估计时，仅采用了INS/DVL组合导航中的DVL的速度信息作为观测值，很难正确估计水流速度，由此很可能引起组合导航滤波器的发散，使导航结果不准。

发明内容

本发明的目的在于提供一种水流速度估算方法，以解决现有方法对水流速度估算不准确的问题。同时，提供一种组合导航方法及装置，以解决现有组合导航结果不准的问题。

为实现上述目的，本发明提出一种水流速度估算方法，包括以下步骤：

1)获取组合导航系统输出的导航系下的速度

和多普勒测速仪在水跟踪模式下输出的速度v_d2，所述组合导航系统包括惯性导航系统和多普勒测速仪；

2)根据获取的速度

和速度v_d2，建立水流速度x_w的系统方程和观测方程，利用卡尔曼滤波算法计算水流速度x_w；

所述系统方程为：

其中，α为时间相关常数，υ_w为高斯白噪声，t为时间变量；

所述观测方程为：z_w＝x_w(t)+η_w；其中，z_w为观测量，

为导航坐标系n到载体坐标系b的转换矩阵，η_w为高斯白噪声。

有益效果是：本发明根据组合导航系统输出的导航系下的速度

和多普勒测速仪在水跟踪模式下输出的速度v_d2，通过卡尔曼滤波，建立水流速度x_w的系统方程和观测方程，计算出水流速度，避免了现有方法仅采用DVL的速度信息作为观测值时很难正确估计水流速度的问题，提高了估算结果的准确性。

另外，本发明还提出一种组合导航方法，包括以下步骤：

1)判断组合导航系统中多普勒测速仪的工作模式，所述组合导航系统包括惯性导航系统和多普勒测速仪；

2)当多普勒测速仪的工作模式是底跟踪模式时，将多普勒测速仪在底跟踪模式下输出的速度v_d1记为多普勒测速仪相对水底的速度v_d；当多普勒测速仪的工作模式是水跟踪模式时，将多普勒测速仪在水跟踪模式下输出的速度v_d2与水流速度x_w之和记为多普勒测速仪相对水底的速度v_d，计算水流速度x_w的过程包括以下步骤：

①获取组合导航系统输出的导航系下的速度

和多普勒测速仪在水跟踪模式下输出的速度v_d2；

②根据获取的速度

和速度v_d2，建立水流速度x_w的系统方程和观测方程，利用卡尔曼滤波算法计算水流速度x_w；

所述系统方程为：

其中，α为时间相关常数，υ_w为高斯白噪声，t为时间变量；

所述观测方程为：z_w＝x_w(t)+η_w；其中，z_w为观测量，

为导航坐标系n到载体坐标系b的转换矩阵，η_w为高斯白噪声；

3)根据多普勒测速仪相对水底的速度v_d进行组合导航。

另外，本发明还提出一种组合导航装置，包括惯性导航系统、多普勒测速仪、存储器、处理器以及存储在所述存储器中并可在处理器上运行的计算机程序，惯性导航系统的数据输出端与处理器连接，多普勒测速仪的数据输出端与处理器连接，所述处理器在执行所述计算机程序时实现上述组合导航方法。

有益效果是：本发明的组合导航方法和组合导航装置在多普勒测速仪处于水跟踪工作模式时，根据组合导航系统输出的导航系下的速度

和多普勒测速仪在水跟踪模式下输出的速度v_d2，通过卡尔曼滤波，建立水流速度x_w的系统方程和观测方程，计算出水流速度，然后根据将多普勒测速仪在水跟踪模式下输出的速度v_d2与水流速度x_w之和进行组合导航，提高了多普勒测速仪在水跟踪模式下时导航结果的准确性。

进一步地，所述组合导航方法和组合导航装置中，判断是否处于底跟踪模式的过程包括：

A：根据公式

计算λ_k，其中r_k＝z_k-H_kx_k|k-1，r_k为残差序列，z_k是量测向量，H_k为k时刻的量测矩阵，x_k|k-1为k时刻的状态预测值，状态包括速度误差δvⁿ、失准角φⁿ、陀螺零偏▽_g和加速度计零偏▽_a；

为状态转移矩阵A_k的逆矩阵；λ_k服从χ²分布；

B：判断λ_k是否大于等于门限值h_D，若λ_k大于等于h_D，则判断为处于底跟踪模式；若λ_k小于h_D，则判断为水跟踪模式。

进一步地，所述组合导航方法和组合导航装置中，λ_k服从自由度为3的χ²分布。

进一步地，所述组合导航方法和组合导航装置中，步骤3)采用卡尔曼滤波方式进行组合导航。

附图说明

图1是本发明组合导航方法实施例中组合导航系统结构示意图；

图2是本发明组合导航方法实施例中组合导航方法流程图。

具体实施方式

组合导航方法实施例：

本实施例中组合导航系统包括惯性导航系统和多普勒测速仪，根据多普勒测速仪相对水底的速度v_d进行组合导航的过程为：建立惯性导航系统在导航坐标系下的误差模型，如图1所示，多普勒测速仪相对水底的速度v_d经惯导系统的姿态矩阵转换到导航系下，与惯性导航系统输出的速度作差作为卡尔曼滤波器的输入，通过卡尔曼滤波估计导航系统的状态误差，从而输出最优的导航结果。选取北-天-东(NUE)地理坐标系为导航参考坐标系，记为n系，载体坐标系即为b系，多普勒坐标系记为d系。组合导航系统的状态可以选取为：北、天、东方向的速度误差δvⁿ；失准角φⁿ；陀螺零偏▽_g；加速度计零偏▽_a。则系统的状态量可以表示为：

x(t)＝[δvⁿ,φⁿ,▽_g,▽_a]^T (1)

系统的状态方程：

其中F(t)为状态转移矩阵：

F(t)中的各个矩阵表示如下：

其中：V_N、V_U、V_E分别为北、天、东三个方向的速度；ω_U、ω_N分别为地球自转角速度在天向、北向的分量；t为时间；g为重力加速度；L为当前地理纬度；R为地球半径。

且：

C为方向余弦矩阵，如：

表示b系到n系的转换矩阵。f^b为加速度计测量值，ω_ie为地球自转角速度。多普勒测速仪相对水底的速度v_d在导航坐标系下的投影

可以表示为：

为多普勒载体坐标系d到惯导载体坐标系b的方向余弦矩阵，含有误差的方向余弦矩阵

可以表示为：

其中，φⁿ为姿态误差，把式子(13)带入(12)，可得：

其中，vⁿ表示n系下载体的真实速度，

为多普勒测量值的速度误差。

惯导输出速度

和多普勒输出速度

之差可以表示为：

其中，

为惯导的速度误差。则系统的量测方程可以表示为：

υ(t)为高斯白噪声，量测矩阵H(t)可以表示为：

根据系统方程(2)和观测方程(16)以卡尔曼滤波的方式进行组合导航。对方程(2)、(16)进行离散化可得:

x_k+1＝A_kx_k+Γ_kw_k (18)

z_k＝H_kx_k+υ_k (19)

式中x_k是状态向量,z_k是量测向量,w_k、υ_k分别为系统噪声向量和量测噪声向量，它们是互不相关的零均值高斯白噪声,Q_k、R_k分别为w_k、υ_k的方差阵。卡尔曼滤波的具体实现如下：

预测更新：

x_k|k-1＝A_k-1x_k-1 (20)

测量更新：

x_k＝x_k|k-1+K_k[z_k-H_kx_k|k-1] (22)

P_k＝[I-K_kH_k]P_k|k-1 (23)

式中x_k|k-1为k时刻的状态预测值，x_k为k时刻的最优状态估计值，P为协方差矩阵，K为卡尔曼滤波增益。

多普勒测速仪的工作模式分为两种，一种是底跟踪模式，另一种是水跟踪模式。当多普勒测速仪的工作模式是底跟踪模式时，多普勒测速仪输出的速度v_d1为多普勒测速仪相对水底的速度v_d；当多普勒测速仪的工作模式是水跟踪模式时，多普勒测速仪输出的速度v_d2与水流速度x_w之和为多普勒测速仪相对水底的速度v_d。

本实施例中判断多普勒测速仪工作模式的方法为：

对于INS/DVL组合导航系统，定义新息如下：

r_k＝z_k-H_kx_k|k-1 (26)

其对应的方差阵为

构造检测变量：

则λ_k服从自由度为3的χ²分布，即：λ_k～χ²(3)，设定误警率之后查χ²分布表可以得到检测门限，记为h_D，按以下原则即可检测多普勒是否工作在底跟踪模式：

本实施例在多普勒测速仪处于水跟踪模式时，计算水流速度x_w的过程如下：

由于水流是一个缓慢变化的量，采用一阶Gauss-Markov过程来建模。水流的速度记为x_w，可以建模为：

其中α为时间相关常数，υ_w为高斯白噪声。

以当前时刻组合导航输出的速度减去多普勒的速度即为水流的速度观测值：

其中，

为导航坐标系n到载体坐标系b的转换矩阵，

为组合导航输出的导航系下的速度。水流速度的观测方程为：

z_w＝x_w(t)+η_w (31)

η_w为高斯白噪声。

如图2所示，本实施例的组合导航方法具体流程：组合导航系统工作于INS/DVL组合导航状态，在组合导航的过程中，通过χ²检验的方式检测多普勒是否工作于底跟踪模式，若多普勒工作于底跟踪模式则直接进行组合导航，若多普勒不是工作在底跟踪模式(也即多普勒工作于水跟踪模式)，则先估计水流的速度，再计算相对于水底的速度，然后进行组合导航。

水流速度估算方法实施例：

本实施例中的水流速度估算方法已在组合导航方法实施例中介绍，这里不做过多赘述。

组合导航装置实施例：

本实施例中组合导航装置包括包括惯性导航系统、多普勒测速仪、存储器、处理器以及存储在所述存储器中并可在处理器上运行的计算机程序，惯性导航系统的数据输出端与处理器连接，多普勒测速仪的数据输出端与处理器连接，所述处理器在执行所述计算机程序时实现组合导航方法实施例中的组合导航方法。

组合导航方法的具体实施过程在上述组合导航方法实施例中已经介绍，这里不做过多赘述。

Claims (6)

1.一种水流速度估算方法，其特征在于，包括以下步骤：

1)获取组合导航系统输出的导航系下的速度

和多普勒测速仪在水跟踪模式下输出的速度v_d2，所述组合导航系统包括惯性导航系统和多普勒测速仪；

2)根据获取的速度

和速度v_d2，建立水流速度x_w的系统方程和观测方程，利用卡尔曼滤波算法计算水流速度x_w；

所述系统方程为：

其中，α为时间相关常数，υ_w为高斯白噪声，t为时间变量；

所述观测方程为：z_w＝x_w(t)+η_w；其中，z_w为观测量，

为导航坐标系n到载体坐标系b的转换矩阵，η_w为高斯白噪声。

2.一种组合导航方法，其特征在于，包括以下步骤：

1)判断组合导航系统中多普勒测速仪的工作模式，所述组合导航系统包括惯性导航系统和多普勒测速仪；

2)当多普勒测速仪的工作模式是底跟踪模式时，将多普勒测速仪在底跟踪模式下输出的速度v_d1记为多普勒测速仪相对水底的速度v_d；当多普勒测速仪的工作模式是水跟踪模式时，将多普勒测速仪在水跟踪模式下输出的速度v_d2与水流速度x_w之和记为多普勒测速仪相对水底的速度v_d，计算水流速度x_w的过程包括以下步骤：

①获取组合导航系统输出的导航系下的速度

和多普勒测速仪在水跟踪模式下输出的速度v_d2；

②根据获取的速度

和速度v_d2，建立水流速度x_w的系统方程和观测方程，利用卡尔曼滤波算法计算水流速度x_w；

所述系统方程为：

其中，α为时间相关常数，υ_w为高斯白噪声，t为时间变量；

所述观测方程为：z_w＝x_w(t)+η_w；其中，z_w为观测量，

为导航坐标系n到载体坐标系b的转换矩阵，η_w为高斯白噪声；

3)根据多普勒测速仪相对水底的速度v_d进行组合导航。

3.根据权利要求2所述的组合导航方法，其特征在于，判断是否处于底跟踪模式的过程包括：

A：根据公式

计算λ_k，其中r_k＝z_k-H_kx_kk-1，r_k为残差序列，z_k是量测向量，H_k为k时刻的量测矩阵，x_kk-1为k时刻的状态预测值，状态包括速度误差δvⁿ、失准角φⁿ、陀螺零偏▽_g和加速度计零偏▽_a；

为状态转移矩阵A_k的逆矩阵；λ_k服从χ²分布；

B：判断λ_k是否大于等于门限值h_D，若λ_k大于等于h_D，则判断为处于底跟踪模式；若λ_k小于h_D，则判断为水跟踪模式。

4.根据权利要求3所述的组合导航方法，其特征在于，λ_k服从自由度为3的χ²分布。

5.根据权利要求2所述的组合导航方法，其特征在于，步骤3)采用卡尔曼滤波方式进行组合导航。

6.一种组合导航装置，包括惯性导航系统、多普勒测速仪、存储器、处理器以及存储在所述存储器中并可在处理器上运行的计算机程序，惯性导航系统的数据输出端与处理器连接，多普勒测速仪的数据输出端与处理器连接，其特征在于，所述处理器在执行所述计算机程序时实现如权利要求2-5中任一项所述的组合导航方法。

Images