CN111412917B - Real-time high-precision longitude and latitude calculation method for underwater robot - Google Patents
Real-time high-precision longitude and latitude calculation method for underwater robot Download PDFInfo
- Publication number
- CN111412917B CN111412917B CN201910007248.3A CN201910007248A CN111412917B CN 111412917 B CN111412917 B CN 111412917B CN 201910007248 A CN201910007248 A CN 201910007248A CN 111412917 B CN111412917 B CN 111412917B
- Authority
- CN
- China
- Prior art keywords
- latitude
- longitude
- underwater robot
- usbl
- land
- 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
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
- G01C21/203—Specially adapted for sailing ships
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
- Y02A90/30—Assessment of water resources
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention relates to a real-time high-precision longitude and latitude calculating method of an underwater robot, which comprises the following steps: positioning the underwater robot by using the ultra-short base line, and intermittently acquiring discontinuous longitude and latitude information; acquiring real-time and continuous relative displacement information of the underwater robot by using a dead reckoning method; updating the reference longitude and latitude of the underwater robot when receiving a pack of effective USBL positioning data in the current period; when effective USBL positioning data is not received in the current period, the longitude and latitude of the position where the underwater robot is located are calculated in real time by utilizing the relative displacement information and the reference longitude and latitude. The invention is simple and practical, and greatly improves the accuracy and instantaneity of longitude and latitude of the underwater robot during underwater navigation and positioning.
Description
Technical Field
The invention relates to the technical field of navigation and positioning of underwater robots, in particular to a real-time high-precision longitude and latitude calculation method of an underwater robot.
Background
For developing the ocean, people should know the ocean, observe and examine the ocean, and the underwater robot can observe, pick up, salvage and construct under the water, so the underwater robot is widely applied to ocean development, and the development technology of the underwater robot is greatly paid attention to all countries.
Because the underwater robot goes deep into the water, the underwater robot generally adopts an ultra-short baseline underwater sound positioning system (USBL) to finish the longitude and latitude positioning of the underwater robot on the sea floor, and the underwater robot is guided to reach a working destination, positions of specific targets are calibrated, and the like, so that the underwater robot is one of key technologies for successfully finishing underwater tasks.
The working depth of an underwater robot is typically several kilometers under water, and at such large depths, the USBL underwater sound positioning system can generate delays of up to several seconds and can generate large errors due to the change of sound velocity with depth. In addition, noise generated by a water mother ship and a self propeller in the operation process of the underwater robot can generate huge interference on the USBL, so that the positioning precision is further reduced, and the navigation and positioning requirements of the underwater robot are difficult to meet.
In order to improve the positioning accuracy of the USBL of the underwater robot, a common method is to sit on or hover the underwater robot, acquire positioning data of a plurality of USBL, remove the wild value and average the wild value. In order to obtain higher USBL positioning accuracy, the underwater robot needs to be static as much as possible, and if accurate positioning is needed for many times, more time is consumed, and valuable underwater operation time of the underwater robot is wasted.
Therefore, the simple and practical underwater robot submarine longitude and latitude calculation method is researched, the underwater robot is not required to be approximately static, high-precision longitude and latitude positioning data can be continuously and real-timely output, the effective operation time of the underwater robot submarine is improved, and the method is a problem which needs to be solved in the control of the underwater robot at present.
Disclosure of Invention
Aiming at the defects of the traditional USBL longitude and latitude positioning method, the invention provides a simple and practical real-time high-precision longitude and latitude calculation method, which can be used for fusing positioning data of a navigation sensor and the USBL, effectively improving the longitude and latitude calculation precision of an underwater robot and reducing the ROV seat bottom and hovering time. Meanwhile, compared with the traditional calculation method, the method has the advantage that the added calculation amount can be ignored.
The technical scheme adopted by the invention for achieving the purpose is as follows: a real-time high-precision longitude and latitude calculation method of an underwater robot comprises the following steps:
positioning the underwater robot by using the USBL to acquire discontinuous longitude and latitude information;
acquiring displacement information of the real-time continuous underwater robot relative to the land by using a dead reckoning method;
when positioning data of the USBL are received, estimating the reference longitude and latitude of the periodic underwater robot;
and when the period of the positioning data of the USBL is not received, calculating the longitude and latitude of the underwater robot in real time by utilizing the displacement information of the underwater robot relative to the land and the reference longitude and latitude.
And recording the longitude and latitude of the USBL positioning and the displacement information of the relative land obtained by a ship dead reckoning method when a packet of underwater robot positioning data is obtained through the USBL.
When the positioning data of the USBL is received, the reference longitude and latitude of the periodic underwater robot are calculated, and the method comprises the following steps:
when the number of the received USBL positioning data sets is greater than or equal to N, calculating the longitude and latitude of the underwater robot at the current moment according to the N-1 data sets recorded in the previous step, and acquiring N longitude and latitude data together with the N-th USBL positioning data set; the calculation formula is as follows:
J(i)＝J 0 (k-i)+(S E (k)-S E (k-i))ρ 1
W(i)＝W 0 (k-i)+(S N (k)-S N (k-i))ρ 2
wherein J (i) is longitude obtained by calculating the ith group, and W (i) is latitude obtained by calculating the ith group; j (J) 0 (k-i) locating longitude, W for USBL recorded for k-i group 0 (k-i) the USBL locating latitude recorded for the k-i group, S E (k) East displacement of the underwater robot relative to land recorded for the kth group, S N (k) The north displacement of the underwater robot recorded in the kth group relative to the land is represented by k, wherein k represents the serial number of the current data group, and k is more than or equal to N-1; s is S E (k-i) east displacement of the underwater robot relative to land recorded for the k-i th group, S N (k-i) north displacement of the underwater robot relative to land recorded for the k-i th group, i=0, 1, 2 … … N-1; ρ 1 、ρ 2 The longitude and latitude curvatures of the current location, respectively.
The reference longitude and latitude are obtained through the following steps:
the calculated N longitude and latitude data are respectively sequenced according to the size, the N/2 values in the middle are taken and averaged, the average value is taken as longitude and latitude data with higher time accuracy corresponding to k groups of data, namely, new reference longitude and latitude are respectively recorded as J ref ，W ref 。
In each period of program execution, the longitude and latitude of the underwater robot are calculated in real time by utilizing the displacement information of the underwater robot relative to the land and the reference longitude and latitude, and the method comprises the following steps:
J＝J ref +(S E -S E (k))ρ 1
W＝W ref +(S N -S N (k))ρ 2
wherein J is ref 、W ref Respectively the reference longitude and latitude, J is the longitude calculated in real time, W is the latitude calculated in real time, S E Is the real-time east displacement value relative to land, S N A real-time north displacement value for a relative land; s is S E (k)、S N (k) The northeast displacement of the kth group of underwater robots relative to the land and the northbound displacement of the kth group of underwater robots relative to the land are respectively; ρ 1 、ρ 2 The longitude and latitude curvatures of the current location, respectively.
The invention has the following beneficial effects and advantages:
1. the longitude and latitude calculation method can run on line, output the current longitude and latitude of the underwater robot in real time, and promote the USBL positioning period of more than 3 seconds to the 200ms level.
2. The longitude and latitude calculation method can correct the discontinuous and jumping underwater motion positioning track acquired by the USBL into a continuous and smooth track, and can avoid excessive operations of operators due to jumping of original USBL positioning data.
3. According to the longitude and latitude calculation method, the user does not need to sit on or hover, and after the user reaches the water bottom to run for a period of time, high-precision longitude and latitude information can be output in real time, so that the time for executing tasks on the water bottom of the underwater robot is effectively increased.
4. According to the longitude and latitude estimation method, under the condition that the underwater robot performs tasks at the water bottom and USBL positioning fails, higher-precision longitude and latitude estimation can be maintained for a long time.
5. According to the longitude and latitude estimation method, N USBL positioning data which are continuously acquired are only fused with the relative positions acquired by dead reckoning, so that the burden of a computer is not obviously increased under the condition that the current computer technology is rapidly developed, and the debugging is simple and easy.
Drawings
FIG. 1 is a schematic diagram of the composition of the present invention;
fig. 2 is a flow chart of the method execution of the present invention.
Detailed Description
The present invention will be described in further detail with reference to the accompanying drawings and examples.
The invention consists of an underwater robot, a mother ship, a USBL (ultra short baseline positioning system) and a control computer, wherein the underwater robot is required to be provided with a depth gauge, a DVL (Doppler log), an optical fiber gyro, an acoustic beacon and the like, as shown in figure 1. The method comprises the steps of acquiring positioning longitude and latitude information of an underwater robot by a USBL system, acquiring the distance between the underwater robot and the water surface by a depth gauge, performing fusion filtering on data output by DVL, an optical fiber gyro and the like, and acquiring displacement information (generally north displacement and east displacement) of the underwater robot relative to the land by ship dead reckoning.
An estimation flow chart of the method of the invention is shown in fig. 2. When control software receives valid USBL positioning longitude and latitude data (the data value is in the threshold range), recording the longitude and latitude and the relative displacement at the moment, and updating the reference longitude and latitude; and then calculating the longitude and latitude of the underwater robot in real time according to the reference longitude and latitude and the relative displacement variation information.
The method comprises the steps of firstly, judging whether effective USBL positioning data are received in a current period, if yes, recording longitude and latitude of USBL positioning and relative displacement information calculated by ship positions corresponding to the moment into a memory of a program as a group of data, and entering a second step; and if not, entering a fourth step.
Secondly, judging whether the number of the recorded data sets is greater than or equal to N, if so, calculating and obtaining N-1 longitude and latitude of the current position according to the N-1 data sets recorded in the front, adding N longitude and latitude of the current USBL positioning, and then entering a third step; otherwise, returning to the first step. The N-1 longitude and latitude calculation modes are as follows: firstly, taking the relative displacement (north displacement and east displacement) recorded at present as a reference, respectively, and making differences with the relative displacement (north displacement and east displacement) recorded in front of N-1; and then, respectively calculating longitude and latitude variation corresponding to the N-1 differences, and superposing the longitude and latitude variation on the corresponding longitude and latitude to obtain N-1 longitudes and latitudes in total. The longitude and latitude data of the USBL positioning is connected, N longitude and latitude data of the position are obtained altogether, and the calculation formula is as follows:
J(i)＝J 0 (k-i)+(S E (k)-S E (k-i))ρ 1
W(i)＝W 0 (k-i)+(S N (k)-S N (k-i))ρ 2
wherein J (i) is longitude obtained by calculating the ith group, and W (i) is latitude obtained by calculating the ith group; j (J) 0 (k-i) locating longitude, W for USBL recorded for k-i group 0 (k-i) the USBL locating latitude recorded for the k-i group, S E (k) East displacement of the underwater robot relative to land recorded for the kth group, S N (k) The north displacement of the underwater robot recorded in the kth group relative to the land is represented by k, wherein k represents the serial number of the current data group, and k is more than or equal to N-1; s is S E (k-i) east displacement of the underwater robot relative to land recorded for the k-i th group, S N (k-i) north displacement of the underwater robot relative to land recorded for the k-i th group, i=0, 1, 2 … … N-1; ρ 1 、ρ 2 The longitude and latitude curvatures of the current location, respectively.
Phi is the latitude of the earth (determined by the current position of the underwater machine), a is the average equatorial radius of the earth, b is the average polar radius of the earth,
is the first eccentricity of the earth.
Thirdly, sorting N longitude and latitude data obtained through calculation according to the sizes of longitude and latitude respectively, averaging N/2 data in the middle respectively, and obtaining longitude and latitude data with higher accuracy at the moment k as new reference longitude and latitude, and recording as J respectively ref ，W ref 。
And fourthly, judging whether the reference longitude and latitude exists, if yes, carrying out real-time longitude and latitude estimation, otherwise, returning to the first step. The real-time longitude and latitude calculation mode is as follows: taking the current relative displacement (north displacement and east displacement) as a reference, and making a difference with the latest recorded relative displacement (north displacement and east displacement); then, calculating the longitude and latitude variation corresponding to the difference, and superposing the longitude and latitude variation on the reference longitude and latitude to obtain the longitude and latitude of the current position, wherein the calculation formula is as follows:
J＝J ref +(S E -S E (k))ρ 1
W＝W ref +(S N -S N (k))ρ 2
wherein J is ref 、W ref Respectively the reference longitude and latitude, J is the longitude calculated in real time, W is the latitude calculated in real time, S E Is the real-time east displacement value relative to land, S N A real-time north displacement value for a relative land; s is S E (k)、S N (k) The northeast displacement of the kth group of underwater robots relative to the land and the northbound displacement of the kth group of underwater robots relative to the land are respectively; ρ 1 、ρ 2 The longitude and latitude curvatures of the current location, respectively.
And circularly executing the steps, and continuously calculating the longitude and latitude of the position of the underwater robot at each moment.
From the calculation formula, the greater the N value is, the higher the accuracy is. However, the dead reckoning generally increases with time, and the accumulated error increases, so that N should be selected to be a proper value according to the actual application situation.
The underwater robot is only the best implementation object of the present invention, and is not limited to the present invention, and any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the protection scope of the present invention.
Claims (4)
1. The real-time high-precision longitude and latitude calculation method of the underwater robot is characterized by comprising the following steps of:
positioning the underwater robot by using the USBL to acquire discontinuous longitude and latitude information;
acquiring displacement information of the real-time continuous underwater robot relative to the land by using a dead reckoning method;
when the period of the positioning data of the USBL is received, the reference longitude and latitude of the underwater robot in the period are calculated;
when the period of the positioning data of the USBL is not received, the longitude and latitude of the underwater robot are calculated in real time by utilizing the displacement information of the underwater robot relative to the land and the reference longitude and latitude;
when the period of the positioning data of the USBL is received, the reference longitude and latitude of the underwater robot in the period is calculated, and the method comprises the following steps:
when the number of the received USBL positioning data sets is greater than or equal to N, calculating the longitude and latitude of the underwater robot at the current moment according to the N-1 data sets recorded in the previous step, and acquiring N longitude and latitude data together with the N-th USBL positioning data set; the calculation formula is as follows:
J(i)＝J 0 (k-i)+(S E (k)-S E (k-i))ρ 1
W(i)＝W 0 (k-i)+(S N (k)-S N (k-i))ρ 2
wherein J (i) is longitude obtained by calculating the ith group, and W (i) is latitude obtained by calculating the ith group; j (J) 0 (k-i) locating longitude, W for USBL recorded for k-i group 0 (k-i) the USBL locating latitude recorded for the k-i group, S E (k) East displacement of the underwater robot relative to land recorded for the kth group, S N (k) The north displacement of the underwater robot recorded in the kth group relative to the land is represented by k, wherein k represents the serial number of the current data group, and k is more than or equal to N-1; s is S E (k-i) east displacement of the underwater robot relative to land recorded for the k-i th group, S N (k-i) north displacement of the underwater robot relative to land recorded for the k-i th group, i=0, 1, 2 … … N-1; ρ 1 、ρ 2 The longitude and latitude curvatures of the current location, respectively.
2. The method for estimating the longitude and latitude of the underwater robot in real time with high precision according to claim 1, wherein each time a packet of positioning data of the underwater robot is obtained through the USBL, the longitude and latitude of the USBL positioned at the moment and the displacement information of the relative land obtained through the ship dead reckoning method are recorded.
3. The method for estimating the longitude and latitude of the underwater robot in real time with high precision according to claim 1, wherein the reference longitude and latitude is obtained by the following steps:
the calculated N longitude and latitude data are respectively sequenced according to the size, the N/2 values in the middle are taken and averaged, the average value is taken as longitude and latitude data with higher time accuracy corresponding to k groups of data, namely, new reference longitude and latitude are respectively recorded as J ref ，W ref 。
4. The method for estimating the longitude and latitude of the underwater robot in real time with high precision according to claim 1, wherein when no period of positioning data of the USBL is received, the longitude and latitude of the underwater robot are estimated in real time by using displacement information of the underwater robot relative to land and a reference longitude and latitude, as follows:
J＝J ref +(S E -S E (k))ρ 1
W＝W ref +(S N -S N (k))ρ 2
wherein J is ref 、W ref Respectively the reference longitude and latitude, J is the longitude calculated in real time, W is the latitude calculated in real time, S E Is the real-time east displacement value relative to land, S N A real-time north displacement value for a relative land; s is S E (k)、S N (k) The northeast displacement of the kth group of underwater robots relative to the land and the northbound displacement of the kth group of underwater robots relative to the land are respectively; ρ 1 、ρ 2 The longitude and latitude curvatures of the current location, respectively.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910007248.3A CN111412917B (en) | 2019-01-04 | 2019-01-04 | Real-time high-precision longitude and latitude calculation method for underwater robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910007248.3A CN111412917B (en) | 2019-01-04 | 2019-01-04 | Real-time high-precision longitude and latitude calculation method for underwater robot |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111412917A CN111412917A (en) | 2020-07-14 |
| CN111412917B true CN111412917B (en) | 2023-05-09 |
Family
ID=71488710
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910007248.3A Active CN111412917B (en) | 2019-01-04 | 2019-01-04 | Real-time high-precision longitude and latitude calculation method for underwater robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111412917B (en) |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105547290A (en) * | 2015-12-29 | 2016-05-04 | 北京理工大学 | Slave submersible vehicle navigation method based on ultra-short baseline positioning system |
| CN108151733A (en) * | 2016-12-05 | 2018-06-12 | 中国科学院沈阳自动化研究所 | Towards the INS/USBL combined navigation locating methods of AUV dockings |
| CN108303715A (en) * | 2017-12-19 | 2018-07-20 | 浙江大学 | Underwater mobile node passive location method based on ″Beidou″ beacon and its system |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| FR2974422B1 (en) * | 2011-04-21 | 2014-04-18 | Ixsea Sas | METHOD FOR GLOBALLY ACOUSTIC POSITIONING OF A MARINE OR UNDERWATER TARGET |
-
2019
- 2019-01-04 CN CN201910007248.3A patent/CN111412917B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105547290A (en) * | 2015-12-29 | 2016-05-04 | 北京理工大学 | Slave submersible vehicle navigation method based on ultra-short baseline positioning system |
| CN108151733A (en) * | 2016-12-05 | 2018-06-12 | 中国科学院沈阳自动化研究所 | Towards the INS/USBL combined navigation locating methods of AUV dockings |
| CN108303715A (en) * | 2017-12-19 | 2018-07-20 | 浙江大学 | Underwater mobile node passive location method based on ″Beidou″ beacon and its system |
Non-Patent Citations (2)
| Title |
|---|
| "一种遥控潜水器控制系统的研究与应用";郭威 等;《机器人 ROBOT》;全文 * |
| 水下沉船精确定位系统的研制;陈功;;舰船科学技术(第04期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111412917A (en) | 2020-07-14 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105823480B (en) | Underwater moving target location algorithm based on single beacon | |
| CN104316045B (en) | A kind of AUV based on SINS/LBL interacts aided positioning system and localization method under water | |
| CN111596333B (en) | Underwater positioning navigation method and system | |
| CN109737956A (en) | A kind of SINS/USBL phase difference tight integration navigation locating method based on double response device | |
| CN106054135B (en) | A kind of passive underwater acoustic localization method based on period traveling time window | |
| McPhail et al. | Range-only positioning of a deep-diving autonomous underwater vehicle from a surface ship | |
| Ånonsen et al. | The HUGIN AUV terrain navigation module | |
| CN110274591B (en) | ADCP (advanced deep submersible vehicle) assisted SINS (strapdown inertial navigation system) navigation method of deep submersible manned submersible | |
| CN111076728A (en) | DR/USBL-based deep submersible vehicle combined navigation method | |
| CN1779485A (en) | Combined navigation positioning method of manned submersible | |
| CN104280024B (en) | Device and method for integrated navigation of deepwater robot | |
| CN111174774A (en) | Navigation information fusion method and system under water level mode at certain depth | |
| CN109855649A (en) | A kind of underwater gliding machine platform motion profile passively determines method | |
| Xu et al. | Accurate two-step filtering for AUV navigation in large deep-sea environment | |
| Diamant et al. | A machine learning approach for dead-reckoning navigation at sea using a single accelerometer | |
| CN115096302A (en) | Strapdown inertial base navigation system information filtering robust alignment method, system and terminal | |
| Norgren et al. | Intelligent buoys for aiding AUV navigation under the ice | |
| Abreu et al. | Marine vehicles with streamers for geotechnical surveys: Modeling, positioning, and control | |
| CN111412917B (en) | Real-time high-precision longitude and latitude calculation method for underwater robot | |
| Medagoda et al. | Water column current profile aided localisation for autonomous underwater vehicles | |
| CN110873813B (en) | Water flow velocity estimation method, integrated navigation method and device | |
| CN111829511A (en) | AUV (autonomous Underwater vehicle) integrated navigation method and system based on M estimation | |
| Wang et al. | Novel cooperative navigation method for multi-AUVs based on optimal weight distribution method | |
| CN115390012B (en) | Multi-transponder coordinate measuring method, device and system for HOV (Hov) accurate positioning | |
| CN112241589A (en) | Multi-underwater unmanned ship collaborative motion formation design method based on system observability degree |
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 |