CN113075665A - Underwater positioning method, underwater vehicle navigation device and computer readable storage medium - Google Patents
Underwater positioning method, underwater vehicle navigation device and computer readable storage medium Download PDFInfo
- Publication number
- CN113075665A CN113075665A CN202110316751.4A CN202110316751A CN113075665A CN 113075665 A CN113075665 A CN 113075665A CN 202110316751 A CN202110316751 A CN 202110316751A CN 113075665 A CN113075665 A CN 113075665A
- Authority
- CN
- China
- Prior art keywords
- determining
- value
- angular velocity
- measurement unit
- target
- 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
- 238000000034 method Methods 0.000 title claims abstract description 46
- 238000005259 measurement Methods 0.000 claims abstract description 78
- 230000001133 acceleration Effects 0.000 claims abstract description 61
- 239000011159 matrix material Substances 0.000 claims description 52
- 230000006870 function Effects 0.000 claims description 35
- 230000005484 gravity Effects 0.000 claims description 27
- 238000012937 correction Methods 0.000 claims description 23
- 238000004891 communication Methods 0.000 claims description 5
- 238000001514 detection method Methods 0.000 description 8
- 230000001360 synchronised effect Effects 0.000 description 4
- 238000005457 optimization Methods 0.000 description 3
- 230000016776 visual perception Effects 0.000 description 2
- 239000000654 additive Substances 0.000 description 1
- 230000000996 additive effect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 229910052500 inorganic mineral Inorganic materials 0.000 description 1
- 239000011707 mineral Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000011084 recovery Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
- G01S15/88—Sonar systems specially adapted for specific applications
-
- 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/18—Stabilised platforms, e.g. by gyroscope
-
- 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
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
- G01S15/86—Combinations of sonar systems with lidar systems; Combinations of sonar systems with systems not using wave reflection
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
- G01S15/88—Sonar systems specially adapted for specific applications
- G01S15/89—Sonar systems specially adapted for specific applications for mapping or imaging
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Acoustics & Sound (AREA)
- Manufacturing & Machinery (AREA)
- Navigation (AREA)
Abstract
The invention discloses an underwater positioning method, an underwater vehicle and a computer readable storage medium, wherein the underwater positioning method comprises the following steps: determining first position and attitude data of the inertial measurement unit under a first preset coordinate system; determining a target linear acceleration value and a target angular velocity value according to the first attitude data and the inertial data detected by the inertial measurement unit; determining second attitude data of the inertial measurement unit under a second preset coordinate system according to the target linear acceleration value and the target angular velocity value; and determining underwater positioning information according to the first position data, the second position data and a preset error state cost function, wherein the parameters of the preset error state cost function comprise a first residual parameter corresponding to the inertial measurement unit and a second residual parameter corresponding to the ultra-short baseline sensor, so that the accuracy of the positioning information of the underwater vehicle can be improved.
Description
Technical Field
The invention relates to the technical field of underwater navigation and positioning, in particular to an underwater positioning method, an underwater vehicle and a computer readable storage medium.
Background
When underwater perception is carried out, sonar needs to be used as a visual perception tool, the resolution of the sonar is low, therefore, positioning information of an underwater carrier vehicle is needed to assist splicing when image splicing is carried out, the accuracy of the positioning information influences the accuracy of image splicing, and when the underwater carrier vehicle carries out positioning, an error shaking phenomenon exists in real time, so that the positioning information is not accurate.
Disclosure of Invention
The invention mainly aims to provide an underwater positioning method, an underwater carrier vehicle and a computer readable storage medium, and aims to solve the technical problem that positioning information of the underwater carrier vehicle is inaccurate.
In order to achieve the above object, the present invention provides an underwater positioning method, which is applied to an underwater vehicle, the underwater vehicle including an inertial measurement unit and an ultra-short baseline sensor, the underwater positioning method including:
determining first position and attitude data of the inertial measurement unit under a first preset coordinate system;
determining a target linear acceleration value and a target angular velocity value according to the first attitude data and the inertial data detected by the inertial measurement unit;
determining second attitude data of the inertial measurement unit under a second preset coordinate system according to the target linear acceleration value and the target angular velocity value;
determining underwater positioning information according to the first position and attitude data, the second position and attitude data and a preset error state cost function, wherein parameters of the preset error state cost function comprise a first residual parameter corresponding to the inertial measurement unit and a second residual parameter corresponding to the ultra-short baseline sensor, the ultra-short baseline sensor is used for detecting position and attitude correction data, and the position and attitude correction data is used for determining the second residual parameter.
Optionally, the step of determining underwater positioning information according to the first position and orientation data, the second position and orientation data, and a preset error state cost function includes:
determining a first residual value of the inertial measurement unit according to the first position data and the second position data, wherein the first residual value corresponds to the first residual parameter;
acquiring a second residual value detected by the ultra-short baseline sensor, wherein the second residual value corresponds to the second residual parameter;
determining a minimum value of the preset error state cost function according to the first residual value and the second residual value;
and determining the underwater positioning information according to the minimum value.
Optionally, the first position and orientation data includes a linear velocity of the inertial measurement unit, the second position and orientation data includes a second position and a second direction, and the step of determining the first residual value of the inertial measurement unit according to the first position and orientation data includes:
determining a first variable according to the linear velocity, a first preset angular velocity and a second preset angular velocity, wherein the first preset angular velocity is an angular velocity of a first degree of freedom, the second preset angular velocity is an angular velocity of a second degree of freedom, the first degree of freedom and the second degree of freedom are degree-of-freedom components of reference gravity, the first degree of freedom is different from the second degree of freedom, the first degree of freedom and the second degree of freedom are perpendicular to each other, and the first preset angular velocity and the second preset angular velocity are components of the angular velocity of the inertial measurement unit;
determining a matrix coefficient according to the first degree of freedom, the second degree of freedom and the second position;
determining a second variable according to the first variable and the matrix coefficient;
and determining a first residual value of the inertial measurement unit according to the first variable and the second variable.
Optionally, the inertial measurement unit includes a gyroscope, the inertial data includes angular velocity data detected by the gyroscope and object acceleration detected by an accelerometer, the first attitude data includes a first direction, and the step of determining the target linear acceleration value and the target angular velocity value according to the first attitude data and the inertial data detected by the inertial measurement unit includes:
determining a gyroscope bias value according to the first direction and a first rotation matrix value, wherein the first rotation matrix value is a rotation matrix value of the gyroscope between adjacent frames of the ultra-short baseline sensor;
determining a target angular velocity value of the inertial measurement unit under a second preset coordinate system according to the angular velocity data and the gyroscope bias value;
and determining the target linear acceleration value according to the object acceleration, the acceleration bias value, the target gravity and the second rotation matrix.
Optionally, the step of determining a gyroscope bias value according to the first direction and the first rotation matrix value includes:
and determining the minimum value of a preset loss function according to the first rotation matrix value, the preset correction coefficient of the first rotation matrix value and the first direction, and taking the minimum value of the preset loss function as the bias value of the gyroscope.
Optionally, before the step of determining the linear acceleration of the inertial measurement unit in the second preset coordinate system according to the object acceleration, the acceleration bias value, the target gravity, and the second rotation matrix value, the method further includes:
determining the first degree of freedom and the second degree of freedom of the reference gravity;
determining a first angular velocity corresponding to the first degree of freedom and a second angular velocity corresponding to the second degree of freedom;
and determining the target gravity according to the first degree of freedom, the second degree of freedom, the first angular velocity and the second angular velocity.
Optionally, between the step of determining the target angular velocity value of the inertial measurement unit in the second preset coordinate system according to the angular velocity data and the gyroscope bias value and the step of determining the target linear acceleration value according to the object acceleration, the acceleration bias value, the target gravity and the second rotation matrix, the method further includes:
and integrating the target angular velocity values, and taking the integrated target angular velocity values as the second rotation matrix.
Optionally, after the step of determining the underwater positioning information according to the first position and orientation data, the second position and orientation data, and a preset error state cost function, the method further includes:
acquiring the underwater positioning information in a preset time period, and taking the underwater positioning information in the preset time period as target positioning information;
acquiring sonar information detected by sonar in real time;
and sending the target positioning information and the sonar information to image splicing equipment so that the image splicing equipment carries out sonar image splicing operation according to the target positioning information and the sonar information.
In addition, in order to achieve the above object, the present invention further provides an underwater vehicle, including an inertial measurement unit, an ultra-short baseline sensor, a processor and a memory, wherein the processor is in communication connection with the inertial measurement unit and the ultra-short baseline sensor, the memory stores an underwater positioning program, and the underwater positioning program implements the steps of any one of the above described underwater positioning methods when executed by the processor.
In addition, to achieve the above object, the present invention further provides a computer readable storage medium having an underwater positioning program stored thereon, where the underwater positioning program is executed by a processor to implement the steps of the underwater positioning method according to any one of the above.
The invention provides an underwater positioning method, an underwater vehicle and a computer readable storage medium, wherein first position data of an inertial measurement unit under a first preset coordinate system is determined; determining a target linear acceleration value and a target angular velocity value according to the first attitude data and the inertia data detected by the inertia measurement unit; determining second attitude data of the inertial measurement unit under a second preset coordinate system according to the target linear acceleration value and the target angular velocity value; determining underwater positioning information according to the first position and attitude data, the second position and attitude data and a preset error state cost function, wherein parameters of the preset error state cost function comprise a first residual parameter corresponding to the inertial measurement unit and a second residual parameter corresponding to the ultra-short baseline sensor, the ultra-short baseline sensor is used for detecting position and attitude correction data, and the position and attitude correction data is used for determining a second residual parameter; therefore, the underwater positioning information can be solved according to the residual error of the inertial measurement unit and the residual error of the ultra-short baseline sensor, so that error correction is carried out, and the accuracy of the positioning information of the underwater vehicle is improved.
Drawings
FIG. 1 is a schematic illustration of an underwater vehicle configuration of a hardware environment in which embodiments of the present invention are implemented;
FIG. 2 is a schematic flow chart of a first embodiment of the underwater positioning method of the present invention;
FIG. 3 is a schematic flow chart of a second embodiment of the underwater positioning method of the present invention;
FIG. 4 is a schematic flow chart of a third embodiment of the underwater positioning method of the present invention;
fig. 5 is a schematic flow chart of a fourth embodiment of the underwater positioning method of the present invention.
The implementation, functional features and advantages of the objects of the present invention will be further explained with reference to the accompanying drawings.
Detailed Description
It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.
As shown in fig. 1, fig. 1 is a schematic structural diagram of an underwater vehicle according to an embodiment of the present invention.
As shown in fig. 1, the underwater vehicle can include: an inertial measurement unit 1001 and an ultra-short baseline sensor 1002, a processor 1003, e.g., a CPU, a memory 1004, a communication bus 1005. A communication bus 1005 is used, among other things, to enable connective communication between these components. The memory 1004 may be a high-speed RAM memory or a non-volatile memory (e.g., a disk memory). The memory 1004 may alternatively be a storage device separate from the processor 1003 described above.
Those skilled in the art will appreciate that the underwater vehicle structure shown in fig. 1 does not constitute a limitation of the underwater vehicle, and may include more or fewer components than shown, or some components in combination, or a different arrangement of components.
As shown in fig. 1, the memory 1004, which is a type of computer storage medium, may include an operating system and an underwater positioning program.
In the underwater vehicle shown in fig. 1, the processor 1001 may be configured to invoke an underwater location program stored in the memory 1004 and perform the following operations:
determining first position and attitude data of the inertial measurement unit under a first preset coordinate system;
determining a target linear acceleration value and a target angular velocity value according to the first attitude data and the inertial data detected by the inertial measurement unit;
determining second attitude data of the inertial measurement unit under a second preset coordinate system according to the target linear acceleration value and the target angular velocity value;
determining underwater positioning information according to the first position and attitude data, the second position and attitude data and a preset error state cost function, wherein parameters of the preset error state cost function comprise a first residual parameter corresponding to the inertial measurement unit and a second residual parameter corresponding to the ultra-short baseline sensor, the ultra-short baseline sensor is used for detecting position and attitude correction data, and the position and attitude correction data is used for determining the second residual parameter.
Further, the processor 1001 may invoke a subsea positioning program stored in the memory 1004, and further perform the following operations:
determining a first residual value of the inertial measurement unit according to the first position data and the second position data, wherein the first residual value corresponds to the first residual parameter;
acquiring a second residual value detected by the ultra-short baseline sensor, wherein the second residual value corresponds to the second residual parameter;
determining a minimum value of the preset error state cost function according to the first residual value and the second residual value;
and determining the underwater positioning information according to the minimum value.
Further, the processor 1001 may invoke a subsea positioning program stored in the memory 1004, and further perform the following operations:
determining a first variable according to the linear velocity, a first preset angular velocity and a second preset angular velocity, wherein the first preset angular velocity is an angular velocity of a first degree of freedom, the second preset angular velocity is an angular velocity of a second degree of freedom, the first degree of freedom and the second degree of freedom are degree-of-freedom components of reference gravity, the first degree of freedom is different from the second degree of freedom, the first degree of freedom and the second degree of freedom are perpendicular to each other, and the first preset angular velocity and the second preset angular velocity are components of the angular velocity of the inertial measurement unit;
determining a matrix coefficient according to the first degree of freedom, the second degree of freedom and the second position;
determining a second variable according to the first variable and the matrix coefficient;
and determining a first residual value of the inertial measurement unit according to the first variable and the second variable.
Further, the processor 1001 may invoke a subsea positioning program stored in the memory 1004, and further perform the following operations:
determining a gyroscope bias value according to the first direction and a first rotation matrix value, wherein the first rotation matrix value is a rotation matrix value of the gyroscope between adjacent frames of the ultra-short baseline sensor;
determining a target angular velocity value of the inertial measurement unit under a second preset coordinate system according to the angular velocity data and the gyroscope bias value;
and determining the target linear acceleration value according to the object acceleration, the acceleration bias value, the target gravity and the second rotation matrix.
Further, the processor 1001 may invoke a subsea positioning program stored in the memory 1004, and further perform the following operations:
and determining the minimum value of a preset loss function according to the first rotation matrix value, the preset correction coefficient of the first rotation matrix value and the first direction, and taking the minimum value of the preset loss function as the bias value of the gyroscope.
Further, the processor 1001 may invoke a subsea positioning program stored in the memory 1004, and further perform the following operations:
determining the first degree of freedom and the second degree of freedom of the reference gravity;
determining a first angular velocity corresponding to the first degree of freedom and a second angular velocity corresponding to the second degree of freedom;
and determining the target gravity according to the first degree of freedom, the second degree of freedom, the first angular velocity and the second angular velocity.
Further, the processor 1001 may invoke a subsea positioning program stored in the memory 1004, and further perform the following operations:
determining the first degree of freedom and the second degree of freedom of the reference gravity;
determining a first angular velocity corresponding to the first degree of freedom and a second angular velocity corresponding to the second degree of freedom;
and determining the target gravity according to the first degree of freedom, the second degree of freedom, the first angular velocity and the second angular velocity.
Further, the processor 1001 may invoke a subsea positioning program stored in the memory 1004, and further perform the following operations:
and integrating the target angular velocity values, and taking the integrated target angular velocity values as the second rotation matrix.
Further, the processor 1001 may invoke a subsea positioning program stored in the memory 1004, and further perform the following operations:
acquiring the underwater positioning information in a preset time period, and taking the underwater positioning information in the preset time period as target positioning information;
acquiring sonar information detected by sonar in real time;
and sending the target positioning information and the sonar information to image splicing equipment so that the image splicing equipment carries out sonar image splicing operation according to the target positioning information and the sonar information.
Referring to fig. 2, a first embodiment of the present invention provides an underwater positioning method including:
step S10, determining first attitude data of the inertial measurement unit in a first preset coordinate system;
an Inertial Measurement Unit (IMU) is a device for measuring an attitude angle and an acceleration of an object, a first preset coordinate system is an Inertial coordinate system or a body coordinate system, first attitude data is data related to a position and an attitude corresponding to the IMU, and the first attitude data may include a position, a linear velocity, and a direction.
In order to develop the Underwater world, such as abundant mineral, energy and biological resources in the ocean, it is necessary to use sonar as a visual perception tool for medium and long distance detection, where the sonar has low resolution, and when splicing or matching is performed, it is necessary to use positioning information of An Underwater Vehicle (AUV) for assistance, where the positioning information may be used for real-time positioning and navigation of the Underwater Vehicle, and there is an error jitter phenomenon in the real-time of the positioning information, which causes a dislocation during sonar splicing, and further lacks smoothness, and in order to solve the problem that there is an error jitter phenomenon in the positioning information, which causes inaccurate positioning information, in this embodiment, sensor data is obtained by an IMU and an Ultra-short base line (USBL) and errors are further corrected, and in addition, when error correction is performed, a Doppler velocimeter (Doppler velocimity), DVL) and carrying out error correction by combining the linear velocity detected by the DVL, wherein the IMU and the DVL are fused to deduce the position, the linear velocity and the posture of the AUV, and are tightly coupled with USBL information to obtain smooth pose data, so that the delay requirement of sonar is met.
The IMU, the DVL and the USBL slave station in the underwater vehicle are fixed on the AUV, the USBL master station is independently fixed, the systems where the IMU, the DVL, the USBL master station and the USBL slave station are located are synchronous, and the IMU, the DVL, the USBL master station and the USBL slave station are started simultaneously.
The first position data in this embodiment includes position data and direction data, and the first position data is a position of the inertial measurement unit in a first preset coordinate system.
Step S20, determining a target linear acceleration value and a target angular velocity value according to the first attitude data and the inertia data detected by the inertia measurement unit;
the inertial data may include an object acceleration value detected by the accelerometer and angular velocity data detected by the gyroscope, and other inertial data may be added as needed, which is not limited herein; the target linear acceleration value is corrected linear acceleration of the accelerometer in the body coordinate system, the target angular velocity is corrected angular velocity of the inertial measurement unit in the body coordinate system, and error correction can be performed on angular velocity data and the object acceleration according to the angular velocity offset and the acceleration offset by solving the acceleration offset and the angular velocity offset.
Step S30, determining second attitude data of the inertial measurement unit under a second preset coordinate system according to the target linear acceleration value and the target angular velocity value;
the second preset coordinate system is a global coordinate system or a world coordinate system, the second position and orientation data is position and orientation data of the inertial measurement unit in an adjacent time interval, and the second position and orientation data may include a second position and a second direction.
After the target linear acceleration value and the target angular velocity value are determined, second posture data are determined according to the target angular velocity value and the target linear acceleration value, and therefore more accurate second posture data can be obtained.
Before determining the second position and the second direction, first, a first position and a first direction may be determined, where the first position and the first direction are positions and directions before the time of the second position and the second direction, and both the first position and the first direction are parameters in a second preset coordinate system, the first position is represented by p, the linear velocity is represented by v, and the first direction is represented by q, and then:
where t denotes a current time, Δ t denotes a time interval of adjacent times, t +1 denotes a next time adjacent to the current time, a denotes an acceleration of an object detected by an accelerometer in the inertial measurement unit, and g denotes a gravitational acceleration.
And a second position is denoted by P and a second direction is denoted by Q, then:
r is a first rotation matrix, R is a second rotation matrix,for time i body coordinate system relative to bkA rotation matrix of (2), can beViewed as the first rotation matrix at time i, Δ t is [ t ]k,tk+1]The time interval in between, the linear velocity v is read directly by the DVL.
Step S40, determining underwater positioning information according to the first position and orientation data, the second position and orientation data, and a preset error state cost function, where parameters of the preset error state cost function include a first residual parameter corresponding to the inertial measurement unit and a second residual parameter corresponding to the ultra-short baseline sensor, the ultra-short baseline sensor is configured to detect position and orientation correction data, and the position and orientation correction data is configured to determine the second residual parameter.
After the first position data and the second position data are obtained, determining underwater positioning information according to the first position data, the second position data and a preset error state cost function, wherein the underwater positioning information can comprise position information and posture information, the posture information comprises direction information, the preset error state cost function is a cost function related to a first residual parameter and a second residual parameter, the first residual parameter is a residual parameter corresponding to an inertial measurement unit and used for representing a measurement residual of an IMU, the second residual parameter is a residual parameter corresponding to an ultra-short baseline sensor and used for representing a measurement residual of a USBL, the USBL calculates a second residual parameter through detected pose correction data, and the error state cost function can be solved through a nonlinear optimization library Ceres to obtain the underwater positioning information.
In the embodiment, the first attitude data of the inertial measurement unit in a first preset coordinate system is determined; determining a target linear acceleration value and a target angular velocity value according to the first attitude data and the inertia data detected by the inertia measurement unit; determining second attitude data of the inertial measurement unit under a second preset coordinate system according to the target linear acceleration value and the target angular velocity value; determining underwater positioning information according to the first position and attitude data, the second position and attitude data and a preset error state cost function, wherein parameters of the preset error state cost function comprise a first residual parameter corresponding to the inertial measurement unit and a second residual parameter corresponding to the ultra-short baseline sensor, the ultra-short baseline sensor is used for detecting position and attitude correction data, and the position and attitude correction data is used for determining a second residual parameter; therefore, the underwater positioning information can be solved according to the residual error of the inertial measurement unit and the residual error of the ultra-short baseline sensor, so that error correction is carried out, and the accuracy of the positioning information of the underwater vehicle is improved.
Referring to fig. 3, a second embodiment of the present invention provides an underwater positioning method, based on the first embodiment, step S40 includes:
step S41, determining a first residual value of the inertial measurement unit according to the first position data and the second position data, where the first residual value corresponds to the first residual parameter;
the first position data comprises a linear velocity of the inertial measurement unit, the second position data comprises a second position and a second direction, the second position is a position of the inertial measurement unit under a body coordinate system, the second direction is a direction of the inertial measurement unit under the body coordinate system, when calculation is performed, the first residual value is calculated corresponding to the first residual parameter, and in order to determine the first residual value, the following method can be adopted:
determining a first variable according to the linear velocity, a first preset angular velocity and a second preset angular velocity, wherein the first preset angular velocity is an angular velocity of a first degree of freedom, the second preset angular velocity is an angular velocity of a second degree of freedom, the first degree of freedom and the second degree of freedom are degree of freedom components of reference gravity, the first degree of freedom and the second degree of freedom are different, the first degree of freedom and the second degree of freedom are perpendicular to each other, and the first preset angular velocity and the second preset angular velocity are components of the angular velocity of the inertia measurement unit; linear velocity is denoted by v and ω is1Representing a first predetermined angular velocity, by2Representing a second predetermined angular velocity, w ═ ω1,ω2]TLet χ denote the first variable, then:
determining a matrix coefficient according to the first degree of freedom, the second degree of freedom and the second position;
by b1Representing a first degree of freedom, denoted by b2Denotes a second degree of freedom, P denotes a second position, H denotes a matrix coefficient, and u ═ b1,b2]And then:
determining a second variable according to the first variable and the matrix coefficient;
and z represents a second variable, and R represents a first rotation matrix, then:
and determining a first residual value of the inertial measurement unit according to the first variable and the second variable.
By rBRepresenting the first residual value, then:
step S42, acquiring a second residual value detected by the ultra-short baseline sensor, wherein the second residual value corresponds to the second residual parameter;
and during calculation, corresponding the second residual value to a second residual parameter, wherein the second residual value is obtained by USBL detection.
Step S43, determining a minimum value of the preset error state cost function according to the first residual value and the second residual value;
and step S44, determining the underwater positioning information according to the minimum value.
Constructing a preset error state cost function as follows:
the minimum may be solved by a non-linear optimization library, and the specific non-linear optimization library may be Ceres or other methods. After the minimum value is calculated, the formula is brought into a preset error state cost function by combining the calculation formula of the first residual value, and then underwater positioning information can be calculated, wherein the underwater positioning information comprises position information and attitude information.
In this embodiment, a first residual value of the inertial measurement unit is determined through the first pose data and the second pose data, a second residual value detected by the ultra-short baseline sensor is obtained, a minimum value of a preset error state cost function is determined according to the first residual value and the second residual value, and underwater positioning information is determined according to the minimum value, so that more accurate underwater positioning information is obtained.
Referring to fig. 4, a third embodiment of the present invention provides an underwater positioning method, based on any of the above embodiments, step S30 includes:
step S31, determining a gyroscope bias value according to the first direction and a first rotation matrix value, wherein the first rotation matrix value is a rotation matrix value of the gyroscope between adjacent frames of the ultra-short baseline sensor;
in determining the gyroscope bias, the following may be taken: determining the minimum value of a preset loss function according to the first rotation matrix value, the preset correction coefficient of the first rotation matrix value and the first direction, and taking the minimum value of the preset loss function as a gyroscope bias value, wherein the preset correction coefficient of the first rotation matrix value is represented by gamma, the first direction is represented by q, and the gyroscope bias is represented by bωAnd then:
wherein,
step S32, determining a target angular velocity value of the inertial measurement unit under a second preset coordinate system according to the angular velocity data and the gyroscope bias value;
by usingRepresenting the target angular velocity value, time tThe method comprises the following steps:
and after the target angular velocity value is obtained, integrating the target angular velocity value, and taking the integrated target angular velocity value as a second rotation matrix.
Determining a first degree of freedom and a second degree of freedom of a reference gravity; determining a first angular velocity corresponding to the first degree of freedom and a second angular velocity corresponding to the second degree of freedom; and determining the target gravity according to the first degree of freedom, the second degree of freedom, the first angular speed and the second angular speed.
The first angular velocity is denoted by ω 1, the second angular velocity is denoted by ω 2, and the target gravitational force is denoted by g. The reference gravity is the initially set gravity, the module value of the reference gravity can be 9.8, b1 represents the first degree of freedom, b2 represents the degree of freedom, and then:
u=[b1 b2];
g=uw;
And step S33, determining the target linear acceleration value according to the object acceleration, the acceleration offset value, the target gravity and the second rotation matrix.
The acceleration of the object is denoted by a and baRepresenting acceleration bias of IMU byRepresenting a target linear acceleration value, then:
wherein,is a second predetermined matrix, g is the target gravity, naIs the additive gaussian noise.
In this embodiment, a gyroscope offset value is determined according to a first direction and a first rotation matrix, a target angular velocity value of the inertial measurement unit in a second preset coordinate system is determined according to angular velocity data and the gyroscope offset value, and a target linear acceleration value is determined according to the object acceleration, the acceleration offset value, the target gravity and the second rotation matrix, so that the acceleration is corrected, a more accurate acceleration is obtained, and more accurate underwater positioning information can be further obtained.
Referring to fig. 5, a fourth embodiment of the present invention provides an underwater positioning method, based on any of the above embodiments, after step S40, the method includes:
step S50, acquiring the underwater positioning information in a preset time period, and taking the underwater positioning information in the preset time period as target positioning information;
the frequency of sonar information detection by the sonar is slower than the speed of data detection by the IMU or USBL, so that the time of sonar information detection is not synchronous with the time of IMU data detection and USBL data detection, so that the time of acquiring underwater positioning information and sonar information is not synchronous, and in order to adapt to the time delay conditions of different sonars, the embodiment acquires the underwater positioning information in a preset time period, so that the underwater positioning information and the information acquired by the sonar are synchronous in time, and the problem of low accuracy caused by asynchronous time delay is solved; the preset time period may be determined in advance according to the frequency of sonar detection information and stored.
Step S60, sonar information detected in real time by sonar is obtained;
sonar information is information detected by sonar, and is, for example, distance information.
Step S70, sending the target positioning information and the sonar information to image stitching equipment, so that the image stitching equipment performs sonar image stitching operation according to the target positioning information and the sonar information.
After the target positioning information is obtained, the target positioning information and the sonar information are sent to image splicing equipment, so that the image splicing equipment can finish sonar image splicing; image concatenation equipment can be according to the intensity of sonar echo signal with sonar information recovery for the grey level image of sonar.
In this embodiment, through the underwater positioning information who obtains in the preset time quantum, with the underwater positioning information in the preset time quantum as target positioning information, obtain sonar information of sonar real-time supervision, send target positioning information and sonar information to image splicing equipment, so that image splicing equipment carries out sonar image splicing operation according to target positioning information and sonar information, thereby can be according to actual demand intercepting underwater positioning information, optimize time, be applicable to the demand of different sonars, for example, be applicable to the required location of foresight sonar matching, thereby can reduce the delay problem.
It should be noted that, in this document, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or system that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or system. Without further limitation, an element defined by the phrase "comprising an … …" does not exclude the presence of other like elements in a process, method, article, or system that comprises the element.
The above-mentioned serial numbers of the embodiments of the present invention are merely for description and do not represent the merits of the embodiments.
Through the above description of the embodiments, those skilled in the art will clearly understand that the method of the above embodiments can be implemented by software plus a necessary general hardware platform, and certainly can also be implemented by hardware, but in many cases, the former is a better implementation manner. Based on this understanding, the technical solution of the present invention, which essentially or partly contributes to the prior art, can be embodied in the form of a software product stored on a storage medium (e.g. ROM/RAM, magnetic disk, optical disk) as described above, comprising instructions for causing an underwater vehicle device to perform the method according to the various embodiments of the present invention.
The above description is only a preferred embodiment of the present invention, and not intended to limit the scope of the present invention, and all modifications of equivalent structures and equivalent processes, which are made by using the contents of the present specification and the accompanying drawings, or directly or indirectly applied to other related technical fields, are included in the scope of the present invention.
Claims (10)
1. An underwater positioning method applied to an underwater carrier vehicle, wherein the underwater carrier vehicle comprises an inertial measurement unit and an ultra-short baseline sensor, and the underwater positioning method comprises the following steps:
determining first position and attitude data of the inertial measurement unit under a first preset coordinate system;
determining a target linear acceleration value and a target angular velocity value according to the first attitude data and the inertial data detected by the inertial measurement unit;
determining second attitude data of the inertial measurement unit under a second preset coordinate system according to the target linear acceleration value and the target angular velocity value;
determining underwater positioning information according to the first position and attitude data, the second position and attitude data and a preset error state cost function, wherein parameters of the preset error state cost function comprise a first residual parameter corresponding to the inertial measurement unit and a second residual parameter corresponding to the ultra-short baseline sensor, the ultra-short baseline sensor is used for detecting position and attitude correction data, and the position and attitude correction data is used for determining the second residual parameter.
2. The underwater positioning method of claim 1, wherein the step of determining underwater positioning information from the first position data, the second position data, and a preset error state cost function comprises:
determining a first residual value of the inertial measurement unit according to the first position data and the second position data, wherein the first residual value corresponds to the first residual parameter;
acquiring a second residual value detected by the ultra-short baseline sensor, wherein the second residual value corresponds to the second residual parameter;
determining a minimum value of the preset error state cost function according to the first residual value and the second residual value;
and determining the underwater positioning information according to the minimum value.
3. The underwater positioning method of claim 2 wherein the first position data comprises a linear velocity of the inertial measurement unit, the second position data comprises a second position and a second direction, and the step of determining the first residual value of the inertial measurement unit from the first position data and the second position data comprises:
determining a first variable according to the linear velocity, a first preset angular velocity and a second preset angular velocity, wherein the first preset angular velocity is an angular velocity of a first degree of freedom, the second preset angular velocity is an angular velocity of a second degree of freedom, the first degree of freedom and the second degree of freedom are degree-of-freedom components of reference gravity, the first degree of freedom is different from the second degree of freedom, the first degree of freedom and the second degree of freedom are perpendicular to each other, and the first preset angular velocity and the second preset angular velocity are components of the angular velocity of the inertial measurement unit;
determining a matrix coefficient according to the first degree of freedom, the second degree of freedom and the second position;
determining a second variable according to the first variable and the matrix coefficient;
and determining a first residual value of the inertial measurement unit according to the first variable and the second variable.
4. The underwater positioning method of claim 3, wherein the inertial measurement unit includes a gyroscope, the inertial data includes angular velocity data detected by the gyroscope and object acceleration detected by an accelerometer, the first attitude data includes a first direction, and the step of determining a target linear acceleration value and a target angular velocity value from the first attitude data and the inertial data detected by the inertial measurement unit includes:
determining a gyroscope bias value according to the first direction and a first rotation matrix value, wherein the first rotation matrix value is a rotation matrix value of the gyroscope between adjacent frames of the ultra-short baseline sensor;
determining a target angular velocity value of the inertial measurement unit under a second preset coordinate system according to the angular velocity data and the gyroscope bias value;
and determining the target linear acceleration value according to the object acceleration, the acceleration bias value, the target gravity and the second rotation matrix.
5. An underwater positioning method as claimed in claim 4, wherein the step of determining a gyroscope bias value based on the first direction and a first rotation matrix value comprises:
and determining the minimum value of a preset loss function according to the first rotation matrix value, the preset correction coefficient of the first rotation matrix value and the first direction, and taking the minimum value of the preset loss function as the bias value of the gyroscope.
6. The underwater positioning method of claim 4, wherein before the step of determining the linear acceleration of the inertial measurement unit in the second predetermined coordinate system according to the object acceleration, the acceleration bias value, the target gravity, and the second rotation matrix value, further comprises:
determining the first degree of freedom and the second degree of freedom of the reference gravity;
determining a first angular velocity corresponding to the first degree of freedom and a second angular velocity corresponding to the second degree of freedom;
and determining the target gravity according to the first degree of freedom, the second degree of freedom, the first angular velocity and the second angular velocity.
7. The underwater positioning method as claimed in claim 4, wherein between the step of determining the target angular velocity value of the inertial measurement unit in the second preset coordinate system based on the angular velocity data and the gyroscope bias value and the step of determining the target linear acceleration value based on the object acceleration, the acceleration bias value, the target gravity, and the second rotation matrix, further comprising:
and integrating the target angular velocity values, and taking the integrated target angular velocity values as the second rotation matrix.
8. The underwater positioning method of any of claims 1-7, further comprising, after the step of determining underwater positioning information from the first position attitude data, the second position attitude data, and a preset error state cost function:
acquiring the underwater positioning information in a preset time period, and taking the underwater positioning information in the preset time period as target positioning information;
acquiring sonar information detected by sonar in real time;
and sending the target positioning information and the sonar information to image splicing equipment so that the image splicing equipment carries out sonar image splicing operation according to the target positioning information and the sonar information.
9. An underwater vehicle comprising an inertial measurement unit, an ultra-short baseline sensor, a processor in communication with the inertial measurement unit and the ultra-short baseline sensor, and a memory having an underwater positioning program stored thereon, which when executed by the processor, performs the steps of the underwater positioning method of any of claims 1-8.
10. A computer readable storage medium, characterized in that the computer readable storage medium has stored thereon an underwater positioning program, which when executed by a processor, carries out the steps of the underwater positioning method according to any one of claims 1-8.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110316751.4A CN113075665B (en) | 2021-03-24 | 2021-03-24 | Underwater positioning method, underwater carrier vehicle and computer readable storage medium |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110316751.4A CN113075665B (en) | 2021-03-24 | 2021-03-24 | Underwater positioning method, underwater carrier vehicle and computer readable storage medium |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113075665A true CN113075665A (en) | 2021-07-06 |
CN113075665B CN113075665B (en) | 2023-06-20 |
Family
ID=76610181
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110316751.4A Active CN113075665B (en) | 2021-03-24 | 2021-03-24 | Underwater positioning method, underwater carrier vehicle and computer readable storage medium |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113075665B (en) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113218386A (en) * | 2021-07-08 | 2021-08-06 | 深之蓝海洋科技股份有限公司 | Method and device for high-precision navigation of robot in liquid building |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR101755307B1 (en) * | 2016-05-11 | 2017-07-10 | 한국해양과학기술원 | A position measurement error correcting method of underwater moving objects |
CN109737956A (en) * | 2018-12-17 | 2019-05-10 | 东南大学 | A kind of SINS/USBL phase difference tight integration navigation locating method based on double response device |
CN110006433A (en) * | 2019-04-22 | 2019-07-12 | 哈尔滨工程大学 | The integrated navigation and location system and method for sea-bottom oil-gas pipe detection robot |
CN111380518A (en) * | 2020-03-04 | 2020-07-07 | 哈尔滨工程大学 | SINS/USBL tight combination navigation positioning method introducing radial velocity |
CN111596333A (en) * | 2020-06-05 | 2020-08-28 | 鹏城实验室 | Underwater positioning navigation method and system |
CN112146654A (en) * | 2020-08-25 | 2020-12-29 | 浙江大学 | Foresight imaging sonar underwater positioning and navigation method based on key constraint frame |
CN112197765A (en) * | 2020-09-14 | 2021-01-08 | 浙江大学 | Method for realizing fine navigation of underwater robot |
US20210041240A1 (en) * | 2018-06-22 | 2021-02-11 | Southeast University | Navigation and positioning system for underwater glider and up floating error correction method |
-
2021
- 2021-03-24 CN CN202110316751.4A patent/CN113075665B/en active Active
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
KR101755307B1 (en) * | 2016-05-11 | 2017-07-10 | 한국해양과학기술원 | A position measurement error correcting method of underwater moving objects |
US20210041240A1 (en) * | 2018-06-22 | 2021-02-11 | Southeast University | Navigation and positioning system for underwater glider and up floating error correction method |
CN109737956A (en) * | 2018-12-17 | 2019-05-10 | 东南大学 | A kind of SINS/USBL phase difference tight integration navigation locating method based on double response device |
CN110006433A (en) * | 2019-04-22 | 2019-07-12 | 哈尔滨工程大学 | The integrated navigation and location system and method for sea-bottom oil-gas pipe detection robot |
CN111380518A (en) * | 2020-03-04 | 2020-07-07 | 哈尔滨工程大学 | SINS/USBL tight combination navigation positioning method introducing radial velocity |
CN111596333A (en) * | 2020-06-05 | 2020-08-28 | 鹏城实验室 | Underwater positioning navigation method and system |
CN112146654A (en) * | 2020-08-25 | 2020-12-29 | 浙江大学 | Foresight imaging sonar underwater positioning and navigation method based on key constraint frame |
CN112197765A (en) * | 2020-09-14 | 2021-01-08 | 浙江大学 | Method for realizing fine navigation of underwater robot |
Non-Patent Citations (1)
Title |
---|
徐博;郝芮;王超;张勋;张娇: "水下潜航器的惯导/超短基线/多普勒测速信息融合及容错验证", 光学精密工程, vol. 25, no. 9, pages 2508 - 2515 * |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113218386A (en) * | 2021-07-08 | 2021-08-06 | 深之蓝海洋科技股份有限公司 | Method and device for high-precision navigation of robot in liquid building |
Also Published As
Publication number | Publication date |
---|---|
CN113075665B (en) | 2023-06-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109975792B (en) | Method for correcting point cloud motion distortion of multi-line laser radar based on multi-sensor fusion | |
CN109887057B (en) | Method and device for generating high-precision map | |
US11279045B2 (en) | Robot pose estimation method and apparatus and robot using the same | |
US10788830B2 (en) | Systems and methods for determining a vehicle position | |
CN109116397B (en) | Vehicle-mounted multi-camera visual positioning method, device, equipment and storage medium | |
US20210370970A1 (en) | Positioning Method And Apparatus, Autonomous Driving Vehicle, Electronic Device And Storage Medium | |
CN107588769B (en) | Vehicle-mounted strapdown inertial navigation, odometer and altimeter integrated navigation method | |
EP1760431B1 (en) | Inertial navigation system with a plurality of Kalman filters and vehicle equipped with such a system | |
CN111854740B (en) | Inertial navigation system capable of dead reckoning in a vehicle | |
EP1983304A2 (en) | Heading stabilization for aided inertial navigation systems | |
JP2012173190A (en) | Positioning system and positioning method | |
JP2004239643A (en) | Hybrid navigator | |
CN108627152B (en) | Navigation method of micro unmanned aerial vehicle based on multi-sensor data fusion | |
CN113783652A (en) | Data synchronization method and device of combined navigation system | |
CN110440797A (en) | Vehicle attitude estimation method and system | |
JP2014240266A (en) | Sensor drift amount estimation device and program | |
CN113075665A (en) | Underwater positioning method, underwater vehicle navigation device and computer readable storage medium | |
Spielvogel et al. | A stable adaptive attitude estimator on SO (3) for true-North seeking gyrocompass systems: Theory and preliminary simulation evaluation | |
JP2019082328A (en) | Position estimation device | |
JP4343581B2 (en) | Moving body posture detection device | |
CN116380119A (en) | Calibration method, device and system for integrated navigation | |
CN114413898A (en) | Multi-sensor data fusion method and device, computer equipment and storage medium | |
CN109737950B (en) | Underwater robot positioning system and method based on VIO and dynamic model | |
CN112595325B (en) | Initial position determining method, device, electronic equipment and storage medium | |
JP3863043B2 (en) | Position detection method for underwater vehicle |
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 |