matlab水声定位,基于水平投影的ROV水下GPS精确定位算法

基于水平投影的ROV水下GPS精确定位算法

基于水平投影的ROV水下GPS精确定位算法

张颖军　付本国　刘光

(海军潜艇学院，山东青岛，266046)

摘 要

精确的水下定位是提高水下机器人(ROV)作业性能的关键。本文基于斯涅尔声射线理论，采用水下GPS水平投影和有限迭代计算的方法，推导了水下作业机器人精确定位的计算方法，并采用Argo浮标提供的水声数据和MATLAB软件进行了计算仿真。结果表明，相同声信号传播时间，水平投影距离随分层数量的增加而减小并收敛；相同分层数量，水平投影距离误差随初始入射角减小而减小。该计算方法能够实现水下机器人的精确定位。

关键词：机器人，水下GPS，精确定位，水平投影，算法

0　引言

水下遥控作业机器人是当前进行海洋探索和开发的主要工具之一，在深水作业、救生、打捞等方面展现了巨大的潜力。Leonard把水下机器人的导航问题归结为对“我在哪(Where am I)”、“我要去哪(Where am I going)”及“我如何到达那里(How should I going there)”3个问题的回答［1］，而这三个问题的首要问题是“我在哪”，即定位问题。实现快速精确的定位是提高水下机器人性能的关键。目前典型的水下定位方法有：基于水声的定位方法［2-3］，基于卫星的水下GPS定位方法［4-6］和基于概率算法的定位方法［7］。

水下GPS定位是国际上近几年发展起来的水下定位高新技术。系统主要由GPS卫星、GPS定位浮标、差分GPS基准站(可选)、安装于水下机器人身上的收发机、陆基或船基数据处理与监控中心(数据控制中心)、水上无线电通信、水下水声通信构成。本文基于水下GPS定位方法原理，根据海域已有Argo浮标提供的海洋水声参数计算声剖面曲线，采用水平投影和迭代计算的方法探讨了一种水下精确定位的计算方法，并进行了仿真计算与分析。

1　理论推导

有缆作业机器人自身带有深度压力传感器，可以通过系缆传回母船，因此ROV在某时刻深度值H已知。当采用三相交圆解算交点实现定位时，水平投影(半径)的精度就决定了水下机器人的定位精度。

建立以海平面为xoy平面基准面(z=0)，Z轴向下为正的右手螺旋坐标系。在垂直方向，为计算说明方便，令浮标水听器坐标z=0。根据以上定义，则潜水器水下坐标(x00，y00，H)至水听器浮标M1(x1，y1，0)、M2(x2，y2，0)、M3(x3，y3，0)时间为T1、T2、T3。下面以到达水听器M1的声线为例进行讨论。

声速计算公式采用Mackenzie［8］经验公式：

式(1)中C为声速，单位m/s；T为温度，单位oC；S为盐度，单位‰；D为深度，单位m。

图1　声线折射

图2　计算微元

由于在较小范围内，可以认为声速变化与水平位置无关［9］，则可以根据相应海域的经纬度查询附近Argo浮标数据(2000m以内不同深度的盐度和温度值)，带入公式(1)计算该海域的声线剖面图，基于有限元计算的思想，根据声线剖面图划分ROV深度值H为n份，可以认为声速在第i(i<>

当入射角，和

已知，则可计算出折射角

。令潜水器声源发射至声浮标接收的声射线的初始入射角为，则在第i段声信号折射角为：

则在第i段声信号传播的时间为：

这是一个非线性方程，直接求解困难。从方程可以看出，在0～90o取值范围内，时间T是的单调函数，可以先给定一个初始入射角值，通过与延迟时间T1的比较，再通过迭代计算，当满足计算时间T与实际声射线传播时间T1相等时，也就得到了到达水听器M1的声源发射声线初始入射角。

由欧式几何原理(如图2所示)，可以根据初始入射角计算出各分段折射角微单元的水平投影。

同理，可以计算声源距离另外两声浮标M2(x2，y2，0)，M3(x3，y3，0)水平投影距离S2，S3。则

联立公式(7)，(8)，(9)可得关于x00，y00的二元一次方程，具有唯一解，从而计算出ROV位置坐标(x00，y00)。

然后，采用以上同样的计算方法，设定允许的距离误差参数。计算工作深度H分为n+1段时ROV的坐标(x01，y01，H)，当满足

时认为定位精度满足要求，输出此时ROV的定位坐标(x，y，H)。

2　计算仿真与分析

以Argo编号为2902602的浮标周边附近海域为例，声速剖面如图3所示，最大声速为1539.2m/s，最小声速1486.6m/s。以沈阳自动化所的YQ2A型作业机器人为例(如图4所示)，工作深度为1000m，分别计算相同传播时间，不同分层数量之间的水平投影长度。

图3　声速剖面

图4　作业型ROV-YQ2A

根据公式(2)和声浮标提供的声速剖面参数，当水下作业机器人向水面水听器发射的声射线初始入射角大于75o时，在水深27m附近全反射，水面水听器接收不到水下机器人所发射的水声信号。下面采用MATLAB分别计算声传播时间为0.8s、1s、1.5s和2s，水深方向分层为5、10、20等深度层时的声射线初始入射角和水平投影距离。数值计算结果如表1所示。

表1　数值计算结果(m)

水深方向5层水深方向10层水深方向20层时间初始入射角水平投影初始入射角水平投影初始入射角水平投影0.8s33.46o672.533.37o671.333.36o671.2 1.0s47.70o1130.247.57o1125.147.56o1124.7 1.5s62.47o2045.962.24o2026.462.22o2025.1 2.0s68.76o2895.168.45o2842.468.41o2840.2

通过分析表1所示的数值计算结果，可以得出以下基本规律：

1)分层数量相同，随着水声信号传播时间的增大，水下机器人所发声信号的初始入射角逐渐增大。声信号传播时间相同，随着分层数量的增加，初始入射角逐渐减小并趋于收敛。这表明在布置水下GPS水听器阵时，要考虑声信号在海水中弯曲传播的问题。当到达某一水听器声射线的初始入射角超过临界值时(如本文的75o)，此水听器将失去作用。

2)声射线在海水中传播路径的水平投影表现出与初始入射角一致的变化规律。声信号传播时间相同时，随着分层数量的增加，水平投影距离逐渐变小并趋于收敛，这表明此计算方法可以实现水下精确定位。

分层为5层的水平投影与20分层的水平投影相比，随着传播时间(初始入射角)的增大而增大，投影误差分别为0.19%，0.49%，1.03%和1.93%。计算表明，相同分层数量，声信号传播时间越长，初始入射角越大，测量误差越大，此时需要增大分层的数量。因此，布置水下GPS水听器阵时，在作业机器人作业范围内，应尽量减小声射线初始入射角。

3　结论

本文基于斯涅尔声射线理论，采用水下GPS水平投影和迭代计算的方法，推导了大深度水下作业机器人(ROV)精确定位的计算方法，并采用Argo浮标提供的水声数据和MATLAB软件进行了计算仿真与分析。计算结果表明，传播时间相同时，水平投影距离随分层数量的增加而变小并趋于收敛，这表明此计算方法可以实现水下精确定位。同分层数量，测量误差随初始入射角增大而越大，应尽量减小到达水听器的声射线初始入射角。

参考文献

［1］Leonard J， Durrant-Whyte H F. Mobile robot localizationby tracking geometric beacons［J］. IEEE Transactions on Robotics and Automation， 1991， 7(3)：376-382.

［2］王淑炜，张延顺. 基于罗经/DVL/水声定位系统的水下组合导航方法研究［J］.海洋技术学报， 2014， 33(1)：19-23.

［3］龙小民，王维. 基于平面三元阵的水声定位误差分析［J］.舰船科学技术， 2013， 35(9)：62-65.

［4］李敏，李启虎，杨秀庭.一种水下GPS系统及其在蛙人定位导航中的应用［J］.声学技术，2008，27(6)：812-815.

［5］张宏奎，杨益新，李志伟.水下GPS定位误差分析［J］.电子设计工程，2013， 21(11)： 42-44.

［6］李志伟，杨益新.未知声速分布下的一种水下目标GPS定位修正算法［J］.声学技术，2013， 32(5)：262-267.

［7］ Ko N Y， Kim T G. Comparison of Kalman Filter and Particle Filter Used for Localization of an Underwater Vehicle［C］. 2012 9th International Conference on Ubi-quitous Robots and AmbientIntelligence. Daejeon. Korea： IEEE， 2012： 26-29.

［8］申家双，王瑞，谢锡君.回声测深仪的声速改正［J］.海洋测绘，1995(1)：47-51.

［9］王琪，刘雁春，暴景阳.声纳记录失真分析［J］.海洋技术，2002，21(3)：72-74.