CN111829512A - 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 PDF

Info

Publication number
CN111829512A
CN111829512A CN202010514731.3A CN202010514731A CN111829512A CN 111829512 A CN111829512 A CN 111829512A CN 202010514731 A CN202010514731 A CN 202010514731A CN 111829512 A CN111829512 A CN 111829512A
Authority
CN
China
Prior art keywords
navigation
auv
carrier
coordinate system
representing
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202010514731.3A
Other languages
Chinese (zh)
Other versions
CN111829512B (en
Inventor
卢曼曼
郭眀禹
李宇航
张南南
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
China Academy of Aerospace Aerodynamics CAAA
Original Assignee
China Academy of Aerospace Aerodynamics CAAA
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by China Academy of Aerospace Aerodynamics CAAA filed Critical China Academy of Aerospace Aerodynamics CAAA
Priority to CN202010514731.3A priority Critical patent/CN111829512B/en
Publication of CN111829512A publication Critical patent/CN111829512A/en
Application granted granted Critical
Publication of CN111829512B publication Critical patent/CN111829512B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

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 the environment is used as a main inertial navigation system, a GPS/USBL is used as an auxiliary positioning system, and a DVL is used as an auxiliary speed measuring system, so that the high-precision combined navigation system is designed by applying various sensors with lower precision. The method comprises the following steps: establishing an inertial navigation kinematics model; step two: defining the motion parameters of 3-dimensional space and 6 degrees of freedom of the AUV; the AUV refers to an unmanned underwater vehicle; step three: determining a basic form of the combined navigation system model; step four: and performing combined navigation combining inertial navigation, acoustic navigation and satellite navigation. The combined navigation mode is applied to the water surface and underwater motion test of the AUV, and has good positioning effect, the positioning error is about 10m, and the system has higher stability and practicability.

Description

AUV navigation positioning method and system based on multi-sensor data fusion
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
The underwater navigation problem remains one of the main technical challenges faced by underwater mobile platforms, and underwater positioning of an AUV (unmanned underwater vehicle) is extremely difficult due to the complexity and specificity of the underwater environment. The important point of attention of the scholars is how to overcome the influence of the underwater environment on underwater positioning and improve the accuracy of the underwater positioning, and the scholars are also the research focus in recent years. The influence of the underwater environment on the positioning of the AUV is mainly shown in the following aspects:
(1) the interference is much and the influence is large. In the ocean, not only the interference of sea surface wind, tide, ocean current, sea surface front, thermocline, density thermocline and the like exists, but also the influence of temperature, salinity, density, pressure and the like on the underwater positioning instrument exists.
(2) Visibility is low. Since light is attenuated and scattered much more strongly in seawater than in the atmosphere, the horizontal horizon in seawater is only one thousandth of that in the atmosphere. Usually, the sea visibility is 20m, and the specific value is influenced by the water quality and the water color.
(3) Electromagnetic waves cannot be transmitted in water, the transmission efficiency of acoustic signals is low, and the reliability is low. This has greatly limited 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 weight of the equipment. Due to the limitation of the volume and weight of the AUV, certain restrictions can be formed on the volume and weight of the underwater positioning equipment. Meanwhile, high-precision underwater positioning equipment is generally expensive, and is limited in application to some non-military aircraft.
(5) The problem of concealment. For military use AUVs, covertness is a prerequisite to performing a mission, and some acoustic or visual underwater positioning methods expose themselves.
In practical application, a proper underwater positioning method needs to be selected according to the actual operation requirements of the AUV and by combining the characteristics of the underwater environment. Not only the depth range of the AUV work, but also the flight time of the AUV to perform the task. Different underwater positioning methods have different application ranges, and the combination of various underwater positioning methods can improve the positioning accuracy.
Currently, researchers are striving to find suitable navigation and positioning methods for underwater environments. The conventional underwater navigation methods are classified into the following categories, as shown in fig. 1. However, a single navigation mode always has defects, the requirements of underwater long-time and long-range high-precision navigation cannot be met in terms of precision and reliability, part of navigation means can only be used as assistance, different navigation modes are subjected to advantage complementation through combined navigation means, and higher navigation performance can be obtained.
Disclosure of Invention
The invention aims to: in order to solve the problem of the defect of underwater single navigation, an optimal statistical theory is selected as a method, an information fusion technology is adopted to construct a combined navigation system, and length is selected and shortened according to each single navigation method, so that the precision requirement of a certain single navigation system can be reduced on the premise of not influencing the overall navigation precision, the navigation degree and the technical difficulty are reduced, and the method has the characteristics of high cost performance and high precision.
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:
the method comprises the following steps: establishing an inertial navigation kinematics model;
step two: defining the motion parameters of 3-dimensional space and 6 degrees of freedom of the AUV; the AUV refers to an unmanned underwater vehicle;
step three: determining a basic form of the combined navigation system model;
step four: and performing combined navigation combining inertial navigation, acoustic navigation and satellite navigation.
Further, the inertial navigation kinematics model specifically includes:
the inertial navigation attitude is updated as:
Figure BDA0002529736850000031
wherein Δ θ ═ Δ θxΔθyΔθz]TRepresenting the three-axis angle change information output by the gyroscope in a carrier coordinate system; 2 ═ 2x y z]TRepresenting gyro three-axis constant drift information under a carrier coordinate system; [ q ] of0q1q2q3]TRepresenting quaternion information, and updating quaternion by adopting a rotation vector method during attitude updating; Δ q ═ Δ q0Δq1Δq2Δq3]TIs the quaternion variation;
the inertial navigation speed is updated as:
Figure BDA0002529736850000032
Figure BDA0002529736850000033
wherein V is [ V ]NVEVD]TRepresents three-dimensional velocity information under n,
Figure BDA0002529736850000034
is a strapdown matrix, Δ V ═ Δ VxΔVyΔVz]TRepresenting three-axis speed change information output by the accelerometer under the system b, g representing local gravity acceleration, and delta t representing sampling time interval; Δ a ═ Δ axΔAyΔAz]TRepresenting the constant zero offset information of the accelerometer under the b series;
the inertial navigation position is updated as:
Figure BDA0002529736850000041
wherein: p ═ PNPEPD]TRepresents three-dimensional position information in n.
Further, the n system is a navigation coordinate system: geographical coordinate system, origin, defined as N-E-DAt the current position of the carrier, xn,ynIn the geographic horizontal plane, xnThe axis being north, y, along a tangent to the local meridiannThe axis is east, z along the tangent of the local latitudenThe axis is vertically downward to form a right-hand coordinate system;
the b system is a carrier coordinate system, namely an AUV (autonomous Underwater vehicle) body coordinate system, the origin is located at the center of mass of the carrier, and x isbAxially forward of the carrier, ybTo the right along the vector, zbPerpendicular to the carrier plane, the three axes satisfy the right-hand rule and rotate with the rotation of the carrier.
Further, defining the motion parameters of the AUV in 3-dimensional space and 6 degrees of freedom specifically includes:
(PN,PE,PD): the position components of the AUV under three coordinate axes are shown in a navigation coordinate system;
(VN,VE,VD): representing the velocity components of the AUV under three coordinate axes under a navigation coordinate system;
(γ, θ, ψ): gamma is a transverse roll angle, the anticlockwise direction is selected as the positive direction, and the value range is (-180 degrees and 180 degrees); theta is a pitch angle, the positive direction is the same as the roll angle, and the value range is (-90 degrees and 90 degrees); psi is the yaw angle, the left yaw is positive, the value range is (-180 DEG, 180 DEG).
Further, the basic form of the integrated navigation system model is
Figure BDA0002529736850000042
Wherein X is a state quantity, phik-1Is a state transition matrix, Z is a quantity measurement, HkTo measure the matrix, wk-1And vkRepresenting process noise and measurement noise, obeying a normal distribution wk-1～(0,Qk)，vk～(0,Rk)，QkIs the process noise variance and is not negative; rkIs a measure of the variance of the noise and is positive.
Further, performing combined navigation combining inertial navigation, acoustic navigation and satellite navigation, specifically: (1) determining a state equation, specifically:
the state quantity X ═ q0q1q2q3VNVEVDPNPEPD x y zΔAxΔAyΔAz]T(ii) a State transition matrix phik-1The following were used:
Figure BDA0002529736850000051
wherein:
Figure BDA0002529736850000052
Figure BDA0002529736850000053
Figure BDA0002529736850000054
Figure BDA0002529736850000055
Figure BDA0002529736850000056
Figure BDA0002529736850000057
Figure BDA0002529736850000058
t21＝2q1(ΔVx-ΔAx),t22＝2q0(ΔVy-ΔAy),t23＝2q1(ΔVz-ΔAz)
t24＝2q0(ΔVx-ΔAx)+2q2(ΔVy-ΔAy)+2q3(ΔVz-ΔAz)
t25＝2q0(ΔVx-ΔAx),t26＝2q2(ΔVz-ΔAz),t37＝2q3(ΔVy-ΔAy)
t27＝2q0(ΔVx-ΔAx)+2q2(ΔVz-ΔAz)+2q3(ΔVy-ΔAy)
t28＝2q0q3,t29＝q0 2,t30＝q1 2,t31＝q2 2,t32＝q3 2,t33＝2q2(ΔVx-ΔAx)
t34＝2q3(ΔVx-ΔAx),t35＝2q0(ΔVy-ΔAy),t38＝2q0q1
t36＝2q3(ΔVx-ΔAx)+2q1(ΔVz-ΔAz)+2q0(ΔVy-ΔAy)
state noise wkVariance Q ofkThe method specifically comprises the following steps:
Figure BDA0002529736850000061
wherein:
Qq＝BBT,Qv＝AAT
Figure BDA0002529736850000062
Figure BDA0002529736850000063
(2) determining a measurement equation:
the USBL/GPS measurement matrix is:
Figure BDA0002529736850000064
the DVL measurement matrix is:
Figure BDA0002529736850000065
the magnetometer measurement matrix is:
Hmag＝[-2l8l14(q3l3-q3l4+q3l5+q3l6+2q0q1q2)；-2l8l14(-q2l3+q2l4+q2l5+q2l6+2q0q1q3)；
2l8l14(q1l3+q1l4+q1l5-q1l6+2q0q2q2)；2l8l14(q0l3+q0l4-q0l5+q0l6+2q1q2q3)；o1×12]1×16
wherein:
Figure BDA0002529736850000071
Figure BDA0002529736850000072
Figure BDA0002529736850000073
measurement noise vkVariance R ofkAccording to the actual USBL/GPSSelecting a DVL and magnetometer system model;
(3) estimating a state quantity X through a state equation and a measurement equation;
(4) by [ q ]0q1q2q3]TCalculating gamma, theta, psi;
(5) (P) in the state quantity X obtained in the step (3)N,PE,PD) And (V)N,VE,VD) And (4) performing combined navigation with the gamma, theta and psi obtained in the step (4) to position the AUV in real time.
Further, the acoustic navigation is a navigation mode of providing position information through the USBL and providing speed information through the DVL, the USBL is an ultra-short baseline positioning system, and the DVL is a Doppler velocity measurement system; satellite navigation provides location information via GPS; the magnetometer is integrated in the inertial navigation system and used for providing course information.
Further, the present invention also provides an AUV integrated navigation system, including:
a model building module: establishing an inertial navigation kinematics model;
the motion parameter definition module: defining the motion parameters of 3-dimensional space and 6 degrees of freedom of the AUV; the AUV refers to an unmanned underwater vehicle;
the integrated navigation system model determining module: determining a basic form of the combined navigation system model;
the combined navigation module: and performing combined navigation combining inertial navigation, acoustic navigation and satellite navigation.
Compared with the prior art, the invention has the advantages that:
(1) the invention finally selects the combination mode of an inertial navigation System and a GPS on the water surface, and selects the combination navigation mode of the inertial navigation System, a Doppler Velocity Log (DVL) System and an Ultra-Short Baseline positioning System (USBL) under water, as shown in figure 2.
(1) The GPS signal on the water surface is better, the positioning precision is higher, and the combination of inertial navigation and GPS is adopted, so that the calculated amount is reduced, and the calculation speed is improved.
(2) Because the USBL speed is low, a combination mode of inertial navigation and DVL speed is selected to obtain positioning information, but divergence caused by error accumulation occurs for a long time, and therefore the positioning information of the system is corrected at regular time by taking the USBL position information as a correction auxiliary means.
Drawings
FIG. 1 is a schematic diagram of classification of underwater navigation modes
FIG. 2 is a schematic view of AUV integrated navigation mode
FIG. 3. solving block diagram of inertial navigation system
FIG. 4 is a schematic diagram of the positioning of the ultra-short baseline positioning system
FIG. 5.USBL system coordinate block diagram
FIG. 6 is a four-beam space geometry map
FIG. 7 is a schematic diagram showing the comparison between the GPS track and the combined track
FIG. 8 is a diagram showing the comparison between the reference track and the combined track
Detailed Description
The present invention will be described in further detail with reference to the accompanying 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 the environment is used as a main inertial navigation system, a GPS/USBL is used as an auxiliary positioning system, and a DVL is used as an auxiliary speed measuring system, so that the high-precision combined navigation system is designed by applying various sensors with lower precision. Aiming at the problem of inconsistent updating frequency of each sensor in the integrated navigation, the invention adopts the 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 updates time to complete the state prediction process, and after the information of the external equipment is acquired, the measurement is updated.
The DVL is a common speed measurement navigation system, and can be combined with the inertial navigation system to inhibit the problem that pure inertial navigation errors are dispersed along with time. Therefore, the GPS/USBL can be used for providing position measurement, the DVL is used for providing speed measurement, a combined navigation system is formed, and the inertial navigation system is corrected.
The combined state of the inertial navigation system + GPS + DVL can be preferentially used when the GPS signal is received, and the combined mode of the inertial navigation system + USBL + DVL is adopted when the GPS signal cannot be obtained, so that the system is navigated with high precision. The combined navigation mode is applied to the water surface and underwater motion test of the AUV, so that a better positioning effect is obtained, the positioning error is about 10m, and the system has higher stability and practicability.
The unmanned aerial vehicle navigation control system mostly adopts inertial navigation and optical flow sensors, a GPS positioning system, ultrasonic waves and other integrated navigation methods to carry out real-time navigation control, and the underwater special environment greatly limits the application of some positioning methods, so that when the unmanned underwater vehicle is underwater, the GPS has no signal, and the optical flow sensors cannot acquire speed, so that the sensors replacing the GPS and the optical flow are sought for underwater navigation positioning, and the specific steps are as follows:
the method comprises the following steps: according to the positioning, speed measuring and attitude determining principles of navigation modes such as inertial navigation, acoustic navigation, satellite navigation and the like, a kinematic model is established:
1. inertial navigation system kinematics model
The block diagram of fig. 3 is a resolving process of the inertial navigation system, and a kinematic formula is expressed as follows:
(1) updating the inertial navigation attitude, solving the attitude strapdown matrix is the basis and core of navigation of the SINS,
Figure BDA0002529736850000101
in formula (1), Δ θ ═ Δ θxΔθyΔθz]TRepresenting the three-axis angle change information output by the gyroscope in a carrier coordinate system; 2 ═ 2x y z]TRepresenting gyro three-axis constant drift information under a carrier coordinate system; [ q ] of0q1q2q3]TExpressing quaternion information, and updating quaternion by adopting a rotation vector method during attitude updating; Δ q ═ Δ q0Δq1Δq2Δq3]TIs the quaternion variation.
(2) Inertial navigation velocity update
The velocity updating equation converts the information of the three-axis accelerometer into n systems through the attitude matrix to obtain velocity vectors, and then the vector differential equation in the velocity updating link is as follows:
Figure BDA0002529736850000102
in the formula (2), V ═ VNVEVD]TRepresents three-dimensional velocity information under n,
Figure BDA0002529736850000103
is a strapdown matrix, Δ V ═ Δ VxΔVyΔVz]TRepresenting three-axis speed change information output by the accelerometer under the system b, g representing local gravity acceleration, and delta t representing sampling time interval; Δ a ═ Δ axΔAyΔAz]TAnd b represents the constant zero offset information of the accelerometer.
Unfolding equation (2) into a three-axis form:
Figure BDA0002529736850000111
wherein
Figure BDA0002529736850000112
The n system is a navigation coordinate system: a geographical coordinate system defined as N-E-D, with the origin at the current position of the carrier, xn,ynIn the geographic horizontal plane, xnThe axis being north, y, along a tangent to the local meridiannThe axis is east, z along the tangent of the local latitudenThe axis is vertically downward to form a right-hand coordinate system;
the b system is a carrier coordinate system, namely an AUV (autonomous Underwater vehicle) body coordinate system, the origin is located at the center of mass of the carrier, and x isbAxially forward of the carrier, ybTo the right along the vector, zbPerpendicular to the plane of the carrierThe three axes satisfy the right hand rule and rotate along with the rotation of the carrier.
(3) Inertial navigation position update
The position updating is mainly to obtain the motion of the carrier relative to the earth surface by integrating the velocity, and the position updating equation is
Figure BDA0002529736850000113
Wherein: p ═ PNPEPD]TRepresents three-dimensional position information in n.
2. Ultra-short baseline positioning system (USBL) model
The ultra-short baseline positioning system calculates the slope distance by measuring the round-trip time of a known transponder arranged on the seabed of a target sea area when the transmitter sends underwater acoustic signals underwater. And calculating the azimuth angle of the carrier according to the phase difference of the underwater sound signals transmitted to each hydrophone of the sound head.
FIG. 4 is a schematic diagram of the positioning principle of the USBL. And (3) taking the center O of the basic array as the origin of coordinates, taking the reverse direction of the sky as the forward direction of the z axis, and establishing a basic array coordinate system oxyz according to the right-hand rule. Four hydrophones are located on the x-axis and the y-axis of a coordinate system, the distance between the coaxial hydrophones is d, and the coordinates of each point are (+/-d/2 and +/-d/2). The responder position is at T, with coordinates of (x)a,ya,za) Readily available from fig. 4:
xa＝l cosθx
ya＝l cosθy
Figure BDA0002529736850000121
in the USBL positioning schematic diagram, l is the slope distance, m is the projection of the slope distance l on the horizontal plane oxy, theta is the horizontal angle, namely the included angle between m and the x axis, thetaxAnd thetayRespectively, the included angles of the l, the x axis and the y axis.
Figure BDA0002529736850000122
Figure BDA0002529736850000123
Figure BDA0002529736850000124
The wavelength of the acoustic wave is set to be lambda,
Figure BDA0002529736850000125
phase difference representing the reception of the hydrophone underwater acoustic signals on-axis:
Figure BDA0002529736850000126
Figure BDA0002529736850000127
by substituting the formula (7) into the formula (5)
Figure BDA0002529736850000128
Figure BDA0002529736850000129
Pitch l ═ c · Tri/2，TriThe transmission speed of the sound wave under water is c, which is the round trip time from the interrogator sending the acoustic signal to the transponder returning the acoustic signal.
Figure BDA00025297368500001210
Can be obtained by ultra-short baseline measurement, thereby calculating the horizontal position (x) of the target relative to the matrix coordinate systema,ya) Depth zaAngle of incidence (theta) of sound wavex,θy) And the like.
Coordinate conversion of USBL navigation resolving equation, the geometric center of hydrophone in USBL is set as coordinate origin, and ox is used in northeast sky coordinate systemtytztAnd (4) showing. 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 a parallel relation in a three-axis space. The distribution of three coordinate systems of u system (b system), t system and 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 terrestrial coordinate system, where s is a known position vector since the transponder is previously arranged on the sea floor. r is the position vector of the transponder in the carrier coordinate system, for the transformation matrix from the carrier coordinate system to the terrestrial coordinate system
Figure RE-GDA0002681180990000131
To show that the position vector of the transponder can be obtained by the vector theorem as follows, the USBL positioning model is obtained:
r＝RT(s-p) (9)
3. doppler velocimetry system (DVL) model
The DVL is one of important navigation means for speed measurement of a marine navigation carrier at the present stage, can provide accurate speed measurement information of the carrier during long-term navigation based on the Doppler effect principle, can form good mutual complementarity with a strapdown inertial navigation system, and is further widely applied to various fields.
The doppler effect is a phenomenon related to microwave technology. In 1842, Doppler, a well-known physicist, inadvertently studied acoustical problems, discovered the phenomenon that the transmitted frequency f, when there was a relative displacement between the transmitted and reflected sound objects1And reflection frequency f2And are not consistent. The difference between the transmitted and reflected frequencies is linear with the relative velocity between the two, and this difference is the doppler shift. Based on the effect, scientists developed doppler log, doppler radar and other systems, and the following mainly describes a velocity measurement method of a doppler system based on four beams.
The beam space and reflection point positions of a four beam doppler system are shown in figure 6,
setting the speed of a carrier as 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
Figure BDA0002529736850000141
In the formula, Ai,Bi,Ci,Di(i ═ x, y, z) is the directional cosine of each of the four beams, satisfying
Figure BDA0002529736850000142
By substituting formula (11) into formula (10), the compound can be obtained
Figure BDA0002529736850000143
And (3) simultaneously solving a formula (12) to obtain the three-axis direction speed and obtain a DVL speed measurement model:
Figure BDA0002529736850000144
in the formula
Figure BDA0002529736850000145
And aiming at the principle of the navigation mode, the limitation is analyzed. In order to meet the navigation requirements of long range and high precision, a combined navigation technology of multi-sensor information fusion is provided:
the error accumulation of the Inertial Navigation System (INS) for long-time navigation is serious, and needs to be corrected regularly; the ultra-short baseline positioning system has a small positioning range and poor autonomy and needs to be supported by a ground end; the GPS is greatly influenced by external environmental factors such as weather and the like, and has no signal for a long time in water; the speed measurement precision of the Doppler velocimeter is bound to 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 is large due to factors such as installation errors, water temperature, water speed, water depth and salt concentration in water.
The inertial navigation system measures angular velocity and acceleration through a gyroscope and an accelerometer, and obtains navigation parameters such as position, velocity and attitude through solution. The navigation can be carried out independently without depending on external information, and the navigation positioning system becomes mainstream at present. However, due to the inherent characteristics of the inertial device, the generated error can be accumulated along with time, the DVL is a common speed measurement navigation system, and the DVL and the INS are combined together to form an INS/DVL combined navigation subsystem, so that the problem that the pure inertial navigation error is dispersed along with time can be solved. Therefore, the GPS/USBL can be used for providing position measurement, the DVL can be used for providing speed measurement, and an integrated navigation system is formed to correct the inertial navigation system. The combination state of the INS + GPS + DVL can be preferentially used when the GPS signal is received, and the combination mode of the INS + USBL + DVL is adopted when the GPS signal cannot be obtained, thereby performing high-precision navigation on the system, as shown in fig. 2.
Step two: defining the 3-dimensional space of the AUV unmanned underwater vehicle, and the motion characteristic parameters of 6 degrees of freedom:
when the AUV performs a task underwater, specific parameters are needed to describe the motion characteristics of the AUV. In the following, the motion parameters of the AUV in the navigation coordinate system are defined:
navigation coordinate system: usually given as (N-E-D) with the origin at the current position of the carrier, xn,ynIn the geographic horizontal plane, xnThe axis being north, y, along a tangent to the local meridiannThe axis is east, z along the tangent of the local latitudenThe axis is vertically downward to form a right-hand coordinate system;
(PN,PE,PD): the position components of the AUV under three coordinate axes are shown in a navigation coordinate system;
(VN,VE,VD): representing the velocity components of the AUV under three coordinate axes under a navigation coordinate system;
(γ, θ, ψ): gamma is a transverse roll angle, the anticlockwise direction is selected as the positive direction, and the value range is (-180 degrees and 180 degrees); theta is a pitch angle, the positive direction is the same as the roll angle, and the value range is (-90 degrees and 90 degrees); psi is the yaw angle, the left yaw is positive, the value range is (-180 DEG, 180 DEG).
(p,ωθω): indicating the angular velocity of the AUV along three axes in the navigational coordinate system, defining a counterclockwise direction as positive.
Step three: determining a basic form of the combined navigation system model;
the basic form of the combined navigation system model is
Figure BDA0002529736850000161
Wherein X is a state quantity, phik-1Is a state transition matrix, Z is a quantity measurement, HkTo measure the matrix, wk-1And vkRepresenting process noise and measurement noise, obeying a normal distribution wk-1～(0,Qk)，vk～(0,Rk)，QkIs the process noise variance and is not negative; rkIs a measure of the variance of the noise and is positive.
Step four: and performing combined navigation combining inertial navigation, acoustic navigation and satellite navigation.
Applying the designed integrated navigation algorithm to an entity AUV, wherein the hardware equipment of the navigation system comprises the following components in a table 1:
TABLE 1 navigation hardware device composition
Figure BDA0002529736850000162
The inertial navigation system SBG is internally integrated with a magnetometer and can depend on the device as a course reference.
The method comprises the following steps:
(1) selecting three-axis attitude, three-axis speed and three-axis position of AUV in navigation system, three-axis constant gyroscope drift and three-axis constant accelerometer zero offset as state quantities, and determining state equation
The state quantity X ═ q0q1q2q3VNVEVDPNPEPD x y zΔAxΔAyΔAz]T；
State transition matrix phik-1The following were used:
Figure BDA0002529736850000171
wherein:
Figure BDA0002529736850000172
Figure BDA0002529736850000173
Figure BDA0002529736850000174
Figure BDA0002529736850000175
Figure BDA0002529736850000176
t21＝2q1(ΔVx-ΔAx),t22＝2q0(ΔVy-ΔAy),t23＝2q1(ΔVz-ΔAz)
t24＝2q0(ΔVx-ΔAx)+2q2(ΔVy-ΔAy)+2q3(ΔVz-ΔAz)
t25＝2q0(ΔVx-ΔAx),t26＝2q2(ΔVz-ΔAz),t37＝2q3(ΔVy-ΔAy)
t27＝2q0(ΔVx-ΔAx)+2q2(ΔVz-ΔAz)+2q3(ΔVy-ΔAy)
t28＝2q0q3,t29＝q0 2,t30＝q1 2,t31＝q2 2,t32＝q3 2,t33＝2q2(ΔVx-ΔAx)
t34＝2q3(ΔVx-ΔAx),t35＝2q0(ΔVy-ΔAy),t38＝2q0q1
t36＝2q3(ΔVx-ΔAx)+2q1(ΔVz-ΔAz)+2q0(ΔVy-ΔAy)
Δθx,Δθy,Δθzfor three-axis gyroscope output, Δ Vx,ΔVy,ΔVzIs the triaxial accelerometer output.
State noise wkVariance Q ofkThe method specifically comprises the following steps:
Figure BDA0002529736850000181
wherein:
Qq＝BBT,Qv＝AAT
Figure BDA0002529736850000182
Figure BDA0002529736850000183
(2) and determining a measurement equation. And defining a measurement matrix H according to the second step, and taking the position of the USBL, the speed of the DVL and the magnetic heading of the magnetometer as measurement quantities. Wherein the DVL provides AUV speed under the carrier system, and coordinate conversion exists between the AUV speed and the state quantity; the magnetic heading provided by the magnetometer has no direct relation with the state quantity, and also needs to be transformed, wherein the transformation method comprises the following steps:
1) fused USBL position (same as fused GPS position):
because the USBL equipment provides the relative position of the water surface end and the underwater transducer, the underwater transducer 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:
Figure BDA0002529736850000184
2) velocity of the fused DVL:
converting the speed of the carrier system (b system) to the speed of the navigation system (n system) to obtain the DVL measurement matrix
Figure BDA0002529736850000191
3) Fusing magnetic heading information:
the magnetic heading can be calculated by the triaxial information provided by the magnetometer (mag), the magnetic heading is acted on the state quantity quaternion,
Figure BDA0002529736850000192
wherein, QuatToEual is a quaternion-to-Euler angle function, QuatToTbn is a quaternion-to-attitude matrix function, and EulToQuat is a quaternion-to-Euler angle function. And (19) calculating a course angle through a quaternion at the current moment to serve as one-step pre-measurement, setting the course angle to be 0, calculating an attitude matrix when the course angle is 0, converting magnetometer information under a system b into a system n through the attitude matrix when the course angle is 0, calculating the course at the current moment to serve as measurement, and only carrying out deviation of the course angle between two coordinate systems at the moment.
Derivation is carried out on the measurement formula of the course to obtain a mag measurement matrix:
Figure BDA0002529736850000193
wherein:
Figure BDA0002529736850000194
measurement noise vkVariance R ofkAnd selecting according to the actual USBL/GPS, DVL and magnetometer system models.
The updating frequency of the inertial navigation system is set to be 100Hz, the magnetometer is integrated in 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 frequency inconsistency, the system adopts a fusion mode of sequential Kalman filtering to carry out optimal estimation.
(3) Estimating a state quantity X through a state equation and a measurement equation;
(4) by [ q ]0q1q2q3]TCalculating gamma, theta, psi;
(5) (P) in the state quantity X obtained in the step (3)N,PE,PD) And (V)N,VE,VD) And (4) performing combined navigation with the gamma, theta and psi obtained in the step (4) to position the AUV in real time.
The acoustic navigation is a navigation mode of providing position information through USBL (universal serial bus) and providing speed information through DVL (digital visual layer), wherein USBL is an ultra-short baseline positioning system, and DVL is a Doppler velocity measurement system; satellite navigation provides location information via GPS; the magnetometer is integrated in the inertial navigation system and used for providing course information.
The implementation process comprises the following steps:
the first embodiment is as follows:
and performing a water surface test on the AUV, and performing test verification of a combined navigation algorithm on the official reservoir with the longitude and latitude of 40.3457756 degrees N and 115.7364502 degrees E. And simultaneously, data of the SBG, the DVL and the GPS are collected, and the motion trail is approximately triangular. The error of the navigation equipment is equivalent to zero mean Gaussian white noise, the drift noise of a gyroscope is 0.2rad/s, the zero offset noise of an accelerometer is 0.35m/s2, the magnetic heading noise is 0.1745rad, the error noise of a GPS position is 0.3m, and the speed error noise of a DVL is 0.3m/s, because the GPS signal is good when the test is carried out on the water surface, more than 10 satellites can be searched at the position, the combined navigation algorithm in four steps and the combined mode of IMU + GPS + DVL are adopted for the collected data, and the combined result is compared with the track of the GPS, as shown in figure 7.
Example two:
and carrying out underwater test on the AUV, and carrying out test verification of a combined navigation algorithm on the official reservoir with the longitude and latitude of 40.3434715 degrees N and 115.7399673 degrees E. And simultaneously collecting data of SBG, DVL and USBL, and setting navigation parameters consistent with embodiment 1. At this time, the GPS can not search the satellite, the experimental data is analyzed and processed by adopting the combined positioning mode of IMU + USBL + DVL, and the combined result is compared with the high-precision inertial navigation carried by the AUV body, as shown in FIG. 8
The implementation effect is as follows:
the positioning accuracy of the GPS is higher when the satellite number is more, but the updating frequency of the GPS is lower and is one percent of the updating frequency of the inertial navigation system, and the GPS is greatly influenced by environmental factors and is free of interference at the place, so the GPS is used as a reference. From the comparison results of the first example, it is found that the accuracy of the combination of IMU + GPS + DVL is within 10 m.
From the results of the second example, it was calculated whether the distances between the reference trajectory and the combined trajectory were 450.7m and 438.1m, respectively, with an accuracy of about 10 m.
When the AUV moves on the water surface, the GPS has drift, reaches the maximum (0-8m) when the AUV is static, and has lower updating frequency, and the position error can be reduced to be within 10m after the AUV is combined with an inertial navigation system and a Doppler velocity measurement system. If the underwater motion is performed, the inertial navigation system is only combined with the ultra-short baseline positioning system, the updating 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 clearance of the ultra-short baseline positioning system can be dispersed, so that the combined navigation of the inertial navigation system, the Doppler velocity measurement system and the ultra-short baseline positioning system is adopted, the position error can be reduced to about 10m after the combination, the method can realize the underwater navigation positioning with high precision for a long time, the performance of a single system is obviously improved, and the feasibility is realized.

Claims (10)

1. An AUV integrated navigation method based on multi-sensor data fusion is characterized by comprising the following steps:
the method comprises the following steps: establishing an inertial navigation kinematics model;
step two: defining the motion parameters of 3-dimensional space and 6 degrees of freedom of the AUV; the AUV refers to an unmanned underwater vehicle;
step three: determining a basic form of the combined navigation system model;
step four: and performing combined navigation combining inertial navigation, acoustic navigation and satellite navigation.
2. The AUV integrated navigation method based on multi-sensor data fusion of claim 1, wherein: the inertial navigation kinematics model specifically comprises the following steps:
the inertial navigation attitude is updated as:
Figure FDA0002529736840000011
wherein Δ θ ═ Δ θxΔθyΔθz]TRepresenting the three-axis angle change information output by the gyroscope in a carrier coordinate system; 2 ═ 2x y z]TRepresenting gyro three-axis constant drift information under a carrier coordinate system; [ q ] of0q1q2q3]TExpressing quaternion information, and updating quaternion by adopting a rotation vector method during attitude updating; Δ q ═ Δ q0Δq1Δq2Δq3]TIs the quaternion variation;
the inertial navigation speed is updated as:
Figure FDA0002529736840000021
Figure FDA0002529736840000022
wherein V is [ V ]NVEVD]TRepresents three-dimensional velocity information under n,
Figure FDA0002529736840000024
is a strapdown matrix, Δ V ═ Δ VxΔVyΔVz]TRepresenting three-axis speed change information output by the accelerometer under the system b, g representing local gravity acceleration, and delta t representing sampling time interval; Δ a ═ Δ axΔAyΔAz]TRepresenting the constant zero offset information of the accelerometer under the b series;
the inertial navigation position is updated as:
Figure FDA0002529736840000023
wherein: p ═ PNPEPD]TRepresents three-dimensional position information in n.
3. The AUV integrated navigation method based on multi-sensor data fusion of claim 2, wherein: the n system is a navigation coordinate system: a geographical coordinate system defined as N-E-D, with the origin at the current position of the carrier, xn,ynIn the geographic horizontal plane, xnThe axis being north, y, along a tangent to the local meridiannThe axis is east, z along the tangent of the local latitudenThe axis is vertically downward to form a right-hand coordinate system;
the b system is a carrier coordinate system, namely an AUV (autonomous Underwater vehicle) body coordinate system, the origin is located at the center of mass of the carrier, and x isbAxially forward of the carrier, ybTo the right along the vector, zbPerpendicular to the plane of the carrier, the three axes satisfy the right-hand rule and rotate along with the rotation of the carrier.
4. The AUV integrated navigation method based on multi-sensor data fusion of claim 3, wherein: defining the motion parameters of 3-dimensional space and 6 degrees of freedom of the AUV, specifically:
(PN,PE,PD): is shown in the guideUnder the navigation coordinate system, the position components of the AUV under three coordinate axes;
(VN,VE,VD): representing the velocity components of the AUV under three coordinate axes under a navigation coordinate system;
(γ, θ, ψ): gamma is a transverse roll angle, the anticlockwise direction is selected as the positive direction, and the value range is (-180 degrees and 180 degrees); theta is a pitch angle, the positive direction is the same as the roll angle, and the value range is (-90 degrees and 90 degrees); psi is the yaw angle, the left yaw is positive, the value range is (-180 DEG, 180 DEG).
5. The AUV integrated navigation method based on multi-sensor data fusion of claim 4, wherein: the basic form of the combined navigation system model is
Figure FDA0002529736840000031
Wherein X is a state quantity, phik-1Is a state transition matrix, Z is a quantity measurement, HkTo measure the matrix, wk-1And vkRepresenting process noise and measurement noise, obeying a normal distribution wk-1～(0,Qk)，vk～(0,Rk)，QkIs the process noise variance and is not negative; rkIs a measure of the variance of the noise and is positive.
6. The AUV integrated navigation method based on multi-sensor data fusion of claim 5, wherein: the method for carrying out the combined navigation combining the inertial navigation, the acoustic navigation and the satellite navigation specifically comprises the following steps:
(1) determining a state equation, specifically:
the state quantity X ═ q0q1q2q3VNVEVDPNPEPD x y zΔAxΔAyΔAz]T；
State transition matrix phik-1The following were used:
Figure FDA0002529736840000032
wherein:
Figure FDA0002529736840000041
Figure FDA0002529736840000042
Figure FDA0002529736840000043
Figure FDA0002529736840000044
Figure FDA0002529736840000045
Figure FDA0002529736840000046
t15＝ΔVx-ΔAx,t16＝ΔVy-ΔAy,t17＝ΔVz-ΔAz
Figure FDA0002529736840000047
t21＝2q1(ΔVx-ΔAx),t22＝2q0(ΔVy-ΔAy),t23＝2q1(ΔVz-ΔAz)
t24＝2q0(ΔVx-ΔAx)+2q2(ΔVy-ΔAy)+2q3(ΔVz-ΔAz)
t25＝2q0(ΔVx-ΔAx),t26＝2q2(ΔVz-ΔAz),t37＝2q3(ΔVy-ΔAy)
t27＝2q0(ΔVx-ΔAx)+2q2(ΔVz-ΔAz)+2q3(ΔVy-ΔAy)
t28＝2q0q3,t29＝q0 2,t30＝q1 2,t31＝q2 2,t32＝q3 2,t33＝2q2(ΔVx-ΔAx)
t34＝2q3(ΔVx-ΔAx),t35＝2q0(ΔVy-ΔAy),t38＝2q0q1
t36＝2q3(ΔVx-ΔAx)+2q1(ΔVz-ΔAz)+2q0(ΔVy-ΔAy)
state noise wkVariance Q ofkThe method specifically comprises the following steps:
Figure FDA0002529736840000048
wherein:
Qq＝BBT,Qv＝AAT
Figure FDA0002529736840000051
Figure FDA0002529736840000052
(2) determining a measurement equation:
the USBL/GPS measurement matrix is:
Figure FDA0002529736840000053
the DVL measurement matrix is:
Figure FDA0002529736840000054
the magnetometer measurement matrix is:
Hmag＝[-2l8l14(q3l3-q3l4+q3l5+q3l6+2q0q1q2)；-2l8l14(-q2l3+q2l4+q2l5+q2l6+2q0q1q3)；2l8l14(q1l3+q1l4+q1l5-q1l6+2q0q2q2)；2l8l14(q0l3+q0l4-q0l5+q0l6+2q1q2q3)；o1×12]1×16wherein:
l9＝q0q3l10＝q1q2l2＝l9+l10
Figure FDA0002529736840000055
Figure FDA0002529736840000056
l7＝l3+l4-l5-l6
Figure FDA0002529736840000057
t12＝4l8l11l13＝1/(l12+1.0)
measurement noise vkVariance R ofkSelecting according to the actual USBL/GPS, DVL and magnetometer system models;
(3) estimating a state quantity X through a state equation and a measurement equation;
(4) by [ q ]0q1q2q3]TCalculating gamma, theta, psi;
(5) (P) in the state quantity X obtained in the step (3)N,PE,PD) And (V)N,VE,VD) And (4) realizing the combined navigation real-time positioning of the AUV with the gamma, theta and psi obtained in the step (4).
7. The AUV integrated navigation method based on multi-sensor data fusion of claim 6, wherein: the acoustic navigation is a navigation mode of providing position information through USBL (universal serial bus) and providing speed information through DVL (digital visual layer), wherein USBL is an ultra-short baseline positioning system, and DVL is a Doppler velocity measurement system; satellite navigation provides location information via GPS; the magnetometer is integrated in the inertial navigation system and used for providing course information.
8. The AUV integrated navigation method based on multi-sensor data fusion of claim 6, wherein: when the AUV is on the water surface, the combined navigation mode of inertial navigation, GPS and DVL is adopted, and when the AUV is under the water, the combined navigation mode of inertial navigation, DVL and USBL is selected.
9. An AUV integrated navigation system realized by the AUV integrated navigation method based on multi-sensor data fusion according to claim 1, and comprising:
a model building module: establishing an inertial navigation kinematics model;
the motion parameter definition module: defining the motion parameters of 3-dimensional space and 6 degrees of freedom of the AUV; the AUV refers to an unmanned underwater vehicle;
the integrated navigation system model determining module: determining a basic form of the combined navigation system model;
the combined navigation module: and performing combined navigation combining inertial navigation, acoustic navigation and satellite navigation.
10. The AUV integrated navigation system according to claim 9, wherein: the inertial navigation kinematics model specifically comprises the following steps:
the inertial navigation attitude is updated as:
Figure FDA0002529736840000071
wherein Δ θ ═ Δ θxΔθyΔθz]TRepresenting the three-axis angle change information output by the gyroscope in a carrier coordinate system; 2 ═ 2x y z]TRepresenting gyro three-axis constant drift information under a carrier coordinate system; [ q ] of0q1q2q3]TExpressing quaternion information, and updating quaternion by adopting a rotation vector method during attitude updating; Δ q ═ Δ q0Δq1Δq2Δq3]TIs the quaternion variation;
the inertial navigation speed is updated as:
Figure FDA0002529736840000072
Figure FDA0002529736840000073
wherein V is [ V ]NVEVD]TRepresents three-dimensional velocity information under n,
Figure FDA0002529736840000074
is a strapdown matrix, Δ V ═ Δ VxΔVyΔVz]TRepresenting three-axis speed change information output by the accelerometer under the system b, g representing local gravity acceleration, and delta t representing sampling time interval; Δ a ═ Δ axΔAyΔAz]TIs represented by b isConstant zero offset information of the accelerometer;
the inertial navigation position is updated as:
Figure FDA0002529736840000075
wherein: p ═ PNPEPD]TRepresenting three-dimensional position information under n series;
a geographical coordinate system defined as N-E-D, with the origin at the current position of the carrier, xn,ynIn the geographic horizontal plane, xnThe axis being north, y, along a tangent to the local meridiannThe axis is east, z along the tangent of the local latitudenThe axis is vertically downward to form a right-hand coordinate system;
the b system is a carrier coordinate system, namely an AUV (autonomous Underwater vehicle) body coordinate system, the origin is located at the center of mass of the carrier, and x isbAxially forward of the carrier, ybTo the right along the vector, zbThe three axes meet the right-hand rule and rotate along with the rotation of the carrier, and are vertical to the plane of the carrier;
defining the motion parameters of 3-dimensional space and 6 degrees of freedom of the AUV, specifically:
(PN,PE,PD): the position components of the AUV under three coordinate axes are shown in a navigation coordinate system;
(VN,VE,VD): representing the velocity components of the AUV under three coordinate axes under a navigation coordinate system;
(γ, θ, ψ): gamma is a transverse roll angle, the anticlockwise direction is selected as the positive direction, and the value range is (-180 degrees and 180 degrees); theta is a pitch angle, the positive direction is the same as the roll angle, and the value range is (-90 degrees and 90 degrees); psi is the yaw angle, the left yaw is positive, the value range is (-180 DEG, 180 DEG).
CN202010514731.3A 2020-06-08 2020-06-08 AUV navigation positioning method and system based on multi-sensor data fusion Active CN111829512B (en)

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 true CN111829512A (en) 2020-10-27
CN111829512B 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)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112630769A (en) * 2020-11-30 2021-04-09 东南大学 Radar flow velocity measurement system and method with unmanned aerial vehicle as carrier
CN112987067A (en) * 2021-02-24 2021-06-18 博雅工道（北京）机器人科技有限公司 Ocean current velocity estimation method and device for integrated navigation and processor
CN113608169A (en) * 2021-05-20 2021-11-05 济南大学 Multi-AUV (autonomous Underwater vehicle) cooperative positioning 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
CN115060274A (en) * 2022-08-17 2022-09-16 南开大学 Underwater integrated autonomous navigation device and initial alignment method thereof

Citations (9)

* Cited by examiner, † Cited by third party
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
US20190233070A1 (en) * 2016-10-24 2019-08-01 Seabed Geosolutions B.V. Catenary modeling for a plurality of deployment lines for an offshore seismic system
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
US20200049849A1 (en) * 2017-05-02 2020-02-13 Seabed Geosolutions B.V. System and method for deploying ocean bottom seismic nodes using a plurality of underwater vehicles

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106643709A (en) * 2016-10-10 2017-05-10 东南大学 Combined navigation method and device for offshore carrier
US20190233070A1 (en) * 2016-10-24 2019-08-01 Seabed Geosolutions B.V. Catenary modeling for a plurality of deployment lines for an offshore seismic system
US20200049849A1 (en) * 2017-05-02 2020-02-13 Seabed Geosolutions B.V. System and method for deploying ocean bottom seismic nodes using a plurality of underwater vehicles
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)

* Cited by examiner, † Cited by third party
Title
卢曼曼: "光纤捷联惯导系统的阻尼及校正技术研究", 《中国优秀硕士论文电子期刊》 *

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112630769A (en) * 2020-11-30 2021-04-09 东南大学 Radar flow velocity measurement system and method with unmanned aerial vehicle as carrier
CN112630769B (en) * 2020-11-30 2023-12-12 东南大学 Radar flow velocity measurement system and method with unmanned aerial vehicle as carrier
CN112987067A (en) * 2021-02-24 2021-06-18 博雅工道（北京）机器人科技有限公司 Ocean current velocity estimation method and device for integrated navigation and processor
CN113608169A (en) * 2021-05-20 2021-11-05 济南大学 Multi-AUV (autonomous Underwater vehicle) cooperative positioning method based on sequential fusion algorithm
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
CN115060274A (en) * 2022-08-17 2022-09-16 南开大学 Underwater integrated autonomous navigation device and initial alignment method thereof

Also Published As

Publication number Publication date
CN111829512B (en) 2024-04-09

Similar Documents

Publication Publication Date Title
CN111829512B (en) AUV navigation positioning method and system based on multi-sensor data fusion
Zhang et al. Autonomous underwater vehicle navigation: a review
CN109324330B (en) USBL/SINS tight combination navigation positioning method based on mixed derivative-free extended Kalman filtering
Bao et al. Integrated navigation for autonomous underwater vehicles in aquaculture: A review
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
Kinsey et al. A survey of underwater vehicle navigation: Recent advances and new challenges
CN106643709B (en) Combined navigation method and device for offshore carrier
CN103744098B (en) AUV integrated navigation systems based on SINS/DVL/GPS
CN106767793A (en) A kind of AUV underwater navigation localization methods based on SINS/USBL tight integrations
CN111947651B (en) Underwater combined navigation information fusion method and system and autonomous underwater vehicle
CN113311436A (en) Method for correcting wind measurement of motion attitude of laser wind measuring radar on mobile platform
CN111380518B (en) SINS/USBL tight combination navigation positioning method introducing radial velocity
CN106017460B (en) A kind of underwater hiding-machine navigation locating method of terrain aided inertial navigation tight integration
CN112556697A (en) Shallow coupling data fusion navigation method based on federated structure
CN105928515A (en) Navigation system for unmanned plane
CN110186461A (en) A kind of collaborative navigation method based on gravity gradient information ranging
CN115327588A (en) Network RTK-based high-precision positioning method for unmanned automatic operation special vehicle
CN104061930B (en) A kind of air navigation aid based on strap-down inertial guidance and Doppler log
CN114459476B (en) Underwater unmanned submarine current measuring DVL/SINS integrated navigation method based on virtual speed measurement
CN111735455A (en) Improved Gaussian distance iterative algorithm based butt joint recovery integrated navigation method
Harris et al. Cooperative acoustic navigation of underwater vehicles without a DVL utilizing a dynamic process model: Theory and field evaluation
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
CN103697887B (en) A kind of optimization air navigation aid based on SINS 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
GR01 Patent grant
GR01 Patent grant