Submersible integrated navigation multi-source data fusion method based on elastic random model

Abstract

The invention relates to the technical field of underwater intelligent navigation and integrated navigation, in particular to a submersible integrated navigation multi-source data fusion method based on an elastic random model, which comprises the following steps: preprocessing multi-source navigation data output by an inertial navigation system (INS), a Doppler velocimeter, an ultra-short baseline / long baseline, an altimeter and a depthometer; constructing a state space model containing navigation parameter errors and sensor errors, and screening an optimal sensor combination mode; the method comprises the following steps: constructing an elastic correction parameter model and a dynamic adjustment state and observation noise covariance matrix by combining an elastic PNT framework, performing local filtering on multi-source data by adopting a plurality of parallel elastic self-adaptive robust volume Kalman filters (CKF), outputting a fusion resolving result, and realizing optimal estimation of a global navigation state through a dynamic information distribution strategy. The method has relatively high anti-interference performance, self-adaptive capability and combination switching robustness, and is suitable for navigation and positioning tasks of a high-precision submersible in a complex underwater environment.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater intelligent navigation and integrated navigation technology, 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 exploration, 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, lacking a dynamic, elastic, and fault-resistant allocation mechanism, causing the fusion solution to be sensitive to abnormal data or abnormal filters, 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 for 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: 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 that includes 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; 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: Based on the federated filtering architecture, perform elastic information allocation on the real-time fusion solution results of each output local filter, and complete the global optimal estimation by dynamically adjusting the information allocation coefficient.

[0006] Optionally, S1 includes: 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 with synchronized timestamps; S12: Perform spatial coordinate system conversion processing on the multi-source navigation data with synchronized timestamps 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 marked outliers; S14: Perform data repair processing on the multi-source navigation data with marked outliers 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.

[0007] Optionally, S2 includes: S21: Based on the preprocessed multi-source navigation data, construct a state space model that includes navigation parameter errors and sensor errors; S22: Perform Piecewise Constant System (PWCS) observability analysis on the state space model to generate the observability degree matrix of each error parameter; S23: According to the observability degree matrix, perform observability degree 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 the sensor combination methods, select the combination with the maximum weighted sum of observability as the optimal sensor combination method, which specifically includes: Weighted sum calculation: For each sensor combination in the candidate set, calculate the sum of the observability of its corresponding error parameters and append the sensor reliability weights; 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; S25: Output the optimal sensor combination method and the corresponding reduced state space model to S3 for constructing an elastic random model.

[0008] Optionally, the observability threshold screening in S23 is specifically as follows: 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; 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.

[0009] Optionally, S3 includes: 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; 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; 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; 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.

[0010] Optionally, S3 further includes: S34: According to the optimized set of elastic correction parameters, 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; S35: Output the elasticized state noise covariance matrix, the observation noise covariance matrix and the optimized set of elastic correction parameters to S4.

[0011] Optionally, S4 includes: S41: Initialize multiple parallel elastic adaptive robust cubature Kalman filter (CKF) algorithms according to the optimal sensor combination method, the elastified state noise covariance matrix, and the observation noise covariance matrix, and 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, perform parallel filtering and solution, and generate the real-time fusion solution results of each local filter.

[0012] Optionally, step 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 markers. S45: Output the real-time fusion solution results with health status markers to S5 as the input data for the elastic information allocation of the federated filter.

[0013] Optionally, step S5 includes: 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, and generate a federated filter input data set. S52: Based on the federated filter input data set, calculate the elastic reset of the information allocation coefficients of each local filter to generate a set of information allocation coefficients adapted to faults. S53: For the local filter that triggers the sensor health status alarm or the covariance overrun alarm, perform the elastic reset of the information allocation coefficient to generate a set of information allocation coefficients adapted to faults. S54: Based on the set of information allocation coefficients adapted to faults, calculate the global optimal estimate through the federated filter fusion equation to 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.

[0014] Advantages of the present invention: In the present invention, through preprocessing of multi-source navigation data such as inertial navigation system (INS), Doppler velocity log (DVL), ultra-short baseline (USBL) / long baseline (LBL), altimeter, depth gauge, etc., including timestamp synchronization, spatial coordinate transformation, gross error rejection 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 containing 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.

[0015] In the present invention, in combination 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 state and observability, a multi-dimensional and multi-granularity elastic correction parameter set is constructed, and continuous optimization and adjustment are carried out 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 failures, drifts, or observation degradations, and realizing system-level robust navigation solution.

[0016] 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 achieve sub-combination solution, and a state consistency check and covariance divergence monitoring mechanism are integrated to effectively identify and eliminate unreliable filter solution results. Further, in the federated filter 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 a globally 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. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] In order 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, other drawings can be obtained based on these drawings without creative efforts.

[0018] Figure 1 It is a schematic flowchart of the method according to the embodiment of the present invention; Figure 2 It is a schematic flowchart of the S2 process according to the embodiment of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0019] The present invention will be described in detail below with reference to the accompanying 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 methods for implementation; moreover, the accompanying drawings are only for more specifically describing the embodiments and are not intended to specifically limit the present invention.

[0020] As Figure 1-2 shown, the multi-source data fusion method for the submersible integrated navigation based on the elastic random model includes the following steps: S1: Perform spatio-temporal registration and gross error rejection preprocessing on the multi-source navigation data output by the inertial navigation system INS (Inertial Navigation System), Doppler velocity log DVL (Doppler Velocity Log), ultra-short baseline USBL (Ultra Short Base Line) / long baseline LBL (LongBase Line), altimeter, and depth gauge; 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 of each error parameter, and screen and determine the optimal sensor combination method according to the observability threshold; 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; S4: Based on the generated elastic correction parameters, construct multiple parallel elastic adaptive robust cubature Kalman filter CKF (Cubature Kalman Filter) 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 β.

[0021] S1 includes: 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; The method for timestamp synchronization is as follows: Taking the time reference of the Inertial Navigation System (INS) as the global time axis, the data of other sensors are aligned using linear interpolation method, specifically: ; Among them, is the data output by the i-th type of sensor, , , is the two moments closest to in the original sampling timestamps, is the synchronized data obtained by interpolation; S12: Perform spatial coordinate system transformation processing on the multi-source navigation data after timestamp synchronization to generate multi-source navigation data with unified spatial coordinates; The coordinate system transformation method is as follows: 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) are transformed through the attitude angles (roll angle , pitch angle , and heading angle ) of the submersible. The velocity data transformation formula: ; Among them, is the DVL velocity in the navigation coordinate system, is the direction cosine matrix (transformation matrix from body coordinate system to navigation coordinate system) composed of attitude angles, and synchronously correct the data deviation between the altimeter and the depth gauge: ; 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; 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 marked outliers; The gross error detection method is sliding window residual test, and the calculation method is as follows: Assume the sliding window size is N. For each type of sensor data, calculate the mean value and the standard deviation : ; If the sensor output x at a certain moment satisfies: , then mark this data as an outlier; Among them, is the i-th data sample within the sliding window, and x is the data value to be tested currently; S14: Perform data repair processing on the multi-source navigation data after outlier marking, generate the 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; The data repair processing is as follows: If a certain sensor data is marked as an outlier, cubic spline interpolation is performed using adjacent valid data, and the interpolation formula is: ; where, is the corrected data obtained by interpolation, , , , are the cubic spline interpolation coefficients, is the interpolation reference point.

[0022] If the number of consecutive outliers exceeds the set threshold , then the data in the current time period is excluded, and a sensor health status alarm is triggered.

[0023] S2 includes: S21: Based on the preprocessed multi-source navigation data, construct a state space model including navigation parameter errors and sensor errors; The specific definition of the state space model is: State vector: including the position error of the inertial navigation system INS, the velocity error, the attitude angle error, the gyroscope zero bias, the accelerometer zero bias, the scale factor error of the Doppler velocity log DVL, the sound ray propagation delay error of the ultra-short baseline USBL / long baseline LBL, the vertical measurement deviation of the altimeter, the pressure measurement deviation of the depth gauge; 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 nonlinear discrete state equation, expressed as: , where, is the nonlinear state transition function, is the state noise; Observation equation: Based on the position, velocity, and attitude angles output by the Inertial Navigation System (INS), difference operations are performed 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; S22: Perform Piecewise Constant System (PWCS) observability analysis on the state space model to generate the observability matrix of each error parameter; The specific PWCS observability analysis is as follows: Piecewise linearization: Divide the motion trajectory of the submersible into multiple constant system segments according to time. The acceleration change rate of the submersible within each segment is less than the preset threshold, and the Jacobian matrix linearization is performed on the nonlinear state equation within each segment to obtain the piecewise linearized model, expressed as: , where is the state transition matrix, is the state noise term; Observability calculation: For each piecewise linearized model, calculate the singular values of the observability matrix through Singular Value Decomposition (SVD), normalize the singular values to the observability values within the interval [0, 1], and generate the observability matrix containing the observability of all error parameters, expressed as: ; 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: , where is the maximum singular value.

[0024] 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; The specific observability threshold screening is as follows: 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; Sensor combination generation: According to the sensor types corresponding to the remaining error parameters, generate a candidate set of sensor combination methods that includes 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; S24: Based on the candidate set of sensor combination methods, select the combination with the maximum sum of observability weights 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 append the sensor reliability weight (such as the bottom tracking effectiveness coefficient of the Doppler velocity log (DVL)); 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; S25: Output the reduced state space model, the state noise covariance matrix Q, and the observation noise covariance matrix R corresponding to the optimal sensor combination method to S3 for constructing an elastic stochastic model; S3 includes: 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; 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 ; The specific initialization method is as follows: According to the sensor types included in the optimal sensor combination method, assign initial reliability weights to each sensor: 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; Based on the initial reliability weights, calculate the average weight , and thus obtain: , ; Set the adjustment range limit: ; 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.

[0025] S32: Update the elastic correction parameter set according to the real-time health status of the optimal sensor combination method to generate a dynamically adjusted elastic correction parameter set; The real-time health status is jointly determined by the sensor health status alarm signal in step S14 and the observable threshold screening result in step S23; The specific update method is as follows: If a certain sensor triggers a health status alarm, reduce the corresponding reliability weight by 50%; If the observability of a certain error parameter in the reduced state space model is lower than 0.1, reduce the weight of the sensor corresponding to this parameter by 30%; Calculate the correction coefficient according to the updated weights: , ; Among them, 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 limited: ; 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; The specific online covariance estimation is as follows: Adopt the sliding window method to perform covariance estimation on the state vector residuals and the observation vector residuals in the previous M cycles of the current moment to obtain: ; ; Among them, , are the state / observation noise covariances estimated online, , are the residual means within the sliding window, and the elastic coefficient is dynamically corrected according to the estimation results: , ; Among them, 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.

[0026] 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 ; The adjustment method is: ; ; If > 3.0 or > 3.0, then it is forcibly truncated to 3.0; If < 0.3 or < 0.3, then it is forcibly increased to 0.3; If it exceeds the upper and lower limits, a covariance overrun alarm mechanism is triggered.

[0027] 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.

[0028] S4 includes: S41: Initialize multiple parallel 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 , and generate a local filter bank; The specific initialization method is: 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; Each CKF instance is configured with: The elasticized state noise covariance matrix ; The elasticized observation noise covariance matrix ; 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.

[0029] 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; The specific robust parameter configuration is: Use the Huber cost function to replace the least - squares loss function of the standard CKF, and set the robust threshold: k = 1.345; For each observation residual r, use the following robust weight adjustment function: , where r is the residual value, that is, the difference between the predicted observation and the true observation, is the robust weighted coefficient; This weight participates in the state correction after the volume point propagation in both the time update and measurement update phases.

[0030] S43: Input the preprocessed multi-source navigation data into the robustified local filter bank, perform parallel filtering and solution, and generate the real-time fusion solution results of each local filter; The specific parallel filtering and solution is as follows: Time update: Based on the state equation of the reduced state space model, calculate the state prediction value and covariance prediction value: ; ; where is the i-th propagated volume point, , are the weighted coefficients of the mean and covariance respectively; Measurement update: Update based on the observation data, calculate the innovation covariance , the Kalman gain , and update the state and covariance: ; ; ; ; where is the projection of the volume point in the observation space, is the predicted observation, is the covariance between the state and the observation; 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 with health status markers; The specific health monitoring method is as follows: State consistency verification: Let the state of the main filter (INS) be , and the state of any local filter be , and its difference covariance is:

[0031] If: , then mark this filter as "low confidence"; where: is the matrix trace, is the state consistency detection threshold; Covariance divergence detection: If the covariance trace of a certain local filter Continuous growth exceeding 3 cycles, i.e.: ; Then it is determined that the filter diverges, and a sensor combination switching request is triggered to S2; S45: Output the real-time fusion solution result marked with the health status to S5 as the input data for the federal filtering elastic information allocation.

[0032] S5 includes: S51: Receive the real-time fusion solution result marked with the health status output by S4, and extract the state estimation values of each local filter and the covariance matrix , and generate a federal filtering input data set; The federal filtering input data set includes: The state estimation value of the local filter of the combination of the inertial navigation system INS and the Doppler velocimeter DVL , covariance ; The state estimation value of the local filter of the combination of the inertial navigation system INS and the ultra-short baseline USBL / long baseline LBL , covariance ; The state estimation value and covariance of the local filter of other sensor combinations.

[0033] 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.

[0034] S52: Based on the federal filtering input data set, calculate the information allocation coefficients of each local filter , and generate a dynamic information allocation coefficient set; The calculation method of the information allocation coefficient is specifically as follows: Benchmark weight calculation (the higher the confidence, the greater the weight):

[0035] Health status correction: If the local filter is marked as "low confidence", its information coefficient is reduced to ; Observability weighting: , where is the average observability of the error parameters corresponding to this combination.

[0036] Normalization processing: , so that ; 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 after fault adaptation; The specific elastic reset rule is as follows: Health status alarm: , ; That is, reset its weight to zero and redistribute it according to the ratio of the remaining filters.

[0037] Covariance exceeding the limit alarm: ; That is, force the information weight of this filter to be set to the lower limit value of 0.05.

[0038] S54: Based on the set of information distribution coefficients after fault adaptation, calculate the global optimal estimate through the federated filter fusion equation to generate the global navigation state of the submersible; The specific federated filter fusion equation is as follows: Global state estimate value: ; Global covariance matrix: ; Among them, is the global state estimate value, is the global state covariance matrix.

[0039] S55: Perform divergence detection and integrity protection on the global navigation state of the submersible to generate the final navigation output data; The specific integrity protection method is as follows: Divergence detection: Trigger the reset of the global filter; Among them, is the preset safety threshold. If it exceeds the limit, it is necessary to re-initialize S2 to S5; Data output: Output the global navigation state (position, speed, attitude angle) and covariance matrix in the convergence state: , to the submersible control system as the input of the control command.

[0040] The present invention covers any substitutions, modifications, equivalent methods, and solutions made within the spirit and scope of the present invention. For 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.

[0041] The above are only the preferred embodiments of the present invention. It should be noted that for those of ordinary skill in the art, without departing from the principle of the present invention, several improvements and refinements can be made, 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 submersible integrated navigation based on elastic random model, characterized in that: The following steps are involved: S1: Perform spatiotemporal registration and gross error elimination preprocessing on the multi-source navigation data output by the inertial navigation system INS, Doppler velocimeter DVL, ultra-short baseline USBL / long baseline LBL, altimeter and depth gauge; S2: Based on the preprocessed multi-source navigation data, a state space model including navigation parameter errors and sensor errors is established. The observability of each error parameter is determined by using the observability analysis method based on the PWCS theory of piecewise steady-state systems. The optimal sensor combination is determined based on the observability threshold. S3: construct an error elastic random model in combination with the elastic PNT framework, and dynamically generate elastic correction parameters according to the determined optimal sensor combination mode, wherein 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, multiple parallel elastic adaptive robust cubature Kalman filter CKF algorithms are constructed to perform local filtering on multi-source navigation data and output the real-time fusion solution results of each local filter; S5: Based on the federated filtering architecture, flexible information allocation is performed on the real-time fusion solution results of each local filter output, and the global optimal estimation is completed by dynamically adjusting the information allocation coefficient.

2. The multi-source data fusion method for submersible integrated navigation based on elastic random model according to claim 1 is characterized in that: The S1 includes: S11: performing timestamp synchronization processing on the first navigation data output by the inertial navigation system INS, the second navigation data output by the Doppler velocimeter 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 meter to generate multi-source navigation data after timestamp synchronization; S12: performing spatial coordinate system conversion processing on the multi-source navigation data after the timestamp synchronization to generate multi-source navigation data with unified spatial coordinates; S13: performing gross error detection and marking processing on the multi-source navigation data after the spatial coordinates are unified, to generate multi-source navigation data after outlier marking; S14: performing data repair processing on the multi-source navigation data after the abnormal value marking to generate pre-processed multi-source navigation data; S15: Output the pre-processed multi-source navigation data to S2 as input data for establishing a state space model.

3. The multi-source data fusion method for submersible integrated navigation based on elastic random model according to claim 2 is characterized in that: The S2 includes: S21: constructing a state space model including navigation parameter errors and sensor errors based on the preprocessed multi-source navigation data; S22: performing piecewise steady-state system PWCS observability analysis on the state space model to generate an observability matrix of each error parameter; S23: performing observability threshold screening on the error parameters in the state space model according to the observability matrix to generate a candidate set of sensor combination modes; S24: Based on the candidate set of sensor combination modes, selecting a combination with the largest weighted sum of observability as the best sensor combination mode, specifically including: Weighted sum calculation: For each sensor combination in the candidate set, calculate the sum of the observability of the corresponding error parameters and add the sensor reliability weight; Optimal judgment: Select the sensor combination with the largest weighted sum as the best sensor combination, and output the simplified state space model corresponding to the combination; S25: Outputting the optimal sensor combination mode and the corresponding simplified state space model to S3 for constructing an elastic random model.

4. The multi-source data fusion method for submersible integrated navigation based on elastic random model according to claim 3 is characterized in that: The observability threshold screening in S23 is specifically as follows: Parameter screening rule: If the average observability of a certain error parameter in N consecutive segments is lower than the preset threshold value of 0.1, the parameter is considered unobservable and is removed from the state vector; Sensor combination generation: According to the sensor type corresponding to the residual error parameter, a sensor combination candidate set including at least two combinations of inertial navigation system INS, Doppler velocimeter DVL, ultra-short baseline USBL / long baseline LBL, altimeter and depth meter is generated.

5. The multi-source data fusion method for submersible integrated navigation based on elastic random model according to claim 4 is characterized in that: The S3 includes: S31: Based on the optimal sensor combination mode and the simplified state space model, the elastic correction parameters are initialized in combination with the elastic PNT framework to generate an initial elastic correction parameter set; S32: updating the elasticity correction parameter set according to the real-time health status of the optimal sensor combination mode, and generating a dynamically adjusted elasticity correction parameter set; The real-time health status is determined by the sensor health status alarm signal in S14 and the observability threshold screening result in S23; S33: Based on the preprocessed multi-source navigation data, online covariance estimation is performed on the dynamically adjusted elastic correction parameter set to generate an optimized elastic correction parameter set.

6. The multi-source data fusion method for submersible integrated navigation based on elastic random model according to claim 5 is characterized in that: The S3 further includes: S34: adjusting the state noise covariance matrix and the observation noise covariance matrix according to the optimized elastic correction parameter set to generate an elasticized state noise covariance matrix and an observation noise covariance matrix; S35: Outputting the elasticized state noise covariance matrix, the observation noise covariance matrix and the optimized elastic correction parameter set to S4.

7. The multi-source data fusion method for submersible integrated navigation based on elastic random model according to claim 6 is characterized in that: The S4 includes: S41: Initializing multiple parallel elastic adaptive robust cubature Kalman filter (CKF) algorithms according to the optimal sensor combination mode and the elasticized state noise covariance matrix and observation noise covariance matrix to generate a local filter group; S42: performing robustness parameter configuration on each elastic adaptive robust cubature Kalman filter CKF algorithm in the local filter group to generate a robust local filter group; S43: Inputting the pre-processed multi-source navigation data into the robust local filter group, performing parallel filtering and solving, and generating real-time fusion and solving results of each local filter.

8. The multi-source data fusion method for submersible integrated navigation based on elastic random model according to claim 7 is characterized in that: The S4 further comprises: S44: performing state consistency check and filter health monitoring on the real-time fusion solution results of each local filter, and generating a real-time fusion solution result with a health state mark; S45: Outputting the real-time fusion solution result after the health status marking to S5 as input data for federated filtering elastic information allocation.

9. The multi-source data fusion method for submersible integrated navigation based on elastic random model according to claim 8 is characterized in that: The S5 includes: S51: receiving the real-time fusion solution result after the health status mark output by S4, extracting the state estimation value and covariance matrix of each local filter, and generating a federated filtering input data set; S52: Based on the federated filtering input data set, calculate the elastic reset of the information allocation coefficient of each local filter to generate a fault-adapted information allocation coefficient set; S53: for the local filter that triggers the sensor health status alarm or the covariance limit exceeding alarm, perform elastic reset of the information allocation coefficient to generate a fault-adapted information allocation coefficient set; S54: Based on the information allocation coefficient set after the fault adaptation, a global optimal estimate is calculated through a federated filtering fusion equation to generate a global navigation state of the submersible; S55: Perform divergence detection and integrity protection on the global navigation state of the submersible to generate final navigation output data.

Images