Search

CN-116852367-B - Six-axis mechanical arm obstacle avoidance path planning method based on improvement RRTstar

CN116852367BCN 116852367 BCN116852367 BCN 116852367BCN-116852367-B

Abstract

The invention discloses a six-axis mechanical arm obstacle avoidance path planning method based on improvement RRTstar, and belongs to the technical field of mechanical arm path planning. Aiming at the defects that RRTstar algorithm search space is large, search time is long, calculation complexity is high, and the generated path is too many in inflection angle and is not smooth, which is not beneficial to the operation of a mechanical arm, the method firstly introduces a target bias strategy, combines a dual-tree expansion strategy to expand from a starting point and a target point simultaneously, adds a path shortening strategy and a redundant node removing strategy, and finally performs B spline fitting optimization on a search path.

Inventors

  • LI DING
  • ZHANG YU

Assignees

  • 昆明理工大学

Dates

Publication Date
20260508
Application Date
20230721

Claims (6)

  1. 1. The six-axis mechanical arm obstacle avoidance path planning method based on the improvement RRTstar is characterized by comprising the following steps of: Step 1, acquiring the obstacle distribution condition of a working space of a mechanical arm, establishing a mechanical arm collision model and a kinematic model, and initializing two random trees, namely, a Tree1 and a Tree2, wherein the two random trees comprise a starting point X_start and a finishing point X_ goal, a threshold value Thr, the maximum iteration number MaxIterations, a step size, a search range searchSize and a map environment; step 2, alternately sampling the random trees Tree1 and Tree2 through a target bias strategy to generate a random point x_rand; Step 3, searching for adjacent nodes x_near, then expanding the adjacent nodes x_near along the direction of the connecting line with the x_rand by a step size stepsize to generate new nodes newPoint, performing collision detection after expansion, adding the new nodes into random trees Tree1 and Tree2 if no collision occurs, and resampling if collision occurs; Step 4, taking the expanded new node newPoint as a circle center, searching a neighbor node set q_near with the distance newPoint smaller than the radius r in the existing tree, taking a node x_min with the smallest distance as a newPoint new parent node, performing pruning rewire operation, and reconnecting the new node newPoint to a node with the shortest path except for x_min in the q_near; And 5, calculating the distance between the new node newPoint and the existing nodes in the random trees Tree1 and Tree2, comparing the distance with a distance threshold d, if the distance between the new node and the existing nodes is greater than or equal to the threshold d, reserving the new node, and if the distance is smaller than the threshold d, removing the node overlapped with newPoint in the random Tree, wherein the distance function is as follows: ; a and b are node coordinates, a (1), a (2) and a (3) respectively represent xyz coordinates of a node a in a three-dimensional space, and b (1), b (2) and b (3) respectively represent xyz coordinates of a node b in the three-dimensional space; step 6, after the random Tree Tree1 and the random Tree Tree2 are successfully connected, continuing to alternately sample, outputting a path if the maximum iteration number MaxIterations is reached, shortening the path according to a path shortening strategy, and smoothing the path by using a cubic B spline curve; The target bias strategy in the step 2 considers the influence of the target point by introducing the target bias probability p1, so that a certain probability faces the target point when generating a new random point, and the random trees Tree1 and Tree2 generate the new random point And The sampling mode of (a) is as follows: ; ; Where P is a random number within a range of randomly generated (0, 1), for the 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 toward 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 the random Tree Tree2, if P is greater than the target bias probability P1, then x_rand2 will generate a new random point toward the starting point X_start, and 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. 2. The six-axis mechanical arm obstacle avoidance path planning method based on the improvement RRTstar as set forth in claim 1, wherein in the step 1, a start point X_start point, an end point X_ goal, a search range searchSize and a map environment required by the mechanical arm work are determined according to an actual working scene of the mechanical arm and related parameters of the mechanical arm, and the map environment contains obstacle distribution conditions.
  3. 3. The six-axis mechanical arm obstacle avoidance path planning method based on improvement RRTstar according to claim 1, wherein the pruning rewire in the step 4 is operated to find a set q_near of all nodes with a distance r less than or equal to r for a new node newPoint; For each node xi in q_near, check if there is a path from newPoint to xi such that the cost of the path is less than the current cost value of xi, if there is such a path, set the parent node of xi to newPoint, and update the cost value of xi and the cost values of all children nodes of xi.
  4. 4. The six-axis mechanical arm obstacle avoidance path planning method based on improvement RRTstar as claimed in claim 1, wherein the condition that the random trees Tree1 and Tree2 in the step 6 are successfully connected is that the node distance between the newly added node newPoint and another Tree is smaller than a threshold value Thr and the collision detection is passed.
  5. 5. The six-axis mechanical arm obstacle avoidance path planning method based on improvement RRTstar as set forth in claim 1, wherein the path shortening strategy in the step 6 is to shorten the path when the maximum iteration number is reached, check three continuous points in the path, determine whether there are collision points and points to be deleted, replace the path formed by the three points with a straight line segment between the first and last points, and add the first and last points to the path.
  6. 6. The six-axis mechanical arm obstacle avoidance path planning method based on improvement RRTstar as claimed in claim 1, wherein the path obtained by the path shortening strategy is smoothed by using a cubic B-spline curve in the step 6, and a fitting mode is adopted, wherein the related formula is as follows: ; ; ; ; Wherein t is a node vector and represents the position on the curve, the range of values is [0,1], and real numbers in the interval 、 、 And Are B spline basis functions for constructing cubic B spline curves; The cubic B-spline curve equation is a linear combination of the four basis functions, and the equation is as follows: ; Wherein, the As a result of the general curve equation, , , , Coordinate values of four control points; and transmitting the smoothed final path point to the mechanical arm, and executing a work task by the mechanical arm according to the obtained path point.

Description

Six-axis mechanical arm obstacle avoidance path planning method based on improvement RRTstar Technical Field The invention belongs to the technical field of path planning of mechanical arms, and particularly relates to a six-axis mechanical arm obstacle avoidance path planning method based on improvement RRTstar. Background Robot obstacle avoidance path planning is an important field of robot control, and relates to how to enable a robot to avoid various obstacles independently in a complex environment. Common robot obstacle avoidance path planning algorithms include graph search based algorithms and sampling based algorithms. Algorithms based on graph search, such as Dijkstra algorithm and Astar algorithm, abstract robots and obstacles as nodes in the graph by building an environment map, and search the shortest path by using the graph search algorithm. The algorithm has high calculation efficiency, can find the shortest path, but under the conditions of high-dimensional space and complex constraint environment, the search space becomes very large, so that the calculation complexity is increased, and the calculation time is prolonged. Sampling-based algorithms such as RRT (Rapidly-exploring Random Tree) algorithm and PRM (Probabilistic Roadmap) algorithm, a sampling map of the robot motion space is established by randomly sampling the robot pose and environmental data, and a feasible path is searched by using a map search algorithm. The algorithm has quick exploration capability and random property, and can obtain better performance in a complex environment. Therefore, in the path planning of the mechanical arm, a sampling-based algorithm is adopted, so that the mechanical arm can be helped to find a feasible and efficient path and avoid obstacles. The rapid random expansion tree algorithm is a common mechanical arm path planning algorithm, and a feasible solution can be searched in a large range through a random sampling strategy, so that a globally optimal solution is more likely to be found. However, there may be a situation that the sampling points are unevenly distributed, so that the planned path is not smooth enough, and the mechanical arm may vibrate during movement. However, random sampling may have uneven distribution of sampling points, so that a planned path is not smooth enough, and the quality of the generated path is low, so that the mechanical arm may vibrate during movement. Disclosure of Invention Aiming at the defects of the prior art, the mechanical arm obstacle avoidance path planning method with short search time, short path and smoothness is provided. In order to solve the technical problems, the technical scheme of the invention is that the six-axis mechanical arm obstacle avoidance path planning method based on the improvement RRTstar is characterized by comprising the following steps: Step 1, acquiring the distribution condition of the obstacle in the working space of the mechanical arm, establishing a mechanical arm collision model and a kinematic model, and initializing two random trees, namely Tree1 and Tree2, comprising a starting point X_start and a finishing point X_ goal, a threshold value Thr, the maximum iteration number MaxIterations, a step size, a search range searchSize and a map environment. Step 2, alternately sampling the random trees Tree1 and Tree2 through a target bias strategy to generate a random point x_rand; Step 3, searching for adjacent nodes x_near, then expanding the adjacent nodes x_near along the direction of the connecting line with the x_rand by a step size stepsize to generate new nodes newPoint, performing collision detection after expansion, adding the new nodes into random trees Tree1 and Tree2 if no collision occurs, and resampling if collision occurs; and 4, searching a neighbor node set q_near with the distance newPoint smaller than the radius r in the existing tree by taking the expanded new node newPoint as the circle center, taking the node x_min with the smallest distance as a newPoint new parent node, performing pruning rewire operation, and reconnecting the new node newPoint to the node with the shortest path except for x_min in the q_near. And 5, calculating the distance between the new node newPoint and the existing nodes in the random trees Tree1 and Tree2, comparing the distance with a distance threshold d, if the distance between the new node and the existing nodes is greater than or equal to the threshold d, reserving the new node, and if the distance is smaller than the threshold d, removing the node overlapped with newPoint in the random Tree, wherein the distance function is as follows: a and b are node coordinates, a (1), a (2) and a (3) respectively represent xyz coordinates of a node a in a three-dimensional space, and b (1), b (2) and b (3) respectively represent xyz coordinates of a node b in the three-dimensional space; And 6, after the random trees Tree1 and Tree2 are successfully connected, continuing to alternately samp