CN113945892B - 一种体目标三维运动轨迹测量方法 - Google Patents

一种体目标三维运动轨迹测量方法 Download PDF

Info

Publication number
CN113945892B
CN113945892B CN202111181944.XA CN202111181944A CN113945892B CN 113945892 B CN113945892 B CN 113945892B CN 202111181944 A CN202111181944 A CN 202111181944A CN 113945892 B CN113945892 B CN 113945892B
Authority
CN
China
Prior art keywords
body target
target
coordinate system
transducer
time
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
CN202111181944.XA
Other languages
English (en)
Other versions
CN113945892A (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.)
Harbin Engineering University
Original Assignee
Harbin Engineering University
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 Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN202111181944.XA priority Critical patent/CN113945892B/zh
Publication of CN113945892A publication Critical patent/CN113945892A/zh
Application granted granted Critical
Publication of CN113945892B publication Critical patent/CN113945892B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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/20—Position of source determined by a plurality of spaced direction-finders
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation

Abstract

一种体目标三维运动轨迹测量方法，涉及海洋测绘技术领域。本发明是为了解决长基线的定位模型不完全匹配，模型存在误差，导致定位误差的问题。本发明建立体目标坐标系，在体目标处于航行时，使体目标与水下信标进行信号传输，采集t时刻水下信标接收到的信号，根据该信号的频率获得体目标上换能器的编号，以及t时刻体目标上换能器与水下信标之间信号的传播距离，判断体目标运动过程姿态参数是否已知，若已知则建立体目标轨迹解算模型求解体目标质心在地理坐标系下的坐标，否则建立体目标轨迹和姿态解算模型，然后测量体目标运动过程姿态参数，再求解体目标质心在地理坐标系下的坐标。

Description

一种体目标三维运动轨迹测量方法
技术领域
本发明属于海洋测绘技术领域。
背景技术
海洋覆盖地球7成以上的面积，包含丰富的资源。在国家大力发展海洋的基础下，发展了许多水下航行器设备，如自主式水下航行器(AUV)、遥控无人潜水器(ROV)等。长基线定位是一种高精度的技术手段，可以对局部地区的水下潜器等定位。过去水下潜器的航迹大多水平方向运动，在深度方向改变很小，传统长基线主要对其二维轨迹进行跟踪，对深度方向考虑较少。
随着应用需求的逐步提高，对于长基线的精度需求也随之增大。尤其是水下运动的体目标，使用传统的质点长基线轨迹求解方法，长基线不同信标接收体目标上不同换能器的信号，在这种情况下传统长基线的定位模型不完全匹配，模型存在误差，进而带来了较大的定位误差，尤其是垂向坐标。
发明内容
本发明是为了解决长基线的定位模型不完全匹配，模型存在误差，导致定位误差的问题，现提供一种体目标三维运动轨迹测量方法。
一种体目标三维运动轨迹测量方法，所述体目标上均匀分布有至少4个换能器，使得所有水下信标均至少能接收到1个来自体目标上换能器发出的信号，所述方法具体包括以下步骤：
步骤一：建立体目标坐标系，使得每个换能器均具有体目标坐标系下的坐标，
步骤二：在体目标处于航行时，使体目标与水下信标进行信号传输，
步骤三：采集t时刻水下信标接收到的信号，根据该信号的频率获得体目标上换能器的编号，以及t时刻体目标上换能器与水下信标之间信号的传播距离，所述体目标上换能器为水下信标能够接收到其发送信号的换能器，
步骤四：判断体目标运动过程姿态参数是否已知，是则执行步骤五，否则执行步骤六，
步骤五：建立体目标轨迹解算模型，根据该体目标轨迹解算模型获得t时刻体目标质心在地理坐标系下的坐标，
步骤六：建立体目标轨迹和姿态解算模型，然后测量体目标运动过程姿态参数，根据该体目标轨迹和姿态解算模型获得t时刻体目标质心在地理坐标系下的坐标。
进一步的，在上述步骤五或步骤六之后均执行步骤七，步骤七具体为：
判断t时刻体目标质心在地理坐标系下的坐标是否为终点坐标，
是，则将初始时刻至t时刻步骤五或步骤六获得的所有坐标结果拟合成体目标三维运动轨迹，
否，则使t＝t+1，然后返回步骤三。
进一步的，将体目标上换能器发出信号的时刻与t时刻作差获得时间差，将时间差与当前水域水下声速相乘，获得t时刻体目标上换能器与水下信标之间信号的传播距离。
进一步的，体目标坐标系为原点为o的空间直角坐标系，记为o-xsyszs，其中，xs轴和ys轴均位于体目标平面内、且分别指向右舷和船首方向，zs轴呈竖直方向设置、且指向上方，zs轴垂直于体目标平面，原点o位于体目标的质心处；
地理坐标系为原点为o的空间直角坐标系，记为o-xGyGzG，其中，xG轴指向东方，yG轴指向北方，zG轴指向上方；
体目标运动过程姿态参数为α、β和γ，
当体目标质心坐标从地理坐标系旋转到体目标坐标系时，α为zG轴旋转到zs轴的角度，β为xG轴旋转到xs轴的角度，γ为yG轴旋转到ys轴的角度。
进一步的，地理坐标系到体目标坐标系的旋转矩阵RGS为：
Figure BDA0003297640240000021
进一步的，步骤五所述体目标轨迹解算模型为：
Figure BDA0003297640240000022
其中，
Figure BDA0003297640240000023
为t时刻体目标质心在地理坐标系下的坐标，
Figure BDA0003297640240000024
为体目标上第k个换能器在地理坐标系下的坐标，
Figure BDA0003297640240000025
为第i个水下信标在地理坐标系下的坐标，Ri为t时刻体目标与第i个水下信标之间信号的传播距离，i为水下信标的编号，第i个水下信标为t时刻能够接收到换能器发送信号的水下信标，k为体目标上换能器的编号，第k个换能器为t时刻水下信标能够接收到其发送信号的换能器。
进一步的，t时刻体目标质心在地理坐标系下的坐标
Figure BDA0003297640240000026
的矩阵表达式为：
Figure BDA0003297640240000031
其中，X0为初始时刻体目标质心在地理坐标系下的坐标，P为测量元素总误差的协方差矩阵，
Figure BDA0003297640240000032
Figure BDA0003297640240000033
R为量测得到的体目标与水下信标之间量测的传播距离，R0为将X0代入体目标轨迹解算模型后获得的体目标与水下信标之间信号的传播距离。
进一步的，步骤六所述体目标轨迹和姿态解算模型为：
Figure BDA0003297640240000034
其中，
Figure BDA0003297640240000035
为t时刻体目标质心在地理坐标系下的坐标，RSG为体目标坐标系到地理坐标系的旋转矩阵，
Figure BDA0003297640240000036
为体目标上第k个换能器在体目标坐标系下的坐标，
Figure BDA0003297640240000037
为水下信标在地理坐标系下的坐标，Ri为t时刻体目标与水下信标之间信号的传播距离，i为水下信标的编号，第i个水下信标为t时刻能够接收到换能器发送信号的水下信标，k为体目标上换能器的编号，第k个换能器为t时刻水下信标能够接收到其发送信号的换能器。
本发明公开了一种体目标三维运动轨迹测量方法，能够实现对连续运动的体目标三维轨迹进行高精度的测量。相较于忽略体目标直接当做质点解算轨迹的方法，本发明减小了模型误差，求解精度高，能够满足水下高精度求解的需要。本发明能够应用于水下长基线对高速运动体目标轨迹的跟踪任务。
附图说明
图1为本发明所述的一种体目标三维运动轨迹测量方法的流程图；
图2为体目标结构示意图；
图3为体目标运动是与水下信标的示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。
下面结合附图和具体实施例对本发明作进一步说明，但不作为本发明的限定。
具体实施方式一：参照图1至图3具体说明本实施方式，本实施方式所述的一种体目标三维运动轨迹测量方法，所述体目标为圆柱形结构，目标朝上，体目标圆周面上均匀分布有至少4个换能器，使得所有水下信标均至少能接收到1个来自体目标上换能器发出的信号。
一种体目标三维运动轨迹测量方法具体包括以下步骤：
步骤一：建立体目标坐标系。具体的，体目标坐标系为原点为o的空间直角坐标系，记为o-xsyszs，其中，xs轴和ys轴均位于体目标平面内、且分别指向右舷和船首方向，zs轴呈竖直方向设置、且指向上方，zs轴垂直于体目标平面，原点o位于体目标的质心处。获得了体目标坐标系后就使得每个换能器均具有体目标坐标系下的坐标。
步骤二：在体目标处于航行时，使体目标与水下信标进行信号传输。
步骤三：采集t时刻水下信标接收到的信号。由于运动过程中体目标上每个换能器发射不同频率的信号，所以根据该信号的频率就能够获得体目标上发出信号的换能器编号k。
进一步的，通过声速剖面仪测，获得当前水域的声速。将体目标上第k个换能器发出信号的时刻与t时刻(即水下信标接收到该信号的时刻)作差获得信号传播时间。将声速与信号传播时间相乘，获得t时刻体目标上第k个换能器与第i个水下信标之间信号的传播距离Ri，i为水下信标的编号。所述体目标上第k个换能器为水下信标能够接收到其发送信号的换能器。
步骤四：地理坐标系为原点为o的空间直角坐标系，记为o-xGyGzG，其中，xG轴指向东方，yG轴指向北方，zG轴指向上方。
则有体目标运动过程姿态参数为α、β和γ。具体的，当体目标质心坐标从地理坐标系旋转到体目标坐标系时，地理坐标系到体目标坐标系的旋转矩阵RGS为：
Figure BDA0003297640240000041
α为zG轴旋转到zs轴的角度，β为xG轴旋转到xs轴的角度，γ为yG轴旋转到ys轴的角度。
判断体目标运动过程姿态参数是否已知，是则执行步骤五，否则执行步骤六。
步骤五：建立体目标轨迹解算模型，所述模型如下：
Figure BDA0003297640240000051
其中，
Figure BDA0003297640240000052
为体目标上第k个换能器在地理坐标系下的坐标，
Figure BDA0003297640240000053
为第i个水下信标在地理坐标系下的坐标。
Figure BDA0003297640240000054
和
Figure BDA0003297640240000055
均已知，则根据该体目标轨迹解算模型就能够获得t时刻体目标质心在地理坐标系下的坐标
Figure BDA0003297640240000056
然后执行步骤七。
步骤六：建立体目标轨迹和姿态解算模型，所述模型如下：
Figure BDA0003297640240000057
其中，RSG为体目标坐标系到地理坐标系的旋转矩阵，
Figure BDA0003297640240000058
为体目标上第k个换能器在体目标坐标系下的坐标。
然后测量体目标运动过程姿态参数，根据该体目标轨迹和姿态解算模型获得t时刻体目标质心在地理坐标系下的坐标
Figure BDA0003297640240000059
然后执行步骤七。
步骤七：判断t时刻体目标质心在地理坐标系下的坐标是否为终点坐标，
是，则将初始时刻至t时刻步骤五或步骤六获得的所有坐标结果拟合成体目标三维运动轨迹，
否，则使t＝t+1，然后返回步骤三。
上述t时刻体目标质心在地理坐标系下的坐标
Figure BDA00032976402400000510
的矩阵表达式为：
Figure BDA00032976402400000511
其中，X0为初始时刻体目标质心在地理坐标系下的坐标，P为测量元素总误差的协方差矩阵，
Figure BDA00032976402400000512
Figure BDA00032976402400000513
R为量测得到的体目标与水下信标之间量测的传播距离，R0为将X0代入体目标轨迹解算模型后获得的体目标与水下信标之间信号的传播距离。

Claims (6)

1.一种体目标三维运动轨迹测量方法，其特征在于，所述体目标上均匀分布有至少4个换能器，使得所有水下信标均至少能接收到1个来自体目标上换能器发出的信号，
一种体目标三维运动轨迹测量方法具体包括以下步骤：
步骤一：建立体目标坐标系，使得每个换能器均具有体目标坐标系下的坐标，
步骤二：在体目标处于航行时，使体目标与水下信标进行信号传输，
步骤三：采集t时刻水下信标接收到的信号，根据该信号的频率获得体目标上换能器的编号，以及t时刻体目标上换能器与水下信标之间信号的传播距离，所述体目标上换能器为水下信标能够接收到其发送信号的换能器，
步骤四：判断体目标运动过程姿态参数是否已知，是则执行步骤五，否则执行步骤六，
步骤五：建立体目标轨迹解算模型，根据该体目标轨迹解算模型获得t时刻体目标质心在地理坐标系下的坐标，
步骤六：建立体目标轨迹和姿态解算模型，然后测量体目标运动过程姿态参数，根据该体目标轨迹和姿态解算模型获得t时刻体目标质心在地理坐标系下的坐标；
步骤五所述体目标轨迹解算模型为：
Figure FDA0003538668290000011
其中，
Figure FDA0003538668290000012
为t时刻体目标质心在地理坐标系下的坐标，
Figure FDA0003538668290000013
为体目标上第k个换能器在地理坐标系下的坐标，
Figure FDA0003538668290000014
为第i个水下信标在地理坐标系下的坐标，Ri为t时刻体目标与第i个水下信标之间信号的传播距离，i为水下信标的编号，第i个水下信标为t时刻能够接收到换能器发送信号的水下信标，k为体目标上换能器的编号，第k个换能器为t时刻水下信标能够接收到其发送信号的换能器；
步骤六所述体目标轨迹和姿态解算模型为：
Figure FDA0003538668290000015
其中，
Figure FDA0003538668290000016
为t时刻体目标质心在地理坐标系下的坐标，RSG为体目标坐标系到地理坐标系的旋转矩阵，
Figure FDA0003538668290000017
为体目标上第k个换能器在体目标坐标系下的坐标，
Figure FDA0003538668290000018
为水下信标在地理坐标系下的坐标，Ri为t时刻体目标与水下信标之间信号的传播距离，i为水下信标的编号，第i个水下信标为t时刻能够接收到换能器发送信号的水下信标，k为体目标上换能器的编号，第k个换能器为t时刻水下信标能够接收到其发送信号的换能器。
2.根据权利要求1所述的一种体目标三维运动轨迹测量方法，其特征在于，步骤五或步骤六之后均执行步骤七，步骤七具体为：
判断t时刻体目标质心在地理坐标系下的坐标是否为终点坐标，
是，则将初始时刻至t时刻步骤五或步骤六获得的所有坐标结果拟合成体目标三维运动轨迹，
否，则使t＝t+1，然后返回步骤三。
3.根据权利要求1或2所述的一种体目标三维运动轨迹测量方法，其特征在于，将体目标上换能器发出信号的时刻与t时刻作差获得时间差，将时间差与当前水域水下声速相乘，获得t时刻体目标上换能器与水下信标之间信号的传播距离。
4.根据权利要求1或2所述的一种体目标三维运动轨迹测量方法，其特征在于，
体目标坐标系为原点为o的空间直角坐标系，记为o-xsyszs，其中，xs轴和ys轴均位于体目标平面内、且分别指向右舷和船首方向，zs轴呈竖直方向设置、且指向上方，zs轴垂直于体目标平面，原点o位于体目标的质心处；
地理坐标系为原点为o的空间直角坐标系，记为o-xGyGzG，其中，xG轴指向东方，yG轴指向北方，zG轴指向上方；
体目标运动过程姿态参数为α、β和γ，
当体目标质心坐标从地理坐标系旋转到体目标坐标系时，α为zG轴旋转到zs轴的角度，β为xG轴旋转到xs轴的角度，γ为yG轴旋转到ys轴的角度。
5.根据权利要求4所述的一种体目标三维运动轨迹测量方法，其特征在于，地理坐标系到体目标坐标系的旋转矩阵RGS为：
Figure FDA0003538668290000021
6.根据权利要求1所述的一种体目标三维运动轨迹测量方法，其特征在于，t时刻体目标质心在地理坐标系下的坐标
Figure FDA0003538668290000022
的矩阵表达式为：
Figure FDA0003538668290000023
其中，X0为初始时刻体目标质心在地理坐标系下的坐标，P为测量元素总误差的协方差矩阵，
Figure FDA0003538668290000031
Figure FDA0003538668290000032
R为量测得到的体目标与水下信标之间量测的传播距离，R0为将X0代入体目标轨迹解算模型后获得的体目标与水下信标之间信号的传播距离。
CN202111181944.XA 2021-10-11 2021-10-11 一种体目标三维运动轨迹测量方法 Active CN113945892B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111181944.XA CN113945892B (zh) 2021-10-11 2021-10-11 一种体目标三维运动轨迹测量方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111181944.XA CN113945892B (zh) 2021-10-11 2021-10-11 一种体目标三维运动轨迹测量方法

Publications (2)

Publication Number Publication Date
CN113945892A CN113945892A (zh) 2022-01-18
CN113945892B true CN113945892B (zh) 2022-05-03

Family

ID=79329638

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111181944.XA Active CN113945892B (zh) 2021-10-11 2021-10-11 一种体目标三维运动轨迹测量方法

Country Status (1)

Country Link
CN (1) CN113945892B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115469270B (zh) * 2022-08-16 2023-06-27 哈尔滨工程大学 基于检测前跟踪的长基线定位方法及系统

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105823480A (zh) * 2016-03-18 2016-08-03 中国海洋大学 基于单信标的水下移动目标定位算法
CN108318863A (zh) * 2017-12-28 2018-07-24 浙江大学 基于海底信标的水下无人设备无源定位方法及其系统
CN109596128A (zh) * 2019-01-14 2019-04-09 哈尔滨工程大学 一种基于多水听器提高多auv协同定位性能的方法
CN112977728A (zh) * 2021-03-23 2021-06-18 中国水产科学研究院黄海水产研究所 用于声学评估系统的智能位置校正设备

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107272004B (zh) * 2017-07-24 2020-10-27 哈尔滨工程大学 一种基于水下信标位置修正的单信标测距定位方法
CN108444478B (zh) * 2018-03-13 2021-08-10 西北工业大学 一种用于水下航行器的移动目标视觉位姿估计方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105823480A (zh) * 2016-03-18 2016-08-03 中国海洋大学 基于单信标的水下移动目标定位算法
CN108318863A (zh) * 2017-12-28 2018-07-24 浙江大学 基于海底信标的水下无人设备无源定位方法及其系统
CN109596128A (zh) * 2019-01-14 2019-04-09 哈尔滨工程大学 一种基于多水听器提高多auv协同定位性能的方法
CN112977728A (zh) * 2021-03-23 2021-06-18 中国水产科学研究院黄海水产研究所 用于声学评估系统的智能位置校正设备

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
An Design Method of Underwater Positioning System Based on USBL;Zhang Ling;《Applied Mechanics and Materials》;20141231;全文 *
Underwater target localization using long baseline positioning system;Jucheng Zhang et al.;《ELSEVIER》;20160425;全文 *
基于短基线组合系统实现浮标阵水下目标运动轨迹精确测量;刘百峰 等;《舰船科学技术》;20130930;全文 *
长基线声学定位系统跟踪解算优化方法;韩云峰 等;《声学学报》;20170131;全文 *

Also Published As

Publication number Publication date
CN113945892A (zh) 2022-01-18

Similar Documents

Publication Publication Date Title
CN108614258B (zh) 一种基于单水声信标距离量测的水下定位方法
CN111273298B (zh) 基于波浪滑翔器组网技术的水下声学目标定位与跟踪方法
CN110294080B (zh) 一种利用超短基线实现水下精确作业的方法
CN105547290B (zh) 一种基于超短基线定位系统的从潜器导航方法
CN111829512B (zh) 一种基于多传感器数据融合的auv导航定位方法及系统
CN109738902B (zh) 一种基于同步信标模式的水下高速目标高精度自主声学导航方法
CN111751856B (zh) 一种基于ppp技术的海底大地基准点精确定位方法
CN110389318B (zh) 一种基于立体六元阵的水下移动平台定位系统及方法
CN108413983A (zh) 一种sins/usbl一体化定位系统安装误差标定的机械方法
CN111578944B (zh) 一种基于单信标的水下滑翔机定位方法
CN113945892B (zh) 一种体目标三维运动轨迹测量方法
CN111812694A (zh) 一种长基线船载定位装置及其定位方法
CN110824430A (zh) 基于北斗定位系统的水下定位方法
CN112540371A (zh) 一种近底多波束坐标转换处理方法
CN111735455A (zh) 基于改进的高斯距离迭代算法对接回收组合导航方法
CN108562872A (zh) 一种超短基线水下声学定位校准时异常值探测的方法
CN117146830B (zh) 一种自适应多信标航位推算和长基线的紧组合导航方法
CN113933526B (zh) 一种体目标高精度三维运动速度测量方法
CN112083425A (zh) 一种引入径向速度的sins/lbl紧组合导航方法
CN108803374B (zh) 一种无人艇环境数据仿真方法
CN112666562B (zh) 一种合成孔径声纳运动补偿与成像方法
CN113156369B (zh) 一种海上声学相位中心校准方法
CN112462322A (zh) 一种水下蛙人定位方法及定位系统
CN113176539A (zh) 一种水声信号噪声多级抑制与稳健定位系统及定位方法
CN113218372B (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