Underwater robot formation control method based on virtual structure and cost equalization method

Abstract

The invention discloses an underwater robot formation control method based on a virtual structure and a cost equalization method, and the method comprises the following steps: designing a polymorphic formation library, and importing the polymorphic formation library into a main AUV; establishing a random coordinate system by taking the master AUV as a center, generating a three-dimensional virtual matrix, and introducing a virtual AUV between the slave AUV and the corresponding virtual mass point; selecting a corresponding formation from the polymorphic formation library of the main AUV according to task requirements; calculating the cost of each member to the target point through a cost model according to the state of the current formation member based on the selected formation, and obtaining a cost matrix; task allocation is carried out on the target points through a cost equalization method; and the slave AUV realizes formation reconstruction by tracking the motion trail of the virtual AUV. According to the method, the flexibility and adaptability of the formation are enhanced, the safety and reliability in the task execution process are effectively improved, and a brand new solution is provided for application of the AUV in complex operation.

Description

Technical Field

[0001] The present invention belongs to the field of autonomous underwater vehicles, and relates to underwater robot formation control technology, and in particular to an underwater robot formation control method based on a virtual structure and a cost balance method. Background Art

[0002] In the submarine cable laying mission, AUV (underwater robot) has gradually become an important tool to replace traditional ships and manual operations due to its autonomy, mobility and efficiency. However, since the laying mission usually involves long-distance operations and the submarine environment is complex and changeable, a single AUV often has difficulty in providing sufficient power support when facing high currents, complex terrain and high-resistance equipment, resulting in limited mission efficiency.

[0003] In order to overcome these limitations, AUV formation control technology came into being. Formation technology allows multiple AUVs to work together to form an organic formation, which can effectively share the workload in the mission and reduce the pressure of a single AUV, thereby improving the overall mission execution efficiency. Especially in some high currents and complex environments, AUV formations can enhance the overall propulsion capability and improve the stability and operating efficiency of the system through reasonable formation forms and traction distribution. However, the current AUV formation technology still faces many challenges, especially in formation adjustment, traction distribution, formation stability and flexibility, which still need further optimization and innovation.

[0004] Although existing AUV formation control methods, such as virtual structure method, behavioral model method and distributed control method, have improved system efficiency to a certain extent, their limitations are still obvious. The virtual structure method relies on static geometric constraints, lacks flexibility, and is difficult to adapt to changes in dynamic environments; the behavioral model method and distributed control method face challenges in control accuracy and coordination when facing complex environments. In addition, existing research has paid insufficient attention to AUV traction distribution and real-time formation reconstruction.

[0005] Therefore, a new technical solution is needed to solve these problems. Summary of the invention

[0006] Purpose of the invention: In order to overcome the limitations of existing underwater robot formation control methods in dynamic environments and complex tasks, a method for underwater robot formation control based on virtual structure and cost balance method is provided, which not only enhances the flexibility and adaptability of the formation, but also effectively improves the safety and reliability during task execution, and provides a new solution for the application of AUV in complex operations.

[0007] Technical solution: To achieve the above-mentioned purpose, the present invention provides an underwater robot formation control method based on virtual structure and cost balance method, comprising the following steps:

[0008] S1: Design a multi-form formation library and import it into the main AUV;

[0009] S2: Establish a body coordinate system with the master AUV as the center, generate a three-dimensional virtual matrix, and introduce a virtual AUV between the slave AUV and its corresponding virtual particle;

[0010] S3: According to the mission requirements, select the corresponding formation from the multi-form formation library of the main AUV;

[0011] S4: Based on the selected formation, the cost of each member to the target point is calculated through the cost model according to the status of the current formation members to obtain the cost matrix;

[0012] S5: According to the cost matrix, the task is assigned to the target point through the cost balance method;

[0013] S6: According to the task allocation plan, the slave AUVs realize formation reconstruction by tracking the motion trajectory of the virtual AUV.

[0014] Furthermore, the multi-form formation library in step S1 includes a vertical traction formation, a horizontal traction formation, a triangular traction formation and a prism traction formation.

[0015] Furthermore, the construction of the three-dimensional virtual matrix in step S2 includes:

[0016] A1: Determine the central reference point and establish a body coordinate system with the main AUV as the center. The coordinate system is located at the center of the main AUV and moves with the main AUV.

[0017] A2: In the body coordinate system, a three-dimensional matrix network m×n×p is constructed with the main AUV as the center. The grid spacing is defined by the row spacing L, column spacing H, and layer spacing D. The formation morphology is dynamically scaled by adjusting the parameters of L, H, and D to adapt to different mission requirements.

[0018] Furthermore, the cost model in step S4 includes:

[0019] Assume that there are n members in the formation {U 1 ,U 2 ,...,U n}, the objective function set is {P 1 ,P 2 ,...,P n}; Member U i Assign to position P k The cost matrix C ik Defined as:

[0020]

[0021] Where, d ikrepresents the Euclidean distance from the current position i to the target position k of the member, which is used to measure the path; θ ik Indicates the angular deviation relative to the original formation during the adjustment process; p ij Indicates the current AUV i and other AUVs j The relative position between them is expressed as the collision avoidance penalty term; f(·), g(·), h(·) are weight functions, which measure the weights of distance cost and direction cost in the total cost respectively, and can be dynamically adjusted according to actual task requirements. The expression of weight function f(·), g(·), h(·) is:

[0022]

[0023] g(θ ik )=λ 2 Δθ ik A

[0024] Δθ ik =|θ final,k -θ inital,i |=|(α k -α i )+(β k -β i )+(γ k -γ i )|

[0025]

[0026]

[0027] In the formula, F i Indicates AUV i The traction provided, F xi ,F yi ,F zi Indicates AUV i Provides the components of traction force on the x, y, and z axes in the direction of the paving path, d safe represents the safe distance between AUVs, and δ represents the buffer zone range between AUVs.

[0028] Furthermore, the step S5 specifically includes:

[0029] B1: Assume there are n AUVs and m target points. Each AUV can only visit one target point, and a target point can only be occupied by one AUV. The mathematical model is:

[0030]

[0031] B2: To facilitate subsequent calculations, first convert each element C of the cost matrixij Normalize The normalized matrix C′ is obtained; at the same time, the difference between the maximum distance and the minimum distance of the cost matrix C′ is used to calculate the weighting factor, and the formula is as follows:

[0032]

[0033] Where, ΔC avg is the average distance in the normalized cost matrix;

[0034] B3: Obtain the normalized cost matrix C′ based on the cost matrix, normalize it to the range of [0,1], and initialize the maximum cost M;

[0035] B4: Set the equilibrium threshold, M = n × max (C′), as the cost upper limit;

[0036] B5: Perform column and column processing on the normalized matrix C′. First, subtract the minimum value of each row from the elements of each row. C′ ij =C′ ij -min(C′ i,: ), then subtract the minimum value of each column from the elements of that column, C′ ij =C′ ij -min(C′ :,j ), and then sort the elements in each row and column;

[0037] B6: Find all zero elements in the reduced cost matrix and try to assign tasks; if there is only one zero in a row, assign the corresponding task to the target location, and mark the row and column where the zero is located, and no longer participate in subsequent assignments; if some rows or columns cannot be uniquely matched (the number of zeros is more than 1), record them for subsequent optimization; if all AUVs and target points are uniquely assigned, the assignment is completed and jump to step B10; if the assignment is not completed, go to the next step for adjustment;

[0038] B7: Calculate the weighted sum PQ of each row and column i and RQ i , sort the elements of the i-th row or i-th column from largest to smallest to get [HC′ 1 .....HC′ n ] or [LC′ 1 .....LC′ n ], the weighted mathematical model is as follows:

[0039]

[0040] After step B5, C′ 1 =0, so the weighted gradients of each row and column are as follows:

[0041]

[0042] B8: Determine whether the matrix C′ obtained by the cost matrix is ​​a square matrix. If so, execute step B9; otherwise, execute step B11;

[0043] B9: For the assigned tasks, their matching relationship has been clarified and marked as locked, and they will no longer participate in subsequent adjustments. For the unassigned tasks, the maximum weighted gradient max(PQ i ) or the row with the maximum weighted gradient max(RQ i ) to perform trial allocation on the column where it is located;

[0044] B10: Delete the assigned row or column. If the end condition is met, then end; otherwise, jump to step B5.

[0045] B11: If the number of rows is greater than the number of columns, execute step B12; if the number of rows is less than the number of columns, execute step B13;

[0046] B12: Take the maximum weighted gradient max(PQ i ) and delete the trial allocated row, then jump to step B5;

[0047] B13: Take the maximum weighted gradient max(RQ i ) to perform trial allocation on the column where the trial allocation is located, delete the column where the trial allocation is located, and jump to step B5;

[0048] B14: Get the allocation result Z, check the maximum cost of the current result. If max(Z)≤M, it means that the current maximum cost meets the requirements and the allocation is completed, and the allocation is ended; otherwise, the cost element exceeding the threshold is assigned to M and redistributed;

[0049] B15: Output allocation matrix Z.

[0050] Furthermore, the establishment of the motion trajectory of the virtual AUV in step S6 includes:

[0051] C1: Virtual matrix coordinate transformation and velocity decomposition:

[0052] Assume the position of the main AUV in the global coordinates is η 0 =[x 0 ,y 0 ,z 0 ] T , the position of the i-th slave AUV in the body coordinate system is The coordinates in the virtual matrix are [m i ,n i ,k i ], the subscript i is the serial number of the underwater robot, and the superscript B represents the body coordinate system; through the rotation matrix R(ψ0 ) The global coordinates of AUV are:

[0053]

[0054] The relationship between the coordinates of the AUV in body coordinates and the virtual particles in the corresponding virtual matrix is ​​described as:

[0055]

[0056] When following the virtual particle, the AUV needs to obtain not only the position information but also the speed information of the formation. Assume that the speed of the host in laying the submarine cable is u r ∈R 3 , through Derived to obtain the velocity component of the ith virtual particle

[0057]

[0058] In the formula, is the velocity component generated during the rotation of the virtual matrix, is the velocity component generated when the virtual matrix structure is enlarged or reduced, R(η)[LHD](m i ,n i ,k i ) . It is the velocity component generated when the formation members change their formation;

[0059] C2: Virtual AUV collision avoidance and formation change control:

[0060] The slave AUV only needs to track the motion trajectory of the virtual AUV. When the virtual AUV and the virtual particle coincide, and the slave AUV coincide with the virtual AUV, it indicates that the formation transformation is completed, the formation enters a stable state, and continues to perform the submarine cable laying task according to the optimized traction force distribution strategy.

[0061] Furthermore, the motion model of the i-th virtual AUV in step S6 is as follows:

[0062]

[0063] Where: q i ∈R 3 ,u i ∈R 3 , is the position and velocity of the ith virtual AUV in the ground coordinate system, τ i For its control input, It is used to achieve collision avoidance between virtual AUVs, and thus avoid collisions between AUVs, expressed as the force exerted by the jth virtual AUV on the ith virtual AUV; It is used to achieve obstacle avoidance, which is expressed as the force of the k-th obstacle on the i-th virtual AUV, c 1 and c 2 are the position error and velocity error feedback coefficients respectively.

[0064] Beneficial effects: Compared with the prior art, the method of the present invention provides a flexible and efficient geometric structure for the AUV formation by designing a three-dimensional virtual matrix framework. Based on this framework, the AUV formation can dynamically adjust the formation according to different operating environments and task requirements, optimize the traction distribution, improve energy utilization efficiency, and ensure stability and task execution efficiency in complex environments. To achieve this goal, the present invention proposes a cost balancing method, combines the virtual structure method, and constructs a suitable cost model to achieve the optimization of traction distribution between AUVs. Through the cost balancing algorithm, the formation can not only adjust the task allocation in a dynamic environment, but also effectively reduce the energy consumption of the system to ensure the continuity and stability of the task. Especially in a complex submarine environment, intelligent allocation through the cost balancing algorithm can more accurately optimize the workload of each AUV and ensure the efficient operation of the entire formation. In addition, the present invention enables the AUV formation to flexibly and quickly adapt to different task scenarios by dynamically adjusting the virtual particle number in the virtual matrix. Whether in the case of task path changes, environmental interference, or other emergency conditions, the formation can quickly complete the formation reconstruction to ensure efficient and accurate execution during the laying of submarine cables. The method of the invention not only enhances the flexibility and adaptability of the formation, but also effectively improves the safety and reliability during mission execution, providing a new solution for the application of AUVs in complex operations. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] Figure 1 is a flow chart of the method of the present invention;

[0066] Figure 2 This is a schematic diagram of laying submarine cables with reverse master-slave structure;

[0067] Figure 3 It is a schematic diagram of a virtual matrix;

[0068] Figure 4 It is a schematic diagram of the formation matrix;

[0069] Figure 5 This is a schematic diagram of the formation change. DETAILED DESCRIPTION

[0070] The present invention is further explained below in conjunction with the accompanying drawings and specific embodiments. It should be understood that these embodiments are only used to illustrate the present invention and are not used to limit the scope of the present invention. After reading the present invention, various equivalent forms of modifications to the present invention by those skilled in the art all fall within the scope defined by the claims attached to this application.

[0071] like Figure 1 and Figure 2 As shown, the present invention provides an underwater robot formation control method based on virtual structure and cost balance method, comprising the following steps:

[0072] S1: Design a multi-form formation library and import it into the main AUV;

[0073] The preset standard formations, the multi-form formation library includes vertical traction formation, horizontal traction formation, triangle traction formation and prism traction formation, as shown below: Figure 4 For example, when the main engine needs to be towed on a flat seabed, the formation (a) can be selected, when the main engine needs to be towed to turn, the formation (b) can be selected, and when the main engine needs to be raised and moved forward at the same time or in a specific task, when the traction force needs to be evenly distributed, the formation (c) can be selected. When there are a large number of AUVs, the formation (d) can be selected to enhance the stability of the formation and the efficiency of task execution.

[0074] When you need to switch formations, you only need to select the target formation from the formation library and update the numbers of the virtual particles tracked by each slave AUV. Figure 5 As shown, the formation is changed from vertical traction to triangular traction.

[0075] S2: Establish a body coordinate system with the master AUV as the center, generate a three-dimensional virtual matrix, and introduce a virtual AUV between the slave AUV and its corresponding virtual particle;

[0076] like Figure 3 As shown, the construction of the three-dimensional virtual matrix includes:

[0077] A1: Determine the central reference point and establish a body coordinate system with the main AUV as the center. The coordinate system is located at the center of the main AUV and moves with the main AUV.

[0078] A2: In the body coordinate system, a three-dimensional matrix network m×n×p is constructed with the main AUV as the center. The grid spacing is defined by the row spacing L, column spacing H, and layer spacing D. The formation morphology is dynamically scaled by adjusting the parameters of L, H, and D to adapt to different mission requirements.

[0079] S3: According to the mission requirements, select the corresponding formation from the multi-form formation library of the main AUV;

[0080] S4: Based on the selected formation, the cost of each member to the target point is calculated through the cost model according to the status of the current formation members to obtain the cost matrix;

[0081] In order to achieve the desired formation, the slave AUV tracks the movement of virtual particles in the virtual matrix to tow the master AUV for submarine cable laying. However, during the submarine cable laying process, the formation needs to adjust the formation according to different working conditions to adapt to the working environment and task requirements. In the process of formation reconstruction, if only a simple allocation is made according to the target position, it will easily lead to the AUV's movement path being too long or the formation structure changing dramatically, thereby increasing energy consumption and collision avoidance risks. Therefore, in the allocation process, a reasonable cost model needs to be constructed to ensure the optimality of the formation adjustment.

[0082] The cost model includes:

[0083] Assume that there are n members in the formation {U 1 ,U 2 ,...,U n}, the objective function set is {P 1 ,P 2 ,...,P n}; Member U i Assign to position P k The cost matrix C ik Defined as:

[0084]

[0085] Where, d ik represents the Euclidean distance from the current position i to the target position k of the member, which is used to measure the path; θ ik Indicates the angular deviation relative to the original formation during the adjustment process; p ij Indicates the current AUV i and other AUVs j The relative position between them is expressed as the collision avoidance penalty term; f(·), g(·), h(·) are weight functions, which measure the weights of distance cost and direction cost in the total cost respectively, and can be dynamically adjusted according to actual task requirements. The expression of weight function f(·), g(·), h(·) is:

[0086]

[0087] g(θ ik )=λ 2 Δθ ik A

[0088] Δθ ik =|θ final,k -θ inital,i |=|(α k -αi )+(β k -β i )+(γ k -γ i )|

[0089]

[0090] In the formula, F i Indicates AUV i The traction provided, F xi ,F yi ,F zi Indicates AUV i Provides the components of traction force on the x, y, and z axes in the direction of the paving path, d safe represents the safe distance between AUVs, and - represents the buffer zone range between AUVs.

[0091] S5: According to the cost matrix, the task is assigned to the target point through the cost balance method;

[0092] Specifically include:

[0093] B1: Assume there are n AUVs and m target points. Each AUV can only visit one target point, and a target point can only be occupied by one AUV. The mathematical model is:

[0094]

[0095] B2: To facilitate subsequent calculations, first convert each element C of the cost matrix ij Normalize The normalized matrix C′ is obtained; at the same time, the difference between the maximum distance and the minimum distance of the cost matrix C′ is used to calculate the weighted factor, and the formula is as follows:

[0096]

[0097] Where, ΔC avg is the average distance in the normalized cost matrix;

[0098] B3: Obtain the normalized cost matrix C′ based on the cost matrix, normalize it to the range of [0,1], and initialize the maximum cost M;

[0099] B4: Set the equilibrium threshold, M = n × max (C′), as the cost upper limit;

[0100] B5: Perform column and column processing on the normalized matrix C′. First, subtract the minimum value of each row from the elements of each row. C′ ij =C′ ij -min(C′ i,:), then subtract the minimum value of each column from the elements of that column, C′ ij =C′ ij -min(C′ :,j ), and then sort the elements in each row and column;

[0101] B6: Find all zero elements in the reduced cost matrix and try to assign tasks; if there is only one zero in a row, assign the corresponding task to the target location, and mark the row and column where the zero is located, and no longer participate in subsequent assignments; if some rows or columns cannot be uniquely matched (the number of zeros is more than 1), record them for subsequent optimization; if all AUVs and target points are uniquely assigned, the assignment is completed and jump to step B10; if the assignment is not completed, go to the next step for adjustment;

[0102] B7: Calculate the weighted sum PQ of each row and column i and RQ i , sort the elements of the i-th row or i-th column from largest to smallest to get [HC′ 1 .....HC′ n ] or [LC′ 1 .....LC′ n ], the weighted mathematical model is as follows:

[0103]

[0104] After step B5, C′ 1 =0, so the weighted gradients of each row and column are as follows:

[0105]

[0106] B8: Determine whether the matrix C′ obtained by the cost matrix is ​​a square matrix. If so, execute step B9; otherwise, execute step B11;

[0107] B9: For the assigned tasks, their matching relationship has been clarified and marked as locked, and they will no longer participate in subsequent adjustments. For the unassigned tasks, the maximum weighted gradient max(PQ i ) or the row with the maximum weighted gradient max(RQ i ) to perform trial allocation on the column where it is located;

[0108] B10: Delete the assigned row or column. If the end condition is met, then end; otherwise, jump to step B5.

[0109] B11: If the number of rows is greater than the number of columns, execute step B12; if the number of rows is less than the number of columns, execute step B13;

[0110] B12: Take the maximum weighted gradient max(PQ i) and delete the trial allocated row, then jump to step B5;

[0111] B13: Take the maximum weighted gradient max(RQ i ) to perform trial allocation on the column where the trial allocation is located, delete the column where the trial allocation is located, and jump to step B5;

[0112] B14: Get the allocation result Z, check the maximum cost of the current result. If max(Z)≤M, it means that the current maximum cost meets the requirements and the allocation is completed, and the allocation is ended; otherwise, the cost element exceeding the threshold is assigned to M and redistributed;

[0113] B15: Output allocation matrix Z.

[0114] S6: According to the task allocation plan, the slave AUV realizes formation reconstruction by tracking the motion trajectory of the virtual AUV;

[0115] The establishment of the motion trajectory of the virtual AUV includes:

[0116] C1: Virtual matrix coordinate transformation and velocity decomposition:

[0117] Assume the position of the main AUV in the global coordinates is η 0 =[x 0 ,y 0 ,z 0 ] T , the position of the i-th slave AUV in the body coordinate system is The coordinates in the virtual matrix are [m i ,n i ,k i ], the subscript i is the serial number of the underwater robot, and the superscript B represents the body coordinate system; through the rotation matrix R(ψ 0 ) The global coordinates of AUV are:

[0118]

[0119] The relationship between the coordinates of the AUV in body coordinates and the virtual particles in the corresponding virtual matrix is ​​described as:

[0120]

[0121] When following the virtual particle, the AUV needs to obtain not only the position information but also the speed information of the formation. Assume that the speed of the host in laying the submarine cable is u r ∈R 3 , through Derived to obtain the velocity component of the ith virtual particle

[0122]

[0123] In the formula, is the velocity component generated during the rotation of the virtual matrix, is the velocity component generated when the virtual matrix structure is enlarged or reduced, R(η)[LHD](m i ,n i ,k i ) · It is the velocity component generated when the formation members change their formation;

[0124] C2: Virtual AUV collision avoidance and formation change control:

[0125] During the cable laying process, the position and formation structure of the AUVs are not fixed, but need to be adjusted according to different scenarios. The AUVs need to change from the current formation structure to a new formation layout. In this process, the relative positions of the AUVs may change significantly, increasing the risk of collision and affecting the stability and safety of the operation.

[0126] Therefore, a virtual AUV is introduced between the slave AUV and its corresponding virtual particle. The virtual AUV is mainly responsible for collision avoidance and continuously moves toward the virtual target particle. The slave AUV only needs to track the motion trajectory of the virtual AUV. When the virtual AUV coincides with the virtual particle and the slave AUV coincides with the virtual AUV, it indicates that the formation transformation is completed, the formation enters a stable state, and continues to perform the submarine cable laying task according to the optimized traction distribution strategy.

[0127] The motion model of the i-th virtual AUV is as follows:

[0128]

[0129] Where: q i ∈R 3 ,u i ∈R 3 , is the position and velocity of the ith virtual AUV in the ground coordinate system, τ i For its control input, It is used to achieve collision avoidance between virtual AUVs, and thus avoid collisions between AUVs, expressed as the force exerted by the jth virtual AUV on the ith virtual AUV; It is used to achieve obstacle avoidance, which is expressed as the force of the k-th obstacle on the i-th virtual AUV, c 1 and c 2 are the position error and velocity error feedback coefficients respectively.

Claims

1. A method for controlling underwater robot formation based on virtual structure and cost balance method, characterized in that: The steps include: S1: Design a multi-form formation library and import it into the main AUV; S2: Establish a body coordinate system with the master AUV as the center, generate a three-dimensional virtual matrix, and introduce a virtual AUV between the slave AUV and its corresponding virtual particle; S3: According to the mission requirements, select the corresponding formation from the multi-form formation library of the main AUV; S4: Based on the selected formation, the cost of each member to the target point is calculated through the cost model according to the status of the current formation members to obtain the cost matrix; S5: According to the cost matrix, the task is assigned to the target point through the cost balance method; S6: According to the task allocation plan, the slave AUVs realize formation reconstruction by tracking the motion trajectory of the virtual AUV.

2. The underwater robot formation control method based on virtual structure and cost balance method according to claim 1 is characterized in that: The multi-form formation library in step S1 includes a vertical traction formation, a horizontal traction formation, a triangular traction formation and a prism traction formation.

3. The underwater robot formation control method based on virtual structure and cost balance method according to claim 1 is characterized in that: The construction of the three-dimensional virtual matrix in step S2 includes: A1: Determine the central reference point and establish a body coordinate system with the main AUV as the center. The coordinate system is located at the center of the main AUV and moves with the main AUV. A2: In the body coordinate system, a three-dimensional matrix network m×n×p is constructed with the main AUV as the center. The grid spacing is defined by the row spacing L, column spacing H, and layer spacing D. The formation morphology is dynamically scaled by adjusting the parameters of L, H, and D to adapt to different mission requirements.

4. The underwater robot formation control method based on virtual structure and cost balance method according to claim 1 is characterized in that: The cost model in step S4 includes: Assume that there are n members in the formation {U1,U2,...,U n }, the objective function set is {P1,P2,...,P n }; Member U i Assign to position P k The cost matrix C ik Defined as: Where, d ik represents the Euclidean distance from the current position i to the target position k of the member, which is used to measure the path; θ ik Indicates the angular deviation relative to the original formation during the adjustment process; p ij Indicates the current AUV i and other AUVs j The relative position between them is expressed as the collision avoidance penalty term; f(·), g(·), h(·) are weight functions, which respectively measure the weights of distance cost and direction cost in the total cost.

5. The underwater robot formation control method based on virtual structure and cost balance method according to claim 4 is characterized in that: In the cost model of step S4, the expressions of weight functions f(·), g(·), h(·) are: g(θ ik )=λ2Δθ ik A Dth ik =|θ final,k -θ inital,i |=×|(a k -a i )+(β k -b i )+(c k -c i )| In the formula, F i Indicates AUV i The traction provided, F xi ,F yi ,F zi Indicates AUV i Provides the components of traction force on the x, y, and z axes in the direction of the paving path, d safe represents the safe distance between AUVs, and δ represents the buffer zone range between AUVs.

6. The underwater robot formation control method based on virtual structure and cost balance method according to claim 5 is characterized in that: The step S5 specifically includes: B1: Assume there are n AUVs and m target points. Each AUV can only visit one target point, and a target point can only be occupied by one AUV. The mathematical model is: B2: Each element C of the cost matrix ij Normalize The normalized matrix C′ is obtained; at the same time, the difference between the maximum distance and the minimum distance of the cost matrix C′ is used to calculate the weighted factor, and the formula is as follows: Where, ΔC avg is the average distance in the normalized cost matrix; B3: Obtain the normalized cost matrix C′ based on the cost matrix, normalize it to the range of [0,1], and initialize the maximum cost M; B4: Set the equilibrium threshold, M = n × max (C′), as the cost upper limit; B5: Perform column and column processing on the normalized matrix C′. First, subtract the minimum value of each row from the elements of each row. C′ ij =C′ ij -min(C′ i,: ), then subtract the minimum value of each column from the elements of that column, C′ ij =C′ ij -min(C′ :,j ), and then sort the elements in each row and column; B6: Find all zero elements in the reduced cost matrix and try to assign tasks; if there is only one zero in a row, assign the corresponding task to the target location, and mark the row and column where the zero is located, and no longer participate in subsequent assignments; if some rows or columns cannot be uniquely matched, record them for subsequent optimization; if all AUVs and target points are uniquely assigned, the assignment is completed and jump to step B10; if the assignment is not completed, go to the next step for adjustment; B7: Calculate the weighted sum PQ of each row and column i and RQ i , sort the elements of the i-th row or i-th column from large to small to get [HC′1.....HC′ n ] or [LC′1.....LC′ n ], the weighted mathematical model is as follows: After step B5, C′1=0, so the weighted gradients of each row and column are as follows: B8: Determine whether the matrix C′ obtained by the cost matrix is ​​a square matrix. If so, execute step B9; otherwise, execute step B11; B9: For the assigned tasks, their matching relationship has been clarified and marked as locked, and they will no longer participate in subsequent adjustments. For the unassigned tasks, the maximum weighted gradient max(PQ i ) or the row with the maximum weighted gradient max(RQ i ) to perform trial allocation on the column where it is located; B10: Delete the assigned row or column. If the end condition is met, then end; otherwise, jump to step B5. B11: If the number of rows is greater than the number of columns, execute step B12; if the number of rows is less than the number of columns, execute step B13; B12: Take the maximum weighted gradient max(PQ i ) and delete the trial allocated row, then jump to step B5; B13: Take the maximum weighted gradient max(RQ i ) to perform trial allocation on the column where the trial allocation is located, delete the column where the trial allocation is located, and jump to step B5; B14: Get the allocation result Z, check the maximum cost of the current result. If max(Z)≤M, it means that the current maximum cost meets the requirements and the allocation is completed, and the allocation is ended; otherwise, the cost element exceeding the threshold is assigned to M and redistributed; B15: Output allocation matrix Z.

7. The underwater robot formation control method based on virtual structure and cost balance method according to claim 6 is characterized in that: The establishment of the motion trajectory of the virtual AUV in step S6 includes: C1: Virtual matrix coordinate transformation and velocity decomposition: Assume the position of the main AUV in global coordinates is η0 = [x0, y0, z0] T , the position of the i-th slave AUV in the body coordinate system is The coordinates in the virtual matrix are [m i ,n i ,k i ], the subscript i is the number of the slave underwater robot, and the superscript B represents the body coordinate system; the global coordinates of the slave AUV are obtained by the rotation matrix R(ψ0): The relationship between the coordinates of the AUV in body coordinates and the virtual particles in the corresponding virtual matrix is ​​described as: Assume that the speed of the host laying the submarine cable is u r ∈R 3 , through Derived to obtain the velocity component of the ith virtual particle In the formula, is the velocity component generated during the rotation of the virtual matrix, is the velocity component generated when the virtual matrix structure is enlarged or reduced, R(η)[LHD](m i ,n i ,k i ) is the velocity component generated when the formation members change their formation; C2: Virtual AUV collision avoidance and formation change control: When the virtual AUV and the virtual particle coincide, and the slave AUV coincide with the virtual AUV, it indicates that the formation transformation is completed, the formation enters a stable state, and continues to perform the submarine cable laying task according to the optimized traction force distribution strategy.

8. The underwater robot formation control method based on virtual structure and cost balance method according to claim 7 is characterized in that: The motion model of the i-th virtual AUV in step S6 is as follows: Where: q i ∈R 3 ,u i ∈R 3 , is the position and velocity of the ith virtual AUV in the ground coordinate system, τ i For its control input, It is used to achieve collision avoidance between virtual AUVs, and thus avoid collisions between AUVs, expressed as the force exerted by the jth virtual AUV on the ith virtual AUV; It is used to achieve obstacle avoidance and is expressed as the force of the k-th obstacle detected on the i-th virtual AUV. c1 and c2 are the position error and velocity error feedback coefficients, respectively.

Images