A path planning method for unmanned ship swarm traversing multiple target points

Abstract

The present invention discloses a path planning method for an unmanned ship cluster to traverse multiple target points, belonging to the field of path planning in aquatic environments. Specifically, the method comprises the following steps: based on a simulation field, the unmanned ship cluster and the environment of the water area to be tested are modeled respectively to obtain the simulation field strength of each unmanned ship, each obstacle and ocean current; then, for n unmanned ships in the unmanned ship cluster and m target points in a map, a time cost matrix C is calculated based on the Hungarian algorithm; based on the principle of shortest task completion time, each unmanned ship is assigned its own initial target point; then, new untraversed target points are continuously assigned to the unmanned ships that have been assigned the initial target points; finally, the path of each unmanned ship is planned using an improved RRT* algorithm so that each unmanned ship passes through the assigned target points one by one in sequence until all unmanned ships have completed the path planning. The present invention improves the effectiveness in an ocean current environment, shortens the actual sailing time of the unmanned ship, and reduces the time cost.

Description

Technical Field

[0001] The present invention belongs to the field of path planning in aquatic environments, and in particular to a path planning method for an unmanned vessel cluster to traverse multiple target points. Background Art

[0002] With the development of science and technology and the progress of the times, people are paying more and more attention to the development of marine resources, and their understanding and exploration of the ocean are becoming more and more in-depth.

[0003] Currently, most researchers use unmanned vessels for water exploration. However, due to the limited energy resources, low payload capacity, and low exploration efficiency of a single unmanned vessel, the success rate of these missions is also low. Furthermore, traditional path planning algorithms consider ocean currents and obstacles separately, directly setting obstacles as restricted areas or simply inflating them for safe navigation, resulting in low algorithm efficiency. Therefore, finding a path planning method that enables unmanned vessel swarms to traverse various target points in an environment in a short period of time can improve the efficiency of unmanned vessel swarms in performing exploration missions.

[0004] There are currently a variety of traditional methods and intelligent methods to solve the problem of unmanned ship clusters traversing multiple target points, such as heuristic-based global optimization algorithms and optimal allocation algorithms based on recursive processes.

[0005] The traditional Hungarian algorithm is a combinatorial optimization algorithm that solves task allocation problems in polynomial time. When performing assignments, the algorithm first queries the minimum value of each row and column of the matrix, then reduces the matrix to zero values in each row and column, and finally determines the assignment plan based on these zero values.

[0006] The traditional RRT algorithm is a path planning algorithm with a randomly sampled tree structure. Its basic concept is to construct a path by continuously adding specific random trees. It does not require spatial modeling, and the expansion between nodes does not require preprocessing. The algorithm performs collision detection on the path between nodes to solve the obstacle avoidance problem. It can quickly search for empty areas in space and further reduce the distance between path nodes and the target point. Summary of the Invention

[0007] Aiming at the traversal requirements of multiple target points faced by unmanned ship clusters, this paper proposes a path planning method for traversing multiple target points of unmanned ship clusters by improving the RRT* algorithm and combining it with the Hungarian algorithm. This method improves the task allocation efficiency of multiple target points of the unmanned ship cluster and optimizes the target point allocation strategy through iteration, thereby increasing the accuracy of the algorithm.

[0008] The specific steps of the unmanned ship cluster traversal multi-target point path planning method are as follows:

[0009] Step 1: For an unmanned ship cluster in a certain water area, model the unmanned ships and the environment based on a simulation field, and calculate the simulation field intensities of each unmanned ship, each obstacle, and the ocean current respectively.

[0010] Step 2: For n unmanned ships in the unmanned ship cluster and m target points on the map, calculate the time cost matrix C of the unmanned ship cluster and each target point;

[0011] When n < m, the time cost matrix C is expressed by the following formula:

[0012]

[0013] In the above formula, c ij is the time cost of the j-th unmanned ship corresponding to the i-th target point.

[0014] Step 3: Use the time cost matrix C to assign an initial target point to each unmanned ship;

[0015] The specific steps are as follows:

[0016] Step 301: Perform row and column reduction on the time cost matrix C. Subtract the minimum value of each row or column from each row or column respectively, so that each row and each column have zero values;

[0017] Step 302: Determine whether n independent 0 values are found. If so, assign the target point corresponding to the row coefficient of the 0 position to the unmanned ship corresponding to the column coefficient of the 0 position, and end; otherwise, go to Step 303;

[0018] Step 303: Find the column in matrix C with only one 0 value, mark this 0 value as λ0, and then mark the other 0 values in its corresponding row with H0; at the same time, find the row in matrix C with only one 0 value, mark this 0 value as λ1, and then mark the other 0 values in its corresponding column with H1.

[0019] Step 304: Repeat the above marking work until all 0 values in matrix C are marked, and each column has and only has one λ0, then the optimal assignment ends, and assign the target point corresponding to the row coefficient of λ0 to the unmanned ship corresponding to the column coefficient of λ0.

[0020] Step 305: For the rows without 0 values, select the columns containing 0 values from the columns corresponding to the numbers in this row, and mark all of these columns as E0; then, in the columns without 0 values, mark all the rows containing 0 values corresponding to the numbers in each column as E1;

[0021] Step 306: Repeat the above marking process to find the minimum value in the unmarked row E1, and subtract this minimum value from all elements in the row; find the minimum value in the unmarked column E0, and subtract this minimum value from all elements in the column; until there is only one λ0 in each column of the matrix C, the optimal assignment is completed, and the target point corresponding to the row coefficient of λ0 is assigned to the unmanned vessel corresponding to the column coefficient of λ0.

[0022] Step 4: Continue to assign new untraversed target points to the unmanned ship that has been assigned the initial target point until all the target points are assigned;

[0023] Specifically:

[0024] First, the time cost matrix of the unmanned ship that has been assigned the initial target point to the remaining target points is calculated as C n , which is expressed as follows:

[0025]

[0026] Then, when the number of remaining unassigned target points is less than n, perform the following steps:

[0027] Step 401: Matrix C n The time cost corresponding to each column element in is added to the time cost of the target point assigned by the unmanned ship corresponding to the column to obtain a new time cost matrix C′ n .

[0028] Step 402: Time cost matrix C′ n Perform row and column reduction, subtract the corresponding minimum value from each row or column, so that each row and column has a zero value.

[0029] Step 403: Determine whether m independent zero values are found. If so, assign the target point corresponding to the row coefficient at the zero-value position to the unmanned vessel corresponding to the column coefficient at the zero-value position, and end; otherwise, proceed to step 404;

[0030] Step 404: Find the matrix C′ n There is only one column with 0 value, mark this 0 value as λ2, and then use H hang Mark the other 0 values in the row; at the same time, find the matrix C′ n There is only one row with a value of 0, and this 0 value is marked as λ3, and then H lie Marks other 0 values in the column.

[0031] Among them, H hang The expression is as follows:

[0032]

[0033] H lie The expression is as follows:

[0034]

[0035] Step 405: Repeat the above marking process until the matrix C′ n All 0 values in are marked, and there is only one λ3 in each row, then the optimal assignment is completed, and the target point corresponding to the row coefficient of λ3 is assigned to the unmanned ship corresponding to the column coefficient of λ3.

[0036] Step 406: For rows that do not contain 0 values, select columns containing 0 values from the columns corresponding to the numbers in the row, and mark all of the columns as E2; then, mark all of the rows containing 0 values in the columns that do not contain 0 values as E3;

[0037] Step 407: Repeat the above marking process to find the minimum value in the row not marked with E3, and subtract this minimum value from all elements in the row; find the minimum value in the column not marked with E4, and subtract this minimum value from all elements in the column; until the matrix C′ is n If there is only one λ3 in each row, the optimal assignment ends and the target point corresponding to the row coefficient of λ3 is assigned to the unmanned ship corresponding to the column coefficient of λ3. Otherwise, the target point with the smallest H in each row or column is assigned to the unmanned ship corresponding to the column coefficient of λ3. hang +H lie The value of λ3 or λ2 is assigned to the corresponding unmanned ship.

[0038] Step 5: After assigning target points to all unmanned ships, use the improved RRT* algorithm to plan the path of each unmanned ship so that each unmanned ship passes through the target points assigned to it one by one in the order assigned by the Hungarian algorithm.

[0039] The specific planning process is:

[0040] Step 501: Assume that the sampling range of the improved RRT* algorithm is M, the expansion step is p, T is the random tree path, q init is the initial point, q goal is the target point, N is the maximum number of iterations;

[0041] Step 502: Initialize the random tree T and generate a path point q that tends to the direction of the ocean current based on the random tree T. rand ;

[0042] The specific generation process is:

[0043] First, generate a random number between 0 and 1. If the value of this random number is greater than 0.9, directly set the target point q goal As a waypoint q randOtherwise, the simulation fields established by the ocean current and the obstacle are superimposed to obtain the simulation field B sum ;

[0044] Its expression is as follows:

[0045]

[0046] δ is the node q with the largest index in the random tree T T The ocean current angle, ε is the node q with the largest index in the random tree T T The obstacle simulation field direction, B o is the strength of the obstacle simulation field, B c is the simulated field intensity of the ocean current field;

[0047] Then, calculate the node q with the largest index in the random tree T T The direction of the comprehensive simulation field δ new , which is expressed as follows:

[0048]

[0049] Finally, δ new The value of is assigned to the ocean current angle δ, and combined with the simulated field B sum Calculation point q rand The x-axis coordinate x rand and the y-axis coordinate y rand ;

[0050] x rand The expression is as follows:

[0051]

[0052] Node q T The coordinates of (x T ,y T ), n r1 A random number from 0 to M, s r1 A random number between 0 and |ψ-δ| / 2; ψ is the node q T The navigation angle of the path you are on;

[0053] y rand The expression is as follows:

[0054]

[0055] n r2 0 to Random number, s r2 A random number between 0 and |ψ-δ| / 2;

[0056] Step 503: Select the distance point q in the random tree T randThe nearest node is point q near , from point q near With point q rand Take a point q from the line connecting new , so that point q new With point q near The distance is the step length p;

[0057] Step 504: Take M as the sampling range and point q as the sampling range. new As the center, select the node with the second largest index in the random tree T as point q neighbor ; At the same time, detect each node in the random tree T and point q new Is there an obstacle between the line segments? If there is an obstacle, return to step 502 to resample the points; otherwise, go to step 505;

[0058] Step 505: Using M as the detection range, randomly sample each node in the detection tree T and point q. new Whether the ocean currents between them allow unmanned ships to navigate.

[0059] The specific steps are as follows:

[0060] First, let e be the maximum number of iterations of the function, q s For point q new and dot q T The midpoint of the connecting line, x s is the midpoint q s The x-axis coordinate is generated by generating a random number n from 0 to M a1 , calculate the random detection point q dom The x-axis coordinate x dom ;

[0061] x dom The expression is as follows:

[0062] x dom =x s +n a1

[0063] Then, using the midpoint q s The y-axis coordinate y s , generating a 0 to The random number n a2 , calculate the random detection point q dom The y-axis coordinate is y dom ;

[0064] y dom The expression is as follows:

[0065] y dom =y s +n a2

[0066] Finally, set point q dom The intensity of the simulated ocean current field at the location is B cdom , set point q dom The simulated field strength of the unmanned ship at the location is B sdom , if B cdom >2B sdom , the ocean current does not allow the unmanned ship to pass, and the process ends; otherwise, continue with the following steps.

[0067] Step 506: When B cdom ≤2B sdom When the ocean current allows the unmanned ship to pass, the point q in the random tree T is neighbor With point q new Connect as a path and calculate point q neighbor With point q new The path distance between them is calculated and it is determined whether the distance is shorter than the original path distance. If so, the path segment is returned to the random tree T. Otherwise, the original path is returned to the random tree T.

[0068] Step 507: Point q new As the new path starting point, return to step 504 and repeat the above steps until a path from q init to q goal planning path.

[0069] The advantages of the present invention are:

[0070] (1) The present invention provides a path planning method for an unmanned ship cluster to traverse multiple target points. The unmanned ship, ocean current and obstacles are modeled based on the field, so that the unmanned ship can take into account the influence of ocean current and obstacles during navigation.

[0071] (2) The present invention provides a path planning method for an unmanned ship cluster to traverse multiple target points. By introducing a simulated ocean current field sampling function and an ocean current constraint function, the effectiveness of the algorithm in an ocean current environment is improved, the actual navigation time of the unmanned ship is shortened, and the time cost is reduced. BRIEF DESCRIPTION OF THE DRAWINGS

[0072] Figure 1 This is a flow chart of a method for planning a path for a cluster of unmanned ships to traverse multiple target points according to the present invention;

[0073] Figure 2 Schematic diagram of the unmanned ship simulation field based on simulation field modeling of the present invention;

[0074] Figure 3 Schematic diagram of an obstacle simulation field based on simulation field modeling of the present invention;

[0075] Figure 4Schematic diagram of the extended planning path of random sampling points of the improved RRT* algorithm of the present invention. DETAILED DESCRIPTION

[0076] The present invention will be described in detail below with reference to the accompanying drawings.

[0077] In response to the traversal requirements of multiple target points faced by unmanned ship clusters, the present invention improves the RRT* algorithm and combines the improved RRT* algorithm with the Hungarian algorithm to propose a path planning method for traversing multiple target points for an unmanned ship cluster in a time-varying ocean current environment. The unmanned ships and the environment are modeled based on a simulation field. Target points are assigned to each unmanned ship in the cluster based on the Hungarian algorithm. Based on the principle of shortest task completion time, initial target points are assigned to the unmanned ships in the cluster. Subsequently, based on the traversed target points, untraversed target points are continuously assigned to the unmanned ships in the cluster until all target points are assigned. After all unmanned ships have assigned target points, the improved RRT* algorithm is used to plan the path of a single unmanned ship, passing through the target points assigned to the unmanned ship one by one in the order assigned by the Hungarian algorithm until all unmanned ships have completed path planning.

[0078] The unmanned ship cluster traversal multi-target point path planning method is as follows Figure 1 The specific steps are as follows:

[0079] Step 1: For a cluster of unmanned ships in a certain water area, model the unmanned ships and the environment based on the simulation field, and calculate the simulation field intensity of each unmanned ship, each obstacle, and ocean current respectively.

[0080] First, each unmanned ship is modeled separately, with the front end of the unmanned ship as the S pole and the rear end as the N pole. The simulation field starts from the S pole and points to the N pole inside the unmanned ship, and starts from the N pole and returns to the S pole outside the unmanned ship. Each simulation field is a closed curve, such as Figure 2 As shown, the lines represent the simulated fields and the arrows represent the directions of the simulated fields.

[0081] Then, the simulated field intensity of each point in the simulated field of each unmanned ship is calculated;

[0082] Assume that the simulated field intensity of the current unmanned ship at point P(x,y) is B s , which is expressed as follows:

[0083]

[0084] In the above formula, α is the simulated field intensity gain of the unmanned ship, u is the current forward speed of the unmanned ship, d is the hull width of the current unmanned ship, and r is the distance between point P(x, y) and the center point P of the current unmanned ship. USV (x usv ,yusv ) The Euclidean distance between them is expressed as follows:

[0085]

[0086] In the process of path planning of the present invention, the combined effects of obstacles and ocean currents are considered, and by simulating the field, models of obstacles and ocean currents are established.

[0087] Perform simulated field modeling on each obstacle and calculate the simulated field intensity of each point in the obstacle simulated field;

[0088] As Figure 3 shown, the simulated field intensity of point P′(x′, y′) in the obstacle simulated field is B o , and its expression is as follows:

[0089]

[0090] In the above formula, β is the intensity gain of the obstacle simulated field, and the larger its value, the stronger the simulated field intensity around the obstacle; ρ0 is the influence range of the obstacle simulated field, x o and y o are respectively the x-axis coordinate and y-axis coordinate of the center point of the current obstacle.

[0091] Ocean currents will affect the navigation of the unmanned ship. Perform simulated field modeling on the ocean currents. The direction of the simulated field is the same as the ocean current direction. The simulated field intensity of point P”(x”, y”) in the ocean current field is B c , and its expression is as follows:

[0092]

[0093] In the above formula, ξ is the ocean current simulated field gain, F oc is the ocean current force at point P”, V oc is the ocean current velocity at point P”, and l is the distance length of the ocean current in the same direction at point P”.

[0094] Step 2: For n unmanned ships in the unmanned ship cluster and m target points on the map, calculate the time cost matrix C of the unmanned ship cluster and each target point;

[0095] If n < m, the time cost matrix C is expressed by the following formula:

[0096]

[0097] In the above formula, c ij is the time cost of the j-th unmanned ship corresponding to the i-th target point.

[0098] Assume that the planned path between the j-th unmanned ship and the i-th target point obtained by the path planning algorithm is Pji , planning path P ji It consists of b short paths p, P ji =[p ji.1 ,p ji.2 ,…,p ji.k ,…,p ji.b ], then c ij The expression is as follows:

[0099]

[0100] Where V a.1 , V a.k and V a.b The unmanned ship is sailing on the local path p ji.1 , p ji.k and p ji.b The speed on the surface is the vector sum of the unmanned ship's forward speed and the environmental disturbance speed.

[0101] Step 3: Use the Hungarian algorithm to assign each unmanned ship its own initial target point using the time cost matrix C;

[0102] The specific steps are:

[0103] Step 301: Reduce the time cost matrix C so that every row and every column has a zero value.

[0104] Specifically, the minimum value in each row of matrix C is subtracted, and the minimum value in each column is subtracted.

[0105] Step 302: Determine whether n independent zero values are found. If so, assign the target point corresponding to the row coefficient at the 0 position to the unmanned vessel corresponding to the column coefficient at the 0 position, and end; otherwise, proceed to step 303;

[0106] Step 303: Find a column in the matrix C that has only one 0 value, mark this 0 value as λ0, and then use H0 to mark other 0 values in the row; at the same time, find a row in the matrix C that has only one 0 value, mark this 0 value as λ1, and then use H1 to mark other 0 values in the column.

[0107] Step 304: Repeat the above marking process until all zero values in the matrix C are marked and each column has only one λ0. Then the optimal assignment is completed and the target point corresponding to the row coefficient of λ0 is assigned to the unmanned vessel corresponding to the column coefficient of λ0.

[0108] Step 305: For rows that do not contain 0 values, select columns containing 0 values from the columns corresponding to the numbers in the row, and mark all the columns as E0; then, mark all the rows containing 0 values corresponding to the columns that do not contain 0 values as E1;

[0109] Step 306: Repeat the above marking process to find the minimum value in the unmarked row E1, and subtract this minimum value from all elements in the row; find the minimum value in the unmarked column E0, and subtract this minimum value from all elements in the column; until there is only one λ0 in each column of the matrix C, the optimal assignment is completed, and the target point corresponding to the row coefficient of λ0 is assigned to the unmanned vessel corresponding to the column coefficient of λ0.

[0110] Step 4: Continue to assign new untraversed target points to the unmanned ship that has been assigned the initial target point until all the target points are assigned;

[0111] Specifically:

[0112] First, the time cost matrix of the unmanned ship that has been assigned the initial target point to the remaining target points is calculated as C n , which is expressed as follows:

[0113]

[0114] Then, determine whether the number of remaining target points is greater than n. If so, continue to iterate until the number of remaining unassigned target points is less than n, and perform the following steps:

[0115] Step 401: Matrix C n The time cost corresponding to each column element in is added to the time cost of the target point assigned by the unmanned ship corresponding to the column to obtain a new time cost matrix C′ n .

[0116] Step 402: Matrix C′ n Perform row and column reduction so that every row and column has a zero value.

[0117] Specifically: Matrix C′ n The minimum value in each row is subtracted from the row, and the minimum value in each column is subtracted from the column.

[0118] Step 403: Determine whether m independent zero values are found. If so, assign the target point corresponding to the row coefficient at the zero-value position to the unmanned vessel corresponding to the column coefficient at the zero-value position, and end; otherwise, proceed to step 404;

[0119] Step 404: Find the matrix C′ n There is only one column with 0 value, mark this 0 value as λ2, and then use H hang Mark the other 0 values in the row; at the same time, find the matrix C′ n There is only one row with a value of 0, and this 0 value is marked as λ3, and then H lie Marks other 0 values in the column.

[0120] Among them, H hang The expression is as follows:

[0121]

[0122] H lie The expression is as follows:

[0123]

[0124] Among them, i represents the index number of the row, that is, c ij In row i; j represents the quote in that column, i.e. c ij In the jth column.

[0125] Step 405: Repeat the above marking process until the matrix C′ n All 0 values in are marked, and there is only one λ3 in each row, then the optimal assignment is completed, and the target point corresponding to the row coefficient of λ3 is assigned to the unmanned ship corresponding to the column coefficient of λ3.

[0126] Step 406: For rows that do not contain 0 values, select columns containing 0 values from the columns corresponding to the numbers in the row, and mark all of the columns as E2; then, mark all of the rows containing 0 values in the columns that do not contain 0 values as E3;

[0127] Step 407: Repeat the above marking process to find the minimum value in the row not marked with E3, and subtract this minimum value from all elements in the row; find the minimum value in the column not marked with E4, and subtract this minimum value from all elements in the column; until the matrix C′ is n If there is only one λ3 in each row, the optimal assignment ends and the target point corresponding to the row coefficient of λ3 is assigned to the unmanned ship corresponding to the column coefficient of λ3. Otherwise, the target point with the smallest H in each row or column is assigned to the unmanned ship corresponding to the column coefficient of λ3. hang +H lie The value of λ3 or λ2 is assigned to the corresponding unmanned ship.

[0128] Step 5: After assigning target points to all unmanned ships, use the improved RRT* algorithm to plan the path of each unmanned ship so that each unmanned ship passes through the target points assigned to it one by one in the order assigned by the Hungarian algorithm.

[0129] The specific planning process is:

[0130] Step 501: Assume that the sampling range of the improved RRT* algorithm is M, the expansion step is p, T is the random tree path, q init is the initial point, q goal is the target point, N is the maximum number of iterations;

[0131] The improved RRT* algorithm realizes path planning under time-varying ocean currents. Figure 4 To improve the expansion process of random sampling points of the RRT* algorithm, path points will be generated along the direction of the ocean current to maximize the use of the ocean current to shorten the navigation time of the unmanned ship. At the same time, the path will avoid being generated against the direction of the ocean current to avoid wasting too much time and energy costs.

[0132] Step 502: Initialize the random tree T and generate a path point q that tends to the direction of the ocean current based on the random tree T. rand ; and the path point q rand The distance to the node with the largest index in the random tree T is less than M.

[0133] The specific generation process is:

[0134] First, generate a random number between 0 and 1. If the value of this random number is greater than 0.9, directly set the target point q goal As a waypoint q rand Otherwise, the simulation fields established by the ocean current and the obstacle are superimposed to obtain the simulation field B sum ;

[0135] Its expression is as follows:

[0136]

[0137] δ is the node q with the largest index in the random tree T T The ocean current angle, ε is the node q with the largest index in the random tree T T The direction of the obstacle simulation field.

[0138] Then, calculate the node q with the largest index in the random tree T T The direction of the comprehensive simulation field δ new , which is expressed as follows:

[0139]

[0140] Finally, δ new The value of is assigned to the ocean current angle δ, and combined with the simulated field B sum Calculation point q rand The x-axis coordinate x rand and the y-axis coordinate y rand .

[0141] x rand The expression is as follows:

[0142]

[0143] Node q T The coordinates of (x T ,y T), n r1 A random number from 0 to M, s r1 A random number between 0 and |ψ-δ| / 2; ψ is the node q T The navigation angle of the path you are on;

[0144] y rand The expression is as follows:

[0145]

[0146] n r2 0 to Random number, s r2 A random number between 0 and |ψ-δ| / 2;

[0147] Step 503: Select the distance point q in the random tree T rand The nearest node is point q near , from point q near With point q rand Take a point q from the line connecting new , so that point q new With point q near The distance is the step length p;

[0148] Step 504: Take M as the sampling range and point q as the sampling range. new As the center, select the node with the second largest index in the random tree T as point q neighbor ; At the same time, detect each node in the random tree T and point q new Is there an obstacle between the line segments? If there is an obstacle, return to step 502 to resample point q new , otherwise, go to step 505;

[0149] Step 505: Using M as the detection range, randomly sample each node in the detection tree T and point q. new Whether the ocean currents between them allow unmanned ships to navigate.

[0150] The specific steps are as follows:

[0151] First, let e be the maximum number of iterations of the function, q s q new and q T The midpoint of the connecting line, x s is the midpoint q s The x-axis coordinate is generated by generating a random number n from 0 to M a1 , calculate the random detection point q dom The x-axis coordinate x dom ;

[0152] The midpoint expression is as follows:

[0153]

[0154] x dom The expression is as follows:

[0155] x dom =x s +n a1

[0156] Then, using the midpoint q s The y-axis coordinate y s , generating a 0 to The random number n a2 , calculate the random detection point q dom The y-axis coordinate is y dom ;

[0157] y dom The expression is as follows:

[0158] y dom =y s +n a2

[0159] Finally, set point q dom The intensity of the simulated ocean current field at the location is B cdom , set point q dom The simulated field strength of the unmanned ship at the location is B sdom , if B cdom >2B sdom , the ocean current does not allow the unmanned ship to pass, and the process ends; otherwise, continue with the following steps.

[0160] Step 506: When B cdom ≤2B sdom When the ocean current allows the unmanned ship to pass, the point q in the random tree T is neighbor With point q new Connect as a path and calculate point q neighbor With point q new The path distance between them is calculated and it is determined whether the distance is shorter than the original path distance. If so, the path segment is returned to the random tree T. Otherwise, the original path is returned to the random tree T.

[0161] Step 507: Point q new As the new path starting point, return to step 504 and repeat the above steps until a path from q init to q goal planning path.

Claims

1. A method for planning a path for a swarm of unmanned vessels to traverse multiple target points, characterized by: It is completed through the following steps: First, for an unmanned ship cluster in a certain water area, the unmanned ships and the environment are modeled based on a simulation field, and the simulation field intensities of each unmanned ship, each obstacle, and ocean currents are calculated respectively; Then, for the n unmanned ships in the unmanned ship cluster and the m target points in the map, where n < m, the time cost matrix C of the unmanned ship cluster and each target point is calculated; and the initial target points are assigned to each unmanned ship using the time cost matrix C; The specific steps are as follows: Step 301: Perform row and column reduction on the time cost matrix C. Subtract the minimum value corresponding to each row or column from each row or column, so that each row and each column have zero values; The time cost matrix C is represented by the following formula: In the above formula, c ij is the time cost of the j-th unmanned ship corresponding to the i-th target point; Step 302: Determine whether n independent 0 values are found. If so, assign the target point corresponding to the row coefficient at the 0 position to the unmanned ship corresponding to the column coefficient at the 0 position, and end; Otherwise, go to step 303; Step 303: Find the column in matrix C with only one 0 value, mark this 0 value as λ0, and then mark the other 0 values in its row with H0; at the same time, find the row in matrix C with only one 0 value, mark this 0 value as λ1, and then mark the other 0 values in its column with H1; Step 304: Repeat the above marking work until all 0 values in matrix C are marked, and there is exactly one λ0 in each column. Then the optimal assignment ends, and assign the target point corresponding to the row coefficient of λ0 to the unmanned ship corresponding to the column coefficient of λ0; Step 305: For the rows without 0 values, select the column containing 0 values from the columns corresponding to the numbers in this row, and mark all columns as E0; then, among the columns without 0 values, mark all rows corresponding to the numbers in each column containing 0 values as E1; Step 306: Repeat the above marking work, find the minimum value in the rows not marked with E1, and subtract this minimum value from all elements in the row; find the minimum value in the columns not marked with E0, and subtract this minimum value from all elements in the column; Until there is exactly one λ0 in each column of matrix C, then the optimal assignment ends, and assign the target point corresponding to the row coefficient of λ0 to the unmanned ship corresponding to the column coefficient of λ0; Next, continue to assign new unvisited target points to the unmanned ships that have been assigned initial target points until all target points are assigned; Specifically: First, the time cost matrix of the unmanned ship that has been assigned the initial target point to the remaining target points is calculated as C n , which is expressed as follows: Then, when the number of remaining unassigned target points is less than n, the matrix C n The time cost corresponding to each column element in is added to the time cost of the target point assigned by the unmanned ship corresponding to the column to obtain a new time cost matrix C′ n ; For the new time cost matrix C′ n , the remaining target points are allocated using the same allocation strategy as the initial target points; Finally, after all target points are assigned to all unmanned ships, use the improved RRT* algorithm to plan the paths of each unmanned ship, so that each unmanned ship passes through the target points assigned to this unmanned ship one by one in the order assigned by the Hungarian algorithm; The specific planning process is as follows: Step 501: Assume that the sampling range of the improved RRT* algorithm is M, the expansion step is p, T is the random tree path, q init is the initial point, q goal is the target point, N is the maximum number of iterations; Step 502: Initialize the random tree T and generate a path point q that tends to the direction of the ocean current based on the random tree T. rand ; Step 503: Select the distance point q in the random tree T rand The nearest node is point q near , from point q near With point q rand Take a point q from the line connecting new , so that point q new With point q near The distance is the step length p; Step 504: Take M as the sampling range and point q as the sampling range. new As the center, select the node with the second largest index in the random tree T as point q neighbor ; At the same time, detect each node in the random tree T and point q new Is there an obstacle between the line segments? If there are obstacles, return to step 502 to resample points, otherwise, go to step 505; Step 505: Using M as the detection range, randomly sample each node in the detection tree T and point q. new whether the ocean currents between them allow unmanned vessels to navigate; The specific steps are as follows: First, let e be the maximum number of iterations of the function, q s For point q new and dot q T The midpoint of the connecting line, x s is the midpoint q s The x-axis coordinate is generated by generating a random number n from 0 to M a1 , calculate the random detection point q dom The x-axis coordinate x dom ; x dom The expression is as follows: x dom =x s +n a1 Then, using the midpoint q s The y-axis coordinate y s , generating a 0 to The random number n a2 , calculate the random detection point q dom The y-axis coordinate is y dom ; y dom The expression is as follows: and dom =and s +n a2 Finally, set point q dom The intensity of the simulated ocean current field at the location is B cdom , set point q dom The simulated field strength of the unmanned ship at the location is B sdom , if B cdom >2B sdom , then the ocean current does not allow the unmanned ship to pass, and the process ends; otherwise, the process proceeds to step 506; Step 506: When B cdom ≤2B sdom When the ocean current allows the unmanned ship to pass, the point q in the random tree T is neighbor With point q new Connect as a path and calculate point q neighbor With point q new The path distance between them is calculated and it is determined whether the distance is shorter than the original path distance. If so, the path segment is returned to the random tree T. Otherwise, the original path is returned to the random tree T. Step 507: Point q new As the new path starting point, return to step 504 and repeat the above steps until a path from q init to q goal planning path.

2. The method for planning a path for a swarm of unmanned vessels to traverse multiple target points according to claim 1, wherein: The simulation field refers to: modeling each unmanned ship separately, taking the front end of the unmanned ship as the S pole and the rear end as the N pole. The simulation field starts from the S pole and points to the N pole inside the unmanned ship, and starts from the N pole and returns to the S pole outside the unmanned ship. Each simulation field is a closed curve.

3. The method for planning a path for a swarm of unmanned vessels to traverse multiple target points according to claim 1, wherein: The new time cost matrix C′ n , the process of allocating the remaining target points is as follows: Step 401: Matrix C' n Perform row and column reduction, subtract the minimum value in each row from the minimum value in the row, and subtract the minimum value in each column from the minimum value in the column, so that each row and column has a zero value; Step 402: Determine whether m independent zero values are found. If so, assign the target point corresponding to the row coefficient at the zero-value position to the unmanned vessel corresponding to the column coefficient at the zero-value position, and end. Otherwise, go to step 403; Step 403: Find the matrix C′ n There is only one column with 0 value, mark this 0 value as λ2, and then use H hang Mark the other 0 values in the row; at the same time, find the matrix C′ n There is only one row with a value of 0, and this 0 value is marked as λ3, and then H lie Mark other 0 values in the column; Among them, H hang The expression is as follows: H lie The expression is as follows: Step 404: Repeat the above marking process until the matrix C′ n All 0 values in are marked, and there is only one λ3 in each row, then the optimal assignment is completed, and the target point corresponding to the row coefficient of λ3 is assigned to the unmanned ship corresponding to the column coefficient of λ3; Step 405: For rows that do not contain 0 values, select columns containing 0 values from the columns corresponding to the numbers in the row, and mark all of the columns as E2; then, mark all rows containing 0 values in the columns that do not contain 0 values as E3; Step 406: Repeat the above marking process to find the minimum value in the row not marked E3, and subtract this minimum value from all elements in the row; find the minimum value in the column not marked E4, and subtract this minimum value from all elements in the column; until the matrix C′ is n If there is only one λ3 in each row, the optimal assignment ends and the target point corresponding to the row coefficient of λ3 is assigned to the unmanned ship corresponding to the column coefficient of λ3. Otherwise, the target point with the smallest H in each row or column is assigned to the unmanned ship corresponding to the column coefficient of λ3. hang +H lie The value of λ3 or λ2 is assigned to the corresponding unmanned ship.

4. The method for planning a path for a swarm of unmanned vessels to traverse multiple target points according to claim 1, wherein: In step 502, a path point q is generated based on the random tree T, which is in the direction of the ocean current. rand The specific process is: First, generate a random number between 0 and 1. If the value of this random number is greater than 0.9, directly set the target point q goal As a waypoint q rand Otherwise, the simulation fields established by the ocean current and the obstacle are superimposed to obtain the simulation field B sum ; Its expression is as follows: δ is the node q with the largest index in the random tree T T The ocean current angle, ε is the node q with the largest index in the random tree T T The obstacle simulation field direction, B o is the strength of the obstacle simulation field, B c is the simulated field intensity of the ocean current field; Then, calculate the node q with the largest index in the random tree T T The direction of the comprehensive simulation field δ new , which is expressed as follows: Finally, δ new The value of is assigned to the ocean current angle δ, and combined with the simulated field B sum Calculation point q rand The x-axis coordinate x rand and the y-axis coordinate y rand ; x rand The expression is as follows: Node q T The coordinates of (x T ,y T ), n r1 A random number from 0 to M, s r1 A random number between 0 and |ψ-δ| / 2; ψ is the node q T The navigation angle of the path you are on; y rand The expression is as follows: n r2 0 to Random number, s r2 A random number between 0 and |ψ-δ| / 2.

Images