CN108303079B - 一种水下usbl反向应用的数据平滑方法 - Google Patents

Abstract

本发明涉及一种水下USBL反向应用的数据平滑方法，其技术特点在于：包括以下步骤：步骤1、通过对USBL基阵绝对位置的相对测量获得AUV的绝对位置；步骤2、利用步骤1中获得的AUV的绝对位置与SINS解算位置组合进行AUV组合导航Kalman滤波，计算得到SINS与USBL组合滤波优化后的AUV绝对位置；步骤3：根据步骤2得到的SINS与USBL组合滤波优化后的AUV绝对位置，对USBL相对定位位置回溯计算，得到平滑的USBL相对定位结果。本发明可将USBL的原始相对定位输出数据平滑输出，使得AUV控制系统获得更为理想的控制输入，有利于AUV在水下入舱坞及与水下基站对接的实现。

Description

一种水下USBL反向应用的数据平滑方法

技术领域

本发明属于水下导航定位技术领域，涉及USBL原始输出数据的平滑方法，尤其是一种水下USBL反向应用的数据平滑方法。

背景技术

AUV在海底执行长期任务时需要在海底基站进行能源补给或进行信息交互，都需要AUV具备同海底基站进行对接的能力。通常解决方案为AUV上装备USBL基阵，海底基站安装有配套位置已知的信标(或应答器)，AUV利用USBL测量得到的与信标之间的相对位置及相对方位信息，通过调整自身的姿态实现与基站之间的对接。

目前，USBL直接测量的相对位置及方位数据受时延测量误差、声速误差及水声信号信噪比的影响导致离散度大且数据不连续，以此为AUV控制系统的输入调整AUV的姿态将会产生很大的误差，不利于AUV稳定入坞。

发明内容

本发明的目的在于克服现有技术的不足，提供一种水下USBL反向应用的数据平滑方法，可将USBL的原始相对定位输出数据平滑输出，进而实现AUV在水下稳定入舱坞与水下基站对接的目的。

本发明解决其技术问题是采取以下技术方案实现的：

一种水下USBL反向应用的数据平滑方法，包括以下步骤：

步骤1、在对接坞舱上安装有位置已经标定好的信标，当AUV接近该对接坞舱后由USBL对信标进行相对定位，得到AUV相对坞舱的相对位置和方位信息，同时结合信标的已知位置反向计算获取USBL基阵绝对位置，进而得到AUV的绝对位置；

步骤2、利用步骤1中获得的AUV的绝对位置与SINS解算位置组合进行AUV组合导航Kalman滤波，计算得出AUV的三维位置误差，进而得到SINS与USBL组合滤波优化后的AUV绝对位置；

步骤3：根据步骤2得到的SINS与USBL组合滤波优化后的AUV绝对位置，对USBL相对定位位置回溯计算，得到平滑的USBL相对定位结果。

而且，所述步骤1的具体方法为：

(1)设USBL测得的应答器在基阵坐标系中的相对位置为P_Beacon，在地理坐标系中的绝对位置为P_Earth，AUV载体坐标系向大地坐标系转换的方向余弦矩阵为R_U，基阵坐标系框架与船体坐标系框架之间三个安装误差角构成的方向余弦矩阵为B_Align，USBL基阵绝对位置为P_USBL，忽略可测量补偿的杆臂的影响，则应答器在大地坐标系中的坐标公式为：

P_Earth＝P_USBL+R_UB_Align P_Beacon

(2)当使用AUV在水下搭载USBL时，P_USBL为USBL基阵的绝对位置，R_U可由惯导输出的航姿得到，B_Align为预先标定USBL与SINS之间的安装误差后得到，当水下信标位置P_Earth已知时，则通过应答器在大地坐标系中的坐标公式可获得USBL基阵的绝对位置计算公式：

P_USBL＝P_Earth-R_UB_Align P_Beacon

而且，所述步骤2的具体方法为：

(1)建立AUV装备的SINS模型：将陀螺仪漂移和加速度计偏置均看做随机常数过程，状态变量X选取15维：

其中，[φ_E φ_N φ_U]^T为东向、北向和天向姿态角误差，[δv_E δv_N δv_U]^T为东向、北向和天向速度误差，[δL δλ δS]^T为纬度、经度和高度误差，

为三个陀螺仪的漂移误差，

为三个加速度计的零偏误差；

(2)由惯导系统误差传播方程得到系统的状态方程为：

其中：F为系统状态转移矩阵；G为系统状态噪声矩阵；W为系统状态噪声，

(3)以位置误差为观测量的量测方程为：

Z_p＝H_pX+V

其中，Z_p为位置误差：即惯导解算位置P_SINS与USBL基阵的绝对位置之差P_SINS-P_USBL；H_p为量测矩阵，H_p＝[0_3×6 I_3×3 0_3×6]；V为三维位置观测噪声。

(4)完成组合滤波后可获得AUV的状态变量X的估计值，其中三维位置误差为ΔP＝[δL δλ δS]^T，其中δL为纬度误差，δλ为经度误差，δS为高度误差，设SINS与USBL组合滤波优化后的AUV绝对位置为P_{AUV_Fil}则有：

P_{AUV_Fil}＝P_USBL-ΔP

完成一次滤波后可直接获得△P。

而且，所述步骤3的具体方法为：

由步骤1的USBL基阵的绝对位置计算公式可得到滤波后信标在基阵系中的相对位置的计算公式：

P_{Beacon_Fil}＝B_Align ^-1R_U ^-1(P_Earth-P_{AUV_Fil})

其中，P_{Beacon_Fil}为滤波后信标在基阵系中的相对位置；P_{AUV_Fil}为滤波后AUV的绝对位置；将步骤1的应答器在大地坐标系中的坐标公式代入滤波后信标在基阵系中的相对位置的计算公式整理后有：

P_{Beacon_Fil}＝B_Align ^-1R_U ^-1ΔP+P_Beacon

本发明的优点和积极效果是：

1、本发明的水下装备USBL阵列的AUV在反向应用条件下，采用SINS进行SINS/USBL组合导航，根据组合导航的结果和已知的水下信标位置回溯计算USBL基阵与水下信标的相对位置，可将USBL的原始相对定位输出数据平滑输出，使得AUV控制系统获得更为理想的控制输入，有利于AUV在水下入舱坞及与水下基站对接的实现。

2、本发明通过USBL测量得到的与信标之间的相对位置，结合已知信标的位置可获得AUV自身的绝对位置(即USBL反向应用模式)。通常AUV上装备有SINS等导航传感器，借助于SINS纯惯性导航与USBL反向应用模式获得的AUV绝对位置进行Kalman滤波组合导航，可以校正SINS单独定位时AUV的导航位置并抑制定位误差发散，同时利用组合导航后的位置进行回溯计算可以获得连续平滑的USBL相对定位信息，为AUV的控制系统提供更为理想的控制数据输入。

附图说明

图1是本发明的USBL反向应用对接场景示意图；

图2(a)是本发明USBL单独定位数据(虚线)和组合滤波回溯后的数据(实线)对比图。

图2(b)是本发明USBL单独定位数据(虚线)和组合滤波回溯后的数据(实线)对比图。

图2(c)是本发明USBL单独定位数据(虚线)和组合滤波回溯后的数据(实线)对比图。

具体实施方式

以下结合附图对本发明实施例作进一步详述：

USBL反向应用对接场景如图1所示，AUV上安装有SINS及USBL，对接坞舱上安装有位置已经标定好的信标(应答器)，AUV接近该对接坞舱后USBL对信标进行相对定位，得到AUV相对坞舱的相对位置和方位信息，同时结合信标的已知位置反向计算自身的绝对位置并与SINS的输出位置进行Kalman组合导航滤波，得到自身的滤波位置后进行回溯计算得到平滑后的AUV相对坞舱的相对位置和方位信息，供AUV控制系统控制AUV完成入坞舱。

本发明提供了一种在USBL(超短基线)定位系统反向应用条件下，使用SINS(捷联惯性导航系统)来平滑优化USBL原始输出数据的方法。

一种水下USBL反向应用的数据平滑方法，包括以下步骤：

步骤1、在对接坞舱上安装有位置已经标定好的信标，当AUV接近该对接坞舱后由USBL对信标进行相对定位，得到AUV相对坞舱的相对位置和方位信息，同时结合信标的已知位置反向计算获取USBL基阵绝对位置，进而得到AUV的绝对位置；

所述步骤1的具体方法为：

(1)设USBL测得的应答器在基阵坐标系中的相对位置为P_Beacon，在地理坐标系中的绝对位置为P_Earth，AUV载体坐标系向大地坐标系转换的方向余弦矩阵为R_U，基阵坐标系框架与船体坐标系框架之间三个安装误差角构成的方向余弦矩阵为B_Align，USBL基阵绝对位置为P_USBL，忽略可测量补偿的杆臂的影响，则应答器在大地坐标系中的坐标公式为：

P_Earth＝P_USBL+R_UB_Align P_Beacon (1)

(2)当使用AUV在水下搭载USBL时，P_USBL为USBL基阵的绝对位置，R_U可由惯导输出的航姿得到，B_Align为预先标定USBL与SINS之间的安装误差后得到，当水下信标位置P_Earth已知时，则通过应答器在大地坐标系中的坐标公式(1)可获得USBL基阵的绝对位置计算公式：

P_USBL＝P_Earth-R_UB_Align P_Beacon (2)

则可通过对USBL基阵绝对位置的相对测量获得AUV的绝对位置。

步骤2、利用步骤1中获得的AUV的绝对位置与SINS解算位置组合进行AUV组合导航Kalman滤波，计算得出AUV的三维位置误差，进而得到SINS与USBL组合滤波优化后的AUV绝对位置；

所述步骤2的具体方法为：

(1)建立AUV装备的SINS模型：将陀螺仪漂移和加速度计偏置均看做随机常数过程，状态变量X选取15维：

其中，[φ_E φ_N φ_U]^T为东向、北向和天向姿态角误差，[δv_E δv_N δv_U]^T为东向、北向和天向速度误差，[δL δλ δS]^T为纬度、经度和高度误差，

为三个陀螺仪的漂移误差，

为三个加速度计的零偏误差；

(2)由惯导系统误差传播方程得到系统的状态方程为：

其中：F为系统状态转移矩阵；G为系统状态噪声矩阵；W为系统状态噪声，

(3)以位置误差为观测量的量测方程为：

Z_p＝H_pX+V (4)

其中，Z_p为位置误差：即惯导解算位置P_SINS与由(2)式得到的USBL基阵的绝对位置之差P_SINS-P_USBL；H_p为量测矩阵，H_p＝[0_3×6 I_3×3 0_3×6]；V为三维位置观测噪声。

(4)完成组合滤波后可获得AUV的状态变量X的估计值，其中三维位置误差为ΔP＝[δL δλ δS]^T，其中δL为纬度误差，δλ为经度误差，δS为高度误差，设SINS与USBL组合滤波优化后的AUV绝对位置为P_{AUV_Fil}则有：

P_{AUV_Fil}＝P_USBL-ΔP (5)

完成一次滤波后可直接获得△P。

通过步骤2可抑制SINS误差发散，同时SINS/USBL组合定位误差较USBL单独定位误差分布更为集中，能够起到平滑USBL单独定位误差的效果。

步骤3：根据步骤2得到的SINS与USBL组合滤波优化后的AUV绝对位置，对USBL相对定位位置回溯计算，得到平滑的USBL相对定位结果；

所述步骤3的具体方法为：

由步骤1的USBL基阵的绝对位置计算公式可得滤波后信标在基阵系中的相对位置的计算公式：

P_{Beacon_Fil}＝B_Align ^-1R_U ^-1(P_Earth-P_{AUV_Fil}) (6)

其中，P_{Beacon_Fil}为滤波后信标在基阵系中的相对位置；P_{AUV_Fil}为滤波后AUV的绝对位置，由(5)式获得；将(1)式代入(6)式整理后有：

P_{Beacon_Fil}＝B_Align ^-1R_U ^-1ΔP+P_Beacon (7)

由(7)式可见，经过滤波回溯计算后的USBL相对定位位置与固定信标的位置无关，与滤波位置误差、USBL原始测量误差、安装误差及SINS的航姿误差相关，经组合优化后补偿了B_Align ^-1R_U ^-1ΔP项，可剔除USBL的野值，得到平滑的USBL相对定位结果，有利于AUV对接等任务的完成。

按照本发明的方法，使用半实物数据对实际USBL数据进行仿真，得到的实际结果如图2(a)、(b)和(c)所示。虚线为USBL单独相对定位数据，数据跳跃，不连续，不利于AUV稳定控制，实线为组合滤波回溯后的相对定位数据，数据连续平滑，有利于实现AUV稳定控制入坞。

本发明在水下固定信标条件下，基于AUV上安装的SINS及USBL传感器，使用SINS/USBL组合导航滤波后经过回溯算法计算USBL相对定位位置可提高USBL相对定位数据的连续性及平滑性及AUV控制精度，便于AUV完成与水下基站对接等任务，具有推广价值。

需要强调的是，本发明所述的实施例是说明性的，而不是限定性的，因此本发明包括并不限于具体实施方式中所述的实施例，凡是由本领域技术人员根据本发明的技术方案得出的其他实施方式，同样属于本发明保护的范围。

Claims (2)

1.一种水下USBL反向应用的数据平滑方法，其特征在于：包括以下步骤：

步骤1、在对接坞舱上安装有位置已经标定好的信标，当AUV接近该对接坞舱后由USBL对信标进行相对定位，得到AUV相对坞舱的相对位置和方位信息，同时结合信标的已知位置反向计算获取USBL基阵绝对位置，进而得到AUV的绝对位置；

步骤2、利用步骤1中获得的AUV的绝对位置与SINS解算位置组合进行AUV组合导航Kalman滤波，计算得出AUV的三维位置误差，进而得到SINS与USBL组合滤波优化后的AUV绝对位置；

步骤3：根据步骤2得到的SINS与USBL组合滤波优化后的AUV绝对位置，对USBL相对定位位置回溯计算，得到平滑的USBL相对定位结果；

所述步骤1的具体方法为：

(1)设USBL测得的应答器在基阵坐标系中的相对位置为P_Beacon，在地理坐标系中的绝对位置为P_Earth，AUV载体坐标系向大地坐标系转换的方向余弦矩阵为R_U，基阵坐标系框架与船体坐标系框架之间三个安装误差角构成的方向余弦矩阵为B_Align，USBL基阵绝对位置为P_USBL，忽略可测量补偿的杆臂的影响，则应答器在大地坐标系中的坐标公式为：

P_Earth＝P_USBL+R_UB_AlignP_Beacon

(2)当使用AUV在水下搭载USBL时，P_USBL为USBL基阵的绝对位置，R_U可由惯导输出的航姿得到，B_Align为预先标定USBL与SINS之间的安装误差后得到，当水下信标位置P_Earth已知时，则通过应答器在大地坐标系中的坐标公式可获得USBL基阵的绝对位置计算公式：

P_USBL＝P_Earth-R_UB_AlignP_Beacon

所述步骤2的具体方法为：

(1)建立AUV装备的SINS模型：将陀螺仪漂移和加速度计偏置均看做随机常数过程，状态变量X选取15维：

其中，[φ_E φ_N φ_U]^T为东向、北向和天向姿态角误差，[δv_E δv_N δv_U]^T为东向、北向和天向速度误差，[δL δλ δS]^T为纬度、经度和高度误差，

为三个陀螺仪的漂移误差，

为三个加速度计的零偏误差；

(2)由惯导系统误差传播方程得到系统的状态方程为：

其中：F为系统状态转移矩阵；G为系统状态噪声矩阵；W为系统状态噪声，

(3)以位置误差为观测量的量测方程为：

Z_p＝H_pX+V

其中，Z_p＝P_SINS-P_USBL为位置误差：即惯导解算位置P_SINS与USBL基阵的绝对位置P_USBL之差；H_p为量测矩阵，H_p＝[0_3×6 I_3×3 0_3×6]；V为三维位置观测噪声；

(4)完成组合滤波后可获得AUV的状态变量X的估计值，其中三维位置误差为ΔP＝[δLδλδS]^T，其中δL为纬度误差，δλ为经度误差，δS为高度误差，设SINS与USBL组合滤波优化后的AUV绝对位置为P_{AUV_Fil}则有：

P_{AUV_Fil}＝P_USBL-ΔP

完成一次滤波后可直接获得△P。

2.根据权利要求1所述的一种水下USBL反向应用的数据平滑方法，其特征在于：所述步骤3的具体方法为：

由步骤1的USBL基阵的绝对位置计算公式可得到滤波后信标在基阵系中的相对位置的计算公式：

P_{Beacon_Fil}＝B_Align ^-1R_U ^-1(P_Earth-P_{AUV_Fil})

其中，P_{Beacon_Fil}为滤波后信标在基阵系中的相对位置；P_{AUV_Fil}为滤波后AUV的绝对位置；将步骤1的应答器在大地坐标系中的坐标公式代入滤波后信标在基阵系中的相对位置的计算公式整理后有：

P_{Beacon_Fil}＝B_Align ^-1R_U ^-1ΔP+P_Beacon。

Images