An Adaptive Temperature Control Confidence-Based Navigation Method for Polar Underwater Vehicles

Abstract

This invention discloses an adaptive temperature-controlled confidence-based integrated navigation method for polar underwater vehicles, belonging to the field of navigation and control technology. It is used for integrated navigation of polar underwater vehicles, including integrated navigation system initialization, mechanical calculation and state prediction of the strapdown inertial navigation system, temperature monitoring and power consumption control, parallel sub-filtering, confidence score calculation based on Doppler velocimeter data, and global state optimization estimation by the main filter. This invention, through a symmetrical layout of bidirectional Doppler velocimeters, can automatically switch to the ice layer reference surface when the seabed reference surface is missing, or perform optimal weighted fusion when both reference surfaces are available, significantly improving the system's robustness and long-endurance, high-precision operation capability under complex hydrogeological conditions. Through dynamic duty cycle adjustment and a pulsed heating strategy, it achieves synergistic optimization of stable sensor performance and low-power system operation.

Description

Technical Field

[0001] This invention discloses an adaptive temperature control confidence-based navigation method for polar underwater vehicles, belonging to the field of navigation and control technology. Background Technology

[0002] The current mainstream polar AUV navigation technology typically employs a combined architecture of a strapdown inertial navigation system (SINS) and a single downward-looking Doppler log (DVL). The SINS uses gyroscopes and accelerometers to calculate the vehicle's attitude, velocity, and position, offering advantages such as high-frequency output and independence from external signals; however, its errors accumulate over time. The downward-looking DVL, on the other hand, transmits sound waves towards the seabed and receives the echoes, measuring the AUV's velocity relative to the seabed through the Doppler effect's sound wave frequency shift, thus correcting for the accumulated errors of the SINS.

[0003] However, this traditional architecture has significant limitations in the complex polar environment:

[0004] First, when the AUV travels to deep waters where the depth exceeds the maximum range of DVL, or when the seabed topography is undulating and the seabed is soft, causing severe sound wave scattering, the downward-looking DVL will fail because it cannot lock onto the seabed. At this time, the navigation system degenerates into pure inertial navigation mode, and the position error will diverge rapidly (for example, the error can reach more than 2 meters within 10 minutes), forcing the mission to be aborted.

[0005] Secondly, although the bottom of the ice layer (ice-water interface) physically constitutes the only stable and continuous acoustic reflection surface in the deep water area, and has great potential as a velocity reference source, traditional systems only configure downward-looking DVL, ignoring the use of this "upward" reference source, resulting in a waste of navigation redundancy resources.

[0006] Finally, polar environments often have temperatures as low as -3°C or even lower. Low temperatures can cause a sharp drop in battery performance and a decline in the performance of DVL transducers and inertial devices (such as changes in sound speed and increased zero-bias drift). Existing thermal management strategies are often simplistic and crude (continuous heating consumes power, or no heating leads to a decrease in accuracy), lacking a mechanism that can synergistically optimize temperature, power consumption, and navigation accuracy. Summary of the Invention

[0007] The purpose of this invention is to provide an adaptive temperature-controlled confidence-based navigation method for polar underwater vehicles, in order to solve the problems in the prior art, such as the precipitous drop in navigation accuracy caused by the failure of a single reference surface, the waste of ice layer reference resources and the loss of navigation capability in deep water areas, the difficulty in balancing power consumption and navigation accuracy in low-temperature environments, and the model uncertainty and filter divergence in the fusion of multi-source heterogeneous data.

[0008] An adaptive temperature-controlled confidence-based navigation method for polar underwater vehicles includes:

[0009] S1. Initialize the integrated navigation system;

[0010] S2, the strapdown inertial navigation system performs data acquisition, speed updates, and position updates;

[0011] S3. Based on the feedback from the temperature sensor, control the frequency of the Doppler velocimeter and the power of the heating patch, and the Doppler velocimeter performs data acquisition; the Doppler velocimeter includes an upward-looking Doppler velocimeter and a downward-looking Doppler velocimeter;

[0012] S4. Perform sub-filter processing, including defining the state vector of the strapdown inertial navigation system, deriving the state transition equation of the sub-filter by combining the mechanical arrangement of the strapdown inertial navigation system, forming the observation equation using the nonlinear observation function of the Doppler velocimeter, and using the extended Kalman filter algorithm for time update and measurement update to obtain the state estimate of the Doppler velocimeter and the corresponding covariance matrix; the sub-filter includes the upward sub-filter and the downward sub-filter, and the upward sub-filter and the downward sub-filter are processed in parallel;

[0013] S5. Calculate the confidence score based on the Doppler velocimeter data, set the dynamic threshold, and output the availability status flag and normalized weight coefficient based on the confidence score result and the dynamic threshold.

[0014] S6. The main filter performs a global state-optimal estimation based on the output of the sub-filter, the availability status flag, and the normalized weight coefficients. This includes calculating the Doppler velocimeter fusion weights based on the availability status flags and the normalized weight coefficients, calculating the fusion error covariance matrix based on the Doppler velocimeter fusion weights and the covariance matrix, and calculating the global state-optimal estimation using the fusion error covariance matrix and the state estimation of the Doppler velocimeter.

[0015] S1 includes, after the integrated navigation system is powered on, performing initial position calibration using externally input absolute position information, initializing the state vectors of the strapdown inertial navigation system for the upper and lower sub-filters, initializing the covariance matrix of the Doppler velocimeter, and initializing the sliding window buffer.

[0016] S2 includes reading the angular velocity of the strapdown inertial navigation system gyroscope. Comparison data with accelerometer ,use Update the attitude matrix ;

[0017] By integrating the force ratio in the global navigation satellite system coordinate system and subtracting the gravity and Coriolis force terms, the carrier velocity is calculated. ;

[0018] right Integral calculation to determine the carrier's position , Latitude Longitude For height.

[0019] S3 includes acquiring temperature data of various components of the integrated navigation system using temperature sensors, and adjusting the frequency of the Doppler velocimeter and the power of the heating pad based on the temperature data:

[0020] ;

[0021] In the formula, for Duty cycle at any time For the sensor's real-time temperature, For optimal operating temperature, To the minimum working limit, This is the current remaining battery level. For low battery threshold, , For adjustment coefficients, To maintain the minimum duty cycle that prevents navigation divergence, To find the minimum value, To find the maximum value, As the first index, It is an upward-looking Doppler velocimeter. It is a downward-looking Doppler velocimeter;

[0022] The operating frequency of the sensor is:

[0023] ;

[0024] In the formula, This is the actual operating frequency of the sensor. This is the reference frequency.

[0025] S4 includes S4.1, if the Doppler velocimeter is updated, the upward-looking sub-filter receives the output of the upward-looking Doppler velocimeter, the strapdown inertial navigation system and the heating patch power, and the downward-looking sub-filter receives the output of the downward-looking Doppler velocimeter, the strapdown inertial navigation system, the temperature sensor and the heating patch power;

[0026] Define the state vector of a strapdown inertial navigation system :

[0027] ;

[0028] In the formula, For positional error, For speed error, For attitude error, To achieve zero bias in the gyroscope, To achieve zero bias in the accelerometer, It is the transpose symbol;

[0029] Based on the mechanical arrangement of the strapdown inertial navigation system, the state transition equations for the up-look sub-filter and the down-look sub-filter are constructed:

[0030] ;

[0031] In the formula, Sub-filter exist The state vector at time t, It is a nonlinear state transition function. This is process noise;

[0032] The comparison data output by the strapdown inertial navigation system is angular acceleration is Process noise covariance matrix for:

[0033] ;

[0034] In the formula, The effective duty cycle of the Doppler velocimeter. Based on the matrix, This refers to the amplification factor of inertial device errors caused by low temperature. This is the scaling factor. For temperature.

[0035] S4 includes S4.2, constructing the observation equation based on the nonlinear observation function of the Doppler velocimeter:

[0036] ;

[0037] In the formula, For filter exist The vector of observations at time t, For filter exist Nonlinear measurement function at time, For filter exist The new information of the moment;

[0038] S4 includes S4.3, where the extended Kalman filter, through linearization, performs time updates on the upper and lower view sub-filters, setting a sliding window with a window size of [value missing]. Construct a sequence of new information within the window:

[0039] ;

[0040] In the formula, For discrete-time indexes within a time window. , For filter exist The new information of the moment For filter exist The vector of observations at time t, To utilize The calculated predicted observations, State estimation;

[0041] Calculate the actual Doppler velocimeter error covariance matrix at the current time of the current window. :

[0042] ;

[0043] In the formula, The linearized observation matrix is ​​obtained through a first-order Taylor expansion. Let be the covariance matrix of the one-step prediction error.

[0044] S4 includes S4.4, based on Measurement updates are performed on the upper and lower sub-filters. The upper sub-filter outputs the upper Doppler velocimeter state estimate. and the corresponding error covariance matrix The output of the downward-looking sub-filter provides a state estimate for the downward-looking Doppler velocimeter. and the corresponding error covariance matrix .

[0045] S5 includes S5.1, which extracts temperature correction factors based on a multi-dimensional feature fusion algorithm. Signal-to-noise ratio Beam consistency Normalized echo intensity velocity mutation rate and number of effective beams Calculate real-time confidence scores from feature vectors including those included. :

[0046] ;

[0047] ;

[0048] ;

[0049] In the formula, , To replace the variable, , , , , The weighting coefficients for each feature dimension satisfy... , , For filter signal-to-noise ratio, This represents the saturation value of the signal-to-noise ratio. For filter Beam consistency, For filter Normalized echo intensity, For filter The number of effective beams, for The calculated value of the velocity at time step. For the maximum design speed of the underwater vehicle, The sampling interval;

[0050] The structure is as follows:

[0051] ;

[0052] In the formula, To correspond to the real-time temperature of the Doppler velocimeter's temperature probe, For optimal operating temperature, To the minimum working limit, This is the temperature sensitivity coefficient.

[0053] S5 includes, S5.2, and comparison. and dynamic threshold If a comparison is made, Output availability status flags , And generate normalized weight coefficients. , ;

[0054] like Output , .

[0055] S6 includes the main filter first acquiring the current time... and Perform information allocation coefficient normalization calculation:

[0056] ;

[0057] In the formula, Doppler velocimeters The fusion weight, For Doppler velocimeters index;

[0058] Calculate the fusion error covariance matrix :

[0059] ;

[0060] In the formula, for The inverse matrix;

[0061] The optimal estimate is calculated using the estimates of the up-view sub-filter and the down-view sub-filter:

[0062] ;

[0063] In the formula, This is the optimal estimate of the global state. For the first State estimation of individual sub-filters.

[0064] Compared with existing technologies, the present invention has the following advantages: Through a bidirectional DVL symmetrical layout, the present invention can automatically switch to the ice layer reference surface when the seabed reference surface is missing, or perform optimal weighted fusion when both reference surfaces are available, which significantly improves the robustness of the system under complex hydrogeological conditions and its long-endurance high-precision operation capability; through the upward-looking DVL configuration and confidence evaluation mechanism, the bottom of the ice layer is included in the velocity reference source, realizing the coordination of "upward ice measurement" and "downward bottom measurement", effectively utilizing the navigation redundancy resources unique to the polar sub-ice environment, and avoiding the risk of missions being forced to stop in deep water areas; By employing dynamic duty cycle adjustment and pulsed heating strategies, the heating intensity is adaptively adjusted based on ambient temperature and remaining battery power. Temperature drift compensation is incorporated into the DVL confidence score, achieving synergistic optimization of stable sensor performance and low-power system operation, thus extending the AUV's endurance in polar regions. Through a federated filtering architecture and multi-dimensional confidence assessment, real-time features such as temperature, signal quality, and data consistency are incorporated into the information allocation mechanism. The weights of each sub-filter are adaptively adjusted, effectively suppressing data jumps and filter divergence during reference plane switching, achieving smooth, high-precision, seamless navigation. Attached Figure Description

[0065] Figure 1 This is a structural diagram of the integrated navigation system of the present invention;

[0066] Figure 2 This is a schematic diagram of the modular architecture of the integrated navigation system of the present invention;

[0067] Figure 3 This is a flowchart of the operation of the integrated navigation system of the present invention;

[0068] Figure 4This is a comparison chart of the fusion trajectory using traditional methods and the GPS positioning trajectory;

[0069] Figure 5 This is a comparison diagram of the fusion trajectory and GPS positioning trajectory of the method of the present invention;

[0070] Figure 6 yes Figure 5 Enlarged view of region A in the middle;

[0071] In the diagram, 1-underwater vehicle frame; 2-upward DVL module; 3-downward DVL module; 4-electronics bay; 5-thrust assembly; 6-forward illuminator; 7-battery bay. Detailed Implementation

[0072] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention are described clearly and completely below. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0073] An adaptive temperature-controlled confidence-based navigation method for polar underwater vehicles includes:

[0074] S1. Initialize the integrated navigation system;

[0075] S2, the strapdown inertial navigation system performs data acquisition, speed updates, and position updates;

[0076] S3. Based on the feedback from the temperature sensor, control the frequency of the Doppler velocimeter and the power of the heating patch, and the Doppler velocimeter performs data acquisition; the Doppler velocimeter includes an upward-looking Doppler velocimeter and a downward-looking Doppler velocimeter;

[0077] S4. Perform sub-filter processing, including defining the state vector of the strapdown inertial navigation system, deriving the state transition equation of the sub-filter by combining the mechanical arrangement of the strapdown inertial navigation system, forming the observation equation using the nonlinear observation function of the Doppler velocimeter, and using the extended Kalman filter algorithm for time update and measurement update to obtain the state estimate of the Doppler velocimeter and the corresponding covariance matrix; the sub-filter includes the upward sub-filter and the downward sub-filter, and the upward sub-filter and the downward sub-filter are processed in parallel;

[0078] S5. Calculate the confidence score based on the Doppler velocimeter data, set the dynamic threshold, and output the availability status flag and normalized weight coefficient based on the confidence score result and the dynamic threshold.

[0079] S6. The main filter performs a global state-optimal estimation based on the output of the sub-filter, the availability status flag, and the normalized weight coefficients. This includes calculating the Doppler velocimeter fusion weights based on the availability status flags and the normalized weight coefficients, calculating the fusion error covariance matrix based on the Doppler velocimeter fusion weights and the covariance matrix, and calculating the global state-optimal estimation using the fusion error covariance matrix and the state estimation of the Doppler velocimeter.

[0080] S1 includes, after the integrated navigation system is powered on, performing initial position calibration using externally input absolute position information, initializing the state vectors of the strapdown inertial navigation system for the upper and lower sub-filters, initializing the covariance matrix of the Doppler velocimeter, and initializing the sliding window buffer.

[0081] S2 includes reading the angular velocity of the strapdown inertial navigation system gyroscope. Comparison data with accelerometer ,use Update the attitude matrix ;

[0082] By integrating the force ratio in the global navigation satellite system coordinate system and subtracting the gravity and Coriolis force terms, the carrier velocity is calculated. ;

[0083] right Integral calculation to determine the carrier's position , Latitude Longitude For height.

[0084] S3 includes acquiring temperature data of various components of the integrated navigation system using temperature sensors, and adjusting the frequency of the Doppler velocimeter and the power of the heating pad based on the temperature data:

[0085] ;

[0086] In the formula, for Duty cycle at any time For the sensor's real-time temperature, For optimal operating temperature, To the minimum working limit, This is the current remaining battery level. For low battery threshold, , For adjustment coefficients, To maintain the minimum duty cycle that prevents navigation divergence, To find the minimum value, To find the maximum value, As the first index, It is an upward-looking Doppler velocimeter. It is a downward-looking Doppler velocimeter;

[0087] The operating frequency of the sensor is:

[0088] ;

[0089] In the formula, This is the actual operating frequency of the sensor. This is the reference frequency.

[0090] S4 includes S4.1, if the Doppler velocimeter is updated, the upward-looking sub-filter receives the output of the upward-looking Doppler velocimeter, the strapdown inertial navigation system and the heating patch power, and the downward-looking sub-filter receives the output of the downward-looking Doppler velocimeter, the strapdown inertial navigation system, the temperature sensor and the heating patch power;

[0091] Define the state vector of a strapdown inertial navigation system :

[0092] ;

[0093] In the formula, For positional error, For speed error, For attitude error, To achieve zero bias in the gyroscope, To achieve zero bias in the accelerometer, It is the transpose symbol;

[0094] Based on the mechanical arrangement of the strapdown inertial navigation system, the state transition equations for the up-look sub-filter and the down-look sub-filter are constructed:

[0095] ;

[0096] In the formula, Sub-filter exist The state vector at time t, It is a nonlinear state transition function. This is process noise;

[0097] The comparison data output by the strapdown inertial navigation system is angular acceleration is Process noise covariance matrix for:

[0098] ;

[0099] In the formula, The effective duty cycle of the Doppler velocimeter. Based on the matrix, This refers to the amplification factor of inertial device errors caused by low temperature. This is the scaling factor. For temperature.

[0100] S4 includes S4.2, constructing the observation equation based on the nonlinear observation function of the Doppler velocimeter:

[0101] ;

[0102] In the formula, For filter exist The vector of observations at time t, For filter exist Nonlinear measurement function at time, For filter exist The new information of the moment;

[0103] S4 includes S4.3, where the extended Kalman filter, through linearization, performs time updates on the upper and lower view sub-filters, setting a sliding window with a window size of [value missing]. Construct a sequence of new information within the window:

[0104] ;

[0105] In the formula, For discrete-time indexes within a time window. , For filter exist The new information of the moment For filter exist The vector of observations at time t, To utilize The calculated predicted observations, State estimation;

[0106] Calculate the actual Doppler velocimeter error covariance matrix at the current time of the current window. :

[0107] ;

[0108] In the formula, The linearized observation matrix is ​​obtained through a first-order Taylor expansion. Let be the covariance matrix of the one-step prediction error.

[0109] S4 includes S4.4, based on Measurement updates are performed on the upper and lower sub-filters. The upper sub-filter outputs the upper Doppler velocimeter state estimate. and the corresponding error covariance matrix The output of the downward-looking sub-filter provides a state estimate for the downward-looking Doppler velocimeter. and the corresponding error covariance matrix .

[0110] S5 includes S5.1, which extracts temperature correction factors based on a multi-dimensional feature fusion algorithm. Signal-to-noise ratio Beam consistency Normalized echo intensity velocity mutation rate and number of effective beams Calculate real-time confidence scores from feature vectors including those included. :

[0111] ;

[0112] ;

[0113] ;

[0114] In the formula, , To replace the variable, , , , , The weighting coefficients for each feature dimension satisfy... , , For filter signal-to-noise ratio, This represents the saturation value of the signal-to-noise ratio. For filter Beam consistency, For filter Normalized echo intensity, For filter The number of effective beams, for The calculated value of the velocity at time step. For the maximum design speed of the underwater vehicle, The sampling interval;

[0115] The structure is as follows:

[0116] ;

[0117] In the formula, To correspond to the real-time temperature of the Doppler velocimeter's temperature probe, For optimal operating temperature, To the minimum working limit, This is the temperature sensitivity coefficient.

[0118] S5 includes, S5.2, and comparison. and dynamic threshold If a comparison is made, Output availability status flags , And generate normalized weight coefficients. , ;

[0119] like Output , .

[0120] S6 includes the main filter first acquiring the current time... and Perform information allocation coefficient normalization calculation:

[0121] ;

[0122] In the formula, Doppler velocimeters The fusion weight, For Doppler velocimeters index;

[0123] Calculate the fusion error covariance matrix :

[0124] ;

[0125] In the formula, for The inverse matrix;

[0126] The optimal estimate is calculated using the estimates of the up-view sub-filter and the down-view sub-filter:

[0127] ;

[0128] In the formula, This is the optimal estimate of the global state. For the first State estimation of individual sub-filters.

[0129] The sliding window buffer of the present invention is a fixed-length data storage structure used to temporarily store the innovation vector in sliding window adaptive filtering.

[0130] The following description, in conjunction with the accompanying drawings, further illustrates the hardware structure of the integrated navigation system of the present invention. Figure 1 As shown, the system comprises multiple functionally independent modular units, integrated into one unit via the underwater vehicle frame 1 and connecting cables. The core reference unit is a strapdown inertial navigation system (SINS) module, which integrates a three-axis fiber optic gyroscope and a three-axis quartz accelerometer. This module is fixedly installed at the geometric center of gravity of the vehicle, serving as the physical origin for navigation calculations and ensuring the reference stability of attitude and acceleration measurements. It is integrated within the electronics compartment 4. The velocity sensing unit consists of two independent Doppler velocimeters (DVLs), responsible for "downward bottom measurement" and "upward ice measurement" functions, respectively. The downward DVL module 3 is installed at the bottom of the vehicle with its transducer array facing downward, used to transmit sound waves to the seabed and receive echoes. The upward DVL module 2 is symmetrically installed at the top of the vehicle with its transducer array plane facing upward, used to transmit sound waves to the lower surface of the ice layer. The hardware system integrates distributed temperature sensing and heating actuator components. High-precision temperature sensors are installed near each sensor module (including SINS, upper-view DVL, and lower-view DVL) and controller, among other key components, to collect local temperature data in real time. Simultaneously, heating elements are tightly attached to the exterior of each module, and these heating elements are connected to control switches via independent circuit loops. The central navigation computer, located within the electronic compartment 4, serves as the data processing and control center. It communicates with each sensor module via a data bus and directly drives the heating switches of each module through control lines. The hardware circuitry includes a high-precision clock synchronization module to ensure that the data acquisition from the SINS and the two sets of DVLs remains strictly synchronized within microseconds, reducing dynamic measurement errors caused by time asynchrony. Other components include the thruster group 5, which is arranged symmetrically along three axes (the visible parts are marked in the diagram); the forward illumination unit 6, which provides supplemental lighting for the imaging instruments in the electronic compartment 4; and the battery compartments 7, arranged symmetrically on both sides.

[0131] The modular architecture of the integrated navigation system of this invention is as follows: Figure 2 As shown, it is divided into a hardware layer and a software layer. The hardware layer includes the top-view DVL, temperature and power management module, bottom-view DVL, and SINS. The inputs are respectively to the upward-looking DVL, the temperature and power consumption management module, and the downward-looking DVL. The upward-looking DVL outputs the upward-looking news sequence. Temperature and power management output to the two sub-filters respectively. The downward-looking DVL outputs the downward-looking news sequence. SINS output and The software layer includes a sub-filter (upward view), a sub-filter (downward view), an upward view DVL confidence scoring module, a downward view DVL confidence scoring module, and a main filter; the sub-filter (upward view) receives... and Output Sub-filter (look-down) reception and Output The DVL confidence scoring module of the Shanghai TV channel receives... Confidence score output based on the parameters of the DVL signal. The downward-looking DVL confidence scoring module receives... Confidence score output based on the parameters of the downward-looking DVL signal. The main filter receives , , and The system is fused to output the optimal estimate of the navigation information.

[0132] The operation flow of the integrated navigation system of this invention is as follows: Figure 3 As shown, after the integrated navigation system is initialized, it performs SINS mechanics calculation and state prediction, then temperature monitoring and power consumption control, and determines whether the DVL (Dynamic Value List) has been updated. If not updated, it returns to the SINS mechanics calculation and state prediction step. If updated, it performs parallel sub-filter processing, then DVL confidence scoring, and finally federated filtering fusion. It then determines whether navigation has ended. If not ended, it saves the navigation log and log data and returns to the SINS mechanics calculation and state prediction step to continue navigation. If ended, it stops navigation.

[0133] This invention provides an embodiment of setting the sliding window size. The dynamic threshold is 20. The value can be:

[0134] ;

[0135] Two minutes of data from the dataset were extracted and processed, including GPS positioning data, IMU measurement data, upward-looking DVL measurement data, and downward-looking DVL measurement data. The IMU data was measured at a frequency of 200Hz, while the other sensors measured at a frequency of 1Hz. The IMU and downward-looking DVL measurement data were used as the data source for the traditional EKF algorithm. After fusion using the traditional EKF algorithm, the fused trajectory was compared with the GPS positioning trajectory, as shown in the image below. Figure 4 As shown in the figure, the fused trajectory still has a certain error relative to the GPS reference data. Using IMU and upper and lower DVL measurement data as the data source for the algorithm of this invention, after fusion by the above federated algorithm, the comparison figure between the fused trajectory and the GPS positioning trajectory is shown in the figure. Figure 5 and Figure 6 As shown, where Figure 6 for Figure 5The magnified view of region A, i.e. the magnified image of the trajectory calculated by the two DVLs, shows that there is a certain difference between the measurement data of the two DVLs. After the two-way DVL information and IMU information are fused, with GPS data as a reference, the fused trajectory has a smaller error than the traditional EKF method, proving that the method is effective.

[0136] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for adaptive temperature control confidence-based navigation for polar underwater vehicles, characterized in that, include: S1. Initialize the integrated navigation system; S2, the strapdown inertial navigation system performs data acquisition, speed updates, and position updates; S3. Based on the feedback from the temperature sensor, control the frequency of the Doppler velocimeter and the power of the heating patch, and the Doppler velocimeter performs data acquisition; the Doppler velocimeter includes an upward-looking Doppler velocimeter and a downward-looking Doppler velocimeter; S4. Perform sub-filter processing, including defining the state vector of the strapdown inertial navigation system, deriving the state transition equation of the sub-filter by combining the mechanical arrangement of the strapdown inertial navigation system, forming the observation equation using the nonlinear observation function of the Doppler velocimeter, and using the extended Kalman filter algorithm for time update and measurement update to obtain the state estimate of the Doppler velocimeter and the corresponding covariance matrix; the sub-filter includes the upward sub-filter and the downward sub-filter, and the upward sub-filter and the downward sub-filter are processed in parallel; S5. Calculate the confidence score based on the Doppler velocimeter data, set the dynamic threshold, and output the availability status flag and normalized weight coefficient based on the confidence score result and the dynamic threshold. S6. The main filter performs a global state-optimal estimation based on the output of the sub-filter, the availability status flag, and the normalized weight coefficients. This includes calculating the Doppler velocimeter fusion weights based on the availability status flags and the normalized weight coefficients, calculating the fusion error covariance matrix based on the Doppler velocimeter fusion weights and the covariance matrix, and calculating the global state-optimal estimation using the fusion error covariance matrix and the state estimation of the Doppler velocimeter.

2. The adaptive temperature control confidence-based navigation method for polar underwater vehicles according to claim 1, characterized in that, S1 includes, after the integrated navigation system is powered on, performing initial position calibration using externally input absolute position information, initializing the state vectors of the strapdown inertial navigation system for the upper and lower sub-filters, initializing the covariance matrix of the Doppler velocimeter, and initializing the sliding window buffer.

3. The adaptive temperature control confidence-based navigation method for polar underwater vehicles according to claim 2, characterized in that, S2 includes reading the angular velocity of the strapdown inertial navigation system gyroscope. Comparison data with accelerometer ,use Update the attitude matrix ; By integrating the force ratio in the global navigation satellite system coordinate system and subtracting the gravity and Coriolis force terms, the carrier velocity is calculated. ; right Integral calculation to determine the carrier's position , Latitude Longitude For height.

4. The adaptive temperature control confidence-based navigation method for polar underwater vehicles according to claim 3, characterized in that, S3 includes acquiring temperature data of various components of the integrated navigation system using temperature sensors, and adjusting the frequency of the Doppler velocimeter and the power of the heating pads based on the temperature data: ； In the formula, for Duty cycle at any time For the sensor's real-time temperature, For optimal operating temperature, To the minimum working limit, This is the current remaining battery level. For low battery threshold, , For adjustment coefficients, To maintain the minimum duty cycle that prevents navigation divergence, To find the minimum value, To find the maximum value, As the first index, It is an upward-looking Doppler velocimeter. It is a downward-looking Doppler velocimeter; The operating frequency of the sensor is: ； In the formula, This is the actual operating frequency of the sensor. This is the reference frequency.

5. The adaptive temperature control confidence-based navigation method for polar underwater vehicles according to claim 4, characterized in that, S4 includes S4.1, if the Doppler velocimeter is updated, the upward-looking sub-filter receives the output of the upward-looking Doppler velocimeter, the strapdown inertial navigation system and the heating patch power, and the downward-looking sub-filter receives the output of the downward-looking Doppler velocimeter, the strapdown inertial navigation system, the temperature sensor and the heating patch power; Define the state vector of a strapdown inertial navigation system : ； In the formula, For positional error, For speed error, For attitude error, To achieve zero bias in the gyroscope, To achieve zero bias in the accelerometer, It is the transpose symbol; Based on the mechanical arrangement of the strapdown inertial navigation system, the state transition equations for the up-look sub-filter and the down-look sub-filter are constructed: ； In the formula, Sub-filter exist The state vector at time t, It is a nonlinear state transition function. This is process noise; The comparison data output by the strapdown inertial navigation system is angular acceleration is Process noise covariance matrix for: ； In the formula, The effective duty cycle of the Doppler velocimeter. Based on the matrix, This refers to the amplification factor of inertial device errors caused by low temperature. This is the scaling factor. For temperature.

6. The adaptive temperature control confidence-based navigation method for polar underwater vehicles according to claim 5, characterized in that, S4 includes S4.2, constructing the observation equation based on the nonlinear observation function of the Doppler velocimeter: ； In the formula, For filter exist The vector of observations at time t, For filter exist Nonlinear measurement function at time, For filter exist The latest news; S4 includes S4.3, where the extended Kalman filter, through linearization, performs time updates on the upper and lower view sub-filters, setting a sliding window with a window size of [value missing]. Construct a sequence of new information within the window: ； In the formula, For discrete-time indexes within a time window. , For filter exist The new information of the moment For filter exist The vector of observations at time t, To utilize The calculated predicted observations, State estimation; Calculate the actual Doppler velocimeter error covariance matrix at the current time of the current window. : ； In the formula, The linearized observation matrix is ​​obtained through a first-order Taylor expansion. Let be the covariance matrix of the one-step prediction error.

7. The adaptive temperature control confidence-based navigation method for polar underwater vehicles according to claim 6, characterized in that, S4 includes S4.4, based on Measurement updates are performed on the upper and lower sub-filters. The upper sub-filter outputs the upper Doppler velocimeter state estimate. and the corresponding error covariance matrix The output of the downward-looking sub-filter provides a state estimate for the downward-looking Doppler velocimeter. and the corresponding error covariance matrix .

8. The adaptive temperature control confidence-based navigation method for polar underwater vehicles according to claim 7, characterized in that, S5 includes S5.1, which extracts temperature correction factors based on a multi-dimensional feature fusion algorithm. Signal-to-noise ratio Beam consistency Normalized echo intensity velocity mutation rate and number of effective beams Calculate real-time confidence scores from feature vectors including those included. : ； ； ； In the formula, , To replace the variable, , , , , The weighting coefficients for each feature dimension satisfy... , , For filter signal-to-noise ratio, This represents the saturation value of the signal-to-noise ratio. For filter Beam consistency, For filter Normalized echo intensity, For filter The number of effective beams, for The calculated value of the velocity at time step. For the maximum design speed of the underwater vehicle, The sampling interval; The structure is as follows: ； In the formula, To correspond to the real-time temperature of the Doppler velocimeter's temperature probe, For optimal operating temperature, To the minimum working limit, This is the temperature sensitivity coefficient.

9. The adaptive temperature control confidence-based navigation method for polar underwater vehicles according to claim 8, characterized in that, S5 includes, S5.2, and comparison. and dynamic threshold If a comparison is made, Output availability status flags , And generate normalized weight coefficients. , ; like Output , .

10. The adaptive temperature control confidence-based navigation method for polar underwater vehicles according to claim 9, characterized in that, S6 includes the main filter first acquiring the current time... and Perform information allocation coefficient normalization calculation: ； In the formula, Doppler velocimeters The fusion weight, For Doppler velocimeters index; Calculate the fusion error covariance matrix : ； In the formula, for The inverse matrix; The optimal estimate is calculated using the estimates of the up-view sub-filter and the down-view sub-filter: ； In the formula, This is the optimal estimate of the global state. For the first State estimation of individual sub-filters.

Images