基于DR/USBL的深潜载人潜水器组合导航方法与流程

本发明涉及高精度水下导航定位技术领域，尤其涉及一种基于dr/usbl的深潜载人潜水器组合导航方法，特别适用于水下载体的近海底导航。

背景技术：

载人潜水器能够携带观察人员和专用仪表设备快速精确地到达深海复杂环境中，是进入深海进行科学研究、调查作业不可或缺的重要运载工具。与无人潜水器相比，载人潜水器具有可以使人亲临现场，并与周围环境进行交互的优点，因此被称为海洋学研究领域的重要基石。载人潜水器作为一种水下的直接观测平台，能够充分发挥人的主观能动性去研究生物、化学和物理海洋以及实施水下作业。

导航定位技术是水下潜器进行海洋探索的关键技术之一，决定其能否准确抵达作业地点，顺利进行作业任务并安全返回。事实上，高精度的导航定位对于增强载人潜水器的安全性、提高水下作业效率以及充分挖掘采集样本和收集数据的科学价值至关重要。尽管水下导航技术的发展使得现代载人潜水器的水下作业得以成功实施，然而高精度的导航定位仍然是载人潜水器的一个重大挑战。

在近海底，可以利用多普勒测速仪器(dopplervelocitylog，dvl)辅助捷联惯性导航系统(strapdowninertialnavigationsystem，sins)构成sins/dvl组合导航。然而考虑到sins价格过于昂贵以及技术问题，目前深潜载人潜水器的水下导航定位主要依赖超短基线(ultrashortbaseline，usbl)和长基线(longbaseline，lbl)水声定位技术，例如蛟龙号。lbl由于水下布阵和回收时间长、声信号易被遮挡等缺陷，在实际应用中受到诸多限制。与lbl相比，usbl换能器阵安装在船底而不受相应的制约，且使用方便。因此，usbl成为深潜载人潜水器的主要定位工具。通常，船载usbl每隔8秒对大深度载人潜水器进行一次定位，然后每隔64秒进行一次水声数字通信，数据更新率低。此外，这种单一的导航定位方式存在一定的安全隐患。一旦usbl出现故障，潜水器将不得不返航。

针对深潜载人潜水器导航定位系统存在定位方式单一、容错性差、数据更新率低等问题和不足，以及其水下作业安全、作业效率、样品分析和数据处理等方面对高精度导航定位的迫切需求，构建一套经济可行的近海底组合导航系统，实现高精度高可靠性的水下导航至关重要。

技术实现要素：

发明目的：针对我国现有深潜载人潜水器导航定位系统存在的问题和不足，本发明提出一种基于dr/usbl的深潜载人潜水器组合导航方法，实现深潜载人潜水器在近海底作业过程中高精度导航定位。

技术方案：为实现本发明的目的，本发明所采用的技术方案是：一种深潜载人潜水器近海底组合导航方法，包括以下步骤：

(1)利用载人潜水器上的姿态传感器提供的姿态角信息和多普勒计程仪提供的对地速度信息实时递推计算得到位置信息，从而构成一套自主的航位推算系统(dr)；

(2)利用安装在水面支持母船底部的超短基线(usbl)基阵对载人潜水器进行定位，设计在线数据清洗方案来处理usbl数据中的野值和缺失值，以获取连续一致的位置数据；

(3)设计kalman滤波器，有效地融合步骤1中的航位推算系统(dr)和步骤2中的超短基线水声定位系统(usbl)，构造dr/usbl组合导航系统，从而获得定位误差不随时间累积的近海底组合导航结果。

进一步，步骤1所述航位推算算法如下：

s1.1：选取当地地理坐标系作为导航坐标系，分别指向北、东、地，而载体坐标系则分别指向载体的前、右、下；记导航坐标系为n，载体坐标系为b，得到n系下航位推算的位置微分方程：

其中，vn和ve分别为东向速度和北向速度，可由航向角对b系速度分解得到；h为载体的海拔高度，由深度计精确提供；rn和re分别为子午圈曲率半径和卯酉圈曲率半径，都是关于纬度l的函数；λ为经度。

s1.2：给定初始时刻的位置，通过对位置微分方程进行积分运算，不断的递推得到当前时刻位置。

进一步，步骤2所述利用usbl对载人潜水器进行定位的方法如下：采用高精度的同步时钟来同步触发应答器和水面系统，通过计算同步脉冲触发时刻到收到应答信号的时间差计算距离，完成水面支持母船对潜水器的定位。其定位结果为基阵坐标系下的坐标，结合高精度船载平台罗经和姿态仪以及dgps进行坐标变换，得到潜水器在大地坐标系中的位置。

进一步，步骤2所述设计在线数据清洗方案来处理usbl数据中的野值和缺失值方法如下：

s2.2.1：初始化训练样本参数(n,m)和osvr模型参数(ε,c,σ)；

s2.2.2：构造滑动时间窗训练样本集，并对支持向量机回归模型进行训练；

考虑当前时刻的测量值xk+1(可能不存在)和滑动时间窗将时间序列转换为自相关的数据集，从而可以得到训练样本集

其中，输入为输出为n表示滑动时间窗宽度，m表示嵌入维数；

利用训练样本集对支持向量机回归模型进行训练，可以获取其回归函数：

其中，b通过kkt条件计算得到，αi,表示拉格朗日乘子；k(x,xi)表示核函数；

s2.2.3：利用s2.2.2获取的支持向量机回归模型，给出当前时刻的预测值：

其中，xn-m+1＝{xk-m+1,xk-m+2,…,xk}；

s2.2.4：结合步骤2.2.3获取的当前时刻的预测值和预先设定的阈值δ执行数据清洗：1)假如当前时刻不存在测量值，将其归类为缺失值，并直接用预测值补充；2)假如当前时刻存在测量值，若则将其归类为正常值，并保留正常值，否则将其归类为野值，并用预测值替代；

s2.2.5：依次递推下去，重复步骤2.2.2、2.2.3和2.2.4里面的操作。

进一步地，所述步骤3中利用kalman滤波技术实现dr/usbl组合导航的方法如下：

s3.1：选取dvl刻度因子误差、航向角误差、纬度误差和经度误差作为状态变量，选取位置误差作为量测变量，获得离散化的kalman滤波状态方程和量测方程；

s3.2：给定状态估计和估计误差方差的初始值和p0，根据k时刻的观测值zk，实时递推计算得到k时刻的状态估计

s3.3：利用步骤3.2中得到的状态估计对dr输出的导航参数进行修正。

进一步地，所述步骤s3.1包括：

s3.1.1：选取dvl刻度因子误差、航向角误差、纬度误差和经度误差作为状态变量，状态变量x表示如下：

其中，δk表示dvl刻度因子误差，表示航向角误差，δl表示纬度误差，δλ表示经度误差，则dr/usbl组合导航状态方程为：

式中，为罗经测量的随机误差，状态转移矩阵f为：

s3.1.2：选取航位推算计算得到的经纬度与usbl水声定位系统输出的经纬度之差为系统的量测量，则dr/usbl系统的量测方程为：

式中，v代表usbl水声定位系统的量测噪声；

s3.1.3：根据dr误差传播方程和获取的经纬度信息，得到离散化的卡尔曼滤波状态方程和量测方程：

xk＝φk,k-1xk-1+wk-1

zk＝hkxk+vk

其中，xk为k时刻的状态估计，xk-1为k-1时刻的状态估计，zk为k时刻的观测值，φk,k-1为非奇异状态一步转移矩阵，hk为观测矩阵，wk-1是系统随机过程噪声序列，vk是系统随机量测噪声序列；

s3.1.4：系统过程噪声和观测噪声的统计特性，设定如下：

其中，qk是系统过程噪声wk的方差矩阵，rk是系统量测噪声vk的方差矩阵，k和j表示时间序列中的两个不同时刻，δkj是kronecker-δ函数。

进一步地，所述步骤s3.2具体求解过程如下：

状态一步预测：

状态估计：

滤波增益矩阵：

一步预测误差方差矩阵：

估计误差方差矩阵：pk＝[i-kkhk]pk,k-1；

其中，i表示单位矩阵。

进一步地，所述步骤s3.3中使用的位置校正公式如下：

其中，和表示dr输出结果，δl和δλ表示kalman滤波估计结果，l和λ表示dr/usbl组合导航输出结果。

有益效果：与现有技术相比，本发明的技术方案具有以下有益的技术效果：

1、本发明提出了一种基于dr/usbl的深潜载人潜水器组合导航方法，该方法仅仅只需利用多普勒计程仪(dvl)、姿态传感器和usbl就能实现定位误差不随时间累积的高精度近海底导航，方法简便快捷、经济实惠。

2、本发明提出的导航算法能够实时有效地处理usbl定位数据中的野值和缺失值问题，大大提高了其可靠性。

3、本发明使用usbl作为量测时，对其进行了数据清洗，大大提高了观测信息的质量，采用简单的kalman滤波器就能实现组合导航信息融合。

附图说明

图1为根据本发明实施例的dr/usbl组合导航框架图；

图2为根据本发明实施例的usbl定位示意图。

具体实施方式

下面结合附图对本发明的技术方案作进一步说明。

本发明在不考虑捷联惯性导航系统(sins)的条件下，提出了一种经济可行的深潜载人潜水器近海底组合导航方案。利用载人潜水器搭载的多普勒计程仪(dvl)和姿态传感器构成航位推算系统(dr)；通过构建滑动时间窗样本在线训练支持向量机回归模型，利用训练好的在线支持向量机(osvr)模型实时预测当前时刻超短基线(usbl)数据，进一步将当前时刻usbl数据预测值去执行野值检测、野值替代或缺失值补充，实现usbl数据实时预处理以获取连续一致的定位结果；通过kalman滤波技术，将数据清洗过后的usbl与dr输出的位置信息进行信息融合，构成位置匹配组合导航以抑制dr定位误差发散，从而实现深潜载人潜水器高精度、高可靠性的近海底导航。如图1所示，本发明所述的一种基于dr/usbl的深潜载人潜水器组合导航方法，包括以下步骤：

步骤1：利用载人潜水器上的姿态传感器提供的姿态角信息和多普勒计程仪提供的对地速度信息实时递推计算得到位置信息，从而构成一套自主的航位推算系统(dr)。

载人潜水器上的三轴姿态传感器一般是捷联罗经，或者是磁罗盘+倾角仪组合。考虑到高度或深度信息可以由深度计精确给出，在设计航位推算系统时，可以不考虑高度通道，此时只需要航向角和水平速度信息。步骤1所述导航解算算法如下：

步骤1.1：选取当地地理坐标系作为导航坐标系，分别指向北、东、地，而载体坐标系则分别指向载体的前、右、下；记导航坐标系为n，载体坐标系为b，得到n系下航位推算的位置微分方程为：

其中，vn和ve分别为东向速度和北向速度，可由航向角对b系速度分解得到；h为载体的海拔高度，由深度计精确提供；rn和re分别为子午圈曲率半径和卯酉圈曲率半径，都是关于纬度l的函数。

所述vn、ve、rn和re表达式如下：

其中，和分别为载体前向速度和右向速度，为航向角，r为地球参考椭球体长半轴，e为椭球偏心率。

步骤1.2：给定初始时刻的经纬度，通过对位置微分方程积分运算，不断的递推得到当前时刻位置。

由于姿态传感器提供的航向角信息和dvl提供的速度信息存在误差，从而使得基于积分运算获得的航位推算系统(dr)的导航参数误差随时间累积。

步骤2：利用安装在水面支持母船底部的超短基线(usbl)基阵对载人潜水器进行定位，设计在线数据清洗方案来处理usbl源数据中的野值和缺失值，以获取连续一致的位置数据。

步骤2.1：利用usbl对载人潜水器进行定位。

图2为usbl定位示意图。如图2所示，usbl水声定位系统包含三个组成部分：大地坐标参考单元、船坐标参考单元和水声定位单元。超短基线声基阵安装在水面支持母船的底部，由发射换能器和接收水听器组成。应答器安装在潜水器的背部，其半球形指向性可覆盖整个上半空间，保证在水下各种深度和倾角状态下定位系统能够正常工作。采用高精度的同步时钟来同步触发应答器和水面系统，通过计算同步脉冲触发时刻到收到应答信号的时间差计算距离，完成水面支持母船对潜水器的定位。其定位结果为基阵坐标系下的坐标，需要结合高精度船载平台罗经和姿态仪以及dgps进行坐标变换，得到潜水器在大地坐标系中的位置。在此，usbl输出的结果为载人潜水器的经、纬度信息。

由于水声通信的多径效应、海洋环境噪声干扰等影响，usbl输出数据中包含大量的野值和缺失值，无法直接用于组合导航，需要进行在线数据清洗。

步骤2.2：通过构建滑动时间窗样本在线训练支持向量机回归模型，利用训练好的在线支持向量机(osvr)模型实时预测当前时刻超短基线(usbl)数据，进一步将当前时刻usbl数据预测值去执行野值检测、野值替代或缺失值补充。

具体算法流程可以表示为：

步骤2.2.1：初始化训练样本参数(n,m)和osvr模型参数(ε,c,σ)，其中ε为损失参数，c为惩罚因子,σ为高斯径向基核函数参数。

步骤2.2.2：构造滑动时间窗训练样本集，并对支持向量机回归模型进行训练。

考虑当前时刻的测量值xk+1(可能不存在)和滑动时间窗将时间序列转换为自相关的数据集，从而可以得到训练样本集

其中，输入为输出为n表示滑动时间窗宽度，m表示嵌入维数。

利用训练样本集对支持向量机回归模型进行训练，可以获取其回归函数：

其中，参数b可以通过karush-kuhn-tucker(kkt)条件计算得到，拉格朗日乘子αi,可以通过求解以下对偶优化问题得到：

其中，k(xi,xj)表示核函数，常用的核函数包括常用的核函数包括线性核函数、多项式核函数、rbf核函数、sigmoid核函数等。其中rbf核函数具有调节参数少、计算简单易实现且具有较好的全局逼近性，而获得了广泛应用。其具体形式如下：

步骤2.2.3：利用步骤2.2.2获取的支持向量机回归模型，给出当前时刻的预测值。

由公式(6)可计算得到当前时刻预测值：

其中，xn-m+1＝{xk-m+1,xk-m+2,…,xk}。

步骤2.2.4：结合步骤2.2.3获取的当前时刻的预测值和预先设定的阈值执行数据清洗：1)假如当前时刻不存在测量值，将其归类为缺失值，并直接用预测值补充；2)假如当前时刻存在测量值，若则将其归类为正常值，并保留正常值，否则将其归类为野值，并用预测值替代。

步骤2.2.5：依次递推下去，重复步骤2.2.2、2.2.3和2.2.4里面的操作。

步骤3：设计kalman滤波器，有效地融合步骤1中的航位推算系统(dr)和步骤2中的超短基线水声定位系统(usbl)，构造dr/usbl组合导航系统，从而获得定位误差不随时间累积的近海底组合导航结果。

具体方法如下：

步骤3.1：选取dvl刻度因子误差、航向角误差、纬度误差和经度误差作为状态变量，选取位置误差作为量测变量，获得离散化的kalman滤波状态方程和量测方程。

选取dvl刻度因子误差、航向角误差、纬度误差和经度误差作为状态变量，状态变量x表示如下：

其中，δk表示dvl刻度因子误差，表示航向角误差，δl表示纬度误差，δλ表示经度误差。则dr/usbl组合导航状态方程为：

式中，为罗经测量的随机误差，其噪声方差阵q根据罗经的噪声水平选取。状态转移矩阵f为：

选取航位推算计算得到的经纬度与usbl水声定位系统输出的经纬度之差为系统的量测量，则dr/usbl系统的量测方程为：

式中，v代表usbl水声定位系统的量测噪声。

根据dr误差传播方程和获取的经纬度信息，得到离散化的卡尔曼滤波状态方程和量测方程：

其中，xk为k时刻的状态估计，xk-1为k-1时刻的状态估计，zk为k时刻的观测值，φk,k-1为非奇异状态一步转移矩阵，hk为观测矩阵，wk-1是系统随机过程噪声序列，vk是系统随机量测噪声序列；

系统过程噪声和观测噪声的统计特性，设定如下：

其中，qk是系统过程噪声wk的方差矩阵，rk是系统量测噪声vk的方差矩阵，k和j表示时间序列中的两个不同时刻，δkj是kronecker-δ函数。

步骤3.2：给定状态估计和估计误差方差的初始值和p0，根据k时刻的观测值zk，实时递推计算得到k时刻的状态估计具体求解如下：

状态一步预测：

状态估计：

滤波增益矩阵：

一步预测误差方差矩阵：

估计误差方差矩阵：pk＝[i-kkhk]pk,k-1；

其中，i表示单位矩阵。

步骤3.3：利用步骤3.2中得到的状态估计对dr输出的导航参数进行修正。

位置校正公式如下：

其中，和表示dr输出结果，δl和δλ表示kalman滤波估计结果，l和λ表示dr/usbl组合导航输出结果。