AUV formation tracking control method based on dynamic topology switching strategy, program, equipment and storage medium

Abstract

The invention belongs to the technical field of AUV formation control, and particularly relates to an AUV formation tracking control method based on a dynamic topology switching strategy, a program, equipment and a storage medium. According to the method, a new communication topological structure is generated by using 2-Delaunay, and the problem of non-unique connected components is avoided by solving the shortest path between different sets in combination with the optimal communication distance and the maximum communication distance, so that topological network optimization is completed; a dynamic topology switching scheme based on a Jaccard coefficient is adopted for control, when an intelligent agent communication topology in a formation is interrupted or reconnected, a topology network structure is switched to cover a formation network, and therefore the purpose of achieving real-time monitoring and dynamic management of the topology through measurement between an expected topology and a self topology is achieved. A DMPC is adopted as a formation controller, and efficient and accurate formation control of a plurality of AUVs in a complex environment is realized by utilizing the excellent capability of the DMPC in the aspects of system dynamic characteristic modeling and future behavior prediction.

Description

Technical Field

[0001] The present invention belongs to the technical field of AUV formation control, and in particular relates to an AUV formation tracking control method, program, device and storage medium based on a dynamic topology switching strategy. Background Art

[0002] AUVs can be divided into full-drive, over-drive and under-drive according to the configuration of thrusters. Usually, the number of thrusters to be equipped is determined by the mechanical designer based on the expected functions of the AUV and combined with the cost, weight and difficulty of achieving the task, so as to achieve cost savings and effectively achieve the control objectives. Considering the motion control of the three-degree-of-freedom AUV on the horizontal plane, the AUV horizontal plane dynamic model does not have lateral thrusters installed, resulting in no lateral driving force, thereby saving power load, but putting higher requirements on the algorithm of the controlled system. In the previous design of underwater multi-agent formation control, the unpredictability of underwater acoustic communication interruption is easily overlooked, so directed topology switching can be used to solve these problems. However, when there are agents in the formation exiting or entering the formation, the pre-set topology structure cannot cover the formation network, which leads to reduced formation stability. Summary of the invention

[0003] The purpose of the present invention is to provide an AUV formation tracking control method based on a dynamic topology switching strategy, which can generate a current position topology structure network at any time, thereby ensuring that the underwater AUV formation is fully connected during the control process.

[0004] An AUV formation tracking control method based on a dynamic topology switching strategy comprises the following steps:

[0005] Step 1: At the initial moment, the AUV formation forms an initial communication topology;

[0006] The AUV formation includes a leading AUV and the rest are follower AUVs. Each node in the communication topology structure represents an AUV in the AUV formation, and there is no isolated node in the communication topology structure.

[0007] Step 2: At the current moment, the leading AUV obtains its own position information; the following AUV in the AUV formation obtains the relative distance and relative angle between the adjacent nodes and itself in the communication topology as measurement information, and uses adaptive Kalman filtering to achieve adaptive compensation for process noise and measurement noise based on the state space model under the measurement-communication joint framework, and estimates its own position information;

[0008] Step 3: Obtain the expected position information of the AUV formation at the current moment and construct the expected spanning tree communication topology structure;

[0009] Step 4: Calculate the Jaccard coefficient between the communication topology of the AUV formation at the current moment and the communication topology of the expected spanning tree; if the Jaccard coefficient reaches the preset threshold, the communication topology of the AUV formation at the current moment remains unchanged; otherwise, generate a new communication topology based on 2-Delaunay;

[0010] Step 5: According to the expected position information, communication topology and status information of the AUV formation at the current moment, the optimal control input of each AUV is calculated through the distributed DMPC formation controller, and fed back to each AUV in the AUV formation to complete the tracking control at the current moment;

[0011] Step 6: If the AUV formation fails to complete the mission, return to step 2 to continue.

[0012] Furthermore, the state space model under the measurement-communication joint framework in step 2 is:

[0013]

[0014] in, Indicates the position information of the i-th following AUV at the current k moment; represents the measurement information of the i-th following AUV at the current k moment; Φ is the state transfer matrix; h(·) is the measurement function; w k-1 and δ k are process noise and measurement noise respectively;

[0015] For each following AUV in the AUV formation, the relative distance and relative angle between the adjacent nodes and itself in the communication topology are obtained as measurement information. Adaptive Kalman filtering method is used to adaptively compensate the process noise and measurement noise of the state space model under the measurement-communication joint framework to obtain its own position information

[0016] Furthermore, in step 2, the method of using the adaptive Kalman filtering method to adaptively compensate the process noise and measurement noise of the state space model under the measurement-communication joint framework is specifically as follows:

[0017] Step 2.1: Calculate the process noise mean matrix and the error covariance matrix prediction

[0018]

[0019] Among them, d k-1 =(1-b) / (1-b k ), b is the forgetting factor, 0<b<1; is the position information of the i-th following AUV calculated at time k-1; is the error covariance matrix calculated at time k-1; is the process noise variance matrix calculated at time k-1;

[0020] Step 2.2: Calculate position prediction

[0021]

[0022] Step 2.3: Calculate the measurement noise mean matrix

[0023]

[0024] Where H is the Jacobian matrix of the measurement function h(·) at the origin;

[0025] Step 2.4: Calculate measurement predictions

[0026]

[0027] Step 2.5: Calculate the variance matrix of the measurement noise

[0028]

[0029] Step 2.6: Calculate the adaptive Kalman filter gain

[0030]

[0031] Step 2.7: Calculate the error covariance matrix

[0032]

[0033] Where I is the identity matrix;

[0034] Step 2.8: Calculate the position information of the i-th following AUV at the current k moment and the variance matrix of the process noise

[0035]

[0036] Furthermore, the Jaccard coefficient between the communication topology of the AUV formation at the current moment and the communication topology of the expected spanning tree is calculated in step 4. Specifically:

[0037]

[0038] Among them, G σ(t) represents the communication topology of the AUV formation at the current moment; G σ(e) Indicates the expected spanning tree communication topology at the current moment.

[0039] Furthermore, the method for generating a new communication topology structure based on 2-Delaunay in step 4 is specifically as follows:

[0040] Step 4.1: Let the position of each node in the AUV formation be marked as V = {n 1 ,n 2 ,…,n N},n 1 is the pilot AUV, N is the total number of AUVs in the AUV formation; let the three vertices A, B, C of the super triangle make co(n) = {A, B, C, V} be the convex hull of the vertex set V, that is, add the vertices of the super triangle to the vertex list, and the triangulation of the vertex set V is expressed as Δ(C n );

[0041] For all triangles in the triangle list chain, calculate the center and radius of their circumscribed circle. If the point in the vertex list is within this circumscribed circle, add the three edges of this triangle to the edge set. Finally, delete all duplicate edges and edges containing super triangle vertices to complete the generation of the preliminary topological network, that is, the topological network structure diagram after Delaunay triangulation G=(V,E) with vertex set V={1,2,…,N} and edge set E={(i,j),i,j∈V,i≠j};

[0042] Step 4.2: Set the optimal communication distance R o and the maximum communication distance R m As the initial parameter, R o <R m , the elements in the edge set E that are greater than the optimal communication distance ||e ij ||>R o Delete and get the communication topology network G * =(V,E * ).

[0043] Furthermore, the communication topology network G is obtained in step 4.2. * =(V,E * ) then, if G * =(V,E * ), then take the position of the isolated point as the center of the circle and the maximum communication distance R m The search is performed with radius; if a node is found, an edge is established between the isolated point and the nearest node; if no node is found, an edge is established between the isolated point and the pilot AUV node.

[0044] Furthermore, in step 5, the method for calculating the optimal control input of each AUV according to the expected position information, communication topology and state information of the AUV formation at the current moment is specifically as follows:

[0045] Step 5.1: Establish a distributed DMPC formation controller. The discrete time system of the tracking and control dynamic process is expressed as:

[0046] ξ i (k+1)=A i ξ i (k)+B i u i (k)

[0047] Ξ i (k) = Cξ i (k)

[0048] Among them, ξ i (k) is the status information of the i-th AUV at the current k moment, including position, attitude and speed information; u i (k) is the control input of the i-th AUV at the current k-th time to be solved; i (k) is the predicted output; C = [I 3×3 O 3×3 ], matrix A i With B i According to the motion model of the i-th AUV, we can obtain: M i With D i are the inertia matrix and damping matrix of the i-th AUV respectively;

[0049] Set the prediction interval N p and control interval N c , define the predictive control sequence Predict output sequence for:

[0050]

[0051] ξ i (k) and satisfy:

[0052]

[0053] in,

[0054] Step 5.2: Convert the formation tracking control problem into the following optimization problem:

[0055]

[0056] The constraints are:

[0057] (1)

[0058] (2)Δy min ≤||ξ i (k)-ξ j (k)||<Δy max ;

[0059] (3) s∈[k+1,k+N c -1];

[0060] Among them, N p and N c is the prediction interval and control interval, N c ≤N p ; is the element in the i-th row and j-th column of the adjacency matrix corresponding to the communication topology structure of the AUV formation at the current time k; is the expected distance between the i-th AUV and the j-th AUV; Δy min with Δy max They represent the lower and upper limits of the distance between each AUV in the AUV formation; u min with u max Respectively represent the lower and upper limits of the control input;

[0061] The optimal predictive control sequence is obtained by solving the optimization problem:

[0062]

[0063] Pick The first term in That is, the optimal control input of the i-th AUV is obtained.

[0064] A computer device / equipment / system comprises a memory, a processor and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the AUV formation tracking control method based on the dynamic topology switching strategy.

[0065] A computer-readable storage medium stores a computer program / instruction, which, when executed by a processor, implements the steps of the AUV formation tracking control method based on the dynamic topology switching strategy.

[0066] A computer program product includes a computer program / instruction, which, when executed by a processor, implements the steps of the AUV formation tracking control method based on the dynamic topology switching strategy.

[0067] The beneficial effects of the present invention are:

[0068] The present invention adopts AUV formation tracking control under the dynamic topology switching strategy. The AUV formation control system designed based on this can complete the topology network optimization by solving the shortest path between different sets as a new communication link when the connected components are not unique; the present invention uses 2-Delaunay to generate a new communication topology structure, and combines the optimal communication distance and the maximum communication distance to solve the shortest path between different sets to avoid the problem of non-unique connected components, thereby completing the topology network optimization. The present invention adopts a dynamic topology switching scheme control based on the Jaccard coefficient. When the communication topology of the intelligent body in the formation is interrupted or restored, the switching topology network structure covers the formation network, thereby achieving the purpose of real-time monitoring and dynamic management of the topology by using the metric between the expected topology and its own topology. The present invention adopts DMPC as a formation controller, giving full play to the excellent ability of MPC in modeling system dynamic characteristics and predicting future behaviors, thereby realizing efficient and accurate formation control of multiple AUVs in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0069] Figure 1 Schematic diagram of the AUV formation motion coordinate system.

[0070] Figure 2 The present invention provides an overall flow chart of the AUV formation tracking control method based on the dynamic topology switching strategy.

[0071] Figure 3 Schematic diagram of the non-uniqueness of isolated points and connected components in a topological network structure.

[0072] Figure 4 Generate block diagrams for topological networks based on location information.

[0073] Figure 5 Schematic diagram of dynamic topology switching strategy.

[0074] Figure 6 Design diagram for the formation control framework based on intermittent broadcast and DMPC.

[0075] Figure 7 It is a schematic diagram of the motion trajectory and topological changes. DETAILED DESCRIPTION

[0076] The present invention is further described below in conjunction with the accompanying drawings.

[0077] Usually, multiple AUV formations navigate under constant depth steady-state control. Therefore, this paper takes three-degree-of-freedom AUV as the research object. Assuming that all AUVs in the formation have a fixed posture at the current moment and their state information is known, the planar dynamic model of AUV can be defined as:

[0078]

[0079] Among them, η i =[x i ,y i ,ψ i ] T and v i =[u i ,v i ,r i ] T They are the position heading in the inertial coordinate system and the velocity state in the carrier coordinate system. The models of the distributed AUV formation in the two coordinate systems are as follows: Figure 1 shown. is the rotation matrix, M i =diag{m 11 ,m 22 ,m 33} is the inertia matrix, D i (v i ) = diag{-X u -X u|u| u v -Y v|v| |v|-N r -N r|r| |r|}, are the damping matrix and the Coriolis and centripetal force matrices respectively, where:

[0080]

[0081] τ i and τ iw are the control input and external disturbance of the system respectively.

[0082] Assumption 1: The AUV is considered as a rigid body and its deformation or bending is ignored.

[0083] Assumption 2: The underwater uncertain disturbance is bounded, that is, it satisfies: ||τ iw ||≤W 0 ,W 0 ∈R + ,

[0084] Assumption 3: The virtual reference trajectory of the pilot AUV and its first-order and second-order derivatives exist and are bounded.

[0085] The present invention proposes a formation control strategy under an adaptive measurement-communication joint framework to avoid the impact of a weak communication environment while achieving formation control. The specific block diagram is as follows: Figure 2 As shown:

[0086] Based on the measurement-communication joint framework, the state of itself and adjacent nodes can be estimated using measurement information such as relative distance measurement and relative angle, thereby replacing the direct interaction of state information through underwater acoustic broadcasting and communication. However, due to the complex underwater environment, there are also noise disturbances that cannot be ignored in the measurement-communication link in the measurement-communication joint framework. Therefore, the present invention introduces AKF to achieve adaptive compensation for external noise disturbances. First, the state space model under the communication-measurement framework is established as:

[0087]

[0088] in, represents the state vector of AUV at time k, that is Indicates the position information of the i-th following AUV at the current k moment; represents the measurement vector of AUV at time k, that is, the relative distance and relative angle between the adjacent nodes and itself in the communication topology structure, Φ is the state transfer matrix; h(·) is the measurement function; relative distance and relative angle They are:

[0089]

[0090] For process noise and measurement noise, their statistical characteristics are assumed to meet the following conditions:

[0091]

[0092] Among them, q k and r k represents the mean of process noise and measurement noise; Q k and R k is the corresponding variance matrix. Secondly, an adaptive time-varying noise statistical recursive estimator is established:

[0093]

[0094] Where H is the Jacobian matrix of the measurement function at the origin. The adaptive Kalman filter (AKF) matching the above formula is:

[0095]

[0096] Among them, d k-1 =(1-b) / (1-b k), 0<b<1 is the forgetting factor. Through the above adaptive measurement-communication joint framework, the complex noise in the ranging and heading angle can be compensated, and finally the observation state after compensation at the current moment can be obtained.

[0097] In the previous design of underwater multi-agent formation control, the unpredictability of underwater acoustic communication interruption is easily overlooked, so directed topology switching can be used to solve these problems. However, when there are agents in the formation exiting or entering the formation, the pre-set topology structure cannot cover the formation network, which leads to reduced formation stability.

[0098] Aiming at the problem of underwater multi-agent formation communication topology generation, the general law of rigid graph generation is first analyzed through the properties of rigid graph, and 2-Delaunay triangulation is introduced to generate topological graph based on distance, and then the topological network structure of the formation at different times is generated. First, the position of each node in the formation is marked as V = {n 1 ,n 2 ,…,n N}, let the three vertices of the super triangle A, B, C make co(n) = {A, B, C, V} be the convex hull of the vertex set V, that is, add the vertices of the super triangle to the vertex list. The triangulation of the vertex set V can be expressed as Δ(C n ). For all triangles in the triangle list chain, calculate the center and radius of their circumscribed circle. If the point in the vertex list is within this circumscribed circle, add the three edges of this triangle to the edge set. Finally, delete all duplicate edges and edges containing super triangle vertices to complete the generation of the preliminary topological network.

[0099] After obtaining the Delaunay triangulation G=(V,E) with vertices V={1,2,…,N}(N>3) and edge set E={(i,j),i,j∈V,i≠j} on the plane, the topological network structure graph is connected. However, since underwater acoustic communication cannot communicate frequently over long distances, we set the optimal communication distance R o and the maximum communication distance R m As the initial parameter R o <R m , the elements in the edge set E that are greater than the optimal communication distance ||e ij ||>R o Delete. After completing the above two steps, we can get a new communication topology network G * =(V,E * ) and its adjacency matrix Q, but the new network topology may have isolated and non-unique connected components, such as Figure 3 .

[0100] These two situations often occur due to frequent failures in underwater acoustic transmission. Isolated points are a specific example of non-unique link components in the new communication topology problem, so dealing with them is a top priority. When an isolated point is found, a global search is performed with the position of the isolated point as the center of the circle and the maximum communication distance as the radius. If the distance to the nearest node is less than the maximum communication distance, the information from the node is received, otherwise, the information from the pilot AUV is received. When the connected component is not unique, this project intends to solve the shortest path between different sets as a new communication link. Thereby completing the topological network optimization. The overall block diagram is as follows Figure 4 shown.

[0101] After completing the above operations, a new communication topology with optimal communication cost can be obtained. However, in the previous design of underwater multi-agent formation control, the unpredictability of underwater acoustic communication interruption is easily overlooked, so directed topology switching can be used to solve these problems. However, when an agent in the formation exits or enters the formation, the pre-set topology structure cannot cover the formation network, which leads to reduced formation stability. Therefore, the present invention proposes a dynamic topology switching scheme based on the Jaccard coefficient.

[0102] The strategy block diagram is shown in Figure 5. The topology switching process is defined as σ(t 1 )→σ(t 2 ), the topology at a certain moment can be expressed as G σ(t) ∈G P , The adjacency matrix of the corresponding topology is A σ(t) ∈R N×N , where the elements are represented as

[0103]

[0104] Furthermore, the Jaccard coefficient between the current time and the expected spanning tree for

[0105]

[0106] Among them, G σ(t) represents the communication topology of the AUV formation at the current moment; G σ(e) represents the expected spanning tree communication topology at the current moment. The number of nodes in the multi-agent targeted by the present invention is limited. Therefore, the Jaccard coefficient can be used to measure G σ(t) and G σ(e)The similarity and diversity between them. During the AUV formation movement, the Jaccard coefficient is used to dynamically evaluate the communication status of the formation. If the Jaccard coefficient between the current communication topology and the optimal spanning tree reaches the preset threshold, the formation communication switching flag will be turned off. Otherwise, the elements of the generated topology are:

[0107]

[0108] The generated topological network structure (23) is optimized in the present invention to ensure that the information flow after the topology switching in this design is acceptable.

[0109] Based on the above dynamic topology switching strategy, the present invention intends to realize formation cooperative control based on relative error through distributed DMPC. When the nodes of the underwater multi-AUV formation do not change, the cooperative formation cost function is described based on the virtual trajectory, predictive controller, control constraints and multi-AUV state information in the formation, and is introduced into the local rolling time domain optimization control problem. If the node increases or the node fails, the communication topology at that moment is first regenerated, and then the formation is maintained or reconstructed through distributed DMPC. Figure 6 The controller design block diagram for node changes is described. The four main components of the architecture are dynamic communication topology calculation, leader intermittent broadcast, state prediction and its optimization.

[0110] The first step is to establish a distributed DMPC formation controller. In the design of the distributed controller, AUVi∈I [1,N] The dynamic model of its tracking control dynamic process can be described by the following state space expression:

[0111]

[0112] in, and are state information and system output respectively. Note that the state equations of each AUV in the formation are independent of each other, that is, the dynamic process of formation tracking control is described by N uncoupled state equations. C=[I 3×3 O 3×3 ]. Combined with the AUV motion model, (24) is converted into a discrete time system:

[0113] ξ i (k+1)=A i ξ i (k)+B i u i (k) (25)

[0114] Ξ i (k) = Cξ i (k)

[0115] in,

[0116] Combining the formation communication topology with the formation member set The AUV individual state is calculated to obtain the "optimal" control input with time lag, that is, the "optimal" input at the current moment. To describe the predicted state, define the current predicted state quantity Predictive Control Quantity Predicted output

[0117]

[0118] Where N p and N c is the prediction interval and control interval (N c ≤N p ), if the control interval is much smaller than the prediction interval, the control effect of the system cannot be guaranteed. On the contrary, if the control interval is approximately the same as the prediction interval, the control range of the latter part will have little effect and increase the amount of system calculation. Based on the prediction state, the output can be obtained as:

[0119]

[0120] Defining the Predictive Control Input Sequence Predict output sequence for:

[0121]

[0122] After sorting, the predicted output sequence is:

[0123]

[0124] in:

[0125]

[0126] Considering that the change in the AUV state during the sampling time is affected by its mechanical structure, the system has the following constraints:

[0127] u min ≤Δu i (k)≤u max k=1,2…N c (31)

[0128] Δu i (k) is the input change between the current moment and the previous moment. Combined with the prediction control sequence The control input constraints can be organized as:

[0129]

[0130] in:

[0131]

[0132] In addition, terminal constraints are introduced:

[0133] ξ i (0|t)=ξ i (t),ξ i (N p |t)=0 (34)

[0134] Define the AUV tracking cost function at time k for:

[0135]

[0136] in, is the expected distance between the i-th AUV and the j-th AUV, is the adjacency matrix of the formation communication topology, is the state deviation cost function; is the control input cost function, which describes the stability control objective of individual i.

[0137] When the formation error is approximately 0:

[0138]

[0139] The AUV formation tracking control problem is transformed into the following optimization problem:

[0140]

[0141] The use of a master-slave control strategy can ensure sufficient flexibility and maneuverability in determining the formation connection relationship and the continuous formation morphology. In actual implementation, due to time-varying external disturbances and local communication between agents, overall formation errors are inevitable. Unlike traditional distributed control systems, the state information of all agents in the formation can be summarized to the leader, which means that the control target of the coordinated formation of agents will not be changed due to the interference of local errors. Therefore, the element corresponding to the leader in the communication topology adjacency matrix is ​​defined as:

[0142]

[0143] Among them, λ is a positive constant, τ represents the time interval, represents the leader formation error, and:

[0144]

[0145] Among them, Δi ∈R 3 represents the relative formation of the leader. For formation control systems, the prediction mechanism of model predictive control allows the predicted states of individuals to be considered in the design, and the control actions can be determined in advance.

[0146] In the second step, the formation tracking control problem is transformed into the following optimization problem:

[0147]

[0148] The optimal solution sequence can be obtained by solving the optimization problem:

[0149]

[0150] The distributed control law of the follower is:

[0151]

[0152] The topological changes and trajectory diagrams of the present invention during the formation movement are shown in FIG. Figure 7 shown.

[0153] Based on the above theoretical derivation, the present invention provides an AUV formation tracking control method based on a dynamic topology switching strategy, which specifically includes the following steps:

[0154] An AUV formation tracking control method based on a dynamic topology switching strategy comprises the following steps:

[0155] Step 1: At the initial moment, the AUV formation forms an initial communication topology;

[0156] The AUV formation includes a leading AUV and the rest are follower AUVs. Each node in the communication topology structure represents an AUV in the AUV formation, and there is no isolated node in the communication topology structure.

[0157] Step 2: At the current moment, the leading AUV obtains its own position information; the following AUV in the AUV formation obtains the relative distance and relative angle between the adjacent nodes and itself in the communication topology as measurement information, and uses adaptive Kalman filtering to achieve adaptive compensation for process noise and measurement noise based on the state space model under the measurement-communication joint framework, and estimates its own position information;

[0158] The state space model under the measurement-communication joint framework is:

[0159]

[0160] in, Indicates the position information of the i-th following AUV at the current k moment; represents the measurement information of the i-th following AUV at the current k moment; Φ is the state transfer matrix; h(·) is the measurement function; w k-1 and δ k are process noise and measurement noise respectively;

[0161] For each following AUV in the AUV formation, the relative distance and relative angle between the adjacent nodes and itself in the communication topology are obtained as measurement information. Adaptive Kalman filtering method is used to adaptively compensate the process noise and measurement noise of the state space model under the measurement-communication joint framework to obtain its own position information

[0162] Step 2.1: Calculate the process noise mean matrix and the error covariance matrix prediction

[0163]

[0164] Among them, d k-1 =(1-b) / (1-b k ), b is the forgetting factor, 0<b<1; is the position information of the i-th following AUV calculated at time k-1; is the error covariance matrix calculated at time k-1; is the process noise variance matrix calculated at time k-1;

[0165] Step 2.2: Calculate position prediction

[0166]

[0167] Step 2.3: Calculate the measurement noise mean matrix

[0168]

[0169] Where H is the Jacobian matrix of the measurement function h(·) at the origin;

[0170] Step 2.4: Calculate measurement predictions

[0171]

[0172] Step 2.5: Calculate the variance matrix of the measurement noise

[0173]

[0174] Step 2.6: Calculate the adaptive Kalman filter gain

[0175]

[0176] Step 2.7: Calculate the error covariance matrix

[0177]

[0178] Where I is the identity matrix;

[0179] Step 2.8: Calculate the position information of the i-th following AUV at the current k moment and the variance matrix of the process noise

[0180]

[0181]

[0182] Step 3: Obtain the expected position information of the AUV formation at the current moment and construct the expected spanning tree communication topology structure;

[0183] Step 4: Calculate the Jaccard coefficient between the communication topology of the AUV formation at the current moment and the communication topology of the expected spanning tree; if the Jaccard coefficient reaches the preset threshold, the communication topology of the AUV formation at the current moment remains unchanged;

[0184] Otherwise, a new communication topology is generated based on 2-Delaunay. The specific steps are:

[0185] Step 4.1: Let the position of each node in the AUV formation be marked as V = {n 1 ,n 2 ,…,n N},n 1 is the pilot AUV, N is the total number of AUVs in the AUV formation; let the three vertices A, B, C of the super triangle make co(n) = {A, B, C, V} be the convex hull of the vertex set V, that is, add the vertices of the super triangle to the vertex list, and the triangulation of the vertex set V is expressed as Δ(C n );

[0186] For all triangles in the triangle list chain, calculate the center and radius of their circumscribed circle. If the point in the vertex list is within this circumscribed circle, add the three edges of this triangle to the edge set. Finally, delete all duplicate edges and edges containing super triangle vertices to complete the generation of the preliminary topological network, that is, the topological network structure diagram after Delaunay triangulation G=(V,E) with vertex set V={1,2,…,N} and edge set E={(i,j),i,j∈V,i≠j};

[0187] Step 4.2: Set the optimal communication distance R o and the maximum communication distance R m As the initial parameter, R o <R m , the elements in the edge set E that are greater than the optimal communication distance ||e ij ||>R o Delete and get the communication topology network G * =(V,E * ).

[0188] Step 4.3: If G * =(V,E * ), then take the position of the isolated point as the center of the circle and the maximum communication distance R m The search is performed with radius; if a node is found, an edge is established between the isolated point and the nearest node; if no node is found, an edge is established between the isolated point and the pilot AUV node.

[0189] Step 5: According to the expected position information, communication topology and status information of the AUV formation at the current moment, the optimal control input of each AUV is calculated through the distributed DMPC formation controller, and fed back to each AUV in the AUV formation to complete the tracking control at the current moment;

[0190] Step 5.1: Establish a distributed DMPC formation controller. The discrete time system of the tracking and control dynamic process is expressed as:

[0191] ξ i (k+1)=A i ξ i (k)+B i u i (k)

[0192] Ξ i (k) = Cξ i (k)

[0193] Among them, ξ i (k) is the status information of the i-th AUV at the current k moment, including position, attitude and speed information; u i(k) is the control input of the i-th AUV at the current k-th time to be solved; i (k) is the predicted output; C = [I 3×3 O 3×3 ], matrix A i With B i According to the motion model of the i-th AUV, we can obtain: M i With D i are the inertia matrix and damping matrix of the i-th AUV respectively;

[0194] Set the prediction interval N p and control interval N c , define the predictive control sequence Predict output sequence for:

[0195]

[0196] ξ i (k) and satisfy:

[0197]

[0198] in,

[0199] Step 5.2: Convert the formation tracking control problem into the following optimization problem:

[0200]

[0201] The constraints are:

[0202] (1)

[0203] (2)Δy min ≤||ξ i (k)-ξ j (k)||<Δy max ;

[0204] (3) s∈[k+1,k+N c -1];

[0205] Among them, N p and N c is the prediction interval and control interval, N c ≤N p ; is the element in the i-th row and j-th column of the adjacency matrix corresponding to the communication topology structure of the AUV formation at the current time k; is the expected distance between the i-th AUV and the j-th AUV; Δy min with Δy max They represent the lower and upper limits of the distance between each AUV in the AUV formation; u min with u max Respectively represent the lower and upper limits of the control input;

[0206] The optimal predictive control sequence is obtained by solving the optimization problem:

[0207]

[0208] Pick The first term in That is, the optimal control input of the i-th AUV is obtained.

[0209] The present invention derives the navigation system model of a three-degree-of-freedom AUV under fixed-depth steady-state control, designs a measurement-communication joint framework to estimate the position information and relative speed information of the AUV by replacing underwater acoustic broadcast direct communication with sensor measurement information, designs a topology generation method based on 2-Delaunay and a dynamic topology switching strategy based on the Jaccard coefficient, and realizes distributed formation control based on DMPC according to the position information and topological structure.

[0210] The present invention adopts AUV formation tracking control under the dynamic topology switching strategy. The AUV formation control system designed based on this can complete the topology network optimization by solving the shortest path between different sets as a new communication link when the connected components are not unique; the present invention uses 2-Delaunay to generate a new communication topology structure, and combines the optimal communication distance and the maximum communication distance to solve the shortest path between different sets to avoid the problem of non-unique connected components, thereby completing the topology network optimization. The present invention adopts a dynamic topology switching scheme control based on the Jaccard coefficient. When the communication topology of the intelligent body in the formation is interrupted or restored, the switching topology network structure covers the formation network, thereby achieving the purpose of real-time monitoring and dynamic management of the topology by using the metric between the expected topology and its own topology. The present invention adopts DMPC as a formation controller, giving full play to the excellent ability of MPC in modeling system dynamic characteristics and predicting future behaviors, thereby realizing efficient and accurate formation control of multiple AUVs in complex environments.

[0211] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A method for tracking and controlling an AUV formation based on a dynamic topology switching strategy, characterized in that: The following steps are involved: Step 1: At the initial moment, the AUV formation forms an initial communication topology; The AUV formation includes a leading AUV and the rest are follower AUVs. Each node in the communication topology structure represents an AUV in the AUV formation, and there is no isolated node in the communication topology structure. Step 2: At the current moment, the leading AUV obtains its own position information; the following AUV in the AUV formation obtains the relative distance and relative angle between the adjacent nodes and itself in the communication topology as measurement information, and uses adaptive Kalman filtering to achieve adaptive compensation for process noise and measurement noise based on the state space model under the measurement-communication joint framework, and estimates its own position information; Step 3: Obtain the expected position information of the AUV formation at the current moment and construct the expected spanning tree communication topology structure; Step 4: Calculate the Jaccard coefficient between the communication topology of the AUV formation at the current moment and the communication topology of the expected spanning tree; if the Jaccard coefficient reaches the preset threshold, the communication topology of the AUV formation at the current moment remains unchanged; Otherwise, a new communication topology is generated based on 2-Delaunay; Step 5: According to the expected position information, communication topology and status information of the AUV formation at the current moment, the optimal control input of each AUV is calculated through the distributed DMPC formation controller, and fed back to each AUV in the AUV formation to complete the tracking control at the current moment; Step 6: If the AUV formation fails to complete the mission, return to step 2 to continue.

2. The AUV formation tracking control method based on a dynamic topology switching strategy according to claim 1 is characterized in that: The state space model under the measurement-communication joint framework in step 2 is: in, Indicates the position information of the i-th following AUV at the current k moment; represents the measurement information of the i-th following AUV at the current k moment; Φ is the state transfer matrix; h(·) is the measurement function; w k-1 and δ k are process noise and measurement noise respectively; For each following AUV in the AUV formation, the relative distance and relative angle between the adjacent nodes and itself in the communication topology are obtained as measurement information. Adaptive Kalman filtering method is used to adaptively compensate the process noise and measurement noise of the state space model under the measurement-communication joint framework to obtain its own position information 3. The AUV formation tracking control method based on the dynamic topology switching strategy according to claim 2 is characterized in that: In step 2, the adaptive Kalman filter method is used to adaptively compensate the process noise and measurement noise of the state space model under the measurement-communication joint framework. Specifically, the method is as follows: Step 2.1: Calculate the process noise mean matrix and the error covariance matrix prediction Among them, d k-1 =(1-b) / (1-b k ), b is the forgetting factor, 0<b<1; is the position information of the i-th following AUV calculated at time k-1; is the error covariance matrix calculated at time k-1; is the process noise variance matrix calculated at time k-1; Step 2.2: Calculate position prediction Step 2.3: Calculate the measurement noise mean matrix Where H is the Jacobian matrix of the measurement function h(·) at the origin; Step 2.4: Calculate measurement predictions Step 2.5: Calculate the variance matrix of the measurement noise Step 2.6: Calculate the adaptive Kalman filter gain Step 2.7: Calculate the error covariance matrix Where I is the identity matrix; Step 2.8: Calculate the position information of the i-th following AUV at the current k moment and the variance matrix of the process noise 4. The AUV formation tracking control method based on a dynamic topology switching strategy according to claim 1 is characterized in that: In step 4, the Jaccard coefficient between the communication topology structure of the AUV formation at the current moment and the communication topology structure of the expected spanning tree is calculated. Specifically: Among them, G σ(t) represents the communication topology of the AUV formation at the current moment; G σ(e) Indicates the expected spanning tree communication topology at the current moment.

5. The AUV formation tracking control method based on a dynamic topology switching strategy according to claim 1 is characterized in that: The method for generating a new communication topology structure based on 2-Delaunay in step 4 is specifically as follows: Step 4.1: Let the position of each node in the AUV formation be marked as V = {n1, n2, ..., n N }, n1 is the pilot AUV, N is the total number of AUVs in the AUV formation; let the three vertices A, B, C of the super triangle make co(n) = {A, B, C, V} be the convex hull of the vertex set V, that is, add the vertices of the super triangle to the vertex list, and the triangulation of the vertex set V is expressed as Δ(C n ); For all triangles in the triangle list chain, calculate the center and radius of their circumscribed circle. If the point in the vertex list is within this circumscribed circle, add the three edges of this triangle to the edge set. Finally, delete all duplicate edges and edges containing super triangle vertices to complete the generation of the preliminary topological network, that is, the topological network structure diagram after Delaunay triangulation G=(V,E) with vertex set V={1,2,…,N} and edge set E={(i,j),i,j∈V,i≠j}; Step 4.2: Set the optimal communication distance R o and the maximum communication distance R m As the initial parameter, R o <R m , the elements in the edge set E that are greater than the optimal communication distance ||e ij ||>R o Delete and get the communication topology network G * =(V,E * ).

6. The AUV formation tracking control method based on the dynamic topology switching strategy according to claim 5 is characterized in that: The communication topology network G is obtained in step 4.2 * =(V,E * ) then, if G * =(V,E * ), then take the position of the isolated point as the center of the circle and the maximum communication distance R m The search is performed with radius; if a node is found, an edge is established between the isolated point and the nearest node; if no node is found, an edge is established between the isolated point and the pilot AUV node.

7. The AUV formation tracking control method based on a dynamic topology switching strategy according to claim 1 is characterized in that: In step 5, the method for calculating the optimal control input of each AUV according to the expected position information, communication topology and state information of the AUV formation at the current moment is specifically as follows: Step 5.1: Establish a distributed DMPC formation controller. The discrete time system of the tracking and control dynamic process is expressed as: x i (k+1)=A i x i (k)+B i you i (k) Ξ i (k)=Cξ i (k) Among them, ξ i (k) is the status information of the i-th AUV at the current k moment, including position, attitude and speed information; u i (k) is the control input of the i-th AUV at the current k-th time to be solved; i (k) is the predicted output; C = [I 3×3 O 3×3 ], matrix A i With B i According to the motion model of the i-th AUV, we can obtain: M i With D i are the inertia matrix and damping matrix of the i-th AUV respectively; Set the prediction interval N p and control interval N c , define the predictive control sequence Predict output sequence for: ξ i (k) and satisfy: in, Step 5.2: Convert the formation tracking control problem into the following optimization problem: The constraints are: (1) (2)No min ≤||ξ i (k)-ξ j (k)||<Δy max ; (3) s∈[k+1,k+N c -1]; Among them, N p and N c is the prediction interval and control interval, N c ≤N p ; is the element in the i-th row and j-th column of the adjacency matrix corresponding to the communication topology structure of the AUV formation at the current time k; is the expected distance between the i-th AUV and the j-th AUV; Δy min with Δy max They represent the lower and upper limits of the distance between each AUV in the AUV formation; u min with u max Respectively represent the lower and upper limits of the control input; The optimal predictive control sequence is obtained by solving the optimization problem: Pick The first term in That is, the optimal control input of the i-th AUV is obtained.

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

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

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

Images