CN105628016B - 一种基于超短基线的导航定位方法 - Google Patents
一种基于超短基线的导航定位方法 Download PDFInfo
- Publication number
- CN105628016B CN105628016B CN201410605690.3A CN201410605690A CN105628016B CN 105628016 B CN105628016 B CN 105628016B CN 201410605690 A CN201410605690 A CN 201410605690A CN 105628016 B CN105628016 B CN 105628016B
- Authority
- CN
- China
- Prior art keywords
- underwater robot
- underwater
- ultra
- moment
- location information
- 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
Links
Landscapes
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开一种基于超短基线(USBL)的导航定位方法,该发明将水下无人无缆水下机器人(水下机器人)的超短基线定位信息通过声通讯机发给水下机器人,以实现水下机器人的组合导航功能。该发明分成2个阶段,第一阶段是利用USBL对水下机器人进行跟踪定位,并将定位信息通过声通讯机发给水下机器人,第二阶段是水下机器人接收到定位信息,再经过运动补偿对导航算法进行状态更新。本发明操作简单,能准确有效地利用USBL定位信息实现水下机器人组合导航,进而提高水下机器人在水下的导航定位精度。
Description
技术领域
本发明涉及水下机器人技术领域,尤其涉及一种基于超短基线(USBL)的导航定位的方法。
背景技术
水下机器人在复杂的海底区域进行长时间作业时,其自身具备较高的导航定位精度是至关重要的。自主导航方法由于有时间累积误差,因此需要外部的导航信息进行实时修正。长基线基阵可以为水下机器人提供高精度的定位信息,但缺点是布阵和回收过程繁琐,而且受作业现场环境的影响较大。超短基线安装在母船上,可以实时监控水下水下机器人位置,它既不需要布阵,同时可随母船跟踪水下机器人,缺点是现有的超短基线没有水声通讯功能,因而定位信息无法提供给水下机器人。利用声通讯机可以将USBL的定位信息发给水下机器人,再由水下机器人进行数据融合,进而可以实现USBL组合导航功能,这既避免了布阵和回收过程的繁琐,又能使水下机器人的作业区域更广,使用更加灵活。
发明内容
为了克服现有方法的不足,本发明要解决的技术问题是提供一种基于超短基线的导航定位方法,利用USBL跟踪定位水下机器人并通过声通讯机将定位信息回发给水下机器人使其实现组合导航,同时具备对声通讯时延进行补偿的能力。
本发明为实现上述目的所采用的技术方案是:一种基于超短基线的导航定位方法,包括以下步骤:
利用超短基线对水下机器人进行定位;
通过声通讯机将母船位置信息和超短基线定位到的水下机器人位置信息发给水下机器人;
水下机器人接收声通讯机发出的定位信息;
结合运动补偿算法计算水下机器人收到声通讯机信息时自身的位置信息;
将经过运动补偿后的位置信息作为观测量对水下机器人的实际位置进行更新。
所述的利用超短基线对水下机器人进行定位具体为:通过超短基线基阵与水下机器人搭载的应答器通信得到水下机器人的位置信息。
所述运动补偿包括以下步骤:
第1步:获得母船和水下机器人位置信息;
第2步:获得水下机器人收到声通讯机发出信息时刻的深度数据;
第3步:解算水下机器人当前时刻的位置:
其中,d1是k时刻母船与水下机器人之间的水平距离;d2是k-1时刻母船与水下机器人间的水平距离,该变量为已知量;d3是Δt时刻里水下机器人所走过的水平距离;Δt是从声通讯机发送数据到水下机器人接收数据所经历的时间;hk是k时刻水下机器人所在的深度;Xk-1是k-1时刻超短基线获得的水下机器人位置信息;f(·)是水声信号在水下的传播距离和传播时间的关系函数;是水下机器人在接收声通讯机数据过程中的速度矢量,为已知量;k-1时刻为超短基线得到水下机器人位置信息的时刻;k时刻为水下机器人收到其位置信息的时刻;
解上面方程组得到Δt,则k时刻水下机器人所在的位置Xk为:
所述第2步中的深度数据由水下机器人搭载的深度计提供。
所述对水下机器人的实际位置进行更新,包括以下步骤:
计算观测新息:
其中,v为观测新息,Xk为k时刻水下机器人所在的位置;H为观测矩阵,由系统的观测模型得来,是已知量;为基于k-1时刻得到的k时刻水下机器人状态的预测;
计算系统增益:
Kk=Pk|k-1HT(HPk|k-1HT+Rk)-1
其中,Kk为系统增益,Pk|k-1为系统的状态协方差预测矩阵,Rk为观测噪声协方差矩阵;
更新:
本发明具有以下优点及有益效果:
1.利用USBL跟踪定位水下机器人并通过声通讯机将定位信息回发给水下机器人使其实现组合导航
2.操作简单,能准确有效地利用USBL定位信息实现水下机器人组合导航,进而提高水下机器人在水下的导航定位精度。
3.具备对声通讯时延进行补偿的能力。
附图说明
图1是本发明的组成示意图;
图2是本发明的运动补偿算法流程图。
具体实施方式
下面结合附图及实施例对本发明做进一步的详细说明。
本发明由的水下机器人、母船、USBL系统、水面监控计算机、声通讯机组成,其中水下机器人需要搭载深度计、DVL(多普勒测速仪)以及角速度陀螺,如图1所示。
USBL基阵与水下机器人上搭载的应答器通信,再由USBL-BOX计算出水下机器人位置,然后通过声通讯机将该位置及母船位置信息发给水下机器人。
USBL定位数据的判别方法如图2所示,水下机器人收到母船声通讯机发来的信息并解算它与上一时刻位置数据之间距离(如果是第一个数据则不计算),当该距离小于则计数器i加1,当i大于等于N时,则判别该位置信息是有效的,可以用来更新水下机器人的位置。
当得到有效的位置数据以后,水下机器人控制系统通过运动补偿算法算出水下机器人当前的位置。
第1步:获得母船和水下机器人的位置信息。
第2步:获得水下机器人收到声通讯机信息时当前的深度数据。
水下机器人当前的深度数据由水下机器人上搭载的深度计设备提供。
第3步:解算水下机器人当前时刻的位置。
假设USBL在k-1时刻得到水下机器人位置信息,并通过声通讯机发送出去,水下机器人在k时刻收到该信息。则以上方程组中,hk是k时刻水下机器人所在的深度,可以由深度计得到,Xk-1是k-1时刻USBL获得的水下机器人位置,f(·)是水声信号在水下的传播距离和传播时间的关系函数。d1是k时刻母船与水下机器人之间的水平距离,d2是k-1时刻母船与水下机器人间的水平距离,该变量为已知量,d3是Δt时段里水下机器人所走过的水平距离。
解上述方程组可以得到从声通讯机发送数据到水下机器人接收数据所经历的时间Δt,则k时刻水下机器人所在的位置Xk为
按照本发明的方法将补偿得到的水下机器人当前位置用于更新导航算法的状态量。
第1步:计算观测新息
第2步:计算系统增益
Kk=Pk|k-1HT(HPk|k-1HT+Rk)-1
第3步:更新
式中,H为观测矩阵,Xk为k时刻水下机器人的状态量,Pk为系统的状态协方差矩阵,Rk为观测噪声协方差矩阵。
Claims (4)
1.一种基于超短基线的导航定位方法,其特征在于,包括以下步骤:
利用超短基线对水下机器人进行定位;
通过声通讯机将母船位置信息和超短基线定位到的水下机器人位置信息发给水下机器人;
水下机器人接收声通讯机发出的定位信息;
结合运动补偿算法计算水下机器人收到声通讯机信息时自身的位置信息;
将经过运动补偿后的位置信息作为观测量对水下机器人的实际位置进行更新;
所述运动补偿包括以下步骤:
第1步:获得母船和水下机器人位置信息;
第2步:获得水下机器人收到声通讯机发出信息时刻的深度数据;
第3步:解算水下机器人当前时刻的位置:
其中,d1是k时刻母船与水下机器人之间的水平距离;d2是k-1时刻母船与水下机器人间的水平距离,该变量为已知量;d3是Δt时刻里水下机器人所走过的水平距离;Δt是从声通讯机发送数据到水下机器人接收数据所经历的时间;hk是k时刻水下机器人所在的深度;Xk-1是k-1时刻超短基线获得的水下机器人位置信息;f(·)是水声信号在水下的传播距离和传播时间的关系函数;是水下机器人在接收声通讯机数据过程中的速度矢量,为已知量;k-1时刻为超短基线得到水下机器人位置信息的时刻;k时刻为水下机器人收到其位置信息的时刻;
解上面方程组得到Δt,则k时刻水下机器人所在的位置Xk为:
2.根据权利要求1所述的一种基于超短基线的导航定位方法,其特征在于,所述的利用超短基线对水下机器人进行定位具体为:通过超短基线基阵与水下机器人搭载的应答器通信得到水下机器人的位置信息。
3.根据权利要求1所述的一种基于超短基线的导航定位方法,其特征在于,所述第2步中的深度数据由水下机器人搭载的深度计提供。
4.根据权利要求1所述的一种基于超短基线的导航定位方法,其特征在于,所述对水下机器人的实际位置进行更新,包括以下步骤:
计算观测新息:
其中,v为观测新息,Xk为k时刻水下机器人所在的位置;H为观测矩阵,由系统的观测模型得来,是已知量;为基于k-1时刻得到的k时刻水下机器人状态的预测;
计算系统增益:
Kk=Pk|k-1HT(HPk|k-1HT+Rk)-1
其中,Kk为系统增益,Pk|k-1为系统的状态协方差预测矩阵,Rk为观测噪声协方差矩阵;
更新:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410605690.3A CN105628016B (zh) | 2014-10-30 | 2014-10-30 | 一种基于超短基线的导航定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410605690.3A CN105628016B (zh) | 2014-10-30 | 2014-10-30 | 一种基于超短基线的导航定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN105628016A CN105628016A (zh) | 2016-06-01 |
CN105628016B true CN105628016B (zh) | 2018-09-21 |
Family
ID=56043166
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201410605690.3A Active CN105628016B (zh) | 2014-10-30 | 2014-10-30 | 一种基于超短基线的导航定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN105628016B (zh) |
Families Citing this family (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107990891B (zh) * | 2016-10-26 | 2021-05-28 | 中国科学院沈阳自动化研究所 | 基于长基线和信标在线标定的水下机器人组合导航方法 |
CN108151733B (zh) * | 2016-12-05 | 2021-06-08 | 中国科学院沈阳自动化研究所 | 面向auv入坞的ins/usbl组合导航定位方法 |
CN107839859A (zh) * | 2017-10-19 | 2018-03-27 | 丁建玲 | 一种海底光缆巡检水下航行器及巡检方法 |
CN109765594B (zh) * | 2017-11-09 | 2023-04-07 | 中国科学院沈阳自动化研究所 | 一种深海潜水器导航后处理方法 |
CN109050835B (zh) * | 2018-08-06 | 2021-07-13 | 江苏科技大学 | 全驱动自主水下机器人结构及回收三维路径跟踪方法 |
CN109540154B (zh) * | 2019-01-23 | 2019-12-27 | 中国科学院声学研究所 | 一种基于粒子滤波算法的水声导航定位方法 |
CN110749890A (zh) * | 2019-10-15 | 2020-02-04 | 哈尔滨工程大学 | 一种多水下机器人协同海流估计方法 |
CN112815949B (zh) * | 2021-02-02 | 2023-11-24 | 中国科学院沈阳自动化研究所 | 一种适用水下回收过程的超短组合导航方法 |
CN114234964B (zh) * | 2021-11-05 | 2024-06-14 | 河北汉光重工有限责任公司 | 一体化自主式水下潜器在冰层下的定位方法及系统 |
Family Cites Families (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2004112050A (ja) * | 2002-09-13 | 2004-04-08 | Pioneer Electronic Corp | 通信端末装置、その接続制御方法、そのプログラム |
JP2004309346A (ja) * | 2003-04-08 | 2004-11-04 | Mitsubishi Electric Corp | ナビゲーション装置、情報提供サーバ及びこれらを用いた情報提供システム |
CN1325932C (zh) * | 2004-11-17 | 2007-07-11 | 中国科学院沈阳自动化研究所 | 载人潜水器的组合导航定位方法 |
CN101441266B (zh) * | 2008-12-30 | 2011-08-03 | 哈尔滨工程大学 | 水下多应答器组合导航方法 |
CN201966955U (zh) * | 2010-12-31 | 2011-09-07 | 中国科学院沈阳自动化研究所 | 一种载人潜水器水面监视装置 |
CN102495420B (zh) * | 2011-12-13 | 2013-06-05 | 大连海事大学 | 一种水下目标精确定位系统及方法 |
-
2014
- 2014-10-30 CN CN201410605690.3A patent/CN105628016B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN105628016A (zh) | 2016-06-01 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN105628016B (zh) | 一种基于超短基线的导航定位方法 | |
CN105319534B (zh) | 一种基于水声双程测距的多auv协同定位方法 | |
CN108444478B (zh) | 一种用于水下航行器的移动目标视觉位姿估计方法 | |
CN105823480B (zh) | 基于单信标的水下移动目标定位算法 | |
CN104316045B (zh) | 一种基于sins/lbl的auv水下交互辅助定位系统及定位方法 | |
CN106017467B (zh) | 一种基于多水下应答器的惯性/水声组合导航方法 | |
CN108490433B (zh) | 基于序贯滤波的空时偏差联合估计与补偿方法及系统 | |
CN110554359B (zh) | 一种融合长基线与单信标定位的海底飞行节点定位方法 | |
CN111596333B (zh) | 一种水下定位导航方法及系统 | |
CN107390177B (zh) | 一种基于纯测向的被动水下声学定位方法 | |
CN109459040A (zh) | 基于rbf神经网络辅助容积卡尔曼滤波的多auv协同定位方法 | |
CN106772524B (zh) | 一种基于秩滤波的农业机器人组合导航信息融合方法 | |
CN110855343B (zh) | 一种水声定位与授时浮标及其工作方法 | |
CN110207695B (zh) | 一种适用于深海auv的无速度辅助单信标定位方法 | |
CN105973238B (zh) | 一种基于范数约束容积卡尔曼滤波的飞行器姿态估计方法 | |
CN109974706A (zh) | 一种基于双运动模型的主从式多auv协同导航方法 | |
CN105547290B (zh) | 一种基于超短基线定位系统的从潜器导航方法 | |
CN108759824A (zh) | 高精度定位导航系统及方法 | |
CN103047983A (zh) | 水下机器人的面地形匹配导航方法 | |
CN108614258A (zh) | 一种基于单水声信标距离量测的水下定位方法 | |
CN104280025A (zh) | 基于无色卡尔曼滤波的深海机器人超短基线组合导航方法 | |
CN110794409A (zh) | 一种可估计未知有效声速的水下单信标定位方法 | |
CN110186461A (zh) | 一种基于重力梯度信息测距的协同导航方法 | |
CN110926466A (zh) | 一种面向无人船组合导航信息融合的多尺度数据分块算法 | |
CN104280024B (zh) | 一种深水机器人组合导航装置和方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |