CN104457754A - 一种基于sins/lbl紧组合的auv水下导航定位方法 - Google Patents

Abstract

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

Description

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

技术领域

本发明主要涉及AUV水下导航技术领域，尤其涉及一种基于SINS/LBL紧组合的AUV水下导航定位方法，特别适用于水下自主航行器AUV的跟踪定位。

背景技术

AUV(Autonomous Underwater Vehicle，自主式水下航行器)是一种可以完成水下探测、攻击、运载、打捞等多种功能的水下工具，因其活动范围广、体积小、重量轻、隐蔽性高等特点，现已成为国内外军事海洋技术研究的一个重要方向。

AUV水下高精度自主导航和定位跟踪技术是完成其水下作业的前提和关键。在现有的定位技术中，SINS(Strapdown Inertial Navigation Systems，捷联式惯性导航系统)因其具有隐蔽性强、自主性、抗干扰、数据更新频率高、且在短时间内具有较高精度等特点，因而成为AUV水下自主导航定位的首选定位方法。目前，尽管捷联惯性导航技术的发展已日趋成熟，其导航定位误差随时间积累发散的这一动态特性却未改变，在远程、长期航行及武器发射等高精度导航时还不能完全满足要求。组合导航技术的出现为这一问题的解决提供了一种有效途径。

LBL(Long Base Line，长基线)水声定位系统是由安装在海底的基线长度为几千米的应答器基阵和安装在载体上的问答机组成，其定位原理是利用载体上的问答机与海底应答器阵之间的距离信息来求解AUV位置。LBL因其作用范围广、定位精度高已广泛应用于水下潜器。

近年来，应用于AUV的水下自主导航技术主要以SINS与DVL(Doppler Velocity Log，多普勒测速仪)的组合导航为主，辅以水面GPS(Global Positioning System，全球定位系统)修正。在若干次实验中取得了良好的导航精度，但航程相对较短，对于DVL，当声纳传感器远离海底时测量速度精度很差，仅仅对AUV贴近海底时精度较好，而对于GPS，AUV需中断潜行，浮到水面才能利用GPS信息，这在深海的情况下将浪费大量的时间与能源，严重影响AUV的水下作业效率。

发明内容

针对现有AUV水下导航精度的问题，本发明提供了一种基于SINS/LBL紧组合的AUV水下导航定位方法。

本发明的目的可以通过以下技术方案实现，具体为：

(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。

所述的SINS两两基元与目标斜距差推算模块(3)计算SINS斜距差的方法如下：

(1)根据长基线水声定位系统LBL中水听器位置P_i(x_i,y_i,z_i)和SINS解算AUV位置P_SINS(x_s,y_s,z_s)计算得到水听器i(i＝1,2,3)与AUV之间的斜距与水听器0与AUV之间的斜距之差 ${\mathrm{\ρ}}_{\mathrm{SINSi}}=\sqrt{{(x_S-x_i)}^2+{(y_S-y_i)}^2+{(z_S-z_i)}^2}-\sqrt{{(x_S-x_0)}^2+{(y_S-y_0)}^2+{(z_S-z_0)}^2}(i=\mathrm{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}{\mathrm{\ρ}}_{\mathrm{SINSi}}=\sqrt{{(x-x_i)}^2+{(y-y_i)}^2+{(z-z_i)}^2}-\sqrt{{(x-x_0)}^2+{(y-y_0)}^2+{(z-z_0)}^2}\\ +\frac{\∂{\mathrm{\ρ}}_{\mathrm{SINSi}}}{\∂x}\mathrm{\δx}+\frac{\∂{\mathrm{\ρ}}_{\mathrm{SINSi}}}{\∂y}\mathrm{\δy}+\frac{\∂{\mathrm{\ρ}}_{\mathrm{SINSi}}}{\∂z}\mathrm{\δz}\end{array}$

设 $\begin{array}{c}\frac{\∂{\mathrm{\ρ}}_{\mathrm{SINSi}}}{\∂x}=\frac{x_S-x_i}{\sqrt{{(x_S-x_i)}^2+{(y_S-y_i)}^2+{(z_S-z_i)}^2}}-\frac{x_S-x_0}{\sqrt{{(x_S-x_i)}^2+{(y_S-y_0)}^2+{(z_S-z_0)}^2}}\\ =\frac{x_S-x_i}{R_i}-\frac{x_S-x_0}{R_0}=G_{\mathrm{ix}}-G_{0x}\end{array}$

同理 $\frac{\∂{\mathrm{\ρ}}_{\mathrm{SINSi}}}{\∂y}=\frac{y_S-y_i}{R_i}-\frac{y_S-y_0}{R_0}=G_{\mathrm{iy}}-G_{0y}$

$\frac{\∂{\mathrm{\ρ}}_{\mathrm{SINSi}}}{\∂z}=\frac{z_S-z_i}{R_i}-\frac{z_S-z_0}{R_0}=G_{\mathrm{iz}}-G_{0z},i=\mathrm{1,2,3}.$

其中， $R_i=\sqrt{{(x_s-x_i)}^2+{(y_s-y_i)}^2+{(z_s-z_i)}^2}(i=\mathrm{0,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

于是：

ρ_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

所述的SINS/LBL紧组合模块(4)的具体实现步骤如下：

(1)建立LBL斜距差模型

由于时延差测量、声传播的多途径效应等将引起斜距差测量有误差，为简化模型，可认为斜距差误差是由常值偏置和随机噪声组成，则LBL水听器i(i＝1,2,3)与AUV的斜距与水听器0与AUV的斜距之差可表示为：

$\left\{\begin{array}{c}\mathrm{\Δ}{R}_{\mathrm{meas}}=\mathrm{\ΔR}+\mathrm{\δR}+{\mathrm{\ν}}_{\mathrm{\ΔR}}\\ \mathrm{\δ}\stackrel{\·}{R}=0\end{array}\right.$

式中，ΔR_meas为LBL水听器i(i＝1,2,3)与AUV的斜距与水听器0与AUV的斜距之差，ΔR为斜距差真值，δR＝[δR₁ δR₂ δR₃]^T为随机常值，ν_δR(t)～N(0,Q_ΔR)为高斯白噪声。

(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为紧组合系统噪声向量。

根据捷联惯性导航系统长期工作时的误差特点，选择位置误差、速度误差、姿态误差、陀螺漂移和加速度计零偏作为状态量：

X_SINS＝[δV_E δV_N δV_U φ_E φ_N φ_U δL δL δh ▽_bx ▽_by ▽_bz ε_bx ε_by ε_bz]^T

式中，δV_E、δV_N、δV_U分别是捷联东向、北向、天向的速度误差，分别是捷联东向、北向、天向的失准角，δL、δλ、δh分别是捷联纬度、经度、高度误差，三个位置误差由地球坐标系描述，▽_bx、▽_by、▽_bz是捷联加表三个轴向的偏置误差，ε_bx、ε_by、ε_bz是捷联陀螺的三个轴向漂移。

X_LBL＝[δR₁ δR₂ δR₃]^T

式中，δR₁、δR₂、δR₃分别为LBL水听器i(i＝1,2,3)与AUV的斜距与水听器0与AUV的斜距之差的随机常值漂移。

系统噪声阵 $W_{\mathrm{SINS}}={\left[\begin{array}{ccccccccccccccc}{\mathrm{\ω}}_{V_E}& {\mathrm{\ω}}_{V_N}& {\mathrm{\ω}}_{V_U}& {\mathrm{\ω}}_{{\mathrm{\φ}}_E}& {\mathrm{\ω}}_{{\mathrm{\φ}}_N}& {\mathrm{\ω}}_{{\mathrm{\φ}}_U}& {\mathrm{\ω}}_{\mathrm{\δL}}& {\mathrm{\ω}}_{\mathrm{\δ\λ}}& {\mathrm{\ω}}_{\mathrm{\δh}}& 0& 0& 0& 0& 0& 0\end{array}\right]}^T$

W_LBL＝[0 0 0]^T

系统状态转移矩阵 $F_{\mathrm{SINS}}=\left[\begin{array}{cc}{F}_{9\×9}& C_{9\×6}\\ 0_{6\×9}& 0_{6\×6}\end{array}\right]$

式中， $F_{9\×9}=\left[\begin{array}{ccccccccc}{F}_{11}& F_{12}& F_{13}& 0& -f_U& f_N& F_{17}& 0& 0\\ F_{21}& F_{22}& F_{23}& f_U& 0& f_E& F_{27}& 0& 0\\ F_{31}& F_{32}& 0& -f_N& f_E& 0& F_{37}& 0& 0\\ 0& F_{42}& 0& 0& F_{45}& F_{46}& 0& 0& 0\\ F_{51}& 0& 0& F_{54}& 0& F_{56}& F_{57}& 0& 0\\ F_{61}& 0& 0& F_{64}& F_{65}& 0& F_{67}& 0& 0\\ 0& F_{72}& 0& 0& 0& 0& 0& 0& 0\\ F_{81}& 0& 0& 0& 0& 0& F_{87}& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0\end{array}\right]$

其中：F_ij为F_9×9的元素

R_N为参考椭球体子午面內的曲率半径,R_N＝R_e(1-2e+3e sin² L)

R_E为垂直子午面內的曲率半径,R_E＝R_e(1+e sin² L)

其中：R_e为参考椭球体的长轴半径；e为椭球的椭圆度。

$\begin{array}{cc}{F}_{11}=\frac{V_N}{R_E+h}\mathrm{tgL}-\frac{V_U}{R_E+h}& F_{12}=2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_E+h}\tan L\end{array}$

$\begin{array}{cc}{F}_{13}=-2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_E+h}& F_{17}=(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}{\sec }^{2}L)V_N+2{\mathrm{\ω}}_{\mathrm{ie}}\sin {\mathrm{LV}}_U\end{array}$

$\begin{array}{cc}{F}_{21}=-2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_E+h}\mathrm{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{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_E+h}{\sec }^{2}L)V_E\end{array}$

$\begin{array}{cc}{F}_{31}=2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_E+h})& F_{32}=\frac{2V_N}{R_N+h}\end{array}$

F₃₇＝-2ω_ie cos LV_E  $F_{42}=-\frac{1}{R_N+h}$

$\begin{array}{cc}{F}_{45}={\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_E+h}\mathrm{tgL}& F_{46}=-({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_E+h})\end{array}$

$\begin{array}{cc}{F}_{51}=\frac{1}{R_E+h}& F_{54}=-({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_E+h}\mathrm{tgL})\end{array}$

$F_{56}=-\frac{V_N}{R_N+h}$  F₅₇＝-ω_ie sin L

$\begin{array}{cc}{F}_{61}=\frac{1}{R_E+h}\mathrm{tgL}& F_{64}={\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_E+h}\end{array}$

$\begin{array}{cc}{F}_{65}=\frac{V_N}{R_N+h}& F_{67}={\mathrm{\ω}}_{\mathrm{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 \mathrm{LtgL}$

$C_{9\×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(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δz

LBL斜距差 ${\mathrm{\ρ}}_{\mathrm{LBLi}}=R_i-R_0+\mathrm{\δ}{R}_i+{\mathrm{\ν}}_{{\mathrm{\ΔR}}_i}$

则量测可写成 $\mathrm{\δ}{\mathrm{\ρ}}_i={\mathrm{\ρ}}_{\mathrm{SINSi}}-{\mathrm{\ρ}}_{\mathrm{LBLi}}=e_{\mathrm{iz}}\mathrm{\δx}+e_{\mathrm{iy}}\mathrm{\δy}+e_{\mathrm{iz}}\mathrm{\δz}-\mathrm{\δ}{R}_i-{\mathrm{\ν}}_{{\mathrm{\ΔR}}_i}(i=\mathrm{1,2,3})$

则有：

$\mathrm{\δ\ρ}=\left[\begin{array}{c}\mathrm{\δ}{\mathrm{\ρ}}_1\\ \mathrm{\δ}{\mathrm{\ρ}}_2\\ {\mathrm{\δ\ρ}}_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}\mathrm{\δx}\\ \mathrm{\δy}\\ \mathrm{\δz}\\ {\mathrm{\δR}}_1\\ \mathrm{\δ}{R}_2\\ \mathrm{\δ}{R}_3\end{array}\right]+\left[\begin{array}{c}{\mathrm{\ν}}_{{\mathrm{\δR}}_1}\\ {\mathrm{\ν}}_{{\mathrm{\δR}}_2}\\ {\mathrm{\ν}}_{{\mathrm{\δR}}_3}\end{array}\right]$

当系统采用地球直角坐标系(Ox_ey_ez_e)作为导航坐标系时，可用上式构造系统量测方程。实际应用中是以经纬度和高度定位的，因此要把dx,dy,dz用dl,dλ,dh表示。

由 $\left\{\begin{array}{c}x=(R_N+h)\cos L\cos \mathrm{\λ}\\ y=(R_N+h)\cos L\sin \mathrm{\λ}\\ z=[R_N(1-e^2)+h]\sin L\end{array}\right.$

$\left\{\begin{array}{c}\mathrm{\δx}=\mathrm{\δ}h\cos L\cos \mathrm{\λ}-(R_N+h)\sin L\cos \mathrm{\λ\δL}-(R_N+h)\cos L\sin \mathrm{\λ\δ\λ}\\ \mathrm{\δy}=\mathrm{\δ}h\cos L\sin \mathrm{\λ}-(R_N+h)\sin L\sin \mathrm{\λ\δL}-(R_N+h)\cos L\cos \mathrm{\λ\δ\λ}\\ \mathrm{\δz}=\mathrm{\δ}h\sin L+[R_N(1-e^2)+h]\cos \mathrm{L\δL}\end{array}\right.$

量测方程为Z_3×1＝H_3×18X_18×1+V_ΔR(3×1)

式中，

设 $H_1=\left[\begin{array}{ccc}{a}_{11}& a_{12}& a_{13}\\ a_{21}& a_{22}& a_{23}\\ a_{31}& a_{32}& a_{33}\end{array}\right],$ 其中a_ij(i＝1,2,3；j＝1,2,3)为矩阵H₁的元素

H₁非零元素如下:

a_i1＝-(R_N+h)sin L cos λe_i1-(R_N+h)sin L sin λe_i2+[R_N(1-e²)+h]e_i3

a_i2＝-(R_N+h)cos L sin λe_i1-(R_N+h)cos L cos λe_i2

a_i3＝cos L cos λe_i1+cos L sin λe_i2+sin Le_i3  (i＝1,2,3)

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

与现有技术相比，本发明具有如下优点：

(1)解决了SINS系统误差随时间积累的问题，保证了AUV在水下长期自主导航定位的精度，同时避免了GPS及其他无线电定位系统的使用，为水下作业节约时间和能耗，提高了AUV水下作业效率。

(2)本发明重点介绍了SINS与LBL紧组合，对惯性导航系统与声学系统组合应用的研究有一定的意义。

附图说明

图1为SINS/LBL紧组合定位系统原理框图；

图2为长基线水声定位系统LBL示意图；

图3为水听器节点定位示意图。

具体实施方式

下面结合附图，进一步阐明本发明。

如图1所示，本发明由安装在AUV上的捷联惯性导航系统SINS(1)、布放在海底的长基线水声定位系统LBL(2)和数据处理单元三大部分组成。数据处理单元包括SINS两两基元与AUV斜距差计算模块(3)、SINS/LBL紧组合模块(4)和校正模块(5)。通过采用SINS与LBL紧组合的方法完成AUV水下自主导航，具体实现步骤如下：

(1)对惯性测量元件(IMU)输出数据通过捷联解算获得AUV位置信息，用地球大地坐标P_SINS(L_S,λ_S,h_S)表示，并将P_SINS(L_S,λ_S,h_S)转化为用地球直角坐标P_SINS(x_S,y_S,z_S)表示。

所述的SINS(1)系统包括IMU(Inertial Measurement Unit，惯性测量单元)元件及捷联解算模块，IMU元件用于得到惯性数据，捷联解算模块用于通过捷联解算，得到导航信息，其中包括位置信息P_SINS。

1)SINS姿态矩阵及姿态角计算

采用四元数法计算姿态矩阵，根据欧拉定理,动坐标系相对参考坐标系的方位等效于动坐标系绕某个等效转轴转动一个角度θ，如果用u表示等效转轴方向的单位矢量，则动坐标系的方位完全由u和θ两个参数来确定。

用u和θ可构造一个四元数：

$Q=\cos \frac{\mathrm{\θ}}{2}+u\sin \frac{\mathrm{\θ}}{2}$

对上式求导并化简可得四元数微分方程：

$Q\left(\stackrel{\·}{q}\right)=\frac{1}{2}{M}^{*}\left({\mathrm{\ω}}_b\right)Q\left(q\right)$

式中 $M^{*}\left({\mathrm{\ω}}_b\right)=\left[\begin{array}{cccc}0& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& 0& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& 0& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}\\ {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& -{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& 0\end{array}\right]$

根据毕卡逼近法求解四元数微分方程得：

$q\left(t\right)=\{\cos \frac{\mathrm{\Δ}{\mathrm{\θ}}_0}{2}I+\frac{\sin \frac{\mathrm{\Δ}{\mathrm{\θ}}_0}{2}}{\mathrm{\Δ}{\mathrm{\θ}}_0}[\mathrm{\Δ\θ}\left]\right\}q\left(0\right)$

式中

$\mathrm{\Δ}{\mathrm{\θ}}_0=\sqrt{\mathrm{\Δ}{\mathrm{\θ}}_x^2+\mathrm{\Δ}{\mathrm{\θ}}_y^2+\mathrm{\Δ}{\mathrm{\θ}}_z^2}$

$\left[\mathrm{\Δ\θ}\right]={\∫}_{t_1}^{t_1+h}{M}^{*}\left({\mathrm{\ω}}_{\mathrm{nb}}^b\right)\mathrm{dt}=\left[\begin{array}{cccc}0& -\mathrm{\Δ}{\mathrm{\θ}}_x& -\mathrm{\Δ}{\mathrm{\θ}}_y& -\mathrm{\Δ}{\mathrm{\θ}}_z\\ \mathrm{\Δ}{\mathrm{\θ}}_x& 0& \mathrm{\Δ}{\mathrm{\θ}}_z& -\mathrm{\Δ}{\mathrm{\θ}}_y\\ \mathrm{\Δ}{\mathrm{\θ}}_y& -\mathrm{\Δ}{\mathrm{\θ}}_z& 0& \mathrm{\Δ}{\mathrm{\θ}}_x\\ \mathrm{\Δ}{\mathrm{\θ}}_z& \mathrm{\Δ}{\mathrm{\θ}}_y& -\mathrm{\Δ}{\mathrm{\θ}}_x& 0\end{array}\right]$

式中

${\mathrm{\Δ\θ}}_i={\∫}_t^{t+h}{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bi}}\mathrm{dt},i=x,y,z.$

令地球坐标系相对惯性坐标系的自转角速度为ω_ie，(其值为15.04088°/h)，L表示当地纬度,λ表示当地经度，则

ω_ie ⁿ：地球坐标系相对惯性坐标系的自转角速度在地理坐标系中的矢量，为：

${\mathrm{\ω}}_{\mathrm{ie}}^n={\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\cos L& {\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right]}^T$

ω_ie ^b：地球坐标系相对惯性坐标系的自转角速度在载体坐标系中的矢量，为：

${{\mathrm{\ω}}_{\mathrm{ie}}}^b=C_n^b{{\mathrm{\ω}}_{\mathrm{ie}}}^n$

式中的姿态矩阵在载体静止时，由初始角度决定；当载体相对地理坐标系转动时，姿态矩阵跟着变化，由四元数即时修正后求得(下同)。

ω_en ⁿ:地理坐标相对地球坐标系转动角速度在地理坐标系中的矢量，为：

${\mathrm{\ω}}_{\mathrm{en}}^n={\left[\begin{array}{ccc}-V_N/R_N& V_E/R_E& V_E\tan L/R_E\end{array}\right]}^T$

V_E、V_N分别为载体运动的东向和北向速度；

R_N为参考椭球体子午面内的曲率半径，R_N＝R_e(1-2e+3e sin² L)；

R_E为垂直子午面的法线平面内的曲率半径，R_E＝R_e(1+e sin² L)；

其中R_e为参考椭球体的长轴半径；e为椭球的椭圆度。

又因为, $\stackrel{\·}{L}=V_N/R_N,\stackrel{\·}{\mathrm{\λ}}=V_E/\left(R_E\cos L\right)$ 则

${\mathrm{\ω}}_{\mathrm{en}}^n={\left[\begin{array}{ccc}-\stackrel{\·}{L}& \stackrel{\·}{\mathrm{\λ}}\cos L& \stackrel{\·}{\mathrm{\λ}}\sin L\end{array}\right]}^T$

ω_en ^b:地理坐标相对地球坐标系转动角速度在载体坐标系中的矢量，为：

${{\mathrm{\ω}}_{\mathrm{en}}}^b=C_n^b{{\mathrm{\ω}}_{\mathrm{en}}}^n$

ω_ib ^b：陀螺输出角速度，记为

${\mathrm{\ω}}_{\mathrm{ib}}^b={\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{ib}}^{\mathrm{bx}}& {\mathrm{\ω}}_{\mathrm{ib}}^{\mathrm{by}}& {\mathrm{\ω}}_{\mathrm{ib}}^{\mathrm{bz}}\end{array}\right]}^T$

ω_nb ^b：载体坐标系相对地理坐标系的转动角速度在载体坐标系中的矢量，记为

${\mathrm{\ω}}_{\mathrm{nb}}^b={\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bx}}& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{by}}& {\mathrm{\ω}}_{\mathrm{nb}}^{\mathrm{bz}}\end{array}\right]}^T$

则可得

ω_nb ^b＝ω_ib ^b-ω_ie ^b-ω_en ^b

四元数即时修正后，根据下式可由四元数的元实时更新姿态矩阵

$C_n^b=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2+q_0{q}_3)& 2(q_1{q}_3-q_0{q}_2)\\ 2(q_1{q}_2-q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3+q_0{q}_1)\\ 2(q_1{q}_3+q_0{q}_2)& 2(q_2{q}_3-q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$

从姿态阵中即可提取实时姿态角

2)SINS速度计算

载体坐标系中的比力矢量为f^b，则地理坐标系中有：

$f^n=C_b^n{f}^b$

式中的方向余弦矩阵在载体静止时，由初始角度决定；当载体相对地理坐标系转动时，方向余弦矩阵跟着变化，由四元数即时修正后求得。

载体在惯导系内的比力方程为：

${\stackrel{\·}{V}}^n=f^n-(2{\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×V^n+g^n$

写成分量形式有：

$\left[\begin{array}{c}{\stackrel{\·}{V}}_E\\ {\stackrel{\·}{V}}_N\\ {\stackrel{\·}{V}}_U\end{array}\right]=\left[\begin{array}{c}{f}_E\\ f_N\\ f_U\end{array}\right]+\left[\begin{array}{ccc}0& (\stackrel{\·}{\mathrm{\λ}}+2{\mathrm{\ω}}_{\mathrm{ie}})\sin L& -(\stackrel{\·}{\mathrm{\λ}}+2{\mathrm{\ω}}_{\mathrm{ie}})\cos L\\ -(\stackrel{\·}{\mathrm{\λ}}+2{\mathrm{\ω}}_{\mathrm{ie}})\sin L& 0& -\stackrel{\·}{L}\\ (\stackrel{\·}{\mathrm{\λ}}+2{\mathrm{\ω}}_{\mathrm{ie}})\cos L& \stackrel{\·}{L}& 0\end{array}\right]\left[\begin{array}{c}{V}_E\\ V_N\\ V_U\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ -g\end{array}\right]$

式中：fⁿ为载体加速度在导航坐标系上的投影，fⁿ＝[f_E f_N f_U]^T；Vⁿ表示船体在导航坐标系中的速度矢量，Vⁿ＝[V_E V_N V_U]^T；gⁿ为重力加速度矢量，gⁿ＝[0 0 -g]^T。

积分上式，即可求得运载体在导航坐标系上的各个速度分量V_E、V_N、V_U。

3)位置计算

得到经纬度高度的微分方程可表示如下：

$\left\{\begin{array}{c}\stackrel{\·}{L}=\frac{V_N}{R_N+h}\\ \stackrel{\·}{\mathrm{\λ}}=\frac{V_E}{(R_E+h)\cos L}\\ \stackrel{\·}{h}=V_U\end{array}\right.$

式中，h为高度。

积分上式的经纬度高度的更新公式即可得到经纬度和高度：

$\left\{\begin{array}{c}L=\∫\stackrel{\·}{L}\mathrm{dt}+L\left(0\right)\\ \mathrm{\λ}=\∫\stackrel{\·}{\mathrm{\λ}}\mathrm{dt}+\mathrm{\λ}\left(0\right)\\ h=\∫\stackrel{\·}{h}\mathrm{dt}+h\left(0\right)\end{array}\right.$

则得到位置P(λ,L,h)。

4)将3)得到的AUV在地球直角坐标系的坐标P_SINS(L_S,λ_S,h_S)转化为其在地球大地坐标系的坐标P_SINS(x_S,y_S,z_S)。

可由公式 $\left\{\begin{array}{c}x=(R_N+h)\cos L\cos \mathrm{\λ}\\ y=(R_N+h)\cos L\sin \mathrm{\λ}\\ z=[R_N(1-e^2)+h]\sin L\end{array}\right.$

获得P_SINS(x_S,y_S,z_S)。

式中:R_N为参考椭球体子午面內的曲率半径，R_N＝R_e(1-2e+3e sin² L)

R_E为垂直子午面內的曲率半径，R_E＝R_e(1+e sin² L)

其中：R_e为参考椭球体的长轴半径；e为椭球的椭圆度。

(2)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之间的斜距之差 ${\mathrm{\ρ}}_{\mathrm{SINSi}}=\sqrt{{(x_S-x_i)}^2+{(y_S-y_i)}^2+{(z_S-z_i)}^2}-\sqrt{{(x_S-x_0)}^2+{(y_S-y_0)}^2+{(z_S-z_0)}^2}(i=\mathrm{1,2,3}).$

所述的长基线水声定位系统LBL(2)由布放在海底的四个位置已知的水听器组成，如图2所示，各水听器之间的距离为4km。如图3所示，利用母船，采用超短基线系统对水听器进行精确定位，计算精确坐标值。母船上安装有GPS、IMU和罗经，母船底安装有换能器基阵。根据超短基线系统计算出每个水听器在换能器基阵坐标下的相对位置，结合母船GPS位置、母船姿态以及各安装误差等因素可以计算出各水听器节点在地球坐标下的绝对位置。

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}{\mathrm{\ρ}}_{\mathrm{SINSi}}=\sqrt{{(x-x_i)}^2+{(y-y_i)}^2+{(z-z_i)}^2}-\sqrt{{(x-x_0)}^2+{(y-y_0)}^2+{(z-z_0)}^2}\\ +\frac{\∂{\mathrm{\ρ}}_{\mathrm{SINSi}}}{\∂x}\mathrm{\δx}+\frac{\∂{\mathrm{\ρ}}_{\mathrm{SINSi}}}{\∂y}\mathrm{\δy}+\frac{\∂{\mathrm{\ρ}}_{\mathrm{SINSi}}}{\∂z}\mathrm{\δz}\end{array}$

设 $\begin{array}{c}\frac{\∂{\mathrm{\ρ}}_{\mathrm{SINSi}}}{\∂x}=\frac{x_S-x_i}{\sqrt{{(x_S-x_i)}^2+{(y_S-y_i)}^2+{(z_S-z_i)}^2}}-\frac{x_S-x_0}{\sqrt{{(x_S-x_i)}^2+{(y_S-y_0)}^2+{(z_S-z_0)}^2}}\\ =\frac{x_S-x_i}{R_i}-\frac{x_S-x_0}{R_0}=G_{\mathrm{ix}}-G_{0x}\end{array}$

同理 $\frac{\∂{\mathrm{\ρ}}_{\mathrm{SINSi}}}{\∂y}=\frac{y_S-y_i}{R_i}-\frac{y_S-y_0}{R_0}=G_{\mathrm{iy}}-G_{0y}$

$\frac{\∂{\mathrm{\ρ}}_{\mathrm{SINSi}}}{\∂z}=\frac{z_S-z_i}{R_i}-\frac{z_S-z_0}{R_0}=G_{\mathrm{iz}}-G_{0z},i=\mathrm{1,2,3}.$

其中， $R_i=\sqrt{{(x_s-x_i)}^2+{(y_s-y_i)}^2+{(z_s-z_i)}^2}(i=\mathrm{0,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

于是：

ρ_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

(3)SINS/LBL紧组合

1)建立LBL斜距差模型

由于时延差测量、声传播的多途径效应等将引起斜距差测量有误差，为简化模型，可认为斜距差误差是由常值偏置和随机噪声组成，则LBL水听器i(i＝1,2,3)与AUV的斜距与水听器0与AUV的斜距之差可表示为：

$\left\{\begin{array}{c}{\mathrm{\ΔR}}_{\mathrm{meas}}=\mathrm{\ΔR}+\mathrm{\δR}+{\mathrm{\ν}}_{\mathrm{\ΔR}}\\ \mathrm{\δ}\stackrel{\·}{R}=0\end{array}\right.$

式中，ΔR_meas为LBL水听器i(i＝1,2,3)与AUV的斜距与水听器0与AUV的斜距之差，ΔR为斜距差真值，δR＝[δR₁ δR₂ δR₃]^T为随机常值，ν_δR(t)～N(0,Q_ΔR)为高斯白噪声。

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为紧组合系统噪声向量。

根据捷联惯性导航系统长期工作时的误差特点，选择位置误差、速度误差、姿态误差、陀螺漂移和加速度计零偏作为状态量：

X_SINS＝[δV_E δV_N δV_U φ_E φ_N φ_U δL δL δh ▽_bx ▽_by ▽_bz ε_bx ε_by ε_bz]^T

式中，δV_E、δV_N、δV_U分别是捷联东向、北向、天向的速度误差，分别是捷联东向、北向、天向的失准角，δL、δλ、δh分别是捷联纬度、经度、高度误差，三个位置误差由地球坐标系描述，▽_bx、▽_by、▽_bz是捷联加表三个轴向的偏置误差，ε_bx、ε_by、ε_bz是捷联陀螺的三个轴向漂移。

X_LBL＝[δR₁ δR₂ δR₃]^T

式中，δR₁、δR₂、δR₃分别为LBL水听器i(i＝1,2,3)与AUV的斜距与水听器0与AUV的斜距之差的随机常值误差。

系统噪声阵 $W_{\mathrm{SINS}}={\left[\begin{array}{ccccccccccccccc}{\mathrm{\ω}}_{V_E}& {\mathrm{\ω}}_{V_N}& {\mathrm{\ω}}_{V_U}& {\mathrm{\ω}}_{{\mathrm{\φ}}_E}& {\mathrm{\ω}}_{{\mathrm{\φ}}_N}& {\mathrm{\ω}}_{{\mathrm{\φ}}_U}& {\mathrm{\ω}}_{\mathrm{\δL}}& {\mathrm{\ω}}_{\mathrm{\δ\λ}}& {\mathrm{\ω}}_{\mathrm{\δh}}& 0& 0& 0& 0& 0& 0\end{array}\right]}^T$

W_LBL＝[0 0 0]^T

系统状态转移矩阵 $F_{\mathrm{SINS}}=\left[\begin{array}{cc}{F}_{9\×9}& C_{9\×6}\\ 0_{6\×9}& 0_{6\×6}\end{array}\right]$

式中， $F_{9\×9}=\left[\begin{array}{ccccccccc}{F}_{11}& F_{12}& F_{13}& 0& -f_U& f_N& F_{17}& 0& 0\\ F_{21}& F_{22}& F_{23}& f_U& 0& f_E& f_{27}& 0& 0\\ F_{31}& F_{32}& 0& -f_N& f_E& 0& F_{37}& 0& 0\\ 0& F_{42}& 0& 0& F_{45}& F_{46}& 0& 0& 0\\ F_{51}& 0& 0& F_{54}& 0& F_{56}& F_{57}& 0& 0\\ F_{61}& 0& 0& F_{64}& F_{65}& 0& F_{67}& 0& 0\\ 0& F_{72}& 0& 0& 0& 0& 0& 0& 0\\ F_{81}& 0& 0& 0& 0& 0& F_{87}& 0& 0\\ 0& 0& 1& 0& 0& 0& 0& 0& 0\end{array}\right]$

其中：F_ij为F_9×9的元素,

R_N为参考椭球体子午面內的曲率半径，R_N＝R_e(1-2e+3e sin² L)

R_E为垂直子午面內的曲率半径，R_E＝R_e(1+e sin² L)

其中：R_e为参考椭球体的长轴半径；e为椭球的椭圆度。

$\begin{array}{cc}{F}_{11}=\frac{V_N}{R_E+h}\mathrm{tgL}-\frac{V_U}{R_E+h}& F_{12}=2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_E+h}\tan L\end{array}$

$\begin{array}{cc}{F}_{13}=-2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_E+h}& F_{17}=(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_N+h}{\sec }^{2}L)V_N+2{\mathrm{\ω}}_{\mathrm{ie}}\sin L{V}_U\end{array}$

$\begin{array}{cc}{F}_{21}=-2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_E+h}\mathrm{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{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_E+h}{\sec }^{2}L)V_E\end{array}$

$\begin{array}{cc}{F}_{31}=2({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_E+h})& F_{32}=\frac{2V_N}{R_N+h}\end{array}$

F₃₇＝-2ω_ie cos LV_E  $F_{42}=-\frac{1}{R_N+h}$

$\begin{array}{cc}{F}_{45}={\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_E+h}\mathrm{tgL}& F_{46}=-({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_E+h})\end{array}$

$\begin{array}{cc}{F}_{51}=\frac{1}{R_E+h}& F_{54}=-({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_E}{R_E+h}\mathrm{tgL})\end{array}$

$F_{56}=-\frac{V_N}{R_N+h}$  F₅₇＝-ω_ie sin L

$\begin{array}{cc}{F}_{61}=\frac{1}{R_E+h}\mathrm{tgL}& F_{64}={\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_E}{R_E+h}\end{array}$

$\begin{array}{cc}{F}_{65}=\frac{V_N}{R_N+h}& F_{67}={\mathrm{\ω}}_{\mathrm{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 \mathrm{LtgL}$

$C_{9\×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(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斜距差： $\begin{array}{c}{\mathrm{\ρ}}_{\mathrm{SINSi}}=R_i-R_0+(G_{\mathrm{ix}}-G_{0x})\mathrm{\δx}+(G_{\mathrm{iy}}-G_{0y})\mathrm{\δy}+(G_{\mathrm{iz}}-G_{0z})\mathrm{\δz}\\ =R_i-R_0+e_{\mathrm{ix}}\mathrm{\δx}+e_{\mathrm{iy}}\mathrm{\δy}+e_{\mathrm{iz}}\mathrm{\δz}\end{array}$

LBL斜距差 ${\mathrm{\ρ}}_{\mathrm{LBLi}}=R_i-R_0+\mathrm{\δ}{R}_i+{\mathrm{\ν}}_{{\mathrm{\ΔR}}_i}$

则量测可写成 $\mathrm{\δ}{\mathrm{\ρ}}_i={\mathrm{\ρ}}_{\mathrm{SINSi}}-{\mathrm{\ρ}}_{\mathrm{LBLi}}=e_{\mathrm{iz}}\mathrm{\δx}+e_{\mathrm{iy}}\mathrm{\δy}+e_{\mathrm{iz}}\mathrm{\δz}-\mathrm{\δ}{R}_i-{\mathrm{\ν}}_{{\mathrm{\ΔR}}_i}(i=\mathrm{1,2,3})$

则有：

$\mathrm{\δ\ρ}=\left[\begin{array}{c}{\mathrm{\δ\ρ}}_1\\ {\mathrm{\δ\ρ}}_2\\ {\mathrm{\δ\ρ}}_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}\mathrm{\δx}\\ \mathrm{\δy}\\ \mathrm{\δz}\\ {\mathrm{\δR}}_1\\ {\mathrm{\δR}}_2\\ {\mathrm{\δR}}_3\end{array}\right]+\left[\begin{array}{c}{\mathrm{\ν}}_{{\mathrm{\δR}}_1}\\ {\mathrm{\ν}}_{{\mathrm{\δR}}_2}\\ {\mathrm{\ν}}_{{\mathrm{\δR}}_3}\end{array}\right]$

当系统采用地球直角坐标系(Ox_ey_ez_e)作为导航坐标系时，可用上式构造系统量测方程。实际应用中是以经纬度和高度定位的，因此要把dx,dy,dz用dl,dλ,dh表示。

由 $\left\{\begin{array}{c}x=(R_N+h)\cos L\cos \mathrm{\λ}\\ y=(R_N+h)\cos L\sin \mathrm{\λ}\\ z=[R_N(1-e^2)+h]\sin L\end{array}\right.$

$\left\{\begin{array}{c}\mathrm{\δx}=\mathrm{\δ}h\cos L\cos \mathrm{\λ}-(R_N+h)\sin L\cos \mathrm{\λ\δL}-(R_N+h)\cos L\sin \mathrm{\λ\δ\λ}\\ \mathrm{\δy}=\mathrm{\δ}h\cos L\sin \mathrm{\λ}-(R_N+h)\sin L\sin \mathrm{\λ\δL}-(R_N+h)\cos L\cos \mathrm{\λ\δ\λ}\\ \mathrm{\δz}=\mathrm{\δ}h\sin L+[R_N(1-e^2)+h]\cos \mathrm{L\δL}\end{array}\right.$

量测方程为Z_3×1＝H_3×18X_18×1+V_ΔR(3×1)

式中，

$H_1=\left[\begin{array}{ccc}{a}_{11}& a_{12}& a_{13}\\ a_{21}& a_{22}& a_{23}\\ a_{31}& a_{32}& a_{33}\end{array}\right],$ 其中a_ij(i＝1,2,3；j＝1,2,3)为矩阵H₁的元素

H₁非零元素如下:

a_i1＝-(R_N+h)sin L cos λe_i1-(R_N+h)sin L sin λe_i2+[R_N(1-e²)+h]e_i3

a_i2＝-(R_N+h)cos L sin λe_i1-(R_N+h)cos L cos λe_i2

a_i3＝cos L cos λe_i1+cos L sin λe_i2+sin Le_i3  (i＝1,2,3)

4)系统状态方程及量测方程的离散化

X_k＝φ_k,k-1X_k-1+Γ_k-1W_k-1

Z_k＝H_kX_k+V_k

式中，X_k为k时刻的状态向量，也就是被估计矢量；Z_k为k时刻的测量序列；W_k-1为k-1时刻的系统噪声；V_k为k时刻的测量噪声序列；Φ_k,k-1为k-1时刻到k时刻的一步状态转移矩阵；Γ_k-1是系统噪声输入矩阵，H_k为k时刻的测量矩阵，

利用标准卡尔曼滤波方程计算状态的最优估计：

状态一步预测向量

X_k/k-1＝φ_k,k-1X_k-1

状态估值计算

X_k＝X_k/k-1+K_k(Z_k-H_kX_k/k-1)

滤波增益

K_k＝P_k/k-1H_k ^T(H_kP_k/k-1H_k ^T+R_k)^-1

一步预测均方误差矩阵

$P_{k/k-1}={\mathrm{\φ}}_{k,k-1}{P}_{k-1}{\mathrm{\φ}}_{k,k-1}^T+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T$

估计均方误差方程

$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T$

(4)校正

根据滤波得到的状态估计，通过下述方法进行校正。

1)速度和位置校正

下次滤波前，每次捷联解算得到的速度和位置均通过下式进行校正：

$\left\{\begin{array}{c}{\hat{V}}_E=V_E-\mathrm{\δ}{\hat{V}}_E\\ {\hat{V}}_N=V_N-\mathrm{\δ}{\hat{V}}_N\\ {\hat{V}}_U=V_U-\mathrm{\δ}{\hat{V}}_U\end{array}\right.$

$\left\{\begin{array}{c}\hat{L}=L-\mathrm{\δ}\hat{L}\\ \widehat{\mathrm{\λ}}=\mathrm{\λ}-\mathrm{\δ}\widehat{\mathrm{\λ}}\\ \hat{h}=h-\mathrm{\δ}\hat{h}\end{array}\right.$

2)惯性仪表输出校正

下次滤波前，每次捷联解算时所需的惯性仪表输出在使用前通过下式进行校正：

${\widehat{\mathrm{\ω}}}_{\mathrm{ib}}^{\mathrm{bk}}={\mathrm{\ω}}_{\mathrm{ib}}^{\mathrm{bk}}-{\mathrm{\ϵ}}_{\mathrm{bk}},k=x,y,z$

3)姿态矩阵、四元数校正

姿态校正：下次滤波前，按下式对每次捷联解算得到的进行校正。

$C_n^b=C_{n^{\′}}^b{C}_n^{n^{\′}}$

$C_n^{n^{\′}}=\left[\begin{array}{ccc}1& {\widehat{\mathrm{\φ}}}_U& -{\widehat{\mathrm{\φ}}}_U\\ -{\widehat{\mathrm{\φ}}}_U& 1& {\widehat{\mathrm{\φ}}}_E\\ {\widehat{\mathrm{\φ}}}_N& -{\widehat{\mathrm{\φ}}}_E& 1\end{array}\right]$

四元数校正：因为捷联解算采用的是四元数算法，算法中是是采用四元数进行迭代更新的，所有还需对四元数进行校正。四元数可由更新的姿态矩阵转换得到。

Claims (4)

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。

3.根据权利要求1所述的一种基于SINS/LBL紧组合的AUV水下导航定位方法，其特征在于：所述SINS/LBL紧组合模块(4)的具体实现步骤如下：

(1)建立LBL斜距差模型；由于时延差测量、声传播的多途径效应等将引起斜距差测量有误差，为简化模型，可认为斜距差误差是由常值偏置和随机噪声组成，则LBL水听器i(i＝1,2,3)与AUV的斜距与水听器0与AUV的斜距之差可表示为：

式中，ΔR_meas为LBL水听器i(i＝1,2,3)与AUV的斜距与水听器0与AUV的斜距之差，ΔR为斜距差真值，δR＝[δR₁ δR₂ δR₃]^T为随机常值，ν_δR(t)～N(0,Q_ΔR)为高斯白噪声；

(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为紧组合系统噪声向量。根据捷联惯性导航系统长期工作时的误差特点，选择位置误差、速度误差、姿态误差、陀螺漂移和加速度计零偏作为状态量：

X_SINS＝[δV_E δV_N δV_U φ_E φ_N φ_U δL δL δh ▽_bx ▽_by ▽_bz ε_bx ε_by ε_bz]^T

式中，δV_E、δV_N、δV_U分别是捷联东向、北向、天向的速度误差，分别是捷联东向、北向、天向的失准角，δL、δλ、δh分别是捷联纬度、经度、高度误差，三个位置误差由地球坐标系描述，▽_bx、▽_by、▽_bz是捷联加表三个轴向的偏置误差，ε_bx、ε_by、ε_bz是捷联陀螺的三个轴向漂移；

X_LBL＝[δR₁ δR₂ δR₃]^T

式中，δR₁、δR₂、δR₃分别为LBL水听器i(i＝1,2,3)与AUV的斜距与水听器0与AUV的斜距之差的随机常值漂移；

系统噪声阵

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为椭球的椭圆度；

F₃₇＝-2ω_ie cosLV_E            

           F₅₇＝-ω_ie sinL

C_ij为姿态转移矩阵的元素

F_LBL＝0_3×3；

(3)建立SINS/LBL紧组合量测方程；

紧组合系统采用SINS推算的水听器与AUV的斜距差与LBL测量得到的斜距差之差作为观测量；在紧组合系统中，设LBL测得的斜距差为ρ_LBLi,水底应答器阵基元的位置为P(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δz

LBL斜距差

则量测可写成

则有：

当系统采用地球直角坐标系(Ox_ey_ez_e)作为导航坐标系时，可用上式构造系统量测方程；实际应用中是以经纬度和高度定位的，因此要把dx,dy,dz用dl,dλ,dh表示；

由

量测方程为Z_3×1＝H_3×18X_18×1+V_ΔR(_3×1)

式中，

设其中a_ij(i＝1,2,3；j＝1,2,3)为矩阵H₁的元素

H₁非零元素如下:

a_i1＝-(R_N+h)sinLcosλe_1x-(R_N+h)sinLsinλe_1y+[R_N(1-e²)+h]cosLe_1z

a_i2＝-(R_N+h)cosLsinλe_2x-(R_N+h)cosLcosλe_2y

a_i3＝cosLcosλe_3x+cosLsinλe_3y+sinLe_3z  (i＝1,2,3)。

4.根据权利要求1所述的一种基于SINS/LBL紧组合的AUV水下导航定位方法，其特征 在于：所述的校正模块(5)根据SINS/LBL紧组合模块(4)的卡尔曼滤波结果对SINS(1)进行校正，最终得到精确的AUV位置信息P_AUV。