A dynamic steering RRT* method for robot arm path planning

Abstract

This invention discloses a path planning method for a dynamically guided RRT* robotic arm. Based on environmental information and the current growth status of the growth tree, an environmental complexity coefficient is introduced to obtain an adaptive probability p. v Variable growth step size r* and adaptive range radius d, through adaptive probability p v By setting the target point as a random point and employing a vertical sampling strategy, the sampling range is dynamically guided towards the target point, significantly improving sampling efficiency and mitigating growth disorder. A variable growth step size strategy is adopted to make the growth tree more adaptable to regions with varying environmental complexity. Within the adaptive range, parent nodes are reselected and nodes are reconnected to optimize the path, reduce computational load, and improve convergence speed and efficiency. Finally, a collision-free path is generated by continuously tracing the parent nodes from the target point back to the starting point.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of mechanical arm path planning, and particularly relates to a mechanical arm path planning method based on a dynamic guiding RRT*. BACKGROUND

[0002] Intelligent mechanical arms are widely used in various industries, such as assembly robots, agricultural fruit picking robots, medical robots, etc. At present, there are few studies on leafy medicinal plant picking robots in the agricultural field, such as honeysuckle picking robots. In unstructured environments, the work target is complex, and the robot needs to safely pass through obstacles in the environment during the picking process to reach the target point. Obstacle avoidance path planning is an important part of mechanical arm technology, and the purpose is to enable the mechanical arm to avoid obstacles in the environment and complete the entire planning process with autonomous path planning decision-making functions.

[0003] So far, the path planning methods of mechanical arms mainly include search-based path planning algorithms, sampling-based path planning algorithms, and intelligent bionic algorithms. The search-based path planning algorithms mainly include A* algorithm, Dijkstra algorithm, D* algorithm, and artificial potential field method; the sampling-based path planning algorithms include PRM algorithm and RRT series algorithm; and the intelligent bionic path planning algorithms include neural network algorithm, ant colony algorithm, and genetic algorithm. However, each of the above methods has its own advantages and disadvantages, such as long time consumption of A* algorithm and easy falling into local minimum of artificial potential field method. The search-based path planning algorithms are not suitable for application in high-dimensional space due to large calculation scale, and the picking mechanical arm usually has 6 degrees of freedom, and the kinematic relationship during the movement of the mechanical arm is extremely complex, so such algorithms cannot be used. Intelligent bionic algorithms such as reinforcement learning need to rely on training results, and local shaking and oscillation are prone to occur during actual operation of the mechanical arm, which is unstable. The RRT algorithm reduces the calculation amount and can generate a feasible path for the operation of the mechanical arm, but due to the existence of many invalid nodes and poor path quality in the algorithm, the mechanical arm cannot accurately avoid obstacles during operation, time is wasted, and the mechanical arm often moves in invalid space.

[0004] In summary, there is an urgent need for a mechanical arm path planning method that can effectively solve the technical problems of excessive invalid movement during the movement of the mechanical arm, poor quality of the operation path, and long operation time. SUMMARY

[0005] In order to solve the problems in the prior art, the application provides a dynamic guiding RRT* mechanical arm path planning method, which can solve the technical problems of excessive invalid movement during the movement of the mechanical arm, poor quality of the operation path, and long operation time.

[0006] The technical scheme adopted by the application is as follows:

[0007] A dynamic guiding RRT* method for robot arm path planning, comprising the following steps:

[0008] Step 1, obtaining three-dimensional environment information of the robot arm work, in the three-dimensional environment information, obtaining the starting point X int , target point X goal and obstacle three-dimensional coordinates;

[0009] Step 2, based on the environment information in step 1, realizing the dynamic guiding RRT* method for robot arm path planning, the process is as follows:

[0010] Step 2.1, based on the environment information in step 1, growing a random tree, initializing the growth step length r, the adaptive range basic distance d, and the adaptive basic probability p;

[0011] Step 2.2, calculating the adaptive probability p v of the current growing tree;

[0012] Step 2.3, based on the adaptive probability p v and the tangent plane sampling, obtaining a new node X new of the growing tree;

[0013] Step 2.4, reselecting the parent node of the new node X new in the adaptive range, and reconnecting other nodes in the adaptive range with the node X new as the parent node; performing collision detection on the reselected parent node and the updated path of the reconnection, generating a collision-free path from the target point to the starting point, and completing the robot arm path planning.

[0014] Further, an image acquisition device is used to obtain three-dimensional environment information of the agricultural picking robot arm work, and a starting point X int , a target point X goal and an obstacle are selected in the three-dimensional space C.

[0015] Further, the method for calculating the adaptive probability p v of the current growing tree in step 2.2 is as follows:

[0016] Traversing the generated node set of the growing tree, finding the node closest to the target point and calculating the environmental relative complexity coefficient h nearest of the node,

[0017] The adaptive probability p v of the current growing tree is represented as:

[0018]

[0019] Wherein, p is the adaptive basic probability.

[0020] Furthermore, the relative environmental complexity coefficient h of the node closest to the target point nearest Represented as:

[0021]

[0022] in, S is the sum of the straight-line distances from the nearest node to the target point to the centers of all obstacles in 3D space C. int Let X be the starting point in three-dimensional space C. int S is the sum of the straight-line distances to the centers of all obstacles. goal Let X be the target point in three-dimensional space C. goal S is the sum of the straight-line distances to the centers of all obstacles. mid X is the midpoint of the line connecting the starting point to the target point in three-dimensional space C. mid The sum of the straight-line distances to the centers of all obstacles, S is S int S goal S mid The average of the three.

[0023] Furthermore, in step 2.3, a new node X of the growth tree is obtained. new The method is as follows:

[0024] Step 2.3.1: Randomly generate a percentage. If it is not greater than the adaptive probability p of the current growth tree... v Then the random node X rand Set as target point X goal Then proceed to step 2.3.4; if it is greater than the adaptive probability p of the current growth tree. v Then proceed to step 2.3.2;

[0025] Step 2.3.2: Based on the random sampling function, randomly sample and generate random nodes X in the three-dimensional space C. rand ;

[0026] Step 2.3.3, search for the distance to the random node X rand The nearest node X nearest Based on X nearest and X goal Vertical sampling is performed on the connection lines, and valid random nodes are assigned to the node set T.

[0027] Step 2.3.4: Traverse the set of nodes T that have been generated in the growing tree, and search for the node X at a distance from the random node. rand The nearest node X nearest The nearest node X is calculated. nearest The variable growth step size r* at the location;

[0028] Step 2.3.5, with node X nearest The growth step size r* at node X, from node Xnearest To random node X rand Direction growth this step, generate new node X new .

[0029] Further, the method of vertical surface sampling in step 2.3.3 is:

[0030] Connect node X nearest And target point X goal , through X nearest Make X nearest And X goal The vertical line of the connecting line, get the vertical surface through node X nearest This vertical surface divides three-dimensional space C into two spaces, if random node X rand With target point X goal In the same space, the random node is effective, and the effective random node is classified into node set T; if random node X rand With target point X goal Not in the same space, then discard the random node and resample.

[0031] Further, calculate the growth step r* at the nearest node X nearest , expressed as:

[0032]

[0033] Wherein, r is the initial growth step, h is the relative complexity coefficient of the environment of the nearest node X nearest Of random node, S is the sum of the straight line distance from the nearest node X rand Of random node X nearest To the center of all obstacles in three-dimensional space C.

[0034] Further, the specific process of the robot path planning in step 2.4 includes:

[0035] Step 2.4.1, collision detection is carried out on the path from node X nearest To node X new , if not pass, turn to step 2.2; if pass, turn to step 2.4.2;

[0036] Step 2.4.2, reselect parent node for new node X new In adaptive range, reconnection is carried out on other nodes in adaptive range with node X new As the parent node, update node tree and path;

[0037] Step 2.4.3, collision detection is carried out on the path of reselected parent node and reconnection update, if not pass, cancel the update; if pass, turn to step 2.4.4;

[0038] Step 2.4.4, updating the shortest path and the parent node of the shortest path with path length as the evaluation index;

[0039] Step 2.4.5, judging whether the node X new enters the target point X goal the set field, if not, the node X new is added to the generated node set T, and step 2.2 is entered; if yes, the parent node is continuously traced back to obtain a collision-free path, and the process is ended. new

[0040] Further, the radius d i of the adaptive range is represented as:

[0041]

[0042] wherein h i is the environmental relative complexity coefficient at the i-th new node X new , n is the number of all existing nodes in the growing tree, d is the basic distance of the adaptive range, and f is the adaptive range coefficient.

[0043] Further, in step 2.4.4, if the generated path length is less than the original path length, the parent node of the node is updated, and the newly generated path is added to the tree.

[0044] The beneficial effects of the present application are as follows:

[0045] (1) For the problem that the invalid motion is too much in the motion process of the mechanical arm, the time cannot be efficiently utilized, the strategy of generating random sampling points based on the vertical plane sampling and setting the target point as a random point with an adaptive probability are used to remove the sampling points that do not meet the requirements, so that the sampling region is dynamically guided to the target point, and the motion of the mechanical arm is always close to the target point;

[0046] (2) For the problem that the number of obstacles is different at different positions of the mechanical arm, the environmental relative complexity coefficient h and the variable growth step r* are introduced to change the growth step of the fast-growing random tree, so that the growth of the tree is more adaptive to the space with different complexity of the environment, and the operation of the mechanical arm is more rapid and sensitive in an open space, and more stable in a complex environment;

[0047] (3) For the problem that the operation path of the mechanical arm is poor in quality and the operation time is long, the following improvements are made: in the path optimization and the reselection of the parent node and the reconnection of the node, the adaptive range coefficient f and the adaptive range radius d i ​The adaptive range is larger when the number of nodes is smaller, and the adaptive range is correspondingly reduced when the number of nodes is larger, thereby reducing the calculation amount and improving the convergence efficiency. The algorithm effectively optimizes the path quality and reduces the time consumed by the movement of the robot arm. BRIEF DESCRIPTION OF DRAWINGS

[0048] Figure 1 is a total flow chart of the robot arm path planning method based on dynamic steering RRT* of the application.

[0049] Figure 2 is a schematic diagram of the two-dimensional plane based on the tangential plane sampling in the application.

[0050] Figure 3 is a schematic diagram of the two-dimensional plane of the spherical center straight line distance from the starting point, target point and midpoint of the connecting line to all spherical obstacles in the space C in the application.

[0051] Figure 4 is a schematic diagram of the reselection of the parent node of the new node Xnew in the adaptive range in the application.

[0052] Figure 5 is a schematic diagram of the reconnection of other nodes with the node Xnew as the parent node in the adaptive range in the application. DETAILED DESCRIPTION

[0053] In order to make the purpose, technical scheme and advantages of the application clearer, the application is further described in detail below in combination with the drawings and examples. It should be understood that the specific examples described herein are only used to explain the application and do not limit the application.

[0054] A robot arm path planning method based on dynamic steering RRT* includes the following steps:

[0055] Step 1, obtain the three-dimensional environment information of the working of the mechanical arm, in the three-dimensional environment information, obtain the starting point Xint, the target point Xgoal and the three-dimensional coordinates (x, y, z) of the obstacle. In this embodiment, taking the agricultural picking mechanical arm as an example for description, the three-dimensional environment information of the working of the agricultural picking mechanical arm can be obtained by using an image acquisition device, such as a honeysuckle picking environment; and the starting point Xint, the target point Xgoal and the obstacle in the three-dimensional space C are selected. The obstacle in the environment is usually irregular, in order to reduce the amount of calculation, the model of the obstacle is usually simplified into a sphere, a cuboid and a cylinder, for the convenience of description, the obstacle in the present application is simplified into a sphere (two-dimensional representation is spherical), if it is a cuboid and a cylinder, the sphere center coordinates are the shape centers of the objects, and the remaining steps are the same. Therefore, in this embodiment, the obstacle is regarded as a spherical obstacle, and the three-dimensional coordinates of the obstacle are represented by the coordinates (x, y, z) of the sphere center of the obstacle.

[0056] Step 2, based on the environment information in step 1, realize the path planning of the mechanical arm of the dynamic guidance RRT*, the specific process is as follows:

[0057] Step 2.1, based on the environment information in step 1, grow a random tree, initialize the growth step length r, the adaptive range basic distance d and the adaptive basic probability p.

[0058] Step 2.2, calculate the adaptive probability p of the current growing tree v . The specific process is as follows:

[0059] Traverse the generated node set T of the growing tree, find the node closest to the target point, calculate the environmental relative complexity coefficient h of the node nearest , which is expressed as:

[0060]

[0061] wherein, is the sum of the straight line distances from the node closest to the target point to the sphere centers of all spherical obstacles in the three-dimensional space C, S int is the sum of the straight line distances from the starting point X int to the sphere centers of all spherical obstacles in the three-dimensional space C, S goal is the sum of the straight line distances from the target point X goal to the sphere centers of all spherical obstacles in the three-dimensional space C, S mid is the sum of the straight line distances from the midpoint X mid of the line connecting the starting point to the target point in the three-dimensional space C to the sphere centers of all spherical obstacles, S is the average of S int , S goal and S mid , which are respectively expressed as:

[0062]

[0063]

[0064]

[0065]

[0066] Among them, S inti For X int The straight-line distance S to the center of the i-th spherical obstacle goali For X goal The straight-line distance S to the center of the i-th spherical obstacle midi Midpoint X mid The straight-line distance to the center of the i-th spherical obstacle, where n is the number of spherical obstacles.

[0067] The adaptive probability p of the current growing tree v The expression is:

[0068]

[0069] Among them, h nearest is the relative environmental complexity coefficient at the node closest to the target point; p is the adaptive basic probability.

[0070] Step 2.3, based on adaptive probability p v By sampling from the vertical plane, a new node X of the growing tree is obtained. new ;

[0071] Step 2.3.1: Randomly generate a percentage. If it is not greater than the adaptive probability p of the current growth tree... v Then the random node X rand Set as target point X goal Then proceed to step 2.3.4; if it is greater than the adaptive probability p of the current growth tree. v Then proceed to step 2.3.2.

[0072] Step 2.3.2: Based on the random sampling function, randomly sample and generate random nodes X in the three-dimensional space C. rand .

[0073] Step 2.3.3, search for the distance to the random node X rand The nearest node X nearest Connect node X nearest and target point X goal , through X nearest As X nearest and X goal The perpendicular line connecting the points obtains the line passing through node X. nearest A perpendicular plane, which divides the three-dimensional space C into two spaces, if a random node Xrand With target point X goal If the random node is valid in the same space, add the valid random node to the node set T and proceed to step 2.3.4; if random node X rand With target point X goal If the nodes are not in the same space, discard the random node and proceed to step 2.3.2 for resampling. (Refer to...) Figure 2 .

[0074] Step 2.3.4: Traverse the set of nodes T that have been generated in the growing tree, and search for the node X at a distance from the random node. rand The nearest node X nearest The nearest node X is calculated. nearest The variable growth step size r* is given at a given location. The process for calculating the variable growth step size r* is as follows:

[0075] First, calculate the nearest node X of the random node. nearest The relative environmental complexity coefficient h is expressed as:

[0076]

[0077] Among them, S * For node X nearest The sum of the straight-line distances to the centers of all spherical obstacles in three-dimensional space C can be referenced. Figure 3 ;

[0078] Furthermore, calculate the nearest node X. nearest The growth step size r* at that point is expressed as:

[0079]

[0080] Step 2.3.5, with node X nearest The growth step size r* at node X, from node X nearest To random node X rand Grove the direction by this step size to generate a new node X. new .

[0081] Step 2.4, within the adaptive range, adjust the new node X. new Perform parent node reselection, and for other nodes within the adaptive range, use node X. new Reconnect the parent node; perform collision detection on the path updated by reconnecting and reselecting the parent node, generate a collision-free path from the target point to the starting point, and complete the robot arm path planning.

[0082] Step 2.4.1, for node X nearest To node X new Collision detection is performed on the path. If the collision fails, proceed to step 2.2; if the collision succeeds, proceed to step 2.4.2.

[0083] Step 2.4.2, within the adaptive range, adjust the new node X. new Perform parent node reselection, and for other nodes within the adaptive range, use node X. new Reconnect the parent node, update the node tree and path;

[0084] The expression for the adaptive range coefficient f:

[0085]

[0086] Adaptive range radius d i The expression is:

[0087]

[0088] Among them, h i Let Xnew be the relative environmental complexity coefficient at the i-th new node Xnew, n be the number of all existing nodes in the growth tree, d be the basic distance of the adaptive range, and f be the adaptive range coefficient.

[0089] Step 2.4.3: Perform collision detection on the paths updated by reselecting the parent node and reconnecting. If the collision fails, cancel the update; otherwise, proceed to step 2.4.4.

[0090] Step 2.4.4: Using path length as the evaluation metric, update the shortest path and its parent node. (Refer to...) Figure 4 , Figure 5 More specifically, if the length of the generated path is less than the length of the original path, the parent node of the node is updated, and the newly generated path is added to the tree.

[0091] Step 2.4.5: Determine node X new Have we entered target point X? goal If the specified domain is not specified, then node X will be... new Add it to the already generated node set T and proceed to step 2.2; if so, then start from that node X. new Continue tracing back to the parent node until a collision-free path is found, then terminate.

[0092] More specifically, from the new node X within the defined domain new Tracing its parent node X parent Obtain a new node X new To its parent node X parent The path; then, with parent node X parent Starting from the first node, trace back to its parent node and obtain the path segment; repeat the above steps to obtain all the paths, and then concatenate them to obtain the final collision-free path.

[0093] To sum up, the mechanical arm path planning method based on dynamic guiding RRT* provided by the application improves the sampling strategy compared with the traditional RRT* algorithm, and effectively reduces the generated invalid nodes; when generating a random sampling point, the adaptive probability p v The target point is set as the sampling point, the convergence speed to the target point is improved, the growing step length mechanism is introduced, the tree growing is more suitable for the space with different complexity of environment, the parent node is reselected in the adaptive range, the node is reconnected, and the path is gradually optimal. In the case that the starting point, the target point and the obstacle information are known, the growing tree based on the environment information and the growing condition is introduced, the sampling range is dynamically guided to the target point, the convergence speed of the path is improved, the number of redundant nodes can be significantly reduced by the above improvements, the quality of the generated path is improved, and the application has universal applicability in various complex environments. Further, the technical problems of too much invalid motion in the motion process of the mechanical arm, poor quality of the running path, long running time and the like can be solved.

[0094] The above examples are only used for describing the design idea and characteristics of the application, the purpose is to enable the person skilled in the art to understand the content of the application and to implement the application, and the protection scope of the application is not limited to the above examples. Therefore, any equivalent changes or modifications made according to the principles and design ideas disclosed by the application are within the protection scope of the application.

Claims

1. A path planning method for a robotic arm with dynamic guidance RRT*, characterized in that, Includes the following steps: Step 1: Obtain the 3D environment information of the robotic arm's operation. From this 3D environment information, obtain the starting point X. int Target point X goal and the three-dimensional coordinates of the obstacle; Step 2: Based on the environmental information in Step 1, implement the dynamic guidance RRT* robot path planning, as follows: Step 2.1: Based on the environmental information in Step 1, perform random tree growth and initialize the growth step size r, the adaptive range basic distance d, and the adaptive basic probability p. Step 2.2, calculate the adaptive probability p of the current growth tree. v ; Step 2.3, based on adaptive probability p v By sampling from the vertical plane, a new node X of the growing tree is obtained. new ; Step 2.4, within the adaptive range, adjust the new node X. new Perform parent node reselection, and for other nodes within the adaptive range, use node X. new Reconnect the parent node; perform collision detection on the path updated by reconnecting and reselecting the parent node, generate a collision-free path from the target point to the starting point, and complete the robot arm path planning.

2. The method for path planning of a robotic arm with dynamic guidance RRT* according to claim 1, characterized in that, An image acquisition device is used to acquire three-dimensional environmental information of the agricultural harvesting robotic arm, and a starting point X is selected in the three-dimensional space C. int Target point X goal and obstacles.

3. The method for path planning of a robotic arm with dynamic guidance RRT* according to claim 1, characterized in that, In step 2.2, the adaptive probability p of the current growing tree is calculated. v The method is as follows: Traverse the set of nodes generated in the growing tree, find the node closest to the target point, and calculate the relative environmental complexity coefficient h of that node. nearest , The adaptive probability p of the current growing tree v Represented as: Where p is the adaptive basic probability.

4. The method for path planning of a robotic arm with dynamic guidance RRT* according to claim 3, characterized in that, The relative environmental complexity coefficient h of the node closest to the target point nearest Represented as: in, S is the sum of the straight-line distances from the nearest node to the target point to the centers of all obstacles in 3D space C. int Let X be the starting point in three-dimensional space C. int S is the sum of the straight-line distances to the centers of all obstacles. goal Let X be the target point in three-dimensional space C. goal S is the sum of the straight-line distances to the centers of all obstacles. mid X is the midpoint of the line connecting the starting point to the target point in three-dimensional space C. mid The sum of the straight-line distances to the centers of all obstacles, S is S int S goal S mid The average of the three.

5. The method for path planning of a robotic arm with dynamic guidance RRT* according to claim 1, characterized in that, In step 2.3, a new node X of the growth tree is obtained. new The method is as follows: Step 2.3.1: Randomly generate a percentage. If it is not greater than the adaptive probability p of the current growth tree... v Then the random node X rand Set as target point X goal Then proceed to step 2.3.4; if it is greater than the adaptive probability p of the current growth tree. v Then proceed to step 2.3.2; Step 2.3.2: Based on the random sampling function, randomly sample and generate random nodes X in the three-dimensional space C. rand ; Step 2.3.3, search for the distance to the random node X rand The nearest node X nearest Based on X nearest and X goal Vertical sampling is performed on the connection lines, and valid random nodes are assigned to the node set T. Step 2.3.4: Traverse the set of nodes T that have been generated in the growing tree, and search for the node X at a distance from the random node. rand The nearest node X nearest The nearest node X is calculated. nearest The variable growth step size r* at the location; Step 2.3.5, with node X nearest The growth step size r* at node X, from node X nearest To random node X rand Grove the direction by this step size to generate a new node X. new .

6. The method for path planning of a robotic arm with dynamic guidance RRT* according to claim 5, characterized in that, The method for vertical plane sampling in step 2.3.3 is as follows: Connect node X nearest and target point X goal , through X nearest As X nearest and X goal The perpendicular line connecting the points obtains the line passing through node X. nearest A perpendicular plane, which divides the three-dimensional space C into two spaces, if a random node X rand With target point X goal In the same space, if the random node is valid, add the valid random node to the node set T; if random node X rand With target point X goal If they are not in the same space, the random node is discarded and resampling is performed.

7. The method for path planning of a robotic arm with dynamic guidance RRT* according to claim 5, characterized in that, The growth step size r* at the nearest node Xnearest is calculated as follows: Where r is the initial growth step size, and h is the nearest node X of the random node. nearest The relative complexity coefficient of the environment is S, which is the distance from the random node X. rand The nearest node X nearest The sum of the straight-line distances to the centers of all obstacles in three-dimensional space C.

8. The method for path planning of a robotic arm with dynamic guidance RRT* according to claim 5, characterized in that, The specific process of robotic arm path planning in step 2.4 includes: Step 2.4.1, for node X nearest To node X new Collision detection is performed on the path. If the collision fails, proceed to step 2.2; if the collision succeeds, proceed to step 2.4.

2. Step 2.4.2, within the adaptive range, adjust the new node X. new Perform parent node reselection, and for other nodes within the adaptive range, use node X. new Reconnect the parent node and update the node tree and path; Step 2.4.3: Perform collision detection on the paths updated by reselecting the parent node and reconnecting. If the collision fails, cancel the update; otherwise, proceed to step 2.4.

4. Step 2.4.4: Using path length as the evaluation metric, update the shortest path and its parent node. Step 2.4.5, determine node X new Have we entered target point X? goal If the specified domain is not specified, then node X will be... new Add it to the already generated node set T and proceed to step 2.2; if so, then start from that node X. new Continue tracing back to the parent node until a collision-free path is found, then terminate.

9. The method for path planning of a robotic arm with dynamic guidance RRT* according to claim 8, characterized in that, The radius d of the adaptive range i Represented as: Among them, h i For the i-th new node X new The relative complexity coefficient of the environment is given by n, the number of all existing nodes in the growth tree is given by d, the basic distance of the adaptive range is given by f, and the adaptive range coefficient is given by f.

10. A method for path planning of a robotic arm with dynamic guidance RRT* according to claim 8, characterized in that, In step 2.4.4, if the length of the generated path is less than the length of the original path, the parent node of the node is updated and the newly generated path is added to the tree.

Images