Elastic Adaptive Federated H ∞ Filter-Based Underwater Navigation and Positioning Data Fusion Method

Abstract

The present invention relates to the technical field of underwater navigation and positioning, and specifically relates to an underwater navigation and positioning data fusion method based on elastic adaptive federated filtering, including the following steps: Through the fusion of data from multiple sensors (including inertial measurement units, Doppler log, ultra-short baseline beacons, and depth gauges), using dynamic weighting coefficients and an elastic adaptive mechanism, it can adjust the data fusion process in real time according to the health of the sensors and measurement errors, and optimize the positioning parameters of the underwater vehicle. In addition, the present invention introduces a topology reconstruction mechanism. When sensor anomalies or data noise are detected, the system can automatically adjust the data interaction paths between sub-filters to ensure stable operation of the system in complex environments. The present invention can effectively suppress the influence of abnormal data, improve the positioning accuracy, and has good real-time performance and adaptability, and is widely applicable to fields such as ocean exploration, deep-sea exploration, and ocean engineering.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater navigation and positioning, and particularly to an underwater navigation and positioning data fusion method based on elastic adaptive federated filtering. Background Art

[0002] With the continuous development of underwater vehicle technology, underwater positioning and navigation systems play a crucial role in various fields such as ocean exploration, scientific research tasks, deep-sea exploration, and ocean engineering.

[0003] Although multi-sensor fusion technology has been widely applied to the navigation systems of underwater vehicles, the existing technologies still face many challenges in practical applications. Traditional sensor fusion methods often ignore the dynamic changes and uncertainties between sensors and cannot effectively process abnormal data caused by sensor failures or environmental changes. In some complex underwater environments, the errors of sensors are large, or there are missing or distorted measurement data, resulting in a significant reduction in the navigation accuracy of the system. Secondly, the existing fusion methods fail to fully utilize the health indicators and reliability information of sensors, making it difficult to dynamically adjust the weights in the data fusion process and lacking adaptability. In addition, although some methods can perform filtering and data correction, when multiple sensors are abnormal simultaneously, the existing methods usually cannot effectively adjust the data interaction paths between sub-filters, resulting in insufficient robustness and fault tolerance of the system. Summary of the Invention

[0004] Based on the above purposes, the present invention provides an underwater navigation and positioning data fusion method based on elastic adaptive federated filtering.

[0005] The underwater navigation and positioning data fusion method based on elastic adaptive federated filtering includes the following steps:

[0006] S1. Raw navigation parameters are obtained in real time through an inertial measurement unit, a Doppler log, an ultra-short baseline beacon, and a depth gauge on an underwater vehicle, and the raw navigation parameters include the acceleration, angular velocity, speed information, position information, and depth information of the underwater vehicle;

[0007] S2. The raw navigation parameters obtained in S1 are input into a preset elastic adaptive federated filtering architecture, which includes a main filter and four sub-filters. Dynamic coupling relationships are established between the sub-filters through an elastic adaptive mechanism to generate a preliminary state estimate;

[0008] S3. Perform robust correction on the preliminary state estimate of each sub-filter and adjust based on the elastic factor at the current moment Generate a corrected local state estimate using the filtered robust boundary parameters;

[0009] S4. Adopt a dynamic weight allocation algorithm to evaluate the credibility of the corrected local state estimates of each sub-filter in S3, calculate the residual anomaly degree based on the corrected state estimates, and generate a dynamic weighting coefficient in combination with the sensor health index for optimizing the weight of data fusion;

[0010] S5. If the residual anomaly degree calculated in S4 exceeds the preset threshold, trigger the topology reconstruction mechanism to reconfigure the data interaction path between sub-filters to cope with abnormal data and optimize the filtering effect;

[0011] S6. Input the dynamically weighted local state estimates generated in S4 into the main filter for fusion processing, and output the final navigation and positioning parameters, where the final navigation and positioning parameters include the accurate position, speed, acceleration, and heading information of the underwater vehicle for achieving high-precision navigation and positioning.

[0012] Optionally, S1 includes:

[0013] S11. Preset the sensor operating modes of the underwater vehicle: In the inertial measurement unit, Doppler log, and depth gauge of the underwater vehicle, preset the operating mode of each sensor, including the data acquisition frequency, acquisition accuracy, and data transmission method;

[0014] S12. Collect the acceleration and angular velocity information of the inertial measurement unit: Use the inertial measurement unit to collect the acceleration and angular velocity data of the underwater vehicle in real time;

[0015] S13. Collect the speed information of the Doppler log: Use the Doppler log to obtain the speed information of the vehicle in real time by measuring the relative speed of the underwater vehicle and the relative speed of the water flow;

[0016] S14. Collect the position information of the ultra-short baseline beacon: Obtain the position information of the beacon by the azimuth-distance method;

[0017] S15. Collect the depth information of the depth gauge: Use the depth gauge to obtain the depth information of the underwater vehicle in real time.

[0018] S16. Data synchronization and timestamp calibration: Synchronize the time of the collected acceleration, angular velocity, speed information, and depth information to ensure that the data of all sensors are calibrated under the same time reference. During the synchronization process, use the timestamp technology to align the data of each sensor;

[0019] S17. Generate the original navigation parameter set: Integrate the acceleration, angular velocity, speed information, position information, and depth information after data synchronization and timestamp calibration into an original navigation parameter set.

[0020] Optionally, S2 includes:

[0021] S21, initialize the elastic adaptive federated filtering architecture: In the navigation system of the underwater vehicle, initialize the elastic adaptive federated filtering architecture, which includes a main filter and four sub-filters. The main filter is used to receive the output results of the four sub-filters and perform final fusion, and the four sub-filters respectively receive data from different sensors for preliminary state estimation;

[0022] S22, set the input data sources of the sub-filters: According to the different sensors, set the input data sources for each sub-filter respectively;

[0023] The first sub-filter receives data from the inertial measurement unit, including the acceleration and angular velocity of the underwater vehicle;

[0024] The second sub-filter receives data from the Doppler log, including the speed information of the underwater vehicle;

[0025] The third sub-filter receives the position information from the ultra-short baseline positioning system;

[0026] The fourth sub-filter receives data from the depth gauge, including the depth information of the underwater vehicle;

[0027] Each sub-filter processes the assigned sensor data and independently generates a preliminary state estimate;

[0028] S23, establish an elastic adaptive mechanism: Establish a dynamic coupling relationship among the four sub-filters through the elastic adaptive mechanism;

[0029] S24, data fusion and generation of preliminary state estimate: Under the action of the elastic adaptive mechanism, each sub-filter filters its own input data and outputs a preliminary state estimate.

[0030] Optionally, S3 includes:

[0031] S31, obtain the preliminary state estimate and the elastic factor: Obtain the generated preliminary state estimate and the elastic factor determined in the elastic adaptive mechanism as the input of robust correction;

[0032] S32, calculate the robust boundary parameter: According to the determined elastic factor, calculate the robust boundary parameter of filtering;

[0033] S33, perform robust correction: According to the calculated robust boundary parameter, perform robust correction on the preliminary state estimate of each sub-filter respectively Robust correction;

[0034] S34, generate a corrected local state estimate: After performing robust correction, generate a corrected local state estimate.

[0035] Optionally, the S4 includes:

[0036] S41, obtain the corrected local state estimate and the sensor health index: After the local state estimate of each sub-filter after robust correction is used as input, and at the same time, obtain the sensor health index of each sub-filter;

[0037] S42, calculate the residual abnormality: Based on the corrected local state estimate and the sensor health index, calculate the residual abnormality of each sub-filter;

[0038] S43, perform credibility evaluation using the dynamic weight allocation algorithm: Use the dynamic weight allocation algorithm to perform credibility evaluation on the corrected local state estimate of each sub-filter, and the credibility evaluation is based on the health index and the calculated residual abnormality to evaluate the reliability of each sub-filter;

[0039] S44, normalize the weighted coefficient: Normalize the dynamic weighted coefficient.

[0040] Optionally, the S5 includes:

[0041] S51, determine whether the residual abnormality exceeds a preset threshold: According to the calculated residual abnormality, compare it with the preset threshold to determine whether it exceeds the preset threshold;

[0042] S52, activate the topology reconstruction mechanism: If the residual abnormality exceeds the preset threshold, activate the topology reconstruction mechanism;

[0043] S53, reconfigure the data interaction path between sub-filters: According to the source and type of abnormal data, reconfigure the data interaction path between sub-filters;

[0044] S54, optimize the filtering effect: The reconfigured topology optimizes the data flow between sub-filters, making the information transfer more efficient and accurate, and verify the optimized filtering effect;

[0045] S55, confirm the effectiveness of the topology reconstruction: After each topology reconstruction, perform a performance evaluation to confirm that the reconstructed structure can effectively improve the filtering effect.

[0046] Optionally, the S6 includes:

[0047] S61. Receive the locally estimated state after dynamic weighting: Receive the locally estimated state after dynamic weighting generated by the dynamic weight allocation algorithm.

[0048] S62. Apply the weighted fusion algorithm: The main filter uses the weighted average algorithm to fuse the received locally estimated state after dynamic weighting. Using the weighted average algorithm and combining the weighting coefficients of each sub-filter, calculate the finally fused state estimate.

[0049] S63. Generate the final navigation and positioning parameters: Based on the fused state estimate, generate the final navigation and positioning parameters.

[0050] Advantages of the present invention:

[0051] In the present invention, through a scheme based on elastic adaptive federated filtering, the data of multiple sensors (including inertial measurement unit, Doppler log, ultra-short baseline beacon, and depth gauge) are efficiently fused. By using dynamic weighting coefficients and an elastic adaptive mechanism, the data of each sensor can dynamically adjust its weight according to its health status and measurement accuracy, thereby significantly improving the estimation accuracy of the position, speed, acceleration, and heading of the underwater vehicle. In a dynamically complex environment, it can respond to sensor failures or abnormal data in real time, and through the topology reconstruction mechanism, flexibly adjust the data path, further enhancing the robustness of the system and ensuring that the underwater vehicle maintains high-precision navigation under different conditions.

[0052] In the present invention, by introducing the topology reconstruction mechanism, it is possible to adjust the data interaction path between sub-filters in real time according to the residual anomaly degree and sensor health index of each sub-filter. This mechanism can cope with sensor data anomalies or noise effects, automatically optimize the data fusion process, and reduce the impact of abnormal data on the final navigation and positioning parameters. This fusion method based on dynamic weight allocation and topology adjustment can effectively suppress the impact of abnormal data on the positioning accuracy and ensure the stable operation of the navigation system in a harsh or unstable environment.

[0053] In the present invention, the main filter fuses the dynamically weighted state estimates from different sub-filters and outputs the final navigation and positioning parameters (including position, speed, acceleration, and heading). This process fully considers the reliability and measurement quality of each sensor, optimizes the weights of data fusion, and thus improves the accuracy and stability of the positioning result. This system not only has high real-time performance and can provide accurate navigation and positioning information, but also can continuously improve through verifying the fusion effect and optimization strategy, enabling the system to maintain good performance and the ability to handle emergencies in a complex underwater environment. Description of the Drawings

[0054] To more clearly illustrate the technical solutions in the present invention or the prior art, the following will briefly introduce the drawings required for the description of the embodiments or 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.

[0055] Figure 1 Schematic diagram of the method flow of the embodiment of the present invention;

[0056] Figure 2 Schematic diagram of the S5 process of the embodiment of the present invention. Detailed implementation manners

[0057] The following will describe the present invention in detail with reference to 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 methods for implementation; moreover, the drawings are only for more specific description of the embodiments, and are not intended to specifically limit the present invention.

[0058] As Figure 1 - Figure 2 shown, the underwater navigation and positioning data fusion method based on elastic adaptive federated filtering includes the following steps:

[0059] S1. The original navigation parameters are obtained in real time through the inertial measurement unit, Doppler log, ultra-short baseline beacon, and depth gauge on the underwater vehicle. The original navigation parameters include the acceleration, angular velocity, speed information, position information, and depth information of the underwater vehicle.

[0060] S2. The original navigation parameters obtained in S1 are input into a preset elastic adaptive federated filtering architecture. The elastic adaptive federated filtering architecture includes a main filter and four sub-filters. The sub-filters establish a dynamic coupling relationship through an elastic adaptive mechanism to generate a preliminary state estimate.

[0061] S3. Perform robust correction on the preliminary state estimate of each sub-filter, adjust the robust boundary parameters of the filter based on the elastic factor at the current moment, and generate a corrected local state estimate to ensure that abnormal data can be effectively corrected in a dynamic environment.

[0062] S4. Adopt a dynamic weight allocation algorithm to evaluate the credibility of the corrected local state estimates of each sub-filter in S3, calculate the residual abnormality based on the corrected state estimate, and generate a dynamic weighting coefficient in combination with the sensor health index to optimize the weight of data fusion.

[0063] S5. If the residual anomaly calculated in S4 exceeds the preset threshold, trigger the topology reconstruction mechanism to reconfigure the data interaction path between sub-filters to handle abnormal data and optimize the filtering effect.

[0064] S6. Input the dynamically weighted local state estimate generated in S4 into the main filter for fusion processing, and output the final navigation and positioning parameters. The final navigation and positioning parameters include the accurate position, speed, acceleration, and heading information of the underwater vehicle, which are used to achieve high-precision navigation and positioning.

[0065] S1 includes:

[0066] S11. Preset the working mode of the sensors of the underwater vehicle: In the inertial measurement unit, Doppler log, ultra-short baseline beacon, and depth gauge of the underwater vehicle, preset the working mode of each sensor, including the data acquisition frequency, acquisition accuracy, and data transmission method, to ensure real-time acquisition of accurate original navigation parameters. The working mode of the sensors is optimized according to the specific application scenario and the required navigation accuracy.

[0067] S12. Collect the acceleration and angular velocity information of the inertial measurement unit: Use the inertial measurement unit to collect the acceleration and angular velocity data of the underwater vehicle in real time. Specifically, the inertial measurement unit obtains the linear acceleration of the underwater vehicle through the accelerometer and the angular velocity of the vehicle through the gyroscope. The acceleration and angular velocity data have a certain timestamp to ensure alignment with the data of other sensors.

[0068] S13. Collect the speed information of the Doppler log: Use the Doppler log to obtain the speed information of the vehicle in real time by measuring the relative speed of the underwater vehicle and the relative speed of the water flow. The Doppler log emits and receives acoustic signals to measure the moving speed of the target object and collects the data at a certain time interval to ensure accurate recording of the movement trajectory and speed change of the vehicle.

[0069] S14. Collect the position information of the ultra-short baseline beacon: Obtain the azimuth and distance information of the beacon through the ultra-short baseline positioning system, so as to obtain the position information of the beacon. Specifically, the ultra-short baseline system uses multiple transducers separated by a certain distance to calculate the position of the underwater vehicle relative to the beacon by measuring the time difference of arrival and azimuth angle of the acoustic wave. When absolute positioning is required, positioning correction can be carried out by means of a seabed base station or a mother ship-borne base station deployed at a known position.

[0070] S15. Collect the depth information of the depth gauge: Use the depth gauge to obtain the depth information of the underwater vehicle in real time. The depth gauge measures the hydrostatic pressure at the position where the submersible is located underwater and converts it into depth data through calculation. The depth information is used to judge the current underwater position change of the vehicle and ensure the accuracy of the depth information during navigation.

[0071] S16, Data synchronization and timestamp calibration: Synchronize the time of the collected acceleration, angular velocity, velocity information, position information, and depth information to ensure that the data of all sensors are calibrated under the same time reference. During the synchronization process, use timestamp technology to align the data of each sensor to ensure that the data of different sensors can correctly correspond to the actual situation at the same moment;

[0072] S17, Generate the original navigation parameter set: Integrate the acceleration, angular velocity, velocity information, position information, and depth information after data synchronization and timestamp calibration into an original navigation parameter set. The parameter set contains the data of each sensor and provides the basic data input for the subsequent elastic adaptive federated filtering architecture.

[0073] S2 includes:

[0074] S21, Initialize the elastic adaptive federated filtering architecture: In the navigation system of the underwater vehicle, initialize the elastic adaptive federated filtering architecture. The elastic adaptive federated filtering architecture includes a main filter and four sub-filters. The main filter is used to receive the output results of the four sub-filters and perform final fusion. The four sub-filters respectively receive data from different sensors for preliminary state estimation;

[0075] S22, Set the input data sources of the sub-filters: According to the different sensors, set the input data sources for each sub-filter respectively;

[0076] The first sub-filter receives data from the inertial measurement unit, including the acceleration and angular velocity of the underwater vehicle;

[0077] The second sub-filter receives data from the Doppler log, including the velocity information of the underwater vehicle;

[0078] The third sub-filter receives the position information from the ultra-short baseline positioning system;

[0079] The fourth sub-filter receives data from the depth gauge, including the depth information of the underwater vehicle;

[0080] Each sub-filter processes the allocated sensor data and independently generates a preliminary state estimation;

[0081] S23, Establish an elastic adaptive mechanism: Establish a dynamic coupling relationship between the four sub-filters through the elastic adaptive mechanism. Specifically, the elastic factor is dynamically adjusted according to the real-time performance of the sensor and the measurement environment to control the response intensity of each sub-filter to the input data. The elastic factor can be dynamically adjusted according to the output error, data quality, and current measurement environment of each sub-filter to ensure that the output results of each sub-filter are fused under optimal conditions;

[0082] S24, Data Fusion and Initial State Estimation Generation: Under the action of the elastic adaptive mechanism, each sub-filter filters its respective input data and outputs an initial state estimate, which includes the position, velocity, acceleration, and depth parameters of the underwater vehicle. The four sub-filters perform mutual coupling corrections according to the change of the elastic factor to further improve the accuracy of their respective state estimates.

[0083] S3 includes:

[0084] S31, Obtain the Initial State Estimate and the Elastic Factor: Obtain the generated initial state estimate and and the elastic factor determined in the elastic adaptive mechanism and , as the input for robust correction;

[0085] The elastic factor is dynamically adjusted according to the measurement quality of each sub-filter and the current environment, reflecting the reliability of the data;

[0086] S32, Calculate the Robust Boundary Parameters: According to the determined elastic factor, calculate the robust boundary parameters of the filter;

[0087] Specifically, use the elastic factor and to adjust the robust boundary parameters of each filter and , ensuring that the noise and abnormal data in the measurement can be effectively suppressed. The calculation formula of the robust boundary parameters is as follows:

[0088] , , , ;

[0089] Wherein, , , and are the robust boundary parameters of the first, second, third, and fourth sub-filters respectively, and are the elastic factors of the first, second, third, and fourth sub-filters respectively, is the initial robust boundary parameter, determined by initial setting or theoretical analysis;

[0090] S33, Perform robust correction: According to the calculated robust boundary parameters, perform Robust correction is ensured to suppress the influence of abnormal data. Specifically, The robust filter uses the following correction formula:

[0091] ;

[0092] where is the corrected state estimate, is the preliminary state estimate (i.e., is or ), K is the gain matrix, is the actual measurement value, is the predicted value obtained based on the current state estimate, is the robust boundary parameter (i.e., is , , or );

[0093] S34, generate the corrected local state estimate: After robust correction, generate the corrected local state estimate , and the corrected state estimate contains higher precision, can better reflect the actual state of the underwater vehicle, and has better suppression ability for abnormal data.

[0094] S4 includes:

[0095] S41, obtain the corrected local state estimate and sensor health metrics: The local state estimate of each sub-filter after robust correction is used as the input. At this time, the local state estimate already has high precision and can reflect the actual state of the underwater vehicle. Meanwhile, obtain the sensor health metrics , , and of each sub-filter. These metrics reflect the working state, measurement accuracy, and the influence of the current environment of each sensor. The sensor health metrics can be evaluated through the historical performance, current state, and measurement error of the sensor;

[0096] S42, calculate the residual anomaly: Based on the corrected local state estimate and sensor health metrics, calculate the residual anomalies , , and of the four sub-filters. The residual anomaly is used to measure the difference between the actual measurement value and the estimated value. The calculation formula is as follows:

[0097] ;

[0098] ;

[0099] ;

[0100] ;

[0101] Among them, is the value actually measured by the sensor, , , and are the predicted values calculated by the first, second, third, and fourth sub - filters based on the preliminary state estimation respectively, , , and are the residual abnormality degrees corresponding to the four sub - filters respectively;

[0102] The residual abnormality degree reflects the deviation between the measurement and the estimation. A large abnormal value indicates that there may be a problem with the sensor measurement;

[0103] S43, credibility evaluation using the dynamic weight allocation algorithm: The dynamic weight allocation algorithm is used to evaluate the credibility of the corrected local state estimation of each sub - filter. The credibility evaluation is based on the health indicators , , and as well as the calculated residual abnormality degrees , , and to evaluate the reliability of each sub - filter. The weighting coefficients and are used to reflect the credibility and contribution degree of each sub - filter in the fusion process. The calculation formula is as follows:

[0104] , , , ;

[0105] Among them, , , and are the sensor health indicators of the first, second, third, and fourth sub - filters respectively, , , and are the residual abnormality degrees of the first, second, third, and fourth sub - filters respectively, , , and are the dynamic weighting coefficients of the first, second, third, and fourth sub - filters;

[0106] The weighting coefficients reflect the contribution degree of each sub - filter in the data fusion process. When the sensor health degree is high and the residual abnormality degree is small, the weighting coefficient of this sub - filter is large, indicating that its credibility in data fusion is high;

[0107] S44, Normalize the weighting coefficients: Normalize the dynamic weighting coefficients to ensure that the sum of the weighting coefficients is 1. The specific formula is as follows:

[0108] ;

[0109] ;

[0110] ;

[0111] ;

[0112] where, , , and are the normalized dynamic weighting coefficients;

[0113] Dynamically adjust the weighting coefficients , , and according to the real - time environment and task requirements to optimize the data fusion process. Under some specific conditions (such as sensor failure or environmental change), the weighting coefficients can be further optimized by adjusting the sensor health degree index and abnormality weight to ensure optimal data fusion in a dynamic environment.

[0114] S5 includes:

[0115] S51, Judge whether the residual abnormality degree exceeds the preset threshold: According to the calculated residual abnormality degrees , , and , compare with the preset thresholds , , and to judge whether it exceeds the preset threshold. The specific judgment conditions are as follows:

[0116] , ; or

[0117] Among them, 、 、 and are respectively set thresholds, representing the maximum acceptable range of abnormality;

[0118] S52, activate the topology reconstruction mechanism: If the residual abnormality exceeds the preset threshold, activate the topology reconstruction mechanism, which aims to adjust the data interaction path between sub-filters to handle abnormal data and optimize the filtering effect. Specifically, the reconstruction mechanism reconfigures the data flow direction and information sharing method between sub-filters, enabling the system to process abnormal data more effectively and reduce errors in data fusion;

[0119] S53, reconfigure the data interaction path between sub-filters: According to the source and type of abnormal data, reconfigure the data interaction path between sub-filters. Under normal circumstances, each sub-filter shares data and performs fusion in a predetermined order and manner. When an abnormality occurs, the topology reconstruction mechanism adjusts the data flow direction between sub-filters, enabling sub-filters with higher credibility to participate more in data fusion while reducing the influence of sub-filters more affected by abnormal data;

[0120] Specific reconfiguration strategies include:

[0121] Selective data flow: If the abnormality of a certain sub-filter is too large, reduce the data transmission of this sub-filter to the main filter;

[0122] Information weighting: According to the abnormality and reliability of sub-filters, adjust the weighting ratio of data, so that reliable data occupies a greater weight in the fusion process;

[0123] S54, optimize the filtering effect: The reconfigured topology optimizes the data flow between sub-filters, making information transmission more efficient and accurate. Verify the optimized filtering effect through the following methods:

[0124] Reduce the filtering error: Evaluate the reduction of the filtering error by comparing the corrected estimated value and the actual measured value;

[0125] Improve the stability of data fusion: Ensure that the data fusion process after topology reconstruction is more stable and can adapt to dynamic environments and abnormal data;

[0126] S55, confirm the effectiveness of topology reconstruction: After each topology reconstruction, perform a performance evaluation to confirm that the reconstructed structure can effectively improve the filtering effect. The evaluation criteria include:

[0127] Filtering accuracy: Calculate the error between the corrected state estimate and the actual value, and confirm whether the error is within the acceptable range;

[0128] System robustness: By simulating different types of abnormal situations, verify the stability and robustness of the system when processing abnormal data.

[0129] If the evaluation result shows that the reconstructed topological structure has a good effect, continue to use the new configuration; otherwise, make further adjustments or restore the original configuration.

[0130] S6 includes:

[0131] S61, Receive the locally weighted state estimate: Receive the locally weighted state estimate generated by the dynamic weight allocation algorithm, which comes from the first and second sub-filters respectively. At this time, the locally weighted state estimate has considered the credibility of each sub-filter and has been weighted and corrected according to its measurement error and health status. The main filter receives these four weighted locally weighted state estimates as inputs and is ready for fusion processing;

[0132] S62, Apply the weighted fusion algorithm: The main filter uses the weighted average algorithm to fuse the received locally weighted state estimates. Using the weighted average algorithm, combined with the weighted coefficients of each sub-filter, calculate the finally fused state estimate. The weighted fusion calculation formula is as follows:

[0133] ;

[0134] Among them, is the fused state estimate output by the main filter, , , and are the locally weighted state estimates of the first, second, third, and fourth sub-filters after dynamic weighting, , , and are the normalized dynamic weighting coefficients,

[0135] This weighted fusion algorithm adjusts the respective contributions according to the credibility and correction effect of the sub-filters to generate an optimal fusion result;

[0136] S63, Generate the final navigation and positioning parameters: Based on the fused state estimate, generate the final navigation and positioning parameters, specifically including:

[0137] Position: According to the position information in the state estimate, calculate the current accurate position of the underwater vehicle;

[0138] Speed: Calculate the current motion speed of the underwater vehicle based on the speed information in the state estimation;

[0139] Acceleration: Calculate the acceleration of the underwater vehicle according to the acceleration information in the state estimation;

[0140] Heading: Calculate the heading direction of the underwater vehicle based on the heading angle information in the state estimation;

[0141] These final navigation and positioning parameters will provide the precise position information and dynamic state of the underwater vehicle, which are used to guide navigation decisions and control systems.

[0142] The present invention covers any alternatives, 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 the description of these details. Additionally, well-known methods, processes, procedures, components, and circuits are not described in detail to avoid unnecessary confusion to the essence of the present invention.

[0143] 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. Based on elastic adaptive federation The filtered underwater navigation positioning data fusion method is characterized by: The following steps are involved: S1, obtaining original navigation parameters in real time through an inertial measurement unit, a Doppler log, an ultra-short baseline beacon and a depth gauge on the underwater vehicle, wherein the original navigation parameters include acceleration, angular velocity, speed information, position information and depth information of the underwater vehicle; S2, inputting the original navigation parameters obtained in S1 into a preset elastic adaptive federated filtering architecture, wherein the elastic adaptive federated filtering architecture includes a main filter and four sub-filters, and a dynamic coupling relationship is established between the sub-filters through an elastic adaptive mechanism to generate a preliminary state estimation; S3, the preliminary state estimate of each sub-filter is performed Robust correction, based on the elasticity factor adjustment at the current moment The robust boundary parameters of the filter generate the corrected local state estimate; S4, using a dynamic weight allocation algorithm to perform credibility assessment on the local state estimates of each sub-filter corrected in S3, calculating residual abnormality based on the corrected state estimates, and generating a dynamic weighting coefficient in combination with the sensor health index for optimizing the weight of data fusion; S5, if the residual abnormality calculated in S4 exceeds a preset threshold, the topology reconstruction mechanism is triggered to reconfigure the data interaction path between the sub-filters to deal with abnormal data and optimize the filtering effect; S6, inputs the dynamically weighted local state estimation generated by S4 into the main filter for fusion processing, and outputs final navigation positioning parameters, which include the accurate position, speed, acceleration and heading information of the underwater vehicle, for achieving high-precision navigation positioning.

2. The elastic adaptive federation according to claim 1 The filtered underwater navigation positioning data fusion method is characterized by: The S1 includes: S11, preset the sensor working mode of the underwater vehicle: preset the working mode of each sensor in the inertial measurement unit, Doppler log, ultra-short baseline beacon and depth gauge of the underwater vehicle, including data collection frequency, collection accuracy and data transmission mode; S12, collecting acceleration and angular velocity information of an inertial measurement unit: using an inertial measurement unit to collect acceleration and angular velocity data of the underwater vehicle in real time; S13, collecting speed information of a Doppler log: using a Doppler log to measure the relative speed of the underwater vehicle and the relative speed of the water flow, thereby obtaining the speed information of the vehicle in real time; S14, collecting the position information of the ultra-short baseline beacon: obtaining the position information of the beacon by using the azimuth-distance method; S15, collecting depth information of the depth meter: using the depth meter to obtain the depth information of the underwater vehicle in real time; S16, data synchronization and timestamp calibration: time synchronization of the collected acceleration, angular velocity, speed information, position information and depth information. During the synchronization process, the timestamp technology is used to align the data of each sensor; S17, generating an original navigation parameter set: integrating the acceleration, angular velocity, speed information, position information and depth information after data synchronization and time stamp calibration into an original navigation parameter set.

3. The elastic adaptive federation according to claim 2 The filtered underwater navigation positioning data fusion method is characterized by: The S2 includes: S21, initializing a flexible adaptive federated filtering architecture: in a navigation system of an underwater vehicle, initializing a flexible adaptive federated filtering architecture, wherein the flexible adaptive federated filtering architecture comprises a main filter and four sub-filters, wherein the main filter is used to receive output results of the four sub-filters and perform final fusion, and the four sub-filters respectively receive data from different sensors to perform preliminary state estimation; S22, setting the input data source of the sub-filter: setting the input data source for each sub-filter according to different sensors; The first sub-filter receives data from the inertial measurement unit, including the acceleration and angular velocity of the underwater vehicle; The second sub-filter receives data from the Doppler log, including the velocity information of the underwater vehicle; The third sub-filter receives position information from the ultra-short baseline positioning system; The fourth subfilter receives data from the depth gauge, including the depth information of the underwater vehicle; Each sub-filter processes the assigned sensor data and independently generates a preliminary state estimate; S23, establishing an elastic adaptive mechanism: establishing a dynamic coupling relationship between the four sub-filters through an elastic adaptive mechanism; S24, data fusion and preliminary state estimation generation: Under the action of the elastic adaptive mechanism, each sub-filter filters its own input data and outputs a preliminary state estimation.

4. The elastic adaptive federation according to claim 3 The filtered underwater navigation positioning data fusion method is characterized by: The S3 includes: S31, obtaining a preliminary state estimate and elasticity factor: obtaining the generated preliminary state estimate and the elasticity factor determined in the elastic adaptation mechanism as Robust correction input; S32, calculate robust boundary parameters: According to the determined elasticity factor, calculate Robust boundary parameters of filtering; S33, proceed Robust correction: Based on the calculated robust boundary parameters, the initial state estimate of each sub-filter is Robust correction; S34, generating a revised local state estimate: After robust correction, a corrected local state estimate is generated.

5. The elastic adaptive federation according to claim 4 The filtered underwater navigation positioning data fusion method is characterized by: The S4 includes: S41, obtain the corrected local state estimation and sensor health index: The robustly corrected local state estimate of each sub-filter is taken as input, and at the same time, the sensor health indicator of each sub-filter is obtained; S42, calculating residual constant: calculating residual constant of each sub-filter based on the corrected local state estimation and sensor health indicator; S43, using a dynamic weight allocation algorithm to perform credibility assessment: using a dynamic weight allocation algorithm to perform credibility assessment on the corrected local state estimate of each sub-filter, wherein the credibility assessment is based on a health index and a calculated residual abnormality to assess the reliability of each sub-filter; S44, normalizing weighting coefficients: performing normalization processing on the dynamic weighting coefficients.

6. The elastic adaptive federation according to claim 5 The filtered underwater navigation positioning data fusion method is characterized by: The S5 includes: S51, determining whether the residual abnormality exceeds a preset threshold: comparing the calculated residual abnormality with a preset threshold to determine whether it exceeds the preset threshold; S52, activating the topology reconstruction mechanism: if the residual abnormality exceeds a preset threshold, activating the topology reconstruction mechanism; S53, reconfiguring the data interaction path between the sub-filters: reconfiguring the data interaction path between the sub-filters according to the source and type of the abnormal data; S54, optimize filtering effect: the reconfigured topology optimizes the data flow between sub-filters and verifies the optimized filtering effect; S55, confirming the effectiveness of topology reconstruction: performing performance evaluation after each topology reconstruction.

7. The elastic adaptive federation according to claim 6 The filtered underwater navigation positioning data fusion method is characterized by: The S6 includes: S61, receiving a dynamically weighted local state estimate: receiving a dynamically weighted local state estimate generated by a dynamic weight allocation algorithm; S62, applying a weighted fusion algorithm: the main filter uses a weighted average algorithm to fuse the received dynamically weighted local state estimates; S63, generating final navigation positioning parameters: generating final navigation positioning parameters based on the fused state estimation.

Images