CN112504298B - GNSS-assisted DVL error calibration method - Google Patents
GNSS-assisted DVL error calibration method Download PDFInfo
- Publication number
- CN112504298B CN112504298B CN202011338896.6A CN202011338896A CN112504298B CN 112504298 B CN112504298 B CN 112504298B CN 202011338896 A CN202011338896 A CN 202011338896A CN 112504298 B CN112504298 B CN 112504298B
- Authority
- CN
- China
- Prior art keywords
- dvl
- moment
- coordinate system
- carrier
- deflection angle
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 32
- 238000009434 installation Methods 0.000 claims abstract description 80
- 239000011159 matrix material Substances 0.000 claims abstract description 70
- 238000006243 chemical reaction Methods 0.000 claims abstract description 46
- 238000001914 filtration Methods 0.000 claims abstract description 30
- 238000005259 measurement Methods 0.000 claims abstract description 23
- 230000009466 transformation Effects 0.000 claims description 9
- 101000802640 Homo sapiens Lactosylceramide 4-alpha-galactosyltransferase Proteins 0.000 claims description 3
- 102100035838 Lactosylceramide 4-alpha-galactosyltransferase Human genes 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 238000004088 simulation Methods 0.000 description 9
- 238000010586 diagram Methods 0.000 description 3
- 241000764238 Isis Species 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
Landscapes
- Engineering & Computer Science (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Navigation (AREA)
Abstract
The invention discloses a GNSS assisted DVL error calibration method, which specifically comprises the following steps: collecting experimental data including SINS/GNSS integrated navigation system outputAnd carrier attitude angle, and DVL outputAnd utilize the carrier posture to makeConverting into a carrier coordinate system b to obtainSelecting installation offset angle errorAnd the scale factor error zeta is a state quantity, and a Kalman filtering state equation is established and discretized; installation deflection angle conversion matrix obtained by k-1 moment feedback correctionWill be at time kConverting into a carrier coordinate system b to obtainWill be at time kAnd (3) withTaking the difference value of the K moment as the observed quantity of Kalman filtering, and establishing a Kalman filtering measurement equation at the k moment; performing Kalman filtering; correcting the installation deflection angle conversion matrix by using the installation deflection angle error estimation feedback and collocating zero installation deflection angle error estimation; the last 4 steps are repeated until the errors of the installation deflection angle conversion matrix and the scale factors are converged to respective accurate amounts. The invention can accurately, quickly and conveniently mark the installation deflection angle with any size of DVL and scale factor error.
Description
Technical Field
The invention relates to a GNSS assisted DVL error calibration method, belongs to the technical field of navigation, and is particularly suitable for the field of SINS/DVL integrated navigation.
Background
The Doppler Velocimeter (DVL) is an instrument for realizing high-precision measurement of carrier speed based on Doppler effect, and is often combined with a Strapdown Inertial Navigation System (SINS) to form an SINS/DVL integrated navigation system. The installation offset angle between the DVL and the SINS and the DVL speed measurement scale factor error are main factors influencing the positioning accuracy of the SINS/DVL integrated navigation system, and the DVL error is required to be calibrated before integrated navigation.
Under the effective condition of a Global Navigation Satellite System (GNSS), GNSS and inertial navigation are commonly adopted for combined navigation, and DVL error calibration is carried out by utilizing the gesture, speed and position after combined navigation. At present, according to the calibration method based on Kalman filtering, the model is provided with a small installation deflection angle, and the measurement equation is approximated in a first order, so that the calibration result is not very accurate.
Disclosure of Invention
In order to accurately, quickly and conveniently mark any installation deflection angle and scale factor error of DVL, the invention provides a GNSS-assisted DVL error calibration method.
The above object of the present invention is achieved by the following technical solutions:
a GNSS assisted DVL error calibration method specifically comprises the following steps:
step S1: collecting experimental data, including projection of carrier speed vector output by SINS/GNSS integrated navigation system under navigation coordinate system nAnd carrier attitude angle, and projection of the carrier velocity vector of the DVL output in the DVL device coordinate system d +.>And the projection of the vector speed vector output by the SINS/GNSS integrated navigation system under the navigation coordinate system n is carried out by utilizing the vector attitude>Converting to carrier coordinate system b to obtain projection of carrier speed vector outputted by SINS/GNSS integrated navigation system in carrier coordinate system b>
Step S2: selecting installation offset angle errorAnd the scale factor error zeta is a state quantity, and a Kalman filtering state equation of the system is established and discretized;
step S3: installation deflection angle conversion matrix obtained by k-1 moment feedback correctionProjection of the vector velocity vector output by DVL at time k onto the DVL system d>Converting to carrier coordinate system b to obtain projection of carrier velocity vector outputted by DVL in carrier coordinate system b>
Step S4: will be at time kAnd->Taking the difference value of the K moment as the observed quantity of Kalman filtering, and establishing a Kalman filtering measurement equation at the k moment;
step S5: performing Kalman filtering, and outputting installation deflection angle error estimation and scale factor error estimation;
step S6: correcting the installation deflection angle conversion matrix by using the installation deflection angle error estimation feedback and collocating zero installation deflection angle error estimation;
step S7: and repeating the steps S3-S6 until the errors of the installation deflection angle conversion matrix and the scale factors are converged to respective accurate amounts.
Further, the step S1 specifically includes the following steps:
s1.1, collecting experimental data, including projection of a carrier speed vector output by an SINS/GNSS integrated navigation system under a navigation coordinate system nAnd carrier attitude angle, and projection of the carrier velocity vector of the DVL output in the DVL device coordinate system d +.>
S1.2 utilizing carrier gesture to project carrier speed vector outputted by SINS/GNSS combined navigation system under navigation coordinate system nConverting to carrier coordinate system b to obtain projection of carrier speed vector outputted by SINS/GNSS integrated navigation system in carrier coordinate system b>
Wherein,the coordinate transformation matrix from the navigation coordinate system n at the moment k to the carrier coordinate system b is obtained by transforming the carrier attitude angle output by the SINS/GNSS integrated navigation system at the moment k; />The projection of the carrier speed vector output by the SINS/GNSS integrated navigation system at the moment k under the navigation coordinate system n; />And (3) the projection of the carrier speed vector output by the SINS/GNSS integrated navigation system at the moment k under the carrier coordinate system b.
Further, the step S2 specifically includes the following steps:
s2.1 selecting constant installation offset angle errorAnd taking the scale factor error zeta as a state quantity, and establishing a Kalman filtering state equation of the system:
wherein X is a state vector, for mounting the offset angle error vector, < >>ζ is a scale factor error scalar; f is a system matrix, f=0 4×4 The method comprises the steps of carrying out a first treatment on the surface of the W is the system noise vector.
S2.2 discretizing the state equation:
X(k)＝Φ(k,k-1)X(k-1)+G(k,k-1)W(k-1)
wherein X (k) is a state vector at time k; phi (k, k-1) is the state one-step transition moment from moment k-1 to moment kMatrix, Φ (k, k-1) =i 4×4 The method comprises the steps of carrying out a first treatment on the surface of the W (k-1) is the system noise vector at time k-1 and G (k, k-1) is the system noise input matrix from time k-1 to time k.
Further, the step S3 specifically includes the following steps:
installation deflection angle conversion matrix obtained by k-1 moment feedback correctionProjection of the vector velocity vector output by DVL at time k onto the DVL system d>Converting to carrier coordinate system b to obtain projection +.f of carrier velocity vector outputted by DVL on carrier velocity vector outputted by DVL under DVL equipment coordinate system d>
Wherein,the projection of the vector velocity vector output by the DVL at the k moment under the DVL equipment coordinate system d;the installation deflection angle conversion matrix obtained for the feedback correction of the k-1 moment represents the carrier coordinate system b and the estimated DVL equipment coordinate system of the k-1 moment +.>Is a conversion relation of (a); />The projection of the vector velocity vector of the carrier output for the moment k DVL in the carrier coordinate system b.
Further, the step S4 specifically includes the following steps:
s4.1 to time kAnd->As a Kalman filtered observance of the difference of (2):
wherein Z (k) is a measurement vector at the moment k;the conversion relation between a real DVL equipment coordinate system d and a carrier coordinate system b is represented by a real installation deflection angle conversion matrix; /> Installation of the offset error estimate for time k-1 +.>Is an antisymmetric matrix of>Installation of the offset error estimate for time k-1 +.>Corresponding transformation matrix, representing k-1 moment estimated DVL device coordinate system +.>The conversion relation with a real DVL equipment coordinate system d; />For the measurement of the DVL speed at time k +.>Is an antisymmetric matrix of (a); />Estimating a scale factor error at the moment k; />υ d (k) Noise is measured for DVL at time k.
S4.2, establishing a Kalman filtering discretization measurement equation at k moment:
Z(k)＝H(k)X(k)+V(k)
H(k)＝[H 1 (k) H 2 (k)]
wherein H (k) is a measurement matrix at k moment; c (C) ij (i, j=1, 2, 3) isIs an element of (2); v (k) is the noise vector measured at time k.
Further, the step S5 specifically includes the following steps:
performing Kalman filtering, and outputting an installation deflection angle error estimation and a scale factor error estimation:
P(k,k-1)＝Φ(k,k-1)P(k-1)Φ(k,k-1) T +G(k,k-1)Q(k-1)G(k,k-1) T
K(k)＝P(k,k-1)H(k) T (H(k)P(k,k-1)H(k) T +R(k)) -1
P(k)＝(I-K(k)H(k))P(k,k-1)
wherein,p (k, k-1) is a state one-step prediction and its corresponding mean square error matrix, respectively; />And P (k) is the state estimation at k time and the corresponding mean square error matrix; k (K) is a K moment filtering gain array; q (k-1) is a system noise variance matrix at the moment k-1; r (k) is the measurement noise variance matrix at k moment.
Further, the step S6 specifically includes the following steps:
s6.1, correcting the installation deflection angle conversion matrix by using installation deflection angle error estimation feedback:
wherein,installation deflection angle error estimation for k time>Corresponding transformation matrix, representing k-1 moment estimated DVL device coordinate system +.>Estimating DVL device coordinate System with time k +.>Is a conversion relation of (a); />For the installation deflection angle conversion matrix obtained after the feedback correction of the k moment, the installation deflection angle conversion matrix represents a carrier coordinate system b and a k moment estimated DVL equipment coordinate system +.>Is a conversion relation of (a).
S6.2, zeroing installation offset angle error estimation:
compared with the prior art, the method has the following advantages and beneficial effects:
(1) The invention can accurately and rapidly mark the installation deflection angle and scale factor error of any size of DVL.
(2) The method is simple, easy to realize and has good engineering application value.
Drawings
FIG. 1 is a schematic diagram of a GNSS assisted DVL error calibration method according to the present invention.
Fig. 2 is a diagram of a motion trace of a carrier in the embodiment.
FIG. 3 is a graph showing the calibration simulation results of the DVL installation bias angle in example 1.
FIG. 4 is a graph showing the result of the DVL scale factor error calibration simulation in example 1.
FIG. 5 is a graph showing the calibration simulation results of the DVL installation bias angle in example 2.
FIG. 6 is a graph of the DVL scale factor error calibration simulation results of example 2.
Detailed Description
The technical scheme provided by the present invention will be described in detail below with reference to the accompanying drawings and specific examples, and it should be understood that the following specific embodiments are only for illustrating the present invention and are not intended to limit the scope of the present invention.
The invention provides a GNSS assisted DVL error calibration method, and the implementation principle is shown in figure 1. In one embodiment, it specifically comprises the steps of:
step S1: collecting experimental data, including projection of carrier speed vector output by SINS/GNSS integrated navigation system under navigation coordinate system nAnd carrier attitude angle, and projection of the carrier velocity vector of the DVL output in the DVL device coordinate system d +.>And the projection of the vector speed vector output by the SINS/GNSS integrated navigation system under the navigation coordinate system n is carried out by utilizing the vector attitude>Converting to carrier coordinate system b to obtain projection of carrier speed vector outputted by SINS/GNSS integrated navigation system in carrier coordinate system b>The method specifically comprises the following steps:
s1.1, collecting experimental data, including projection of a carrier speed vector output by an SINS/GNSS integrated navigation system under a navigation coordinate system nAnd carrier attitude angle, and projection of the carrier velocity vector of the DVL output in the DVL device coordinate system d +.>
S1.2 utilizing carrier gesture to project carrier speed vector outputted by SINS/GNSS combined navigation system under navigation coordinate system nConversion ofUnder the carrier coordinate system b, the projection of the carrier speed vector outputted by the SINS/GNSS integrated navigation system under the carrier coordinate system b is obtained>
Wherein,the coordinate transformation matrix from the navigation coordinate system n at the moment k to the carrier coordinate system b is obtained by transforming the carrier attitude angle output by the SINS/GNSS integrated navigation system at the moment k; />The projection of the carrier speed vector output by the SINS/GNSS integrated navigation system at the moment k under the navigation coordinate system n; />And (3) the projection of the carrier speed vector output by the SINS/GNSS integrated navigation system at the moment k under the carrier coordinate system b.
Step S2: selecting installation offset angle errorAnd taking the scale factor error zeta as a state quantity, establishing a Kalman filtering state equation of the system and discretizing. The method specifically comprises the following steps:
s2.1 selecting constant installation offset angle errorAnd taking the scale factor error zeta as a state quantity, and establishing a Kalman filtering state equation of the system:
wherein X is a state vector, for mounting the offset angle error vector, < >>ζ is a scale factor error scalar; f is a system matrix, f=0 4×4 The method comprises the steps of carrying out a first treatment on the surface of the W is the system noise vector.
S2.2 discretizing the state equation:
X(k)＝Φ(k,k-1)X(k-1)+G(k,k-1)W(k-1)
wherein X (k) is a state vector at time k; phi (k, k-1) is a state one-step transition matrix from moment k-1 to moment k, phi (k, k-1) =i 4×4 The method comprises the steps of carrying out a first treatment on the surface of the W (k-1) is the system noise vector at time k-1 and G (k, k-1) is the system noise input matrix from time k-1 to time k.
Step S3: installation deflection angle conversion matrix obtained by k-1 moment feedback correctionProjection of the vector velocity vector output by DVL at time k onto the DVL system d>Converting to carrier coordinate system b to obtain projection of carrier velocity vector outputted by DVL in carrier coordinate system b>The method specifically comprises the following steps:
installation deflection angle conversion matrix obtained by k-1 moment feedback correctionProjection of the vector velocity vector output by DVL at time k onto the DVL system d>Converting to carrier coordinate system b to obtain projection of carrier velocity vector outputted by DVL in carrier coordinate system b>
Wherein,the projection of the vector velocity vector output by the DVL at the k moment under the DVL equipment coordinate system d;the installation deflection angle conversion matrix obtained for the feedback correction of the k-1 moment represents the carrier coordinate system b and the estimated DVL equipment coordinate system of the k-1 moment +.>Is a conversion relation of (a); />The projection of the vector velocity vector of the carrier output for the moment k DVL in the carrier coordinate system b.
Step S4: will be at time kAnd->And (3) taking the difference value of the k moment as the observed quantity of Kalman filtering, and establishing a Kalman filtering measurement equation at the k moment. The method specifically comprises the following steps:
s4.1 to time kAnd->As a Kalman filtered observance of the difference of (2):
wherein Z (k) is a measurement vector at the moment k;the conversion relation between a real DVL equipment coordinate system d and a carrier coordinate system b is represented by a real installation deflection angle conversion matrix; /> Mounting the offset error estimate for time k-1 +.>An antisymmetric matrix>Installation of the offset error estimate for time k-1 +.>Corresponding transformation matrix, representing k-1 moment estimated DVL device coordinate system +.>The conversion relation with a real DVL equipment coordinate system d; />For the measurement of the DVL speed at time k +.>Is an antisymmetric matrix of (a); />Estimating a scale factor error at the moment k; />υ d (k) Noise is measured for DVL at time k.
S4.2, establishing a Kalman filtering discretization measurement equation at k moment:
Z(k)＝H(k)X(k)+V(k)
H(k)＝[H 1 (k) H 2 (k)]
wherein H (k) is a measurement matrix at k moment, C ij (i, j=1, 2, 3) isIs an element of (2); v (k) is the noise vector measured at time k.
Step S5: and performing Kalman filtering to output an installation deflection angle error estimation and a scale factor error estimation. The method specifically comprises the following steps:
performing Kalman filtering, and outputting an installation deflection angle error estimation and a scale factor error estimation:
P(k,k-1)＝Φ(k,k-1)P(k-1)Φ(k,k-1) T +G(k,k-1)Q(k-1)G(k,k-1) T
K(k)＝P(k,k-1)H(k) T (H(k)P(k,k-1)H(k) T +R(k)) -1
P(k)＝(I-K(k)H(k))P(k,k-1)
wherein,p (k, k-1) is a state one-step prediction and its corresponding mean square error matrix, respectively; />And P (k) is the state estimation at k time and the corresponding mean square error matrix; k (K) is a K moment filtering gain array; q (k-1) is a system noise variance matrix at the moment k-1; r (k) is the measurement noise variance matrix at k moment.
Step S6: and correcting the installation deflection angle conversion matrix by using the installation deflection angle error estimation feedback and collocating zero installation deflection angle error estimation. The method specifically comprises the following steps:
s6.1, correcting the installation deflection angle conversion matrix by using installation deflection angle error estimation feedback:
wherein,installation deflection angle error estimation for k time>Corresponding transformation matrix, representing k-1 moment estimated DVL device coordinate system +.>Estimating DVL device coordinate System with time k +.>Is rotated by (a)Changing the relation; />For the installation deflection angle conversion matrix obtained after the feedback correction of the k moment, the installation deflection angle conversion matrix represents a carrier coordinate system b and a k moment estimated DVL equipment coordinate system +.>Is a conversion relation of (a).
S6.2, zeroing installation offset angle error estimation:
step S7: and repeating the steps S3-S6 until the error of the installation deflection angle conversion matrix and the scale factor is converged to a very accurate value.
The effect of the present invention is verified by two examples below.
Example 1:
and (3) collecting experimental data: selecting carrier speed output by SINS/GNSS integrated navigation system of certain Yangtze river experimentMeasured data such as attitude angle and position (not required); simulation generation of DVL speed measurement data>The specific formula is->
Simulation parameter setting: DVL installation deflection angle is [1 °;3 °;0.5 degree]The method comprises the steps of carrying out a first treatment on the surface of the The DVL scale factor error is-0.02; DVL measurement noise is Gaussian white noise with the mean value of 0 and standard deviation of 0.005 m/s; the DVL output frequency is 2Hz; the initial given installation offset angle is [0 °;0 °;0 degree (degree)]The method comprises the steps of carrying out a first treatment on the surface of the Kalman filtering initial estimation error variance matrix is diag ([ 5 DEG) 2 ；5° 2 ；5° 2 ；0.1 2 ]) The measured noise variance matrix is diag ([ 0.005 ]) 2 ；0.005 2 ；0.005 2 ]) A system noise variance matrix of 0 4×4 。
Example 2:
example 2 only the DVL installation offset angle, scale factor error and initial given installation offset angle settings differ from example 1, specifically the DVL installation offset angle is [1 °; -2 °;88 ° ]; the DVL scale factor error is 0.01; the initial given installation offset angle is [0 °;0 °;90 ° ].
Fig. 2 is a diagram of the trajectory of a selected experimental vessel motion in a Yangtze river. Fig. 3 and fig. 4 are graphs showing the simulation results of the small-angle installation bias angle and scale factor error calibration of the DVL in example 1, respectively. Fig. 5 and 6 are graphs of simulation results of the large-angle installation bias angle and scale factor error calibration of the DVL in example 2, respectively. Simulation results show that the invention can accurately and rapidly mark the installation deflection angle with any DVL size and scale factor error.
Claims (4)
1. The GNSS assisted DVL error calibration method is characterized by comprising the following steps of:
s1: collecting experimental data, including projection of carrier speed vector output by SINS/GNSS integrated navigation system under navigation coordinate system nAnd carrier attitude angle, and projection of the carrier velocity vector of the DVL output in the DVL device coordinate system d +.>And use carrier posture to apply->Converting into carrier coordinate system b to obtain +.>The step S1 specifically comprises the following steps:
s1.1, collecting experimental data, wherein the experimental data comprise a carrier speed vector output by an SINS/GNSS integrated navigation system at navigation coordinatesProjection under nAnd carrier attitude angle, and projection of the carrier velocity vector of the DVL output in the DVL device coordinate system d +.>
S1.2 utilizing carrier gesture to project carrier speed vector outputted by SINS/GNSS combined navigation system under navigation coordinate system nConverting to carrier coordinate system b to obtain projection of carrier speed vector outputted by SINS/GNSS integrated navigation system in carrier coordinate system b>
Wherein,the coordinate transformation matrix from the navigation coordinate system n at the moment k to the carrier coordinate system b is obtained by transforming the carrier attitude angle output by the SINS/GNSS integrated navigation system at the moment k; />The projection of the carrier speed vector output by the SINS/GNSS integrated navigation system at the moment k under the navigation coordinate system n; />The projection of the carrier speed vector output by the SINS/GNSS integrated navigation system at the moment k under the carrier coordinate system b;
s2: selecting installation offset angle errorAnd the scale factor error zeta is a state quantity, and a Kalman filtering state equation is established and discretized;
the step S2 specifically includes the following steps:
s2.1 selecting constant installation offset angle errorAnd taking the scale factor error zeta as a state quantity, and establishing a Kalman filtering state equation of the system:
wherein X is a state vector, for mounting the offset angle error vector, < >>ζ is a scale factor error scalar; f is a system matrix, f=0 4×4 The method comprises the steps of carrying out a first treatment on the surface of the W is a system noise vector;
s2.2 discretizing the state equation:
X(k)＝Φ(k,k-1)X(k-1)+G(k,k-1)W(k-1)
wherein X (k) is a state vector at time k; phi (k, k-1) is a state one-step transition matrix from moment k-1 to moment k, phi (k, k-1) =i 4×4 The method comprises the steps of carrying out a first treatment on the surface of the W (k-1) is a system noise vector at time k-1, and G (k, k-1) is a system noise input matrix from time k-1 to time k;
s3: installation deflection angle conversion matrix obtained by k-1 moment feedback correctionLet k time->Conversion to carrier coordinate system bObtaining the projection of the vector velocity vector of the DVL output under the vector coordinate system b>The step S3 specifically comprises the following steps:
installation deflection angle conversion matrix obtained by k-1 moment feedback correctionProjection of the vector velocity vector output by DVL at time k onto the DVL system d>Converting to carrier coordinate system b to obtain projection of carrier velocity vector outputted by DVL in carrier coordinate system b>
Wherein,the projection of the vector velocity vector output by the DVL at the k moment under the DVL equipment coordinate system d; />The installation deflection angle conversion matrix obtained for the feedback correction of the k-1 moment represents the carrier coordinate system b and the estimated DVL equipment coordinate system of the k-1 moment +.>Is a conversion relation of (a); />The projection of the carrier velocity vector output for the k moment DVL under the carrier coordinate system b;
s4: will be at time kAnd->Taking the difference of the K moment as the observed quantity of Kalman filtering, and establishing a Kalman filtering measurement equation at the k moment;
s5: performing Kalman filtering;
s6: correcting the installation deflection angle conversion matrix by using the installation deflection angle error estimation feedback and collocating zero installation deflection angle error estimation;
s7: and repeating the steps S3-S6 until the errors of the installation deflection angle conversion matrix and the scale factors are converged to an accurate value.
2. The GNSS assisted DVL error calibration method according to claim 1, wherein step S4 specifically includes the following procedures:
s4.1 to time kAnd->As a Kalman filtered observance of the difference of (2):
wherein Z (k) is a measurement vector at the moment k;the conversion relation between a real DVL equipment coordinate system d and a carrier coordinate system b is represented by a real installation deflection angle conversion matrix; /> Installation offset angle error estimation for k-1 timeIs an antisymmetric matrix of>Installation of the offset error estimate for time k-1 +.>Corresponding transformation matrix, representing k-1 moment estimated DVL device coordinate system +.>The conversion relation with a real DVL equipment coordinate system d; />For the measurement of the DVL speed at time k +.>Is an antisymmetric matrix of (a); />Estimating a scale factor error at the moment k; />υ d (k) Measuring noise for DVL at k time;
s4.2, establishing a Kalman filtering discretization measurement equation at k moment:
Z(k)＝H(k)X(k)+V(k)
H(k)＝[H 1 (k) H 2 (k)]
wherein H (k) is the moment measured at time kAn array; c (C) ij Is thatWherein i, j=1, 2,3; v (k) is the noise vector measured at time k.
3. The GNSS assisted DVL error calibration method according to claim 1, wherein step S5 specifically includes the following procedures:
performing Kalman filtering, and outputting an installation deflection angle error estimation and a scale factor error estimation:
P(k,k-1)＝Φ(k,k-1)P(k-1)Φ(k,k-1) T +G(k,k-1)Q(k-1)G(k,k-1) T
K(k)＝P(k,k-1)H(k) T (H(k)P(k,k-1)H(k) T +R(k)) -1
P(k)＝(I-K(k)H(k))P(k,k-1)
wherein,p (k, k-1) is a state one-step prediction and its corresponding mean square error matrix, respectively; />And P (k) is the state estimation at k time and the corresponding mean square error matrix; k (K) is a K moment filtering gain array; q (k-1) is a system noise variance matrix at the moment k-1; r (k) is the measurement noise variance matrix at k moment.
4. The GNSS assisted DVL error calibration method according to claim 1, wherein step S6 specifically includes the following procedures:
s6.1, correcting the installation deflection angle conversion matrix by using installation deflection angle error estimation feedback:
wherein,installation deflection angle error estimation for k time>Corresponding transformation matrix, representing k-1 moment estimated DVL device coordinate system +.>Estimating DVL device coordinate System with time k +.>Is a conversion relation of (a); />For the installation deflection angle conversion matrix obtained after the feedback correction of the k moment, the installation deflection angle conversion matrix represents a carrier coordinate system b and a k moment estimated DVL equipment coordinate system +.>Is a conversion relation of (a);
s6.2, zeroing installation offset angle error estimation:
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011338896.6A CN112504298B (en) | 2020-11-25 | 2020-11-25 | GNSS-assisted DVL error calibration method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011338896.6A CN112504298B (en) | 2020-11-25 | 2020-11-25 | GNSS-assisted DVL error calibration method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112504298A CN112504298A (en) | 2021-03-16 |
| CN112504298B true CN112504298B (en) | 2024-03-15 |
Family
ID=74958594
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011338896.6A Active CN112504298B (en) | 2020-11-25 | 2020-11-25 | GNSS-assisted DVL error calibration method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112504298B (en) |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113049005A (en) * | 2021-03-18 | 2021-06-29 | 苏州大学 | GNSS position method assisted DVL error calibration method and system |
| CN113503891B (en) * | 2021-04-22 | 2022-11-29 | 中国人民解放军海军工程大学 | SINSDVL alignment correction method, system, medium and equipment |
| CN113703018B (en) * | 2021-07-12 | 2022-08-05 | 山东大学 | DVL internal and external error calibration method |
| CN114252089B (en) * | 2021-09-30 | 2024-05-31 | 河北汉光重工有限责任公司 | Combined calibration method for DVL speed measurement errors |
| CN115435817B (en) * | 2022-11-07 | 2023-03-14 | 开拓导航控制技术股份有限公司 | MEMS inertial navigation installation error calibration method, storage medium and control computer |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103163508A (en) * | 2013-02-01 | 2013-06-19 | 中国人民解放军国防科学技术大学 | Doppler velocity log (DVL) parameter calibration method used for integrated navigation system of underwater inertial navigation system (INS) and DVL |
| CN105091907A (en) * | 2015-07-28 | 2015-11-25 | 东南大学 | Estimation method of installation error of DVL direction in SINS and DVL combination |
| CN106052686A (en) * | 2016-07-10 | 2016-10-26 | 北京工业大学 | Full-autonomous strapdown inertial navigation system based on DSPTMS 320F28335 |
| CN110031882A (en) * | 2018-08-02 | 2019-07-19 | 哈尔滨工程大学 | A kind of outer measurement information compensation method based on SINS/DVL integrated navigation system |
| CN110514203A (en) * | 2019-08-30 | 2019-11-29 | 东南大学 | A kind of underwater Combinated navigation method based on ISR-UKF |
| CN110542438A (en) * | 2019-09-30 | 2019-12-06 | 中国船舶重工集团公司第七0七研究所 | SINS/DVL-based integrated navigation error calibration method |
| CN110567454A (en) * | 2019-09-08 | 2019-12-13 | 东南大学 | SINS/DVL tightly-combined navigation method in complex environment |
| CN111323012A (en) * | 2020-04-24 | 2020-06-23 | 东南大学 | INS-assisted DVL speed measurement error compensation method under high dynamic environment of carrier |
-
2020
- 2020-11-25 CN CN202011338896.6A patent/CN112504298B/en active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103163508A (en) * | 2013-02-01 | 2013-06-19 | 中国人民解放军国防科学技术大学 | Doppler velocity log (DVL) parameter calibration method used for integrated navigation system of underwater inertial navigation system (INS) and DVL |
| CN105091907A (en) * | 2015-07-28 | 2015-11-25 | 东南大学 | Estimation method of installation error of DVL direction in SINS and DVL combination |
| CN106052686A (en) * | 2016-07-10 | 2016-10-26 | 北京工业大学 | Full-autonomous strapdown inertial navigation system based on DSPTMS 320F28335 |
| CN110031882A (en) * | 2018-08-02 | 2019-07-19 | 哈尔滨工程大学 | A kind of outer measurement information compensation method based on SINS/DVL integrated navigation system |
| CN110514203A (en) * | 2019-08-30 | 2019-11-29 | 东南大学 | A kind of underwater Combinated navigation method based on ISR-UKF |
| CN110567454A (en) * | 2019-09-08 | 2019-12-13 | 东南大学 | SINS/DVL tightly-combined navigation method in complex environment |
| CN110542438A (en) * | 2019-09-30 | 2019-12-06 | 中国船舶重工集团公司第七0七研究所 | SINS/DVL-based integrated navigation error calibration method |
| CN111323012A (en) * | 2020-04-24 | 2020-06-23 | 东南大学 | INS-assisted DVL speed measurement error compensation method under high dynamic environment of carrier |
Non-Patent Citations (5)
| Title |
|---|
| A novel robust Kalman filter for SINS/GPS integration;Min Zhong;IEEE;全文 * |
| A Novel SINS/DVL Tightly Integrated Navigation Method for Complex Environment;Di Wang;IEEE;第69卷(第7期);5183-5196 * |
| DVL Aided SINS Coarse Alignment Solution With High Dynamics;YIQING YAO;IEEE;第8卷;169922-169929 * |
| 基于自适应滤波的水下SINS/相控阵DVL组合导航算法设计;唐康华;江明明;翁炬;;中国惯性技术学报(01);全文 * |
| 水下SINS/DVL紧组合导航算法;陈建华;朱海;王超;于化鹏;;海军工程大学学报(02);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112504298A (en) | 2021-03-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112504298B (en) | GNSS-assisted DVL error calibration method | |
| JP4876204B2 (en) | Small attitude sensor | |
| CN111323050B (en) | Strapdown inertial navigation and Doppler combined system calibration method | |
| CN105737823B (en) | A kind of GPS/SINS/CNS Combinated navigation methods based on five rank CKF | |
| CN105136145A (en) | Kalman filtering based quadrotor unmanned aerial vehicle attitude data fusion method | |
| CN107063245B (en) | SINS/DVL combined navigation filtering method based on 5-order SSRCKF | |
| CN105806363B (en) | The underwater large misalignment angle alignment methods of SINS/DVL based on SRQKF | |
| JP2012173190A (en) | Positioning system and positioning method | |
| CN112798021B (en) | Inertial navigation system inter-travelling initial alignment method based on laser Doppler velocimeter | |
| CN112146655A (en) | Elastic model design method for BeiDou/SINS tight integrated navigation system | |
| KR101741076B1 (en) | Apparatus for compensating time delay of inertial navigation system and method thereof | |
| CN110702110A (en) | Ship heave motion measurement method based on unscented Kalman filtering | |
| CN113203429A (en) | Online estimation and compensation method for temperature drift error of gyroscope | |
| CN114777812B (en) | Inter-advancing alignment and attitude estimation method for underwater integrated navigation system | |
| CN111220151B (en) | Inertia and milemeter combined navigation method considering temperature model under load system | |
| CN110207698B (en) | Polar region grid inertial navigation/ultra-short baseline tight combination navigation method | |
| CN115979253A (en) | Underwater robot multi-sensor tight combination navigation method based on robust filtering | |
| CN114459466A (en) | MEMS multi-sensor data fusion processing method based on fuzzy control | |
| Quadri et al. | Error and Noise Analysis in an IMU using Kalman Filter | |
| CN114323007A (en) | Carrier motion state estimation method and device | |
| CN111982126B (en) | Design method of full-source BeiDou/SINS elastic state observer model | |
| RU2661446C1 (en) | Method of determining navigation parameters of object and strapdown inertial navigation system for implementation of method | |
| CN103954288B (en) | A kind of Satellite Attitude Determination System precision response relation determines method | |
| CN107228683B (en) | Slow-variation error real-time on-orbit correction method among multiple star sensors | |
| CN110954081A (en) | Quick calibration device and method for magnetic compass |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |