A six-axis robot obstacle avoidance path planning method based on improved RRTstar

Abstract

The application discloses a kind of six-axis robot obstacle avoidance path planning methods based on improved RRTstar, belong to mechanical arm path planning technical field.The present application is aimed at the shortcomings that RRTstar algorithm searches large space, search time is long, and the path generated is too much and not smooth for the high complexity of calculation, and the excessive angle of the mechanical arm work is not conducive to these disadvantages, first introduce target bias strategy, and expand from starting point and target point simultaneously in combination with double-tree expansion strategy, add shortening path strategy and remove redundant node strategy, finally, the search path is optimized by B-spline fitting, by this method, search time can be effectively reduced, search complexity is reduced, path is shortened while realizing path smoothing, so that the end of mechanical arm successfully avoids obstacles in the working process, realizes efficient and safe path planning.

Description

Technical Field

[0001] This invention belongs to the field of robotic arm path planning technology, specifically relating to a six-axis robotic arm obstacle avoidance path planning method based on an improved RRTstar. Background Technology

[0002] Obstacle avoidance path planning is a crucial area of ​​robot control, involving how to enable robots to autonomously avoid various obstacles in complex environments. Common robot obstacle avoidance path planning algorithms include graph search-based algorithms and sampling-based algorithms. Graph search-based algorithms, such as Dijkstra's algorithm and Astar's algorithm, create an environment map, abstracting the robot and obstacles as nodes in the graph, and then use graph search algorithms to find the shortest path. This algorithm is computationally efficient and can find the shortest path, but in high-dimensional spaces and complex constraint environments, the search space becomes very large, increasing computational complexity and leading to longer computation time.

[0003] Sampling-based algorithms, such as Rapidly-exploring Random Tree (RRT) and Probabilistic Roadmap (PRM), construct a sampled map of the robot's motion space by randomly sampling robot pose and environmental data, and then use graph search algorithms to find feasible paths. These algorithms possess rapid exploration capabilities and stochastic properties, enabling them to perform well in complex environments. Therefore, in robotic arm path planning, employing sampling-based algorithms can help the robotic arm find feasible and efficient paths and avoid obstacles.

[0004] The Fast Random Expanded Tree (FAST) algorithm is a commonly used path planning algorithm for robotic arms. Its random sampling strategy allows for searching a wide range of feasible solutions, increasing the likelihood of finding the global optimum. However, uneven distribution of sampling points can lead to an unevenly distributed path, resulting in a less smooth path and potential vibrations during robotic arm movement. Summary of the Invention

[0005] To address the shortcomings of existing technologies, a robotic arm obstacle avoidance path planning method is provided, which features shorter search time, shorter paths, and smoother paths.

[0006] To solve the above-mentioned technical problems, the technical solution of the present invention is: a six-axis robotic arm obstacle avoidance path planning method based on the improved RRTstar, characterized in that the method includes the following steps:

[0007] Step 1: Obtain the obstacle distribution in the robotic arm's workspace, establish the robotic arm collision model and kinematic model, and initialize two random trees: Tree1 and Tree2, including the starting point X_start and the ending point X_goal, the threshold Thr, the maximum number of iterations MaxIterations, the step size stepsize, the search range searchSize, and the map environment.

[0008] Step 2: Random trees Tree1 and Tree2 are sampled alternately using a target bias strategy to generate random points x_rand;

[0009] Step 3: Search for the nearest neighbor node x_near, and then expand x_near along the direction of the line connecting x_rand with a step size stepsize to generate a new node newPoint. After expansion, perform collision detection. If no collision occurs, add the new node to the random trees Tree1 and Tree2. If a collision occurs, resample.

[0010] Step 4: Using the newly expanded node newPoint as the center, search the existing tree for the set of nearest neighbors q_near that are less than the radius r away from newPoint. Take the node x_min with the smallest distance as the new parent node of newPoint. Then perform a pruning rewire operation, and the new node newPoint is reconnected to the node with the shortest path in q_near except for x_min.

[0011] Step 5: Calculate the distance between the new node newPoint and existing nodes in the random trees Tree1 and Tree2, and compare it with the distance threshold d. If the distance between the new node and an existing node is greater than or equal to the threshold d, the new node is retained; otherwise, nodes in the random trees that overlap with newPoint are removed. The distance function is as follows:

[0012]

[0013] Where a and b are node coordinates, a(1), a(2), a(3) represent the xyz coordinates of node a in three-dimensional space, and b(1), b(2), b(3) represent the xyz coordinates of node b in three-dimensional space.

[0014] Step 6: After the random trees Tree1 and Tree2 are successfully connected, continue to perform alternating sampling. If the maximum number of iterations MaxIterations is reached, output the path, shorten the path according to the path shortening strategy, and finally smooth the path with a cubic B-spline curve.

[0015] Furthermore, in step 1, the starting point X_start, the ending point X_goal, the search range searchSize, and the map environment required for the operation of the robotic arm are determined based on the actual working scenario of the robotic arm and the relevant parameters of the robotic arm. The map environment includes the distribution of obstacles.

[0016] Furthermore, the target bias strategy in step 2 introduces a target bias probability p1 to consider the influence of the target point, ensuring that new random points generated have a certain probability of pointing towards the target point. The sampling method for generating new random points x_rand1 and x_rand2 using random trees Tree1 and Tree2 is as follows:

[0017]

[0018]

[0019] Where P is a randomly generated random number within the range (0,1). For random tree Tree1, if the random number P is greater than the target bias probability p1, then x_rand1 will generate a new random point in the direction of the target point X_goal; if the random number P is less than or equal to the target bias probability p1, then x_rand1 will be a randomly sampled point Sample. For random tree Tree2, if P is greater than the target bias probability p1, then x_rand2 will generate a new random point in the direction of the starting point X_start; if the random number P is less than or equal to the target bias probability p1, then x_rand2 will be a randomly sampled point Sample.

[0020] Furthermore, in step 4, the rewire operation finds the set q_near of all nodes that are less than or equal to r away from the new node newPoint.

[0021] For each node xi in q_near, check if there exists a path from newPoint to xi such that the cost of the path is less than the current cost of xi. If such a path exists, set the parent node of xi to newPoint, and update the cost of xi and the costs of all its child nodes.

[0022] Furthermore, the condition for the successful connection of the random trees Tree1 and Tree2 in step 6 is that the newly added node newPoint (a node extended from Tree1 and Tree2) is less than the threshold Thr of the node in the other tree and passes the collision detection.

[0023] Furthermore, the path shortening strategy in step 6 shortens the path when the maximum number of iterations has been reached. The basic idea is to check three consecutive points in the path to determine if there is a collision and which points need to be deleted. Then, the path formed by the original three points is replaced with a straight line segment between the first and last points, and the first and last points are added back to the path.

[0024] Furthermore, in step 6, the cubic B-spline curve is used to smooth the path obtained by the path shortening strategy using a fitting method, and the formulas involved are as follows:

[0025] B 0,3 (t)=(1-t) 3 / 6

[0026] B 1,3 (t)=1 / 6(3t 3 -6t 2 +4)

[0027] B 2,3 (t)=1 / 6(-3t 3 +3t 2 +3t+1)

[0028] B 3,3 (t)=t 3 / 6

[0029] Where t is a node vector, representing the position on the curve, with a value range of [0,1], and B is a real number within the interval [0,1]. 0,3 (t), B 1,3 (t), B 2,3 (t) and B 3,3 (t) are all B-spline basis functions used to construct cubic B-spline curves.

[0030] The equation of a cubic B-spline curve is a linear combination of the four basis functions mentioned above, and the equation is as follows:

[0031] P(t) = P0B 0,3 (t)+P1B 1,3 (t)+P2B 2,3 (t)+P3B 3,3 (t)

[0032] Where P(t) is the overall curve equation, and P0, P1, P2, P3 are the coordinates of the four control points.

[0033] The smoothed final path points are transmitted to the robotic arm, which then performs its tasks based on these path points.

[0034] The beneficial effects of this invention are: the obstacle avoidance path planning method for a six-axis robotic arm based on the improved RRTstar provided by this invention can effectively reduce search time, reduce search complexity, shorten the path and achieve path smoothing, so that the end effector of the robotic arm can smoothly avoid obstacles during operation and achieve efficient and safe path planning. Attached Figure Description

[0035] Figure 1 This is a flowchart of the improved RRTstar algorithm of the present invention;

[0036] Figure 2 This is a schematic diagram of the path shortening strategy of the present invention;

[0037] Figure 3 This is a diagram of the improved RRTstar algorithm path search process in three-dimensional space according to an embodiment of the present invention;

[0038] Figure 4 This is the unoptimized initial obstacle avoidance path diagram according to an embodiment of the present invention;

[0039] Figure 5 This is an obstacle avoidance path diagram after B-spline smoothing according to an embodiment of the present invention;

[0040] Figure 6 This is a comparison diagram of the initial obstacle avoidance path and the smoothed obstacle avoidance path in an embodiment of the present invention. Detailed Implementation

[0041] The present invention will be further described below with reference to the accompanying drawings and specific embodiments:

[0042] Example 1: As Figures 1 to 6 As shown, the present invention provides a six-axis robotic arm obstacle avoidance path planning method based on an improved RRTstar, comprising the following steps:

[0043] Step 1: Obtain the obstacle distribution in the robotic arm's workspace, establish the robotic arm collision model and kinematic model, and initialize two random trees: Tree1 and Tree2, including the starting point X_start and the ending point X_goal, the threshold Thr, the maximum number of iterations MaxIterations, the step size stepsize, the search range searchSize, and the map environment.

[0044] In this step, the starting point X_start, the ending point X_goal, the search range searchSize, and the map environment are determined based on the actual working scenario of the robotic arm and the relevant parameters of the robotic arm. The map environment includes the distribution of obstacles.

[0045] In this embodiment, the method of the present invention is used on a HW700 six-axis robotic arm with an effective load of 3 kg.

[0046] First, the robotic arm is initialized. Then, the end effector of the robotic arm is moved to the endpoint. The start and end points of the robotic arm's working path and the distribution of obstacles in space are obtained through sensors. At the same time, a 3D spatial map model is constructed, marking the start point X_start and the end point X_goal. Then, two random trees are initialized, and the start point X_start, end point X_goal, threshold Thr, maximum number of iterations MaxIterations, step size stepsize, search range searchSize, and map environment are passed to the improved RRTstar algorithm.

[0047] Step 2: Random trees Tree1 and Tree2 are sampled alternately using a target bias strategy to generate random points x_rand.

[0048] The target bias strategy in this step considers the influence of the target point by introducing a target bias probability p1, so that when generating new random points, there is a certain probability that they will be oriented towards the target point. The sampling method for generating new random points x_rand1 and x_rand2 using random trees Tree1 and Tree2 is as follows:

[0049]

[0050]

[0051] Where P is a randomly generated random number within the range (0,1). For random tree Tree1, if the random number P is greater than the target bias probability p1, then x_rand1 will generate a new random point in the direction of the target point X_goal; if the random number P is less than or equal to the target bias probability p1, then x_rand1 will be a randomly sampled point Sample. For random tree Tree2, if P is greater than the target bias probability p1, then x_rand2 will generate a new random point in the direction of the starting point X_start; if the random number P is less than or equal to the target bias probability p1, then x_rand2 will be a randomly sampled point Sample.

[0052] In this step, before generating random points, it is necessary to determine whether the maximum number of iterations MaxIterations has been exceeded. If it has, the sampling ends and the result is output. If it has not been exceeded, the iteration count is incremented by one, and sampling continues to generate new random points x_rand.

[0053] Step 3: Search for the nearest neighbor node x_near, and then expand x_near along the direction of the line connecting it to x_rand with a step size stepsize to generate a new node newPoint. After expansion, perform collision detection. If no collision occurs, add the new node to the random tree. If a collision occurs, resample.

[0054] Step 4: Using the newly expanded node newPoint as the center, search the existing tree for the set of nearest neighbors q_near that are less than the radius r away from newPoint. Take the node x_min with the smallest distance as the new parent node of newPoint. Then perform a pruning rewire operation, and the new node newPoint is reconnected to the node with the shortest path in q_near except for x_min.

[0055] The rewire operation in this step involves finding the set q_near of all nodes that are less than or equal to r away from the new node newPoint.

[0056] For each node xi in q_near, check if there exists a path from newPoint to xi such that the cost of the path is less than the current cost of xi. If such a path exists, set the parent node of xi to newPoint, and update the cost of xi and the costs of all its child nodes.

[0057] Step 5: Calculate the distance between the new node and existing nodes in the random tree, and compare it with the distance threshold d. If the distance between the new node and the nearest node is less than the threshold d, remove the nodes in the random tree that overlap with newPoint. The distance function is as follows:

[0058]

[0059] Where x and y are the coordinates of the two nodes.

[0060] Step 6: If the maximum number of iterations (MaxIterations) is reached, output the path, shorten the path according to the path shortening strategy, and finally smooth the path with a cubic B-spline curve.

[0061] In this step, the conditions for a successful connection between random trees Tree1 and Tree2 are: the distance between the newly added node newPoint (a node extended from Tree1 and Tree2) and a node in the other tree is less than the threshold Thr, and the collision detection is passed.

[0062] The path shortening strategy in this step shortens the path when the maximum number of iterations has been reached. The basic idea is to check three consecutive points in the path to determine if there are any collisions and which points need to be deleted. Then, the original path consisting of these three points is replaced with a straight line segment between the first and last points, and the first and last points are added back to the path.

[0063] In this step, the cubic B-spline curve is used to smooth the path obtained by the path shortening strategy, employing a fitting method. The formulas involved are as follows:

[0064] B 0,3 (t)=(1-t) 3 / 6

[0065] B 1,3 (t)=1 / 6(3t 3 -6t 2 +4)

[0066] B 2,3 (t)=1 / 6(-3t 3 +3t 2 +3t+1)

[0067] B 3,3 (t)=t 3 / 6

[0068] Where t is a node vector, representing the position on the curve, with a value range of [0,1], and B is a real number within the interval [0,1]. 0,3 (t), B 1,3 (t), B 2,3 (t) and B 3,3 (t) are all B-spline basis functions used to construct cubic B-spline curves.

[0069] The equation of a cubic B-spline curve is a linear combination of the four basis functions mentioned above, and the equation is as follows:

[0070] P(t) = P0B 0,3 (t)+P1B 1,3 (t)+P2B 2,3 (t)+P3B 3,3 (t)

[0071] Where P(t) is the overall curve equation, and P0, P1, P2, P3 are the coordinates of the four control points.

[0072] The smoothed final path points are transmitted to the robotic arm, which then performs its tasks based on these path points.

[0073] This simulation was conducted in Matlab 2022b on a 3D spatial map with five spherical obstacles. The starting point X_start coordinates were (15,15,15), and the ending point X_goal coordinates were (160,160,160). The coordinates of the center of each spherical obstacle were (100,100,105), (50,54,50), (100,47,64), (143,100,108), and (55,137,53). The radius of each obstacle was 14.5. The search size was [250 250 250], the step size was 13, and the threshold Thr was 10.

[0074] Figure 3 The diagram shows the path search process of the improved RRTstar algorithm in three-dimensional space. The diagram illustrates the search results after incorporating a target bias strategy, alternating expansion of two trees, a path shortening strategy, pruning and rewire operations, and removal of redundant nodes.

[0075] Figure 4 and Figure 5 The comparison shows that the curvature and length of the path are significantly reduced, the smoothness of the path is improved, there are no sharp corners in the path, the planned path is more concise, and the smoothed path is also a collision-free path, which will not cause the robotic arm to collide with obstacles in the workspace.

[0076] Figure 6 In the figure, the solid line represents the fitted and optimized path after three B-spline smoothing of the original obstacle avoidance path planned by the improved RRTstar algorithm, and the line with star represents the initial path. As can be seen from the figure, the path planned by the algorithm in this invention is shorter and smoother.

[0077] To those skilled in the art, the embodiments disclosed herein are intended to help readers understand the principles and implementation of the invention, rather than to limit the scope of protection of the invention. Therefore, those skilled in the art can make various modifications and combinations based on the technical teachings disclosed herein, as long as these modifications and combinations do not depart from the essence of the invention and remain within the scope of protection of the invention.

Claims

1. A method for obstacle avoidance path planning for a six-axis robotic arm based on an improved RRTstar, characterized in that, Includes the following steps: Step 1: Obtain the obstacle distribution in the robotic arm's workspace, establish the robotic arm collision model and kinematic model, and initialize two random trees: Tree1 and Tree2, including the starting point X_start and the ending point X_goal, the threshold Thr, the maximum number of iterations MaxIterations, the step size stepsize, the search range searchSize, and the map environment. Step 2: Random trees Tree1 and Tree2 are sampled alternately using a target bias strategy to generate random points x_rand; Step 3: Search for the nearest neighbor node x_near, and then expand x_near along the direction of the line connecting x_rand with a step size stepsize to generate a new node newPoint. After expansion, perform collision detection. If no collision occurs, add the new node to the random trees Tree1 and Tree2. If a collision occurs, resample. Step 4: Using the newly expanded node newPoint as the center, search the set of nearest neighbor nodes q_near in the existing tree that are less than the radius r away from newPoint. Take the node x_min with the smallest distance as the new parent node of newPoint. Then perform the pruning rewire operation. The new node newPoint is reconnected to the node with the shortest path in q_near except for x_min. Step 5: Calculate the distance between the new node newPoint and existing nodes in the random trees Tree1 and Tree2, and compare it with the distance threshold d. If the distance between the new node and an existing node is greater than or equal to the threshold d, the new node is retained; otherwise, nodes in the random trees that overlap with newPoint are removed. The distance function is as follows: ； Where a and b are node coordinates, a(1), a(2), a(3) represent the xyz coordinates of node a in three-dimensional space, and b(1), b(2), b(3) represent the xyz coordinates of node b in three-dimensional space. Step 6: After the random trees Tree1 and Tree2 are successfully connected, continue to perform alternating sampling. If the maximum number of iterations MaxIterations is reached, output the path, shorten the path according to the path shortening strategy, and finally smooth the path with a cubic B-spline curve. The target bias strategy in step 2 introduces a target bias probability p1 to account for the influence of the target point, ensuring that new random points have a certain probability of pointing towards the target point when generated. Random trees Tree1 and Tree2 then generate new random points. and The sampling method is as follows: ； ； Where P is a randomly generated random number within the range (0,1). For random tree Tree1, if the random number P is greater than the target bias probability p1, then x_rand1 will generate a new random point in the direction of the target point X_goal; if the random number P is less than or equal to the target bias probability p1, then x_rand1 will be a randomly sampled point Sample. For random tree Tree2, if P is greater than the target bias probability p1, then x_rand2 will generate a new random point in the direction of the starting point X_start; if the random number P is less than or equal to the target bias probability p1, then x_rand2 will be a randomly sampled point Sample.

2. The obstacle avoidance path planning method for a six-axis robotic arm based on an improved RRTstar as described in claim 1, characterized in that: In step 1, the starting point X_start, ending point X_goal, search range searchSize, and map environment required for the operation of the robotic arm are determined based on the actual working scenario of the robotic arm and the relevant parameters of the robotic arm. The map environment includes the distribution of obstacles.

3. The obstacle avoidance path planning method for a six-axis robotic arm based on an improved RRTstar as described in claim 1, characterized in that: In step 4, the rewire operation involves finding the set q_near of all nodes that are less than or equal to r away from the new node newPoint. For each node xi in q_near, check if there exists a path from newPoint to xi such that the cost of the path is less than the current cost of xi. If such a path exists, set the parent node of xi to newPoint, and update the cost of xi and the cost of all child nodes of xi.

4. The obstacle avoidance path planning method for a six-axis robotic arm based on an improved RRTstar as described in claim 1, characterized in that: The conditions for successfully connecting the random trees Tree1 and Tree2 in step 6 are: the distance between the newly added node newPoint and a node in the other tree is less than the threshold Thr and the collision detection is passed.

5. The obstacle avoidance path planning method for a six-axis robotic arm based on an improved RRTstar as described in claim 1, characterized in that: The path shortening strategy in step 6 is to shorten the path when the maximum number of iterations is reached. It checks three consecutive points in the path to determine whether there is a collision and which points need to be deleted. Then, it replaces the original path composed of these three points with a straight line segment between the first and last two points and adds the first and last two points to the path.

6. The obstacle avoidance path planning method for a six-axis robotic arm based on an improved RRTstar as described in claim 1, characterized in that: In step 6, the path obtained by the path shortening strategy is smoothed using a cubic B-spline curve through fitting. The formulas involved are as follows: ； ； ； ； Where t is a node vector, representing the position on the curve, and its value ranges from [0,1], where t is a real number within the interval [0,1]. , , and These are all B-spline basis functions, used to construct cubic B-spline curves; The equation of a cubic B-spline curve is a linear combination of the four basis functions mentioned above, and the equation is as follows: ； in, The equation of the overall curve is . , , , These are the coordinates of the four control points; The smoothed final path points are transmitted to the robotic arm, which then performs its tasks based on these path points.

Images