CN111829512B - AUV navigation positioning method and system based on multi-sensor data fusion - Google Patents
AUV navigation positioning method and system based on multi-sensor data fusion Download PDFInfo
- Publication number
- CN111829512B CN111829512B CN202010514731.3A CN202010514731A CN111829512B CN 111829512 B CN111829512 B CN 111829512B CN 202010514731 A CN202010514731 A CN 202010514731A CN 111829512 B CN111829512 B CN 111829512B
- Authority
- CN
- China
- Prior art keywords
- navigation
- auv
- carrier
- representing
- degrees
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 46
- 230000004927 fusion Effects 0.000 title claims abstract description 16
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims abstract description 24
- 230000033001 locomotion Effects 0.000 claims abstract description 19
- 238000005259 measurement Methods 0.000 claims description 45
- 239000011159 matrix material Substances 0.000 claims description 31
- 239000013598 vector Substances 0.000 claims description 11
- 230000008569 process Effects 0.000 claims description 10
- 230000008859 change Effects 0.000 claims description 8
- 230000007704 transition Effects 0.000 claims description 6
- 230000001133 acceleration Effects 0.000 claims description 5
- 238000005070 sampling Methods 0.000 claims description 4
- 238000012360 testing method Methods 0.000 abstract description 7
- 230000000694 effects Effects 0.000 abstract description 6
- 238000010586 diagram Methods 0.000 description 10
- 238000006243 chemical reaction Methods 0.000 description 5
- 238000004422 calculation algorithm Methods 0.000 description 4
- 230000003287 optical effect Effects 0.000 description 3
- 238000009825 accumulation Methods 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 238000012937 correction Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 238000001914 filtration Methods 0.000 description 2
- 238000009434 installation Methods 0.000 description 2
- 239000013535 sea water Substances 0.000 description 2
- 238000012795 verification Methods 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 230000000295 complement effect Effects 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 150000003839 salts Chemical class 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 238000011426 transformation method Methods 0.000 description 1
- 230000000007 visual effect 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/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/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/20—Instruments for performing navigational calculations
-
- 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
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
-
- 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
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
Abstract
The invention relates to an AUV navigation positioning method and system based on multi-sensor data fusion. In the method, an inertial navigation system which does not depend on external information and is not influenced by environment is adopted as a main inertial navigation system, GPS/USBL is adopted as an auxiliary positioning system, DVL is adopted as an auxiliary speed measuring system, and the aim of designing a high-precision combined navigation system by applying various low-precision sensors is achieved. Step one: establishing an inertial navigation kinematic model; step two: defining a 3-dimensional space of the AUV and motion parameters of 6 degrees of freedom; the AUV refers to an unmanned underwater vehicle; step three: determining a basic form of the integrated navigation system model; step four: and performing combined navigation of inertial navigation, acoustic navigation and satellite navigation. The combined navigation mode is applied to the water surface and underwater motion test of the AUV, achieves good positioning effect, has positioning error of about 10m, and has higher stability and practicability.
Description
Technical Field
The invention relates to an AUV navigation positioning method and system based on multi-sensor data fusion, and belongs to the technical field of underwater integrated navigation.
Background
Underwater navigation problems remain one of the main technical challenges faced by underwater mobile platforms, and underwater positioning of AUVs (unmanned underwater vehicles) is extremely difficult due to the complexity and specificity of the underwater environment. The focus of the students is on how to overcome the influence of the underwater environment on underwater positioning, and improve the accuracy of underwater positioning, which is also a research hot spot in recent years. The impact of the underwater environment on AUV positioning is mainly manifested in the following aspects:
(1) The interference is more and the influence is large. In the ocean, not only the interferences of sea surface wind, tide, ocean current, sea surface front, temperature jump layer, density jump layer and the like exist, but also the influences of temperature, salinity, density, pressure and the like on the underwater positioning instrument exist.
(2) The visibility is low. Since light attenuates and scatters much more strongly in seawater than in the atmosphere, the horizontal view in seawater is only one thousandth of that in the atmosphere. Typically, the visibility in the sea is 20m, and the specific value is affected by the water quality and water color.
(3) Electromagnetic waves cannot propagate in water, and acoustic signals have low propagation efficiency and low reliability. This places a great limit on the use of some accurate positioning devices (e.g., GPS) on land.
(4) The AUV is inconvenient to carry due to high cost, large volume and heavy equipment. Due to the limitation of the volume and the weight of the AUV, certain restrictions are formed on the volume and the weight of the underwater positioning equipment. At the same time, high precision underwater positioning equipment is generally expensive and has limited application to some non-military aircraft.
(5) Concealment problems. For an AUV for military use, concealment is a prerequisite for performing tasks, and some acoustic or visual underwater positioning methods expose themselves.
In practical application, a proper underwater positioning method is selected according to the practical operation requirement of the AUV and by combining the characteristics of the underwater environment. Not only the depth range in which the AUV works, but also the voyage time of the AUV to perform the task should be considered. Different underwater positioning methods have different application ranges, and the combination of a plurality of underwater positioning methods can improve the positioning precision.
At present, scholars are striving to find a navigation positioning method suitable for underwater environment. The general underwater navigation methods are classified into the following categories, as shown in fig. 1. However, because a single navigation mode always has defects, the high-precision navigation requirements of long-time and long-range underwater can not be met in terms of precision and reliability, part of navigation means can only be used as assistance, and the advantages of different navigation modes are complemented by the combined navigation means, so that higher navigation performance can be obtained.
Disclosure of Invention
The invention aims at: in order to solve the problem of the underwater single navigation defect, the optimal statistical theory is selected as a method, an information fusion technology is adopted to construct a combined navigation system, the method is complementary to each other according to each single navigation method, the accuracy requirement of a certain single navigation system can be reduced on the premise of not influencing the overall navigation accuracy, the navigation degree and technical difficulty are reduced, and the method has the characteristics of high cost performance and high accuracy.
The invention is realized by adopting the following technical scheme:
an AUV integrated navigation method based on multi-sensor data fusion comprises the following steps:
step one: establishing an inertial navigation kinematic model;
step two: defining a 3-dimensional space of the AUV and motion parameters of 6 degrees of freedom; the AUV refers to an unmanned underwater vehicle;
step three: determining a basic form of the integrated navigation system model;
step four: and performing combined navigation of inertial navigation, acoustic navigation and satellite navigation.
Further, the inertial navigation kinematic model specifically includes:
the inertial navigation attitude is updated as:
wherein Δθ= [ Δθ ] x Δθ y Δθ z ] T Three-axis angle change information output by a gyroscope under a carrier coordinate system is represented; epsilon= [ epsilon ] x ε y ε z ] T Representing the three-axis constant drift information of the gyro under the carrier coordinate system; [ q ] 0 q 1 q 2 q 3 ] T Representing quaternion information, and updating quaternions by adopting a rotation vector method during gesture updating; Δq= [ Δq ] 0 Δq 1 Δq 2 Δq 3 ] T The quaternion variation is used;
the inertial navigation speed is updated as follows:
wherein V= [ V N V E V D ] T Represents three-dimensional velocity information under the n-series,as a strapdown matrix, Δv= [ Δv ] x ΔV y ΔV z ] T Representing three-axis speed change information output by the accelerometer under the system b, g representing local gravitational acceleration, and delta t representing sampling time interval; Δa= [ Δa ] x ΔA y ΔA z ] T Constant zero offset information of the accelerometer under the b system is represented;
the inertial navigation position is updated as:
wherein: p= [ P ] N P E P D ] T Representing three-dimensional position information under n series.
Further, the n system is a navigation coordinate system: the geographic coordinate system is defined as N-E-D, the origin is positioned at the current position of the carrier, and x n ,y n In the geographical horizontal plane x n The axis is north along the tangent line of the local warp yarn, y n The axis being east, z along the tangent to the local weft n The axis is vertically downward to form a right hand coordinate system;
the b system refers to a carrier coordinate system, namely an AUV body coordinate system, the origin is positioned at the center of mass of the carrier, and x is the same as the origin b Axially forward of the carrier, y b Along the carrier to the right, z b Perpendicular to the carrier plane, the triaxial satisfies the right rule and rotates with the rotation of the carrier.
Further, defining the motion parameters of the AUV in 3-dimensional space and 6 degrees of freedom, specifically:
(P N ,P E ,P D ): representing the position components of the AUV in three coordinate axes under a navigation coordinate system;
(V N ,V E ,V D ): representing the velocity components of the AUV in three coordinate axes under the navigational coordinate system;
(γ, θ, ψ): gamma is a roll angle, a counterclockwise direction is selected as a positive direction, and the value range is (-180 degrees, 180 degrees); θ is a pitch angle, the positive direction is the same as the roll angle, and the value range is (-90 degrees, 90 degrees); psi is yaw angle, left yaw is positive, and the value range is (-180 degrees, 180 degrees).
Further, the basic form of the integrated navigation system model is that
Wherein X is a state quantity, phi k-1 For state transition matrix, Z is the measurement, H k To measure matrix, w k-1 And v k Representing process noise and measurement noise, subject to normal distribution w k-1 ～(0,Q k )，v k ～(0,R k )，Q k Is a process noise variance and is not negative; r is R k The noise variance is measured and determined.
Further, the combined navigation combining inertial navigation, acoustic navigation and satellite navigation is specifically:
(1) The state equation is determined, specifically:
state quantity x= [ q ] 0 q 1 q 2 q 3 V N V E V D P N P E P D ε x ε y ε z ΔA x ΔA y ΔA z ] T ；
State transition matrix Φ k-1 The following are provided:
wherein:
t 15 ＝ΔV x -ΔA x ,t 16 ＝ΔV y -ΔA y ,t 17 ＝ΔV z -ΔA z
t 19 ＝2q 1 (ΔV y -ΔA y ),t 20 ＝2q 0 (ΔV z -ΔA z )
t 21 ＝2q 1 (ΔV x -ΔA x ),t 22 ＝2q 0 (ΔV y -ΔA y ),t 23 ＝2q 1 (ΔV z -ΔA z )
t 24 ＝2q 0 (ΔV x -ΔA x )+2q 2 (ΔV y -ΔA y )+2q 3 (ΔV z -ΔA z )
t 25 ＝2q 0 (ΔV x -ΔA x ),t 26 ＝2q 2 (ΔV z -ΔA z ),t 37 ＝2q 3 (ΔV y -ΔA y )
t 27 ＝2q 0 (ΔV x -ΔA x )+2q 2 (ΔV z -ΔA z )+2q 3 (ΔV y -ΔA y )
t 28 ＝2q 0 q 3 ,t 29 ＝q 0 2 ,t 30 ＝q 1 2 ,t 31 ＝q 2 2 ,t 32 ＝q 3 2 ,t 33 ＝2q 2 (ΔV x -ΔA x )
t 34 ＝2q 3 (ΔV x -ΔA x ),t 35 ＝2q 0 (ΔV y -ΔA y ),t 38 ＝2q 0 q 1
t 36 ＝2q 3 (ΔV x -ΔA x )+2q 1 (ΔV z -ΔA z )+2q 0 (ΔV y -ΔA y )
state noise w k Variance Q of k The method comprises the following steps:
wherein:
Q q ＝BB T ,Q v ＝AA T
(2) Determining a measurement equation:
the USBL/GPS measurement matrix is:
the DVL measurement matrix is:
the magnetic force measurement matrix is as follows:
H mag ＝[-2l 8 l 14 (q 3 l 3 -q 3 l 4 +q 3 l 5 +q 3 l 6 +2q 0 q 1 q 2 )；-2l 8 l 14 (-q 2 l 3 +q 2 l 4 +q 2 l 5 +q 2 l 6 +2q 0 q 1 q 3 )；
2l 8 l 14 (q 1 l 3 +q 1 l 4 +q 1 l 5 -q 1 l 6 +2q 0 q 2 q 2 )；2l 8 l 14 (q 0 l 3 +q 0 l 4 -q 0 l 5 +q 0 l 6 +2q 1 q 2 q 3 )；o 1×12 ] 1×16
wherein:
l 9 ＝q 0 q 3 l 10 ＝q 1 q 2 l 2 ＝l 9 +l 10
l 7 ＝l 3 +l 4 -l 5 -l 6
t 12 ＝4l 8 l 11 l 13 ＝1/(l 12 +1.0)
measuring noise v k Variance R of (2) k Selecting according to actual USBL/GPS, DVL and magnetometer system models;
(3) Estimating a state quantity X through a state equation and a measurement equation;
(4) Through [ q ] 0 q 1 q 2 q 3 ] T Calculating gamma, theta and phi;
(5) According to (P) in the state quantity X obtained in the step (3) N ,P E ,P D ) Sum (V) N ,V E ,V D ) And (3) carrying out integrated navigation on the AUV with the gamma, theta and phi obtained in the step (4) to locate the AUV in real time.
Further, acoustic navigation refers to a navigation mode of providing position information through USBL and providing speed information through DVL, wherein USBL refers to an ultra-short baseline positioning system, and DVL refers to a Doppler speed measurement system; satellite navigation provides position information through a GPS; magnetometers are integrated within the inertial navigation system for providing heading information.
Furthermore, the invention also provides an AUV integrated navigation system, which comprises:
and a model building module: establishing an inertial navigation kinematic model;
motion parameter definition module: defining a 3-dimensional space of the AUV and motion parameters of 6 degrees of freedom; the AUV refers to an unmanned underwater vehicle;
the integrated navigation system model determining module: determining a basic form of the integrated navigation system model;
and the integrated navigation module is used for: and performing combined navigation of inertial navigation, acoustic navigation and satellite navigation.
Compared with the prior art, the invention has the advantages that:
(1) The invention finally selects a combined navigation mode of an inertial navigation system and a GPS on the water surface, and selects a combined navigation mode of an inertial navigation system, a Doppler velocity measurement system (Doppler Velocity Log, DVL) and an Ultra-short baseline positioning system (Ultra-Short Baseline System, USBL) under water, which is shown in figure 2.
(1) The GPS signal on the water surface is good, the positioning accuracy is high, the calculated amount is reduced by adopting a combination mode of inertial navigation and GPS, and the calculation speed is improved.
(2) Because the speed of the USBL is lower, the combination mode of inertial navigation and DVL speed is selected to acquire the positioning information, but error accumulation can occur for a long time to cause divergence, so that the positioning information of the system is corrected at regular time by taking the position information of the USBL as a correction auxiliary means.
Drawings
FIG. 1 is a schematic diagram of the classification of underwater navigation
FIG. 2 is a schematic diagram of an AUV integrated navigation method
FIG. 3A is a block diagram of an inertial navigation system
FIG. 4 positioning schematic diagram of ultra-short baseline positioning system
Fig. 5.Usbl system coordinate block diagram
FIG. 6 four-beam space geometry positioning map
FIG. 7 is a schematic diagram showing the comparison of GPS traces and combined traces
FIG. 8 is a diagram showing the comparison of the reference trace and the combined trace
Detailed Description
The invention will be described in further detail with reference to the drawings and examples.
The invention relates to an AUV navigation positioning method and system based on multi-sensor data fusion. In the method, an inertial navigation system which does not depend on external information and is not influenced by environment is adopted as a main inertial navigation system, GPS/USBL is adopted as an auxiliary positioning system, DVL is adopted as an auxiliary speed measuring system, and the aim of designing a high-precision combined navigation system by applying various low-precision sensors is achieved. Aiming at the problem that the update frequency of each sensor in the integrated navigation is inconsistent, the invention adopts a sequential Kalman filtering technology to solve the problem, and under the condition that the external equipment does not provide measurement, the inertial navigation system only carries out time update, completes the state prediction process, and carries out measurement update after the information of the external equipment is acquired.
Due to the inherent characteristics of inertial devices, errors generated by the inertial navigation system can accumulate with time, and DVL is a common speed measurement navigation system which is combined with the inertial navigation system, so that the problem that the pure inertial navigation errors diverge with time can be solved. Therefore, GPS/USBL can be used for providing position measurement, DVL can be used for providing speed measurement, so that the integrated navigation system is formed, and the inertial navigation system is corrected.
The combined state of the inertial navigation system, the GPS and the DVL can be preferentially used when the GPS signals are received, and the combined mode of the inertial navigation system, the USBL and the DVL can not be used when the GPS signals are not obtained, so that the system can be navigated with high precision. The combined navigation mode is applied to the water surface and underwater motion test of the AUV, and has good positioning effect, positioning error of about 10m, and higher stability and practicability.
In the unmanned aerial vehicle navigation control system, the pilot of combined navigation such as inertial navigation, optical flow sensor, GPS positioning system, supersonic wave is mostly adopted to carry on the real-time navigation control, and the special environment under water makes some application of positioning method limited greatly, when unmanned underwater submarine is underwater, GPS has no signal, the optical flow sensor can not obtain the speed, therefore seek to replace GPS and optical flow sensor to carry on the underwater navigation positioning, the concrete steps are as follows:
step one: according to the principle of positioning, speed measuring and attitude determining of navigation modes such as inertial navigation, acoustic navigation and satellite navigation, a kinematic model is established:
1. kinematic model of inertial navigation system
The block diagram of fig. 3 shows the solution process of the inertial navigation system, and the kinematic formula is expressed as follows:
(1) Inertial navigation attitude update and the process of solving the attitude strapdown matrix are the basis and the core of the SINS for navigation,
in the formula (1), Δθ= [ Δθ ] x Δθ y Δθ z ] T Representing three-axis angle change of gyroscope output under carrier coordinate systemInformation; epsilon= [ epsilon ] x ε y ε z ] T Representing the three-axis constant drift information of the gyro under the carrier coordinate system; [ q ] 0 q 1 q 2 q 3 ] T Representing quaternion information, and updating quaternions by adopting a rotation vector method during gesture updating; Δq= [ Δq ] 0 Δq 1 Δq 2 Δq 3 ] T Is the quaternion variation.
(2) Inertial navigation speed update
The speed update equation converts the accelerometer information of the three axes into n series through the gesture matrix to obtain a speed vector, and then the vector differential equation in the speed update link is as follows:
in the formula (2), v= [ V N V E V D ] T Represents three-dimensional velocity information under the n-series,as a strapdown matrix, Δv= [ Δv ] x ΔV y ΔV z ] T Representing three-axis speed change information output by the accelerometer under the system b, g representing local gravitational acceleration, and delta t representing sampling time interval; Δa= [ Δa ] x ΔA y ΔA z ] T And represents constant zero offset information of the accelerometer under the b system.
Expanding formula (2) into a triaxial form:
wherein the method comprises the steps of
The n system is a navigation coordinate system: the geographic coordinate system is defined as N-E-D, the origin is positioned at the current position of the carrier, and x n ,y n At the geographical levelIn, x n The axis is north along the tangent line of the local warp yarn, y n The axis being east, z along the tangent to the local weft n The axis is vertically downward to form a right hand coordinate system;
the b system refers to a carrier coordinate system, namely an AUV body coordinate system, the origin is positioned at the center of mass of the carrier, and x is the same as the origin b Axially forward of the carrier, y b Along the carrier to the right, z b Perpendicular to the carrier plane, the triaxial satisfies the right rule and rotates with the rotation of the carrier.
(3) Inertial navigation location update
The position update principle is that the velocity integration obtains the motion of the carrier relative to the earth surface, and then the position update equation is that
Wherein: p= [ P ] N P E P D ] T Representing three-dimensional position information under n series.
2. Ultra-short baseline positioning system (USBL) model
The ultra-short baseline positioning system calculates the standoff by measuring the round trip time of a transmitter sending underwater an underwater acoustic signal to a known transponder located on the ocean floor of the target ocean. And calculating the azimuth angle of the carrier according to the phase difference of the underwater acoustic signals transmitted to each hydrophone of the acoustic head.
Fig. 4 is a schematic diagram of the positioning principle of USBL. And taking the center O of the matrix as the origin of coordinates, taking the opposite direction of the sky as the positive direction of the z axis, and establishing a matrix coordinate system oxyz according to the right hand rule. The four hydrophones are located on the x axis and the y axis of the coordinate system, the distance between the coaxial hydrophones is d, and then the coordinates of each point are (+/-d/2 ). The transponder is located at a specific position T and its coordinates are (x a ,y a ,z a ) As readily available from fig. 4:
x a ＝lcosθ x
y a ＝lcosθ y
in the USBL positioning schematic diagram, l is an inclined distance, m is the projection of the inclined distance l on a horizontal plane oxy, and theta is a horizontal azimuth angle, namely an included angle between m and an x axis, and theta is x And theta y The included angles between the axis l and the axes x and y are respectively.
The wavelength of the sound wave is set to lambda,representing the phase difference of the acoustic signals received by the hydrophones on the same axis:
substituting the formula (7) into the formula (5) to obtain
Skew l=c·t ri /2，T ri Transmitting acoustic signals to transponders for interrogatorThe round trip time of the acoustic signal is returned, and the transmission speed of the acoustic wave under water is c.Can be obtained from ultra-short baseline measurements, from which the horizontal position (x a ,y a ) Depth z a And the incidence angle of sound wave (θ) x ,θ y ) Etc.
Coordinate conversion of USBL navigation solving equation, the geometric center of hydrophone in USBL is set as the origin of coordinates, and ox is used in northeast coordinate system t y t z t And (3) representing. When the hydrophone installation error is not considered, the coordinate system (u system) of the ultra-short baseline system and the coordinate system (b system) of the carrier are in parallel relation in the triaxial space. The distribution of the three coordinate systems of the u-system (b-system), the t-system and the e-system in space is shown in fig. 5.
As can be seen from fig. 5, s and p are the position vectors of the transponder and the carrier, respectively, in the earth's coordinate system, where s is a known position vector since the transponder is arranged in advance on the sea floor. r is the position vector of the transponder in the carrier coordinate system, for the transformation matrix of the carrier coordinate system into the earth coordinate systemRepresenting that the position vector of the transponder is obtainable by the vector theorem is that the USBL positioning model is obtained:
r＝R T (s-p) (9)
3. doppler velocity measurement system (DVL) model
The DVL is one of the important navigation means for measuring the speed of the offshore navigation carrier at the present stage, can provide accurate speed measurement information of the carrier at long voyage based on the Doppler effect principle, and can form good complementarity with the strapdown inertial navigation system, so that the DVL is widely applied to various fields.
The doppler effect is a phenomenon related to microwave technology. In 1842, the well-known physicist doppler has inadvertently found a phenomenon in which there is a relative displacement between the object emitting sound waves and reflecting sound waves when studying acoustic problemsAt the time, the transmission frequency f 1 And reflection frequency f 2 And are not uniform. The difference between the transmitted and reflected frequencies is exactly linear with the relative velocity between the two, and this difference is the doppler shift. Scientists develop systems such as a Doppler log, a Doppler radar and the like based on the effect, and a four-beam-based Doppler system speed measurement method is mainly described below.
The beam space and reflection point positions of a four-beam doppler system are shown in figure 6,
the carrier speed is v, four beams have the same inclination angle alpha and deflection angle beta, the Doppler frequency shift of the sound wave along each direction is the sum of the Doppler frequency shifts of three vertical direction speed components, and the Doppler frequency shifts of the four beams are respectively
Wherein A is i ,B i ,C i ,D i (i=x, y, z) are the directional cosine of the four beams, respectively, satisfying
Substituting the formula (11) into the formula (10) to obtain
And (3) solving a formula (12) simultaneously to obtain triaxial directional velocity and a DVL velocity measurement model:
in the middle of
The limitations of the above-described navigation methods are analyzed with respect to their principle. In order to meet the navigation requirements of long voyage and high precision, a combined navigation technology for multi-sensor information fusion is provided:
the inertial navigation system (Inertial navigation system, INS) has serious error accumulation in long-time navigation and needs timing correction; the ultra-short baseline positioning system has a small positioning range, poor autonomy and needs ground end support; the GPS positioning system is greatly influenced by external environmental factors such as weather and the like, and has no signal in water for a long time; the Doppler velocimeter speed measurement precision is limited in the effective range, once the wave beam of the DVL exceeds the set mileage, the influence on the speed measurement accuracy is obvious, and the influence on the DVL by factors such as installation errors, water temperature, water speed, water depth, salt concentration in water and the like is large.
The inertial navigation system measures angular velocity and acceleration through a gyroscope and an accelerometer, and navigation parameters such as position, velocity, gesture and the like are obtained through calculation. Can navigate independently without depending on external information, and becomes a main stream navigation positioning system in the current stage. However, due to the inherent characteristics of inertial devices, errors generated by the inertial devices can accumulate with time, the DVL is a common speed measurement navigation system, and the DVL and the INS are combined to form an INS/DVL combined navigation subsystem, so that the problem that the pure inertial navigation errors diverge with time can be solved. Therefore, GPS/USBL can be used for providing position measurement, DVL can be used for providing speed measurement, so that the integrated navigation system is formed, and the inertial navigation system is corrected. When the combined state of INS+GPS+DVL can be preferentially used when the GPS signal can be received, and when the GPS signal can not be obtained, the combined mode of INS+USBL+DVL is adopted, so that the system can be navigated with high precision, as shown in figure 2.
Step two: defining the 3-dimensional space of the AUV unmanned underwater vehicle, and the motion characteristic parameters of 6 degrees of freedom:
the AUV requires specific parameters to describe its motion characteristics when performing tasks underwater. Below, the kinematic parameters of the AUV in the navigation coordinate system are defined:
navigation coordinate system: the geographical coordinate system, usually designated (N-E-D), has an origin at the current location of the carrier, x n ,y n In the geographical horizontal plane，x n The axis is north along the tangent line of the local warp yarn, y n The axis being east, z along the tangent to the local weft n The axis is vertically downward to form a right hand coordinate system;
(P N ,P E ,P D ): representing the position components of the AUV in three coordinate axes under a navigation coordinate system;
(V N ,V E ,V D ): representing the velocity components of the AUV in three coordinate axes under the navigational coordinate system;
(γ, θ, ψ): gamma is a roll angle, a counterclockwise direction is selected as a positive direction, and the value range is (-180 degrees, 180 degrees); θ is a pitch angle, the positive direction is the same as the roll angle, and the value range is (-90 degrees, 90 degrees); psi is yaw angle, left yaw is positive, and the value range is (-180 degrees, 180 degrees).
(p,ω θ ω): indicating the angular velocity of the AUV along three axes in the navigational coordinate system, the prescribed counter-clockwise direction is positive.
Step three: determining a basic form of the integrated navigation system model;
the basic form of the integrated navigation system model is that
Wherein X is a state quantity, phi k-1 For state transition matrix, Z is the measurement, H k To measure matrix, w k-1 And v k Representing process noise and measurement noise, subject to normal distribution w k-1 ～(0,Q k )，v k ～(0,R k )，Q k Is a process noise variance and is not negative; r is R k The noise variance is measured and determined.
Step four: and performing combined navigation of inertial navigation, acoustic navigation and satellite navigation.
The designed integrated navigation algorithm is applied to the entity AUV, and the hardware equipment composition of the navigation system is shown in Table 1:
TABLE 1 navigation hardware device composition
Wherein, the SBG of the inertial navigation system is internally integrated with a magnetometer, and the device can be relied on as a heading reference.
The method comprises the following steps:
(1) Selecting three-axis posture, three-axis speed, three-axis position and three-axis constant value gyro drift of an AUV in a navigation system, taking zero offset of a three-axis constant value accelerometer as state quantity, and determining a state equation
State quantity x= [ q ] 0 q 1 q 2 q 3 V N V E V D P N P E P D ε x ε y ε z ΔA x ΔA y ΔA z ] T ；
State transition matrix Φ k-1 The following are provided:
wherein:
t 15 ＝ΔV x -ΔA x ,t 16 ＝ΔV y -ΔA y ,t 17 ＝ΔV z -ΔA z
t 19 ＝2q 1 (ΔV y -ΔA y ),t 20 ＝2q 0 (ΔV z -ΔA z )
t 21 ＝2q 1 (ΔV x -ΔA x ),t 22 ＝2q 0 (ΔV y -ΔA y ),t 23 ＝2q 1 (ΔV z -ΔA z )
t 24 ＝2q 0 (ΔV x -ΔA x )+2q 2 (ΔV y -ΔA y )+2q 3 (ΔV z -ΔA z )
t 25 ＝2q 0 (ΔV x -ΔA x ),t 26 ＝2q 2 (ΔV z -ΔA z ),t 37 ＝2q 3 (ΔV y -ΔA y )
t 27 ＝2q 0 (ΔV x -ΔA x )+2q 2 (ΔV z -ΔA z )+2q 3 (ΔV y -ΔA y )
t 28 ＝2q 0 q 3 ,t 29 ＝q 0 2 ,t 30 ＝q 1 2 ,t 31 ＝q 2 2 ,t 32 ＝q 3 2 ,t 33 ＝2q 2 (ΔV x -ΔA x )
t 34 ＝2q 3 (ΔV x -ΔA x ),t 35 ＝2q 0 (ΔV y -ΔA y ),t 38 ＝2q 0 q 1
t 36 ＝2q 3 (ΔV x -ΔA x )+2q 1 (ΔV z -ΔA z )+2q 0 (ΔV y -ΔA y )
Δθ x ,Δθ y ,Δθ z for three-axis gyroscope output, deltaV x ,ΔV y ,ΔV z Is output by a triaxial accelerometer.
State noise w k Variance Q of k The method comprises the following steps:
wherein:
Q q ＝BB T ,Q v ＝AA T
(2) A measurement equation is determined. And defining a measuring matrix H, the position of the USBL, the speed of the DVL and the magnetic heading of the magnetometer as measurement quantities according to the second step. Wherein the DVL provides the speed of the AUV under the carrier system, and coordinate conversion exists between the speed and the state quantity; the magnetic heading provided by the magnetometer has no direct relation with the state quantity, and the magnetic heading is also required to be transformed, and the transformation method is as follows:
1) Fused USBL location (same as fused GPS location):
because the USBL equipment provides the relative position of the water surface end and the underwater end transducer, the underwater end is arranged on the AUV, and the water surface end is supported by the GPS, the longitude and latitude of the underwater AUV can be calculated, and the USBL/GPS measurement matrix is as follows:
2) Velocity of fusion DVL:
converting the velocity under the carrier system (b system) to the navigation system (n system) to obtain the DVL measurement matrix
3) Fusing magnetic heading information:
the magnetic heading can be calculated through the triaxial information provided by the magnetometer (mag), the magnetic heading is applied to the state quantity quaternion,
wherein QuatToEual is a quaternion conversion Euler angle function, quatToTbn is a quaternion conversion gesture matrix function, and EulToQuat is a Euler angle conversion quaternion function. Equation (19) shows that the course angle is calculated as one-step pre-measurement through the quaternion at the current moment, the course angle is made to be 0, the gesture matrix when the course angle is calculated to be 0 is converted into n-system through the gesture matrix when the course angle is 0, the course at the current moment is calculated as measurement, and at the moment, only the deviation of the course angle exists between the two coordinate systems.
Deriving a measurement formula of the heading to obtain a mag measurement matrix:
wherein:
measuring noise v k Variance R of (2) k And selecting according to the actual USBL/GPS, DVL and magnetometer system model.
The updating frequency of the inertial navigation system is set to be 100Hz, the magnetometer is integrated inside the inertial navigation system, the frequency is the same as that of the inertial navigation system, the updating frequency of DVL is approximately 5Hz, the position updating frequency of GPS is 2Hz, the speed updating frequency is 10Hz, the updating frequency of USBL is 0.5Hz, and due to the inconsistency of the frequencies, the system adopts a fusion mode of sequential Kalman filtering to perform optimal estimation.
(3) Estimating a state quantity X through a state equation and a measurement equation;
(4) Through [ q ] 0 q 1 q 2 q 3 ] T Calculating gamma, theta and phi;
(5) According to (P) in the state quantity X obtained in the step (3) N ,P E ,P D ) Sum (V) N ,V E ,V D ) And (3) carrying out integrated navigation on the AUV with the gamma, theta and phi obtained in the step (4) to locate the AUV in real time.
The acoustic navigation is to provide position information through USBL, provide the navigation mode of the speed information through DVL, USBL refers to the ultra-short baseline positioning system, DVL refers to the Doppler speed measurement system; satellite navigation provides position information through a GPS; magnetometers are integrated within the inertial navigation system for providing heading information.
The implementation process comprises the following steps:
embodiment one:
and (3) carrying out water surface test on the AUV, and carrying out test verification of a combined navigation algorithm in a water reservoir of an official house with the longitude and latitude of 40.3457756 DEG N and 115.7364502 DEG E. And meanwhile, SBG, DVL, GPS data are acquired, and the motion trail is approximately triangular. The errors of the navigation equipment are equivalent to zero-mean Gaussian white noise, gyro drift noise is 0.2rad/s, accelerometer zero bias noise is 0.35m/s2, magnetic heading noise is 0.1745rad, GPS position error noise is 0.3m, DVL speed error noise is 0.3m/s, and because the GPS signal is good in water surface test, more than 10 satellites can be searched at the position, the acquired data are subjected to a combined navigation algorithm in the fourth step and a combined mode of IMU+GPS+DVL, and the combined result is compared with the track of the GPS, as shown in figure 7.
Embodiment two:
and (3) performing underwater test on the AUV, and performing test verification of a combined navigation algorithm in a water reservoir of an official house with the longitude and latitude of 40.3434715 degrees N and 115.7399673 degrees E. Data of SBG, DVL, USBL were collected simultaneously and navigation parameter settings were consistent with example 1. At this time, the GPS cannot search for satellites, the experimental data is analyzed and processed by adopting a combined positioning mode of IMU+USBL+DVL, and the combined result is compared with high-precision inertial navigation carried by the AUV body, as shown in FIG. 8
The implementation effect is as follows:
the GPS has higher positioning precision when the number of satellites is large, but the updating frequency is lower, which is one percent of the updating frequency of the inertial navigation system, and the GPS is greatly influenced by environmental factors, and the GPS is free from interference, so that the GPS is used as a reference. As can be seen from the comparison result of the first embodiment, the accuracy of the combination mode of IMU+GPS+DVL is within 10 m.
According to the result of the second embodiment, the distances between the reference track and the combined track are respectively 450.7m and 438.1m, and the precision is about 10 m.
When the AUV water surface moves, the GPS has drift, reaches the maximum (0-8 m) when stationary, has lower updating frequency, and can be easily seen from the above embodiment, and the position error can be reduced to be within 10m after being combined with the inertial navigation system and the Doppler velocity measurement system. If the inertial navigation system is only combined with the ultra-short baseline positioning system during underwater movement, the update frequency of the ultra-short baseline positioning system is lower, the accuracy of the inertial navigation system adopted in the AUV is lower, and the updated gap of the ultra-short baseline positioning system can diverge, so that the combined navigation of three devices of the inertial navigation system, the Doppler velocity measurement system and the ultra-short baseline positioning system is adopted, and the position error after combination can be reduced to about 10 m.
Claims (6)
1. An AUV integrated navigation method based on multi-sensor data fusion is characterized by comprising the following steps:
step one: establishing an inertial navigation kinematic model;
step two: defining a 3-dimensional space of the AUV and motion parameters of 6 degrees of freedom; the AUV refers to an unmanned underwater vehicle;
step three: determining a basic form of the integrated navigation system model;
step four: performing combined navigation combining inertial navigation, acoustic navigation and satellite navigation;
the inertial navigation kinematic model is specifically as follows:
the inertial navigation attitude is updated as:
wherein Δθ= [ Δθ ] x Δθ y Δθ z ] T Three-axis angle change information output by a gyroscope under a carrier coordinate system is represented; epsilon= [ epsilon ] x ε y ε z ] T Representing the three-axis constant drift information of the gyro under the carrier coordinate system; [ q ] 0 q 1 q 2 q 3 ] T Representing quaternion information, and updating quaternions by adopting a rotation vector method during gesture updating; Δq= [ Δq ] 0 Δq 1 Δq 2 Δq 3 ] T The quaternion variation is used;
the inertial navigation speed is updated as follows:
wherein V= [ V N V E V D ] T Represents three-dimensional velocity information under the n-series,as a strapdown matrix, Δv= [ Δv ] x ΔV y ΔV z ] T Representing three-axis speed change information output by the accelerometer under the system b, g representing local gravitational acceleration, and delta t representing sampling time interval; Δa= [ Δa ] x ΔA y ΔA z ] T Constant zero offset information of the accelerometer under the b system is represented;
the inertial navigation position is updated as:
wherein: p= [ P ] N P E P D ] T Representing three-dimensional position information under n series;
defining the motion parameters of the AUV in 3-dimensional space and 6 degrees of freedom, wherein the motion parameters are specifically as follows:
(P N ,P E ,P D ): representing the position components of the AUV in three coordinate axes under a navigation coordinate system;
(V N ,V E ,V D ): representing the velocity components of the AUV in three coordinate axes under the navigational coordinate system;
(γ, θ, ψ): gamma is a roll angle, a counterclockwise direction is selected as a positive direction, and the value range is (-180 degrees, 180 degrees); θ is a pitch angle, the positive direction is the same as the roll angle, and the value range is (-90 degrees, 90 degrees); psi is yaw angle, left yaw is positive, and the value range is (-180 degrees, 180 degrees);
the basic form of the integrated navigation system model is that
Wherein X is a state quantity, phi k-1 For state transition matrix, Z is the measurement, H k To measure matrix, w k-1 And v k Representing process noise and measurement noise, subject to normal distribution w k-1 ～(0,Q k )，v k ～(0,R k )，Q k Is a process noise variance and is not negative; r is R k The noise variance is measured and determined.
2. The AUV integrated navigation method based on multi-sensor data fusion of claim 1, wherein: the n system is a navigation coordinate system: the geographic coordinate system is defined as N-E-D, the origin is positioned at the current position of the carrier, and x n ,y n In the geographical horizontal plane x n The axis is north along the tangent line of the local warp yarn, y n The axis being east, z along the tangent to the local weft n The axis is vertically downward to form a right hand coordinate system;
the saidThe b system refers to a carrier coordinate system, namely an AUV body coordinate system, the origin is positioned at the center of mass of the carrier, and x is the coordinate system b Axially forward of the carrier, y b Along the carrier to the right, z b Perpendicular to the carrier plane, the triaxial satisfies the right rule and rotates with the rotation of the carrier.
3. The AUV integrated navigation method based on multi-sensor data fusion of claim 1, wherein: the combined navigation combining inertial navigation, acoustic navigation and satellite navigation is carried out, and specifically comprises the following steps:
(1) The state equation is determined, specifically:
state quantity x= [ q ] 0 q 1 q 2 q 3 V N V E V D P N P E P D ε x ε y ε z ΔA x ΔA y ΔA z ] T ；
State transition matrix Φ k-1 The following are provided:
wherein:
t 15 ＝ΔV x -ΔA x ,t 16 ＝ΔV y -ΔA y ,t 17 ＝ΔV z -ΔA z
t 19 ＝2q 1 (ΔV y -ΔA y ),t 20 ＝2q 0 (ΔV z -ΔA z )
t 21 ＝2q 1 (ΔV x -ΔA x ),t 22 ＝2q 0 (ΔV y -ΔA y ),t 23 ＝2q 1 (ΔV z -ΔA z )
t 24 ＝2q 0 (ΔV x -ΔA x )+2q 2 (ΔV y -ΔA y )+2q 3 (ΔV z -ΔA z )
t 25 ＝2q 0 (ΔV x -ΔA x ),t 26 ＝2q 2 (ΔV z -ΔA z ),t 37 ＝2q 3 (ΔV y -ΔA y )
t 27 ＝2q 0 (ΔV x -ΔA x )+2q 2 (ΔV z -ΔA z )+2q 3 (ΔV y -ΔA y )
t 28 ＝2q 0 q 3 ,t 29 ＝q 0 2 ,t 30 ＝q 1 2 ,t 31 ＝q 2 2 ,t 32 ＝q 3 2 ,t 33 ＝2q 2 (ΔV x -ΔA x )
t 34 ＝2q 3 (ΔV x -ΔA x ),t 35 ＝2q 0 (ΔV y -ΔA y ),t 38 ＝2q 0 q 1
t 36 ＝2q 3 (ΔV x -ΔA x )+2q 1 (ΔV z -ΔA z )+2q 0 (ΔV y -ΔA y )
state noise w k Variance Q of k The method comprises the following steps:
wherein:
Q q ＝BB T ,Q v ＝AA T
(2) Determining a measurement equation:
the USBL/GPS measurement matrix is:
the DVL measurement matrix is:
the magnetic force measurement matrix is as follows:
H mag ＝[-2l 8 l 14 (q 3 l 3 -q 3 l 4 +q 3 l 5 +q 3 l 6 +2q 0 q 1 q 2 )；-2l 8 l 14 (-q 2 l 3 +q 2 l 4 +q 2 l 5 +q 2 l 6 +2q 0 q 1 q 3 )；
2l 8 l 14 (q 1 l 3 +q 1 l 4 +q 1 l 5 -q 1 l 6 +2q 0 q 2 q 2 )；2l 8 l 14 (q 0 l 3 +q 0 l 4 -q 0 l 5 +q 0 l 6 +2q 1 q 2 q 3 )；o 1×12 ] 1×16 wherein:
l 9 ＝q 0 q 3 l 10 ＝q 1 q 2 l 2 ＝l 9 +l 10
l 7 ＝l 3 +l 4 -l 5 -l 6
t 12 ＝4l 8 l 11 l 13 ＝1/(l 12 +1.0)
measuring noise v k Variance R of (2) k Selecting according to actual USBL/GPS, DVL and magnetometer system models;
(3) Estimating a state quantity X through a state equation and a measurement equation;
(4) Through [ q ] 0 q 1 q 2 q 3 ] T Calculating gamma, theta and phi;
(5) According to (P) in the state quantity X obtained in the step (3) N ,P E ,P D ) Sum (V) N ,V E ,V D ) And (3) realizing the integrated navigation real-time positioning of the AUV with the gamma, theta and phi obtained in the step (4).
4. The AUV integrated navigation method based on multi-sensor data fusion of claim 3, wherein: the acoustic navigation is to provide position information through USBL, provide the navigation mode of the speed information through DVL, USBL refers to the ultra-short baseline positioning system, DVL refers to the Doppler speed measurement system; satellite navigation provides position information through a GPS; magnetometers are integrated within the inertial navigation system for providing heading information.
5. The AUV integrated navigation method based on multi-sensor data fusion of claim 4, wherein: when the AUV is on the water surface, a combined mode of inertial navigation, GPS and DVL navigation is adopted, and when the AUV is under water, a combined navigation mode of inertial navigation, DVL and USBL is selected.
6. An AUV integrated navigation system implemented by the multi-sensor data fusion-based AUV integrated navigation method of claim 1, comprising:
and a model building module: establishing an inertial navigation kinematic model;
motion parameter definition module: defining a 3-dimensional space of the AUV and motion parameters of 6 degrees of freedom; the AUV refers to an unmanned underwater vehicle;
the integrated navigation system model determining module: determining a basic form of the integrated navigation system model;
and the integrated navigation module is used for: performing combined navigation combining inertial navigation, acoustic navigation and satellite navigation;
the inertial navigation kinematic model is specifically as follows:
the inertial navigation attitude is updated as:
wherein,Δθ＝[Δθ x Δθ y Δθ z ] T three-axis angle change information output by a gyroscope under a carrier coordinate system is represented; epsilon= [ epsilon ] x ε y ε z ] T Representing the three-axis constant drift information of the gyro under the carrier coordinate system; [ q ] 0 q 1 q 2 q 3 ] T Representing quaternion information, and updating quaternions by adopting a rotation vector method during gesture updating; Δq= [ Δq ] 0 Δq 1 Δq 2 Δq 3 ] T The quaternion variation is used;
the inertial navigation speed is updated as follows:
wherein V= [ V N V E V D ] T Represents three-dimensional velocity information under the n-series,as a strapdown matrix, Δv= [ Δv ] x ΔV y ΔV z ] T Representing three-axis speed change information output by the accelerometer under the system b, g representing local gravitational acceleration, and delta t representing sampling time interval; Δa= [ Δa ] x ΔA y ΔA z ] T Constant zero offset information of the accelerometer under the b system is represented;
the inertial navigation position is updated as:
wherein: p= [ P ] N P E P D ] T Representing three-dimensional position information under n series;
set as N-E-DIs positioned at the current position of the carrier, x n ,y n In the geographical horizontal plane x n The axis is north along the tangent line of the local warp yarn, y n The axis being east, z along the tangent to the local weft n The axis is vertically downward to form a right hand coordinate system;
the b system refers to a carrier coordinate system, namely an AUV body coordinate system, the origin is positioned at the center of mass of the carrier, and x is the same as the origin b Axially forward of the carrier, y b Along the carrier to the right, z b Perpendicular to the plane of the carrier, the triaxial satisfies the right rule and rotates along with the rotation of the carrier;
defining the motion parameters of the AUV in 3-dimensional space and 6 degrees of freedom, wherein the motion parameters are specifically as follows:
(P N ,P E ,P D ): representing the position components of the AUV in three coordinate axes under a navigation coordinate system;
(V N ,V E ,V D ): representing the velocity components of the AUV in three coordinate axes under the navigational coordinate system;
(γ, θ, ψ): gamma is a roll angle, a counterclockwise direction is selected as a positive direction, and the value range is (-180 degrees, 180 degrees); θ is a pitch angle, the positive direction is the same as the roll angle, and the value range is (-90 degrees, 90 degrees); psi is yaw angle, left yaw is positive, and the value range is (-180 degrees, 180 degrees).
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010514731.3A CN111829512B (en) | 2020-06-08 | 2020-06-08 | AUV navigation positioning method and system based on multi-sensor data fusion |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010514731.3A CN111829512B (en) | 2020-06-08 | 2020-06-08 | AUV navigation positioning method and system based on multi-sensor data fusion |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111829512A CN111829512A (en) | 2020-10-27 |
| CN111829512B true CN111829512B (en) | 2024-04-09 |
Family
ID=72898524
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010514731.3A Active CN111829512B (en) | 2020-06-08 | 2020-06-08 | AUV navigation positioning method and system based on multi-sensor data fusion |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111829512B (en) |
Families Citing this family (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112630769B (en) * | 2020-11-30 | 2023-12-12 | 东南大学 | Radar flow velocity measurement system and method with unmanned aerial vehicle as carrier |
| CN112987067B (en) * | 2021-02-24 | 2023-06-27 | 博雅工道（北京）机器人科技有限公司 | Combined navigation ocean current speed estimation method and device and processor |
| CN113608169B (en) * | 2021-05-20 | 2023-08-25 | 济南大学 | Multi-AUV (autonomous Underwater vehicle) co-location method based on sequential fusion algorithm |
| CN113776514A (en) * | 2021-08-10 | 2021-12-10 | 深之蓝海洋科技股份有限公司 | Relative navigation method and device for detecting vertical surface under liquid by robot |
| CN113959434A (en) * | 2021-09-22 | 2022-01-21 | 河北汉光重工有限责任公司 | Adjustable SINS, DVL and USBL combined navigation method |
| CN114689039A (en) * | 2022-06-01 | 2022-07-01 | 青岛海舟科技有限公司 | Navigation method and system for short-time positioning missing of wave glider |
| CN115060274B (en) * | 2022-08-17 | 2022-11-18 | 南开大学 | Underwater integrated autonomous navigation device and initial alignment method thereof |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106643709A (en) * | 2016-10-10 | 2017-05-10 | 东南大学 | Combined navigation method and device for offshore carrier |
| CN109324330A (en) * | 2018-09-18 | 2019-02-12 | 东南大学 | Based on USBL/SINS tight integration navigation locating method of the mixing without derivative Extended Kalman filter |
| CN109579850A (en) * | 2019-01-16 | 2019-04-05 | 中国海洋大学 | Deep water intelligent navigation method based on speed through water auxiliary inertial navigation |
| CN109765523A (en) * | 2018-12-21 | 2019-05-17 | 山东省科学院海洋仪器仪表研究所 | Single transponder oblique distance hydrolocation method and system based on adaptive AKF |
| CN110006433A (en) * | 2019-04-22 | 2019-07-12 | 哈尔滨工程大学 | The integrated navigation and location system and method for sea-bottom oil-gas pipe detection robot |
| CN110057365A (en) * | 2019-05-05 | 2019-07-26 | 哈尔滨工程大学 | A kind of depth AUV dive localization method latent greatly |
| CN110207694A (en) * | 2019-05-27 | 2019-09-06 | 哈尔滨工程大学 | A kind of polar region grid inertial navigation/ultra-short baseline Combinated navigation method based on relative position information |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2018080921A1 (en) * | 2016-10-24 | 2018-05-03 | Seabed Geosolutions B.V. | Catenary modeling for a plurality of deployment lines for an offshore seismic system |
| WO2018204084A1 (en) * | 2017-05-02 | 2018-11-08 | Seabed Geosolutions B.V. | System and method for deploying ocean bottom seismic nodes using a plurality of underwater vehicles |
-
2020
- 2020-06-08 CN CN202010514731.3A patent/CN111829512B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106643709A (en) * | 2016-10-10 | 2017-05-10 | 东南大学 | Combined navigation method and device for offshore carrier |
| CN109324330A (en) * | 2018-09-18 | 2019-02-12 | 东南大学 | Based on USBL/SINS tight integration navigation locating method of the mixing without derivative Extended Kalman filter |
| CN109765523A (en) * | 2018-12-21 | 2019-05-17 | 山东省科学院海洋仪器仪表研究所 | Single transponder oblique distance hydrolocation method and system based on adaptive AKF |
| CN109579850A (en) * | 2019-01-16 | 2019-04-05 | 中国海洋大学 | Deep water intelligent navigation method based on speed through water auxiliary inertial navigation |
| CN110006433A (en) * | 2019-04-22 | 2019-07-12 | 哈尔滨工程大学 | The integrated navigation and location system and method for sea-bottom oil-gas pipe detection robot |
| CN110057365A (en) * | 2019-05-05 | 2019-07-26 | 哈尔滨工程大学 | A kind of depth AUV dive localization method latent greatly |
| CN110207694A (en) * | 2019-05-27 | 2019-09-06 | 哈尔滨工程大学 | A kind of polar region grid inertial navigation/ultra-short baseline Combinated navigation method based on relative position information |
Non-Patent Citations (1)
| Title |
|---|
| 光纤捷联惯导系统的阻尼及校正技术研究;卢曼曼;《中国优秀硕士论文电子期刊》;正文全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111829512A (en) | 2020-10-27 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111829512B (en) | AUV navigation positioning method and system based on multi-sensor data fusion | |
| CN104316045B (en) | A kind of AUV based on SINS/LBL interacts aided positioning system and localization method under water | |
| CN107314768B (en) | Underwater terrain matching auxiliary inertial navigation positioning method and positioning system thereof | |
| Bao et al. | Integrated navigation for autonomous underwater vehicles in aquaculture: A review | |
| Gade | The seven ways to find heading | |
| Zhang et al. | Autonomous underwater vehicle navigation: a review | |
| US8004930B2 (en) | Methods and systems for determining coordinates of an underwater seismic component in a reference frame | |
| CN106643709B (en) | Combined navigation method and device for offshore carrier | |
| CN103744098B (en) | AUV integrated navigation systems based on SINS/DVL/GPS | |
| CN111947651B (en) | Underwater combined navigation information fusion method and system and autonomous underwater vehicle | |
| CN113311436B (en) | Method for correcting wind measurement of motion attitude of laser wind measuring radar on mobile platform | |
| CN102829777A (en) | Integrated navigation system for autonomous underwater robot and method | |
| CN111380518B (en) | SINS/USBL tight combination navigation positioning method introducing radial velocity | |
| CN210719199U (en) | Multi-equipment combined navigation system of underwater robot | |
| Xu et al. | A novel self-adapting filter based navigation algorithm for autonomous underwater vehicles | |
| CN110806209A (en) | Underwater robot multi-device combined navigation system and method | |
| CN112556697A (en) | Shallow coupling data fusion navigation method based on federated structure | |
| CN110186461A (en) | A kind of collaborative navigation method based on gravity gradient information ranging | |
| CN106017460B (en) | A kind of underwater hiding-machine navigation locating method of terrain aided inertial navigation tight integration | |
| CN105547289A (en) | Underwater vehicle combined navigation system and navigation information fusion method | |
| CN111735455A (en) | Improved Gaussian distance iterative algorithm based butt joint recovery integrated navigation method | |
| CN112747748A (en) | Pilot AUV navigation data post-processing method based on reverse solution | |
| Armstrong et al. | Field measurement of surface ship magnetic signature using multiple AUVs | |
| CN112083425A (en) | SINS/LBL tight combination navigation method introducing radial velocity | |
| CN115930951A (en) | Short-term underwater AUV hidden navigation method based on multi-sensor combination |
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 |