Robot motion path planning method and system based on improved RRT-Connect algorithm

Abstract

The application provides a robot motion path planning method based on an improved RRT-Connect algorithm, and comprises the following steps: a free space model of robot motion is established, and a starting point and a target point of robot motion in the free space are determined; meanwhile, RRT random trees are respectively expanded with the starting point and the target point as starting points; in a search process, new nodes are searched based on a dynamic step length strategy and a target bias strategy; each searched new node is added to a corresponding random tree, until the two random trees meet in the free space, an optimal path generated after the two random trees are connected is selected and saved; and a generated optimal path is smoothed by using a greedy algorithm. The dynamic step length strategy is introduced to control the step length in the random tree expansion process, so that the path search efficiency is effectively improved; meanwhile, the target bias strategy is introduced to constrain the direction in the random sampling process, so that each sampling is more close to the target point, and the sampling efficiency of the algorithm is improved.

Description

Technical Field

[0001] This invention relates to the field of path planning technology, and in particular to a robot motion path planning method and system based on an improved RRT-Connect algorithm. Background Technology

[0002] Intelligent robots inevitably encounter various obstacles during their movement, and the ability to flexibly and in real-time avoid these obstacles is a key performance indicator. Path planning algorithms refer to finding a collision-free path between the starting point and the target point within a finite time, ensuring that the robot will not collide during its movement. Currently, they are widely used in fields such as autonomous driving and autonomous exploration.

[0003] The Extended Random Tree (RRT) algorithm, as a widely used path planning algorithm, can detect sampling points in the state space and obtain information on sampling points and obstacles that may collide. It can directly solve path planning problems in high-dimensional space without the need for spatial modeling.

[0004] While the RRT algorithm has significant advantages, it also has notable drawbacks. The strong randomness of node expansion necessitates a large number of iterations to find feasible paths, consuming considerable time and failing to guarantee that the selected path is optimal. Furthermore, the fixed expansion step size limits the effectiveness of the random tree when approaching threat areas, and bypassing threat areas is time-consuming. Summary of the Invention

[0005] This invention proposes a robot motion path planning method and system based on an improved RRT-Connect algorithm, which solves the problems of long time consumption and inability to guarantee that the selected path is the optimal path in the existing path planning algorithm.

[0006] The technical solution of this invention is implemented as follows:

[0007] According to one aspect of the present invention, a robot motion path planning method based on an improved RRT-Connect algorithm is provided, comprising the following steps:

[0008] Establish a free-space model of the robot's motion and determine the starting point Q of the robot's motion in free space. init and target point Q goal ;

[0009] Simultaneously starting from point Q init and target point Q goal Starting from the initial point, extend the RRT random trees T1 and T2 respectively. During the search process, a new node Q is searched based on a dynamic step size strategy and a target bias strategy. newEach time a new node is found, it is added to the corresponding random tree until the two random trees meet in free space. The optimal path generated by connecting the two random trees is selected and saved.

[0010] A greedy algorithm is used to smooth the generated optimal path.

[0011] As a preferred embodiment of the present invention, the method for searching new nodes based on a dynamic step-size strategy is as follows:

[0012] Expand the random tree to new nodes with an initial step size ρ0;

[0013] When expanding to a new node, if no collision between the random tree and an obstacle is detected, and the two random trees are not connected, then the random tree expansion step size is set to s. i =s i-1 +ρ′, continue to expand new nodes; where s i s represents the current expansion step size of the random tree. i-1 ρ' represents the step size of one expansion on the random tree, and ρ′ represents the fixed expansion step size.

[0014] If a collision between the random tree and an obstacle is detected, the current new node is discarded, and the random tree expansion step size is set to s. i =s i-1 -ρ′, continues to expand new nodes based on the previous expansion;

[0015] If a connection between two random trees is detected, the expansion of the random trees is terminated.

[0016] As a preferred embodiment of the present invention, the method for searching new nodes based on a target bias strategy is as follows:

[0017] During the random sampling process of the random tree, a probability value p is randomly generated with uniform probability.

[0018] If the probability value p is less than the set threshold p bias Then the sampling point Q rand Select target point Q goal ;

[0019] If the probability value p is greater than the set threshold p bias Then the sampling points expand randomly in free space, and the direction of new node generation is constrained. The calculation process of the new node is as follows:

[0020]

[0021]

[0022]

[0023] Qnew =Q near +ρ·cosθ

[0024] Among them, Q near ρ represents the node in the random tree that is closest to the random sampling point, and ρ is the current expansion step size of the random tree.

[0025] As a preferred embodiment of the present invention, the method for smoothing the optimal path is as follows:

[0026] The set of nodes corresponding to the optimal path obtained by connecting two random trees is as follows:

[0027] path(Q0, Q1, ..., Q) n )

[0028] Where Q0 represents the starting point Q init Q n Represents the target point Q goal ;

[0029] Let Q temp =Q0, and use Q temp With Q1, Q2, ..., Q respectively n Connection, when Q temp With Q i When the connection encounters an obstacle, Q will... i-1 Store the path in the path cache array T′, where i = 1, 2, ..., n;

[0030] Let Q temp =Q i-1 and use Q temp respectively with Q i Q i+1 Q n Connection, when Q temp With Q m When the connection encounters an obstacle, Q will... m-1 Store it in the path cache array T′, where m = i, i+1, ..., n;

[0031] Repeat the above steps until Q temp With Q n The connection encountered no obstacles;

[0032] Connect Q0, the adjacent nodes in array T′, and Q in sequence. n Generate the path after pruning the random tree.

[0033] According to another aspect of the present invention, a robot motion path planning system is provided, comprising:

[0034] Model building unit: used to build a free space model and determine the starting point and target point of the robot's movement in free space;

[0035] Optimal path determination unit: Based on the RRT-Connect algorithm, a dynamic step size strategy and a target bias strategy are introduced to expand the random tree and determine the optimal path from the starting point to the target point;

[0036] Path optimization module: The optimal path is smoothed using a greedy algorithm to obtain the optimized planned path.

[0037] According to another aspect of the present invention, a storage medium is provided that stores a computer program, which, when executed by a processor, implements the steps of the path planning method described above.

[0038] Beneficial effects

[0039] Compared with existing technologies, the advantages of this invention are as follows: Based on the RRT-Connect algorithm, this invention introduces a dynamic step size strategy to control the step size during the random tree expansion process, effectively improving the path search efficiency; at the same time, it introduces a target bias strategy to constrain the direction during the random sampling process, so that each sampling can get closer to the target point, reducing the amount of computation and improving the sampling efficiency of the algorithm; finally, it uses a greedy algorithm to smooth the search path, which significantly improves the convergence speed of the improved algorithm, while also ensuring the quality of the path planned in this way. Attached Figure Description

[0040] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0041] Figure 1 This is a flowchart illustrating a robot motion path planning method based on an improved RRT-Connect algorithm according to the present invention.

[0042] Figure 2 This is a schematic diagram illustrating the simulation results of three path planning algorithms in a simple obstacle environment according to an embodiment of the present invention.

[0043] Figure 3 This is a schematic diagram illustrating the simulation effects of three path planning algorithms in a complex obstacle environment according to an embodiment of the present invention. Detailed Implementation

[0044] The technical solution of the present invention will be clearly and completely described below with reference to the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present invention.

[0045] Reference Figure 1 As shown, this embodiment provides a robot motion path planning method based on the improved RRT-Connect algorithm, including the following steps:

[0046] Establish a free-space model of the robot's motion and determine the starting point Q of the robot's motion in free space. init and target point Q goal ;

[0047] Simultaneously starting from point Q init and target point Q goal Extend the RRT random trees T1 and T2 respectively, starting from the same point;

[0048] During the search process, since the step size in the RRT-Connect algorithm is fixed, using a given step size will limit the expansion speed of the random tree when exploring the barrier-free space. Therefore, this embodiment introduces a dynamic step size strategy to search for new nodes. The specific method is as follows:

[0049] First, expand the new nodes of the random tree with an initial step size ρ0;

[0050] When expanding to a new node, if no collision between the random tree and an obstacle is detected, and the two random trees are not connected, then the random tree expansion step size is set to s. i =s i-1 +ρ′, continue to expand new nodes; where s i s represents the current expansion step size of the random tree. i-1 ρ' represents the step size of one expansion on the random tree, and ρ′ represents the fixed expansion step size.

[0051] If a collision between the random tree and an obstacle is detected, the current new node is discarded, and the random tree expansion step size is set to s. i =s i-1 -ρ′, continues to expand new nodes based on the previous expansion;

[0052] If a connection between two random trees is detected, the expansion of the random trees is terminated.

[0053] The RRT-Connect algorithm can significantly improve search speed by expanding bidirectional random trees; however, considering that there may be a large randomness of sampling points during the expansion process using this algorithm, as well as the situation that the sampling area is too scattered, it is easy to have low search result efficiency and the corresponding path may be relatively coarse.

[0054] To address the aforementioned issues, this embodiment introduces a target-biased strategy to search for new nodes. The specific method is as follows:

[0055] During the random sampling process of the random tree, a probability value p is randomly generated with uniform probability, p←rand(0,1);

[0056] If the probability value p is less than the set threshold p bias Then the sampling point Q rand Select target point Q goal ;

[0057] If the probability value p is greater than the set threshold p bias Then the sampling points expand randomly in free space, and the direction of new node generation is constrained.

[0058] This embodiment introduces a random point Q obtained from random sampling. rand to target point Q goal The direction vector of the random node Q makes the random node Q rand The generation is directed toward the target point Q. goal The expansion proceeds in the direction of the target point. A direction vector is introduced, meaning that during the expansion process, the selection of the new node's position is not only related to the random point but also to the target point, within the original direction. Add direction above The component in the direction of the target point will affect the position of the new node, at which point the new node will not be in Q. near With Q rand The growth direction is not along the line connecting the points, but rather towards the target point Q. goal The calculation process for the new node is as follows:

[0059]

[0060]

[0061]

[0062] Q new =Q near +ρ·cosθ

[0063] Among them, Q nearρ represents the node on the random tree that is closest to the random sampling point, and ρ is the current expansion step size of the random tree. By constraining the sampling process, we can ensure that each sampling tends to the target point. This can prevent the random sampling point from searching backwards while ensuring the directionality of the sampling and improving the efficiency of feasible paths.

[0064] Each time a new node is found, it is added to the corresponding random tree until the two random trees meet in free space. The optimal path generated by connecting the two random trees is selected and saved.

[0065] Finally, a greedy algorithm is used to smooth the generated optimal path. The specific method is as follows:

[0066] The set of nodes corresponding to the optimal path obtained by connecting two random trees is as follows:

[0067] path(Q0, Q1, ..., Q) n )

[0068] Where Q0 represents the starting point Q init Q n Represents the target point Q goal ;

[0069] Let Q temp =Q0, and use Q temp With Q1, Q2, ..., Q respectively n Connection, when Q temp With Q i When the connection encounters an obstacle, Q will... i-1 Store the data in the path cache array T′, where i = 1, 2, ..., n;

[0070] Let Q temp =Q i-1 and use Q temp respectively with Q i Q i+1 Q n Connection, when Q temp With Q m When the connection encounters an obstacle, Q will... m-1 Store it in the path cache array T′, where m = i, i+1, ..., n;

[0071] Repeat the above steps until Q temp With Q n The connection encountered no obstacles;

[0072] Connect Q0, the adjacent nodes in array T′, and Q in sequence. n Generate the path after pruning the random tree.

[0073] Corresponding to the path planning method described above, this embodiment also provides a robot motion path planning system, including:

[0074] Model building unit: used to build a free space model and determine the starting point and target point of the robot's movement in free space;

[0075] Optimal path determination unit: Based on the RRT-Connect algorithm, a dynamic step size strategy and a target bias strategy are introduced to expand the random tree and determine the optimal path from the starting point to the target point;

[0076] Path optimization module: The optimal path is smoothed using a greedy algorithm to obtain the optimized planned path.

[0077] This embodiment also provides a storage medium storing a computer program that, when executed by a processor, implements the steps of the path planning method described above.

[0078] like Figure 2 The diagram shows the simulation results of three path planning algorithms in a simple obstacle environment. Figure 2 Figure (a) shows the effect of using the traditional RRT algorithm for path planning. Figure 2 Figure (b) shows the effect of using the traditional RRT-Connect algorithm for path planning. Figure 2 Figure (c) shows the effect of using the improved RRT-Connect algorithm of this embodiment for path planning, and Table 1 below shows the simulation data of three path planning algorithms under simple obstacle conditions:

[0079] Table 1 Simulation data of three path planning algorithms in simple obstacle environments

[0080]

[0081]

[0082] As can be seen from Figure 2 and Table 1 above, the improved RRT-Connect algorithm of this embodiment saves 91% of the time compared to the RRT algorithm and 40% of the time compared to the RRT-Connect algorithm; it also saves a lot of path length compared to both the RRT algorithm and the RRT-Connect algorithm.

[0083] like Figure 3 The diagram shows the simulation results of three path planning algorithms in a complex obstacle environment. Figure 3 Figure (a) shows the effect of using the traditional RRT algorithm for path planning. Figure 3Figure (b) shows the effect of using the traditional RRT-Connect algorithm for path planning. Figure 3 Figure (c) shows the effect of using the improved RRT-Connect algorithm of this embodiment for path planning, and Table 1 below shows the simulation data of three path planning algorithms in complex obstacle environments:

[0084] Table 2 Simulation data of three path planning algorithms in complex obstacle environments

[0085] algorithm Time / s Path length RRT 21.7249 9.728123e+02 RRT-Connect 5.1772 9.425457e+02 Improved RRT-Connect 1.7475 9.217032e+02

[0086] Depend on Figure 3 As shown in Table 2 above, the improved RRT-Connect algorithm of this embodiment saves 91% of the time compared to the original RRT algorithm and 66% compared to the RRT-Connect algorithm; it also saves a significant amount of path length compared to both the original RRT and RRT-Connect algorithms. The improved RRT-Connect algorithm reduces unnecessary expansion during the random tree expansion process compared to the original RRT and RRT-Connect algorithms, greatly shortening the search time.

[0087] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A robot motion path planning method based on an improved RRT-Connect algorithm, characterized in that, The method comprises the following steps: establishing a free space model of the robot motion and determining a starting point for the robot motion in the free space and a target point ; At the same time, the starting point and the target point are taken as the starting points to respectively expand the RRT random trees T1 and T2, and in the searching process, a new node is searched based on a dynamic step length strategy and a target bias strategy , and the new node is added to the corresponding random tree after each new node is searched, until the two random trees meet in the free space, and an optimal path generated after the two random trees are connected is selected and saved. The method for searching a new node based on a dynamic step strategy is: with an initial step size expand the new node of the random tree; When extending to a new node, if no collision between the random tree and the obstacle is detected, and the two random trees are not connected, the step length of the random tree is set to , and the new node is continued to be extended. wherein, s i denotes the current expansion step size of the random tree, s i-1 denotes the last expansion step size of the random tree, denotes the fixed expansion step size; If a collision between the random tree and the obstacle is detected, the current new node is discarded, and the random tree expansion step size is set to the new node is continued to be expanded on the basis of the last expansion; If two random trees are detected to be connected, the expansion of the random trees is ended; The method for searching a new node based on a target bias strategy is: In the process of random sampling in a random tree, a probability value is randomly generated with uniform probability p ; If the probability value p is less than a set threshold , then the sampling point is selected as the target point ; If the probability value p is greater than a set threshold value , the sampling point is randomly expanded in free space, and the direction of the new node is constrained. The calculation process of the new node is: ； wherein, denotes the node on the random tree that is closest to the randomly sampled point, is the current expansion step size of the random tree; The optimal path is smoothed by using a greedy algorithm.

2. The robot motion path planning method based on the improved RRT-Connect algorithm according to claim 1, wherein, The method for smoothing the optimal path is: The node set corresponding to the optimal path obtained after the connection of the two random trees is: ； wherein, represents a starting point , represents a target point ; Let and use respectively with connection, when with connection will encounter obstacles, will stored in the path cache array , where i =1,2,…, n ; Let and use respectively with connection, when with connection will encounter obstacles, will stored in the path cache array , where m = i , i +1,…, n ; Repeat the above steps until With No obstacles encountered on the connection; sequentially connecting , array adjacent nodes in the array and , generating a pruned path of the random tree.

3. A robot motion path planning system based on the path planning method of claim 1 or 2, characterized by Comprise: The model establishing unit is used for establishing a free space model and determining a starting point and a target point of the movement of the robot in the free space; The optimal path determining unit is used for expanding the random trees by introducing a dynamic step strategy and a target bias strategy on the basis of the RRT-Connect algorithm, and determining an optimal path between the starting point and the target point; The path optimization module is used for smoothing the optimal path by using a greedy algorithm, so as to obtain an optimized planning path.

4. A storage medium, characterized by The storage medium stores a computer program, and the computer program is executed by the processor to realize the steps of the path planning method in claim 1 or 2.

Images