[发明专利]一种水下USBL反向应用的数据平滑方法

说明书

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

技术领域

本发明属于水下导航定位技术领域，涉及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基阵的绝对位置计算公式：