CN108332756B - 一种基于拓扑信息的水下航行器协同定位方法 - Google Patents

一种基于拓扑信息的水下航行器协同定位方法 Download PDF

Info

Publication number
CN108332756B
CN108332756B CN201810143849.2A CN201810143849A CN108332756B CN 108332756 B CN108332756 B CN 108332756B CN 201810143849 A CN201810143849 A CN 201810143849A CN 108332756 B CN108332756 B CN 108332756B
Authority
CN
China
Prior art keywords
underwater
underwater vehicle
underwater vehicles
moment
distance
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
CN201810143849.2A
Other languages
English (en)
Other versions
CN108332756A (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.)
Northwestern Polytechnical University
Original Assignee
Northwestern Polytechnical 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 Northwestern Polytechnical University filed Critical Northwestern Polytechnical University
Priority to CN201810143849.2A priority Critical patent/CN108332756B/zh
Publication of CN108332756A publication Critical patent/CN108332756A/zh
Application granted granted Critical
Publication of CN108332756B publication Critical patent/CN108332756B/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/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明涉及一种基于拓扑信息的水下航行器协同定位方法，用matlab模拟出四个水下航行器的运行轨迹图，用gtsam构建出因子图，以水下航行器之间的距离作为约束条件，加入到对位姿的优化中去，使得定位更加的精确。本发明解决水下航行器由于接收不到高精度的GPS信号而产生的定位误差，经过实验验证，该方法确实可以提高定位的精度，减小误差。

Description

一种基于拓扑信息的水下航行器协同定位方法
技术领域
本发明属于水下航行器的协同定位领域，涉及一种基于拓扑信息的水下航行器协同定位方法。
背景技术
水下航行器在水下作业时，由于接收不到精准的GPS信号，只能依赖内部的传感器进行导航，像惯性导航等，在这种传统的方式下，误差会随着时间的延长而积累。鉴于此，本文发明了一种利用拓扑因子的协同定位方法，此方法把水下航行器间的距离作为约束因子，使得水下航行器不用浮上水面也可以获得比之前高的定位精度。而且，在加上GPS信号的情况下，使用了拓扑因子作为约束的定位方法也比传统方法的定位精度高。
发明内容
要解决的技术问题
为了避免现有技术的不足之处，本发明提出一种基于拓扑信息的水下航行器协同定位方法，利用拓扑因子增加协同定位的精确度。实验用Georgia Tech Smoothing AndMapping(gtsam)来进行。
技术方案
一种基于拓扑信息的水下航行器协同定位方法，其特征在于步骤如下：
步骤1：采用matlab模拟出四个水下航行器的运行轨迹图，并取得四个水下航行器的运行轨迹图的测量数据：
步骤2：以下式表示拓扑信息,计算因子图中的水下航行器之间的距离
Figure GDA0002957258830000011
Figure GDA0002957258830000012
其中：l表示航行器的数量，其中
Figure GDA0002957258830000013
表示水下航行器之间的距离，
Figure GDA0002957258830000014
和
Figure GDA0002957258830000015
分别是第i个和第j个航行器的横坐标，
Figure GDA0002957258830000021
和
Figure GDA0002957258830000022
分别是第i个和第j个航行器的纵坐标，t0表示某个具体时刻；
步骤3：根据计算的拓扑信息，采用gtsam构建因子图；x1,x2,x3,x4分别是四个水下航行器各个时刻的位置状态，每个水下航行器本时刻坐标与下一时刻坐标之间的联系为内部导航系统的里程计因子fIN，每个时刻四个水下航行器之间的联系为四个水下航行器之间的距离约束ftop；
步骤4：采用Levenberg-Marquardt法对拓扑约束的因子图进行优化，得到水下航行器各个时刻的估计位姿。
有益效果
本发明提出的一种基于拓扑信息的水下航行器协同定位方法，用matlab模拟出四个水下航行器的运行轨迹图，用gtsam构建出因子图，以水下航行器之间的距离作为约束条件，加入到对位姿的优化中去，使得定位更加的精确。本发明解决水下航行器由于接收不到高精度的GPS信号而产生的定位误差，经过实验验证，该方法确实可以提高定位的精度，减小误差。
附图说明
图1：水下航行器的拓扑因子图；
图2：水下航行器的行驶轨迹；
图3：使用拓扑因子协同定位与传统方法的实验结果图。
具体实施方式
现结合实施例、附图对本发明作进一步描述：
在本实验中，水面船和四个水下航行器都配备有声呐和GPS,且水下航行器有内部的导航系统。
第一步：我们用matlab模拟出四个水下航行器的运行轨迹图(设定初始位置，方向，和增长速度的直线)，轨迹图如附图2，并记录下数据：速度，位置坐标，行驶距离。
第二步：用gtsam构建出因子图并进行优化。因子图如附图1，其中，变量x1,x2,x3,x4分别是四个水下航行器各个时刻的位置状态，fIN是内部导航系统的里程计因子，表示两个位置之间的距离变化，把两个相邻的位置关联起来。ftop是四个水下航行器之间的距离约束，也就是拓扑因子，把四个航行器在同一时刻的位置关联起来。
在构建拓扑因子时，采用如下公式计算因子图中的水下航行器之间的距离
Figure GDA0002957258830000031
Figure GDA0002957258830000032
上式中l表示航行器的数量，其中
Figure GDA0002957258830000033
表示水下航行器之间的距离，
Figure GDA0002957258830000034
和
Figure GDA0002957258830000035
分别是第i个和第j个航行器的横坐标，
Figure GDA0002957258830000036
和
Figure GDA0002957258830000037
分别是第i个和第j个航行器的纵坐标，t0表示某个具体时刻，此公式就计算出来在t0时刻水下航行器之间的距离和，即拓扑信息。
在计算拓扑信息时，由误差引起的不确定性也会随之传递，我们用下面的公式来表示拓扑信息的不确定性：
Cy＝FxCxFx T (2)
其中Cx是指从声呐测量的最初的不确定性，是一个协方差矩阵。Fx是传递函数
Figure GDA0002957258830000038
的雅克比矩阵，Fx T是雅克比矩阵的转置，Cy就是得到的拓扑信息的不确定性，即所求的ftop的不确定性为Cy。
由以上的数据关联和计算公式，我们可以用gtsam构建出因子图，构建的因子图如附图1。
其中，变量x1,x2,x3,x4分别是四个水下航行器各个时刻的位置状态，fIN是内部导航系统的里程计因子，ftop是四个水下航行器之间的距离约束，也就是拓扑因子。
得到有拓扑约束的因子图后，此处采用Levenberg-Marquardt法进行优化，得到水下航行器各个时刻的估计位姿。
第三步：比较加上拓扑约束的协同定位方法和传统方法的差别，得出此发明可以更精确的定位。
在此发明中，采用计算均方根偏差(RMSE)来比较两种方法的误差大小，计算公式如下：
Figure GDA0002957258830000041
上式中n表示测量的总步数，k表示第k步，l表示航行器的个数，其中
Figure GDA0002957258830000042
是对第l个水下航行器横坐标的估计值，
Figure GDA0002957258830000043
是第l个水下航行器横坐标的真值，
Figure GDA0002957258830000044
是对第l个水下航行器纵坐标的估计值，
Figure GDA0002957258830000045
是第l个水下航行器纵坐标的真值。
本发明的一个具体实施步骤如下：
⑴设置GPS精度为10倍的方差为1的高斯白噪声，内部里程计的精度为方差为1的高斯白噪声，声呐的精度为0.1倍的方差为1的高斯白噪声，以此进行实验。
⑵确定水下航行器的初始位置和x，y方向的速度
航行器①(100,200,18,5)航行器②(-100,300，5,20)
航行器③(-300,-200，-17,20)航行器④(300，-200,19，-17)
括号里的四个数据分别为航行器的x，y初始位置和x，y方向上的速度。以此速度状态进行300步，得出每个水下航行器的运行轨迹图。在图2中，每个航行器都朝着不同的方向行驶了不同的距离，在上面采集真值和测量值。
⑶实验中对每个水下航行器进行300次数据采集，得出仿真图进行对比分析。在图3中，实线代表使用了拓扑因子作为约束后的误差大小，虚线表示传统方法的误差大小。由图3可以看出，使用拓扑信息后的协同定位方法比传统方法的均方根误差要小1.5，可见，引入拓扑因子作为约束能使得对水下航行器的定位更加的精准。

Claims (1)

1.一种基于拓扑信息的水下航行器协同定位方法，其特征在于步骤如下：
步骤1：采用matlab模拟出四个水下航行器的运行轨迹图，并取得四个水下航行器的运行轨迹图的测量数据，所述测量数据为速度、位置坐标、行驶距离；
步骤2：以下式表示拓扑信息,计算因子图中的水下航行器之间的距离
Figure FDA0002929884800000011
Figure FDA0002929884800000012
其中：l表示航行器的数量，其中
Figure FDA0002929884800000013
表示水下航行器之间的距离，
Figure FDA0002929884800000014
和
Figure FDA0002929884800000015
分别是第i个和第j个航行器的横坐标，
Figure FDA0002929884800000016
和
Figure FDA0002929884800000017
分别是第i个和第j个航行器的纵坐标，t0表示某个具体时刻；
步骤3：根据计算的拓扑信息，采用gtsam构建因子图；x1,x2,x3,x4分别是四个水下航行器各个时刻的位置状态，每个水下航行器本时刻坐标与下一时刻坐标之间的联系为内部导航系统的里程计因子fIN，每个时刻四个水下航行器之间的联系为四个水下航行器之间的距离约束ftop；
步骤4：采用Levenberg-Marquardt法对拓扑约束的因子图进行优化，得到水下航行器各个时刻的估计位姿。
CN201810143849.2A 2018-02-12 2018-02-12 一种基于拓扑信息的水下航行器协同定位方法 Active CN108332756B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810143849.2A CN108332756B (zh) 2018-02-12 2018-02-12 一种基于拓扑信息的水下航行器协同定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810143849.2A CN108332756B (zh) 2018-02-12 2018-02-12 一种基于拓扑信息的水下航行器协同定位方法

Publications (2)

Publication Number Publication Date
CN108332756A CN108332756A (zh) 2018-07-27
CN108332756B true CN108332756B (zh) 2021-05-07

Family

ID=62929166

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810143849.2A Active CN108332756B (zh) 2018-02-12 2018-02-12 一种基于拓扑信息的水下航行器协同定位方法

Country Status (1)

Country Link
CN (1) CN108332756B (zh)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110749890A (zh) * 2019-10-15 2020-02-04 哈尔滨工程大学 一种多水下机器人协同海流估计方法
CN111025229B (zh) * 2019-12-19 2022-11-01 哈尔滨工程大学 一种水下机器人纯方位目标估计方法
CN112698266B (zh) * 2020-12-04 2023-06-27 哈尔滨工程大学青岛船舶科技有限公司 一种基于概率图模型的水下目标定位方法
CN114577211A (zh) * 2022-02-25 2022-06-03 哈尔滨工程大学 考虑洋流影响的基于因子图的主从式auv协同定位方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102980579A (zh) * 2012-11-15 2013-03-20 哈尔滨工程大学 一种自主水下航行器自主导航定位方法
CN103674015A (zh) * 2013-12-13 2014-03-26 国家电网公司 一种无轨化定位导航方法及装置
CN106525042A (zh) * 2016-09-27 2017-03-22 哈尔滨工程大学 一种基于蚁群与扩展卡尔曼滤波相结合的多auv协同定位方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8666661B2 (en) * 2006-03-31 2014-03-04 The Boeing Company Video navigation
US8504891B2 (en) * 2009-03-27 2013-08-06 University Of Connecticut Apparatus, systems and methods including nonbinary low density parity check coding for enhanced multicarrier underwater acoustic communications
CN102914786B (zh) * 2012-09-25 2014-05-14 北京理工大学 水下自主航行器auv自主定位方法
US10579926B2 (en) * 2015-12-21 2020-03-03 Disney Enterprises, Inc. Method and device for multi-agent path planning
CN107132580A (zh) * 2017-04-28 2017-09-05 任勇 水下目标的探测系统

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102980579A (zh) * 2012-11-15 2013-03-20 哈尔滨工程大学 一种自主水下航行器自主导航定位方法
CN103674015A (zh) * 2013-12-13 2014-03-26 国家电网公司 一种无轨化定位导航方法及装置
CN106525042A (zh) * 2016-09-27 2017-03-22 哈尔滨工程大学 一种基于蚁群与扩展卡尔曼滤波相结合的多auv协同定位方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Vehicle Infrastructure Cooperative Localization Using Factor Graphs;Feihu Zhang,等;《2016 IEEE Intelligent Vehicles Symposium (IV)》;20160808;第1085-1090 *
移动水下网络的协作跟踪研究;宁学侃;《中国新通信》;20160715(第13期);第1-2页 *

Also Published As

Publication number Publication date
CN108332756A (zh) 2018-07-27

Similar Documents

Publication Publication Date Title
CN108332756B (zh) 一种基于拓扑信息的水下航行器协同定位方法
CN109459040B (zh) 基于rbf神经网络辅助容积卡尔曼滤波的多auv协同定位方法
CN106885576B (zh) 一种基于多点地形匹配定位的auv航迹偏差估计方法
CN111948602A (zh) 基于改进Taylor级数的二维UWB室内定位方法
CN107990891B (zh) 基于长基线和信标在线标定的水下机器人组合导航方法
CN109282808B (zh) 用于桥梁三维巡航检测的无人机与多传感器融合定位方法
CN108715166B (zh) 基于深度学习的车辆稳定性指标估计方法
CN105136145A (zh) 一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法
CN114526745A (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
CN110132281B (zh) 一种基于询问应答模式的水下高速目标高精度自主声学导航方法
CN111504318B (zh) 一种基于多磁偶极子反演的海洋导航辅助方法
CN110320520B (zh) 一种测深信息同步定位与建图的鲁棒后端图优化方法
CN110726415B (zh) 一种自适应的水下多波束同步定位与建图方法
CN110906933A (zh) 一种基于深度神经网络的auv辅助导航方法
CN107966145B (zh) 一种基于稀疏长基线紧组合的auv水下导航方法
CN109738902B (zh) 一种基于同步信标模式的水下高速目标高精度自主声学导航方法
CN113433553B (zh) 一种水下机器人多源声学信息融合精确导航方法
CN104280024B (zh) 一种深水机器人组合导航装置和方法
CN112666519B (zh) 一种基于广义二阶时延差的水下目标高精度定位方法
CN106896361A (zh) 一种深水机器人多模型ekf组合导航装置及方法
CN111708009A (zh) 一种水下声学异步测距方法
CN114111840B (zh) 一种基于组合导航的dvl误差参数在线标定方法
CN114998540A (zh) 一种智慧城市传感器探测主动同步定位与建图方法
CN114763998A (zh) 基于微型雷达阵列的未知环境并行导航方法和系统
CN110749327B (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