A robot arm path planning method based on improved rapid extended random tree

Abstract

The application discloses a mechanical arm path planning method based on an improved fast extended random tree and belongs to the technical field of industrial robot control. According to a set safety distance, a collision-free six-axis joint value sequence set from a starting point to an end point is quickly obtained. In initial path searching, a space preset tree strategy is used to guarantee the connection between an explored mechanical arm pose and initial and final mechanical arm poses and to make different poses as possible as to spread in space; in a path searching algorithm, a bidirectional search tree algorithm, a target bias, a greedy strategy and a variable step length strategy are used to accelerate algorithm convergence; a loop pruning algorithm is used in path optimization to greatly reduce a redundant path and to guarantee the local optimality of the path in an iteration process. The method improves the path planning efficiency of the mechanical arm in a three-dimensional space, solves the problems of long kinematics solving time of the mechanical arm in a multi-dimensional space and a redundant path and realizes a collision-free motion task in a joint space.

Description

Technical Field

[0001] This invention belongs to the field of industrial robot control technology, specifically relating to a robotic arm path planning method based on an improved fast expanding random tree. Background Technology

[0002] Path planning is one of the processes for achieving intelligence in industrial robots. A good path planning algorithm can save robot movement time and reduce joint wear. Unlike mobile robots, the path planning problem for industrial robots in space involves multiple kinematics solutions, resulting in high computational cost and low efficiency. Currently, commonly used methods for robot path planning include A* algorithm, artificial potential field method, and biomimetic algorithms. However, most algorithms are not suitable for path planning in robotic arms, mainly due to the increased spatial dimension, which leads to a surge in computational cost or getting stuck in local minima. The Rapid Extended Random Tree (RRT) algorithm improves this problem. As a sampling-based planning method, it effectively avoids modeling the spatial environment and possesses probabilistic completeness, thus gaining widespread use. However, this algorithm also has certain drawbacks, such as blindly generating random points leading to excessively long convergence time, redundant path generation, and unevenness. Therefore, it is necessary to improve the RRT algorithm to address its shortcomings in robotic arm path planning. Summary of the Invention

[0003] To address the issues of slow path planning speed and path redundancy in robotic arms, this invention discloses a robotic arm path planning method based on an improved fast expanding random tree. This method rapidly obtains a set of collision-free six-axis joint value sequences from the starting point to the endpoint based on a set safety distance. In the initial path search, a spatial pre-set tree strategy is used to ensure the connection between nodes and the starting point, while simultaneously spreading different poses of the robotic arm throughout the space as much as possible, laying the foundation for subsequent path searches. The path search algorithm utilizes the RRT-connect algorithm, along with target bias, greedy strategies, and variable step size strategies to accelerate convergence. For path optimization, a cyclic pruning algorithm significantly reduces redundant paths, ensuring local optima during iteration. The robotic arm path planning method based on the improved fast expanding random tree improves the efficiency of path planning in three-dimensional space, solving the problems of excessively long kinematic solution time and path redundancy after path planning in multi-dimensional space, successfully achieving collision-free motion tasks in joint space.

[0004] To achieve the above objectives, the technical solution of the present invention is as follows:

[0005] A robotic arm path planning method based on an improved fast expanding random tree includes the following steps:

[0006] Step 1: Read the location information of spatial obstacles, perform simplified spherical model envelope on the obstacles, set the end pose information of the robotic arm, establish the DH parameter model of the robotic arm, and at the same time perform inverse kinematics solution of the robot based on the end pose of the given robotic arm end effector to determine whether the joint values ​​in this state are within the range of motion of the robotic arm, and set the safe distance r from the obstacle.

[0007] Step 2: Set up a pre-set tree for the robotic arm in the joint space. Distribute the possible poses of the robotic arm throughout the space as much as possible, while maintaining a certain connection with the initial and final poses. Then, establish a bidirectional fast random search tree for the robotic arm in the joint space. Random trees are generated simultaneously for both the starting and ending poses. A greedy strategy is used to ensure that newly generated nodes in the second tree always expand towards the nearest node in the first tree. During the random node generation process in the bidirectional tree, a target bias strategy is used. Based on the number of bi-tree swaps t and the relationship between the target bias coefficient p and the random number Q, the endpoint direction of the first and second randomly expanded trees is determined. During the exploration of the bidirectional tree, a variable step size strategy is adopted: large step size for exploration and small step size for searching, quickly obtaining a collision-free path from the starting point to the endpoint.

[0008] Step 3: Using the iterative pruning algorithm, continuously optimize the searched nodes, and alternately perform pruning operations from the starting pose and the ending pose to obtain a locally optimal collision-free path.

[0009] The beneficial effects of this invention are as follows:

[0010] This application uses a robotic arm path planning method based on an improved fast expanding random tree. It proposes a pre-set tree method when planning the path of the robotic arm, and combines target bias, greedy strategy, and variable step size strategy on the basis of the RRT-connect algorithm. At the same time, it uses a cyclic pruning scheme to realize obstacle avoidance of a six-degree-of-freedom robotic arm, thereby improving the planning speed and efficiency of the algorithm. Attached Figure Description

[0011] Figure 1 This is a flowchart of a robotic arm path planning method based on an improved fast expanding random tree.

[0012] Figure 2 The image shows the preset tree effect of the improved RRT algorithm of the present invention, wherein (a) is the XY view, (b) is the XZ view, (c) is the YZ view, and (d) is the stereoscopic view.

[0013] Figure 3 This is a schematic diagram of the planning effect of the cyclic pruning algorithm of the present invention in three-dimensional space, wherein (a) is the XY view, (b) is the XZ view, (c) is the YZ view, and (d) is the stereo view.

[0014] Figure 4 This is a schematic diagram of the path planning effect of the robotic arm in an example of the present invention, wherein (a) is an XY view, (b) is an XZ view, (c) is a YZ view, and (d) is a stereoscopic view. Detailed Implementation

[0015] The present invention will be further illustrated below with reference to the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are for illustrative purposes only and are not intended to limit the scope of the invention.

[0016] like Figure 1 As shown in the figure, the robotic arm path planning method based on an improved fast expanding random tree described in this embodiment includes the following steps:

[0017] Step 1: Obtain the homogeneous transformation matrix based on the fixed coordinate system on the link and establish the motion model of the robotic arm.

[0018] An improved DH method is used to establish a fixed coordinate system on each link of the robotic arm. This fixed coordinate system describes the displacement relationships between links and between links and joints. A homogeneous transformation matrix of the end effector coordinate system relative to the base coordinate system is obtained, and a kinematic model of the robotic arm is established based on this homogeneous transformation matrix. This homogeneous transformation matrix is ​​a function of each joint variable and is used to obtain the forward kinematic solution from the joint space of the robotic arm to Cartesian space. The forward kinematic solution is the mapping from the joint space of the robotic arm to Cartesian space, thereby determining the end effector pose of the robotic arm under given posture and joint angles, thus achieving path planning. The DH parameter table of the robotic arm used as the research object is shown below:

[0019] Joint i <![CDATA[a i-1 ]]> <![CDATA[α i-1 / mm]]> <![CDATA[d i / mm]]> <![CDATA[θ i ]]> Joint rotation limit / ° 1 0 0 0 0 -165～165 2 180 -90 0 -90 -180～65 3 600 0 0 0 -200～70 4 115 -90 664 0 -170～170 5 0 90 0 0 -135～135 6 0 -90 0 0 -360～360

[0020] In the improved DH description method, it is assumed that the axis of each joint is the z-axis, and the common normal direction of the two axes of the next joint is the x-axis. Then, the y-axis is determined according to the right-hand rule. In the table: a i-1 Is along X i-1 Axis, from Z i-1 To Z i The distance moved; d i Along Z i The axis is from X i-1 To X i The distance traveled; α i-1 In the reference coordinate system, around X i-1 Rotation, from Z i-1 To Z i The rotation angle; θ i It is around Z i Axis, from X i-1 To X iThe rotation angle. The homogeneous transformation matrix of each link of the robotic arm is:

[0021]

[0022] in The end effector's body coordinate system O represents the coordinate system of the end effector. s -X s Y s Z s The rotation transformation matrix of the attitude relative to the base coordinate system O-xyz. Using the above equation, the inverse kinematics can be solved, and the joint angles θ can be solved from the position and attitude of the end effector. i =[θ1, θ2, θ3, θ4, θ5, θ6], θ i It is a 1×6 row vector.

[0023] Step 2: Read environmental information, including the location and size of the enveloping obstacles, the size and shape of the robotic arm, and the radius of the enveloping robotic arm. Assume the current starting pose is represented as P. s =[X s Y s Z s α s β s γ s The target pose is represented as P. g =[X g Y g Z g α g β g γ g The pose vector is a 1×6 row vector, with the first three rows representing position information and the last three rows representing attitude. Simultaneously, inverse kinematics is performed to obtain the starting point θ. s θ g At the same time, it is determined whether the target point joint value is reachable.

[0024] Step 3: Generate a pre-defined tree in 3D space and record the starting point six-axis joint value θ. s And store it in RRTree1, and record the endpoint six-axis joint value θ. g The data is stored in RRTree2, with a preset tree pointer k=1, indicating that it currently points to RRTree1. When k=2, it indicates that the current state points to RRTree2. The step length Step1=0.5, which is the total Euler distance of the six-axis joint value movement. The starting six-axis joint value θ is calculated. s and endpoint six-axis joint value θ g The direction vector θ between m Obtain the unit vector θ using the sign function. m The sign of the sign is determined, and a different fluctuation value is randomly added to each element of the direction vector to form a new direction vector, where randi It is an array with one row and six columns, and each element is a random value between 0 and 1. The new direction vector formed by it is shown in equation (3). Two unit direction vectors θ are obtained by this method. m1 θ m2 And denote θ m3 =θ m At this point, the values ​​of i are 1, 2, and 3, which represent the three sets of direction vectors θ pointing from the starting point to the ending point. m1 θ m2 θ m3 The robot continuously steps from the initial pose, performs forward kinematics on newly generated points, obtains the Cartesian space-mapped pose, and uses a bounding sphere collision detection algorithm to determine whether a collision occurs. If no collision occurs, the robot stores the point and the row number of the nearest point in RRTree1 into RRTree1. The robot continues to step until the joint value exceeds the soft limit or a collision space obstacle is encountered. Otherwise, the robot proceeds to the next direction of the preset tree arrangement, i.e., i = i + 1. When i = 4, it indicates that the preset tree arrangement from the starting point to the ending point is completed. The preset tree is set to k = k + 1, and the robot enters the second set of preset number search and resets i to 1. Similarly, the vector θ of different directions from the ending point to the starting point is obtained according to equation (3). n1 θ n2 And denote θ n3 =θ n Continuously advance, storing the points that meet the requirements into RRTree2. When the pre-set tree arrangement in the three directions is completed, that is, when i=4, it means that the pre-set tree arrangement from the end point to the starting point is completed. Let the pre-set tree point k=k+1. When k=3, the pre-set tree arrangement ends, and then the path planning of the improved fast expansion random tree begins.

[0025]

[0026] wave i =sign(θ) m )*rand i (3)

[0027]

[0028] In the formula rand i Represents a random value within the range [0,1].

[0029] Step 4: Construct a bidirectional fast random search tree. Let t be the number of tree swaps at this point, and p be the target bias parameter. Implement a target bias sampling strategy: randomly generate a number Q between [0,1]. When this value is less than p, randomly generate a set of X. rand Otherwise, based on the number of exchanges t, a fixed point is generated. When t is odd, the fixed point is set to the endpoint six-axis joint value θ. gConversely, set the starting six-axis joint value θ. s .

[0030]

[0031] Search distance X in RRRTree1 rand The set of numbers with the shortest Euler distance is denoted as X. near1 Its row number on RTree1 is denoted as sequence number1. To X rand The direction step is Step1 length, and this point is denoted as X. new1 .

[0032]

[0033] The obtained Q new1 Perform the forward kinematics solution for the robot. Based on the formula...

[0034]

[0035] The solution yields a 4×4 transformation matrix. The first three rows of its fourth column represent the positions of the ends of each axis, labeled P1, P2, P3, P4, P5, and P6 respectively. The lever P... 12 Leverage P 23 Leverage P 34 Leverage P 56 Perform collision detection separately. If a collision occurs, regenerate a new random point X. rand If there is no collision, then [X] new1 The number1] is stored in row RRTree1.

[0036] Search distance X in RRRTree2 new1 The point with the shortest total Eulerian distance is denoted as X. near2 Its row number on RRTree2 is denoted as number2. near2 To X new1 The direction step is ε, and the point is denoted as X. new2 X new2 Perform the forward kinematics solution for the robot to obtain Transform the matrix to obtain the positions of the ends of each axis, denoted as P7, P8, P9, P1, P2, P3, P4, P5, P6, P7, P8, P9, P1, P1, P1, P1, P1, P2 ...1, P2, P1, P1, 10 P 11 P 12 Perform collision detection on each of the four levers. If a collision occurs, swap all elements in RRTree1 and RRTree2, increment the swap count, and repeat the above steps to generate a new X. rand Otherwise calculate X new2 With X new1The distance between them is checked, and it is determined whether the distance is less than a set value β. If it is less than β, it is considered that a path has been found, and [X] is set to [X]. new2 Store [number2] in row RRTree2, and use the marked sequence number to backtrack the path to end the search; otherwise, use a greedy algorithm to store [X] in row RRTree2. new2 The number2] is stored in the RRTree2 row, and the forward direction is maintained, continuously stepping until it collides or exceeds the joint soft limit.

[0037] Step 5: After obtaining the initial path, perform a cyclic pruning operation to trim redundant paths.

[0038] S3.1 Let i = 1, j = n, where n is the number of rows in the path set; based on obtaining the initial path, add the path joint value path i path joint value j Interpolation is performed between the intervals, with a step size of T each time. The expression for the interpolation function is as follows:

[0039]

[0040] S3.2 Perform forward kinematics on all interpolation points within the two points to obtain the position under the Cartesian space mapping. Determine whether all interpolation points within the two points will cause collisions with spatial obstacles or mechanical bodies. If a collision occurs, let j = j-1; otherwise, store it in the final path set, let i = j, j = n, and perform the next search.

[0041] S3.3 Discretize the resulting final path set, reverse the order, and then perform pruning again;

[0042] S3.4 Repeat steps S3.2 to S3.3 until the path sequence lengths of the forward pruning discrete length and the reverse pruning discrete length are the same, indicating that the final path has been found.

Claims

1. A robotic arm path planning method based on an improved fast expanding random tree, characterized in that, Includes the following steps: Step 1: Read the location information of spatial obstacles, construct a simplified spherical model envelope for the obstacles, set the end-effector pose information of the robotic arm, establish the DH parameter model of the robotic arm, and simultaneously perform inverse kinematics of the robot based on the given end-effector pose of the robotic arm, determine whether the joint values ​​in this state are within the range of motion of the robotic arm, and set a safe distance from the obstacles. ; Step 2: Set up a pre-set tree for the robotic arm in the joint space. Under the premise of maintaining a certain relationship with the initial and final poses, spread the possible poses of the robotic arm as widely as possible throughout the space. Then, build a bidirectional fast random search tree for the robotic arm in the joint space. Generate random trees at the starting pose and the ending pose at the same time, and use a greedy strategy to ensure that the newly generated nodes of the second tree always expand towards the nearest node in the first tree. In the process of randomly generating nodes in a bidirectional tree, a target bias strategy is used, based on the number of bidirectional tree swaps. and target bias coefficient With random numbers The relationship between the two-way tree determines the endpoint direction of the first and second random expansion trees. During the exploration of the bidirectional tree, a variable step size strategy is adopted, with large step size for exploration and small step size for searching, to quickly obtain a collision-free path from the starting point to the endpoint. In step two, during the exploration of the bidirectional tree, a variable step size strategy is adopted: large step size for exploration and small step size for searching, to quickly obtain a collision-free path from the starting point to the ending point. This includes the following steps: S2.1 Initialize the random tree, and store the set of no-collision points generated by the pre-set tree accordingly. , middle; S2.2 generates a random numbers within ,according to and target bias parameters The relationship is used to obtain a set of joint sample values. Select Mid-range The latest set of data ,make Towards Step unit Gain new points ; (7) S2.3 performs the forward kinematics solution for the robot and obtains... The corresponding Cartesian position is determined, and the link collision detection system determines whether the six-axis robot experiences a spatial collision. The robot arm's own collision detection system determines whether a self-collision occurs. If a collision occurs, the process returns to step 2.2, regenerating points until no collisions occur. Then... deposit ; S2.4 Selection Mid-range The most recent set of data is denoted as ,make Towards Step unit Gain new points As shown in the following formula: (8) Perform the forward kinematics solution for the robot and determine... Check if the corresponding Cartesian position causes the aforementioned collision. If a collision occurs, swap the two trees and repeat step S2.2; otherwise, determine... and Is the distance between them less than the convergence condition value? If the value is less than the value, it indicates that a path has been found; otherwise, a greedy strategy or a variable step size strategy is used to accelerate the progress. Step 3: Using the iterative pruning algorithm, continuously optimize the searched nodes, and alternately perform pruning operations from the starting pose and the ending pose to obtain a locally optimal collision-free path.

2. The robotic arm path planning method based on an improved fast expanding random tree according to claim 1, characterized in that, Step one specifically includes the following steps: S1.1 Establish the robotic arm model based on the DH parameter table, and simultaneously obtain the rotation transformation matrix corresponding to the end effector of each joint: Let the axis of each joint be the z-axis, and the common normal direction of the two axes of the next joint be the x-axis, then determine the y-axis according to the right-hand rule. It is along Axis, from arrive The distance traveled; Along The axis is from arrive The distance traveled; In the reference coordinate system, around Rotate, from arrive The rotation angle; It is around Axis, from arrive The rotation angle, the transformation matrix expression for any two adjacent joints is shown in Equation (1), and the pose rotation transformation matrix of different joints relative to the robot arm base coordinate system is shown in Equation (2): (1) (2) in, This is the joint number value. , The first The z-axis and x-axis of the first joint; The position of the joint end is indicated in In the elements of the first to third rows of the fourth column of the matrix; S1.2 The envelope of each obstacle is set as a sphere, and the envelope of each lever is set as a cylinder. S1.3 Solve the six-axis joint angles inversely according to the rotation transformation matrix of formulas (1) and (2), and determine whether the joint values ​​in the corresponding state are reachable. The reachability refers to whether the six-axis joint value of the target point exceeds the soft limit of the robot body, whether the six-axis joint value of the target point collides with the spatial obstacle, and whether the link self-collides.

3. The robotic arm path planning method based on an improved fast expanding random tree according to claim 2, characterized in that, In step two, a pre-set tree for the robotic arm is set up in the joint space. The possible poses of the robotic arm are distributed throughout the space as widely as possible while maintaining a certain relationship with the initial and final poses. Then, a bidirectional fast random search tree for the robotic arm is built in the joint space. Random trees are generated simultaneously for the starting and ending poses, and a greedy strategy is used to ensure that newly generated nodes in the second tree always expand towards the nearest node in the first tree. Specifically: the starting six-axis joint values ​​are recorded. And save Record the six-axis joint values ​​at the endpoint. And save Preset tree pointer position =1, indicating that it currently points to... ,when When =2, it indicates that the current state points to Step length =0.5, representing the total Euler distance of the six-axis joint values. The starting six-axis joint values ​​are calculated. and endpoint six-axis joint values Direction vectors between ,according to Function to get unit vector The sign of the sign is determined, and a sign is randomly appended to each element of the direction vector. Fluctuation values ​​in the same direction As the new direction vector, where It is an array with one row and six columns, and each element is a random value between 0 and 1. The new direction vector formed by them is shown in equation (4): (3) (4) (5) Indicates according to The offset random vector obtained from the sign of the sign. This method yields two direction vectors, representing the random unit vector to be explored. And record ,at this time The values ​​1, 2, and 3 represent three sets of direction vectors pointing from the starting point to the ending point. , The robot continuously steps from the initial pose, performs forward kinematics on newly generated nodes to obtain the Cartesian-space mapped pose, and uses collision detection algorithms for bounding cylinders and bounding spheres, as well as self-collision detection algorithms for two bounding cylinders, to determine whether a collision occurs at a point. If no collision occurs, the point is moved to the next node. Store the row number of the point closest to this point. Continue moving forward Collision detection is continuously performed until the joint value exceeds the soft limit, the robotic arm self-collides, or collides with an obstacle. Otherwise, the pre-set tree arrangement in the next direction is performed. = +1, when When =4, it indicates that the pre-set tree layout from the starting point to the ending point is complete. Set the pre-set tree pointer to position 4. = +1, enter the second group of preset number search, and reset. If the value is 1, then similarly, find the unit direction vector from the endpoint to the starting point. According to equation (5), obtain the other two sets of unit direction vectors. And record Repeat the above steps, performing collision detection while continuously advancing, and storing the points that meet the requirements. In the middle, when the pre-set tree arrangement in three directions is completed, that is, when When =4, it indicates that the pre-set tree layout from the endpoint to the starting point is complete, and the pre-set tree is set to position. = +1, when When the value is 3, the pre-set tree layout ends, and then the path planning for improving the fast expansion random tree begins.

4. The robotic arm path planning method based on an improved fast expanding random tree according to claim 3, characterized in that, In step two, during the random node generation process of the bidirectional tree, a target bias strategy is used, based on the number of bidirectional tree swaps. and target bias coefficient With random numbers The relationship determines the endpoint direction of the first random expansion tree and the endpoint direction of the second random expansion tree. Specifically, let the target bias parameter be... ,Pick Randomly generate one number within ,like If the sampling node is randomly generated within the joint's soft constraint, then a fixed target is generated as the sampling node; otherwise, the generation of the fixed target depends on the number of failed swaps in the dual-tree system. Record the initial number of bitree swaps. It is 1, and each time it is swapped Add one, when When the number is odd, the fixed target points to the endpoint. ,when When the number is even, the fixed target points to the starting point. ; (6) At the same time, always maintain a bidirectional tree Using a greedy strategy and a variable step size strategy, continuously... Newly generated nodes Towards The step length of the point closest to its position in the middle If no collision occurs, then the new step length will be... Rapidly increase step length, collision will cause Then swap the twin trees and start the search again.

5. The robotic arm path planning method based on an improved fast expanding random tree according to claim 4, characterized in that, Step three includes the following steps: S3.1 Order , , The number of rows in the path set; based on obtaining the initial path, the path joint value... Path joint value Interpolation is performed between them, with each step length being [value missing]. The expression for the interpolation function is as follows: (9) S3.2 Perform forward kinematics on all interpolated points within the two points to obtain their positions in Cartesian space. Determine whether any interpolated points within the two points will collide with spatial obstacles or the mechanical body. If a collision occurs, then... Otherwise, store it in the final path set, let , Proceed to the next search; S3.3 Discretize the resulting final path set, reverse the order, and then perform pruning again; S3.4 Repeat steps S3.2 to S3.3 until the path sequence lengths of the forward pruning discrete length and the reverse pruning discrete length are the same, indicating that the final path has been found.

Images