A method for calibrating Doppler velocity error of underwater integrated navigation

Abstract

The present application relates to a kind of underwater integrated navigation Doppler velocity error calibration method, belong to underwater integrated navigation system technical field.The calibration method of the present application utilizes underwater vehicle itself configured GNSS, according to the speed difference of inertial navigation system and Doppler velocity meter and the position difference of inertial navigation system and GNSS establishes observation equation, the position information of GNSS is used as observation, overcome the defect that only SINS / DVL combined navigation is used to calibrate scale factor error, installation deviation angle estimation effect is poor, improves the observability of scale factor error, installation deviation angle.In addition, the course direction in calibration process is further improved, and the convergence speed of scale factor error and installation deviation angle in calibration is further improved, so that rapid and accurate calibration can be realized.

Description

Technical Field

[0001] This invention relates to a Doppler velocity measurement error calibration method for underwater integrated navigation, belonging to the technical field of underwater integrated navigation systems. Background Technology

[0002] Currently, the means available for underwater navigation remain relatively limited. Navigation systems for underwater vehicles must possess long-range, long-endurance, and high-precision navigation capabilities. Strapdown Inertial Navigation System / Doppler Velocity Log (SINS / DVL) integrated navigation is one of the main methods for achieving autonomous underwater navigation.

[0003] Doppler is an instrument that uses an ultrasonic transducer mounted on a carrier to emit ultrasonic waves into the seabed and measures the carrier's velocity based on the Doppler effect. The principle of SINS / DVL integrated navigation is shown in the attached figure. Figure 1 As shown, using Doppler velocity observation information can suppress the error growth of the inertial navigation system, thereby achieving integrated navigation. In SINS / DVL integrated navigation, the Doppler (DVL) velocity measurement error is the decisive factor affecting the accuracy of integrated navigation. The Doppler velocity measurement error parameter includes the scaling factor error; that is, the Doppler velocity measurement value can be expressed as:

[0004]

[0005] δv is the Doppler velocity measurement; k is the scale factor error; d To measure the noise, it can be approximated as Gaussian white noise. The velocity measured by Doppler is in the Doppler carrier coordinate system d. In practical use, it needs to be transformed to the inertial navigation carrier coordinate system b. Under ideal installation conditions, the axes of the inertial navigation carrier coordinate system b and the Doppler carrier coordinate system d coincide. However, in actual use, installation deviations are unavoidable, and the relationship between the two coordinate systems is as follows: Figure 2 As shown. We can assume the installation error angle is small. The velocity measured by Doppler can be expressed as:

[0006]

[0007] These are Doppler velocity measurements in the b-frame. The transformation matrix from the d-system to the b-system can be approximated for small angles as follows:

[0008]

[0009] Where α, β, and γ are the roll, heading, and pitch installation deviation angles, respectively. The so-called Doppler velocity measurement error calibration is to obtain the scale factor error and installation deviation angle in formulas (1) and (3).

[0010] There are two main methods to address this issue: offline calibration and online calibration.

[0011] The common offline calibration method involves introducing additional velocity or position observation information as a calibration benchmark and establishing a relationship model between the Doppler velocity sequence and the reference benchmark to achieve calibration. This method requires additional position and velocity reference information provided by satellite navigation systems (GNSS), long baseline navigation systems, etc. Currently used offline calibration methods do not consider the impact of vehicle maneuvering on calibration. Some scholars have proposed schemes to complete calibration using a limited amount of observation information. However, these methods have certain limitations in accuracy.

[0012] Online self-calibration algorithms, during integrated navigation, use the installation deviation angle and scale factor error as state variables of the SINS / DVL integrated navigation filter for online estimation. This method typically requires the vehicle to have a certain degree of maneuverability during operation. However, due to the limited maneuverability of underwater vehicles, the online estimation accuracy of error parameters (scale factor error, installation deviation angle) is affected to some extent. Summary of the Invention

[0013] The purpose of this invention is to provide a Doppler velocity measurement error calibration method for underwater integrated navigation, so as to solve the problems of low observability and poor estimation effect caused by error parameters such as calibration scale factor error and installation deviation angle in the current online calibration process.

[0014] To solve the above-mentioned technical problems, this invention provides a Doppler velocity measurement error calibration method for underwater integrated navigation, which includes the following steps:

[0015] 1) After the underwater vehicle surfaces, it performs initial alignment of the integrated navigation system, which includes an inertial navigation system, a satellite navigation system, and a Doppler velocimeter;

[0016] 2) After initial alignment, real-time measurements from the inertial system, satellite navigation system, and Doppler velocimeter are acquired, including Doppler velocity measurements, inertial navigation system velocity measurements and position information, and satellite navigation system position information. The real-time acquired measurements are input into a Kalman filter, which performs real-time calculations based on the established integrated navigation model, and outputs the scaling factor error and installation deviation angle in real time until the calibration termination condition is met. The integrated navigation model includes state equations and observation equations. The observation equations are established based on the velocity difference between the inertial navigation system and the Doppler velocimeter, and the position difference between the inertial navigation system and the satellite navigation system.

[0017] The calibration method of this invention utilizes the GNSS configured on the underwater vehicle itself. It establishes an observation equation based on the speed difference between the inertial navigation system and the Doppler velocimeter, as well as the position difference between the inertial navigation system and the GNSS. The position information of the GNSS is used as the observation, and a Kalman filter is used for real-time calibration. This method overcomes the shortcomings of poor scaling factor error and installation deviation angle estimation when using only SINS / DVL integrated navigation for calibration, and improves the observability of scaling factor error and installation deviation angle.

[0018] Furthermore, the established observation equation is as follows:

[0019] z = Hx + η

[0020] z = [z1 z2] T

[0021]

[0022]

[0023]

[0024] in Velocity measured by an inertial navigation system; The speed measured by the Doppler velocimeter; Position measured by the inertial navigation system; Position measured by a satellite navigation system; I 3×3 It is a 3×3 identity matrix; 0 3×3 It is a 3×3 zero matrix. This represents the current velocity vector value. It is a skew-symmetric matrix for the velocity vector.

[0025] This invention utilizes only GNSS position observations and not velocity observations when establishing the observation equations. This improves the observability of scaling factor errors and prevents GNSS velocity information from contaminating the estimation results of scaling factor errors and installation deviation angles.

[0026] Furthermore, the established state equation is as follows:

[0027]

[0028] Where δv N δv E δv D These represent the velocity errors in the north, east, and ground directions, respectively; φ N φ E φ D The attitude error angle is δL, δλ, and δh, which represent latitude error, longitude error, and altitude error, respectively. The accelerometers in the three directions under the load system have zero bias; ε x ε y ε z The gyroscopes in the three directions are zero biased under the load system; F is the state transition matrix; G is the noise distribution matrix; w is Gaussian white noise.

[0029] Furthermore, the calibration termination conditions include a calibration travel greater than a set distance and a calibration factor error variation range within a set time period less than a set range.

[0030] By setting calibration termination conditions, this invention can avoid the problems of low calibration accuracy caused by excessively short calibration processes and the impact of long-term calibration on normal navigation, thus balancing calibration efficiency and accuracy.

[0031] Furthermore, the set distance is 10 kilometers, the set time is 5 minutes, and the set range is 0.05%.

[0032] By setting specific calibration termination conditions, this invention can further ensure the efficiency and accuracy of calibration.

[0033] Furthermore, during the calibration process, the underwater vehicle needs to be controlled to make several turns on the water surface so that the course of the underwater vehicle on the water surface is not a straight line.

[0034] This invention proposes controlling the underwater vehicle to make multiple turns on the water surface during the calibration process, which can improve the calibration process and further increase the convergence speed of calibration factor error and installation deviation angle.

[0035] Furthermore, in step 1), after the underwater vehicle surfaces, the time interval or mileage since the last calibration is determined. Calibration is then performed when the time interval or mileage reaches the corresponding set threshold.

[0036] By setting the above-mentioned calibration activation conditions, this invention can adapt to situations where the speed measurement error parameters of underwater vehicle integrated navigation components change during long-term use. Attached Figure Description

[0037] Figure 1 This is a schematic diagram of the current SINS / DVL integrated navigation structure;

[0038] Figure 2 This is a schematic diagram illustrating the relationship between the inertial navigation system and the Doppler coordinate system in SINS / DVL integrated navigation.

[0039] Figure 3 This is a schematic diagram of the INS / GNSS / DVL integrated navigation structure of the present invention;

[0040] Figure 4 This is a schematic diagram of the underwater vehicle's navigation direction on the water surface during the Doppler velocity measurement error calibration process of the integrated navigation system of this invention. Detailed Implementation

[0041] The specific embodiments of the present invention will be further described below with reference to the accompanying drawings.

[0042] This invention utilizes the GNSS system configured on the underwater vehicle for calibration. To ensure accurate reception of GNSS signals, calibration must be performed after the underwater vehicle surfaces. Furthermore, the underwater vehicle must remain on the surface throughout the calibration process. Observation equations are established based on the speed difference between the inertial navigation system and the Doppler velocimeter, as well as the position difference between the inertial navigation system and the satellite navigation system.

[0043] like Figure 3 The integrated navigation system employed includes an inertial system (SINS), a satellite navigation system (GNSS), and a Doppler velocimeter (DVL), referred to as SINS / GNSS / DVL integrated navigation. This prevents GNSS velocity information from contaminating the estimation of scaling factor errors and installation deviation angles, and improves the observability of scaling factor errors. For GNSS observations, only position observations are used, not velocity observations. To achieve Doppler velocity measurement error calibration for this integrated navigation system, this invention first needs to establish the integrated navigation model, namely the state equation and the observation equation.

[0044] 1) Establishment of the state equations:

[0045] This invention selects the North-East-Down (NED) geographic coordinate system as the navigation system, denoted as n. The velocity error equation, attitude error equation, and position error equation of the inertial navigation system can be expressed as:

[0046]

[0047] Where δv is the velocity error, φ is the attitude error, δr is the position error, and f nTo represent the force in the navigation coordinate system, [f n ×] is f n A skew-symmetric matrix, This represents the Earth's rotational angular velocity in the navigation coordinate system. Let be the representation of the rotational angular velocity of the n-frame relative to the e-frame in the navigation coordinate system. This represents the direction cosine matrix from the b-frame to the n-frame. Accelerometer zero bias. If we model the gyroscope's zero bias ε as a constant, then we have:

[0048]

[0049] If the Doppler scaling factor error k and installation deviation angles α, β, and γ are modeled as constants, then:

[0050]

[0051] The state variables selected for the SINS / GNSS / DVL integrated navigation system are:

[0052]

[0053] Where, δv N δv E δv D These represent the velocity errors in the north, east, and ground directions, respectively; φ N φ E φ D The attitude error angle is δL, δλ, and δh, which represent latitude error, longitude error, and altitude error, respectively. The accelerometers in the three directions under the load system have zero bias; ε x ε y ε z The gyroscopes in the three directions are zero-biased under the system. Based on equations (4), (5), and (6), the state equations of the integrated navigation system can be derived:

[0054]

[0055] Where F is the state transition matrix, G is the noise distribution matrix, and its specific form can be obtained from equations (4), (5), and (6), and w is Gaussian white noise.

[0056] 2) Establishment of observation equations:

[0057] The observations for SINS / GNSS / DVL integrated navigation include Doppler velocity measurements, inertial navigation system velocity measurements and position information, and satellite navigation system velocity measurements and position information. First, the Doppler velocity measurements are transformed to the navigation coordinate system using the following transformation relationship:

[0058]

[0059] Among them, I 3×3 Let φ be a 3×3 identity matrix, and φ× be the skew-symmetric matrix of attitude error. The superscript “~” indicates the calculated value, and the value without the superscript is the true value. Substituting equation (1) into (9) and ignoring second-order minor quantities, we can obtain:

[0060]

[0061] This is the current velocity vector value. Let η1 be the skew-symmetric matrix of the velocity vector, and let η1 be Gaussian white noise. The velocity difference between the inertial navigation system and the Doppler velocimeter is used as one of the observations in the integrated navigation system, i.e.:

[0062]

[0063] The position difference between the inertial navigation system and the satellite navigation system is used as the second observation in the integrated navigation system, namely:

[0064]

[0065] Combining equations (11) and (12), we can obtain the observation equations for the SINS / GNSS / DVL integrated navigation system:

[0066] z = Hx + η (13)

[0067] Where the observed quantity z = [z1 z2] T White noise η=[η1η2] T The specific form of the observation matrix H can be derived from equations (11) and (12):

[0068]

[0069] Position measured by the inertial navigation system; Position measured by a satellite navigation system; I 3×3 It is a 3×3 identity matrix; 0 3×3 It is a 3×3 zero matrix.

[0070] Based on the integrated navigation model established above (Equation (8) and Equation (13)), the Doppler velocity measurement error in the integrated navigation will be calibrated below. The calibration process is as follows.

[0071] 1. Once the underwater vehicle surfaces, activate the integrated navigation system and perform initial alignment to prepare for subsequent calibration.

[0072] Since GNSS can only receive data normally on the water surface, and this invention requires GNSS positioning data for calibration, the subsequent calibration work can only begin after the underwater vehicle surfaces. To ensure the accuracy of the subsequent calibration, the inertial system, satellite navigation system and Doppler velocimeter in the integrated navigation system also need to be initially aligned, including time synchronization.

[0073] 2. Utilize the established integrated navigation model.

[0074] After initial alignment, the system acquires real-time measurements from the inertial system, satellite navigation system, and Doppler velocimeter, including Doppler velocity measurements, inertial navigation system velocity measurements and position information, and satellite navigation system position information. The real-time measurements are then input into a Kalman filter (KF), which performs real-time calculations based on the established integrated navigation model and outputs the scaling factor error and installation deviation angle in real time, thereby calibrating the scaling factor error and installation deviation angle.

[0075] To ensure calibration accuracy, this invention requires controlling the underwater vehicle's course on the water surface during the calibration process. To prevent the underwater vehicle from traveling in a straight line continuously on the surface during calibration, this invention also controls the underwater vehicle's course direction on the surface during the calibration process, causing the underwater vehicle to make several turns while traveling on the surface, such as... Figure 4 As shown, the underwater vehicle can be made to run along a "Z"-shaped route on the water surface. By using the "Z"-shaped navigation method, the observability of the two state variables, calibration factor error and installation deviation angle, in the integrated navigation model can be effectively improved, thereby further improving the convergence speed of calibration factor error and installation deviation angle.

[0076] Furthermore, to avoid the problems of low calibration accuracy due to excessively short calibration processes and the impact of prolonged calibration on normal navigation, this invention also sets termination conditions for the calibration process. These termination conditions include a calibration distance greater than a set distance and a calibration factor error variation range within a set time being less than a set range. In this embodiment, the set distance is 10 kilometers, the set time is 5 minutes, and the set range is 0.05%. That is, if the calibration distance is greater than 10 kilometers and the scaling factor error variation range within 5 minutes is less than 0.05%, the calibration is determined to be terminated; otherwise, calibration continues.

[0077] Furthermore, to address the issue of changing speed measurement error parameters due to long-term use of underwater vehicle navigation components, this invention can also set calibration activation conditions. After the underwater vehicle surfaces, it first determines the time interval or mileage since the last calibration. Calibration is only performed when the time interval or mileage reaches a corresponding set threshold; otherwise, calibration is not performed. Each set threshold can be configured according to actual conditions.

[0078] As can be seen from the above process, the calibration method of this invention utilizes the GNSS configured on the underwater vehicle itself. It establishes observation equations based on the velocity difference between the inertial navigation system and the Doppler velocimeter, as well as the position difference between the inertial navigation system and the GNSS. Using the GNSS position information as the observation, it overcomes the shortcomings of poor estimation of scaling factor error and installation deviation angle when using only SINS / DVL integrated navigation for calibration, thus improving the observability of scaling factor error and installation deviation angle. By controlling the course direction during the calibration process, the convergence speed of the calibration scaling factor error and installation deviation angle is further improved, ensuring rapid and accurate calibration of these parameters.

Claims

1. A method for calibrating Doppler velocity measurement errors in underwater integrated navigation, characterized in that, The calibration method includes the following steps: 1) After the underwater vehicle surfaces, it performs initial alignment of the integrated navigation system, which includes an inertial navigation system, a satellite navigation system, and a Doppler velocimeter; 2) After initial alignment, real-time measurements from the inertial system, satellite navigation system, and Doppler velocimeter are acquired, including Doppler velocity measurements, inertial navigation system velocity measurements and position information, and satellite navigation system position information. These real-time measurements are input into a Kalman filter, which performs real-time calculations based on the established integrated navigation model, outputting the scaling factor error and installation deviation angle in real time until the calibration termination condition is met. The integrated navigation model includes state equations and observation equations. The observation equations are established based on the velocity difference between the inertial navigation system and the Doppler velocimeter, and the position difference between the inertial navigation system and the satellite navigation system. The established state equations include a Doppler scaling factor error k. During the calibration process, the course direction of the underwater vehicle on the water surface is controlled to make the underwater vehicle run along a "Z" shaped course direction on the water surface, so as to improve the observability of the two state variables, calibration factor error and installation deviation angle, in the integrated navigation model.

2. The Doppler velocity measurement error calibration method for underwater integrated navigation according to claim 1, characterized in that, The established observation equation is: , , , , ,in Velocity measured by an inertial navigation system; The speed measured by the Doppler velocimeter; Position measured by the inertial navigation system; Position measured by a satellite navigation system; for The identity matrix; for The zero matrix, This represents the current velocity vector value. It is a skew-symmetric matrix for the velocity vector.

3. The Doppler velocity measurement error calibration method for underwater integrated navigation according to claim 1 or 2, characterized in that, The established state equation is: , in , , These are the velocity errors in the north, east, and ground directions, respectively. , , The attitude error angle; , , These represent latitude error, longitude error, and altitude error, respectively. , , The accelerometers in the three directions under the load system have zero bias; , , Zero bias of the gyroscope in three directions under the load system; This is the state transition matrix; Assign a matrix to the noise; It is Gaussian white noise.

4. The Doppler velocity measurement error calibration method for underwater integrated navigation according to claim 1 or 2, characterized in that, The calibration termination conditions include a calibration travel greater than a set distance and a calibration factor error variation range within a set time period that is less than a set range.

5. The Doppler velocity measurement error calibration method for underwater integrated navigation according to claim 4, characterized in that, The set distance is 10 kilometers, the set time is 5 minutes, and the set range is 0.05%.

6. The Doppler velocity measurement error calibration method for underwater integrated navigation according to claim 1 or 2, characterized in that, In step 1), after the underwater vehicle surfaces, the time interval or mileage since the last calibration is determined. Calibration is then performed when the time interval or mileage reaches the corresponding set threshold.

Images