Unmanned ship path planning method based on improved RRT algorithm

Abstract

The invention provides an unmanned ship path planning method based on an improved RRT algorithm, and the method comprises the following steps: collecting environment information and depth information around an unmanned ship in real time through a sensor, and fusing the environment information and the depth information, thereby obtaining a planning map; calculating the boundary information of the obstacle in the planning map based on an edge detection algorithm to obtain the boundary information of the obstacle; constructing a heuristic function according to the obstacle boundary information and the path target point of the unmanned ship, and guiding an RRT algorithm to sample through the heuristic function to obtain a first global path; pruning and smoothing the first global path to obtain a second global path; performing local planning according to the second global path and the environment information to obtain a local planning path; and the unmanned ship travels according to the local planning path. Compared with the prior art, the unmanned ship path planning efficiency and quality are effectively improved.

Description

Technical Field

[0001] The present invention is applicable to the technical field of path planning, and in particular relates to an unmanned boat path planning method based on an improved RRT algorithm. Background Art

[0002] An unmanned boat is a small offshore platform with environmental perception and autonomous navigation capabilities, and can complete corresponding tasks autonomously. Based on these characteristics, when performing tasks in unknown waters, unmanned boats need to have the ability to quickly plan paths and sail comfortably in order to successfully complete their tasks.

[0003] In practical applications, the global path planning algorithm commonly used by unmanned boats is RRT*. The main function of the global algorithm is to plan a path as a reference for local path planning. However, for the traditional RRT* algorithm, the search efficiency is low in complex environments and it is difficult to bypass obstacles, thereby planning a path. At the same time, there are many useless nodes in the planned path that do not meet the kinematic constraints of the unmanned boat. If the environment is very complex and the map is very large, the RRT* algorithm may take a long time to search for a path, and the path quality is poor. It may even be difficult to bypass complex obstacles so that the path cannot be found and the search fails.

[0004] Therefore, a new unmanned boat path planning method based on the improved RRT algorithm is urgently needed to solve the above technical problems. Summary of the invention

[0005] The present invention proposes an unmanned boat path planning method based on an improved RRT algorithm, aiming to improve the efficiency and quality of unmanned boat path planning.

[0006] The present invention proposes a path planning method for an unmanned boat based on an improved RRT algorithm, comprising the following steps:

[0007] S1. Collecting environmental information and depth information around the unmanned boat in real time through sensors, and fusing the environmental information and the depth information to obtain a planning map;

[0008] S2. Calculating the boundary information of the obstacles in the planning map based on an edge detection algorithm to obtain the obstacle boundary information;

[0009] S3, constructing a heuristic function according to the obstacle boundary information and the path target point of the unmanned boat, and guiding the RRT algorithm to perform sampling through the heuristic function to obtain a first global path;

[0010] S4, performing pruning and smoothing on the first global path to obtain a second global path;

[0011] S5. Perform local planning according to the second global path and the environmental information to obtain a local planned path;

[0012] S6. The unmanned boat travels according to the local planned path.

[0013] Preferably, step S2 includes the following sub-steps:

[0014] S21, performing Gaussian smoothing on the planning map to obtain a smoothed planning map;

[0015] S22, calculating the gradient of the smooth planning map by an edge detection operator to obtain a gradient score; wherein the gradient includes an amplitude and a direction;

[0016] S23, calculating the gradient magnitude and gradient direction of each pixel in the smooth planning map according to the gradient score to obtain a gradient magnitude map;

[0017] S24, performing non-maximum suppression processing on the gradient amplitude map;

[0018] S25, performing edge detection and edge connection on the gradient amplitude map after non-maximum suppression processing according to preset rules to obtain edge point information;

[0019] S26. Calculate according to the edge point information to obtain the obstacle boundary information.

[0020] Preferably, the preset rule is: the edge of the gradient amplitude image is detected according to a first preset threshold and a second preset threshold, wherein a pixel point greater than the first preset threshold is a strong edge, a pixel point less than the second preset threshold is a weak edge, the strong edge is retained, the weak edge is removed, and a pixel point less than the first preset threshold and greater than the second preset threshold and connected to the strong edge is retained;

[0021] The strong edge and the weak edge are connected by an edge tracking algorithm.

[0022] Preferably, the edge detection operator is a Sobel operator.

[0023] Preferably, step S3 includes the following sub-steps:

[0024] S31, randomly generating sampling points based on the heuristic function, and determining whether the sampling points are obstacle points in the obstacle boundary information, if so, regenerating new sampling points; if not, proceeding to step S32;

[0025] S32, calculating a child node on the line connecting the current node and the sampling point according to a preset step length, and determining whether the line connecting the current node and the sampling point touches an obstacle, if so, returning to step S31; if not, taking the current node as the parent node of the child node, and proceeding to step S33;

[0026] S33, reselecting the parent node of the child node;

[0027] S34, determining whether the child node is the path target point, if so, backtracking to obtain the first global path output, if not, returning to step S31.

[0028] Preferably, the edge detection algorithm is a canny edge detection algorithm.

[0029] Compared with the prior art, the present invention collects the environmental information and depth information around the unmanned boat in real time through sensors, and fuses the environmental information and depth information to obtain a planning map; calculates the boundary information of obstacles in the planning map based on the edge detection algorithm to obtain the obstacle boundary information; constructs a heuristic function based on the obstacle boundary information and the path target point of the unmanned boat, guides the RRT algorithm to sample through the heuristic function to obtain the first global path; performs pruning and smoothing on the first global path to obtain the second global path; performs local planning based on the second global path and environmental information to obtain the local planning path; the unmanned boat travels according to the local planning path. In this way, the present invention effectively improves the efficiency and quality of path planning. BRIEF DESCRIPTION OF THE DRAWINGS

[0030] The present invention will be described in detail below in conjunction with the accompanying drawings. The above and other aspects of the present invention will become clearer and easier to understand through the detailed description made in conjunction with the following drawings. In the accompanying drawings:

[0031] Figure 1 It is a flowchart of an unmanned boat path planning method based on an improved RRT algorithm provided in an embodiment of the present invention;

[0032] Figure 2 is a schematic diagram of a planning map and obstacle boundary information based on an improved RRT algorithm provided in an embodiment of the present invention;

[0033] Figure 3 is a schematic diagram of sampling points randomly generated by a heuristic function based on an improved RRT algorithm provided in an embodiment of the present invention;

[0034] Figure 4 It is a logical schematic diagram of pruning processing based on the improved RRT algorithm provided by an embodiment of the present invention;

[0035] Figure 5This is a logical schematic diagram of smoothing processing based on an improved RRT algorithm provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0036] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0037] Please refer to Figure 1-Figure 5 The present invention proposes an unmanned boat path planning method based on an improved RRT algorithm, comprising the following steps:

[0038] S1. Collecting environmental information and depth information around the unmanned boat in real time through sensors, and fusing the environmental information and the depth information to obtain a planning map;

[0039] S2. Calculating the boundary information of the obstacles in the planning map based on an edge detection algorithm to obtain the obstacle boundary information;

[0040] In the embodiments of the present invention, please refer to Figure 2 , Figure 2 Schematic diagram of planning map and obstacle boundary information based on improved RRT algorithm provided by an embodiment of the present invention. The edge detection algorithm is canny edge detection algorithm. Step S2 includes the following sub-steps:

[0041] S21, performing Gaussian smoothing on the planning map to obtain a smoothed planning map. By performing Gaussian smoothing on the planning map, noise in the image is reduced, so that subsequent edge detection results are more stable.

[0042] S22, calculating the gradient of the smooth planning map by edge detection operator to obtain a gradient score; wherein the gradient includes magnitude and direction; the edge detection operator is Sobel operator, and the gradient of the image is calculated by using Sobel operator, including the magnitude and direction of the gradient. It can help find the area with large changes in the image, which usually represents the possible location of the edge.

[0043] S23, calculating the gradient magnitude and gradient direction of each pixel in the smooth planning map according to the gradient score to obtain a gradient magnitude map;

[0044] S24, performing non-maximum suppression processing on the gradient amplitude map, thereby thinning the edge, retaining the pixel points with the maximum local gradient amplitude, and suppressing non-edge pixel points.

[0045] S25, performing edge detection and edge connection on the gradient amplitude map after non-maximum suppression processing according to preset rules to obtain edge point information;

[0046] S26. Calculate according to the edge point information to obtain the obstacle boundary information.

[0047] In an embodiment of the present invention, the preset rule is: the edge of the gradient amplitude image is detected according to a first preset threshold and a second preset threshold, wherein a pixel point greater than the first preset threshold is a strong edge, a pixel point less than the second preset threshold is a weak edge, the strong edge is retained, the weak edge is removed, and a pixel point less than the first preset threshold and greater than the second preset threshold and connected to the strong edge is retained; the strong edge and the weak edge are connected by an edge tracking algorithm.

[0048] Specifically, the preset rule is a dual threshold strategy, which defines two thresholds: a low threshold (low_threshold) and a high threshold (high_threshold). The gradient amplitude image is converted into a binary image according to the threshold, where pixels greater than the high threshold are considered strong edges, pixels less than the low threshold are excluded, and pixels between the two are retained as weak edges based on whether they are connected to strong edges. At the same time, an edge tracking algorithm is used to connect weak edges and strong edges to form a complete edge contour.

[0049] S3, constructing a heuristic function according to the obstacle boundary information and the path target point of the unmanned boat, and guiding the RRT algorithm to perform sampling through the heuristic function to obtain a first global path;

[0050] In the embodiment of the present invention, step S3 includes the following sub-steps:

[0051] S31, randomly generate a sampling point, and determine whether the sampling point is an obstacle point in the obstacle boundary information, if so, regenerate a new sampling point; if not, proceed to step S32;

[0052] S32, calculating a child node on the line connecting the current node and the sampling point according to a preset step length, and determining whether the line connecting the current node and the sampling point touches an obstacle, if so, returning to step S31; if not, taking the current node as the parent node of the child node, and proceeding to step S33;

[0053] S33, reselecting the parent node of the child node;

[0054] S34, determining whether the child node is the path target point, if so, backtracking to obtain the first global path output, if not, returning to step S31.

[0055] Specifically, the specific steps of guiding RRT* sampling through the heuristic function are as follows:

[0056] The functions and parameters used below are explained as follows:

[0057] Sample_random(): heuristic sampling function;

[0058] Sample(): A sampling function that randomly generates points on the map;

[0059] Bias: bias probability;

[0060] distance: the distance threshold to the nearest obstacle;

[0061] xu_l: the maximum value of the map x-axis;

[0062] yu_l: the maximum value of the map y axis;

[0063] a, b: weight coefficients;

[0064] Container r: container for storing sampling points;

[0065] Is_obs(): A function to determine whether a node is an obstacle node;

[0066] Xnear: is the current node, initialized as the starting point;

[0067] Xrand: randomly generated sampling nodes;

[0068] step_size: the specified step size (constant);

[0069] Is_free(): A function that determines whether the connection between two nodes is safe;

[0070] rewir e (): function to reselect the parent node;

[0071] Xparent: parent node;

[0072] Is_goal(): A function that determines whether a node is a target point;

[0073] getPath(): A function that traces back from the end point to obtain the planned path.

[0074] The sampling point Xrand is obtained according to the selected sampling function, and the function Is_obs() is called to determine whether Xrand belongs to an obstacle. If so, it is regenerated; if not, it goes to step A32.

[0075] The specific steps of the sampling point generation function sample_random() in step S31 are as follows:

[0076] Randomly generate a number between 0 and 1, and select the sampling function according to formula 1:

[0077]

[0078] The specific implementation process of the heuristic sampling function Sample_random() is as follows:

[0079] Randomly generate node Xr (non-obstacle node), traverse container Edge according to formula 2 to calculate the obstacle boundary point Edge between Xr and container Edge i (represents the i-th element of container Edge) the minimum distance d, determines whether d is less than distance, if so, stores node Xr in container r, repeats the loop to obtain m nodes that meet the conditions and stores them in container r. All nodes in container r are sampled around obstacles in the map, so that obstacles can be bypassed faster, search efficiency is improved, and the speed of finding the path is accelerated.

[0080]

[0081] Xr.[0] and Xr.[1] represent the x and y values ​​of node Xr, respectively. i .[0]、Edge i .[1] Similarly, according to formula 3, calculate each element X in container r k The heuristic value h k (k represents the kth element of container r).

[0082] h i =a*d_obj_min+b*d_goal (Formula 3);

[0083] d_obj_min is the element X of container r k The minimum value of all elements in the container Edge is calculated according to formula 2, and d_goal is the element X of the container r k The distance to the target point.

[0084] According to the heuristic value h calculated for all elements in container r k , and set the value h k The smallest node X k As randomly generated sampling points Xrand.

[0085] The sampling points found by the heuristic sampling function Sample_random() are all stored in the container r.

[0086] The elements of container r are distributed as follows Figure 3 As shown (assuming m is 15), Figure 3It is a schematic diagram of sampling points randomly generated by a heuristic function based on an improved RRT algorithm provided in an embodiment of the present invention.

[0087] The specific form of the Sample() function is shown in Formula 4:

[0088]

[0089] According to the sampling point Xrand obtained in step S31, the current node is Xnear, and the next node Xnew is obtained on the line connecting Xnear and Xrand according to the specified step size step_size, and the function Is_free() is called to determine whether the line connecting Xnear and Xnew is safe. If so, the parent node of Xnew is set to Xnear and the process goes to step S33; if not, it goes back to step S31.

[0090] Call the rewire() function to reselect the parent node Xparent for Xnew, and call the Is_free() function to determine whether the connection between Xparent and Xnew is safe. If so, change the parent node of Xnew to Xparent, update Xnear to Xnew, and enter step S34; if not, do not make any changes and enter step S34.

[0091] Call the function Is_goal() to determine whether Xnew is the target point Xgoal. If so, set the parent node of Xgoal to Xnew, call the function getPath() to backtrack to get the path, and exit the loop; if not, return to step A31 and continue searching for the path.

[0092] S4. Perform pruning and smoothing on the first global path to obtain a second global path.

[0093] In the embodiment of the present invention, the main purpose of path pruning is to remove redundant nodes in the path. The specific steps of implementing path pruning are as follows:

[0094] For ease of understanding, the following parameters are explained as follows:

[0095] The container Path is the first global path obtained in the above steps.

[0096] 1. Initialize node n to be equal to the end point of the Path and node m to be the starting point of the Path.

[0097] 2. Starting from the last element n=Path[c] (the initial value of c is the size of the container - 1) of the container Path, traverse the container Path forward to check whether the mn connection is safe. If it is safe, proceed to the next step; if not, update the n node to Path[--c], do not update the m node, and continue to determine whether the mn connection is safe until the mn connection is safe and proceed to the next step.

[0098] 3. Determine whether n is the end point. If so, pruning ends. If not, update the m node to the n node and return to step 2 until the n node is the end point and pruning ends.

[0099] The effect of path pruning is as follows Figure 4 As shown, Figure 4 This is a schematic diagram of clipping processing based on the improved RRT algorithm provided by an embodiment of the present invention. It can be seen that redundant nodes in the path are removed after pruning.

[0100] Path smoothing is mainly to adjust the path points with angles less than 90° in the path, so that the angles between every three nodes of the path are as large as possible, which meets the kinematic constraints of the unmanned boat.

[0101] Now assume that there are three nodes X in the path Path i , X j , X k , the angle θ formed by the three nodes is less than 90°, such as Figure 5 As shown, Figure 5 This is a logical diagram of smoothing based on the improved RRT algorithm provided by an embodiment of the present invention. The specific implementation steps of path smoothing are as follows: Assume that all points taken during smoothing will not be obstacles

[0102] 1. Let X iimid For X i , X j The midpoint of the line, X jkmid For X j , X k The midpoint of the line, and use Formula 2 to find X ijmid With X i The Euclidean distance d ij , X jkmid With X j The Euclidean distance d jk .

[0103] 2. Determine X first i , X ijmid , X k Is the angle θ1 between them greater than 90°? i , X jkmid , X kAnd whether the angle θ2 between them is also greater than 90°. If both are greater than 90°, assuming that θ1>=θ2, then X ijmid Insert to X i , X k Remove X at the same time j (The same applies if θ2>=θ1); if only one of θ1 and θ2 is greater than 90°, insert the point greater than 90° into the path. Here, assuming that only θ1 is greater than 90°, insert X ijmid Insert to X i , X k Remove X at the same time j (Only when θ2 is greater than 90°, the same applies); if neither is greater than 90°, proceed to the next step of judgment.

[0104] 3. Judge X i , X ijmid , X jkmid Is the angle θ3 between them greater than 90°? ijmid , X jkmid , X k And whether the angle θ4 between them is also greater than 90°, if all are satisfied, then X ijmid , X jkmid Insert to X i , X k Remove X at the same time j ; If only one of θ3 and θ4 satisfies greater than 90°, here we assume that θ3 satisfies greater than 90° and θ4 does not satisfy greater than 90° (and vice versa), then X ijmid Insert to X i Then, after X jkmid and X k On the line (direction from X jkmid Point to X k ) Take k points on the connecting line and continue to determine whether the angle is greater than 90° until it is satisfied.

[0105] S5. Perform local planning according to the second global path and the environmental information to obtain a local planned path;

[0106] S6. The unmanned boat travels according to the local planned path.

[0107] Compared with the prior art, the present invention collects the environmental information and depth information around the unmanned boat in real time through sensors, and fuses the environmental information and depth information to obtain a planning map; calculates the boundary information of obstacles in the planning map based on the edge detection algorithm to obtain the obstacle boundary information; constructs a heuristic function based on the obstacle boundary information and the path target point of the unmanned boat, guides the RRT algorithm to sample through the heuristic function to obtain the first global path; performs pruning and smoothing on the first global path to obtain the second global path; performs local planning based on the second global path and environmental information to obtain the local planning path; the unmanned boat travels according to the local planning path. In this way, the present invention effectively improves the efficiency and quality of path planning.

[0108] It should be noted that, in this article, the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, an element defined by the sentence "comprises a ..." does not exclude the existence of other identical elements in the process, method, article or device including the element.

[0109] The embodiments of the present invention are described above in conjunction with the accompanying drawings. What is disclosed is only the preferred embodiment of the present invention. However, the present invention is not limited to the above-mentioned specific implementation manner. The above-mentioned specific implementation manner is only illustrative rather than restrictive. Under the enlightenment of the present invention, ordinary technicians in this field can also make many forms and equivalent changes without departing from the scope of protection of the purpose of the present invention and the claims, all of which are within the protection of the present invention.

Claims

1. A path planning method for an unmanned boat based on an improved RRT algorithm, characterized in that: The following steps are involved: S1. Collecting environmental information and depth information around the unmanned boat in real time through sensors, and fusing the environmental information and the depth information to obtain a planning map; S2. Calculating the boundary information of the obstacles in the planning map based on an edge detection algorithm to obtain the obstacle boundary information; S3, constructing a heuristic function according to the obstacle boundary information and the path target point of the unmanned boat, and guiding the RRT algorithm to perform sampling through the heuristic function to obtain a first global path; S4, performing pruning and smoothing on the first global path to obtain a second global path; S5. Perform local planning according to the second global path and the environmental information to obtain a local planned path; S6. The unmanned boat travels according to the local planned path.

2. The unmanned boat path planning method based on the improved RRT algorithm as claimed in claim 1, characterized in that: Step S2 includes the following sub-steps: S21, performing Gaussian smoothing on the planning map to obtain a smoothed planning map; S22, calculating the gradient of the smooth planning map by an edge detection operator to obtain a gradient score; The gradient includes magnitude and direction; S23, calculating the gradient magnitude and gradient direction of each pixel in the smooth planning map according to the gradient score to obtain a gradient magnitude map; S24, performing non-maximum suppression processing on the gradient amplitude map; S25, performing edge detection and edge connection on the gradient amplitude map after non-maximum suppression processing according to preset rules to obtain edge point information; S26. Calculate according to the edge point information to obtain the obstacle boundary information.

3. The unmanned boat path planning method based on the improved RRT algorithm as claimed in claim 2, characterized in that: The preset rule is: the edge of the gradient amplitude image is detected according to a first preset threshold and a second preset threshold, wherein a pixel point greater than the first preset threshold is a strong edge, a pixel point less than the second preset threshold is a weak edge, the strong edge is retained, the weak edge is removed, and a pixel point less than the first preset threshold and greater than the second preset threshold and connected to the strong edge is retained; The strong edge and the weak edge are connected by an edge tracking algorithm.

4. The unmanned boat path planning method based on the improved RRT algorithm as claimed in claim 2, characterized in that: The edge detection operator is a Sobel operator.

5. The unmanned boat path planning method based on the improved RRT algorithm as claimed in claim 1, characterized in that: Step S3 includes the following sub-steps: S31, randomly generating sampling points based on the heuristic function, and determining whether the sampling points are obstacle points in the obstacle boundary information, if so, regenerating new sampling points; if not, proceeding to step S32; S32, calculating a child node on the line connecting the current node and the sampling point according to a preset step length, and determining whether the line connecting the current node and the sampling point touches an obstacle, and if so, returning to step S31; If not, the current node is used as the parent node of the child node and step S33 is performed; S33, reselecting the parent node of the child node; S34, determining whether the child node is the path target point, if so, backtracking to obtain the first global path output, if not, returning to step S31.

6. The unmanned boat path planning method based on the improved RRT algorithm as claimed in claim 1, characterized in that: The edge detection algorithm is a canny edge detection algorithm.

Images