RRT-based path planning method for unmanned ship cluster to traverse multiple target points

Abstract

The application discloses a path planning method for unmanned ship cluster to traverse multiple target points based on RRT, and belongs to the field of lake path planning; aiming at the path planning requirement of unmanned ship cluster to multiple target points, complex obstacles are converted into equivalent elliptical obstacles, the existing RRT algorithm is improved, and child nodes are generated based on multiple nodes until all target points are traversed, so that the RRT algorithm can plan multiple target points of multiple ships. The gravity lines are generated by connecting the paths between the starting points and the target points in sequence, the gravity lines are used to exert gravity on the path nodes, and the obstacles are used to exert repulsion on the path nodes, and the position of the sampling path node is adjusted by using the repulsion and the gravity, so that the length of the planned path is shortened, and the path is smoother. Finally, the international maritime rules are introduced to constrain the obstacle avoidance action of the RRT algorithm, the rationality of obstacle avoidance of the RRT algorithm is improved, the length of the planned path is shortened, and the time cost of the planned path is reduced.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of lake water quality sampling and path planning, and particularly relates to a path planning method for a cluster of unmanned ships to traverse multiple target points based on RRT (Rapid Random Tree). BACKGROUND

[0002] With the development and progress of science and technology, people's living standards have been improved, but water resources have been damaged to a certain extent, so the protection of water resources has become a top priority. Nowadays, most scientific researchers will choose to use unmanned ships to traverse the target points in the order set in advance to sample the water quality and thus protect the water resources.

[0003] Therefore, finding a method that can make the cluster of unmanned ships plan paths for multiple target points in a short time can improve the sampling efficiency of the cluster of unmanned ships for multiple target points.

[0004] There are currently various traditional methods and intelligent methods to solve the multiple target point traversal problem, such as grey wolf algorithm, particle swarm algorithm and ant colony algorithm. However, the above algorithms are prone to local optimization and sometimes cannot find the optimal solution.

[0005] The RRT algorithm is an incremental construction method, which is mainly used to solve the path planning problem from the starting point to the target point in global path planning, and can also solve the multiple target point path planning problem. This method generates state points in the search space at random, and when the state point is located in a collision-free position, the nearest point in the search tree to the state point is found as the reference node, and the reference node is extended to the state point at a fixed step to achieve the purpose of finding a path. SUMMARY

[0006] The application proposes a path planning method for a cluster of unmanned ships to traverse multiple target points based on RRT, which improves the traditional RRT algorithm, converts complex obstacles into equivalent elliptical obstacles, and sets a safety threshold to simplify the map, improve the operation efficiency of the algorithm, and improve the safety of unmanned ship navigation.

[0007] The path planning method for a cluster of unmanned ships to traverse multiple target points based on RRT has the following specific steps:

[0008] First, an equivalent model of each obstacle in the water environment is established for the water area to be planned.

[0009] Then, the improved RRT algorithm is used to plan the path of each unmanned ship in the water area, and the constraint that all target points are traversed only once is met.

[0010] For each unmanned ship planning path, the gravity line is generated by connecting the starting point to the first target point and subsequent target points in turn, and the sample path node generated by the RRT algorithm is subjected to gravity by using the gravity line, and the path node is subjected to repulsion by using the obstacle, and the position of each sample path node is adjusted;

[0011] The specific steps are as follows:

[0012] Step 301: For the planning path of the ith unmanned ship containing k target points, the starting point to the kth target point is connected in turn by a straight line according to the order of the planning path, and these straight line segments are gravity lines;

[0013] Wherein 0<k and k<M; M is the total number of all target points;

[0014] Step 302: The sample path node A is a path node generated by the sampling function of the RRT algorithm, when the ith unmanned ship travels along the planning path, for the sample path node A, a perpendicular segment L is made to the current corresponding gravity line, and the gravity F of the current gravity line on the sample path node A is calculated y ;

[0015] Its expression is as follows:

[0016] F y =-F z ·sinα,

[0017] Wherein α is the angle between the planning path and the gravity line; F z is the tension of the planning path;

[0018] Step 303: Let the repulsive force of the obstacle on the path node A be F c , its expression is as follows:

[0019]

[0020] Wherein, η is the repulsive force coefficient; ω max is the maximum angular velocity of the unmanned ship; R min is the minimum turning radius of the unmanned ship; ρ is the influence range of the obstacle on the node; m' is the mass of the unmanned ship;

[0021] Step 304: The difference between the gravity F y and the repulsive force F c of the current path node is obtained to obtain the gravity difference F y * ;

[0022] Step 305: The compensation distance ΔL is obtained by using the gravity difference F y * ;

[0023] The expression is as follows:

[0024]

[0025] E is the inertia coefficient; and omega is the angular velocity of the unmanned ship;

[0026] Step 306: the new perpendicular segment L' is obtained by subtracting the compensation distance Delta L from the current perpendicular segment L; and the sampling path node A is corrected to the new path node A', so that the perpendicular distance between the new path node A' and the gravitational line is L'.

[0027] Step 307: after the update, it is judged whether the path segment passes through the obstacle, if not, the position update of the subsequent sampling path node is continued, if yes, the intersection point between the path segment and the obstacle is set as the new sampling path node, and step 302 is repeated.

[0028] When the unmanned ship encounters a dynamic obstacle or avoids other unmanned ships during navigation, the avoidance action of the unmanned ship is constrained according to the international maritime rules;

[0029] Finally, the planning paths of all unmanned ships are weighted, and the planning path quality is compared to screen out the comprehensive optimal path of all unmanned ships under the current situation.

[0030] The advantages of the present application are:

[0031] (1) The path planning method for the RRT-based unmanned ship cluster to traverse multiple target points simplifies the map by converting complex obstacles into equivalent elliptical obstacles and setting a safety threshold, improves the operation efficiency of the algorithm, and improves the safety of the unmanned ship navigation.

[0032] (2) The path planning method for the RRT-based unmanned ship cluster to traverse multiple target points improves the RRT algorithm and generates child nodes based on multiple nodes until all target points are traversed, so that the RRT algorithm can plan multiple target points for multiple ships.

[0033] (3) The path planning method for the RRT-based unmanned ship cluster to traverse multiple target points generates gravitational lines by connecting the paths between the starting points and the target points in sequence, applies gravity to the sampled path nodes by using the gravitational lines and applies repulsion to the sampled path nodes by using the obstacles, and adjusts the positions of the sampled path nodes by using the repulsion and the gravity, so as to shorten the length of the planning path and make the path smoother.

[0034] (4) The path planning method for the RRT-based unmanned ship cluster to traverse multiple target points introduces the international maritime rules to constrain the avoidance action of the RRT algorithm, improves the rationality of the avoidance of the RRT algorithm, and shortens the length of the planning path. BRIEF DESCRIPTION OF DRAWINGS

[0035] Figure 1 Flow chart of the path planning method for the RRT-based unmanned ship cluster to traverse multiple target points of the application;

[0036] Figure 2 Diagram for establishing an equivalent model based on obstacles of the application;

[0037] Figure 3 Flow chart of the path planning method for the RRT-based unmanned ship cluster to traverse multiple target points of the application;

[0038] Figure 4 Diagram for the path planning when the planned path intersects with obstacles of the application;

[0039] Figure 5 Diagram for the planned path after the path node is regenerated when the planned path intersects with obstacles of the application;

[0040] Figure 6 Flow chart of the method for generating an attractive and repulsive optimized planning path based on the attractive line of the application;

[0041] Figure 7 Diagram for the attractive line generated based on target points of the application;

[0042] Figure 8 Diagram for adjusting the perpendicular segment and generating a new path for compensating the distance of the application;

[0043] Figure 9 Schematic diagram for obstacle avoidance according to the international maritime rules in the application;

[0044] Figure 10 Diagram for the RRT algorithm planning path after the international maritime rules are added in the application. DETAILED DESCRIPTION

[0045] The application will be described in detail below with reference to the accompanying drawings.

[0046] The application provides a path planning method for solving the multi-target point sampling task of an unmanned ship cluster, the existing RRT algorithm is improved, and a child node is generated based on multiple nodes until all target points are traversed, so that the RRT algorithm can plan a multi-ship multi-target point. The gravity lines are generated by connecting the paths between the starting point and the target point in sequence, the gravity lines are used to exert gravity on the path nodes and obstacles are used to exert repulsion on the path nodes, and the repulsion and gravity are used to adjust the position of the sampling path node, thereby shortening the length of the planned path and making the path smoother. Finally, the international maritime rules are introduced to constrain the obstacle avoidance action of the RRT algorithm, improve the rationality of the obstacle avoidance of the RRT algorithm, shorten the length of the planned path, and reduce the time cost of the planned path.

[0047] As shown in Figure 1 , the specific steps are as follows:

[0048] Step one, for the water area to be planned, an equivalent model of each obstacle in the water area environment is established.

[0049] The specific steps are as follows:

[0050] Step 101: for the current obstacle, N points are uniformly sampled on the edge of the obstacle at equal angles, and are saved to set B;

[0051] The expression is as follows:

[0052] B=(b1,b2,b3,...,b i ,...,b N ),

[0053] Wherein, b i =(x i ,y i ), x i and y i are the x-axis coordinate and y-axis coordinate of point b i .

[0054] Step 102: select the maximum difference of the x-axis coordinate from set B as the maximum length l x of the equivalent model in the x-axis direction, and select the maximum difference of the y-axis coordinate as the maximum length l y of the equivalent model in the y-axis direction;

[0055] The expression is as follows:

[0056] l x =x max -x min

[0057] l y =y max -ymin

[0058] x max is the maximum value of the x-axis coordinate in set B; x min is the minimum value of the x-axis coordinate in set B; y max is the maximum value of the y-axis coordinate in set B, y min is the minimum value of the y-axis coordinate in set B;

[0059] Step 103: Perform inflation processing on the current obstacle, which is expressed as follows:

[0060]

[0061] x is the x-axis coordinate value of the boundary of the equivalent model of the obstacle; y is the y-axis coordinate value of the boundary of the equivalent model of the obstacle; a is the inflation gain of the obstacle on the x-axis, which is set by a person; b is the inflation gain of the obstacle on the y-axis, which is set by a person; v b is the current speed of the obstacle, ω b is the current angular speed of the obstacle, v bmax is the maximum speed of the obstacle, ω bmax is the maximum angular speed of the obstacle.

[0062] Step 104, repeat the above steps, and establish the equivalent model of each obstacle in the water area to be planned respectively; as Figure 2 shown, which is the equivalent model of the obstacle;

[0063] Step two, use the improved RRT algorithm to plan the path of each unmanned ship in the water area, and satisfy the constraint that all target points are traversed only once.

[0064] As Figure 3 shown, the specific steps are as follows:

[0065] Step 201: initialization: m unmanned ships, the expansion step is p, the maximum iteration number is n, the starting point set and the target point set are input, and the starting point set is set as the current node set.

[0066] The starting points of all unmanned ships form the starting point set, and the target points of all unmanned ships form the target point set;

[0067] Step 202: for the current iteration, select the ith node from the current node set, judge whether there is a target point within the range of p, if yes, set the target point as the child node, and enter step 206; otherwise, enter step 203;

[0068] The initial value of i is 1;

[0069] Step 203: Randomly generate a child node according to the extended step p, and connect the current node with the child node;

[0070] Step 204: Determine whether the connection between the current node and the child node passes through the obstacle, if not intersecting or tangent to the obstacle region, go to step 205; otherwise, randomly generate a child node according to p, and return to step 203;

[0071] As shown in Figure 4 and Figure 5 , respectively, are the position relationship diagrams of the planned path and the obstacle;

[0072] Step 205: Take the child node as the current node, return to step 202, and re-determine whether there is a target point in the range of p;

[0073] Step 206: Remove the target point set by the child node from the target point set, and determine whether there is a target point in the target point set, if not, there is no target point in the set, end. Otherwise, return to step 202, increment the value of i by 1, and re-determine as the current node; until the value of i is greater than m.

[0074] Step 207: The current iteration search is complete, re-set the initial value of i to 1; return to step 202 until the iteration number reaches n or there is no target point in the target point set.

[0075] Step three, for each unmanned ship planning path, generate a gravitational line by connecting the child nodes between the starting point and the target point in turn, exert a gravitational force on the path node using the gravitational line, and exert a repulsive force on the path node using the obstacle, adjust the position of each sampling path node; shorten the distance of the planned path, and reduce unnecessary path segments in the planned path.

[0076] As shown in Figure 6 , the specific steps are as follows:

[0077] Step 301: The planning path of the i-th unmanned ship contains k target points, according to the order of the planning path, connect the starting point to the k-th target point in turn with straight lines, these straight line segments are gravitational lines;

[0078] Where 0<k and k<M; M is the number of all target nodes; the generated gravitational lines are as shown in Figure 7 .

[0079] Step 302: The sampling path node A is generated by the sampling function of the RRT algorithm, when the i-th unmanned ship travels along the planned path, for the sampling path node A, draw a perpendicular segment L on the current corresponding gravitational line, and calculate the gravitational force F y of the current gravitational line on the sampling path node A.

[0080] The expression is as follows:

[0081] F y = -F z sinα,

[0082] Wherein, α is the angle between the planned path and the gravity line; F z is the tension of the planned path; its value is obtained according to the average angular velocity ω of the unmanned ship in the current water area, the mass m of the unmanned ship, the vertical segment L and the inertia coefficient E, and the expression is as follows:

[0083]

[0084] Step 303: Set the repulsive force of the obstacle to the path node A as F c , and the expression is as follows:

[0085]

[0086] Wherein, η is the repulsive force coefficient; ω max is the maximum angular velocity of the unmanned ship; R min is the minimum turning radius of the unmanned ship; and ρ is the influence range of the obstacle to the node. m' is the mass of the unmanned ship;

[0087] Step 304: The difference between the attractive force F y and the repulsive force F c of the current path node is obtained to obtain the attractive force difference F y * ;

[0088]

[0089] Step 305: The compensation distance ΔL is obtained by using the attractive force difference F y * ;

[0090] The expression is as follows:

[0091]

[0092] E is the inertia coefficient; and ω is the maximum angular velocity of the unmanned ship;

[0093] Step 306: The difference between the current vertical segment L and the compensation distance ΔL is obtained to obtain the new vertical segment L'; and the sampling path node A is corrected to the new path node A', so that the vertical distance between the new path node A' and the attractive line is L'.

[0094] As Figure 8 shown, the expression is as follows: L' = L - ΔL;

[0095] Step 307: After updating, determine whether the path segment passes through the obstacle. If not, continue to update the position of the subsequent sampling path node. If so, set the intersection of the path segment and the obstacle as the new sampling path node, and repeat step 302.

[0096] Step four, when the unmanned ship encounters dynamic obstacles during navigation, or avoids other unmanned ships, set a unified standard for unmanned ship obstacle avoidance movement, and introduce international maritime rules to constrain obstacle avoidance actions.

[0097] Step 401: According to the COLREGS rules, set the unmanned ship as the center of the circle, its heading as the polar axis to establish a polar coordinate system, and the direction directly behind the unmanned ship as 0 degrees, with the angle increasing counterclockwise.

[0098] When the relative bearing angle of the obstacle with respect to the unmanned ship is in the Head-on region, θ ∈ [165°, 195°] at this time, indicating a meeting state; when the relative bearing angle is in the L-Crossing region, θ ∈ [195°, 315°] at this time, indicating a left side crossing encounter state; when the relative bearing angle is in the R-Crossing region, θ ∈ [45°, 165°] at this time, indicating a right side crossing encounter state; when the relative bearing angle is in the Overtaking region, θ ∈ [0, 45°] ∪ [315°, 360°] at this time, indicating an overtaking state, as shown in Figure 9 .

[0099] Step 402: When the dynamic obstacle appears in the Head-on or R-Crossing region of the unmanned ship, the RRT algorithm will not plan a path in the L-Crossing region behind the dynamic obstacle, but only plan a path in the region from the right side of the front of the ship to the 165° region. When the dynamic obstacle appears in the L-Crossing region, the RRT algorithm can plan any path;

[0100] As shown in Figure 10 .

[0101] Step five, weight all the planned paths of the unmanned ships and compare the quality of the planned paths to select the comprehensive optimal path of all unmanned ships under the current situation.

[0102] For each group of planned paths, the sum of the lengths of the paths corresponding to each group of paths is calculated;

[0103] The path length of the i-th unmanned ship in the j-th group of planned paths is s ij The total length of the planned paths of all ships in the j-th group of planned paths is S j , which is expressed as follows:

[0104]

[0105] The total length of the planning path of the N groups is compared, and the smallest planning path is selected as the optimized path of the final unmanned ship cluster:

[0106] X* = argmin S j j ∈ [1, N].

Claims

1. A path planning method for an unmanned ship cluster to traverse multiple target points based on RRT, characterized in that, The specific steps are as follows: Firstly, an equivalent model of each obstacle in the water area to be planned is established; Then, the improved RRT algorithm is used to plan the path of each unmanned ship in the water area, and a child node is generated based on multiple nodes to meet the constraint of traversing all target points, and each target point is only passed once; For the planned path of each unmanned ship, the gravitational lines are generated by connecting the starting point to the first target point and the subsequent target points in turn, the sampling path nodes generated by the RRT algorithm are subjected to gravity by using the gravitational lines, and the path nodes are subjected to repulsion by using the obstacles to adjust the positions of the sampling path nodes; The specific steps are as follows: Step 301: The planned path of the ith unmanned ship contains k target points, and the starting point to the kth target point is connected in turn by straight lines according to the order of the planned path, and these straight line segments are gravitational lines; Where 0<k and k<M; M is the number of all target nodes; Step 302: generating a sampling path node A by a sampling function of the RRT algorithm, when the ith unmanned ship travels along the planned path, a perpendicular segment L is drawn to the current corresponding force line for the sampling path node A, and the force F of the current force line on the sampling path node A is calculated y ; The expression is as follows: ， where a is the angle between the planned path and the line of force; F z is the tension of the planned path; Step 303: Set the repulsive force of the obstacle to the path node A as F c The expression is as follows: ， wherein, is a repulsion coefficient; is a maximum angular velocity of the unmanned ship; is a minimum turning radius of the unmanned ship; is an influence range of the obstacle on the node; is a mass of the unmanned ship; Step 304: Utilize the current path node's attractive force F y and repulsive force F c Subtract, to get the attractive force difference F y * ; Step 305: Utilizing the gravitational difference F y * Compensating distance is obtained ; The expression is as follows: is the inertia coefficient; is the bank angle velocity of the unmanned ship; Step 306: use the current perpendicular segment L and the compensation distance Subtract to get a new perpendicular segment L ; At the same time, correct the sampling path node A to the new path node A , so that the new path node A is at a perpendicular distance L from the gravitational line Step 307: After updating, it is judged whether the path segment passes through the obstacle, if not, the position updating of the subsequent sampling path node is continued, if yes, the intersection point of the path segment and the obstacle is set as a new sampling path node, and step 302 is repeated; When the unmanned ship encounters a dynamic obstacle or avoids other unmanned ships during navigation, the avoidance action of the unmanned ship is constrained according to the international maritime rules; Finally, the planned paths of all unmanned ships are weighted, and the quality of the planned paths is compared to screen out the comprehensive optimal path of all unmanned ships under the current situation.

2. The path planning method for RRT-based unmanned ship swarm to traverse multi-target points according to claim 1, wherein, The specific steps of establishing the equivalent model of each obstacle are as follows: Step 101: For the current obstacle, N points are uniformly sampled on the edge of the obstacle at equal angles and saved in set B; The expression is as follows: ， wherein b i = (x i , y i ), x i and y i are the x-axis and y-axis coordinates of point b i , respectively; Step 102: Select the maximum difference of x-axis coordinates from set B as the maximum length l of its equivalent model in the x-axis direction x Similarly, select the maximum difference of y-axis coordinates as the maximum length l of its equivalent model in the y-axis direction y ; The expression is as follows: is the maximum value of the x-axis coordinate in the set B; is the minimum value of the x-axis coordinate in the set B; is the maximum value of the y-axis coordinate in the set B, is the minimum value of the y-axis coordinate in the set B; Step 103: The current obstacle is subjected to inflation processing, and the expression is as follows: a is the inflation gain of the obstacle in the x-axis; b is the inflation gain of the obstacle in the y-axis; is the current speed of the obstacle, is the current angular speed of the obstacle, is the maximum speed of the obstacle, is the maximum angular speed of the obstacle; Step 104: Repeat the above steps to establish the equivalent model of each obstacle in the water area to be planned.

3. The path planning method for RRT-based unmanned ship swarm to traverse multi-target points according to claim 1, wherein, The specific steps of planning the path of each unmanned ship in the water area by using the improved RRT algorithm are as follows: Step 201: initialization: m unmanned ships, the expansion step is p, the maximum number of iterations is n, the starting point set and the target point set are input, and the starting point set is set as the current node set; The starting points of all unmanned ships form the starting point set, and the target points of all unmanned ships form the target point set; Step 202: for the current iteration, the ith node is selected from the current node set, and it is judged whether there is a target point within the range of p around the node, if yes, the target point is set as a child node, and step 206 is entered; Otherwise, step 203 is entered; The initial value of i is 1; Step 203: a child node is randomly generated according to p, and the current node and the child node are connected; Step 204: it is judged whether the line connecting the current node and the child node passes through the obstacle, if not, step 205 is entered; otherwise, a child node is randomly generated again according to p, and step 203 is returned; Step 205: take the child node as the current node, return to step 202, and rejudge whether there is a target point in the p range; Step 206: remove the target point set by the child node from the target point set, and judge whether there is a target point in the target point set. If not, there is no target point in the set, and the process ends; Otherwise, return to step 202, and increase the value of i by 1 to rejudge as the current node; Until the value of i is greater than m; Step 207: the current iteration search is complete, and the initial value of i is reset to 1; Return to step 202 until the iteration number reaches n or there is no target point in the target point set.

4. The RRT-based path planning method for a swarm of unmanned ships to traverse multiple target points according to claim 1, wherein, The comprehensive optimal path of all unmanned ships is screened out, and the specific process is as follows: For each group of planned paths, the sum of the lengths of each group of paths is calculated respectively. The path length of the ith unmanned ship in the jth group of planned paths is s ij The length sum of all planned paths in the jth group of planned paths is S j The expression is as follows: Compare the total lengths of the N groups of planned paths, and select the planned path with the smallest total length as the optimized path of the unmanned ship cluster: 。

Images