CN108332756A - 一种基于拓扑信息的水下航行器协同定位方法 - Google Patents
一种基于拓扑信息的水下航行器协同定位方法 Download PDFInfo
- Publication number
- CN108332756A CN108332756A CN201810143849.2A CN201810143849A CN108332756A CN 108332756 A CN108332756 A CN 108332756A CN 201810143849 A CN201810143849 A CN 201810143849A CN 108332756 A CN108332756 A CN 108332756A
- Authority
- CN
- China
- Prior art keywords
- submarine navigation
- navigation device
- topology information
- moment
- aircraft
- 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.)
- Granted
Links
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 SmoothingAndMapping(gtsam)来进行。
技术方案
一种基于拓扑信息的水下航行器协同定位方法,其特征在于步骤如下:
步骤1:采用matlab模拟出四个水下航行器的运行轨迹图,并取得四个水下航行器的运行轨迹图的测量数据:
步骤2:以下式表示拓扑信息,计算因子图中的水下航行器之间的距离
其中:l表示航行器的数量,其中表示水下航行器之间的距离,和分别是第i个和第j个航行器的横坐标,和分别是第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是四个水下航行器之间的距离约束,也就是拓扑因子,把四个航行器在同一时刻的位置关联起来。
在构建拓扑因子时,采用如下公式计算因子图中的水下航行器之间的距离
上式中l表示航行器的数量,其中表示水下航行器之间的距离,和分别是第i个和第j个航行器的横坐标,和分别是第i个和第j个航行器的纵坐标,t0表示某个具体时刻,此公式就计算出来在t0时刻水下航行器之间的距离和,即拓扑信息。
在计算拓扑信息时,由误差引起的不确定性也会随之传递,我们用下面的公式来表示拓扑信息的不确定性:
Cy=FxCxFx T (2)
其中Cx是指从声呐测量的最初的不确定性,是一个协方差矩阵。Fx是传递函数的雅克比矩阵,Fx T是雅克比矩阵的转置,Cy就是得到的拓扑信息的不确定性,即所求的ftop的不确定性为Cy。
由以上的数据关联和计算公式,我们可以用gtsam构建出因子图,构建的因子图如附图1。
其中,变量x1,x2,x3,x4分别是四个水下航行器各个时刻的位置状态,fIN是内部导航系统的里程计因子,ftop是四个水下航行器之间的距离约束,也就是拓扑因子。
得到有拓扑约束的因子图后,此处采用Levenberg-Marquardt法进行优化,得到水下航行器各个时刻的估计位姿。
第三步:比较加上拓扑约束的协同定位方法和传统方法的差别,得出此发明可以更精确的定位。
在此发明中,采用计算均方根偏差(RMSE)来比较两种方法的误差大小,计算公式如下:
上式中n表示测量的总步数,k表示第k步,l表示航行器的个数,其中是对第l个水下航行器横坐标的估计值,是第l个水下航行器横坐标的真值,是对第l个水下航行器纵坐标的估计值,是第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:以下式表示拓扑信息,计算因子图中的水下航行器之间的距离
其中:l表示航行器的数量,其中表示水下航行器之间的距离,和分别是第i个和第j个航行器的横坐标,和分别是第i个和第j个航行器的纵坐标,t0表示某个具体时刻;
步骤3:根据计算的拓扑信息,采用gtsam构建因子图;x1,x2,x3,x4分别是四个水下航行器各个时刻的位置状态,每个水下航行器本时刻坐标与下一时刻坐标之间的联系为内部导航系统的里程计因子fIN,每个时刻四个水下航行器之间的联系为四个水下航行器之间的距离约束ftop;
步骤4:采用Levenberg-Marquardt法对拓扑约束的因子图进行优化,得到水下航行器各个时刻的估计位姿。
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 true CN108332756A (zh) | 2018-07-27 |
CN108332756B 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) |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110749890A (zh) * | 2019-10-15 | 2020-02-04 | 哈尔滨工程大学 | 一种多水下机器人协同海流估计方法 |
CN111025229A (zh) * | 2019-12-19 | 2020-04-17 | 哈尔滨工程大学 | 一种水下机器人纯方位目标估计方法 |
CN112698266A (zh) * | 2020-12-04 | 2021-04-23 | 哈尔滨工程大学青岛船舶科技有限公司 | 一种基于概率图模型的水下目标定位方法 |
CN114577211A (zh) * | 2022-02-25 | 2022-06-03 | 哈尔滨工程大学 | 考虑洋流影响的基于因子图的主从式auv协同定位方法 |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20090125223A1 (en) * | 2006-03-31 | 2009-05-14 | Higgins Robert P | Video navigation |
US20110029845A1 (en) * | 2009-03-27 | 2011-02-03 | University Of Connecticut | Apparatus, Systems and Methods Including Nonbinary Low Density Parity Check Coding For Enhanced Multicarrier Underwater Acoustic Communications |
CN102914786A (zh) * | 2012-09-25 | 2013-02-06 | 北京理工大学 | 水下自主航行器auv自主定位方法 |
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协同定位方法 |
US20170176994A1 (en) * | 2015-12-21 | 2017-06-22 | Disney Enterprises, Inc. | Method and device for multi-agent path planning |
CN107132580A (zh) * | 2017-04-28 | 2017-09-05 | 任勇 | 水下目标的探测系统 |
-
2018
- 2018-02-12 CN CN201810143849.2A patent/CN108332756B/zh active Active
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20090125223A1 (en) * | 2006-03-31 | 2009-05-14 | Higgins Robert P | Video navigation |
US20110029845A1 (en) * | 2009-03-27 | 2011-02-03 | University Of Connecticut | Apparatus, Systems and Methods Including Nonbinary Low Density Parity Check Coding For Enhanced Multicarrier Underwater Acoustic Communications |
CN102914786A (zh) * | 2012-09-25 | 2013-02-06 | 北京理工大学 | 水下自主航行器auv自主定位方法 |
CN102980579A (zh) * | 2012-11-15 | 2013-03-20 | 哈尔滨工程大学 | 一种自主水下航行器自主导航定位方法 |
CN103674015A (zh) * | 2013-12-13 | 2014-03-26 | 国家电网公司 | 一种无轨化定位导航方法及装置 |
US20170176994A1 (en) * | 2015-12-21 | 2017-06-22 | Disney Enterprises, Inc. | Method and device for multi-agent path planning |
CN106525042A (zh) * | 2016-09-27 | 2017-03-22 | 哈尔滨工程大学 | 一种基于蚁群与扩展卡尔曼滤波相结合的多auv协同定位方法 |
CN107132580A (zh) * | 2017-04-28 | 2017-09-05 | 任勇 | 水下目标的探测系统 |
Non-Patent Citations (3)
Title |
---|
FEIHU ZHANG,等: "Vehicle Infrastructure Cooperative Localization Using Factor Graphs", 《2016 IEEE INTELLIGENT VEHICLES SYMPOSIUM (IV)》 * |
刘亚龙: "多水下航行器协同定位算法研究", 《中国博士学位论文全文数据库》 * |
宁学侃: "移动水下网络的协作跟踪研究", 《中国新通信》 * |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110749890A (zh) * | 2019-10-15 | 2020-02-04 | 哈尔滨工程大学 | 一种多水下机器人协同海流估计方法 |
CN111025229A (zh) * | 2019-12-19 | 2020-04-17 | 哈尔滨工程大学 | 一种水下机器人纯方位目标估计方法 |
CN111025229B (zh) * | 2019-12-19 | 2022-11-01 | 哈尔滨工程大学 | 一种水下机器人纯方位目标估计方法 |
CN112698266A (zh) * | 2020-12-04 | 2021-04-23 | 哈尔滨工程大学青岛船舶科技有限公司 | 一种基于概率图模型的水下目标定位方法 |
CN114577211A (zh) * | 2022-02-25 | 2022-06-03 | 哈尔滨工程大学 | 考虑洋流影响的基于因子图的主从式auv协同定位方法 |
CN114577211B (zh) * | 2022-02-25 | 2024-06-28 | 哈尔滨工程大学 | 考虑洋流影响的基于因子图的主从式auv协同定位方法 |
Also Published As
Publication number | Publication date |
---|---|
CN108332756B (zh) | 2021-05-07 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108332756A (zh) | 一种基于拓扑信息的水下航行器协同定位方法 | |
CN105823480B (zh) | 基于单信标的水下移动目标定位算法 | |
CN107990891B (zh) | 基于长基线和信标在线标定的水下机器人组合导航方法 | |
CN106643709B (zh) | 一种海上运载体的组合导航方法及装置 | |
CN106679662B (zh) | 一种基于tma技术的水下机器人单信标组合导航方法 | |
CN108614258B (zh) | 一种基于单水声信标距离量测的水下定位方法 | |
CN106123850A (zh) | Auv配载多波束声呐水下地形测绘修正方法 | |
CN109738902B (zh) | 一种基于同步信标模式的水下高速目标高精度自主声学导航方法 | |
CN105973268B (zh) | 一种基于共基座安装的传递对准精度定量评估方法 | |
CN105547290B (zh) | 一种基于超短基线定位系统的从潜器导航方法 | |
CN111504318B (zh) | 一种基于多磁偶极子反演的海洋导航辅助方法 | |
CN113433553B (zh) | 一种水下机器人多源声学信息融合精确导航方法 | |
CN110646783B (zh) | 一种水下航行器的水下信标定位方法 | |
CN110132281B (zh) | 一种基于询问应答模式的水下高速目标高精度自主声学导航方法 | |
CN104280024B (zh) | 一种深水机器人组合导航装置和方法 | |
CN105738869B (zh) | 一种适用于单水听器的深水信标搜索定位方法 | |
CN110007318B (zh) | 风场干扰下基于卡尔曼滤波的单无人机判断gps欺骗的方法 | |
CN113933526B (zh) | 一种体目标高精度三维运动速度测量方法 | |
CN106643297A (zh) | 一种运动平台矢量脱靶量参数估计修正方法 | |
CN111220146A (zh) | 一种基于高斯过程回归学习的水下地形匹配定位方法 | |
CN112611376B (zh) | 一种RGI-Lidar/SINS紧耦合AUV水下导航定位方法与系统 | |
CN106896361A (zh) | 一种深水机器人多模型ekf组合导航装置及方法 | |
CN117146830A (zh) | 一种自适应多信标航位推算和长基线的紧组合导航方法 | |
CN202837553U (zh) | 距离及方向校正的位置估测装置 | |
CN111220158B (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 |