Multi-UUV cooperative water surface target positioning method based on adaptive vector information distribution factor information fusion, program, device and storage medium

Abstract

The invention discloses a multi-UUV (Unmanned Underwater Vehicle) cooperative water surface target positioning method based on adaptive vector information distribution factor information fusion, a program, equipment and a storage medium, and relates to the technical field of underwater unmanned vehicle cooperative navigation and target positioning. The method comprises the following steps: firstly, establishing a multi-UUV cooperative positioning system model, and initializing target motion state parameters and a noise covariance matrix; a main-sub-filter two-stage federated architecture is adopted, a sub-filter performs local state estimation based on UUV position and target distance observation, and a main filter fuses global information and dynamically optimizes weight distribution; and calculating a standardized residual error to evaluate the observation reliability, constructing an adaptive vector information distribution factor, dynamically adjusting the contribution degree of a sub-filter by using an exponential decay function, and suppressing outlier interference. According to the method, the target positioning precision and robustness are remarkably improved in a dynamic environment, and the method is suitable for a multi-UUV cooperative positioning task in a complex ocean scene.

Description

TECHNICAL FIELD

[0001] The application relates to the technical field of underwater unmanned vehicle cooperative navigation and target positioning, and in particular to a multi-UUV cooperative water surface target positioning method based on adaptive vector information distribution factor information fusion, a program, equipment and a storage medium. BACKGROUND

[0002] The underwater unmanned vehicle (UUV) cooperative target positioning technology is a core research direction in the fields of ocean exploration and underwater rescue. Traditional multi-UUV cooperative positioning methods usually rely on centralized filtering architecture or fixed-weight distributed fusion strategy, which is prone to sensor observation quality differences and insufficient robustness in dealing with outliers. In recent years, multi-source sensor fusion methods based on federated filtering have been introduced into the field of multi-UUV cooperative positioning, balancing the calculation efficiency and accuracy through distributed estimation and global fusion. However, traditional federated filtering still has some problems, such as the inability of adaptive information distribution and the lack of corresponding suppression mechanism for outliers. To this end, the application proposes a multi-UUV cooperative positioning method based on adaptive vector information distribution factor and federated filtering framework, which improves the positioning accuracy and robustness in complex underwater environments through dynamic information distribution weight and outlier suppression mechanism. SUMMARY

[0003] The application aims to provide a target positioning method, program, equipment and storage medium that realizes high-precision fusion of water surface target position information of multi-UUV by constructing a multi-UUV cooperative positioning system model and combining a federated filtering framework.

[0004] The application proposes a multi-UUV cooperative water surface target positioning method based on adaptive vector information distribution factor information fusion, program, equipment and storage medium, the core of which includes the following technical solutions:

[0005] A multi-UUV cooperative water surface target positioning method based on adaptive vector information distribution factor information fusion includes the following steps:

[0006] Step 1: Obtain the high-precision position of each UUV and the distance between each UUV and the target, and construct a multi-UUV federated filtering cooperative target positioning model containing a master filter and a sub-filter, each UUV corresponding to a sub-filter; initialize the state prediction vector and error covariance matrix of the target.

[0007] Step 2: Each filter simultaneously performs local state estimation on its own UUV and calculates the measurement residual of each sub-filter, which is normalized to obtain the final normalized innovation.

[0008] Step 3: Calculate the adaptive vector information distribution factor according to the final normalized innovation and the state prediction covariance matrix of each sub-filter.

[0009] Step 4: The master filter fuses the local state estimates of each sub-filter according to the adaptive vector information allocation factor to obtain the global state estimate.

[0010] Step 5: Output the global state estimate as the positioning result of the water surface target.

[0011] Further, the state prediction process model of each sub-filter in the multi-UUV federated filtering cooperative target positioning model of step 1 comprises:

[0012]

[0013]

[0014]

[0015]

[0016] wherein, is the state prediction vector of the sub-filter at the time is the state transition matrix of the sub-filter at the time is the process noise of the sub-filter at the time is the state prediction covariance matrix of the sub-filter at the time is the process noise covariance matrix of the sub-filter at the time is the observation matrix of the sub-filter at the time is the transpose calculation, is the actual position of the target relative to the radar measured by the sub-filter at the time is the measurement noise of the sub-filter at the time is the actual position of the radar, is the distance between the actual position of the radar and the estimated UUV position, is the observation matrix of the sub-filter at the time is the actual position of the radar, is the distance between the actual position of the radar and the estimated UUV position, is the measurement noise of the sub-filter at the time

[0017] Further, step 2 specifically comprises the following steps:

[0018] Step 2.1: Calculate the one-step state prediction vector of each sub-filter​​ and the one-step state prediction covariance matrix .

[0019]

[0020]

[0021] wherein, is the sub-filter is the input matrix at time .

[0022] Step 2.2: Each sub-filter performs measurement update, and updates the state prediction vector and the state prediction covariance matrix, to obtain the local state estimation of each sub-filter.

[0023]

[0024]

[0025] wherein, is the updated state prediction vector, is the updated state prediction covariance matrix, is the Kalman gain.

[0026] Step 2.3: Calculate the measurement residual of each sub-filter , and normalize it to obtain the preliminary normalized innovation .

[0027]

[0028]

[0029] wherein, is the innovation covariance matrix.

[0030] Step 2.4: Standardize the preliminary normalized innovation to obtain the standardized residual .

[0031] Step 2.5: Determine whether the standardized residual value is greater than the threshold value. If yes, adjust the preliminary normalized innovation by an exponential decay function to control the sensitivity to outliers, to obtain the final normalized innovation , and execute Step 3; if not, directly take the preliminary normalized innovation as the final normalized innovation , and execute Step 3.

[0032] Further, the method of adjusting the preliminary normalized innovation by an exponential decay function to control the sensitivity to outliers in Step 2.5 specifically includes:

[0033]

[0034] wherein, is the attenuation coefficient.

[0035] Further, the step 3 specifically comprises the following steps:

[0036] Step 3.1: Calculate the information amount of each sub-filter according to the updated state prediction covariance matrix .

[0037]

[0038] .

[0039] Step 3.2: Calculate the information reliability weight by the attenuation exponential function according to the final normalized innovation . .

[0040] .

[0041] Step 3.3: Calculate the scalar information allocation factor according to the information reliability weight and the information amount of each sub-filter . .

[0042] .

[0043] Step 3.4: Extend the scalar information allocation factor to the vector information allocation factor .

[0044]

[0045] wherein, is the identity matrix.

[0046] Further, the calculation method of the global state estimation of the step 4 specifically comprises:

[0047]

[0048]

[0049] wherein, is the global state prediction covariance matrix at the time , and is the number of UUVs.

[0050] A computer device comprising a memory, a processor and a computer program stored on the memory, the processor executing the computer program to implement the steps of the above method.

[0051] A computer-readable storage medium having stored thereon a computer program which, when executed by a processor, implements the steps of the above method.

[0052] A computer program product comprising computer instructions which, when executed by a processor, implement the steps of the above method.

[0053] The beneficial effects of the present application are:

[0054] Compared with the prior art, the present application is aimed at the problems of time-varying radar sensor observation noise and frequent outliers in multi-UUV cooperative positioning. Firstly, through an adaptive fusion mechanism of dynamic information matrix trace, the information matrix trace value of each sub-filter is calculated in real time, and the reciprocal of the state estimation accuracy is taken as the weight reference to dynamically adjust the information distribution factor, and the differentiated weighted fusion of the local estimation results of multi-UUV is realized in combination with the federal filter architecture, so as to balance the contribution degree of high-precision and low-precision sensors and improve the global positioning accuracy. Secondly, an outlier detection mechanism based on standardized residual statistics is introduced, the abnormal state of the sensor is determined by a preset threshold, the information distribution factor of the abnormal sensor is weighted and attenuated by using an exponential decay function, and the covariance matrix of the effective sensor is feedback corrected, so as to enhance the fault tolerance and robustness of the system under complex interference. The present application effectively solves the error accumulation problem caused by the traditional fixed weight fusion method, and can still maintain stable positioning output through adaptive weight adjustment when the radar signal is disturbed, thereby improving the tracking accuracy, anti-interference ability and system reliability of the surface target in a dynamic environment, and providing key technical support for underwater cooperative detection tasks. BRIEF DESCRIPTION OF DRAWINGS

[0055] Figure 1 The flowchart of the present application is shown.

[0056] Figure 2 The moving relationship diagram of the UUV and the underwater detection target of the present application is shown.

[0057] Figure 3 The adaptive feedforward vector factor and feedback information factor design framework algorithm flowchart of the present application is shown. DETAILED DESCRIPTION

[0058] The following will be described in conjunction with the accompanying Figure 1 The present application is further described.

[0059] The technical scheme of the multi-UUV cooperative surface target positioning method based on adaptive vector information distribution factor information fusion of the present application is as follows:

[0060] Step 1: Obtain the high-precision position of each UUV and the distance between each UUV and the target, construct a multi-UUV federated filtering collaborative target localization model containing a main filter and sub-filters, with each UUV corresponding to a sub-filter; initialize the target's state prediction vector and error covariance matrix.

[0061] The state prediction process model for each sub-filter in the multi-UUV federated filtering cooperative target localization model includes:

[0062]

[0063]

[0064]

[0065]

[0066] in, Sub-filter exist The state prediction vector at time t. Sub-filter exist The state transition matrix at time t, Sub-filter exist Time-based process noise, Sub-filter exist The state prediction covariance matrix at time t. For transpose calculation, Sub-filter exist The process noise covariance matrix at time step 1. Sub-filter exist The actual position of the target relative to the radar is measured at all times. Sub-filter exist The observation matrix at time, Indicates the actual location of the radar. This represents the distance between the actual radar location and the estimated UUV location. Sub-filter exist Measurement noise at any given moment.

[0067] Step 2: Each filter simultaneously performs local state estimation on its own UUV and calculates the measurement residual of each sub-filter, normalizes it, and obtains the final normalized information.

[0068] Step 2 specifically comprises the following steps:

[0069] Step 2.1: Calculate the one-step state prediction vector of each sub-filter and the one-step state prediction covariance matrix .

[0070]

[0071]

[0072] wherein, is the input matrix of the sub-filter at the time .

[0073] Step 2.2: Each sub-filter performs measurement update, and the state prediction vector and the state prediction covariance matrix are updated to obtain the local state estimation of each sub-filter.

[0074]

[0075]

[0076] wherein, is the updated state prediction vector, is the updated state prediction covariance matrix, is the Kalman gain.

[0077] Step 2.3: Calculate the measurement residual of each sub-filter , and perform normalization to obtain the preliminary normalized innovation .

[0078]

[0079]

[0080] wherein, is the innovation covariance matrix.

[0081] Step 2.4: Standardize the preliminary normalized innovation to obtain the standardized residual .

[0082] Step 2.5: Determine whether the standardized residual value is greater than the threshold value. If yes, adjust the preliminary normalized innovation by an exponential decay function to control the sensitivity to outliers, and obtain the final normalized innovation , and then execute Step 3; if not, directly take the preliminary normalized innovation as the final normalized innovation , and execute Step 3.

[0083] The method of adjusting the preliminary normalized innovation by controlling the sensitivity to outliers through an exponential decay function specifically comprises:

[0084]

[0085] wherein, is a decay coefficient.

[0086] Step 3: calculating an adaptive vector information allocation factor according to the final normalized innovation and the state prediction covariance matrix of each sub-filter.

[0087] Step 3 specifically comprises the following steps:

[0088] Step 3.1: calculating the information amount of each sub-filter according to the updated state prediction covariance matrix .

[0089]

[0090]

[0091] Step 3.2: calculating an information reliability weight through an exponential decay function according to the final normalized innovation .

[0092]

[0093] Step 3.3: calculating a scalar information allocation factor according to the information reliability weight and the information amount of each sub-filter .

[0094]

[0095] Step 3.4: expanding the scalar information allocation factor into a vector information allocation factor .

[0096]

[0097] wherein, is a unit matrix.

[0098] Step 4: the master filter fuses the local state estimates of each sub-filter to obtain a global state estimate according to the adaptive vector information allocation factor.

[0099] The calculation method of the global state estimate specifically comprises:

[0100]

[0101]

[0102] wherein, is the global state prediction covariance matrix at time is the number of UUVs.

[0103] Step 5: output the global state estimation as the positioning result of the surface target.

[0104] Embodiment

[0105] The following embodiments will help those skilled in the art to further understand the present application, but do not limit the present application in any form. It should be pointed out that those skilled in the art can make several changes and improvements without departing from the concept of the present application. These all belong to the protection scope of the present application.

[0106] As shown in Figure 1 , a multi-UUV cooperative surface target positioning method based on adaptive vector information allocation factor information fusion includes the following steps:

[0107] Multi-UUV cooperative surface target positioning first determines the accurate position information of each UUV, which is considered known in this patent, and then constructs a federal Kalman filter framework for the multi-UUV system, performs initial information allocation, and iteratively optimizes the estimation through time update and measurement update of the system. The measurement residual is standardized, the feedforward information allocation factor and the feedback information allocation factor are designed according to the standardized measurement residual, and the standardized residual value is compared with the preset threshold. When the preset threshold is exceeded, it is considered that the system has measurement outliers at this time, and the outlier suppression mechanism reconstructs the information allocation factor through an exponential decay factor at this time, reduces the information allocation weight of the radar sensor that introduces the measurement outlier, and finally fuses the global information to output a high-precision navigation result.

[0108] Embodiment 1: Multi-UUV positioning system modeling and parameter initialization

[0109] In this patent, the position diagram of the multi-UUV system and the target to be positioned is shown in Figure 2 , we believe that each UUV in the multi-UUV cooperative positioning system knows its own accurate positioning,

[0110] and the target to be positioned moves along a circle with a radius of 500m, and the speed of the circular motion is , and has the following motion equation:

[0111]

[0112] In the formula, The angular velocity of the circular motion is represented, and according to this, we can assume a target object doing standard circular motion for subsequent positioning detection.

[0113] In the federated filtering process, the state prediction process equation of the sub-filter is as follows:

[0114]

[0115]

[0116] wherein, UUV number is represented, The state prediction of the i-th UUV at the current time, i.e., the predicted target position, is represented, is the process noise, is the covariance matrix of the process noise. is defined as follows:

[0117]

[0118] The sampling time is represented.

[0119] For the measurement process of the system, the measurement of the system can be represented in the following mathematical form:

[0120]

[0121]

[0122] The actual position of the target object relative to the radar measured by the radar is represented, The distance between the actual position of the radar and the estimated UUV position is represented, The actual position of the radar is represented, The measurement noise is represented.

[0123] Embodiment 2: Construction of federated filtering architecture of multi-UUV positioning system

[0124] As a distributed data fusion method, the workflow of federated filtering can be divided into the following processes:

[0125] Information distribution:

[0126] The information distribution process can also be called the information feedback process, which occurs between the sub-filter and the main filter to distribute the information of the system, satisfying the following equation:

[0127]

[0128]

[0129]

[0130] In the formula, The covariance matrix represents the estimation error. Represents the process noise covariance matrix. The state vector representing the sub-filter, indicated by the upper right subscript. This indicates the number of each sub-filter. The main filter is shown in the lower right corner. Indicates time, The information allocation coefficient, or information allocation factor, satisfies the following conditions: It always holds true and satisfies the information allocation principle:

[0131]

[0132] Updated in time:

[0133]

[0134]

[0135] Measurement Update:

[0136] Since there are no measurement values ​​in the main filter, measurement updates only occur in the sub-filters, while the main filter only performs time updates.

[0137]

[0138]

[0139] Information fusion:

[0140] The local estimation information of each sub-filter in the federated filter is fused in the following way to obtain the global optimal estimate:

[0141]

[0142]

[0143] in This represents the optimal estimate. This represents the optimal estimated covariance matrix.

[0144] Through the above steps of information allocation, time update, measurement update and information fusion, the system realizes parallel state estimation of each sub-filter and centralized information fusion of the main filter, thereby effectively integrating multi-source observation information and approximately reconstructing the global optimal solution of the system state.

[0145] Embodiment 3: Dynamic adaptive information allocation factor and outlier rejection fusion design

[0146] In the actual UUV cooperative positioning process, due to the different degrees of uncertainty and system deviation of each UUV-mounted radar in outputting positioning information, the observation quality is uneven. Therefore, in the information fusion stage, the reasonable information weight needs to be allocated to the corresponding sub-filter according to the reliability and estimation accuracy of the radar detection results. At the same time, the same state of different subsystems also has corresponding different characteristics, so the scalar allocation is also difficult to represent the characteristics of different states in the subsystem. This weight allocation process needs to follow the information conservation principle to ensure that the measurement information of each sensor is fairly and effectively used in the fusion process, so as to achieve the optimal fusion accuracy. At the same time, the same state of different subsystems also has corresponding different characteristics, so the scalar allocation is also difficult to represent the characteristics of different states in the subsystem. Therefore, the patent proposes a positioning method based on adaptive vector information allocation factor, which dynamically adjusts the information contribution degree of each UUV system according to the observation performance of each UUV system. At the same time, combined with the outlier rejection mechanism, when detecting observation abnormalities, the corresponding weight is automatically reduced to suppress the interference of abnormal data on global state estimation and improve the robustness and positioning stability of the system.

[0147] Outlier rejection is realized by normalized information detection:

[0148]

[0149] In the formula represents the measurement value at the current time, is the distance between the actual position of the radar and the estimated UUV position, represents the predicted state, represents the number of radars.

[0150] Since the measurement model we designed is nonlinear, we construct the observation matrix by linearization

[0151]

[0152] Innovation covariance matrix:

[0153]

[0154] For this, we perform innovation normalization operation to satisfy the following equation:

[0155]

[0156] Normalized innovation The significance of the deviation between the actual measurement and the predicted value is used to measure the statistical significance of the deviation, which reflects the abnormality of the observation deviation relative to the expected noise level. When the normalized information is significantly large, it usually indicates that there is a potential outlier, and at this time, the adaptive dynamic information allocation factor needs to be designed to suppress the abnormal observation, thereby improving the robustness and reliability of the overall information allocation process.

[0157] The design of the dynamic information allocation factor combines the information quantity and the information reliability, including the feedback information allocation factor and the feedforward information allocation factor , but in this patent, the design ideas of the two are equivalent, so here we choose to elaborate on the feedback information factor. The information quantity of the sub-filter is defined by the trace of the inverse of the covariance matrix:

[0158]

[0159] wherein, represents the covariance matrix of the th sub-filter at the th time.

[0160] The information reliability weight is designed, that is, an exponential decay function is used to suppress the influence of abnormal sensors:

[0161]

[0162] represents the decay coefficient, which controls the sensitivity of the weight to outliers.

[0163] The design of the information allocation factor is the weighted normalization result of the information quantity and the reliability:

[0164]

[0165] At this time, we only need to multiply the scalar feedforward and feedback information allocation factors with the unit matrix to expand the scalar to a vector.

[0166]

[0167] The designed feedforward information allocation factor and the feedback information allocation factor are introduced into the information allocation and information fusion process of the federated filter, and an improved information fusion model is constructed, and then new information allocation and fusion calculation formulas are formed, realizing dynamic weighting of local estimation results and optimal reconstruction of global state.

[0168]

[0169]

[0170]

[0171]

[0172] In summary, the present application proposes a multi-UUV cooperative surface target positioning method based on adaptive vector information distribution factors, the core of which is to fuse dynamic information distribution strategy and outlier suppression mechanism. The key technical breakthrough of this method is to adaptively construct feedforward and feedback information distribution factors, realizing high confidence node dominated information fusion, while self-degrading the weight of low confidence observations. By setting a residual threshold, introducing outlier detection and exponential decay suppression mechanism, the observation failure problem of radar sensors in the positioning process is effectively solved. This method significantly improves the reliability and robustness of multi-source information fusion, and shows excellent target positioning accuracy and system stability in complex underwater environments.

Claims

1. A multi-UUV cooperative surface target positioning method based on adaptive vector information distribution factor information fusion, characterized in that Includes the following steps: Step 1: Obtain the high-precision position of each UUV and the distance between each UUV and the target, and construct a multi-UUV federated filtering cooperative target localization model containing a main filter and sub-filters, with each UUV corresponding to a sub-filter; Initialize the target's state prediction vector and error covariance matrix; Step 2: Each filter simultaneously performs local state estimation on its own UUV and calculates the measurement residual of each sub-filter, normalizes it, and obtains the final normalized information. Step 3: Calculate the adaptive vector information allocation factor based on the final normalized innovation and the state prediction covariance matrix of each sub-filter; Step 4: The main filter assigns factors based on adaptive vector information and performs weighted fusion of the local state estimates of each sub-filter to obtain the global state estimate; Step 5: Output the global state estimate as the localization result of the water surface target.

2. The multi-UUV cooperative surface target positioning method based on adaptive vector information distribution factor information fusion according to claim 1, characterized in that, The state prediction process model for each sub-filter in the multi-UUV federated filtering cooperative target localization model described in step 1 includes: wherein is a sub-filter at the state prediction vector at time denotes a sub-filter at the state transition matrix at time is a sub-filter at the process noise at time is a sub-filter at the state prediction covariance matrix at time is a transpose computation is a sub-filter at the process noise covariance matrix at time is a sub-filter at the actual position of the target relative to the radar at time is a sub-filter at the observation matrix at time denotes the actual position of the radar is the distance between the actual position of the radar and the estimated UUV position is a sub-filter at the measurement noise at time 3. The multi-UUV cooperative surface target localization method based on adaptive vector information allocation factor information fusion according to claim 2, characterized in that, Step 2 specifically includes the following steps: Step 2.1: Calculate the one-step state prediction vector for each sub-filter. and one-step state prediction covariance matrix ; in, Sub-filter exist The input matrix at time step; Step 2.2: Each sub-filter is updated with measurements, and the state prediction vector and state prediction covariance matrix are updated to obtain the local state estimate of each sub-filter; in, This is the updated state prediction vector. To predict the covariance matrix of the updated state, Kalman gain; Step 2.3: Calculate the measurement residuals for each sub-filter Then, normalization is performed to obtain the initial normalized information. ; in, The new information covariance matrix; Step 2.4: Apply the initial normalized information Standardize to obtain standardized residuals ; Step 2.5: Determine if the standardized residual value is greater than the threshold. If so, adjust the initial normalized innovation by controlling the sensitivity to outliers using an exponential decay function to obtain the final normalized innovation. Then, proceed to step 3; otherwise, directly use the preliminary normalized new information as the final normalized new information. Then proceed to step 3.

4. The multi-UUV cooperative surface target localization method based on adaptive vector information allocation factor information fusion according to claim 3, characterized in that, Step 2.5 describes a method for adjusting the preliminary normalized information by controlling the sensitivity to outliers using an exponential decay function. This method specifically includes: in, This is the attenuation coefficient.

5. The multi-UUV cooperative surface target localization method based on adaptive vector information allocation factor information fusion according to claim 4, characterized in that, Step 3 specifically includes the following steps: Step 3.1: Calculate the information content of each sub-filter based on the updated state prediction covariance matrix. ; ； Step 3.2: Based on the final normalized information The information reliability weight is calculated using a decay exponential function. ; ； Step 3.3: Based on the aforementioned information reliability weight and the amount of information in each sub-filter Calculate scalar information allocation factor ; ； Step 3.4: Assign factors to scalar information Extended to vector information allocation factor ; in, It is an identity matrix.

6. The multi-UUV cooperative surface target localization method based on adaptive vector information allocation factor information fusion according to claim 5, characterized in that, Step 4 Global State Estimation The specific calculation methods include: in, for The global state prediction covariance matrix at time t. The number of UUVs.

7. A computer device comprising a memory, a processor, and a computer program stored in the memory, characterized in that: The processor executes the computer program to implement the steps of the method according to any one of claims 1 to 7.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that: When executed by a processor, the computer program implements the steps of the method according to any one of claims 1 to 7.

9. A computer program product comprising computer instructions, characterized in that: When executed by a processor, the computer instructions implement the steps of the method according to any one of claims 1 to 7.

Images