An improved unmanned vessel swarm mission planning method based on CAPT algorithm

Abstract

The present invention proposes a path planning method for unmanned ship clusters under complex tasks based on an improved CAPT algorithm. The method includes the following contents: In view of the fact that the CAPT algorithm cannot solve the cluster path planning problem of complex paths and obstacle avoidance, the Theta* path algorithm is introduced into the cluster path planning, which solves the obstacle avoidance problem of the unmanned ship cluster, and finally realizes the motion planning problem of cluster formation and arbitrary assignment of target positions. The present invention uses the CAPT algorithm based on the unlabeled problem, and introduces the Theta* algorithm to perform path planning when the unmanned ship encounters obstacles. The present invention aims at the problem of unmanned ship cluster task planning under complex sea conditions with surface obstacles, and proposes to use the Theta* algorithm for updating the distance cost matrix in the CAPT algorithm. In the presence of obstacles, the Theta* algorithm is used to perform unmanned ship path planning and update the distance cost matrix, thereby achieving the effect of using the improved CAPT algorithm for unmanned ship cluster task planning, and providing a new idea for the problem of unmanned ship cluster.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned vessel clusters, and in particular relates to an unmanned vessel cluster task planning method based on an improved CAPT algorithm. Background Art

[0002] In recent years, with the continuous development of technologies such as fluid dynamics, automatic control, and unmanned equipment system engineering design, as well as the explosive growth in demand for unmanned equipment in exploration, search, patrol, and surveillance applications, unmanned systems have become a cutting-edge technology that the world's major maritime science and technology powers are currently competing to advance. As unmanned maritime transport platforms, unmanned vessels can perform long-term, low-cost, and large-scale marine scientific research and engineering missions. They have extremely broad potential applications in the civilian sector, such as port patrols, territorial waters surveillance, maritime search and rescue, marine environmental monitoring, seabed resource exploration, and nautical charting.

[0003] Unmanned surface vehicles or USVs are ships-based and carry special functional equipment including control equipment and communication equipment, which can perform certain special operations on the water. The control operation of unmanned ships is mainly completed by remote mother ships on the sea and base stations on land. The control center can grasp the various data and information collected by the unmanned ships through radio and other communication devices. Compared with manually controlled ships, unmanned ships have unique advantages such as high speed, intelligence and modularity. In the military, unmanned ships can be used in occasions including reconnaissance, search and rescue, patrol and surveillance; in the civilian field, the application scope of unmanned ships is even wider, including sea surface environment monitoring, underwater target search, disaster warning, marine garbage cleaning and other aspects.

[0004] A single vessel is typically unable to complete complex missions. In contrast, a coordinated multi-vessel formation offers multiple advantages in maritime search and rescue, marine resource exploration and environmental monitoring, and fleet coordinated operations, including robust fault tolerance, adaptability, and high efficiency. Therefore, research into control methods for multi-vessel formations is essential, enabling effective control of these formations. This approach holds broad application potential and promises in areas such as military defense, production, and transportation. Specific applications include formation detection, ocean-going replenishment, and fleet coordinated operations.

[0005] As unmanned vessels undertake increasingly diverse tasks and the actual marine environment becomes increasingly complex and volatile, individual unmanned vessels often face shortcomings such as low carrying capacity, limited coverage, and weak information processing capabilities. However, multi-vessel collaborative fleets offer multiple advantages in maritime search and rescue, marine resource exploration and environmental monitoring, and fleet coordinated operations, including strong fault tolerance, excellent adaptability, and high execution efficiency. By conducting necessary research on fleet control methods and achieving effective control of multi-vessel formations, these fleets have broad applications and promising development prospects in fields such as military defense, production, and transportation. Summary of the Invention

[0006] Technical Problem: To address the need for multiple unmanned vessels to change formations and complete swarm mission planning in complex sea conditions, this paper proposes an improved unmanned vessel swarm mission planning method based on the CAPT algorithm. This method addresses the CAPT algorithm's inability to avoid obstacles during trajectory planning. By incorporating the Theta* algorithm into the CAPT algorithm, the method enables path planning for unmanned vessels encountering obstacles. This method broadens the application of the CAPT algorithm in unmanned vessel swarming.

[0007] Technical solution: The present invention proposes an unmanned vessel swarm mission planning method based on an improved CAPT algorithm, which includes the following steps:

[0008] A. Import the map and the start and end points of the homogeneous unmanned boat, define the position state vector X(t) and the target state vector G of the unmanned boat, including the following steps:

[0009] (1) Load a binary chart (800*600) with obstacles, convert the binary chart into a matrix containing only 0 and 1, where the points with obstacles are 1 and the points without obstacles are 0. Set the length of the unmanned ship to L, which is the minimum collision avoidance radius. The size of the clustered unmanned ships is the same as the number of target positions of the unmanned ships, which is N. The unmanned ship cluster planning task to be completed is to find the optimal allocation matrix and collision-free trajectory of each unmanned ship to allocate N homogeneous unmanned ships from N starting points to N end points, and ensure that there is no collision between the unmanned ships and between the unmanned ships and obstacles; read the coordinates of the N starting points and the N end points to be allocated of the N homogeneous unmanned ships, and define the initial position of the i-th unmanned ship at time t=0 as x i (0)∈R n , the jth target position is g j ∈R n ;

[0010] (2) Define the position state vector X(t) and target state vector G of the unmanned ship at time t. Let the system state vector X(t) = [x1(t) T ,x2(t)T ,…,x N (t) T ], the system target state vector G=[g1 T ,g2 T ,…,g N T ], where x i (t) is the position coordinate of the i-th ship at time t, and T is the matrix transpose symbol;

[0011] B. Define the unmanned ship target allocation matrix And initialize, the target allocation matrix is used to allocate N unmanned ships from their respective starting points to N target locations;

[0012] Define the target allocation matrix Assign N unmanned ships from their respective starting points to N target locations: where the target assignment matrix The element in row i and column j of express: If the unmanned ship i starts from the starting point x i Assigned to the jth target position g j , 1, otherwise 0; Initialize to an identity matrix of size N*N; that is:

[0013]

[0014] The above matrix There are N 1s in total;

[0015] Define the augmented allocation matrix Among them, I n is the N*N identity matrix, represents the Kronecker product;

[0016] C. Calculate the trajectory distance y from the starting point of any i-th ship to any j-th end point i,j or d i,j The distance matrix D of the N unmanned ships' assigned trajectories is obtained, where i = 1, 2...N; j = 1, 2...N, and the element D in the i-th row and j-th column of the matrix D is i,j ∈R N*N , which includes the following steps:

[0017] (1) According to the sight(i,j) function, determine whether there is an obstacle between the starting point of the i-th ship and the j-th end point. The specific process is as follows:

[0018] 1) First calculate the slope of the line between the starting point of the i-th ship and the j-th end point coordinates k = (y1-y2) / (x1-x2);

[0019] 2) If |k|<1, then the horizontal coordinate x is decreased or increased by 1 unit starting from the starting coordinate, that is, a point a is taken every 1 unit on the 800*600 map in step A. k Until the end coordinate is reached; if |k|>1, the ordinate y is decreased or increased by 1 unit starting from the starting coordinate, that is, a point a is taken every 1 unit. k Until the end coordinate is reached, point a is judged in turn k Is the value at 0?

[0020] 3) If all points a in step 2) k If the values are all 0, there is no obstacle between the starting point i and the end point j, and the sight() function returns 0; if there is point a in step 2) k If the value of is 1, there is an obstacle between the starting point i and the end point j, and the sight() function returns a value of 1;

[0021] (2) According to the return value of the sight(i,j) function in (1), the trajectory distance from the i-th unmanned ship to the j-th destination is calculated: When there is no obstacle between the starting point of the unmanned ship i and the destination j, the Euclidean distance is directly calculated based on the straight line between the starting point and the destination, that is, the straight line trajectory distance is: d i,j =||x i (0)-g j || 2 When there is an obstacle between the starting point i and the end point j, the optimal collision-free trajectory from the starting point i to the end point j is planned in the binary graph according to the Theta* algorithm, and the trajectory y composed of the process points is returned. i,j , and calculate the trajectory distance and ||y i,j ||;

[0022] Among them, according to the signt(i,j) function, it is judged that there is an obstacle between the starting point and the end point of the unmanned boat. When the return value is 1, the steps for obtaining the unmanned boat path planning trajectory according to the Theta* algorithm are as follows:

[0023] Assume that the starting point of the current unmanned boat path planning is start, the target point is goal, the evaluation function of the unmanned boat from the starting point to the target point through any node n is f(n), the actual distance from the starting point to any node n is g(n), and the estimated distance from node n to the target point is h(n); OPEN LIST: save unchecked nodes to this list; CLOSE LIST: save checked nodes to this list;

[0024] 1) Starting from the starting point of the unmanned ship, add start as the first node to be checked and add it to the OPEN LIST list;

[0025] 2) Put the path nodes that can be reached around the starting point of the unmanned boat into the OPEN LIST list, and set the node start as the parent node of these nodes. The maximum number of path nodes that can be reached around is eight. If there are obstacle nodes, they are unreachable.

[0026] 3) Take the starting point start from the OPEN LIST list and put it into the CLOSE LIST list;

[0027] 4) Calculate the f(n) value of each node around the node where the unmanned ship is currently located, that is:

[0028] f(n)=g(n)+h(n)

[0029] 5) Select node a with the lowest f(n) value from the OPEN LIST, remove node a from the OPEN LIST, and put it into the CLOSE LIST;

[0030] 6) Check all the reachable nodes around the current unmanned ship node a, exclude the obstacle nodes and the nodes already in the CLOSELIST list, search for the next adjacent candidate node and consider the following points:

[0031] a) If the surrounding nodes of node a have not been added to the OPEN LIST list, add them to the OPEN LIST list, calculate their f(n), g(n), and h(n) values, and set node a as their parent node;

[0032] b) If a neighboring node c of node a is already in the OPEN LIST set, calculate the g(n) value of the new path of the unmanned boat from the starting point start via node a to node c, and determine whether the unmanned boat needs to update the node based on the g(n) value: if the g(n) value of the new path of the unmanned boat from the starting point start via node a to node c is smaller than the original g(n) value of node c, then change the parent node of the unmanned boat to node a, recalculate the f(n) value, and keep h(n) unchanged; if the g(n) value of the new path is larger than the original g(n) value of node c, then g(n) is not changed;

[0033] 7) Repeat steps 4), 5), and 6 in a loop;

[0034] 8) End judgment: When the destination goal of the unmanned ship appears in the OPEN LIST, it means that the shortest path from the starting point start to the destination goal of the unmanned ship has been found. Starting from the destination goal, each node moves along the parent node to the starting point of the unmanned ship to form the path of the unmanned ship and return the trajectory y of the unmanned ship. i,j As well as trajectory distance and ||y i,j||; When OPENLIST is empty, it means that no suitable path is found;

[0035] That is to say, the element d in the N*N distance cost matrix is obtained i,j for:

[0036]

[0037] The N*N distance matrix D is:

[0038]

[0039] D. Set the unmanned ship collision avoidance constraints and solve the optimal unmanned ship task allocation matrix φ* based on the unmanned ship allocation distance matrix D in step C;

[0040] Define δ(t) as the distance between any two unmanned ships x i and x j The minimum spacing at any time t:

[0041]

[0042] Among them, i≠j∈N,t∈[t0,t f ], t0 is the starting time, t f is the termination time, inf function is the lower bound function of the minimum value, and L is the length of the unmanned ship model;

[0043] In order to ensure collision avoidance for all unmanned ships, it is required that δ(t) always satisfies the constraint condition, that is, the gap is always greater than zero:

[0044] δ(t)>0,t∈[t0,t f ]

[0045] In the unmanned ship swarm mission planning, the purpose of the CAPT algorithm is to find a specific cost function. The optimal trajectory y*(t) when minimized is:

[0046]

[0047] Among them, the C(y(t)) function is the distance cost of the unmanned ship trajectory;

[0048] The CAPT algorithm treats the unlabeled problem of unmanned vessel swarm mission planning as an optimization problem and solves it to determine the optimal allocation plan and trajectory. The optimization objective function is:

[0049]

[0050] Where Φ is the augmented allocation matrix in step B, is x i(t), y(t) is the first-order derivative of each unmanned ship's trajectory, that is, to find the optimal allocation matrix and the optimal trajectory, so that the total trajectory cost of each unmanned ship is the lowest. The solution to the problem consists of a straight line trajectory and a trajectory connecting the midpoints planned by Theta*, and the sum of the squares of the distances traveled by each unmanned ship is the smallest. According to the unmanned ship trajectory distance matrix in step C, the optimal allocation is obtained to minimize the total path length. Therefore, the optimal allocation solution is:

[0051]

[0052] The Hungarian algorithm is used to solve the problem of finding the optimal distance square assignment matrix φ*, as follows:

[0053] The steps of the Hungarian algorithm to solve the optimal distance square distribution matrix φ* based on the unmanned ship distance cost matrix D are as follows:

[0054] 1) The distance cost matrix D for N unmanned ship mission assignments = (d ij ) N*N First, perform a row transformation, subtract the minimum value of each row from the row, and then perform a column transformation, subtract the minimum value of each column from the column, and the elements with a value of zero appear;

[0055] 2) Cover all zero elements in the transformed matrix D in 1) with the least number of horizontal or vertical lines;

[0056] 3) If the total number of horizontal and vertical lines k in 2) is less than N, find the smallest uncovered element a, subtract a from each uncovered row, and add a to each covered column;

[0057] 4) Repeat steps 2) and 3) until k=N;

[0058] 5) Determine the optimal task allocation based on the location of the zero element and solve the optimal allocation matrix φ*;

[0059] E. When allocating tasks according to the optimal allocation matrix φ* obtained in step D, calculate the end time t of all unmanned ships f and the final trajectory y*(t);

[0060] All unmanned ships reach the target location within a certain time, and the end time t at the final location is f for:

[0061]

[0062] In the above formula, v max Define a polynomial function of time for the speed of the fastest unmanned ship Satisfying δ(0)=0,δ(t f )=1,

[0063] The parameter α is:

[0064]

[0065] The trajectory that minimizes the square integral of the velocity is the trajectory with constant velocity and satisfies the constraints at the beginning and end: y*(t)=(1-δ(t))X(0)+δ(t)ΦG.

[0066] Beneficial effects: Compared with the prior art, the technical solution of the present invention has the following beneficial technical effects:

[0067] This paper studies the problem of multi-unmanned vessel cluster task planning. It uses the CAPT algorithm based on the unlabeled problem to solve the motion planning problem of a cluster composed of multiple homogeneous unmanned vessels and where the target positions can be arbitrarily assigned. In addition, the Theta* algorithm is introduced into the CAPT algorithm to address the problem of cluster obstacle avoidance, which the CAPT algorithm cannot achieve. This allows the unmanned vessel to perform path planning when encountering obstacles. Experiments show that the unmanned vessel cluster task planning method based on the CAPT algorithm can achieve good results in both obstacle-free and obstacle-filled nautical charts. It can achieve task allocation and collision-free trajectory generation for multiple unmanned vessels from N starting points to N end points. This shows that combining the Theta* algorithm with the CAPT algorithm to expand the use scenarios of surface unmanned vessel cluster task planning is feasible and effective. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] Figure 1 It is a flow chart of the unmanned ship cluster task planning method based on the CAPT algorithm of the present invention. DETAILED DESCRIPTION

[0069] The present invention proposes an unmanned vessel swarm mission planning method based on an improved CAPT algorithm, which comprises the following steps:

[0070] A. Import the map and the start and end points of the homogeneous unmanned boat, define the position state vector X(t) and the target state vector G of the unmanned boat, including the following steps:

[0071] (1) Load a binary chart (800*600) with obstacles, convert the binary chart into a matrix containing only 0 and 1, where the points with obstacles are 1 and the points without obstacles are 0. Set the length of the unmanned ship to L, which is the minimum collision avoidance radius. The size of the clustered unmanned ships is the same as the number of target positions of the unmanned ships, which is N. The unmanned ship cluster planning task to be completed is to find the optimal allocation matrix and collision-free trajectory of each unmanned ship to allocate N homogeneous unmanned ships from N starting points to N end points, and ensure that there is no collision between the unmanned ships and between the unmanned ships and obstacles; read the coordinates of the N starting points and the N end points to be allocated of the N homogeneous unmanned ships, and define the initial position of the i-th unmanned ship at time t=0 as x i (0)∈R n , the jth target position is g j ∈R n ;

[0072] (2) Define the position state vector X(t) and target state vector G of the unmanned ship at time t. Let the system state vector X(t) = [x1(t) T ,x2(t) T ,…,x N (t) T ], the system target state vector G=[g1 T ,g2 T ,…,g N T ], where x i (t) is the position coordinate of the i-th ship at time t, and T is the matrix transpose symbol;

[0073] B. Define the unmanned ship target allocation matrix And initialize, the target allocation matrix is used to allocate N unmanned ships from their respective starting points to N target locations;

[0074] Define the target allocation matrix Assign N unmanned ships from their respective starting points to N target locations: where the target assignment matrix The element in row i and column j of express: If the unmanned ship i starts from the starting point x i Assigned to the jth target position g j , 1, otherwise 0; Initialize to an identity matrix of size N*N; that is:

[0075]

[0076] The above matrix There are N 1s in total;

[0077] Define the augmented allocation matrix Among them, I n is the N*N identity matrix, represents the Kronecker product;

[0078] C. Calculate the trajectory distance y from the starting point of any i-th ship to any j-th end point i,j or d i,j The distance matrix D of the N unmanned ships' assigned trajectories is obtained, where i = 1, 2...N; j = 1, 2...N, and the element D in the i-th row and j-th column of the matrix D is i,j ∈R N*N , which includes the following steps:

[0079] (1) According to the sight(i,j) function, determine whether there is an obstacle between the starting point of the i-th ship and the j-th end point. The specific process is as follows:

[0080] 1) First calculate the slope of the line between the starting point of the i-th ship and the j-th end point coordinates k = (y1-y2) / (x1-x2);

[0081] 2) If |k|<1, then the horizontal coordinate x is decreased or increased by 1 unit starting from the starting coordinate, that is, a point a is taken every 1 unit on the 800*600 map in step A. k Until the end coordinate is reached; if |k|>1, the ordinate y is decreased or increased by 1 unit starting from the starting coordinate, that is, a point a is taken every 1 unit. k Until the end coordinate is reached, point a is judged in turn k Is the value at 0?

[0082] 3) If all points a in step 2) k If the values are all 0, there is no obstacle between the starting point i and the end point j, and the sight() function returns 0; if there is point a in step 2) k If the value of is 1, there is an obstacle between the starting point i and the end point j, and the sight() function returns a value of 1;

[0083] (2) According to the return value of the sight(i,j) function in (1), the trajectory distance from the i-th unmanned ship to the j-th destination is calculated: When there is no obstacle between the starting point of the unmanned ship i and the destination j, the Euclidean distance is directly calculated based on the straight line between the starting point and the destination, that is, the straight line trajectory distance is: d i,j =||x i (0)-g j || 2 When there is an obstacle between the starting point i and the end point j, the optimal collision-free trajectory from the starting point i to the end point j is planned in the binary graph according to the Theta* algorithm, and the trajectory y composed of the process points is returned.i,j , and calculate the trajectory distance and ||y i,j ||;

[0084] Among them, according to the signt(i,j) function, it is judged that there is an obstacle between the starting point and the end point of the unmanned boat. When the return value is 1, the steps for obtaining the unmanned boat path planning trajectory according to the Theta* algorithm are as follows:

[0085] Assume that the starting point of the current unmanned boat path planning is start, the target point is goal, the evaluation function of the unmanned boat from the starting point to the target point through any node n is f(n), the actual distance from the starting point to any node n is g(n), and the estimated distance from node n to the target point is h(n); OPEN LIST: save unchecked nodes to this list; CLOSE LIST: save checked nodes to this list;

[0086] 1) Starting from the starting point of the unmanned ship, add start as the first node to be checked and add it to the OPEN LIST list;

[0087] 2) Put the path nodes that can be reached around the starting point of the unmanned boat into the OPEN LIST list, and set the node start as the parent node of these nodes. The maximum number of path nodes that can be reached around is eight. If there are obstacle nodes, they are unreachable.

[0088] 3) Take the starting point start from the OPEN LIST list and put it into the CLOSE LIST list;

[0089] 4) Calculate the f(n) value of each node around the node where the unmanned ship is currently located, that is:

[0090] f(n)=g(n)+h(n)

[0091] 5) Select node a with the lowest f(n) value from the OPEN LIST, remove node a from the OPEN LIST, and put it into the CLOSE LIST;

[0092] 6) Check all the reachable nodes around the current unmanned ship node a, exclude the obstacle nodes and the nodes already in the CLOSELIST list, search for the next adjacent candidate node and consider the following points:

[0093] a) If the surrounding nodes of node a have not been added to the OPEN LIST list, add them to the OPEN LIST list, calculate their f(n), g(n), and h(n) values, and set node a as their parent node;

[0094] b) If a neighboring node c of node a is already in the OPEN LIST set, calculate the g(n) value of the new path of the unmanned boat from the starting point start via node a to node c, and determine whether the unmanned boat needs to update the node based on the g(n) value: if the g(n) value of the new path of the unmanned boat from the starting point start via node a to node c is smaller than the original g(n) value of node c, then change the parent node of the unmanned boat to node a, recalculate the f(n) value, and keep h(n) unchanged; if the g(n) value of the new path is larger than the original g(n) value of node c, then g(n) is not changed;

[0095] 7) Repeat steps 4), 5), and 6 in a loop;

[0096] 8) End judgment: When the destination goal of the unmanned ship appears in the OPEN LIST, it means that the shortest path from the starting point start to the destination goal of the unmanned ship has been found. Starting from the destination goal, each node moves along the parent node to the starting point of the unmanned ship to form the path of the unmanned ship and return the trajectory y of the unmanned ship. i,j As well as trajectory distance and ||y i,j ||; When OPENLIST is empty, it means that no suitable path is found;

[0097] That is to say, the element d in the N*N distance cost matrix is obtained i,j for:

[0098]

[0099] The N*N distance matrix D is:

[0100]

[0101] D. Set the unmanned ship collision avoidance constraints and solve the optimal unmanned ship task allocation matrix φ* based on the unmanned ship allocation distance matrix D in step C;

[0102] Define δ(t) as the distance between any two unmanned ships x i and x j The minimum spacing at any time t:

[0103]

[0104] Among them, i≠j∈N,t∈[t0,t f ], t0 is the starting time, t f is the termination time, inf function is the lower bound function of the minimum value, and L is the length of the unmanned ship model;

[0105] In order to ensure collision avoidance for all unmanned ships, it is required that δ(t) always satisfies the constraint condition, that is, the gap is always greater than zero:

[0106] δ(t)>0,t∈[t0,t f ]

[0107] In the unmanned ship swarm mission planning, the purpose of the CAPT algorithm is to find a specific cost function. The optimal trajectory y*(t) when minimized is:

[0108]

[0109] Among them, the C(y(t)) function is the distance cost of the unmanned ship trajectory;

[0110] The CAPT algorithm treats the unlabeled problem of unmanned vessel swarm mission planning as an optimization problem and solves it to determine the optimal allocation plan and trajectory. The optimization objective function is:

[0111]

[0112] Where Φ is the augmented allocation matrix in step B, is x i (t), y(t) is the first-order derivative of each unmanned ship's trajectory, that is, to find the optimal allocation matrix and the optimal trajectory, so that the total trajectory cost of each unmanned ship is the lowest. The solution to the problem consists of a straight line trajectory and a trajectory connecting the midpoints planned by Theta*, and the sum of the squares of the distances traveled by each unmanned ship is the smallest. According to the unmanned ship trajectory distance matrix in step C, the optimal allocation is obtained to minimize the total path length. Therefore, the optimal allocation solution is:

[0113]

[0114] The Hungarian algorithm is used to solve the problem of finding the optimal distance square assignment matrix φ*, as follows:

[0115] The steps of the Hungarian algorithm to solve the optimal distance square distribution matrix φ* based on the unmanned ship distance cost matrix D are as follows:

[0116] 1) The distance cost matrix D for N unmanned ship mission assignments = (d ij ) N*N First, perform a row transformation, subtract the minimum value of each row from the row, and then perform a column transformation, subtract the minimum value of each column from the column, and the elements with a value of zero appear;

[0117] 2) Cover all zero elements in the transformed matrix D in 1) with the least number of horizontal or vertical lines;

[0118] 3) If the total number of horizontal and vertical lines k in 2) is less than N, find the smallest uncovered element a, subtract a from each uncovered row, and add a to each covered column;

[0119] 4) Repeat steps 2) and 3) until k=N;

[0120] 5) Determine the optimal task allocation based on the location of the zero element and solve the optimal allocation matrix φ*;

[0121] E. When allocating tasks according to the optimal allocation matrix φ* obtained in step D, calculate the end time t of all unmanned ships f and the final trajectory y*(t);

[0122] All unmanned ships reach the target location within a certain time, and the end time t at the final location is f for:

[0123]

[0124] In the above formula, v max Define a polynomial function of time for the speed of the fastest unmanned ship Satisfying δ(0)=0,δ(t f )=1, where the constant k is an integer and k>2;

[0125] Among them, the parameter α is:

[0126]

[0127] The trajectory that minimizes the square integral of the velocity is the trajectory with constant velocity and satisfies the constraints at the beginning and end: y*(t)=(1-δ(t))X(0)+δ(t)ΦG;

[0128] Among them, X(0) is the position state vector of N unmanned ships at the initial time t=0.

Claims

1. An improved unmanned vessel swarm mission planning method based on the CAPT algorithm, characterized in that: The method comprises the following steps: A. Import the map and the start and end points of the homogeneous unmanned boat, and define the position state vector X(t) and target state vector G of the unmanned boat; B. Define the unmanned ship target allocation matrix And initialize, the target allocation matrix is used to allocate N unmanned ships from their respective starting points to N target locations; C. Calculate the trajectory distance y from the starting point of any i-th ship to any j-th end point i,j or d i,j The distance matrix D of the N unmanned ships' assigned trajectories is obtained, where i = 1, 2...N; j = 1, 2...N, and the element D in the i-th row and j-th column of the matrix D is i,j ∈R N*N ; D. Set the unmanned ship collision avoidance constraints and solve the optimal unmanned ship task allocation matrix φ based on the unmanned ship allocation distance matrix D in step C. * ; E. According to the optimal allocation matrix φ obtained in step D * When assigning tasks, calculate the end time t of all unmanned ships f and the final trajectory y * (t); The method of step C is as follows: (1) According to the sight(i,j) function, determine whether there is an obstacle between the starting point of the i-th ship and the j-th end point. The specific process is as follows: 1) First calculate the slope of the line between the starting point of the i-th ship and the j-th end point coordinates k = (y1-y2) / (x1-x2); 2) If |k|<1, then the horizontal coordinate x decreases or increases by 1 unit starting from the starting coordinate, that is, take a point a every 1 unit on the map in step A. k , until it reaches the end coordinate; if |k|>1, then the ordinate y decreases or increases by 1 unit starting from the starting coordinate, that is, take a point a every 1 unit k , until reaching the end coordinate, judge point a in turn k Is the value at 0? 3) If all points a in step 2) k If the values are all 0, there is no obstacle between the starting point i and the end point j, and the sight() function returns 0; if there is point a in step 2) k If the value of is 1, there is an obstacle between the starting point i and the end point j, and the sight() function returns a value of 1; (2) According to the return value of the sight(i,j) function in (1), the trajectory distance from the i-th unmanned ship to the j-th destination is calculated: When there is no obstacle between the starting point of the unmanned ship i and the destination j, the Euclidean distance is directly calculated based on the straight line between the starting point and the destination, that is, the straight line trajectory distance is: d i,j =||x i (0)-g j || 2 When there is an obstacle between the starting point i and the end point j, the optimal collision-free trajectory from the starting point i to the end point j is planned in the binary graph according to the Theta* algorithm, and the trajectory y composed of the process points is returned. i,j , and calculate the trajectory distance and ||y i,j ||; Among them, according to the signt(i,j) function, it is judged that there is an obstacle between the starting point and the end point of the unmanned boat. When the return value is 1, the steps for obtaining the unmanned boat path planning trajectory according to the Theta* algorithm are as follows: Assume that the starting point of the current unmanned boat path planning is start, the target point is goal, the evaluation function of the unmanned boat from the starting point to the target point through any node n is f(n), the actual distance from the starting point to any node n is g(n), and the estimated distance from node n to the target point is h(n); OPEN LIST: save unchecked nodes to this list; CLOSE LIST: save checked nodes to this list; 1) Starting from the starting point of the unmanned ship, add start as the first node to be checked and add it to the OPEN LIST list; 2) Put the path nodes that can be reached around the starting point of the unmanned boat into the OPEN LIST list, and set the node start as the parent node of these nodes. The maximum number of path nodes that can be reached around is eight. If there are obstacle nodes, they are unreachable. 3) Take the starting point start from the OPEN LIST list and put it into the CLOSE LIST list; 4) Calculate the f(n) value of each node around the node where the unmanned ship is currently located, that is: f(n)=g(n)+h(n) 5) Select node a with the lowest f(n) value from the OPEN LIST, remove node a from the OPEN LIST, and put it into the CLOSE LIST; 6) Check all the reachable nodes around the current unmanned ship node a, exclude the obstacle nodes and the nodes already in the CLOSE LIST, search for the next adjacent candidate node and consider the following points: a) If the surrounding nodes of node a have not been added to the OPEN LIST list, add them to the OPEN LIST list, calculate their f(n), g(n), and h(n) values, and set node a as their parent node; b) If a neighboring node c of node a is already in the OPEN LIST set, calculate the g(n) value of the new path of the unmanned boat from the starting point start via node a to node c, and determine whether the unmanned boat needs to update the node based on the g(n) value: if the g(n) value of the new path of the unmanned boat from the starting point start via node a to node c is smaller than the original g(n) value of node c, then change the parent node of the unmanned boat to node a, recalculate the f(n) value, and keep h(n) unchanged; if the g(n) value of the new path is larger than the original g(n) value of node c, then g(n) is not changed; 7) Repeat steps 4), 5), and 6 in a loop; 8) End judgment: When the destination goal of the unmanned ship appears in the OPEN LIST, it means that the shortest path from the starting point start to the destination goal of the unmanned ship has been found. Starting from the destination goal, each node moves along the parent node to the starting point of the unmanned ship to form the path of the unmanned ship and return the trajectory y of the unmanned ship. i,j and trajectory distance and ‖y i,j ‖; When OPEN LIST is empty, it means no suitable path is found; That is to say, the element d in the N*N distance cost matrix is obtained i,j for: The N*N distance matrix D is:

2. The unmanned vessel swarm mission planning method based on the improved CAPT algorithm according to claim 1 is characterized in that: The method for step A is as follows: (1) Load a map with obstacles in binary image format and convert the binary image into a matrix containing only 0 and 1. The point with an obstacle is 1 and the point without an obstacle is 0. Set the length of the unmanned ship to L, which is the minimum collision avoidance radius. The size of the clustered unmanned ships is the same as the number of target positions of the unmanned ships, which is N. The unmanned ship cluster planning task to be completed is to find the optimal allocation matrix and collision-free trajectory of each unmanned ship to allocate N homogeneous unmanned ships from N starting points to N end points, and ensure that there is no collision between the unmanned ships and between the unmanned ships and obstacles; read the coordinates of the N starting points and the N end points to be allocated of the N homogeneous unmanned ships, and define the initial position of the i-th unmanned ship at time t = 0 as x i (0)∈R n , the jth target position is g j ∈R n ; (2) Define the position state vector X(t) and target state vector G of the unmanned ship at time t. Let the system state vector X(t) = [x1(t) T ,x2(t) T ,…,x N (t) T ], the system target state vector G=[g1 T ,g2 T ,…,g N T ], where x i (t) is the position coordinate of the i-th ship at time t, and T is the matrix transpose symbol.

3. The unmanned vessel swarm mission planning method based on the improved CAPT algorithm according to claim 2 is characterized in that: The method of step B is as follows: Define the target allocation matrix Assign N unmanned ships from their respective starting points to N target locations: where the target assignment matrix The element in row i and column j of express: If the unmanned ship i starts from the starting point x i Assigned to the jth target position g j , 1, otherwise 0; Initialize to an identity matrix of size N*N; that is: The above matrix There are N 1s in total; Define the augmented allocation matrix Among them, I n is the N*N identity matrix, Represents the Kronecker product.

4. The unmanned vessel swarm mission planning method based on the improved CAPT algorithm according to claim 1 is characterized in that: The method for step D is as follows: Define δ(t) as the distance between any two unmanned ships x i and x j The minimum spacing at any time t: Among them, i≠j∈N,t∈[t0,t f ], t0 is the starting time, t f is the termination time, inf function is the lower bound function of the minimum value, and L is the length of the unmanned ship model. In order to ensure the collision avoidance of all unmanned ships, it is required that δ(t) always meets the constraint condition, that is, the gap is always greater than zero: δ(t)>0,t∈[t0,t f ] In the unmanned ship swarm mission planning, the purpose of the CAPT algorithm is to find a specific cost function. The optimal trajectory y when minimizing * (t), that is: Among them, the C(y(t)) function is the distance cost of the unmanned ship trajectory; The CAPT algorithm treats the unlabeled problem of unmanned vessel swarm mission planning as an optimization problem and solves it to determine the optimal allocation plan and trajectory. The optimization objective function is: Where Φ is the augmented allocation matrix in step B, is x i (t), y(t) is the first-order derivative of each unmanned ship's trajectory, that is, to find the optimal allocation matrix and the optimal trajectory so that the total trajectory distance of each unmanned ship is the lowest. The solution to the problem consists of a straight line trajectory and a trajectory connecting the midpoints planned by Theta*, and the sum of the squares of the distances traveled by each unmanned ship is the smallest. According to the unmanned ship trajectory distance matrix in step C, the optimal allocation is obtained so that the total path length is the lowest. Therefore, the optimal allocation solution is: Use the Hungarian algorithm to solve the optimal distance square assignment matrix φ * The problem to be solved is as follows: The Hungarian algorithm solves the optimal distance square distribution matrix φ based on the unmanned ship distance cost matrix D * The steps are: 1) The distance cost matrix D for N unmanned ship mission assignments = (d ij ) N*N First, perform a row transformation, subtract the minimum value of each row from the row, and then perform a column transformation, subtract the minimum value of each column from the column, and the elements with a value of zero appear; 2) Cover all zero elements in the transformed matrix D in 1) with the least number of horizontal or vertical lines; 3) If the total number of horizontal and vertical lines k in 2) is less than N, find the smallest uncovered element a, subtract a from each uncovered row, and add a to each covered column; 4) Repeat steps 2) and 3) until k=N; 5) Determine the optimal task allocation based on the location of the zero element and solve the optimal allocation matrix φ * .

5. The unmanned vessel swarm mission planning method based on the improved CAPT algorithm according to claim 4 is characterized in that: The method of step E is as follows: All unmanned ships reach the target location within a certain time, and the end time t at the final location is f for: In the above formula, v max Define a polynomial function of time for the speed of the fastest unmanned ship Satisfying δ(0)=0,δ(t f )=1, where k is an integer and k>2; Among them, the parameter α is: The trajectory that minimizes the square integral of velocity is the trajectory with constant velocity and satisfies the constraints at the beginning and end: * (t)=(1-δ(t))X(0)+δ(t)ΦG; Among them, X(0) is the position state vector of N unmanned ships at the initial time t=0.

Images