[发明专利]一种基于SINS/LBL紧组合的AUV水下导航定位方法

权利要求书

1.一种基于SINS/LBL紧组合的AUV水下导航定位方法，其特征在于：所用导航定位系统由安装在AUV上的捷联惯性导航系统SINS(1)、布放在海底的长基线水声定位系统LBL(2)和数据处理单元组成，其中，所述的捷联惯性导航系统SINS(1)包括捷联解算模块，所述的长基线水声定位系统LBL(2)由布放在海底的四个位置已知的水听器基阵组成，所述的数据处理单元包括SINS两两基元与AUV斜距差推算模块(3)、SINS/LBL紧组合模块(4)和校正模块(5)，采用SINS/LBL紧组合方法完成组合导航，所述方法通过下列步骤实现：

(1)捷联惯性导航系统SINS(1)通过捷联解算得到相应的包括AUV的位置信息的导航信息，解算的位置信息用地球大地坐标P_SINS(L_S,λ_S,h_S)表示，并将P_SINS(L_S,λ_S,h_S)转化为用地球直角坐标P_SINS(x_S,y_S,z_S)表示；

(2)SINS两两基元与目标斜距差推算模块(3)根据SINS提供的AUV位置信息P_SINS(x_S,y_S,z_S)和水听器基阵位置P_i(x_i,y_i,z_i)推算SINS斜距差ρ_SINS；

(3)SINS/LBL紧组合模块(4)根据长基线水声定位系统LBL(2)的定位特点建立LBL斜距差模型，将SINS斜距差ρ_SINS和LBL水听器i(i＝1,2,3)与AUV之间的斜距与水听器0与AUV之间的斜距之差ρ_LBL的差值作为外部观测信息输入到卡尔曼滤波器进行滤波；

(4)校正模块(5)根据SINS/LBL紧组合模块(4)的卡尔曼滤波结果对SINS(1)进行校正，最终得到精确的AUV位置信息P_AUV。

2.根据权利要求1所述的一种基于SINS/LBL紧组合的AUV水下导航定位方法，其特征在于：SINS两两基元与目标AUV斜距差推算模块(3)计算SINS斜距差的方法如下：

(1)根据SINS解算的AUV位置P_SINS(x_s,y_s,z_s)和长基线水声定位系统LBL中水听器基元位置P_i(x_i,y_i,z_i)计算得到水听器i(i＝1,2,3)与AUV之间的斜距与水听器0与AUV之间的斜距之差

(2)将ρ_SINSi利用泰勒级数线性化。设AUV真实位置为P_AUV(x,y,z),(δx,δy,δz)为SINS解算AUV位置的误差，则x_S＝x+δx,y_S＝y+δy,z_S＝z+δz。将ρ_SINSi泰勒级数展开取前两项得：

设

同理

其中，G_ij(i＝0,1,2,3；j＝x,y,z)为已知量，可由SINS解算的概略位置P_SINS(x_S,y_S,z_S)和水底应答器阵基元的位置P_i(x_i,y_i,z_i)计算得到，由于SINS解算的概略位置P_SINS(x_S,y_S,z_S)可能有较大误差，这样在进行方程线性化时略去高阶项会引起线性误差，可以利用迭代法解算，即在第一次解之后，用它作为近似值再重新计算；

设：e_ix＝G_ix-G_0x，e_iy＝G_iy-G_0y，e_iz＝G_iz-G_0z，i＝1,2,3

于是：

ρ_SINSi＝R_i-R₀+(G_ix-G_0x)δx+(G_iy-G_0y)δy+(G_iz-G_0z)δz

＝R_i-R₀+e_ixδx+e_iyδy+e_izδz          ，i＝1,2,3。