An improved RRT path planning algorithm based on a mechanical arm

Abstract

The application discloses a kind of improved RRT path planning algorithm based on mechanical arm in the technical field of mechanical arm path planning, specific steps are as follows, S1, obtain planning algorithm initial parameter;S2, start improved RRT path planning algorithm;S3, execute loop;S4, backtrack parent node to obtain path;S5, obtain final path, target trend coefficient k is designed, so that the direction of node expansion is more flexible, gently trends to target direction, and in node expansion step, target trend coefficient k is influenced by obstacle detection, dexterous workspace detection, can be more in line with the characteristics of this working object of mechanical arm, reduce the number of invalid expansion nodes, feasibility detection is carried out in two aspects of target trend coefficient calculation and waypoint detection, but the detection purpose of target trend coefficient calculation is to adjust the direction of new node, avoid invalid expansion to outside workspace and obstacle direction.

Description

Technical Field

[0001] This invention relates to the field of robotic arm path planning technology, and in particular to an improved RRT path planning algorithm based on robotic arms. Background Technology

[0002] A robotic arm is a widely used automated machine capable of performing functions such as handling, grasping, and moving. It is a complex system with high precision, multiple inputs, and outputs, exhibiting high nonlinearity and strong coupling. Due to its unique operational flexibility, it has been widely used in industrial assembly, safety and explosion-proof fields, and other areas.

[0003] In many scenarios, using robotic arms to accomplish tasks requires simultaneously performing obstacle avoidance of dynamic or static obstacles. Therefore, it is necessary to have a robotic arm path planning algorithm that avoids obstacles and is superior to the robotic arm in both distance and time, in order to improve the working efficiency of the robotic arm and ensure safety.

[0004] Due to the large and complex workspace of robotic arms, sampling route planning algorithms are more suitable for application in traditional path planning algorithms. The RRT algorithm is a typical example of a robotic arm routing algorithm, and many improved versions have emerged. Path planning problems for robotic arms are generally considered from Cartesian space or joint space. However, most improved algorithms first plan the end effector of the robotic arm in Cartesian space, and then test whether all points can be executed on the robotic arm after planning, i.e., converting to joint space to confirm execution, which affects the algorithm's efficiency. Some algorithms plan in joint space and then convert to Cartesian space to check execution, but such algorithms sacrifice some of the time and distance optimization advantages in Cartesian space. Furthermore, RRT and some of its algorithms have weak target orientation and strong randomness, leaving room for improvement.

[0005] Therefore, how to improve the path planning algorithm based on robotic arms to make it more suitable for robotic arms, and how to combine the characteristics of planning in Cartesian space and planning in joint space, have become technical problems that urgently need to be solved by those skilled in the art. Summary of the Invention

[0006] The purpose of this section is to outline some aspects of embodiments of the present invention and to briefly describe some preferred embodiments. Simplifications or omissions may be made in this section, as well as in the abstract and title of the invention, to avoid obscuring the purpose of these documents; however, such simplifications or omissions should not be construed as limiting the scope of the invention.

[0007] Therefore, the purpose of this invention is to provide an improved RRT path planning algorithm based on a robotic arm, which can solve the problems of how to improve existing path planning algorithms based on robotic arms to make them more suitable for robotic arms, and how to combine the characteristics of planning in Cartesian space and planning in joint space.

[0008] To address the aforementioned technical problems, this invention provides an improved RRT path planning algorithm based on a robotic arm, employing the following technical solution: Specific steps are as follows:

[0009] S1. Obtain the initial parameters of the planning algorithm: The initial parameters are a necessary condition for the improved algorithm to start execution and need to be obtained before the algorithm is executed. The initial parameters include the following items;

[0010] S2. Start the improved RRT path planning algorithm, add the starting point to tree T, that is, take the starting point as the root node, and gradually add leaf nodes to form an extended tree.

[0011] S3, Execute the loop: The loop part mainly corresponds to step 2, which mainly completes the addition of leaf nodes in tree T. Through continuous looping, an extended tree with the starting point as the root node is gradually formed.

[0012] S4. Backtracking the parent node to obtain the path: After the loop ends, based on q from the last loop... new And the tree T backtracks to its parent node;

[0013] S5. Obtain the final path: Through the backtracking in step S4, all the nodes found are arranged in order, which are the final calculated path points. All path points have distance optimization in Cartesian space, and through the detection in step S35, it is guaranteed that all path points are feasible for the robotic arm, without the need to convert to joint space for verification.

[0014] Optionally, the initial parameters in S1 include the three-dimensional coordinates of the start and end points of the path, the step size of the path planning algorithm, and a threshold. Number of iterations N.

[0015] Optionally, S1 includes the following steps: S11, the start and end point information of the path, represented by three-dimensional coordinates in a Cartesian coordinate system, with the start and end points represented by q. init q goal express;

[0016] S12, stepsize of the path planning algorithm, specifies the step size for each new node expansion;

[0017] S13, Algorithm Threshold This indicates that the distance between the newly expanded node and the endpoint is less than or equal to the threshold. When the time is reached, it is considered that the endpoint has been reached, and the loop steps will no longer be executed;

[0018] S14, the number of iterations N, specifies the upper limit of the number of iterations. The algorithm stops after N iterations.

[0019] S15. Additionally, the three-dimensional information of the obstacle is used as known information in subsequent steps.

[0020] Optionally, step S3 includes the following steps: S31, since the dexterity workspace of the robotic arm is difficult to describe precisely using mathematical methods, let the maximum extended arm length of the robotic arm be the radius r, and roughly define the sampling space as a sphere with radius r, and perform random sampling within it to obtain random points q. rand ;

[0021] S32, obtain random point q rand Then, calculate the relationship between all points in tree T and the random point q. rand The Euclidean distance is used to select the point that is closest to the nearest point q. near , by q near Extend the new node backward;

[0022] S33, Calculate the target tendency coefficient k: In this step, k represents whether the new node is more inclined towards the target direction or the direction of the random point, 0 < k < 1. The value of k is initialized to 0.1 in each loop. First, obtain the random point q... rand Then, first detect the random point q. rand When calculating the inverse kinematics of the robotic arm using 3D coordinates, does it meet the joint constraints? If it does, then check... feasibility =1, otherwise check feasibility =0; when check feasibility When = 1, k = k + m f m f For random point q rand The bias coefficient within the workspace. Further, the random point q is detected. rand Check if it falls within an obstacle. If not, then check... collision =1, otherwise check collision =0; when check collision When = 1, k = k + m c m c For random point q rand The bias coefficient outside the obstacle area. Since the workspace of the robotic arm is basically continuous and smooth, and to avoid planning the robotic arm end effector to enter areas outside the robotic arm workspace, a bias coefficient of 0.1 ≤ m is set. c ≤m f m c +m f +0.1≤0.9.

[0023] The target tendency coefficient k is further used to calculate the new extended node q in step S34. new ;

[0024] S34, via k, q rand q near Calculate the new extended node q new The new extended node q is mainly calculated using formula (1). new :

[0025]

[0026] Where stepsize is the algorithm step size set in advance in step S12, and a is a variable step size coefficient, set to a normal number to avoid random points q. rand With the nearest point q near The appropriate value range for a is 1 < a ≤ 5, which can cause the algorithm to lock up if the a value is too close. Similar to the traditional RRT algorithm, q represents a random point. rand , endpoint q goal With the nearest point q near The Euclidean distance;

[0027] Combining steps S33 and S34, with the target tendency coefficient k initially valued at 0.1, the random point q rand Randomly obtained, for q rand The confidence level is low, and the newly calculated extended node q is then... new It is more biased towards the target direction; through the two types of detection in step S33, the newly expanded node q can be... new The direction gradually moves towards q rand Getting closer allows new nodes to approach the target more gently, bypassing obstacles at the lowest possible cost and keeping the nodes within working range;

[0028] S35, use the function q_Check to determine q new Does it meet two types of constraints: the newly expanded node q? new The detection method, the function q_Check, is mainly divided into two parts, where q_Check... feasibility Indicates the newly expanded node q new Within the dexterous workspace of the robotic arm, and within the joint space of the robotic arm, it can reach; q_Check collision This indicates the use of the newly expanded node q new The nearest point q obtained in step S32 near For paths to both endpoints to avoid collisions with obstacles on the map, check q_Check. feasibility q_Check collisionWhen both of these checks are satisfied, the function q_Check returns Flag = 1, and the newly expanded node q is added. new Add it to tree T as the nearest vertex q near The child nodes; if Flag = 0, then the newly expanded node q new If the requirements are not met, repeat step S3.

[0029] S36, Based on the result of step S35, determine whether to end the loop: Assuming the result of step S35 is Flag = 1, verify the new extended node q. new Is it within the threshold range of the target point, i.e. Is it true? If it is true? Then the loop ends and proceeds to step S4; if the condition is not met... Then proceed to step S3.

[0030] Optionally, step S4 includes the following step: S41, the condition for executing step S4 must be q at the time of the last loop. new Meet the conditions This terminates the loop;

[0031] S42, the new extended node q from the last loop. new Considered as the endpoint q goal The parent node;

[0032] S43, from the new extended node q of the last cycle. new As a child node, it continuously backtracks to its parent node in the extended tree T until it reaches the root node (i.e., the starting point q). init ).

[0033] In summary, the present invention has at least one of the following beneficial effects: a target tendency coefficient k is designed, which makes the direction of node expansion more flexible and gently tends towards the target direction. In the node expansion step, the target tendency coefficient k is affected by obstacle detection and dexterous workspace detection, which can better fit the characteristics of the working object of the robotic arm and reduce the number of invalid expansion nodes.

[0034] Feasibility testing is performed in two aspects: target trend coefficient calculation and waypoint detection. However, the purpose of target trend coefficient calculation is to adjust the direction of new nodes and avoid ineffective expansion outside the workspace or towards obstacles. The purpose of waypoint detection is to ensure the feasibility of new nodes for the robotic arm. Compared with other path planning algorithms for robotic arms, integrating waypoint feasibility testing into the planning algorithm ensures that the path obtained by tracing parent nodes is always feasible for the robotic arm, rather than planning and then verifying the feasibility of waypoints. This improves the efficiency of the overall planning and avoids the trouble of having to replan when there are infeasible points, which is a common practice in the planning-then-detection model. Attached Figure Description

[0035] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0036] Figure 1 This is a flowchart of the improved RRT path planning algorithm based on a robotic arm provided by the present invention;

[0037] Figure 2 This is a schematic diagram of the node expansion steps in the improved algorithm of this invention;

[0038] Figure 3 A schematic diagram illustrating the calculation method of the target tendency coefficient k;

[0039] Figure 4 A schematic diagram illustrating the calculation method for extended node detection steps;

[0040] Figure 5 Improved algorithm overall flowchart;

[0041] Figure 6 This is a schematic diagram illustrating the improved algorithm based on MATLAB simulation of the present invention. Figure 1 ;

[0042] Figure 7 This is a schematic diagram illustrating the improved algorithm based on MATLAB simulation of the present invention. Figure 2 ;

[0043] Figure 8 This is a schematic diagram illustrating the improved algorithm based on MATLAB simulation of the present invention. Figure 3 . Detailed Implementation

[0044] The following is in conjunction with the appendix Figure 1-8 The present invention will be described in further detail below.

[0045] like Figure 1 As shown, the improved RRT path planning algorithm based on a robotic arm provided by this invention includes the following steps performed in sequence:

[0046] S1. Obtain the initial parameters for the planning algorithm: such as... Figure 5 Initial parameters are a necessary condition for the improved algorithm to start execution and need to be obtained before the algorithm is executed. Initial parameters include the following items;

[0047] S11, the start and end point information of the path, represented by three-dimensional coordinates in a Cartesian coordinate system, with the start and end points represented by q. initq goal express;

[0048] S12, stepsize of the path planning algorithm, specifies the step size for each new node expansion;

[0049] S13, Algorithm Threshold This indicates that the distance between the newly expanded node and the endpoint is less than or equal to the threshold. When the time is reached, it is considered that the endpoint has been reached, and the loop steps will no longer be executed;

[0050] S14, the number of iterations N, specifies the upper limit of the number of iterations. The algorithm stops after N iterations.

[0051] S15. Additionally, the three-dimensional information of the obstacle is used as known information in subsequent steps.

[0052] S2. Start the improved RRT path planning algorithm, add the starting point to tree T, that is, take the starting point as the root node, and gradually add leaf nodes to form an extended tree.

[0053] S3, Execution Loop: The loop part mainly corresponds to... Figure 5 Steps 2 to 12 in the process mainly involve adding leaf nodes to tree T, and through continuous iteration, gradually forming an expanded tree with the starting point as the root node. The iterative steps mainly include the following parts:

[0054] S31. Since the dexterity workspace of the robotic arm is difficult to describe precisely using mathematical methods, let the maximum extended arm length of the robotic arm be the radius r. Roughly define the sampling space as a sphere with radius r, and perform random sampling within it to obtain random points q. rand ;

[0055] S32, obtain random point q rand Then, calculate the relationship between all points in tree T and the random point q. rand The Euclidean distance is used to select the point that is closest to the nearest point q. near , by q near Extend the new node backward;

[0056] S33, Calculate the target tendency coefficient k: This step is as follows Figure 3 As shown. k represents whether the new node is more inclined towards the target direction or the direction of the random point, 0 < k < 1, and the value of k is initialized to 0.1 in each loop. First, obtain the random point q... rand Then, first detect the random point q. rand When solving the inverse kinematics of a robotic arm using 3D coordinates, check if it meets the joint constraints (i.e., if there is a solution within the robotic arm's workspace). If it does, then check... feasibility =1, otherwise check feasibility =0; when checkfeasibility When = 1, k = k + m f m f For random point q rand The bias coefficient within the workspace. Further, the random point q is detected. rand Check if it falls within an obstacle. If not, then check... collision =1, otherwise check collision =0; when check collision When = 1, k = k + m c m c For random point q rand The bias coefficient outside the obstacle area. Since the workspace of the robotic arm is basically continuous and smooth, and to avoid planning the robotic arm end effector to enter areas outside the robotic arm workspace, a bias coefficient of 0.1 ≤ m is set. c ≤m f m c +m f +0.1≤0.9.

[0057] The target tendency coefficient k is further used to calculate the new extended node q in step S34. new ;

[0058] S34, via k, q rand q near Calculate the new extended node q new The new extended node q is mainly calculated using formula (1). new ,correspond Figure 5 Step 6 in the process:

[0059]

[0060] Where stepsize is the algorithm step size set in advance in step S12, and a is a variable step size coefficient, set to a normal number to avoid random points q. rand With the nearest point q near The appropriate value range for a is 1 < a ≤ 5, which can cause the algorithm to lock up if the a value is too close. Similar to the traditional RRT algorithm, q represents a random point. rand , endpoint q goal With the nearest point q near The Euclidean distance;

[0061] Combining steps S33 and S34, with the target tendency coefficient k initially valued at 0.1, the random point q rand Randomly obtained, for q rand The confidence level is low, and the newly calculated extended node q is then... new It is more biased towards the target direction; through the two types of detection in step S33, the newly expanded node q can be...new The direction gradually moves towards q rand Getting closer allows new nodes to approach the target more gently, bypassing obstacles at the lowest possible cost and keeping the nodes within working range;

[0062] S35, use the function q_Check to determine q new Does it meet two types of constraints: the newly expanded node q? new The detection methods are as follows Figure 4 As shown, corresponding Figure 5 Step 7. The function q_Check is mainly divided into two parts, where q_Check... feasibility Indicates the newly expanded node q new Within the dexterous workspace of the robotic arm, and within the joint space of the robotic arm, it can reach; q_Check collision This indicates the use of the newly expanded node q new The nearest point q obtained in step S32 near The path between the two endpoints must not collide with obstacles on the map. (In q_Check) feasibility q_Check collision When both of these checks are satisfied, the function q_Check returns Flag = 1, and the newly expanded node q is added. new Add it to tree T as the nearest vertex q near The child nodes; if Flag = 0, then the newly expanded node q new If the requirements are not met, repeat step S3.

[0063] S36, Based on the result of step S35, determine whether to end the loop: This step corresponds to... Figure 5 In steps 9 to 13, assuming the result of step S35 is Flag = 1, verify the new extended node q. new Is it within the threshold range of the target point, i.e. Is it true? If it is true? Then the loop ends and proceeds to step S4; if the condition is not met... Then continue with step S3;

[0064] S4. Backtracking the parent node to obtain the path: After the loop ends, based on q from the last loop... new And the tree T backtracks to its parent node;

[0065] S41, the condition for executing step S4 must be the q of the last loop. new Meet the conditions This terminates the loop;

[0066] S42, the new extended node q from the last loop. new Considered as the endpoint q goal The parent node;

[0067] S43, from the new extended node q of the last cycle. new As a child node, it continuously backtracks to its parent node in the extended tree T until it reaches the root node (i.e., the starting point q). init );

[0068] S5. Obtain the final path: Through backtracking in step S4, all the found nodes are arranged in order, which are the final calculated path points. All path points have optimal distances in Cartesian space, and through the detection in step S35, it is guaranteed that all path points are feasible for the robotic arm, without needing to be converted to joint space for verification.

[0069] The above are all preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Therefore, all equivalent changes made in accordance with the structure, shape and principle of the present invention should be covered within the scope of protection of the present invention.

Claims

1. An improved RRT path planning algorithm based on a robotic arm, characterized in that: The specific steps are as follows: S1. Obtaining Initial Parameters for the Planning Algorithm: Initial parameters are a necessary condition for the improved algorithm to start execution and must be obtained before the algorithm runs. Initial parameters include the following: the three-dimensional coordinates of the path's start and end points, the step size of the path planning algorithm, and the threshold. Number of iterations N; S2. Start the improved RRT path planning algorithm, add the starting point to tree T, that is, take the starting point as the root node, and gradually add leaf nodes to form an extended tree. S3, Execute the loop: The loop part mainly corresponds to step S2, which mainly completes the addition of leaf nodes in tree T. Through continuous looping, an extended tree with the starting point as the root node is gradually formed. Step S3 includes the following steps: S31, since the dexterity workspace of the robotic arm is difficult to describe precisely using mathematical methods, let the maximum extended arm length of the robotic arm be the radius r, and roughly define the sampling space as a sphere with radius r, and perform random sampling within it to obtain random points q. rand ; S32, obtain random point q rand Then, calculate the relationship between all points in tree T and the random point q. rand The Euclidean distance is used to select the point that is closest to the nearest point q. near , by q near Extend the new node backward; S33, Calculate the target tendency coefficient k: In this step, k represents whether the new node is more inclined towards the target direction or the direction of the random point, 0 < k < 1. The value of k is initialized to 0.1 in each loop. First, obtain the random point q... rand Then, first detect the random point q. rand When calculating the inverse kinematics of the robotic arm using 3D coordinates, does it meet the joint constraints? If it does, then check... feasibility =1, otherwise check feasibility =0; when check feasibility When = 1, k = k + m f m f For random point q rand The bias coefficient within the workspace; further, detecting random point q. rand Check if it falls within an obstacle. If not, then check... collision =1, otherwise check collision =0; when check collision When = 1, k = k + m c m c For random point q rand The bias coefficient outside the obstacle area; since the workspace of the robotic arm is basically continuous and smooth, and to avoid planning the robotic arm end effector entering areas outside the robotic arm workspace, it is set to 0.1≤m. c ≤m f m c +m f +0.1≤0.9; The target tendency coefficient k is further used to calculate the new extended node q in step S34. new ; S34, via k, q rand q near Calculate the new extended node q new The new extended node q is mainly calculated using formula (1). new : Where stepsize is the algorithm step size set in advance in step S12, and a is a variable step size coefficient, set to a normal number to avoid random points q. rand With the nearest point q near The appropriate value range for a is 1 < a ≤ 5, which can cause the algorithm to lock up if the a value is too close. Similar to the traditional RRT algorithm, q represents a random point. rand , endpoint q goal With the nearest point q near The Euclidean distance; Combining steps S33 and S34, with the target tendency coefficient k initially valued at 0.1, the random point q rand Randomly obtained, for q rand The confidence level is low, and the newly calculated extended node q is then... new It is more biased towards the target direction; through the two types of detection in step S33, the newly expanded node q can be identified. new The direction gradually moves towards q rand Getting closer allows new nodes to approach the target more gently, bypassing obstacles at the lowest possible cost and keeping the nodes within working range; S35, use the function q_Check to determine q new Does it meet two types of constraints: the newly expanded node q? new The detection method, the function q_Check, is mainly divided into two parts, where q_Check... feasibility Indicates the newly expanded node q new Within the dexterous workspace of the robotic arm, and within the joint space of the robotic arm, it can reach; q_Check collision This indicates the use of the newly expanded node q new The nearest point q obtained in step S32 near For paths to both endpoints to avoid collisions with obstacles on the map, check q_Check. feasibility q_Check collision When both of these checks are satisfied, the function q_Check returns Flag = 1, and the newly expanded node q is added. new Add it to tree T as the nearest vertex q near The child nodes; if Flag = 0, then the newly expanded node q new If the requirements are not met, repeat step S3. S36, Based on the result of step S35, determine whether to end the loop: Assuming the result of step S35 is Flag = 1, verify the new extended node q. new Is it within the threshold range of the target point, i.e. Is it true? If it is true? Then the loop ends and proceeds to step S4; if the condition is not met... Then continue with step S3; S4. Backtracking the parent node to obtain the path: After the loop ends, based on q from the last loop... new And the tree T backtracks to its parent node; S5. Obtain the final path: Through the backtracking in step S4, all the nodes found are arranged in order, which are the final calculated path points. All path points have distance optimization in Cartesian space, and through the detection in step S35, it is ensured that all path points are feasible for the robotic arm, without the need to convert to joint space for verification.

2. The improved RRT path planning algorithm based on a robotic arm according to claim 1, characterized in that: S1 includes the following steps: S11, the start and end point information of the path, represented by three-dimensional coordinates in a Cartesian coordinate system, with the start and end points represented by q. init q goal express; S12, stepsize of the path planning algorithm, specifies the step size for each new node expansion; S13, Algorithm Threshold This indicates that the distance between the newly expanded node and the endpoint is less than or equal to the threshold. When the time is reached, it is considered that the endpoint has been reached, and the loop steps will no longer be executed; S14, the number of iterations N, specifies the upper limit of the number of iterations. The algorithm stops after N iterations. S15. Additionally, the three-dimensional information of the obstacle is used as known information in subsequent steps.

3. The improved RRT path planning algorithm based on a robotic arm according to claim 1, characterized in that: Step S4 includes the following steps: S41, the condition for executing step S4 must be the q at the time of the last loop. new Meet the conditions This terminates the loop; S42, the new extended node q from the last loop. new Considered as the endpoint q goal The parent node; S43, from the newly expanded node q in the last cycle. new As a child node, it continuously backtracks to its parent node in the extended tree T until it reaches the root node, i.e., the starting point q. init .

Images