A branch direct connection-based adaptive multi-state bidirectional RRT mechanical arm path planning method

Abstract

The application discloses a kind of adaptive multistate bidirectional RRT mechanical arm path planning methods based on branch direct connection, the method is distinguished to random tree growth state, different exploration algorithms are used for dense obstacle environment and open environment;Target-oriented mechanism is introduced, adaptive target sampling algorithm is designed, exploration and target orientation are reasonably carried out;Adaptive growth step controller is designed, and the growth step is reasonably adjusted for open environment, complex environment and narrow channel environment;Design branch connection strategy, save computing resources while improving path search speed;In addition, two trees independently add target sampling and growth step controller to fully utilize the characteristics of double search tree.The method is aimed at the problem that path search algorithm is slow and inefficient in different environments, by designing adaptive target sampling probability algorithm, adaptive growth step algorithm and branch direct connection strategy, the performance of the algorithm is effectively improved.

Description

Technical Field

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

[0002] With the widespread application of robotic arms in various industrial automation applications, robotic arm path planning technology has become crucial. Traditional path planning methods typically 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 Randomized Randomized Tracing (RRT) algorithm expands the tree nodes gradually through random sampling, effectively exploring paths in complex workspaces. However, despite its advantages in dynamic environments and high-dimensional spaces, the RRT algorithm also faces several challenges. First, the low smoothness of the paths generated by the RRT algorithm can lead to violent joint movements during robotic arm operations, affecting efficiency and stability. Second, in environments with complex obstacles, the RRT algorithm takes a long time to generate paths and struggles to navigate narrow passages. Furthermore, as the search task progresses, the excessive number of nodes exponentially increases the computational load for path finding. In addition, conventional RRT algorithms have limitations in obstacle avoidance and path optimization, especially in high-precision, high-reliability industrial applications.

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

[0005] To address the problems of long search time and low spatial exploration efficiency in traditional RRT algorithms in practical applications, this invention provides an adaptive polymorphic bidirectional RRT path planning method for robotic arms based on direct branch connections. This method introduces novel sampling strategies, growth strategies, and connection mechanisms to provide a more efficient, accurate, and robust robotic arm path planning approach.

[0006] The objective of this invention is achieved through the following technical solution:

[0007] An adaptive polymorphic bidirectional RRT path planning method for robotic arms based on direct branch connections, applicable to Cartesian space path planning for robotic arms, includes the following steps:

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

[0009] Step 2: Use 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 growing tree. The specific update formula is as follows:

[0011]

[0012] Where p0 is the base target sampling rate; x∈R 3 x is the three-dimensional position in Cartesian space; ()·() is the vector dot product; || || is used to calculate the L2 norm; x last It is the position of the last node in the tree; x last-parent It is the position of the parent node of the last node in the tree; x goal It is the position of the root node of the target tree; k 1～4 It is a constant parameter that satisfies k1+k2+k3+k4=1; n success and n fail These are the most recent n goalsample The number of successful and unsuccessful sampling attempts during each target sampling attempt, n goalsample Take the value between 10 and 20; n iter n is the current iteration number; maxiter This is the total number of iterations;

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

[0014]

[0015] Where k1 and k2 are proportionality coefficients; p min It is the minimum target sampling probability; p max It is the maximum target sampling probability; the function Clip(x,x) min ,x max The function of ) is to constrain the value x within the bounds [x min ,x max ]Inside;

[0016] Step 5: Generate a local target point: Sampling selection is performed based on the current state of the growing tree. If the tree is in a "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 guidance probability threshold p, target tree sampling is performed, and a node is randomly returned as the output from the last n nodes of the target tree, 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 a "restricted growth" state, local target points are explored in the area around the already grown nodes. First, the branch node branch_node of the currently growing tree is obtained. 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 size of the current growth tree. The specific steps are as follows: First, calculate the cosine value of the angle between the direction vector from the node position to the nearest obstacle position (obstacle_direction) and the direction vector from the node position to the target position (steer_direction) (cos_angle). If cos_angle > 0, it is considered that the growth direction is towards the obstacle, and the growth step size is calculated according to k * obstacle_direction / cos_angle, where k is a scaling factor. Otherwise, it is considered that the growth direction is away from the obstacle direction, and the growth step size is calculated according to original_step_size * steer_distance / current_step_size, where original_step_size is the initial step size, steer_distance is the distance between the current position and the target point, and current_step_size is the current step size.

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

[0019]

[0020] Wherein, 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 for existing sampling points in random tree T1, find the node closest to the local target point in step 5, take the direction of the line connecting the nearest node to the local target point as the growth direction, and grow according to the current growth step size of the tree to obtain a new node;

[0022] Step 9: Determine whether the path from the nearest node in Step 8 to the new node's position 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 segments connecting the nearest node in Step 8 to the new node in space. Calculate the forward kinematics based on the obtained angles and perform continuous collision detection to determine whether the robot arm will collide with the obstacle. If no collision occurs, proceed to Step 11; if a collision occurs, discard the new node and proceed to Step 13.

[0024] Step 11: Add the new node as a feasible path point to tree T1. The parent node of the new node is the nearest node obtained in step 8. Mark the new node as a branch node, and set its parent node to a non-branch node. If the new node is the target sampling node, increment the target sampling success count by one. Finally, increment the success count by one in the tree's growth history.

[0025] Step 12: Perform connectivity checks on the new node and the target tree T2. Attempt to connect the new node to the branch nodes in T2 in reverse. If there exists a branch node in T2 that can connect to the new node, and the connection path in space does not collide with obstacles, and the robotic arm does not collide with obstacles when moving continuously in space, then the final path is considered found, the iteration ends, and the final path is returned based on T1 and T2. If no such point exists, proceed to step 14.

[0026] Step 13: If the new node is the target sampling node, increment the target sampling failure count by one, and increment the failure count by one in the tree growth history;

[0027] Step 14: Update the tree state based on the most 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: Swap trees T1 and T2, and return to step 2.

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

[0030] 1. By classifying the growth of trees, random sampling or target-oriented methods are used under normal conditions, while exploration of the area surrounding the original path is conducted under constraints, greatly improving the algorithm's ability to explore paths.

[0031] 2. Design an 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 narrow paths in complex environments.

[0033] 4. The original RRT algorithm needs to traverse all nodes in the target tree when making connections. By using branch connections, the connection time is greatly shortened. At the same time, the branch connectivity test uses reverse connection, that is, the connectivity test is performed starting from the last branch, which further reduces the computation time.

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

[0035] 6. To address the issues of slow speed and low efficiency of path search algorithms under different environments, adaptive target sampling probability algorithm, adaptive growth step size algorithm, and branch direct connection strategy are designed to effectively improve the performance of the algorithm. Attached Figure Description

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

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

[0038] Figure 3 This is a complex three-dimensional environment 2 used to verify an example of the present invention;

[0039] Figure 4 This is a three-dimensional environment 3 where the starting and ending environments are inconsistent, used to verify an example of the present invention;

[0040] Figure 5 The narrow passage three-dimensional environment 4 used to verify an example of the present invention;

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

[0042] Figure 7 This is a time consumption graph of an example of the present invention and other algorithms performing 50 searches in the same 3D environment 2;

[0043] Figure 8 This is a time consumption graph of an example of the present invention and other algorithms performing 50 searches in the same 3D environment 3;

[0044] Figure 9 This is a time consumption graph of an example of the present invention and other algorithms performing 50 searches in the same 3D environment 4;

[0045] Figure 10 This is an example of the present invention demonstrated in a robotic arm simulation environment. Detailed Implementation

[0046] The technical solution of the present invention will be further described below with reference to the accompanying drawings, but it is not limited thereto. Any modifications or equivalent substitutions to the technical solution of the present invention that do not depart from the spirit and scope of the technical solution of the present invention should be covered within the protection scope of the present invention.

[0047] This invention provides an adaptive multi-state bidirectional RRT (Robotic Tracking and Path Planning) robotic arm path planning method based on direct branch connections. The method distinguishes between random tree growth states and employs different exploration algorithms for obstacle-dense and open environments. It introduces a target-oriented mechanism, designs an adaptive target sampling algorithm for reasonable exploration and target orientation, designs an adaptive growth step size controller to adjust the growth step size appropriately for open, complex, and narrow passage environments, and designs a branch connection strategy to save computational resources while improving path search speed. Furthermore, target sampling and growth step size controllers are independently added to both trees to fully utilize the characteristics of the dual-search tree. Taking Cartesian space path search as an example... Figure 1 As shown, the specific steps include the following:

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

[0049] Step 2: Use 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 growing tree. The specific update formula is as follows:

[0051]

[0052] Where p0 is the base target sampling rate; x∈R 3 x is the three-dimensional position in Cartesian space; ( )·( ) is the vector dot product; || || is used to calculate the L2 norm; x last It is the position of the last node in the tree; x last-parent It is the position of the parent node of the last node in the tree; x goalIt is the position of the root node of the target tree; k 1～4 It is a constant parameter that satisfies k1+k2+k3+k4=1; n success and n fail These are the most recent n goalsample The number of successful and unsuccessful sampling attempts during each target sampling attempt, n goalsample Take the value between 10 and 20; n iter n is the current iteration number; maxiter It represents the total number of iterations.

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

[0054]

[0055] Where k1 and k2 are proportionality coefficients; p min It is the minimum target sampling probability; p max It is the maximum target sampling probability; the function Clip(x,x) min ,x max The function of ) is to constrain the value x within the bounds [x min ,x max ]Inside.

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

[0057] Sampling is performed based on the current state of the growing tree. If the tree is in a "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 guidance probability threshold p, the target tree is sampled, and a node is randomly returned as the output from the last n nodes of the target tree (the number parameter can be adjusted according to the effect, generally between 3 and 5). Otherwise, a node is randomly generated in the target task space and returned. If a node is randomly generated, it is marked as a non-target sample; otherwise, it is marked as a target sample node. Algorithm 1 shows the pseudocode for the sampling algorithm in the "normal growth" state. If the tree is in the "restricted growth" state, the local target point is explored in the area around the already grown node. First, the branch node of the currently growing tree is obtained. A reference node, reference_node, is randomly selected from the last m nodes of the branch node, where m is usually 3. Then, a random number sample_type is generated. If the random number is less than the threshold p_type, the area around the reference node is explored. The specific exploration method is as follows: 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. This 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. Finally, the position of the new node is obtained according to the growth step size. If the random number is greater than the threshold, a node is randomly generated in the planning space and returned. Algorithm 2 shows the pseudocode for the sampling algorithm in the "restricted growth" state.

[0058]

[0059]

[0060] Step 6: Update the growth step size of the current growth tree. The specific steps are as follows: First, calculate the cosine of the angle between the direction vector from the node position to the nearest obstacle position (obstacle_direction) and the direction vector from the node position to the target position (steer_direction), valued as cos_angle. If cos_angle > 0, the growth direction is considered to be towards the obstacle direction, and the growth step size is calculated as k * obstacle_direction / cos_angle, where k is a scaling factor. Conversely, if cos_angle > 0, the growth direction is considered to be away from the obstacle direction, and the growth step size is calculated as original_step_size * steer_distance / current_step_size, where original_step_size is the initial step size; steer_distance is the distance between the current position and the target point; and current_step_size is the current step size. The pseudocode for updating the step size 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] Here, 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.

[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, using the direction of the line connecting the nearest node to the local target point as the growth direction, grow the tree according to the current growth step size to obtain a new node.

[0067] Step 9: Determine if the path from the nearest node in Step 8 to the new node's location 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's posture for the line segments connecting the nearest node in Step 8 to the new node in space. Calculate the forward kinematics based on the obtained angles and perform continuous collision detection to determine whether the robot arm will collide with an obstacle. If no collision occurs, proceed to Step 11; if a collision occurs, discard the new node and proceed to Step 13.

[0069] Step 11: Add the new node as a feasible path point to tree T1. The parent node of the new node is the nearest node obtained in step 8. Mark the new node as a branch node, and set its parent node to a non-branch node. If the new node is the target sampling node, increment the target sampling success count by one. Finally, increment the success count in the tree's growth history.

[0070] Step 12: Perform connectivity detection using the new node and the branch nodes in the target tree. This connectivity detection is performed in reverse order. If there exists a branch node in T2 that can connect to the new node, and the connection path in space does not collide with obstacles, and the robotic arm does not collide with obstacles during continuous movement in space, then the final path is considered found, the iteration ends, and the final path is returned based on T1 and T2; otherwise, proceed to step 14.

[0071] Step 13: If the new node is the target sampling node, increment the target sampling failure count by one. Increment the failure count by one in the tree growth history.

[0072] Step 14: Update the tree state based on the most 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".

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

[0074] After the algorithm is completed, it is tested in different environments. Corresponding environments are set up for simple, complex, mixed, and narrow-channel environments, such as... Figure 2 , 3 As shown in Tables 1, 2, 3, and 4, tests were conducted in environments 1, 2, 3, and 4 using average planning time, average number of nodes, average path length, and average number of iterations as metrics. The results are shown in Tables 1, 2, 3, and 4, respectively.

[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 branch connections of this invention demonstrates better performance than bidirectional RRT, adaptive target bias bidirectional RRT, and adaptive target bias and step size bidirectional RRT algorithms in simple, complex, mixed, and narrow channel environments. Specifically, the average planning time is reduced by up to 80% compared to bidirectional RRT, up to 70% compared to adaptive target bias bidirectional RRT, and up to 50% compared to adaptive target bias and step size bidirectional RRT. Furthermore, the average number of nodes, average path length, and average number of iterations are also significantly reduced.

[0084] Figures 6-9 The time consumption graphs for 50 searches using different algorithms under four different environments are presented. It can be seen that the algorithm proposed in this invention can maintain high stability and has good robustness under different random number conditions. Figure 10 The simulation application performed on the robotic arm simulation platform is presented.

Claims

1. An adaptive multimorphic bidirectional RRT robotic arm path planning method based on direct branch connection, characterized in that... The method includes the following steps: Step 1: Initialize the target planning space boundaries, initialize RRT parameters, input path start point and path end point, and initialize two trees. and The two trees were marked as "growing normally"; Step 2: Place the tree It grows as a tree, the tree As a target tree; Step 3: Update the target sampling probability of the current growing tree. Target sampling probability The update formula is: in, It is the base target sampling rate; It is a three-dimensional position in Cartesian space; It is the vector dot product; It is to calculate the L2 norm; It is the position of the last node in the tree; It is the position of the parent node of the last node in the tree; It is the location of the root node of the target tree; It is a constant parameter that satisfies ; and These are the most recent The number of successful and unsuccessful attempts during each target sampling; This is the current iteration number; This is the total number of iterations; Step 4: Sample the target probability Apply constraints: in, and It is the proportionality coefficient; It is the minimum target sampling probability; It is the maximum target sampling probability; function Its function is to set the value Constraints on boundaries Inside; Step 5: Generate a local target point: Sample and select based on the current state of the growing tree. If the tree is in a "normal growth" state, use the function... exist Generate a random number between them If random number Less than the target sampling probability Then, target tree sampling is performed, starting from the last element of the target tree. If the tree is in a "restricted growth" state, then explore local target points in the area surrounding already grown nodes, starting with the branch nodes of the currently growing tree. At the last node of the branch tip Randomly select a reference node from the nodes. Then generate a random number. If the random number is less than the threshold If the random number is greater than the threshold, then explore around the reference node. If the random number is greater than the threshold, then randomly generate a node in the planning space and return it. Step 6: Update the growth step size of the current growth tree: Calculate the direction vector from the node position to the nearest obstacle position. and the direction vector from the node position to the target position cosine value of the included angle ,if The growth direction is assumed to be towards the nearest obstacle, and vice versa. Then the corresponding growth step size is calculated. Step 7: Adjust the upper and lower limits of the growth step size according to the iteration progress: in, and These are the upper and lower limits of the growth step size, respectively; This is the initial growth step size; and The functions return the input values ​​respectively. The maximum and minimum values; Step 8: Search Tree For the existing sampling points, find the node closest to the local target point in step 5, take the direction of the line connecting the nearest node to the local target point as the growth direction, and grow according to the current tree growth step size to obtain a new node; Step 9: Determine whether the path from the nearest node in Step 8 to the new node's position 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 segments connecting the nearest node in Step 8 to the new node in space. Calculate the forward kinematics based on the obtained angles and perform continuous collision detection to determine whether the robot arm will collide with the obstacle. If no collision occurs, proceed to Step 11; if a collision occurs, discard the new node and proceed to Step 13. Step 11: Add the new node to the tree as a path-feasible point. In the process, the parent node of the new node is the nearest node obtained in step 8, and the new node is marked as a branch node. The parent node of the new node is set to a non-branch node. If the new node is the target sampling node, the target sampling success count is incremented by one. Finally, the success count is incremented by one in the tree's growth history. Step 12: Connect the new node to the target tree Perform connectivity checks and attempt to connect the new node with... If the branch nodes in the middle are reverse connected, then... If a branch node can connect to a new node, and the connection path in space does not collide with obstacles, and the robotic arm does not collide with obstacles during continuous movement in space, then the final path is considered found, and the iteration ends. and Return to the final path; if such a point does not exist, proceed to step 14. Step 13: If the new node is the target sampling node, increment the target sampling failure count by one, and increment the failure count by one in the tree growth history; Step 14: Based on the tree The most recent growth history updates the tree's state. If the number of failures far exceeds the number of successes, the tree is marked as "restricted growth"; otherwise, it is marked as "normal growth". Step 15: Swap Trees and Go back to step 2.

2. The adaptive multimorphic bidirectional RRT robotic arm path planning method based on direct branch connection according to claim 1, characterized in that... The Choose between 10 and 20.

3. The adaptive multimorphic bidirectional RRT robotic arm path planning method based on direct branch connection according to claim 1, characterized in that... In step 5 Take between 3 and 5. Take 3.

4. The adaptive multimorphic bidirectional RRT robotic arm path planning method based on direct branch connection according to claim 1, characterized in that... In step 5, the specific method for exploring around the reference node is as follows: calculate the direction vector from the reference node to the nearest obstacle, select a random vector in the space perpendicular to this direction vector as the exploration direction, and finally obtain the position of the new node according to the growth step size.

5. The adaptive multi-state bidirectional RRT robotic arm path planning method based on direct branch connection according to claim 1, characterized in that... In step 6, if it grows towards the nearest obstacle, according to Calculate the growth step size, where It is a proportionality coefficient. It is the direction vector from the node position to the nearest obstacle position; if the growth direction is far from the nearest obstacle direction, it follows... Calculate the growth step size, where It initializes the growth step size. It is the distance between the current location and the target point. This is the current step size.

Images