CN113075665B - Underwater positioning method, underwater carrier vehicle and computer readable storage medium - Google Patents

Underwater positioning method, underwater carrier vehicle and computer readable storage medium Download PDF

Info

Publication number
CN113075665B
CN113075665B CN202110316751.4A CN202110316751A CN113075665B CN 113075665 B CN113075665 B CN 113075665B CN 202110316751 A CN202110316751 A CN 202110316751A CN 113075665 B CN113075665 B CN 113075665B
Authority
CN
China
Prior art keywords
value
determining
angular velocity
preset
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.)
Active
Application number
CN202110316751.4A
Other languages
Chinese (zh)
Other versions
CN113075665A (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 carrier vehicle and a computer readable storage medium, wherein the underwater positioning method comprises the following steps: determining first pose data of the inertial measurement unit under a first preset coordinate system; determining a target line acceleration value and a target angular velocity value according to the first pose data and the inertial data detected by the inertial measurement unit; determining second pose 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 pose data, the second pose data and a preset error state cost function, wherein parameters of the preset error state cost function comprise a first residual error parameter corresponding to the inertial measurement unit and a second residual error parameter corresponding to the ultra-short baseline sensor, so that the accuracy of the positioning information of the underwater carrier vehicle can be improved.

Description

Underwater positioning method, underwater carrier vehicle and computer readable storage medium
Technical Field
The present invention relates to the field of underwater navigation positioning technology, and in particular, to an underwater positioning method, an underwater vehicle, and a computer readable storage medium.
Background
When underwater perception is carried out, sonar is required to be used as a visual perception tool, and the resolution of the sonar is low, so that positioning information of the underwater carrier vehicle is required to assist in splicing when image splicing is carried out, the accuracy of the positioning information influences the accuracy of the image splicing, and when the underwater carrier vehicle is positioned, error jitter phenomenon exists in real-time, so that the positioning information is inaccurate, and the technical problem to be solved by the invention is that the positioning information of the underwater carrier vehicle is inaccurate.
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.
To achieve the above object, the present invention provides an underwater positioning method applied to an underwater carrier vehicle including an inertial measurement unit and an ultra-short baseline sensor, the underwater positioning method including:
determining first pose data of the inertial measurement unit under a first preset coordinate system;
determining a target line acceleration value and a target angular velocity value according to the first pose data and the inertial data detected by the inertial measurement unit;
determining second pose 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 pose data, the second pose data and a preset error state cost function, wherein parameters of the preset error state cost function comprise a first residual error parameter corresponding to the inertial measurement unit and a second residual error parameter corresponding to an ultra-short baseline sensor, the ultra-short baseline sensor is used for detecting pose correction data, and the pose correction data is used for determining the second residual error parameter.
Optionally, the step of determining the underwater positioning information according to the first pose data, the second pose data and a preset error state cost function includes:
determining a first residual value of the inertial measurement unit according to the first pose data and the second pose data, wherein the first residual value corresponds to the first residual parameter;
acquiring a second residual value detected by an ultra-short baseline sensor, wherein the second residual value corresponds to the second residual parameter;
determining the 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 pose data includes a linear velocity of the inertial measurement unit, the second pose 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 pose data and the second pose 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 free degree components of reference gravity, the first degree of freedom is different from the second degree of freedom, the first degree of freedom is perpendicular to the second degree of freedom, 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 pose data includes a first direction, and the step of determining the target line acceleration value and the target angular velocity value according to the first pose 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 line acceleration value according to the object acceleration, the acceleration bias value, the target gravity and the second rotation matrix.
Optionally, the step of determining the 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 gyroscope offset value.
Optionally, before the step of determining the linear acceleration of the inertial measurement unit under 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;
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 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 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 value, and taking the integrated target angular velocity value as the second rotation matrix.
Optionally, after the step of determining the underwater positioning information according to the first pose data, the second pose data and the 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 in real time by using a sonar;
and sending the target positioning information and the sonar information to an image stitching device, so that the image stitching device performs sonar image stitching operation according to the target positioning information and the sonar information.
In addition, in order to achieve the above object, the present invention also provides an underwater vehicle, which includes an inertial measurement unit, an ultra-short baseline sensor, a processor, and a memory, wherein the processor is communicatively connected to the inertial measurement unit and the ultra-short baseline sensor, and an underwater positioning program is stored in the memory, and the underwater positioning program when executed by the processor implements the steps of the underwater positioning method described in any one of the above.
In addition, in order to achieve the above object, the present invention also provides a computer-readable storage medium having stored thereon an underwater positioning program which, when executed by a processor, implements the steps of the underwater positioning method described in any one of the above.
The invention provides an underwater positioning method, an underwater carrier vehicle and a computer readable storage medium, wherein first pose data of an inertial measurement unit under a first preset coordinate system are determined; determining a target line acceleration value and a target angular velocity value according to the first pose data and the inertial data detected by the inertial measurement unit; determining second pose data of the inertial measurement unit under a second preset coordinate system according to the target line acceleration value and the target angular velocity value; determining underwater positioning information according to the first pose data, the second pose data and a preset error state cost function, wherein parameters of the preset error state cost function comprise a first residual error parameter corresponding to an inertial measurement unit and a second residual error parameter corresponding to an ultra-short baseline sensor, the ultra-short baseline sensor is used for detecting pose correction data, and the pose correction data are used for determining the second residual error 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 carrier vehicle is improved.
Drawings
FIG. 1 is a schematic diagram of the architecture of an underwater vehicle in a hardware operating environment in accordance with an embodiment of the present invention;
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 achievement of the objects, functional features and advantages of the present invention will be further described with reference to the accompanying drawings, in conjunction with the embodiments.
Detailed Description
It should be understood that the specific embodiments described herein are for purposes of illustration only and are not intended to limit the scope of the invention.
As shown in fig. 1, fig. 1 is a schematic structural view of an underwater vehicle according to an embodiment of the present invention.
As shown in fig. 1, the underwater vehicle may include: an inertial measurement unit 1001 and an ultra short baseline sensor 1002, a processor 1003, such as a CPU, memory 1004, a communication bus 1005. Wherein a communication bus 1005 is used to enable connected communications between these components. The memory 1004 may be a high-speed RAM memory or a stable memory (non-volatile memory), such as a disk memory. The memory 1004 may alternatively be a storage device separate from the aforementioned processor 1003.
It will be appreciated by those skilled in the art that the underwater vehicle structure shown in fig. 1 is not limiting of the underwater vehicle and may include more or fewer components than shown, or may combine certain components, or a different arrangement of components.
As shown in fig. 1, an operating system and an underwater positioning program may be included in the memory 1004 as one type of computer storage medium.
In the underwater vehicle shown in fig. 1, the processor 1001 may be used to call an underwater positioning program stored in the memory 1004 and perform the following operations:
determining first pose data of the inertial measurement unit under a first preset coordinate system;
determining a target line acceleration value and a target angular velocity value according to the first pose data and the inertial data detected by the inertial measurement unit;
determining second pose 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 pose data, the second pose data and a preset error state cost function, wherein parameters of the preset error state cost function comprise a first residual error parameter corresponding to the inertial measurement unit and a second residual error parameter corresponding to an ultra-short baseline sensor, the ultra-short baseline sensor is used for detecting pose correction data, and the pose correction data is used for determining the second residual error parameter.
Further, the processor 1001 may call the underwater 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 pose data and the second pose data, wherein the first residual value corresponds to the first residual parameter;
acquiring a second residual value detected by an ultra-short baseline sensor, wherein the second residual value corresponds to the second residual parameter;
determining the 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 call the underwater 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 free degree components of reference gravity, the first degree of freedom is different from the second degree of freedom, the first degree of freedom is perpendicular to the second degree of freedom, 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 call the underwater 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 line acceleration value according to the object acceleration, the acceleration bias value, the target gravity and the second rotation matrix.
Further, the processor 1001 may call the underwater 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 gyroscope offset value.
Further, the processor 1001 may call the underwater 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;
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 call the underwater 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;
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 call the underwater positioning program stored in the memory 1004, and further perform the following operations:
and integrating the target angular velocity value, and taking the integrated target angular velocity value as the second rotation matrix.
Further, the processor 1001 may call the underwater 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 in real time by using a sonar;
and sending the target positioning information and the sonar information to an image stitching device, so that the image stitching device performs sonar image stitching 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 pose data of the inertial measurement unit under a first preset coordinate system;
the inertial measurement unit (Inertial Measurement Unit, IMU) is a device for measuring the attitude angle and acceleration of an object, the first preset coordinate system is an inertial coordinate system or a body coordinate system, the first pose data is position and attitude related data corresponding to the inertial measurement unit, and the first pose data can comprise position, linear velocity and direction.
In order to develop abundant mineral products, energy sources and biological resources in the underwater world, such as the ocean, the sonar is required to be used as a visual perception tool for middle-long distance detection, the sonar has low resolution, positioning information of an underwater vehicle (Autonomous Underwater Vehicle, AUV) is required to be used for assistance in splicing or matching application, the positioning information can be used for real-time positioning and navigation of the underwater vehicle, error jitter phenomenon exists in the real-time performance of the positioning information, so that the sonar is misplaced, smoothness is further lacking, error jitter phenomenon exists in the positioning information, the positioning information is inaccurate, sensor data are acquired through an IMU and an Ultra-short Baseline base (USBL) and errors are further corrected, in addition, a Doppler velometer (Doppler Velocity Log, DVL) can be added in combination with the linear speed of DVL detection in error correction, the IMU and the DVL are fused to derive the position, the linear speed and the attitude of the AUV, and the position and attitude of the UBL information are tightly coupled, and the position and attitude of the UBL information are obtained, and the sonar requirements are met.
The IMU, DVL and USBL slave stations in the underwater carrier vehicle are fixed on the AUV, the USBL master station is independently fixed, the IMU, DVL, USBL master station and the USBL slave stations are synchronous, and the IMU, DVL, USBL master station and the USBL slave stations are started simultaneously.
The first pose data in this embodiment includes position data and direction data, where the first pose data is a position of the inertial measurement unit under a first preset coordinate system.
Step S20, determining a target line acceleration value and a target angular velocity value according to the first pose data and the inertial data detected by the inertial measurement unit;
the inertial data can comprise an acceleration value of an object detected by an accelerometer and angular velocity data detected by a gyroscope, and other inertial data can be added according to the requirement, so that the inertial data are not limited; the target linear acceleration value is the corrected linear acceleration of the accelerometer in the body coordinate system, the target angular velocity is the corrected angular velocity of the inertial measurement unit in the body coordinate system, and the angular velocity data and the object acceleration can be respectively error corrected according to the angular velocity bias and the acceleration bias by calculating the acceleration bias and the angular velocity bias.
Step S30, determining second pose 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 pose data is position and pose data of the inertial measurement unit in adjacent time intervals, and the second pose data can comprise a second position and a second direction.
After the target line acceleration value and the target angular velocity value are determined, the second pose data is determined according to the target angular velocity value and the target line acceleration value, so that more accurate second pose data can be obtained.
Before determining the second position and the second direction, a first position and a first direction may be determined, where the first position and the first direction are a position and a direction before a time of the second position and the second direction, the first position and the first direction are parameters under 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 represents the current time, Δt represents the time interval between adjacent times, t+1 represents the next time adjacent to the current time, a represents the acceleration of the object detected by the accelerometer in the inertial measurement unit, and g represents the gravitational acceleration.
The second position is denoted by P and the second direction is denoted by Q:
Figure BDA0002990782430000094
Figure BDA0002990782430000095
r is the first rotation matrix of the optical disc,
Figure BDA0002990782430000096
for the moment i the volume coordinate system is relative to b k Can be +.>
Figure BDA0002990782430000097
Consider as the first rotation matrix at instant i, Δt is [ t ] k ,t k+1 ]The time interval between, the linear velocity v, is directly read by the DVL.
Step S40, determining underwater positioning information according to the first pose data, the second pose 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 an ultrashort baseline sensor, the ultrashort baseline sensor is used for detecting pose correction data, and the pose correction data is used for determining the second residual parameter.
After the first pose data and the second pose data are obtained, determining underwater positioning information according to the first pose data, the second pose data and a preset error state cost function, wherein the underwater positioning information can comprise position information and attitude information, the attitude information comprises direction information, the preset error state cost function is a cost function related to a first residual error parameter and a second residual error parameter, the first residual error parameter is a residual error parameter corresponding to an inertial measurement unit and is used for representing measurement residual errors of the IMU, the second residual error parameter is a residual error parameter corresponding to an ultra-short baseline sensor and is used for representing measurement residual errors of the USBL, the USBL calculates the second residual error 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 pose data of the inertial measurement unit under a first preset coordinate system is determined; determining a target line acceleration value and a target angular velocity value according to the first pose data and the inertial data detected by the inertial measurement unit; determining second pose data of the inertial measurement unit under a second preset coordinate system according to the target line acceleration value and the target angular velocity value; determining underwater positioning information according to the first pose data, the second pose data and a preset error state cost function, wherein parameters of the preset error state cost function comprise a first residual error parameter corresponding to an inertial measurement unit and a second residual error parameter corresponding to an ultra-short baseline sensor, the ultra-short baseline sensor is used for detecting pose correction data, and the pose correction data are used for determining the second residual error 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 carrier 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 pose data and the second pose data, wherein the first residual value corresponds to the first residual parameter;
the first pose data includes a linear velocity of the inertial measurement unit, the second pose data includes 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, and when the first pose data is calculated, the first residual error value corresponds to the first residual error parameter, so that in order to determine the first residual error value, the following manner may 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 the angular velocity of a first degree of freedom, the second preset angular velocity is the angular velocity of a second degree of freedom, the first degree of freedom and the second degree of freedom are the degree of freedom components of the reference gravity, the first degree of freedom is different from the second degree of freedom, the first degree of freedom is perpendicular to the second degree of freedom, and the first preset angular velocity and the second preset angular velocity are the components of the angular velocity of the inertial measurement unit; the linear velocity is denoted by v and ω 1 Represents a first preset angular velocity, expressed in omega 2 Representing a second preset angular velocity, w= [ ω ] 1 ，ω 2 ] T Let χ 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;
with b 1 Representing a first degree of freedom, with b 2 Represents a second degree of freedom, P represents a second position, H represents a matrix coefficient, u= [ b ] 1 ,b 2 ]Then:
Figure BDA0002990782430000112
determining a second variable according to the first variable and the matrix coefficient;
let z denote the second variable and R denote the 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 r B Representing a first residual value, then:
Figure BDA0002990782430000114
step S42, a second residual value detected by an ultra-short baseline sensor is obtained, and the second residual value corresponds to the second residual parameter;
and when the calculation is performed, the second residual value corresponds to the second residual parameter, and the second residual value is obtained by USBL detection.
Step S43, determining the minimum value of the preset error state cost function according to the first residual value and the second residual value;
and S44, determining the underwater positioning information according to the minimum value.
The construction of the cost function of the preset error state is as follows:
Figure BDA0002990782430000121
the minima may be solved by a non-linear optimization library, which may be Ceres or otherwise. After the minimum value is calculated, the calculation formula of the first residual value is combined, and the formula is brought into a preset error state cost function, so that 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 cost function of a preset error state 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 an ultra-short baseline sensor;
in determining the gyroscope bias, the following may be used: determining the minimum value of a preset loss function according to the first rotation matrix value, a preset correction coefficient of the first rotation matrix value and a 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 ω Representation, then:
Figure BDA0002990782430000122
wherein, the liquid crystal display device comprises a liquid crystal display device,
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
Indicating the target angular velocity value, then t moment +.>
Figure BDA0002990782430000125
Is as follows:
Figure BDA0002990782430000126
wherein b ω For gyroscope bias, ω is angular velocity data, n ω Is an additional Gaussian noise, and
Figure BDA0002990782430000131
after the target angular velocity value is obtained, the target angular velocity value is integrated, and the integrated target angular velocity value is used as a second rotation matrix.
Determining a first degree of freedom and a 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; the target gravity is determined based on the first degree of freedom, the second degree of freedom, the first angular velocity, and the second angular velocity.
The first angular velocity is denoted by ω1, the second angular velocity by ω2, and the target gravity by g. The reference gravity is the gravity initially set, the modulus of the reference gravity may be 9.8, the first degree of freedom is denoted by b1, and the degree of freedom is denoted by b2, then:
u＝[b1 b2]；
g＝uw；
Figure BDA0002990782430000132
Figure BDA0002990782430000133
wherein the updating can be performed by the following method
Figure BDA0002990782430000134
Figure BDA0002990782430000135
And step S33, determining the target line 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 b a Representing acceleration bias of IMU by
Figure BDA0002990782430000136
Representing the target line acceleration value, then:
Figure BDA0002990782430000137
wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure BDA0002990782430000138
is a second preset matrix, g is the target gravity, n a Is an additional gaussian noise.
In this embodiment, the gyroscope bias value is determined according to the first direction and the first rotation matrix, the target angular velocity value of the inertial measurement unit under the second preset coordinate system is determined according to the angular velocity data and the gyroscope bias value, and the target linear acceleration value is determined according to the object acceleration, the acceleration bias value, the target gravity and the second rotation matrix, so that the acceleration is corrected, 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, including:
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 is slower than the speed of detecting data through an IMU or a USBL, so that the time for detecting the sonar information is not synchronous with the time for detecting the IMU data and the USBL data, so that the time for acquiring the underwater positioning information and the 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 is synchronous in time with the information acquired by the sonars, and the problem of low accuracy caused by the asynchronous time delay is avoided; the preset time period can be predetermined and stored according to the frequency of sonar information detection.
Step S60, obtaining sonar information detected in real time by the sonar;
the sonar information is information detected by the sonar, such as distance information.
And step S70, the target positioning information and the sonar information are sent to an image stitching device, so that the image stitching device 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 the image stitching equipment, so that the image stitching equipment can finish the sonar image stitching; the image stitching device can restore the sonar information into a gray level image of the sonar according to the intensity of the sonar echo signal.
In this embodiment, by acquiring the underwater positioning information in the preset time period, taking the underwater positioning information in the preset time period as the target positioning information, acquiring the sonar information of the real-time monitoring of the sonar, and sending the target positioning information and the sonar information to the image splicing equipment, so that the image splicing equipment performs the sonar image splicing operation according to the target positioning information and the sonar information, the underwater positioning information can be intercepted according to the actual requirement, the time is optimized, and the method is suitable for requirements of different sonars, such as positioning required by front view sonar matching, so that the time delay problem can be reduced.
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 one … …" does not exclude the presence of other like elements in a process, method, article, or system that comprises the element.
The foregoing embodiment numbers of the present invention are merely for the purpose of description, and do not represent the advantages or disadvantages of the embodiments.
From the above description of the embodiments, it will be clear to those skilled in the art that the above-described embodiment method may be implemented by means of software plus a necessary general hardware platform, but of course may also be implemented by means of hardware, but in many cases the former is a preferred embodiment. Based on such understanding, the technical solutions of the present invention may be embodied essentially or partly in the form of a software product stored in 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 foregoing description is only of the preferred embodiments of the present invention, and is not intended to limit the scope of the invention, but rather is intended to cover any equivalents of the structures or equivalent processes disclosed herein or in the alternative, which may be employed directly or indirectly in other related arts.

Claims (8)

1. An underwater positioning method, characterized in that the underwater positioning method is applied to an underwater carrier vehicle, the underwater carrier vehicle comprises an inertial measurement unit and an ultra-short baseline sensor, the underwater positioning method comprises:
determining first pose data of the inertial measurement unit under a first preset coordinate system;
determining a target line acceleration value and a target angular velocity value according to the first pose data and the inertial data detected by the inertial measurement unit;
determining second pose 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 pose data, the second pose data and a preset error state cost function, wherein parameters of the preset error state cost function comprise a first residual error parameter corresponding to the inertial measurement unit and a second residual error parameter corresponding to an ultra-short baseline sensor, the ultra-short baseline sensor is used for detecting pose correction data, the pose correction data are used for determining the second residual error parameter, the first pose data comprise linear speed of the inertial measurement unit, the second pose data comprise a second position and a second direction, and the first pose data and the second pose data are used according to the first pose data and the second pose data;
the step of determining underwater positioning information according to the first pose data, the second pose data and a preset error state cost function comprises the following steps:
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 free degree components of reference gravity, the first degree of freedom is different from the second degree of freedom, the first degree of freedom is perpendicular to the second degree of freedom, 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;
determining a first residual value of the inertial measurement unit according to the first variable and the second variable, wherein the first residual value corresponds to the first residual parameter;
acquiring a second residual value detected by an ultra-short baseline sensor, wherein the second residual value corresponds to the second residual parameter;
determining the 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.
2. The underwater positioning method as claimed in claim 1, 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 pose data includes a first direction, and the step of determining a target line acceleration value and a target angular velocity value from the first pose 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 line acceleration value according to the object acceleration, the acceleration bias value, the target gravity and the second rotation matrix.
3. The underwater positioning method as claimed in claim 2, 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 gyroscope offset value.
4. The underwater positioning method as claimed in claim 2, wherein the step of determining the linear acceleration of the inertial measurement unit under a second preset coordinate system based on 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;
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.
5. The underwater positioning method as claimed in claim 2, wherein between the steps of determining a target angular velocity value of the inertial measurement unit under a second preset coordinate system based on the angular velocity data and the gyro bias value and 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 value, and taking the integrated target angular velocity value as the second rotation matrix.
6. The underwater positioning method as claimed in any one of claims 1-5, wherein after the step of determining underwater positioning information from the first pose data, the second pose data and a preset error state cost function, further comprises:
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 in real time by using a sonar;
and sending the target positioning information and the sonar information to an image stitching device, so that the image stitching device performs sonar image stitching operation according to the target positioning information and the sonar information.
7. 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 stored thereon an underwater positioning program which when executed by the processor performs the steps of the underwater positioning method of any of claims 1-6.
8. A computer readable storage medium, characterized in that it has stored thereon an underwater positioning program, which when executed by a processor implements the steps of the underwater positioning method according to any of the claims 1-6.
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 CN113075665A (en) 2021-07-06
CN113075665B true 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)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113218386B (en) * 2021-07-08 2021-10-08 深之蓝海洋科技股份有限公司 Method and device for high-precision navigation of robot in liquid building

Citations (7)

* 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

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109253726B (en) * 2018-06-22 2020-05-05 东南大学 Underwater glider navigation positioning system and floating precision correction method

Patent Citations (7)

* 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

Non-Patent Citations (1)

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

Also Published As

Publication number Publication date
CN113075665A (en) 2021-07-06

Similar Documents

Publication Publication Date Title
US11789455B2 (en) Control of autonomous vehicle based on fusion of pose information and visual data
CN111065043B (en) System and method for fusion positioning of vehicles in tunnel based on vehicle-road communication
US11079236B2 (en) Estimation of an individual position
JP5074950B2 (en) Navigation equipment
CN112835085B (en) Method and device for determining vehicle position
CN111854740B (en) Inertial navigation system capable of dead reckoning in a vehicle
CN108627152B (en) Navigation method of micro unmanned aerial vehicle based on multi-sensor data fusion
JP7272910B2 (en) In-vehicle device, state estimation method and program
US20230366680A1 (en) Initialization method, device, medium and electronic equipment of integrated navigation system
CN114179825B (en) Method for obtaining confidence of measurement value through multi-sensor fusion and automatic driving vehicle
KR20190003916A (en) Inertial sensor unit caliberation method for navigation
JP6248559B2 (en) Vehicle trajectory calculation device
CN113075665B (en) Underwater positioning method, underwater carrier vehicle and computer readable storage medium
JP2016218015A (en) On-vehicle sensor correction device, self-position estimation device, and program
JP2016017796A (en) Device and method for measuring vehicle position
JP2016170124A (en) Moving body positioning device and moving body positioning method
JP3095189B2 (en) Navigation device
CN116576849A (en) Vehicle fusion positioning method and system based on GMM assistance
JP6083741B2 (en) GPS reliability determination device, position detection device, and navigation device
CN112835086B (en) Method and device for determining vehicle position
KR20230048324A (en) Extended dead reckoning accuracy
CN116380119A (en) Calibration method, device and system for integrated navigation
CN113093239A (en) Method, corresponding system and program product for providing navigation information
JP2009143413A (en) Driving supporting device
JP6413816B2 (en) Random driving judgment device

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