An improved ship path planning method based on Rapidly-Exploring Random Trees (RRT)

Abstract

The present invention discloses an improved ship path planning method based on the Rapidly-exploring Random Tree (RRT). First, the concept of gravitational potential field centered at the destination is introduced. By superimposing the direction of randomly expanding nodes and the direction of the gravitational potential field, a new node expansion direction is determined to constrain the expansion direction of the random tree in the RRT algorithm, ensuring that the expansion direction of the tree always approaches the destination, thereby reducing the invalid branches during the growth of the tree. And during the expansion process of the path tree, the situation of obstacles around the ship is considered, so that the target point can be reached faster. In addition, this method uses the inscribed circle of a triangle method to smooth the path inflection points, making the finally generated path more suitable for practical applications. Finally, simulation data under the same conditions are used to verify the effectiveness and certain advancement of this method, which is very suitable for real-time path planning in dynamic scenarios, especially for ship path planning in the navigation field.

Description

Technical Field

[0001] The present invention belongs to the technical field of path planning, and particularly relates to a ship path planning method. Background Art

[0002] Maritime transportation plays an indispensable role in international trade. It has the advantages of large transportation volume and low cost. However, the losses caused by maritime accidents due to reasons such as crew judgment errors are often huge, such as loss of life, economic losses, and environmental pollution. The research on ship path planning is considered to be one of the effective ways to solve this problem, which helps to reduce the number of maritime traffic accidents caused by human factors.

[0003] For path planning, planning methods represented by the Rapidly-exploring Random Tree (RRT) have achieved a large number of results in actual planning. The path planned by the RRT algorithm is based on tree-like growth and random sampling, which can quickly explore the map space and is suitable for solving dynamic path planning problems. Secondly, the RRT algorithm based on random sampling does not require all points in the search space, nor does it need to accurately construct the obstacle space. And the growth mode of the RRT path tree is random sampling growth. Although the running results for the same task are not repeatable, the randomness of its planning results has little impact on the feasible paths required for ship path planning. Therefore, the RRT-based algorithm has been widely applied to robot motion planning problems such as robot arm path planning and Unmanned Aerial Vehicle (UAV) motion planning.

[0004] However, due to the randomness of the growth nodes of the RRT algorithm, invalid nodes will be generated during the path planning process, affecting the efficiency of path planning. And the straight-line connection between the nodes of the RRT algorithm makes the finally obtained path generally tortuous and non-smooth, with a large gap from the actual ship navigation path.

[0005] In summary, aiming at the shortcomings existing in the RRT algorithm, it has important research significance to study a ship path planning method suitable for complex sea areas, that is, to enhance the purpose orientation of the method, thereby reducing the invalid nodes generated during the method planning process, and at the same time the finally generated planning path of the method can meet the requirements of ships in complex sea areas. Summary of the Invention

[0006] To overcome the deficiencies of the prior art, the present invention provides an improved ship path planning method based on the Rapidly-exploring Random Tree (RRT). First, the concept of a gravitational potential field centered on the destination is introduced. By superimposing the direction of randomly expanding nodes with the direction of the gravitational potential field, a new node expansion direction is determined to constrain the expansion direction of the random tree in the RRT algorithm, ensuring that the expansion direction of the tree always approaches the destination, thereby reducing the invalid branches during the tree growth process. And during the expansion process of the path tree, the situation of obstacles around the ship is considered, so that the target point can be reached faster. In addition, the method uses the inscribed circle of a triangle to smooth the path inflection points, making the finally generated path more suitable for practical applications. Finally, simulation data under the same conditions is used to verify the effectiveness and certain advancement of the method, which is very suitable for real-time path planning in dynamic scenarios, especially for ship path planning in the navigation field.

[0007] The technical solution adopted by the present invention to solve its technical problems includes the following steps:

[0008] Step 1: Select sea area and ship data, and perform regional obstacle environment modeling;

[0009] Step 1-1: Obtain sea area image data and perform binarization processing;

[0010] The binarized sea area image after binarization processing is represented as F = {f ij}, where f ij is the gray value of the pixel point (i, j); use the raster scanning method to scan the image F. When the gray value f ij ≠0 of the pixel point (i, j) is scanned, the contour of the connected domain of f ij is extracted, and finally the contour point set of the sea area obstacle image is output;

[0011] Step 1-3: Use the least squares circle fitting to fill the sea area obstacles, and establish the least squares fitting circle curve R 2 =(x - A) 2 +(y - B) 2 , where R is the radius of the fitting circle, (A, B) is the coordinate of the center of the fitting circle, and x and y are the coordinates of each point on the smallest circumscribed circle corresponding to the sea area obstacles;

[0012] Step 1-4: For the contour point set (X i , Y i ) obtained in Step 1-2, i ∈ {1, 2, 3,......, N}, the coordinate of the center of the smallest inscribed circle corresponding to each obstacle contour is (A i , B i ), the radius of the circle is R i , X i , Y iThe distance to the center of the circle is d i , N represents the number of obstacles in this area;

[0013] Let When is minimized, solve for a i , b i , c i , then the fitting circle parameters are

[0014] Step 2: Complete the initialization of nodes, single expansion step size, and path tree;

[0015] Take the starting point of the ship as the initial point q init , the initial value of the single expansion step size is equal to the ship speed, and the path tree is initialized to

[0016] Step 3: Perform ship-obstacle collision detection and update the expansion direction according to the gravitational potential field;

[0017] Step 3-1: Define the tendency of the expansion point towards the target point as the gravitational potential field. In the expansion direction of the path tree, adopt the growth direction after superimposing the gravitational potential field and the random direction; the direction of the gravitational potential field is defined as pointing from the growth point to the target point, and the magnitude of the gravitational potential field where is the gravitational gain constant, d 2 (q, q goal ) is the square of the distance between the expansion point q and the target point q goal , S is the area of the obstacle closest to the growth point, and d * is the obstacle influence range;

[0018] Step 3-2: Within the range with the single expansion step size as the radius, judge the presence of obstacles. If there are no obstacles, select the target point as the random tree growth direction, and the calculation of the expansion direction for the new node is given by the formula ; if there are obstacles, select the random growth method combined with the gravitational potential field, and the calculation of the expansion direction for the new node is given by the formula , where t is the single expansion step size;

[0019] Step 3-3: Select a random node q rand according to the expansion direction in Step 3-2;

[0020] Step 4: Perform random point selection and path tree filling;

[0021] Step 4-1: Traverse each node in the random tree, calculate the distance between each node and the random point generated in this loop, find the node closest to this random point, and denote it as q near ;

[0022] Step 4-2: According to the single-step expansion length in Step 2, when q is found near at this time, q near extends this length in the direction of q rand , and a new node q new is generated after the extension; perform a collision detection between q new and the obstacle. If a collision occurs, a new random point is generated again; if no collision occurs, then (q near , q new ) is added to the path tree;

[0023] Step 5: Perform an arrival detection to determine whether the planning method continues;

[0024] Calculate the Euclidean distance between the current point and the target point. If this distance is less than the single-step expansion length, end the path planning and output the planned path set P; if this distance is greater than the single-step expansion length, repeat Step 3 and Step 4;

[0025] Step 6: Obtain the planned path and perform path smoothing;

[0026] Step 6-1: For the planned path set p i ∈P, i ∈ {1, 2, 3,......, n}, and divide the path set p k = {p k , p k+1 , p k+2};

[0027] Step 6-2: Calculate the slopes of the line segments formed by p k , p k+1 and p k+1 , p k+2 . If the slopes are equal, no trajectory smoothing is performed; if the slopes are not equal, go to Step 6-3;

[0028] Step 6-3: Calculate the incenter of the triangle formed by the corresponding three points of P k l1, l2, l3 are the side lengths of the triangle formed by the three points; replace the trajectory of the corresponding path segment with an inscribed circle arc to complete path smoothing. Preferably, the binarization process is specifically:

[0029] According to the function

[0030] perform binarization processing on the image. For the points in the image where the gray value src(x, y) is greater than the threshold thresh, set its gray value to 255, and for the points where the gray value is less than or equal to the threshold, set its gray value to 0. The beneficial effects of the present invention are as follows:

[0031] ​

[0032] The present invention is not only applicable to the environment with complex marine obstacles, but also solves the problems of a large number of invalid nodes and high storage cost existing in the traditional rapidly-exploring random tree (RRT) algorithm by guiding the nodes to face the target point as much as possible during the growth process. On this basis, the time required for path planning is reduced. The present invention realizes the smoothing process of the planned path and solves the drawback that the path planned by the traditional RRT algorithm is tortuous and uneven. The present invention is very suitable for real-time path planning in dynamic scenarios, especially for ship path planning in the navigation field. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] Figure 1 It is a flowchart of the method of the present invention.

[0034] Figure 2 It is a result diagram of ship path planning in Dajiaoshan Sea area in the embodiment of the present invention.

[0035] Figure 3 It is a comparison diagram of the number of path nodes in the embodiment of the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0036] The present invention will be further described below in conjunction with the drawings and embodiments.

[0037] An improved ship path planning method based on rapidly-exploring random tree (RRT) includes the following steps:

[0038] Step 1: Select sea area and ship data, and perform regional obstacle environment modeling;

[0039] Step 1-1: Obtain sea area image data and perform binary processing; according to the function perform binary processing on the image. For the points in the image where the gray value src(x, y) is greater than the threshold thresh, set its gray value to 255, and for the points where the gray value is less than or equal to the threshold, set its gray value to 0;

[0040] Step 1-2: The binary-processed sea area image is represented as F = {f ij}, where f ij is the gray value of the pixel point (i, j); use the raster scanning method to scan the image F, and when the gray value f ij ≠0 of the pixel point (i, j) is scanned, extract the contour of the connected domain of f ij , and finally output the set of contour points of the sea area obstacle image;

[0041] Step 1-3: Use least squares circle fitting to fill the sea area obstacles, and establish a least squares fitting circle curve R 2 =(x - A) 2 +(y - B)2 where R is the radius of the fitted circle, (A, B) are the coordinates of the center of the fitted circle, and x and y are the coordinates of each point on the minimum circumscribed circle corresponding to the sea area obstacle;

[0042] Step 1-4: For the set of contour points (X i , Y i ) obtained in Step 1-2, i ∈ {1, 2, 3,......, N}, the coordinates of the center of the minimum inscribed circle corresponding to each obstacle contour are (A i , B i ), the circle radius is R i , the distance from X i , Y i to the center of the circle is d i , N represents the number of obstacles in the area;

[0043] Let When is minimized, solve for a i , b i , c i , then the fitting circle parameters are

[0044] Step 2: Initialize the nodes, single expansion step size, and path tree;

[0045] Take the starting point of the ship as the initial point q init , initialize the single expansion step size to be equal to the ship's speed, and initialize the path tree to

[0046] Step 3: Perform ship-obstacle collision detection and update the expansion direction based on the gravitational potential field;

[0047] Step 3-1: Define the tendency of the expansion point towards the target point as the gravitational potential field. In the expansion direction of the path tree, use the growth direction after superimposing the gravitational potential field and the random direction; the direction of the gravitational potential field is defined as pointing from the growth point to the target point, and the magnitude of the gravitational potential field where is the gravitational gain constant, d 2 (q, q goal ) is the square of the distance between the expansion point q and the target point q goal , S is the area of the obstacle closest to the growth point, and d * is the obstacle influence range;

[0048] Step 3-2: Within the range with the single expansion step size as the radius, determine the existence of obstacles. If none exist, select the target point as the random tree growth direction. The calculation of the expansion direction for the new node is given by the formula Given; if there are obstacles, a random growth method combined with the gravitational potential field is selected, and the calculation of the expansion direction of the new node is given by the formula where t is the single expansion step length;

[0049] Step 3-3: Select a random node q according to the expansion direction in Step 3-2 rand ;

[0050] Step 4: Perform random point selection and path tree filling;

[0051] Step 4-1: Traverse each node in the random tree, calculate the distance between each node and the randomly generated point in this loop, and find the node closest to this random point, denoted as q near ;

[0052] Step 4-2: According to the single expansion step length in Step 2, when q near is found, q near expands this step length in the direction of q rand , and a new node q new is generated after the expansion; perform a collision detection between q new and the obstacle. If a collision occurs, a new random point is generated again; if no collision occurs, then (q near , q new ) is added to the path tree;

[0053] Step 5: Perform arrival detection to determine whether the planning method continues;

[0054] Calculate the Euclidean distance between the current point and the target point. If this distance is less than the single expansion step length, the path planning ends and the planned path set P is output; if this distance is greater than the single expansion step length, then repeat Step 3 and Step 4;

[0055] Step 6: Obtain the planned path and perform path smoothing;

[0056] Step 6-1: For the planned path set p i ∈P, i ∈ {1, 2, 3,......, n}, and divide the path set p k ={p k , p k+1 , p k+2};

[0057] Step 6-2: Calculate the slopes of the line segments formed by p k , p k+1 and p k+1 , p k+2 . If the slopes are equal, no trajectory smoothing is performed; if the slopes are not equal, then go to Step 6-3;

[0058] Step 6-3: Calculate Pk The incenter of the triangle formed by the corresponding three points Let \(l_1\), \(l_2\), and \(l_3\) be the side lengths of the triangle formed by the three points; replace the trajectory of the corresponding path segment with an inscribed circle arc to complete path smoothing. Specific embodiments:[[]]

[0060] The present invention will be described in detail below in conjunction with the accompanying drawings, data of the sea area near Dajiaoshan Island (longitude range: 122.1681° - 122.1038°, latitude range: 30.2091° - 30.2405°), and an embodiment of a simulated ship (ship speed is 25).

[0061] 1. Select sea area and ship data to perform regional obstacle environment modeling.

[0062] 1.1 Select the image data of the sea area near Dajiaoshan Island here and perform binary threshold processing on the image. For points with a gray value greater than the threshold, set its gray value to 255, and for points with a gray value less than or equal to the threshold, set its gray value to 0.

[0063] 1.2 Determine the input image pixels as \(F\) 大蛟山 =\(\{f\) ij \}\), use the raster scanning method to scan the Dajiaoshan image, and perform contour extraction steps on pixel points with \(f\) ij \(\neq0\), and finally output the contour point set of the obstacle image in the Dajiaoshan sea area.

[0064] 1.3 Use the least squares circle fitting to fill the obstacles and establish a least squares fitting circle curve.

[0065] 1.4 Traverse the contour point set obtained in step 1.2, construct and solve a system of equations based on the distance from each point in the contour to the obstacle center. The obstacle modeling coordinate set in the sea area near Dajiaoshan in this embodiment is \(\{(x i , y i , r i )\}=\{(204, 0, 16), (64, 8, 12), (227, 39, 5), (173, 14, 57), (51, 54, 6), (28, 77, 4), (373, 126, 12), (238, 136, 15), (319, 152, 22)\}, where \(x i , y i represent the obstacle center coordinates, and \(r i represents the radius of the filled obstacle.

[0066] 2. Complete the initialization of nodes, single-step expansion length, and path tree.

[0067] 2.1 Use the starting point of the ship (30.2255, 122.1088) as the initial point, and use the Miller formula to convert longitude and latitude to the Cartesian coordinate system. The initial value of the single-step expansion length is equal to the ship's speed.

[0068] 3 Perform ship-obstacle collision detection and update the expansion direction based on the gravitational potential field.

[0069] 3.1 Within the range with the expansion distance as the radius, judge the existence of obstacles. If there are no obstacles, select the target point as the growth direction of the random tree, and the expansion direction of the new node is θ1 = arctan(x goal -x near , y goal -y near ), where x goal , y goal are the coordinates of the target point, and x near , y near are the coordinates of the nearest neighbor node.

[0070] 3.2 If there are obstacles, select the random growth method combined with the gravitational potential field, and the expansion direction of the new node is θ2 = arctan(x - x near , y - y near ), where the parameter

[0071] 4 Perform random point selection and path tree filling.

[0072] 4.1 Traverse each node in the random tree, calculate the distance between each node and the randomly generated point in this loop, and find the node closest to this random point, denoted as q near .

[0073] 4.2 When q near is found, q near expands this step length in the direction of q rand . Combining the judgment result of the existence of obstacles within the range with the expansion distance as the radius, the new node coordinates generated after expansion are q new = (t * cosθ, t * sinθ), θ = θ1 or θ2. Perform collision detection on the new node and the obstacle. If a collision occurs, generate a new random point again; if no collision occurs, add (q near , q new ) to the path tree.

[0074] 5 Perform arrival detection to determine whether the planning method continues.

[0075] 5.1 Calculate the Euclidean distance between the current point and the target point. If this distance is less than the single-step expansion length, end the path planning; if this distance is greater than the single-step expansion length, repeat steps 3 and 4.

[0076] 6 Near the sea area near Dajiao Mountain, combined with the surrounding obstacles, a ship path planning experiment was carried out to verify the effectiveness of the method, obtain the planned path, and smooth the path. The planning results are as Figure 2 shown. To further prove the superiority of the path planning method proposed in the present invention, it was compared with the RRT algorithm, RRT* algorithm, RRT-inform algorithm, RRT-bias algorithm, and RRT-AFF algorithm in terms of the algorithm running time and the length of the generated path. The experimental results are as Figure 3 shown. The present invention carried out 150 experiments in the same experimental environment, mainly comparing the mean and variance of indicators such as running time, planned path length, and success rate, and obtained the experimental results shown in Table 1 and Table 2.

[0077] Table 1 Comparison of the maximum and minimum values of running time and path length

[0078]

[0079] Table 2 Comparison of the mean and variance of running time and path length

[0080]

[0081] The experimental results verify that the method of the present invention is effective and has a certain degree of advancement, and is very suitable for real-time path planning in dynamic scenarios, especially for ship path planning in the navigation field.

Claims

1. An improved ship path planning method based on the Rapidly-exploring Random Tree (RRT), characterized in that, It includes the following steps: Step 1: Select sea area and ship data, and perform regional obstacle environment modeling; Step 1-1: Obtain sea area image data and perform binarization processing; Step 1-2: The binarized sea area image is represented as F = {f ij}, where f ij is the gray value of the pixel point (i, j); Scan the image F using the raster scanning method. When the gray value f ij ≠ 0 of the pixel point (i, j) is scanned, extract the contour of the connected domain of f ij , and finally output the contour point set of the sea area obstacle image; Step 1-3: Use least squares circle fitting to fill the sea area obstacles and establish a least squares fitting circle curve \(R\) 2 =(x - A) 2 +(y - B) 2 , where \(R\) is the radius of the fitting circle, \((A, B)\) are the coordinates of the center of the fitting circle, and \(x\) and \(y\) are the coordinates of each point on the minimum circumscribed circle corresponding to the sea area obstacles; Steps 1-4: For the set of contour points (X i , Y i ) obtained in Steps 1-2, where i ∈ {1, 2, 3, ……, N}, the coordinates of the center of the minimum inscribed circle corresponding to each obstacle contour are (A i , B i ), the radius of the circle is R i , the distance from X i , Y i to the center of the circle is d i , N represents the number of obstacles in this area; Let When is minimized, solve for a i , b i , c i , then the fitting circle parameters are Step 2: Complete the initialization of nodes, single expansion step length, and path tree; Take the starting point of the ship as the initial point q init , the initial value of the single expansion step size is equal to the ship speed, and the path tree is initialized to Step 3: Perform ship-obstacle collision detection and update the expansion direction according to the gravitational potential field; Step 3-1: Define the tendency of the expansion point towards the target point as the gravitational potential field. In the expansion direction of the path tree, adopt the growth direction after superimposing the gravitational potential field and the random direction; the direction of the gravitational potential field is defined as pointing from the growth point to the target point, and the magnitude of the gravitational potential field where is the gravitational gain constant, d 2 (q, q goal ) is the square of the distance between the expansion point q and the target point q goal , S is the area of the obstacle closest to the growth point, and d * is the obstacle influence range; Step 3-2: Within the range with a single expansion step length as the radius, determine the presence of obstacles. If there are no obstacles, select the target point as the growth direction of the random tree. The calculation of the expansion direction for the new node is given by the formula ; if there are obstacles, select the random growth method combined with the gravitational potential field. The calculation of the expansion direction for the new node is given by the formula where t is the single expansion step length. Step 3-3: Select a random node q according to the expansion direction in Step 3-2 rand ; Step 4: Perform random point selection and path tree filling; Step 4-1: Traverse each node in the random tree, calculate the distance between each node and the random point generated in this loop, find the node closest to this random point, and denote it as q near ; Step 4-2: According to the single-step expansion length in Step 2, when q near is found, q near expands this length in the direction of q rand , and a new node q new is generated after the expansion; perform a collision detection on q new and the obstacle. If a collision occurs, a new random point is generated again; if no collision occurs, then (q near , q new ) is added to the path tree. Step 5: Perform arrival detection to determine whether the planning method continues; Calculate the Euclidean distance between the current point and the target point. If the distance is less than the single expansion step length, end the path planning and output the planned path set P; if the distance is greater than the single expansion step length, repeat Step 3 and Step 4; Step 6: Obtain the planned path and perform path smoothing; Step 6-1: For the set of planned paths p i ∈P, i ∈ {1, 2, 3, ……, n}, and partition the set of paths p k = {p k , p k+1 , p k+2}; Step 6-2: Calculate p k , p k+1 and p k+1 , p k+2 of the slopes of the line segments formed. If the slopes are equal, no trajectory smoothing is performed; if the slopes are not equal, go to Step 6-3; Step 6-3: Calculate P k The incenter of the triangle formed by the corresponding three points Let l1, l2, and l3 be the side lengths of the triangle formed by the three points; replace the trajectory of the corresponding path segment with an inscribed circle arc to complete path smoothing.

2. An improved ship path planning method based on the rapidly-exploring random tree according to claim 1, characterized in that The specific binarization processing is as follows: According to the function Binarize the image. For the points in the image where the gray value src(x, y) is greater than the threshold thresh, set their gray values to 255, and for the points where the gray value is less than or equal to the threshold, set their gray values to 0.

Images