Self-adaptive step length RRT path planning method based on collision detection

Abstract

The invention relates to a self-adaptive step length RRT (Rapidly-expansion Random Tree, RRT) path planning method based on collision detection, and belongs to the field of robot path planning. The method comprises the following steps: firstly, initializing a system, namely initializing map information, determining a robot obstacle space and a non-obstacle space, determining position coordinates of a path initial point and a target point, a cardinal number of each connection step length, a distance target point threshold value and the like; then, path planning is carried out by using an improved RRT algorithm, including sampling in a non-obstacle space, optimization of random sampling points in combination with a target bias strategy and an APF algorithm, and expansion in a random tree expansion direction according to a dynamic step length strategy; then, judging whether the new node is smaller than a threshold value, if so, finding a path, otherwise, jumping to the sampling step; and finally, after the path is found, the path length is shortened by using a path pruning strategy, the final path is smoothed, and the path is output. Compared with a traditional RRT algorithm, the improved RRT algorithm has the advantages that the path searching speed is increased, redundant nodes are eliminated, and the path quality is improved.

Description

Technical Field

[0001] The invention relates to an adaptive step length RRT path planning method based on collision detection, and belongs to the field of robot path planning. Background Art

[0002] With the rapid development of robotics technology, mobile robotic arms have begun to be widely used in various fields, such as intelligent manufacturing, health care, warehousing and logistics, agricultural planting, military security, transportation, home services, entertainment and education. Traditional fixed bases and base-linked single rail robotic arms can only be used for operations in a small range in simple environments. They are difficult to operate in complex environments. The limited working space cannot meet people's current demand for the use of robotic arms. Robotic arms with high degrees of freedom cannot play their maximum value. Therefore, the demand for autonomous motion planning technology to generate collision-free paths has become more urgent, and path planning has become a core issue in the field of robotics.

[0003] Robot path planning refers to the process of how a robot moves from a starting position to a target position in a given environment while avoiding obstacles and satisfying robot kinematics and other conditions. Depending on the robot's working environment, path planning methods can be divided into global path planning and local path planning. Global path planning refers to planning an optimal path without collision in a known map environment; local path planning refers to planning a robot path that can avoid obstacles in real time in an unknown map environment or with dynamic obstacles. For global path planning problems, path methods are mainly divided into sampling-based motion planning methods and graph search-based motion planning methods. Traditional sampling-based motion planning methods mainly include RRT and PRM; search-based path planning methods mainly include A* algorithm and Dijkstra algorithm. Commonly used algorithms for local path planning problems include DWA and APF.

[0004] RRT is a classic sampling-based path planning algorithm. The main steps of the algorithm are to use the starting point as the root node, randomly sample in the robot's workspace to find the root node closest to the sampling point in the expansion tree, and connect the nearest node with the random sampling point according to the specified step length. After collision detection, if there is no collision, the sampling point is added to the tree. By connecting the nodes, the random tree grows continuously until the sampling stops within the threshold of the target point, and the path search is completed. As can be seen from the above steps, the generation of random nodes in the RRT algorithm is not goal-oriented, resulting in a large number of redundant points in the sampling process, and the expansion of nodes according to a fixed step length leads to a long path finding time. The turning point of the final path will cause the robot to change its acceleration greatly here, and even a singular point will appear that does not satisfy the robot kinematics. Summary of the invention

[0005] The technical problem to be solved by the present invention is to provide an adaptive step-size RRT path planning method based on collision detection. In the sampling process of the traditional RRT algorithm, a large number of redundant points will appear, and the expansion of nodes according to a fixed step size will result in a long path finding time. The turning point of the final path will cause the robot to change its acceleration greatly here, and even a singularity will occur that does not satisfy the robot's kinematics.

[0006] The technical solution of the present invention is: an adaptive step-length RRT path planning method based on collision detection, the specific steps are:

[0007] Step 1: Determine the robot's obstacle space C obj and non-obstacle space C free 、Initial point X start With the target point X goal Information, distance to target point threshold Thr, set step length base T according to environmental information step , bias probability P 0 , APF algorithm gravity coefficient ε and repulsion coefficient σ and obstacle influence distance ρ 0 .

[0008] Step 2: Generate random sampling points X rand , according to the target bias strategy with probability P 0 Point to target point X goal , with (1-P 0 ) in the non-obstacle space C free Random sampling, non-target point X goal The random sampling point Sample is combined with the APF potential field principle, and the target point X goal Given a random sample point X rand Attraction, obstacles in the obstacle space give it repulsion, and the random sampling point Sample forms a new sampling point X' in the direction of random tree expansion under the influence of the combined force rand .

[0009] Step 3: Find the distance sampling point X' in the tree rand Closest Node X near .

[0010] Step 4: Follow a step base T step Along the nearest node X near Point to random sampling point X rand Expand new node X in the direction new .

[0011] Step 5: New Node X new With random sampling point X rand If there is a collision, return to step 2. If there is no collision, a dynamic step size strategy is adopted, by increasing a step size base T.step To change the step size, the new node X new According to a step cardinality T step Update the position and perform collision detection again. If there is no collision, update the new node position again according to the dynamic step strategy until the collision detection fails. Return the step size twice before as the expansion step size to generate the final new node X' new .

[0012] Step 6: Determine the new node X new Distance to target point X goal Is it less than the threshold Thr? If it is less than the threshold Thr, directly connect the two points and find the path. Otherwise, jump to the second step and repeat the cycle.

[0013] Step 7: Path pruning. This step is performed after the path search is successful. The final path is pruned to remove redundant nodes in the path and optimize the original path.

[0014] Step 8: Combine the cubic B-spline curve method with the final path to eliminate the turning points where the curvature suddenly changes, and optimize the final path.

[0015] Step 9: Output the path.

[0016] The improvement of the present invention is that in step 1, a step base T is added step , so the distance through the obstacle in steps 4 and 5 is calculated based on the step length T step Dynamically changing the step size improves the efficiency of the algorithm. The formula for the dynamic step size step is:

[0017]

[0018] In the formula, step is the expansion step in the expansion direction. new Located in non-obstructed space C free When , enlarge the dynamic step length step until the new node X new First time in obstacle space C obj To ensure a safe distance from obstacles, return to the new node X twice before. new Location.

[0019] The improvement of the present invention is that in step 1, a bias probability P is added 0 Therefore, the sampling process in step 2 adopts the target bias strategy. In the random sampling process, the rand function is used to generate a random tree between 0 and 1 as the sampling probability p. When the sampling probability p is less than P 0 When the sampling point is the target point X goal ; When the sampling probability p is greater than P 0 When the sampling point is the non-obstacle space C free Any point in the. Sampling method:

[0020]

[0021] Where P 0 The value range is (0,1), and Sample is the non-obstacle space C free Any point in .

[0022] The improvement of the present invention is that in step 2, the random sampling point X rand = Sample, the APF idea is introduced, and the samples are respectively affected by the target point X goal The gravitational force of the obstacle is greater the farther the distance is; the repulsive force of the obstacle is greater the closer the distance is. When the distance is greater than the obstacle influence distance ρ 0 When , the repulsive force of the obstacle on Sample is 0. Finally, Sample forms a new sampling point X' according to the magnitude and direction of the resultant force. rand , the calculation method of attraction and repulsion:

[0023] F grav =ερ(q goal -q)

[0024]

[0025] Where ε is the gravitational constant, ρ(q goal -q) represents the Euclidean distance between the random point and the target point; σ is the repulsive force constant, ρ(q,q obj ) represents the Euclidean distance between the random point and the obstacle, ρ 0 Indicates the obstacle impact distance.

[0026] Optimized sampling point X' rand The calculation formula is:

[0027]

[0028] In the formula, only when ρ(q,q obj )<ρ 0 When X rand The repulsive force needs to be subtracted.

[0029] The improvement of the present invention is that the final new node X' new Combining the target bias strategy, non-obstacle space sampling, APF potential field principle and dynamic step size strategy selection, the calculation formula is:

[0030]

[0031] Where step represents the expansion step size suitable for the expansion direction.

[0032] The improvement of the present invention is that in step 7, the determined path is pruned, redundant nodes of the path are removed, and redundant nodes are eliminated by using the principle that the sum of two sides of a triangle is greater than the third side. In the final path, the parent node of a node on the target point is connected in sequence starting from the target point. If a collision occurs, the next node is replaced to continue the judgment. If there is no collision, the two points are connected. The target point continues to perform collision detection with the previous node. If there is no collision, the two points are connected until the collision returns to the previous connection, the middle point is pruned, and then the previous point of the last connection point is replaced to perform pruning judgment again until it is connected to the starting point and pruning ends. The expression method is:

[0033] a+b>c

[0034] Where a, b, and c are any sides of the triangle.

[0035] The improvement of the present invention is that in step 8, the turning point of the sudden change of curvature is eliminated by combining the method of cubic B-spline curve, so that the path of the robot arm is smoother and each joint does not have a large angle change. The formula of cubic B-spline curve is:

[0036]

[0037] Where C(t) is the coordinate of the curve under parameter t, N i,3 (t) is the cubic B-spline basis function, P i is the i-th control point and n is the number of control points minus 1.

[0038] The beneficial effects of the present invention are:

[0039] (1) The present invention introduces a biased sampling strategy into the traditional RRT algorithm, limits the sampling space in the random sampling process, and integrates the APF algorithm principle to optimize the random sampling points, thereby solving the problems of slow path finding and excessive redundant points of the RRT algorithm in complex environments.

[0040] (2) The present invention further improves the efficiency of the algorithm by improving the fixed step size to a dynamic step size in the process of expanding the path toward the random point, solves the problem of continuous sampling and judgment when there are few obstacles, and better prepares for the next step of pruning.

[0041] (3) After finding the path, the present invention uses the path pruning strategy based on the triangle inequality principle to prune the final path, so that the path is close to the optimal one; finally, the path is smoothed using a cubic B-spline curve, so that the path is smoother and more in line with the robot kinematics. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 is a flow chart of the steps of the present invention;

[0043] Figure 2 This is a comparison diagram of random sampling point global sampling and non-obstacle space sampling of the present invention;

[0044] Figure 3 It is a schematic diagram of the sampling point fusion APF of the present invention;

[0045] Figure 4 It is the principle diagram of the dynamic step extension tree of the present invention;

[0046] Figure 5 It is the principle diagram of path pruning of the present invention;

[0047] Figure 6 It is a comparison diagram of the three-dimensional obstacle avoidance paths of the traditional RRT algorithm and the improved RRT algorithm of the present invention;

[0048] Figure 7 is the path diagram after pruning of the present invention;

[0049] Figure 8 It is the path diagram after the final path of the present invention is combined with the cubic B-spline curve method. DETAILED DESCRIPTION

[0050] The present invention will be further described below in conjunction with the accompanying drawings and specific implementation methods.

[0051] Embodiment 1: The adaptive step-size RRT path planning method based on collision detection provided by the present invention is as follows: Figure 1 As shown, the following steps are included:

[0052] Step 1: Initialization

[0053] In this step, the robot obstacle space C is determined obj There are three rectangular obstacles, three cylindrical obstacles and two spherical obstacles; the initial point X start Set to (50, 50, 200); target point X goal Set to (1000, 1000, 1000); set the distance threshold Thr to 20; set the step base T according to the environmental information step Set to 10, bias probability P 0 is set to 0.6, the APF algorithm gravity coefficient ε is set to 0.000001, the repulsion coefficient σ is set to 2000000, and the obstacle influence distance ρ 0 Set to 30.

[0054] Step 2: Generate random sampling points

[0055] In this step, the sampling point X rand And according to the target bias strategy, it points to the target point X with a probability of 0.6 goal, with a probability of 0.4 in the non-obstruction space C free Random sampling, random sampling point X rand Under the influence of the combined force, a sampling point X' is formed that is more suitable for the expansion direction of the random tree rand .

[0056] Random sampling point selection method:

[0057]

[0058] Where P 0 The value range is (0,1), and Sample is the non-obstacle space C free Any point in .

[0059] Sample integrates the attraction of the target point to it and the repulsion of obstacles into the APF algorithm to generate a new sampling point X' rand .

[0060] The sampling points are subject to the APF algorithm gravity formula:

[0061] F grav =ερ(q goal -q)

[0062] Where ε is the gravitational constant, ρ(q goal -q) represents the Euclidean distance between the random point and the target point.

[0063] The sampling point is subject to the repulsive force formula of the APF algorithm:

[0064]

[0065] σ is the repulsive force constant, ρ(q,q obj ) represents the Euclidean distance between the random point and the obstacle, ρ 0 Indicates the obstacle impact distance.

[0066] Optimized sampling point X' rand The calculation formula is:

[0067]

[0068] In the formula, only when ρ(q,q obj )<ρ 0 When X rand The repulsive force needs to be subtracted.

[0069] Step 3: Find X near

[0070] In this step, find the distance sampling point X' in the tree rand Closest Node X near .

[0071] Step 4: Expand by one step

[0072] In this step, according to a step base T step Along the nearest node X near Point to random sampling point X rand Expand new node X in the direction new .

[0073] Step 5: Collision Detection

[0074] In this step, X new With X rand If there is no collision, the dynamic step size strategy is adopted to change the step size until the collision detection fails and the step size before the return is used as the step size of this expansion to generate a new node X. new .

[0075] Ways to change the step size:

[0076]

[0077] In the formula, C free is the obstacle-free space, C obj is the obstacle space, when the new node X new Located in non-obstructed space C free When , enlarge the step length step until the new node X new First time in obstacle space C obj When , return the new node X twice before new Location.

[0078] The final new node X' new The selection can be calculated according to the following formula:

[0079]

[0080] In the formula, step represents the expansion step size suitable for the expansion direction.

[0081] Step 6: Thr determination

[0082] In this step, determine the new node X new Distance to target point X goal Is it less than the threshold Thr? If it is less than the threshold Thr, directly connect the two points and find the path. Otherwise, jump to the second step and repeat the cycle.

[0083] Step 7: Path Trimming

[0084] In this step, the path search is performed after success, and the final path is pruned to remove redundant nodes of the path, thus optimizing the original path. Specifically, in the expansion tree, the parent node of the previous node is connected from the target point. If there is a collision, the next node is changed to continue the judgment. If there is no collision, the connection is forwarded again until the collision returns to the previous connection, the middle point is pruned, and the next point is changed to judge again. This method effectively shortens the path.

[0085] The representation method is:

[0086] a+b>c

[0087] Where a, b, and c are any sides of the triangle.

[0088] Step 8: Smoothing the Path

[0089] In this step, the final path is optimized by combining the cubic B-spline curve method to eliminate the turning points of sudden curvature changes.

[0090] The formula for the cubic B-spline curve is:

[0091]

[0092] Where C(t) is the coordinate of the curve under parameter t, N i,3 (t) is the cubic B-spline basis function, P i is the i-th control point and n is the number of control points minus 1.

[0093] Step 9: Output Path

[0094] In this step, the position information of the output path point is transmitted to the robot, and the robot obtains the angles of each joint of the robot through the inverse solution of the position information, thereby completing the operation under the path.

[0095] The specific implementation schemes described above further illustrate the purpose, technical scheme and beneficial effects of the present invention, which are only preferred embodiments of the present invention and are not intended to limit the present invention in any other form. Therefore, those skilled in the art can make various modifications or deformations based on the technical enlightenment disclosed by the present invention, as long as the modifications or deformations do not deviate from the essence of the present invention, they are still within the protection scope of the present invention.

Claims

1. An adaptive step-size RRT path planning method based on collision detection, characterized in that: Step 1: Initialize environmental parameters to provide basic information for path planning; Step 2: Generate random sampling points. The random sampling points generate new sampling points through the target bias strategy and APF potential field principle; Step 3: Find the node closest to the new sampling point in the random tree; Step 4: Expand the new node along the direction of the nearest node pointing to the random sampling point; Step 5: The new node is collided with the random sampling point, and the final node is determined according to the dynamic step strategy; Step 6: Determine the path based on the final node; Step 7: Complete path trimming; Step 8: Perform path smoothing operations, determine the final path, and complete path planning.

2. The collision detection-based adaptive step-size RRT path planning method according to claim 1, characterized in that: The initialization environment parameters are specifically: determining the obstacle space C obj and non-obstacle space C free 、Initial point X start With the target point X goal Information, distance to target point threshold Thr, set step length base T according to environmental information step , bias probability P0, APF algorithm attraction coefficient ε and repulsion coefficient σ, and obstacle influence distance ρ0.

3. The collision detection-based adaptive step-size RRT path planning method according to claim 1, characterized in that: The step 2 is specifically as follows: generating a random sampling point X rand , according to the target bias strategy, point to the target point X with probability P0 goal , with a probability of (1-P0) in the non-obstruction space C free Random sampling, non-target point X goal The random sampling point Sample is combined with the APF potential field principle, and the target point X goal Given a random sample point X rand Attraction, obstacles in the obstacle space give it repulsion, and the random sampling point Sample forms a new sampling point X' in the direction of random tree expansion under the influence of the combined force rand .

4. The collision detection-based adaptive step-size RRT path planning method according to claim 3, characterized in that: The target bias strategy is as follows: in the random sampling process, the rand function is used to generate a random tree between 0 and 1 as the sampling probability p. When the sampling probability p is less than P0, the sampling point is the target point X. goal ; When the sampling probability p is greater than P0, the sampling point is the non-obstacle space C free At any point in the , the sampling method is as follows: In the formula, the value range of P0 is (0,1), and Sample is the non-obstacle space C free Any point in .

5. The collision detection-based adaptive step-size RRT path planning method according to claim 3, characterized in that: The APF potential field principle is specifically as follows: at the random sampling point X rand =Sample, the non-obstacle space C free Any point Sample in the goal The gravitational force of the obstacle and the repulsive force of the obstacle, the final non-obstacle space C free Any point Sample forms a new sampling point X' according to the magnitude and direction of the resultant force rand , the calculation formulas of attraction and repulsion are as follows: F grav =ερ(q goal -q) Where ε is the gravitational constant, ρ(q goal -q) represents the Euclidean distance between the random point and the target point; σ is the repulsive force constant, ρ(q,q obj ) represents the Euclidean distance between the random point and the obstacle, and ρ0 represents the obstacle influence distance; New sampling point X' rand The calculation formula is: In the formula, only when ρ(q,q obj )<ρ0, X rand The repulsive force needs to be subtracted.

6. The collision detection-based adaptive step-size RRT path planning method according to claim 1, characterized in that: The step 4 is specifically as follows: step Point to the random sampling point X along the nearest node rand Expand new node X in the direction new .

7. The collision detection-based adaptive step-size RRT path planning method according to claim 1, characterized in that: The step 5 is specifically as follows: new node X new With random sampling point X rand If there is a collision, return to step 2. If there is no collision, a dynamic step size strategy is adopted, by increasing a step size base T. step To change the step size, the new node X new According to a step cardinality T step Update the position and perform collision detection again. If there is no collision, update the new node position again according to the dynamic step strategy until the collision detection fails. Return the step size twice before as the expansion step size to generate the final new node X' new ; The formula for dynamically changing the step size is: In the formula, step is the expansion step in the expansion direction. new Located in non-obstructed space C free When , enlarge the dynamic step length step until the new node X new First time in obstacle space C obj To ensure a safe distance from obstacles, return to the new node X twice before. new Location; The final new node X' new Combining the target bias strategy, non-obstacle space sampling, APF potential field principle and dynamic step size strategy selection, the calculation formula is: Where step represents the expansion step in the expansion direction.

8. The collision detection-based adaptive step-size RRT path planning method according to claim 1, characterized in that: The step 6 is specifically as follows: determining the final new node X' new Distance to target point X goal Is it less than the target point threshold Thr? If so, directly connect to the final new node X' new With the target point X goal , determine the path, otherwise return to step 2.

9. The collision detection-based adaptive step-size RRT path planning method according to claim 1, characterized in that: The step 7 is specifically as follows: pruning the determined path, removing redundant nodes of the path, eliminating redundant nodes by using the principle that the sum of two sides of a triangle is greater than the third side, and in the final path, starting from the target point, sequentially connecting the parent node of a node on the target point, replacing a node to continue judging if a collision occurs, connecting the two points if there is no collision, the target point continues to perform collision detection with the previous node, connecting the two points if there is no collision, until the collision returns to the previous connection, pruning the middle point, and then replacing the previous point of the last connection point to perform pruning judgment again until it is connected to the starting point, and pruning ends.

10. The collision detection-based adaptive step-size RRT path planning method according to claim 1, characterized in that: The path smoothing operation is specifically: combining the method of cubic B-spline curve to eliminate the turning points of curvature mutation, so as to make the path smooth. The formula of cubic B-spline curve is: Where C(t) is the coordinate of the curve under parameter t, N i,3 (t) is the cubic B-spline basis function, P i is the i-th control point and n is the number of control points minus 1.

Images