一种改进的水下三阵元辅助惯导系统的定位算法

张 涛，胡贺庆

（1. 东南大学 仪器科学与工程学院，南京 210096；

2. 东南大学 微惯性仪表与先进导航技术教育部重点实验室，南京 210096）

一种改进的水下三阵元辅助惯导系统的定位算法

张 涛，胡贺庆

（1. 东南大学 仪器科学与工程学院，南京 210096；

2. 东南大学 微惯性仪表与先进导航技术教育部重点实验室，南京 210096）

针对水下惯导系统定位误差随时间发散的缺陷，设计了一种基于DVL、声学定位辅助惯导的组合导航系统。声学定位系统由安装在水下潜器上的换能器阵和布放在海底的三阵元定位基阵组成，传统长基线定位水下至少要布放四个阵元，三个阵元无法直接定位求解。根据三阵元测得的距离信息和惯导系统输出的位置信息建立距离耦合模型，给出了惯导、DVL和声学定位系统的状态方程和量测方程；同时针对水下声速垂直分布特性，提出了一种等效声速的修正方法。仿真结果表明：在三阵元定位基阵作用范围内，组合导航系统能有效修正惯导位置误差，定位误差小于4 m，具有良好的适用性。

水下潜器；惯导系统；三阵元基阵；距离耦合

21世纪将是海洋的世纪，随着对海洋中生物和矿物资源的需求增大，水下定位导航技术也成为当今研究的重点。水下潜器在海洋数据采集、灾难预警、战场监视和矿产探测等方面发挥着重要的作用，具有活动范围大、潜水深度深、体积小、成本优等诸多优点[1-2]。为了确保潜器水下任务的顺利完成，就需要通过一定的技术对其进行精确定位导航。惯性导航系统因其隐蔽性、自主性强以及能获取载体完备运动信息的独特优点而在水下导航中得到广泛应用，但由于惯性导航系统在长时间工作之后，导航误差会随着时间累积而累积增大，从而使得惯导的长时间精确导航成为一个需要突破的难题。为了实现长航时高精度的导航目标，需要进行误差补偿或利用外界位置信息对其进行周期的调整和校正，从而提高其精度[3-5]。

在水下环境中，电磁波信号具有严重衰减的特性；在深海或者冰面下，采用GPS导航及其他无线电定位方式会使用较长的时间来浮出水面，甚至可能暴露自己的当前位置，极大程度的降低了潜器的隐蔽性以及对信息采集的实时性。上述水下环境中遭遇的困境严重限制了卫星导航系统或其它无线电导航系统在水下航行器中的应用。声波在海洋中的传播相比其他任何物理场能够传播更远的距离，具有明显的优势。随着声学定位技术的进步，水声高精度定位技术有了长足的进步。水下长基线系统基线长，定位精度高，但是一般布放的基阵个数较多，因而布放、校准和回收需要较长时间，且这些作业过程较为复杂[6-10]。

针对水下惯导系统误差存在随时间累积、GPS系统无法在水下定位以及传统长基线系统复杂等问题，本文设计了一种基于 DVL和三阵元定位辅助惯导的组合导航系统，利用惯导位置定位信息，建立了惯导系统和三阵元定位基阵的距离耦合模型，同时针对水下声速垂直分布特性，提出了一种等效声速的修正方法。仿真结果表明，该方法有效可行，能够在保证导航精度的前提下，简化声学定位系统，降低成本，具有一定的实际可用性。

1 系统原理与结构

1.1 三阵元基阵布放和校准

水下三阵元基阵精确定位的前提是阵元的绝对地理位置已知，且得到准确校准，常利用超短基线测出阵元在声学基阵坐标系下的位置，通过坐标转化，可以得到阵元的绝对地理位置。校准原理可用下式表示：

式中：Ps为阵元的绝对地理坐标，为目标所求；PGPS为测量船的位置，可以由GPS精确定位；p为大地坐标系下阵元相对测量船的位置坐标。

如图1所示，测量船上安装有GPS、高精度惯性测量单元、罗经系统，用于提供测量船的精确地理位置和姿态角信息。测量船上安装有超短基线系统，声学基阵接收阵元发出的信号，根据超短基线测距侧向原理计算出阵元在声学基阵坐标系下的位置坐标为。设地理坐标系为n系，声学基阵坐标系为a系，载体坐标系为b系，由声学基阵坐标系到载体坐标系的坐标转换可以得到阵元在载体坐标系下的位置，δp为基阵坐标系在载体坐标系的安装误差，为声学基阵坐标系到载体坐标系的变换矩阵。由载体坐标系到地理坐标系的坐标变换可以得到在大地坐标系下阵元相对测量船的位置，由式(1)即可得到阵元的绝对地理位置。

图1 阵元校准定位模型Fig.1 Array element calibration positioning model

1.2 三阵元基阵定位原理

水下定位系统模型主要有球面交汇模型和双曲面交汇模型，在实际情况中，一般很少用双曲线交汇模型，而采用高精度的同步式球面交汇定位模型，主要通过计算信号的传播时间和声速的乘积来获得水下潜器和阵元的距离，然后解算方程组来对目标进行定位。定位模型如图2所示。

图2 三阵元定位模型Fig.2 Three-element array positioning model

2 距离耦合组合导航模型

将式(2)中方程减消去二次项，得到如下线性方程：

由于该方程组有3个未知数，可知采用三阵元基阵定位系统，当水下潜器深度未知时，不能得到准确解，此时需要增加阵元的个数，至少需要4个阵元，才能得到唯一解。特别需要注意的是，当有较大测距误差时，四阵元基阵定位结果误差较大，可以通过增加阵元个数，计算方程组的最小二乘解得到定位结果。当阵元大于4个时，将式(3)表示成矩阵形式：

由该方程组根据最小二乘法得到最佳解为

增加阵元数量能有效提高声学定位的精度，但是水下阵元布放、校准和回收需要较长的时间，耗费较大的财力和物力，同时阵元数量的增加使得后端处理系统变得复杂。本文在在三阵元基阵定位基础上，利用惯导给出的位置定位信息，建立了惯导系统和三阵元基阵定位的距离紧耦合组合导航模型，避免了三阵元基阵定位求解模糊问题，同时能够有效修正惯导位置误差。

2.1 距离耦合组合导航模型状态方程

距离耦合组合导航算法误差状态模型主要包括惯导系统的误差模型和三阵元基阵定位误差模型。惯导系统误差模型主要是速度误差、姿态误差、位置误差、陀螺漂移、加表误差。三阵元基阵定位主要是距离测量误差，在三阵元基阵同步定位模型下，由于时钟同步误差、水下声速垂直分布等特性将会引起距离测量有误差，为简化模型，可将距离测量误差用一阶马尔科夫过程表示，则距离误差模型可表示为

式中：R为距离真值，δR为距离测量误差，τ、ω分别是一阶马尔科夫过程的相关时间和驱动白噪声，υ为高斯白噪声。状态方程可表示为

即：

式中：X为状态变量，F为状态转移矩阵，W为系统噪声矩阵。

选择速度误差、姿态误差、位置误差、加速度计零偏和陀螺漂移作为状态量：

式中：δVE、δVN、δVU分别是捷联东向、北向、天向的速度误差，Eφ、Nφ、Uφ分别是捷联东向、北向、天向的失准角，δL、δλ、δh分别是捷联纬度、经度、高度误差，∇bx、∇by、∇bz为捷联加表三个轴向的偏置误差，εbx、εby、εbz是捷联陀螺的三个轴向漂移。F可由SINS误差方程确定。

选择三阵元基阵到每个阵元的距离误差作为状态量：

2.2 距离耦合组合导航模型观测方程

导航系统观测方程为

式中：VSINS是惯导系统输出的速度信息，VDVL是DVL系统输出的速度信息，RSINS是惯导系统解算的位置与每个阵元之间的距离信息，Racou是三阵元基阵定位系统输出的距离信息。

式中：i=1,2,3。

相对水下航行器的真实位置(x,y,z)，将式(12)进行泰勒线性化得：

由三阵元基阵定位系统距离误差模型可知：

将式(18)中的δx、δy、δz用δL、δλ、δh表示为

由以上分析可得观测矩阵为

3 等效声速修正方法

由水下三阵元基阵定位原理可知，定位误差不仅与测量时间误差有关，还与水下声速有关。一般假设水下声速是恒定的，为1500 m/s，同时还假设声音是沿着直线传播的，但是这些假设都有一定的局限性，水下环境中，声速随着水下温度、压力以及盐度不断变化，从而导致测距产生误差。本文提出一种基于卡尔曼滤波的组合导航系统辅助的等效声速修正方法，该算法计算过程简单，能有效减小测距误差。

4 系统仿真及分析

4.1 水下Bellhop仿真模型

一般来说，水下声信道是时变、空变的随机信道，其必须用时变、空变的随机滤波器来描述，但是在大多数实际应用中，声信道可以看作是缓慢时变的相干多途信道，若观察或者处理的时间不是过分长，则声信道可以用时不变滤波器来描述。

水下声音传播可以简化为下式：

Bellhop是一个描述海洋环境中声压场的射线传播模型。由于采用了射线传播模型，使得针对海洋环境声压场获得的算法变得简单易于实现，其中具体使用了Gaussian和hat-shaped射线的几何特性以及物理传播规律。Bellhop可以输出包括声波传输能量损失、本征声线、到达时间序列等海洋中声速传播特征。同时利用环境文件对海洋中的声速、声源位置、水听器布放数量及位置等进行设置，可以得到水下信道响应。本次仿真实验中采用的环境文件为在某湖某区域采集的数据，声速分布如图3所示。

图3 某湖区域声速分布图Fig.3 Sound velocity distribution in a lake area

4.2 距离耦合组合导航模型仿真

为了验证算法定位效果，在Matlab环境下，分别对本文的三阵元距离耦合算法和常用的五阵元最小二乘算法进行仿真比较。为模拟实际海情，设水下航行器做三轴摇摆运动，摇摆参数如下：横摇幅度为 8°，摇摆周期为 8 s；纵摇幅度为8°，摇摆周期为 10 s；航向摇摆幅度为 6°，摇摆周期为 6 s。陀螺仪随机漂移和常值漂移均为 0.05 (°)/h，加速度计随机漂移为50μg，常值偏置为50μg；初始失准角分别为：纵摇角0.01°，横摇角0.01°，航向角0.01°；DVL速度误差为0.3 m/s；仿真时间为1500 s。

假设潜器初始位置为（119.995°, 32.995°），深度为10 m，以航向角30°且航速为1 m/s做匀速直线航行。由某湖试得到的水下声速分布数据，按照 Bellhop模型模拟阵元接收到的信号，阵元地理位置为（120.01°, 40.01°）、（120.01°, 40.00°）、（120.02°, 40.01°）、（120.01°, 40.02°）、（120°, 40°），深度均为30 m。分别采用三阵元距离耦合算法和五阵元最小二乘算法辅助惯性导航系统进行定位仿真。图4为水下航行器理想轨迹及阵元基阵布放图；图5～7分别为东向误差曲线对比图、北向误差曲线对比图、高度误差曲线对比图。

图4 水下航行器理想轨迹及阵元基阵布放图Fig.4 Real track and element array

图5 北向误差曲线对比图Fig.5 North error curves

图6 东向误差曲线比较图Fig.6 East error curves

图7 高度误差曲线比较图Fig.7 Height error curves

由图 5～7可知：当水下潜器仅采用惯导和 DVL组合导航系统时，水下潜器的位置误差会随着时间的推移产生发散；采用三阵元基阵定位时，组合导航系统的位置误差在4 m以内。本文提出的三阵元基阵距离耦合定位算法避免了在潜器深度未知时，位置求解模糊问题，能够在三阵元基阵定位范围内有效修正惯导位置误差。增加水下阵元个数，采用五阵元最小二乘算法的组合导航系统位置误差在2 m以内，可见增加阵元的个数对水下导航系统的定位精度改善明显，但是阵元个数的增加使声学定位系统变得复杂，同时阵元布放、回收和校准耗时耗力，经济适用性差。本文提出的三阵元基阵距离耦合定位算法，虽然精度不及五阵元定位精度高，但是系统简单实用，在三阵元基阵定位区域内仍能有效修正惯导位置误差，满足水下航行要求。

5 结 论

本文针对水下三阵元基阵定位技术的缺陷，建立了惯导、DVL和三阵元基阵定位的距离耦合组合导航模型，详细推导了距离耦合组合导航系统的状态方程和观测方程，同时，针对水下声速垂直分布特性，提出了提出一种基于卡尔曼滤波的组合导航系统辅助的等效声速修正方法。

该算法计算过程简单，能有效减小测距误差。通过系统仿真结果说明，三阵元基阵距离耦合算法能够避免传统三阵元基阵同步定位求解模糊问题，同时在三阵元基阵定位范围内能够有效修正惯导误差，保证水下潜器的导航精度，具有良好的适用性。

（References）：

[1] Gill B, Schipf D, Borth R, et al. Hybrid baseline localization for portable AUV navigation[C]//Oceans ’13 MTS/IEEE San Diego Conference. 2013: 1-10.

[2] Paull L, Saeedi S, Seto M, et al. AUV navigation and localization: A review[J]. IEEE Journal of Oceanic Engineering, 2014, 39(1): 131-149.

[3] 张涛, 徐晓苏, 李瑶, 等. 基于惯导及水下声学辅助系统的AUV容错导航技术[J]. 中国惯性技术学报, 2013, 21(4): 512-516. Zhang Tao, Xu Xiao-su, Li Yao, et al. AUV fault-tolerant technology based on inertial navigation and underwater acoustics assisted navigation system[J]. Journal of Chinese Inertial Technology, 2013, 21(4): 512-516.

[4] Larsen M B. High performance doppler-inertial navigation experimental results[J]. Journal of System Engineering and Electronics, 2012, 23(2): 256-264.

[5] Zhang T, Chen L, Li Y. AUV underwater positioning algorithm based on interactive assistance of SINS and LBL[J]. Sensors, 2016, 16(1): 1-22.

[6] Miller B S, Barlow J, Calderan S, et al. Validating the reliability of passive acoustic localisation: a novel method for encountering rare and remote Antarctic blue whales[J]. Endanger. Species Res, 2015, 26: 257-269.

[7] Zhang T, Shi H, Chen L, et al. AUV positioning method based on tightly coupled SINS/LBL for underwater acoustic multipath propagation[J]. Sensors, 2016, 16(3): 1-16.

[8] 王小峰. 水下航行器 INS/GPS/DVL 组合导航方法[J].鱼雷技术, 2010, 18(4): 287-290. Wang Xiao-feng. New application of fedearted filter in INS/GPS/DVL integrated navigation system for AUV[J]. Torpedo Technology, 2010, 18(4): 287-290.

[9] Chen Y, Zheng D, Miller P A, et al. Underwater inertial navigation with long baseline transceivers: A near-realtime approach[J]. IEEE Transactions on Control Systems Technology, 2016, 24(1): 240-251.

[10] Kashima T, Asada A, Ura T. The positioning system integrated LBL and SSBL using seafloor acoustic mirror transponder[C]//2013 IEEE International Conference on Underwater Technology Symposium. 2013: 1-4.

Improved positioning algorithm of inertial navigation system with aiding of underwater three-element array

ZHANG Tao, HU He-qing
(1. School of Instrument Science and Engineering, Southeast University, Nanjing 210096, China; 2. Key Lab of Micro Inertial Instruments and Advanced Navigation Technology of Education Ministry, Southeast University, Nanjing 210096, China)

Aiming at the error divergence of the inertial navigation system, an integrated navigation system based on INS/DVL and acoustic positioning is designed for underwater vehicle. The acoustic positioning system is made up of transducer array on underwater vehicles and three positioning array elements on seabed. Traditional LBL system should have at least four elements, while the location of three elements can’t be directly calculated. To overcome this problem, a range coupling mode is built based on the distance information measured by the three-element array and the positioning information outputted by the inertial navigation system. The state and measurement equations of inertial navigation, DVL and acoustic positioning system are given. In addition, an equivalent sound velocity correcting method is given based on the vertical distribution characteristics of underwater sound velocity. Simulation results show that the integrated navigation system can effectively correct the position error of the inertial navigation system, the positioning error is less than 4 m, and it has good applicability.

underwater vehicle; SINS; three-element array; range coupling

V249.3

A

1005-6734(2017)02-0192-05

10.13695/j.cnki.12-1222/o3.2017.02.010

2017-01-04；

2017-03-25

国家自然科学基金资助项目（51375088）；微惯性仪表与先进导航技术教育部重点实验室基金（201403）；优秀青年教师教学科研资助计划（2242015R30031）

张涛（1980—），男，博士、副研究员，从事组合导航。E-mail: ztandyy@163.com