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

Large-scale UUV cluster formation method based on grid method Download PDF

Info

Publication number
CN111930116B
CN111930116B CN202010724118.4A CN202010724118A CN111930116B CN 111930116 B CN111930116 B CN 111930116B CN 202010724118 A CN202010724118 A CN 202010724118A CN 111930116 B CN111930116 B CN 111930116B
Authority
CN
China
Prior art keywords
uuv
slave
master
formation
pso
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202010724118.4A
Other languages
Chinese (zh)
Other versions
CN111930116A (en
Inventor
陈涛
范云垚
徐达
李娟�
吴迪
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Engineering University
Original Assignee
Harbin Engineering University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN202010724118.4A priority Critical patent/CN111930116B/en
Publication of CN111930116A publication Critical patent/CN111930116A/en
Application granted granted Critical
Publication of CN111930116B publication Critical patent/CN111930116B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0287—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling
    • G05D1/0289—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling with means for avoiding collisions between vehicles
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0287—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling
    • G05D1/0291—Fleet control

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

The invention belongs to the technical field of UUV formation control, and particularly relates to a grid method-based formation method of a large-scale UUV cluster formation form. According to the invention, the formation space of the UUV cluster formation forms is divided into the row area and the column area of the grid space, so that the collision between UUV in the formation process of the formation forms can be effectively avoided. The invention uses the particle swarm optimization algorithm to carry out unified planning on the general routes of four maneuvering processes of row dispersion, row maneuvering and mechanical maneuvering, can realize ordered maneuvering coordination in the formation process of the UUV cluster formation, and is beneficial to realizing the formation of the formation of the UUV cluster in a large scale. In the invention, the information interaction amount among UUV clusters is small, the calculation is simple, the planning speed is high, the coordinated maneuvering logic is clear, and the engineering realization is easy.

Description

Large-scale UUV cluster formation method based on grid method
Technical Field
The invention belongs to the technical field of UUV formation control, and particularly relates to a grid method-based formation method of a large-scale UUV cluster formation form.
Background
UUV has very important significance in the fields of ocean resource development, national economy development and military application. With the development of the UUV technology, cooperative work by using the UUV cluster is more and more apt to greatly improve the work capacity and work efficiency. During the operation process of the UUV cluster, such as cooperative detection and target search, a formation navigation mode in a certain expected geometric formation form is generally adopted. The formation navigation firstly solves the problem of how to realize the formation of the desired geometric formation form from the initial randomly distributed and disordered geometric formation form of the UUV clusters, and the process is the formation of the UUV clusters.
Formation of formation belongs to the field of UUV formation control, common formation control methods such as an artificial potential field method, a pilot following method, a virtual structure method and the like are more suitable for maintaining the formation of the formation, but the problems of large communication information interaction amount, complex planning and coordination logic, collision risks among UUV and the like generally exist when the formation control methods are applied to the formation of the formation. Particularly, the UUV cluster adopts underwater acoustic communication with a small communication bandwidth and a large communication delay, and the UUV cluster is large in scale and has higher difficulty. Therefore, it is very necessary to develop a formation method applicable to a large-scale UUV cluster.
Patent document No. 201910917112.6 discloses a method for forming a formation form of UUV cluster formation based on circumferential hierarchical planning. The method mainly solves the problem of forming the small-scale UUV cluster formation aiming at communication delay ordered maneuvering. Firstly, the patent adopts a fan-shaped and circumferential space region dividing method, which is different from the grid space region dividing method of the invention that the row region and the column region are intersected. Secondly, the patent mainly provides a formation method of small-scale UUV cluster formations which are planned step by step, maneuvered step by step and crossed with maneuvers, and the formation method is different from the formation method of large-scale UUV cluster formations which are unified to plan a total path and maneuver.
Disclosure of Invention
The invention aims to provide a grid method-based formation method of a formation form of a large-scale UUV cluster, which can be applied to formation of the formation form of the large-scale UUV cluster.
The purpose of the invention is realized by the following technical scheme: the method comprises 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
Figure BDA0002601060720000011
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 3, step 3: when formation of a formation begins, all UUV keep fixed points and 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
Figure BDA0002601060720000021
i denotes the respective UUV code number, i =1,2, ·, N num_UUV_F (ii) a The master UUV sends the current position information of the master UUV to all the slave UUV
Figure BDA0002601060720000022
And heading information theta H_L ；
And 5: a ship body rectangular coordinate system is established by taking the position of the main UUV as the origin, and the interval is 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:
Figure BDA0002601060720000023
Figure BDA0002601060720000024
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 coordinate of the slave UUV on the y axis of the ship body coordinate system of the master UUV;
no. i is from the row region row that UUV current position is in f (i) And column area rank f (i) Comprises the following steps:
Figure BDA0002601060720000025
Figure BDA0002601060720000026
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 the step 7; 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;
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)),
Figure BDA0002601060720000031
x M_PSO (i) Line zone coordinates, y, scattered from UUV line for No. i M_PSO (i) For column region coordinates, P, of No. i scattered from UUV columns 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,
Figure BDA0002601060720000032
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 | < 1 |, (rank) f (i))(y M_PSO (i))＞0；
Step 7.2: calculating coordinates of maneuvering target points of the UUV in four stages of row dispersion, row maneuvering and traveling maneuvering under a fixed coordinate system;
coordinates of the line dispersed points in a fixed coordinate system
Figure BDA0002601060720000033
Figure BDA0002601060720000034
Figure BDA0002601060720000041
Figure BDA0002601060720000042
Figure BDA0002601060720000043
Coordinates of line scatter points in a fixed coordinate system and
Figure BDA0002601060720000044
Figure BDA0002601060720000045
Figure BDA0002601060720000046
Figure BDA0002601060720000047
Figure BDA0002601060720000048
coordinates of train maneuvering target point under fixed coordinate system
Figure BDA0002601060720000049
Figure BDA00026010607200000410
Coordinates of mobile maneuvering target point under fixed coordinate system
Figure BDA00026010607200000411
Figure BDA00026010607200000412
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 slave UUV, and the step 4 is returned after all UUV are stopped;
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 finishes 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 column maneuver instruction to all the slave UUV, all the slave UUV move to a column maneuver target point from the current position, and each slave UUV feeds back information to the master UUV after completing the column maneuver;
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 the slave UUV, and the step 4 is returned after all the UUV stops;
step 15: the master UUV sends a mechanical action instruction to all the slave UUV, all the slave UUV move from the current position to a mechanical action target point, 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 that all the slave UUV complete the maneuver, the step 17 is executed; 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;
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.
The invention has the beneficial effects that:
according to the invention, the formation space of the UUV cluster formation forms is divided into the row area and the column area of the grid space, so that the collision between UUV in the formation process of the formation forms can be effectively avoided. The invention uses the particle swarm optimization algorithm to uniformly plan the general route of four maneuvering processes of column dispersion, row dispersion, column maneuvering and maneuvering, can realize the ordered maneuvering coordination in the formation process of the UUV cluster formation, and is beneficial to realizing the formation of the formation of the large-scale UUV cluster. In the invention, the information interaction amount among UUV clusters is small, the calculation is simple, the planning speed is high, the coordinated maneuvering logic is clear, and the engineering implementation is easy.
Drawings
Fig. 1 is a schematic diagram of formation of a UUV cluster formation queue.
Fig. 2 is a schematic diagram of division of spaces where UUV cluster formation expects formation and formation of formation.
Fig. 3 is a flowchart of a formation method of a large-scale UUV cluster formation queue based on a grid method.
Fig. 4 is a schematic diagram illustrating a UUV cluster queuing dispersion principle.
Fig. 5 is a schematic diagram of a UUV cluster formation row dispersion principle.
Fig. 6 is a schematic diagram of a UUV cluster queuing maneuver principle.
Figure 7 is a schematic diagram of the operation principle of the UUV cluster formation machine.
Detailed Description
The invention is further described below with reference to the accompanying drawings.
The invention aims to provide a formation method based on a grid method and suitable for a large-scale UUV cluster. The method can enable the large-scale UUV cluster to quickly and safely form the expected formation from the initial random distribution and disordered state.
A large-scale UUV cluster formation method based on a grid method comprises the following steps:
the method comprises the following steps: initializing, setting a UUV master-slave identity instruction, setting formation parameters of a formation and expecting the formation instruction;
setting formation parameter N num_UUV_F And L res In which N is num_UUV_F Is the number of slave UUV, L res Is the row and column region width.
Setting desired formation instructions
Figure BDA0002601060720000061
Wherein x exp_E (n) denotes the x-coordinate, y, of each fixed coordinate system from the desired point exp_E (N) denotes the y coordinate of each slave expectation point in the fixed coordinate system, N is the serial number of each slave expectation point, N =1,2, ·, N num_UUV_F 。
Step two: the formation of the formation begins, all UUV keep fixed point, fixed heading, and determine the master-slave identity of the UUV;
step three: and the UUV cluster carries out information interaction. All slave UUV send own current position information to master UUV
Figure BDA0002601060720000062
i denotes the respective UUV code number, i =1,2, ·, N num_UUV_F . The master UUV sends the current position information to all slave UUV
Figure BDA0002601060720000063
And heading information theta H_L ；
Step four: and the master UUV judges whether all the slave UUV position information is received. If the master UUV judges that the current position information of all the slave UUV is received within 120 seconds, turning to the step five, otherwise, turning to the step twenty one if the formation of the formation fails;
step five: and the master UUV uses a particle swarm optimization algorithm to mark maneuvering target points of the slave UUV in four stages of column dispersion, row dispersion, column maneuvering and maneuvering. The master UUV carries out unified planning on the four stages according to a particle swarm optimization algorithm, when the shortest total path is solved, the line area coordinates scattered from the rows of the slave UUV, the column area coordinates scattered from the columns and the matrix X = (X (1), X (2), \8230;, X (i)) of the sequence number of the slave expected points to be reached are solved,
Figure BDA0002601060720000071
represents the ithPosition information, x, solved from UUV M_PSO (i) Line zone coordinates, y, scattered from UUV line for number i M_PSO (i) Column region coordinates, P, for the dispersion of the i number from the UUV column ID_PSO (i) N, i =1,2, assigned to UUV No. i from the expected point sequence number num_UUV_F Further, the maneuvering target points of the four phases of the slave UUV are calculated, and the coordinates of the scattered target points are listed
Figure BDA0002601060720000072
Line scatter target point coordinates
Figure BDA0002601060720000073
Train maneuver target point coordinates
Figure BDA0002601060720000074
Coordinates of moving target point
Figure BDA0002601060720000075
In the fifth step, the step of marking out maneuvering target points of the four phases of each slave UUV by the master UUV by applying a particle swarm optimization algorithm specifically comprises the following steps:
step five (A): solving row area row of each slave UUV current position f (i) And column area rank f (i)；
Figure BDA0002601060720000076
Figure BDA0002601060720000077
Wherein x f_pos_B (i) The slave UUV has x-axis coordinate and y-axis coordinate in the coordinate system of the ship body of the master UUV f_pos_B (i) And the Y-axis coordinate of the slave UUV under the ship body coordinate system of the master UUV is shown.
Step five (B): solving a matrix X = (X (1), X (2), \8230;, X (i)) by applying a particle swarm algorithm;
the particle swarm matrix obtained should satisfy the following conditions:
(1) After the dispersion process is finished, all slave UUV in different row areas and different column areas are distributed with different slave expectation points, and the following formula is established:
Figure BDA0002601060720000081
wherein i ≠ j, i =1,2 num_UUV_F ，j＝1,2,...,N num_UUV_F 。
(2) In order to avoid collision between the slave UUVs in the dispersion process, the relative positions of the slave UUVs in the same row area are not changed, and the following conditions are met:
if row f (i)＝row f (j) If i ≠ j, then the following equation holds:
(y M_PSO (i)-y M_PSO (j))(rank f (i)-rank f (j))＞0。
(3) After the train maneuvering process is completed, a plurality of slave UUVs may exist in the same train area at the same time, and in order to avoid collision between the slave UUVs in the train maneuvering process, the relative positions of the slave UUVs in the same train area in the train maneuvering process should not change, and the following conditions should be satisfied:
if y exp_E (P ID_PSO (i))＝y exp_E (P ID_PSO (j) Then the following equation 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 that each slave UUV does not collide with the master UUV in the maneuvering process, the absolute values of the coordinates of the row area and the column area in the dispersing process are greater than 1, and the following conditions are met:
|x M_PSO (i)|＞1，|y M_PSO (i)|＞1，i＝1,2,...,N num_UUV_F
(5) In order to avoid collision between the slave UUV and the master UUV in the moving process, the following conditions are met:
if y exp_M (P ID_PSO (i) Is less than or equal to 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 position value of each slave UUV is planned to be an integer, and the following conditions are 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 wherein< >Indicating that the logarithm is rounded.
(7) In order to avoid collision between the slave UUV and the master UUV in the column dispersion process, the following conditions should be satisfied:
if | row f (i) If | ≦ 1, the following equation holds:
(rank f (i))(y M_PSO (i))＞0，i＝1,2,...,N num_UUV_F 。
step five (C): solving the coordinates of the line and row regions of the slave UUV line scattered target points
Figure BDA0002601060720000082
Line and row area coordinates of line-sum dispersion target points
Figure BDA0002601060720000091
And calculating coordinates of each slave UUV row scattered point and each slave UUV column scattered point in a fixed coordinate system
Figure BDA0002601060720000092
And
Figure BDA0002601060720000093
in the column dispersion movement, the slave UUV only changes the column area where the slave UUV is located, the line area coordinates are not changed, and the line area coordinates of the column dispersion target points are solved
Figure BDA0002601060720000094
Wherein x d_rank_M (i) Dispersing the line area coordinates of the target points for the columns, y d_rank_M (i) For the column area coordinates of the column scatter target points, the following formula is used:
Figure BDA0002601060720000095
in the row dispersion movement, the slave UUV only changes the row area where the slave UUV is located, the column area coordinates are not changed, and the column area coordinates of the row dispersion target point are solved
Figure BDA0002601060720000096
Wherein x is d_row_M (i) Dispersing the line area coordinates, y, of the target points for a line d_row_M (i) For the column area coordinates of the line scatter target points, the following formula is used:
Figure BDA0002601060720000097
calculating the coordinates of the dispersed points of the UUV rows in a fixed coordinate system
Figure BDA0002601060720000098
The following formula is utilized:
Figure BDA0002601060720000099
Figure BDA00026010607200000910
Figure BDA00026010607200000911
calculating the coordinates of the dispersed points of the slave UUV rows in a fixed coordinate system
Figure BDA00026010607200000912
The following formula is utilized:
Figure BDA0002601060720000101
Figure BDA0002601060720000102
Figure BDA0002601060720000103
step five (D): by the row and column area coordinates from the desired point
Figure BDA0002601060720000104
And coordinates of dispersed points of the slave UUV rows in a fixed coordinate system
Figure BDA0002601060720000105
Solving the coordinates of the train maneuvering target point under the fixed coordinate system
Figure BDA0002601060720000106
And coordinates of the mobile target point in a fixed coordinate system
Figure BDA0002601060720000107
Figure BDA0002601060720000108
Figure BDA0002601060720000109
Step six: the master UUV sends maneuvering target point information of each stage to all slave UUV;
step seven: and the master UUV judges whether all the slave UUV receive the information of the four maneuvering target points. If the master UUV judges that all slave UUV receive the coordinates of the column scattered points, the coordinates of the row scattered points, the coordinates of the column maneuvering points and the coordinates of the maneuvering points within 120 seconds, turning to the step eight, and if not, turning to the step six, considering that some slave UUV do not receive the information of the four maneuvering target points;
step eight: the master UUV sends column dispersion instructions to all slave UUV;
step nine: column scatter is done from UUV. All the slave UUV move from the current position to the column dispersion target point, and each slave UUV informs the master UUV after completing the column dispersion;
step ten: and the master UUV judges whether all the slave UUV complete the column dispersion. If the master UUV judges that all slave UUV complete column dispersion within 120 seconds, turning to the eleventh step, otherwise, turning to the twenty-one step if the formation of the formation fails;
step eleven: the master UUV sends a line dispersion instruction to all slave UUV;
step twelve: line scatter is done from UUV. All the slave UUV move from the current position to the line dispersion target point, and each slave UUV informs the master UUV after completing the line dispersion;
step thirteen: and the master UUV judges whether all the slave UUV complete the line dispersion. And if the master UUV judges that all slave UUV complete line dispersion within 120 seconds, turning to the fourteen step, otherwise, turning to the twenty-one step, wherein formation of the formation is considered to fail.
Fourteen steps: the master UUV sends a column maneuver instruction to all the slave UUV;
a fifteenth step: the slave UUV performs a column maneuver. All the slave UUV move to the column maneuver target point from the current position, and each slave UUV informs the master UUV after completing the column maneuver;
sixthly, the steps are as follows: and the master UUV judges whether all the slave UUV complete the column maneuver. And if the master UUV judges that all slave UUV complete the train maneuver within 120 seconds, turning to the seventeen step, otherwise, turning to the twenty-one step, wherein the formation of the formation is considered to fail.
Seventeen steps: the master UUV sends a mobile instruction to all the slave UUV;
eighteen, the steps are: the slave UUV performs a maneuver. All the slave UUV move from the current position to a mechanical movement target point, and each slave UUV informs the master UUV after completing the mechanical movement;
nineteen steps: the master UUV determines whether all slave UUVs have completed the maneuver. If the master UUV judges that all slave UUV complete the movement within 120 seconds, turning to the step twenty, otherwise, considering that the formation of the formation fails, and turning to the step twenty-one;
twenty steps: the master UUV sends formation success instructions to all slave UUV, all UUV keep fixed point and fixed heading, and go to the twenty-two step;
twenty one is as follows: the master UUV sends a formation failure instruction to all slave UUV, and all UUV stop;
twenty-two steps: and finishing formation of the UUV cluster formation queue.
The invention has the beneficial effects that:
1. according to the invention, the formation space of the UUV cluster formation forms is divided into the row area and the column area of the grid space, so that the collision between UUV in the formation process of the formation forms can be effectively avoided.
2. The invention uses the particle swarm optimization algorithm to carry out unified planning on the general routes of four maneuvering processes of row dispersion, row maneuvering and mechanical maneuvering, can realize ordered maneuvering coordination in the formation process of the UUV cluster formation, and is beneficial to realizing the formation of the formation of the UUV cluster in a large scale.
3. In the invention, the information interaction amount among UUV clusters is small, the calculation is simple, the planning speed is high, the coordinated maneuvering logic is clear, and the engineering realization is easy.
The process of forming the formation of the UUV cluster formation queue is described in conjunction with FIG. 1.
As shown in fig. 1, the introduction is made with a UUV cluster containing 5 members. In the initial state, the geometrical forms of the cluster UUV are randomly distributed and disordered, each UUV is in a standby state, and a formation starting instruction is waited. The geometric formation desired to be formed is a rectangular formation as shown in the figure. When formation of the formation starts, all the UUVs determine whether the UUV is a master UUV or a slave UUV according to the identity information given to the UUV in advance, for example, the UUV No. 0 is the master UUV, and the others are the slave UUVs. The main UUV keeps positioning and heading, and after information interaction, autonomous planning and coordinated maneuvering are carried out among the UUV clusters, an expected rectangular formation is formed on the premise that mutual collision is avoided. In the formation process of the formation, the slave UUV performs maneuvering, and the master UUV always keeps fixed point and fixed heading.
With reference to fig. 2, the space division where UUV cluster formation takes place and the desired formation are described.
As shown in fig. 2, the desired geometric formation is illustrated by using a rectangle as the desired geometric formation, and a UUV cluster including 5 members as an example. The desired points 0, 1,2, 3, 4 make up the desired rectangular formation. After the UUV clusters form a rectangular formation, each UUV is positioned on an expectation point, wherein a main UUV is positioned on an expectation point 0, and the expectation point 0 is called a main expectation point. The other 4 slave UUVs are distributed on the expected points 1,2, 3 and 4, and the 4 expected points are called slave expected points.
In order to better utilize a grid method and a particle swarm optimization algorithm to carry out formation planning on a large-scale UUV cluster formation, a space area where the UUV cluster is located is divided into a plurality of row areas and column areas, and the specific division process comprises the following steps:
(1) A line area. As shown in fig. 2, in a ship rectangular coordinate system established by taking the position of the main UUV as the origin, the interval between adjacent UUVs is L res Straight line of (x = k · L) res The space in the x-axis direction is divided into different line regions, and it is specified that the line region coordinates in the positive direction of the x-axis are positive values and the line region coordinates in the negative direction are negative values, and:
N l_min -N num_UUV_F ≤k≤N l_max +N num_UUV_F
(2) Column area. Similarly, when dividing the column region, the space in the y-axis direction is divided into different column regions, and the interval between adjacent column regions is L res Straight line y = m · L res The space is divided into a plurality of column areas, and the coordinates of the row area in the positive direction of the y axis are defined to be positive values, the coordinates of the row area in the negative direction are defined to be negative values, and the method comprises the following steps:
N r_min -N num_UUV_F ≤m≤N r_max +N num_UUV_F
and the row area and the column area are superposed to jointly form a space grid model of the horizontal plane of the UUV. Wherein k and m are each an integer, N l_min And N l_max The minimum value and the maximum value of the line areas of the positions of all the slave UUV and the slave expectation points under the rectangular coordinate system of the main UUV hull; n is a radical of r_min And N r_max The minimum value and the maximum value of the column areas of the positions of all the slave UUV and the slave expectation points under the rectangular coordinate system of the hull of the master UUV are obtained.
For a more clear description of the embodiments of the invention the following definitions are provided:
(1) The coordinate of any point in the space under a fixed coordinate system is defined as
Figure BDA0002601060720000131
The coordinate of the point under the coordinate system of the main UUV hull is
Figure BDA0002601060720000132
The calculation method comprises the following steps:
Figure BDA0002601060720000133
wherein
Figure BDA0002601060720000134
Is the coordinate of the main UUV in a fixed coordinate system, theta H_L Is the heading of the main UUV.
(2) The method is defined in a space model established based on a grid method, and coordinates of any point in space under a main UUV ship body coordinate system
Figure BDA0002601060720000135
The coordinates of the row and column region of the point in the space model can be calculated as
Figure BDA0002601060720000136
The calculation method comprises the following steps:
Figure BDA0002601060720000137
wherein x is M As the coordinates of the row area where the point is located, y M Is the coordinates of the area where the point is located. If x M If =0, x is defined M =1; if y M If =0, y is defined M ＝1。
From the above definition, in conjunction with fig. 2, then if the coordinates in space under the fixed coordinate system of slave UUV No. 1 are knownIs composed of
Figure BDA0002601060720000138
Then the coordinates of No. 1 slave UUV under the ship body coordinate system of the master UUV can be calculated according to the formula (1) and the formula (2)
Figure BDA0002601060720000139
And the coordinates of the line and column region of No. 1 slave UUV in the space model
Figure BDA00026010607200001310
Figure BDA00026010607200001311
Figure BDA0002601060720000141
Figure BDA0002601060720000142
The other slave UUVs work the same way.
(3) If the coordinates of the line and column region of any point in the space are known
Figure BDA0002601060720000143
The coordinates of the point in the fixed coordinate system can be calculated as
Figure BDA0002601060720000144
The calculation method comprises the following steps:
Figure BDA0002601060720000145
Figure BDA0002601060720000146
if No. 1 slave UUV is known in the space modelRow and column region coordinates of
Figure BDA0002601060720000147
Then the coordinates of No. 1 slave UUV under the rectangular coordinate system of the main UUV hull can be calculated according to the formulas (3) and (4)
Figure BDA0002601060720000148
And coordinates in a fixed coordinate system
Figure BDA0002601060720000149
Figure BDA00026010607200001410
Figure BDA00026010607200001411
Figure BDA00026010607200001412
The other slave UUVs work the same way.
Similarly, the coordinates of the desired point in the fixed coordinate system can be calculated by using the formulas (1), (2), (3) and (4)
Figure BDA0002601060720000151
Coordinate under rectangular coordinate system of main UUV hull
Figure BDA0002601060720000152
And row-column region coordinates in the spatial model
Figure BDA0002601060720000153
I =1,2, ·, N num_UUV_F 。
With reference to fig. 3, 4, 5, 6, and 7, a flow of a method for forming formation of a large-scale UUV cluster formation based on a grid method is described.
The method comprises the following steps:and (5) initializing. Setting UUV master-slave identity instruction, setting formation parameter N num_UUV_F And L res Wherein L is res Setting a desired formation instruction for the width of each row area and column area
Figure BDA0002601060720000154
Wherein x exp_E (n) denotes the x-coordinate, y, of each fixed coordinate system from the desired point exp_E (N) denotes the y coordinate of each slave expectation point in the fixed coordinate system, N is the serial number of each slave expectation point, N =1,2, ·, N num_UUV_F 。
Step two: and when the formation of the formation begins, all UUV keep fixed points and fixed heading, and determine the master-slave identity of the UUV.
Step three: and the UUV cluster carries out information interaction. All slave UUV send own current position information to master UUV
Figure BDA0002601060720000155
i denotes the respective slave UUV code number, i =1,2, ·, N num_UUV_F . The master UUV sends the current position information to all slave UUV
Figure BDA0002601060720000156
And heading theta H_L 。
Step four: and the master UUV judges whether position information of all slave UUV is received. And if the master UUV judges that the current position information of all the slave UUV is received within 120 seconds, turning to the fifth step, and if not, considering that the formation of the formation fails, turning to the twenty-one step.
Step five: and the master UUV uses a particle swarm optimization algorithm to mark maneuvering target points of the slave UUV in four stages of column dispersion, row dispersion, column maneuvering and maneuvering. The master UUV carries out unified planning on the four stages according to a particle swarm optimization algorithm, under the condition that the total path is shortest, the row area coordinates scattered from the rows of the slave UUV, the column area coordinates scattered from the columns and the matrix X = (X (1), X (2), \8230;, X (i)) of the sequence number of the slave expected point to be reached are solved, and then the coordinates of the maneuvering target points of the four stages of the slave UUV in a fixed coordinate system and the coordinates of the column scattered target points are calculated
Figure BDA0002601060720000157
Line scatter target point coordinates
Figure BDA0002601060720000158
Train maneuver target point coordinates
Figure BDA0002601060720000159
Coordinates of moving target point
Figure BDA00026010607200001510
Wherein i =1,2 num_UUV_F ，
Figure BDA0002601060720000161
Indicating the i-th position information, x, solved by the UUV M_PSO (i) Line zone coordinates, y, scattered from UUV line for number i M_PSO (i) Column region coordinates, P, for the dispersion of the i number from the UUV column ID_PSO (i) The slave expected point sequence number assigned to slave UUV for number i.
Step five (A): solving row area row where each slave UUV current position is f (i) And column area rank f (i) In that respect Obtaining the coordinates of the current position point of each slave UUV in the ship body coordinate system of the master UUV by using a formula (1) according to the current positions of the master UUV and the slave UUV
Figure BDA0002601060720000162
Solving the row area row where the current position point of each slave UUV is according to the formula (2) f (i) And column area rank f (i)：
Figure BDA0002601060720000163
Figure BDA0002601060720000164
Wherein x is f_pos_B (i) The slave UUV has x-axis coordinate and y-axis coordinate in the coordinate system of the ship body of the master UUV f_pos_B (i) Is a slave UAnd the Y-axis coordinate of the UV is in a coordinate system of the hull of the main UUV.
Step five (B): solving the matrix X = (X (1), X (2), \8230;, X (i)) by applying a particle swarm algorithm, and meeting the following requirements:
(1) After the dispersing process is finished, all the slave UUVs are in different row areas and different column areas, the distributed slave expectation points are different, and the following formula is established:
Figure BDA0002601060720000165
wherein i ≠ j, i =1,2 num_UUV_F ，j＝1,2,...,N num_UUV_F 。
(2) In order to avoid mutual collision between the slave UUVs in the dispersion process, the relative positions of the slave UUVs in the same row region are not changed, and the following conditions should be met:
norow (Row) f (i)＝row f (j) If i ≠ j, then the following equation holds:
(y M_PSO (i)-y M_PSO (j))(rank f (i)-rank f (j))＞0。
(3) After the train maneuvering process is completed, a plurality of slave UUVs may exist in the same train area at the same time, and in order to avoid collision among the slave UUVs in the train maneuvering process, the relative positions of the slave UUVs in the same train area are not changed in the train maneuvering process, the following conditions should be met:
if y exp_E (P ID_PSO (i))＝y exp_E (P ID_PSO (j) Then the following equation 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 that each slave UUV does not collide with the master UUV in the maneuvering process, the absolute values of the coordinates of the row area and the column area in the dispersing process are more than 1, and the following conditions are met:
|x M_PSO (i)|＞1，|y M_PSO (i)|＞1，i＝1,2,...,N num_UUV_F
(5) In order to avoid collision between the slave UUV and the master UUV in the moving process, the following conditions are met:
if y exp_M (P ID_PSO (i) Is less than or equal to 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 position value of each slave UUV is planned to be an integer, and the following conditions are 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 wherein< >Indicating that the logarithm is rounded.
(7) In order to avoid collision between the slave UUV and the master UUV in the column dispersion process, the following conditions should be satisfied:
if row f (i) If | ≦ 1, the following equation holds:
(rank f (i))(y M_PSO (i))＞0，i＝1,2,...,N num_UUV_F 。
step five (C): solving the coordinates of the line and row regions of the slave UUV line scattered target points
Figure BDA0002601060720000171
And the line and column area coordinates of the line-scattered target points
Figure BDA0002601060720000172
And calculating coordinates of each slave UUV row scattered point and each slave UUV column scattered point in a fixed coordinate system
Figure BDA0002601060720000173
And
Figure BDA0002601060720000174
in the column dispersion movement, the slave UUV changes only the column region where the slave UUV is located, so that the row region coordinates are not changed, and in the row dispersion movement, the slave UUV changes only the row region where the slave UUV is located, so that the column region coordinates are not changed. Optimization algorithm by particle swarmObtaining the matrix, and obtaining the coordinates of the line and row regions of the dispersed target points according to the formula (5)
Figure BDA0002601060720000181
Figure BDA0002601060720000182
Calculating the coordinates of the row and column regions of the travel scattered target points according to a formula (6)
Figure BDA0002601060720000183
Figure BDA0002601060720000184
Will be provided with
Figure BDA0002601060720000185
And
Figure BDA0002601060720000186
substituting the formula (3) and the formula (4) to calculate the coordinates of the column scattered target point and the row scattered target point in a fixed coordinate system
Figure BDA0002601060720000187
And
Figure BDA0002601060720000188
step five (D): by row and column area coordinates from the desired point
Figure BDA0002601060720000189
And coordinates of dispersed points from the UUV row under a fixed coordinate system
Figure BDA00026010607200001810
Solving the coordinates of the train maneuvering target point under the fixed coordinate system
Figure BDA00026010607200001811
And coordinates of the mobile target point in a fixed coordinate system
Figure BDA00026010607200001812
Since it is a column maneuver followed by a row maneuver, the region coordinates can be listed from the desired point
Figure BDA00026010607200001813
Reversely deducing the coordinates of the train maneuvering target point under the fixed coordinate system according to the formula (7)
Figure BDA00026010607200001814
Figure BDA00026010607200001815
Then obtaining the coordinates of the mechanical movement target point under the fixed coordinate system according to the formula (8)
Figure BDA00026010607200001816
Figure BDA0002601060720000191
Step six: and the master UUV sends the maneuvering target point information of each stage to all the slave UUV.
Step seven: and the master UUV judges whether all the slave UUV receive the information of the four maneuvering target points. And if the master UUV judges that all the slave UUV receive the coordinates of the column scattered points, the coordinates of the row scattered points, the coordinates of the column maneuvering points and the coordinates of the maneuvering points within 120 seconds, turning to the step eight, and if the master UUV does not receive the information of the four maneuvering target points, turning to the step six.
Step eight: the master UUV sends a column scatter instruction to all slave UUVs.
Step nine: column scatter is done from UUV. All the slave UUV move from the current position to the column dispersion target point, and each slave UUV informs the master UUV after completing the column dispersion.
As shown in fig. 4, since the slave UUV No. 4 is in the same column region as the slave UUV No. 3, the slave UUV No. 4 needs to move from the current position point (dotted line) to the column dispersion target point (solid line) of the slave UUV No. 4, so that the slave UUVs No. 3 and No. 4 are in different column regions, and the other slave UUVs remain unchanged in position.
Step ten: and the master UUV judges whether all the slave UUV complete the column dispersion. And if the master UUV judges that all slave UUV complete column dispersion within 120 seconds, turning to the eleventh step, otherwise, turning to the twenty-one step, wherein formation of the formation is considered to fail.
Step eleven: the master UUV sends a line scatter instruction to all slave UUVs.
Step twelve: line scatter is done from UUV. And all the slave UUV move from the current position to the line dispersion target point, and each slave UUV informs the master UUV after completing the line dispersion.
As shown in fig. 5, the slave UUV No. 1 is in the same row area as the slave UUV No. 2, so that the slave UUV No. 1 needs to move from the current position point (at the dotted line) to the row scattered target point of the slave UUV No. 1 (at the solid line), and the other slave UUVs keep the original positions.
Step thirteen: and the master UUV judges whether all the slave UUV complete the line dispersion. And if the master UUV judges that all slave UUV complete line dispersion within 120 seconds, turning to the fourteen step, otherwise, turning to the twenty-one step, wherein formation of the formation is considered to fail.
Fourteen steps: the master UUV sends column maneuver instructions to all slave UUVs.
Step fifteen: the slave UUV performs a train maneuver. All the slave UUV move to the maneuver target point from the current position, and each slave UUV informs the master UUV after the maneuver is completed.
As shown in fig. 6, the slave UUV No. 1 needs to move from the current position point (at the dotted line) to the column maneuvering target point (at the solid line) of the slave UUV No. 1, and the other slave UUVs do the same.
Sixthly, the steps are as follows: and the master UUV judges whether all the slave UUV complete the column maneuver. And if the master UUV judges that all slave UUV complete the train maneuver within 120 seconds, turning to the seventeen step, otherwise, turning to the twenty-one step, wherein the formation of the formation is considered to fail.
Seventeen steps: the master UUV sends a mobile command to all slave UUVs.
Eighteen steps: the slave UUV performs a maneuver. All the slave UUV move from the current position to the mobile target point, and each slave UUV informs the master UUV after completing the mobile.
As shown in fig. 7, the slave UUV No. 1 needs to move from the current position point (at the dotted line) to the mechanical movement target point (at the solid line) of the slave UUV No. 1, and the other slave UUVs do the same.
Nineteen steps: the master UUV determines whether all slave UUVs have completed the maneuver. And if the master UUV judges that all slave UUV complete the running within 120 seconds, turning to the step twenty, otherwise, turning to the step twenty-one, wherein the formation is considered to fail.
Twenty steps: and the master UUV sends formation success instructions to all the slave UUV, all the UUV keep fixed points and fixed heading, and the step is transferred to twenty-two.
Twenty one is as follows: and the master UUV sends a formation failure instruction to all the slave UUV, and all the UUV stops.
Step twenty-two: and finishing formation of the UUV cluster formation queue.
The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention, and various modifications and changes may be made by those skilled in the art. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in 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
Figure FDA0002601060710000011
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
Figure FDA0002601060710000012
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
Figure FDA0002601060710000013
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:
Figure FDA0002601060710000014
Figure FDA0002601060710000015
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:
Figure FDA0002601060710000016
Figure FDA0002601060710000021
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)),
Figure FDA0002601060710000022
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,
Figure FDA0002601060710000023
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
Figure FDA0002601060710000031
Figure FDA0002601060710000032
Figure FDA0002601060710000033
Figure FDA0002601060710000034
Figure FDA0002601060710000035
Coordinates of line scatter points in a fixed coordinate system and
Figure FDA0002601060710000036
Figure FDA0002601060710000037
Figure FDA0002601060710000038
Figure FDA0002601060710000039
Figure FDA00026010607100000310
coordinates of train maneuvering target point under fixed coordinate system
Figure FDA00026010607100000311
Figure FDA00026010607100000312
Coordinates of mobile maneuvering target point under fixed coordinate system
Figure FDA00026010607100000313
Figure FDA0002601060710000041
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.
CN202010724118.4A 2020-07-24 2020-07-24 Large-scale UUV cluster formation method based on grid method Active CN111930116B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010724118.4A CN111930116B (en) 2020-07-24 2020-07-24 Large-scale UUV cluster formation method based on grid method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010724118.4A CN111930116B (en) 2020-07-24 2020-07-24 Large-scale UUV cluster formation method based on grid method

Publications (2)

Publication Number Publication Date
CN111930116A CN111930116A (en) 2020-11-13
CN111930116B true CN111930116B (en) 2022-10-14

Family

ID=73315472

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010724118.4A Active CN111930116B (en) 2020-07-24 2020-07-24 Large-scale UUV cluster formation method based on grid method

Country Status (1)

Country Link
CN (1) CN111930116B (en)

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104685432A (en) * 2012-05-01 2015-06-03 5D机器人公司 Distributed positioning and collaborative behavior determination
CN106950970A (en) * 2017-05-05 2017-07-14 东华大学 A kind of multirobot collaboration formation method based on client server architecture
CN107133416A (en) * 2017-05-24 2017-09-05 西北工业大学 A kind of multi-level confusion type Immune agent structure modelling methods of UUV
WO2017210365A1 (en) * 2016-05-31 2017-12-07 Lockheed Martin Corporation Magneto-optical detecting apparatus and methods
CN108663939A (en) * 2018-06-12 2018-10-16 哈尔滨工程大学 Consider the UUV level of constellation face path tracking control method of communication packet loss
CN108663938A (en) * 2018-05-25 2018-10-16 哈尔滨工程大学 A kind of UUV cluster-coordinator control methods considering communication topological transformation
CN108681321A (en) * 2018-04-10 2018-10-19 华南理工大学 A kind of undersea detection method that unmanned boat collaboration is formed into columns
CN108829132A (en) * 2018-06-12 2018-11-16 哈尔滨工程大学 A kind of more UUV space maneuver control methods considering large dead time
CN110580057A (en) * 2019-09-26 2019-12-17 哈尔滨工程大学 UUV cluster formation method based on circumference layered planning
CN110609552A (en) * 2019-09-12 2019-12-24 哈尔滨工程大学 Method for planning formation plane flight path of underwater unmanned aircraft

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20040068415A1 (en) * 2002-04-22 2004-04-08 Neal Solomon System, methods and apparatus for coordination of and targeting for mobile robotic vehicles
EP3211447B1 (en) * 2011-05-06 2020-09-30 Hadal, Inc. Systems and methods for synthetic aperture sonar
CN103863948B (en) * 2014-03-24 2015-09-30 合肥工业大学 A kind of crane works in coordination with the Navigation Control Unit of carrying and the method for Navigation Control
US10183732B2 (en) * 2015-04-09 2019-01-22 University of New Hamphire Pose detection and control of unmanned underwater vehicles (UUVs) utilizing an optical detector array
DE102015212932A1 (en) * 2015-07-10 2017-01-12 Kuka Roboter Gmbh Method for controlling a robot and / or an autonomous driverless transport system
KR101695557B1 (en) * 2015-07-17 2017-01-24 고려대학교 산학협력단 Automated guided vehicle system based on autonomous mobile technique and a method for controlling the same
CN111207754A (en) * 2020-02-28 2020-05-29 上海交通大学 Multi-robot formation positioning method and robot equipment based on particle filter

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104685432A (en) * 2012-05-01 2015-06-03 5D机器人公司 Distributed positioning and collaborative behavior determination
WO2017210365A1 (en) * 2016-05-31 2017-12-07 Lockheed Martin Corporation Magneto-optical detecting apparatus and methods
CN106950970A (en) * 2017-05-05 2017-07-14 东华大学 A kind of multirobot collaboration formation method based on client server architecture
CN107133416A (en) * 2017-05-24 2017-09-05 西北工业大学 A kind of multi-level confusion type Immune agent structure modelling methods of UUV
CN108681321A (en) * 2018-04-10 2018-10-19 华南理工大学 A kind of undersea detection method that unmanned boat collaboration is formed into columns
CN108663938A (en) * 2018-05-25 2018-10-16 哈尔滨工程大学 A kind of UUV cluster-coordinator control methods considering communication topological transformation
CN108663939A (en) * 2018-06-12 2018-10-16 哈尔滨工程大学 Consider the UUV level of constellation face path tracking control method of communication packet loss
CN108829132A (en) * 2018-06-12 2018-11-16 哈尔滨工程大学 A kind of more UUV space maneuver control methods considering large dead time
CN110609552A (en) * 2019-09-12 2019-12-24 哈尔滨工程大学 Method for planning formation plane flight path of underwater unmanned aircraft
CN110580057A (en) * 2019-09-26 2019-12-17 哈尔滨工程大学 UUV cluster formation method based on circumference layered planning

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
A Formation-Constrained Cooperative Path Planning Method for Multi-Autonomous Underwater Vehicles;Shuangshuang DU,等;《2017 International Conference on Computing Intelligence and Information System》;20171231;第313-319页 *
基于人工势场法的AUV避障算法研究综述;郭银景，等;《计算机工程与应用》;20200130;第16-23页 *

Also Published As

Publication number Publication date
CN111930116A (en) 2020-11-13

Similar Documents

Publication Publication Date Title
Shen et al. Research on large-scale additive manufacturing based on multi-robot collaboration technology
CN107491086B (en) Unmanned aerial vehicle formation obstacle avoidance method and system under time-varying network topology
CN108415425B (en) Distributed swarm robot cooperative clustering algorithm based on improved gene regulation and control network
CN106020189B (en) Vacant lot heterogeneous robot system paths planning method based on neighborhood constraint
CN110580057B (en) UUV cluster formation method based on circumference layered planning
CN108829113B (en) Self-adaptive zero-space behavior fusion method for multi-robot formation
WO2018176596A1 (en) Unmanned bicycle path planning method based on weight-improved particle swarm optimization algorithm
CN106502250A (en) The path planning algorithm of multi-robot formation in three dimensions
CN110308740A (en) A kind of unmanned aerial vehicle group dynamic task allocation method towards mobile target tracking
CN108413963B (en) Self-learning ant colony algorithm-based strip robot path planning method
CN110231821B (en) Improved self-adaptive zero-space behavior fusion method for multi-robot formation
CN108073185A (en) Multiple no-manned plane reaches cooperative control method simultaneously
CN110162053A (en) The adaptive behavior fusion method that the more unmanned boats of isomery are formed into columns
CN113156954B (en) Multi-agent cluster obstacle avoidance method based on reinforcement learning
CN112731942A (en) Multi-AUV formation control method based on improved navigator virtual structure method
Shang et al. Collaborative path planning of multiple carrier-based aircraft based on multi-agent reinforcement learning
Wang et al. Intelligent welding robot path optimization based on discrete elite PSO
CN114428516A (en) Unmanned aerial vehicle cluster obstacle avoidance method
CN108958202B (en) Multi-robot collaborative exploration method
CN111930116B (en) Large-scale UUV cluster formation method based on grid method
Pohan et al. Path Planning using Combined Informed Rapidly-exploring Random Tree Star and Particle Swarm Optimization Algorithms
CN111007848B (en) Multi-agent cooperative operation control method based on bounded space
CN117193359A (en) Optimal formation control method of four-rotor unmanned aerial vehicle based on non-cooperative differential game
CN114454178B (en) Heterogeneous integrated manufacturing-oriented double-robot collaborative path planning method and system
CN114625134B (en) Rapid control method for ship navigation situation

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant