CN103744098A

CN103744098A - Ship's inertial navigation system (SINS)/Doppler velocity log (DVL)/global positioning system (GPS)-based autonomous underwater vehicle (AUV) combined navigation system

Info

Publication number: CN103744098A
Application number: CN201410032817.7A
Authority: CN
Inventors: 程向红; 许立平; 王磊; 朱倚娴; 冯骥; 韩旭
Original assignee: Southeast University
Current assignee: Southeast University
Priority date: 2014-01-23
Filing date: 2014-01-23
Publication date: 2014-04-23
Anticipated expiration: 2034-01-23
Also published as: CN103744098B

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

SINS/DVL/GPS-based AUV integrated navigation system

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:

(1) computing Kalman Filter at t_kAnd (3) time state estimation:

(2) calculating the H ∞ filter at t_kAnd (3) time state estimation:

(3) computing Kalman filteringPerformance index M of device_k；

(4) Calculating a self-adaptive adjusting weight value:

(5) calculating the comprehensive state estimation of the system:

wherein M is₂Upper 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 is_kA 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 filter_kIf the value is greater than 1, the trust level of the Kalman filter estimation value should be reduced; d_k∈[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 d_kThe 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.

(1) computing Kalman Filter at t_kAnd (3) time state estimation:

(2) calculating the H ∞ filter at t_kAnd (3) time state estimation:

(3) calculating a performance index M of a Kalman filter_k：

(4) Calculating a self-adaptive adjusting weight value:

(5) calculating the comprehensive state estimation of the system:

wherein,is t_kAn innovation sequence calculation formula of the time,

one-step prediction of the estimated value, phi, for the system state_k,k-1Is t_k-1Time to t_kThe state of the moment is transferred to the matrix by one step; tr (A) denotes a trace-finding operation on an arbitrary matrix A, Z_kIs t_kMeasurement vector of time, H_kFor the measurement matrix, R_kFor measuring arrays of variances of noise sequences, P_k,k-1Is t_kSystematic one-step prediction error variance matrix of time, M₂High precision operation for Kalman filtering methodUpper limit of (M)_∞Lower threshold for Kalman filtering method low precision operation, d_k∈[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 d_kThe 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 R_e=6378137.0m, ellipsoid ellipticity e = 1/298.257223563; meridian plane radius of curvature R_NCurvature radius R of unitary and quartile ring_ECan be obtained from the following equation:

\{\begin{matrix} R_{N} = \frac{R_{e} {(1 - e)}^{2}}{{[{(1 - e)}^{2} \sin^{2} L + \cos^{2} L]}^{\frac{3}{2}}} \\ R_{E} = \frac{R_{e}}{{[{(1 - e)}^{2} \sin^{2} L + \cos^{2} L]}^{\frac{1}{2}}} \end{matrix}

earth rotation angular rate omega_ie=15.0411°/h=7.29211585×10^-5rad/s

(iii) acceleration of Earth gravity

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 ox_iy_iz_iIndicating that the origin is at the center of the earth, ox_iAnd oy_iAxis in the equatorial plane of the earth, ox_iAxis pointing to spring equinox, oz_iAxis coincident with the earth's axis of rotation, oy_iWith ox_i、oz_iAnd 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 ox_ey_ez_eIndicating that the origin is at the center of the earth, oz_eAxis coincident with the earth's axis of rotation, ox_eAxis along the intersection of the Greenwich meridian plane and the equatorial plane of the Earth, oy_eAxis in equatorial plane, ox_e、oy_e、oz_eThe 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 earth_ie。

③ geographic coordinate system: g is a

With ox_gy_gz_gMeaning that the origin is at the center of gravity of the carrier, typically using the northeast sky coordinate system as the geographic coordinate system, ox_gThe axis points to east (E), oy_gThe axis pointing to north (N), oz_gThe axis points to the day (U).

Fourthly, navigation coordinate system: n is a

With ox_ny_nz_nIndicating that the northeast geographic coordinate system (ENU) is used as the navigation coordinate system.

A carrier coordinate system: b is a series of

With ox_by_bz_bIndicating that the origin is generally taken at the geometric center of the IMU, ox_bAxis to the right along the transverse axis of the carrier, oy_bAxis forward along the longitudinal axis of the carrier, oz_bThe 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 oy_bThe 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 ox_bThe 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-oz_gAngle of pivot Ψ, axis ox₁Angle of rotation of axis theta, rewind of axis oy₂Shaft 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,

the specific force equation of the carrier in the navigation system is as follows:

wherein,

f^bfor the specific force measured by the accelerometer,

V^{n} = {[\begin{matrix} V_{E}^{n} & V_{N}^{n} & V_{U}^{n} \end{matrix}]}^{T}

for the size of the carrier to the ground speed in the navigation system，gⁿ＝[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: epsilon_i=ε_bi+ε_wi(i=x,y,z)，ε_biIs a random constant, epsilon_wiIs white noise.

An accelerometer error model: v_i＝▽_bi+▽_wi(i＝x，y，z)，▽_biIs a random constant +_wiIs white noise.

The attitude error equation is:

φⁿ=[φ_x φ_y φ_z]^Tis the attitude error.

The velocity error equation is:

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 System_kAnd measure the noise sequence V_kA 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 moment_k，

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 system

And initial estimation error variance matrix P₀According to the measurement of time k_kThe optimal state estimate to time k can be deduced

(k=1，2，3，…)。

Wherein the terms are defined as follows:

X_kis t_kA state vector of a system to be estimated at a moment;

Φ_k,k-1is t_k-1Time to t_kThe state of the moment is transferred to the matrix by one step;

Γ_k-1driving a matrix for system process noise;

Q_kthe variance matrix of the system noise sequence is a non-negative array;

R_kmeasuring a variance matrix of the noise sequence, and taking the variance matrix as a positive definite matrix;

Z_kis a measurement state vector;

H_kis a measurement matrix;

W_k-1noise sequence for a systematic random process;

V_krandomly measuring a noise sequence for the system;

X_k,k-1predicting the system state in one step;

P_kan estimation error variance matrix is obtained;

K_kis a filter gain matrix.

Introduction of H infinity filtering algorithm

Consider the following Krein-based spatial discrete state space system model:

y_kis a given system state X_kLinear combination observations of (2). Wherein, W_kAnd V_kIs 1₂Energy-bounded noise, i.e.

No assumptions are made about its statistical properties. Let the initial state of the system be X₀The initial estimation error variance matrix is P₀，

Is the initial state X of the system₀An estimate of (2). Order toIndicates at a given measured value { Z_kFor y under the condition_kIs estimated.

The suboptimal H-infinity filtering recursion algorithm is as follows:

{\hat{y}}_{k} = L_{k} {\hat{X}}_{k}

K_{k} = P_{k} H_{k}^{T} {(H_{k} P_{k} H_{k}^{T} + I)}^{- 1}

h infinity filtering algorithm in P_kGiven a positive number gamma, through R_e,kFurther adjust P_k. 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, W_kAnd V_kAre zero-mean Gaussian white noise sequences which are uncorrelated with each other and their variance Q_k、R_kIf it is accurate, the kalman filter algorithm finds the sequence of innovation { r_kIs a zero mean, white noise sequence, and the formula for calculating the new sequence is

r_{k} = Z_{k} - H_{k} {\hat{X}}_{k / k - 1},

And the variance is:

E (r_{k} r_{k}^{T}) = H_{k} P_{k, k - 1} H_{k}^{T} + R_{k} .

in ideal conditions, the residual error is output in the Kalman filtering processAnd

the 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 output

And

the ratio of (A) deviates from 1; estimating residual error

Information reflecting the actual estimation error is obtained,

and

the ratio of (c) reflects the filter performance. Defining a Kalman filter performance index M_kThe calculation method comprises the following steps:

M_{k} = \frac{r_{k}^{T} r_{k}}{tr (H_{k} P_{k, k - 1} H_{k}^{T} + R_{k})}

definition M₂Upper bound for Kalman filter high accuracy operation, M₂Close to 1, i.e. for each instant there is M_k≤M₂And 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 M_k≥M_∞The Kalman filter performs poorly and even divergently at this time. When M is₂≤M_k≤M_∞The Kalman filter performance is general.

The process of the optimized filtering algorithm is as follows:

1) solving t by Kalman filtering algorithm_kAnd (3) time state estimation:

2) solving t by using H infinity filtering algorithm_kAnd (3) time state estimation:

3) computing Kalman Filter Performance index M_k；

4) Adaptive adjustment of weight d_kAnd (3) calculating:

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 d_kSensitivity to Kalman filtering performance index changes and precision and robustness of the fusion filter.

5) And (3) estimating the comprehensive state of the system:

(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 error

Attitude angle error (phi)_x,φ_y,φ_z) Random constant bias (v) of accelerometer_x，▽_y) And gyro random constant drift (epsilon)_x,ε_y,ε_z) And 10 dimensions in total:

W=[w_ax,w_ay,w_gx,w_gy,w_gz,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:

Z = [\begin{matrix} V_{SINSe}^{n} - C_{b}^{n} V_{DVLe} \\ V_{SINSn}^{n} - C_{b}^{n} V_{DVLn} \end{matrix}] = HX + V

wherein the system quantity is measured as

The measured noise sequence is V = [ w = [)_vx,w_vy]^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 X

Attitude angle error (phi)_x,φ_y,φ_z) Random constant bias (v) of accelerometer_x，▽_y) And gyro random constant drift (epsilon)_x,ε_y,ε_z) GPS clock error and clock frequency error (δ t)_u,δt_ru) And 14 dimensions in total:

the system noise vector is: w = [0,0, W_ax,w_ay,w_gx,w_gy,w_gz,0,0,0,0,0,w_tu,w_tru]^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 of

The pseudorange rate measurement matrix is

And 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 is₂=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

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 GPS receiver is used for receiving and outputting navigation measurement information, available ephemeris data and satellite health state of the AUV;

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;

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:

(1) computing Kalman Filter at t_kAnd (3) time state estimation:

(2) calculating the H ∞ filter at t_kAnd (3) time state estimation:

(3) calculating a performance index M of a Kalman filter_k；

(4) Calculating a self-adaptive adjusting weight value:

(5) calculating the comprehensive state estimation of the system:

wherein M is₂Upper bound for Kalman filtering method high accuracy operation, M_∞Lower threshold for Kalman filtering method low precision operation, d_k∈[0,1]Representing the degree of confidence in the Kalman filter, a and b being design parameters, 1<a≤5，1<b≤10。