An adaptive step RRT path planning method based on collision detection

Abstract

The application relates to a collision detection-based adaptive step RRT (Rapidly-exploring Random Tree) path planning method, and belongs to the field of robot path planning. First, system initialization is carried out, including initialization of map information, determination of robot obstacle space and non-obstacle space, determination of path initial point and target point position coordinates, base number of each connection step and distance threshold value of the target point and the like; then, an improved RRT algorithm is used for path planning, including sampling in the non-obstacle space, optimization of a random sampling point in combination with a target bias strategy and an APF algorithm and expansion in the expansion direction of the random tree according to a dynamic step strategy; next, it is judged whether a new node is smaller than the threshold value; if yes, a path is found, and if not, the sampling step is jumped to; finally, after the path is found, a path pruning strategy is used to shorten the path length, the final path is smoothed, and the path is output. Compared with a traditional RRT algorithm, the improved RRT algorithm improves the path searching speed, eliminates redundant nodes and improves the path quality.

Description

TECHNICAL FIELD

[0001] The application relates to a collision detection-based adaptive step RRT path planning method and belongs to the field of robot path planning. BACKGROUND

[0002] With the rapid development of robot technology, mobile manipulators begin to be widely used in various fields, such as intelligent manufacturing, health care, warehouse logistics, agricultural planting, military security, transportation, home service and entertainment education. The manipulators with traditional fixed bases and base-linked single guide rails can only be used for small-range operation requirements in simple environments, and cannot meet the current people's use requirements for manipulators in complex environments and the time limit of the workspace. Manipulators with high degrees of freedom cannot exert their maximum value. Therefore, the demand for autonomous motion planning technology for generating collision-free paths of robots has become more urgent, and path planning has become a core problem in the field of robots.

[0003] Robot path planning refers to the process of how a robot moves from a starting position to a target position while avoiding obstacles and meeting robot kinematics conditions in a given environment. According to the different working environments of robots, path planning methods can be divided into global path planning and local path planning. Global path planning refers to planning an optimal collision-free path 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 in the presence of dynamic obstacles. Among them, the global path planning problem, the path method is mainly divided into sampling-based motion planning method and graph search-based motion planning method. The traditional sampling-based motion planning method mainly has RRT and PRM, etc.; the search-based path planning method mainly has A* algorithm and Dijkstra algorithm, etc. The commonly used algorithms for local path planning problems are DWA and APF, etc.

[0004] RRT is a classic sampling-based path planning algorithm. The main steps of the algorithm are as follows: the starting point is taken as the root node, a random sample is found in the workspace of the robot to find the nearest root node in the expansion tree, and the nearest node and the random sample point are connected according to the specified step length. After collision detection, the sample point is added to the tree, and the random tree is continuously grown by connecting the nodes until the sampling is stopped within the threshold value at 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 has no target orientation, which leads to a large number of redundant points in the sampling process, and the expansion of nodes according to the fixed step length leads to a long search time. The turning points of the final path will make the robot change a large acceleration at this point, and even a singularity that does not meet the robot kinematics. SUMMARY

[0005] The technical problem to be solved by the present application is to provide an adaptive step RRT path planning method based on collision detection.

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

[0007] Step 1: Determine the robot obstacle space C obj and the non-obstacle space C free , the initial point X start and the target point X goal , the distance threshold value Thr of the target point, the step base T step set according to the environment information, the bias probability P0, the gravitational coefficient ε and the repulsive coefficient σ of the APF algorithm, and the obstacle influence distance ρ0.

[0008] Step 2: Generate a random sampling point X rand , and direct to the target point X goal with a probability P0 according to the target bias strategy, and sample in the non-obstacle space C free with a probability of (1-P0), the random sampling point Sample of the non-target point X goal , the random sampling point X goal attracted by the target point X rand , the repulsion of the obstacle in the obstacle space, and the new sampling point X' rand formed by the random sampling point Sample under the influence of the resultant force to form the expansion direction of the random tree.

[0009] Step 3: Find the nearest node X rand in the tree to the sampling point X' near .

[0010] Step 4: Expand a new node X step in the direction of the random sampling point X near from the nearest node X rand with a step base T new .

[0011] Step 5: Collision detection between the new node X new and the random sampling point X rand , if collision occurs, return to step 2, if no collision occurs, adopt a dynamic step strategy, change the step length by increasing a step base T step , and expand a new node X new in the direction of the random sampling point X stepUpdate the position, and then perform collision detection, if there is no collision, then update the new node position according to the dynamic step strategy again, until the collision detection fails twice, and the step length of the last two times is returned as the expanded step length, and the final new node X' is generated new .

[0012] Step 6: judge the new node X new whether the distance to the target point X goal is less than the threshold Thr, if yes, directly connect the two points to find the path, otherwise jump to the second step of the loop.

[0013] Step 7: path pruning, after the path search is successful, the step is performed, the final path is pruned, the redundant nodes of the path are removed, and the original path is optimized.

[0014] Step 8: the turning point of the curvature mutation is eliminated by combining the method of the third B-spline curve to the final path, and the final path is optimized.

[0015] Step 9: output the path.

[0016] The improvement of the application lies in that in the step 1, the step base T step is added, so that the distance to the obstacle in the step 4 and the step 5 is T step per step, and the step length is dynamically changed, and the algorithm efficiency is improved.

[0017]

[0018] In the formula, step is the expansion step length in the expansion direction, when the new node X new is located in the non-obstacle space C free , the dynamic step length step is enlarged until the new node X new is located in the obstacle space C obj for the first time, and the position of the new node X new two times ago is returned.

[0019] The improvement of the application lies in that in the step 1, the bias probability P0 is added, so that in the sampling process in the step 2, the target bias strategy is adopted, and the rand function is used to generate a random tree between 0 and 1 as a sampling probability p in the random sampling process, when the sampling probability p is less than P0, the sampling point is the target point X goal , and when the sampling probability p is greater than P0, the sampling point is any point in the non-obstacle space C free . The sampling mode is as follows:

[0020]

[0021] In the formula, the value range of P0 is (0, 1), and Sample is the non-obstacle space Cfree Any one of the points.

[0022] The improvement of the present application is that in step 2, the APF idea is introduced when Sample is equal to X rand The Sample is affected by the gravity of the target point X goal , the farther the distance, the greater the gravity; the repulsion of the obstacle, the closer the distance, the greater the repulsion, when the distance is greater than the influence distance of the obstacle ρ0, the repulsion of the obstacle to the Sample is 0. Finally, the Sample forms a new sampling point X' rand according to the size and direction of the resultant force.

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

[0024]

[0025] In the formula, ε is the gravity constant, ρ(q goal -q) represents the Euclidean distance between the random point and the target point; σ is the repulsion constant, ρ(q,q obj ) represents the Euclidean distance between the random point and the obstacle, and ρ0 represents the influence distance of the obstacle.

[0026] The calculation formula of the optimized sampling point X' rand :

[0027]

[0028] In the formula, only when ρ(q,q obj )<ρ0, X rand needs to subtract the repulsion.

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

[0030]

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

[0032] The improvement of the present application is that the determined path is pruned in step 7, the redundant nodes of the path are removed, the principle that the sum of two sides of a triangle is greater than the third side is used to eliminate the redundant nodes, in the final path, the parent node of the node before the target point is connected in sequence from the target point, if collision occurs, the previous node is replaced to continue to judge, if there is no collision, the two points are connected, the target point continues to detect collision with the previous node, if there is no collision, the two points are connected, until collision returns to the last connection, the intermediate point is pruned, then the previous point of the last connection point is replaced to prune again, until the starting point is connected, the pruning is finished, and the representation method is:

[0033] a+b>c

[0034] In the formula, a, b and c are any side of a triangle.

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

[0036]

[0037] In the formula, C(t) is the coordinate of the curve at the parameter t, N i,3 (t) is the cubic B-spline base 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 application are:

[0039] (1) The present application introduces a bias sampling bias strategy in the traditional RRT algorithm, limits the sampling space in the random sampling process, and optimizes the random sampling point by combining the principle of APF algorithm, so that the problems of slow path finding speed and excessive redundant points of the RRT algorithm in a complex environment are solved.

[0040] (2) In the process of expanding the path to the random point direction, the fixed step is improved into a dynamic step, which further improves the efficiency of the algorithm, solves the problem of always sampling and judging in the case of few obstacles, and better prepares for the next pruning process.

[0041] (3) The present application uses the principle of triangle inequality pruning strategy to prune the final path after finding the path, so that the path is close to optimal; finally, the path is smoothed by using cubic B-spline curve, so that the path is more stable and more consistent with robot kinematics. BRIEF DESCRIPTION OF DRAWINGS

[0042] Figure 1 is a step flow chart of the present application;

[0043] Figure 2 is a global sampling of random sampling points of the present application and non-obstacle space sampling comparison chart;

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

[0045] Figure 4 is a schematic diagram of the dynamic step expansion tree of the present application;

[0046] Figure 5 is a schematic diagram of the path pruning of the present application;

[0047] Figure 6 is a three-dimensional obstacle avoidance path comparison chart of the traditional RRT algorithm and the improved RRT algorithm of the present application;

[0048] Figure 7 is a path chart after pruning of the present application;

[0049] Figure 8 is a path chart after the final path of the present application is combined with the cubic B-spline curve method. DETAILED DESCRIPTION

[0050] The present application will be further described below in conjunction with the drawings and specific embodiments.

[0051] Example 1: The adaptive step RRT path planning method based on collision detection provided by the present application, as shown in the following steps: Figure 1

[0052] Step 1: initialization

[0053] In this step, the robot obstacle space C obj is three cuboid obstacles, three cylindrical obstacles and two spherical obstacles; the initial point X start is set to (50, 50, 200); the target point X goal is set to (1000, 1000, 1000); the distance threshold value Thr is set to 20; the step base T step is set to 10, the bias probability P0 is set to 0.6, the APF algorithm gravitational coefficient ε is set to 0.000001, the repulsive coefficient σ is set to 2000000, and the obstacle influence distance ρ0 is set to 30.

[0054] Step 2: generate random sampling points

[0055] In this step, the sampling point X rand is generated according to the target bias strategy with a probability of 0.6 pointing to the target point X goal , and a probability of 0.4 in the non-obstacle space C free ​Random sampling, random sampling point X rand Under the influence of the resultant force to form more suitable for random tree expansion direction of sampling point X' rand .

[0056] The selection method of random sampling point:

[0057]

[0058] In the formula, the value range of P0 is (0, 1), and Sample is any point in the non-obstacle space C free .

[0059] Sample into the APF algorithm target point to its attractive force, repulsive force of obstacle idea to generate new sampling point X' rand .

[0060] Sampling point is subject to APF algorithm gravity formula:

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

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

[0063] Sampling point is subject to APF algorithm repulsive force formula:

[0064]

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

[0066] The calculation formula of the optimized sampling point X' rand :

[0067]

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

[0069] Step 3: find X near

[0070] In this step, find the nearest node X rand X near in the tree distance from the sampling point X'

[0071] Step 4: expand a step

[0072] In this step, according to a step base Tstep Along the nearest node X near Point to random sampling point X rand Directional expansion of new node X new .

[0073] Step 5: Collision detection

[0074] In this step, X new and X rand between the collision detection, collision then return to the second step resampling, no collision then use dynamic step strategy, change the size of the step, until the collision detection not to return twice before the step size as the step size of this expansion, generate new node X new .

[0075] Change the way of step:

[0076]

[0077] In the formula, C free is the non-obstacle space, C obj is the obstacle space, when the new node X new is located in the non-obstacle space C free , the step size is enlarged until the new node X new first in the obstacle space C obj , return to the position of the new node X new twice before.

[0078] The final selection of new node X' new 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 judgment

[0082] In this step, it is judged whether the distance between the new node X new and the target point X goal is less than the threshold value Thr, which is less than the connection between the two points, and the path is found, otherwise jump to the second step to reciprocating cycle.

[0083] Step 7: Path pruning

[0084] This step, performed after a successful path search, prunes the final path, removing redundant nodes and optimizing the original path. Specifically, in the extended tree, starting from the target point, the tree connects to the parent node of the previous node. If a collision occurs, the tree moves to the next node and continues the search. If there is no collision, the tree continues forward until a collision returns to the previous connection. The intermediate point is pruned and the next point is selected for further search. This method effectively shortens the path.

[0085] Its 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 turning points with sudden changes in curvature.

[0090] The formula of 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 angle of each joint of the robot through the inverse solution of the position information, and then completes the operation under the path.

[0095] The specific embodiments described above further illustrate the objectives, technical solutions, and beneficial effects of the present invention. They are merely preferred embodiments of the present invention and are not intended to limit the present invention in any other manner. Therefore, those skilled in the art may make various modifications or variations based on the technical teachings disclosed herein. As long as the modifications or variations do not depart from the essence of the present invention, they remain within the scope of protection of the present invention.

Claims

1. A collision detection-based adaptive step RRT path planning method, characterized in that: Step 1: initialize the environment parameters to provide basic information for path planning; Step 2: generate random sampling points, and generate new sampling points through target bias strategy and APF potential field principle; Step 3: find the nearest node in the random tree to the new sampling point; Step 4: expand the new node in the direction of the nearest node pointing to the random sampling point; Step 5: perform collision detection between the new node and the random sampling point, and determine the final node according to the dynamic step strategy; Step 6: determine the path according to the final node; Step 7: complete path pruning; Step 8: perform path smoothing operation to determine the final path and complete path planning; The step 2 is specifically: generating random sampling points , according to the target bias strategy with a probability pointing to the target point , with a probability of 1- random sampling in the non-obstacle space , the random sampling point of the non-target point , combining the APF potential field principle, the target point attracts the random sampling point , and the obstacle in the obstacle space repels the random sampling point , and the random sampling point forms a new sampling point in the direction of the random tree expansion under the influence of the resultant force The APF potential field principle is specifically as follows: in the random sampling point Sample, any point in the non-obstacle space Sample respectively receives the attraction of the target point and the repulsion of the obstacle, and finally any point in the non-obstacle space Sample forms a new sampling point according to the size and direction of the resultant force The calculation formula of the attraction and the repulsion is as follows: ； ； In the formula, is the gravitational constant, represents the Euclidean distance between the random point and the target point; is the repulsive force constant, represents the Euclidean distance between the random point and the obstacle, represents the obstacle influence distance; New sampling points The formula for calculating: ； In the formula, only when , repulsive force needs to be subtracted; The step 5 is specifically: the new node and the random sampling point between the new node and the random sampling point are detected, if collision occurs, it returns to step 2, if no collision occurs, a dynamic step strategy is adopted, a step base is added to change the step, the new node is updated according to a step base , and the collision detection is performed again, if no collision occurs, the new node position is updated again according to the dynamic step strategy, until the collision detection fails twice, and the step before the last time is returned as the step for expansion, and the final new node is generated ; the formula of the dynamic step is: ； In the formula, step is the expansion step in the expansion direction, when the new node is located in the non-obstacle space , the dynamic step is enlarged until the new node is located in the obstacle space for the first time , the position of the new node two steps ago is returned to ensure the safety distance from the obstacle . The final new node Combined with the target bias strategy, non-obstacle space sampling, the principle of APF potential field and dynamic step length strategy selection, the calculation formula is: ； In the formula, represents the extension step length in the extension direction.

2. The collision detection based self-adaptive step RRT path planning method according to claim 1, characterized in that, The initialization environment parameters are specifically: determining the obstacle space and non-obstacle space , initial point and target point , distance target point threshold , setting step base according to environment information , bias probability , APF algorithm gravitational coefficient and repulsive force coefficient , and obstacle influence distance .

3. The collision detection based self-adaptive step-size RRT path planning method according to claim 1, wherein, The target bias strategy is specifically: using the rand function to generate A random number between , when the sampling probability Less than When the sampling point is the target point ; When the sampling probability Greater than When the sampling point is the non-obstacle space At any point in , the sampling method is as follows: ； wherein, Sample is a non-obstacle space any point in the range (0, 1).

4. The collision detection based self-adaptive step-size RRT path planning method according to claim 1, wherein, The step 4 is specifically: according to a step base Pointing to the random sampling point along the nearest node Direction expansion new node .

5. The collision detection based self-adaptive step-size RRT path planning method according to claim 1, wherein, The step 6 is specifically: judging whether the final new node is less than the target point threshold value from the target point , if yes, directly connecting the final new node with the target point , determining the path, otherwise returning to step 2.

6. The collision detection based self-adaptive step-size RRT path planning method according to claim 1, wherein, The step 7 is specifically: pruning the determined path, eliminating redundant nodes of the path, eliminating redundant nodes using the principle that the sum of two sides of a triangle is greater than the third side, and connecting the parent node of the last node of the target point in sequence in the final path, replacing the last node to continue the judgment if collision occurs, connecting the two points if there is no collision, and continuing the collision detection between the target point and the previous node, connecting the two points if there is no collision, until the collision returns to the last connection, pruning the intermediate points, then replacing the last connection point with the previous point to perform pruning judgment again, until it is connected to the starting point, and the pruning is completed.

7. The collision detection based self-adaptive step-size RRT path planning method according to claim 1, wherein, The path smoothing operation is specifically: eliminating the turning points of curvature mutation by combining the method of cubic B-spline curve to make the path smooth, and the formula of cubic B-spline curve is: ； Where, are the coordinates of the curve under parameter t, is the cubic B-spline basis function, It is control points, is the number of control points minus 1.

Images