CN109765594B - 一种深海潜水器导航后处理方法 - Google Patents

Abstract

本发明涉及一种深海潜水器导航后处理方法，包括以下步骤：第一步：获取AUV的速度信息、航向信息、GPS定位信息，使用最小二乘辨识方法对DVL安装角偏差、速度系数偏差进行辨识，并将辨识结果更新到自主导航方程中；第二步：获取USBL定位信息，并对定位信息进行解析和滤波；第三步：对AUV和USBL的信息进行融合与时间同步；第四步：查找USBL成功发射并接收的定位点，并记录对应的时间；返回第二步。本发明在不具备GPS信号的深海环境下可以获得潜水器真实的定位信息。加入了参数辨识环节，增加了***的可靠性。加入时间匹配环节，解决了USBL定位易丢失的问题。加入了定位深度判别环节，减小了错误定位信息的干扰。

Description

一种深海潜水器导航后处理方法

技术领域

本发明属于深海潜水器(AUV)导航定位技术领域，具体来说是深海潜水器超短基导航定位技术领域，是一种在深海环境下融合超短基线(USBL)、多普勒计程仪(DVL)、深度计等多传感器的导航后处理方法。

背景技术

USBL即超短基线定位***，由于安装简单、应用灵活、能够对水下移动目标进行实时定位等优点从而在自主水下机器人的定位和跟踪方面上得到了广泛的应用。但是USBL是水下声学定位设备，一般情况下，USBL定位周期为固定值，但因为水下环境因素经常会发生定位信号丢失导致周期不确定。而且由于水声通讯环境的复杂性，USBL定位野值较多，与传统GPS相比定位精度不高。但是由于深海中没有GPS定位的条件，因此解决USBL深海导航定位问题意义重大。

由于这种不稳定性而出现的定位错误，使用算术平均值滤波法、卡尔曼滤波法、递推平均值滤波法等方法是很难完成滤波的，因为这些野值是无规律的、随机的，而且还会连续出现，这给深海作业工程应用带来了困难。

发明内容

本发明方法的任务是解决上述超短基线导航定位存在的问题，在处理过程中加入了时间匹配环节，避免因超短基线定位信号丢失而带来的位置误差干扰。当USBL定位出现周期间断或定位偏差错误时，可以使用本方法实现深海潜水器的定位。

本发明解决其技术问题所采用的技术方案是：一种深海潜水器导航后处理方法，包括以下步骤：

第一步：获取AUV的速度信息、航向信息、GPS定位信息，使用最小二乘辨识方法对DVL安装角偏差、速度系数偏差进行辨识，并将辨识结果更新到自主导航方程中；

第二步：获取USBL定位信息，并对定位信息进行解析和滤波；

第三步：对AUV和USBL的信息进行融合与时间同步；

第四步：查找USBL成功发射并接收的定位点，并记录对应的时间；返回第二步，直到达到USBL定位结束为止。

所述第一步包括以下步骤：

定义导航系下东向速度为V_E，北向速度为V_N则，

V_E＝V_fcosβ+V_rsinβ

V_N＝V_fsinβ-V_rcosβ                (1)

DVL安装偏角为δ，速度系数为μ，弧度近似为sinδ≈δ、cosδ≈1-δ²/2，此时航向角为β＝β+δ，速度为V_f＝μV_f、V_r＝μV_r代入上式得，

化简为，

辨识过程中，λ_k、L_k为GPS测得的经度、纬度，V_f、V_r、β、T_auv分别为AUV的前向速度、右向速度、航向、导航周期，R_N为卯酉圈曲率半径、R_M为子午圈曲率半径、μ为DVL速度补偿系数，δ为DVL安装角偏差，ΔLon为GPS定位信号两两相邻经度差，ΔLat为GPS定位信号两两相邻纬度差；定义(4)式中，

带入(2)式得，

令上式中，

ζ＝[a₁ a₂]^T

ΔP＝[ΔLon ΔLat]^T

则由最小二乘原理得，

其中，L为GPS测得的当地纬度，

为ζ的最优估计，求得ζ即可得到a₁、a₂，进而得到辨识结果μ、δ。

所述第二步包括以下步骤：

若USBL的定位深度与深度计测得的深度相差大于γ，γ为最大深度定位误差的2-3倍；则过滤掉此次的USBL定位值。

所述第三步包括以下步骤：

DVL传感器周期为T₁，USBL传感器周期为T₂，将DVL的数据融合到USBL数据中，具体如下：

其中，

分别表示USBL在T₁、T₂时刻的水平位置量，V_t为潜水器在t时刻的航行速度，

为时间同步的补偿值，即补偿步长为ΔT＝T₂-T₁，λ^auv、L^auv表示时间同步得到的潜水器经纬度信息，

为k时刻的AUV的状态估计，其同步方程为，

其中，

ΔT期间的USBL定位信息由λ^auv、L^auv进行补偿，而由λ^auv、L^auv得到的位置信息作为推算位置，其计算如下，

V_E＝μV_fcos(β+δ)+μV_rsin(β+δ)

V_N＝μV_fsin(β+δ)-μV_rcos(β+δ)            (13)

其中，T_auv为导航周期，V_f、V_r为AUV的前向速度和右向速度，V_E、V_N为导航系的东向速度和北向速度；

将第一步所求得的参数μ、δ更新到上式(13)中，并带入式子(12)中。

所述第四步包括以下步骤：

首先，将此次的定位点的深度信息和深度计对应时间的深度信息进行比较；

若二者差的绝对值大于γ，γ为最大深度定位误差的2-3倍，将对应的USBL定位经纬度信息用对应时间的λ^auv、L^auv进行替换；

若二者差的绝对值小于γ，则将对应的USBL定位经纬度信息与第三步中对应时间的λ^auv、L^auv进行比较；若二者差的绝对值小于σ，σ为USBL定位误差的3-5倍，则接受此次的USBL定位，否则由对应时间的λ^auv、L^auv进行替换。

所述二者差的绝对值小于σ具体为：

当USBL定位信号有效时，二者的位置关系判别公式如下，

其中，λ^usbl、λ^auv分别为USBL的纬度和潜水器推算纬度，L^usbl、L^auv分别为USBL的经度和潜水器推算经度。

本发明具有以下有益效果及优点：

1.在不具备GPS信号的深海环境下可以获得潜水器真实的定位信息。

2.加入了参数辨识环节，增加了***的可靠性。

3.加入时间匹配环节，解决了USBL定位易丢失的问题。

4.加入了定位深度判别环节，减小了错误定位信息的干扰。

附图说明

图1是本方法深海潜水器所使用的传感器；

图2是深海潜水器导航定位图；

图3是本方法参数辨识流程图；

图4是本方法整个流程图。

具体实施方式

下面结合实施例对本发明做进一步的详细说明。

该方法主要由参数辨识、时间匹配、野值判断、滤波替换过程组成。具体实现步骤如下：

第一步：获取AUV的速度(前向及侧向速度)信息、航向信息、GPS定位信息，使用最小二乘辨识方法对DVL安装角偏差、速度系数偏差进行辨识，并将辨识结果更新到自主导航方程中。定义导航系下东向速度为V_E，北向速度为V_N则，

V_E＝V_fcosβ+V_rsinβ

V_N＝V_fsinβ-V_rcosβ      (1)

若DVL安装偏角为δ，速度系数为μ，其中δ一般小于5°。所以可以进行弧度近似为sinδ≈δ、cosδ≈1-δ²/2，此时航向角为β＝β+δ，速度为V_f＝μV_f、V_r＝μV_r代入上式得，

化简为，

辨识过程中，λ_k、L_k为GPS测得的经度、纬度，V_f、V_r、β、T_auv分别为AUV的前向速度、右向速度、航向、导航周期，R_N为卯酉圈曲率半径、R_M为子午圈曲率半径、μ为DVL速度补偿系数，δ为DVL安装角偏差，ΔLon、ΔLat为GPS定位信号两两相邻之差，ΔLon为GPS定位信号两两相邻经度差，ΔLat为GPS定位信号两两相邻纬度差。定义(4)式中，

带入(2)式得，

令上式中，

ζ＝[a₁ a₂]^T

ΔP＝[ΔLon ΔLat]^T

则由最小二乘原理得，

其中

为ζ的最优估计，求得ζ即可得到a₁、a₂，进而得到待求参数μ、δ。

第二步：获取USBL定位信息，并对定位信息进行解析和初步滤波。若USBL的定位深度与深度计测得的深度相差大于γ，γ为最大深度定位误差的2-3倍。则过滤掉此次的USBL定位值，此次缺失的定位值由时间同步补上。

第三步：传感器融合与时间同步。设DVL传感器周期为T₁，USBL传感器周期为T₂，将DVL的数据融合到USBL数据中，实现USBL的平滑滤波，其算法如下。

其中，U_T1、U_T2分别表示USBL在T₁、T₂时刻的水平位置量，V_t为潜水器在t时刻的航行速度，

为时间同步的补偿值，即补偿步长为ΔT＝T₂-T₁，设λ^auv、L^auv表示时间同步得到的潜水器经纬度信息，

为k时刻的AUV的状态估计，其同步方程为，

其中，

这里为了计算方便，ΔT期间的USBL定位信息由λ^auv、L^auv进行补偿，而由λ^auv、L^auv得到的位置信息作为推算位置，其计算如下，

V_E＝μV_fcos(β+δ)+μV_rsin(β+δ)

V_N＝μV_fsin(β+δ)-μV_rcos(β+δ)            (13)

其中，T_auv为推算周期，V_f、V_r为AUV的前向速度和右向速度，V_E、V_N为导航系的东向速度和北向速度，将第一步所求得的参数μ、δ更新到(13)式中，并带入式子(12)中。

第四步：查找USBL成功发射并接收的定位点，并记录对应的时间。首先，将此次的定位点的深度信息和深度计对应时间的深度信息进行比较，若二者差的绝对值为大于γ，γ为最大深度定位误差的2-3倍。将对应的USBL定位点应用对应时间的推算位置点进行替换；若二者差的绝对值小于γ，则将对应的USBL定位点与第三步中对应时间的推算水平位置进行比较，若二者差的绝对值小于σ，σ为USBL定位误差的3-5倍，则接受此次的USBL定位，否则由对应时间的推算位置点进行替换。当USBL定位信号有效时，二者的位置关系判别公式如下，

其中，λ^usbl、λ^auv分别为USBL的纬度和潜水器推算纬度，L^usbl、L^auv分别为USBL的经度和潜水器推算经度。

第五步:重复第二步到第四步。

本发明的深海潜水器导航***的要求是一套超短基线定位设备、一台GPS设备、一台多普勒测速仪、一台深度计、一台电子罗盘，如图1所示。

第一步：获取深海潜水器的导航信息。应用深海潜水器的速度(前向及侧向速度)信息、航向信息、GPS定位信息对多普勒测速仪参数进行辨识。参数辨识过程中，AUV在水面沿直线航行，时间尽量长一些。例如此过程AUV沿着45°航向角在水面航行，测试段位移大于3000m。AUV全程记录速度信息、航向变化信息及GPS定位信息，深海潜水器导航定位关系如图2所示，整个参数辨识如图3所示。

第二步：获取USBL的定位信息。提取超短基线的输出数据，应用对应的协议对提取的数据进行解析，从而获USBL的初始定位信息。

第三步：提取深度计的深度信息，与USBL定位信息时间同步后，进行初始滤波。若USBL的定位深度与深度计测得的深度相差为大于γ，γ为最大深度定位误差的2-3倍。则滤波掉此次的USBL定位值，此次缺失的定位值由时间同步补上。

第四步：将第一步中参数辨识的结果更新到导航方程中，使用多普勒测速仪的速度信息和电子罗盘的航向信息进行位置推算。

第五步：将第三步未进行剔除的USBL定位信息与对应时间的推算位置进行比较，判断差值。若二者差的绝对值小于σ，σ为USBL定位误差的3-5倍，则接受此次的USBL定位，否则应用对应时间的推算位置进行替换。本方法整个滤波过程如图4所示。

Claims (2)

1.一种深海潜水器导航后处理方法，其特征在于包括以下步骤：

第一步：获取AUV的速度信息、航向信息、GPS定位信息，使用最小二乘辨识方法对DVL安装角偏差、速度系数偏差进行辨识，并将辨识结果更新到自主导航方程中；

第二步：获取USBL定位信息，并对定位信息进行解析和滤波；

第三步：对AUV和USBL的信息进行融合与时间同步；

第四步：查找USBL成功发射并接收的定位点，并记录对应的时间；返回第二步，直到达到USBL定位结束为止；

所述第一步包括以下步骤：

定义导航系下东向速度为V_E，北向速度为V_N则，

V_E＝V_fcosβ+V_rsinβ

V_N＝V_fsinβ-V_rcosβ                    (1)

DVL安装偏角为δ，速度系数为μ，弧度近似为sinδ≈δ、cosδ≈1-δ²/2，此时航向角为β＝β+δ，速度为V_f＝μV_f、V_r＝μV_r代入上式得，

化简为，

辨识过程中，λ_k、L_k为GPS测得的经度、纬度，V_f、V_r、β、T_auv分别为AUV的前向速度、右向速度、航向、导航周期，R_N为卯酉圈曲率半径、R_M为子午圈曲率半径，ΔLon为GPS定位信号两两相邻经度差，ΔLat为GPS定位信号两两相邻纬度差；定义(4)式中，

带入(2)式得，

令上式中，

ζ＝[a₁ a₂]^T

ΔP＝[ΔLon ΔLat]^T

则由最小二乘原理得，

其中，L为GPS测得的当地纬度，

为ζ的最优估计，求得ζ即可得到a₁、a₂，进而得到辨识结果μ、δ；

所述第二步包括以下步骤：

若USBL的定位深度与深度计测得的深度相差大于γ，γ为最大深度定位误差的2-3倍；则过滤掉此次的USBL定位值；

所述第三步包括以下步骤：

DVL传感器周期为T₁，USBL传感器周期为T₂，将DVL的数据融合到USBL数据中，具体如下：

其中，

分别表示USBL在T₁、T₂时刻的水平位置量，V_t为潜水器在t时刻的航行速度，

为时间同步的补偿值，即补偿步长为ΔT＝T₂-T₁，λ^auv、L^auv表示时间同步得到的潜水器经纬度信息，

为k时刻的AUV的状态估计，其同步方程为，

其中，

ΔT期间的USBL定位信息由λ^auv、L^auv进行补偿，而由λ^auv、L^auv得到的位置信息作为推算位置，其计算如下，

V_E＝μV_fcos(β+δ)+μV_rsin(β+δ)

V_N＝μV_fsin(β+δ)-μV_rcos(β+δ)               (13)

其中，T_auv为导航周期，V_f、V_r为AUV的前向速度和右向速度，V_E、V_N为导航系的东向速度和北向速度；

将第一步所求得的参数μ、δ更新到上式(13)中，并带入式子(12)中；

所述第四步包括以下步骤：

首先，将此次的定位点的深度信息和深度计对应时间的深度信息进行比较；

若二者差的绝对值大于γ，γ为最大深度定位误差的2-3倍，将对应的USBL定位经纬度信息用对应时间的λ^auv、L^auv进行替换；

若二者差的绝对值小于γ，则将对应的USBL定位经纬度信息与第三步中对应时间的λ^auv、L^auv进行比较；若二者差的绝对值小于σ，σ为USBL定位误差的3-5倍，则接受此次的USBL定位，否则由对应时间的λ^auv、L^auv进行替换。

2.根据权利要求1所述的一种深海潜水器导航后处理方法，其特征在于所述二者差的绝对值小于σ具体为：

当USBL定位信号有效时，二者的位置关系判别公式如下，

其中，λ^usbl、λ^auv分别为USBL的纬度和潜水器推算纬度，L^usbl、L^auv分别为USBL的经度和潜水器推算经度。

Images