CN103744098A - Ship's inertial navigation system (SINS)/Doppler velocity log (DVL)/global positioning system (GPS)-based autonomous underwater vehicle (AUV) combined navigation system - Google Patents
Ship's inertial navigation system (SINS)/Doppler velocity log (DVL)/global positioning system (GPS)-based autonomous underwater vehicle (AUV) combined navigation system Download PDFInfo
- Publication number
- CN103744098A CN103744098A CN201410032817.7A CN201410032817A CN103744098A CN 103744098 A CN103744098 A CN 103744098A CN 201410032817 A CN201410032817 A CN 201410032817A CN 103744098 A CN103744098 A CN 103744098A
- Authority
- CN
- China
- Prior art keywords
- msub
- sins
- mrow
- mtd
- auv
- 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
Links
- 238000001914 filtration Methods 0.000 claims abstract description 112
- 230000004927 fusion Effects 0.000 claims abstract description 33
- 238000004364 calculation method Methods 0.000 claims abstract description 22
- 238000012937 correction Methods 0.000 claims abstract description 14
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 claims abstract description 11
- 238000005259 measurement Methods 0.000 claims description 28
- 238000000034 method Methods 0.000 claims description 16
- 238000006243 chemical reaction Methods 0.000 claims description 11
- 238000012545 processing Methods 0.000 claims description 11
- 238000013461 design Methods 0.000 claims description 8
- 230000008569 process Effects 0.000 claims description 7
- 230000001133 acceleration Effects 0.000 claims description 5
- 230000000694 effects Effects 0.000 claims description 4
- 239000000835 fiber Substances 0.000 claims description 3
- 230000036541 health Effects 0.000 claims description 3
- 239000010453 quartz Substances 0.000 claims description 3
- VYPSYNLAJGMNEJ-UHFFFAOYSA-N silicon dioxide Inorganic materials O=[Si]=O VYPSYNLAJGMNEJ-UHFFFAOYSA-N 0.000 claims description 3
- 238000004422 calculation algorithm Methods 0.000 description 44
- 239000011159 matrix material Substances 0.000 description 23
- 239000013598 vector Substances 0.000 description 8
- 230000003044 adaptive effect Effects 0.000 description 4
- 230000007547 defect Effects 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 230000035945 sensitivity Effects 0.000 description 3
- 238000009825 accumulation Methods 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 230000010355 oscillation Effects 0.000 description 2
- 230000002035 prolonged effect Effects 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 230000006978 adaptation Effects 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 230000002238 attenuated effect Effects 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 230000009189 diving Effects 0.000 description 1
- 238000002592 echocardiography Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 230000026676 system process Effects 0.000 description 1
- 230000009897 systematic effect Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
-
- 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
- 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
- G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Automation & Control Theory (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
- Navigation (AREA)
Abstract
The invention discloses a ship's inertial navigation system (SINS)/Doppler velocity log (DVL)/global positioning system (GPS)-based autonomous underwater vehicle (AUV) combined navigation system, which comprises an SINS, a GPS receiver, a DVL and a data fusion center, wherein the SINS, the GPS receiver, the DVL and the data fusion center are arranged on an AUV; when the AUV is positioned on the water surface, an optimized filter module carries out filtering fusion calculation by combining navigation information of the SINS, a pseudo-range and a pseudo-range rate corresponding to the SINS and available ephemeris data output by the GPS receiver to obtain correction information; when the AUV is positioned underwater, the optimized filter module carries out filtering fusion calculation by combining the navigation information output by the SINS and three-dimensional navigational speed information output by the DVL to obtain correction information. The navigational positioning accuracy and the robustness of the system are improved, and the system realizes an uninterrupted high-accuracy underwater and water surface carrier navigating and tracking function.
Description
Technical Field
The invention relates to an AUV (autonomous underwater vehicle) integrated navigation system based on SINS (strapdown inertial navigation system)/DVL (dynamic velocity logging)/GPS (global positioning system), which can be used for navigation, tracking and positioning of underwater vehicles and water surface vehicles.
Background
AUV (Autonomous Underwater Vehicle) is an important tool for exploring and developing and utilizing marine environmental resources, and plays an important role in marine military application. The problem of navigation of AUVs remains one of the major technical challenges facing today due to their remoteness, concealment, and autonomy. Because the underwater environment is complex, an integrated navigation mode is often adopted to perform navigation positioning on the AUV, and the design of a filter of an integrated navigation system is the key for ensuring the navigation precision. The navigation positioning precision of the conventional AUV can reach about <0.8nmile/h at present, and the accuracy requirement of AUV navigation positioning is met to a certain extent.
The SINS (Strapdown Inertial Navigation system) system has autonomous Navigation capability, is not influenced by environment, carrier maneuvering and radio interference, can continuously provide Navigation positioning information such as carrier position, speed and attitude, has high data updating frequency and higher relative precision in a short time. However, as the system operation time is prolonged, the navigation error of the SINS system is accumulated and increased, and at this time, the SINS system needs to be corrected and compensated by a filtering algorithm using the observation information of the external sensor so as to suppress the error accumulated with time.
The GPS (Global Positioning System) is a Global satellite navigation Positioning System with sea, land and air omnibearing real-time three-dimensional navigation and Positioning capability. The GPS system calculates navigation positioning information by receiving wireless signals sent by 24 navigation satellites distributed in the air, and has the advantages of globality, all-weather performance, real-time performance and the like. However, electromagnetic wave signals are attenuated rapidly underwater, and when the electromagnetic wave signals reach a certain depth, GPS signals cannot be captured, navigation and positioning functions cannot be provided, and the functional requirements of some underwater vehicles needing real-time positioning cannot be met.
The DVL (Doppler Velocity Log Doppler Log) is a speed measuring device widely used in combined systems of underwater and surface navigation systems, and has an acoustic navigation positioning system that outputs the three-dimensional speed and range capability of a carrier in real time. The DVL system obtains the sound wave frequency shift through the information of the four transducers arranged at the bottom of the AUV, and then calculates to obtain the three-dimensional speed information of the carrier in the carrier system, and has the advantages of high precision, reliability, real-time property and the like.
Due to the complex underwater environment, the accumulation of the position error of the dead reckoning system along with time and the long-endurance and concealment requirements of the AUV, the precision of the conventional filtering algorithm of the SINS/DVL-based dead reckoning combined system is difficult to meet the requirement of a high-precision navigation positioning function; when the accuracy of the filtering algorithm is improved, the AUV needs to float to the water surface after a certain time interval, and the position of the AUV is corrected by introducing a GPS signal, so that the AUV can sail along a set track.
When the system model and the noise statistical characteristics are accurately known, Kalman filtering is the optimal estimation, the accuracy is high, but in practical application, only an approximate mathematical model can be established, and the accurate noise statistical characteristics are difficult to obtain.
The H infinity filtering algorithm based on the Krein space does not make any assumption on the statistical characteristics of the system, and has the advantages of simple filtering model and good robustness. However, the H-infinity filtering algorithm obtains system robustness at the expense of certain precision, and the precision is difficult to guarantee.
Comprehensively considering the high precision of the Kalman filtering algorithm and the robustness of the H-infinity filtering algorithm, and combining the Kalman filtering algorithm and the H-infinity filtering algorithm to obtain a high-performance optimized filtering algorithm; the filtering algorithm has stronger robustness about model uncertainty and higher precision tracking capability on the system state; the SINS navigation information, the GPS receiver ephemeris data and the three-dimensional speed information of the DVL log are subjected to data fusion calculation by using the proposed optimized filtering technology, the robust performance of the system can be remarkably improved on the basis of ensuring the navigation precision, and the navigation positioning performance of the strapdown inertial navigation system is further improved.
Disclosure of Invention
The purpose of the invention is as follows: in order to overcome the defects in the prior art, the invention provides an AUV integrated navigation system based on SINS/DVL/GPS, DVL speed measurement information and GPS position information are introduced into the AUV integrated system to assist the SINS in navigation, tracking and positioning; the system integrates information of a plurality of sub-navigation systems, and overcomes the defects that an AUV navigation system is easily influenced by the environment, navigation positioning errors of an SINS (strapdown inertial navigation system) continuously accumulate along with the time to meet the precision requirement, and Kalman filtering precision is reduced due to inaccurate filtering models or colored noise interference of the system.
The technical scheme is as follows: in order to achieve the purpose, the invention adopts the technical scheme that:
the AUV integrated navigation system based on the SINS/DVL/GPS comprises an SINS system, a GPS receiver, a DVL log and a data fusion center, wherein the SINS system, the GPS receiver, the DVL log and the data fusion center are all arranged on an AUV;
the SINS system comprises an IMU component and an IMU processing unit, wherein the IMU sensor is used for detecting inertial data of the AUV and outputting the inertial data to the IMU processing unit, and the IMU processing unit carries out navigation integral calculation on the inertial data to obtain navigation information of the AUV under a navigation coordinate system;
the GPS receiver is used for receiving and outputting navigation measurement information of the AUV, available ephemeris data, satellite health state and other data;
the DVL log is used for measuring longitudinal and transverse three-dimensional navigational speed information of the AUV relative to the seabed;
the data fusion center is used for information fusion and outputting corrected integrated navigation information and comprises a sensor information conversion module and an optimized filtering module; the sensor information conversion module is used for calculating a pseudo range and a pseudo range rate corresponding to the SINS system by combining available ephemeris data output by the GPS receiver and navigation information output by the SINS system; the optimized filtering module combines navigation information of the SINS, pseudo range and pseudo range rate corresponding to the SINS, available ephemeris data output by the GPS receiver and three-dimensional navigational speed information output by the DVL log to carry out filtering fusion calculation to obtain correction information; the navigation information output by the SINS is corrected through the correction information, and finally corrected combined navigation information is obtained;
when the AUV is positioned on the water surface, the optimized filtering module combines navigation information of the SINS, pseudo range and pseudo range rate corresponding to the SINS and available ephemeris data output by the GPS receiver to carry out filtering fusion calculation to obtain correction information;
and when the AUV is underwater, the optimized filtering module performs filtering fusion calculation by combining navigation information output by the SINS and three-dimensional navigational speed information output by the DVL to obtain correction information.
Preferably, the inertial sensors in the IMU include a three-axis orthogonally mounted gyroscope for providing three-axis angular velocity measurements and a three-axis orthogonally mounted accelerometer for providing three-axis acceleration measurements.
Preferably, the gyroscope is a fiber optic gyroscope, and the accelerometer is a quartz flexible accelerometer.
Preferably, the DVL log is a doppler effect based four-beam absolute velocity measurement device.
Preferably, the DVL log comprises a transducer, a transceiver and an interface unit, the transceiver comprising a transmitting system, a receiving system and a computational compensation circuit; the transducers are acoustic transducers, are arranged at the position, away from the bow part by about 1/3, of the bottom end of the AUV, are four in number, are symmetrically arranged according to the fore-aft line, and are used for converting an electric signal output by the transmitting system into an acoustic signal and then sending the acoustic signal, converting an echo signal into an electric signal and then outputting the electric signal to the receiving system; the receiving system is used for amplifying the electric signals output by the transducer, obtaining Doppler frequency shift, converting the Doppler frequency shift into three-dimensional navigational speed analog signals and sending the three-dimensional navigational speed analog signals to the interface unit, and the interface unit converts the three-dimensional navigational speed analog signals output by the transmitting system into three-dimensional navigational speed and outputs the three-dimensional navigational speed in a digital mode.
Preferably, the optimized filtering module integrates a Kalman filter and an H ∞ filter, and for the input observed value Z, the filtering process of the optimized filtering module includes the following steps:
(3) computing Kalman filteringPerformance index M of devicek;
(4) Calculating a self-adaptive adjusting weight value: <math>
<mrow>
<msub>
<mi>d</mi>
<mi>k</mi>
</msub>
<mo>=</mo>
<mfenced open='{' close=''>
<mtable>
<mtr>
<mtd>
<mn>1</mn>
<mo>,</mo>
</mtd>
<mtd>
<msub>
<mi>M</mi>
<mi>k</mi>
</msub>
<mo>≤</mo>
<msub>
<mi>M</mi>
<mn>2</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msup>
<mi>a</mi>
<mrow>
<mo>-</mo>
<mfrac>
<msub>
<mi>M</mi>
<mi>k</mi>
</msub>
<mi>b</mi>
</mfrac>
</mrow>
</msup>
<mo>,</mo>
</mtd>
<mtd>
<msub>
<mi>M</mi>
<mn>2</mn>
</msub>
<mo><</mo>
<msub>
<mi>M</mi>
<mi>k</mi>
</msub>
<mo>≤</mo>
<msub>
<mi>M</mi>
<mo>∞</mo>
</msub>
<mo>;</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
<mo>,</mo>
</mtd>
<mtd>
<msub>
<mi>M</mi>
<mi>k</mi>
</msub>
<mo>></mo>
<msub>
<mi>M</mi>
<mo>∞</mo>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
</math>
(5) calculating the comprehensive state estimation of the system:
wherein M is2Upper bound for Kalman filtering method high accuracy operation, M∞The lower critical value of the Kalman filtering method for low-precision operation is obtained; when the system model and the noise statistical characteristics are accurately known, the Kalman filtering is the optimal estimation, the precision is high, and the performance index M of the Kalman filter iskA value close to 1; if the accuracy of the system model is poor or the system noise is non-Gaussian white noise, the Kalman filtering accuracy is reduced and even filtering divergence occurs, and the performance index M of the Kalman filterkIf the value is greater than 1, the trust level of the Kalman filter estimation value should be reduced; dk∈[0,1]Representing the degree of confidence in the Kalman filter, a and b being design parameters, 1<a≤5,1<b is less than or equal to 10; the value of a and b determines the adaptive regulation weight value dkThe sensitivity to the change of the performance index of the Kalman filter shows the precision and robustness of the optimized filtering module.
Has the advantages that: the AUV integrated navigation system based on the SINS/DVL/GPS provided by the invention introduces DVL speed measurement information and GPS position information into the AUV integrated system to assist the SINS in navigation, tracking and positioning; the system integrates a plurality of sub-navigation information, and overcomes the defects that an AUV navigation system is easily influenced by the environment and the navigation positioning error of an SINS (strapdown inertial navigation system) continuously accumulates along with the time and cannot meet the precision requirement; meanwhile, the high precision of the Kalman filtering algorithm and the robustness of the H-infinity filtering algorithm are comprehensively considered, the Kalman filtering and the H-infinity filtering are combined, the robustness about model uncertainty and the high-precision tracking capability of the system state are strong, and the defect of Kalman filtering precision reduction caused by inaccurate filtering model or colored noise interference of the system is overcome.
Drawings
FIG. 1 is a main block diagram of the system components of the present invention;
FIG. 2 is a block diagram of a system assembly model;
FIG. 3 is a schematic diagram of the SINS system architecture;
fig. 4 is a graph of the position accuracy versus the effect.
Detailed Description
The present invention will be further described with reference to the accompanying drawings.
Description of the design principles
AUV is an important tool for exploring and developing and utilizing marine environmental resources, and plays an important role in the aspect of marine military application. Because of its long-range, disguise and complicated underwater environment, often adopt the integrated navigation mode to navigate the location to AUV. The SINS has autonomous navigation capability, is not influenced by environment, carrier maneuvering and radio interference, can continuously provide navigation positioning information such as carrier position, speed and attitude, has high data updating frequency and has high relative precision in a short time. However, as the system operation time is prolonged, the navigation error of the SINS system will accumulate and increase, and at this time, the SINS system needs to be corrected and compensated by using the observation information of the external sensor through a filtering algorithm so as to suppress the error accumulated over time. DVL is a speed measuring device widely applied to underwater and water surface navigation combined systems, and has an acoustic navigation positioning system capable of outputting the three-dimensional speed and range of a carrier in real time. The method has the advantages of high precision, reliability, real-time performance and the like. Due to the complex underwater environment, the accumulation of the position error of the dead reckoning system along with time and the long-endurance and concealment requirements of the AUV, the precision of the conventional filtering algorithm of the SINS/DVL-based dead reckoning combined system is difficult to meet the requirement of a high-precision navigation positioning function; when the accuracy of the filtering algorithm is improved, the AUV floats to the water surface after a certain time interval, and the position of the AUV is corrected by introducing a GPS signal, so that the AUV can sail along a set track.
When the statistical characteristics of the system model and the noise are accurately known, Kalman filtering is the optimal estimation, the accuracy is high, and if the accuracy of the system model is poor or the system noise is non-Gaussian white noise, the Kalman filtering accuracy is reduced and even filtering divergence occurs. The H infinity filtering algorithm based on the Krein space does not make any assumption on the statistical characteristics of the system, and has the advantages of simple filtering model and good robustness. However, the H-infinity filtering algorithm obtains system robustness at the expense of certain precision, and the precision is difficult to guarantee. Comprehensively considering the high precision of the Kalman filtering algorithm and the robustness of the H-infinity filtering algorithm, and combining the Kalman filtering algorithm and the H-infinity filtering algorithm to obtain a high-performance optimized filtering algorithm; the filtering algorithm has stronger robustness about model uncertainty and higher precision tracking capability on the system state; the SINS navigation information, the GPS receiver ephemeris data and the three-dimensional speed information of the DVL log are subjected to data fusion calculation by using the proposed optimized filtering technology, the robust performance of the system can be remarkably improved on the basis of ensuring the navigation precision, and the navigation positioning performance of the strapdown inertial navigation system is further improved.
Description of System design
As shown in fig. 1 to 4, an AUV integrated navigation system based on SINS/DVL/GPS is characterized in that: the system comprises an SINS system, a GPS receiver, a DVL log and a data fusion center, wherein the SINS system, the GPS receiver, the DVL log and the data fusion center are all arranged on an AUV; the respective sections are explained below.
The SINS system comprises an IMU component and an IMU processing unit, wherein the IMU sensor is used for detecting inertial data of the AUV and outputting the inertial data to the IMU processing unit, and the IMU processing unit carries out navigation integral calculation on the inertial data to obtain navigation information of the AUV under a navigation coordinate system. Wherein inertial sensors in the IMU include a three-axis orthogonally mounted gyroscope for providing three-axis angular velocity measurements and a three-axis orthogonally mounted accelerometer for providing three-axis acceleration measurements. The gyroscope is a fiber optic gyroscope, and the accelerometer is a quartz flexible accelerometer.
The GPS receiver is used for receiving and outputting navigation measurement information, available ephemeris data, satellite health state and other data of the AUV.
The DVL log is used for measuring longitudinal and transverse three-dimensional navigational speed information of the AUV relative to the seabed; the DVL log is a Doppler effect-based four-beam absolute velocity measuring device, and comprises a transducer, a transceiver and an interface unit, wherein the transceiver comprises a transmitting system, a receiving system and a calculation compensation circuit; the transducers are acoustic transducers, are arranged at the position, away from the bow part by about 1/3, of the bottom end of the AUV, are four in number, are symmetrically arranged according to the fore-aft line, and are used for converting an electric signal output by the transmitting system into an acoustic signal and then sending the acoustic signal, converting an echo signal into an electric signal and then outputting the electric signal to the receiving system; the receiving system is used for amplifying the electric signals output by the transducer, obtaining Doppler frequency shift, converting the Doppler frequency shift into three-dimensional navigational speed analog signals and sending the three-dimensional navigational speed analog signals to the interface unit, and the interface unit converts the three-dimensional navigational speed analog signals output by the transmitting system into three-dimensional navigational speed and outputs the three-dimensional navigational speed in a digital mode.
The data fusion center is used for information fusion and outputting corrected integrated navigation information and comprises a sensor information conversion module and an optimized filtering module; the sensor information conversion module is used for calculating a pseudo range and a pseudo range rate corresponding to the SINS system by combining available ephemeris data output by the GPS receiver and navigation information output by the SINS system; the optimized filtering module combines navigation information of the SINS, pseudo range and pseudo range rate corresponding to the SINS, available ephemeris data output by the GPS receiver and three-dimensional navigational speed information output by the DVL log to carry out filtering fusion calculation to obtain correction information; the navigation information output by the SINS is corrected through the correction information, and finally corrected combined navigation information is obtained;
when the AUV is positioned on the water surface, the optimized filtering module combines navigation information of the SINS, pseudo range and pseudo range rate corresponding to the SINS and available ephemeris data output by the GPS receiver to carry out filtering fusion calculation to obtain correction information;
and when the AUV is underwater, the optimized filtering module performs filtering fusion calculation by combining navigation information output by the SINS and three-dimensional navigational speed information output by the DVL to obtain correction information.
Preferably, the optimized filtering module integrates a Kalman filter and an H ∞ filter, and for the input observed value Z, the filtering process of the optimized filtering module includes the following steps:
(1) computing Kalman Filter at tkAnd (3) time state estimation:
(3) calculating a performance index M of a Kalman filterk:
(4) Calculating a self-adaptive adjusting weight value: <math>
<mrow>
<msub>
<mi>d</mi>
<mi>k</mi>
</msub>
<mo>=</mo>
<mfenced open='{' close=''>
<mtable>
<mtr>
<mtd>
<mn>1</mn>
<mo>,</mo>
</mtd>
<mtd>
<msub>
<mi>M</mi>
<mi>k</mi>
</msub>
<mo>≤</mo>
<msub>
<mi>M</mi>
<mn>2</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msup>
<mi>a</mi>
<mrow>
<mo>-</mo>
<mfrac>
<msub>
<mi>M</mi>
<mi>k</mi>
</msub>
<mi>b</mi>
</mfrac>
</mrow>
</msup>
<mo>,</mo>
</mtd>
<mtd>
<msub>
<mi>M</mi>
<mn>2</mn>
</msub>
<mo><</mo>
<msub>
<mi>M</mi>
<mi>k</mi>
</msub>
<mo>≤</mo>
<msub>
<mi>M</mi>
<mo>∞</mo>
</msub>
<mo>;</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
<mo>,</mo>
</mtd>
<mtd>
<msub>
<mi>M</mi>
<mi>k</mi>
</msub>
<mo>></mo>
<msub>
<mi>M</mi>
<mo>∞</mo>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
</math>
wherein,is tkAn innovation sequence calculation formula of the time,one-step prediction of the estimated value, phi, for the system statek,k-1Is tk-1Time to tkThe state of the moment is transferred to the matrix by one step; tr (A) denotes a trace-finding operation on an arbitrary matrix A, ZkIs tkMeasurement vector of time, HkFor the measurement matrix, RkFor measuring arrays of variances of noise sequences, Pk,k-1Is tkSystematic one-step prediction error variance matrix of time, M2High precision operation for Kalman filtering methodUpper limit of (M)∞Lower threshold for Kalman filtering method low precision operation, dk∈[0,1]Representing the degree of confidence in the Kalman filter, a and b being design parameters, 1<a≤5,1<b is less than or equal to 10; the value of a and b determines the adaptive regulation weight value dkThe sensitivity to the change of the performance index of the Kalman filter shows the precision and robustness of the optimized filtering module.
Description of design derivation
(1) Geometric model of the earth and physical parameters
Description of the shape of the Earth
Equatorial radius major semi-axis Re=6378137.0m, ellipsoid ellipticity e = 1/298.257223563; meridian plane radius of curvature RNCurvature radius R of unitary and quartile ringECan be obtained from the following equation:
earth rotation angular rate omegaie=15.0411°/h=7.29211585×10-5rad/s
(iii) acceleration of Earth gravity <math>
<mrow>
<mi>g</mi>
<mo>=</mo>
<mn>9.7803267714</mn>
<mo>×</mo>
<mfrac>
<mrow>
<mn>1</mn>
<mo>+</mo>
<mn>0.00193185138639</mn>
<msup>
<mi>sin</mi>
<mn>2</mn>
</msup>
<mi>L</mi>
</mrow>
<msqrt>
<mn>1</mn>
<mo>-</mo>
<mn>0.00669437999013</mn>
<msup>
<mi>sin</mi>
<mn>2</mn>
</msup>
<mi>L</mi>
</msqrt>
</mfrac>
<msup>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>+</mo>
<mfrac>
<mi>h</mi>
<msub>
<mi>R</mi>
<mi>e</mi>
</msub>
</mfrac>
<mo>)</mo>
</mrow>
<mrow>
<mo>-</mo>
<mn>2</mn>
</mrow>
</msup>
</mrow>
</math>
Wherein L represents the local latitude and h represents the altitude.
(2) Definition of coordinate system
The geocentric inertial coordinate system: i is a
With oxiyiziIndicating that the origin is at the center of the earth, oxiAnd oyiAxis in the equatorial plane of the earth, oxiAxis pointing to spring equinox, oziAxis coincident with the earth's axis of rotation, oyiWith oxi、oziAnd a right-hand rectangular coordinate system is formed, and the directions of the three coordinate axes in the inertial space are fixed. The output of IMU is referenced to i.
A global coordinate system: e is a series of
With oxeyezeIndicating that the origin is at the center of the earth, ozeAxis coincident with the earth's axis of rotation, oxeAxis along the intersection of the Greenwich meridian plane and the equatorial plane of the Earth, oyeAxis in equatorial plane, oxe、oye、ozeThe three axes form a right-hand rectangular coordinate system. The earth coordinate system is fixedly connected with the earth, and the angular rate of the e system rotating relative to the i system is the rotational angular speed omega of the earthie。
③ geographic coordinate system: g is a
With oxgygzgMeaning that the origin is at the center of gravity of the carrier, typically using the northeast sky coordinate system as the geographic coordinate system, oxgThe axis points to east (E), oygThe axis pointing to north (N), ozgThe axis points to the day (U).
Fourthly, navigation coordinate system: n is a
With oxnynznIndicating that the northeast geographic coordinate system (ENU) is used as the navigation coordinate system.
A carrier coordinate system: b is a series of
With oxbybzbIndicating that the origin is generally taken at the geometric center of the IMU, oxbAxis to the right along the transverse axis of the carrier, oybAxis forward along the longitudinal axis of the carrier, ozbThe axis is vertical to the carrier axis.
(3) Conversion relation between attitude angle and attitude matrix
Defining of attitude angle
Course angle: longitudinal axis of the carrier oybThe angle between the projection on the horizontal plane and the north of the geographic meridian, called the heading angle, is denoted as Ψ. The heading angle value is calculated clockwise by taking the geographic north direction as a starting point, and the definition range of the heading angle value is 0-360 degrees.
Pitch angle: the carrier being wound about a transverse axis oxbThe angle between the longitudinal axis of the carrier and the horizontal plane when rotating, called pitch angle, is denoted as θ. The pitch angle is positive when the pitch angle is up and negative when the pitch angle is down from the horizontal plane, and the range of the pitch angle is-90 degrees to +90 degrees.
Roll angle: the included angle between the longitudinal symmetric plane of the carrier and the longitudinal plumb plane is called the roll angle and is marked as gamma. The roll angle is positive when the right inclination is positive and negative when the left inclination is negative from the vertical plane, and the definition range is-180 DEG to +180 deg.
Matrix of the posture 2
The transformation from the carrier coordinate system b to the geographic coordinate system (navigation system n system) can be obtained by three rotations according to the following sequenceThe specific sequence is as follows: around-ozgAngle of pivot Ψ, axis ox1Angle of rotation of axis theta, rewind of axis oy2Shaft rotation gamma angle; obtaining a conversion matrix from the geographic coordinate system to the carrier coordinate system as follows:
taking the geographic coordinate system as a navigation coordinate system, the strapdown inertial navigation system attitude matrix is as follows:
and the conversion relation from the attitude matrix to the attitude angle of the main value interval is as follows:
(4) basic principle of SINS system
SINS system differential equation
The attitude matrix differential equation is:
wherein,representing vectorsIn the form of a cross-product of (c),is the angular velocity of the carrier, is measured by a gyroscope, <math>
<mrow>
<msubsup>
<mi>ω</mi>
<mi>in</mi>
<mi>n</mi>
</msubsup>
<mo>=</mo>
<msubsup>
<mi>ω</mi>
<mi>ie</mi>
<mi>n</mi>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>ω</mi>
<mi>en</mi>
<mi>n</mi>
</msubsup>
<mo>.</mo>
</mrow>
</math>
the specific force equation of the carrier in the navigation system is as follows:
wherein,fbfor the specific force measured by the accelerometer, for the size of the carrier to the ground speed in the navigation system,gn=[0 0 -g]T。
Position update differential equation: in a system with a geographic coordinate system as a navigation coordinate system, the differential equations for latitude L, longitude λ, and altitude h are:
SINS system error model
Gyroscope error model: epsiloni=εbi+εwi(i=x,y,z),εbiIs a random constant, epsilonwiIs white noise.
An accelerometer error model: vi=▽bi+▽wi(i=x,y,z),▽biIs a random constant +wiIs white noise.
The attitude error equation is: <math>
<mrow>
<msup>
<mover>
<mi>φ</mi>
<mo>·</mo>
</mover>
<mi>n</mi>
</msup>
<mo>=</mo>
<mo>-</mo>
<msubsup>
<mi>ω</mi>
<mi>in</mi>
<mi>n</mi>
</msubsup>
<mo>×</mo>
<msup>
<mi>φ</mi>
<mi>n</mi>
</msup>
<mo>+</mo>
<mi>δ</mi>
<msubsup>
<mi>ω</mi>
<mi>ie</mi>
<mi>n</mi>
</msubsup>
<mo>+</mo>
<mi>δ</mi>
<msubsup>
<mi>ω</mi>
<mi>en</mi>
<mi>n</mi>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
<msup>
<mi>ϵ</mi>
<mi>b</mi>
</msup>
<mo>,</mo>
</mrow>
</math> φn=[φx φy φz]Tis the attitude error.
The velocity error equation is: <math>
<mrow>
<mi>δ</mi>
<msup>
<mover>
<mi>V</mi>
<mo>·</mo>
</mover>
<mi>n</mi>
</msup>
<mo>=</mo>
<mo>-</mo>
<mi>φ</mi>
<mo>×</mo>
<msup>
<mi>f</mi>
<mi>n</mi>
</msup>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>2</mn>
<mi>δ</mi>
<msubsup>
<mi>ω</mi>
<mi>ie</mi>
<mi>n</mi>
</msubsup>
<mo>+</mo>
<mi>δ</mi>
<msubsup>
<mi>ω</mi>
<mi>en</mi>
<mi>n</mi>
</msubsup>
<mo>)</mo>
</mrow>
<mo>×</mo>
<msup>
<mi>V</mi>
<mi>n</mi>
</msup>
<mo>-</mo>
<mrow>
<mo>(</mo>
<msubsup>
<mrow>
<mn>2</mn>
<mi>ω</mi>
</mrow>
<mi>ie</mi>
<mi>n</mi>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>ω</mi>
<mi>en</mi>
<mi>n</mi>
</msubsup>
<mo>)</mo>
</mrow>
<mo>×</mo>
<msup>
<mi>δV</mi>
<mi>n</mi>
</msup>
<mo>+</mo>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
<msup>
<mo>▿</mo>
<mi>b</mi>
</msup>
<mo>.</mo>
</mrow>
</math>
the position error equation is:
(5) filtering algorithm
Phi Kalman filtering algorithm
Consider the following linear time-varying discrete system model:
process noise sequence W for Kalman Filter hypothesis SystemkAnd measure the noise sequence VkA zero mean gaussian white noise sequence. After the continuous time system is subjected to discretization processing, if the statistical characteristics of a system model and noise are accurately known and a Kalman filter is stable, measuring a vector Z according to the k momentk,The (linear minimum variance) optimal estimation recursion procedure of (a) can be briefly described as follows:
the above equation is the basic equation of discrete Kalman filtering. Given the initial state of the systemAnd initial estimation error variance matrix P0According to the measurement of time kkThe optimal state estimate to time k can be deduced(k=1,2,3,…)。
Wherein the terms are defined as follows:
Xkis tkA state vector of a system to be estimated at a moment;
Φk,k-1is tk-1Time to tkThe state of the moment is transferred to the matrix by one step;
Γk-1driving a matrix for system process noise;
Qkthe variance matrix of the system noise sequence is a non-negative array;
Rkmeasuring a variance matrix of the noise sequence, and taking the variance matrix as a positive definite matrix;
Zkis a measurement state vector;
Hkis a measurement matrix;
Wk-1noise sequence for a systematic random process;
Vkrandomly measuring a noise sequence for the system;
Xk,k-1predicting the system state in one step;
Pkan estimation error variance matrix is obtained;
Kkis a filter gain matrix.
Introduction of H infinity filtering algorithm
Consider the following Krein-based spatial discrete state space system model:
ykis a given system state XkLinear combination observations of (2). Wherein, WkAnd VkIs 12Energy-bounded noise, i.e. No assumptions are made about its statistical properties. Let the initial state of the system be X0The initial estimation error variance matrix is P0,Is the initial state X of the system0An estimate of (2). Order toIndicates at a given measured value { ZkFor y under the conditionkIs estimated.
The suboptimal H-infinity filtering recursion algorithm is as follows:
h infinity filtering algorithm in PkGiven a positive number gamma, through Re,kFurther adjust Pk. The parameter γ determines the robustness of the H ∞ filter.
(iii) improved Algorithm
When the statistical characteristics of the system model and the noise are accurately known, Kalman filtering is the optimal estimation, the accuracy is high, and if the accuracy of the system model is poor or the system noise is non-Gaussian white noise, the Kalman filtering accuracy is reduced and even filtering divergence occurs. The H infinity filtering algorithm based on the Krein space does not make any assumption on the statistical characteristics of the system, and has the advantages of simple filtering model and good robustness. However, the H-infinity filtering algorithm obtains system robustness at the expense of certain precision, and the precision is difficult to guarantee. The high precision of the Kalman filtering algorithm and the robustness of the H-infinity filtering algorithm are comprehensively considered, and the Kalman filtering algorithm and the H-infinity filtering algorithm are combined to obtain the high-performance optimized filtering algorithm.
If the system model is accurate, WkAnd VkAre zero-mean Gaussian white noise sequences which are uncorrelated with each other and their variance Qk、RkIf it is accurate, the kalman filter algorithm finds the sequence of innovation { rkIs a zero mean, white noise sequence, and the formula for calculating the new sequence is And the variance is: in ideal conditions, the residual error is output in the Kalman filtering processAndthe ratio of (a) is close to 1. When the state estimation value of the filter deviates from the system state due to the influence of factors such as model uncertainty and the like, residual errors are outputAndthe ratio of (A) deviates from 1; estimating residual errorInformation reflecting the actual estimation error is obtained,andthe ratio of (c) reflects the filter performance. Defining a Kalman filter performance index MkThe calculation method comprises the following steps:
definition M2Upper bound for Kalman filter high accuracy operation, M2Close to 1, i.e. for each instant there is Mk≤M2And then the Kalman filter has good performance. Definition M∞Lower bound for Kalman filter low accuracy operation, M∞Significantly greater than 1, i.e. for each instant there is Mk≥M∞The Kalman filter performs poorly and even divergently at this time. When M is2≤Mk≤M∞The Kalman filter performance is general.
The process of the optimized filtering algorithm is as follows:
1) solving t by Kalman filtering algorithmkAnd (3) time state estimation:
2) solving t by using H infinity filtering algorithmkAnd (3) time state estimation:
3) computing Kalman Filter Performance index Mk;
4) Adaptive adjustment of weight dkAnd (3) calculating: <math>
<mrow>
<msub>
<mi>d</mi>
<mi>k</mi>
</msub>
<mo>=</mo>
<mfenced open='{' close=''>
<mtable>
<mtr>
<mtd>
<mn>1</mn>
<mo>,</mo>
</mtd>
<mtd>
<msub>
<mi>M</mi>
<mi>k</mi>
</msub>
<mo>≤</mo>
<msub>
<mi>M</mi>
<mn>2</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msup>
<mi>a</mi>
<mrow>
<mo>-</mo>
<mfrac>
<msub>
<mi>M</mi>
<mi>k</mi>
</msub>
<mi>b</mi>
</mfrac>
</mrow>
</msup>
<mo>,</mo>
</mtd>
<mtd>
<msub>
<mi>M</mi>
<mn>2</mn>
</msub>
<mo><</mo>
<msub>
<mi>M</mi>
<mi>k</mi>
</msub>
<mo>≤</mo>
<msub>
<mi>M</mi>
<mo>∞</mo>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
<mo>,</mo>
</mtd>
<mtd>
<msub>
<mi>M</mi>
<mi>k</mi>
</msub>
<mo>></mo>
<msub>
<mi>M</mi>
<mo>∞</mo>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
</math>
in the formula (d)k∈[0,1]Representing the trust degree of the optimized fusion filter on the Kalman filtering performance, and a and b are design parametersNumber, 1<a≤5,1<b is less than or equal to 10; the value of a and b determines the adaptive regulation weight value dkSensitivity to Kalman filtering performance index changes and precision and robustness of the fusion filter.
5) And (3) estimating the comprehensive state of the system: <math>
<mrow>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mi>k</mi>
</msub>
<mo>=</mo>
<msub>
<mi>d</mi>
<mi>k</mi>
</msub>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mrow>
<mn>2</mn>
<mi>k</mi>
</mrow>
</msub>
<mo>+</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>-</mo>
<msub>
<mi>d</mi>
<mi>k</mi>
</msub>
<mo>)</mo>
</mrow>
<msub>
<mover>
<mi>X</mi>
<mo>^</mo>
</mover>
<mrow>
<mo>∞</mo>
<mi>k</mi>
</mrow>
</msub>
<mo>.</mo>
</mrow>
</math>
(6) AUV System Filter model description
When the AUV is positioned under water for underwater navigation, establishing a continuous system state equation based on SINS/DVL as follows:
making SINS asFor reference systems, the state variable X is taken as the speed errorAttitude angle error (phi)x,φy,φz) Random constant bias (v) of accelerometerx,▽y) And gyro random constant drift (epsilon)x,εy,εz) And 10 dimensions in total: <math>
<mrow>
<mi>X</mi>
<mo>=</mo>
<msup>
<mrow>
<mo>[</mo>
<msubsup>
<mi>δV</mi>
<mi>e</mi>
<mi>n</mi>
</msubsup>
<mo>,</mo>
<msubsup>
<mi>δV</mi>
<mi>n</mi>
<mi>n</mi>
</msubsup>
<mo>,</mo>
<msub>
<mi>φ</mi>
<mi>x</mi>
</msub>
<mo>,</mo>
<msub>
<mi>φ</mi>
<mi>y</mi>
</msub>
<mo>,</mo>
<msub>
<mi>φ</mi>
<mi>z</mi>
</msub>
<mo>,</mo>
<msub>
<mo>▿</mo>
<mi>x</mi>
</msub>
<mo>,</mo>
<msub>
<mo>▿</mo>
<mi>y</mi>
</msub>
<mo>,</mo>
<msub>
<mi>ϵ</mi>
<mi>x</mi>
</msub>
<mo>,</mo>
<msub>
<mi>ϵ</mi>
<mi>y</mi>
</msub>
<mo>,</mo>
<msub>
<mi>ϵ</mi>
<mi>z</mi>
</msub>
<mo>]</mo>
</mrow>
<mi>T</mi>
</msup>
<mo>;</mo>
</mrow>
</math> W=[wax,way,wgx,wgy,wgz,0,0,0,0,0]Tis a system noise vector;
the difference between the velocity calculated by SINS and the velocity of the carrier system measured by Doppler log after being converted into the navigation system is used as the measurement value of the system, and then the measurement equation is expressed as:
wherein the system quantity is measured asThe measured noise sequence is V = [ w = [)vx,wvy]T. And discretizing the system state equation and the measurement equation to obtain a discrete system filtering model.
Establishing a continuous system state equation based on the SINS/GPS when the AUV floats on the water surface as follows:
taking SINS as a reference system, and taking position error (delta L, delta lambda) and speed error as state variables XAttitude angle error (phi)x,φy,φz) Random constant bias (v) of accelerometerx,▽y) And gyro random constant drift (epsilon)x,εy,εz) GPS clock error and clock frequency error (δ t)u,δtru) And 14 dimensions in total: <math>
<mrow>
<mi>X</mi>
<mo>=</mo>
<msup>
<mrow>
<mo>[</mo>
<mi>δL</mi>
<mo>,</mo>
<mi>δλ</mi>
<mo>,</mo>
<msubsup>
<mi>δV</mi>
<mi>e</mi>
<mi>n</mi>
</msubsup>
<mo>,</mo>
<msubsup>
<mi>δV</mi>
<mi>n</mi>
<mi>n</mi>
</msubsup>
<mo>,</mo>
<msub>
<mi>φ</mi>
<mi>x</mi>
</msub>
<mo>,</mo>
<msub>
<mi>φ</mi>
<mi>y</mi>
</msub>
<mo>,</mo>
<msub>
<mi>φ</mi>
<mi>z</mi>
</msub>
<mo>,</mo>
<msub>
<mo>▿</mo>
<mi>x</mi>
</msub>
<mo>,</mo>
<msub>
<mo>▿</mo>
<mi>y</mi>
</msub>
<mo>,</mo>
<msub>
<mi>ϵ</mi>
<mi>x</mi>
</msub>
<mo>,</mo>
<msub>
<mi>ϵ</mi>
<mi>y</mi>
</msub>
<mo>,</mo>
<msub>
<mi>ϵ</mi>
<mi>z</mi>
</msub>
<mo>,</mo>
<msub>
<mi>δt</mi>
<mi>u</mi>
</msub>
<mo>,</mo>
<msub>
<mi>δt</mi>
<mi>ru</mi>
</msub>
<mo>]</mo>
</mrow>
<mi>T</mi>
</msup>
<mo>;</mo>
</mrow>
</math> the system noise vector is: w = [0,0, Wax,way,wgx,wgy,wgz,0,0,0,0,0,wtu,wtru]T;
According to satellite ephemeris data output by a GPS, converting the position and the speed calculated by the SINS into a pseudo range and a pseudo range rate corresponding to the SINS; and respectively making difference with the pseudo range and the pseudo range rate measured by the GPS, and taking the difference as a measurement value of the system, wherein the measurement equation is expressed as follows:
wherein Z is used for pseudo-range measurementρDenoted pseudo-range metrology noise sequence VρPseudorange measurement matrix Hρ(ii) a Pseudo range rate measurementExpressed as a pseudorange rate measurement noise vector ofThe pseudorange rate measurement matrix isAnd discretizing the system state equation and the measurement equation to obtain a discrete system filtering model.
(6) Description of the simulation
When the AUV is in an underwater diving stage, establishing a continuous system filtering model based on SINS/DVL, and respectively adopting a Kalman filtering algorithm, an H infinity filtering algorithm and an H2/H infinity filtering algorithm to estimate the system state so as to correct the SINS; wherein M is2=1.5,M∞=50, a =2.72, b =5.0, simulation time of 2 hours; the result of comparison of the positional accuracy (latitude error Δ L, longitude error Δ λ) is shown in fig. 4.
Description of the working procedure
The working process of the AUV integrated navigation system based on the SINS/DVL/GPS comprises the following steps:
1. three-axis angular velocity information of the carrier is obtained through the sensitivity of a gyroscope in the IMU assembly, three-axis acceleration information is measured through an accelerometer, the IMU processing module receives sensor information output by the IMU, and navigation information such as the position, the velocity and the posture of the carrier is obtained through navigation integral calculation;
2. transmitting ultrasonic waves with certain frequency and certain pulse width and receiving reflected echoes with frequency shift characteristics through four transducers arranged at the bottom end of the AUV;
3. generating an electric oscillation signal with certain frequency, certain pulse width and power by using a transmitting system in a transceiver, and sending the electric oscillation signal to a transducer;
4. the receiving system in the transceiver is used for amplifying the echo signal sent by the transducer, obtaining Doppler frequency shift, converting the echo signal into a navigational speed analog signal and sending the navigational speed analog signal to the interface unit;
5. converting the four navigational speed analog signals sent by the transceiver into navigational speed by using an interface unit in the transceiver, and outputting the navigational speed to an external unit in a digital mode;
6. a sensor information conversion module in the data fusion center periodically calculates the real-time three-dimensional carrier system speed of the carrier according to the four-beam speed information output by the interface unit;
7. a sensor information conversion module in the data fusion center converts relevant information obtained by SINS resolving into a pseudo range and a pseudo range rate corresponding to the SINS according to ephemeris data output by a GPS;
8. and an optimized filtering module in the data fusion center performs filtering fusion calculation on the received SINS navigation information, the ephemeris data of the GPS receiver and the three-dimensional speed information of the DVL log to obtain the final carrier navigation positioning information of the AUV combination system with higher precision.
The above description is only of the preferred embodiments of the present invention, and it should be noted that: it will be apparent to those skilled in the art that various modifications and adaptations can be made without departing from the principles of the invention and these are intended to be within the scope of the invention.
Claims (6)
1. The AUV integrated navigation system based on the SINS/DVL/GPS is characterized in that: the system comprises an SINS system, a GPS receiver, a DVL log and a data fusion center, wherein the SINS system, the GPS receiver, the DVL log and the data fusion center are all arranged on an AUV;
the SINS system comprises an IMU component and an IMU processing unit, wherein the IMU sensor is used for detecting inertial data of the AUV and outputting the inertial data to the IMU processing unit, and the IMU processing unit carries out navigation integral calculation on the inertial data to obtain navigation information of the AUV under a navigation coordinate system;
the GPS receiver is used for receiving and outputting navigation measurement information, available ephemeris data and satellite health state of the AUV;
the DVL log is used for measuring longitudinal and transverse three-dimensional navigational speed information of the AUV relative to the seabed;
the data fusion center is used for information fusion and outputting corrected integrated navigation information and comprises a sensor information conversion module and an optimized filtering module; the sensor information conversion module is used for calculating a pseudo range and a pseudo range rate corresponding to the SINS system by combining available ephemeris data output by the GPS receiver and navigation information output by the SINS system; the optimized filtering module combines navigation information of the SINS, pseudo range and pseudo range rate corresponding to the SINS, available ephemeris data output by the GPS receiver and three-dimensional navigational speed information output by the DVL log to carry out filtering fusion calculation to obtain correction information; the navigation information output by the SINS is corrected through the correction information, and finally corrected combined navigation information is obtained;
when the AUV is positioned on the water surface, the optimized filtering module combines navigation information of the SINS, pseudo range and pseudo range rate corresponding to the SINS and available ephemeris data output by the GPS receiver to perform filtering fusion calculation to obtain correction information;
and when the AUV is underwater, the optimized filtering module performs filtering fusion calculation by combining navigation information output by the SINS and three-dimensional navigational speed information output by the DVL to obtain correction information.
2. The SINS/DVL/GPS based AUV integrated navigation system according to claim 1, wherein: inertial sensors in the IMU include a three-axis orthogonally mounted gyroscope for providing three-axis angular velocity measurements and a three-axis orthogonally mounted accelerometer for providing three-axis acceleration measurements.
3. The SINS/DVL/GPS based AUV integrated navigation system according to claim 2, wherein: the gyroscope is a fiber optic gyroscope, and the accelerometer is a quartz flexible accelerometer.
4. The SINS/DVL/GPS based AUV integrated navigation system according to claim 1, wherein: the DVL log is a four-beam absolute velocity measuring device based on the Doppler effect.
5. The SINS/DVL/GPS based AUV integrated navigation system according to claim 1, wherein: the DVL log comprises a transducer, a transceiver and an interface unit, wherein the transceiver comprises a transmitting system, a receiving system and a calculation compensation circuit; the transducers are acoustic transducers, are arranged at the position, away from the bow part by about 1/3, of the bottom end of the AUV, are four in number, are symmetrically arranged according to the fore-aft line, and are used for converting an electric signal output by the transmitting system into an acoustic signal and then sending the acoustic signal, converting an echo signal into an electric signal and then outputting the electric signal to the receiving system; the receiving system is used for amplifying the electric signals output by the transducer, obtaining Doppler frequency shift, converting the Doppler frequency shift into three-dimensional navigational speed analog signals and sending the three-dimensional navigational speed analog signals to the interface unit, and the interface unit converts the three-dimensional navigational speed analog signals output by the transmitting system into three-dimensional navigational speed and outputs the three-dimensional navigational speed in a digital mode.
6. The SINS/DVL/GPS based AUV integrated navigation system according to claim 1, wherein: the optimized filtering module synthesizes a Kalman filter and an H-infinity filter, and for an input observed value Z, the filtering process of the optimized filtering module comprises the following steps:
(3) calculating a performance index M of a Kalman filterk;
(4) Calculating a self-adaptive adjusting weight value: <math>
<mrow>
<msub>
<mi>d</mi>
<mi>k</mi>
</msub>
<mo>=</mo>
<mfenced open='{' close=''>
<mtable>
<mtr>
<mtd>
<mn>1</mn>
<mo>,</mo>
</mtd>
<mtd>
<msub>
<mi>M</mi>
<mi>k</mi>
</msub>
<mo>≤</mo>
<msub>
<mi>M</mi>
<mn>2</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msup>
<mi>a</mi>
<mrow>
<mo>-</mo>
<mfrac>
<msub>
<mi>M</mi>
<mi>k</mi>
</msub>
<mi>b</mi>
</mfrac>
</mrow>
</msup>
<mo>,</mo>
</mtd>
<mtd>
<msub>
<mi>M</mi>
<mn>2</mn>
</msub>
<mo><</mo>
<msub>
<mi>M</mi>
<mi>k</mi>
</msub>
<mo>≤</mo>
<msub>
<mi>M</mi>
<mo>∞</mo>
</msub>
<mo>;</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
<mo>,</mo>
</mtd>
<mtd>
<msub>
<mi>M</mi>
<mi>k</mi>
</msub>
<mo>></mo>
<msub>
<mi>M</mi>
<mo>∞</mo>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
</math>
wherein M is2Upper bound for Kalman filtering method high accuracy operation, M∞Lower threshold for Kalman filtering method low precision operation, dk∈[0,1]Representing the degree of confidence in the Kalman filter, a and b being design parameters, 1<a≤5,1<b≤10。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410032817.7A CN103744098B (en) | 2014-01-23 | 2014-01-23 | AUV integrated navigation systems based on SINS/DVL/GPS |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410032817.7A CN103744098B (en) | 2014-01-23 | 2014-01-23 | AUV integrated navigation systems based on SINS/DVL/GPS |
Publications (2)
Publication Number | Publication Date |
---|---|
CN103744098A true CN103744098A (en) | 2014-04-23 |
CN103744098B CN103744098B (en) | 2017-03-15 |
Family
ID=50501133
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201410032817.7A Active CN103744098B (en) | 2014-01-23 | 2014-01-23 | AUV integrated navigation systems based on SINS/DVL/GPS |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN103744098B (en) |
Cited By (18)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104316045A (en) * | 2014-11-06 | 2015-01-28 | 东南大学 | AUV (autonomous underwater vehicle) interactive auxiliary positioning system and AUV interactive auxiliary positioning method based on SINS (strapdown inertial navigation system)/LBL (long base line) |
CN104457754A (en) * | 2014-12-19 | 2015-03-25 | 东南大学 | SINS/LBL (strapdown inertial navigation systems/long base line) tight combination based AUV (autonomous underwater vehicle) underwater navigation positioning method |
CN105115492A (en) * | 2015-08-14 | 2015-12-02 | 武汉大学 | Underwater topography matching navigation system based on acoustics Doppler log |
CN107015259A (en) * | 2016-01-27 | 2017-08-04 | 北京中联星通投资管理有限公司 | The tight integration method of pseudorange/pseudorange rates is calculated using Doppler anemometer |
CN107063245A (en) * | 2017-04-19 | 2017-08-18 | 东南大学 | A kind of SINS/DVL integrated navigation filtering methods based on 5 rank SSRCKF |
CN108345313A (en) * | 2018-01-19 | 2018-07-31 | 浙江大学 | A kind of interconnection method of submarine navigation device and connection station |
CN108955678A (en) * | 2018-08-03 | 2018-12-07 | 国家深海基地管理中心 | A kind of deep-sea vehicle communication location navigation time service integral method and system |
CN109141436A (en) * | 2018-09-30 | 2019-01-04 | 东南大学 | The improved Unscented kalman filtering algorithm application method in integrated navigation under water |
CN109856659A (en) * | 2019-01-21 | 2019-06-07 | 同济大学 | Preventing seabed base positions time service and data record system and method |
CN109883444A (en) * | 2019-02-25 | 2019-06-14 | 航天科工防御技术研究试验中心 | A kind of attitude angle coupling error compensation method, device and electronic equipment |
CN109974695A (en) * | 2019-04-09 | 2019-07-05 | 东南大学 | The robust adaptive filtering method of surface ship navigation system based on the space Krein |
CN110567454A (en) * | 2019-09-08 | 2019-12-13 | 东南大学 | SINS/DVL tightly-combined navigation method in complex environment |
CN110579786A (en) * | 2019-07-19 | 2019-12-17 | 北京理工新源信息科技有限公司 | positioning method and system, navigation method and system and vehicle management terminal |
CN112083465A (en) * | 2020-09-18 | 2020-12-15 | 德明通讯(上海)有限责任公司 | Position information acquisition system and method |
CN112284384A (en) * | 2020-10-26 | 2021-01-29 | 东南大学 | Cooperative positioning method of clustered multi-deep-sea submersible vehicle considering measurement abnormity |
CN112484721A (en) * | 2020-11-18 | 2021-03-12 | 中国海洋大学 | Underwater mobile platform navigation method and underwater mobile platform navigation device |
CN112835107A (en) * | 2020-12-31 | 2021-05-25 | 华中科技大学 | Submarine cable electromagnetic detection system and autonomous underwater robot equipment |
CN117606491A (en) * | 2024-01-24 | 2024-02-27 | 中国海洋大学 | Combined positioning navigation method and device for autonomous underwater vehicle |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101464935A (en) * | 2009-01-09 | 2009-06-24 | 哈尔滨工程大学 | AUV intelligent fault-tolerance combined navigation simulation system based on network |
CN101900558A (en) * | 2010-06-04 | 2010-12-01 | 浙江大学 | Combined navigation method of integrated sonar micro navigation autonomous underwater robot |
CN102004447A (en) * | 2010-11-11 | 2011-04-06 | 西北工业大学 | Integrated-navigation and control hardware-in-the-loop simulation test system of underwater vehicle |
CN102818567A (en) * | 2012-08-08 | 2012-12-12 | 浙江大学 | AUV (autonomous underwater vehicle) integrated navigation method integrating Kalman filtering and particle filtering |
CN102829777A (en) * | 2012-09-10 | 2012-12-19 | 江苏科技大学 | Integrated navigation system for autonomous underwater robot and method |
CN103063212A (en) * | 2013-01-04 | 2013-04-24 | 哈尔滨工程大学 | Integrated navigation method based on non-linear mapping self-adaptive hybrid Kalman/H infinite filters |
-
2014
- 2014-01-23 CN CN201410032817.7A patent/CN103744098B/en active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101464935A (en) * | 2009-01-09 | 2009-06-24 | 哈尔滨工程大学 | AUV intelligent fault-tolerance combined navigation simulation system based on network |
CN101900558A (en) * | 2010-06-04 | 2010-12-01 | 浙江大学 | Combined navigation method of integrated sonar micro navigation autonomous underwater robot |
CN102004447A (en) * | 2010-11-11 | 2011-04-06 | 西北工业大学 | Integrated-navigation and control hardware-in-the-loop simulation test system of underwater vehicle |
CN102818567A (en) * | 2012-08-08 | 2012-12-12 | 浙江大学 | AUV (autonomous underwater vehicle) integrated navigation method integrating Kalman filtering and particle filtering |
CN102829777A (en) * | 2012-09-10 | 2012-12-19 | 江苏科技大学 | Integrated navigation system for autonomous underwater robot and method |
CN103063212A (en) * | 2013-01-04 | 2013-04-24 | 哈尔滨工程大学 | Integrated navigation method based on non-linear mapping self-adaptive hybrid Kalman/H infinite filters |
Non-Patent Citations (4)
Title |
---|
段世梅等: "基于SINS-DVL-GPS的AUV组合导航技术", 《火力与指挥控制》 * |
段世梅等: "基于SINS-DVL-GPS的AUV组合导航技术", 《火力与指挥控制》, vol. 34, no. 12, 31 December 2009 (2009-12-31) * |
王圣洁等: "基于SINS-GPS-DVL组合导航定位半实物仿真系统研究", 《计算机测量与控制》 * |
王圣洁等: "基于SINS-GPS-DVL组合导航定位半实物仿真系统研究", 《计算机测量与控制》, vol. 2011, no. 6, 30 June 2011 (2011-06-30) * |
Cited By (24)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104316045A (en) * | 2014-11-06 | 2015-01-28 | 东南大学 | AUV (autonomous underwater vehicle) interactive auxiliary positioning system and AUV interactive auxiliary positioning method based on SINS (strapdown inertial navigation system)/LBL (long base line) |
CN104316045B (en) * | 2014-11-06 | 2017-06-16 | 东南大学 | A kind of AUV based on SINS/LBL interacts aided positioning system and localization method under water |
CN104457754A (en) * | 2014-12-19 | 2015-03-25 | 东南大学 | SINS/LBL (strapdown inertial navigation systems/long base line) tight combination based AUV (autonomous underwater vehicle) underwater navigation positioning method |
CN104457754B (en) * | 2014-12-19 | 2017-04-26 | 东南大学 | SINS/LBL (strapdown inertial navigation systems/long base line) tight combination based AUV (autonomous underwater vehicle) underwater navigation positioning method |
CN105115492A (en) * | 2015-08-14 | 2015-12-02 | 武汉大学 | Underwater topography matching navigation system based on acoustics Doppler log |
CN107015259A (en) * | 2016-01-27 | 2017-08-04 | 北京中联星通投资管理有限公司 | The tight integration method of pseudorange/pseudorange rates is calculated using Doppler anemometer |
CN107015259B (en) * | 2016-01-27 | 2021-03-19 | 中联天通科技(北京)有限公司 | Method for calculating pseudorange/pseudorange rate by using Doppler velocimeter |
CN107063245A (en) * | 2017-04-19 | 2017-08-18 | 东南大学 | A kind of SINS/DVL integrated navigation filtering methods based on 5 rank SSRCKF |
CN108345313A (en) * | 2018-01-19 | 2018-07-31 | 浙江大学 | A kind of interconnection method of submarine navigation device and connection station |
CN108955678A (en) * | 2018-08-03 | 2018-12-07 | 国家深海基地管理中心 | A kind of deep-sea vehicle communication location navigation time service integral method and system |
CN109141436A (en) * | 2018-09-30 | 2019-01-04 | 东南大学 | The improved Unscented kalman filtering algorithm application method in integrated navigation under water |
WO2020062807A1 (en) * | 2018-09-30 | 2020-04-02 | 东南大学 | Method for application of improved unscented kalman filter algorithm in underwater integrated navigation |
CN109856659A (en) * | 2019-01-21 | 2019-06-07 | 同济大学 | Preventing seabed base positions time service and data record system and method |
CN109883444A (en) * | 2019-02-25 | 2019-06-14 | 航天科工防御技术研究试验中心 | A kind of attitude angle coupling error compensation method, device and electronic equipment |
CN109974695A (en) * | 2019-04-09 | 2019-07-05 | 东南大学 | The robust adaptive filtering method of surface ship navigation system based on the space Krein |
CN110579786A (en) * | 2019-07-19 | 2019-12-17 | 北京理工新源信息科技有限公司 | positioning method and system, navigation method and system and vehicle management terminal |
CN110567454A (en) * | 2019-09-08 | 2019-12-13 | 东南大学 | SINS/DVL tightly-combined navigation method in complex environment |
CN112083465A (en) * | 2020-09-18 | 2020-12-15 | 德明通讯(上海)有限责任公司 | Position information acquisition system and method |
CN112284384A (en) * | 2020-10-26 | 2021-01-29 | 东南大学 | Cooperative positioning method of clustered multi-deep-sea submersible vehicle considering measurement abnormity |
CN112284384B (en) * | 2020-10-26 | 2023-11-17 | 东南大学 | Co-positioning method of clustered multi-deep sea submarine considering measurement abnormality |
CN112484721A (en) * | 2020-11-18 | 2021-03-12 | 中国海洋大学 | Underwater mobile platform navigation method and underwater mobile platform navigation device |
CN112835107A (en) * | 2020-12-31 | 2021-05-25 | 华中科技大学 | Submarine cable electromagnetic detection system and autonomous underwater robot equipment |
CN117606491A (en) * | 2024-01-24 | 2024-02-27 | 中国海洋大学 | Combined positioning navigation method and device for autonomous underwater vehicle |
CN117606491B (en) * | 2024-01-24 | 2024-04-26 | 中国海洋大学 | Combined positioning navigation method and device for autonomous underwater vehicle |
Also Published As
Publication number | Publication date |
---|---|
CN103744098B (en) | 2017-03-15 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN103744098B (en) | AUV integrated navigation systems based on SINS/DVL/GPS | |
CN109324330B (en) | USBL/SINS tight combination navigation positioning method based on mixed derivative-free extended Kalman filtering | |
Kinsey et al. | A survey of underwater vehicle navigation: Recent advances and new challenges | |
Kepper et al. | A navigation solution using a MEMS IMU, model-based dead-reckoning, and one-way-travel-time acoustic range measurements for autonomous underwater vehicles | |
CN102829777B (en) | Autonomous underwater vehicle combined navigation system and method | |
CN104457754B (en) | SINS/LBL (strapdown inertial navigation systems/long base line) tight combination based AUV (autonomous underwater vehicle) underwater navigation positioning method | |
CN108594272B (en) | Robust Kalman filtering-based anti-deception jamming integrated navigation method | |
CN104316045A (en) | AUV (autonomous underwater vehicle) interactive auxiliary positioning system and AUV interactive auxiliary positioning method based on SINS (strapdown inertial navigation system)/LBL (long base line) | |
CN103697910B (en) | The correction method of autonomous underwater aircraft Doppler log installation error | |
US20120022820A1 (en) | Method for inertial navigation under water | |
CN110133700B (en) | Shipborne integrated navigation positioning method | |
CN110274591B (en) | ADCP (advanced deep submersible vehicle) assisted SINS (strapdown inertial navigation system) navigation method of deep submersible manned submersible | |
CN109737956A (en) | A kind of SINS/USBL phase difference tight integration navigation locating method based on double response device | |
CN102252677A (en) | Time series analysis-based variable proportion self-adaptive federal filtering method | |
CN104061930B (en) | A kind of air navigation aid based on strap-down inertial guidance and Doppler log | |
CN111829512A (en) | AUV navigation positioning method and system based on multi-sensor data fusion | |
CN112684207B (en) | ADCP (advanced digital control Performance) speed estimation and correction algorithm for deep submersible vehicle | |
Ramesh et al. | Development and performance validation of a navigation system for an underwater vehicle | |
Geng et al. | Accuracy analysis of DVL/IMU/magnetometer integrated navigation system using different IMUs in AUV | |
CN114459476B (en) | Underwater unmanned submarine current measuring DVL/SINS integrated navigation method based on virtual speed measurement | |
CN117970248A (en) | Collaborative navigation positioning method for underwater vehicle based on accompanying acoustic beacon assistance | |
CN117848339A (en) | Water surface and underwater integrated navigation method and device for small unmanned autonomous vehicle | |
CN117870687A (en) | AUV (autonomous Underwater vehicle) underwater integrated navigation positioning system and implementation method | |
CN103697887B (en) | A kind of optimization air navigation aid based on SINS and Doppler log | |
CN117029872A (en) | AUV navigation method and system based on INS/DVL/LBL tight combination |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant |