CN104316045A - 一种基于sins/lbl的auv水下交互辅助定位系统及定位方法 - Google Patents

Abstract

本发明提供了一种基于SINS/LBL的AUV交互辅助定位系统及定位方法，其中定位系统由安装在AUV上的捷联惯性导航系统SINS、布放在海底的长基线水声定位系统LBL和数据处理单元组成：长基线水声定位系统LBL由布放在海底的四个已知位置的水听器组成，数据处理单元包括广义互相关计算模块、筛选相关峰模块、位置解算模块、时延差解算模块以及卡尔曼滤波器模块；捷联惯性导航系统SINS包括IMU元件及IMU处理单元。本发明既解决了SINS系统长时误差积累问题，也补偿了由声信号在水中多路径传播所造成的定位误差，保证了AUV在水下长期自主的定位导航的精度，还避免了GPS及其他无线电定位系统的使用，解决了AUV水下作业需浮上水面校正误差的问题，为水下作业节约时间和能耗，提高了AUV水下作业效率。

Description

一种基于SINS/LBL的AUV水下交互辅助定位系统及定位方法

技术领域

本发明涉及一种基于SINS/LBL的AUV水下交互辅助定位系统及定位方法，特别适用于水下自主航行器AUV的跟踪定位。

背景技术

AUV(Autonomous Underwater Vehicle，自主式水下航行器)是一种可以完成水下探测、攻击、运载、打捞等多种功能的水下工具，要求在水下具有长期自主的定位导航和返航能力，并具有隐蔽性。水下高精度定位技术是保证其顺利解决水下作业的前提和关键。SINS(Strapdown Inertial Navigation Systems，捷联式惯性导航系统)具有自主性、隐蔽性、抗干扰性，数据更新频率高，且在短时间内具有较高的精度。然而，惯导系统的积分式工作原理使得定位误差随着时间累积增大，难以进行长时间的精确定位，必须利用外部传感器的信息对系统进行校正。LBL(Long Base Line，长基线)水声定位系统通常是由基线长度为几千米的海底应答器阵和被定为载体上的问答机组成，利用水下目标与海底阵元之间的距离信息来求解目标位置，可在局部区域内对水下载体进行精确定位，但是由于应答距离较远，数据的更新频率较低，同时，声音信号在水中的多路径传播问题也会造成较大的定位误差。

目前，应用于AUV水下定位导航的技术主要采用了基于SINS/DVL(Doppler Velocity Log，多普勒测速仪)/GPS(Global Positioning System，全球定位系统)的组合导航系统。由于电磁波在水中严重衰减的特性迫使航行器只有上浮接近水面才能获取卫星导航系统的导航信息，因此采用“潜航-水面校正-潜航”的导航模式，利用SINS/DVL组合导航系统作为水下导航系统，将SINS/GPS组合导航系统作为水面校正系统。由于水下环境复杂，DVL的探测范围有时无法到达海底，仍然会不可避免地产生累积误差，为了保证定位精度，AUV必须在必要的时候，中断潜行，上浮到水面利用GPS信息进行校正。如果航行器在水下相当深处航行，浮到水面需要很多时间和能耗，严重影响AUV水下作业的效率。而采用LBL水下定位系统，作用范围受到限制，由于水中声信号传播具有多路径效应，采用广义相关法解算时延差会出现相关峰模糊现象，造成较大定位误差。

发明内容

为了克服现有AUV水下导航技术的缺陷，本发明提供一种基于SINS/LBL的AUV交互辅助定位系统及定位方法。

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

一种基于SINS/LBL的AUV水下交互辅助定位系统，其特征在于：该系统由安装在AUV上的捷联惯性导航系统SINS、布放在海底的长基线水声定位系统LBL和数据处理单元组成：

所述的长基线水声定位系统LB由布放在海底的四个已知位置的水听器组成，所述的数据处理单元包括广义互相关计算模块、筛选相关峰模块、位置解算模块、时延差解算模块以及卡尔曼滤波器模块块组成；

所述的捷联惯性导航系统SINS包括IMU元件及IMU处理单元，所述的IMU元件用于得到惯性数据，所述的IMU处理单元用于通过捷联解算，得到包括位置信息P_SINS的导航信息；

所述的水听器用于接收AUV上声源发出的声信号；所述的广义互相关计算模块用于求取两两水听器接收信号的一组模糊相关峰；所述的时延差解算模块用于根据位置信息P_SINS计算两两水听器接收声源信号的时延差t′_ij；所述的筛选相关峰模块用于根据时延差t′_ij在所述的一组模糊相关峰中选出最接近真值的相关峰作为主峰并得到对应主峰的时延差值；所述的位置解算模块用于根据所述的时延差值计算得到AUV位置信息P_LBL；所述的卡尔曼滤波器模块用于根据位置信息P_LBL和位置信息P_SINS对捷联惯性导航系统SINS进行校正并得到精确的AUV位置信息P_AUV。

所述的时延差解算模块计算时延差t′_ij的方法如下：

(1)根据长基线水声定位系统LBL中水听器位置P_i(x_i,y_i,z_i)和P_SINS(x,y,z)计算得到水听器与AUV的距离 $D_i=\sqrt{{(x_i-x)}^2+{(y_i-y)}^2+{(z_i-z)}^2};$

(2)计算任意两个应答器与AUV之间的距离差D_ij＝|D_i-D_j|(i≠j)；

(3)计算两两水听器接收信号时延差其中c_ij是声信号在水中传播的等效声速。

所述广义互相关计算模块的具体实现步骤如下：

(1)声源产生声波信号x(t)，第i个水听器接收到的信号为x_i(t)＝α_ix(t-τ_i)+n_i(t)，第j个水听器接收到的信号为x_j(t)＝α_jx(t-τ_j)+n_j(t)，其中α_i、α_j为声信号在水中传播的衰减系数，n_i(t)、n_j(t)为互不相关的噪声信号，τ_i、τ_j为传播时间；

(2)x_i(t)与x_j(t)的互相关函数为其中τ＝τ_j-τ_i，表示到达时间差，T表示观测时间，由于声音在水下传播具有多径效应，因此会产生多个相关峰。

一种AUV水下交互辅助定位系统的定位方法，其特征在于，步骤如下：

(1)长基线水声定位系统LBL的水听器接收AUV上声源发出的声信号，并经过广义互相关计算模块求取两两水听器接收信号的一组模糊相关峰；

(2)捷联惯性导航系统SINS通过捷联解算得到相应的导航信息，并提供AUV位置信息P_SINS给时延差解算模块，计算两两水听器接收声源信号的时延差t′_ij，将其作为筛选相关峰模块的依据；

(3)筛选相关峰模块根据时延差t′_ij从所述的一组模糊相关峰中选出时延差最接近t′_ij的相关峰作为主峰，得到对应主峰的时延差t_ij；

(4)根据筛选出的时延差t_ij，进行基于TDOA的AUV定位解算，得到AUV位置信息P_LBL；

(5)将P_LBL和P_SINS的差值作为外部观测信息输入到卡尔曼滤波器模块进行滤波，滤波结果对SINS进行校正，最终得到精确的AUV位置信息P_AUV。

有益效果

与现有技术相比，本发明通过将LBL水声定位系统提供的定位信息与SINS的导航信息相互进行深层次的融合，通过SINS提供的位置信息修正水声定位中的相关峰时延解算误差，再由LBL定位系统提供的位置信息校正SINS导航误差，既解决了SINS系统长时误差积累问题，也补偿了由声信号在水中多路径传播所造成的定位误差，保证了AUV在水下长期自主的定位导航的精度，还避免了GPS及其他无线电定位系统的使用，解决了AUV水下作业需浮上水面校正误差的问题，为水下作业节约时间和能耗，提高了AUV水下作业效率。

附图说明

图1为SINS/LBL交互定位系统结构图；

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

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

图4(a)图为SINS/DVL组合位置误差曲线，(b)图为SINS/LBL组合位置误差曲线。

具体实施方式

下面结合附图对本发明进一步详细说明。

如图1所示，本发明由安装在AUV上的捷联惯性导航系统SINS3、布放在海底的长基线水声定位系统LBL1和数据处理单元组成，其中数据处理单元包括广义互相关计算模块2、筛选相关峰模块5、位置解算模块6、时延差解算模块4以及卡尔曼滤波器模块7，通过采用SINS和LBL交互辅助定位为AUV提供连续的高精度位置信息，具体通过下列步骤实现：

(1)长基线水声定位系统LBL1由布放在海底的四个已知位置的水听器组成，水听器接收AUV上声源发出的声信号，并经过广义互相关计算模块2，通过求取两两水听器接收信号的相关峰得到对应的时延差。由于声信号在水下传播具有多径效应，因此会产生一组模糊相关峰,需从中选出最接近真值的相关峰，得到对应的时延差值；

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

所述广义互相关计算模块2主要由以下步骤完成：

1)声源产生声波信号x(t)，第i个水听器接收到的信号为x_i(t)＝α_ix(t-τ_i)+n_i(t)，第j个水听器接收到的信号为x_j(t)＝α_jx(t-τ_j)+n_j(t)，其中α_i、α_j为声信号在水中传播的衰减系数，n_i(t)、n_j(t)为互不相关的噪声信号，τ_i、τ_j为传播时间；

2)x_i(t)与x_j(t)的互相关函数为其中τ＝τ_j-τ_i，表示到达时间差，T表示观测时间；

(2)捷联惯性导航系统SINS3通过捷联解算得到相应的导航信息，并提供AUV位置信息P_SINS给时延差解算模块4，计算两两水听器接收声源信号的时延差t′_ij，将其作为筛选相关峰模块5的依据；

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

SINS捷联解算计算位置信息：

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

采用四元数法计算姿态矩阵，根据欧拉定理,动坐标系相对参考坐标系的方位等效于动坐标系绕某个等效转轴转动一个角度θ，如果用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{\Δ\θ}}_0}{2}I+\frac{\sin \frac{{\mathrm{\Δ\θ}}_0}{2}}{{\mathrm{\Δ\θ}}_0}[\mathrm{\Δ\θ}]\}q\left(0\right)$

式中

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

$\left[\mathrm{\Δ\θ}\right]={\∫}_{t_1}^{t_1+h}{M}^{*}\left({\mathrm{\ω}}_{\mathrm{nb}}^b\right)\mathrm{dt}=\left[\begin{array}{cccc}0& -{\mathrm{\Δ\θ}}_x& -{\mathrm{\Δ\θ}}_y& -{\mathrm{\Δ\θ}}_z\\ {\mathrm{\Δ\θ}}_x& 0& {\mathrm{\Δ\θ}}_z& -{\mathrm{\Δ\θ}}_y\\ {\mathrm{\Δ\θ}}_y& -{\mathrm{\Δ\θ}}_z& 0& {\mathrm{\Δ\θ}}_x\\ {\mathrm{\Δ\θ}}_z& {\mathrm{\Δ\θ}}_y& -{\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+3esin²L)；

R_E为垂直子午面的法线平面内的曲率半径，R_E＝R_e(1+esin²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)速度计算

得到的载体坐标系中的比力矢量为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通过以下步骤实现：

1)根据LBL系统中水听器位置P_i(x_i,y_i,z_i)和P_SINS(x,y,z)计算得到水听器与AUV的距离 $D_i=\sqrt{{(x_i-x)}^2+{(y_i-y)}^2+{(z_i-z)}^2};$

2)计算任意两个应答器与AUV之间的距离差D_ij＝|D_i-D_j|(i≠j)；

3)计算两两水听器接收信号时延差其中c_ij是声信号在水中传播的等效声速。

可以用上次定位的两个接收点到目标源的距离差除以时延差的值作为等效声速，具体计算如下：

1)设上一次(在t_k-1时刻)的定位结果为(x(k-1),y(k-1),z(k-1))，LBL基阵的应答器的位置为P_i(x_i,y_i,z_i)，则应答器与AUV的距离为：

$R_i(k-1)=\sqrt{{(x_i-x(k-1))}^2+{(y_i-y(k-1))}^2+{(z_i-z(k-1))}^2}$

2)计算不同位置的应答器i、j与AUV之间的距离差ΔR_ij(k-1)＝|R_i(k-1)-R_j(k-1)|，筛选得到的时延差为t_ij(k-1)，则得到当前等效声速为 $c_{\mathrm{ij}}\left(k\right)=\frac{{\mathrm{\ΔR}}_{\mathrm{ij}}(k-1)}{t_{\mathrm{ij}}(k-1)}.$

(3)在筛选相关峰模块5中，根据时延差t′_ij，从步骤(1)所述的一组模糊相关峰中选出时延差最接近t′_ij的相关峰作为主峰，得到对应主峰的时延差t_ij；

(4)根据筛选出的时延差t_ij，进行基于LBL的AUV定位解算，得到AUV位置信息P_LBL；所述的LBL定位解算模块6采用测距交汇法列出双曲线数学模型方程由以下步骤组成：

1)根据时延差t_ij、等效声速c_ij可列出方程：|R_i-R_j|＝c_ijt_ij(i,j＝1,2,3,4,i≠j)，一共六个方程组成的方程组

2)解出上述方程组的最小二乘解得到定位位置信息。

(5)将P_LBL和P_SINS的差值作为外部观测信息输入到卡尔曼滤波器7进行滤波，滤波结果对SINS3进行校正，最终得到精确的AUV位置信息P_AUV。

所述的卡尔曼滤波器7校正过程由以下步骤组成：

1)建立的SINS系统误差模型及状态方程：

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

式中，δV_E、δV_N分别是东向、北向速度误差；φ_E、φ_N、φ_U分别是东向、北向、天向失准角；δL、δλ分别是纬度、经度误差；▽_bx、▽_by分别是x、y向的加速度计偏置；ε_bx、ε_by、ε_bz分别是x、y、z向的陀螺漂移。选取东北天坐标系作为导航坐标系，载体坐标系x轴沿水下潜器横轴指向右舷，y轴沿航行器纵轴指向前，z轴垂直于x与y轴所确定的平面构成右手坐标系。状态方程为

$\stackrel{\·}{X}=\mathrm{FX}+W$

系统噪声矩阵

$F=\left[\begin{array}{cccccccccccc}\frac{V_N}{R}\mathrm{tgL}& F_{12}& 0& -f_u& f_N& F_{16}& 0& C_{11}& C_{21}& 0& 0& 0\\ F_{21}& 0& f_u& 0& -f_E& F_{26}& 0& C_{12}& C_{22}& 0& 0& 0\\ 0& -\frac{1}{R}& 0& F_{34}& F_{35}& 0& 0& 0& 0& -C_{11}& -C_{21}& -C_{31}\\ \frac{1}{R}& 0& F_{43}& 0& -\frac{V_N}{R}& F_{46}& 0& 0& 0& -C_{12}& -C_{22}& -C_{32}\\ \frac{\mathrm{tgL}}{R}& 0& F_{53}& \frac{V_N}{R}& 0& F_{56}& 0& 0& 0& -C_{13}& {-C}_{23}& -C_{33}\\ 0& \frac{1}{R}& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ \frac{\sec L}{R}& 0& 0& 0& 0& F_{76}& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

其中：

2)建立量测方程

Z＝HX+V，其中H＝[0 0 0 0 0 1 1 0 0 0 0 0]，Z是每次测量水声定位得到的位置信息与SINS定位信息差值。

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

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时刻的测量矩阵，

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

状态一步预测向量

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$

5)校正SINS

利用当前的误差最优估计可以立即校正SINS每次的由测量数据得到的状态量。

速度和位置校正可以通过惯导系统对这两个参数的估计值与估计误差简单相减来修正：

$X_c=\hat{X}-X$

式中，X_c是校正后的状态量。

本发明分别采用SINS/DVL定位与SINS/LBL交互辅助定位，在下面的仿真条件下进行仿真：陀螺的常值漂移为0.04°/h，随机漂移为加速度计的零偏为50μg，随机漂移为载体以10m/s的速度沿正北方向做匀速直线运动，得到结果如图4所示。1.5小时时，SINS/DVL组合纬度方向定位误差为200m，经度方向定位误差为29m，高度方向定位误差为5m。而SINS/LBL组合能有效将3个轴方向上的位置误差控制在15m以内，使得1.5小时总定位误差为25.98m。因此SINS/LBL组合能够有效抑制位置误差的发散，在导航过程中能有有效提高定位精度。

Claims (4)

1.一种基于SINS/LBL的AUV水下交互辅助定位系统，其特征在于：该系统由安装在AUV上的捷联惯性导航系统SINS(3)、布放在海底的长基线水声定位系统LBL(1)和数据处理单元组成：

所述的长基线水声定位系统LBL(1)由布放在海底的四个已知位置的水听器组成，所述的数据处理单元包括广义互相关计算模块(2)、筛选相关峰模块(5)、位置解算模块(6)、时延差解算模块(4)以及卡尔曼滤波器模块(7)；

所述的捷联惯性导航系统SINS(3)包括IMU元件及IMU处理单元，所述的IMU元件用于得到惯性数据，所述的IMU处理单元用于通过捷联解算，得到包括位置信息P_SINS的导航信息；

所述的水听器用于接收AUV上声源发出的声信号；所述的广义互相关计算模块(2)用于求取两两水听器接收信号的一组模糊相关峰；所述的时延差解算模块(4)用于根据位置信息P_SINS计算两两水听器接收声源信号的时延差t'_ij；所述的筛选相关峰模块(5)用于根据时延差t'_ij在所述的一组模糊相关峰中选出最接近真值的相关峰作为主峰并得到对应主峰的时延差值；所述的位置解算模块(6)用于根据所述的时延差值计算得到AUV位置信息P_LBL；所述的卡尔曼滤波器模块(7)用于根据位置信息P_LBL和位置信息P_SINS对捷联惯性导航系统SINS(3)进行校正并得到精确的AUV位置信息P_AUV。

2.根据权利要求1所述的一种基于SINS/LBL的AUV水下交互辅助定位系统，其特征在于：所述的时延差解算模块(4)计算时延差t'_ij的方法如下：

(1)根据长基线水声定位系统LBL中水听器位置P_i(x_i,y_i,z_i)和P_SINS(x,y,z)计算得到水听器与AUV的距离 $D_i=\sqrt{{(x_i-x)}^2+{(y_i-y)}^2+{(z_i-z)}^2};$

(2)计算任意两个应答器与AUV之间的距离差D_ij＝|D_i-D_j|(i≠j)；

(3)计算两两水听器接收信号时延差其中c_ij是声信号在水中传播的等效声速。

3.根据权利要求1所述的一种基于SINS/LBL的AUV水下交互辅助定位系统，其特征在于：所述广义互相关计算模块(2)的具体实现步骤如下：

(1)声源产生声波信号x(t)，第i个水听器接收到的信号为x_i(t)＝α_ix(t-τ_i)+n_i(t)，第j个水听器接收到的信号为x_j(t)＝α_jx(t-τ_j)+n_j(t)，其中α_i、α_j为声信号在水中传播的衰减系数，n_i(t)、n_j(t)为互不相关的噪声信号，τ_i、τ_j为传播时间；

(2)x_i(t)与x_j(t)的互相关函数为 $R_{x_i,x_j}\left(\mathrm{\τ}\right)=\frac{1}{T-\mathrm{\τ}}{\∫}_{\mathrm{\τ}}^T{x}_i\left(t\right)x_j(t-\mathrm{\τ})\mathrm{dt},$ 其中τ＝τ_j-τ_i，表示到达时间差，T表示观测时间，由于声音在水下传播具有多径效应，因此会产生多个相关峰。

4.一种基于权利要求1所述AUV水下交互辅助定位系统的定位方法，其特征在于，步骤如下：

(1)长基线水声定位系统LBL(1)的水听器接收AUV上声源发出的声信号，并经过广义互相关计算模块(2)求取两两水听器接收信号的一组模糊相关峰；

(2)捷联惯性导航系统SINS(3)通过捷联解算得到相应的导航信息，并提供AUV位置信息P_SINS给时延差解算模块(4)，计算两两水听器接收声源信号的时延差t'_ij，将其作为筛选相关峰模块(5)的依据；

(3)筛选相关峰模块(5)根据时延差t'_ij从(1)所述的一组模糊相关峰中选出时延差最接近t'_ij的相关峰作为主峰，得到对应主峰的时延差t_ij；

(4)根据筛选出的时延差t_ij，进行基于TDOA的AUV定位解算(6)，得到AUV位置信息P_LBL；

(5)将P_LBL和P_SINS的差值作为外部观测信息输入到卡尔曼滤波器模块(7)进行滤波，滤波结果对SINS(3)进行校正，最终得到精确的AUV位置信息P_AUV。