CN104457754B  SINS/LBL (strapdown inertial navigation systems/long base line) tight combination based AUV (autonomous underwater vehicle) underwater navigation positioning method  Google Patents
SINS/LBL (strapdown inertial navigation systems/long base line) tight combination based AUV (autonomous underwater vehicle) underwater navigation positioning method Download PDFInfo
 Publication number
 CN104457754B CN104457754B CN201410796735.XA CN201410796735A CN104457754B CN 104457754 B CN104457754 B CN 104457754B CN 201410796735 A CN201410796735 A CN 201410796735A CN 104457754 B CN104457754 B CN 104457754B
 Authority
 CN
 China
 Prior art keywords
 sins
 lbl
 auv
 delta
 difference
 Prior art date
Links
 239000011159 matrix materials Substances 0.000 claims description 33
 238000005259 measurements Methods 0.000 claims description 12
 230000004807 localization Effects 0.000 claims description 8
 230000000694 effects Effects 0.000 claims description 4
 239000004744 fabrics Substances 0.000 claims description 4
 280000867207 Lambda companies 0.000 claims 9
 235000020127 ayran Nutrition 0.000 claims 1
 239000000969 carriers Substances 0.000 description 15
 239000011901 water Substances 0.000 description 9
 238000005516 engineering processes Methods 0.000 description 6
 238000001914 filtration Methods 0.000 description 6
 239000000203 mixtures Substances 0.000 description 4
 238000004364 calculation methods Methods 0.000 description 3
 238000010586 diagrams Methods 0.000 description 3
 230000001133 acceleration Effects 0.000 description 2
 241000209094 Oryza Species 0.000 description 1
 235000007164 Oryza sativa Nutrition 0.000 description 1
 238000009795 derivation Methods 0.000 description 1
 238000005265 energy consumption Methods 0.000 description 1
 230000005484 gravity Effects 0.000 description 1
 235000009566 rice Nutrition 0.000 description 1
 239000002699 waste materials Substances 0.000 description 1
Classifications

 G—PHYSICS
 G01—MEASURING; TESTING
 G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
 G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00  G01C19/00
 G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00  G01C19/00 by using measurements of speed or acceleration
 G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00  G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
 G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00  G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
 G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00  G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with noninertial navigation instruments
Abstract
Description
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 wellsuited 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 highprecision 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, strapdown inertial navigation system) Because its have disguised strong, autonomy, antiinterference, data renewal frequency it is high, and the features such as there is degree of precision at short notice, Thus become the firstselected localization method of AUV Camera calibrations under water.At present, although the development of strapdown inertial technology day Become ripe, its navigation positioning error does not but change with this dynamic characteristic that time integral dissipates, in longrange, longterm 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 hidingmachine.
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 deepsea, 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) twobytwo_{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 ρ_{SINS}And oblique distances of LBL hydrophone i (i=1,2,3) and AUV between and hydrophone 0 with Difference ρ of the oblique distance between AUV_{LBL}Difference 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 twobytwo 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 ρ_{SINSi}Using 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 ρ_{SINSi}Taylor 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 waterbed 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_{0}+(G_{ix}G_{0x})δx+(G_{iy}G_{0y})δy+(G_{iz}G_{0z})δz
=R_{i}R_{0}+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, multipathway 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_{meas}For 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_{1} δR_{2} δR_{3}]^{T}For 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_{SINS}For the state vector of SINS, X_{LBL}For the state vector of LBL, F_{SINS}For the transfer matrix of SINS, F_{LBL} For the transfer matrix of LBL, W_{SINS}For the system noise vector of SINS, W_{LBL}For 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 longterm 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_{U}Be 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}、▽_{bz}It is biased error that strapdown adds three axial directions of table, ε_{bx}、ε_{by}、ε_{bz}It is Strapdown Gyro Using Three are axially drifted about.
X_{LBL}=[δ R_{1} δR_{2} δR_{3}]^{T}
In formula, δ R_{1}、δR_{2}、δR_{3}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_{ij}For F_{9×9}Element
R_{N}For the radius of curvature of reference ellipsoid meridian plane Inner, R_{N}=R_{e}(12e+3e sin^{2} L)
R_{E}For the radius of curvature of vertical meridian plane Inner, R_{E}=R_{e}(1+e sin^{2} L)
Wherein：R_{e}For the major axis radius of reference ellipsoid；Ovalitys of the e for ellipsoid.
F_{37}=2 ω_{ie} cos LV_{E}
F_{57}=ω_{ie} sin L
C_{ij}For 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 waterbed 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 waterbed 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_{0}+(G_{ix}G_{0x})δx+(G_{iy}G_{0y})δy+(G_{iz}G_{0z})δz
=R_{i}R_{0}+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_{e}y_{e}z_{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×18}X_{18×1}+V_{ΔR(3×1)}
In formula,
IfWherein a_{ij}(i=1,2,3；J=1,2,3 it is) matrix H_{1}Element
H_{1}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}(1e^{2})+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 longterm 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 twobytwo 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} ^{n}：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} ^{n}:Vector of the geographical coordinate with respect to terrestrial coordinate system rotational angular velocity in geographic coordinate system, be：
V_{E}、V_{N}The respectively east orientation and north orientation speed of carrier movement；
R_{N}For the radius of curvature in reference ellipsoid meridian plane, R_{N}=R_{e}(12e+3e sin^{2}L)；
R_{E}For the radius of curvature in the plane normal of vertical meridian plane, R_{E}=R_{e}(1+e sin^{2}L)；
Wherein R_{e}For 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 realtime update attitude matrix of quaternary number according to following formula
Realtime 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^{n}For the projection that carrier acceleration is fastened in navigation coordinate, f^{n}=[f_{E} f_{N} f_{U}]^{T}；V^{n}Represent that hull is being led Velocity in boat coordinate system, V^{n}=[V_{E} V_{N} V_{U}]^{T}；g^{n}For gravity acceleration, g^{n}=[0 0g]^{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_{N}For the radius of curvature of reference ellipsoid meridian plane Inner, R_{N}=R_{e}(12e+3e sin^{2} L)
R_{E}For the radius of curvature of vertical meridian plane Inner, R_{E}=R_{e}(1+e sin^{2} L)
Wherein：R_{e}For the major axis radius of reference ellipsoid；Ovalitys of the e for ellipsoid.
(2) primitive is calculated SINS with target oblique distance difference twobytwo
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 ρ_{SINSi}Using 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 ρ_{SINSi}Taylor 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 waterbed 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_{0}+(G_{ix}G_{0x})δx+(G_{iy}G_{0y})δy+(G_{iz}G_{0z})δz
=R_{i}R_{0}+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, multipathway 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_{meas}For 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_{1} δR_{2} δR_{3}]^{T}For 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_{SINS}For the state vector of SINS, X_{LBL}For the state vector of LBL, F_{SINS}For the transfer matrix of SINS, F_{LBL} For the transfer matrix of LBL, W_{SINS}For the system noise vector of SINS, W_{LBL}For 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 longterm 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_{U}Be 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}、▽_{bz}It is biased error that strapdown adds three axial directions of table, ε_{bx}、ε_{by}、ε_{bz}It is Strapdown Gyro Using Three are axially drifted about.
X_{LBL}=[δ R_{1} δR_{2} δR_{3}]^{T}
In formula, δ R_{1}、δR_{2}、δR_{3}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_{ij}For F_{9×9}Element,
R_{N}For the radius of curvature of reference ellipsoid meridian plane Inner, R_{N}=R_{e}(12e+3e sin^{2} L)
R_{E}For the radius of curvature of vertical meridian plane Inner, R_{E}=R_{e}(1+e sin^{2} L)
Wherein：R_{e}For the major axis radius of reference ellipsoid；Ovalitys of the e for ellipsoid.
F_{37}=2 ω_{ie} cos LV_{E}
F_{57}=ω_{ie} sin L
C_{ij}For 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 waterbed 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 waterbed 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_{e}y_{e}z_{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×18}X_{18×1}+V_{ΔR(3×1)}
In formula,
Wherein a_{ij}(i=1,2,3；J=1,2,3 it is) matrix H_{1}Element
H_{1}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}(1e^{2})+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,k1}X_{k1}+Γ_{k1}W_{k1}
Z_{k}=H_{k}X_{k}+V_{k}
In formula, X_{k}For the state vector at k moment, that is, it is estimated vector；Z_{k}For the measurement sequence at k moment；W_{k1}For k1 The system noise at moment；V_{k}For the measurement noise sequence at k moment；Φ_{k,k1}For the step state transfer square at k1 moment to k moment Battle array；Γ_{k1}It is system noise input matrix, H_{k}For the calculation matrix at k moment,
The optimal estimation of state is calculated using standard Kalman filtering equations：
State onestep prediction vector
X_{k/k1}=φ_{k,k1}X_{k1}
State Estimation is calculated
X_{k}=X_{k/k1}+K_{k}(Z_{k}H_{k}X_{k/k1})
Filtering gain
K_{k}=P_{k/k1}H_{k} ^{T}(H_{k}P_{k/k1}H_{k} ^{T}+R_{k})^{1}
Onestep 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)
Priority Applications (1)
Application Number  Priority Date  Filing Date  Title 

CN201410796735.XA CN104457754B (en)  20141219  20141219  SINS/LBL (strapdown inertial navigation systems/long base line) tight combination based AUV (autonomous underwater vehicle) underwater navigation positioning method 
Applications Claiming Priority (1)
Application Number  Priority Date  Filing Date  Title 

CN201410796735.XA CN104457754B (en)  20141219  20141219  SINS/LBL (strapdown inertial navigation systems/long base line) tight combination based AUV (autonomous underwater vehicle) underwater navigation positioning method 
Publications (2)
Publication Number  Publication Date 

CN104457754A CN104457754A (en)  20150325 
CN104457754B true CN104457754B (en)  20170426 
Family
ID=52904190
Family Applications (1)
Application Number  Title  Priority Date  Filing Date 

CN201410796735.XA CN104457754B (en)  20141219  20141219  SINS/LBL (strapdown inertial navigation systems/long base line) tight combination based AUV (autonomous underwater vehicle) underwater navigation positioning method 
Country Status (1)
Country  Link 

CN (1)  CN104457754B (en) 
Families Citing this family (9)
Publication number  Priority date  Publication date  Assignee  Title 

CN106483498A (en) *  20150901  20170308  北京自动化控制设备研究所  A kind of SINSUSBL close coupling algorithm 
CN105526931A (en) *  20151202  20160427  中国人民解放军91388部队  Combined navigation method for underwater vehicle based on individual seabed transponder 
CN105676855B (en) *  20160129  20180619  中国船舶重工集团公司第七一〇研究所  A kind of navigation pose calibrating system and method for longrange selfpropelled mine approximately level 
CN106643709B (en) *  20161010  20191231  东南大学  Combined navigation method and device for offshore carrier 
CN106767793A (en) *  20170119  20170531  东南大学  A kind of AUV underwater navigation localization methods based on SINS/USBL tight integrations 
CN107966145A (en) *  20171221  20180427  中国船舶重工集团公司第七0七研究所  A kind of AUV underwater navigation methods based on sparse Long baselines tight integration 
CN108344999B (en) *  20180109  20200811  浙江大学  Sonar map construction and repositioning method for underwater robot navigation 
CN108834050A (en) *  20180615  20181116  哈尔滨工程大学  A kind of Long baselines positioning system base station and communication means 
CN109186593A (en) *  20180913  20190111  深圳市吉影科技有限公司  A kind of motion profile record method for drafting of underwater unmanned plane 
Citations (2)
Publication number  Priority date  Publication date  Assignee  Title 

CN103744098A (en) *  20140123  20140423  东南大学  Ship's inertial navigation system (SINS)/Doppler velocity log (DVL)/global positioning system (GPS)based autonomous underwater vehicle (AUV) combined navigation system 
CN104061930A (en) *  20131205  20140924  东南大学  Navigation method based on strapdown inertial guidance and Doppler log 

2014
 20141219 CN CN201410796735.XA patent/CN104457754B/en active IP Right Grant
Patent Citations (2)
Publication number  Priority date  Publication date  Assignee  Title 

CN104061930A (en) *  20131205  20140924  东南大学  Navigation method based on strapdown inertial guidance and Doppler log 
CN103744098A (en) *  20140123  20140423  东南大学  Ship's inertial navigation system (SINS)/Doppler velocity log (DVL)/global positioning system (GPS)based autonomous underwater vehicle (AUV) combined navigation system 
NonPatent Citations (3)
Title 

"Autonomous Underwater Vehicle Navigation";Paul A. Miller;《IEEE JOURNAL OF OCEANIC ENGINEERING》;20100731;正文第663676页 * 
"GAS Tightly Coupled LBL/USBL Position and Velocity Filter for Underwater Vehicles";Pedro Batista;《2013 European Control Conference (ECC)》;20130719;正文第29822987页 * 
"长基线声学定位系统发展现状及其应用";宁津生;《海洋测绘》;20140630;第4卷(第1期);正文第221224页 * 
Also Published As
Publication number  Publication date 

CN104457754A (en)  20150325 
Similar Documents
Publication  Publication Date  Title 

Noureldin et al.  Fundamentals of inertial navigation, satellitebased positioning and their integration  
Wu et al.  Velocity/position integration formula part I: Application to inflight coarse alignment  
US8416130B2 (en)  Land survey system  
Lee et al.  Simulation of an inertial acoustic navigation system with range aiding for an autonomous underwater vehicle  
CN102648394B (en)  The spacial alignment of Inertial Measurement Unit (IMU) is determined  
CN103245360B (en)  Carrierborne aircraft rotation type strapdown inertial navigation system Alignment Method under swaying base  
Costanzi et al.  An attitude estimation algorithm for mobile robots under unknown magnetic disturbances  
CN103575299B (en)  Utilize dualaxis rotation inertial navigation system alignment and the error correcting method of External Observation information  
Kinsey et al.  A survey of underwater vehicle navigation: Recent advances and new challenges  
CN102829785B (en)  Air vehicle fullparameter navigation method based on sequence image and reference image matching  
CN102506857B (en)  Relative attitude measurement realtime dynamic filter method based on dualinertial measurement unit/differential global positioning system (IMU/DGPS) combination  
Chang et al.  Initial alignment for a Doppler velocity logaided strapdown inertial navigation system with limited information  
CN101354253B (en)  Geomagnetic auxiliary navigation algorithm based on matching degree  
KR20130093526A (en)  Determining spatial orientation information of a body from multiple electromagnetic signals  
Larsen  High performance Dopplerinertial navigationexperimental results  
CN102829777B (en)  Autonomous underwater vehicle combined navigation system and method  
CN103674021B (en)  Integrated navigation system based on inertial navigation and star sensor and method  
US9618344B2 (en)  Digital map tracking apparatus and methods  
CN104075715B (en)  A kind of underwater navigation localization method of Combining with terrain and environmental characteristic  
Jalving et al.  DVL velocity aiding in the HUGIN 1000 integrated inertial navigation system  
Hong et al.  A car test for the estimation of GPS/INS alignment errors  
Gade  The seven ways to find heading  
CN104655131B (en)  Inertial navigation Initial Alignment Method based on ISTSSRCKF  
CN103235328B (en)  GNSS (global navigation satellite system) and MEMS (microelectromechanical systems) integrated navigation method  
CN101788296B (en)  SINS/CNS deep integrated navigation system and realization method thereof 
Legal Events
Date  Code  Title  Description 

C06  Publication  
PB01  Publication  
SE01  Entry into force of request for substantive examination  
GR01  Patent grant  
GR01  Patent grant 