AUV直线航迹下的单信标测距定位

曹俊， 郑翠娥， 孙大军， 张殿伦

(1.哈尔滨工程大学 水声工程学院，黑龙江 哈尔滨 150001； 2.哈尔滨工程大学 水声技术重点实验室，黑龙江 哈尔滨 150001)



AUV直线航迹下的单信标测距定位

曹俊1，2， 郑翠娥1，2， 孙大军1，2， 张殿伦1，2

(1.哈尔滨工程大学 水声工程学院，黑龙江 哈尔滨 150001； 2.哈尔滨工程大学 水声技术重点实验室，黑龙江 哈尔滨 150001)

常规的单信标测距定位基于的是非直线航迹，本文针对直线航迹下的单信标测距定位进行了研究。同时对于直线航迹，常规的直接降阶求解的算法已不适用，提出线性化迭代求解的算法对载体进行定位。另一方面，引入长基线阵列的概念，从构建虚拟阵列的角度，研究了不同误差源对虚拟阵列的影响。仿真分析了虚拟阵列基线总长度对定位影响、虚拟阵列信标个数对定位影响，以及载体相对于信标在不同位置时的定位精度分布。海试数据结果表明：所提出的线性化迭代求解的定位算法能获得较好的定位效果，单标定位结果与组合导航偏差在5 m之内。

单信标测距；水声定位；直线航迹；虚拟信标；虚拟阵列；无人水下航行器

随着世界经济的发展，人类已将资源开发和利用的重点转向海洋。海底光缆管线的铺设及其维护，水下载体作业时是直线航迹；海洋地貌绘制，水下载体作业时是梳形航迹；深水油气、天然气水合物及矿产资源的勘查，水下载体作业时是梳形航迹。水下载体的导航定位是顺利完成各项任务的首要前提和关键因素，基于单信标测距的定位是水下辅助导航的发展趋势，已成为国内外导航定位领域的研究重点[1-5]。文献[6]使用最小二乘法解算出无人水下航行器(unmanned underwater vehicle，UUV)的水平初始位置，但载体需要在3个不同的直线航迹上对信标测距才能够进行解算。文献[7]提出单信标定位用于UUV回收的想法，UUV在信标附近进行环形机动。文献[8]主要基于方形航迹利用协方差矩阵进行仿真分析。国内西北工业大学和哈尔滨工程大学两家单位从数学上研究单信标定位解算，(研究过程)既没有从定位的角度分析各种误差源对定位的影响，也没有真实试验数据的处理[9-10]。

自主式水下航行器(autonomous underwater vehicle,AUV)是UUV的一种，摆脱了系缆的牵绊。作业过程更加灵活。常规的解算方法中，需要航迹的航向必须要有改变，这也就是说当水下载体长时间直线航行时，很长一段时间内不能够利用声学进行单信标定位解算。

1 单信标测距定位

传统长基线(long base line，LBL)定位[11]，利用AUV至各信标间的距离信息，采用球面交汇模型解算AUV的位置。单信标测距定位如图1所示，水下布放的单只信标为AT，要定位的位置为AUV4。原理上，单次距离测量不能确定水下载体的具体位置，所以我们需要结合水下载体工作时的运动参数如速度和姿态来实时解算出水下载体的位置。根据姿态和航速，建立AUV位于AUV4时的虚拟信标阵列VT4、VT3、VT2和VT1[12]。从图中可以看出，虚拟信标阵列与传统长基线阵列一样。如果航向和航速均不存在误差，则虚拟信标阵列就没有误差，其定位精度与传统长基线完全一样。

图1 单信标测距定位Fig.1 Single beacon ranging

当AUV的航行轨迹为如图2所示的直线航迹时，真实信标为AT=[ATxATyATz]T，航向角为φ，测距周期为T，测距个数为n，要定位的位置为AUVn=[xayaza]T，AUV深度za通过深度传感器获得，AUVi处的测距信息为Ri。根据航向和航速，建立的虚拟信标为VTi=[xtiytizti]T，虚拟信标的总长度为L。对于所有测量点中的前n个测量点，要定位的位置是AUVn，前n-1个测量点是不求解的。对于第n个测量点，利用从1～n的信息用于解算；对于第n+1个测量点，利用从2～n+1的信息用于解算；依次类推。则从第n个测量点及以后，每个点都能够直接求解。

图2 直线航迹示意图Fig.2 Sketch of straight line track

第i个测距信息对应的定位方程为

(xa-xti)2+(ya-yti)2+(za-zti)2=Ri2

(1)

常规的LBL解算方法是经过降次处理，得到二元一次方程组：

B=AX

(2)

其中

Ai×2=[(xt(i+1)-xti)，(yt(i+1)-yti)]

当矩阵ATA为可逆阵时，根据最小二乘法，可得到最优解为

(3)

当航行轨迹为直线时，构建的虚拟信标也是直线，由虚拟信标构成的矩阵A的秩为1，小于未知数个数，方程无解。所以对于直线航迹，常规解算方法的系数矩阵不满秩，不能够进行解算。为了能够对直线航迹进行解算，令

(4)

将方程(4)利用泰勒级数展开进行线性化[13]，构成方程组为

N=MΔX

(5)

其中，初值X=(x0，y0)，i=1，2，…，n

当矩阵MTM为可逆阵时，根据最小二乘法，可得到最优解为：

(6)

要使方程有解，AUV航行轨迹不能为过信标的直线。因为当航行轨迹为过信标的直线时，矩阵M的秩为1，小于未知数个数，方程无解。对修正参数ΔX进行精度判断，多次迭代直至修正参数的精度高于直接观测量测距精度一个数量级。由第k次迭代到第k+1次迭代的公式为：Xk+1=Xk+ΔXk。

2 虚拟阵列对定位影响

定位方程式(1)中的已知量为虚拟信标的位置。下面引入长基线阵列的概念，将各虚拟信标构建成虚拟信标阵列，分析其对定位的影响。

2.1 构建虚拟阵列

载体姿态和载体速度是在载体坐标系下测量得到，而信标的位置是在大地坐标下，所以虚拟信标与实际布放的单只信标的关系式如下

(7)

(8)

式中：vx为载体东向速度，vy为载体北向速度，vz为载体天向速度，φ为载体航向，vf为载体前向速度，vl为载体左向速度，vv为载体垂向速度。

2.2 误差源对虚拟阵列影响

虚拟信标的位置由信标标定结果、载体前向速度、载体左向速度和载体航向角共同决定。下面分别分析这些误差源对定位的影响。

2.2.1 信标标定误差

当AT存在误差ΔAT=[ΔATxΔATy]T时，虚拟阵列信标的位置偏差为ΔVTi=ΔAT。

从公式可以看出，虚拟阵列的每一个虚拟信标都会存在同样大小的位置偏差，位置偏差的大小与信标标定误差相同。

2.2.2 载体前向速度误差

当vf存在误差Δvf时，引起的速度误差为

(9)

虚拟阵列信标的位置偏差为

(10)

其中，Δv=[Δvf·sinφΔvf·cosφ]T。

从式(10)可以看出，虚拟阵列的虚拟信标存在位置偏差，且后构建虚拟信标的位置偏差按照Δv逐渐增大。

2.2.3 载体左向速度误差

当vl存在误差Δvl时，引起的速度误差为

(11)

虚拟阵列信标的位置偏差为

(12)

其中，Δv=[ΔvlcosφΔvlsinφ]T。

从公式可以看出，虚拟阵列的虚拟信标存在位置偏差，且后构建虚拟信标的位置偏差按照Δv逐渐增大。

2.2.4 载体航向误差

当φ存在误差Δφ时，引起的速度误差为

(13)

虚拟阵列信标的位置偏差为：

(14)

其中，

从式(14)可以看出，虚拟阵列的虚拟信标存在位置偏差，且后构建虚拟信标的位置偏差按照Δv逐渐增大。

2.3 仿真分析

仿真参数：布放的单只信标位置为(0，0)。直线航迹如图2所示，以1 m/s的前向航速匀速运动，航向角为90°。假设误差分别为5 m的固定东向标定误差，0.01 m/s的固定前向速度误差，0.01 m/s的固定左向速度误差和0.01°的固定测向误差。

2.3.1 虚拟阵列基线总长度对定位影响

要定位的位置AUVn为定值(1 000，1 000)。虚拟阵列信标个数为11个，虚拟阵列信标间距从1m增加至400m，仿真分析虚拟阵列基线总长度对水平定位结果的影响。

定位结果如图3所示。从图中可以看出，存在信标标定误差、左向速度误差和测向误差时，定位误差为固定值，虚拟阵列基线总长度对定位误差没有影响。存在前向速度误差时，定位偏差会随着基线总长度的增加先减小再增大。当虚拟信标VT1与要定位的位置AUVn的距离最小时，即虚拟阵列基线总长度为1 000m时，载体前向速度误差引起的单标测距定位误差最小。

图3 虚拟阵列基线总长度对水平定位结果的影响Fig.3 Effect of baseline total length of virtual array on positioning results

2.3.2 虚拟阵列信标个数对定位影响

要定位的位置AUVn为定值(1 000，1 000)。虚拟阵列基线总长度为固定值400m，虚拟阵列信标个数从5个增加至401个，仿真分析虚拟阵列信标个数对水平定位结果的影响。

定位结果如图4所示。从图中可以看出，存在信标标定误差、前向速度误差、左向速度误差和测向误差时，改变虚拟阵列信标个数对定位误差没有影响，定位误差为固定值。

图4 虚拟阵列信标个数对水平定位结果的影响Fig.4 Effect of beacon number of virtual array on positioning results

2.3.2 载体相对于信标在不同位置时的定位精度分布

AUV在6 000m×6 000m的范围内运动，目标在区域内的定位精度分布如图5所示。

从图5(a)可知，存在信标标定误差时，定位偏差为固定值，大小与信标标定误差相同。从图5(b)可知，存在固定前向测速误差时，经过信标的航迹不能够定位，水平定位误差是对称分布的，对称轴是航向为方向经过信标的直线，对称轴的两端定位误差大，远离对称轴两端的定位误差小。从图5(c)可知，存在固定左向速度误差时，水平定位误差近似呈对称分布，信标处定位误差最小，以信标为中心向外辐射，距离信标越远的区域定位误差越大。从图5(d)可知，存在固定测向误差时，水平定位误差是对称分布的，信标处定位误差最小，以信标为中心向外辐射，距离信标越远的区域定位误差越大。

3 海试数据分析

3.1 试验1

2015年4月，在中国南海进行了6 000 m AUV实用化改造的综合试航海上试验。试验区域平均水深3 720 m左右。试验平台为中科院沈阳自动化研究所自主研制的AUV。试验设备包括：惯性导航系统、多普勒计程仪、深度传感器和长基线定位系统。长基线定位系统选择的是法国IXSEA公司的RAMSES6000测距仪及RT系列信标。试验期间，LBL的定位周期为24 s；基线长度3 000 m；AUV离底50 m工作；航行时的前向速度大概为1 m/s。AUV航行轨迹如图6所示，S为起点；布放的4只声信标，标号为1、2、3、4；长基线阵列的中心为原点。AUV的深度通过高精度压力传感器测得。

单信标测距定位结果如图7所示，从图中可以看出，单标定位1表示信标1的单信标测距定位结果，定位偏差随着AUV与信标1距离的减小而减小；单标定位3表示信标3的单信标测距定位结果，定位偏差随着AUV与信标1距离的增大而增大。综合仿真分析可知，这说明AUV航行参数存在着固定偏差。

本次试验中，通过LBL对水面试验船进行自主定位，并将定位结果与GPS结果比对，验证了标定结果是比较精确的。下面从载体航向、载体前向速度和载体左向速度进行分析，如图8所示。从图中可以看出，左向速度的精度和前向速度的精度也不高，起伏达到0.04 m/s。载体航向角的起伏范围从90°～94°，起伏程度严重，航向角误差很大。

3.1 试验2

2015年7月，在中国南海第二次进行了4 500 m AUV的综合试航海上试验。与2015年4月不同的是惯性导航系统选用的是法国IXSEA公司的PHINS，该设备的测向精度和速度精度都比较高。与RAMSES测距系统结合使用的情况下，航向角测量精度能达到0.05°。图9为AUV航行过程中测量的载体航向、载体前向速度和载体左向速度。航向角的起伏范围从-0.25°～0.25°，航向角精度得到了很大的提高。

图5 不同误差源下的定位精度分布Fig.5 Distribution of positioning accuracy under different error sources

图6 AUV航行轨迹Fig.6 AUV navigation trajectory

图7 单信标测距定位结果Fig.7 Positioning result based on single beacon ranging

图8 AUV航行参数Fig.8 AUV navigation parameters

图9 AUV航行参数Fig.9 AUV navigation parameters

单信标测距定位结果如图10所示。从图中可以看出，定位结果与组合导航偏差不大。对定位结果与组合导航偏差进行统计，传统LBL定位偏差1.616 m，信标1单标测距定位偏差4.268 m，信标2单标测距定位偏差2.805 m，信标3单标测距定位偏差4.339 m，信标4单标测距定位偏差3.395 m。海试数据处理结果表明：基于单信标测距的定位是以载体的航速和航向精度比较高为前提的。试验2使用了高精度的PHINS，误差比较小，虚拟阵列的信标误差就小，单信标定位能够获得比较好的定位效果。

图10 单信标测距定位Fig.10 Positioning deviation based on single beacon ranging

4 结论

仿真分析结果表明：1)信标标定误差对每一个虚拟信标引入大小等于标定误差的位置偏差，载体前向速度误差、载体左向速度误差和载体测向误差对每一个虚拟信标引入大小不等的位置偏差，后构建的虚拟信标的位置偏差逐渐增大。2)存在载体前向速度误差时，虚拟阵列基线长度影响定位结果。对于其他误差，虚拟阵列基线总长度和虚拟阵列信标个数对定位结果没有影响。

海试数据结果表明：1)存在载体测向误差时，单标定位结果与组合导航的偏差随着AUV航迹与信标距离的增大而增大。2)当AUV航行参数精度比较高时，单标定位结果与组合导航的偏差在5 m之内，虚拟阵列的信标误差越小，单信标定位能够获得比较好的定位效果。

[1]DJAPIC V，NAD D，FERRI G，et al. Novel method for underwater navigation aiding using a companion underwater robot as a guiding platforms[C]// Proceedings of Oceans 2013，Bergen，Norway， 2013： 1-10.

[2]HEGRENAS O，GADE K，HAGEN O K，et al. Underwater transponder positioning and navigation of autonomous underwater vehicles[C]// IEEE Oceanic Engineering Society and Marine Technology Society. MTS/IEEE Biloxi-Marine Technology for Our Future: Global and Local Challenges， OCEANS 2009， Biloxi， MS， USA， 2009： 1-7.

[3]MCEWEN R， THOMAS H， WEBER D， et al. Performance of an AUV navigation system at arctic latitudes[J]. Oceanic engineering， IEEE Journal of， 2005， 30(2)： 443-454.

[4]THOMSON D， DOSSO S. AUV localization in an underwater acoustic positioning system[J]. Inverse theory， 2013， 40(3)： 1-6.

[5]孙大军， 郑翠娥. 水声导航、定位技术发展趋势探讨[J]. 海洋技术学报， 2015，34(3)： 64-65.

SUN Dajun, ZHENG Cuie. Discussion on development trend of underwater acoustic navigation and positioning technology[J]. Journal of ocean technology， 2015， 34(3)： 64-65.

[6]ALEXANDER P S. The AUV positioning using ranges from one transponder LBL[C]// Proceedings of the 1995 MTS/IEEE Oceans Conference， San Diego， CA， USA， 1995:1620-1623

[7]VAGANAY J， BACCOU P， JOUVENCEL B. Homing by acoustic ranging to a single beacon [C]// Proceedings of the 2000 MTS/IEEE Oceans Conference， Providence， RI， USA， 2000:1457-1462.

[8]LARSEN M. Synthetic long baseline navigation of underwater vehicles[C]// Proceedings of the 2000 MTS/IEEE Oceans Conference， Providence， RI， USA， 2000: 2043-2050.

[9]刘明雍， 李闻白， 刘富樯， 等. 基于单信标测距的水下导航系统可观测性分析[J]. 西北工业大学学报， 2011， 29(1)： 87-92.

LIU Mingyong， LI Wenbai， LIU Fuqiang， et al. Observability analysis of underwater navigation system based on single beacon ranging[J]. Journal of Northwestern Polytechnical University， 2011， 29(1)： 87-92.

[10]王久光. 基于UTP的UUV远程航海水下位置校准及控制方法研究[D]. 哈尔滨:哈尔滨工程大学， 2012: 38-52.

WANG Jiuguang. The calibration and control method of the UTP based UUV remote navigation water position calibration and control method[D]. Harbin :Harbin Engineering University， 2012: 38-52.

[11]田坦.水下定位与导航[M].北京：国防工业出版社， 2007.

[12]CARE E G, LA P. Virtual long baseline (VLBL) autonomous underwater vehicle navigation using a single transponder[D]. Massachusetts: Massachusetts institute of technology， 2006: 66-75.

[13]YAMASHITA N， FUKUSHIMA M. On the rate of convergence of the levenberg-marquardt method [J]. Journal of global optimization, 2010， 47(1)： 133-159.

[14]潘国荣， 汪大超， 周跃寅. 两种大转角空间坐标转换模型研究[J]. 山东科技大学学报， 2015， 34(1)： 61-67.

PAN Guorong， WANG Dachao， ZHOU Yueyin. Two spatial coordinate transformation model of large angle[J]. Journal of Shandong University of Science and Technology， 2015， 34(1)： 61-67.

本文引用格式：

曹俊， 郑翠娥， 孙大军，等.AUV直线航迹下的单信标测距定位[J]. 哈尔滨工程大学学报， 2017， 38(5)： 733-738.

CAO Jun，ZHENG Cuie，SUN Dajun，et al. AUV positioning based on single-beacon ranging in straight-line trajectory[J]. Journal of Harbin Engineering University， 2017， 38(5)： 733-738.

AUV positioning based on single-beacon ranging in straight-line trajectory

CAO Jun1，2，ZHENG Cuie1，2，SUN Dajun1，2，ZHANG Dianlun1，2

(1.College of Underwater Acoustic Engineering， Harbin Engineering University， Harbin 150001， Heilongjiang， China; 2.Acoustic Science and Technology Laboratory， Harbin Engineering University， Harbin 150001， Heilongjiang， China)

The conventional localization of single-beacon ranging is based on the nonlinear trajectory. In this paper， the localization of single-beacon ranging was studied based on the linear trajectory. On the one hand， the conventional direct order-reducing method was not applicable for the straight-line trajectory; a linear iterative algorithm was proposed to locate the vector. On the other hand， the concept of the long baseline array was introduced; the effects of different error sources on the virtual array were studied from the aspect of constructing the virtual array. The influence of total length of the virtual array baseline， the influence of the number of the virtual array beacons， and the location accuracy of the vector with respect to the beacon at different positions were analyzed. Sea trial results show that the proposed algorithm can realize good localization results， the deviation between the single-beacon positioning and integrated navigation is within 5 m.

single-beacon ranging; acoustic positioning; straight-line track; virtual beacon; virtual array; linear iterative; unmanned underwater vehide (UUV)

2015-12-28.

日期：2017-04-26.

国家国防科技工业局基础科研项目(B2420133002);国家自然科学基金(61531012).

曹俊(1987-)， 男， 博士研究生； 孙大军(1972-)， 男， 教授， 博士生导师.

孙大军，E-mail:sundajun@hrbeu.edu.cn.

10.11990/jheu.201512093

TB568

A

1006-7043(2017)05-0733-06

网络出版地址：http://www.cnki.net/kcms/detail/23.1390.u.20170426.1040.012.html