[发明专利]一种基于双应答器的SINS/USBL相位差紧组合导航定位方法

权利要求书

1.一种基于双应答器的SINS/USBL相位差紧组合导航定位方法，由安装在AUV上的捷联惯导系统和超短基线定位系统组成，其特征在于：所述超短基线定位系统的水听器接收基阵和惯导系统固联在一起且已完成安装误差的标定，两个应答器布放在海底，通过双应答器与多个水听器之间构建相位双差方程，其具体步骤如下：

(1)超短基线定位系统由安装在AUV上的四个水听器组成的正方形基阵和两个布放在海底的应答器组成，换能器发送声波信号，应答器接收信号后经过一段延时发送应答信号，四个水听器接收到应答信号后，根据声波到达时间确定水听器与应答器间的斜距信息，以及每两个水听器间的相位差信息；

(2)捷联惯性导航系统SINS通过捷联解算得到自主水下航行器AUV的位置信息，并建立组合导航系统状态方程；

所述步骤(2)中建立组合导航系统状态方程的具体步骤为：

状态量由以下15个变量组成；

其中，φ_E，φ_N，φ_U是捷联惯导的三个失准角，δV_E，δV_N，δV_U是东北天三个方向上的速度误差，δL，δλ，δh是捷联惯导的三个位置误差在导航系下的描述，ε_x,ε_y,ε_z是陀螺的三个轴向漂移，是加速度计三个轴向的零偏误差；

系统状态方程为；

其中，X(t)为系统的状态向量，F(t)是惯导系统误差状态方程状态转移矩阵，W(t)是关于捷联惯导系统的噪声向量；

(3)根据水声信号在水下环境中的传播特性，分析超短基线定位系统的误差因素，并以此建立应答器相对于水听器的相位斜距观测方程；

所述步骤(3)中建立超短基线定位系统相位斜距观测方程的具体步骤为：

两个应答器用m,n表示，在地球直角坐标系中的位置坐标分别为(x_m y_m z_m)、(x_n y_nz_n)，四个水听器用h、j、k、l表示，在地球直角坐标系中的位置坐标分别为(x_h y_h z_h)、(x_j y_jz_j)、(x_k y_k z_k)、(x_l y_l z_l)；

惯导解算的载体位置为(x_I y_I z_I)，则利用惯导位置推算得到的四个水听器位置分别为(x_Ih y_Ih z_Ih)、(x_Ij y_Ij z_Ij)、(x_Ik y_Ik z_Ik)、(x_Il y_Il z_Il)；

水听器h与应答器m之间的相位斜距观测方程建立如下：

式中，λ为水声信号的波长，为水听器h与应答器m之间不足整周期的相位，为水听器h与应答器m之间整周期的相位，为水听器h与应答器m之间的斜距，c为水下声速，δt_h为水听器h的时钟误差，δt^m为应答器m的时钟误差，T^m为水下介质引起的声速变化等效延时，为多路径效应，为水听器h和应答器m间的测量误差；

其中，水听器h与应答器m之间的斜距定义为

将相位斜距观测方程中的斜距方程在(x_Ih y_Ih z_Ih)处按泰勒级数展开，只取一次项得：

其中表示惯导推算的水听器h的位置(x_Ih y_Ih z_Ih)与应答器m之间的距离，并记

则有

(4)根据步骤(3)建立的相位斜距观测方程，结合步骤(1)建立的超短基线定位系统结构，将双应答器与多个水听器间的相位斜距观测方程进行双差处理，从而得到相位差紧组合观测方程；

所述步骤(4)中建立相位差紧组合观测方程的具体步骤为：

建立水听器j与应答器m之间的相位斜距观测方程为：

其中

由于水听器h与水听器j之间距离较近，认为两个声速弯曲等效误差及多路径误差近似相等，将应答器m与水听器h和水听器j之间的相位斜距观测方程相减得,

式中，表示水听器h与水听器j相对于应答器m之间的非整周相位差，表示水听器h与水听器j相对于应答器m之间的整周相位差，

将两个斜距和分别用和表示，基于应答器m的相位单差方程转换为

同理，基于应答器n的单差方程为

将前述基于应答器m的单差方程和前述基于应答器n的单差方程相减得水听器h，j和应答器m，n之间基于相位差的双差方程

其中

同理可以推得基于水听器hk、hl、jk、kl与应答器m、n的双差方程；

将前述步骤推导的所有双差方程综合整理得到

其中，

(5)根据步骤(2)和步骤(4)建立的状态方程及观测方程，设计卡尔曼滤波模型，滤波结果校正SINS位置输出，提高组合导航系统定位精度。