CN111750865B - 一种用于双功能深海无人潜器导航系统的自适应滤波导航方法 - Google Patents

Abstract

本发明提出一种用于双功能深海无人潜器导航系统的水下自适应滤波导航方法，根据深海潜器不同工作模式实际需求的不同而进行滤波算法的调整，深海潜器处于滑翔模式时，系统对稳定性要求高，对导航精度要求一般，滤波算法以保证稳定性为主要目的；当深海潜器处于巡航模式时，滤波需在保证稳定性的基础上尽可能提升导航精度。本发明在增强滤波稳定性、有效避免滤波发散的同时保留其解算精度；还使得滤波更加重视当前的观测信息同时减弱历史观测信息对滤波的影响，增加了对量测噪声变化的跟踪能力；有助于保持滤波的自适应度和鲁棒性，能有效应对水下复杂多变的环境干扰，实现水下高精度导航定位。

Description

一种用于双功能深海无人潜器导航系统的自适应滤波导航方法

技术领域

本发明属于属于导航技术领域，具体涉及一种用于双功能深海无人潜器导航系统的改进自适应滤波方法。

背景技术

深海无人潜器在水下实际导航定位工作中需要兼顾精度和能耗的基本条件，不同精度要求下其传感器使用、算法复杂度都有一定差异，继而影响到系统功耗的差异。基于以上考虑，我们设计一种双功能深海无人潜器，具备滑翔和巡航两种工作模式。滑翔模式下，其工作任务为跨海域迁徙，无探测任务，此时系统要求的稳定性较高，导航精度较低，启用MEMS-IMU和磁力计完成导航解算，系统功耗较小；巡航模式下，工作任务为探测与搜寻，此时要在保证稳定性基础上提升导航精度，系统启用FOG-IMU、DVL和深度计完成高精度的导航解算。

线性离散的系统状态方程和量测方程为：

其中，X_k是线性离散系统的状态向量，Z_k为系统观测向量，Φ_k,k-1为系统的状态转移矩阵，H_k为系统的观测矩阵，W_k和V_k分别为系统噪声和量测噪声，其统计特性满足以下条件：

其中，q_k代表线性离散系统的系统噪声均值，r_k代表量测噪声均值，Q_k表示系统噪声方差阵，R_k表示量测噪声方差阵，两种噪声方差阵都是不确定的，自适应滤波算法的优势在于能够将量测信息Z_k利用起来，并且估计出噪声方差阵Q_k和R_k，实时调整滤波的增益K。

在实际应用滤波算法进行导航估计解算时，系统噪声方差阵Q和观测噪声方差阵R基本都是预先未知不确定的，当系统模型不够精确便会导致滤波精度的下降，甚至会发散。进一步的，如果系统模型精确，但在实际工程中，外界复杂多变的环境会导致数学模型产生扰动，即Q、R发生了不可预知的变化。若对系统噪声方差阵Q和量测噪声方差阵R同时进行估计，每次滤波都需要估计出系统噪声Q和量测噪声R的值，相比于传统的卡尔曼滤波算法此算法的计算量较大，滤波的实时性不够理想，从而导致极易影响到导航算法的实时性，另外Q和R同时估计的过程中，当系统噪声和量测噪声同时增强时，仅仅根据新息的值是无法分辨误差来源的，进而有较大可能导致滤波的效果变差，甚至发散。

深海无人潜器处于任一模式下进行工作时，都会受到水下复杂多变环境的干扰，若不能对环境噪声进行良好的估计与补偿，系统的导航精度就会受到较大的影响。

发明内容

为解决上述问题，本发明提出一种用于双功能深海无人潜器导航系统的水下自适应滤波导航方法，针对不同工作模式下的不同导航需求，滤波算法对应地进行不同的调整，以期有效应对水下复杂多变的环境干扰，实现水下高精度导航定位。深海无人潜器导航系统具有滑翔和巡航两种工作状态，分别对应低精度跨海域迁徙和高精度探测搜寻两种工作模式。为了满足两种模式不同的导航精度需求，自适应滤波算法分别作出了相应调整。

为了达到上述目的，本发明提供如下技术方案：

一种双功能深海无人潜器导航系统，具备滑翔和巡航两种工作模式，在不同工作模式情况下进行不同的调整来满足对应的导航需求，当深海潜器处于低精度滑翔模式时，在传统卡尔曼滤波的基础上加入对量测噪声方差阵R进行自适应更新的过程，将R表达式中负项去除，在滤波自适应过程中对量测噪声方差阵R表达式中自适应系数进行阶段性更新重置；当深海潜器处于高精度巡航模式时，只对量测噪声方差阵R进行迭代更新，在R更新过程中加入对R矩阵的正定性在线判定，并根据判定结果修正R的表达式；在滤波自适应时对量测噪声方差阵R进行自适应系数更新，并在滤波全过程中加入对状态误差协方差矩阵P和量测噪声协方差矩阵R的上下界限定，将P矩阵的二范数作为判定滤波收敛的必要条件。

进一步的，当深海潜器处于滑翔模式时，包括如下步骤：

(1)在传统卡尔曼滤波基础上加入R矩阵的迭代更新过程实现对环境噪声的跟踪能力，R的具体更新表达式为：

其中自适应系数d_k-1＝(1-b)/(1-b^k-1)，b为遗忘因子；

(2)此时导航系统对稳定性要求极高，R矩阵的正定性是滤波稳定的必要条件，为保证R矩阵的正定性，此处进一步将R表达式中负项去除，即：

(4)当滤波递推次数k达到阈值时，令d_k中k＝0，重新开始自适应过程。

进一步的，在步骤(1)之前还包括如下步骤：

建立状态向量X、量测向量Z，并根据已知的器件误差和设定的姿态、速度、位置误差初始化系统噪声协方差矩阵Q₀、量测噪声协方差矩阵R₀和状态误差协方差矩阵P₀；所述的状态向量X为：

X＝[δv_x δv_y φ_x φ_y φ_z δL δλ ▽_x ▽_y ε_x ε_y ε_z]^T

其中，δv_x、δv_y表示东向、北向速度误差，φ_x、φ_y、φ_z表示俯仰、横滚、航向角误差，δL、δλ表示位置误差，▽_x、▽_y表示加速度计误差，ε_x、ε_y、ε_z表示陀螺随机漂移误差，量测向量Z为单维向量，Z＝δφ_z表示航向角测量误差，Q₀、R₀、P₀均为对角矩阵；

进行常规卡尔曼滤波更新。

进一步的，滤波递推次数k的更新阈值为900。

进一步的，当深海潜器处于巡航模式时，包括如下步骤：

(1)在传统卡尔曼滤波基础上加入R矩阵的迭代更新过程实现对环境噪声的跟踪能力，R的具体更新表达式为：

(2)在R每次更新时对R的正定性进行判定，当

与同阶单位矩阵合同时，即

满足正定性条件时，表达式保持不变，依旧如下：

当

不满足与同阶单位矩阵合同条件时，则将R中负项去除如下：

(3)巡航模式下，为选择在滤波次数k达到阈值时，令d_k中k＝0，重新开始自适应过程；

(4)对滤波过程中状态误差协方差矩阵P和量测噪声协方差矩阵R的上下界进行限定；

(5)对矩阵P的二范数进行监测，并将其作为判定滤波最终收敛的必要条件。

进一步的，在步骤(1)之前还包括如下步骤：

(1)建立状态向量X、量测向量Z，并根据已知的器件误差和设定的姿态、速度、位置误差初始化系统噪声协方差矩阵Q₀、量测噪声协方差矩阵R₀和状态误差协方差矩阵P₀；所述的状态向量X(不考虑天向通道)为：

X＝[δv_x δv_y φ_x φ_y φ_z δL δλ ▽_x ▽_y ε_x ε_y ε_z δv_d δΔ δc]^T

其中，δv_x、δv_y表示东向、北向速度误差，φ_x、φ_y、φ_z表示俯仰、横滚、航向角误差，δL、δλ表示位置误差，▽_x、▽_y表示加速度计误差，ε_x、ε_y、ε_z表示陀螺随机漂移误差，δv_d表示多普勒计程仪的速度偏移误差，δΔ表示偏流角误差，δc表示标度因数误差；

量测向量Z为：

其中v_x表示真实东向速度，v_dx表示多普勒计程仪东向速度，v_x-v_dx表示东向速度测量误差，v_y-v_dy表示北向速度测量误差，Q₀、R₀、P₀均为对角矩阵；

进行常规卡尔曼滤波更新。

进一步的，滤波递推次数k的更新阈值为500。

进一步的，下界值取初值的0.01倍，上界取初值的10倍。

与现有技术相比，本发明具有如下优点和有益效果：

本发明可根据双功能深海潜器不同模式的实际导航需求进行滤波方式的调整，在增强滤波稳定性、有效避免滤波发散的同时保留其解算精度；还使得滤波更加重视当前的观测信息同时减弱历史观测信息对滤波的影响，增加了对量测噪声变化的跟踪能力；有助于保持滤波的自适应度和鲁棒性，能有效应对水下复杂多变的环境干扰，实现水下高精度导航定位。

附图说明

图1为适用于双功能深海无人潜器导航系统的自适应滤波流程示意图。

具体实施方式

以下将结合具体实施例对本发明提供的技术方案进行详细说明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。

本发明提供了一种用于双功能深海无人潜器导航系统的自适应滤波导航方法。深海潜器具备滑翔和巡航两种工作模式，两种模式分别配备不同的导航传感器以满足不同模式下的工作需求。针对跨海域长途迁徙的工作任务，系统处于滑翔模式。滑翔模式下，其工作任务为跨海域迁徙，无探测任务，此时系统要求的导航精度较低，启用传感器件MEMS-IMU和磁力计完成导航解算，此时导航精度较低，系统的能耗很低。传感器具体为ADI公司推出的一款工业级IMU单元ADIS16405，它采用iSensor技术与混合信号处理技术，集成了完整的三轴陀螺仪、三轴加速度计和三轴磁力计，工作时功耗较低，是高集成度的解决方案。针对高精度的搜寻和探测任务，系统切换到巡航模式，巡航模式下，工作任务为探测与搜寻，此时精度要求高，系统启用FOG-IMU进行惯性导航解算，结合多普勒测速仪(DVL)和深度计进行辅助，完成高精度的导航解算，此时导航定位精度较高。

经过研究发现，在深海潜器导航系统中，系统噪声主要由惯性器件随机误差引起，且一般在工作以前已经经过了建模补偿，而量测噪声主要是由水下外界环境引起的，此类噪声具有较大的不确定性，无法建模补偿，综合考虑下本发明在传统卡尔曼滤波基础上只加入对量测噪声方差阵的估计。

基础的卡尔曼滤波公式如下：

P_k＝(I-K_kH_k)P_k/k-1

公式含义及相关变量定义可参见《GNSS与惯性及多传感器组合导航系统原理》P79。

据此，本发明提出了一种自适应滤波导航方法，如图1所示，包括如下两方面：

1、当深海潜器处于滑翔模式时，自适应滤波导航方法包括以下步骤：

(1)建立状态向量X、量测向量Z，并根据已知的器件误差和设定的姿态、速度、位置误差初始化系统噪声协方差矩阵Q₀、量测噪声协方差矩阵R₀和状态误差协方差矩阵P₀。所述的状态向量X(不考虑天向通道)为：

X＝[δv_x δv_y φ_x φ_y φ_z δL δλ ▽_x ▽_y ε_x ε_y ε_z]^T

其中，δv_x、δv_y表示东向、北向速度误差，φ_x、φ_y、φ_z表示俯仰、横滚、航向角误差，δL、δλ表示位置误差，▽_x、▽_y表示加速度计误差，ε_x、ε_y、ε_z表示陀螺随机漂移误差。

量测向量Z为单维向量，Z＝δφ_z表示航向角测量误差，Q₀、R₀、P₀均为对角矩阵，设置方法可参考《GNSS与惯性及多传感器组合导航系统原理》P77-80。

(2)常规卡尔曼滤波更新。具体更新过程可参考《GNSS与惯性及多传感器组合导航系统原理》P79-82。

(3)在传统卡尔曼滤波基础上加入R矩阵的迭代更新过程实现对环境噪声的跟踪能力，R的具体更新表达式为：

其中自适应系数d_k-1＝(1-b)/(1-b^k-1)，b为遗忘因子，一般取值为0.95～0.99之间。

(4)此时导航系统对稳定性要求极高，R矩阵的正定性是滤波稳定的必要条件，为保证R矩阵的正定性，此处进一步将R表达式中负项去除，即：

(5)由严格数学计算可知，当滤波递推次数k达到约900次后，自适应系数d_k逐渐收敛为定值1-b，这就意味着滤波对新近量测信息置信程度较低，原有信息具有较大占比，此时滤波的自适应程度迅速降低，效果较差，不符合预期滤波效果，因此需要对自适应系数进行阶段性更新重置避免其趋于定值，即当滤波递推次数k达到900时，令d_k中k＝0，重新开始自适应过程。

2、当深海潜器处于巡航模式时，自适应滤波导航方法包括以下步骤：

(1)建立状态向量X、量测向量Z，并根据已知的器件误差和设定的姿态、速度、位置误差初始化系统噪声协方差矩阵Q₀、量测噪声协方差矩阵R₀和状态误差协方差矩阵P₀。所述的状态向量X(不考虑天向通道)为：

X＝[δv_x δv_y φ_x φ_y φ_z δL δλ ▽_x ▽_y ε_x ε_y ε_z δv_d δΔ δc]^T

其中，δv_x、δv_y表示东向、北向速度误差，φ_x、φ_y、φ_z表示俯仰、横滚、航向角误差，δL、δλ表示位置误差，▽_x、▽_y表示加速度计误差，ε_x、ε_y、ε_z表示陀螺随机漂移误差，δv_d表示多普勒计程仪的速度偏移误差，δΔ表示偏流角误差，δc表示标度因数误差。

量测向量Z为：

其中v_x表示真实东向速度，v_dx表示多普勒计程仪东向速度，v_x-v_dx表示东向速度测量误差，同理，v_y-v_dy表示北向速度测量误差。Q₀、R₀、P₀均为对角矩阵，设置方法同样参考《GNSS与惯性及多传感器组合导航系统原理》P77-80。

(2)常规卡尔曼滤波更新。具体更新过程可参考《GNSS与惯性及多传感器组合导航系统原理》P79-82。

(3)同样在传统卡尔曼滤波基础上加入R矩阵的迭代更新过程实现对环境噪声的跟踪能力，R的具体更新表达式为：

(4)进一步地，在R每次更新时对R的正定性进行判定，当

与同阶单位矩阵合同时，即

满足正定性条件时，表达式保持不变，依旧如下：

当

不满足与同阶单位矩阵合同条件时，则将R中负项去除如下：

(5)巡航模式下，为增强滤波对量测信息的跟踪能力，进一步提升滤波自适应能力和导航精度，选择在滤波次数k达到500时，令d_k中k＝0，重新开始自适应过程。

(6)进一步地，对滤波过程中状态误差协方差矩阵P和量测噪声协方差矩阵R的上下界进行限定，临界值取决于实际场景，一般下界值取初值的0.01倍，上界取初值的10倍，此举可有效加快滤波的收敛过程，提升滤波精度。

(7)对矩阵P的二范数进行监测，并将其作为判定滤波最终收敛的必要条件。滤波的收敛过程是P矩阵二范数不断减小的过程，一般小于某个阈值可认定滤波收敛完毕，阈值往往是一个小值，可在滤波调试过程中凭经验选取。

本发明方案所公开的技术手段不仅限于上述实施方式所公开的技术手段，还包括由以上技术特征任意组合所组成的技术方案。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。

Claims (6)

1.一种用于双功能深海无人潜器导航系统的自适应滤波导航方法，具备滑翔和巡航两种工作模式，其特征在于：在不同工作模式情况下进行不同的调整来满足对应的导航需求，当深海潜器处于低精度滑翔模式时，在传统卡尔曼滤波的基础上加入对量测噪声方差阵R进行自适应更新的过程，将R表达式中负项去除，在滤波自适应过程中对量测噪声方差阵R表达式中自适应系数进行阶段性更新重置；当深海潜器处于高精度巡航模式时，只对量测噪声方差阵R进行迭代更新，在R更新过程中加入对R矩阵的正定性在线判定，并根据判定结果修正R的表达式；在滤波自适应时对量测噪声方差阵R进行自适应系数更新，并在滤波全过程中加入对状态误差协方差矩阵P和量测噪声协方差矩阵R的上下界限定，将P矩阵的二范数作为判定滤波收敛的必要条件；

当深海潜器处于滑翔模式时，包括如下步骤：

(1)在传统卡尔曼滤波基础上加入R矩阵的迭代更新过程实现对环境噪声的跟踪能力，R的具体更新表达式为：

其中自适应系数d_k-1＝(1-b)(1-b^k-1)，b为遗忘因子；

(2)此时导航系统对稳定性要求极高，R矩阵的正定性是滤波稳定的必要条件，为保证R矩阵的正定性，此处进一步将R表达式中负项去除，即：

当滤波递推次数k达到阈值时，令d_k中k＝0，重新开始自适应过程；

当深海潜器处于巡航模式时，包括如下步骤：

(1)在传统卡尔曼滤波基础上加入R矩阵的迭代更新过程实现对环境噪声的跟踪能力，R的具体更新表达式为：

(2)在R每次更新时对R的正定性进行判定，当

与同阶单位矩阵合同时，即

满足正定性条件时，表达式保持不变，依旧如下：

当

不满足与同阶单位矩阵合同条件时，则将R中负项去除如下：

(3)巡航模式下，为选择在滤波次数k达到阈值时，令d_k中k＝0，重新开始自适应过程；

(4)对滤波过程中状态误差协方差矩阵P和量测噪声协方差矩阵R的上下界进行限定；

(5)对矩阵P的二范数进行监测，并将其作为判定滤波最终收敛的必要条件。

2.根据权利要求1所述的用于双功能深海无人潜器导航系统的自适应滤波导航方法，其特征在于，当深海潜器处于滑翔模式时，在步骤(1)之前还包括如下步骤：

建立状态向量X、量测向量Z，并根据已知的器件误差和设定的姿态、速度、位置误差初始化系统噪声协方差矩阵Q₀、量测噪声协方差矩阵R₀和状态误差协方差矩阵P₀；所述的状态向量X为：

其中，δv_x、δv_y表示东向、北向速度误差，φ_x、φ_y、φ_z表示俯仰、横滚、航向角误差，δL、δλ表示位置误差，

表示加速度计误差，ε_x、ε_y、ε_z表示陀螺随机漂移误差，量测向量Z为单维向量，Z＝δφ_z表示航向角测量误差，Q₀、R₀、P₀均为对角矩阵；

进行常规卡尔曼滤波更新。

3.根据权利要求1或2所述的用于双功能深海无人潜器导航系统的自适应滤波导航方法，其特征在于，当深海潜器处于滑翔模式时，滤波递推次数k的更新阈值为900。

4.根据权利要求1所述的用于双功能深海无人潜器导航系统的自适应滤波导航方法，其特征在于，当深海潜器处于巡航模式时，在步骤(1)之前还包括如下步骤：

(1)建立状态向量X、量测向量Z，并根据已知的器件误差和设定的姿态、速度、位置误差初始化系统噪声协方差矩阵Q₀、量测噪声协方差矩阵R₀和状态误差协方差矩阵P₀；不考虑天向通道，所述的状态向量X为：

其中，δv_x、δv_y表示东向、北向速度误差，φ_x、φ_y、φ_z表示俯仰、横滚、航向角误差，δL、δλ表示位置误差，

表示加速度计误差，ε_x、ε_y、ε_z表示陀螺随机漂移误差，δv_d表示多普勒计程仪的速度偏移误差，δΔ表示偏流角误差，δc表示标度因数误差；

量测向量Z为：

其中v_x表示真实东向速度，v_dx表示多普勒计程仪东向速度，v_x-v_dx表示东向速度测量误差，v_y-v_dy表示北向速度测量误差，Q₀、R₀、P₀均为对角矩阵；

进行常规卡尔曼滤波更新。

5.根据权利要求1或4所述的用于双功能深海无人潜器导航系统的自适应滤波导航方法，其特征在于，当深海潜器处于巡航模式时，滤波递推次数k的更新阈值为500。

6.根据权利要求1或4所述的用于双功能深海无人潜器导航系统的自适应滤波导航方法，其特征在于，当深海潜器处于巡航模式时，下界值取初值的0.01倍，上界取初值的10倍。

Images