一种基于SINS/LBL紧组合的AUV水下导航定位方法

摘要

本发明提供了一种基于SINS/LBL紧组合的AUV水下导航定位方法，其特征在于：由安装在AUV上的捷联惯性导航系统SINS、布放在海底的长基线水声定位系统LBL和数据处理单元三大部分组成，具体步骤为：首先，对IMU数据捷联解算获得AUV位置等信息，并将解算的位置信息用地球直角坐标表示；其次，根据SINS提供的AUV位置信息和水听器基阵位置坐标推算SINS斜距差；再次，根据LBL的定位特点建立LBL斜距差模型，将SINS斜距差和LBL斜距差的差值作为卡尔曼滤波器的观测量，经滤波估计补偿校正SINS的导航定位信息。该方法解决了SINS系统误差随时间积累的问题，保证了AUV在水下长期自主导航定位的精度，同时避免了GPS及其他无线电定位系统的使用，提高了AUV水下作业效率。

主权项

一种基于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两两基元与AUV斜距差推算模块(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与AUV之间的斜距与水听器0与AUV之间的斜距之差ρ_LBL的差值作为外部观测信息输入到卡尔曼滤波器进行滤波，其中，i＝1,2,3；(4)校正模块(5)根据SINS/LBL紧组合模块(4)的卡尔曼滤波结果对SINS(1)进行校正，最终得到精确的AUV位置信息P_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与AUV之间的斜距与水听器0与AUV之间的斜距之差其中，i＝1,2,3；(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泰勒级数展开取前两项得：$\begin{array}{c}{\rho }_{SINSi}=\sqrt{{\left(x-x_i\right)}^2+{\left(y-y_i\right)}^2+{\left(z-z_i\right)}^2}-\sqrt{{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2+{\left(z-z_0\right)}^2}\\ +\frac{\partial {\rho }_{SINSi}}{\partial x}\delta x+\frac{\partial {\rho }_{SINSi}}{\partial y}\delta y+\frac{\partial {\rho }_{SINSi}}{\partial z}\delta z\end{array}$设同理$\frac{\partial {\rho }_{SINSi}}{\partial z}=\frac{z_S-z_i}{R_i}-\frac{z_S-z_0}{R_0}=G_{iz}-G_{0z},i=1,2,3;$其中，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于是：$\begin{array}{c}{\rho }_{SINSi}=R_i-R_0+\left(G_{ix}-G_{0x}\right)\delta x+\left(G_{iy}-G_{0y}\right)\delta y+\left(G_{iz}-G_{0z}\right)\delta z\\ =R_i-R_0+e_{ix}\delta x+e_{iy}\delta y+e_{iz}\delta z\end{array},i=1,2,3;$所述SINS/LBL紧组合模块(4)的具体实现步骤如下：(1)建立LBL斜距差模型；由于时延差测量、声传播的多途径效应等将引起斜距差测量有误差，为简化模型，认为斜距差误差是由常值偏置和随机噪声组成，则LBL水听器i与AUV的斜距与水听器0与AUV的斜距之差可表示为：$\left\{\begin{array}{c}{\mathrm{\Delta R}}_{meas}=\Delta R+\delta R+{\nu }_{\Delta R}\\ \delta \stackrel{·}{R}=0\end{array}\right.$式中，ΔR_meas为LBL水听器i与AUV的斜距与水听器0与AUV的斜距之差，ΔR为斜距差真值，δR＝[δR₁ δR₂ δR₃]^T为随机常值，v_ΔR～N(0,Q_ΔR)为高斯白噪声，i＝1,2,3；(2)建立SINS/LBL紧组合状态方程；SINS/LBL紧组合状态方程描述为：其中：X_SINS为SINS的状态向量，X_LBL为LBL的状态向量，F_SINS为SINS的转移矩阵，F_LBL为LBL的转移矩阵，W_SINS为SINS的系统噪声向量，W_LBL为LBL的系统噪声向量，F为紧组合系统转移矩阵，X为紧组合系统状态向量，W为紧组合系统噪声向量；根据捷联惯性导航系统长期工作时的误差特点，选择位置误差、速度误差、姿态误差、陀螺漂移和加速度计零偏作为状态量：式中，δV_E、δV_N、δV_U分别是捷联东向、北向、天向的速度误差，分别是捷联东向、北向、天向的失准角，δL、δλ、δh分别是捷联纬度、经度、高度误差，三个位置误差由地球坐标系描述，是捷联加表三个轴向的偏置误差，ε_bx、ε_by、ε_bz是捷联陀螺的三个轴向漂移；X_LBL＝[δR₁ δR₂ δR₃]^T式中，δR₁、δR₂、δR₃分别为LBL水听器i与AUV的斜距与水听器0与AUV的斜距之差的随机常值漂移，其中，i＝1,2,3；系统噪声阵$W_{SINS}={\left[\begin{array}{ccccccccccccccc}{\omega }_{V_E}& {\omega }_{V_N}& {\omega }_{V_U}& {\omega }_{{\phi }_E}& {\omega }_{{\phi }_N}& {\omega }_{{\phi }_U}& {\omega }_{\delta L}& {\omega }_{\delta \lambda }& {\omega }_{\delta h}& 0& 0& 0& 0& 0& 0\end{array}\right]}^T$W_LBL＝[0 0 0]^T系统状态转移矩阵式中，其中：F_ij为F_9×9的元素R_N为参考椭球体子午面內的曲率半径，R_N＝R_e(1‑2e+3esin²L)R_E为垂直子午面內的曲率半径,R_E＝R_e(1+esin²L)其中：R_e为参考椭球体的长轴半径；e为椭球的椭圆度，L为当地纬度；$\begin{array}{cc}{F}_{11}=\frac{V_N}{R_E+h}tgL-\frac{V_U}{R_E+h}& F_{12}=2{\omega }_{ie}\sin L+\frac{V_E}{R_E+h}\tan L\end{array}$$\begin{array}{cc}{F}_{13}=-2{\omega }_{ie}\cos L+\frac{V_E}{R_E+h}& F_{17}=(2{\omega }_{ie}\cos L+\frac{V_E}{R_N+h}{\sec }^{2}L)V_N+2{\omega }_{ie}\sin {\mathrm{LV}}_U\end{array}$$\begin{array}{cc}{F}_{21}=-2({\omega }_{ie}\sin L+\frac{V_E}{R_E+h}tgL)& F_{22}=-\frac{V_U}{R_N+h}\end{array}$$\begin{array}{cc}{F}_{23}=-\frac{V_N}{R_N+h}& F_{27}=-(2{\omega }_{ie}\cos L+\frac{V_E}{R_E+h}{\sec }^{2}L)V_E\end{array}$$\begin{array}{cc}{F}_{31}=2({\omega }_{ie}\cos L+\frac{V_E}{R_E+h})& F_{32}=\frac{2V_N}{R_N+h}\end{array}$F₃₇＝‑2ω_ie cosLV_E$\begin{array}{cc}{F}_{45}={\omega }_{ie}\sin L+\frac{V_E}{R_E+h}tgL& F_{46}=-({\omega }_{ie}\cos L+\frac{V_E}{R_E+h})\end{array}$$\begin{array}{cc}{F}_{51}=\frac{1}{R_E+h}& F_{54}=-({\omega }_{ie}\sin L+\frac{V_E}{R_E+h}tgL)\end{array}$F₅₇＝‑ω_ie sinL$\begin{array}{cc}{F}_{61}=\frac{1}{R_E+h}tgL& F_{64}={\omega }_{ie}\cos L+\frac{V_E}{R_E+h}\end{array}$$\begin{array}{cc}{F}_{65}=\frac{V_N}{R_N+h}& F_{67}={\omega }_{ie}\cos L+\frac{V_E}{R_E+h}{\sec }^{2}L\end{array}$$\begin{array}{cc}{F}_{72}=\frac{1}{R_N+h}& F_{81}=\frac{1}{R_E+h}\sec L\end{array}$$F_{87}=\frac{V_E}{R_E+h}\sec LtgL$$C_{9\times 6}=\left[\begin{array}{cccccc}{C}_{11}& C_{21}& C_{31}& 0& 0& 0\\ C_{12}& C_{22}& C_{32}& 0& 0& 0\\ C_{13}& C_{23}& C_{33}& 0& 0& 0\\ 0& 0& 0& -C_{11}& -C_{21}& -C_{31}\\ 0& 0& 0& -C_{12}& -C_{22}& -C_{32}\\ 0& 0& 0& -C_{13}& -C_{23}& -C_{33}\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right]$C_ij为姿态转移矩阵的元素F_LBL＝0_3×3；(3)建立SINS/LBL紧组合量测方程；紧组合系统采用SINS推算的水听器与AUV的斜距差与LBL测量得到的斜距差之差作为观测量；在紧组合系统中，设LBL测得的斜距差为ρ_LBLi,水底应答器阵基元的位置为P_i(x_i,y_i,z_i)，SINS测得的AUV位置为P_SINS(x_S,y_S,z_S),由SINS测得的AUV位置P_SINS(x_S,y_S,z_S)和水底应答器阵基元的位置为P_i(x_i,y_i,z_i)所确定的斜距差为ρ_SINSi；SINS斜距差ρ_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δzLBL斜距差则量测可写成则有：$\delta \rho =\left[\begin{array}{c}{\mathrm{\delta \rho }}_1\\ {\mathrm{\delta \rho }}_2\\ {\mathrm{\delta \rho }}_3\end{array}\right]=\left[\begin{array}{cccccc}{e}_{1x}& e_{1y}& e_{1z}& -1& 0& 0\\ e_{2x}& e_{2y}& e_{3z}& 0& -1& 0\\ e_{3x}& e_{3y}& e_{3z}& 0& 0& -1\end{array}\right]\left[\begin{array}{c}\delta x\\ \delta y\\ \delta z\\ {\mathrm{\delta R}}_1\\ {\mathrm{\delta R}}_2\\ {\mathrm{\delta R}}_3\end{array}\right]+\left[\begin{array}{c}{\nu }_{{\mathrm{\delta R}}_1}\\ {\nu }_{{\mathrm{\delta R}}_2}\\ {\nu }_{{\mathrm{\delta R}}_3}\end{array}\right]$当系统采用地球直角坐标系(Ox_ey_ez_e)作为导航坐标系时，用上式构造系统量测方程；实际应用中是以经纬度和高度定位的，因此要把dx,dy,dz用dl,dλ,dh表示；由$\left\{\begin{array}{c}\delta x=\delta h\cos Lcos\lambda -(R_N+h)\sin Lcos\lambda \delta L-(R_N+h)\cos Lsin\lambda \delta \lambda \\ \delta y=\delta h\cos Lsin\lambda -(R_N+h)\sin L\sin \lambda \delta L-(R_N+h)\cos L\cos \lambda \delta \lambda \\ \delta z=\delta h\sin L+[R_N(1-e^2)+h]\cos L\delta L\end{array}\right.$量测方程为Z_3×1＝H_3×18X_18×1+V_ΔR(3×1)式中，设其中a_ij为矩阵H₁的元素，i＝1,2,3；j＝1,2,3；H₁非零元素如下:a_i1＝‑(R_N+h)sin L cosλe_1x‑(R_N+h)sin L sinλe_1y+[R_N(1‑e²)+h]cos Le_1za_i2＝‑(R_N+h)cos L sinλe_2x‑(R_N+h)cos L cosλe_2ya_i3＝cos L cosλe_3x+cos L sinλe_3y+sin Le_3z i＝1,2,3。