A multi-auv adaptive cooperative positioning method and system based on a factor graph

Abstract

The application discloses a multi-AUV adaptive cooperative positioning method and system based on a factor graph, relates to the technical field of AUV cooperative positioning, and aims at solving the problem that the prior art cannot cope with the dynamic change of a system topological structure and the long-term interference on positioning accuracy caused by the time-varying noise mixed in measurement information. The application comprises the following processes: collecting the dynamic topological structure information of a multi-AUV cooperative positioning system at the current time; updating the information of a slave vehicle and its neighbor master vehicles; constructing a factor graph model, defining the state variable of the slave vehicle and the measurement information of the master vehicle as variable nodes, performing transfer updating on the state variable node X k of the slave vehicle by using a state equation node, estimating the measurement noise covariance matrix based on an adaptive EKF filter and by using an EM algorithm, performing fusion updating on the measurement information node Z k of the master vehicle and the state variable node X k of the slave vehicle by using a measurement equation node, and obtaining the estimation value of the position information of the slave vehicle at the current time. The application realizes real-time and accurate positioning of multi-AUV cooperation.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of AUV cooperative positioning, in particular to a multi-AUV adaptive cooperative positioning method and system based on a factor graph. BACKGROUND

[0002] Due to the complexity of the underwater environment, the number of AUVs in the multi-AUV cooperative positioning system may change during the execution of underwater tasks, resulting in dynamic changes in the system topology. Moreover, for multi-AUV cooperative positioning based on a factor graph under a dynamic topology, due to various influences such as water temperature changes, uneven medium density, and ocean current interference, the underwater sound speed often changes unpredictably, and the measurement information is often mixed with fuzzy time-varying noise. The statistical characteristics of such noise are unknown and have an uncertain covariance matrix, which can easily interfere with the measurement update part of the cooperative positioning algorithm. The factor graph has uncertainty in transmitting information at related nodes, and this error accumulates with transmission, causing the positioning accuracy of the system to decrease, and in severe cases, even causing the system to diverge. In the case of insufficient data, the estimation accuracy of the noise covariance matrix of most current multi-AUV adaptive cooperative positioning algorithms is affected, and the adaptive estimation effect is reduced. In addition, the calculation complexity of some adaptive estimation algorithms is very high, and cannot meet the real-time requirements of the cooperative positioning system under a dynamic topology. Therefore, there is an urgent need for a multi-AUV adaptive cooperative positioning method for fuzzy time-varying noise interference under a dynamic topology. SUMMARY

[0003] The technical problem to be solved by the present application is:

[0004] Due to the complexity of the marine environment, the measurement information is often mixed with fuzzy time-varying noise, and the statistical characteristics of such noise are unknown and have an uncertain covariance matrix, which can easily interfere with the measurement update part of the cooperative positioning algorithm, causing the positioning accuracy of the system to decrease, and in severe cases, even causing the system to diverge.

[0005] The technical solution adopted by the present application to solve the above technical problems is:

[0006] The present application provides a multi-AUV adaptive cooperative positioning method based on a factor graph, comprising the following steps:

[0007] S1, collecting the dynamic topology structure information of the multi-AUV cooperative positioning system at the current time;

[0008] S2, updating 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, constructing a factor graph model of the multi-AUV adaptive cooperative positioning system;

[0011] The slave vessel's state variables and the main vessel's measurement information are defined as variable nodes, and the state equations and measurement equations are defined as function nodes. The state equation function nodes are used to apply the slave vessel's state variable nodes X. k The transmission and update are performed using the measurement equation function node to update the main vessel measurement information node Z. k and the state variable node X of the boat k Perform fusion updates; simultaneously, construct adaptive iterative estimation function node I. k Based on the adaptive EKF filter, the measurement noise covariance matrix is ​​estimated using the EM algorithm;

[0012] S5. Based on the sum-product algorithm, the information is transmitted and updated in the factor graph model of the multi-AUV adaptive cooperative positioning system. The information is transmitted once in each of the two directions in the factor graph to realize the transmission and update of global factor graph node information.

[0013] S6. Iteratively update the measurement noise covariance matrix based on the adaptive iterative estimation function node;

[0014] S7. Merge and update the information of the state variable node and the measurement variable node to obtain the estimated value of the submarine's position information at the current moment.

[0015] Furthermore, S1 collects the dynamic topology information of the multi-AUV cooperative positioning system at the current moment, including: the position information, velocity information v, angular velocity information and heading angle information θ of the master AUV and slave AUVs, and calculates the variance of each collected quantity, the distance information d between the target slave vessel and each master vessel, the number of master AUVs and slave AUVs, and the collection period is T.

[0016] Furthermore, the initialization of master and slave boat information described in S3 includes: initializing the position information, speed information v, heading angle information θ, and distance measurement information d between the master and slave boats.

[0017] Furthermore, S5 includes the following process:

[0018] The system conditional probability density function at time k is decomposed as follows:

[0019]

[0020] In the formula, N represents the number of main AUV nodes; This represents the measurement information of the nth (n = 1, 2, ..., N) main AUV; X m,n This represents the position information of the nth (n = 1, 2, ..., N) main AUV; f i This represents the probability factor corresponding to each AUV node, i.e.:

[0021]

[0022] where h i (.) denotes the measurement function; z i denotes the true value of the measurement; ∑ i denotes the covariance matrix of the measurement error;

[0023] The ranging information d, the heading angle θ and the velocity v of the AUV collected by the system are defined as Gaussian distribution:

[0024]

[0025] where d i denotes the ranging information between the AUV and the ith master AUV;

[0026] The information passed from the state equation function node f(X k |X k-1 ) to the variable node X k is:

[0027]

[0028] The information passed from the variable node X k to the state equation function node f(X k |X k-1 ) is:

[0029]

[0030] where and represent the prior estimate and the variance of the state variable X k , respectively;

[0031] According to the state equation of cooperative positioning,

[0032]

[0033] where (x k , y k ) denotes the coordinates of the AUV in the reference coordinate system at time k; v k denotes the forward velocity of the AUV at time k; θ k denotes the heading angle of the AUV at time k; and Δt denotes the sampling interval.

[0034] The state transition formula is obtained as:

[0035]

[0036] where Q k is the system process noise covariance matrix, R k is the measurement noise covariance matrix, and the expression of P is:

[0037]

[0038] where θ k is the heading angle information corresponding to the k moment when k = 1, 2, 3, …;

[0039] Substituting formula (6) and formula (7) into formula (5), the following formula is obtained:

[0040]

[0041] where S k The expression of S is as follows:

[0042]

[0043] Further, in S6, the state variable equation of the slave is constructed according to the EKF filtering algorithm, that is:

[0044]

[0045] where x represents the estimated value of k moment obtained at k-1 moment, F represents the state transition matrix, and u k represents the control input,

[0046] The corresponding estimation error covariance matrix is constructed, that is:

[0047] P k|k-1 = FP k|k-1 F T + Q k-1 (12)

[0048] The state variable and the estimation error covariance matrix of the slave are updated.

[0049] According to the EM algorithm, the initial value is first determined:

[0050]

[0051] The iterative update of the filtering gain matrix in the l+1 step is performed:

[0052]

[0053] where x is:

[0054]

[0055] The state variable is updated by using formula (14):

[0056]

[0057] Updating the error covariance matrix:

[0058]

[0059] Estimating the measurement noise covariance matrix R k :

[0060]

[0061] After N iterations, the estimate of R is obtained: k

[0062]

[0063] Further, S7 includes the following process:

[0064] The coordinate difference between the slave AUV and the ith master AUV at time k is and Calculating the variable node The belief information is:

[0065]

[0066] where and represent the standard deviations of and , respectively, denotes the distance between the slave AUV and the ith master AUV at time k;

[0067] Calculating the variable node The belief information passed is:

[0068]

[0069] where represents the standard deviation of ;

[0070] Calculating the variable node The belief information of and

[0071]

[0072]

[0073] Calculating the variable node The belief information of and

[0074]

[0075] ​

[0076] Estimating the position of the slave vehicle from the position estimates of the master vehicles Passing to x k i.e.

[0077]

[0078] where, and are the variance and expectation of x k

[0079] Similarly, the information of y k is:

[0080]

[0081] where, and are the variance and expectation of y k

[0082] The position estimate of the slave vehicle is a weighted average of the position estimates and the dead reckoning estimates of the slave vehicle:

[0083]

[0084]

[0085] Further, the distance between the slave AUV and the ith master AUV at time k in S7 is and and

[0086]

[0087] A multi-AUV adaptive cooperative positioning system based on a factor graph, which has program modules corresponding to the steps of any of the above technical solutions, and when running, executes the steps in the above multi-AUV adaptive cooperative positioning method based on a factor graph.

[0088] A computer-readable storage medium, which stores a computer program configured to realize the steps of the multi-AUV adaptive cooperative positioning method based on a factor graph in any of the above technical solutions when called by a processor.

[0089] Compared with the prior art, the beneficial effects of the present application are:

[0090] ​​​The application discloses a multi-AUV adaptive cooperative positioning method and system based on a factor graph. BRIEF DESCRIPTION OF DRAWINGS

[0091] Figure 1 A flow chart of the multi-AUV adaptive cooperative positioning method based on a factor graph in the embodiment of the application is shown in the figure.

[0092] Figure 2 A factor graph model of the adaptive algorithm in the embodiment of the application is shown in the figure.

[0093] Figure 3 A local factor graph model of f(Z k |X k ) in the embodiment of the application is shown in the figure.

[0094] Figure 4 A system structure and an actual track of an AUV in the embodiment of the application are shown in the figure.

[0095] Figure 5 A schematic diagram of overall change of a dynamic system structure in the embodiment of the application is shown in the figure.

[0096] Figure 6 A positioning error comparison chart in the embodiment of the application is shown in the figure.

[0097] Figure 7 X-direction and Y-direction error comparison charts in the embodiment of the application are shown in the figures. DETAILED DESCRIPTION

[0098] In the description of the present application, it should be noted that the terms "first", "second", "third" mentioned in the embodiments of the present application are only for the purpose of description, and cannot be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined with "first", "second", "third" can be explicitly or implicitly included one or more of the features.

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

[0100] Specific implementation scheme one: as shown in the present application provides a kind of multi-AUV adaptive cooperative positioning method based on factor graph, comprising the following steps: Figure 1

[0101] S1, the dynamic topology structure information of multi-AUV cooperative positioning system current time is collected;

[0102] S2, update slave and its neighbor master information;

[0103] S3, initialize master and slave information;

[0104] S4, the factor graph model of multi-AUV adaptive cooperative positioning system is constructed;

[0105] Slave state variable and master measurement information are defined as variable nodes, state equation and measurement equation are defined as function nodes, state equation function node is used to update slave state variable node X k , measurement equation function node is used to fuse update master measurement information node Z k And slave state variable node X k ; At the same time, adaptive iterative estimation function node I k Is constructed, based on adaptive EKF filter, and measurement noise covariance matrix is estimated using EM algorithm;

[0106] S5, based on sum-product algorithm in the factor graph model of multi-AUV adaptive cooperative positioning system, information is transmitted in two directions in the factor graph, global factor graph node information transmission and update are realized;

[0107] S6, based on the adaptive iterative estimation function node, the measurement noise covariance matrix is iteratively updated;

[0108] S7, the state variable node and the measurement variable node information are fused and updated to obtain the current time slave position information estimate value.

[0109] As Figure 2 ​As shown in this embodiment, the slave state variable node X at time k-1 and time k in the factor graph model... k-1 With node X k Through the state equation function node f(X) k |X k-1 Connected to each other; Main vessel measurement information node Z k With the state variable node X of the submarine k The nodes of the measurement equation function f(Z) are connected. k |X k Connected to each other; from the boat state variable node X k-1 X k and main vessel measurement information node Z k Estimating function node I through adaptive iteration k Connected.

[0110] like Figure 3 As shown, structure I is the function node f(Z) k |X k The specific structure after decomposition completes the data fusion between the main AUV and the slave AUV; Structure II is the measurement information Z. k The specific structure includes the ranging information corresponding to each main AUV at time k.

[0111] Specific Implementation Scheme Two: In S1, the dynamic topology information of the multi-AUV cooperative positioning system at the current moment is collected, including: the position information, velocity information v, angular velocity information, and heading angle information θ of the master AUV and slave AUVs, and the variance of each collected quantity is calculated, as well as the distance information d between the target slave AUV and each master AUV, and the number of master AUVs and slave AUVs. The collection period is T. All other aspects of this implementation scheme are the same as in Specific Implementation Scheme One.

[0112] In this implementation plan, the method for determining the number of neighboring main vessels of the target AUV is as follows: a circular area with a selected diameter centered on the target AUV is set, and the number of its neighboring main vessels is determined.

[0113] Specific Implementation Scheme 3: The initialization of master and slave vessel information as described in S3 includes: initializing the position information, velocity information v, heading angle information θ, and distance measurement information d between the master and slave vessels. All other aspects of this implementation scheme are the same as in Specific Implementation Scheme 2.

[0114] Specific implementation plan four: S5 includes the following processes:

[0115] The system conditional probability density function at time k is decomposed as follows:

[0116]

[0117] In the formula, N represents the number of main AUV nodes; measurement information of the nth(n = 1, 2, …, N) main AUV; X m,n position information of the nth(n = 1, 2, …, N) main AUV; f i probability factor corresponding to each AUV node, that is,

[0118]

[0119] wherein h i (.) represents a measurement function; z i represents a measurement true value; ∑ i represents a covariance matrix corresponding to a measurement error;

[0120] The ranging information d, the heading angle θ and the speed v of the slave AUV collected by the system are defined to be subject to Gaussian distribution:

[0121]

[0122] wherein d i represents ranging information between the slave AUV and the ith main AUV;

[0123] The information transmitted by the state equation function node f(X k |X k-1 ) to the variable node X k is:

[0124]

[0125] The information transmitted by the variable node X k to the state equation function node f(X k |X k-1 ) is:

[0126]

[0127] wherein and respectively represent the prior estimation and the variance of the state variable X k ;

[0128] According to the state equation of the cooperative positioning,

[0129]

[0130] wherein (x k , y k ) represents the coordinates of the AUV in the reference coordinate system at the k time; v k represents the forward speed of the AUV at the k time; θ k represents the heading angle of the AUV at the k time; and Δt represents the sampling interval.

[0131] The state transition equation is obtained as follows:

[0132]

[0133] In the formula, Q k is the system process noise covariance matrix, R k is the measurement noise covariance matrix, The expression of S

[0134]

[0135] In the formula, θ k is the heading angle information corresponding to the k moment; and

[0136] Substitute formula (6) and formula (7) into formula (5), and the following is obtained:

[0137]

[0138] In the formula, S k is the expression of S

[0139] The other embodiments of the present embodiment are the same as the first embodiment.

[0140] The fifth embodiment is that the state variable equation of the slave vessel is constructed according to the EKF filtering algorithm in S6, that is:

[0141]

[0142] In the formula, x represents the estimation value of k moment obtained at k-1 moment, F represents the state transition matrix, u k represents the control input,

[0143] The corresponding estimation error covariance matrix is constructed, that is:

[0144] P k|k-1 = FP k|k-1 F T +Q k-1 (12)

[0145] The state variable and the estimation error covariance matrix of the slave vessel are updated.

[0146] When no measurement information is received, the position information is updated by dead reckoning

[0147] According to the EM algorithm, the initial value is first determined as follows:

[0148]

[0149] The iterative update of the filtering gain matrix in the l+1 step is performed:

[0150]

[0151] wherein,

[0152]

[0153] The state variable is updated using equation (14):

[0154]

[0155] The error covariance matrix is updated:

[0156]

[0157] The estimate of the measurement noise covariance matrix R k is obtained:

[0158]

[0159] After N iterations, the estimate of R k is obtained:

[0160] The other aspects of this embodiment are the same as in embodiment four.

[0161] Embodiment six: As shown in FIG. 7, S7 includes the following processes: Figure 3

[0162] The coordinate difference between the slave AUV and the ith master AUV at time k is and The variable nodes and are updated by the function nodes C i , and the belief information of the variable nodes is calculated as:

[0163]

[0164] wherein and represent the standard deviations of and respectively, and represents the distance between the slave AUV and the ith master AUV at time k;

[0165] The belief information of the variable nodes transmitted by the function nodes C i is calculated as:

[0166] ​​​

[0167] where represents the standard deviation;

[0168] The position information is transformed by function nodes A i and B i , i.e. function node A i passes the position information to variable nodes and The belief information of variable nodes and are calculated as:

[0169]

[0170]

[0171] The belief information of variable nodes and are calculated as:

[0172]

[0173]

[0174] The position estimates of each master-to-slave pair are combined with the prior estimate of the slave's position by function nodes D and E to obtain the final position estimate: the position estimates of each master-to-slave pair are passed to x k , i.e.

[0175]

[0176] where, and are the variance and expectation of x k ;

[0177] Similarly, the information of y k is:

[0178]

[0179] where, and are the variance and expectation of y k ;

[0180] The position estimate of the slave is obtained by a weighted average of the position estimate of the slave and the dead-reckoning estimate of the slave:

[0181]

[0182] The other aspects of this embodiment are the same as those of embodiment five.

[0183] Embodiment seven: the distance between the slave AUV and the ith master AUV at time k in S7 and and The relationship is:

[0184] The other aspects of this embodiment are the same as those of embodiment six.

[0185] Embodiment eight: a factor graph-based multi-AUV adaptive cooperative positioning system, which has program modules corresponding to the steps of any one of the above embodiments one to seven, and executes the steps of the above factor graph-based multi-AUV adaptive cooperative positioning method.

[0186] Embodiment nine: a computer-readable storage medium, which stores a computer program configured to realize the steps of the factor graph-based multi-AUV adaptive cooperative positioning method of any one of embodiments one to seven when called by a processor.

[0187] Example 1

[0188] The factor graph-based multi-AUV adaptive cooperative positioning method (Dynamic Structure-based Adaptive Factor Graph, DS-AFG) of the present application, the traditional extended Kalman filter (Extended Kalman Filter, EKF), the unscented Kalman filter (Unscented Kalman Filter, UKF) cooperative positioning algorithm, and the factor graph-based multi-AUV cooperative positioning method (Dynamic Structure-based Factor Graph, DSFG) based on the present application without processing the measurement noise covariance matrix are simulated and analyzed.

[0189] The basic parameters of the algorithm are set as follows: a cooperative positioning system simulation experiment of 10 master ships and 20 slave ships is designed. The total simulation time is set to 1000s, and the state update period Δt = 1s. The dynamic topology structure detection period ΔT = 5s. In order to meet the system observability, a trajectory diagram as shown in Figure 4 is designed, the master AUV moves at a constant speed, and the average speed is v m = 2m / s, and the slave AUV moves in an S-shaped curve, and the speed is 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 to a circular region with a diameter of 3000m centered on itself, and the system dynamic change is as shown inFigure 5 The ranging variances of the speed and the heading angle of the master AUV and the slave AUV are respectively set as and In the contrast experiment of the DS-AFG method, in order to simulate the fuzzy time-varying noise in the actual application scene, the mean value of the measurement noise is set as μ d = 1m, and the variance is The variance of the measurement noise is generated by a normal distribution random number, which meets the characteristics of the fuzzy time-varying noise.

[0190] Simulation results and analysis

[0191] The DS-AFG method, the EKF, the UKF and the DSFG are respectively applied to the dynamic topology multi-AUV cooperative positioning under the fuzzy time-varying noise, simulation results as shown in Figure 6 and Figure 7 are obtained, and the root mean square error (RMSE) results of each method are listed in Table 1. It can be seen that the positioning error of the DS-AFG algorithm is the smallest, and the positioning RMSE is reduced by 29.68%, 34.39% and 60.18% respectively than the DSFG, the UKF and the EKF algorithms. This is because the DS-AFG algorithm can estimate the covariance matrix of the measurement noise, reduce the interference of the fuzzy time-varying noise on the system, and achieve higher positioning accuracy.

[0192] Table 1

[0193]

[0194] As shown in Table 2, the running time of each method is shown. It can be seen that the DS-AFG algorithm consumes the longest time, because it adds the adaptive filtering processing step, needs to iteratively estimate the measurement noise covariance matrix, and therefore causes the time consumption to be slightly larger than other algorithms, but is within an acceptable range.

[0195] Table 2

[0196]

[0197] It can be found through the simulation experiment verification that the DS-AFG method of the present application has a slightly longer time consumption than other algorithms because of the added adaptive filtering processing step, but the overall positioning effect is significantly better than other algorithms, has better anti-interference ability, and is more adaptive.

[0198] Although the present application discloses as above, the protection scope of the present application is not limited to this. The person skilled in the art can make various changes and modifications without departing from the spirit and scope of the present application, and these changes and modifications will fall within the protection scope of the present application.

Claims

1. A multi-AUV adaptive cooperative localization method based on factor graphs, characterized in that, Includes the following steps: S1. Collect the dynamic topology information of the multi-AUV cooperative positioning system at the current moment; S2. Update information on the slave vessel and its neighboring master vessels; S3. Initialize the master and slave boat information; S4. Construct a factor graph model of a multi-AUV adaptive cooperative positioning system; The slave vessel's state variables and the main vessel's measurement information are defined as variable nodes, and the state equations and measurement equations are defined as function nodes. The state equation function nodes are used to apply the slave vessel's state variable nodes. The transmission and update are performed using measurement equation function nodes to transmit the main vessel's measurement information nodes. and the state variable node of the boat The system performs a fusion update; simultaneously, it constructs an adaptive iterative estimation function node, based on an adaptive EKF filter, and uses the EM algorithm to estimate the measurement noise covariance matrix. S5. Based on the sum-product algorithm, the information is transmitted and updated in the factor graph model of the multi-AUV adaptive cooperative positioning system. The information is transmitted once in each of the two directions in the factor graph to realize the transmission and update of global factor graph node information. S6. Iteratively update the measurement noise covariance matrix based on the adaptive iterative estimation function node; S7. Merge and update the information of the state variable node and the measurement variable node to obtain the estimated value of the submarine's position information at the current moment; S5 includes the following process: The system conditional probability density function at time k is decomposed as follows: （1） In the formula, Indicates the number of primary AUV nodes; This represents the measurement information of the nth main AUV, where ; This indicates the position information of the nth main AUV; This represents the probability factor corresponding to each AUV node, i.e.: （2） In the formula, Represents the measurement function; Indicates the true measurement value; This represents the covariance matrix corresponding to the measurement error; Define the ranging information d and heading angle collected by the system. The velocity v from the AUV both follow a Gaussian distribution: （3） in Indicates from AUV and the first Distance measurement information between main AUVs; Through state equation function nodes To variable node The message being conveyed is: （4） Variable Node To the state equation function node The message being conveyed is: （5） In the formula, and Representing state variables respectively Prior estimates and variance; According to the state equation of cooperative localization （6） In the formula, This represents the coordinates of the AUV in the reference coordinate system at time k; This represents the forward velocity of the AUV at time k; This represents the heading angle of the AUV at time k; Indicates the sampling interval; The state transition formula is as follows: （7） In the formula, It is the system process noise covariance matrix. To measure the noise covariance matrix, The expression is: （8） In the formula, Is it a formula? The heading angle information corresponding to the time; Substituting equations (6) and (7) into equation (5), we get: （9） In the formula, The expression is: （10）。 2. The multi-AUV adaptive cooperative localization method based on factor graphs according to claim 1, characterized in that, S1 collects the current dynamic topology information of the multi-AUV cooperative positioning system, including: position information, velocity information v, angular velocity information, and heading angle information of the master AUV and slave AUVs. The variance of each collected data is calculated, as well as the distance information d between the target vessel and each main vessel, the number of main AUVs and slave AUVs, and the data collection period is T.

3. The multi-AUV adaptive cooperative localization method based on factor graphs according to claim 2, characterized in that, The initialization of master and slave boat information described in S3 includes: initializing the position information, velocity information v, and heading angle information of the master and slave boats. And the distance measurement information d between the master and slave vessels.

4. The multi-AUV adaptive cooperative localization method based on factor graphs according to claim 3, characterized in that, In S6, the state variable equations of the submarine are constructed according to the EKF filtering algorithm, namely: （11） In the formula, This represents the estimated value at time k obtained at time k-1. Represents the state transition matrix. Indicates control input, ; Construct the corresponding estimation error covariance matrix, i.e.: （12） Update the submarine state variables and the estimated error covariance matrix; According to the EM algorithm, the initial values ​​are first determined: （13） Conduct the first Iterative update of the filter gain matrix: （14） In the formula, for: （15） Update the state variables using equation (14): （16） Update the error covariance matrix: （17） Estimating the measurement noise covariance matrix : （18） Iteration After convergence, we obtain The estimation results are as follows: （19）。 5. The multi-AUV adaptive cooperative localization method based on factor graphs according to claim 4, characterized in that, S7 includes the following process: In the time system, from AUV and the first The coordinate difference between the main AUVs is and Calculate variable nodes The reliability information obtained is as follows: （20） In the formula and Represent and standard deviation Indicates the time k from AUV and the first The distance between the main AUVs; Calculate variable nodes The reliability information conveyed is as follows: （21） In the formula represent Standard deviation; Calculate variable nodes and The reliability information is as follows: （22） （23） Calculate variable nodes and The reliability information is as follows: （24） （25） Estimate the positions of each master vessel relative to its slave vessels Passed to ,Right now: （26） In the formula, and for The variance and expected value; Similarly, The information is: （27） In the formula, and for The variance and expected value; A weighted average of the follower's position estimate and the follower's dead reckoning estimate is performed: （28） （29）。 6. The multi-AUV adaptive cooperative localization method based on factor graphs according to claim 5, characterized in that, At time k in S7, from AUV and the... Distance between main AUVs and and The relationship is: （30）。 7. A multi-AUV adaptive cooperative localization system based on factor graphs, characterized in that, The system has a program module corresponding to the steps of any one of claims 1 to 6 above, and executes the steps in the factor graph-based multi-AUV adaptive cooperative localization method described above when running.

8. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program configured to implement the steps of the factor graph-based multi-AUV adaptive cooperative localization method as described in any one of claims 1 to 6 when invoked by a processor.

Images