A multi-AUV measurement filtering method and system based on factor graph under dynamic topology

Abstract

The present invention provides a multi-AUV measurement filtering method and system based on factor graph under dynamic topology, which belongs to the field of multi-AUV collaborative positioning. In order to solve the problem that wild values ​​will be generated in the measurement information of the existing multi-AUV collaborative positioning system, reducing the positioning performance of the system. In the case where there are wild values ​​in the underwater multi-AUV collaborative positioning measurement information, the present invention introduces the k-means algorithm to the multi-AUV collaborative positioning algorithm based on factor graph under dynamic topology to filter the measurement wild values, thereby ensuring that the underwater collaborative positioning greatly improves the positioning accuracy and robustness while slightly increasing the running time, and ensures the requirements for positioning and navigation accuracy during underwater operations; and through simulation experiments, it can be seen that the present invention improves the positioning accuracy by 64.54%, and is more stable in the face of measurement wild values.

Description

Technical Field

[0001] The present invention relates to the technical field of multi-AUV collaborative positioning, and in particular to a multi-AUV measurement filtering method and system based on factor graph under dynamic topology. Background Art

[0002] AUV is widely used as a powerful tool for ocean exploration and development as well as maintaining ocean safety. AUV is a cable-free underwater robot, which is mainly used for dangerous and complex tasks that are not suitable for human participation underwater. Among them, the collaborative exploration technology based on multiple AUVs to complete tasks together is undoubtedly a major development direction in the field of deep-sea exploration in the future, providing better conditions for completing various difficult underwater operations and deeper ocean exploration. However, due to the multipath effect in actual underwater communications, wild values ​​will be generated in the measurement information of the multi-AUV collaborative positioning system, which will reduce the positioning performance of the system. Therefore, the present invention takes into account the measurement wild value problem existing in actual communication conditions, and improves the existing collaborative positioning algorithm (DSFG) of a multi-AUV system based on a factor graph under a dynamic topology, so as to deal with the wild value problem in the measurement information of the collaborative positioning system. Summary of the invention

[0003] The technical problems to be solved by the present invention are:

[0004] In order to solve the problem that outliers will be generated in the measurement information of the existing multi-AUV collaborative positioning system, which will reduce the positioning performance of the system.

[0005] The present invention adopts the following technical solutions to solve the above technical problems:

[0006] The present invention provides a multi-AUV measurement filtering method based on factor graph under dynamic topology, comprising the following steps:

[0007] S100, dynamic topology detection of the system, obtaining the prior information of the position and relative distance between the master and slave AUVs through sensors;

[0008] S200, updating the measurement information, using the k-means algorithm to determine the outliers, and after determining whether the outliers are outliers, correcting the measurement information using the least squares method;

[0009] S300, after obtaining the prior information of the positions of the master and slave AUVs obtained in step S100 and the corrected measurement information in step S200, the positioning result is estimated using the message passing characteristics of the factor graph.

[0010] Furthermore, in step S200, it includes:

[0011] S210, randomly select K objects as initial cluster centers, calculate the distance between each object and each cluster center, and then assign each object to the cluster center closest to it. The cluster center is recalculated according to the existing objects in the cluster, and the process is repeated until the set termination condition is met;

[0012] At the kth moment in the repetitive process, the measurement information of the previous m moments is selected to perform outlier determination and revision of the measurement information at the current moment according to the outlier determination and revision criteria.

[0013] Furthermore, the criteria for outlier determination and revision include:

[0014] When there are two outliers A and B, outlier B is a relatively far outlier, and outlier A is a relatively close outlier;

[0015] S211, when outlier point B is not identified as an outlier point, but outlier point B can be identified as an outlier point after outlier point A is removed, outlier point A is the point that deviates the farthest after outlier point B is removed, and outlier point B is considered to be an outlier point; and at this time, the measurement information at time k is corrected by other prior measurement information after outlier point A is removed;

[0016] S212, when outlier B is identified as an outlier point, and outlier A is also identified as an outlier point after outlier B is removed and the threshold is reduced, the measurement information at time k is corrected by other prior measurement information after outlier A is removed;

[0017] S212: When outlier B is identified as an outlier point, and outlier A is still not identified as an outlier point after removing outlier B and reducing the threshold, the measurement information at time k is corrected by the first m prior measurement information.

[0018] Furthermore, in step S200, it also includes:

[0019] S220, using the least squares method to correct the outliers determined in step S210,

[0020] Before correcting the measurement information at time k, the prior measurement information is subjected to kernel smoothing.

[0021] Assume that there are m prior measurement information d = {d k-m ,d k-m+1 ,…,d k-1}Kernel smoothing is required:

[0022]

[0023] Among them, h is the kernel smoothing coefficient, which is generally set to 1; m is the number of selected prior information;

[0024] The least squares method is used to fit and correct the measurement information after kernel smoothing, and the correction function is defined as θ1, θ2, θ3 are all coefficients of the correction function, so the cost function can be obtained as:

[0025]

[0026] untie We can get:

[0027]

[0028] In the formula,

[0029]

[0030] Get the correction function at time k That is, the corrected measurement information between the i-th master and slave AUV.

[0031] Furthermore, in step S300, it specifically includes:

[0032] The state variable node X at time k-1 and time k k-1 and X k Through the function node f(X k |X k-1 ) and use the function node Processing measurement information Z k Outliers in the processed measurement information node With X k Through the function node f(Z k |X k ) are connected;

[0033] The global function is decomposed by using factor graph theory, and the node F i For the measurement data set Filter outliers and get Then, through the function node C, d k i Connect to the relative coordinate node Δx k and Δy k , and then by function node A i and B i Coordinate transformation is performed to obtain the coordinate estimation results, which are generated by nodes D and E and each estimation result. and The nodes are connected, and the estimated results in the system at the current moment are integrated to obtain the final estimated result x at the current moment. k and k .

[0034] Further, in step S300:

[0035] Function node f(X k |X k-1 ) to variable node X k The information transmitted and the variable node X k To the function node f(X k |X k-1 ) The information transmitted is:

[0036]

[0037] In the formula, and Represents the state variable X k A priori estimate and variance of Q k is the system process noise covariance matrix, is the measurement noise matrix, The expression is:

[0038]

[0039] In the formula, θ k is the heading angle information corresponding to the AUV at time k, and T is the sampling period;

[0040] Then perform the function node f(Z k |X k ) and the information transfer between related variable nodes is analyzed through the local factor graph model.

[0041] Function Node C i Towards and The information transmitted is:

[0042]

[0043] In the formula, represents the distance between the slave AUV and the i-th master AUV at time k, is the measured distance after outliers are filtered out in step S200, and Respectively represent and The standard deviation of

[0044] A i Pass to variable node and The reliability information is:

[0045]

[0046] Similarly, Bi Pass to variable node and The reliability information is:

[0047]

[0048] Function node D passes to x k The information is:

[0049]

[0050] In the formula, and For x k The variance and expectation of , the specific expression is:

[0051]

[0052] Similarly, the function node E is passed to y k The information expression is:

[0053]

[0054] The positioning estimation result is:

[0055] BI(x k )=BI(f(x k |x k-1 ),x k )·BI(D,x k ) (19)

[0056] BI(y k )=BI(f(y k |y k-1 ),x k )·BI(E,y k ) (20).

[0057] A multi-AUV measurement filtering system based on factor graph under dynamic topology, the system has a program module corresponding to the above steps, and executes the steps in the multi-AUV measurement filtering method based on factor graph under dynamic topology when running.

[0058] A computer-readable storage medium stores a computer program, wherein the computer program is configured to implement the steps of a multi-AUV measurement filtering method based on a factor graph under a dynamic topology when called by a processor.

[0059] Compared with the prior art, the present invention has the following beneficial effects:

[0060] The present invention discloses a multi-AUV measurement filtering method and system based on factor graph under dynamic topology. Based on kmeans algorithm, a set of judgment criteria for measurement wild values ​​are designed by using prior measurement information. Then, the determined measurement wild values ​​are corrected. Then, the measurement filtering module is inserted into the DSFG algorithm to obtain the measurement information. When there are wild values ​​in the underwater multi-AUV collaborative positioning measurement information, the present invention introduces k-mea algorithm to the multi-AUV collaborative positioning algorithm based on factor graph under dynamic topology to filter the measurement wild values, thereby ensuring that the underwater collaborative positioning greatly improves the positioning accuracy and robustness while slightly increasing the running time, and ensuring the requirements for positioning and navigation accuracy during underwater operations. Moreover, through simulation experiments, it can be seen that the present invention improves the positioning accuracy by 64.54%, and is more stable in the face of measurement wild values. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1 It is a flow chart of a multi-AUV measurement filtering method based on factor graph under dynamic topology in an embodiment of the present invention;

[0062] Figure 2 This is a schematic diagram of the first situation that occurs during the measurement wild value determination process in step 2 of the embodiment of the present invention;

[0063] Figure 3 A schematic diagram of the second situation occurring during the measurement wild value determination process in step 2 of the embodiment of the present invention;

[0064] Figure 4 A schematic diagram of a third situation occurring during the measurement wild value determination process in step 2 of an embodiment of the present invention;

[0065] Figure 5 A global factor graph model diagram in an embodiment of the present invention;

[0066] Figure 6 A local factor graph model diagram in an embodiment of the present invention;

[0067] Figure 7 This is a diagram of the master-slave AUV measurement information after adding wild values ​​in the simulation experiment of the present invention;

[0068] Figure 8 This is a diagram of the master-slave AUV measurement information after removing outliers in the simulation experiment of the present invention;

[0069] Fig. 9 This is a comparison of the positioning performance of the DS-AFFG algorithm in the simulation experiment of the present invention.

[0070] The DS-AFFG algorithm is a multi-AUV measurement filtering method based on factor graph under dynamic topology of the present invention, and the A-AECKF algorithm is an adaptive extended dimension EKF algorithm. DETAILED DESCRIPTION

[0071] In the description of the present invention, it should be noted that the terminology nouns in the various embodiments, such as "up", "down", "front", "back", "left", "right", etc., which indicate directions, are only for simplifying the description of the positional relationship based on the drawings in the specification, and do not mean that the referred elements and devices must be operated according to the specific directions and defined operations and methods and structures in the specification. Such directional nouns do not constitute a limitation on the present invention.

[0072] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, specific embodiments of the present invention are described in detail below with reference to the accompanying drawings.

[0073] Specific implementation plan 1: Combine Figures 1 to 6 As shown, the present invention provides a multi-AUV measurement filtering method based on factor graph under dynamic topology, comprising the following steps:

[0074] S100, dynamic topology detection of the system,

[0075] Since the application scenario is a dynamic topology, the number and structure of the master and slave AUVs in the system will change, so the system structure is detected first. At this time, the prior information of the position and relative distance between the master and slave AUVs will be obtained from the sensor;

[0076] S200, update measurement information,

[0077] After receiving the measurement information of the master and slave AUVs, the k-means algorithm is used to determine the outliers and determine which of the three outlier situations it belongs to. Then, the least squares method is used to correct the outliers for the corresponding situation. If it is not determined to be an outlier, the measurement information does not need to be corrected. Through this step, the outliers in the measurement information will be filtered out;

[0078] Specifically include:

[0079] S210. In the collaborative positioning system, the measurement information at consecutive moments will not change too dramatically. If the measurement information has an outlier at a certain moment, it will be mathematically expressed as an outlier from the prior measurement information. Based on this idea, the present invention selects the k-means algorithm to process the outliers in the prior measurement information, including:

[0080] Randomly select K objects as the initial cluster centers, then calculate the distance between each object and each cluster center, and then assign each object to the cluster center closest to it.

[0081] The cluster centers and the objects assigned to them represent a cluster. Each time a sample is assigned, the cluster center is recalculated based on the existing objects in the cluster. This process is repeated until the set termination condition is met.

[0082] Because the prior measurement information will affect the judgment threshold of the outlier, sometimes the outlier cannot be judged. Figures 2 to 4 As shown in the figure, a certain amount of prior information is used for processing when determining the wild value of the measurement information. When a large amount of prior information is selected, the real-time performance of the collaborative positioning system will be affected, while when a small amount of prior information is selected, the determination of the wild value will be affected. At time k, the measurement information of the previous m moments is selected to determine and revise the wild value of the measurement information at the current moment.

[0083] The present invention is generally more conservative when using prior measurement information to judge outliers. Figure 2 The point A in the figure has a smaller deviation than other points, so it is considered that point A is not an outlier point. Figure 3 The distance of point B in the figure is larger than that of other points, so point B can be identified as an outlier point. Figure 4 In the case of the above, point A is identified as a non-outlier point, which will increase the threshold when using prior information to determine whether point B is an outlier point. Point B will also be identified as a non-outlier point at this time. This is why continuous outliers are not easy to identify.

[0084] The k-means algorithm used in the present invention to filter outliers in the measurement information can solve this problem. The specific outlier determination method is as follows:

[0085] The following three situations may occur during the determination of outliers:

[0086] S211, Figure 4 Point B is not identified as an outlier point, but after removing point A, point B can be identified as an outlier point. Point A is the point that deviates the farthest after removing point B. At this time, point B is considered to be an outlier point. At this time, the measurement information at time k is corrected by other prior measurement information after removing point A.

[0087] S212, Figure 4 Point B is identified as an outlier point. After removing point B and reducing a certain threshold, point A is also identified as an outlier point. At this time, the measurement information at time k is corrected by other prior measurement information after removing point A.

[0088] S212, Figure 4 Point B is identified as an outlier point. After removing point B and reducing a certain threshold, point A is still not identified as an outlier point. At this time, the measurement information at time k is corrected by the first m prior measurement information.

[0089] S220, using the least squares method to correct the outliers determined in step S210,

[0090] When detecting outliers and making corrections, sometimes the corrected values ​​still have a large deviation from the actual values, resulting in reduced positioning accuracy. In this case, kernel smoothing can be performed on the priori measurement information before correcting the measurement information at time k. This can prevent the corrected values ​​from diverging. Specifically, the following are included:

[0091] Assume that there are m prior measurement information d = {d k-m ,d k-m+1 ,…,d k-1}Kernel smoothing is required:

[0092]

[0093] Among them, h is the kernel smoothing coefficient, which is generally set to 1; m is the number of selected prior information;

[0094] Next, the least squares principle is used to fit and correct the measurement information after kernel smoothing, and the correction function is defined as: θ1, θ2, θ3 are all coefficients of the correction function, so the cost function can be obtained as:

[0095]

[0096] untie We can get:

[0097]

[0098] In the formula,

[0099]

[0100] At this time, the correction function at time k can be calculated It is the corrected measurement information between the i-th master and slave AUV at this time, that is, the measurement information after removing the wild value;

[0101] S300, after obtaining the prior information of the master and slave AUV positions obtained in step S200 and the corrected measurement information, the positioning result is estimated using the message passing characteristics of the factor graph, specifically including:

[0102] The global factor graph model of the DS-AFFG algorithm is as follows Figure 5 As shown, the state variable node X at time k-1 and time k k-1 and X k Through the function node f(X k |X k-1 ) are connected, the function node The function is to process the measurement information Z k Outliers in the processed measurement information node With X kThrough the function node f(Z k |X k ) are connected;

[0103] The global function is decomposed using factor graph theory, and the local factor graph model is as follows: Figure 6 As shown, the type I structure is the global function f(Z k |X k ) decomposition, including the data fusion model of each master AUV and slave AUV. The type II structure is the measurement information Z k and outlier processing nodes The specific structure includes the ranging information corresponding to the slave AUV and each master AUV at the current moment and the information of outlier filtering;

[0104] Through node F i For the measurement data set Filter outliers and get Then, through the function node C Connect to the relative coordinate node Δx k and Δy k ; Then by function node A i and B i Perform coordinate transformation to obtain coordinate estimation results;

[0105] Since the collaborative positioning system is a dynamic topological structure, nodes D and E are set to solve the problem of dynamic information quantity. Nodes D and E are generated by each estimation result. and The nodes are connected, and the estimated results in the system at the current moment are integrated to obtain the final estimated result x at the current moment. k and k .

[0106] Preferably, the function node f(X k |X k-1 ) to variable node X k The information transmitted and the variable node X k To the function node f(X k |X k-1 ) The information transmitted is:

[0107]

[0108] In the formula, and Represents the state variable X k A priori estimate and variance of Q k is the system process noise covariance matrix, is the measurement noise matrix, The expression is:

[0109]

[0110] In the formula, θ k is the heading angle information corresponding to the AUV at time k, and T is the sampling period;

[0111] Next is the function node f(Z k |X k ) and the information transfer between related variable nodes is analyzed through the local factor graph model.

[0112] Function Node C i Towards and The information transmitted is:

[0113]

[0114] In the formula, represents the distance between the slave AUV and the i-th master AUV at time k. is the measured distance after filtering out the outliers in step 2, and Respectively represent and The standard deviation of

[0115] Function Node A i and B i The role of A is to convert position information. i Pass to variable node and The reliability information is:

[0116]

[0117] Similarly, B i Pass to variable node and The reliability information is:

[0118]

[0119] Function node D passes to x k The information is:

[0120]

[0121] In the formula, and For x k The variance and expectation of , the specific expression is:

[0122]

[0123] Similarly, the function node E is passed to y k The information is also the corresponding expression:

[0124]

[0125] This completes the local function related positioning solution, and the obtained x k and k The corresponding confidence information expectation is the positioning estimation result of the DS-AFFG algorithm:

[0126] BI(x k )=BI(f(x k |x k-1 ),x k )·BI(D,x k ) (19)

[0127] BI(y k )=BI(f(y k |y k-1 ),x k )·BI(E,y k ) (20).

[0128] Specific implementation scheme 2: The present invention provides a multi-AUV measurement filtering system based on factor graph under dynamic topology, which has a program module corresponding to the above steps, and executes the steps in the multi-AUV measurement filtering method based on factor graph under dynamic topology during operation.

[0129] The other combinations and connection relationships of this embodiment are the same as those of the first embodiment.

[0130] Specific implementation scheme three: The present invention provides a computer-readable storage medium, which stores a computer program, and the computer program is configured to implement the steps of a multi-AUV measurement filtering method based on a factor graph under a dynamic topology when called by a processor.

[0131] The other combinations and connection relationships of this embodiment are the same as those of the first embodiment.

[0132] Simulation experiment

[0133] Experimental environment and configuration:

[0134] (1) In the multi-AUV collaborative positioning system, 10 master boats and 20 slave boats are set. For the convenience of research, only the positioning effect of one slave boat is observed here;

[0135] (2) To ensure observability, the master AUV is set to move in a uniform straight line, and the slave AUV is set to move in an S-curve. The speeds of the slave AUV and the master AUV are inconsistent, but they can maintain relative motion.

[0136] (3) To realize the dynamic topology of the system structure, the processing range of the slave AUV is set to a circular area with a diameter of 3000m centered on itself. That is, when operating underwater, the slave AUV only processes the position information broadcast by the master AUV within a diameter range of 3000m. As the relative positions of the AUVs in the system change continuously, the system structure also changes accordingly;

[0137] (4) The specific settings of simulation parameters include: the speed of the master AUV is 2m / s, the speed of the slave AUV is about 6m / s, and the initial coordinates are set to (500m, 500m). The initial heading angle of the master AUV is 45°, and the initial heading angle of the slave AUV is 135°; the total duration of the simulation experiment is set to 1000s, the state update cycle is 1s; the dynamic topology structure detection cycle is 5s; based on the underwater acoustic ranging scenario, the observation noise variance between each master AUV and the slave AUV is set to The ranging variances of velocity and heading angle are and

[0138] (5) External settings in measurement information: ten groups of measurement information, in each group of measurement information, 10% of the time points with an amplitude of ±(0-100)σ(m) are randomly inserted as outliers with random amplitudes to simulate the impact of underwater communication multipath effects on the received measurement information;

[0139] (6) Experimental environment: CPU: 12th Gen Intel(R) Core(TM) i5-1240P 1.70GHz, 16G RAM, Windows 11;

[0140] Performance analysis of the present invention under actual environment

[0141] (1) Analysis of the filtering capability of the present invention for wild values ​​and abnormal values ​​in measurement information

[0142] Figure 7 This is a simulation diagram after adding outliers to the measurement information. It can be seen that there are many outliers in the measurement information. These outliers will have a serious impact on the positioning results. In the present invention, the DS-AFFG algorithm is used to process such outliers. The results after processing are as follows: Figure 8 As shown in the figure, it can be seen that after being processed by the DS-AFFG algorithm, the anomalies in the original measurement information are basically filtered out;

[0143] (2) Comparison of the algorithm used in the present invention with the A-AECKF algorithm

[0144] When there are outliers in the measurement information, the EKF, CKF, and DSFG algorithms will easily cause positioning divergence problems, so they are not compared with the DSFG algorithm here. The A-AECKF algorithm is an adaptive extended-dimensional EKF algorithm previously proposed, which can also handle the problems of reduced positioning accuracy and positioning divergence caused by outliers in the measurement information. In order to verify the performance of the present invention in handling outliers in the measurement information, the DS-AFFG algorithm is compared with the A-AECKF algorithm. The comparison results are shown in the figure. Fig. 9 As shown, it can be seen that the positioning trajectory of the DS-AFFG algorithm is smoother and the positioning accuracy is higher.

[0145] As shown in Table 1 below, the DS-AFFG algorithm can still maintain a high positioning accuracy when there are outliers in the measurement information, and overcomes the problem that the DSFG algorithm is prone to positioning divergence. Compared with the A-AECKF algorithm, the DS-AFFG algorithm improves the positioning accuracy by 64.54%. This is mainly because although the A-AECKF algorithm can handle the outlier problem, the small errors caused by the outliers will accumulate, resulting in poor positioning accuracy, while the DS-AFFG algorithm will be more stable in the face of measurement outliers, and positioning divergence will hardly occur. The running time has decreased by 26.28%, because the DS-AFFG algorithm needs to determine and repair outliers in the measurement information, which will make the running time longer, but compared with the significant improvement in positioning accuracy, the slight reduction in real-time performance is acceptable.

[0146] Table 1 Performance comparison of DS-AFFG algorithm in the presence of measurement outliers

[0147]

[0148] Although the present invention is disclosed as above, the protection scope of the present invention is not limited thereto. Those skilled in the art may make various changes and modifications without departing from the spirit and scope of the present invention, and these changes and modifications will fall within the protection scope of the present invention.

Claims

1. A multi-AUV measurement filtering method based on factor graph under dynamic topology, characterized in that: The following steps are involved: S100, dynamic topology detection of the system, obtaining the prior information of the position and relative distance between the master and slave AUVs through sensors; S200, updating the measurement information, using the k-means algorithm to determine the outliers, and after determining whether the outliers are outliers, correcting the measurement information using the least squares method, including: S210, randomly select K objects as initial cluster centers, calculate the distance between each object and each cluster center, and then assign each object to the cluster center closest to it. The cluster center is recalculated according to the existing objects in the cluster, and the process is repeated until the set termination condition is met; At the kth moment in the repetitive process, the measurement information at the previous m moments is selected to perform outlier determination and revision of the current moment's measurement information according to the outlier determination and revision criteria; The criteria for outlier determination and revision include: When there are two outliers A and B, outlier B is a relatively far outlier, and outlier A is a relatively close outlier; S211, when outlier point B is not identified as an outlier point, but outlier point B can be identified as an outlier point after outlier point A is removed, outlier point A is the point that deviates the farthest after outlier point B is removed, and outlier point B is considered to be an outlier point; and at this time, the measurement information at time k is corrected by other prior measurement information after outlier point A is removed; S212, when outlier B is identified as an outlier point, and outlier A is also identified as an outlier point after outlier B is removed and the threshold is reduced, the measurement information at time k is corrected by other prior measurement information after outlier A is removed; S212, when outlier point B is identified as an outlier point, and outlier point A is still not identified as an outlier point after removing outlier point B and reducing the threshold, the measurement information at time k is corrected by the first m prior measurement information; S220, using the least squares method to correct the outliers determined in step S210, Before correcting the measurement information at time k, the prior measurement information is subjected to kernel smoothing. Assume there are m prior measurement information Kernel smoothing is required: Among them, h is the kernel smoothing coefficient, which is generally set to 1; m is the number of selected prior information; The least squares method is used to fit and correct the measurement information after kernel smoothing, and the correction function is defined as α1, α2, α3 are all coefficients of the correction function, so the cost function can be obtained as: untie We can get: In the formula, Get the correction function at time k That is, the corrected measurement information between the i-th master and slave AUV; S300, after obtaining the prior information of the positions of the master and slave AUVs obtained in step S100 and the corrected measurement information in step S200, the positioning result is estimated using the message passing characteristics of the factor graph.

2. According to claim 1, a multi-AUV measurement filtering method based on factor graph under dynamic topology is characterized in that: In step S300, it specifically includes: The state variable node X at time k-1 and time k k-1 and X k Through the function node f(X k |X k-1 ) and use the function node Processing measurement information Z k Outliers in the processed measurement information node With X k Through the function node f(Z k |X k ) are connected; The global function is decomposed by using factor graph theory, and the node F i For the measurement data set Filter outliers and get Then, through the function node C i Will Connect to the relative coordinate node Δx k and Δy k , and then by function node A i and B i Coordinate transformation is performed to obtain the coordinate estimation results, which are generated by nodes D and E and each estimation result. and The nodes are connected, and the estimated results in the system at the current moment are integrated to obtain the final estimated result x at the current moment. k and k .

3. According to claim 2, a multi-AUV measurement filtering method based on factor graph under dynamic topology is characterized in that: In step S300: Function node f(X k |X k-1 ) to the state variable node X k The information transmitted and the state variable node X k To the function node f(X k |X k-1 ) The information transmitted is: In the formula, and Represents the state variable node X k A priori estimate and variance of Q k is the system process noise covariance matrix, is the measurement noise matrix, The expression is: In the formula, θ k is the heading angle information corresponding to the AUV at time k, and T is the sampling period; Then perform the function node f(Z k |X k ) and the information transfer between related variable nodes is analyzed through the local factor graph model. Function Node C i Towards and The information transmitted is: In the formula, represents the distance between the slave AUV and the i-th master AUV at time k, is the measured distance after outliers are filtered out in step S200, and Respectively represent and The standard deviation of A i Pass to variable node and The reliability information is: Similarly, B i Pass to variable node and The reliability information is: Function node D passes to x k The information is: In the formula, and For x k The variance and expectation of , the specific expression is: Similarly, the function node E is passed to y k The information expression is: The positioning estimation result is: BI(x k )=BI(f(x k |x k-1 ),x k )·BI(D,x k )(19) BI(y k )=BI(f(y k |y k-1 ),y k )·BI(E,y k )(20)。 4. A multi-AUV measurement filtering system based on factor graph under dynamic topology, characterized by: The system has a program module corresponding to the steps of any one of claims 1 to 3 above, and executes the steps of the multi-AUV measurement filtering method based on factor graph under dynamic topology when running.

5. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and the computer program is configured to implement the steps of the multi-AUV measurement filtering method based on factor graph under dynamic topology according to any one of claims 1 to 3 when called by a processor.

Images