Multi-source Data Fusion Method for Submersible Integrated Navigation Based on Elastic Random Model

Abstract

The present invention relates to the field of underwater intelligent navigation and integrated navigation technology, and particularly relates to a multi-source data fusion method for the integrated navigation of a submersible based on an elastic stochastic model, including: preprocessing multi-source navigation data output by an inertial navigation system (INS), a Doppler velocity log, an ultra-short baseline / long baseline, an altimeter, and a depth gauge; constructing a state space model including navigation parameter errors and sensor errors, and screening the optimal sensor combination method; constructing an elastic correction parameter model in combination with an elastic PNT framework, dynamically adjusting the state and observation noise covariance matrices, and using multiple parallelly set elastic adaptive robust cubature Kalman filters (CKFs) to perform local filtering on the multi-source data, outputting a fusion solution result, and achieving an optimal estimation of the global navigation state through a dynamic information distribution strategy. The present invention has strong anti-interference ability, adaptability, and combination switching robustness, and is applicable to high-precision submersible navigation and positioning tasks in complex underwater environments.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater intelligent navigation and integrated navigation, and particularly to a multi-source data fusion method for the integrated navigation of a submersible based on an elastic stochastic model. Background Art

[0002] In the fields of deep-sea resource development, underwater survey, and autonomous operation of intelligent submersibles, submersibles have increasingly stringent requirements for high-precision and autonomous navigation and positioning capabilities. The underwater environment usually relies on an inertial navigation system (INS) and a variety of acoustic, ranging, and attitude-aiding sensors (such as a Doppler velocity log DVL, ultra-short / long baseline USBL / LBL, altimeter, and depth gauge, etc.) to construct an integrated navigation system to obtain the real-time position, velocity, and attitude information of the submersible. The introduction of multi-source heterogeneous sensors has improved the system's observation ability and positioning reliability, but it has also brought complex challenges such as inconsistent sampling frequencies of multi-source data, different spatio-temporal reference systems, and uneven data quality, significantly affecting the stability and accuracy of the fusion solution algorithm.

[0003] Existing technologies still have significant technical bottlenecks when facing complex working conditions such as sudden sensor failures, abnormal observations, and dynamic switching of integration strategies in the marine environment. Specifically, it is manifested in the following aspects: First, traditional multi-source data fusion methods mostly adopt static or fixed-structure state-space models, lacking an effective discrimination mechanism for the observability of error parameters under different integration configurations, which easily leads to unobservable states or error divergence; Second, most filtering algorithms only adjust the covariance matrix based on prior statistics and are difficult to respond to sensor degradation or environmental changes in real time, resulting in insufficient system robustness; Third, information allocation strategies generally rely on static weighting and lack dynamic, elastic, and fault-resistant allocation mechanisms, causing the fusion solution to be sensitive to abnormal data or abnormal filters and affecting the global navigation estimation accuracy. Summary of the Invention

[0004] Based on the above objectives, the present invention provides a multi-source data fusion method for the integrated navigation of a submersible based on an elastic stochastic model, a multi-source integrated navigation data fusion method facing dynamic integration strategies, with elastic regulation capabilities and a robust adaptive mechanism, to improve the navigation stability, accuracy, and reliability of the submersible under complex mission conditions.

[0005] The multi-source data fusion method for the integrated navigation of a submersible based on an elastic stochastic model includes the following steps:

[0006] S1: Perform spatio-temporal registration and gross error rejection preprocessing on multi-source navigation data output by an inertial navigation system INS, a Doppler velocity log DVL, an ultra-short baseline USBL / long baseline LBL, an altimeter, and a depth gauge;

[0007] S2: Based on the preprocessed multi-source navigation data, establish a state space model including navigation parameter errors and sensor errors, use the observability analysis method based on the Piecewise Constant System (PWCS) theory to determine the observability degrees of each error parameter, and screen and determine the optimal sensor combination method according to the observability degree threshold;

[0008] S3: Combine the elastic PNT framework to construct an error elastic stochastic model, and dynamically generate elastic correction parameters according to the determined optimal sensor combination method, where the elastic correction parameters are used to adjust the state noise covariance matrix and the observation noise covariance matrix;

[0009] S4: Based on the generated elastic correction parameters, construct multiple parallel elastic adaptive robust cubature Kalman filter (CKF) algorithms to perform local filtering on the multi-source navigation data, and output the real-time fusion solution results of each local filter;

[0010] S5: Based on the federated filter architecture, perform elastic information allocation on the real-time fusion solution results of each local filter output, and complete the global optimal estimation by dynamically adjusting the information allocation coefficient.

[0011] Optionally, S1 includes:

[0012] S11: Perform timestamp synchronization processing on the first navigation data output by the Inertial Navigation System (INS), the second navigation data output by the Doppler Velocity Log (DVL), the third navigation data output by the Ultra-Short Baseline (USBL) / Long Baseline (LBL), the fourth navigation data output by the altimeter, and the fifth navigation data output by the depth gauge, and generate multi-source navigation data after timestamp synchronization;

[0013] S12: Perform spatial coordinate system conversion processing on the multi-source navigation data after timestamp synchronization, and generate multi-source navigation data with unified spatial coordinates;

[0014] S13: Perform gross error detection and marking processing on the multi-source navigation data with unified spatial coordinates, and generate multi-source navigation data with outlier markings;

[0015] S14: Perform data repair processing on the multi-source navigation data with outlier markings, and generate preprocessed multi-source navigation data;

[0016] S15: Output the preprocessed multi-source navigation data to S2 as the input data for establishing the state space model.

[0017] Optionally, S2 includes:

[0018] S21: Based on the preprocessed multi-source navigation data, construct a state space model including navigation parameter errors and sensor errors;

[0019] S22: Perform piecewise constant system (PWCS) observability analysis on the state space model to generate an observability matrix for each error parameter;

[0020] S23: Based on the observability matrix, perform observability threshold screening on the error parameters in the state space model to generate a candidate set of sensor combination methods;

[0021] S24: Based on the candidate set of sensor combination methods, select the combination with the maximum weighted sum of observability as the optimal sensor combination method, which specifically includes:

[0022] Weighted sum calculation: For each sensor combination in the candidate set, calculate the sum of the observability of its corresponding error parameters and attach the sensor reliability weight;

[0023] Optimal determination: Select the sensor combination with the maximum weighted sum as the optimal sensor combination method and output the reduced state space model corresponding to this combination;

[0024] S25: Output the optimal sensor combination method and the corresponding reduced state space model to S3 for constructing an elastic stochastic model.

[0025] Optionally, the observability threshold screening in S23 is specifically:

[0026] Parameter screening rule: If the average observability of a certain error parameter in N consecutive segments is lower than the preset threshold of 0.1, it is determined that this parameter is unobservable and it is removed from the state vector;

[0027] Sensor combination generation: According to the sensor types corresponding to the remaining error parameters, generate a candidate set of sensor combination methods including at least two combinations of inertial navigation system (INS), Doppler velocity log (DVL), ultra-short baseline (USBL) / long baseline (LBL), altimeter, and depth gauge.

[0028] Optionally, S3 includes:

[0029] S31: Based on the optimal sensor combination method and the reduced state space model, initialize the elastic correction parameters in combination with the elastic PNT framework to generate an initial set of elastic correction parameters;

[0030] S32: Update the set of elastic correction parameters according to the real-time health status of the optimal sensor combination method to generate a dynamically adjusted set of elastic correction parameters;

[0031] The real-time health status is jointly determined by the sensor health status alarm signal in S14 and the observability threshold screening result in S23;

[0032] S33: Based on the preprocessed multi-source navigation data, perform online covariance estimation on the dynamically adjusted elastic correction parameter set to generate an optimized elastic correction parameter set.

[0033] Optionally, S3 further includes:

[0034] S34: According to the optimized elastic correction parameter set, adjust the state noise covariance matrix and the observation noise covariance matrix to generate an elasticized state noise covariance matrix and an observation noise covariance matrix;

[0035] S35: Output the elasticized state noise covariance matrix, the observation noise covariance matrix, and the optimized elastic correction parameter set to S4.

[0036] Optionally, S4 includes:

[0037] S41: According to the optimal sensor combination method and the elasticized state noise covariance matrix and the observation noise covariance matrix, initialize multiple parallel elastic adaptive robust cubature Kalman filter (CKF) algorithms to generate a local filter bank;

[0038] S42: Configure the robust parameters for each elastic adaptive robust cubature Kalman filter (CKF) algorithm in the local filter bank to generate a robustified local filter bank;

[0039] S43: Input the preprocessed multi-source navigation data into the robustified local filter bank, perform parallel filtering and solution to generate the real-time fusion solution results of each local filter.

[0040] Optionally, S4 further includes:

[0041] S44: Perform state consistency verification and filter health monitoring on the real-time fusion solution results of each local filter to generate the real-time fusion solution results with health status markers;

[0042] S45: Output the real-time fusion solution results with health status markers to S5 as the input data for federated filtering elastic information allocation.

[0043] Optionally, S5 includes:

[0044] S51: Receive the real-time fusion solution results with health status markers output by S4, extract the state estimation values and covariance matrices of each local filter to generate a federated filtering input data set;

[0045] S52: Based on the federated filtering input data set, calculate the elastic reset of the information allocation coefficients of each local filter to generate a failure-adapted information allocation coefficient set;

[0046] S53: For a local filter that triggers an alarm for the health status of the sensor or an alarm for covariance exceeding the limit, perform an elastic reset of the information distribution coefficient to generate a set of information distribution coefficients adapted to the fault.

[0047] S54: Based on the set of information distribution coefficients adapted to the fault, calculate the global optimal estimate through the federated filtering fusion equation to generate the global navigation state of the submersible.

[0048] S55: Perform divergence detection and integrity protection on the global navigation state of the submersible to generate the final navigation output data.

[0049] Advantages of the present invention:

[0050] In the present invention, through preprocessing of multi-source navigation data such as an inertial navigation system INS, a Doppler velocity log DVL, an ultra-short baseline USBL / long baseline LBL, an altimeter, and a depth gauge, including timestamp synchronization, spatial coordinate transformation, gross error elimination, and data repair, a unified high-quality data stream input basis is constructed, effectively overcoming the problems of inconsistent sampling frequencies of different sensors, non-unified reference systems, and interference from measurement outliers. On this basis, a state space model including navigation errors and sensor errors is established, and a piecewise constant system PWCS observability analysis method is introduced to screen error parameters and optimize the best sensor combination, ensuring the convergence and practicality of the model, thereby significantly improving the adaptive fusion ability and usability of the multi-source integrated navigation system.

[0051] In the present invention, combined with the elastic PNT framework, an elastic correction model of the state noise and observation noise covariance matrix is dynamically constructed. Using the feedback of the sensor health status and observability, a multi-dimensional and multi-granularity set of elastic correction parameters is constructed and continuously optimized and adjusted based on the sliding window covariance estimation. By constructing an upper and lower limit constraint mechanism, the covariance correction amplitude is effectively limited to avoid model divergence. In addition, when the local filter is abnormal, elastic weight reallocation is automatically performed, significantly improving the system's adaptability to single-sensor faults, drifts, or observation degradations, and realizing system-level robust navigation solution.

[0052] In the present invention, a multi-instance parallel elastic adaptive robust cubature Kalman filter CKF algorithm is designed. Combining the Huber robust function to suppress the influence of large residuals, a local filter bank is constructed to realize sub-combination solution, and a state consistency test and covariance divergence monitoring mechanism are integrated to effectively identify and eliminate unreliable filter solution results. Further, in the federated filtering architecture, a dynamic information distribution coefficient and a fault-adaptive elastic reset mechanism are introduced, and comprehensive weighting is performed based on multiple indicators such as state estimation, covariance, and observability, and finally the global optimal navigation solution result is output. This architecture can maintain the consistency and optimality of state fusion, and has good scalability, real-time performance, and engineering adaptability. Description of the Drawings

[0053] To more clearly illustrate the technical solutions in the present invention or the prior art, the following will briefly introduce the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings in the following description are only those of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can also be obtained based on these drawings.

[0054] Figure 1 It is a schematic flowchart of the method according to the embodiment of the present invention;

[0055] Figure 2 It is a schematic flowchart of the S2 process according to the embodiment of the present invention. Detailed implementation manners

[0056] The present invention will be described in detail below in conjunction with the drawings and specific embodiments. At the same time, it should be noted here that in order to make the embodiments more detailed, the following embodiments are the best and preferred embodiments. For some well-known technologies, those skilled in the art can also adopt other alternative ways for implementation; moreover, the drawings are only for more specifically describing the embodiments, and are not intended to specifically limit the present invention.

[0057] As Figure 1-2 shown, the multi-source data fusion method for the combined navigation of a submersible based on an elastic random model includes the following steps:

[0058] S1: Perform spatio-temporal registration and gross error rejection preprocessing on the multi-source navigation data output by an inertial navigation system INS (Inertial Navigation System), a Doppler velocity log DVL (Doppler Velocity Log), an ultra-short baseline USBL (Ultra Short Base Line) / long baseline LBL (Long Base Line), an altimeter, and a depth gauge;

[0059] S2: Based on the preprocessed multi-source navigation data, establish a state space model including navigation parameter errors and sensor errors, use an observability analysis method based on the piecewise constant system PWCS theory to determine the observability of each error parameter, and screen and determine the optimal sensor combination method according to the observability threshold;

[0060] S3: Combine the elastic PNT (Positioning, Navigation, Timing) framework to construct an error elastic random model, and dynamically generate elastic correction parameters according to the determined optimal sensor combination method. The elastic correction parameters are used to adjust the state noise covariance matrix Q and the observation noise covariance matrix R;

[0061] S4: Based on the generated elastic correction parameters, construct multiple parallel elastic adaptive robust cubature Kalman filter (CKF) algorithms to perform local filtering on multi-source navigation data and output the real-time fusion solution results of each local filter;

[0062] S5: Based on the federated filter architecture, perform elastic information allocation on the real-time fusion solution results of each local filter output, and complete the global optimal estimation by dynamically adjusting the information allocation coefficient β.

[0063] S1 includes:

[0064] S11: Perform timestamp synchronization processing on the first navigation data output by the inertial navigation system (INS), the second navigation data output by the Doppler velocity log (DVL), the third navigation data output by the ultra-short baseline (USBL) / long baseline (LBL), the fourth navigation data output by the altimeter, and the fifth navigation data output by the depth gauge to generate multi-source navigation data after timestamp synchronization;

[0065] The timestamp synchronization processing method is as follows:

[0066] Take the time reference of the inertial navigation system (INS) as the global time axis, and use the linear interpolation method to align other sensor data. Specifically:

[0067] ;

[0068] where is the data output by the i-th type of sensor, , , is the two moments closest to in the original sampling timestamps, and is the synchronized data obtained by interpolation;

[0069] S12: Perform spatial coordinate system conversion processing on the multi-source navigation data after timestamp synchronization to generate multi-source navigation data with unified spatial coordinates;

[0070] The coordinate system conversion method is as follows: Transform the body coordinate system velocity data output by the Doppler velocity log (DVL) and the geographical coordinate system position data output by the ultra-short baseline (USBL) / long baseline (LBL) through the submersible's attitude angles (roll angle , pitch angle , and heading angle ). The velocity data transformation formula:

[0071] ;

[0072] Among them, is the DVL velocity in the navigation coordinate system, is the direction cosine matrix (transformation matrix from the body coordinate system to the navigation coordinate system) composed of attitude angles, and synchronously corrects the data deviation between the altimeter and the depth gauge:

[0073] ;

[0074] Among them, is the vertical height measured by the altimeter, is the depth estimated by the navigation system, is the depth measured by the depth gauge based on pressure;

[0075] S13: Conduct gross error detection and marking processing on the multi-source navigation data after unifying the space coordinates to generate multi-source navigation data with outliers marked;

[0076] The gross error detection method is sliding window residual test, and the calculation method is as follows:

[0077] Assume the sliding window size is N, and for each type of sensor data, calculate the mean value and the standard deviation :

[0078] ;

[0079] If the sensor output x at a certain moment satisfies: , then mark this data as an outlier;

[0080] Among them, is the i-th data sample within the sliding window, and x is the data value to be tested currently;

[0081] S14: Conduct data repair processing on the multi-source navigation data with outliers marked to generate preprocessed multi-source navigation data, and output the preprocessed multi-source navigation data to S2 as the input data for establishing the state space model;

[0082] The data repair processing is as follows:

[0083] If a certain sensor data is marked as an outlier, use adjacent valid data for cubic spline interpolation, and the interpolation formula:

[0084] ;

[0085] Among them, is the corrected data obtained by interpolation, , , , are the cubic spline interpolation coefficients, is the interpolation reference point.

[0086] If the number of consecutive outliers exceeds the set threshold , the data for the current period is excluded, and an alarm for the health status of the sensor is triggered.

[0087] S2 includes:

[0088] S21: Based on the preprocessed multi-source navigation data, construct a state space model that includes navigation parameter errors and sensor errors;

[0089] The specific definition of the state space model is as follows:

[0090] State vector: Includes the position error of the inertial navigation system INS , velocity error , attitude angle error , gyroscope zero bias , accelerometer zero bias , scale factor error of the Doppler velocity log DVL , sound ray propagation delay error of the ultra-short baseline USBL / long baseline LBL , vertical measurement deviation of the altimeter , pressure measurement deviation of the depth gauge ;

[0091] State equation: Based on the INS error propagation model, fuse the velocity error equation of the Doppler velocity log DVL and the geometric positioning error equation of the ultra-short baseline USBL / long baseline LBL to establish a non-linear discrete state equation, expressed as: , where is the non-linear state transition function, is the state noise;

[0092] Observation equation: Based on the position, velocity, and attitude angle output by the INS, perform a difference operation with the velocity data of the Doppler velocity log DVL, the position data of the ultra-short baseline USBL / long baseline LBL, the vertical height data of the altimeter, and the pressure depth data of the depth gauge to construct a linear observation equation, expressed as: , where is the observation noise, is the system state vector at time k, is the observation vector, and H is the observation matrix;

[0093] S22: Perform piecewise constant system PWCS observability analysis on the state space model to generate an observability matrix for each error parameter;

[0094] The specific PWCS observability analysis is as follows:

[0095] Piecewise linearization: The motion trajectory of the submersible is divided into multiple steady-state system segments according to time. The acceleration change rate of the submersible within each segment is less than a preset threshold, and the Jacobian matrix of the nonlinear state equation is linearized within each segment to obtain a piecewise linearized model, which is expressed as: , where is the state transition matrix, is the state noise term;

[0096] Observability calculation: For each piecewise linearized model, the singular values of the observability matrix are calculated through singular value decomposition (SVD), and the singular values are normalized to the observability values within the interval [0,1] to generate an observability matrix containing the observability of all error parameters, which is expressed as:

[0097] ;

[0098] Perform singular value decomposition (SVD) on : , where is the singular value matrix, extract the singular values , and perform normalization processing to obtain the observability of each error parameter:

[0099] , where is the maximum singular value.

[0100] S23: According to the observability matrix, perform observability threshold screening on the error parameters in the state space model to generate a candidate set of sensor combination methods;

[0101] The specific process of the observability threshold screening is as follows:

[0102] Parameter screening rule: If the average observability of a certain error parameter in N consecutive segments is lower than the preset threshold of 0.1, it is determined that the parameter is unobservable and it is removed from the state vector;

[0103] Sensor combination generation: According to the sensor types corresponding to the remaining error parameters, generate a candidate set of sensor combination methods including at least two combinations of inertial navigation system INS, Doppler velocity log DVL, ultra-short baseline USBL / long baseline LBL, altimeter and depth gauge;

[0104] S24: Based on the candidate set of sensor combination methods, select the combination with the maximum weighted sum of observability as the best sensor combination method, which specifically includes:

[0105] Weighted sum calculation: For each sensor combination in the candidate set, calculate the sum of the observability of its corresponding error parameters and attach the sensor reliability weight (such as the bottom tracking effectiveness coefficient of the Doppler velocity log DVL);

[0106] Optimal determination: Select the sensor combination with the largest weighted sum as the best sensor combination method, and output the reduced state space model corresponding to this combination;

[0107] S25: Output the reduced state space model, state noise covariance matrix Q, and observation noise covariance matrix R corresponding to the best sensor combination method to S3 for constructing an elastic stochastic model;

[0108] S3 includes:

[0109] S31: Based on the best sensor combination method and the reduced state space model, initialize the elastic correction parameters in combination with the elastic PNT framework to generate an initial set of elastic correction parameters;

[0110] The initial set of elastic correction parameters includes the elastic correction coefficient of the state noise covariance matrix and the elastic correction coefficient of the observation noise covariance matrix ;

[0111] The specific initialization method is as follows:

[0112] According to the types of sensors included in the best sensor combination method, assign initial reliability weights to each sensor:

[0113] Inertial Navigation System INS: 1.0, Doppler Velocity Log DVL: 0.8, Ultra-Short Baseline USBL / Long Baseline LBL: 0.7, Altimeter: 0.6, Depth Gauge: 0.9;

[0114] Based on the initial reliability weights, calculate the average weight , and thus obtain:

[0115] , ;

[0116] Set the adjustment range limit: ;

[0117] Among them, is the average reliability weight of the sensors in the current combination, , : The elastic coefficient used for subsequent dynamic correction of Q and R.

[0118] S32: Update the set of elastic correction parameters according to the real-time health status of the best sensor combination method to generate a dynamically adjusted set of elastic correction parameters;

[0119] The real-time health status is jointly determined by the sensor health status alarm signal in step S14 and the observable degree threshold screening result in step S23;

[0120] The specific update method is as follows:

[0121] If a certain sensor triggers a health status alarm, reduce its corresponding reliability weight by 50%;

[0122] If the observability of a certain error parameter in the simplified state space model is lower than 0.1, reduce the weight of the sensor corresponding to this parameter by 30%;

[0123] Calculate the correction coefficient according to the updated weight: , ;

[0124] Wherein, is the updated average weight, 0.2 is a stability factor set to avoid division by zero and amplification effects, and the update range is defined: ;

[0125] S33: Based on the preprocessed multi-source navigation data, perform online covariance estimation on the dynamically adjusted elastic correction parameter set to generate an optimized elastic correction parameter set;

[0126] The specific online covariance estimation is as follows:

[0127] Adopt the sliding window method to perform covariance estimation on the state vector residuals and the observation vector residuals in the previous M cycles before the current moment, and obtain:

[0128] ;

[0129] ;

[0130] Wherein, 、 are the state / observation noise covariances estimated online, 、 are the mean values of the residuals within the sliding window, and the elastic coefficient is dynamically corrected according to the estimation results:

[0131] , ;

[0132] Wherein, represents the trace of the matrix (the sum of the main diagonal elements), and Q and R are the original covariance matrices output by S25.

[0133] S34: According to the optimized elastic correction parameter set, adjust the state noise covariance matrix Q and the observation noise covariance matrix R to generate an elasticized state noise covariance matrix and an observation noise covariance matrix ;

[0134] The adjustment method is as follows: ; ;

[0135] If > 3.0 or > 3.0, then it is forcibly truncated to 3.0;

[0136] If <0.3 or <0.3, then it is forcibly increased to 0.3;

[0137] If it exceeds the upper and lower limits, a covariance overrun alarm mechanism is triggered.

[0138] S35: Output the elasticized state noise covariance matrix , the observation noise covariance matrix and the optimized elastic correction parameter set to S4 for constructing an elastic adaptive robust cubature Kalman filter (CKF) algorithm.

[0139] S4 includes:

[0140] S41: Initialize multiple parallel-set elastic adaptive robust cubature Kalman filter (CKF) algorithms according to the optimal sensor combination method and the elasticized state noise covariance matrix , the observation noise covariance matrix to generate a local filter bank;

[0141] The specific initialization method is as follows:

[0142] For each sensor sub-combination in the optimal sensor combination method (e.g., INS + DVL, INS + USBL), independently construct an elastic adaptive robust cubature Kalman filter (CKF) instance;

[0143] Each CKF instance is configured with:

[0144] The elasticized state noise covariance matrix ;

[0145] The elasticized observation noise covariance matrix ;

[0146] Cubature point sampling parameters: = 1.0, L = 2n, where is the sampling scale factor, L is the number of cubature points, and n is the dimension of the state vector.

[0147] S42: Configure the robust parameters for each elastic adaptive robust cubature Kalman filter (CKF) algorithm in the local filter bank to generate a robust local filter bank;

[0148] The specific robust parameter configuration is as follows:

[0149] Use the Huber cost function to replace the least - squares loss function of the standard CKF, and set the robust threshold: k = 1.345;

[0150] For each observation residual r, adopt the following robust weight adjustment function:

[0151] , where r is the residual value, that is, the difference between the predicted observation and the true observation, is the robust weighting coefficient;

[0152] This weight participates in the state correction after the volume point propagation in both the time update and measurement update stages.

[0153] S43: Input the pre - processed multi - source navigation data into the robust local filter bank, perform parallel filtering and solution, and generate the real - time fusion solution results of each local filter;

[0154] The specific parallel filtering and solution is as follows:

[0155] Time update: Based on the state equation of the reduced - order state - space model, calculate the state prediction value and covariance prediction value:

[0156] ;

[0157] ;

[0158] Among them, is the i - th propagated volume point, , are the weighted coefficients of the mean and covariance respectively;

[0159] Measurement update: Update based on the observation data, calculate the innovation covariance , the Kalman gain , and update the state and covariance:

[0160] ;

[0161] ;

[0162] ;

[0163] ;

[0164] Among them, is the projection of the volume point in the observation space, is the predicted observation, is the covariance between the state and the observation;

[0165] S44: Perform state consistency verification and filter health monitoring on the real-time fusion solution results of each local filter, and generate the real-time fusion solution results after health status marking;

[0166] The specific health monitoring method is as follows:

[0167] State consistency verification: Assume that the state of the main filter (INS) is , and the state of any local filter is , and its difference covariance is:

[0168] If: , then mark this filter as "low confidence";

[0169] Among them: is the matrix trace, is the state consistency detection threshold;

[0170] Covariance divergence detection:

[0171] If the covariance trace of a certain local filter continuously increases for more than 3 cycles, that is:

[0172] ;

[0173] Then it is determined that this filter diverges, and a sensor combination switching request is triggered to S2;

[0174] S45: Output the real-time fusion solution results after the health status marking to S5 as the input data for the federal filter elastic information allocation.

[0175] S5 includes:

[0176] S51: Receive the real-time fusion solution results after the health status marking output by S4, extract the state estimation values and the covariance matrix of each local filter, and generate a federal filter input data set;

[0177] The federal filter input data set includes:

[0178] The state estimation value of the local filter combined by the inertial navigation system INS and the Doppler velocimeter DVL , covariance ;

[0179] Local filter state estimation values of the inertial navigation system INS combined with the ultra-short baseline USBL / long baseline LBL , covariance ;

[0180] Local filter state estimation values and covariance of other sensor combinations.

[0181] Among them, is the state estimation value of the i-th local filter, is the covariance matrix of the i-th local filter, is the total number of current local filters.

[0182] S52: Based on the federated filtering input data set, calculate the information allocation coefficients of each local filter, and generate a dynamic information allocation coefficient set;

[0183] The calculation method of the information allocation coefficient is specifically as follows:

[0184] Benchmark weight calculation (the higher the confidence level, the greater the weight):

[0185] Health state correction: If its local filter is marked as "low confidence", its information coefficient is reduced to ;

[0186] Observable degree weighting: , where is the average observability of the error parameters corresponding to this combination.

[0187] Normalization processing: , so that ;

[0188] S53: For the local filter that triggers the sensor health state alarm or covariance overrun alarm, perform the elastic reset of the information allocation coefficient , and generate a fault-adapted information allocation coefficient set;

[0189] The specific elastic reset rule is as follows:

[0190] Health state alarm: , ;

[0191] That is, set its weight to zero and reallocate it according to the proportion of the remaining filters.

[0192] Covariance overrun alarm: ;

[0193] That is, force the information weight of this filter to be set to the lower limit value of 0.05.

[0194] S54: Based on the information distribution coefficient set after fault adaptation, calculate the global optimal estimation through the federated filtering fusion equation to generate the global navigation state of the submersible;

[0195] The specific form of the federated filtering fusion equation is as follows:

[0196] Global state estimation value: ;

[0197] Global covariance matrix: ;

[0198] where is the global state estimation value, is the global state covariance matrix.

[0199] S55: Conduct divergence detection and integrity protection on the global navigation state of the submersible to generate the final navigation output data;

[0200] The specific integrity protection method is as follows:

[0201] Divergence detection: Trigger the reset of the global filter;

[0202] where is a preset safety threshold. If it is exceeded, it is necessary to re-initialize S2 to S5;

[0203] Data output: Output the global navigation state (position, velocity, attitude angle) and covariance matrix in the convergence state: , to the submersible control system as the input of the control command.

[0204] The present invention covers any substitutions, modifications, equivalent methods and solutions made within the spirit and scope of the present invention. To enable the public to have a thorough understanding of the present invention, specific details are described in detail in the following preferred embodiments of the present invention. However, those skilled in the art can fully understand the present invention without these detailed descriptions. In addition, well-known methods, processes, procedures, components and circuits are not described in detail to avoid unnecessary confusion to the essence of the present invention.

[0205] The above are only the preferred embodiments of the present invention. It should be noted that for those of ordinary skill in the art, several improvements and refinements can be made without departing from the principle of the present invention, and these improvements and refinements should also be regarded as the protection scope of the present invention.

Claims

1. A multi-source data fusion method for the integrated navigation of a submersible based on an elastic random model, characterized in that It includes the following steps: S1: Perform spatio-temporal registration and gross error rejection preprocessing on multi-source navigation data output by an inertial navigation system (INS), a Doppler velocity log (DVL), an ultra-short baseline (USBL) / long baseline (LBL), an altimeter, and a depth gauge; S2: Based on the preprocessed multi-source navigation data, establish a state space model including navigation parameter errors and sensor errors, use an observability analysis method based on the piecewise constant system (PWCS) theory to determine the observability of each error parameter, and screen and determine the optimal sensor combination method according to the observability threshold; S3: Combine the elastic PNT framework to construct an error elastic stochastic model, and dynamically generate elastic correction parameters according to the determined optimal sensor combination method. The elastic correction parameters are used to adjust the state noise covariance matrix and the observation noise covariance matrix; S4: Based on the generated elastic correction parameters, construct multiple parallel elastic adaptive robust cubature Kalman filter (CKF) algorithms to perform local filtering on the multi-source navigation data, and output the real-time fusion solution results of each local filter; S5: Perform elastic information allocation on the real-time fusion solution results of each local filter output based on the federated filter architecture, and complete the global optimal estimation by dynamically adjusting the information allocation coefficient.

2. The multi-source data fusion method for the integrated navigation of a submersible based on an elastic random model according to claim 1, wherein The S1 includes: S11: Perform timestamp synchronization processing on the first navigation data output by the INS, the second navigation data output by the DVL, the third navigation data output by the USBL / LBL, the fourth navigation data output by the altimeter, and the fifth navigation data output by the depth gauge to generate multi-source navigation data after timestamp synchronization; S12: Perform spatial coordinate system conversion processing on the multi-source navigation data after timestamp synchronization to generate multi-source navigation data with unified spatial coordinates; S13: Perform gross error detection and marking processing on the multi-source navigation data with unified spatial coordinates to generate multi-source navigation data with outlier markings; S14: Perform data repair processing on the multi-source navigation data with outlier markings to generate preprocessed multi-source navigation data; S15: Output the preprocessed multi-source navigation data to S2 as the input data for establishing the state space model.

3. The multi-source data fusion method for the combined navigation of a submersible based on an elastic random model according to claim 2, wherein The S2 includes: S21: Based on the preprocessed multi-source navigation data, construct a state space model including navigation parameter errors and sensor errors; S22: Perform piecewise constant system (PWCS) observability analysis on the state space model to generate an observability matrix of each error parameter; S23: According to the observability matrix, perform observability threshold screening on the error parameters in the state space model to generate a candidate set of sensor combination methods; S24: Based on the candidate set of sensor combination methods, select the combination with the largest weighted sum of observability as the optimal sensor combination method, specifically including: Weighted sum calculation: For each sensor combination in the candidate set, calculate the sum of the observabilities of its corresponding error parameters and attach the sensor reliability weight; Optimal determination: Select the sensor combination with the largest weighted sum as the best sensor combination method, and output the reduced state space model corresponding to this combination; S25: Output the best sensor combination method and the corresponding reduced state space model to S3 for constructing an elastic random model.

4. The multi-source data fusion method for the combined navigation of a submersible based on an elastic random model according to claim 3, characterized in that, The specific filtering of the observability threshold in S23 is as follows: Parameter filtering rule: If the average observability of a certain error parameter in N consecutive segments is lower than the preset threshold of 0.1, it is determined that the parameter is unobservable and removed from the state vector; Sensor combination generation: Generate a candidate set of sensor combination methods including at least two combinations of an inertial navigation system INS, a Doppler velocity log DVL, an ultra-short baseline USBL / long baseline LBL, an altimeter, and a depth gauge according to the sensor types corresponding to the remaining error parameters.

5. The multi-source data fusion method for the combined navigation of a submersible based on an elastic random model according to claim 4, wherein, The S3 includes: S31: Based on the best sensor combination method and the reduced state space model, initialize the elastic correction parameters in combination with the elastic PNT framework to generate an initial set of elastic correction parameters; S32: Update the set of elastic correction parameters according to the real-time health status of the best sensor combination method to generate a dynamically adjusted set of elastic correction parameters; The real-time health status is jointly determined by the sensor health status alarm signal in S14 and the filtering result of the observability threshold in S23; S33: Based on the preprocessed multi-source navigation data, perform online covariance estimation on the dynamically adjusted set of elastic correction parameters to generate an optimized set of elastic correction parameters.

6. The multi-source data fusion method for the integrated navigation of a submersible based on an elastic random model according to claim 5, wherein The S3 further includes: S34: Adjust the state noise covariance matrix and the observation noise covariance matrix according to the optimized set of elastic correction parameters to generate an elasticized state noise covariance matrix and an observation noise covariance matrix; S35: Output the elasticized state noise covariance matrix, the observation noise covariance matrix, and the optimized set of elastic correction parameters to S4.

7. The multi-source data fusion method for the combined navigation of a submersible based on an elastic random model according to claim 6, characterized in that The S4 includes: S41: Initialize multiple parallel elastic adaptive robust cubature Kalman filter CKF algorithms according to the best sensor combination method and the elasticized state noise covariance matrix and the observation noise covariance matrix to generate a local filter bank; S42: Configure the robust parameters for each elastic adaptive robust cubature Kalman filter CKF algorithm in the local filter bank to generate a robustified local filter bank; S43: Input the preprocessed multi-source navigation data into the robustified local filter bank and perform parallel filtering and solution to generate the real-time fusion solution results of each local filter.

8. The multi-source data fusion method for the integrated navigation of a submersible based on an elastic random model according to claim 7, wherein, The S4 further includes: S44: Perform state consistency verification and filter health monitoring on the real-time fusion solution results of each local filter to generate the real-time fusion solution results with health status marks; S45: Output the real-time fusion solution results with health status marks to S5 as the input data for the elastic information distribution of the federated filter.

9. The multi-source data fusion method for the combined navigation of a submersible based on an elastic random model according to claim 8, characterized in that, The S5 includes: S51: Receive the real-time fusion solution result after the health status marker output by S4, extract the state estimation values and covariance matrices of each local filter, and generate a federated filter input data set; S52: Based on the federated filter input data set, calculate the elastic reset of the information distribution coefficients of each local filter, and generate a set of information distribution coefficients after fault adaptation; S53: For the local filter that triggers the health status alarm of the sensor or the covariance overrun alarm, perform the elastic reset of the information distribution coefficient, and generate a set of information distribution coefficients after fault adaptation; S54: Based on the set of information distribution coefficients after fault adaptation, calculate the global optimal estimate through the federated filter fusion equation, and generate the global navigation state of the submersible; S55: Perform divergence detection and integrity protection on the global navigation state of the submersible to generate the final navigation output data.

Images