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 PDF

Info

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
Application number
CN202110316751.4A
Other languages
Chinese (zh)
Other versions
CN113075665B (en
Inventor
杨巍
侯西龙
王超
杨健
陈明
罗明成
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Peng Cheng Laboratory
Original Assignee
Peng Cheng Laboratory
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Peng Cheng Laboratory filed Critical Peng Cheng Laboratory
Priority to CN202110316751.4A priority Critical patent/CN113075665B/en
Publication of CN113075665A publication Critical patent/CN113075665A/en
Application granted granted Critical
Publication of CN113075665B publication Critical patent/CN113075665B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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

Underwater positioning method, underwater vehicle navigation device and computer readable storage medium
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:
Figure BDA0002990782430000091
Figure BDA0002990782430000092
Figure BDA0002990782430000093
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:
Figure BDA0002990782430000094
Figure BDA0002990782430000095
r is a first rotation matrix, R is a second rotation matrix,
Figure BDA0002990782430000096
for time i body coordinate system relative to bkA rotation matrix of (2), can be
Figure BDA0002990782430000097
Viewed 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:
Figure BDA0002990782430000111
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:
Figure BDA0002990782430000112
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:
Figure BDA0002990782430000113
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:
Figure BDA0002990782430000114
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:
Figure BDA0002990782430000121
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:
Figure BDA0002990782430000122
wherein the content of the first and second substances,
Figure BDA0002990782430000123
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 using
Figure BDA0002990782430000124
Representing the target angular velocity value, time t
Figure BDA0002990782430000125
The method comprises the following steps:
Figure BDA0002990782430000126
wherein, bωFor gyroscope bias, ω is angular velocity data, nωIs added Gaussian noise, and
Figure BDA0002990782430000131
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；
Figure BDA0002990782430000132
Figure BDA0002990782430000133
wherein the updating can be performed in the following way
Figure BDA0002990782430000134
Figure BDA0002990782430000135
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 by
Figure BDA0002990782430000136
Representing a target linear acceleration value, then:
Figure BDA0002990782430000137
wherein the content of the first and second substances,
Figure BDA0002990782430000138
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.
CN202110316751.4A 2021-03-24 2021-03-24 Underwater positioning method, underwater carrier vehicle and computer readable storage medium Active CN113075665B (en)

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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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

Patent Citations (8)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
Title
徐博;郝芮;王超;张勋;张娇: "水下潜航器的惯导/超短基线/多普勒测速信息融合及容错验证", 光学精密工程, vol. 25, no. 9, pages 2508 - 2515 *

Cited By (1)

* Cited by examiner, † Cited by third party
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
CN109116397B (en) Vehicle-mounted multi-camera visual positioning method, device, equipment and storage medium
US10788830B2 (en) Systems and methods for determining a vehicle position
US11279045B2 (en) Robot pose estimation method and apparatus and robot using the same
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
EP2952930B1 (en) System and method for augmenting a gnss/ins navigation system in a cargo port environment
EP1983304A2 (en) Heading stabilization for aided inertial navigation systems
CN112835085B (en) Method and device for determining vehicle position
CN111854740B (en) Inertial navigation system capable of dead reckoning in a vehicle
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
CN112964291A (en) Sensor calibration method and device, computer storage medium and terminal
JP2014240266A (en) Sensor drift amount estimation device and program
CN113075665A (en) Underwater positioning method, underwater vehicle navigation device and computer readable storage medium
CN113137973A (en) Image semantic feature point truth value determining method and device
CN112363196A (en) Vehicle attribute determination method and device, storage medium and electronic equipment
JP4343581B2 (en) Moving body posture detection device
CN114413898A (en) Multi-sensor data fusion method and device, computer equipment and storage medium
CN115344033A (en) Monocular camera/IMU/DVL tight coupling-based unmanned ship navigation and positioning method
CN109737950B (en) Underwater robot positioning system and method based on VIO and dynamic model
CN116380119A (en) Calibration method, device and system for integrated navigation

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