A multi-AUV collaborative positioning method and system based on factor graph under dynamic topology
By adopting a factor graph-based method under dynamic topology in a multi-AUV collaborative positioning system, the system topology structure is detected and updated in real time, and the problems of positioning accuracy, real-timeness and efficiency in the existing technology are solved, and the dynamic topology environment is realized. High precision and high efficiency positioning.
Patent Information
- Application Number
- CN202310850613.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2023-07-12
- Publication Date
- 2024-11-15
- Estimated Expiration
- 2043-07-12
AI Technical Summary
The existing multi-AUV collaborative positioning algorithm cannot effectively cope with the dynamic changes in the system topology structure, resulting in the impact of positioning accuracy, real-timeness and efficiency. Especially when nonlinear problems and dynamic changes in structure, there are high-order truncation errors and large calculation amounts. question.
Using the multi-AUV collaborative positioning method based on factor graph under dynamic topology, the system topology structure is detected in real time, the factor graph nodes are added and decreased, and the slave boat state and main boat measurement information are defined as variable nodes, and the coordinated positioning factor graph model under dynamic topology is constructed. , use the summation algorithm to transmit and update information to realize the information transmission and update of global factor graph nodes.
This method takes into account positioning accuracy, efficiency, system expansion and flexibility, reduces the computational complexity of filter estimation, ensures the real-time positioning of AUV, and maintains the accuracy of positioning results in nonlinear problems.
Smart Images

Figure CN116793366B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of AUV collaborative positioning, and in particular to a multi-AUV collaborative positioning method and system based on a factor graph under dynamic topology. Background Art
[0002] Due to the complexity of the underwater environment, the number of AUVs in the multi-AUV collaborative positioning system may change during the execution of underwater tasks, resulting in dynamic changes in the system topology. In the collaborative positioning process under dynamic topology, data fusion becomes a dynamic process. However, the system structure in the existing collaborative positioning algorithm is often static and fixed. Using traditional filtering methods to achieve collaborative positioning under dynamic topology will bring many problems. The Kalman filter (KF) method is suitable for solving linear problems, while positioning under dynamic topology is often a nonlinear problem; the extended Kalman filter (EKF) algorithm that can solve nonlinear problems will introduce high-order truncation errors in the linearization process of nonlinear problems, which may lead to discontinuous estimated probabilities; other positioning algorithms such as the unscented Kalman filter (UKF) have a large amount of calculation and are not suitable for collaborative positioning systems with dynamically changing structures. The particle filter (PF) will experience weight degradation as the flight time increases, resulting in increased positioning errors. In addition, when the system structure changes dynamically, the types and number of data sources for data fusion will also change accordingly, and the system needs to be reconstructed. When the AUV nodes in the system are large or the frequency of changes is high, the real-time performance of the positioning algorithm will be seriously reduced, affecting the system positioning efficiency. Therefore, a multi-AUV collaborative positioning method for dynamic topology is urgently needed. Summary of the invention
[0003] The technical problems to be solved by the present invention are:
[0004] Existing technologies cannot cope with the dynamic changes in the system topology, and cannot take into account the positioning accuracy, real-time positioning, positioning efficiency, and system scalability and flexibility.
[0005] The present invention adopts the following technical solutions to solve the above technical problems:
[0006] The present invention provides a multi-AUV collaborative positioning method based on factor graph under dynamic topology, comprising the following steps:
[0007] S1, collecting the current dynamic topological structure information of the multi-AUV collaborative positioning system;
[0008] S2, update the information of the slave boat and its neighbor master boat;
[0009] S3, initializing the information of the master boat and the slave boat;
[0010] S4, construct a factor graph model of multi-AUV collaborative positioning system;
[0011] The slave boat state variables and the master boat measurement information are defined as variable nodes, the state equation and measurement equation are defined as function nodes, and the state equation function node is used to measure the slave boat state variable node X. k To update the transmission, the measurement equation function node is used to update the main boat measurement information node Z k and slave state variable node X k Perform integration updates;
[0012] S5. Based on the transfer update of the sum-product algorithm in the factor graph model of the multi-AUV collaborative positioning system, information is transferred once in each of the two directions in the factor graph to realize the information transfer and update of the global factor graph nodes;
[0013] S6. The state variable node and the measurement variable node information are integrated and updated to obtain the estimated value of the position information of the slave boat at the current moment.
[0014] Furthermore, in S1, the current dynamic topological structure information of the system is collected in each collection period T, including: the number of master AUVs and slave AUVs, the position information, velocity information v, angular velocity information and heading angle information θ of the master AUV and the slave AUV, and the variance of each collected quantity is calculated and detected, as well as the distance measurement information d between the detection target slave boat and each master boat.
[0015] Furthermore, the initializing the information of the master boat and the slave boat in S3 includes: initializing the position information, speed information v, heading angle information θ of the master boat and the slave boat, and the distance measurement information d between the master boat and the slave boat.
[0016] Furthermore, S5 includes the following process:
[0017] At the kth moment, the system conditional probability density function is decomposed into:
[0018]
[0019] In the formula, N represents the number of main AUV nodes; represents the measurement information of the nth (n=1, 2, ..., N) master AUV; X m,n represents the position information of the nth (n=1, 2, ..., N) master AUV; fi represents the probability factor corresponding to each AUV node, that is:
[0020]
[0021] In the formula, h i (.) represents the measurement function; z iIndicates the true value of the measurement; ∑ i represents the covariance matrix corresponding to the measurement error;
[0022] It is defined that the distance information d, heading angle θ and speed v of the AUV collected by the system all obey Gaussian distribution:
[0023]
[0024] where d i represents the distance measurement information between the slave AUV and the i-th master AUV;
[0025] Through the state equation function node f(X k |X k-1 ) to variable node X k The information transmitted is:
[0026]
[0027] Variable Node X k To the state equation function node f(X k |X k-1 ) The information transmitted is:
[0028]
[0029] In the formula, and Represents the state variable X k A priori estimate and variance of ;
[0030] According to the co-location position equation:
[0031]
[0032] In the formula, (x k ,y k ) represents the coordinates of AUV in the reference coordinate system at time k; v k represents the forward speed of the AUV at time k; θ k represents the heading angle of the AUV at time k; Δt represents the sampling interval;
[0033] The state transition formula is:
[0034]
[0035] In the formula, Q k is the system process noise covariance matrix, is the measurement noise matrix, The expression is:
[0036]
[0037] In the formula, θ k is the heading angle information corresponding to time k;
[0038] Substituting equation (6) and equation (7) into equation (5), we can get the final reliability information:
[0039]
[0040] In the formula, S k The expression is:
[0041]
[0042] Finally, the global factor graph node information transmission and update are realized.
[0043] Furthermore, S6 includes the following process:
[0044] The coordinate difference between the slave AUV and the i-th master AUV in the system at time k is: and Compute Variable Node The reliability information is:
[0045]
[0046] In the formula and Respectively represent and The standard deviation of represents the distance between the slave AUV and the i-th master AUV at time k;
[0047] Compute Variable Node The credibility information transmitted is:
[0048]
[0049] In the formula represent The standard deviation of
[0050] Compute Variable Node and The reliability information is:
[0051]
[0052]
[0053] Compute Variable Node and The reliability information is:
[0054]
[0055]
[0056] Estimate the position of each master boat relative to the slave boat Pass to x k ,Right now:
[0057]
[0058] In the formula, and For x k The variance and expectation of
[0059] Similarly, k The information is:
[0060]
[0061] In the formula, and for y k The variance and expectation of
[0062] Take a weighted average of the slave ship's position estimate and the slave ship's dead reckoning estimate:
[0063]
[0064]
[0065] Furthermore, the distance between the slave AUV and the i-th master AUV at time k in S6 is and and The relationship is:
[0066]
[0067] A multi-AUV collaborative positioning system based on factor graph under dynamic topology, the system has a program module corresponding to the steps of any one of the above technical solutions, and executes the steps of the multi-AUV collaborative positioning method based on factor graph under dynamic topology during operation.
[0068] A computer-readable storage medium stores a computer program, wherein the computer program is configured to implement the steps of the multi-AUV collaborative positioning method based on factor graph under dynamic topology described in any one of the above technical solutions when called by a processor.
[0069] Compared with the prior art, the present invention has the following beneficial effects:
[0070] The present invention discloses a multi-AUV collaborative positioning method and system based on factor graph under dynamic topology. On the basis of the traditional collaborative positioning algorithm based on factor graph, the dynamic topology structure of the real-time detection system is detected, and the factor graph nodes are increased or decreased to realize the plug-and-play function of the sensor. The slave boat status information and the master boat measurement information are defined as variable nodes, and a collaborative positioning factor graph model under dynamic topology based on node optimization is constructed. The positioning accuracy and positioning efficiency of the algorithm and the expansibility and flexibility of the system are taken into account.
[0071] The present invention uses the sum-product algorithm to resolve complex global problems into local problems to simplify the computational complexity, solves the problem of low efficiency of state estimation in filtering estimation, especially in high-order systems, and ensures the real-time positioning of the AUV; at the same time, it uses the advantage of factor graphs in dealing with nonlinear problems to ensure the accuracy of the positioning results. BRIEF DESCRIPTION OF THE DRAWINGS
[0072] Figure 1 It is a flow chart of a multi-AUV collaborative positioning method based on factor graph under dynamic topology in an embodiment of the present invention;
[0073] Figure 2 Schematic diagram of dynamic topological structure change in an embodiment of the present invention; Figure (a) is a schematic diagram before the system structure changes, and Figure (b) is a schematic diagram of the system after dynamic changes;
[0074] Figure 3 Schematic diagram of a global factor graph model in an embodiment of the present invention;
[0075] Figure 4 is f(Z in the embodiment of the present invention k |X k )’s local factor graph model;
[0076] Figure 5 Schematic diagram of the system structure and actual trajectory of the AUV in an embodiment of the present invention;
[0077] Figure 6 It is a schematic diagram of the overall change of the dynamic system structure in an embodiment of the present invention;
[0078] Figure 7 is a positioning result trajectory diagram in an embodiment of the present invention;
[0079] Figure 8 This is a comparison diagram of positioning errors in an embodiment of the present invention;
[0080] Fig. 9 1 is a comparison diagram of positioning errors in the X-axis direction and the Y-axis direction in an embodiment of the present invention. DETAILED DESCRIPTION
[0081] In the description of the present invention, it should be noted that the terms "first", "second", and "third" mentioned in the embodiments of the present invention are only used for descriptive purposes and cannot be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first", "second", and "third" may explicitly or implicitly include one or more of the features.
[0082] 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.
[0083] like Figure 2 As shown in the figure, due to the complexity of the underwater environment, the number of AUVs in the multi-AUV collaborative positioning system may change during the execution of underwater tasks, resulting in dynamic changes in the system topology. Compared with Figure (a), the number of slave boats in the system in Figure (b) has increased, and the number of master boats around the slave boats has also changed. Based on this, the present invention provides a multi-AUV collaborative positioning method based on factor graph under dynamic topology,
[0084] like Figure 1 As shown, specific implementation scheme 1: a multi-AUV collaborative positioning method based on factor graph under dynamic topology, comprising the following steps:
[0085] S1, collecting the current dynamic topological structure information of the multi-AUV collaborative positioning system;
[0086] S2, update the information of the slave boat and its neighbor master boat;
[0087] S3, initializing the information of the master boat and the slave boat;
[0088] S4, construct a factor graph model of multi-AUV collaborative positioning system;
[0089] The slave boat state variables and the master boat measurement information are defined as variable nodes, the state equation and measurement equation are defined as function nodes, and the state equation function node is used to measure the slave boat state variable node X. k To update the transmission, the measurement equation function node is used to update the main boat measurement information node Z k and slave state variable node X k Perform integration updates;
[0090] S5. Based on the transfer update of the sum-product algorithm in the factor graph model of the multi-AUV collaborative positioning system, information is transferred once in each of the two directions in the factor graph to realize the information transfer and update of the global factor graph nodes;
[0091] S6. The state variable node and the measurement variable node information are integrated and updated to obtain the estimated value of the position information of the slave boat at the current moment.
[0092] like Figure 3 As shown, in the system factor graph model constructed in this embodiment, the slave state variable node X at time k-1 and time k k-1 With X k Through the state equation function node f(X k |X k-1 ) connected; main boat measurement information node Z k With the slave state variable node X k By measuring the function node f(Z k |X k ) are connected.
[0093] like Figure 4 As shown, structure I is a function node f(Z k |X k ) after decomposition, complete the data fusion of the master AUV and the slave AUV, structure II is the measurement information Z k The specific structure of includes the ranging information corresponding to each master AUV at time k.
[0094] Specific implementation scheme 2: In S1, the current dynamic topological structure information of the system is collected in each collection cycle T, including: the number of master AUVs and slave AUVs, the position information, speed information v, angular velocity information and heading angle information θ of the master AUV and slave AUV, and the variance of each collected quantity is calculated and detected, and the distance measurement information d between the detection target slave boat and each master boat is detected. The rest of this implementation scheme is the same as the specific implementation scheme 1.
[0095] In this embodiment, the method for determining the number of neighboring master boats of the target slave AUV is as follows: a circular area with a selected diameter centered on the target slave AUV is set to determine the number of its neighboring master boats. Based on the collected information, neighbor node information is established with the slave AUV as the center, where the variance represents the uncertainty of the corresponding information.
[0096] Specific implementation scheme three: Initializing the information of the master boat and the slave boat in S3 includes: initializing the position information, speed information v, heading angle information θ and distance measurement information d between the master boat and the slave boat. The rest of this implementation scheme is the same as the specific implementation scheme two.
[0097] Specific implementation scheme 4: S5 includes the following process:
[0098] At the kth moment, the system conditional probability density function is decomposed into:
[0099]
[0100] In the formula, N represents the number of main AUV nodes; represents the measurement information of the nth (n=1, 2, ..., N) master AUV; X m,nrepresents the position information of the nth (n=1,2,…,N) master AUV; f i Represents the probability factor corresponding to each AUV node, that is:
[0101]
[0102] In the formula, h i (.) represents the measurement function; z i Indicates the true value of the measurement; ∑ i represents the covariance matrix corresponding to the measurement error;
[0103] It is defined that the distance information d, heading angle θ and speed v of the AUV collected by the system all obey Gaussian distribution:
[0104]
[0105] where d i represents the distance measurement information between the slave AUV and the i-th master AUV;
[0106] Through the state equation function node f(X k |X k-1 ) to variable node X k The information transmitted is:
[0107]
[0108] Variable Node X k To the state equation function node f(X k |X k-1 ) The information transmitted is:
[0109]
[0110] In the formula, and Represents the state variable X k A priori estimate and variance of ;
[0111] According to the co-location position equation:
[0112]
[0113] In the formula, (x k ,y k ) represents the coordinates of AUV in the reference coordinate system at time k; v k represents the forward speed of the AUV at time k; θ k represents the heading angle of the AUV at time k; Δt represents the sampling interval;
[0114] The state transition formula is:
[0115]
[0116] In the formula, Q k is the system process noise covariance matrix, is the measurement noise matrix, The expression is:
[0117]
[0118] In the formula, θ k is the heading angle information corresponding to time k;
[0119] Substituting equation (6) and equation (7) into equation (5), we can get the final reliability information:
[0120]
[0121] In the formula, S k The expression is:
[0122]
[0123] Finally, the global factor graph node information transmission and update are realized. The rest of this implementation is the same as the specific implementation plan 1.
[0124] In this implementation, the function node f(X k |X k-1 ) Based on the state function of the slave boat, the slave boat position information at the previous moment, the slave boat speed and heading angle information at this moment are used to calculate the slave boat position information at this moment.
[0125] Specific implementation plan five: Figure 4 As shown, the coordinate difference between the slave AUV and the i-th master AUV in the system at time k is and Variable Node and Through the function node C i Complete information update and calculate variable nodes The reliability information is:
[0126]
[0127] In the formula and Respectively represent and The standard deviation of represents the distance between the slave AUV and the i-th master AUV at time k;
[0128] Function Node C i To variable node Information transmitted, calculation variable node The credibility information transmitted is:
[0129]
[0130] In the formula represent The standard deviation of
[0131] Through the function node A i and B i Convert the position information, that is, function node A i Pass to variable node and Compute Variable Node and The reliability information is:
[0132]
[0133]
[0134] Compute Variable Node and The reliability information is:
[0135]
[0136]
[0137] The position estimates of each master boat to the slave boat are combined with the prior estimates of the slave boat’s position through function nodes D and E to obtain the final position estimate: Pass to x k ,Right now:
[0138]
[0139] In the formula, and For x k The variance and expectation of
[0140] Similarly, k The information is:
[0141]
[0142] In the formula, and for y k The variance and expectation of
[0143] Take a weighted average of the slave ship's position estimate and the slave ship's dead reckoning estimate:
[0144]
[0145]
[0146] Finally, the estimated value of the position information of the slave boat at the current moment is obtained. The rest of this implementation scheme is the same as the specific implementation scheme 4.
[0147] In this implementation scheme, the function node f(Z k |X k ) Based on the measurement equation of the master boat, the coordinate difference between the master and slave boats is fused and updated with the measurement information of the master boat, and then the coordinate difference between the master and slave boats is fused and updated with the position estimation information of the slave boat. Finally, the position estimation of the slave boat is obtained by taking the weighted average of the position estimation of each master boat to the slave boat and the dead reckoning estimation of the slave boat.
[0148] Specific implementation plan 6: The distance between the slave AUV and the i-th master AUV at time k in S6 and and The relationship is:
[0149] The rest of this implementation plan is the same as the specific implementation plan five.
[0150] Specific implementation scheme seven: A multi-AUV collaborative positioning system based on factor graph under dynamic topology, the system has a program module corresponding to the steps of any one of the above-mentioned specific implementation schemes one to six, and executes the steps in the above-mentioned multi-AUV collaborative positioning method based on factor graph under dynamic topology during operation.
[0151] Specific implementation scheme eight: 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 collaborative positioning method based on factor graph under dynamic topology of any one of the above-mentioned specific implementation schemes one to six when called by a processor. Specific embodiment 1
[0153] The dynamic structure-based factor graph multi-AUV collaborative positioning method under dynamic topology (Dynamic Structure-based Factor Graph, DSFG) of the present invention is simulated and analyzed with the traditional extended Kalman filter (Extended Kalman Filter, EKF) and unscented Kalman filter (Unscented Kalman Filter, UKF) collaborative positioning algorithms respectively.
[0154] The basic parameters are set as follows: A simulation experiment of a collaborative positioning system with 10 master boats and 20 slave boats is designed. The total simulation time is set to 1000s, and the state update cycle Δt = 1s. The dynamic topology structure information collection cycle T = 5s. In order to meet the observability of the system, the following is designed: Figure 5 As shown in the trajectory diagram, the main AUV moves at a uniform speed with an average speed of v m =2m / s, AUV moves in an S-curve with a speed of v s =6m / s; In order to realize the dynamic topology of the system structure, the information processing range of the slave AUV is set as a circular area with a diameter of 3000m centered on itself. The dynamic changes of the system are as follows Figure 6 As shown in Figure 2, based on the underwater acoustic ranging scenario, the observation noise variances of the master AUV and the slave AUV are set to The ranging variances of velocity and heading angle are and
[0155] Simulation Results and Analysis
[0156] In order to provide a comparison, the time period is cut according to the changes in the system structure. During the period when the system structure remains unchanged, the traditional extended Kalman filter EKF and unscented Kalman filter UKF collaborative positioning algorithm are used to locate the slave AUV. The positioning results of the filter algorithm are spliced in chronological order to avoid the system structure change affecting the real-time performance of the algorithm; the DSFG collaborative positioning method proposed in this invention is used to locate the slave AUV in a dynamic topology environment. Figure 7 and Figure 8 As shown in the figure, the positioning error of the EKF algorithm is large and unstable; the positioning accuracy of the UKF algorithm and the DSFG method is similar, indicating that the DSFG collaborative positioning method of the present invention can maintain a high accuracy in a dynamic topology environment. Fig. 9 The figure shows the comparison of positioning errors in the X-axis direction and the Y-axis direction. The specific error results of the three positioning methods are shown in Table 1.
[0157] Table 1
[0158]
[0159]
[0160] It can be seen from Table 1 that the overall RMSE of the DSFG collaborative positioning method of the present invention is reduced by 74.38% and 12.47% compared with EKF and UKF, respectively. It can be seen that in the dynamic topology scenario, the DSFG collaborative positioning method can track the changes in the system structure, adapt to the dynamic structure changes, and maintain a high-precision positioning effect, meeting the requirements of multi-AUV collaborative positioning under the dynamic topology structure. As shown in Table 2, the running time results of the three positioning methods are compared.
[0161] Table 2
[0162]
[0163] It can be seen from Table 2 that the UKF algorithm takes the longest time, the EKF algorithm takes the shortest time, and the DSFG collaborative positioning method has a longer running time than the EKF algorithm. It should be noted that the DSFG collaborative positioning method of the present invention includes collecting system structure information and updating structure information once in each collection cycle, while the running time of the two comparison algorithms EKF and UKF does not include the time for collecting structure information and updating structure information. Otherwise, in actual scenarios, the running time should be greater than the results listed in Table 2.
[0164] In summary, through simulation experiments, it can be found that in a dynamic topology scenario, the DSFG method of the present invention can maintain a high positioning efficiency while ensuring a high accuracy.
[0165] 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 collaborative positioning method based on factor graph under dynamic topology, characterized in that: The steps include: S1, collecting the current dynamic topological structure information of the multi-AUV collaborative positioning system; S2, update the information of the slave boat and its neighbor master boat; S3, initializing the information of the master boat and the slave boat; S4, construct a factor graph model of multi-AUV collaborative positioning system; The slave boat state variables and the master boat measurement information are defined as variable nodes, the state equation and measurement equation are defined as function nodes, and the state equation function node is used to measure the slave boat state variable node X. k To update the transmission, the measurement equation function node is used to update the main boat measurement information node Z k and slave state variable node X k Perform integration updates; S5. Based on the transfer update of the sum-product algorithm in the factor graph model of the multi-AUV collaborative positioning system, information is transferred once in each of the two directions in the factor graph to realize the information transfer and update of the global factor graph nodes; S6, integrating and updating the state variable node and the measurement variable node information to obtain the estimated value of the position information of the slave boat at the current moment; S5 includes the following processes: At the kth moment, the system conditional probability density function is decomposed into: In the formula, N represents the number of main AUV nodes; represents the measurement information of the nth master AUV, n=1,2,…,N; X m,n represents the position information of the nth master AUV, n=1,2,…,N; f i Represents the probability factor corresponding to each AUV node, that is: In the formula, h i (.) represents the measurement function; z i Indicates the true value of the measurement; ∑ i represents the covariance matrix corresponding to the measurement error; It is defined that the distance information d, heading angle θ and speed v of the AUV collected by the system all obey Gaussian distribution: where d i represents the distance measurement information between the slave AUV and the i-th master AUV; Through the state equation function node f(X k |X k-1 ) to variable node X k The information transmitted is: Variable Node X k To the state equation function node f(X k |X k-1 ) The information transmitted is: In the formula, and Represents the state variable X k A priori estimate and variance of ; According to the co-location position equation: In the formula, (x k ,y k ) represents the coordinates of AUV in the reference coordinate system at time k; v k represents the forward speed of the AUV at time k; θ k represents the heading angle of the AUV at time k; Δt represents the sampling interval; The state transition formula is: In the formula, 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 time k; Substituting equation (6) and equation (7) into equation (5), we can get the final reliability information: In the formula, S k The expression is: Finally, the global factor graph node information transmission and update are realized; S6 includes the following process: The coordinate difference between the slave AUV and the i-th master AUV in the system at time k is: and Compute Variable Node The reliability information is: In the formula and Respectively represent and The standard deviation of represents the distance between the slave AUV and the i-th master AUV at time k; Compute Variable Node The credibility information transmitted is: In the formula represent The standard deviation of Compute Variable Node and The reliability information is: Compute Variable Node and The reliability information is: Estimate the position of each master boat relative to the slave boat Pass to x k ,Right now: In the formula, and For x k The variance and expectation of Similarly, k The information is: In the formula, and for y k The variance and expectation of Take a weighted average of the slave ship's position estimate and the slave ship's dead reckoning estimate:
2. The multi-AUV collaborative positioning method based on factor graph under dynamic topology according to claim 1 is characterized in that: In S1, the current dynamic topological structure information of the system is collected in each collection cycle T, including: the number of master AUVs and slave AUVs, the position information, speed information v, angular velocity information and heading angle information θ of the master AUV and slave AUV, and the variance of each collected quantity is calculated and detected, as well as the distance measurement information d between the detection target slave boat and each master boat.
3. The multi-AUV collaborative positioning method based on factor graph under dynamic topology according to claim 2 is characterized in that: Initializing the information of the master boat and the slave boat in S3 includes: initializing the position information, speed information v, heading angle information θ of the master boat and the slave boat, and the distance measurement information d between the master boat and the slave boat.
4. The multi-AUV collaborative positioning method based on factor graph under dynamic topology according to claim 1 is characterized in that: The distance between the slave AUV and the i-th master AUV at time k in S6 and and The relationship is:
5. A multi-AUV collaborative positioning system based on factor graph under dynamic topology, characterized in that: The system has a program module corresponding to the steps of any one of claims 1 to 4 above, and executes the steps of the multi-AUV collaborative positioning method based on factor graph under dynamic topology when running.
6. 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 collaborative positioning method based on factor graph under dynamic topology according to any one of claims 1 to 4 when called by a processor.
