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

权利要求书

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。