CN117870687A - AUV (autonomous Underwater vehicle) underwater integrated navigation positioning system and implementation method - Google Patents
AUV (autonomous Underwater vehicle) underwater integrated navigation positioning system and implementation method Download PDFInfo
- Publication number
- CN117870687A CN117870687A CN202410044329.1A CN202410044329A CN117870687A CN 117870687 A CN117870687 A CN 117870687A CN 202410044329 A CN202410044329 A CN 202410044329A CN 117870687 A CN117870687 A CN 117870687A
- Authority
- CN
- China
- Prior art keywords
- auv
- navigation
- information
- underwater
- longitude
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 17
- 238000004364 calculation method Methods 0.000 claims abstract description 36
- 239000011159 matrix material Substances 0.000 claims description 31
- 230000004927 fusion Effects 0.000 claims description 16
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims description 14
- 238000004891 communication Methods 0.000 claims description 12
- 238000012937 correction Methods 0.000 claims description 12
- 230000004044 response Effects 0.000 claims description 8
- 230000005236 sound signal Effects 0.000 claims description 7
- 238000005259 measurement Methods 0.000 claims description 6
- 238000012544 monitoring process Methods 0.000 claims description 6
- 150000003839 salts Chemical class 0.000 claims description 5
- 230000005484 gravity Effects 0.000 claims description 3
- 230000001360 synchronised effect Effects 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 238000005516 engineering process Methods 0.000 abstract description 5
- 230000007547 defect Effects 0.000 abstract description 4
- 238000001914 filtration Methods 0.000 abstract description 4
- 230000002194 synthesizing effect Effects 0.000 abstract description 2
- 230000008901 benefit Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 229910052500 inorganic mineral Inorganic materials 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000011835 investigation Methods 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 239000011707 mineral Substances 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000005096 rolling process Methods 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/203—Specially adapted for sailing ships
-
- 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/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
- G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
-
- 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
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/393—Trajectory determination or predictive tracking, e.g. Kalman filtering
-
- 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
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/40—Correcting position, velocity or attitude
-
- 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
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- General Physics & Mathematics (AREA)
- Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Life Sciences & Earth Sciences (AREA)
- Geology (AREA)
- General Life Sciences & Earth Sciences (AREA)
- Environmental & Geological Engineering (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention provides an AUV (autonomous Underwater vehicle) underwater integrated navigation positioning system and an AUV underwater integrated navigation positioning method, wherein a plurality of sensors and an ultra-short baseline navigation system are introduced into the AUV integrated navigation to assist DR (digital navigation) to perform navigation positioning; the Kalman filtering is adopted to integrate a plurality of sub navigation information for the ultra-short baseline subsystem and the navigation position calculation subsystem, so that the AUV navigation information is not affected by the environment, and the defect that the error of the DR navigation system continues along with time and continuously accumulates to fail the precision requirement is overcome. Because the AUV receives the position information of the AUV at the last moment when the position information obtained by the acoustic signal from the ultra-short baseline subsystem is the position information of the AUV, the invention can calculate the distance travelled by the AUV in the acoustic signal propagation time period, and the real-time position information of the AUV can be obtained by synthesizing the position information obtained from the short baseline subsystem, thereby having strong superiority compared with the existing integrated navigation technology.
Description
Technical Field
The invention relates to the technical field of underwater navigation, in particular to an AUV (autonomous Underwater vehicle) underwater integrated navigation positioning system and an implementation method.
Background
The 21 st century is the century of the ocean, which has become one of the main battlefields of the fierce angles of various countries, and AUV (Autonomous Underwater Vehicle) has become the key point of research of various countries due to the characteristics of small volume, wide moving range, high concealment and the like. AUV can be applied to marine environment investigation, marine mineral resource exploration, emergency collection, construction and maintenance of marine engineering projects and the like in civil field, and can be applied to underwater mine drainage, information reconnaissance, marine data detection, communication relay and the like in military field.
The development of intelligent underwater robots has a plurality of technical points and difficulties, and the underwater navigation technology is one of the technical points and difficulties. In order to ensure that the AUV can successfully complete related tasks under water, the AUV navigation system is required to have long-term autonomous navigation positioning and return capability under water. The most basic and economical method of underwater navigation is Dead Reckoning (DR) at present, because of the difficulty in receiving radio signals underwater and the limitations of using GPS positioning systems. The Cotter has defined for dead reckoning navigation, namely: "from a given initial position, the navigation process of the position information at the next moment can be deduced according to the navigation speed, navigation direction and navigation time of the moving body at the point. The underwater robot only needs to be provided with a depth gauge, a speedometer, an attitude sensor and the like, and under the premise of giving initial navigation position information, the underwater navigation system with certain precision can be constructed by completing the calculation through the calculation system, but the dead reckoning navigation precision is limited, and accumulated errors exist.
Because the acoustic wave is far away from underwater transmission and has small attenuation loss, the underwater acoustic positioning technology is fully developed, the most widely applied underwater acoustic positioning system is an Ultra-Short Base Line (USBL) which does not need to lay a matrix on the seabed, a smaller acoustic matrix is arranged on a carrier, and high positioning precision can be obtained by accurately calibrating installation errors and offset errors, so that the acoustic positioning system can be used for correcting dead reckoning errors.
Disclosure of Invention
The purpose of the invention is that: the defect that the AUV navigation system is easily affected by the environment, and the navigation positioning errors of the navigation position calculation subsystem are continuously accumulated along with the time and cannot meet the precision requirement is overcome.
The technical scheme of the invention provides an AUV (autonomous Underwater vehicle) underwater integrated navigation positioning system which is characterized by comprising a navigation position calculation subsystem, an ultra-short baseline subsystem and a data fusion unit;
the navigation position calculation subsystem is arranged in the AUV and comprises an underwater control computer, an altimeter, a magnetic compass, a temperature and salt depth meter and a DVL;
the altimeter is used for measuring the height information of the AUV, the magnetic compass is used for measuring the three-dimensional attitude information of the AUV, and the warm salt depth meter and the depth meter are used for measuring the depth information of the AUV; the DVL is used for measuring the three-dimensional speed of the AUV under the carrier coordinate system, then calculating the three-dimensional speed information of the AUV under the geodetic coordinate system through a coordinate transformation matrix, and then calculating the longitude and latitude information of the AUV under the geodetic coordinate system by an underwater control unit through a dead reckoning algorithm;
the ultra-short baseline subsystem comprises an underwater matrix, an attitude sensor, a water surface monitoring computer, a GPS signal receiving device, an acoustic communication machine and a transponder;
the attitude sensor and the GPS signal receiving device are arranged on the mother ship and are used for acquiring the coordinate position of the mother ship under the geodetic coordinate system; the attitude sensor and the satellite positioning device are connected with a water surface control unit (a water surface monitoring computer); the underwater matrix is fixed on the mother ship, the transponder is arranged on the AUV and used for receiving and sending out acoustic signals to obtain the position coordinates of the AUV relative to the mother ship, the position coordinates are converted into longitude and latitude information of the AUV relative to the mother ship under a geodetic coordinate system, and the longitude and latitude information of the AUV relative to the mother ship is transmitted to the AUV through the acoustic communication machine;
the data fusion unit is used for fusing the longitude and latitude information of the AUV obtained by the ultra-short baseline subsystem and the longitude and latitude information of the AUV obtained by the navigation position calculation subsystem, outputting correction amounts of the longitude and latitude information after error correction to correct the AUV underwater integrated navigation positioning system, and therefore AUV navigation positioning is achieved.
Preferably, the three-dimensional attitude information includes a heading angle, a roll angle, and a pitch angle.
Preferably, the underwater matrix and the transponder are both provided with an acoustic communication function.
The embodiment of the invention also provides a realization method of the AUV underwater integrated navigation positioning system, which is characterized by being applied to the AUV underwater integrated navigation positioning system, and comprising the following steps:
step 1: determining a coordinate system of the AUV underwater integrated navigation positioning system, wherein the coordinate system comprises a fixed coordinate system fixed on the ground and a carrier coordinate system with an origin consistent with the gravity center of the underwater robot;
step 2: initializing the AUV underwater integrated navigation positioning system, so that the underwater matrix, the transponder and the navigation position calculation subsystem are kept synchronous in time and space, and longitude and latitude information of the position where the AUV is located is obtained as initial coordinates; the navigation position calculation subsystem calculates longitude and latitude information of the AUV under a geodetic coordinate system;
step 3: the ultra-short baseline subsystem sends out an acoustic signal, records the transmitting time, and the acoustic signal comprises the initial coordinate of the AUV at the last time;
step 4: acquiring three-dimensional speed information and three-dimensional attitude information from the navigation position calculation subsystem and longitude and latitude information of the AUV under a geodetic coordinate system, and calculating to obtain a navigation position running track of the AUV;
step 5: the transponder determining whether an acoustic signal transmitted by the ultra-short baseline subsystem is received; if the acoustic signal is not received, the AUV runs according to the space-level running track obtained by the space-level calculation subsystem; if the acoustic signal is received, performing motion compensation on the initial coordinates in the acoustic signal to obtain real-time longitude and latitude information of the AUV, sending a response signal to the ultra-short baseline subsystem, and recording the moment;
step 6: the ultra-short baseline subsystem receives the response signal, obtains the time difference and the phase difference between the underwater array and the transponder, and calculates the longitude and latitude information of the AUV according to the propagation speed of the acoustic signal in water;
step 7: and (3) transmitting the longitude and latitude information of the AUV obtained by the ultra-short baseline subsystem in the step (6) and the longitude and latitude information of the AUV under the geodetic coordinate system obtained by the navigation position calculation subsystem in the step (2) to a Kalman filter in a data fusion unit for optimal information fusion, carrying out error correction in an output correction mode, and repeating the steps (3) to (7) until the task is finished, thereby realizing navigation positioning of the AUV.
Preferably, the fixed coordinate system points in north, east and ground directions.
Preferably, the carrier coordinate system takes the advancing direction of the robot as an x-axis, the translation direction as a y-axis, the submerging direction as a z-axis, the pitching direction q around the y-axis, the rolling direction p around the x-axis and the swaying direction r around the z-axis.
Preferably, the deriving the longitude and latitude information of the location of the AUV as the initial coordinates includes the following steps:
setting the time of the ultra-short baseline subsystem and the time of the navigation position calculation subsystem from the moment 0, and determining the initial coordinate of the mother ship under the geodetic coordinate system;
determining the position information of the AUV relative to the mother ship by taking the initial coordinate of the mother ship as the origin of a fixed coordinate system;
and according to the position information of the AUV relative to the mother ship, the longitude and latitude information of the position of the AUV is converted to be used as initial coordinates.
Preferably, the underwater array comprises a transmitting array and a receiving array; in said step 3, the transmitting matrix emits an acoustic signal; in the step 6, the receiving matrix receives the response signal.
Preferably, the motion compensation comprises the steps of:
obtaining position information of the AUV at the last (K-1) moment and a time difference delta T between the sound signal sent at the last moment and the received signal at the moment from the received sound signal;
obtaining three-dimensional speed information and three-dimensional attitude information from the step 4, and longitude and latitude information of the AUV under a geodetic coordinate system;
the real-time position of the AUV is calculated according to the following calculation formula:
wherein (X) k-1 ，Y k-1 ) The coordinate position of the robot K-1 under a fixed coordinate system is set; (X) k ，Y k ) The coordinate position of the robot K at the moment under a fixed coordinate system; v (V) ξ 、V η 、V ζ The velocity components of the robot in the x, y and z directions under a fixed coordinate system are respectively shown; v (V) x 、V y 、V z The velocity components of the robot in the x, y and z directions under the carrier coordinate system are respectively shown; delta T is the time difference between time K-1 and time K, L 0 、λ 0 Is the longitude and latitude of the initial position of the mother ship, R is the earth radius, L k 、λ k The altitude information of the AUV is directly measured by an altimeter for the longitude and latitude of the AUV in the geodetic coordinate system at the moment of the robot K.
Preferably, the optimal information fusion specifically includes the following steps:
the Kalman filter takes heading, speed and position errors as state estimators, longitude and latitude information of the AUV measured by the ultra-short baseline subsystem in step 6 and longitude and latitude information of the AUV under a geodetic coordinate system obtained by the dead reckoning subsystem in step 2 are taken as measurement information, and the Kalman filter estimates the error amount of the longitude and latitude information of the AUV and feeds back the error amount to the dead reckoning system to correct navigation results.
The invention provides an AUV (autonomous Underwater vehicle) underwater integrated navigation positioning system and an AUV underwater integrated navigation positioning method, wherein a plurality of sensors and an ultra-short baseline navigation system are introduced into the AUV integrated navigation to assist DR (digital navigation) to perform navigation positioning; the Kalman filtering is adopted to integrate a plurality of sub navigation information for the ultra-short baseline subsystem and the navigation position calculation subsystem, so that the AUV navigation information is not affected by the environment, and the defect that the error of the DR navigation system continues along with time and continuously accumulates to fail the precision requirement is overcome. Because the AUV receives the position information of the AUV at the last moment when the position information obtained by the acoustic signal from the ultra-short baseline subsystem is the position information of the AUV, the invention can calculate the distance travelled by the AUV in the acoustic signal propagation time period, and the real-time position information of the AUV can be obtained by synthesizing the position information obtained from the short baseline subsystem, thereby having strong superiority compared with the existing integrated navigation technology.
Drawings
FIG. 1 is a diagram of an integrated navigation system according to an embodiment of the present invention;
FIG. 2 is a graph of an embodiment of the present invention
Fig. 3 is a flowchart of an implementation method of an embodiment provided by the present invention.
Detailed Description
The invention will be further illustrated with reference to specific examples. It is to be understood that these examples are illustrative of the present invention and are not intended to limit the scope of the present invention. Further, it is understood that various changes and modifications may be made by those skilled in the art after reading the teachings of the present invention, and such equivalents are intended to fall within the scope of the claims appended hereto.
As shown in FIG. 1, the embodiment of the invention provides an AUV underwater integrated navigation positioning system based on DR/USBL, which comprises a navigation position calculation subsystem, an ultra-short baseline subsystem and a data fusion unit.
The navigation position calculation subsystem is arranged in the AUV and consists of an underwater control computer, an altimeter, a depth meter, a magnetic compass, a temperature and salt depth meter (Conductivity Temperature Depth, CTD) and a Doppler log (Doppler velocity log, DVL). The altimeter is used for measuring AUV altitude information; magnetic compass is used to measure three-dimensional attitude information (i.e., heading angle) of AUVRoll angle γ and pitch angle θ), wherein pitch angle θ is the pitch angle in the carrier coordinate system; CTD and depth gauge measurement AUV depth information h Measuring The method comprises the steps of carrying out a first treatment on the surface of the DVL is used for measuring three-dimensional speed information V of AUV in carrier coordinate system b Then via a coordinate transformation matrixThree-dimensional speed V of AUV in geodetic coordinate system is calculated n And then the underwater control unit adopts a dead reckoning algorithm to calculate the longitude and latitude information of the AUV in the geodetic coordinate system.
The ultra-short baseline subsystem consists of an underwater matrix (comprising a transmitting matrix and a receiving matrix with acoustic communication), an attitude sensor, a water surface monitoring computer, a GPS signal receiving device, an acoustic communication machine and a transponder with acoustic communication. The attitude sensor and the GPS signal receiving device are arranged on the mother ship, so that the coordinate position of the mother ship under the geodetic coordinate system is obtained; the attitude sensor and the satellite positioning device are connected with a water surface control unit (a water surface monitoring computer); the underwater matrix is fixed on the mother ship, the transponder is arranged on the AUV and used for receiving and sending out sound signals, so that the position coordinate of the AUV relative to the mother ship is obtained, the position coordinate is converted into longitude and latitude information under a geodetic coordinate system, and the position information is transmitted to the AUV through the sound communication machine.
The data fusion unit is used for fusing the longitude and latitude information of the AUV obtained by the ultra-short baseline subsystem and the longitude and latitude information of the AUV obtained by the navigation position calculation subsystem, and then outputting the correction quantity of the longitude and latitude to correct the integrated navigation system, so that the long-time and high-precision navigation positioning of the AUV is realized.
If the integrated navigation system can work normally, namely the transponder can receive the acoustic signal from the USBL underwater matrix, the AUV position information measured by the ultra-short baseline subsystem and the AUV position information calculated by the navigation position calculation subsystem enter the data fusion unit to carry out error correction on the integrated navigation system in an output correction mode, and if the integrated navigation system cannot receive the acoustic signal, the integrated navigation system continues to navigate by dead reckoning, so that the advantage of higher accuracy in a short time is exerted.
As shown in fig. 2, the invention further provides an implementation method of the AUV underwater integrated navigation positioning system based on DR/USBL, which comprises the following steps:
(1) And determining a coordinate system of the integrated navigation system, wherein the coordinate system comprises a fixed coordinate system and a carrier coordinate system. As shown in FIG. 3, the fixed coordinate system E- ζ ηζ is fixed on the ground, and points in the north, east and ground directions, which is the fixed coordinate system. The origin of the carrier coordinate system O-xyz is consistent with the gravity center of the underwater robot, and the carrier coordinate system O-xyz moves along with the robot and is a satellite coordinate system (carrier coordinate system). The forward direction of the robot is taken as the x axis, the translation direction is taken as the y axis, and the submerging direction is taken as the z axis. Pitch direction q is about the y-axis, roll direction p is about the x-axis, and yaw direction r is about the z-axis.
(2) The integrated navigation system is initialized to keep the ultra-short baseline array (underwater array), the transponder carried by the AUV and the dead reckoning system synchronized in time and space. The time of setting the ultra-short baseline subsystem and the navigation position calculation subsystem is from the moment 0, the initial coordinate of the mother ship under the geodetic coordinate system is determined, the position of the mother ship is the origin of the fixed coordinate system, the position information of the AUV relative to the mother ship is measured, and finally the longitude and latitude of the position of the AUV are calculated as the initial coordinate. The navigation position calculation subsystem calculates longitude and latitude information of the AUV in a geodetic coordinate system.
(3) The transmitting matrix of the ultra-short baseline subsystem transmits an acoustic signal, the acoustic signal comprises the position information of the AUV at the last moment, namely an initial coordinate (communicated through an acoustic communication machine), the initial position is the initial position for the first time, and the transmitting moment is recorded.
(4) And acquiring three-dimensional speed information and attitude angle information from the navigation position calculation subsystem and longitude and latitude coordinate information of the AUV at the initial moment, and calculating to obtain the navigation position running track of the AUV.
(5) The transponder on the AUV judges whether the acoustic signal transmitted by the transmitting matrix is received or not, if not, the AUV runs according to the navigation position running track obtained by the navigation position calculation subsystem; if the acoustic signal is received, motion compensation is performed on the initial coordinates in the acoustic signal, real-time position information (real-time longitude and latitude information) of the AUV is obtained, a response signal is immediately sent to the receiving matrix, and the moment is recorded, wherein the acoustic signal comprises the position information and the time information at the moment.
The motion compensation specifically comprises the following steps:
obtaining position information of the AUV at the last (K-1) moment and a time difference delta T between the sound signal sent at the last moment and the received signal at the moment from the received sound signal;
obtaining three-dimensional speed information and three-dimensional attitude information from the step 4, and longitude and latitude information of the AUV under a geodetic coordinate system;
the real-time position of the AUV is calculated according to the following calculation formula:
wherein (X) k-1 ，Y k-1 ) The coordinate position of the robot K-1 under a fixed coordinate system is set; (X) k ，Y k ) The coordinate position of the robot K at the moment under a fixed coordinate system; v (V) ξ 、V η 、V ζ The velocity components of the robot in the x, y and z directions under a fixed coordinate system are respectively shown; v (V) x 、V y 、V z The velocity components of the robot in the x, y and z directions under the carrier coordinate system are respectively shown; delta T is the time difference between time K-1 and time K, L 0 、λ 0 Is the longitude and latitude of the initial position of the mother ship, R is the earth radius, L k 、λ k The altitude information of the AUV is directly measured by an altimeter for the longitude and latitude of the AUV in the geodetic coordinate system at the moment of the robot K.
(6) And after receiving the response signal, the receiving matrix of the ultra-short baseline subsystem obtains the time difference and the phase difference between the underwater matrix and the transponder, and then calculates the position information of the AUV (longitude and latitude information of the AUV) according to the propagation speed of the acoustic signal in water.
(7) And (3) transmitting the longitude and latitude information (step 6) of the AUV obtained by the ultra-short baseline subsystem and the longitude and latitude information (step 2) of the AUV under the geodetic coordinate system obtained by the navigation position calculation subsystem to a Kalman filter positioned in the data fusion unit for optimal information fusion, correcting errors of the integrated navigation system by adopting an output correction mode, and repeating the steps (3), 4, 5, 6, 7) until the task is finished, thereby realizing long-time and high-precision navigation positioning of the AUV.
The optimal information fusion specifically comprises the following steps:
and taking the dead reckoning system as a reference system, and fusing data by adopting Kalman filtering to realize AUV underwater high-precision navigation positioning. The Kalman filter takes the heading, speed and position errors of the Kalman filter as state estimation values, the position information measured by the USBL system is taken as measurement information, the Kalman filter estimates the error amount of the position information, and then the error amount is fed back to the dead reckoning system to correct the navigation result, so that high-precision navigation positioning is realized.
The state equation of the integrated navigation system is described as:
X k ＝φ k,k-1 X k-1 +Γ k-1 W k-1 (2)
wherein X is k 、X k-1 The states at the time of k and k-1 respectively; w (W) k-1 A system noise sequence at the moment k-1; Γ -shaped structure k-1 A noise driving array for a k-1 system; phi (phi) k,k-1 Is a transition matrix from time k-1 to time k.
For X k The measurement of (2) satisfies the linear relation, and the measurement equation is:
wherein Z is k For the observed quantity at time k, L DR 、λ DR For the obtained AUV position information, L, of the navigation position calculation subsystem USBL 、λ USBL AUV position information H obtained for ultra-short baseline subsystem k For measuring array, V k To measure noise sequences.
According to the AUV underwater integrated navigation positioning system and method based on DR/USBL, provided by the embodiment of the invention, a plurality of sensors and an ultra-short baseline navigation system are introduced into the AUV integrated navigation so as to assist DR to perform navigation positioning; the system integrates a plurality of sub navigation information, so that the AUV navigation information is not affected by the environment, and the defect that the error of the DR navigation system continues along with time and continuously accumulates to not meet the precision requirement is overcome.
Since the position information obtained by the AUV receiving the acoustic signal from the ultra-short baseline subsystem is the position information of the AUV at the last moment, the position information received by the AUV is regarded as the position information of the AUV at the moment in the traditional method and is used for correcting dead reckoning, and finally larger errors are caused.
According to the AUV underwater integrated navigation positioning system and the implementation method based on DR/USBL, provided by the embodiment of the invention, the navigation information of the ultra-short baseline subsystem and the navigation information of the dead-reckoning subsystem are fused by adopting Kalman filtering, when the ultra-short baseline system works normally, the dead-reckoning subsystem is corrected by the ultra-short baseline system, and when the ultra-short baseline system works abnormally or the AUV cannot receive an acoustic signal, the navigation is continued by adopting dead reckoning, so that the advantage of higher precision in a short time is exerted.
Compared with the common SINS/USBL integrated navigation, the AUV underwater integrated navigation positioning method based on the DR/USBL provided by the embodiment of the invention is more economical, has strong replaceability and portability, has higher accuracy and higher practicability under water compared with the DR/GPS integrated navigation, and has strong superiority compared with the existing integrated navigation technology.
It should be understood that the AUV underwater integrated navigation positioning system and the implementation method are equally applicable to other underwater robots and various submarines. The foregoing description is only of the preferred embodiments of the present invention and is not intended to limit the scope of the present invention. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
Claims (10)
1. An AUV underwater integrated navigation positioning system is characterized by comprising a navigation position calculation subsystem, an ultra-short baseline subsystem and a data fusion unit;
the navigation position calculation subsystem is arranged in the AUV and comprises an underwater control computer, an altimeter, a depth meter, a magnetic compass, a temperature and salt depth meter and a DVL;
the altimeter is used for measuring the height information of the AUV, the magnetic compass is used for measuring the three-dimensional attitude information of the AUV, and the warm salt depth meter and the depth meter are used for measuring the depth information of the AUV; the DVL is used for measuring the three-dimensional speed of the AUV under the carrier coordinate system, then calculating the three-dimensional speed information of the AUV under the geodetic coordinate system through a coordinate transformation matrix, and then calculating the longitude and latitude information of the AUV under the geodetic coordinate system by an underwater control unit through a dead reckoning algorithm;
the ultra-short baseline subsystem comprises an underwater matrix, an attitude sensor, a water surface monitoring computer, a GPS signal receiving device, an acoustic communication machine and a transponder;
the attitude sensor and the GPS signal receiving device are arranged on the mother ship and are used for acquiring the coordinate position of the mother ship under the geodetic coordinate system; the attitude sensor and the satellite positioning device are connected with a water surface monitoring computer; the underwater matrix is fixed on the mother ship, the transponder is arranged on the AUV and used for receiving and sending out acoustic signals to obtain the position coordinates of the AUV relative to the mother ship, the position coordinates are converted into AUV longitude and latitude information under a geodetic coordinate system, and the AUV longitude and latitude information relative to the mother ship is transmitted to the AUV through the acoustic communication machine;
the data fusion unit is used for fusing the longitude and latitude information of the AUV obtained by the ultra-short baseline subsystem and the longitude and latitude information of the AUV obtained by the navigation position calculation subsystem, outputting correction amounts of the longitude and latitude information after error correction to correct the AUV underwater integrated navigation positioning system, and therefore AUV navigation positioning is achieved.
2. The AUV underwater integrated navigational positioning system of claim 1 wherein the three-dimensional attitude information includes heading angle, roll angle and pitch angle.
3. An AUV underwater integrated navigation and positioning system as claimed in claim 1, wherein the underwater matrix and the transponder are provided with acoustic communication functions.
4. The method for realizing the AUV underwater integrated navigation positioning system is characterized by being applied to the AUV underwater integrated navigation positioning system in claim 1 and comprising the following steps:
step 1: determining a coordinate system of the AUV underwater integrated navigation positioning system, wherein the coordinate system comprises a fixed coordinate system fixed on the ground and a carrier coordinate system with an origin consistent with the gravity center of the underwater robot;
step 2: initializing the AUV underwater integrated navigation positioning system, so that the underwater matrix, the transponder and the navigation position calculation subsystem are kept synchronous in time and space, and longitude and latitude information of the position where the AUV is located is obtained as initial coordinates; the navigation position calculation subsystem calculates longitude and latitude information of the AUV under a geodetic coordinate system;
step 3: the ultra-short baseline subsystem sends out an acoustic signal, records the transmitting time, and the acoustic signal comprises the initial coordinate of the AUV at the last time;
step 4: acquiring three-dimensional speed information and three-dimensional attitude information from the navigation position calculation subsystem and longitude and latitude information of the AUV under a geodetic coordinate system, and calculating to obtain a navigation position running track of the AUV;
step 5: the transponder determining whether an acoustic signal transmitted by the ultra-short baseline subsystem is received; if the acoustic signal is not received, the AUV runs according to the space-level running track obtained by the space-level calculation subsystem; if the acoustic signal is received, performing motion compensation on the initial coordinates in the acoustic signal to obtain real-time longitude and latitude information of the AUV, sending a response signal to the ultra-short baseline subsystem, and recording the moment;
step 6: the ultra-short baseline subsystem receives the response signal, obtains the time difference and the phase difference between the underwater array and the transponder, and calculates the longitude and latitude information of the AUV according to the propagation speed of the acoustic signal in water;
step 7: and (3) transmitting the longitude and latitude information of the AUV obtained by the ultra-short baseline subsystem in the step (6) and the longitude and latitude information of the AUV under the geodetic coordinate system obtained by the navigation position calculation subsystem in the step (2) to a Kalman filter in a data fusion unit for optimal information fusion, carrying out error correction in an output correction mode, and repeating the steps (3) to (7) until the task is finished, thereby realizing navigation positioning of the AUV.
5. The AUV underwater integrated navigational positioning system of claim 4 wherein said fixed coordinate system points in north, east and ground directions.
6. The AUV underwater integrated navigation and positioning system of claim 5, wherein the carrier coordinate system is characterized by an advancing direction of the robot being an x-axis, a translating direction being a y-axis, a submerging direction being a z-axis, a pitch direction q being about the y-axis, a roll direction p being about the x-axis, and a yaw direction r being about the z-axis.
7. The AUV underwater integrated navigation and positioning system of claim 5, wherein the deriving longitude and latitude information of the location of the AUV as initial coordinates includes the steps of:
setting the time of the ultra-short baseline subsystem and the time of the navigation position calculation subsystem from the moment 0, and determining the initial coordinate of the mother ship under the geodetic coordinate system;
determining the position information of the AUV relative to the mother ship by taking the initial coordinate of the mother ship as the origin of a fixed coordinate system;
and according to the position information of the AUV relative to the mother ship, the longitude and latitude information of the position of the AUV is converted to be used as initial coordinates.
8. The AUV underwater integrated navigational positioning system of claim 7 wherein said underwater matrix includes a transmit matrix and a receive matrix; in said step 3, the transmitting matrix emits an acoustic signal; in the step 6, the receiving matrix receives the response signal.
9. The AUV underwater integrated navigation and positioning system of claim 1, wherein the motion compensation includes the steps of:
obtaining position information of the AUV at the last moment (K-1) and a time difference delta T between the sound signal sent at the last moment and the received signal at the moment from the received sound signal;
obtaining three-dimensional speed information and three-dimensional attitude information from the step 4, and longitude and latitude information of the AUV under a geodetic coordinate system;
the real-time position of the AUV is calculated according to the following calculation formula:
wherein (X) k-1 ，Y k-1 ) The coordinate position of the robot K-1 under a fixed coordinate system is set; (X) k ，Y k ) The coordinate position of the robot K at the moment under a fixed coordinate system; v (V) ξ 、V η 、V ζ The velocity components of the robot in the x, y and z directions under a fixed coordinate system are respectively shown; v (V) x 、V y 、V z The velocity components of the robot in the x, y and z directions under the carrier coordinate system are respectively shown; delta T is the moment K-1Time difference of K moment, L 0 、λ 0 Is the longitude and latitude of the initial position of the mother ship, R is the earth radius, L k 、λ k The altitude information of the AUV is directly measured by an altimeter for the longitude and latitude of the AUV in the geodetic coordinate system at the moment of the robot K.
10. The AUV underwater integrated navigation and positioning system of claim 1, wherein the optimal information fusion specifically includes the steps of:
the Kalman filter takes heading, speed and position errors as state estimators, longitude and latitude information of the AUV measured by the ultra-short baseline subsystem in step 6 and longitude and latitude information of the AUV under a geodetic coordinate system obtained by the dead reckoning subsystem in step 2 are taken as measurement information, and the Kalman filter estimates the error amount of the longitude and latitude information of the AUV and feeds back the error amount to the dead reckoning system to correct navigation results.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410044329.1A CN117870687A (en) | 2024-01-11 | 2024-01-11 | AUV (autonomous Underwater vehicle) underwater integrated navigation positioning system and implementation method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410044329.1A CN117870687A (en) | 2024-01-11 | 2024-01-11 | AUV (autonomous Underwater vehicle) underwater integrated navigation positioning system and implementation method |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117870687A true CN117870687A (en) | 2024-04-12 |
Family
ID=90582598
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410044329.1A Pending CN117870687A (en) | 2024-01-11 | 2024-01-11 | AUV (autonomous Underwater vehicle) underwater integrated navigation positioning system and implementation method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117870687A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118330701A (en) * | 2024-06-13 | 2024-07-12 | 青岛哈尔滨工程大学创新发展中心 | Underwater positioning method of underwater vehicle capable of being separated from main and sub |
-
2024
- 2024-01-11 CN CN202410044329.1A patent/CN117870687A/en active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN118330701A (en) * | 2024-06-13 | 2024-07-12 | 青岛哈尔滨工程大学创新发展中心 | Underwater positioning method of underwater vehicle capable of being separated from main and sub |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109782323B (en) | Navigation positioning and calibrating method for autonomous underwater vehicle in deep sea | |
| CN103744098B (en) | AUV integrated navigation systems based on SINS/DVL/GPS | |
| US9372255B2 (en) | Determining a position of a submersible vehicle within a body of water | |
| CN109737956B (en) | SINS/USBL phase difference tight combination navigation positioning method based on double transponders | |
| CN101900558B (en) | Combined navigation method of integrated sonar micro navigation autonomous underwater robot | |
| CN104316045A (en) | AUV (autonomous underwater vehicle) interactive auxiliary positioning system and AUV interactive auxiliary positioning method based on SINS (strapdown inertial navigation system)/LBL (long base line) | |
| CN111595348B (en) | Master-slave mode cooperative positioning method of autonomous underwater vehicle combined navigation system | |
| Eustice et al. | Recent advances in synchronous-clock one-way-travel-time acoustic navigation | |
| CN1325932C (en) | Combined navigation positioning method of manned submersible | |
| CN103697910B (en) | The correction method of autonomous underwater aircraft Doppler log installation error | |
| KR100906362B1 (en) | Underwater Navigation System for a Platoon of Multiple Unmanned Underwater Vehicles Using Range Measurements on Two Reference Stations and Inertial Sensors | |
| CN111829512B (en) | AUV navigation positioning method and system based on multi-sensor data fusion | |
| CN102829777A (en) | Integrated navigation system for autonomous underwater robot and method | |
| CN104180804A (en) | Single reference node underwater vehicle integrated navigation method based on underwater information network | |
| CN107966145B (en) | AUV underwater navigation method based on sparse long baseline tight combination | |
| CN106054135A (en) | Passive underwater sound positioning method based on moving time window periodically | |
| CN104197939A (en) | Multi-reference-point under-water vehicle combination navigation method based on underwater information network | |
| CN113048983A (en) | Improved layered AUV (autonomous Underwater vehicle) cooperative navigation positioning method for different-time sequential measurement | |
| CN105547289A (en) | Underwater vehicle combined navigation system and navigation information fusion method | |
| CN108871379B (en) | DVL speed measurement error online calibration method | |
| CN112684207B (en) | ADCP (advanced digital control Performance) speed estimation and correction algorithm for deep submersible vehicle | |
| CN117870687A (en) | AUV (autonomous Underwater vehicle) underwater integrated navigation positioning system and implementation method | |
| CN114459476B (en) | Underwater unmanned submarine current measuring DVL/SINS integrated navigation method based on virtual speed measurement | |
| CN112747748A (en) | Pilot AUV navigation data post-processing method based on reverse solution | |
| CN104061930A (en) | Navigation method based on strapdown inertial guidance and Doppler log |
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 |