Mechanical arm path planning method based on RRT-Connect algorithm

Abstract

The application discloses a kind of mechanical arm path planning methods based on RRT-Connect algorithm, comprising: in the free space constituted by starting point and terminal point, randomly generate an intermediate point, respectively in starting point, intermediate point and terminal point simultaneously start to carry out path search, generate random tree, accelerate the speed of convergence;The direction of sampling region is dynamically adjusted, so as to constrain the direction of random tree expansion, reduce the blindness of sampling node;Then dynamically adjust sampling step, let random tree jump out of local minimum value in time, improve the search efficiency of random tree, simultaneously using greedy strategy, accelerate the search speed of random tree in open area;Using path optimization method, remove the redundant nodes generated in random tree;On the basis of optimized path, using the B-spline curve based on least square method is smoothed, obtains a collision-free smooth curve close to original path, reduces the buffeting of mechanical arm, improves the planning efficiency of mechanical arm.

Description

Technical Field

[0001] The present invention relates to a path planning technology, and in particular to a robot arm path planning method based on an RRT-Connect algorithm. Background Art

[0002] Path planning technology was first proposed in the 1970s, and various algorithms have been developed and applied to it. Path planning involves obtaining a series of path points in Cartesian space without considering their relationship to time, such as speed and acceleration.

[0003] Currently, the commonly used path planning algorithms mainly include three categories: search-based path planning: Dijkstra algorithm, A* algorithm, artificial potential field method, etc.; sampling-based path planning: PRM algorithm and RRT algorithm; intelligent bionic path planning: legacy algorithm, neural network algorithm, ant colony algorithm, etc.

[0004] The basic concept of the Dijkstra algorithm is greedy thinking. Its main feature is that it expands outward from the starting point until it reaches the target point. Obviously, the disadvantage of the Dijkstra algorithm is that it expands too many nodes, which is relatively inefficient. The A* algorithm combines the Dijkstra algorithm with the breadth-first search algorithm (BFS), but it is not suitable for searching in complex dynamic environments. The artificial potential field method is a local path planning algorithm based on a virtual force field, but its biggest problem is that it is prone to falling into local optimality. In short, search-based algorithms divide the image into a certain number of grids and search for paths within the grids. Their complexity is relatively high and is not suitable for robot path planning in three-dimensional space.

[0005] Intelligent bionic path planning algorithms are computationally intensive, require long planning times, and consume large amounts of memory, making them unsuitable for high-DOF path planning for robotic arms. In the sampling PRM algorithm, the number of sampling points and the maximum sampling distance determine the success of path planning. A small number of sampling points can lead to path planning failure, while a larger number of sampling points reduces search efficiency, making it unsuitable for robotic arm path planning. Consequently, RRT and its variants are widely used in robotic arms. Summary of the Invention

[0006] The present invention aims to provide a robot arm path planning method based on the RRT-Connect algorithm, which solves the problem that the sampling nodes of the traditional sampling algorithm are very blind, reduces the number of iterations, and shortens the search path.

[0007] The technical solution to implement the present invention is: a robot arm path planning method based on the RRT-Connect algorithm, comprising the following steps:

[0008] Step 1: Randomly generate an intermediate point in the free space between the starting point and the target point, and perform RRT-Connect path searches simultaneously between the starting point and the target point, and between the intermediate point and the target point;

[0009] Step 2: Dynamically adjust the direction of the sampling area and directly use the target point as the sampling node with a certain probability;

[0010] Step 3: Dynamically adjust the sampling step size, add constraint factors, and add a greedy strategy to node expansion;

[0011] Step 4: After the search is completed, the path optimization method is used to remove the redundant nodes generated in the random tree;

[0012] Step 5: Based on the optimized path, use the B-spline curve based on the least squares method for smoothing.

[0013] Compared with the prior art, the significant innovations of the present invention are:

[0014] (1) The technical solution of the present invention constrains the generated sampling nodes, so that the generated nodes have purpose and can converge to the target point more quickly;

[0015] (2) The technical solution of the present invention innovatively incorporates various strategies, including dynamic node strategy, dynamic step strategy, greedy strategy, etc., which improves the efficiency of random tree expansion;

[0016] (3) The technical solution of the present invention incorporates a path optimization method to remove redundant nodes in the search path;

[0017] (4) The technical solution of the present invention smoothes the path and uses a B-spline curve based on the least squares method to obtain a smooth collision-free curve;

[0018] The following is a further description with reference to the accompanying drawings and specific implementations. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 Schematic diagram of the implementation of the constrained RRT-Connect algorithm in the present invention.

[0020] Figure 2 This is an extended schematic diagram of the constrained RRT-Connect algorithm in the present invention.

[0021] Figure 3 This is a schematic diagram of the path optimization scenario in the present invention. Figure 3 (a) is a schematic diagram of scene 1 in the embodiment, Figure 3 (b) is a schematic diagram of scene 2 in the embodiment.

[0022] Figure 4The following are the simulation results of the RRT algorithm, RRT-Connect algorithm, and constrained RRT-Connect algorithm under the same environment.

[0023] Figure 5 This is a simulation result diagram obtained by using the B-spline curve based on the least squares method in the present invention. DETAILED DESCRIPTION

[0024] based on Figure 1 A robot arm path planning method based on the RRT-Connect algorithm includes the following steps:

[0025] Step 1: At the starting point x init and x goal In the free space formed by the target points, a middle point x is randomly generated mid , respectively at the starting point x init and the target point x goal , midpoint x mid and the target point x goal The RRT-Connect path search is performed simultaneously, and the extended diagram is as follows Figure 2 As shown;

[0026] Step 1-1: Based on the RRT-connect algorithm, in order to improve the speed of path search, at the starting point x init and x goal In the free space formed by the end points, a random middle point x is generated mid ;

[0027] Step 1-2: Start path search between the starting point, the intermediate point, and the end point simultaneously, generate random trees, and form two RRT-Connect extended random trees;

[0028] Step 2: Dynamically adjust the direction of the sampling area and directly use the target point as the sampling node with a certain probability. The specific work is as follows:

[0029] Step 2-1: Because the blindness of sampling nodes generated by traditional sampling algorithms is too strong, the constrained RRT-Connect algorithm adds a gravity function to directly make the target point a sampling point with a certain probability, thereby constraining the sampling direction and reducing the blindness of sampling nodes;

[0030]

[0031] Where, P newi Indicates that the random tree is directly based on P newi The probability of the target point x goal As sampling node x rand , d(x new(i-1) ,xgoal ) represents point x new(i-1) To point x goal The Euclidean distance of

[0032] It can be seen that the farther away from the target point, the target point is directly used as the sampling node x rand The greater the probability, the more effectively the random tree can grow towards the target point in a constrained manner;

[0033] Step 2-2: When the random tree expands to a certain distance near the target point, the target point is directly made the next newly expanded node;

[0034] In order to prevent the above formula from new(i-1) Close to x goal When P newi Tends to 0, this paper proposes a sampling domain strategy:

[0035] |x new(i-1) -x goal |≤d thre

[0036] When x new(i-1) to x goal The distance is less than a certain threshold d thre , we directly find the end point by default, that is, let x newi =x goal ;

[0037] Step 3: Dynamically adjust the sampling step size, add constraint factors, and add a greedy strategy to node expansion. Specifically:

[0038] Step 3-1: Introduce a dynamic step size strategy. In node expansion, add a constraint factor. That is, the closer to the target point and the wider the search space, the smaller the step size of the tree; the farther away from the target point and with more obstacles, the longer the step size of the tree.

[0039] The step size step_l is selected according to the following principles:

[0040]

[0041] Where, d(x near ,x goal ) is the node x near To node x goal The distance between them, so the farther away from the end point, the longer the sampling step, and vice versa; N1 is the number of random tree generated sampling points in the free space; N2 is the number of sampling points that failed the collision detection; λ is the weight that affects the speed of change, and the default value here is 3;

[0042] If N2 / N1 is small, it means that the exploration space is open and the sampling range is narrowed to the vicinity of the target point, thereby reducing the blindness of the random tree growing in the open space and accelerating the convergence of the random tree to the target point; if the exploration space is blocked by obstacles, N2 increases to expand the sampling step size, so that the random tree jumps out of the local minimum around the obstacle;

[0043] Step 3-2: Use the greedy strategy to speed up the growth of random trees in open areas;

[0044] When N1>2*N2, the default node is in the open area, so we directly set x rand =x goal , the step length is dynamic step_l, and it keeps growing along the direction of the target point until it encounters an obstacle;

[0045] Step 4: After the search is completed, the path optimization method is used to remove the redundant nodes generated in the random tree. The specific work is as follows:

[0046] Step 4-1: When two line segments in the extended random tree have an intersection, the path optimization condition is triggered and redundant nodes are removed, such as Figure 3 Scene 1 in

[0047] On line segment S i-3 S i-2 and line segment S i-1 S i There is an intersection point S between them. At this time, discard the point S in the tree i-2 and S i-1 , and add point S to the random tree;

[0048] Line segment S i-3 S i-2 and line segment S i-1 S i The intersection conditions are met:

[0049] ((S i -S i-3 )×(S i-2 -S i-3 ))*((S i-1 -S i-3 )×(S i-2 -S i-3 ))<0

[0050] Step 4-2: When the angle between two line segments in the expanded random tree is less than a certain value, the path optimization condition is triggered and redundant nodes are removed, such as Figure 3 Scene 2 in

[0051] Found line segment S i-2 S i-1 and line segment S i-1 Si The angle is very small, which slows down the convergence speed. At this time, point S is directly abandoned. i-1 , connect S i-2 S i , generate a new path;

[0052] It is known that the line segment S i-2 S i-1 and line segment S i-1 S i The equations of the straight lines are:

[0053]

[0054]

[0055] Abandon node S i-1 The condition is that the angle θ satisfies:

[0056]

[0057] Step 5: Based on the optimized path, use the B-spline curve based on the least squares method for smoothing. The specific work is as follows:

[0058] Step 5-1: The B-spline curve can fit the original path into a smooth curve;

[0059] It is known that the B-spline curve is defined as:

[0060]

[0061] Assume that there are n+1 data points on the path before graph fitting, namely {D0, D1, ..., D n}, specify that the basis function of the B-spline curve is p-order and the number of control points is m+1, where n>m≥p≥1;

[0062] Data points {D0, D1, ..., D n}The corresponding parameter vector {t0,t1,…,t n}. It is known that the B-spline curve passes through D0 and D n Data point, the corresponding parameters are t0=0 and t n =1, we can find two control points: P0 = D0, P m =D n , substituting into the B-spline curve formula, we get:

[0063]

[0064] Step 5-2: The least squares method can prevent the fitted curve from colliding with obstacles;

[0065] The least squares function of the present invention does not need to consider the starting point D0 and the ending point D n , so the least squares function of the fitting curve is:

[0066]

[0067] Among them, {P1,…,P m-1} represents the m-1 intermediate control points to be found, k represents the subscript of the n-1 intermediate data points to be fitted, D k represents the kth data point, C(t k ) represents the relationship between the fitting curve and D k corresponding points;

[0068] Step 5-3: Combine the least squares method and B-spline curve to obtain a smooth collision-free path, reduce the vibration of the robot arm, and improve the life of the robot arm;

[0069] Substituting the least squares function into the B-spline curve formula, we get:

[0070]

[0071] Among them, Q k =D k -N 0,p (t k )D0-N m,p (t k )D n is a constant term. For each P in the above formula g Find the partial derivative and set it to 0, and we get:

[0072]

[0073] In the above equations, the left side is the known terms, which are derived from the parameter vector, node vector and data points. The right side of the equation is the basis function and the P to be solved. i We can get:

[0074] Q=(N T N)P

[0075] in:

[0076]

[0077]

[0078] Solving the equation for P gives us the fitted B-spline curve. The final fitted smooth collision-free curve is as follows: Figure 5 The bold smooth curve in .

Claims

1. A robot arm path planning method based on the RRT-Connect algorithm, characterized in that: The following steps are involved: In the free space between the starting point and the target point, a random intermediate point is generated. RRT-Connect path searches are performed simultaneously between the starting point and the target point, and between the intermediate point and the target point. When performing RRT-Connect path searches: Dynamically adjust the direction of the sampling area and directly use the target point as the sampling node with a certain probability; Dynamically adjust the sampling step size, add constraint factors, and add a greedy strategy to node expansion; After the path search is completed, the path optimization method is used to remove the redundant nodes generated in the random tree; On the basis of the optimized path, the B-spline curve based on the least squares method is used for smoothing to obtain the final planned path; The method of dynamically adjusting the direction of the sampling area and directly using the target point as a sampling node with a certain probability specifically includes the following steps: Step 2-1: RRT-Connect adds a gravity function to directly make the target point a sampling point with a certain probability; Step 2-2: When the random tree is extended to a point less than the set distance from the target point, the target point is directly made the next newly extended node. When the sampling point x new(i-1) to x goal The distance is less than a certain threshold d thre , find the end point by default, that is, let x newi =x goal ; The probabilities are: Where, P newi Indicates that the target point x is directly goal As sampling node x rand The probability of d(x new(i-1) ,x goal ) represents point x new(i-1) To point x goal The Euclidean distance, x init is the starting point; The step size is: Among them, d(x near ,x goal ) is the node x near To node x goal The distance between them, N1 is the number of random tree generated sampling points in the free space; N2 is the number of sampling points that fail the collision detection; λ is the weight.

2. The robot arm path planning method based on the RRT-Connect algorithm according to claim 1, characterized in that: The dynamic adjustment of the sampling step size is specifically as follows: the closer to the target point and the wider the search space, the smaller the tree step size; the farther away from the target point and the more obstacles there are, the longer the tree step size.

3. The robot arm path planning method based on the RRT-Connect algorithm according to claim 1, characterized in that: The greedy strategy is: when N1>2*N2, the default node is in the open area, and directly set x rand =x goal , the step size is dynamic step_l, and it keeps growing along the direction of the target point until it encounters an obstacle.

4. The robot arm path planning method based on the RRT-Connect algorithm according to claim 1, characterized in that: After the path search is completed, the method of removing redundant nodes generated in the random tree using a path optimization method specifically includes: Step 4-1: When the line segment S in the expanded random tree i-3 S i-2 and line segment S i-1 S i When there is an intersection, discard the point S in the tree i-2 and S i-1 , add the intersection point to the random tree; Step 4-2: When the angle between two line segments in the expanded random tree is less than a certain value, remove redundant nodes.

5. The robot arm path planning method based on the RRT-Connect algorithm according to claim 4, characterized in that: For the line segment S in step 4-1 i-3 S i-2 and line segment S i-1 S i The intersection conditions are met: ((S i -S i-3 )×(S i-2 -S i-3 ))*((S i-1 -S i-3 )×(S i-2 -S i-3 ))<0。 6. The robot arm path planning method based on the RRT-Connect algorithm according to claim 4, characterized in that: The step 4-2 is specifically as follows: When line segment S i-2 S i-1 and line segment S i-1 S i When the angle is less than a certain value, point S is abandoned directly. i-1 , connect S i-2 S i , generate a new path; abandon node S i-1 The condition is that the angle θ satisfies:

7. The robot arm path planning method based on the RRT-Connect algorithm according to claim 1, characterized in that: The fitting curve is: Among them, Q k =D k -N 0,p (t k )D0-N m,p (t k )D n is a constant term, Solving the equation for P gives us the fitted B-spline curve.

Images