CN110441736B - 多关节水下无人潜行器变基线三维空间定位方法 - Google Patents

多关节水下无人潜行器变基线三维空间定位方法 Download PDF

Info

Publication number
CN110441736B
CN110441736B CN201910680198.5A CN201910680198A CN110441736B CN 110441736 B CN110441736 B CN 110441736B CN 201910680198 A CN201910680198 A CN 201910680198A CN 110441736 B CN110441736 B CN 110441736B
Authority
CN
China
Prior art keywords
hydrophone
coordinate system
joint
auv
hydrophones
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201910680198.5A
Other languages
English (en)
Other versions
CN110441736A (zh
Inventor
卢奂采
刘祖斌
唐纳德·迈克尔·马克法兰德
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhejiang University of Technology ZJUT
Original Assignee
Zhejiang University of Technology ZJUT
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Zhejiang University of Technology ZJUT filed Critical Zhejiang University of Technology ZJUT
Priority to CN201910680198.5A priority Critical patent/CN110441736B/zh
Publication of CN110441736A publication Critical patent/CN110441736A/zh
Application granted granted Critical
Publication of CN110441736B publication Critical patent/CN110441736B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/18—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using ultrasonic, sonic, or infrasonic waves
    • G01S5/22—Position of source determined by co-ordinating a plurality of position lines defined by path-difference measurements

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)
  • Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)

Abstract

多关节水下无人潜行器变基线三维空间定位方法，包括：步骤1，根据水听器阵列上阵元和潜标声源的信号，获得潜标声源发出的声波到达水听器的时间迟延，进而计算出多关节水下无人潜行器的水听器阵列上每个水听器至潜标声源的斜距；步骤2，根据步骤1得到的结果、多关节水下无人潜行器的尺寸、各关节的转角，计算得到多关节水下无人潜行器的基线的长度L、俯仰角α和偏航角γ；步骤3，根据步骤1和2得到的结果，以及几何坐标关系建立单潜标、变基线长度的多关节水下航行器上水听器位置求解的数学模型，计算得到每个水听器的三维坐标的两组解，再通过多解判别算法求出真解，得到水听器在大地坐标系下的真实坐标，即获得多关节水下无人潜行器的三维空间坐标。

Description

多关节水下无人潜行器变基线三维空间定位方法
技术领域：
本发明属于无人机、无人潜行器等自主运动、自主导航系统的无关节或多关节的声学定位技术领域，具体涉及自主导航技术、无缆单潜标技术、可变基线长度定位技术、多关节无人潜行器技术、无缆水下机器人技术、水声定位方法。
背景技术：
水下无人潜行器(Autonomous Underwater Vehicle,AUV)是应用于深海自然资源(如热液、冷泉或油田的勘探)、海底电缆或海洋观测网铺设、水下施工和作业等领域的重要装备，准确地发现水下或海底重要自然资源的三维立体空间分布位置以及其它施工或作业对象的三维立体空间位置的水声定位技术研究和开发，对于国家的深海大洋资源发现以及深海开发和作业，有着重要的战略和应用价值。因此，深海水下无人潜行器的精确定位技术是大国间激烈竞争的尖端技术。由于海水对光波和电波的强烈吸收的特性，而全球定位技术(Global Positioning Systems,GPS)只能对海水表面以上空气中的目标进行定位。由于声波是压力波，在海水中传播远，衰减慢，故已成为水下探测、目标识别与信息传递的主要手段。
现有的水下定位技术，如超短基线、短基线和长基线(Ultra-short Baseline，USBL，Short Baseline，SBL，Long Baseline，LBL)定位系统，是相对成熟的深海定位技术。但是，超短基线和短基线定位系统，定位精度较低；长基线定位系统定位精度较高，但是需要在海底布置长基线，施工难度极大且成本高。另外，现有的水下定位技术的最重要的缺陷是：不能应用于基线长度随时都在变化的多关节水下无人潜行器的深海定位。而多关节水下无人潜行器是满足深海混合区域勘探时，转弯半径小、具有一定垂直爬升能力，机动性强的能够实现区域三维精细化海底物理参数探测的无人潜行器。多关节无人潜行器的精细化高精度三维空间定位，则需要开发新的定位方法和技术。
发明内容
本发明要克服现有技术的上述局限性，提供一种多关节水下无人潜行器变基线三维空间定位方法。
本发明提出了针对深海多关节无人潜行器空间定位的单潜标、变基线长度的水声定位方法，通过在海底布放单个潜标声源，利用多关节无人潜行器携带的水听器阵列、多普勒计程仪、三轴陀螺仪、深度计、温盐深仪等仪器输出的参数，构成深海多关节AUV的高精度三维空间定位方法和系统，为深海区域的精细化探测提供多关节AUV的空间坐标。
本发明的多关节水下无人潜行器变基线三维空间定位方法，所采用的设备包括安装在多关节AUV本体上的水听器阵列、布置在海底的单潜标声源；
具体实施步骤包括：
步骤1，计算水听器至潜标的斜距。
设定海面上大地全局坐标系P(Oxyz)，潜标位置为r0＝(x0,y0,z0)，设定水听器阵列由头尾两个水听器组成。两个水听器分别为为第一水听器1和第二水听器2，其坐标分别为r1＝(x1,y1,z1)和r2＝(x2,y2,z2)。声波从潜标声源r0传播至第一水听器1和第二水听器2的时间，由水听器测量得到的水声声压信号与潜标声源的信号进行互相关处理，可得到潜标发射的声波到达两个水听器的时间迟延分别为t1和t2，由CTD测得的声速C，由此可得到两个水听器至潜标的斜距分别为R1和R2。
步骤2，通过坐标变换计算得到多关节AUV的基线的长度L、俯仰角α和偏航角γ。
水下无人潜行器上两个水听器之间的直线段定义为基线L，通过三轴陀螺仪的输出和AUV关节的旋转角度可得到基线俯仰角α，基线偏航角γ。基线指向海面时，俯仰角α为正，
Figure BDA0002144469180000021
基线指向北方时，偏航角γ为0°，偏东为正；γ∈[-π,π]。
在海面上大地全局坐标系P(Oxyz)中，P1,P2,P3为多关节AUV三节末端的局部坐标系。设P2基准面，第一节的俯仰角和偏航角为α1和γ1，第三节的俯仰角和偏航角为α3和γ1。第二节的俯仰、偏航和横摇角是AUV位置在全局坐标系P中的参考坐标，由三轴陀螺仪给出，分别用αg,γg,βg表示。多关节AUV三节的长度分别为l1,l2,l3，第一水听器1与第一节末端的距离d1，第二水听器2与第三节末端的距离d3。AUV的轴向半径为R，在全局坐标系P中，设o2的位置为
Figure BDA0002144469180000022
在P1坐标系中，第一水听器1的位置为
Figure BDA0002144469180000023
在P2坐标系中，第一水听器1和第二水听器2的位置分别为
Figure BDA0002144469180000031
和
Figure BDA0002144469180000032
可表示为
Figure BDA0002144469180000033
Figure BDA0002144469180000034
在P3坐标系中，第二水听器2的位置为
Figure BDA0002144469180000035
Figure BDA0002144469180000036
为从P1坐标系原点到P2坐标系原点的向量，
Figure BDA0002144469180000037
为从P3坐标系原点到P2坐标系原点的向量，可表示为
Figure BDA0002144469180000038
Figure BDA0002144469180000039
Figure BDA00021444691800000310
为从P1坐标系变换到P2坐标系的旋转矩阵，
Figure BDA00021444691800000311
为从P3坐标系变换到P2坐标系的旋转矩阵，可表示为
Figure BDA00021444691800000312
Figure BDA00021444691800000313
在全局坐标系P中，第一水听器1和第二水听器2的位置分别为Pri,i＝1,2，
Figure BDA00021444691800000314
Figure BDA00021444691800000315
是从P2坐标系原点到P坐标系原点的向量，
Figure BDA00021444691800000316
Figure BDA00021444691800000317
是旋转矩阵从P2坐标系变换到P坐标系，
Figure BDA0002144469180000041
根据式(7)，第一水听器1和第二水听器2的位置重新分别表示为Pr1＝(Px1,Py1,Pz1)和Pr2＝(Px2,Py2,Pz2)。基线的长度L，基线的俯仰角α，基线的偏航角γ，可以计算为:
L＝||Pr1-Pr2|| (10)
Figure BDA0002144469180000042
Figure BDA0002144469180000043
步骤3，根据步骤1和2获得的结果，计算出两个水听器的坐标r1＝(x1,y1,z1)和r2＝(x2,y2,z2)。
AUV携带的测深仪测得的第一水听器1至潜标与深度差为H1。根据几何坐标关系可以建立如下几何关系：
Figure BDA0002144469180000044
Figure BDA0002144469180000045
求解(13)(14)时，两个水听器的z1和z2坐标分别为
Figure BDA0002144469180000046
为了简洁地表示两个水听器x和y方向的坐标，引入中间变量A，
Figure BDA0002144469180000051
简化后，可得
Figure BDA0002144469180000052
上式中的计算结果有两组解：x11和y11为第一水听器1的第一解，x21和y21为第二水听器2的第一解；x12和y12为第一水听器1的第二解，x22和y22为第二水听器2的第二解。其中r11＝(x11,y11,z1)和r21＝(x21,y21,z2)为第一解，r12＝(x12,y12,z1)和r22＝(x22,y22,z2)为第二解。
对于步骤3中求得的两组解，设计多解判别算法求出真解，得到水听器在全局大地坐标系P下的真实坐标。
步骤3中所得两组解具有相同的偏航角和基线长度。但只有一个解是真值，通过确定哪一组解更接近于水听器的预测位置来实现多解判别算法。
定义第一水听器1在前一时刻的位置为r1prev＝(x1prev,y1prev,z1prev)，第一水听器1的预测位置为r′1＝(x′1,y′1,z′1)，第一水听器1在一个定位周期内的预测位移为Δr＝(Δx,Δy,Δz)。由此可以表达第一水听器1的预测位置
r′1＝r1prev+Δr (18)
第一水听器1在定位周期内的位移可以通过三轴陀螺仪的数据、AUV的关节角度和多普勒计程仪的输出来计算。基线的俯仰角和偏航角α和γ，可以根据AUV三轴陀螺仪和关节角的数据计算，坐标变换在步骤2中给出。多普勒计程仪提供了水下航行器的速度V。这三个参数随时间而变化。第一水听器1在定位周期K内的位移可表示为
Figure BDA0002144469180000061
其中τ为当前时间。然后计算|r11-r′1|和|r12-r′1|的长度。式(17)中最接近式(18)所预测位置的解被认为是第一水听器1的真实的当前位置。第二水听器2真实的当前位置，可以由式(13)确定。
多关节水下无人潜行器变基线三维空间定位方法，包括：步骤1，根据水听器和潜标声源的信号，获得潜标声源发出的声波到达各个水听器的时间迟延，进而计算出多关节水下无人潜行器水听器阵列上每个水听器至潜标声源的斜距；步骤2，根据步骤1得到的结果、多关节水下无人潜行器的尺寸、各关节的实际转角，计算得到多关节水下无人潜行器的基线的长度L、俯仰角α和偏航角γ；步骤3，根据步骤1和2得到的结果，以及几何坐标关系建立单潜标、变基线长度的多关节水下航行器上水听器位置求解的数学模型，计算得到每个水听器的三维坐标的两组解。再通过多解判别算法求出真解，得到水听器在大地坐标系下的真实坐标，即获得多关节水下无人潜行器的三维空间坐标。
本发明的有益效果是：
提出多关节水下无人潜行器变基线三维空间定位方法，即单潜标、变基线、多关节AUV的深海定位方法。此定位方法能满足深海局部区域三维空间精细化探测对空间坐标获取的需求。相对于USBL、SBL和LBL定位方法和技术，单潜标、变基线、多关节AUV深海定位系统，具有适应具有任意多的关节个数的、柔性的AUV，或任意形状的、刚性的AUV的高精度定位的优点。
附图说明
图1是AUV与潜标声源的相对位置的示意图，图中AUV上携带三轴陀螺仪、深度计、DVL和CTD。
图2是AUV上的水听器阵列与潜标声源的几何关系的示意图。
图3是全局坐标中式(5)多解的原理图。
图4是多解坐标位置选择原理图，图中●连续两次的真实位置，■预测位置，◆第一水听器(1)的假位置。
图5是坐标变换原理图，图中P(Oxyz)为全局坐标系,P1、P2、P3为局部坐标系,○表示水听器在r1和r2处的位置。
具体实施方式
下面结合附图进一步说明本发明的技术方案。
本发明的多关节水下无人潜行器变基线三维空间定位方法，所采用的设备包括安装在多关节AUV本体上的水听器阵列、布置在海底的单潜标声源；
以两关节共三节的AUV为例。该AUV携带三轴陀螺仪、深度计、速度剖面仪(DopplerVelocity Log,DVL)和温盐深度仪(Conductivity-Depth-Temperature,CTD)，头尾两节各安装一个水听器构成水听器阵列，如图1所示。
具体实施步骤包括：
步骤1，计算水听器至潜标的斜距。
设定海面上大地全局坐标系P(Oxyz)，多关节AUV和潜标声源的几何关系如图2所示。潜标位置为r0＝(x0,y0,z0)，两个水听器的坐标分别为r1＝(x1,y1,z1)和r2＝(x2,y2,z2)。声波从潜标声源r0传播至第一水听器1和第二水听器2的时间，分别为t1和t2。由水听器测量得到的水声声压信号与潜标声源的信号进行互相关处理，可得到潜标发射的声波到达两个水听器的时间迟延分别为t1和t2，由CTD测得的声速C，由此可得到两个水听器至潜标的斜距分别为R1和R2。
步骤2，通过坐标变换计算得到多关节AUV的基线的长度L、俯仰角α和偏航角γ。
水下无人潜行器上两个水听器之间的直线段定义为基线L，通过三轴陀螺仪的输出和AUV关节的旋转角度可得到基线俯仰角α，基线偏航角γ。基线指向海面时，俯仰角α为正，
Figure BDA0002144469180000081
基线指向北方时，偏航角γ为0°，偏东为正；γ∈[-π,π]。
在海面上大地全局坐标系P(Oxyz)中，P1,P2,P3为多关节AUV三节末端的局部坐标系。设P2基准面，第一节的俯仰角和偏航角为α1和γ1，第三节的俯仰角和偏航角为α3和γ1。第二节的俯仰、偏航和横摇角是AUV位置在全局坐标系P中的参考坐标，由三轴陀螺仪给出，分别用αg,γg,βg表示。多关节AUV三节的长度分别为l1,l2,l3，第一水听器1与第一节末端的距离d1，第二水听器2与第三节末端的距离d3。AUV的轴向半径为R，在全局坐标系P中，设o2的位置为
Figure BDA00021444691800000812
在P1坐标系中，第一水听器1的位置为
Figure BDA00021444691800000813
在P2坐标系中，第一水听器1和第二水听器2的位置分别为
Figure BDA00021444691800000814
和
Figure BDA00021444691800000815
可表示为
Figure BDA0002144469180000082
Figure BDA0002144469180000083
在P3坐标系中，第二水听器2的位置为
Figure BDA0002144469180000084
Figure BDA0002144469180000085
为从P1坐标系原点到P2坐标系原点的向量，
Figure BDA0002144469180000086
为从P3坐标系原点到P2坐标系原点的向量，可表示为
Figure BDA0002144469180000087
Figure BDA0002144469180000088
Figure BDA0002144469180000089
为从P1坐标系变换到P2坐标系的旋转矩阵，
Figure BDA00021444691800000810
为从P3坐标系变换到P2坐标系的旋转矩阵，可表示为
Figure BDA00021444691800000811
Figure BDA0002144469180000091
在全局坐标系P中，第一水听器1和第二水听器2的位置分别为Pri,i＝1,2，
Figure BDA0002144469180000092
Figure BDA0002144469180000093
是从P2坐标系原点到P坐标系原点的向量，
Figure BDA0002144469180000094
Figure BDA0002144469180000095
是旋转矩阵从P2坐标系变换到P坐标系，
Figure BDA0002144469180000096
根据式(7)，第一水听器1和第二水听器2的位置重新分别表示为Pr1＝(Px1,Py1,Pz1)和Pr2＝(Px2,Py2,Pz2)。基线的长度L，基线的俯仰角α，基线的偏航角γ，可以计算为:
L＝||Pr1-Pr2|| (10)
Figure BDA0002144469180000097
Figure BDA0002144469180000098
步骤3，根据步骤1和2获得的结果，计算出两个水听器的坐标r1＝(x1,y1,z1)和r2＝(x2,y2,z2)。
AUV携带的测深仪测得的第一水听器1至潜标与深度差为H1。根据几何坐标关系可以建立如下几何关系：
Figure BDA0002144469180000101
Figure BDA0002144469180000102
求解(13)(14)时，两个水听器的z1和z2坐标分别为
Figure BDA0002144469180000103
为了简洁地表示两个水听器x和y方向的坐标，引入中间变量A，
Figure BDA0002144469180000104
简化后，可得
Figure BDA0002144469180000105
上式中的计算结果有两组解：x11和y11为第一水听器1的第一解，x21和y21为第二水听器2的第一解；x12和y12为第一水听器1的第二解，x22和y22为第二水听器2的第二解。其中r11＝(x11,y11,z1)和r21＝(x21,y21,z2)为第一解，r12＝(x12,y12,z1)和r22＝(x22,y22,z2)为第二解。
对于步骤3中求得的两组解，设计多解判别算法求出真解，得到水听器在全局大地坐标系P下的真实坐标。
步骤3中所得两组解具有相同的偏航角和基线长度。但只有一个解是真值，通过确定哪一组解更接近于水听器的预测位置来实现多解判别算法。
定义第一水听器1在前一时刻的位置为r1prev＝(x1prev,y1prev,z1prev)，第一水听器1的预测位置为r′1＝(x′1,y′1,z′1)，第一水听器1在一个定位周期内的预测位移为Δr＝(Δx,Δy,Δz)。由此可以表达第一水听器1的预测位置
r′1＝r1prev+Δr (18)
第一水听器1在定位周期内的位移可以通过三轴陀螺仪的数据、AUV的关节角度和多普勒计程仪的输出来计算。基线的俯仰角和偏航角α和γ，可以根据AUV三轴陀螺仪和关节角的数据计算，坐标变换在步骤2中给出。多普勒计程仪提供了水下航行器的速度V。这三个参数随时间而变化。第一水听器1在定位周期K内的位移可表示为
Figure BDA0002144469180000111
其中τ为当前时间。然后计算|r11-r′1|和|r12-r′1|的长度。式(17)中最接近式(18)所预测位置的解被认为是第一水听器1的真实的当前位置。第二水听器2真实的当前位置，可以由式(13)确定。
本说明书实施例所述的内容仅仅是对发明构思的实现形式的列举，本发明的保护范围不应当被视为仅限于实施例所陈述的具体形式和参数，本发明的保护范围也及于本领域技术人员根据本发明构思所能够想到的等同技术手段。

Claims (2)

1.多关节水下无人潜行器变基线三维空间定位方法，所采用的设备包括安装在多关节AUV本体上的水听器阵列、布置在海底的单潜标声源；其特征在于：在每个多关节AUV的空间定位周期内，定位系统按照以下的步骤获得多关节水下无人潜行器的空间坐标；具体实施步骤包括：
步骤1，计算水听器至潜标的斜距；
设定海面上大地全局坐标系P(Oxyz)，潜标位置为r0＝(x0，y0，z0)，设定水听器阵列由头尾两个水听器组成；两个水听器分别为第一水听器(1)和第二水听器(2)，其坐标分别为r1＝(x1，y1，z1)和r2＝(x2，y2，z2)；声波从潜标声源r0传播至第一水听器(1)和第二水听器(2)的时间，由水听器测量得到的水声声压信号与潜标声源的信号进行互相关处理，可得到潜标发射的声波到达两个水听器的时间迟延分别为t1和t2，由CTD测得的声速C，由此可得到两个水听器至潜标的斜距分别为R1和R2；
步骤2，通过坐标变换计算得到多关节AUV的基线的长度L、俯仰角α和偏航角γ；
水下无人潜行器上两个水听器之间的直线段定义为基线L，通过三轴陀螺仪的输出和AUV关节的旋转角度可得到基线俯仰角α，基线偏航角γ；基线指向海面时，俯仰角α为正，
Figure FDA0002831136200000011
基线指向北方时，偏航角γ为0°，偏东为正；γ∈[-π，π]；
在海面上大地全局坐标系P(Oxyz)中，P1，P2，P3为多关节AUV三节末端的局部坐标系；设P2基准面，第一节的俯仰角和偏航角为α1和γ1，第三节的俯仰角和偏航角为α3和γ1；第二节的俯仰、偏航和横摇角是AUV位置在全局坐标系P中的参考坐标，由三轴陀螺仪给出，分别用αg，γg，βg表示；多关节AUV三节的长度分别为l1，l2，l3，第一水听器(1)与第一节末端的距离d1，第二水听器(2)与第三节末端的距离d3；AUV的轴向半径为R，在全局坐标系P中，设局部坐标系P2的原点o2的位置为
Figure FDA0002831136200000021
在P1坐标系中，第一水听器(1)的位置为
Figure FDA0002831136200000022
在P2坐标系中，第一水听器(1)和第二水听器(2)的位置分别为
Figure FDA0002831136200000023
和
Figure FDA0002831136200000024
可表示为
Figure FDA0002831136200000025
Figure FDA0002831136200000026
在P3坐标系中，第二水听器(2)的位置为
Figure FDA0002831136200000027
Figure FDA0002831136200000028
为从P1坐标系原点到P2坐标系原点的向量，
Figure FDA0002831136200000029
为从P3坐标系原点到P2坐标系原点的向量，可表示为
Figure FDA00028311362000000210
Figure FDA00028311362000000211
Figure FDA00028311362000000212
为从P1坐标系变换到P2坐标系的旋转矩阵，
Figure FDA00028311362000000213
为从P3坐标系变换到P2坐标系的旋转矩阵，可表示为
Figure FDA00028311362000000214
Figure FDA00028311362000000215
在全局坐标系P中，第一水听器(1)和第二水听器(2)的位置分别为Pri，i＝1，2，
Figure FDA0002831136200000031
Figure FDA0002831136200000032
是从P2坐标系原点到P坐标系原点的向量，
Figure FDA0002831136200000033
Figure FDA0002831136200000034
是旋转矩阵从P2坐标系变换到P坐标系，
Figure FDA0002831136200000035
根据式(7)，第一水听器(1)和第二水听器(2)的位置重新分别表示为Pr1＝(Px1，Py1，Pz1)和Pr2＝(Px2，Py2，Pz2)；基线的长度L，基线的俯仰角α，基线的偏航角γ，可以计算为：
L＝||Pr1-Pr2|| (10)
Figure FDA0002831136200000036
Figure FDA0002831136200000037
步骤3，根据步骤1和2获得的结果，计算出两个水听器的坐标r1＝(x1，y1，z1)和r2＝(x2，y2，z2)；
AUV携带的测深仪测得的第一水听器(1)至潜标与深度差为H1；根据几何坐标关系可以建立如下几何关系：
Figure FDA0002831136200000041
Figure FDA0002831136200000042
求解式(13)和式(14)时，两个水听器的z1和z2坐标分别为
Figure FDA0002831136200000043
为了简洁地表示两个水听器x和y方向的坐标，引入中间变量A，
Figure FDA0002831136200000044
简化后，可得
Figure FDA0002831136200000045
Figure FDA0002831136200000046
Figure FDA0002831136200000047
Figure FDA0002831136200000048
上式中的计算结果有两组解：x11和y11为第一水听器(1)的第一解，x21和y21为第二水听器(2)的第一解；x12和y12为第一水听器(1)的第二解，x22和y22为第二水听器(2)的第二解；其中r11＝(x11，y11，z1)和r21＝(x21，y21，z2)为第一解，r12＝(x12，y12，z1)和r22＝(x22，y22，z2)为第二解；
对于步骤3中求得的两组解，设计多解判别算法求出真解，得到水听器在全局大地坐标系P下的真实坐标；
步骤3中所得两组解具有相同的偏航角和基线长度；但只有一个解是真值，通过确定哪一组解更接近于水听器的预测位置来实现多解判别算法；
定义第一水听器1在前一时刻的位置为r1prev＝(x1prev，y1prev，z1prev)，第一水听器(1)的预测位置为r′1＝(x′1，y′1，z′1)，第一水听器(1)在一个定位周期内的预测位移为Δr＝(Δx，Δy，Δz)；由此可以表达第一水听器(1)的预测位置
r′1＝r1prev+Δr (18)
第一水听器1在定位周期内的位移可以通过三轴陀螺仪的数据、AUV的关节角度和多普勒计程仪的输出来计算；基线的俯仰角和偏航角α和γ，可以根据AUV三轴陀螺仪和关节角的数据计算，坐标变换在步骤2中给出；多普勒计程仪提供了水下航行器的速度V；这三个参数随时间而变化；第一水听器(1)在定位周期K内的位移可表示为
Figure FDA0002831136200000051
其中τ为当前时间；然后计算|r11-r′1|和|r12-r′1|的长度；式(17)中最接近式(18)所预测位置的解被认为是第一水听器(1)的真实的当前位置；第二水听器(2)真实的当前位置，可以由式(13)确定。
2.如权利要求1所述的多关节水下无人潜行器变基线三维空间定位方法，其特征在于：所采用的AUV是多关节的AUV，该AUV携带三轴陀螺仪、深度计、速度剖面仪(DopplerVelocity Log，DVL)和温盐深度仪(Conductivity-Depth-Temperature，CTD)，头尾两节各安装一个或圆周向多个水听器构成水听器阵列。
CN201910680198.5A 2019-07-26 2019-07-26 多关节水下无人潜行器变基线三维空间定位方法 Active CN110441736B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910680198.5A CN110441736B (zh) 2019-07-26 2019-07-26 多关节水下无人潜行器变基线三维空间定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910680198.5A CN110441736B (zh) 2019-07-26 2019-07-26 多关节水下无人潜行器变基线三维空间定位方法

Publications (2)

Publication Number Publication Date
CN110441736A CN110441736A (zh) 2019-11-12
CN110441736B true CN110441736B (zh) 2021-05-07

Family

ID=68431565

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910680198.5A Active CN110441736B (zh) 2019-07-26 2019-07-26 多关节水下无人潜行器变基线三维空间定位方法

Country Status (1)

Country Link
CN (1) CN110441736B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111427010A (zh) * 2020-04-20 2020-07-17 中国电子科技集团公司电子科学研究院 Rov水下定位系统及定位方法
CN112346016B (zh) * 2020-10-28 2023-11-28 苏州触达信息技术有限公司 一种水中人员的定位方法和可穿戴设备
CN113359182B (zh) * 2021-06-02 2021-10-29 国家深海基地管理中心 一种深海热液喷口快速搜寻定位装置、方法及系统

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105738869A (zh) * 2014-12-08 2016-07-06 中国科学院声学研究所 一种适用于单水听器的深水信标搜索定位方法
CN108089155A (zh) * 2017-12-28 2018-05-29 西北工业大学 一种深海环境下单水听器声源被动定位方法
CN109870694A (zh) * 2019-02-21 2019-06-11 哈尔滨工程大学 基于多无人艇平台的高精度长基线定位系统

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8972164B2 (en) * 2012-04-26 2015-03-03 The United States Of America, As Represented By The Secretary Of The Navy Collaborative robot manifold tracker

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105738869A (zh) * 2014-12-08 2016-07-06 中国科学院声学研究所 一种适用于单水听器的深水信标搜索定位方法
CN108089155A (zh) * 2017-12-28 2018-05-29 西北工业大学 一种深海环境下单水听器声源被动定位方法
CN109870694A (zh) * 2019-02-21 2019-06-11 哈尔滨工程大学 基于多无人艇平台的高精度长基线定位系统

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
USBL positioning system based Adaptive Kalman filter in AUV;Yulin Xu等;《2018 IEEE》;20181231;全文 *
基于SINS/DVL与声学定位系统的水下组合导航技术研究;李旻;《舰船电子工程》;20181231;第38卷(第12期);全文 *

Also Published As

Publication number Publication date
CN110441736A (zh) 2019-11-12

Similar Documents

Publication Publication Date Title
CN110441736B (zh) 多关节水下无人潜行器变基线三维空间定位方法
Chen et al. Review of AUV underwater terrain matching navigation
CN107390177B (zh) 一种基于纯测向的被动水下声学定位方法
CN105547290B (zh) 一种基于超短基线定位系统的从潜器导航方法
Guan et al. An effective method for submarine buried pipeline detection via multi-sensor data fusion
CN104316045A (zh) 一种基于sins/lbl的auv水下交互辅助定位系统及定位方法
AU2009255340A1 (en) Marine seismic streamer system configurations, systems, and methods
RU2563332C2 (ru) Способ навигации автономного необитаемого подводного аппарата
CN110207695A (zh) 一种适用于深海auv的无速度辅助单信标定位方法
CN105738869B (zh) 一种适用于单水听器的深水信标搜索定位方法
EP2796901A2 (en) Remotely Operated Modular Positioning Vehicle and Method
CN111829512A (zh) 一种基于多传感器数据融合的auv导航定位方法及系统
CN110294080A (zh) 一种利用超短基线实现水下精确作业的方法
CN108919324B (zh) 一种水下滑翔机的定位方法
CN112540371A (zh) 一种近底多波束坐标转换处理方法
CN110333369B (zh) 基于水面gps校正的uuv的dvl测速系统及自适应去噪方法
Zhang et al. Use of the Jiaolong manned submersible for accurate mapping of deep-sea topography and geomorphology
Maki et al. AUV Tri-TON—A hover-capable platform for 3D visualization of complicated surfaces
CN111735455A (zh) 基于改进的高斯距离迭代算法对接回收组合导航方法
CN108761470A (zh) 一种基于拖缆形态方程解析的目标定位方法
CN112904428A (zh) 海洋浅地层剖面探测系统及方法
CN115390012B (zh) 用于hov精准定位的多应答器坐标测量方法、装置及系统
CN113703050B (zh) 一种深海地震垂直缆二次定位方法
CN115930951A (zh) 一种基于多传感器组合的短期水下auv隐蔽导航方法
CN113933526B (zh) 一种体目标高精度三维运动速度测量方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant