CN111930116B - Large-scale UUV cluster formation method based on grid method - Google Patents

Abstract

The invention belongs to the technical field of UUV formation control, and in particular relates to a large-scale UUV cluster formation formation method based on a grid method. The invention divides the UUV cluster formation formation space into the row area and the column area of the grid space, which can effectively avoid the collision between UUVs in the formation formation process. The invention uses the particle swarm optimization algorithm to uniformly plan the general route of the four maneuvering processes of column dispersion, row dispersion, column maneuvering and row maneuvering, which can realize the orderly maneuvering coordination in the formation process of the UUV cluster formation, which is beneficial to the realization of large-scale Formation formation of UUV clusters. In the present invention, the amount of information interaction between the UUV clusters is small, the calculation is simple, the planning speed is fast, the coordination and maneuvering logic is clear, and the engineering implementation is easy.

Description

A grid-based method for large-scale UUV cluster formation formation

technical field

The invention belongs to the technical field of UUV formation control, and in particular relates to a large-scale UUV cluster formation formation method based on a grid method.

Background technique

UUV is of great significance in the development of marine resources, the development of the national economy and military applications. With the development of UUV technology, it is more and more inclined to use UUV clusters to work together to greatly improve the operation capacity and efficiency. In the process of performing cooperative detection, target search and other operations, the UUV cluster generally adopts the method of navigating in a certain desired geometric formation. The first problem to be solved in formation sailing is how to realize the initial random distribution and disordered geometric formation of UUV clusters to form the desired geometric formation. This process is the formation of UUV clusters.

Formation formation belongs to the field of UUV formation control. Common formation control methods such as artificial potential field method, pilot following method, virtual structure method, etc. are more suitable for solving formation maintenance after formation formation, but they are usually applied to formation formation. There will be problems such as a large amount of communication and information interaction, complex planning and coordination logic, and the risk of collision between UUVs. In particular, the UUV cluster adopts underwater acoustic communication with small communication bandwidth and large communication delay, and it is more difficult when the scale of the UUV cluster is large. Therefore, it is very necessary to develop a formation formation method that can be applied to large-scale UUV clusters.

The patent document with the application number of 201910917112.6 discloses "a method for forming a UUV cluster formation based on circular hierarchical planning". It mainly solves the formation method of small-scale UUV cluster formation for orderly maneuvering of communication delay. First of all, this patent adopts the space area division method of sector and circle, which is different from the grid space area division method in which the row area and the column area intersect in the present invention. Secondly, the patent mainly proposes a formation method of small-scale UUV clusters with step-by-step planning, step-by-step maneuvering, planning and maneuvering intersecting, and the present invention focuses on solving the large-scale UUV cluster team with unified planning of the overall path and unified maneuvering. Forms are formed in different ways.

SUMMARY OF THE INVENTION

The purpose of the present invention is to provide a large-scale UUV cluster formation formation method based on the grid method that can be applied to the formation formation of large-scale UUV clusters.

The object of the present invention is achieved through the following technical solutions: comprise the following steps:

Step 1: Select the master UUV from the UUV cluster, and the rest of the UUVs are slave UUVs;

Step 2: Set the width of the row and column area L _res and the desired formation command

x _{exp_E} (n) is the x coordinate of the nth expected point in the fixed coordinate system, y _{exp_E} (n) is the y coordinate of the nth expected point in the fixed coordinate system, n=1,2,·· ·,N _{num_UUV_F} ; N _{num_UUV_F} is the number of slave UUVs;

Step 3: The formation begins, all UUVs maintain fixed point and heading, and determine their master-slave identity;

i表示各从UUV代号，i＝1,2,···,N_{num_UUV_F}；主UUV向所有从UUV发送自身当前位置信息

和艏向信息θ_{H_L}；Step 4: The UUV cluster conducts information exchange, and all slave UUVs send their current location information to the master UUV

i represents the code of each slave UUV, i=1,2,...,N _{num_UUV_F} ; the master UUV sends its current position information to all slave UUVs

and heading information θ _{H_L} ;

Step 5: Use the position of the main UUV as the origin to establish the hull Cartesian coordinate system, divide the space of the hull Cartesian coordinate system into a space grid composed of row area and column area superimposition by a straight line with an interval of L _res , and set the row area and column area. The maximum and minimum values of the column area;

The coordinate P _{f_pos_B} (i) of the ith slave UUV in the main UUV hull coordinate system is:

x_{f_pos_B}(i)为从UUV在主UUV船体坐标系下的x轴坐标，y_{f_pos_B}(i)为从UUV在主UUV船体坐标系下的y轴坐标；

x _{f_pos_B} (i) is the x-axis coordinate of the slave UUV in the main UUV hull coordinate system, y _{f_pos_B} (i) is the y-axis coordinate of the slave UUV in the main UUV hull coordinate system;

The row area row _f (i) and the column area rank _f (i) of the i-th from the row area where the UUV's current position is located is:

Step 6: The master UUV judges whether the position information of all the slave UUVs has been received; if the master UUV receives the current position information of all the slave UUVs within the specified time, step 7 is performed; Send the formation formation failure command, and return to step 4 after all UUVs stop;

Step 7: The master UUV plans the maneuvering target points of the four stages of the UUV column dispersion, row dispersion, column maneuvering, and row maneuvering;

Step 7.1: The main UUV solves the matrix X of all the row area coordinates scattered from the UUV row, the column area coordinates scattered by the column and the desired point number to be reached, and filters out all the secondary UUVs with the shortest total movement distance. matrix X;

x_{M_PSO}(i)为第i号从UUV行分散的行区域坐标，y_{M_PSO}(i)为第i号从UUV列分散的列区域坐标，P_{ID_PSO}(i)为第i号从UUV分配的从期望点序号；Matrix X=(x(1),x(2),...,x(i)),

x _{M_PSO} (i) is the row area coordinate of the ith scattered from the UUV row, y _{M_PSO} (i) is the column area coordinate of the ith scattered from the UUV column, _{PID_PSO} (i) is the ith allocated from the UUV expected point number;

The conditions that matrix X satisfies are:

1) When i≠j,

2) When row _f (i)=row _f (j) and i≠j, (y _{M_PSO} (i)-y _{M_PSO} (j))(rank _f (i)-rank _f (j))>0;

3) When y _{exp_E} (P _{ID_PSO} (i))=y _{exp_E} (P _{ID_PSO} (j)),

(x _{M_PSO} (i)-x _{M_PSO} (j))(y _{exp_E} (P _{ID_PSO} (i))-y _{exp_E} (P _{ID_PSO} (j)))>0;

4) |x _{M_PSO} (i)|>1, |y _{M_PSO} (i)|>1;

5) When |y _{exp_M} (P _{ID_PSO} (i))|≤1, (x _{M_PSO} (i))(x _{exp_M} (P _{ID_PSO} (i)))>0;

6)|x _{M_PSO} (i)-<x _{M_PSO} (i)>|+|y _{M_PSO} (i)-<y _{M_PSO} (i)>|+|P _{ID_PSO} (i)-<P _{ID_PSO} (i)>|= 0, where <> represents the rounding operation;

7) When |row _f (i)|≤1, (rank _f (i))(y _{M_PSO} (i))>0;

Step 7.2: Calculate the coordinates of the maneuvering target points in the fixed coordinate system from the four stages of UUV column dispersion, row dispersion, column maneuvering, and row maneuvering;

The coordinates of the column scatter points in the fixed coordinate system

The coordinates of the row scatter points in the fixed coordinate system and

The coordinates of the maneuvering target point in the fixed coordinate system

The coordinates of the mobile target point in the fixed coordinate system

Step 8: The master UUV sends the maneuvering target point information of each stage to all the slave UUVs. After all the slave UUVs have received the information of the four maneuvering target points, step 9 is performed;

Step 9: The master UUV sends a column dispersion command to all the slave UUVs, all the slave UUVs move from the current position to the column dispersion target point, and each slave UUV feeds back information to the master UUV after completing the column dispersion;

Step 10: The master UUV judges whether all the slave UUVs have completed the column dispersion; if the master UUV receives the feedback information that all the slave UUVs have completed the column dispersion within the specified time, step 11 is executed; Send formation failure command from UUV, and return to step 4 after all UUVs stop;

Step 11: The master UUV sends a line dispersion command to all the slave UUVs, all the slave UUVs move from the current position to the line dispersion target point, and each slave UUV feeds back information to the master UUV after completing the line dispersion;

Step 12: The master UUV judges whether all the slave UUVs have completed row dispersion; if the master UUV receives the feedback information that all the slave UUVs have completed row dispersion within the specified time, step 13 is executed; Send formation failure command from UUV, and return to step 4 after all UUVs stop;

Step 13: The master UUV sends a column maneuver command to all the slave UUVs, all the slave UUVs move from the current position to the target point of the column maneuver, and each slave UUV feeds back information to the master UUV after completing the column maneuver;

Step 14: The master UUV judges whether all the slave UUVs have completed the row maneuver; if the master UUV receives the feedback information that all the slave UUVs have completed the row maneuver within the specified time, step 15 is executed; Send formation failure command from UUV, and return to step 4 after all UUVs stop;

Step 15: The master UUV sends a maneuvering instruction to all the slave UUVs, all the slave UUVs move from the current position to the target point of the maneuver, and each slave UUV feeds back information to the master UUV after completing the maneuver;

Step 16: The master UUV judges whether all the slave UUVs have completed the maneuver; if the master UUV receives the feedback information that all the slave UUVs have completed the maneuver within the specified time, step 17 is executed; Send formation failure command from UUV, and return to step 4 after all UUVs stop;

Step 17: The master UUV sends a formation formation success command to all the slave UUVs, and all UUVs maintain the fixed point and heading to complete the UUV cluster formation.

The beneficial effects of the present invention are:

The invention divides the UUV cluster formation formation space into the row area and the column area of the grid space, which can effectively avoid the collision between UUVs in the formation formation process. The invention uses the particle swarm optimization algorithm to uniformly plan the general route of the four maneuvering processes of column dispersion, row dispersion, column maneuvering and row maneuvering, which can realize the orderly maneuvering coordination in the formation process of the UUV cluster formation, which is beneficial to the realization of large-scale Formation formation of UUV clusters. In the present invention, the amount of information interaction between the UUV clusters is small, the calculation is simple, the planning speed is fast, the coordination and maneuvering logic is clear, and the engineering implementation is easy.

Description of drawings

Figure 1 is a schematic diagram of the formation of a UUV cluster formation.

Figure 2 is a schematic diagram of the space division of the expected formation of the UUV cluster formation and the formation of the formation.

Fig. 3 is a flow chart of a large-scale UUV cluster formation formation method based on the grid method.

FIG. 4 is a schematic diagram of the principle of UUV cluster formation and queue dispersion.

Figure 5 is a schematic diagram of the principle of UUV cluster formation line dispersion.

FIG. 6 is a schematic diagram of the maneuvering principle of UUV cluster formation.

Figure 7 is a schematic diagram of the UUV swarm formation maneuvering principle.

Detailed ways

The present invention will be further described below with reference to the accompanying drawings.

The purpose of the present invention is to provide a method for forming a formation based on a grid method, which is suitable for large-scale UUV clusters. The present invention enables large-scale UUV clusters to quickly and safely form a desired formation from an initial random distribution and a disordered form.

A large-scale UUV cluster formation formation method based on grid method, comprising the following steps:

Step 1: Initialize, set the UUV master-slave identity command, set the formation formation parameters, and expect the formation command;

Set the formation parameters N _{num_UUV_F} and L _res , where N _{num_UUV_F} is the number of slave UUVs, and L _res is the width of the row and column area.

其中x_{exp_E}(n)表示每个从期望点固定坐标系下x坐标，y_{exp_E}(n)表示每个从期望点固定坐标系下y坐标，n为各从期望点的序号，n＝1,2,···,N_{num_UUV_F}。Set the desired formation formation command

Where x _{exp_E} (n) represents the x coordinate of each slave expected point fixed coordinate system, y _{exp_E} (n) represents the y coordinate of each slave expected point fixed coordinate system, n is the serial number of each slave expected point, n=1, 2,...,N _{num_UUV_F} .

Step 2: The formation begins, all UUVs maintain fixed point and heading, and determine their master-slave identity;

i表示各从UUV代号，i＝1,2,···,N_{num_UUV_F}。主UUV向所有从UUV发送当前位置信息

和艏向信息θ_{H_L}；Step 3: The UUV cluster conducts information exchange. All slave UUVs send their current location information to the master UUV

i represents each slave UUV code, i=1, 2, ···, N _{num_UUV_F} . The master UUV sends current location information to all slave UUVs

and heading information θ _{H_L} ;

Step 4: The master UUV judges whether all slave UUV location information is received. If the master UUV judges that it has received all the current position information of the slave UUVs within 120 seconds, go to step 5, otherwise it is considered that the formation has failed, and go to step (21);

表示第i个从UUV所解得的位置信息，x_{M_PSO}(i)为i号从UUV行分散的行区域坐标，y_{M_PSO}(i)为i号从UUV列分散的列区域坐标，P_{ID_PSO}(i)为i号UUV分配的从期望点序号，i＝1,2,...,N_{num_UUV_F}，进而计算出各从UUV四个阶段的机动目标点，列分散目标点坐标

行分散目标点坐标

列机动目标点坐标

和行机动目标点坐标

Step 5: The main UUV uses the particle swarm optimization algorithm to plan the maneuvering target points of each of the four stages of column dispersion, row dispersion, column maneuvering, and row maneuvering of the UUVs. The main UUV performs unified planning for the four stages according to the particle swarm optimization algorithm, and solves the matrix X= (x(1),x(2),…,x(i)),

Represents the i-th position information obtained from the UUV solution, x _{M_PSO} (i) is the row area coordinate scattered from the UUV row of the i number, y _{M_PSO} (i) is the column area coordinate of the i number scattered from the UUV column, P _{ID_PSO} ( i) The sequence number of the expected points assigned to the UUV of No. i, i=1,2,...,N _{num_UUV_F} , and then calculate the maneuvering target points of each of the four stages of the UUV, and list the coordinates of the scattered target points

Line scatter target point coordinates

Column maneuver target point coordinates

and the coordinates of the moving target point

In step 5, the master UUV uses the particle swarm optimization algorithm to plan the maneuvering target points of each of the four stages of the slave UUV, including:

Step 5 (A): Solve the row area row _f (i) and the column area rank _f (i) where the current position of the UUV is located;

Where x _{f_pos_B} (i) is the x-axis coordinate of the slave UUV in the main UUV hull coordinate system, and y _{f_pos_B} (i) is the y-axis coordinate of the slave UUV in the main UUV hull coordinate system.

Step 5 (B): Apply the particle swarm algorithm to solve the matrix X=(x(1), x(2),...,x(i));

The obtained particle swarm matrix should satisfy the following conditions:

(1) After the end of the dispersion process, all the slave UUVs in different row areas and different column areas have different expected points allocated, and the following formula is established:

where i≠j, i=1,2,...,N _{num_UUV_F} , j=1,2,...,N _{num_UUV_F} .

(2) In order to avoid the collision between the slave UUVs in the dispersion process, the relative position of the slave UUVs in the same row area does not change, and the following conditions should be met:

If row _f (i)=row _f (j), i≠j, the following formula holds:

(y _{M_PSO} (i)-y _{M_PSO} (j))(rank _f (i)-rank _f (j))>0.

(3) After the column maneuvering process is completed, there may be multiple slave UUVs in the same column area at the same time. In order to avoid collision between the slave UUVs during the row maneuvering process, during the row maneuvering process, the relative positions of the slave UUVs in the same column area are different. Changes should meet the following conditions:

If y _{exp_E} (P _{ID_PSO} (i))=y _{exp_E} (P _{ID_PSO} (j)), the following formula holds:

(x _{M_PSO} (i)-x _{M_PSO} (j))(y _{exp_E} (P _{ID_PSO} (i))-y _{exp_E} (P _{ID_PSO} (j)))>0.

(4) In order for each slave UUV not to collide with the master UUV during the maneuvering process, the absolute value of the coordinates of the row area and column area of the dispersion process is greater than 1, and the following conditions should be met:

|x _{M_PSO} (i)|>1, |y _{M_PSO} (i)|>1, i=1,2,...,N _{num_UUV_F}

(5) In order to avoid the collision between the slave UUV and the master UUV during the maneuver, the following conditions should be met:

If |y _{exp_M} (P _{ID_PSO} (i))|≤1, the following formula holds:

(x _{M_PSO} (i))(x _{exp_M} (P _{ID_PSO} (i)))>0, i=1, 2, . . . , N _{num_UUV_F} .

(6) The value of the position of each slave UUV is planned to be an integer, and the following conditions should be met:

|x _{M_PSO} (i)-<x _{M_PSO} (i)>|+|y _{M_PSO} (i)-<y _{M_PSO} (i)>|+|P _{ID_PSO} (i)-<P _{ID_PSO} (i)>|=0, i=1,2,...,N _{num_UUV_F} , where <> indicates that the logarithm is rounded.

(7) In order to avoid the collision between the slave UUV and the master UUV during the column dispersion process, the following conditions should be met:

If |row _f (i)|≤1, the following formula holds:

(rank _f (i))(y _{M_PSO} (i))>0, i=1, 2, . . . , N _{num_UUV_F} .

和行分散目标点的行列区域坐标

并计算各从UUV列分散点和行分散点在固定坐标系下的坐标

和

Step 5 (C): Solve the coordinates of the row and column area of each target point scattered from the UUV column

and the row-column area coordinates of the row scatter target point

And calculate the coordinates of each from the UUV column scatter point and row scatter point in a fixed coordinate system

and

其中x_{d_rank_M}(i)为列分散目标点的行区域坐标，y_{d_rank_M}(i)为列分散目标点的列区域坐标，利用如下公式：In the column dispersion movement, only the column area where the UUV is located is changed, and the coordinates of the row area remain unchanged, and the row and column area coordinates of the column dispersion target point are solved.

Where x _{d_rank_M} (i) is the row area coordinate of the column scatter target point, y _{d_rank_M} (i) is the column area coordinate of the column scatter target point, using the following formula:

其中x_{d_row_M}(i)为行分散目标点的行区域坐标，y_{d_row_M}(i)为行分散目标点的列区域坐标，利用如下公式：In the row dispersion movement, only the row area where the UUV is located is changed, and the coordinates of the column area remain unchanged, and the row and column area coordinates of the target point of the row dispersion are solved.

Where x _{d_row_M} (i) is the row area coordinate of the row dispersion target point, y _{d_row_M} (i) is the column area coordinate of the row dispersion target point, using the following formula:

利用如下公式：Calculate the coordinates of the scattered points from the UUV column in a fixed coordinate system

Use the following formula:

利用如下公式：Calculate the coordinates of the scattered points from the UUV row in a fixed coordinate system

Use the following formula:

和从UUV行分散点在固定坐标系下的坐标

求解出列机动目标点在固定坐标系下的坐标

和行机动目标点在固定坐标系下的坐标

Step 5 (D): From the desired point row and column area coordinates

and scatter the coordinates of the points in the fixed coordinate system from the UUV row

Solve the coordinates of the dequeuing maneuver target point in a fixed coordinate system

and the coordinates of the mobile target point in the fixed coordinate system

Step 6: The master UUV sends the maneuvering target point information of each stage to all the slave UUVs;

Step 7: The master UUV judges whether all the slave UUVs have received the information of the four maneuvering target points. If the master UUV judges that all the slave UUVs have received the coordinates of the column scatter point, the row scatter point, the column maneuver point coordinate and the row maneuver point coordinate within 120 seconds, go to step 8, otherwise it is considered that some of the slave UUVs have not received the four maneuvering targets Click information, go to step 6;

Step 8: The master UUV sends a column scatter command to all slave UUVs;

Step 9: Column dispersion from UUV. All slave UUVs move from the current position to the nematic dispersion target point, and each slave UUV informs the master UUV after completing the column dispersion;

Step 10: The master UUV judges whether all the slave UUVs have completed column dispersion. If the master UUV judges that all the slave UUVs have completed the column dispersion within 120 seconds, go to step 11, otherwise it is considered that the formation has failed, and go to step (21);

Step 11: The master UUV sends line scatter instructions to all slave UUVs;

Step twelve: Row dispersion from UUV. All slave UUVs move from the current position to the line dispersion target point, and each slave UUV informs the master UUV after the line dispersion is completed;

Step 13: The master UUV judges whether all the slave UUVs have completed row dispersion. If the master UUV judges that all the slave UUVs have completed line dispersion within 120 seconds, go to step 14; otherwise, it is considered that the formation has failed, and go to step (21).

Step 14: The master UUV sends a column maneuver command to all slave UUVs;

Step fifteen: Column maneuver from UUV. All slave UUVs move from the current position to the target point of nematic maneuver, and each slave UUV informs the master UUV after completing the maneuver;

Step 16: The master UUV judges whether all the slave UUVs have completed the column maneuver. If the master UUV judges that all the slave UUVs have completed the column maneuver within 120 seconds, go to step 17; otherwise, it is considered that the formation has failed, and go to step (21).

Step 17: The master UUV sends a maneuvering command to all the slave UUVs;

Step 18: Do the maneuvering from the UUV. All the slave UUVs move from the current position to the maneuvering target point, and each slave UUV informs the master UUV after completing the maneuvering;

Step 19: The master UUV judges whether all the slave UUVs have completed the maneuver. If the master UUV judges that all the slave UUVs have completed the maneuver within 120 seconds, go to step 20, otherwise it is considered that the formation has failed, and go to step (21);

Step 20: The master UUV sends a successful formation formation command to all the slave UUVs, all UUVs keep fixed point and heading, and go to step 22;

Step 21: The master UUV sends a formation failure instruction to all slave UUVs, and all UUVs stop;

Step 22: The formation of the UUV cluster formation is completed.

The beneficial effects of the present invention are:

1. The present invention divides the UUV cluster formation formation space into the row area and the column area of the grid space, which can effectively avoid the collision between UUVs in the formation formation process.

2. The present invention uses the particle swarm optimization algorithm to uniformly plan the general route of the four maneuvering processes of column dispersion, row dispersion, column maneuvering and row maneuvering, which can realize the orderly maneuvering coordination in the formation of the UUV cluster formation, which is beneficial to the realization of Formation formation of large-scale UUV clusters.

3. In the present invention, the amount of information interaction between UUV clusters is small, the calculation is simple, the planning speed is fast, the coordination and maneuvering logic is clear, and the engineering implementation is easy.

Combined with Figure 1, the process of UUV cluster formation formation is introduced.

As shown in Figure 1, it is introduced as a UUV cluster with 5 members. In the initial state, the geometry of the cluster UUVs is randomly distributed and disordered, and each UUV is in a standby state, waiting for the formation to form a start command. The desired geometric formation is the rectangular formation shown in the figure. When the formation starts, all UUVs determine whether they are the master UUV or the slave UUV according to the identity information given to them in advance. In the picture, UUV No. 0 is the master UUV, and the rest are slave UUVs. The main UUV maintains its positioning and heading. After information exchange, autonomous planning, and coordinated maneuvering, the UUV clusters form a desired rectangular formation on the premise of ensuring that they do not collide with each other. In the process of formation formation, the slave UUV maneuvers, and the main UUV always maintains a fixed point and a fixed heading.

Combined with Figure 2, the space division and expected formation of UUV cluster formation formation are introduced.

As shown in Figure 2, the desired formation is introduced by taking a rectangle as the desired geometric formation and a UUV cluster containing 5 members as an example. The desired points 0, 1, 2, 3, and 4 form the desired rectangular formation. After the UUV clusters form a rectangular formation, each UUV is located on a desired point, where the main UUV is located on the desired point 0, and the desired point 0 is called the main desired point. The other 4 slave UUVs are distributed on desired points 1, 2, 3, and 4, and these 4 desired points are called slave desired points.

In order to better utilize the grid method and particle swarm optimization algorithm to plan the formation of large-scale UUV clusters, the space area where the UUV clusters are located is divided into multiple row areas and column areas. The specific division process is as follows:

(1) Line area. As shown in Figure 2, in the hull Cartesian coordinate system established with the position of the main UUV as the origin, the space in the x-axis direction is divided into different lines by using a straight line x=k·L _res with an interval of L _res between adjacent ones. area, and specifies that the line area coordinate in the positive direction of the x-axis is a positive value, and the line area coordinate in the negative direction is a negative value, and there are:

N _{l_min} -N _{num_UUV_F} ≤k≤N _{l_max} +N _{num_UUV_F}

(2) Column area. Similarly, when dividing the column area, the space in the y-axis direction is divided into different column areas, and the space is divided into multiple column areas by using a straight line y=m· _Lres with an interval of L _res between adjacent ones, and it is specified at y The coordinate of the line area in the positive direction of the axis is positive, and the coordinate of the line area in the negative direction is negative, and there are:

N _{r_min} -N _{num_UUV_F} ≤m≤N _{r_max} +N _{num_UUV_F}

The superposition of the row area and the column area together constitutes the spatial grid model of the horizontal plane where the UUV is located. Where k and m are integers, N _{l_min} and N _{l_max} are the minimum and maximum values of the row area where all slave UUVs and slave expected points are located in the main UUV hull Cartesian coordinate system; N _{r_min} and N _{r_max} are all slave UUVs The minimum and maximum values of the column area of the UUV and the position from the desired point in the main UUV hull Cartesian coordinate system.

In order to introduce the specific embodiments of the present invention more clearly, there are the following definitions:

则该点在主UUV船体坐标系下的坐标为

计算方法为：(1) Defined in the space model established based on the grid method, the coordinates of any point in the space in the fixed coordinate system are

Then the coordinates of the point in the main UUV hull coordinate system are

The calculation method is:

为主UUV在固定坐标系下的坐标，θ_{H_L}为主UUV的艏向。in

is the coordinate of the main UUV in a fixed coordinate system, and θ _{H_L} is the heading of the main UUV.

可计算得到该点在空间模型中的行列区域坐标为

计算方法为：(2) Defined in the space model established based on the grid method, according to the coordinates of any point in the space in the main UUV hull coordinate system

The coordinates of the row and column area of the point in the space model can be calculated as

The calculation method is:

Where x _M is the coordinate of the row area where the point is located, and y _M is the coordinate of the column area where the point is located. If x _M =0, x _M =1 is specified; if y _M =0, y _M =1 is specified.

则根据公式(1)和公式(2)可计算1号从UUV在主UUV船体坐标系下的坐标

及1号从UUV在空间模型中的行列区域坐标

According to the above definition, combined with Figure 2, if the coordinates of No. 1 slave UUV in the fixed coordinate system in the known space are

Then according to formula (1) and formula (2), the coordinates of No. 1 slave UUV in the main UUV hull coordinate system can be calculated

and No. 1 from the UUV in the spatial model of the row-column area coordinates

Others are the same from UUV.

则可计算该点的固定坐标系下的坐标为

计算方法为：(3) If the coordinates of the row and column area of any point in the space are known

Then the coordinates of the point in the fixed coordinate system can be calculated as

The calculation method is:

则根据公式(3)和(4)可计算出1号从UUV在主UUV船体直角坐标系下的坐标

和在固定坐标系下的坐标

If the coordinates of the row and column area of No. 1 from the UUV in the space model are known

Then according to formulas (3) and (4), the coordinates of No. 1 slave UUV in the rectangular coordinate system of the main UUV hull can be calculated.

and coordinates in a fixed coordinate system

Others are the same from UUV.

在主UUV船体直角坐标系下的坐标

和在空间模型中的行列区域坐标

之间的相互变换，i＝1,2,···,N_{num_UUV_F}。Similarly, using the formulas (1), (2), (3) and (4), the coordinates of the desired point in the fixed coordinate system can be calculated

Coordinates in the main UUV hull Cartesian coordinate system

and the row-column region coordinates in the spatial model

Mutual transformation between, i=1, 2, ···, N _{num_UUV_F} .

Combined with Figure 3, Figure 4, Figure 5, Figure 6 and Figure 7, the flow of the formation method of large-scale UUV cluster formation based on the grid method is introduced.

其中x_{exp_E}(n)表示每个从期望点固定坐标系下x坐标，y_{exp_E}(n)表示每个从期望点固定坐标系下y坐标，n为各从期望点的序号，n＝1,2,···,N_{num_UUV_F}。Step 1: Initialization. Set the UUV master-slave identity instruction, set the formation formation parameters N _{num_UUV_F} and L _res , where L _res is the width of each row area and column area, and set the desired formation formation instruction

Where x _{exp_E} (n) represents the x coordinate of each slave expected point fixed coordinate system, y _{exp_E} (n) represents the y coordinate of each slave expected point fixed coordinate system, n is the serial number of each slave expected point, n=1, 2,...,N _{num_UUV_F} .

Step 2: The formation begins, all UUVs maintain fixed point and heading, and determine their master-slave identity.

i表示各从UUV代号，i＝1,2,···,N_{num_UUV_F}。主UUV向所有从UUV发送当前个位置信息

和艏向θ_{H_L}。Step 3: The UUV cluster conducts information exchange. All slave UUVs send their current location information to the master UUV

i represents each slave UUV code, i=1, 2, ···, N _{num_UUV_F} . The master UUV sends the current location information to all slave UUVs

and heading θ _{H_L} .

Step 4: The master UUV judges whether all slave UUV location information is received. If the master UUV judges that it has received all the current position information of the slave UUVs within 120 seconds, go to step 5; otherwise, it is considered that the formation has failed, and go to step (21).

行分散目标点坐标

列机动目标点坐标

和行机动目标点坐标

Step 5: The main UUV uses the particle swarm optimization algorithm to plan the maneuvering target points of each of the four stages of column dispersion, row dispersion, column maneuvering, and row maneuvering of the UUVs. The main UUV performs unified planning for the four stages according to the particle swarm optimization algorithm, and solves the matrix X= (x(1),x(2),...,x(i)), and then calculate the coordinates of the maneuvering target points from the four stages of UUV in a fixed coordinate system, and list the coordinates of the scattered target points

Line scatter target point coordinates

Column maneuver target point coordinates

and the coordinates of the moving target point

表示第i个从UUV所解得的位置信息，x_{M_PSO}(i)为i号从UUV行分散的行区域坐标，y_{M_PSO}(i)为i号从UUV列分散的列区域坐标，P_{ID_PSO}(i)为i号从UUV分配的从期望点序号。where i=1,2,...,N _{num_UUV_F} ,

Represents the i-th position information obtained from the UUV solution, x _{M_PSO} (i) is the row area coordinate scattered from the UUV row of the i number, y _{M_PSO} (i) is the column area coordinate of the i number scattered from the UUV column, P _{ID_PSO} ( i) Slave expected point sequence number assigned to slave UUV of number i.

在根据公式(2)求解出各从UUV当前位置点所在的行区域row_f(i)和列区域rank_f(i)：Step 5 (A): Solve the row area row _f (i) and the column area rank _f (i) where the current position of each slave UUV is located. According to the current positions of the master UUV and the slave UUV, the coordinates of the current position of each slave UUV in the master UUV hull coordinate system are obtained by using formula (1).

According to formula (2), the row area row _f (i) and the column area rank _f (i) where the current position point of each slave UUV is located are solved:

Where x _{f_pos_B} (i) is the x-axis coordinate of the slave UUV in the main UUV hull coordinate system, and y _{f_pos_B} (i) is the y-axis coordinate of the slave UUV in the main UUV hull coordinate system.

Step 5 (B): Apply the particle swarm algorithm to solve the matrix X=(x(1),x(2),...,x(i)), and meet the following requirements:

(1) After the end of the dispersion process, all the slave UUVs in different row areas and different column areas have different expected points allocated, and the following formula is established:

where i≠j, i=1,2,...,N _{num_UUV_F} , j=1,2,...,N _{num_UUV_F} .

(2) In order to avoid the collision between the slave UUVs in the dispersion process, the relative position of the slave UUVs in the same row area does not change, and the following conditions should be met:

If row _f (i)=row _f (j), i≠j, the following formula holds:

(y _{M_PSO} (i)-y _{M_PSO} (j))(rank _f (i)-rank _f (j))>0.

(3) After the column maneuvering process is completed, there may be multiple slave UUVs in the same column area at the same time. In order to avoid collision between the slave UUVs during the row maneuvering process, during the row maneuvering process, the relative positions of the slave UUVs in the same column area are different. Changes should meet the following conditions:

If y _{exp_E} (P _{ID_PSO} (i))=y _{exp_E} (P _{ID_PSO} (j)), the following formula holds:

(x _{M_PSO} (i)-x _{M_PSO} (j))(y _{exp_E} (P _{ID_PSO} (i))-y _{exp_E} (P _{ID_PSO} (j)))>0.

(4) In order for each slave UUV not to collide with the master UUV during the maneuvering process, the absolute value of the coordinates of the row area and column area of the dispersion process is greater than 1, and the following conditions should be met:

|x _{M_PSO} (i)|>1, |y _{M_PSO} (i)|>1, i=1,2,...,N _{num_UUV_F}

(5) In order to avoid the collision between the slave UUV and the master UUV during the maneuver, the following conditions should be met:

If |y _{exp_M} (P _{ID_PSO} (i))|≤1, the following formula holds:

(x _{M_PSO} (i))(x _{exp_M} (P _{ID_PSO} (i)))>0, i=1, 2, . . . , N _{num_UUV_F} .

(6) The value of the position of each slave UUV is planned to be an integer, and the following conditions should be met:

|x _{M_PSO} (i)-<x _{M_PSO} (i)>|+|y _{M_PSO} (i)-<y _{M_PSO} (i)>|+|P _{ID_PSO} (i)-<P _{ID_PSO} (i)>|=0, i=1,2,...,N _{num_UUV_F} , where <> indicates that the logarithm is rounded.

(7) In order to avoid the collision between the slave UUV and the master UUV during the column dispersion process, the following conditions should be met:

If |row _f (i)|≤1, the following formula holds:

(rank _f (i))(y _{M_PSO} (i))>0, i=1, 2, . . . , N _{num_UUV_F} .

和行分散目标点的行列区域坐标

并计算各从UUV列分散点和行分散点在固定坐标系下的坐标

和

Step 5 (C): Solve the coordinates of the row and column area of each target point scattered from the UUV column

and the row-column area coordinates of the row scatter target point

And calculate the coordinates of each from the UUV column scatter point and row scatter point in a fixed coordinate system

and

Because in the column dispersion movement, the slave UUV only changes the column area, so the row area coordinates remain unchanged. During the row dispersion movement, the slave UUV only changes the row area, so the column area coordinates remain unchanged. The matrix obtained by the particle swarm optimization algorithm, according to the formula (5), the row and column area coordinates of the column scattered target points are obtained

Calculate the row and column area coordinates of the travel dispersion target point according to formula (6)

和

代入公式(3)和(4)计算出列分散目标点和行分散目标点在固定坐标系下的坐标

和

Will

and

Substitute into formulas (3) and (4) to calculate the coordinates of the column scatter target point and the row scatter target point in the fixed coordinate system

and

和从UUV行分散点在固定坐标系下的坐标

求解出列机动目标点在固定坐标系下的坐标

和行机动目标点在固定坐标系下的坐标

Step 5 (D): From the desired point row and column area coordinates

and scatter the coordinates of the points in the fixed coordinate system from the UUV row

Solve the coordinates of the dequeuing maneuver target point in a fixed coordinate system

and the coordinates of the mobile target point in the fixed coordinate system

根据公式(7)反推出列机动目标点在固定坐标系下的坐标

Since it is a first maneuver and then a maneuver, the coordinates of the area can be determined from the desired point.

According to formula (7), the coordinates of the maneuvering target point in the fixed coordinate system can be reversed

Then according to formula (8), the coordinates of the mobile target point in the fixed coordinate system are obtained

Step 6: The master UUV sends the maneuvering target point information of each stage to all the slave UUVs.

Step 7: The master UUV judges whether all the slave UUVs have received the information of the four maneuvering target points. If the master UUV judges that all the slave UUVs have received the coordinates of the column scatter point, the row scatter point, the column maneuver point coordinate and the row maneuver point coordinate within 120 seconds, go to step 8, otherwise it is considered that some of the slave UUVs have not received the four maneuvering targets Click Information, and go to Step 6.

Step 8: The master UUV sends a column scatter command to all slave UUVs.

Step 9: Column dispersion from UUV. All slave UUVs move from the current position to the nematic dispersion target point, and each slave UUV informs the master UUV after completing the dispersion.

As shown in Figure 4, No. 4 Slave UUV and No. 3 Slave UUV are in the same column area, so No. 4 Slave UUV needs to move from the current position point (dotted line) to No. 4 Slave UUV's column dispersion target point (solid line) ), so that two slave UUVs No. 3 and No. 4 are in different column areas, and the other slave UUVs keep their original positions.

Step 10: The master UUV judges whether all the slave UUVs have completed column dispersion. If the master UUV judges that all the slave UUVs have completed the column dispersion within 120 seconds, go to step 11; otherwise, it is considered that the formation has failed, and go to step (21).

Step 11: The master UUV sends a line scatter command to all slave UUVs.

Step twelve: Row dispersion from UUV. All the slave UUVs move from the current position to the line dispersion target point, and each slave UUV informs the master UUV after the line dispersion is completed.

As shown in Figure 5, No. 1 Slave UUV and No. 2 Slave UUV are in the same line area, so No. 1 Slave UUV needs to move from the current position point (dotted line) to No. 1 Slave UUV's line dispersion target point (solid line) ), other slave UUVs keep their original positions.

Step 13: The master UUV judges whether all the slave UUVs have completed row dispersion. If the master UUV judges that all the slave UUVs have completed line dispersion within 120 seconds, go to step 14; otherwise, it is considered that the formation has failed, and go to step (21).

Step 14: The master UUV sends a column maneuver command to all slave UUVs.

Step fifteen: Column maneuver from UUV. All slave UUVs move from the current position to the target point of nematic maneuver, and each slave UUV informs the master UUV after completing the maneuver.

As shown in Figure 6, the No. 1 slave UUV needs to move from the current position point (dotted line) to the No. 1 slave UUV's column maneuver target point (solid line), and the other slave UUVs are the same.

Step 16: The master UUV judges whether all the slave UUVs have completed the column maneuver. If the master UUV judges that all the slave UUVs have completed the column maneuver within 120 seconds, go to step 17; otherwise, it is considered that the formation has failed, and go to step (21).

Step 17: The master UUV sends a movement command to all the slave UUVs.

Step 18: Do the maneuvering from the UUV. All the slave UUVs move from the current position to the maneuvering target point, and each slave UUV informs the master UUV after completing the maneuvering.

As shown in Figure 7, the No. 1 slave UUV needs to move from the current position point (dotted line) to the No. 1 slave UUV's travel target point (solid line), and the other slave UUVs are the same.

Step 19: The master UUV judges whether all the slave UUVs have completed the maneuver. If the master UUV judges that all slave UUVs have completed the maneuver within 120 seconds, go to step 20; otherwise, it is considered that the formation has failed, and go to step (21).

Step 20: The master UUV sends a formation formation success command to all the slave UUVs, all UUVs keep fixed point and heading, and go to step 22.

Step 21: The master UUV sends a formation failure instruction to all slave UUVs, and all UUVs stop.

Step 22: The formation of the UUV cluster formation is completed.

The above descriptions are only preferred embodiments of the present invention, and are not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and changes. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included within the protection scope of the present invention.

Claims (1)

1. A method for forming formation of a large-scale UUV cluster formation queue based on a grid method is characterized by comprising the following steps:

step 1: selecting a master UUV from the UUV cluster, wherein the other UUV are slave UUV;

and 2, step: setting the width L of the row and column region _res And expected formation instructions

x _{exp_E} (n) is the x coordinate, y, of the nth slave desired point in the fixed coordinate system _{exp_E} (N) is the y coordinate of the nth point in the fixed coordinate system, N =1,2, ·, N _{num_UUV_F} ；N _{num_UUV_F} The number of slave UUV;

and step 3: the formation of the formation begins, all UUV keep fixed point, fixed heading, and determine the master-slave identity of the UUV;

and 4, step 4: UUV cluster carries out information interaction, and all slave UUV send own current position information to master UUV

i denotes the respective UUV code number, i =1,2, ·, N _{num_UUV_F} (ii) a The master UUV sends the current position information to all the slave UUV

And heading information theta _{H_L} ；

And 5: establishing a ship body rectangular coordinate system by taking the position of the main UUV as an origin, and taking the interval as L _res The ship body rectangular coordinate system space is divided into a space grid formed by overlapping a row area and a column area by the straight line, and the maximum value and the minimum value of the row area and the column area are set;

no. i slave UUV coordinate P under main UUV hull coordinate system _{f_pos_B} (i) Comprises the following steps:

x _{f_pos_B} (i) Is the x-axis coordinate, y, of the slave UUV in the coordinate system of the hull of the master UUV _{f_pos_B} (i) The y-axis coordinate of the slave UUV under the ship body coordinate system of the master UUV;

no. i is from the line region row that UUV current position is located _f (i) And column area rank _f (i) Comprises the following steps:

step 6: the master UUV judges whether position information of all slave UUV is received; if the master UUV receives the current position information of all the slave UUV within the specified time, executing step 7; otherwise, the formation of the formation is considered to be failed, the master UUV sends formation failure instructions to all the slave UUV, and the step 4 is returned after all the UUV stops;

and 7: the master UUV plans maneuvering target points of four stages of column dispersion, row dispersion, column maneuvering and mechanical maneuvering of each slave UUV;

step 7.1: the master UUV solves all row area coordinates of the slave UUV rows, column area coordinates of the columns and a matrix X of the slave expected point sequence number to be reached, which meet the conditions, and screens out the matrix X which enables the total movement distance of all the slave UUV to be shortest;

matrix X = (X (1), X (2) \8230;, X (i)),

x _{M_PSO} (i) Line zone coordinates, y, scattered from UUV line for No. i _{M_PSO} (i) Column region coordinates, P, scattered from UUV column for No. i _{ID_PSO} (i) A slave expected point sequence number assigned to the i-th slave UUV;

the matrix X satisfies the condition:

1) When i ≠ j,

2) When row _f (i)＝row _f (j) And i ≠ j, (y) _{M_PSO} (i)-y _{M_PSO} (j))(rank _f (i)-rank _f (j))＞0；

3) When y is _{exp_E} (P _{ID_PSO} (i))＝y _{exp_E} (P _{ID_PSO} (j) In the case of a hot press machine),

(x _{M_PSO} (i)-x _{M_PSO} (j))(y _{exp_E} (P _{ID_PSO} (i))-y _{exp_E} (P _{ID_PSO} (j)))＞0；

4)|x _{M_PSO} (i)|＞1，|y _{M_PSO} (i)|＞1；

5) When y _{exp_M} (P _{ID_PSO} (i) When (x) is less than or equal to 1 |, (x) _{M_PSO} (i))(x _{exp_M} (P _{ID_PSO} (i)))＞0；

6)|x _{M_PSO} (i)-<x _{M_PSO} (i)>|+|y _{M_PSO} (i)-<y _{M_PSO} (i)>|+|P _{ID_PSO} (i)-<P _{ID_PSO} (i)>(= 0), wherein<>Representing a rounding operation;

7) When screw _f (i) When | is less than or equal to 1, (rank) _f (i))(y _{M_PSO} (i))＞0；

And 7.2: calculating coordinates of maneuvering target points of the slave UUV in four stages of row dispersion, row maneuvering and mechanical maneuvering under a fixed coordinate system;

coordinates of row dispersed points in a fixed coordinate system

Coordinates of line scatter points in a fixed coordinate system and

coordinates of train maneuvering target point under fixed coordinate system

Coordinates of mobile maneuvering target point under fixed coordinate system

And 8: the master UUV sends the maneuvering target point information of each stage to all the slave UUV, and after all the slave UUV feeds back the information of the four maneuvering target points, step 9 is executed;

and step 9: the master UUV sends column dispersion instructions to all the slave UUV, all the slave UUV move from the current position to the column dispersion target point, and each slave UUV feeds back information to the master UUV after completing the column dispersion;

step 10: the master UUV judges whether all slave UUV complete the column dispersion; if the master UUV receives the feedback information that all the slave UUV complete the column dispersion within the specified time, executing step 11; otherwise, the formation of the formation is considered to be failed, the master UUV sends formation failure instructions to all the slave UUV, and the step 4 is returned after all the UUV stops;

step 11: the master UUV sends a line dispersion instruction to all the slave UUV, all the slave UUV move from the current position to a line dispersion target point, and each slave UUV feeds back information to the master UUV after completing the line dispersion;

step 12: the master UUV judges whether all slave UUV complete line dispersion; if the master UUV receives the feedback information that all the slave UUV complete the line dispersion within the specified time, executing step 13; otherwise, the formation of the formation is considered to be failed, the master UUV sends formation failure instructions to all slave UUV, and the step 4 is returned after all UUV are stopped;

step 13: the master UUV sends a train maneuvering instruction to all the slave UUV, all the slave UUV move to a train maneuvering target point from the current position, and each slave UUV feeds back information to the master UUV after finishing train maneuvering;

step 14: the master UUV judges whether all slave UUV complete the train maneuver; if the master UUV receives the feedback information that all slave UUV complete the train maneuver within the specified time, executing step 15; otherwise, the formation of the formation is considered to be failed, the master UUV sends formation failure instructions to all slave UUV, and the step 4 is returned after all UUV are stopped;

step 15: the master UUV sends a mechanical action command to all the slave UUV, all the slave UUV move to a mechanical action target point from the current position, and each slave UUV feeds back information to the master UUV after completing the mechanical action;

step 16: the master UUV judges whether all slave UUV complete the maneuver; if the master UUV receives the feedback information of all slave UUV completion row maneuvers within the specified time, executing step 17; otherwise, the formation of the formation is considered to be failed, the master UUV sends formation failure instructions to all the slave UUV, and the step 4 is returned after all the UUV stops;

and step 17: and the master UUV sends the formation to all the slave UUV to form a success instruction, and all the UUV keep a fixed point and a fixed heading to finish the formation of the UUV cluster formation.

Images