CN109737956A - 一种基于双应答器的sins/usbl相位差紧组合导航定位方法 - Google Patents

Abstract

一种基于双应答器的SINS/USBL相位差紧组合导航定位方法，由安装在AUV上的捷联惯导系统和超短基线定位系统组成，超短基线系统的水听器接收基阵和惯导系统固联在一起且已完成安装误差的标定，两个应答器布放在海底，建立超短基线系统水听器与应答器间斜距相位观测模型；双应答器结构的超短基线定位系统，将斜距相位方程分别在水听器层面以及应答器层面进行差分处理；然后将双层差分处理后的斜距相位差分方程提炼为组合导航系统观测方程进行滤波。本发明采用双差处理方法能够有效抵消超短基线定位系统中的共性误差，且采用超短基线相位差作为观测量进行紧组合，避免了USBL直接解算位置带来的坐标转换误差和基阵偏移误差，可有效提高AUV组合导航定位系统的精度。

Description

一种基于双应答器的SINS/USBL相位差紧组合导航定位方法

技术领域

本发明属于水下导航技术，具体涉及一种基于双应答器的SINS/USBL相位差紧组合导航定位方法。

背景技术

在整个世界范围内，人们在水下航行器对巨大的海洋资源的高精度勘测能力方面的关注度越来越高。水下航行器作为自主式水下移动载体，具有活动范围广、体积小、重量轻、噪音低、隐蔽性高等优势，现已成为各国家海洋技术研究的一个重要方向。为保证自主水下航行器能够顺利地完成水下的相关任务，要求其配备的导航系统具有水下长期自主的导航定位和返航能力。

目前已有的定位技术中，惯性导航系统(INS)因其隐蔽性强、短时精度高、导航参数全和自主性特点，在水下航行器中得到广泛应用。但惯导系统定位误差随时间积累需要外部信息来对其进行修正。其中水声定位技术是通过水下声波信号获取水下航行器相对于已知应答器的距离和方位信息，基于几何定位原理确定水下航行器的位置，是AUV定位导航采用的主要手段。目前常用的3种声学基线定位系统具有各自的优势和特点，其中超短基线更具有便携性和独立性，因此成为水声定位设备发展的热点。

随着水下航行器的演变，单一的惯性导航系统和水声定位系统已不能满足导航载体的需要，以惯性导航为核心，辅以超短基线定位技术的组合导航系统的研究已成为当前水下航行器导航的重要发展方向；另一方面，考虑到超短基线位置辅助的惯导/超短基线松组合方案对于系统精度的提升效果是有限的，因此，研究捷联惯导系统与超短定位系统更深层次的组合导航方法，对于水下航行器高精度长时航行具有重要的意义。

对超短基线声学定位系统研究较早的是挪威的Kongsberg Simrad公司，该公司于1997年推出了世界领先水平的高精度长程超短基线定位系统—HiPAP35O，作用距离可达3000米，距离测量精度优于20cm，随后推出HiPAP500，作用水深达4000m，测距精度优于20cm；新近推出的HiPAPP700，作用水深达10000米，测距精度优于50cm，也是世界上唯一工作水深上万米的长程超短基线定位系统。另外，法国OCEANO Technologies公司推出的Posidonia 6000长程超短基线定位系统，工作水深6000m，最大作用距离8000m，在6000m水深300度开角范围内，测距精度为0.5％，询问频率为8-14kHZ，应答频率为14-18kHz，该系统已经成功推向市场。由于国内对AUV技术开始研究的时间较晚，与欧美等发达国家相比，在很多方面都存在着不足。但随着不断的投入也取得了很大的进展。国内具有代表性的水声定位系统是哈尔冰工程大学水声工程学院研制出了四种基于声学定位的系统：深水重潜装潜水员超短基线定位系统、“探索者”号水下机器人超短基线定位系统，灭雷具配套水声跟踪定位装置、长程超短基线定位系统。

在水下航行器组合定位算法研究中，一方面，目前研究大多以惯性/长基线组合定位方法为主，对于惯性/超短基线组合定位算法的研究不够充分；另一方面，在现有的惯性/超短基线组合定位方法中，主要集中在超短基线位置松组合方面，但这种位置松组合方法无法有效抑制水下环境噪声对超短基线定位系统精度的影响，对于组合导航系统定位精度的提升效果非常有限，无法保证组合导航系统的定位精度。

发明内容

发明目的：本发明的目的在于解决现有组合方法技术中定位精度的不足，提供一种基于双应答器的SINS/USBL相位差紧组合导航定位方法。

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

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

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

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

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

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

作为本发明进一步改进，所述步骤(2)中建立组合导航系统状态方程的具体步骤为：

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

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

系统状态方程为；

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

作为本发明进一步改进，所述步骤(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)中建立相位差紧组合观测方程的具体步骤为：

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

其中

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

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

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

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

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

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

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

其中，

本发明通过建立一种双应答器的超短基线定位系统结构，并构造了应答器与水听器之间的斜距相位观测方程，一方面通过将斜距相位观测方程分别在水听器和应答器两个层面上进行差分处理，可以消除超短基线定位系统中的时钟误差，声线弯曲误差等共性误差，一定程度上减小超短基线系统中的噪声对于定位精度的影响；另一方面通过斜距相位双差方程与SINS系统状态方程共同构建紧组合滤波模型，直接利用了超短基线系统相位差等原始信号，避免了超短基线系统直接解算带来的坐标转换误差，提高了系统定位精度。

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

(1)本申请通过对水声信号在水下环境传播特性及超短基线定位系统的误差分析，建立了超短基线定位系统中水听器和应答器之间的斜距相位方程，该方程将超短基线定位系统中存在的各种误差均考虑在内；

(2)本申请在前期建立的水听器及应答器间的斜距相位方程基础上，通过对斜距相位方程分别在水听器层面和应答器层面进行差分处理，通过这种差分处理方式，可以消除超短基线定位系统中的水听器时钟误差，应答器时钟误差；并在一定程度上减弱超短基线定位系统中的声速弯曲误差及多径误差；

(3)本申请通过前期建立的双差方程作为组合导航系统观测方程，并以超短基线定位系统的相位差信息为观测量，将SINS/USBL组合导航系统进行组合导航滤波，并将滤波结果用于纠正捷联惯导系统定位误差，提高组合导航系统精度。

附图说明

图1为本发明的整体流程图；

图2为本发明中组合导航系统结构示意图；

图3为本发明中的坐标系关系图；

具体实施方式

本发明一种基于双应答器的SINS/USBL相位差紧组合导航定位方法该方法解决了捷联惯导系统误差随时间积累的问题，一方面采用双差处理方法能够有效抵消超短基线定位系统中的共性误差，另一方面采用超短基线相位差作为观测量进行紧组合，避免了USBL直接解算位置带来的坐标转换误差和基阵偏移误差，可有效提高AUV组合导航定位系统的精度。

如图1所示，本发明由安装在AUV上的捷联惯导系统(SINS)和超短基线定位系统(USBL)组成，其中超短基线系统的水听器接收基阵和惯导系统固联在一起且已完成安装误差的标定，两个应答器布放在海底。通过建立双应答器超短基线定位系统结构，并设计基于双应答器的SINS/USBL相位差紧组合导航定位方法完成AUV水下自主导航，具体实现步骤如下。

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

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

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

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

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

一、步骤(2)中捷联惯性导航系统SINS通过捷联解算得到自主水下航行器AUV的位置信息，并建立组合导航系统状态方程，具体实现方式如下：

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

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

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

式中 表示载体系相对于导航系的转动角速度在导航系下的投影。

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

式中I为四阶单位矩阵，

式中

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

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

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

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

陀螺输出角速度，记为

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

则可得

其中姿态矩阵由归一化后的姿态四元数求得，求解过程如下：

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

(7)速度计算

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

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

写成分量形式有：

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

(8)位置解算

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

式中，h为高度。

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

则得到惯导系统的位置。

(9)捷联惯导系统状态方程

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

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

系统状态方程为

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

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

(10)水听器基阵及双应答器的位置关系如图2所示，两个应答器用m,n表示，在地球直角坐标系中的位置坐标分别为(x_m y_m z_m)、(x_n y_n z_n)。四个水听器用h、j、k、l表示，在地球直角坐标系中的位置坐标分别为(x_h y_h z_h)、(x_j y_j z_j)、(x_k y_k z_k)、(x_l y_l z_l)。

(11)惯导解算的载体位置为(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)。

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

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

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

(13)将步骤(12)中的斜距方程在(x_Ih y_Ih z_Ih)处按泰勒级数展开，只取一次项得：

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

则有

三、步骤(4)中根据步骤(2)建立的相位斜距观测方程，结合步骤(1)建立的超短基线系统结构，在双应答器与多个水听器间的相位斜距观测方程进行双差处理，从而得到相位差紧组合观测方程，具体实现方式如下：

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

其中

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

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

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

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

(18)将步骤(16)中基于应答器m的单差方程和步骤(17)中基于应答器n的单差方程相减得水听器h，j和应答器m，n之间基于相位差的双差方程

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

(19)将步骤(18)推导的所有双差方程综合整理得到

其中，

四个水听器与两个应答器构成的组合导航系统观测方程为：

Z(t)＝H(t)X(t)+V(t)

式中，Z(t)为组合导航系统观测方程的观测矢量，H(t)为组合系统观测方程的观测矩阵，V(t)为组合系统的观测噪声。

H(t)＝[0_6×3 0_6×3 H₁₃ 0_6×3 0_6×3

四、步骤(5)中根据步骤(2)和步骤(4)建立的状态方程及观测方程，设计卡尔曼滤波，滤波输出校正SINS位置输出，提高组合导航系统定位精度，具体实现方式如下：

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

X_k＝Φ_k,k-1X_k-1+W_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时刻的一步状态转移矩阵；H_k为k时刻的测量矩阵，

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

状态一步预测向量

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

状态估值计算

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

滤波增益

一步预测均方误差矩阵

估计均方误差方程

(22)校正SINS

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

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

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

以上所述，仅是本发明的较佳实施例而已，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。

Claims (4)

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

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

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

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

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

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

2.根据权利要求1所述的基于双应答器的SINS/USBL相位差紧组合导航定位方法，其特征在于，所述步骤(2)中建立组合导航系统状态方程的具体步骤为：

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

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

系统状态方程为；

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

3.根据权利要求1所述的基于双应答器的SINS/USBL相位差紧组合导航定位方法，其特征在于，所述步骤(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.根据权利要求1所述的基于双应答器的SINS/USBL相位差紧组合导航定位方法，其特征在于，所述步骤(4)中建立相位差紧组合观测方程的具体步骤为：

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

其中

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

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

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

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

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