Self-adaptive multi-state bidirectional RRT mechanical arm path planning method based on branch tip direct connection

Abstract

The invention discloses a self-adaptive multi-state bidirectional RRT mechanical arm path planning method based on branch tip direct connection, which is characterized in that the growth state of a random tree is distinguished, and different exploration algorithms are adopted for an obstacle dense environment and an open environment; a target guiding mechanism is introduced, an adaptive target sampling algorithm is designed, and exploration and target guiding are reasonably carried out; a self-adaptive growth step length controller is designed, and the growth step length is reasonably adjusted for an open environment, a complex environment and a narrow channel environment; a branch tip connection strategy is designed, so that the path search speed is increased while computing resources are saved; in addition, target sampling and growth step size controllers are independently added to the two trees so as to make full use of the characteristics of the double search trees. According to the method, the performance of the algorithm is effectively improved by designing a self-adaptive target sampling probability algorithm, a self-adaptive growth step length algorithm, a branch tip direct connection strategy and the like aiming at the problems of low speed and low efficiency of a path search algorithm in different environments.

Description

Technical Field

[0001] The present invention belongs to the field of robot and manipulator path planning, and relates to a manipulator path planning method, and specifically to an adaptive polymorphic bidirectional RRT (Rapidly-exploring Random Tree) manipulator path planning method based on direct branch connection. Background Art

[0002] With the widespread use of robotic arms in various industrial automation applications, robotic arm path planning technology has become crucial. Traditional path planning methods usually rely on grid-based algorithms, optimization algorithms or sampling algorithms, among which the sampling-based RRT algorithm is widely used due to its good computational efficiency and ability to handle complex environments.

[0003] The RRT algorithm gradually expands the nodes of the tree through random sampling, and can effectively explore paths in complex workspaces. However, although the RRT algorithm has advantages in dynamic environments and high-dimensional spaces, it also faces some problems. First, the path smoothness generated by the RRT algorithm is low, which may cause violent joint movements when the robot arm performs tasks, thus affecting efficiency and stability. Secondly, the RRT algorithm takes a long time to generate paths in environments with complex obstacles, and it is difficult to pass through narrow passages. In addition, when the search task reaches the later stage, too many nodes cause the path search calculation to double. In addition, the conventional RRT algorithm has certain limitations in obstacle avoidance and path optimization, especially in high-precision and high-reliability industrial application scenarios.

[0004] Therefore, how to further optimize path quality, improve obstacle avoidance capabilities, and enhance the robustness of path search algorithms in different environments remains a key issue that needs to be urgently addressed in the field of robotic arm path planning. Summary of the invention

[0005] In order to solve the problems of long search time and low spatial exploration efficiency of traditional RRT algorithms in practical applications, the present invention provides an adaptive polymorphic bidirectional RRT manipulator path planning method based on direct branch connection. This method provides a more efficient, accurate and robust manipulator path planning method by introducing new sampling strategies, growth strategies and connection mechanisms.

[0006] The objective of the present invention is achieved through the following technical solutions:

[0007] An adaptive polymorphic bidirectional RRT robot path planning method based on branch direct connection can be used for robot Cartesian space path planning, including the following steps:

[0008] Step 1: Initialize the target planning space boundary, initialize the RRT parameters, input the path start point and path end point, initialize two trees T1 and T2, and mark the two trees as "normal growth" state;

[0009] Step 2: Grow tree T1 as the growth tree and tree T2 as the target tree;

[0010] Step 3: Update the target sampling probability p of the current growth tree. The specific update formula is:

[0011]

[0012] Where p0 is the base target sampling rate; x∈R 3 is the three-dimensional position in Cartesian space; ()·() is the vector dot product; || || is the calculation of the two-norm; x last is the position of the last node in the tree; x last-parent is the position of the parent node of the last node in the tree; x goal is the location of the root node of the target tree; k 1～4 is a constant parameter, satisfying k1+k2+k3+k4=1; n success and n fail The most recent n goalsample The number of successes and failures when sampling the target, n goalsample Take between 10 and 20; n iter is the current iteration number; n maxiter is the total number of iterations;

[0013] Step 4: Constrain the target sampling probability p:

[0014]

[0015] Among them, k1 and k2 are proportional coefficients; p min is the minimum target sampling probability; p max is the maximum target sampling probability; the function Clip(x,x min ,x max ) is to constrain the value x to the limit [x min ,x max ]Inside;

[0016] Step 5: Generate a local target point: Sampling selection is performed according to the state of the currently growing tree. If the tree is in the "normal growth" state, a random number sample_type is generated between [0,1] using the function Random(0,1). If the random number sample_type is less than the target-oriented probability threshold p, the target tree is sampled and a node is randomly returned from the last n nodes of the target tree as output, where n is between 3 and 5. Otherwise, a node is randomly generated in the target task space and returned. If the node is a randomly generated node, it is marked as a non-target sampling node, otherwise it is marked as a target sampling node. If the tree is in the "restricted growth" state, the local target point is explored in the surrounding area of ​​the already grown node. First, the branch node branch_node of the currently growing tree is obtained, and a reference node reference_node is randomly selected from the last m nodes of the branch node, where m is generally 3. Then a random number sample_type is generated. If the random number is less than the threshold p_type, exploration is performed around the reference node. If the random number is greater than the threshold, a node is randomly generated in the planning space and returned.

[0017] Step 6: Update the growth step of the current growth tree. The specific steps are as follows: First, calculate the cosine value of the angle cos_angle between the direction vector obstacle_direction from the node position to the nearest obstacle position and the direction vector steer_direction from the node position to the target position. If cos_angle>0, it is considered that the growth direction is facing the obstacle, and the growth step is calculated according to k*obstacel_direction / cos_angle, where k is the proportional coefficient; otherwise, it is considered that the growth direction is away from the obstacle direction. At this time, the growth step is calculated according to original_step_size*steer_distance / current_step_size, where original_step_size is the initialization step, steer_distance is the distance between the current position and the target point, and current_step_size is the current step;

[0018] Step 7: Adjust the upper and lower limits of the growth step size according to the iteration progress:

[0019]

[0020] Among them, min_step_size and max_step_size are the upper and lower limits of the growth step size respectively; original_step_size is the initial growth step size; max(a,b) and min(a,b) functions return the maximum and minimum values ​​of the input values ​​a and b respectively;

[0021] Step 8: Search the existing sampling points in the random tree T1, find the node closest to the local target point in step 5, take the line direction from the nearest node to the local target point as the growth direction, grow according to the growth step of the current tree, and get a new node;

[0022] Step 9: Determine whether the path from the position of the nearest node in step 8 to the position of the new node collides with an obstacle. If no collision occurs, proceed to step 10; if a collision occurs, discard the new node and proceed to step 13;

[0023] Step 10: Solve the continuous inverse kinematics of the robot arm posture for the line segment connecting the nearest node in step 8 to the new node in space, calculate the forward kinematics based on the obtained angle and perform continuous collision detection to determine whether the robot arm will collide with the obstacle. If no collision occurs, go to step 11; if a collision occurs, discard the new node and go to step 13;

[0024] Step 11: Add the new node as a feasible path point to the tree T1. The parent node of the new node is the nearest node obtained in step 8. At the same time, mark the new node as a branch node, and set the parent node of the new node to a non-branch node. If the new node is the target sampling node, add one to the target sampling success count. Finally, add one to the success count in the tree growth history;

[0025] Step 12: Check the connectivity between the new node and the target tree T2, and try to reversely connect the new node to the branch nodes in T2. ​​If there is a branch node in T2 that can be connected to the new node, and the connection path in space does not collide with obstacles, and the robot arm does not collide with obstacles when it moves continuously in space, then it is considered that the final path is found, the iteration ends, and the final path is returned according to T1 and T2; if there is no such point, go to step 14;

[0026] Step 13: If the new node is the target sampling node, increase the number of target sampling failures by one, and increase the number of failures by one in the tree growth history;

[0027] Step 14: Update the state of the tree according to the recent growth history of tree T1. If the number of failures is much greater than the number of successes, mark the tree as "restricted growth"; otherwise, mark the tree as "normal growth";

[0028] Step 15: Exchange trees T1 and T2 and return to step 2.

[0029] Compared with the prior art, the present invention has the following advantages:

[0030] 1. Classify the growth of the tree, perform random sampling or target-oriented sampling under normal circumstances, and explore the area around the original path under restricted conditions, which greatly improves the algorithm's ability to explore the path.

[0031] 2. Design adaptive goal-oriented probability to adaptively adjust the tendency of random exploration and goal-oriented actions according to environmental conditions.

[0032] 3. Design an adaptive growth step size to efficiently explore space in open environments and search for narrow paths in complex environments.

[0033] 4. The original RRT algorithm needs to traverse all nodes in the target tree when connecting. The connection time is greatly shortened through branch connection. At the same time, the branch connectivity detection adopts reverse connection, that is, the connectivity test starts from the last branch, which further reduces the calculation time.

[0034] 5. The two trees grow independently, and each has an adaptive controller, so they can perform path search well even when the starting and ending environments are inconsistent.

[0035] 6. In order to solve the problem of slow speed and low efficiency of path search algorithm in different environments, the performance of the algorithm was effectively improved by designing adaptive target sampling probability algorithm, adaptive growth step algorithm and branch direct connection strategy. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 It is the overall structural diagram of the adaptive polymorphic bidirectional RRT manipulator path planning method based on branch direct connection;

[0037] Figure 2 A simple three-dimensional environment 1 is used to verify an example of the present invention;

[0038] Figure 3 It is a complex three-dimensional environment 2 used for verification of an example of the present invention;

[0039] Figure 4 A three-dimensional environment 3 in which the starting and ending environments are inconsistent is used for verification of an example of the present invention;

[0040] Figure 5 It is a narrow channel three-dimensional environment 4 used for verification of an example of the present invention;

[0041] Figure 6 It is a time-consuming graph of an example of the present invention and other algorithms performing 50 searches in the same three-dimensional environment 1;

[0042] Figure 7 It is a time-consuming graph of an example of the present invention and other algorithms performing 50 searches in the same three-dimensional environment 2;

[0043] Figure 8 It is a time-consuming graph of an example of the present invention and other algorithms performing 50 searches in the same three-dimensional environment 3;

[0044] Fig. 9 It is a time-consuming graph of an example of the present invention and other algorithms performing 50 searches in the same three-dimensional environment 4;

[0045] Fig.10 It is a demonstration of an example of the present invention in a robot arm simulation environment. DETAILED DESCRIPTION

[0046] The technical solution of the present invention is further described below in conjunction with the accompanying drawings, but is not limited thereto. Any modification or equivalent replacement of the technical solution of the present invention without departing from the spirit and scope of the technical solution of the present invention should be included in the protection scope of the present invention.

[0047] The present invention provides an adaptive polymorphic bidirectional RRT manipulator path planning method based on direct branch connection. The method distinguishes the growth state of random trees, adopts different exploration algorithms for obstacle-intensive environments and open environments; introduces a goal-oriented mechanism, designs an adaptive target sampling algorithm, and reasonably conducts exploration and goal orientation; designs an adaptive growth step controller to reasonably adjust the growth step for open environments, complex environments, and narrow channel environments; designs a branch connection strategy to save computing resources while improving the path search speed; in addition, two trees independently add target sampling and growth step controllers to fully utilize the characteristics of dual search trees. Taking Cartesian space path search as an example, Figure 1 As shown, the specific steps include:

[0048] Step 1: Initialize the target planning space boundary, initialize the RRT parameters, input the path start point and path end point, initialize two trees T1 and T2, and mark the two trees as "normal growth" state.

[0049] Step 2: Grow tree T1 as the growth tree and tree T2 as the target tree.

[0050] Step 3: Update the target sampling probability p of the current growth tree. The specific update formula is:

[0051]

[0052] Where p0 is the base target sampling rate; x∈R 3 is the three-dimensional position in Cartesian space; ()·() is the vector dot product; || || is the calculation of the two-norm; x last is the position of the last node in the tree; x last-parent is the position of the parent node of the last node in the tree; x goalis the location of the root node of the target tree; k 1～4 is a constant parameter, satisfying k1+k2+k3+k4=1; n success and n fail The most recent n goalsample The number of successes and failures when sampling the target, n goalsample Take between 10 and 20; n iter is the current iteration number; n maxiter is the total number of iterations.

[0053] Step 4: Constrain the target sampling probability p:

[0054]

[0055] Among them, k1 and k2 are proportional coefficients; p min is the minimum target sampling probability; p max is the maximum target sampling probability; the function Clip(x,x min ,x max ) is to constrain the value x to the limit [x min ,x max ]Inside.

[0056] Step 5: Generate a local target point. The specific steps are as follows:

[0057] Sampling selection is performed based on the state of the currently growing tree. If the tree is in the "normal growth" state, the function Random(0,1) is used to generate a random number sample_type between [0,1]. If the random number sample_type is less than the target-oriented probability threshold p, the target tree is sampled and a node is randomly returned from the last n nodes of the target tree (the number parameter can be adjusted according to the effect, generally between 3 and 5) as output; otherwise, a node is randomly generated in the target task space and returned. If the node is a randomly generated node, it is marked as a non-target sampling, otherwise it is marked as a target sampling node. Algorithm 1 shows the pseudo code of the sampling algorithm in the "normal growth" state; if the tree is in the "restricted growth" state, then choose to explore the local target point in the surrounding area of ​​the node that has grown. First, obtain the branch node branch_node of the currently growing tree, and randomly select a reference node reference_node from the last m nodes of the branch node. m is generally 3, and then generate a random number sample_type. If the random number is less than the threshold p_type, then explore around the reference node. The specific exploration method is: calculate the direction vector from the reference node to the nearest obstacle, and select a random vector in the space perpendicular to this direction vector as the exploration direction. The present invention uses a random three-dimensional vector to calculate its cross product with the obstacle direction to obtain a random exploration direction to achieve the goal, and finally obtain the position of the new node according to the growth step; if the random number is greater than the threshold, a node is randomly generated in the planning space and returned. Algorithm 2 shows the pseudo code of the sampling algorithm in the "restricted growth" state.

[0058]

[0059]

[0060] Step 6: Update the growth step of the current growth tree. The specific steps are as follows: First, calculate the cosine value of the angle cos_angle between the direction vector obstacle_direction from the node position to the nearest obstacle position and the direction vector steer_direction from the node position to the target position. If cos_angle>0, it is considered that the growth direction is towards the obstacle direction, and the growth step is calculated according to k*obstacle_direction / cos_angle, where k is the proportional coefficient; otherwise, it is considered that the growth direction is away from the obstacle direction. At this time, the growth step is calculated according to original_step_size*steer_distance / current_step_size, where original_step_size is the initialization step; steer_distance is the distance between the current position and the target point; current_step_size is the current step. The pseudo code for updating the step is as follows:

[0061]

[0062]

[0063] Step 7: Adjust the upper and lower limits of the growth step size according to the iteration progress:

[0064]

[0065] Among them, mni_step_size and max_step_size are the upper and lower limits of the growth step size respectively; original_step_size is the initialization growth step size; max(a,b) and min(a,b) functions return the maximum and minimum values ​​of the input values ​​a and b respectively.

[0066] Step 8: Search the existing sampling points in the random tree T1 to find the node closest to the local target point in step 5. Next, take the direction of the line from the nearest node to the local target point as the growth direction, grow according to the growth step of the current tree, and get a new node.

[0067] Step 9: Determine whether the path from the position of the nearest node in step 8 to the position of the new node collides with an obstacle. If no collision occurs, proceed to step 10; if a collision occurs, discard the new node and proceed to step 13.

[0068] Step 10: Solve the continuous inverse kinematics of the robot arm posture for the line segment connecting the nearest node in step 8 to the new node in space, calculate the forward kinematics based on the obtained angle and perform continuous collision detection to determine whether the robot arm will collide with the obstacle. If no collision occurs, go to step 11; if a collision occurs, discard the new node and go to step 13.

[0069] Step 11: Add the new node as a feasible path point to the tree T1. The parent node of the new node is the nearest node obtained in step 8. At the same time, mark the new node as a branch node, and set the parent node of the new node to a non-branch node. If the new node is the target sampling node, add one to the target sampling success count. Finally, add one to the success count in the tree growth history.

[0070] Step 12: Use the new node and the branch nodes in the target tree to perform connectivity detection, which is a reverse order detection. If there is a branch node in T2 that can be connected to the new node, and the connection path in space does not collide with obstacles, and the robot does not collide with obstacles when it moves continuously in space, then it is considered that the final path is found, the iteration ends, and the final path is returned according to T1 and T2; if there is no such point, go to step 14.

[0071] Step 13: If the new node is the target sampling node, increase the number of target sampling failures by 1. Increase the number of failures by 1 in the tree growth history.

[0072] Step 14: Update the state of the tree according to the recent growth history of tree T1. If the number of failures is much greater than the number of successes, mark the tree as "restricted growth" state; otherwise, mark the tree as "normal growth" state.

[0073] Step 15: Exchange trees T1 and T2 and return to step 2.

[0074] After the algorithm is completed, it is tested in different environments. The corresponding environments are built according to simple environments, complex environments, mixed environments, and narrow channel environments, such as Figure 2 , 3 , 4, and 5. The average planning time, average number of nodes, average path length, and average number of iterations were used as measurement indicators to test in environments 1, 2, 3, and 4, and Tables 1, 2, 3, and 4 were obtained, respectively, as shown below:

[0075] Table 1

[0076]

[0077] Table 2

[0078]

[0079] Table 3

[0080]

[0081] Table 4

[0082]

[0083] Analyzing the data in the table, the adaptive polymorphic bidirectional RRT based on direct connection of branches and tips in the present invention has better effects than bidirectional RRT, adaptive target bias bidirectional RRT and adaptive target bias and step bidirectional RRT algorithms in simple environments, complex environments, mixed environments and narrow channel environments. Among them, the average planning time is reduced by up to 80% compared with bidirectional RRT, up to 70% compared with adaptive target bias bidirectional RRT, and up to 50% compared with adaptive target bias and step bidirectional RRT; in addition, the average number of nodes, average path length and average number of iterations are also greatly reduced.

[0084] Figure 6 to Figure 9 The time consumption graphs of 50 searches for different algorithms under four environments are given, from which it can be seen that the algorithm proposed in the present invention can maintain high stability under different random number conditions and has good robustness. Fig.10 The simulation application on the robotic arm simulation platform is given.

Claims

1. An adaptive polymorphic bidirectional RRT manipulator path planning method based on branch direct connection, characterized in that The method comprises the following steps: Step 1: Initialize the target planning space boundary, initialize the RRT parameters, input the path start point and path end point, initialize two trees T1 and T2, and mark the two trees as "normal growth" state; Step 2: Grow tree T1 as the growth tree and tree T2 as the target tree; Step 3: Update the target sampling probability p of the current growth tree; Step 4: Constrain the target sampling probability p: Among them, k1 and k2 are proportional coefficients; p min is the minimum target sampling probability; p max is the maximum target sampling probability; the function Clip(x,x min ,x max ) is to constrain the value x to the limit [x min ,x max ]Inside; Step 5: Generate a local target point: Sampling selection is performed according to the state of the currently growing tree. If the tree is in the "normal growth" state, a random number sample_type is generated between [0,1] using the function Random(0,1). If the random number sample_type is less than the target-oriented probability threshold p, the target tree is sampled and a node is randomly returned from the last n nodes of the target tree as the output. Otherwise, a node is randomly generated in the target task space and returned. If the node is a randomly generated node, it is marked as a non-target sampling node, otherwise it is marked as a target sampling node. If the tree is in the "restricted growth" state, the local target point is explored in the surrounding area of ​​the already grown node. First, the branch node branch_node of the currently growing tree is obtained, and a reference node reference_node is randomly selected from the last m nodes of the branch node. Then a random number sample_type is generated. If the random number is less than the threshold p_type, exploration is performed around the reference node. If the random number is greater than the threshold, a node is randomly generated in the planning space and returned. Step 6: Update the growth step of the current growth tree: calculate the cosine value of the angle cos_angle between the direction vector obstacle_direction from the node position to the nearest obstacle position and the direction vector steer_direction from the node position to the target position. If cos_angle>0, it is considered that the growth direction is towards the obstacle direction. Otherwise, it is considered that the growth direction is away from the obstacle direction. Then calculate the corresponding growth step lengths respectively. Step 7: Adjust the upper and lower limits of the growth step size according to the iteration progress: Among them, min_step_size and max_step_size are the upper and lower limits of the growth step size respectively; original_step_size is the initial growth step size; max(a,b) and min(a,b) functions return the maximum and minimum values ​​of the input values ​​a and b respectively; Step 8: Search the existing sampling points in the random tree T1, find the node closest to the local target point in step 5, take the line direction from the nearest node to the local target point as the growth direction, grow according to the growth step of the current tree, and get a new node; Step 9: Determine whether the path from the position of the nearest node in step 8 to the position of the new node collides with an obstacle. If no collision occurs, proceed to step 10; if a collision occurs, discard the new node and proceed to step 13; Step 10: Solve the continuous inverse kinematics of the robot arm posture for the line segment connecting the nearest node in step 8 to the new node in space, calculate the forward kinematics based on the obtained angle and perform continuous collision detection to determine whether the robot arm will collide with the obstacle. If no collision occurs, go to step 11; if a collision occurs, discard the new node and go to step 13; Step 11: Add the new node as a feasible path point to the tree T1. The parent node of the new node is the nearest node obtained in step 8. At the same time, mark the new node as a branch node, and set the parent node of the new node to a non-branch node. If the new node is the target sampling node, add one to the target sampling success number, and finally add one to the success number in the tree growth history. Step 12: Check the connectivity between the new node and the target tree T2, and try to reversely connect the new node to the branch nodes in T2. ​​If there is a branch node in T2 that can be connected to the new node, and the connection path in space does not collide with obstacles, and the robot arm does not collide with obstacles when it moves continuously in space, then it is considered that the final path is found, the iteration ends, and the final path is returned according to T1 and T2; if there is no such point, go to step 14; Step 13: If the new node is the target sampling node, increase the number of target sampling failures by one, and increase the number of failures by one in the tree growth history; Step 14: Update the state of the tree according to the recent growth history of tree T1. If the number of failures is much greater than the number of successes, mark the tree as "restricted growth"; otherwise, mark the tree as "normal growth"; Step 15: Exchange trees T1 and T2 and return to step 2.

2. The adaptive multi-state bidirectional RRT manipulator path planning method based on branch direct connection according to claim 1 is characterized in that In step 3, the update formula of the target sampling probability p is: Where p0 is the base target sampling rate; x∈R 3 is the three-dimensional position in Cartesian space; ( )·( ) is the vector dot product; || || is the calculation of the binorm; x last is the position of the last node in the tree; x last-parent is the position of the parent node of the last node in the tree; x goal is the location of the root node of the target tree; k 1～4 is a constant parameter, satisfying k1+k2+k3+k4=1; n success and n fail The most recent n goalsample The number of successes and failures when sampling the target; m iter is the current iteration number; n maxiter is the total number of iterations.

3. The adaptive multi-state bidirectional RRT manipulator path planning method based on branch direct connection according to claim 2 is characterized in that The goalsample Take between 10 and 20.

4. The adaptive multi-state bidirectional RRT manipulator path planning method based on branch direct connection according to claim 1 is characterized in that In step 5, n is between 3 and 5, and m is 3.

5. The adaptive multi-state bidirectional RRT manipulator path planning method based on branch direct connection according to claim 1 is characterized in that In step 5, the specific method of exploring around the reference node is: calculating the direction vector from the reference node to the nearest obstacle, selecting a random vector in the space perpendicular to the direction vector as the exploration direction, and finally obtaining the position of the new node according to the growth step.

6. The adaptive multi-state bidirectional RRT manipulator path planning method based on branch direct connection according to claim 1 is characterized in that In step 6, if the growth is towards the nearest obstacle, the growth step size is calculated according to k*obstacle_direction / cos_angle, where k is the proportional coefficient and ngoalsample_direction is the direction vector of the node position toward the obstacle position; if the growth direction is away from the obstacle direction, the growth step size is calculated according to original_step_size*steer_distance / current_step_size, where original_step_size is the initialization step size, steer_distance is the distance between the current position and the target point, and current_step_size is the current step size.

Images