CN111504324B - 一种噪声自适应滤波的水下组合导航方法 - Google Patents

Abstract

本发明涉及一种噪声自适应滤波的水下组合导航方法，在AUV组合导航***中，采用深度传感器作为航行深度；对滤波中有关SINS的误差方程进行简化，基于残差的χ²检测法，利用SINS/DVL组合滤波过程中信息的统计特性，判断DVL输出的测量数据是否出现测量噪声过大等异常情况，利用SINS/转速计组合进行导航。有益效果：把AUV无刷直流推进电机的转动速度作为转速计使用，对AUV硬件成本增加很小，将转速计的信息用于在DVL异常状态下的SINS/转速计组合导航，抑制了AUV位置等误差的发散速度，提高了AUV的导航定位精度。

Description

一种噪声自适应滤波的水下组合导航方法

技术领域

本发明属于多传感器组合导航领域，涉及针对SINS/DVL/转速计/深度传感器信息高效融合问题的一种噪声自适应滤波的水下组合导航方法。

背景技术

自主水下航行器(Autonomous Underwater Vehicle，AUV)是人类进行海洋探索的主要工具，而精确地水下导航定位是决定水下航行器是否能正常、稳定工作的保障，特别是对于在水下进行长航时工作的水下航行器更为至关重要。

对于AUV导航***而言，由于水下的复杂环境以及隐蔽性、自主性、可靠性等方面的要求，单一形式的导航方式很难较好地满足需要，所以通常使用几种导航方式组合增加***精度及可靠性。目前，可在AUV中使用的导航***主要有捷联式惯性导航***(SINS)、多普勒计程仪(DVL)、转速计、长基线***(LBL)、短基线***(SBL)、全球卫星定位***(GPS)、磁航向仪(MCP)等等。

水下组合导航***，因其自身特殊性，在水下工作时无法接收到来自水面的信号，所以GPS将失去用处，而基于声学的长基线、短基线导航设备需提前布置，应用范围有限。所以一般的AUV都采用捷联惯性导航***为主导航***，DVL传感器和深度传感器为辅助，利用DVL输出速度和深度传感器精度较高的特点来补偿SINS随时间累积的误差，以此大幅度提高SINS导航定位精度。

对于SINS/DVL/深度传感器组合导航***来说，存在几个问题：SINS进行自主航位推算时，会随时间的增加产生累积误差；其二，在复杂水文环境下或者AUV的运动在较大的俯仰角或者横滚角等情况下，声学的DVL提供的速度信息可能会出现精度瞬时下降，甚至无法提供有效数据等情况。

综上所述，解决SINS/DVL/深度传感器组合导航***存在的问题，提高组合导航***定位精度和可靠性，具有很强的理论意义与工程应用价值。

发明内容

要解决的技术问题

为了避免现有技术的不足之处，本发明提出一种噪声自适应滤波的水下组合导航方法，解决DVL输出的测量数据出现异常状况时AUV组合导航***导航精度快速下降问题，提高组合导航***的鲁棒性和导航定位精度，实现SINS/DVL/转速计/深度传感器测量数据的有效融合。

技术方案

一种噪声自适应滤波的水下组合导航方法，其特征在于步骤如下：

步骤1：在AUV组合导航***中，以深度传感器的测量值作为航行深度，对滤波中的SINS的误差方程进行简化，得到简化后的SINS的误差模型：

位置误差方程：

速度误差方程：

姿态误差方程：

式中：δL、δλ分别表示AUV的纬度误差、经度误差；δV_E、δV_N、δV_U分别表示AUV的东向、北向、天向速度误差；φ_E、φ_N、φ_U分别表示AUV姿态角误差在东向、北向、天向的投影；L、λ、h分别表示AUV所在位置的纬度、经度和航行深度；R_M、R_N分别为AUV所处位置的地球子午圈和卯酉圈曲率半径；V_E、V_N、V_U分别表示AUV在东向、北向、天向的航行速度；ω_ie为地球自转角速率；f_E、f_N、f_U分别表示加速度计在东向、北向、天向测得的比力值；▽_E、▽_N、▽_U分别表示加速度计的偏置误差在东向、北向、天向的投影；ε_E、ε_N、ε_U分别表示陀螺的随机漂移误差在东向、北向、天向的投影；

为载体坐标系到导航坐标系的坐标转换矩阵；ε_bx、ε_by、ε_bz分别表示x轴、y轴、z轴的陀螺仪随机漂移误差；w_gx、w_gy、w_gz分别三轴陀螺的零均值高斯白噪声；▽_bx、▽_by、▽_bz分别表示x轴、y轴、z轴的加速度计偏置误差；w_ax、w_ay、w_az分别三轴加速度计的零均值高斯白噪声；

取状态变量为

X＝[δL δλ δV_E δV_N δV_U φ_E φ_N φ_U ε_bx ε_by ε_bz ▽_bx ▽_by ▽_bz]^T

w＝[w_gx w_gx w_gx w_gx w_gx w_gx]^T

***状态方程为

式中：F和G是关于时间参数t的确定性时变矩阵；

步骤2：建立SINS/多普勒速度仪组合滤波的量测方程：

多普勒速度仪测得AUV载体坐标系速度分别V_dx、V_dy、V_dz，利用坐标转换矩阵

将其由载体坐标系转换到导航坐标系；利用SINS计算得到的导航系下的速度与转换到导航系下的多普勒速度仪的速度做差，得到SINS/多普勒速度仪组合的量测方程：

其中，H_D为SINS/多普勒速度仪组合的量测阵，v_D为其量测噪声，为零均值的高斯白噪声；

步骤3：根据SINS的计算周期T，对***状态方程和量测方程进行离散化得

其中：

量测噪声方差阵R_k为：

式中，

取初值β₀＝1，而0＜b＜1称为渐消因子；随着k逐渐增大，近似有β_k≈1-b；渐消因子b取值范围为0.9～0.999；采用序贯滤波在进行第i个标量序贯量测更新时，标量量测方程为：

简记：

再利用下式所示的下限条件

以保证

为正，同时利用上限条件

以快速降低量测

的可信度：

通过上述处理，将量测噪声

始终限制在区间

之内；

步骤4：利用SINS/多普勒速度仪组合滤波过程中信息的统计特性，通过故障函数λ_k，判断多普勒速度仪输出的测量数据是否异常：

根据给定的虚警概率α，由χ²分布得到门限值T_D，判断多普勒速度仪工作状态的方法为：当λ_k≤T_D时，多普勒速度仪工作正常；当λ_k＞T_D时，多普勒速度仪工作出现异常；

1、当多普勒速度仪工作正常时，采用SINS/多普勒速度仪进行组合导航，运用卡尔曼滤波进行状态估计，利用每次滤波得到位置、速度、姿态误差估计结果对SINS计算得到位置、速度、姿态进行误差修正，输出修正后的位置、速度、姿态等导航参数。

卡尔曼滤波公式

状态一步预测

状态一步预测均方误差阵

滤波增益

状态估计

状态估计均方误差阵P_k＝(I-K_kH_k)P_k,k-1

计算AUV周围海水相对于海底的流速V_wk。一个滤波周期T内，经过预处理的转速计输出的AUV相对于周围海水的在载体系y轴方向的k时刻的平均速度为V_{py_k}，AUV周围海水k时刻的速度V_{w_k}计算公式如下：

用指数加权移动平均法计算AUV周围海水相对于海底的流速V_wk，具体如下

V_wk＝βV_{w_k}+(1-β)V_{w_(k-1)}

其中，β可根据AUV周围海流的流速变化速度设定，当变化速度较快时，β值取得大些，如β＝0.6；当变化速度较慢时，β值取得小些，如β＝0.2；

2、当多普勒速度仪工作异常时，采用SINS/转速计进行组合导航，其量测方程为

式中，V_py为转速计输出的AUV相对于周围海水的速度在载体系y轴方向的投影，V_w为多普勒速度仪工作异常前AUV周围海水的流速估计值，H_W为SINS/转速计组合的量测阵，v_W为其量测噪声，其为零均值的高斯白噪声；

运用卡尔曼滤波对SINS/转速计组合导航进行状态估计，利用每次滤波得到位置、速度、姿态误差估计结果对SINS计算得到位置、速度、姿态进行误差修正，输出修正后的位置、速度、姿态导航参数。

有益效果

本发明提出的一种噪声自适应滤波的水下组合导航方法，在AUV组合导航***中，采用深度传感器作为航行深度；对滤波中有关SINS的误差方程进行简化，基于残差的χ²检测法，利用SINS/DVL组合滤波过程中信息的统计特性，判断DVL输出的测量数据是否出现测量噪声过大等异常情况，利用SINS/转速计组合进行导航。有益效果：把AUV无刷直流推进电机的转动速度作为转速计使用，对AUV硬件成本增加很小，将转速计的信息用于在DVL异常状态下的SINS/转速计组合导航，抑制了AUV位置等误差的发散速度，提高了AUV的导航定位精度。

附图说明

图1为本发明的方法流程示意图

图2为具体实施方式流程示意图

具体实施方式

现结合实施例、附图对本发明作进一步描述：

本发明的技术方案，包括以下步骤：

步骤1：在AUV组合导航***中，采用测量精度较高的深度传感器，将其测量值直接作为航行深度；

基于深度传感器测量精度较高这一特点，忽略其存在的测量误差，将其输出值作为深度真值，对滤波中有关SINS的误差方程进行简化；

步骤2：建立SINS/多普勒速度仪组合滤波的量测方程：

多普勒速度仪测得AUV载体坐标系速度分别V_dx、V_dy、V_dz，利用坐标转换矩阵

将其由载体坐标系转换到导航坐标系；利用SINS计算得到的导航系下的速度与转换到导航系下的多普勒速度仪的速度做差，得到SINS/多普勒速度仪组合的量测方程：

其中，H_D为SINS/多普勒速度仪组合的量测阵，v_D为其量测噪声，为零均值的高斯白噪声。

步骤3：根据SINS的计算周期T，对***状态方程和量测方程进行离散化得

其中：

考虑到多普勒速度仪在工作过程中速度测量噪声可能发生变化，量测噪声方差阵R_k确定方法如下：

式中，

取初值β₀＝1，而0＜b＜1称为渐消因子；随着k逐渐增大，近似有β_k≈1-b，渐消因子b根据多普勒速度仪量测噪声等情况，取值范围为0.9～0.999；同时，采用序贯滤波方法对R_k对角线每个元素的大小进行限制，即采用序贯滤波在进行第i个标量序贯量测更新时，标量量测方程为：

简记：

再利用下式所示的下限条件

以保证

为正，同时利用上限条件

以快速降低量测

的可信度。

通过上述处理，便可将量测噪声

始终限制在区间

之内，从而具有较好的自适应能力及滤波可靠性。

步骤4：利用SINS/多普勒速度仪组合滤波过程中信息的统计特性，通过故障函数λ_k，判断多普勒速度仪输出的测量数据是否异常：

根据给定的虚警概率α，由χ²分布得到门限值T_D，判断多普勒速度仪工作状态的方法为：当λ_k≤T_D时，多普勒速度仪工作正常；当λ_k＞T_D时，多普勒速度仪工作出现异常。

步骤5：当多普勒速度仪工作正常时，采用SINS/多普勒速度仪进行组合导航，运用卡尔曼滤波进行状态估计，利用每次滤波得到位置、速度、姿态误差估计结果对SINS计算得到位置、速度、姿态进行误差修正，输出修正后的位置、速度、姿态等导航参数。

卡尔曼滤波公式

状态一步预测

状态一步预测均方误差阵

滤波增益

状态估计

状态估计均方误差阵P_k＝(I-K_kH_k)P_k,k-1

计算AUV周围海水相对于海底的流速V_wk。一个滤波周期T内，经过预处理的转速计输出的AUV相对于周围海水的在载体系y轴方向的k时刻的平均速度为V_{py_k}，AUV周围海水k时刻的速度V_{w_k}计算公式如下：

基于AUV周围海流的流速变化相对缓慢这一特点，为了降低转速计的测量噪声影响，用指数加权移动平均法计算AUV周围海水相对于海底的流速V_wk，具体如下

V_wk＝βV_{w_k}+(1-β)V_{w_(k-1})

其中，β可根据AUV周围海流的流速变化速度设定，当变化速度较快时，β值取得大些，如β＝0.6；当变化速度较慢时，β值取得小些，如β＝0.2。

步骤6：当多普勒速度仪工作异常时，采用SINS/转速计进行组合导航，其量测方程为

式中，V_py为转速计输出的AUV相对于周围海水的速度在载体系y轴方向的投影，V_w为多普勒速度仪工作异常前AUV周围海水的流速估计值，H_W为SINS/转速计组合的量测阵，v_W为其量测噪声，其为零均值的高斯白噪声，主要包括转速计的量测噪声等。

运用卡尔曼滤波对SINS/转速计组合导航进行状态估计，利用每次滤波得到位置、速度、姿态误差估计结果对SINS计算得到位置、速度、姿态进行误差修正，输出修正后的位置、速度、姿态等导航参数。

SINS的误差模型：

滤波器的状态变量的选取：取AUV各状态的误差量，包括：定位误差(纬度误差、经度误差)δL、δλ，AUV的东向、北向、天向速度误差δV_E、δV_N、δV_U，姿态角误差在东向、北向、天向的投影φ_E、φ_N、φ_U，x轴、y轴、z轴陀螺仪随机漂移误差ε_bx、ε_by、ε_bz，x轴、y轴、z轴加速度计的偏置误差▽_bx、▽_by、▽_bz，假设姿态角误差为小角度并忽略重力场模型误差，可得SINS的近似线性误差模型：

位置误差方程：

速度误差方程：

姿态误差方程：

其中：L、λ、h分别表示AUV所在位置的纬度、经度和航行深度，R_M、R_N分别为AUV所处位置的地球子午圈和卯酉圈曲率半径，V_E、V_N、V_U分别表示AUV在东向、北向、天向的航行速度，ω_ie为地球自转角速率，f_E、f_N、f_U分别表示加速度计在东向、北向、天向测得的比力值，▽_E、▽_N、▽_U分别表示加速度计的偏置误差在东向、北向、天向的投影，ε_E、ε_N、ε_U分别表示陀螺的随机漂移误差在东向、北向、天向的投影。

其中：

为载体坐标系到导航坐标系的坐标转换矩阵，w_gx、w_gy、w_gz分别三轴陀螺的零均值高斯白噪声，w_ax、w_ay、w_az分别三轴加速度计的零均值高斯白噪声。

取状态变量为

X＝[δL δλ δV_E δV_N δV_U φ_E φ_N φ_U ε_bx ε_by ε_bz ▽_bx ▽_by ▽_bz]^T

w＝[w_gx w_gx w_gx w_gx w_gx w_gx]^T

***状态方程

F和G是关于时间参数t的确定性是时变矩阵，可根据SINS的位置误差方程、速度误差方程、姿态误差方程整理得到。

DVL测得AUV载体坐标系速度分别V_dx、V_dy、V_dz，利用坐标转换矩阵

将其由载体坐标系转换到导航坐标系。利用SINS计算得到的导航系下的速度与转换到导航系下的DVL的速度做差，建立量测方程

其中，H_D为SINS/DVL组合的量测阵，v_D为其量测噪声，为零均值的高斯白噪声，主要包括DVL的量测噪声等。

根据SINS的计算周期T，对***状态方程和量测方程进行离散化得

其中：

基于残差的DVL工作状态判别方法

DVL非正常工作状态可以分为两类：一类情况DVL速度测量精度下降，像AUV受到海流冲击或进行转弯等动作，主要表现为量测噪声增大，可被划分为噪声突变类型；二类情况主要表现为输出信息的变化，像遇到强吸声物质声波无法返回或处于俯仰角和横滚角较大情况下，输出速度信息可能为0，这类情况可被划分为信息突变类型。

基于残差的χ²检测法，利用信息的统计特性来判断DVL工作是否正常，以及非正常状态的类型。当DVL工作正常时，量测误差是零均值高斯白噪声；当DVL工作在非正常状态下时，量测误差不再是零均值高斯白噪声。

当DVL工作正常时，滤波过程中残差向量为零均值高斯白噪声过程，其协方差矩阵为：

设残差序列即新息为：

故障检测函数为：

由统计特性可知，λ_k服从自由度m(向量Z_k维数)的χ²分布。设λ_k大于某一个门限值T_D的概率为α，即：

其中，α为允许的虚警概率。

根据给定的虚警概率α，可由χ²分布得到门限值T_D，此时检测过程为：

当λ_k≤T_D时，DVL工作正常；

当λ_k＞T_D时，DVL工作出现异常。

常规基于残差的χ²检测法，虽然可以迅速判别***是否存在突变型故障，但无法区分故障类别。为了解决这一问题，将残差的均值作为判据引入到故障检测***中，噪声突变型其残差均值会在0附近震荡，接近于0；若为信息突变类型，则残差均值与0值会有一定偏差。残差均值计算方法为：

上式中，

表示残差均值，m是滑动窗长度，λ_i是故障发生后i时刻SINS/DVL组合***残差。此时可以通过

判断DVL产生了何种误差，当

接近于0，为噪声突变类型误差，即DVL测量数据包含较大扰动噪声；反之则为信息突变类型误差，DVL短时失效。

当DVL工作正常或者工作在出现噪声突变类型误差的非正常状态时，采用自适应滤波，对SINS/DVL组合导航数据进行融合，量测预测误差公式为：

量测噪声方差阵R_k获取方法如下：

其中，

取初值β₀＝1，而0＜b＜1称为渐消因子。随着k逐渐增大，近似有β_k≈1-b，渐消因子b根据DVL量测噪声等情况，取值范围为0.9～0.999。

采用序贯滤波方法对R_k对角线每个元素的大小进行限制。假设R_k为对角线矩阵，采用序贯滤波在进行第i个标量序贯量测更新时，标量量测方程为：

简记：

再利用下式所示的下限条件

以保证

为正，同时利用上限条件

以快速降低量测

的可信度。

通过上述处理，便可将量测噪声

始终限制在区间

之内，从而具有较好的自适应能力及滤波可靠性。

在AUV采用SINS/DVL组合导航过程中，同时计算AUV周围海水相对于海底的流速V_wk。一个滤波周期T内，经过预处理的转速计输出的AUV相对于周围海水的在载体系y轴方向的k时刻的平均速度为V_{py_k}，AUV周围海水k时刻的速度V_{w_k}计算公式如下：

基于AUV周围海流的流速变化相对缓慢这一特点，为了降低转速计的测量噪声影响，用指数加权移动平均法计算AUV周围海水相对于海底的流速V_wk，具体如下

V_wk＝βV_{w_k}+(1-β)V_{w_(k-1)}

其中，β可根据AUV周围海流的流速变化速度设定，当变化速度较快时，β值取得大些，如β＝0.6；当变化速度较慢时，β值取得小些，如β＝0.2。

当DVL数据误差类型为信息突变类型误差，则DVL输出的速度信息完全失效，组合导航***SINS/DVL无法正常工作，需切换到SINS/转速计组合导航。待DVL工作正常后，切换回SINS/DVL组合导航。

SINS/转速计组合导航的量测方程：

其中，V_w为根据DVL失效前最后一次测量的速度数据计算得到的AUV相对于水底的速度，H_W为SINS/转速计组合的量测阵，v_W为其量测噪声，为零均值的高斯白噪声，主要包括转速计的量测噪声等。

Claims (1)

1.一种噪声自适应滤波的水下组合导航方法，其特征在于步骤如下：

步骤1：在AUV组合导航***中，以深度传感器的测量值作为航行深度，对滤波中的SINS的误差方程进行简化，得到简化后的SINS的误差模型：

位置误差方程：

速度误差方程：

姿态误差方程：

式中：δL、δλ分别表示AUV的纬度误差、经度误差；δV_E、δV_N、δV_U分别表示AUV的东向、北向、天向速度误差；φ_E、φ_N、φ_U分别表示AUV姿态角误差在东向、北向、天向的投影；L、λ、h分别表示AUV所在位置的纬度、经度和航行深度；R_M、R_N分别为AUV所处位置的地球子午圈和卯酉圈曲率半径；V_E、V_N、V_U分别表示AUV在东向、北向、天向的航行速度；ω_ie为地球自转角速率；f_E、f_N、f_U分别表示加速度计在东向、北向、天向测得的比力值；▽_E、▽_N、▽_U分别表示加速度计的偏置误差在东向、北向、天向的投影；ε_E、ε_N、ε_U分别表示陀螺的随机漂移误差在东向、北向、天向的投影；

为载体坐标系到导航坐标系的坐标转换矩阵；ε_bx、ε_by、ε_bz分别表示x轴、y轴、z轴的陀螺仪随机漂移误差；w_gx、w_gy、w_gz分别三轴陀螺的零均值高斯白噪声；▽_bx、▽_by、▽_bz分别表示x轴、y轴、z轴的加速度计偏置误差；w_ax、w_ay、w_az分别三轴加速度计的零均值高斯白噪声；

取状态变量为

X＝[δL δλ δV_E δV_N δV_U φ_E φ_N φ_U ε_bx ε_by ε_bz ▽_bx ▽_by ▽_bz]^T

w＝[w_gx w_gx w_gx w_gx w_gx w_gx]^T

***状态方程为

式中：F和G是关于时间参数t的确定性时变矩阵；

步骤2：建立SINS/多普勒速度仪组合滤波的量测方程：

多普勒速度仪测得AUV载体坐标系速度分别V_dx、V_dy、V_dz，利用坐标转换矩阵

将其由载体坐标系转换到导航坐标系；利用SINS计算得到的导航系下的速度与转换到导航系下的多普勒速度仪的速度做差，得到SINS/多普勒速度仪组合的量测方程：

其中，H_D为SINS/多普勒速度仪组合的量测阵，v_D为其量测噪声，为零均值的高斯白噪声；

步骤3：根据SINS的计算周期T，对***状态方程和量测方程进行离散化得

其中：

量测噪声方差阵R_k为：

式中，

取初值β₀＝1，而0＜b＜1称为渐消因子；随着k逐渐增大，近似有β_k≈1-b；渐消因子b取值范围为0.9～0.999；采用序贯滤波在进行第i个标量序贯量测更新时，标量量测方程为：

简记：

再利用下式所示的下限条件

以保证

为正，同时利用上限条件

以快速降低量测

的可信度：

通过上述处理，将量测噪声

始终限制在区间

之内；

步骤4：利用SINS/多普勒速度仪组合滤波过程中信息的统计特性，通过故障函数λ_k，判断多普勒速度仪输出的测量数据是否异常：

根据给定的虚警概率α，由χ²分布得到门限值T_D，判断多普勒速度仪工作状态的方法为：当λ_k≤T_D时，多普勒速度仪工作正常；当λ_k＞T_D时，多普勒速度仪工作出现异常；

1、当多普勒速度仪工作正常时，采用SINS/多普勒速度仪进行组合导航，运用卡尔曼滤波进行状态估计，利用每次滤波得到位置、速度、姿态误差估计结果对SINS计算得到位置、速度、姿态进行误差修正，输出修正后的位置、速度、姿态导航参数；卡尔曼滤波公式

状态一步预测

状态一步预测均方误差阵

滤波增益

状态估计

状态估计均方误差阵P_k＝(I-K_kH_k)P_k,k-1

计算AUV周围海水相对于海底的流速V_wk，一个滤波周期T内，经过预处理的转速计输出的AUV相对于周围海水的在载体系y轴方向的k时刻的平均速度为V_{py_k}，AUV周围海水k时刻的速度V_{w_k}计算公式如下：

用指数加权移动平均法计算AUV周围海水相对于海底的流速V_wk，具体如下

V_wk＝βV_{w_k}+(1-β)V_{w_(k-1)}

其中，β可根据AUV周围海流的流速变化速度设定，当变化速度较快时，β值取得大些，如β＝0.6；当变化速度较慢时，β值取得小些，如β＝0.2；

2、当多普勒速度仪工作异常时，采用SINS/转速计进行组合导航，其量测方程为

式中，V_py为转速计输出的AUV相对于周围海水的速度在载体系y轴方向的投影，V_w为多普勒速度仪工作异常前AUV周围海水的流速估计值，H_W为SINS/转速计组合的量测阵，v_W为其量测噪声，其为零均值的高斯白噪声；

运用卡尔曼滤波对SINS/转速计组合导航进行状态估计，利用每次滤波得到位置、速度、姿态误差估计结果对SINS计算得到位置、速度、姿态进行误差修正，输出修正后的位置、速度、姿态导航参数。

Images