[发明专利]一种深海潜水器导航后处理方法

权利要求书

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的经度和潜水器推算经度。