Optimization Method for SINS / DVL Compact Integration Navigation Based on ST-EKF under Beam Failure Conditions

Abstract

This invention relates to an optimization method for SINS / DVL tightly integrated navigation based on ST-EKF under beam failure conditions, comprising: constructing a SINS / DVL tightly integrated navigation model based on ST-EKF; determining whether the Doppler frequency shift information returned by the four DVL beams is valid; if none of them return valid values, performing a time update and continuing to determine whether the returned Doppler frequency shift information is valid; otherwise, determining the missing Doppler frequency shift information of the beams and reconstructing it according to the missing information. Based on the structural characteristics of the four-beam Janus array, an objective function is constructed based on the Doppler frequency shift information of the four DVL beams, and the noise matrix Q and R are optimized in real time using a particle swarm optimization algorithm; the optimized noise matrix Q and R data are used for measurement updates. This invention applies ST-EKF to the SINS / DVL tightly integrated navigation system, taking into account the differences in magnitude and direction of the velocity vector in the real navigation coordinate system and the calculated navigation coordinate system, thus improving the initial alignment and integrated navigation accuracy in a quasi-stationary environment.

Description

Technical Field

[0001] This invention belongs to the field of integrated navigation optimization, specifically relating to a SINS / DVL tight integrated navigation optimization method based on ST-EKF under beam failure conditions. Background Technology

[0002] Autonomous Underwater Vehicles (AUVs) possess advantages such as autonomy, stealth, and high maneuverability, playing a crucial role in ocean exploration and development. The autonomous navigation capability of an AUV is one of the important indicators for evaluating its operational performance, and also one of the key technologies restricting its development. Therefore, how to build an underwater autonomous navigation system with higher precision is key to further improving AUV performance.

[0003] Strapdown Inertial Navigation Systems (SINS) do not radiate energy. Although navigation errors accumulate over time and they cannot provide AUVs with high-precision navigation information for extended periods, their stealth and autonomy are excellent, making them a common primary navigation system for AUVs. In the selection of auxiliary navigation systems, Doppler Velocity Logs (DVLs) measure the AUV's high-precision velocity relative to the seabed or water column using bottom-tracking or water-tracking modes. Their velocity errors do not accumulate over time, and the high-precision velocity information provided by DVLs can suppress the accumulated velocity errors of the primary inertial navigation system, providing high-precision velocity information for AUVs over extended periods. Therefore, SINS / DVL integrated navigation systems are widely used in underwater environments.

[0004] SINS / DVL integrated navigation can be divided into two forms: loosely integrated and tightly integrated. Loosely integrated SINS / DVL navigation uses velocity matching and can only perform navigation tasks when three or more beams in the DVL return valid information. Tightly integrated SINS / DVL navigation, on the other hand, uses beam velocity or beam Doppler shift information for matching, providing high-precision navigation when one or more beams in the DVL return valid information. Researchers have compared the integrated navigation accuracy of these two SINS / DVL tightly integrated navigation methods, and the results show that the SINS / DVL tightly integrated navigation method based on beam Doppler shift information has higher navigation accuracy.

[0005] Current SINS / DVL compact navigation systems and intelligent optimization algorithms address noise matrix issues. Q and R The main problems with real-time optimization methods are as follows:

[0006] (1) In traditional integrated navigation systems based on Extended Kalman Filter (EKF), the velocity error is simply defined as the difference in magnitude between the velocity vectors in the real navigation coordinate system and the calculated navigation coordinate system, without considering the difference in direction of the velocity vectors. In a quasi-stationary environment, this can lead to a discrepancy between the theoretical and actual estimated values ​​of the variance, resulting in a decrease in navigation accuracy or even the divergence of the filter.

[0007] (2) The literature "SINS / DVL Tightly Integrated Navigation Method under Measurement Field Values ​​and Beam Failure Conditions" uses a fault handling method for SINS / DVL tightly integrated navigation systems based on beam velocity reconstruction, which effectively improves navigation accuracy under beam failure conditions. First, the reconstructed missing beam velocity information cannot accurately reflect its true value, which will cause a mismatch between the system measurement noise covariance matrix and the measurement measurements, affecting the navigation accuracy of SINS / DVL tightly integrated navigation. Second, this method still uses SINS / DVL tightly integrated navigation based on beam velocity.

[0008] (3) Some scholars have already used particle swarm optimization algorithm to optimize the noise matrix in SINS / GPS loosely coupled navigation. Q and R The optimization algorithm is designed to find the optimal target function, but the objective function is based on the loosely integrated SINS / GPS navigation system and is not applicable to the tightly integrated SINS / DVL navigation system. Summary of the Invention

[0009] The purpose of this invention is to address the above-mentioned problems by providing a SINS / DVL tightly coupled navigation optimization method based on ST-EKF under beam failure conditions, which takes into account the differences in magnitude and direction of velocity vectors in the navigation coordinate system and the calculated navigation coordinate system, thereby improving the accuracy of the coupled navigation.

[0010] The technical solution of this invention is a SINS / DVL compact combination navigation optimization method based on ST-EKF under beam failure conditions, comprising the following steps: Step 1: Construct a SINS / DVL compact navigation model based on ST-EKF; Step 1.1: Construct the state equations of the SINS / DVL compactly integrated navigation system based on ST-EKF; Step 1.2: Construct the measurement equations for the ST-EKF-based SINS / DVL tightly integrated navigation system; Step 2: Determine whether the Doppler frequency shift information returned by the four beams of DVL is a valid value. If none of them return a valid value, perform a time update and repeat Step 2 to continue to determine whether the returned Doppler frequency shift information is a valid value. Step 3: Determine the extent of missing Doppler frequency shift information in the beam and reconstruct it accordingly; Step 3.1: If the Doppler frequency shift information of a single beam is missing, the Doppler frequency shift information of the missing beam can be reconstructed based on the structural characteristics of the DVL itself; Step 3.2: If the Doppler frequency shift information of two adjacent beams is missing, the projection of the celestial velocity calculated by SINS into the carrier coordinate system is introduced, and the Doppler frequency shift information of the missing beam is reconstructed according to the working principle of DVL. Step 3.3: If the Doppler frequency shift information of the two opposing beams is missing and DVL uses " The configuration method introduces the projection of the eastward velocity calculated by SINS into the carrier coordinate system, and then reconstructs the Doppler frequency shift information of the missing beam according to the working principle of DVL. Step 3.4: If the Doppler frequency shift information of the two opposing beams is missing and DVL uses " In the "type" configuration, when beams 2 and 4 are missing, the projection of the eastward velocity calculated by SINS onto the carrier coordinate system is introduced, and the Doppler frequency shift information of the missing beam is reconstructed according to the working principle of DVL. When beams 1 and 3 are missing, the projection of the northward velocity calculated by SINS onto the carrier coordinate system is introduced, and the Doppler frequency shift information of the missing beam is reconstructed according to the working principle of DVL. Step 3.5: If the Doppler frequency shift information is missing for a three-beam system and DVL uses " The configuration method introduces the projection of the eastward and celestial velocities calculated by SINS onto the carrier coordinate system, and then calculates the Doppler frequency shift information of the reconstructed beam according to the working principle of DVL. Step 3.6: If the Doppler frequency shift information is missing for a three-beam system and DVL uses " In the "type" configuration, if the missing beam is 1, 2, 4 or 2, 3, 4, the projection of the eastward and celestial velocities calculated by SINS onto the carrier coordinate system is introduced, and then the Doppler frequency shift information of the missing beam is reconstructed according to the working principle of DVL. If the missing beam is 1, 2, 3 or 1, 3, 4, the projection of the northward and celestial velocities calculated by SINS onto the carrier coordinate system is introduced, and then the Doppler frequency shift information of the missing beam is reconstructed according to the working principle of DVL. Step 4: Based on the structural characteristics of the four-beam Janus array and using the Doppler frequency shift information of the four beams in the DVL array as a foundation, construct the objective function and use the Particle Swarm Optimization (PSO) algorithm to analyze the noise matrix. Q and R Perform real-time optimization.

[0011] Step 5: Use the optimized noise matrixQ and R The data is measured and updated.

[0012] Furthermore, the "East-North-Sky" coordinate system is selected as the navigation coordinate system, that is... The coordinate system is defined as the "right-front-up" coordinate system, i.e. The coordinate system in which DVL is located is... Tie.

[0013] Preferably, in step 4, based on the characteristics of the four-beam Janus array structure and using the Doppler frequency shift information of the four beams of the DVL array as a basis, an objective function is constructed, and the PSO algorithm is used to analyze the noise matrix. Q and R Real-time optimization is performed. The objective function of the PSO algorithm is as follows:

[0014] In the formula This represents the number of Kalman filters performed so far. , , , It is expressed as the estimated Doppler frequency shift values ​​of the four beams of DVL.

[0015] Compared with the prior art, the beneficial effects of the present invention include: 1) This invention applies ST-EKF to the SINS / DVL tightly integrated navigation system. The definition of velocity error by ST-EKF takes into account the differences in magnitude and direction of the velocity vector in both the real navigation coordinate system and the calculated navigation coordinate system, thereby improving the initial alignment and integrated navigation accuracy in a quasi-stationary environment.

[0016] 2) Based on the SINS / DVL tightly coupled navigation system fault handling method based on beam reconstruction, this invention uses the PSO optimization algorithm to optimize the noise matrix of the navigation system. Q and R Real-time tracking can solve the problem that the reconstructed beam Doppler frequency shift information cannot accurately reflect the true value due to the introduction of inertial navigation calculation speed during reconstruction, resulting in measurement noise imbalance.

[0017] 3) Starting from the structural characteristics of the four-beam Janus array, this invention proposes a new objective function suitable for underwater SINS / DVL compact combination, which makes reasonable use of the beam configuration structure characteristics of DVL and effectively improves the navigation accuracy of the SINS / DVL compact combination navigation system based on beam Doppler frequency shift information reconstruction. Attached Figure Description

[0018] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0019] Figure 1 This is a flowchart illustrating the compact combination navigation optimization method according to an embodiment of the present invention.

[0020] Figure 2 This is a structural diagram of the four-beam Janus array configuration in an example of the present invention.

[0021] Figure 3 This is a simulation trajectory diagram of the present invention.

[0022] Figure 4 This is a comparison chart of the eastward positioning error of the present invention with simulation experiments using other methods.

[0023] Figure 5 This is a comparison chart of the northward positioning error of the present invention with simulation experiments of other methods.

[0024] Figure 6 This is a comparison diagram of the navigation trajectory of the present invention with simulation experiments of other methods. Detailed Implementation

[0025] like Figure 1 As shown, the SINS / DVL compact navigation optimization method based on ST-EKF under beam failure conditions includes the following steps: Step 1: Construct a SINS / DVL compact navigation model based on ST-EKF; Step 1.1: Construct the state equations of the SINS / DVL compactly integrated navigation system based on ST-EKF; Step 1.2: Construct the measurement equations for the ST-EKF-based SINS / DVL tightly integrated navigation system; Step 2: Determine whether the Doppler frequency shift information returned by the four beams of DVL is a valid value. If none of them return a valid value, perform a time update and repeat Step 2 to continue to determine whether the returned Doppler frequency shift information is a valid value. Step 3: Determine the extent of missing Doppler frequency shift information in the beam and reconstruct it accordingly; Step 3.1: If the Doppler frequency shift information of a single beam is missing, the Doppler frequency shift information of the missing beam can be reconstructed based on the structural characteristics of the DVL itself; Step 3.2: If the Doppler frequency shift information of two adjacent beams is missing, the projection of the celestial velocity calculated by SINS into the carrier coordinate system is introduced, and the Doppler frequency shift information of the missing beam is reconstructed according to the working principle of DVL. Step 3.3: If the Doppler frequency shift information of the two opposing beams is missing and DVL uses " The configuration method introduces the projection of the eastward velocity calculated by SINS into the carrier coordinate system, and then reconstructs the Doppler frequency shift information of the missing beam according to the working principle of DVL. Step 3.4: If the Doppler frequency shift information of the two opposing beams is missing and DVL uses " In the "type" configuration, when beams 2 and 4 are missing, the projection of the eastward velocity calculated by SINS onto the carrier coordinate system is introduced, and then the Doppler frequency shift information of the missing beam is reconstructed according to the working principle of DVL. When beams 1 and 3 are missing, the projection of the northward velocity calculated by SINS onto the carrier coordinate system is introduced, and then the Doppler frequency shift information of the missing beam is reconstructed according to the working principle of DVL. Step 3.5: If the Doppler frequency shift information is missing for a three-beam system and DVL uses " The configuration method introduces the projection of the eastward and celestial velocities calculated by SINS onto the carrier coordinate system, and then calculates the Doppler frequency shift information of the reconstructed beam according to the working principle of DVL. Step 3.6: If the Doppler frequency shift information is missing for a three-beam system and DVL uses " In the "type" configuration, if the missing beam is 1, 2, 4 or 2, 3, 4, the projection of the eastward and celestial velocities calculated by SINS onto the carrier coordinate system is introduced, and then the Doppler frequency shift information of the missing beam is reconstructed according to the working principle of DVL. If the missing beam is 1, 2, 3 or 1, 3, 4, the projection of the northward and celestial velocities calculated by SINS onto the carrier coordinate system is introduced, and then the Doppler frequency shift information of the missing beam is reconstructed according to the working principle of DVL. Step 4: Based on the structural characteristics of the four-beam Janus array and using the Doppler frequency shift information of the four beams in the DVL array as a foundation, construct the objective function and use the Particle Swarm Optimization (PSO) algorithm to analyze the noise matrix. Q and R Perform real-time optimization.

[0026] Step 5: Use the optimized noise matrix Q and R The data is measured and updated.

[0027] In step 1.1, the state equations of the SINS / DVL compactly integrated navigation system based on ST-EKF are constructed: The "East-North-Sky" coordinate system is selected as the navigation coordinate system, that is... The coordinate system is defined as the "right-front-up" coordinate system, i.e. The coordinate system in which DVL is located is... The coordinate system in DVL where the beam is located is... Tie.

[0028]

[0029] in: Let be the error state vector. The derivative of the error state vector, For the system matrix, The noise transfer matrix, This is the process noise vector; ; In the formula The misalignment angles are for the east, north, and sky directions; For the speed errors in the east, north, and sky directions; This includes errors in longitude, latitude, and altitude. To add a constant value with zero bias to the table; This represents the constant drift of the gyroscope.

[0030]

[0031] In the formula The velocity calculated by the inertial navigation system is in the calculation n The projection under the system, This represents transforming a vector into an antisymmetric matrix. Let be the attitude matrix. , , To add white noise to the three axes of the table, , , The white noise of the three axes of the gyroscope. It is a zero matrix;

[0032]

[0033]

[0034]

[0035]

[0036] ;

[0037]

[0038] In the formula: for The block matrix; is a block matrix; , , , , are all matrices; is the local gravity vector; is the projection of the angular rate of the system relative to the geocentric inertial coordinate system in the system; is the projection of the angular rate of the Earth's rotation relative to the geocentric inertial coordinate system in the system; is the projection of the angular rate generated by the movement of the system relative to the Earth in the local , respectively represent the eastward, northward, and upward velocities calculated by the INS; is the radius of curvature of the prime vertical; is the radius of curvature of the卯酉圈; is the latitude where the vehicle is located; is the altitude where the vehicle is located; is the Earth's rotation speed.

[0039] In step 1.2, the measurement equation of the SINS / DVL tightly coupled navigation system based on ST-EKF is:

[0040] where is the measured quantity, is the measurement transfer matrix; is the measurement noise.

[0041]

[0042]

[0043]

[0044]

[0045]

[0046] <000024​​​​​​​The values ​​represent the Doppler frequency shifts of the four beams in the DVL waveform. The velocity vector calculated by the inertial navigation system. The frequency at which the DVL emits sound waves. The speed of sound waves emitted by DVL in water. The calibrated DVL installation error matrix is ​​shown below. for System projection to The coordinate transformation matrix of the system. The fixed beam transmission angle is determined by the DVL structure. Configure the mounting angle for the four-beam Janus, In the "type configuration mode, β is In the "+" configuration, β is , This is the corresponding measurement white noise.

[0049] In step 3.1, for the missing Doppler frequency shift information of a single beam, the Doppler frequency shift information of the missing beam is reconstructed based on the structural characteristics of the DVL itself. The reconstructed Doppler frequency shift information is as follows:

[0050] In the formula , This is represented as the Doppler frequency shift information of the beam adjacent to the faulty beam, and they are in a diagonal relationship. This refers to the beam Doppler frequency shift information relative to the faulty beam.

[0051] In step 3.2, if it is two adjacent beams When the Doppler frequency shift information is missing, the projection of the celestial velocity calculated by SINS onto the carrier coordinate system is introduced, and then the Doppler frequency shift information of the missing beam is reconstructed according to the working principle of DVL. The reconstructed Doppler frequency shift information is as follows:

[0052] In the formula The astronautical velocity calculated in the sky is The projection of the system.

[0053] In step 3.3, if the Doppler frequency shift information of the two relative beams is missing and DVL uses " The configuration method introduces the projection of the eastward velocity calculated by SINS onto the carrier coordinate system, and then reconstructs the Doppler frequency shift information of the missing beam according to the working principle of DVL. The reconstructed Doppler frequency shift information is as follows:

[0054]

[0055] In the formula The eastward velocity calculated by the inertial navigation system is The projection of the system.

[0056] In step 3.4, if the Doppler frequency shift information of the two relative beams is missing and DVL uses " In the "type" configuration, when beams 2 and 4 are missing, the projection of the eastward velocity calculated by SINS onto the carrier coordinate system is introduced, and then the Doppler frequency shift information of the missing beams is reconstructed according to the working principle of DVL. When beams 1 and 3 are missing, the projection of the northward velocity calculated by SINS onto the carrier coordinate system is introduced, and then the Doppler frequency shift information of the missing beams is reconstructed according to the working principle of DVL. The reconstructed Doppler frequency shift information is as follows:

[0057]

[0058] In the formula The northward velocity calculated by the inertial navigation system is The projection of the system.

[0059] In step 3.5, if the Doppler frequency shift information is missing for a three-beam system and DVL uses " The configuration method incorporates the projections of the eastward and celestial velocities calculated by SINS onto the carrier coordinate system. Then, based on the working principle of DVL, the Doppler frequency shift information of the reconstructed beam is calculated. The reconstructed Doppler frequency shift information is as follows:

[0060]

[0061] In step 3.6, if the Doppler frequency shift information is missing for a three-beam system and DVL uses " In the "type" configuration, when the missing beam is 1, 2, 4 or 2, 3, 4, the projections of the eastward and celestial velocities calculated by SINS onto the carrier coordinate system are introduced. Then, based on the working principle of DVL, the Doppler frequency shift information of the missing beam is reconstructed. When the missing beam is 1, 2, 3 or 1, 3, 4, the projections of the northward and celestial velocities calculated by SINS onto the carrier coordinate system are introduced. Then, based on the working principle of DVL, the Doppler frequency shift information of the missing beam is reconstructed. The reconstructed Doppler frequency shift information is as follows:

[0062]

[0063] In step 4, based on the structural characteristics of the four-beam Janus array and using the Doppler frequency shift information of the four beams of the DVL array as a basis, an objective function is constructed, and the PSO algorithm is used to analyze the noise matrix. Q and R Real-time optimization is performed. The objective function of the PSO algorithm is as follows:

[0064] In the formula This represents the number of Kalman filters performed so far. , , , It is expressed as the estimated Doppler frequency shift values ​​of the four beams of DVL.

[0065] Example 1: To verify the effectiveness of the method provided by this invention, the following simulation experiments were conducted: 1) The trajectory is set as follows: Total exercise time t =3600s; Initial position: , , The initial velocity is: , , Initial attitude: Pitch angle: Roll angle: Heading angle: Initial attitude misalignment angle: , , The simulated trajectory is as follows: Figure 2 As shown.

[0066] 2) IMU settings IMU sampling frequency: 200 Hz ; Gyroscope zero bias: Gyroscope white noise: ; Accelerometer zero bias: Accelerometer white noise: .

[0067] 3) DVL settings DVL emitted sound waves at a frequency of 300kHz; the speed of sound was: DVL sampling frequency: 1 Hz DVL speed measurement error: The four-wavelength Janus configuration is a "+" type configuration.

[0068] 4) Particle Swarm Optimization Algorithm Settings Set the initial population size : ; Particle's Personal Learning Factor : ; Social learning factor of particles : ; Maximum speed of particles : ; Termination condition for optimization: set to the maximum number of iterations, 20; Inertia weight : .

[0069] 5) DVL working status settings When set to 400s~600s , , Missing; 1700s~1900s Missing; 2500s~2700s , Missing.

[0070] Compare the navigation accuracy of the following four SINS / DVL tightly coupled navigation methods under this simulation condition: Navigation Method 1: When the Doppler frequency shift information of the beam is missing, the missing information is reconstructed and then EKF-based SINS / DVL compact combination navigation is performed. Navigation Method 2: When the Doppler frequency shift information of the beam is missing, the missing information is reconstructed and then SINS / DVL compact combination navigation based on ST-EKF is performed; Navigation Method 3: When the Doppler frequency shift information of the beam is missing, after reconstructing the missing information, the PSO algorithm is performed using the existing objective function to process the noise matrix. Q and R Real-time optimization is performed, followed by ST-EKF-based SINS / DVL tight combination navigation; Navigation Method 4: When the Doppler frequency shift information of the beam is missing, after reconstructing the missing information, the PSO algorithm is performed using the objective function in this patent to process the noise matrix. Q and R Real-time optimization is performed, followed by ST-EKF-based SINS / DVL tight combination navigation.

[0071] Finally, a comparison of the eastward and northward positioning errors of the four methods is shown below. Figure 3 and Figure 4 As shown, the trajectory of the navigation method is as follows: Figure 5 As shown. From Figure 3 and Figure 4The error curves of the four navigation methods show that the navigation and positioning accuracy decreases in the event of beam failure. Comparing navigation method 1 and navigation method 2, ST-EKF has a better filtering effect than EKF, improving the final navigation accuracy. Comparing navigation method 2 and navigation method 3, navigation method 3 shows a significant improvement in navigation accuracy in the east and north directions. Comparing navigation method 3 and navigation method 4, navigation method 4 further improves positioning accuracy in the east and north directions. The difference between navigation method 3 and navigation method 4 lies in the objective function used in the particle swarm optimization algorithm. The objective function used in navigation method 3 is:

[0072] In the formula To measure the projection of velocity in the n-frame for DVL, This represents the number of Kalman filters performed so far. The estimated carrier velocity after ST-EKF filtering.

[0073] The objective function used in navigation method 4 is:

[0074] At 2500s~2700s , Taking the missing information as an example, the known frequency shift information of the beam is... , Reconstructed , for:

[0075]

[0076] In the formula The actual velocity of the carrier in the b system. The error value between the projection of the carrier's celestial velocity in the b-frame calculated by the inertial navigation system and the carrier's true celestial velocity in the b-frame.

[0077] The above reconstructed , Frequency shift information of known beams , After calculating the current DVL-measured carrier velocity, and substituting it into the objective function of the particle swarm optimization algorithm used in navigation method 3, the initial state of the current objective function is:

[0078] It can be seen that the initial state of the objective function contains This error value. In the particle swarm optimization algorithm, the smaller the error in the initial state under the same number of iterations, the higher the final optimization accuracy. Using the above objective function requires more iterations, but the particle swarm optimization algorithm has a large computational load and consumes a lot of time. Increasing the number of iterations will affect the working efficiency of the navigation system. Under the same conditions, the above reconstructed... , Frequency shift information of known beams , Substituting the objective function of the particle swarm optimization algorithm used in navigation method 4, the initial state of the current objective function is:

[0079] Comparing the initial states of the two particle swarm optimization algorithms using different objective functions, we can see that the initial state of the objective function used in navigation scheme 4 contains... With a smaller error value, navigation method 4 will achieve a better final optimization result than navigation method 3 in the same number of iterations.

[0080] noise matrix Q and R Navigation methods 1 and 2 for real-time optimization and noise matrix Q and R Comparing navigation methods 3 and 4, which perform real-time optimization, the method of optimizing the system's noise matrix in real time before applying Kalman filtering effectively suppresses error peaks caused by changes in motion state, significantly improving the final navigation accuracy. Figure 5 The trajectories of the four navigation methods are shown in the figure. , , When missing, the trajectory offset is large, while... , Missing and When missing, the trajectory offset is relatively small. The root mean square error statistics for the four methods are shown in Table 1.

[0081] Table 1. Root Mean Square Error of the Four Navigation Methods in the Examples

[0082] As can be seen from Table 1, regarding the noise matrix Q and RAfter real-time optimization, combined navigation showed a slight reduction in both attitude and velocity errors. Navigation method 3 exhibited poor suppression of altitude error. The navigation method 4 proposed in this invention improves upon the large altitude error to some extent. The root mean square errors in both horizontal directions are smaller than those of other navigation methods. The root mean square error of position is reduced by 44%–53% compared to navigation method 1, by 6%–40% compared to navigation method 2, and by 21%–39% compared to navigation method 3.

Claims

1. A method for optimizing SINS / DVL tightly coupled navigation based on ST-EKF under beam failure conditions, characterized in that, Includes the following steps: Step 1: Construct a SINS / DVL compact navigation model based on ST-EKF; Step 1.1: Construct the state equations of the SINS / DVL compactly integrated navigation system based on ST-EKF; Step 1.2: Construct the measurement equations for the ST-EKF-based SINS / DVL tightly integrated navigation system; Step 2: Determine whether the Doppler frequency shift information returned by the four beams of DVL is a valid value. If none of them return a valid value, perform a time update and repeat Step 2 to continue to determine whether the returned Doppler frequency shift information is a valid value. Step 3: Determine the extent of missing Doppler frequency shift information in the beam and reconstruct it accordingly; Step 3.1: If the Doppler frequency shift information of a single beam is missing, reconstruct the Doppler frequency shift information of the missing beam based on the structural characteristics of the DVL itself; Step 3.2: If the Doppler frequency shift information of two adjacent beams is missing, the projection of the celestial velocity calculated by SINS into the carrier coordinate system is introduced, and the Doppler frequency shift information of the missing beam is reconstructed according to the working principle of DVL. Step 3.3: If the Doppler frequency shift information of the two opposing beams is missing and DVL uses " The configuration method introduces the projection of the eastward velocity calculated by SINS into the carrier coordinate system, and then reconstructs the Doppler frequency shift information of the missing beam according to the working principle of DVL. Step 3.4: If the Doppler frequency shift information of the two opposing beams is missing and DVL uses " The configuration method introduces the projection of the eastward and northward velocity vectors calculated by SINS into the carrier coordinate system, and then reconstructs the Doppler frequency shift information of the missing beam according to the working principle of DVL. Step 3.5: If the Doppler frequency shift information is missing for a three-beam system and DVL uses " The configuration method introduces the projection of the eastward and celestial velocities calculated by SINS onto the carrier coordinate system, and then calculates the Doppler frequency shift information of the reconstructed beam according to the working principle of DVL. Step 3.6: If the Doppler frequency shift information is missing for a three-beam system and DVL uses " The configuration method is as follows: based on the specific missing beam, the projection of the east, north, and sky velocity vectors calculated by SINS into the carrier coordinate system is introduced, and then the Doppler frequency shift information of the missing beam is reconstructed according to the working principle of DVL. Step 4: Based on the structural characteristics of the four-beam Janus array and using the Doppler frequency shift information of the four DVL beams as a basis, construct the objective function and use the particle swarm optimization algorithm to analyze the noise matrix. Q and R Perform real-time optimization; Step 5: Use the optimized noise matrix Q and R The data is measured and updated.

2. The SINS / DVL compact navigation optimization method based on ST-EKF under beam failure conditions according to claim 1, characterized in that, In step 1.1, the "East-North-Sky" coordinate system is selected as the navigation coordinate system, i.e. The coordinate system is defined as the "right-front-up" coordinate system, i.e. The coordinate system in which DVL is located is... The coordinate system in DVL where the beam is located is... Tie; The state equation of the ST-EKF-based SINS / DVL compact navigation system is: ； in: Let be the error state vector. The derivative of the error state vector, For the system matrix, The noise transfer matrix, This is the process noise vector; ； In the formula The misalignment angles are for the east, north, and sky directions; For the speed errors in the east, north, and sky directions; This includes errors in longitude, latitude, and altitude. To add a constant value with zero bias to the table; This represents the constant drift of the gyroscope. ； ； In the formula The velocity calculated by the inertial navigation system is in the calculation n The projection under the system, This represents transforming a vector into an antisymmetric matrix. Let be the attitude matrix. , , To add white noise to the three axes of the table, , , The white noise of the three axes of the gyroscope. It is a 3x3 zero matrix. It is a zero matrix with 9 rows and 3 columns; ； ； ； ； ； ； ； ； Where: is a block matrix of; is a block matrix of; , , , , are all matrices; is the local gravity vector; is the projection of the angular rate of the system relative to the geocentric inertial coordinate system on the is the projection of the angular rate of the Earth's rotation relative to the geocentric inertial coordinate system on the system; is the projection of the angular rate generated by the relative motion of the system relative to the Earth on the local , respectively represent the eastward, northward, and upward velocities calculated by the inertial navigation; is the radius of curvature of the prime vertical; is the radius of curvature of the卯酉圈主曲率半径; is the latitude where the carrier is located; is the altitude where the carrier is located; is the Earth's rotation speed.

3. The SINS / DVL compact navigation optimization method based on ST-EKF under beam failure conditions according to claim 2, characterized in that, The measurement equations for the ST-EKF-based SINS / DVL compact navigation system are as follows: ； In the formula Measurement Measurement transition matrix; Measurement noise; ； ； ； ； ； ； ； In the formula Derive the Doppler frequencies of the four beams for SINS. The values ​​represent the Doppler frequency shifts of the four beams in the DVL waveform. It is a zero matrix with 4 rows and 9 columns. The velocity vector calculated by the inertial navigation system. The frequency at which the DVL emits sound waves. The speed of sound waves emitted by DVL in water. The calibrated DVL installation error matrix is ​​shown below. for System projection to The coordinate transformation matrix of the system. The fixed beam transmission angle is determined by the DVL structure. Configure the mounting angle for the four-beam Janus. This is the corresponding measurement white noise.

4. The SINS / DVL compact navigation optimization method based on ST-EKF under beam failure conditions according to claim 3, characterized in that, In step 3.1, for the missing Doppler frequency shift information of a single beam, the Doppler frequency shift information of the missing beam can be reconstructed based on the structural characteristics of the DVL itself. The reconstructed Doppler frequency shift information is as follows: ； In the formula , This is represented as the Doppler frequency shift information of the beam adjacent to the faulty beam, and they are in a diagonal relationship. This refers to the beam Doppler frequency shift information relative to the faulty beam.

5. The SINS / DVL compact navigation optimization method based on ST-EKF under beam failure conditions according to claim 4, characterized in that, In step 3.2, two adjacent beams When the Doppler frequency shift information is missing, the projection of the celestial velocity calculated by SINS onto the carrier coordinate system is introduced, and then the Doppler frequency shift information of the missing beam is reconstructed according to the working principle of DVL. The reconstructed Doppler frequency shift information is as follows: ； In the formula The astronautical velocity calculated in the sky is The projection of the system.

6. The SINS / DVL compact navigation optimization method based on ST-EKF under beam failure conditions according to claim 5, characterized in that, In step 3.3, the reconstructed Doppler frequency shift information is as follows: ； ； In the formula The eastward velocity calculated by the inertial navigation system is The projection of the system.

7. The SINS / DVL compact navigation optimization method based on ST-EKF under beam failure conditions according to claim 6, characterized in that, In step 3.4, when beams 2# and 4# are missing, the projection of the eastward velocity calculated by SINS onto the carrier coordinate system is introduced, and then the Doppler frequency shift information of the missing beams is reconstructed according to the working principle of DVL. Beam 2# represents the second beam among the four beams of DVL; beam 4# represents the fourth beam among the four beams of DVL. When beams 1# and 3# are missing, the projection of the northward velocity calculated by SINS onto the carrier coordinate system is introduced, and then the Doppler frequency shift information of the missing beams is reconstructed according to the working principle of DVL. Beam 1# represents the first beam among the four beams of DVL; beam 3# represents the third beam among the four beams of DVL. The reconstructed Doppler frequency shift information is as follows: ； ； In the formula The northward velocity calculated by the inertial navigation system is The projection of the system.

8. The SINS / DVL compact navigation optimization method based on ST-EKF under beam failure conditions according to claim 7, characterized in that, In step 3.5, the reconstructed Doppler frequency shift information is as follows: ； 。 9. The SINS / DVL compact navigation optimization method based on ST-EKF under beam failure conditions according to claim 8, characterized in that, In step 3.6, if the missing beams are beams 1#, 2#, 4# or 2#, 3#, 4#, then the projections of the eastward and celestial velocities calculated by SINS onto the carrier coordinate system are introduced. Then, based on the working principle of DVL, the Doppler frequency shift information of the missing beams is reconstructed. If the missing beams are beams 1#, 2#, 3# or 1#, 3#, 4#, then the projections of the northward and celestial velocities calculated by SINS onto the carrier coordinate system are introduced. Then, based on the working principle of DVL, the Doppler frequency shift information of the missing beams is reconstructed. The reconstructed Doppler frequency shift information is as follows: ； 。 10. The SINS / DVL compact navigation optimization method based on ST-EKF under beam failure conditions according to claim 9, characterized in that, In step 4, based on the structural characteristics of the four-beam Janus array and using the Doppler frequency shift information of the four beams of the DVL array as a basis, an objective function is constructed, and the PSO algorithm is used to analyze the noise matrix. Q and R For real-time optimization, the objective function of the PSO algorithm is as follows: ； In the formula This represents the number of Kalman filters performed so far. , , , These are the estimated Doppler frequency shift values ​​for the four beams of the DVL.

Images