CN109579850A - 基于对水速度辅助惯导的深水智能导航方法 - Google Patents

Abstract

本发明公开一种基于对水速度辅助惯导的深水智能导航方法，拟从声学信息预处理、对水速度辅助惯导***等方面采取措施，以解决声学测量、海洋环境带来的声学信息可信度差，以及AUV距海底过远无DVL对地速度辅助时的导航累积误差大等相关问题。具体包括(1)基于运动约束思想，引入舵鳍浆变化信息，对声学信息进行预处理以提高声学信息的可信度；(2)引入虚拟计程仪方案，设计无延迟HMM/KF滤波器，将DVL与INS的速度进行综合处理，以减少AUV颠簸摇摆、加减速、转弯带来的DVL速度误差和INS的长周期误差；(3)采用图优化算法，利用非线性最优化控制误差水平，并通过递归方式进行边缘化和最优化，实现实时性的构图以提高导航精度。

Description

基于对水速度辅助惯导的深水智能导航方法

技术领域

本发明涉及水下航行器智能导航领域，具体涉及一种基于对水速度辅助惯导的深水智能导航方法。

背景技术

海洋是人类发展的四大战略空间中继陆地之后的第二大空间，是能源、生物资源、金属资源、水资源的战略性开发基地，对经济社会的发展有着巨大的支撑作用。对海洋进行广泛而深入的探索开发已成为21世纪的发展主题之一，国家也将“关心海洋、认识海洋、经略海洋”提到前所未有的战略高度，深入开展了“两洋一海”、“透明海洋”、“健康海洋”等计划，构建了“21世纪海上丝绸之路”海洋环境安全保障体系。

作为人类探索开发海洋的重要助手，自主式水下航行器(Autonomous UnderwaterVehicle,AUV)在海洋开发中的作用不亚于人造卫星之于宇宙探索，拥有高性能水下作业能力的AUV已成为一个国家海洋竞争力的综合体现。AUV完成海洋科学研究、资源调查、应急搜救等预定任务离不开水下导航技术，导航定位信息的精度是决定AUV发展与应用的瓶颈问题。

在AUV水下导航阶段，常采用多普勒测速仪(Doppler Velocity Vehicle,DVL)提供速度信息辅助惯导***(Inertial Navigaion System,INS)导航，传感器技术成熟，体积、功耗和成本满足AUV的要求，且不同传感器信息之间取长补短，可提高导航精度。但由于海洋环境及海水介质的特殊性，声信号需要与外部环境发生信息交互，测量结果具有不可完全依赖性。

目前，实现DVL辅助INS的组合导航一般以DVL提供的对地速度作为外部观测信息，滤波算法估计INS的姿态、速度和位置误差，以开环或反馈的方式进行修正，最终获得较为准确的姿态、速度和位置导航信息。但深水AUV在执行任务的时候，难免工作于海洋上层和海洋中层，无法保证DVL发射的超声波束到达海底。受体积、重量和功耗限制，AUV无法装配大深度的DVL，AUV除了在距底1-300米的深海层作业外，将一直处于对流模式，仅能提供对水速度，无法提供对地速度。

发明内容

本发明提出一种基于对水速度辅助惯导的深水智能导航方法，拟从声学信息预处理、对水速度辅助惯导***等方面采取措施，以解决声学测量、海洋环境带来的声学信息可信度差，以及DVL对水速度辅助时的导航累积误差大等相关问题。

本发明是采用以下的技术方案实现的，一种基于对水速度辅助惯导的深水智能导航方法，包括以下步骤：

步骤A、基于运动约束与模型辅助的声学信息预处理，包括：

步骤A1、采集水下航行器AUV的导航相关传感器信息，所述传感器信息包括INS的速度、加速度、角度和位置信息，DVL的速度信息，舵鳍浆变化时间信息，以及GPS的定位信息；

步骤A2、以AUV当前运动速度、加速度以及舵鳍浆变化时间间隔为输入，以DVL下一时刻速度和INS下一时刻加速度信息为观测量，建立DVL速度主动跟踪模型，以抑制声学***测量噪声，并在DVL速度发生失效、跳变时提供模型输出；

步骤B、构建虚拟计程仪，对步骤A中基于DVL速度跟踪模型的输出进行优化处理，包括：

步骤B1、利用DVL对水速度和惯导速度的高精度部分，设计DVL与INS测速方程综合运算；

步骤B2、设计无延迟HMM/KF滤波器，滤除惯导速度和DVL速度相应频段的速度误差，输出优化后的对水速度；

步骤C、基于所获得的优化后的对水速度，实现自主导航定位。

进一步的，所述步骤A2中，所建立的DVL速度主动跟踪模型为：

其中，V为DVL的速度，a为INS输出的加速度，α为舵鳍浆变化时间的倒数，由AUV控制策略计算得到，当载体一直做匀速直线运动时，其机动时间常数为无穷大值；为惯导提供的当前更新周期内的平均加速度，此速度跟踪模型可抑制声学***测量噪声，检测声学辅助信息中的失效、跳变，在DVL发生失效、跳变时提供模型输出。

进一步的，所述步骤B1中，DVL与INS测速方程为：

其中，V_DVL和V_INS分别为DVL的对水速度和惯导的输出速度；V_current和V_AUV分别为洋流速度和AUV实际速度；δV_DVL(H)和δV_INS(L)分别为DVL的高频分量误差和惯导的低频分量误差，其中洋流速度V_current为一阶马尔科夫过程，相关时间为数小时，充分利用DVL对水速度和惯导速度的高精度部分，设计DVL与INS测速方程综合运算，得到初步的高精度对水速度。

进一步的，所述步骤B2通过以下方式实现：

构建惯导INS和DVL的速度综合处理HMM模型，由以下两个随机过程组成：

其中ξ_k，ν_k为需要滤除的高频段分量，***状态矩阵A和观测矩阵H满足：

分析HMM模型特性并构建简单的二维HMM/KF滤波模型，其差分方程如下：

将差分方程形式的滤波器转换成频率域的数字滤波形式：

进而得到HMM/KF滤波器频率响应的模、相位以及截止频率，实现滤除DVL和惯导各自的频域误差分量，得到优化后的高精度对水速度，实现虚拟计程仪的高精度功能，且虚拟计程仪综合考虑DVL和INS的速度，求出的对水速度精度高于INS的对地速度，且具有较高的频率，将虚拟计程仪所得输入导航***。

进一步的，所述步骤C中采用以下方式实现：AUV在水面时结合GPS定位信息进行位置校正，AUV在水下时，基于DVL的对水速度、INS的角度以及采样时间间隔，利用非线性最优化控制误差，并通过递归方式进行边缘化和最优化，实现实时性的构图以提高导航精度。

与现有技术相比，本发明的优点和积极效果在于：

本发明结合运动约束思想，使用主动跟踪方法，同时利用了AUV的舵鳍浆、加速度信息，为声学传感器测量值做了初步处理，滤除部分干扰噪声；通过构造虚拟计程仪，对DVL对水速度和惯导速度进行综合处理后使用无延迟滤波器滤除各自的干扰分量，输出优化的对水速度；避免了基于递归Bayes状态估计方法很难保证地图一致性和精度的缺陷，使用基于图优化的导航方法，利用非线性最优化控制误差水平，有效控制累积误差，实现实时性的构图并提高导航精度。

附图说明

图1为本发明实施例所述AUV主要传感器的安装示意图；

图2为本发明实施例所述导航方法的原理框图；

图3为本发明实施例HMM/KF预测更新流程图；

图4为图2中所示优化算法的原理示意图；

其中：1、超短基线换能器；2、全球定位***GPS；3、惯导***INS；4、多普勒测速仪DVL；5、深度计IPS。

具体实施方式

为了能够更加清楚地理解本发明的上述目的、特征和优点，下面结合附图及实施例对本发明做进一步说明。需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。

基于对水速度辅助惯导的深水智能导航方法，如图2所示，包括以下步骤：

步骤一、基于运动约束与模型辅助的声学信息预处理，具体包括：

(1)采集水下航行器AUV的导航相关传感器信息，图1为主要传感器的安装位置示意图，所述传感器信息包括惯导INS的速度、加速度、角度和位置信息，DVL的速度信息，舵鳍浆变化时间信息，以及GPS的定位信息；

(2)本方案引入运动约束思想，采用当前“统计模型”对DVL速度进行预处理，所述的运动约束思想来源于动力学模型：在下潜/潜航过程中，AUV一般总以确定的“预设轨迹缓慢”前行，这属于一种运动约束，当上述运动约束与跟踪思想应用于声学定位与速度信息的预处理时，AUV的速度同样具有“运动约束”特点，且具有主动性，具体的：

以AUV当前运动速度、加速度以及舵鳍浆变化时间间隔为输入，以DVL下一时刻速度和INS下一时刻加速度信息为观测量，建立DVL速度主动跟踪模型：

其中，V为DVL的速度；a为INS输出的加速度；α为舵鳍浆变化时间的倒数，由AUV控制策略计算得到，当载体一直做匀速直线运动时，其机动时间常数为无穷大值；为惯导提供的当前更新周期内的平均加速度，此速度跟踪模型可抑制声学***测量噪声，检测声学辅助信息中的失效、跳变，在DVL发生失效、跳变时提供模型输出，即DVL对水速度；

步骤二、对步骤一中基于DVL速度跟踪模型的输出进行优化处理：

(1)构建虚拟计程仪，充分利用DVL对水速度和惯导速度的高精度部分，设计DVL与INS测速方程综合运算：

拟采用DVL测得的对水速度，其测速误差可分为与AUV航速相关的常系数误差，与AUV转弯、回转机动相关的侧滑误差等。结合DVL的多普勒效应测速原理可知，其测速误差主要体现为高频分量，而INS的测速误差主要体现为八十多分钟的低频分量，DVL与INS测速方程为：

其中，V_DVL和V_INS分别为DVL的对水速度和惯导的输出速度；V_current和V_AUV分别为洋流速度和AUV实际速度；δV_DVL(H)和δV_INS(L)分别为DVL的高频分量误差和惯导的低频分量误差，其中洋流速度V_current为一阶马尔科夫过程，相关时间为数小时，充分利用DVL对水速度和惯导速度的高精度部分，设计DVL与INS测速方程综合运算，得到初步的高精度对水速度；

(2)设计无延迟HMM/KF滤波器，滤除惯导速度和DVL速度相应频段的速度误差，输出最优的对水速度；

经典的数字滤波器和滑动平均滤波器存在时间延迟，无法满足实时输出导航信息的要求，本方案设计一款无延迟的HMM/KF数字滤波器，其工作原理为：通过观测值序列与已知的HMM模型建立HMM/KF滤波方程，从当前观测值与上一时刻的预测值中求出所需频段内的最佳状态当前值；

构建惯导INS和DVL的速度综合处理HMM模型，主要由以下两个随机过程组成：

其中ξ_k，ν_k为需要滤除的高频段分量，***状态矩阵A和观测矩阵H满足：

如图3所示，分析HMM模型特性并构建简单的二维HMM/KF滤波模型，其差分方程如下：

将差分方程形式的滤波器转换成频率域的经典数字滤波形式如下：

分析上式得到HMM/KF滤波器频率响应的模、相位以及截止频率，继而可实现滤除DVL和惯导各自的频域误差分量，得到优化后的高精度DVL对水速度输出，实现虚拟计程仪的高精度功能。且虚拟计程仪综合考虑DVL和INS的速度，求出的对水速度精度高于INS的对地速度，且具有较高的频率，将虚拟计程仪所得输入导航***。

步骤三、基于获得的优化后的DVL对水速度，实现自主导航定位，如图4所示，本方案基于图优化方法，利用非线性最优化控制误差水平，有效控制累积误差：

AUV在水面时结合GPS位置进行位置校正，AUV在水下时，由于GPS无信号，仅使用DVL的对水速度、INS的角度以及采样时间间隔，利用非线性最优化控制误差，并通过递归方式进行边缘化和最优化，实现实时性的构图以提高导航精度：

根据上述所求的高精度对水速度、时间间隔、角度进行智能导航，首先采用信度网模型得到X、L、U、Z变量的联合概率密度如下：

其中X代表位姿点，L代表路标位置，U为控制量，Z为测量值，P(x₀)代表先验概率，P(x_i|x_i-1,u_i)为控制输入u_i条件下的运动概率，P(z_k|x_ik,l_jk)表示测量概率。将上式转换为等价的基于最大后验(MAP)估计的最小二乘法问题，并取负对数形式可得：

根据运动方程和测量方程的噪声项都是均值为零的正态分布噪声，AUV的运动概率模型和测量概率模型可分别写为：

其中f(·)和h(·)分别为无噪声项的运动和测量方程，其噪声项的均值都为0，Λ_i和Γ_k分别为运动方程和测量方程的噪声项的协方差，带入上述最小二乘方程可得

将上式一阶泰勒展开得到如下标准最小二乘表示：

上式将X和L两个列向量合成一个θ向量，A由运动方程在x点处的雅克比矩阵、测量方程在x点处的雅克比矩阵、测量方程在l点处的雅克比矩阵组成，b由状态预测误差和测量预测误差组成。求解最小二乘用下式：

Aθ＝b

考虑到AUV运行过程中需要实时计算，将A用吉文斯旋转可实现更为快捷地求得位置信息，更新和分解后的矩阵如下

根据上式，将最小二乘等式两边同乘Q^T得

其中令d的行数与R的行数相同，由于Q为正交矩阵，Q^TQ为单位阵，上式可简化得

Rθ＝d

由于R是上三角矩阵，在已知之前结果的前提下，可通过回代法一步求得对应的解，对于AUV实时求解非常有效。根据实际导航情况设置一个阈值，超过此阈值时进行变量重排，以确保矩阵的稀疏性并减小计算复杂度，通过Gauss-Newton法、Levenberg-Marquardt法或者Dog-Leg法进行非线性最优化后，重新计算整个解，实现实时性的构图以提高导航精度。

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作其它形式的限制，任何熟悉本专业的技术人员可能利用上述揭示的技术内容加以变更或改型为等同变化的等效实施例应用于其它领域，但是凡是未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与改型，仍属于本发明技术方案的保护范围。

Claims (5)

1.基于对水速度辅助惯导的深水智能导航方法，其特征在于，包括以下步骤：

步骤A、基于运动约束与模型辅助的声学信息预处理，包括：

步骤A1、采集水下航行器AUV的导航相关传感器信息，所述传感器信息包括INS的速度、加速度、角度和位置信息，DVL的速度信息，舵鳍浆变化时间信息，以及GPS的定位信息；

步骤A2、以AUV当前运动速度、加速度以及舵鳍浆变化时间间隔为输入，以DVL下一时刻速度和INS下一时刻加速度信息为观测量，建立DVL速度主动跟踪模型，以抑制声学***测量噪声，并在DVL速度发生失效、跳变时提供模型输出；

步骤B、构建虚拟计程仪，对步骤A中基于DVL速度跟踪模型的输出进行优化处理，包括：

步骤B1、利用DVL对水速度和惯导速度的高精度部分，设计DVL与INS测速方程综合运算；

步骤B2、设计无延迟HMM/KF滤波器，滤除惯导速度和DVL速度相应频段的速度误差，输出优化后的对水速度；

步骤C、基于所获得的优化后的对水速度，实现自主导航定位。

2.根据权利要求1所述的基于对水速度辅助惯导的深水智能导航方法，其特征在于：所述步骤A2中，所建立的DVL速度主动跟踪模型为：

其中，V为DVL的速度，a为INS输出的加速度，α为舵鳍浆变化时间的倒数；为惯导提供的当前更新周期内的平均加速度。

3.根据权利要求1或2所述的基于对水速度辅助惯导的深水智能导航方法，其特征在于：所述步骤B1中，DVL与INS测速方程为：

其中，V_DVL和V_INS分别为DVL的对水速度和惯导的输出速度；V_current和V_AUV分别为洋流速度和AUV实际速度；δV_DVL(H)和δV_INS(L)分别为DVL的高频分量误差和惯导的低频分量误差，其中洋流速度V_current为一阶马尔科夫过程。

4.根据权利要求3所述的基于对水速度辅助惯导的深水智能导航方法，其特征在于：所述步骤B2通过以下方式实现：

构建惯导INS和DVL的速度综合处理HMM模型，由以下两个随机过程组成：

其中ξ_k，ν_k为需要滤除的高频段分量，***状态矩阵A和观测矩阵H满足：

分析HMM模型特性并构建简单的二维HMM/KF滤波模型，其差分方程如下：

将差分方程形式的滤波器转换成频率域的数字滤波形式：

进而得到HMM/KF滤波器频率响应的模、相位以及截止频率，实现滤除DVL和惯导各自的频域误差分量，得到优化后的高精度对水速度。

5.根据权利要求4所述的基于对水速度辅助惯导的深水智能导航方法，其特征在于：所述步骤C中采用以下方式实现：

AUV在水面时结合GPS定位信息进行位置校正，AUV在水下时，则基于DVL的对水速度、INS的角度以及采样时间间隔，利用非线性最优化控制误差，并通过递归方式进行边缘化和最优化，实现实时性的构图以提高导航精度。