CN104457754B - SINS/LBL (strapdown inertial navigation systems/long base line) tight combination based AUV (autonomous underwater vehicle) underwater navigation positioning method - Google Patents

Abstract

The invention provides an SINS/LBL (strapdown inertial navigation systems/long base line) tight combination based AUV (autonomous underwater vehicle) underwater navigation positioning method. The SINS/LBL tight combination based AUV underwater navigation positioning method is characterized by comprising three major parts, namely an SINS mounted on an AUV, an LBL underwater sound positioning system laid on the seabed, and a data processing unit. The method comprises the following specific steps: firstly performing a strapdown algorithm on IMU (inertial measurement unit) data to obtain AUV position information, and representing the position information by using earth rectangular coordinates; secondly reckoning an SINS slant-range difference according to the AUV position information provided by the SINS and hydrophone array position coordinates; and thirdly establishing an LBL slant-range difference model according to LBL positioning characteristics, and correcting SINS navigation positioning information according to filter estimation compensation by taking the difference value between the SINS slant-distance difference and the LBL slant-distance difference as an observed quantity of a kalman filter. According to the SINS/LBL tight combination based AUV underwater navigation positioning method, the use of GPS and other radio positioning systems is avoided at the same, and the AUV underwater operation efficiency is improved.

Description

A kind of AUV underwater navigation localization methods based on SINS/LBL tight integrations

Technical field

The invention mainly relates to AUV underwater navigation technical fields, more particularly to a kind of AUV based on SINS/LBL tight integrations Underwater navigation localization method, is particularly well-suited to the track and localization of autonomous underwater vehicle AUV.

Background technology

AUV (Autonomous Underwater Vehicle, Autonomous Underwater Vehicle) is that one kind can be completed under water The underwater tool of the several functions such as detection, attack, delivery, salvaging, because its range of activity is wide, small volume, lightweight, disguised height The features such as, become an important directions of military affairs marine technology research both at home and abroad.

AUV high-precision independent navigation under water and locating and tracking technology are the premises and key for completing its underwater performance.Existing In some location technologies, SINS (Strapdown Inertial Navigation Systems, strap-down inertial navigation system) Because its have disguised strong, autonomy, anti-interference, data renewal frequency it is high, and the features such as there is degree of precision at short notice, Thus become the first-selected localization method of AUV Camera calibrations under water.At present, although the development of strap-down inertial technology day Become ripe, its navigation positioning error does not but change with this dynamic characteristic that time integral dissipates, in long-range, long-term navigation and force The high accuracy such as device transmitting can't fully meet requirement when navigating.The solution for appearing as this problem of integrated navigation technology is provided A kind of effective way.

LBL (Long Base Line, Long baselines) acoustic positioning system is for thousand of by the length of base installed in seabed The transponder basic matrix of rice and the composition of the interrogator on carrier, its positioning principle is using the interrogator on carrier and seabed The distance between transponder arrays information is solving AUV positions.LBL is widely used to because its sphere of action is wide, positioning precision is high Underwater hiding-machine.

In recent years, the autonomous navigation technology under water of AUV is applied to mainly with SINS and DVL (Doppler Velocity Log, Doppler anemometer) integrated navigation based on, (Global Positioning System, the whole world are fixed to be aided with water surface GPS Position system) amendment.Good navigation accuracy is achieved in testing several times, but voyage is relatively short, for DVL, works as sonar Sensor is very poor away from measuring speed precision during seabed, and when only pressing close to seabed to AUV, precision is preferable, and for GPS, AUV is needed Interruption is moved under water, and climbs up on top of the water and could utilize GPS information, and this will waste substantial amounts of time and the energy in the case of deep-sea, seriously Affect the underwater performance efficiency of AUV.

The content of the invention

For the problem of existing AUV underwater navigations precision, the invention provides a kind of AUV based on SINS/LBL tight integrations Underwater navigation localization method.

The purpose of the present invention can be achieved through the following technical solutions, specially：

(1) strapdown inertial navigation system SINS (1) is resolved by strapdown and is obtained leading including the positional information of AUV accordingly Boat information, the positional information earth geodetic coordinates P of resolving_SINS(L_S,λ_S,h_S) represent, and by P_SINS(L_S,λ_S,h_S) it is converted into use Earth rectangular coordinate P_SINS(x_S,y_S,z_S) represent；

(2) the SINS AUV positional information P that primitive is provided according to SINS with target oblique distance difference reckoning module (3) two-by-two_SINS (x_S,y_S,z_S) and hydrophone array position P_i(x_i,y_i,z_i) calculate SINS oblique distance difference ρ_SINS；

(3) SINS/LBL tight integrations module (4) sets up LBL according to the localization characteristics of long baseline acoustic positioning system LBL (2) Oblique distance differential mode type, by SINS oblique distances difference ρ_SINSAnd oblique distances of LBL hydrophone i (i=1,2,3) and AUV between and hydrophone 0 with Difference ρ of the oblique distance between AUV_LBLDifference be filtered to Kalman filter as external observation information input；

(4) correction module (5) is carried out to SINS (1) according to the Kalman filtered results of SINS/LBL tight integration modules (4) Correction, finally gives accurate AUV positional informationes P_AUV。

The primitive method for calculating that module (3) calculating SINS oblique distances are poor poor with target oblique distance is as follows two-by-two for described SINS：

(1) according to hydrophone position P in long baseline acoustic positioning system LBL_i(x_i,y_i,z_i) and SINS resolving AUV positions P_SINS(x_s,y_s,z_s) it is calculated the oblique distance of (i=1,2,3) and AUV between hydrophone i and the oblique distance between hydrophone 0 and AUV Difference

(2) by ρ_SINSiUsing Taylor series linearisation.If AUV actual positions are P_AUV(x, y, z), (δ x, δ y, δ z) is SINS resolves the error of AUV positions, then x_S=x+ δ x, y_S=y+ δ y, z_S=z+ δ z.By ρ_SINSiTaylor series expansion takes first two ：

If

In the same manner

Wherein,G_ij(i=0,1,2,3；J=x, y, z) For known quantity, the general location P that can be resolved by SINS_SINS(x_S,y_S,z_S) and water-bed transponder arrays primitive position P_i(x_i,y_i, z_i) be calculated, due to the general location P that SINS is resolved_SINS(x_S,y_S,z_S) there may be larger error, so carrying out equation line Property when omit higher order term and can cause linearity error, it is possible to use iterative method is resolved, i.e., after first time solution, use it as near Recalculated like value again.

If：e_ix=G_ix-G_0x, e_iy=G_iy-G_0y, e_iz=G_iz-G_0z, i=1,2,3

Then：

ρ_SINSi=R_i-R₀+(G_ix-G_0x)δx+(G_iy-G_0y)δy+(G_iz-G_0z)δz

=R_i-R₀+e_ixδx+e_iyδy+e_izδz

Described SINS/LBL tight integration modules (4) to implement step as follows：

(1) LBL oblique distance differential mode types are set up

As time delay difference measurements, multi-pathway effect of acoustic propagation etc. will cause oblique distance difference measurements to have error, it is simplified model, It is believed that oblique distance mistake difference is made up of constant value biasing and random noise, then LBL hydrophone i (i=1,2,3) with the oblique distance of AUV It is represented by with difference of the hydrophone 0 with the oblique distance of AUV：

In formula, Δ R_measFor LBL hydrophone i (i=1,2, the 3) difference with the oblique distance of AUV and hydrophone 0 and the oblique distance of AUV, Δ R is oblique distance difference true value, δ R=[δ R₁ δR₂ δR₃]^TFor random constant value, ν_δR(t)～N (0, Q_ΔR) for white Gaussian noise.

(2) SINS/LBL tight integration state equations are set up

SINS/LBL tight integration state equations are described as：

Wherein：X_SINSFor the state vector of SINS, X_LBLFor the state vector of LBL, F_SINSFor the transfer matrix of SINS, F_LBL For the transfer matrix of LBL, W_SINSFor the system noise vector of SINS, W_LBLFor the system noise vector of LBL, F is tight integration system Transfer matrix, X are tight integration system mode vector, and W is tight integration system noise vector.

According to error features during strapdown inertial navigation system long-term work, site error, velocity error, attitude is selected to miss Difference, gyroscopic drift and accelerometer bias are used as quantity of state：

X_SINS=[δ V_E δV_N δV_U φ_E φ_N φ_U δL δL δh ▽_bx ▽_by ▽_bz ε_bx ε_by ε_bz]^T

In formula, δ V_E、δV_N、δV_UBe respectively strapdown east orientation, north orientation, day to velocity error,It is prompt respectively Connection east orientation, north orientation, day to misalignment, δ L, δ λ, δ h are strapdown latitude, longitude, height error respectively, three site errors by Terrestrial coordinate system is described, ▽_bx、▽_by、▽_bzIt is biased error that strapdown adds three axial directions of table, ε_bx、ε_by、ε_bzIt is Strapdown Gyro Using Three are axially drifted about.

X_LBL=[δ R₁ δR₂ δR₃]^T

In formula, δ R₁、δR₂、δR₃The oblique distance of respectively LBL hydrophone i (i=1,2,3) and AUV is with hydrophone 0 with AUV's The Random Constant Drift of the difference of oblique distance.

System noise acoustic matrix

W_LBL=[0 0 0]^T

Systematic state transfer matrix

In formula,

Wherein：F_ijFor F_9×9Element

R_NFor the radius of curvature of reference ellipsoid meridian plane Inner, R_N=R_e(1-2e+3e sin² L)

R_EFor the radius of curvature of vertical meridian plane Inner, R_E=R_e(1+e sin² L)

Wherein：R_eFor the major axis radius of reference ellipsoid；Ovalitys of the e for ellipsoid.

F₃₇=-2 ω_ie cos LV_E

F₅₇=-ω_ie sin L

C_ijFor attitude transfer matrixElement

F_LBL=0_3×3

(3) SINS/LBL tight integration measurement equations are set up.

Tight integration system is using the hydrophone that SINS the is calculated difference poor with the oblique distance that LBL measurements are obtained with the oblique distance difference of AUV As observed quantity.In tight integration system, if the oblique distance difference that LBL is measured is ρ_LBLi, the position of water-bed transponder arrays primitive is P (x_i,y_i,z_i), the AUV positions that SINS is measured are P_SINS(x_S,y_S,z_S), the AUV positions P measured by SINS_SINS(x_S,y_S,z_S) and The position of water-bed transponder arrays primitive is P_i(x_i,y_i,z_i) determined by oblique distance difference be ρ_SINSi。

SINS oblique distances are poor：

ρ_SINSi=R_i-R₀+(G_ix-G_0x)δx+(G_iy-G_0y)δy+(G_iz-G_0z)δz

=R_i-R₀+e_ixδx+e_iyδy+e_izδz

LBL oblique distances are poor

Then measure and can be write as

Then have：

When system adopts earth rectangular coordinate system (Ox_ey_ez_e) as navigational coordinate system when, can with above formula construct system measurements Equation.It is that, with longitude and latitude and altitude location, therefore dx, dy, dz dl, d λ, dh are represented in practical application.

By

Measurement equation is Z_3×1=H_3×18X_18×1+V_ΔR(3×1)

In formula,

IfWherein a_ij(i=1,2,3；J=1,2,3 it is) matrix H₁Element

H₁Nonzero element is as follows:

a_i1=-(R_N+h)sin L cos λe_i1-(R_N+h)sin L sin λe_i2+[R_N(1-e²)+h]e_i3

a_i2=-(R_N+h)cos L sin λe_i1-(R_N+h)cos L cos λe_i2

a_i3=cos L cos λ e_i1+cos L sin λe_i2+sin Le_i3(i=1,2,3)

Described correction module (5) enters to SINS (1) according to the Kalman filtered results of SINS/LBL tight integration modules (4) Row correction, finally gives accurate AUV positional informationes P_AUV。

Compared with prior art, the invention has the advantages that：

(1) solve the problems, such as SINS systematic errors with time integral, it is ensured that AUV long-term autonomous navigator fixs under water Precision, while avoid the use of GPS and other radio positioning systems, be that underwater performance saves time and energy consumption, improve AUV underwater performance efficiency.

(2) present invention introduces SINS and LBL tight integrations, to inertial navigation system and sound system combination application Research has certain meaning.

Description of the drawings

Fig. 1 is SINS/LBL tight integration alignment system theory diagrams；

Fig. 2 is long baseline acoustic positioning system LBL schematic diagrams；

Fig. 3 is hydrophone node locating schematic diagram.

Specific embodiment

Below in conjunction with the accompanying drawings, further elucidate the present invention.

As shown in figure 1, the present invention is placed on the length in seabed by the strapdown inertial navigation system SINS (1) on AUV, cloth Baseline acoustic positioning system LBL (2) and data processing unit three parts composition.Data processing unit includes SINS primitives two-by-two With the poor computing module of AUV oblique distances (3), SINS/LBL tight integration modules (4) and correction module (5).By tight with LBL using SINS The method of combination completes AUV independent navigations under water, implements step as follows：

(1) inertial measurement component (IMU) output data is resolved by strapdown and obtains AUV positional informationes, use earth ground Coordinate P_SINS(L_S,λ_S,h_S) represent, and by P_SINS(L_S,λ_S,h_S) be converted into earth rectangular coordinate P_SINS(x_S,y_S,z_S) represent.

Described SINS (1) system includes IMU (Inertial Measurement Unit, Inertial Measurement Unit) element And strapdown resolves module, for obtaining inertial data, strapdown resolves module is used to be resolved by strapdown, is navigated IMU elements Information, including positional information P_SINS。

1) SINS attitude matrixs and attitude angle are calculated

Attitude matrix is calculated using Quaternion Method, according to Euler's theorem, orientation etc. of the moving coordinate system with respect to reference frame Imitate and rotate an angle, θ around certain Equivalent Axis in moving coordinate system, if the unit vector in Equivalent Axis direction is represented with u, The orientation of moving coordinate system is determined completely by two parameters of u and θ.

A quaternary number can be constructed with u and θ：

To above formula derivation, simultaneously abbreviation can obtain quaternion differential equation：

In formula

Quaternion differential equation is solved according to complete card approximatioss to obtain：

In formula

In formula

The spin velocity for making terrestrial coordinate system relative inertness coordinate system is ω_ie, (its value is 15.04088 °/h), L is represented Local latitude, λ represent local longitude, then

ω_ie ⁿ：Vector of the spin velocity of terrestrial coordinate system relative inertness coordinate system in geographic coordinate system, be：

ω_ie ^b：Vector of the spin velocity of terrestrial coordinate system relative inertness coordinate system in carrier coordinate system, be：

Attitude matrix in formula is determined by initial angle in carrier stationary；When carrier is rotated relative to geographic coordinate system, Attitude matrix and then changes, and tries to achieve (similarly hereinafter) after being corrected by quaternary number immediately.

ω_en ⁿ:Vector of the geographical coordinate with respect to terrestrial coordinate system rotational angular velocity in geographic coordinate system, be：

V_E、V_NThe respectively east orientation and north orientation speed of carrier movement；

R_NFor the radius of curvature in reference ellipsoid meridian plane, R_N=R_e(1-2e+3e sin²L)；

R_EFor the radius of curvature in the plane normal of vertical meridian plane, R_E=R_e(1+e sin²L)；

Wherein R_eFor the major axis radius of reference ellipsoid；Ovalitys of the e for ellipsoid.

And because,Then

ω_en ^b:Vector of the geographical coordinate with respect to terrestrial coordinate system rotational angular velocity in carrier coordinate system, be：

ω_ib ^b：Gyro output angle speed, is designated as

ω_nb ^b：Carrier coordinate system is designated as with respect to the vector of the rotational angular velocity in carrier coordinate system of geographic coordinate system

Can then obtain

ω_nb ^b=ω_ib ^b-ω_ie ^b-ω_en ^b

After quaternary number is corrected immediately, can be by first real-time update attitude matrix of quaternary number according to following formula

Real-time attitude angle is can extract from attitude battle array

2) SINS speed calculation

Ratio force vector in carrier coordinate system is f^b, then have in geographic coordinate system：

Direction cosine matrix in formulaIn carrier stationary, determined by initial angle；When the relative geographic coordinate system of carrier During rotation, direction cosine matrixAnd then change, try to achieve after being corrected by quaternary number immediately.

Specific force equation of the carrier in inertial navigation system be：

Being write as component form has：

In formula：fⁿFor the projection that carrier acceleration is fastened in navigation coordinate, fⁿ=[f_E f_N f_U]^T；VⁿRepresent that hull is being led Velocity in boat coordinate system, Vⁿ=[V_E V_N V_U]^T；gⁿFor gravity acceleration, gⁿ=[0 0-g]^T。

Integration above formula, you can try to achieve each velocity component V that carrier is fastened in navigation coordinate_E、V_N、V_U。

3) position calculation

The differential equation for obtaining longitude and latitude height can be expressed as follows：

In formula, h is height.

The more new formula of the longitude and latitude height of integration above formula can obtain longitude and latitude and height：

Then obtain position P (λ, L, h).

4) by the AUV for 3) obtaining earth rectangular coordinate system coordinate P_SINS(L_S,λ_S,h_S) which is converted in earth ground The coordinate P of coordinate system_SINS(x_S,y_S,z_S)。

Can be by formula

Obtain P_SINS(x_S,y_S,z_S)。

In formula:R_NFor the radius of curvature of reference ellipsoid meridian plane Inner, R_N=R_e(1-2e+3e sin² L)

R_EFor the radius of curvature of vertical meridian plane Inner, R_E=R_e(1+e sin² L)

Wherein：R_eFor the major axis radius of reference ellipsoid；Ovalitys of the e for ellipsoid.

(2) primitive is calculated SINS with target oblique distance difference two-by-two

1) the AUV positions P resolved according to SINS_SINS(x_s,y_s,z_s) and long baseline acoustic positioning system LBL in hydrophone base First position P_i(x_i,y_i,z_i) be calculated between oblique distance and hydrophone 0 and AUV of the hydrophone i (i=1,2,3) and AUV between The difference of oblique distance

Described long baseline acoustic positioning system LBL (2) four positions in seabed are placed on by cloth known to hydrophone constitute, As shown in Fig. 2 the distance between each hydrophone is 4km.As shown in figure 3, lash ship is utilized, using ultra short base line to hydrophone It is accurately positioned, is calculated accurate coordinates value.GPS, IMU and compass are installed on lash ship, lash ship bottom is provided with transducer array. Relative position of each hydrophone under transducer array coordinate is calculated according to ultra short base line, with reference to lash ship GPS location, The factor such as lash ship attitude and each alignment error can calculate absolute position of each hydrophone node under terrestrial coordinates.

2) by ρ_SINSiUsing Taylor series linearisation.If AUV actual positions are P_AUV(x, y, z), (δ x, δ y, δ z) are SINS The error of AUV positions is resolved, then x_S=x+ δ x, y_S=y+ δ y, z_S=z+ δ z.By ρ_SINSiTaylor series expansion takes first two and obtains：

If

In the same manner

Wherein,G_ij(i=0,1,2,3；J=x, y, z) For known quantity, the general location P that can be resolved by SINS_SINS(x_S,y_S,z_S) and water-bed transponder arrays primitive position P_i(x_i,y_i, z_i) be calculated, due to the general location P that SINS is resolved_SINS(x_S,y_S,z_S) there may be larger error, so carrying out equation line Property when omit higher order term and can cause linearity error, it is possible to use iterative method is resolved, i.e., after first time solution, use it as near Recalculated like value again.

If：e_ix=G_ix-G_0x, e_iy=G_iy-G_0y, e_iz=G_iz-G_0z, i=1,2,3

Then：

ρ_SINSi=R_i-R₀+(G_ix-G_0x)δx+(G_iy-G_0y)δy+(G_iz-G_0z)δz

=R_i-R₀+e_ixδx+e_iyδy+e_izδz

(3) SINS/LBL tight integrations

1) LBL oblique distance differential mode types are set up

As time delay difference measurements, multi-pathway effect of acoustic propagation etc. will cause oblique distance difference measurements to have error, it is simplified model, It is believed that oblique distance mistake difference is made up of constant value biasing and random noise, then LBL hydrophone i (i=1,2,3) with the oblique distance of AUV It is represented by with difference of the hydrophone 0 with the oblique distance of AUV：

In formula, Δ R_measFor LBL hydrophone i (i=1,2, the 3) difference with the oblique distance of AUV and hydrophone 0 and the oblique distance of AUV, Δ R is oblique distance difference true value, δ R=[δ R₁ δR₂ δR₃]^TFor random constant value, ν_δR(t)～N (0, Q_ΔR) for white Gaussian noise.

2) SINS/LBL tight integration state equations are set up

SINS/LBL tight integration state equations are described as：

Wherein：X_SINSFor the state vector of SINS, X_LBLFor the state vector of LBL, F_SINSFor the transfer matrix of SINS, F_LBL For the transfer matrix of LBL, W_SINSFor the system noise vector of SINS, W_LBLFor the system noise vector of LBL, F is tight integration system Transfer matrix, X are tight integration system mode vector, and W is tight integration system noise vector.

According to error features during strapdown inertial navigation system long-term work, site error, velocity error, attitude is selected to miss Difference, gyroscopic drift and accelerometer bias are used as quantity of state：

X_SINS=[δ V_E δV_N δV_U φ_E φ_N φ_U δL δL δh ▽_bx ▽_by ▽_bz ε_bx ε_by ε_bz]^T

In formula, δ V_E、δV_N、δV_UBe respectively strapdown east orientation, north orientation, day to velocity error,It is prompt respectively Connection east orientation, north orientation, day to misalignment, δ L, δ λ, δ h are strapdown latitude, longitude, height error respectively, three site errors by Terrestrial coordinate system is described, ▽_bx、▽_by、▽_bzIt is biased error that strapdown adds three axial directions of table, ε_bx、ε_by、ε_bzIt is Strapdown Gyro Using Three are axially drifted about.

X_LBL=[δ R₁ δR₂ δR₃]^T

In formula, δ R₁、δR₂、δR₃The oblique distance of respectively LBL hydrophone i (i=1,2,3) and AUV is with hydrophone 0 with AUV's The random constant error of the difference of oblique distance.

System noise acoustic matrix

W_LBL=[0 0 0]^T

Systematic state transfer matrix

In formula,

Wherein：F_ijFor F_9×9Element,

R_NFor the radius of curvature of reference ellipsoid meridian plane Inner, R_N=R_e(1-2e+3e sin² L)

R_EFor the radius of curvature of vertical meridian plane Inner, R_E=R_e(1+e sin² L)

Wherein：R_eFor the major axis radius of reference ellipsoid；Ovalitys of the e for ellipsoid.

F₃₇=-2 ω_ie cos LV_E

F₅₇=-ω_ie sin L

C_ijFor attitude transfer matrixElement

F_LBL=0_3×3

3) SINS/LBL tight integration measurement equations are set up

Tight integration system is using the hydrophone that SINS the is calculated difference poor with the oblique distance that LBL measurements are obtained with the oblique distance difference of AUV As observed quantity.In tight integration system, if the oblique distance difference that LBL is measured is ρ_LBLi, the position of water-bed transponder arrays primitive is P (x_i,y_i,z_i), the AUV positions that SINS is measured are P_SINS(x_S,y_S,z_S), the AUV positions P measured by SINS_SINS(x_S,y_S,z_S) and The position of water-bed transponder arrays primitive is P_i(x_i,y_i,z_i) determined by oblique distance difference be ρ_SINSi。

SINS oblique distances are poor：

LBL oblique distances are poor

Then measure and can be write as

Then have：

When system adopts earth rectangular coordinate system (Ox_ey_ez_e) as navigational coordinate system when, can with above formula construct system measurements Equation.It is that, with longitude and latitude and altitude location, therefore dx, dy, dz dl, d λ, dh are represented in practical application.

By

Measurement equation is Z_3×1=H_3×18X_18×1+V_ΔR(3×1)

In formula,

Wherein a_ij(i=1,2,3；J=1,2,3 it is) matrix H₁Element

H₁Nonzero element is as follows:

a_i1=-(R_N+h)sin L cos λe_i1-(R_N+h)sin L sin λe_i2+[R_N(1-e²)+h]e_i3

a_i2=-(R_N+h)cos L sin λe_i1-(R_N+h)cos L cos λe_i2

a_i3=cos L cos λ e_i1+cos L sin λe_i2+sin Le_i3(i=1,2,3)

4) discretization of system state equation and measurement equation

X_k=φ_k,k-1X_k-1+Γ_k-1W_k-1

Z_k=H_kX_k+V_k

In formula, X_kFor the state vector at k moment, that is, it is estimated vector；Z_kFor the measurement sequence at k moment；W_k-1For k-1 The system noise at moment；V_kFor the measurement noise sequence at k moment；Φ_k,k-1For the step state transfer square at k-1 moment to k moment Battle array；Γ_k-1It is system noise input matrix, H_kFor the calculation matrix at k moment,

The optimal estimation of state is calculated using standard Kalman filtering equations：

State one-step prediction vector

X_k/k-1=φ_k,k-1X_k-1

State Estimation is calculated

X_k=X_k/k-1+K_k(Z_k-H_kX_k/k-1)

Filtering gain

K_k=P_k/k-1H_k ^T(H_kP_k/k-1H_k ^T+R_k)^-1

One-step prediction mean square error matrix

Estimate mean square error equation

(4) correct

According to the state estimation that filtering is obtained, it is corrected by following methods.

1) speed and position correction

Before filtering next time, the speed and position that each strapdown resolving is obtained is corrected by following formula：

2) inertia type instrument output calibration

Before filtering next time, the inertia type instrument output required when resolving of each strapdown is being corrected by following formula using front：

3) attitude matrix, the correction of quaternary number

Attitude updating：Before filtering next time, each strapdown is resolved obtain as the following formulaBe corrected.

Quaternary number is corrected：Because strapdown is resolved and uses Quaternion Algorithm, changed using quaternary number in algorithm What in generation, updated, it is all also to need to be corrected quaternary number.Quaternary number can be by the attitude matrix for updatingIt is converted to.

Claims (1)

1. a kind of AUV underwater navigation localization methods based on SINS/LBL tight integrations, it is characterised in that：Navigation positioning system used By the strapdown inertial navigation system SINS (1) on the AUV, cloth be placed on seabed long baseline acoustic positioning system LBL (2) and Data processing unit is constituted, wherein, described strapdown inertial navigation system SINS (1) is including strapdown resolves module, described length Baseline acoustic positioning system LBL (2) four positions in seabed are placed on by cloth known to hydrophone array constitute, at described data Reason unit includes that primitive calculates module (3), SINS/LBL tight integration modules (4) and correction module with AUV oblique distances difference to SINS two-by-two (5) integrated navigation is completed using SINS/LBL tight integration methods, methods described is realized through the following steps：

(1) strapdown inertial navigation system SINS (1) resolves the navigation letter for obtaining positional information accordingly including AUV by strapdown Breath, the positional information earth geodetic coordinates P of resolving_SINS(L_S,λ_S,h_S) represent, and by P_SINS(L_S,λ_S,h_S) be converted into and use the earth Rectangular coordinate P_SINS(x_S,y_S,z_S) represent；

(2) the SINS AUV positional information P that primitive is provided according to SINS with AUV oblique distances difference reckoning module (3) two-by-two_SINS(x_S,y_S, z_S) and hydrophone array position P_i(x_i,y_i,z_i) calculate SINS oblique distance difference ρ_SINS；

(3) SINS/LBL tight integrations module (4) sets up LBL oblique distances according to the localization characteristics of long baseline acoustic positioning system LBL (2) Differential mode type, by SINS oblique distances difference ρ_SINSAnd the oblique distance and the oblique distance between hydrophone 0 and AUV between LBL hydrophone i and AUV it Difference ρ_LBLDifference be filtered to Kalman filter as external observation information input, wherein, i=1,2,3；

(4) correction module (5) is corrected to SINS (1) according to the Kalman filtered results of SINS/LBL tight integration modules (4), Finally give accurate AUV positional informationes P_AUV；With AUV oblique distances difference, primitive calculates that module (3) calculating SINS oblique distances are poor to SINS two-by-two Method it is as follows：

(1) the AUV positions P resolved according to SINS_SINS(x_s,y_s,z_s) and long baseline acoustic positioning system LBL in hydrophone primitive position Put P_i(x_i,y_i,z_i) oblique distance that is calculated between hydrophone i and AUV and the oblique distance between hydrophone 0 and AUV differenceWherein, i=1,2, 3；

(2) by ρ_SINSiUsing Taylor series linearisation, if AUV actual positions are P_AUV(x, y, z), (δ x, δ y, δ z) are solved for SINS The error of AUV positions is calculated, then x_S=x+ δ x, y_S=y+ δ y, z_S=z+ δ z；By ρ_SINSiTaylor series expansion takes first two and obtains：

$\begin{array}{c}{\mathrm{\ρ}}_{SINSi}=\sqrt{{\left(x-x_i\right)}^2+{\left(y-y_i\right)}^2+{\left(z-z_i\right)}^2}-\sqrt{{\left(x-x_0\right)}^2+{\left(y-y_0\right)}^2+{\left(z-z_0\right)}^2}\\ +\frac{\∂{\mathrm{\ρ}}_{SINSi}}{\∂x}\mathrm{\δ}x+\frac{\∂{\mathrm{\ρ}}_{SINSi}}{\∂y}\mathrm{\δ}y+\frac{\∂{\mathrm{\ρ}}_{SINSi}}{\∂z}\mathrm{\δ}z\end{array}$

If

In the same manner

$\frac{\∂{\mathrm{\ρ}}_{SINSi}}{\∂z}=\frac{z_S-z_i}{R_i}-\frac{z_S-z_0}{R_0}=G_{iz}-G_{0z},i=1,2,3;$

Wherein,G_ijFor known quantity, i= 0,1,2,3, j=x, y, z, the general location P that can be resolved by SINS_SINS(x_S,y_S,z_S) and water-bed transponder arrays primitive position P_i (x_i,y_i,z_i) be calculated, due to the general location P that SINS is resolved_SINS(x_S,y_S,z_S) there may be larger error, so entering Higher order term is omitted during row equation linearisation and can cause linearity error, resolved using iterative method, i.e., after first time solution, made with it Recalculated for approximation again；

If：e_ix=G_ix-G_0x, e_iy=G_iy-G_0y, e_iz=G_iz-G_0z, i=1,2,3

Then：

$\begin{array}{c}{\mathrm{\ρ}}_{SINSi}=R_i-R_0+\left(G_{ix}-G_{0x}\right)\mathrm{\δ}x+\left(G_{iy}-G_{0y}\right)\mathrm{\δ}y+\left(G_{iz}-G_{0z}\right)\mathrm{\δ}z\\ =R_i-R_0+e_{ix}\mathrm{\δ}x+e_{iy}\mathrm{\δ}y+e_{iz}\mathrm{\δ}z\end{array},i=1,2,3;$

The SINS/LBL tight integrations module (4) to implement step as follows：

(1) LBL oblique distance differential mode types are set up；As time delay difference measurements, multi-pathway effect of acoustic propagation etc. will cause oblique distance difference measurements There is error, be simplified model, it is believed that oblique distance mistake difference is made up of constant value biasing and random noise, then LBL hydrophone i and AUV Difference of the oblique distance with hydrophone 0 with the oblique distance of AUV be represented by：

$\left\{\begin{array}{c}{\mathrm{\ΔR}}_{meas}=\mathrm{\Δ}R+\mathrm{\δ}R+{\mathrm{\ν}}_{\mathrm{\Δ}R}\\ \mathrm{\δ}\stackrel{\·}{R}=0\end{array}\right.$

In formula, Δ R_measFor the oblique distance and hydrophone 0 and the difference of the oblique distance of AUV of LBL hydrophone i and AUV, Δ R is that oblique distance difference is true Value, δ R=[δ R₁ δR₂ δR₃]^TFor random constant value, v_ΔR～N (0, Q_ΔR) for white Gaussian noise, i=1,2,3；

(2) SINS/LBL tight integration state equations are set up；

SINS/LBL tight integration state equations are described as：

Wherein：X_SINSFor the state vector of SINS, X_LBLFor the state vector of LBL, F_SINSFor the transfer matrix of SINS, F_LBLFor LBL Transfer matrix, W_SINSFor the system noise vector of SINS, W_LBLSystem noise for LBL is vectorial, and F is that tight integration system is shifted Matrix, X are tight integration system mode vector, and W is tight integration system noise vector；According to strapdown inertial navigation system long-term work When error features, select site error, velocity error, attitude error, gyroscopic drift and accelerometer bias as quantity of state：

In formula, δ V_E、δV_N、δV_UBe respectively strapdown east orientation, north orientation, day to velocity error,It is strapdown east respectively To, north orientation, day to misalignment, δ L, δ λ, δ h are strapdown latitude, longitude, height error respectively, and three site errors are by the earth Coordinate system is described,It is biased error that strapdown adds three axial directions of table, ε_bx、ε_by、ε_bzIt is three of Strapdown Gyro Using Axially drift about；

X_LBL=[δ R₁ δR₂ δR₃]^T

In formula, δ R₁、δR₂、δR₃The respectively oblique distance of LBL hydrophone i and AUV is random with the difference of the oblique distance of AUV with hydrophone 0 Constant value drift, wherein, i=1,2,3；System noise acoustic matrix

$W_{SINS}={\left[\begin{array}{ccccccccccccccc}{\mathrm{\ω}}_{V_E}& {\mathrm{\ω}}_{V_N}& {\mathrm{\ω}}_{V_U}& {\mathrm{\ω}}_{{\mathrm{\φ}}_E}& {\mathrm{\ω}}_{{\mathrm{\φ}}_N}& {\mathrm{\ω}}_{{\mathrm{\φ}}_U}& {\mathrm{\ω}}_{\mathrm{\δ}L}& {\mathrm{\ω}}_{\mathrm{\δ}\mathrm{\λ}}& {\mathrm{\ω}}_{\mathrm{\δ}h}& 0& 0& 0& 0& 0& 0\end{array}\right]}^T$

W_LBL=[0 0 0]^T

Systematic state transfer matrix

In formula,

Wherein：F_ijFor F_9×9Element

R_NFor the radius of curvature of reference ellipsoid meridian plane Inner, R_N=R_e(1-2e+3esin²L)

R_EFor the radius of curvature of vertical meridian plane Inner, R_E=R_e(1+esin²L)

Wherein：R_eFor the major axis radius of reference ellipsoid；Ovalitys of the e for ellipsoid, L is local latitude；

$\begin{array}{cc}{F}_{11}=\frac{V_N}{R_E+h}tgL-\frac{V_U}{R_E+h}& F_{12}=2{\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R_E+h}\tan L\end{array}$

$\begin{array}{cc}{F}_{13}=-2{\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R_E+h}& F_{17}=(2{\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R_N+h}{\sec }^{2}L)V_N+2{\mathrm{\ω}}_{ie}\sin {\mathrm{LV}}_U\end{array}$

$\begin{array}{cc}{F}_{21}=-2({\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R_E+h}tgL)& F_{22}=-\frac{V_U}{R_N+h}\end{array}$

$\begin{array}{cc}{F}_{23}=-\frac{V_N}{R_N+h}& F_{27}=-(2{\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R_E+h}{\sec }^{2}L)V_E\end{array}$

$\begin{array}{cc}{F}_{31}=2({\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R_E+h})& F_{32}=\frac{2V_N}{R_N+h}\end{array}$

F₃₇=-2 ω_ie cosLV_E

$\begin{array}{cc}{F}_{45}={\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R_E+h}tgL& F_{46}=-({\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R_E+h})\end{array}$

$\begin{array}{cc}{F}_{51}=\frac{1}{R_E+h}& F_{54}=-({\mathrm{\ω}}_{ie}\sin L+\frac{V_E}{R_E+h}tgL)\end{array}$

F₅₇=-ω_ie sinL

$\begin{array}{cc}{F}_{61}=\frac{1}{R_E+h}tgL& F_{64}={\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R_E+h}\end{array}$

$\begin{array}{cc}{F}_{65}=\frac{V_N}{R_N+h}& F_{67}={\mathrm{\ω}}_{ie}\cos L+\frac{V_E}{R_E+h}{\sec }^{2}L\end{array}$

$\begin{array}{cc}{F}_{72}=\frac{1}{R_N+h}& F_{81}=\frac{1}{R_E+h}\sec L\end{array}$

$F_{87}=\frac{V_E}{R_E+h}\sec LtgL$

$C_{9\×6}=\left[\begin{array}{cccccc}{C}_{11}& C_{21}& C_{31}& 0& 0& 0\\ C_{12}& C_{22}& C_{32}& 0& 0& 0\\ C_{13}& C_{23}& C_{33}& 0& 0& 0\\ 0& 0& 0& -C_{11}& -C_{21}& -C_{31}\\ 0& 0& 0& -C_{12}& -C_{22}& -C_{32}\\ 0& 0& 0& -C_{13}& -C_{23}& -C_{33}\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right]$

C_ijFor attitude transfer matrixElement

F_LBL=0_3×3；

(3) SINS/LBL tight integration measurement equations are set up；

Tight integration system is using the hydrophone that SINS the is calculated difference conduct poor with the oblique distance that LBL measurements are obtained with the oblique distance difference of AUV Observed quantity；In tight integration system, if the oblique distance difference that LBL is measured is ρ_LBLi, the position of water-bed transponder arrays primitive is P_i(x_i, y_i,z_i), the AUV positions that SINS is measured are P_SINS(x_S,y_S,z_S), the AUV positions P measured by SINS_SINS(x_S,y_S,z_S) and it is water-bed The position of transponder arrays primitive is P_i(x_i,y_i,z_i) determined by oblique distance difference be ρ_SINSi；

SINS oblique distances are poor

ρ_SINSi=R_i-R₀+(G_ix-G_0x)δx+(G_iy-G_0y)δy+(G_iz-G_0z)δz

=R_i-R₀+e_ixδx+e_iyδy+e_izδz

LBL oblique distances are poor

Then measure and can be write as

Then have：

$\mathrm{\δ}\mathrm{\ρ}=\left[\begin{array}{c}{\mathrm{\δ\ρ}}_1\\ {\mathrm{\δ\ρ}}_2\\ {\mathrm{\δ\ρ}}_3\end{array}\right]=\left[\begin{array}{cccccc}{e}_{1x}& e_{1y}& e_{1z}& -1& 0& 0\\ e_{2x}& e_{2y}& e_{3z}& 0& -1& 0\\ e_{3x}& e_{3y}& e_{3z}& 0& 0& -1\end{array}\right]\left[\begin{array}{c}\mathrm{\δ}x\\ \mathrm{\δ}y\\ \mathrm{\δ}z\\ {\mathrm{\δR}}_1\\ {\mathrm{\δR}}_2\\ {\mathrm{\δR}}_3\end{array}\right]+\left[\begin{array}{c}{\mathrm{\ν}}_{{\mathrm{\δR}}_1}\\ {\mathrm{\ν}}_{{\mathrm{\δR}}_2}\\ {\mathrm{\ν}}_{{\mathrm{\δR}}_3}\end{array}\right]$

When system adopts earth rectangular coordinate system (Ox_ey_ez_e) as navigational coordinate system when, with above formula construct system measurements equation； It is that, with longitude and latitude and altitude location, therefore dx, dy, dz dl, d λ, dh are represented in practical application；

By

$\left\{\begin{array}{c}\mathrm{\δ}x=\mathrm{\δ}h\cos Lcos\mathrm{\λ}-(R_N+h)\sin Lcos\mathrm{\λ}\mathrm{\δ}L-(R_N+h)\cos Lsin\mathrm{\λ}\mathrm{\δ}\mathrm{\λ}\\ \mathrm{\δ}y=\mathrm{\δ}h\cos Lsin\mathrm{\λ}-(R_N+h)\sin L\sin \mathrm{\λ}\mathrm{\δ}L-(R_N+h)\cos L\cos \mathrm{\λ}\mathrm{\δ}\mathrm{\λ}\\ \mathrm{\δ}z=\mathrm{\δ}h\sin L+\[R_N(1-e^2)+h\]\cos L\mathrm{\δ}L\end{array}\right.$

Measurement equation is Z_3×1=H_3×18X_18×1+V_ΔR(3×1)

In formula,

IfWherein a_ijFor matrix H₁Element, i=1,2,3；J=1,2,3；

H₁Nonzero element is as follows:

a_i1=-(R_N+h)sin L cosλe_1x-(R_N+h)sin L sinλe_1y+[R_N(1-e²)+h]cos Le_1z

a_i2=-(R_N+h)cos L sinλe_2x-(R_N+h)cos L cosλe_2y

a_i3=cos L cos λ e_3x+cos L sinλe_3y+sin Le_3zI=1,2,3.