A multi-auv self-adaptive cooperative positioning method and system based on factor graph node optimization

Abstract

The application discloses a multi-AUV self-adaptive cooperative positioning method and system based on a node optimization factor graph, relates to the technical field of multi-AUV cooperative positioning, and aims at solving the problems that the existing technology cannot be applied to the dynamic change of a system topological structure, cannot effectively eliminate the interference of fuzzy time-varying noise and cannot reduce the communication pressure caused by a large system scale. The application comprises the following steps: collecting dynamic topological structure information at a current moment; updating slave ship and neighbor master ship information; constructing a factor graph model; adopting a state equation function node to perform transfer updating on a slave ship state variable node X k , adopting an adaptive EKF filter based on an EM algorithm to estimate a measurement noise covariance matrix, adopting a CRLB k and a ranging evaluation factor to calculate and optimize the master ship node at the current moment, adopting a measurement equation function node to perform fusion updating on a master ship measurement information node Z k and the slave ship state variable node X k , and obtaining the estimated value of the slave ship position information at the current moment. 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 multi-AUV cooperative positioning, in particular to a multi-AUV adaptive cooperative positioning method and system based on node optimization of 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 the underwater task, resulting in dynamic changes in the system topology. In the cooperative positioning process under the dynamic topology, data fusion becomes a dynamic process. However, the system structure in the existing cooperative positioning algorithm is often static and fixed, and is not suitable for AUV cooperative positioning under dynamic topology. Moreover, due to the complexity of the marine environment, 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 system positioning accuracy to decrease, and in severe cases, even causing the system to diverge. When the number of master AUV nodes in the system under dynamic topology is large and the system scale is large, communication with all nodes will increase the data interaction of the cooperative positioning system, reducing the real-time performance of the system. In the dynamic topology system where the cooperative positioning accuracy decreases due to the relatively low-precision positioning information, the node scale is large, or the underwater bandwidth is limited, communication with every master AUV node in the system will inevitably increase the data interaction of the cooperative positioning system, affecting the real-time performance of the system. Therefore, there is an urgent need for a multi-AUV cooperative positioning method that can deal with fuzzy time-varying noise interference and a large number of master AUV nodes under dynamic topology. SUMMARY

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

[0004] The existing technology cannot adapt to the dynamic changes in the system topology, and cannot effectively eliminate the interference of fuzzy time-varying noise and the communication pressure caused by a large system scale.

[0005] The technical scheme 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 node optimization of 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 slave boat and its neighbor master boat information;

[0009] S3, initializing master and slave information;

[0010] S4, constructing a multi-AUV cooperative positioning system factor graph model;

[0011] The slave state variable, master position information and master measurement information are defined as variable nodes, the state equation and measurement equation are defined as function nodes, and an adaptive iterative estimation function node and a node optimization function node are defined; the state equation function node is used to perform transmission update on the slave state variable node X k The measurement equation function node is used to perform fusion update on the master measurement information node Z k and the slave state variable node X k The adaptive iterative estimation function node is based on an adaptive EKF filter of the EM algorithm to estimate the measurement noise covariance matrix, and the node optimization function node is used to calculate the CRLB k and the ranging evaluation factor of the estimated position information node Φ k of the system master and the master measurement information node Z k , respectively, to optimize the master node at the current time;

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

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

[0014] S7, the master node is optimized through the node optimization function node;

[0015] S8, based on the optimized master node, the state variable node and the measurement variable node information are fused and updated to obtain the current time slave position information estimate value.

[0016] Further, in S1, the current dynamic topology structure information of the system is collected in each collection period 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 AUVs and slave AUVs, and the variances of the collected quantities are calculated, and the ranging information d between the target slave and each master is detected.

[0017] Further, in S3, the initialization of the master and slave information includes the initialization of the position information, speed information v, heading angle information θ of the master and slave, and the ranging information d between the master and slave.

[0018] Further, S5 includes the following processes:

[0019] The system conditional probability density function at the kth moment is decomposed as:

[0020]

[0021] where N represents the number of master 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; f i represents the probability factor corresponding to each AUV node, that is:

[0022]

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

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

[0025]

[0026] where d i represents the ranging information between the slave AUV and the ith master AUV;

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

[0028]

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

[0030]

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

[0032] According to the position equation of cooperative positioning:

[0033]

[0034] where (x k , yk represents the coordinate of AUV in the reference coordinate system at time k; v k represents the forward velocity of AUV at time k; θ k represents the heading angle of AUV at time k; Δt represents the sampling interval;

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

[0036]

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

[0038]

[0039] In the formula, θ k is the heading angle information corresponding to time k;

[0040] Substitute formula (6) and formula (7) into formula (5), combine, and obtain the final confidence information:

[0041]

[0042] In the formula, S k The expression of S

[0043]

[0044] Finally, the global factor graph node information transmission and update are realized.

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

[0046]

[0047] In the formula, represents the estimated value of time k obtained at time k-1, F represents the state transition matrix, u k represents the control input,

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

[0049] P k|k-1 = FP k|k-1 FT+Q k-1 (12)

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

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

[0052]

[0053] Perform the (l+1)th step iterative update of the filter gain matrix:

[0054]

[0055] In the formula, for:

[0056]

[0057] Update the state variables using equation (14):

[0058]

[0059] Update the error covariance matrix:

[0060]

[0061] Estimate the measurement noise covariance matrix R k :

[0062]

[0063] After N iterations and convergence, R is obtained. k The estimation results are as follows:

[0064]

[0065] Furthermore, S7 includes the following process:

[0066] Calculate the Cramer-Rao lower bound (CRLB) for each master AUV node. k and ranging evaluation factor α i :

[0067]

[0068] Among them, X k-1 =[x k-1 ,y k-1 ] T This represents the state variable from the AUV at time k-1; This represents the position information of the i-th main AUV at time k; R X X represents k-1 The error covariance matrix; R Φ express The error covariance matrix; d i This represents the ranging information between the AUV and the i-th main AUV; d i Standard deviation;

[0069] The node selection parameter matrix at time k is established:

[0070]

[0071] wherein tr(·) represents the trace of a matrix; N represents the number of main AUVs included in the system at time k;

[0072] The parameters in the node selection parameter matrix NSPM are weighted using the information entropy method. First, the respective proportions p of the evaluation indexes are calculated i :

[0073]

[0074] wherein r1 represents the CRLB evaluation parameter; and r2 represents the ranging evaluation factor;

[0075] The entropy value of the parameter is calculated:

[0076] e i =-p i ln(p i ) (22)

[0077] The difference coefficient is calculated:

[0078] g i =1-e i (23)

[0079] The weight of the two indexes is calculated:

[0080]

[0081] The weight vector Ω is constructed:

[0082] Ω=[ω1 ω2] (25)

[0083] The node selection parameter matrix NSPM is weighted:

[0084] H k =Ω·NSPM k (26)

[0085] The 1×N matrix H obtained from equation (26) k , wherein the value of each column corresponds to the final evaluation result of the corresponding main AUV in the system, and the M smallest results are selected, and the corresponding main AUV is the optimal result.

[0086] Further, S8 includes the following process:

[0087] The coordinate difference between the slave AUV and the ith main AUV in the system at time k is and Calculate variable nodes The reliability information is as follows:

[0088]

[0089] In the formula and Represent and standard deviation This represents the distance between the AUV at time k and the i-th main AUV;

[0090] Calculate variable nodes The reliability information conveyed is as follows:

[0091]

[0092] In the formula represent Standard deviation;

[0093] Calculate variable nodes and The reliability information is as follows:

[0094]

[0095]

[0096] Calculate variable nodes and The reliability information is as follows:

[0097]

[0098]

[0099] Estimate the positions of each master vessel relative to its slave vessels Passed to x k ,Right now:

[0100]

[0101] In the formula, and For x k The variance and expected value;

[0102] Similarly, y k The information is:

[0103]

[0104] In the formula, and For y kthe variance and expectation of the distance between the AUV and the i-th master AUV at time k in S8

[0105] The weighted average of the position estimation from the boat and the dead reckoning estimation from the boat is obtained:

[0106]

[0107]

[0108] Further, the distance between the AUV and the i-th master AUV at time k in S8 and and The relationship is:

[0109]

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

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

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

[0113] The multi-AUV adaptive cooperative positioning method and system based on factor graph node optimization of the present application can increase or decrease factor graph nodes by real-time detection of the dynamic topology of the system, define the slave boat state information, master boat position information and master boat measurement information as variable nodes, and construct a multi-AUV cooperative positioning system factor graph model under dynamic topology. Further considering the interference of fuzzy time-varying noise on the cooperative positioning based on factor graph on the basis of the dynamic topology of the cooperative positioning system, the adaptive EKF filter of the maximum expectation algorithm (Expectation Maximization Algorithm, EM) is introduced to estimate the measurement noise covariance, and remove the uncertainty in the measurement noise covariance. Using the method based on the Cramer-Rao lower bound and the ranging evaluation factor, the high-quality master AUV node with more accurate positioning information in the system is selected, and the factor graph nodes are increased or decreased with the target to reduce the data interaction amount of the system, ensure the positioning accuracy and reduce the data interaction amount of the system, and efficiently utilize the underwater communication bandwidth resources.

[0114] The application provides a solution for the dynamic topology of the system, the large number of main AUVs and the interference of fuzzy time-varying noise, and takes into account the positioning accuracy, positioning efficiency and real-time performance of the algorithm. BRIEF DESCRIPTION OF DRAWINGS

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

[0116] Figure 2 A global factor graph model in the embodiment of the application is shown in the figure.

[0117] Figure 3 A factor graph model of h(Z,Φ,X) in the embodiment of the application is shown in the figure.

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

[0119] Figure 5 A system structure and actual trajectory of an AUV in the embodiment of the application are shown in the figure.

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

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

[0122] Figure 8 An error comparison chart in the X direction and the Y direction in the embodiment of the application is shown in the figure. DETAILED DESCRIPTION

[0123] In the description of the application, it should be noted that the terms "first", "second", "third" mentioned in the embodiments of the 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 limited by "first", "second", "third" can explicitly or implicitly include one or more of the features.

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

[0125] Specific implementation scheme one: as shown in the figure, the application provides a multi-AUV adaptive cooperative positioning method based on node optimization of a factor graph, which comprises the following steps: Figure 1

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

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

[0128] S3, initialize the master and slave information;

[0129] S4, construct a multi-AUV cooperative positioning system factor graph model;

[0130] The slave state variable, master position information and master measurement information are defined as variable nodes, the state equation and measurement equation are defined as function nodes, and adaptive iterative estimation function nodes and node optimization function nodes are defined; the state equation function node is used to update the slave state variable node X k The measurement equation function node is used to update the master measurement information node Z k and the slave state variable node X k The adaptive iterative estimation function node is based on the adaptive EKF filter of the EM algorithm to estimate the measurement noise covariance matrix, and the node optimization function node is used to calculate the CRLB k and the ranging evaluation factor of the estimated position information node Φ k and the master measurement information node Z k of the system master, and the master node is optimized at the current time;

[0131] S5, based on the sum-product algorithm in the multi-AUV cooperative positioning system factor graph model, the information is transmitted in both directions in the factor graph once, realizing the global factor graph node information transmission and update;

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

[0133] S7, the master node is optimized through the node optimization function node;

[0134] S8, based on the optimized master node, the state variable node and the measurement variable node information are fused and updated to obtain the current time slave position information estimate value.

[0135] As shown in Figure 2 , the state variable nodes X k-1 and X k at time k-1 and time k in the system factor graph model constructed in the embodiment are connected through the state equation function node f(X k |X k-1 ); the master measurement information node Z k and the slave state variable node X kThe 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; Main vessel measurement information node Z k Main vessel position information node Φ k and the state variable node X of the boat k-1 With X k The nodes are connected through the node optimization function h(Z,Φ,X) to optimize the selection of the main vessel nodes.

[0136] like Figure 3 As shown in the figure, the structures L1, L2, ..., L N These represent the information of the N main AUVs in the system at time k, with each structure containing the position information node Φ of the main vessel. k and distance measurement related information Function nodes F and G use information from the main vessel to calculate CRLB and ranging evaluation factors, respectively, and obtain α. k and β k Function node H utilizes variable node α k and β k The information is used to perform weighted calculations to obtain the final evaluation result h. k Filter the M results with the smallest values. The corresponding primary AUV is the preferred result. The Type III structure in the figure corresponds to the optimization selection process of a single primary AUV node.

[0137] like Figure 4 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.

[0138] Specific Implementation Scheme Two: In S1, within each acquisition cycle T, the current dynamic topology information of the system is acquired, including: the number of main AUVs and slave AUVs, the position information, velocity information v, angular velocity information, and heading angle information θ of the main AUVs and slave AUVs, and the variance of each acquired quantity is calculated, as well as the distance information d between the detected slave vessel and each main vessel. Other aspects of this implementation scheme are the same as in Specific Implementation Scheme One.

[0139] Specific implementation three: the initialization of master and slave information in S3 includes: initialization of position information, velocity information v, heading angle information θ of master and slave, and ranging information d between master and slave. Other aspects of this embodiment are the same as specific implementation two.

[0140] Specific implementation four: S5 includes the following processes:

[0141] At the kth moment, the system conditional probability density function is decomposed as:

[0142]

[0143] In the formula, N represents the number of master 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; f i represents the probability factor corresponding to each AUV node, that is:

[0144]

[0145] In the formula, h i (.) represents the measurement function; z i represents the measurement true value; ∑ i represents the covariance matrix corresponding to the measurement error;

[0146] Define that the ranging information d, the heading angle θ and the speed v of the slave AUV collected by the system are subject to Gaussian distribution:

[0147]

[0148] Where d i represents the ranging information between the slave AUV and the ith master AUV;

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

[0150]

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

[0152]

[0153] In the formula, and represent the state variable Xk priori estimation and variance of

[0154] According to the position equation of cooperative positioning:

[0155]

[0156] wherein (x k ,y k ) represents the coordinates of the 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; and Δt represents the sampling interval.

[0157] The state transition equation is obtained as:

[0158]

[0159] wherein Q k is the system process noise covariance matrix, is the measurement noise matrix, and the expression of H

[0160]

[0161] wherein θ k is the heading angle information corresponding to time k;

[0162] Substitute equation (6) and equation (7) into equation (5), combine, and obtain the final confidence information:

[0163]

[0164] wherein the expression of S k is:

[0165]

[0166] Finally, the global factor graph node information transmission and update are realized. The other aspects of the embodiment are the same as those of the first embodiment.

[0167] In the embodiment, the function node f(X k |X k-1 ) is based on the state function of the slave boat, and the position information of the slave boat at the previous time, the speed and heading angle information of the slave boat at this time are used to calculate the position information of the slave boat at this time.

[0168] Embodiment five: the state variable equation of the slave boat is constructed according to the EKF filtering algorithm in S6, that is:

[0169]

[0170] wherein, Let u represent the estimated value at time k obtained at time k-1, F represent the state transition matrix, and u represent the state transition matrix. k Indicates control input,

[0171] Construct the corresponding estimation error covariance matrix, i.e.:

[0172] P k|k-1 =FP k|k-1 FT+Q k-1 (12)

[0173] Update the boat state variables and estimate the error covariance matrix.

[0174] According to the EM algorithm, the initial values ​​are first determined:

[0175]

[0176] Perform the (l+1)th step iterative update of the filter gain matrix:

[0177]

[0178] In the formula, for:

[0179]

[0180] Update the state variables using equation (14):

[0181]

[0182] Update the error covariance matrix:

[0183]

[0184] Estimate the measurement noise covariance matrix R k :

[0185]

[0186] After N iterations and convergence, R is obtained. k The estimation results are as follows:

[0187] This implementation plan is otherwise the same as Specific Implementation Plan Four.

[0188] Specific Implementation Plan Six: (e.g.) Figure 3 As shown, S7 includes the following process:

[0189] Calculate the Cramer-Rao lower bound (CRLB) for each master AUV node. k and ranging evaluation factor α i :

[0190]

[0191] where X k-1 = [x k-1 , y k-1 ] T represents the state variable of the AUV at time k-1; represents the position information of the i-th master AUV at time k; R X represents the error covariance matrix of X k-1 ; R Φ represents the error covariance matrix of d ; d i represents the ranging information from the AUV to the i-th master AUV; represents the standard deviation of d i ;

[0192] The node selection parameter matrix at time k is established as follows:

[0193]

[0194] where tr(·) represents the trace of a matrix; N represents the number of master vessels included in the system at time k;

[0195] The parameters in the node selection parameter matrix NSPM are weighted using the information entropy method. First, the respective proportions p i of the evaluation indexes are calculated:

[0196]

[0197] where r1 represents the CRLB evaluation parameter; r2 represents the ranging evaluation factor;

[0198] The entropy value of the parameter is calculated as follows:

[0199] e i = -p i ln(p i ) (22)

[0200] The difference coefficient is calculated as follows:

[0201] g i = 1-e i (23)

[0202] The weight of the two indexes is calculated as follows:

[0203]

[0204] The weight vector Ω is constructed as follows:

[0205] Ω = [ω1 ω2] (25)

[0206] The node optimization parameter matrix NSPM is weighted and processed as follows:

[0207] H k =Ω·NSPM k (26)

[0208] The 1×N matrix H obtained from equation (26) k In this implementation plan, the values ​​in each column correspond to the final evaluation results of the corresponding main AUV in the system. The M smallest results are selected, and the corresponding main AUVs are chosen as the preferred results. This implementation plan is otherwise the same as specific implementation plan five.

[0209] Specific Implementation Plan Seven: (e.g.) Figure 4 As shown, S8 includes the following process:

[0210] For time k, the coordinate difference between the auxiliary AUV and the i-th main AUV in the system is: and Variable Node and Through function node C respectively i Complete information update and calculate variable nodes. The reliability information is as follows:

[0211]

[0212] In the formula and Represent and standard deviation This represents the distance between the AUV at time k and the i-th main AUV;

[0213] Function node C i To variable node The information transmitted, the variable nodes are calculated The reliability information conveyed is as follows:

[0214]

[0215] In the formula represent Standard deviation;

[0216] Through function node A i and B i Perform location information conversion, i.e., function node A i Passed to variable node and Calculate variable nodes and The reliability information is as follows:

[0217]

[0218]

[0219] The belief information of the calculation variable node and are respectively:

[0220]

[0221]

[0222] The final position estimate of the slave is obtained by combining the position estimate of the slave from each master with the prior estimate of the slave position through function nodes D and E: the position estimate of the slave from each master is passed to x k i.e.

[0223]

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

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

[0226]

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

[0228] The position estimate of the slave from each master is weighted averaged with the dead-reckoning estimate of the slave:

[0229]

[0230]

[0231] Finally, the information estimate of the slave position at the current time is obtained. The other parts of this embodiment are the same as Embodiment 6.

[0232] In this embodiment, the function node f(Z k |X k ) is based on the measurement equation of the master, which fuses and updates the master-slave coordinate difference and the measurement information of the master, then fuses and updates the master-slave coordinate difference and the position estimate information of the slave, and finally obtains the position estimate of the slave by weighted averaging the position estimate of the slave from each master with the dead-reckoning estimate of the slave.

[0233] ​​​Eighth embodiment: the distance between the slave AUV and the ith master AUV at time k in S8 and and The relationship is:

[0234] The other aspects of the embodiment are the same as the seventh embodiment.

[0235] A multi-AUV adaptive cooperative positioning system based on factor graph node optimization, which has program modules corresponding to the steps of any one of the above embodiments one to eight, and executes the steps of the above multi-AUV adaptive cooperative positioning method based on factor graph node optimization.

[0236] A computer-readable storage medium stores a computer program, which is configured to be called by a processor to implement the steps of the multi-AUV adaptive cooperative positioning method based on factor graph node optimization in any one of embodiments one to eight. Specific embodiment 1

[0238] The multi-AUV adaptive cooperative positioning method based on factor graph node optimization (Dynamic Structure-based Adaptive Optimized Selection and Factor Graph, DS-AOSFG) of the present application is simulated and analyzed with the multi-AUV cooperative positioning method based on node optimization selection and factor graph (Dynamic Structure-based Optimized Selection and Factor Graph, DS-OSFG) based on the present application without processing the measurement noise covariance matrix.

[0239] The basic parameters of the algorithm are set as follows: a cooperative positioning system simulation experiment of 10 master boats and 20 slave boats is designed. The total simulation time is set to 1000s, and the state update period Δt = 1s. The dynamic topology detection period ΔT = 5s. In order to meet the system observability, a trajectory diagram as shown in Figure 5 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 area with a diameter of 3000m centered on itself, and the system dynamic change is as shown in Figure 6 ; based on the underwater acoustic ranging scene, the ranging variance of the speed and heading angle of the master AUV and the slave AUV is set to and In the DS-AOSFG algorithm comparison experiment, the position information of 1, 3, 5, 7-9, a total of six main AUVs is superimposed with a mean value of 0 and a variance of Gaussian white noise, and the measurement noise mean value is set to 0 and the variance is 2, 4, 6, 10, a total of four main AUV nodes, the position information is superimposed with a mean value of 0 and a variance of Gaussian white noise, and the measurement noise mean value is set to 1 and the variance is

[0240] Measurement noise mean value estimation initial value selection Variance estimation initial value selection The system state estimation initial value is:

[0241]

[0242] P 0|0 = diag[1 1 0.01]

[0243] Simulation results and analysis

[0244] The optimal node number is set to M=6, and the DS-AOSFG algorithm and the DS-OSFG algorithm are applied to the simulation scene. The positioning error of the two algorithms is shown in Figure 7 and Figure 8 The positioning error of the DS-OSFG algorithm is larger and fluctuates obviously, because the fuzzy time-varying noise interferes with the selection of high-quality AUV nodes, and the increased ranging error also affects the positioning effect of the algorithm; DS-AOSFG estimates the measurement noise covariance matrix adaptively, which not only improves the state variable estimation accuracy in the measurement update process, but also provides more accurate measurement noise variance estimation results for the node selection mechanism. Table 1 shows the root mean square error (RMSE) of the two algorithms. From the table, it can be seen that the RMSE of the DS-AOSFG algorithm is reduced by 48.63% compared with the DS-OSFG algorithm, because the adaptive EKF filter is used in the DS-AOSFG algorithm, which can ensure the adaptive estimation of the measurement noise variance during positioning, reducing the positioning error. The experimental results verify the effectiveness of the DS-AOSFG algorithm in improving the AUV node selection effect and positioning accuracy.

[0245] Table 1

[0246]

[0247] Through simulation experiment verification, it can be found that under the interference of fuzzy time-varying noise, the DS-AOSFG algorithm can better select high-quality nodes compared with the DS-OSFG algorithm, and improve the system positioning accuracy.

[0248] Although the present application has been disclosed with reference to the above examples, the scope of the present application is not limited to the above examples. Those skilled in the art to which the present application pertains will be able to make various changes and modifications without departing from the spirit and scope of the present application, and such changes and modifications will fall within the scope of the present application.

Claims

1. A multi-AUV self-adaptive cooperative localization method based on factor graph node optimization, characterized in that, Comprising the following steps: S1, collecting the dynamic topology structure information of the multi-AUV cooperative positioning system at the current time; S2, updating the slave boat and its neighbor master boat information; S3, initializing the master boat and slave boat information; S4, constructing a multi-AUV cooperative positioning system factor graph model; The slave state variable, the master position information and the master measurement information are defined as variable nodes, the state equation and the measurement equation are defined as function nodes, and an adaptive iterative estimation function node and a node optimization function node are defined; the state equation function node is used to perform a transfer update on the slave state variable node , the measurement equation function node is used to perform a fusion update on the master measurement information node and the slave state variable node , the adaptive iterative estimation function node is an adaptive EKF filter based on an EM algorithm, and is used to estimate a measurement noise covariance matrix, and the node optimization function node is used to perform Cramer-Rao lower bound and range evaluation factor calculation on the position information node and the measurement information node of the system master, and to optimize the master node at the current time. S5, based on the sum-product algorithm in the multi-AUV cooperative positioning system factor graph model, transmitting information in both directions in the factor graph once, realizing global factor graph node information transmission and update; S6, based on the adaptive iteration estimation function node, iteratively updating the measurement noise covariance matrix; S7, through the node optimization function node, optimizing the master boat node; S8, based on the optimized master boat node, fusing and updating the state variable node and the measurement variable node information to obtain the current time slave boat position information estimate value; S5 comprises the following process: At the kth time, the system conditional probability density function is decomposed as: wherein, denotes the number of master AUV nodes; denotes the measurement information of the nth master AUV, wherein ; denotes the position information of the nth master AUV; denotes the probability factor corresponding to each AUV node, that is: wherein denotes the measurement function; denotes the measurement true value; denotes the covariance matrix corresponding to the measurement error; The ranging information d and the heading angle collected by the system are defined as and the velocity v of the AUV obeys a Gaussian distribution: wherein represents the ranging information from the AUV and the first main AUV; By a state equation function node To a variable node The information passed is: Variable node To state equation function node The information passed is: where and represent the prior estimate and variance of the state variable respectively; According to the cooperative positioning position equation: wherein, represents coordinates of the AUV in the reference coordinate system at time k; represents a forward velocity of the AUV at time k; represents a heading angle of the AUV at time k; represents a sampling interval; The state transition formula is obtained as: wherein is the system process noise covariance matrix, is the measurement noise matrix, The expression for is In the formula, is heading angle information corresponding to the moment; Substitute formula (6) and formula (7) into formula (5), combine to get the final confidence information: In the formula, The expression of the formula is: Finally, realize global factor graph node information transmission and update.

2. The factor graph based node-preferred multi-AUV self-adaptive cooperative localization method according to claim 1, characterized in that, In S1, the current dynamic topology information of the system is collected in each collection period T, including: the number of master AUVs and slave AUVs, position information, speed information v, angular velocity information and heading angle information of the master AUVs and the slave AUVs And the variance of each collected quantity is calculated, and the ranging information d between the target and each master boat is detected.

3. The factor graph based node-preferred multi-AUV self-adaptive cooperative localization method according to claim 2, characterized in that, The initialization of master and slave boat information described in S3 includes: initializing the position information, speed 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 factor graph based node-preferred multi-AUV self-adaptive cooperative localization method according to claim 3, characterized in that, In S6, the slave boat state variable equation is constructed according to the EKF filtering algorithm, that is: （11） In the formula, denotes the estimate of the kth time point obtained at the k-1th time point, denotes a state transition matrix, denotes a control input, ; The corresponding estimation error covariance matrix is constructed, that is: （12） Update the slave boat state variable and estimation error covariance matrix; According to the EM algorithm, first determine the initial value: （13） The first Iterative update of the step filter gain matrix: （14） In the formula, is: （15） Update the state variable using formula (14): （16） Update the error covariance matrix: （17） Estimating a measurement noise covariance matrix : （18） iterations the estimate after the sub-convergence is obtained 。 5. The factor graph based node-preferred multi-AUV adaptive cooperative localization method according to claim 4, wherein, S7 comprises the following process: calculating the Cramer-Rao lower bound for each master AUV node and the ranging evaluation factor : （19） in, express The state variables of the AUV at any given time; ,express Time of the first Location information of the main AUV; ; express The error covariance matrix; express The error covariance matrix; Indicates from AUV and the first Distance measurement information of the main AUV; express Standard deviation; establishing node preferred parameter matrix at the moment: （20） wherein denotes the trace of a matrix; denotes the number of mother ships contained in the system at the moment The parameters in the node optimization parameter matrix NSPM are weighted by using the information entropy method. First, the proportion of each evaluation index is calculated : （21） wherein denotes the CRLB evaluation parameter; denotes the ranging evaluation factor; Calculate the entropy value of the parameter: （22） Calculate the difference coefficient: （23） Calculate the weight of the two indexes: （24） Constructing weight vector : （25） Weighted processing is performed on the node optimization parameter matrix NSPM: （26） The final evaluation result of each column corresponds to the final evaluation result of the corresponding master AUV in the system, and the master AUV corresponding to the smallest result is selected as the preferred result. Matrix In the matrix, the numerical value of each column corresponds to the final evaluation result of the corresponding master AUV in the system, and the master AUV corresponding to the smallest result is selected as the preferred result. In the matrix, the numerical value of each column corresponds to the final evaluation result of the corresponding master AUV in the system, and the master AUV corresponding to the smallest result is selected as the preferred result 6. The factor graph based node-preferred multi-AUV self-adaptive cooperative localization method according to claim 5, characterized in that, S8 comprises the following process: For The coordinate difference between the AUV and the first The coordinate difference between the AUV and the first and The belief information of the variable node is calculated as （27） wherein and respectively represent and the standard deviation of represents the distance between the AUV and the first master AUV at time k. Computing variable nodes The passed belief information is: （28） In the formula representing the standard deviation; Computing variable nodes and the belief information of the variable nodes （29） （30） Computing variable nodes and the belief information of （31） （32） Estimating the position of each master to slave passing to i.e.: （33） wherein and is the variance and expectation of The same applies to the information of : （34） wherein and is the variance and expectation of Weighted average is performed on the slave boat position estimation and the slave boat dead reckoning estimation: （35） （36）。 7. The factor graph based node-preferred multi-AUV self-adaptive cooperative localization method according to claim 6, characterized in that, The distance between the AUV and the first master AUV at time k in S8 The relationship is:​​​ （37）。 8. A multi-AUV self-adaptive cooperative localization system based on factor graph and node preference, characterized in that, The system has program modules corresponding to the steps of any one of claims 1-7, and when running, it executes the steps of the above multi-AUV adaptive cooperative positioning method based on factor graph node optimization.

9. A computer-readable storage medium, characterized in that, The computer readable storage medium stores a computer program, which is configured to realize the steps of the multi-AUV adaptive cooperative positioning method based on factor graph node optimization of any one of claims 1-7 when called by the processor.

Images