CN111412917A - 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
- CN111412917A CN111412917A CN201910007248.3A CN201910007248A CN111412917A CN 111412917 A CN111412917 A CN 111412917A CN 201910007248 A CN201910007248 A CN 201910007248A CN 111412917 A CN111412917 A CN 111412917A
- Authority
- CN
- China
- Prior art keywords
- latitude
- longitude
- underwater robot
- usb
- displacement
- 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
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 calculation method for an underwater robot, which comprises the following steps of positioning the underwater robot by utilizing an ultra-short base line, discontinuously acquiring discontinuous longitude and latitude information, acquiring real-time continuous relative displacement information of the underwater robot by utilizing a dead reckoning method, updating the reference longitude and latitude of the underwater robot when receiving a packet of effective USB L positioning data in the current period, and calculating the longitude and latitude of the position of the underwater robot in real time by utilizing the relative displacement information and the reference longitude and latitude when not receiving the effective USB L positioning data in the current period.
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 for an underwater robot.
Background
In order to develop the ocean, people should understand the ocean, observe and examine the ocean, and since the underwater robot can perform observation, photographing, fishing and construction operations under the ocean, the underwater robot is widely applied to ocean development, and the development technology of the underwater robot is greatly emphasized by various countries.
Because the underwater robot goes deep into the water, the underwater robot generally adopts an ultra-short baseline underwater sound positioning system (USB L) to complete the longitude and latitude positioning of the underwater robot on the seabed, guide the underwater robot to reach a working destination, perform position calibration on a specific target and the like, and is one of the key technologies for the underwater robot to smoothly complete an underwater task.
In addition, noise generated by a water surface mother ship and a self propeller in the operation process of the underwater robot can generate great interference on the USB L, so that the positioning precision is further reduced, and the requirement of navigation and positioning of the underwater robot is difficult to meet.
In order to improve the USB L positioning accuracy of the underwater robot, the underwater robot is usually set at the bottom or hovered, then positioning data of a plurality of USB L are obtained, outliers are removed, and then the underwater robot is averaged.
Therefore, the method for calculating the underwater longitude and latitude of the underwater robot is simple and practical, 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 underwater effective operation time of the underwater robot is prolonged, and the method is a key problem to be solved in the control of the existing underwater robot.
Disclosure of Invention
Aiming at the defects of the traditional USB L longitude and latitude positioning method, the invention provides a simple and practical longitude and latitude real-time high-precision calculation method, which can be used for fusing the positioning data of a navigation sensor and a USB L, effectively improving the longitude and latitude calculation precision of the underwater robot and reducing the time for ROV (remote operated vehicle) to sit at the bottom and hover.
The technical scheme adopted by the invention for realizing the purpose is as follows: a real-time high-precision longitude and latitude calculation method for an underwater robot comprises the following steps:
positioning the underwater robot by using a USB L to acquire discontinuous longitude and latitude information;
acquiring real-time continuous displacement information of the underwater robot relative to the land by using a dead reckoning method;
when the positioning data of the USB L is received, calculating the reference longitude and latitude of the underwater robot in the period;
and when the period of the positioning data of the USB L is not received, calculating the latitude and longitude of the underwater robot in real time by using the displacement information of the underwater robot relative to the land and the reference latitude and longitude.
Every time when a packet of underwater robot positioning data is acquired through the USB L, the longitude and latitude of the USB L positioning at the moment and the displacement information of the relative land acquired through a dead reckoning method are recorded.
When the positioning data of the USB L is received, the calculation of the reference longitude and latitude of the underwater robot in the period comprises the following steps:
when the number of the received positioning data groups of the USB L is more than or equal to N, the longitude and latitude of the underwater robot at the current moment are calculated by the previously recorded N-1 group data, and N longitude and latitude data are obtained together with the Nth group of USB L positioning data, wherein the calculation formula is as follows:
J(i)＝J0(k-i)+(SE(k)-SE(k-i))ρ1
W(i)＝W0(k-i)+(SN(k)-SN(k-i))ρ2
wherein J (i) is longitude obtained by calculation of the ith group, and W (i) is latitude obtained by calculation of the ith group; j. the design is a square0(k-i) locate longitude, W, for USB L recorded for group k-i0(k-i) locating the latitude, S, for the recorded USB L of the k-i th groupE(k) East displacement of the underwater robot relative to land, S, recorded for the kth groupN(k) Recording the north displacement of the underwater robot relative to the land for the kth group, wherein k represents the serial number of the current data group, and is more than or equal to N-1; sE(k-i) east displacement of the underwater robot relative to the land recorded in the k-i group, SN(k-i) is the north displacement of the underwater robot relative to the land recorded in the k-i group, i is 0, 1, 2 … … N-1; rho1、ρ2Respectively the longitude of the current positionDegree curvature and latitude curvature.
The reference longitude and latitude is obtained through the following steps:
respectively sorting the calculated N longitude and latitude data according to the size, taking N/2 values in the middle, averaging, taking the average value as longitude and latitude data with higher precision at the moment corresponding to the k groups of data, namely new reference longitude and latitude, and respectively recording the new reference longitude and latitude as Jref，Wref。
In each period of program execution, the latitude and longitude 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 latitude and longitude, and the method comprises the following steps:
J＝Jref+(SE-SE(k))ρ1
W＝Wref+(SN-SN(k))ρ2
wherein, Jref、WrefRespectively as reference longitude and latitude, J as longitude calculated in real time, W as latitude calculated in real time, SEIs a real-time east displacement value of relative land, SNReal-time north displacement values relative to land; sE(k)、SN(k) The displacement of the kth group of underwater robots in the east direction relative to the land and the displacement of the kth group of underwater robots in the north direction relative to the land are respectively; rho1、ρ2Respectively longitude and latitude curvatures for the current location.
The invention has the following beneficial effects and advantages:
1. the longitude and latitude calculation method can be operated on line, outputs the current longitude and latitude of the underwater robot in real time, and improves the USB L positioning period of more than 3 seconds to 200ms level.
2. The longitude and latitude calculation method can correct the discontinuous and jumping underwater motion positioning track acquired by the USB L into a continuous and smooth track, and can avoid excessive operation of operators caused by jumping of original USB L positioning data.
3. The longitude and latitude calculation method does not need to sit on the bottom or hover, and can output high-precision longitude and latitude information in real time after the underwater robot runs for a period of time when the underwater robot arrives at the bottom, so that the time for the underwater robot to execute tasks is effectively increased.
4. The longitude and latitude calculation method can maintain high-precision longitude and latitude calculation within a long period of time under the condition that the underwater robot executes a task under the water bottom and the USB L positioning fails.
5. The longitude and latitude calculation method only fuses the continuously acquired N USB L positioning data and the relative position acquired by dead reckoning, and under the condition of rapid development of the current computer technology, the load of a computer is not obviously increased, and the debugging is simple and easy.
Drawings
FIG. 1 is a schematic 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 is composed of an underwater robot, a mother ship, a USB L (ultra short baseline positioning system) and a control computer, wherein the underwater robot needs to be provided with a depth meter, a DV L (Doppler log), a fiber optic gyroscope, a sound beacon and the like, as shown in figure 1, the USB L system is used for obtaining the positioning longitude and latitude information of the underwater robot, the depth meter is used for obtaining the distance between the underwater robot and the water surface, the DV L, the fiber optic gyroscope and the like output data are subjected to fusion filtering, and then the ship dead reckoning is used for obtaining the displacement information (generally north displacement and east displacement) of the underwater robot relative to the land.
The calculation flow chart of the method is shown in figure 2. when the control software receives effective (the data value is within the threshold range) USB L positioning longitude and latitude data, the longitude and latitude and the relative displacement at the moment are recorded, the reference longitude and latitude are updated, and then the longitude and latitude of the underwater robot are calculated in real time according to the reference longitude and latitude and the relative displacement variation information.
The method comprises the first step of judging whether effective USB L positioning data are received in the current period, if yes, recording longitude and latitude of USB L positioning and relative displacement information of dead reckoning corresponding to the moment into a program memory as a group of data, and entering the second step, and if not, entering the fourth step.
The second step, judge whether the data set number of record has already been greater than or equal to N, if yes, calculate and obtain the longitude and latitude of N-1 present positions according to N-1 data set of record before, add the longitude and latitude that the present USB L positions, N totally, then enter the third step, otherwise return to the first step. the way of calculating of N-1 longitude and latitude is to first of all, regard relative displacement (north displacement and east displacement) of present record as the reference respectively, make the difference with the relative displacement (north displacement and east displacement) of the previous N-1 records separately, then, calculate the longitude and latitude variation quantity that N-1 difference correspond separately, and superpose it on corresponding longitude and latitude, obtain N-1 longitude and latitude altogether, connect the longitude and latitude data of this USB L location, obtain N longitude and latitude data of the position altogether, calculate the formula:
J(i)＝J0(k-i)+(SE(k)-SE(k-i))ρ1
W(i)＝W0(k-i)+(SN(k)-SN(k-i))ρ2
wherein J (i) is longitude obtained by calculation of the ith group, and W (i) is latitude obtained by calculation of the ith group; j. the design is a square0(k-i) locate longitude, W, for USB L recorded for group k-i0(k-i) locating the latitude, S, for the recorded USB L of the k-i th groupE(k) East displacement of the underwater robot relative to land, S, recorded for the kth groupN(k) Recording the north displacement of the underwater robot relative to the land for the kth group, wherein k represents the serial number of the current data group, and is more than or equal to N-1; sE(k-i) east displacement of the underwater robot relative to the land recorded in the k-i group, SN(k-i) is the north displacement of the underwater robot relative to the land recorded in the k-i group, i is 0, 1, 2 … … N-1; rho1、ρ2Respectively longitude and latitude curvatures for the current location.
Phi is the latitude of the earth (determined by the current position of the underwater machine), a is the mean equatorial radius of the earth, b is the mean polar radius of the earth,
the first eccentricity of the earth.
Thirdly, sorting the N calculated longitude and latitude data according to the longitude and the latitude respectively, then averaging the N/2 data in the middle, obtaining the longitude and latitude data with higher precision at the moment k as new reference longitude and latitude, and recording the new longitude and latitude data as J respectivelyref，Wref。
And fourthly, judging whether the reference longitude and latitude exists, if so, carrying out real-time longitude and latitude calculation, and if not, returning to the first step. The real-time longitude and latitude calculation mode is as follows: taking the current relative displacement (the north displacement and the east displacement) as a reference, and making a difference with the relative displacement (the north displacement and the east displacement) which is recorded latest; 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＝Jref+(SE-SE(k))ρ1
W＝Wref+(SN-SN(k))ρ2
wherein, Jref、WrefRespectively as reference longitude and latitude, J as longitude calculated in real time, W as latitude calculated in real time, SEIs a real-time east displacement value of relative land, SNReal-time north displacement values relative to land; sE(k)、SN(k) The displacement of the kth group of underwater robots in the east direction relative to the land and the displacement of the kth group of underwater robots in the north direction relative to the land are respectively; rho1、ρ2Respectively longitude and latitude curvatures for the current location.
The steps are executed circularly, and the longitude and the latitude of the position of the underwater robot at each moment can be continuously calculated.
It can be seen from the calculation formula that the larger the value of N, the higher the accuracy. However, the dead reckoning generally increases the accumulated error with the increase of time, so that N is selected to have an appropriate value according to the actual application.
The underwater robot described above is only a preferred embodiment of the present invention, and is not intended to limit the present invention, and any modifications, equivalent replacements, improvements, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
Claims (5)
1. A real-time high-precision longitude and latitude calculation method for an underwater robot is characterized by comprising the following steps:
positioning the underwater robot by using a USB L to acquire discontinuous longitude and latitude information;
acquiring real-time continuous displacement information of the underwater robot relative to the land by using a dead reckoning method;
when the positioning data of the USB L is received, calculating the reference longitude and latitude of the underwater robot in the period;
and when the period of the positioning data of the USB L is not received, calculating the latitude and longitude of the underwater robot in real time by using the displacement information of the underwater robot relative to the land and the reference latitude and longitude.
2. The method as claimed in claim 1, wherein every time a packet of underwater robot positioning data is obtained through USB L, the latitude and longitude of the current USB L positioning and the displacement information of the relative land obtained by the dead reckoning method are recorded.
3. The method for estimating the real-time high-precision longitude and latitude of the underwater robot as claimed in claim 1, wherein the step of estimating the reference longitude and latitude of the underwater robot in the period when receiving the positioning data of the USB L comprises the following steps:
when the number of the received positioning data groups of the USB L is more than or equal to N, the longitude and latitude of the underwater robot at the current moment are calculated by the previously recorded N-1 group data, and N longitude and latitude data are obtained together with the Nth group of USB L positioning data, wherein the calculation formula is as follows:
J(i)＝J0(k-i)+(SE(k)-SE(k-i))ρ1
W(i)＝W0(k-i)+(SN(k)-SN(k-i))ρ2
wherein J (i) is longitude obtained by calculation of the ith group, and W (i) is latitude obtained by calculation of the ith group; j. the design is a square0(k-i) locate longitude, W, for USB L recorded for group k-i0(k-i) locating the latitude, S, for the recorded USB L of the k-i th groupE(k) East displacement of the underwater robot relative to land, S, recorded for the kth groupN(k) Recording the north displacement of the underwater robot relative to the land for the kth group, wherein k represents the serial number of the current data group, and is more than or equal to N-1; sE(k-i) east displacement of the underwater robot relative to the land recorded in the k-i group, SN(k-i) is the north displacement of the underwater robot relative to the land recorded in the k-i group, i is 0, 1, 2 … … N-1; rho1、ρ2Respectively longitude and latitude curvatures for the current location.
4. The underwater robot real-time high-precision longitude and latitude calculation method according to claim 1, wherein the reference longitude and latitude is obtained by the following steps:
respectively sorting the calculated N longitude and latitude data according to the size, taking N/2 values in the middle, averaging, taking the average value as longitude and latitude data with higher precision at the moment corresponding to the k groups of data, namely new reference longitude and latitude, and respectively recording the new reference longitude and latitude as Jref，Wref。
5. The method for calculating the latitude and longitude of the underwater robot in real time with high precision according to claim 1, wherein in each period of program execution, the latitude and longitude of the underwater robot are calculated in real time by using displacement information of the underwater robot relative to the land and the reference latitude and longitude, and the method comprises the following steps:
J＝Jref+(SE-SE(k))ρ1
W＝Wref+(SN-SN(k))ρ2
wherein, Jref、WrefRespectively as reference longitude and latitude, J as longitude calculated in real time, W as latitude calculated in real time, SEIs a relative terrestrial realityeast-Time Displacement value, SNReal-time north displacement values relative to land; sE(k)、SN(k) The displacement of the kth group of underwater robots in the east direction relative to the land and the displacement of the kth group of underwater robots in the north direction relative to the land are respectively; rho1、ρ2Respectively longitude and latitude curvatures for the current location.
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 true CN111412917A (en) | 2020-07-14 |
| CN111412917B 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 (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20130128700A1 (en) * | 2011-04-21 | 2013-05-23 | Ixblue Industries S.A.S | Method for Global Acoustic Positioning of a Marine or Submarine Target |
| 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 |
-
2019
- 2019-01-04 CN CN201910007248.3A patent/CN111412917B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20130128700A1 (en) * | 2011-04-21 | 2013-05-23 | Ixblue Industries S.A.S | Method for Global Acoustic Positioning of a Marine or Submarine Target |
| 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》 * |
| 陈功;: "水下沉船精确定位***的研制", 舰船科学技术 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111412917B (en) | 2023-05-09 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN104316045B (en) | A kind of AUV based on SINS/LBL interacts aided positioning system and localization method under water | |
| CN107990891B (en) | Underwater robot combined navigation method based on long baseline and beacon online calibration | |
| CN109901205B (en) | Underwater robot multi-sensor fusion and motion trajectory prediction method | |
| CN111595348B (en) | Master-slave mode cooperative positioning method of autonomous underwater vehicle combined navigation system | |
| CN109059964B (en) | Inertial navigation and gravity measurement double-calibration method based on gravity peak | |
| CN107966145B (en) | AUV underwater navigation method based on sparse long baseline tight combination | |
| CN102213594A (en) | Method for fusing ocean current observation data of unmanned undersea vehicle (UUV) | |
| CN109085655B (en) | Underwater platform gravity measurement scheme and verification method | |
| CN101907711A (en) | Simultaneous detecting and positioning method for autonomous underwater robot | |
| CN110906930A (en) | AUV (autonomous underwater vehicle) combined underwater gravity lighthouse submarine navigation method and system | |
| CN113433553B (en) | Precise navigation method for multi-source acoustic information fusion of underwater robot | |
| Ånonsen et al. | The HUGIN AUV terrain navigation module | |
| CN106679662A (en) | Combined underwater robot navigation method based on TMA (target motion analysis) technology and single beacon | |
| CN1779485A (en) | Combined navigation positioning method of manned submersible | |
| CN110220510B (en) | Underwater robot submarine topography matching navigation path planning method considering map accuracy | |
| CN111596333A (en) | Underwater positioning navigation method and system | |
| CN103968838A (en) | Co-location method of AUVs (Autonomous Underwater Vehicles) in curvilinear motion state based on polar coordinate system | |
| Rock et al. | Return-to-site of an AUV using terrain-relative navigation: Field trials | |
| Allotta et al. | Localization algorithm for a fleet of three AUVs by INS, DVL and range measurements | |
| CN109655060A (en) | Based on the KF/FIR and LS-SVM INS/UWB Integrated Navigation Algorithm merged and system | |
| CN202928583U (en) | Offshore drilling platform attitude monitor and location device | |
| CN111412917A (en) | Real-time high-precision longitude and latitude calculation method for underwater robot | |
| CN106908036B (en) | A kind of AUV multi-beam Bathymetric Data patterning process based on local offset | |
| CN112445243B (en) | Submarine target searching method suitable for autonomous underwater robot | |
| CN111523200B (en) | Method for reconstructing full-depth sound velocity profile by combining WOA2018 model and actually measured temperature and salinity data |
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 |