Mechanical arm motion planner path optimization method

Abstract

The invention relates to a mechanical arm motion planner path optimization method, which comprises the following steps of: performing mechanical arm kinematics modeling to obtain an inverse kinematics equation; an RRT * mechanical arm path planning algorithm is improved, and the method comprises the steps that a target bias sampling strategy is adopted; introducing an artificial potential field method, constructing a target gravitational field and an obstacle repulsive force to form a potential field resultant force, adaptively adjusting the weight of the random extension performance and the weight of the target deviation performance according to an obstacle near a target point, and enhancing the target deviation weight in a range far away from the obstacle; the higher the density of the obstacles near the target point is, the more the target deviation weight is reduced, and the more the random expansibility weight is increased, so that rapid avoidance of the obstacles is realized; adjusting an extension step length according to a sampling point distance repulsive force field range; iteratively solving the collision-free path to obtain a joint group solution set corresponding to path points, and further obtaining the position and posture of an end effector of the mechanical arm; and designing a trajectory interpolation function.

Description

Technical Field

[0001] The invention relates to a path optimization method for a robot arm motion planner. Background Art

[0002] Although the commonly used motion planners for robotic arms have good performance, the path planning time is long and the path is not optimal. Taking the path planner based on the rapid exploration random tree (RRT) as an example, the use of spatial random sampling makes the controller path planning problem difficult to solve by interpolation fitting due to the problems of sampling blindness, disorder, poor convergence, etc. It is necessary to establish a problem description model in combination with performance indicators, transform it into a path optimization problem, and use a path planning algorithm with fast convergence and asymptotic optimality to solve it. The rapid exploration random tree star (RRT*) algorithm was proposed by Sertac Karaman and Emilio Frazzoli in 2010 (S. Karaman, E. Frazzoli. Optimal kinodynamic motion planning using incremental sampling-based methods, IE EE Conference on Decision and Control (CDC), 2010: 7681-7687). It is improved on the basis of RRT. By introducing the cost function of parent node reselection and node reconnection, in each iteration, the optimal parent node is reselected and the node is reconnected within a certain range with the new node as the origin, so that the generated path is asymptotically optimal.

[0003] The Rapid Exploration Random Tree Star (RRT*) algorithm has global search and expansion capabilities and is widely used in global path planning in high-dimensional space. It introduces a parent node reselection mechanism and a node cost function reconnection mechanism based on RRT, making the planned path asymptotically optimal and shortening the path length. The main problems of the RRT* algorithm are the blindness of random tree node expansion and low exploration efficiency. The exploration of useless space by the RRT* algorithm is mainly due to the lack of target bias in the expansion nodes. Summary of the invention

[0004] The present invention proposes a robot path planning method based on the fusion of rapid exploration of random tree stars and artificial potential fields, which overcomes the problems of blind expansion, easy exploration of useless space, and low exploration efficiency in existing path planning methods. Based on the RRT* algorithm, this method adopts a target bias sampling method to improve sampling efficiency. Secondly, it constructs a target gravitational field and an obstacle repulsive field to form a potential field attraction, enhances the target bias of the expansion node, and adaptively adjusts the target bias and random expansion of the expansion node. An adaptive dynamic step function based on the obstacle repulsive field is constructed to avoid invalid expansion and inability to avoid obstacles in the process of generating new nodes, thereby improving the global path planning efficiency. The technical solution is as follows:

[0005] A method for optimizing a robot arm motion planner path comprises the following steps:

[0006] S1: Perform kinematic modeling of the robot arm and obtain the inverse kinematics equation;

[0007] S2: Improve the RRT* robot path planning algorithm by:

[0008] S21: adopt target bias sampling strategy;

[0009] S22: Introduce the artificial potential field method to construct the target gravitational field and the obstacle repulsion to form a potential field force. According to the obstacles near the target point, the weights of random expansion performance and target bias performance are adaptively adjusted. When far away from the obstacle range, the target bias weight is enhanced; the higher the obstacle density near the target point, the lower the target bias weight is, and the higher the random expansion weight is, so as to achieve rapid avoidance of obstacles;

[0010] S23: adjusting the expansion step size according to the range of the sampling point distance from the repulsive field;

[0011] S24: Iteratively solve the collision-free path to obtain the joint group solution set corresponding to the path point, and then obtain the position and posture of the end effector of the robot arm;

[0012] S3: Design trajectory interpolation function.

[0013] Furthermore, the target bias sampling strategy of S21 is:

[0014]

[0015] α is the preset threshold. Each time the random tree T samples in three-dimensional space, it generates probability P according to the uniform probability random_sample(). If P is less than α, the sampling point X rand is the target point, and the random tree T samples in the target direction. If P is greater than α, X rand By random_sample()

[0016] Function random generation; X goal is the target point.

[0017] Furthermore, S22 includes:

[0018] Establish the target gravitational field model:

[0019]

[0020] Establish an obstacle repulsion field model:

[0021]

[0022] New node X new The update formula is as follows:

[0023]

[0024] Among them, F aat , F repu , F are the target attraction, obstacle repulsion, and potential field attraction, respectively; k a , k rep , σ are the gravitational factor, repulsive factor, and obstacle influence factor respectively, and λ is the weight coefficient.

[0025] Furthermore, S23 is as follows:

[0026] Let d be the expansion step size; p(X near ,X goal ), p(X rand -X obs ) are the distance between the nearest node and the target point, and the distance between the random node and the obstacle;

[0027] The expansion step length d is adjusted according to the range of the sampling point distance from the repulsive field. The calculation formula of the expansion step length d is as follows:

[0028] β i＝1,2,3 Represents the boundary threshold of the distance between the sampling point and the obstacle. The space is divided by the boundary threshold, that is, 0 <p(X rand -X obs )<β1,β1 <p(X rand -X obs )<β2,p(X rand -X obs )>β2 represent collision space, approach space, and free space respectively.

[0029] Furthermore, S24 is as follows:

[0030] Either iterate n times or when X new With X goalIf the distance is less than the preset value s, the target point is considered to be found, and a collision-free path is obtained from the target point back to the starting point, and a random tree T = {x1, x2, ..., x n}, {x1,x2,...,x n} is a collision-free path point in three-dimensional space, and {x1,x2,...,x n The path point is used as the target position and posture of the end effector of the robot in the motion space. It is substituted into the inverse kinematics equation of the robot to obtain the set of joint group solutions corresponding to the path point, and then the position and posture of the end effector of the robot are obtained.

[0031] Furthermore, S3 is specifically to fit the generated path points into a smooth path through quintic polynomial interpolation, so that the robot arm moves smoothly along the planned path.

[0032] The present invention first performs kinematic modeling on the six-axis robot arm, and then designs an inverse kinematics solver, uses the improved RRT* algorithm to adopt a target bias sampling method, improves sampling efficiency, constructs a target gravitational field and an obstacle repulsion field to form a potential field interaction, enhances the target bias of the expansion node, and adaptively adjusts the target bias and random extensibility of the expansion node, constructs an adaptive dynamic step function based on the obstacle repulsion field, avoids invalid expansion and inability to avoid obstacles in the process of generating new nodes, and improves the efficiency of global path planning. This method generates a series of collision-free path points, calculates the joint angles of the corresponding path points through an inverse kinematics solver, and then fits the path points through trajectory interpolation, so that the robot arm motion trajectory is smoother. Specifically, the beneficial effects are as follows:

[0033] 1. The target bias sampling method is adopted, and by introducing the artificial potential field, the target gravitational field and obstacle repulsive field are constructed to make the new nodes have target bias.

[0034] 2. Adjust the random scalability and target bias performance by adjusting the factors, so that an effective path can be quickly found in a narrow channel.

[0035] 3. According to the range of obstacle repulsion, an adaptive dynamic step method is used to speed up the convergence. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 flow chart

[0037] Figure 2 Six-axis robotic arm structure diagram

[0038] Figure 3 Obstacle potential field map based on adaptive step size

[0039] Figure 4 Flowchart of path planning algorithm based on improved RRT*

[0040] Figure 5 When using this method for path planning, the path generation process diagram

[0041] Figure 6 The robot moves from the starting point to the end point, and the trajectory of the 6-axis joint moves over time DETAILED DESCRIPTION

[0042] The present invention is described below in conjunction with the accompanying drawings and embodiments.

[0043] Step 1: Generalized kinematic modeling of the six-axis robot arm:

[0044] The six-axis robot arm is a multi-axis control system composed of connecting rods and joints. To design a controller, we must first establish the mapping relationship between the joint space and the Cartesian space, so as to establish a reasonable and accurate kinematic model. Figure 2 In the figure, the posture of the end effector of the robot arm is determined by the rotation angle of each joint, and the position is described by (x, y, z). According to the DH modeling method, a coordinate system is established on each link of the robot, and the coordinate transformation between adjacent link coordinate systems is realized through homogeneous coordinate transformation, so as to establish the kinematic model of the six-axis robot arm as follows:

[0045] Represents the coordinate system T i-1 , to coordinate system T i The homogeneous transformation matrix of i represents the joint rotation angle between adjacent links, a i Indicates the connecting rod length, d i is the distance between the two connecting rods, α i The joint space variables of the known robot arm are q = (θ1θ2θ3θ4θ5θ6) T , the description matrix of the end position of the robot arm can be obtained:

[0046]

[0047] Among them, R 3*3 It describes the posture of the end of the robot arm, P 3*1 Describes the position of the end of the robot arm.

[0048] Since the end effector of the robot arm has a specific task configuration, it is usually necessary to infer a specific configuration space based on the position space. This process is called inverse kinematics. The joint angle can be inferred from the position and posture of the end effector, thereby accurately controlling the motion of the joint. The inverse kinematics equation is:

[0049] Δθ=J -1 (θ)·(x d-x(θ)) (3)

[0050] where J(θ) is the Jacobian matrix, x d is the desired position and posture of the end effector of the robot arm, and x(θ) is the currently calculated position and posture of the end effector of the robot arm

[0051] Step 2: Design of robot path planning algorithm based on improved RRT*

[0052] 1) RRT* robot path planning algorithm

[0053] The RRT* algorithm introduces node reconnection and parent node reselection mechanisms based on RRT. First, the search times, starting point and target point positions are initialized, and the starting point is used as the root node to build a tree T containing only the starting point. Then a point X is randomly generated from the map. rand , and the point is not within the obstacle. Then find the leaf node closest to the sampling point, from X near To X rand Extend the fixed extension step d in the direction to obtain the new node X new If there is no collision in the path generated by the new node, the new node X needs to be new Reselect the parent node, then reconnect the random tree within a certain range, and enter the next cycle of sampling and growth. After iterating n times, trace back from the target point to the starting point to obtain a feasible path.

[0054] X new The process of generating a new node is as follows:

[0055]

[0056] 2) Improve the RRT* robot arm path planning algorithm

[0057] The RRT* algorithm has the advantages of global planning and asymptotic optimization, but the blindness of random expansion makes the newly generated nodes lack target bias, resulting in excessive exploration of invalid space and slow convergence. In order to solve the lack of target bias of new nodes while maintaining a certain random exploration performance, the target bias sampling strategy is first adopted:

[0058]

[0059] Set the threshold α. Each time the random tree T samples in three-dimensional space, it generates a probability P according to the uniform probability random_sample(). If P is less than α, the sampling point X rand is the target point, and the random tree T samples the target point as the target direction sampling. If P is greater than α, X randRandomly generated by the random_sample() function, query the leaf node X closest to the sampling point in the random tree T near , used to generate new node X new .

[0060] The artificial potential field method is introduced into the new node expansion strategy to construct the target gravitational field and the obstacle repulsion to form a potential field force, and the weights of random expansion performance and target bias performance are adaptively adjusted. When far away from the obstacle range, the target bias weight is enhanced. When there are many obstacles near the target point, the target bias weight is reduced and the random expansion weight is enhanced to quickly bypass the obstacles.

[0061] The target gravitational field model is as follows:

[0062]

[0063] The obstacle repulsion field model is as follows:

[0064]

[0065] X new The generation process of a new node is as follows:

[0066]

[0067] Among them, F aat , F repu , F are the target attraction, obstacle repulsion, and potential field attraction, respectively; k a , k rep , σ are the gravitational factor, repulsive factor, and obstacle influence factor, d is the expansion step length, X near is the nearest node; p(X near ,X goal ), p(X rand -X obs ) are the distance between the nearest node and the target point, and the distance between the random node and the obstacle; λ is the weight coefficient.

[0068] Since the traditional RRT* algorithm uses a fixed extension step size d, which leads to long search time and slow convergence, this method uses an adaptive step size method to construct an obstacle repulsion field and adjust the extension step size d according to the distance of the sampling point from the repulsion field. The calculation of the extension step size d is shown in Formula 9.

[0069]

[0070] β i＝1,2,3 Represents the boundary threshold of the distance between the sampling point and the obstacle. The space is divided by the boundary threshold, that is, 0 <p(X rand -X obs )<β1,β1 <p(Xrand -X obs )<β2,p(X rand -X obs )>β2 represent Figure 5 The collision space, approach space, and free space shown in Formula 9 have the same remaining parameters as those of Formula 8.

[0071] Either iterate n times, or when X new With X goal If the distance is less than the preset value s, the target point is considered to be found, and a collision-free path is obtained from the target point back to the starting point, and a random tree T = {x1, x2, ..., x n}, {x1,x2,...,x n} is a collision-free path point in three-dimensional space, and {x1,x2,...,x n The path point is used as the target position and posture of the end effector of the robot in the motion space. It is introduced into the inverse kinematics solution formula (3) of the robot in step 1 to obtain the joint group solution set corresponding to the path point:

[0072] X i ={θ1,θ2,θ3,θ4,θ5,θ6} i ,i=1,2,...n

[0073] The joint group solution X is calculated i ={θ1,θ2,θ3,θ4,θ5,θ6} i Substitute the formula (2) in step 1 to obtain the position and posture of the end effector of the robot arm:

[0074] When P 3*1 =x i , then the planned path points are considered to meet the kinematic constraints of the robot arm.

[0075] Step 3: Design trajectory interpolation function

[0076] In the six-axis robot motion planner, the path planning algorithm generates a series of collision-free path points. To make the robot move smoothly along these path points, firstly, the inverse kinematics solver is used to calculate the joint angles corresponding to each point in step (1), and then the generated path points are fitted into a smooth path through quintic polynomial interpolation, so that the robot moves smoothly along the planned path. The interpolation algorithm is as follows

[0077] θ(t)=a0+a1t+a2t 2 +a3t 3 +a4t 4 +a5t 5 (10) Its constraints are:

[0078]

[0079] It can be solved

[0080]

[0081] Where θ0, are the joint position, joint velocity and joint acceleration at the starting point of the known path segment, θ f , are the joint position, joint velocity and joint acceleration at the end point of the path segment, respectively, and θ(t) is the function of the joint angle changing over time.

Claims

1. A method for optimizing a path of a robot motion planner, comprising the following steps: S1: Perform kinematic modeling of the robot arm and obtain the inverse kinematics equation; S2: Improve the RRT* robot path planning algorithm by: S21: adopt target bias sampling strategy; S22: Introduce the artificial potential field method to construct the target gravitational field and the obstacle repulsion to form a potential field force. According to the obstacles near the target point, the weights of random expansion performance and target bias performance are adaptively adjusted. When far away from the obstacle range, the target bias weight is enhanced; the higher the obstacle density near the target point, the lower the target bias weight is, and the higher the random expansion weight is, so as to achieve rapid avoidance of obstacles; S23: adjusting the expansion step size according to the range of the sampling point distance from the repulsive field; S24: Iteratively solve the collision-free path to obtain the joint group solution set corresponding to the path point, and then obtain the position and posture of the end effector of the robot arm; S3: Design trajectory interpolation function.

2. The robot arm motion planner path optimization method according to claim 1, characterized in that: The target bias sampling strategy of S21 is: α is the preset threshold. Each time the random tree T samples in three-dimensional space, it generates probability P according to the uniform probability random_sample(). If P is less than α, the sampling point X rand is the target point, and the random tree T samples in the target direction. If P is greater than α, X rand Randomly generated by random_sample() function; X goal is the target point.

3. The robot arm motion planner path optimization method according to claim 2, characterized in that: S22 includes: Establish the target gravitational field model: Establish an obstacle repulsion field model: New node X new The update formula is as follows: Among them, F aat , F repu , F are the target attraction, obstacle repulsion, and potential field attraction, respectively; k a , k rep , σ are the gravitational factor, repulsive factor, and obstacle influence factor respectively, and λ is the weight coefficient.

4. The robot arm motion planner path optimization method according to claim 3, characterized in that: S23 is as follows: Let d be the expansion step size; p(X near ,X goal ), p(X rand -X obs ) are the distance between the nearest node and the target point, and the distance between the random node and the obstacle; The expansion step length d is adjusted according to the range of the sampling point distance from the repulsive field. The calculation formula of the expansion step length d is as follows: β i＝1,2,3 Represents the boundary threshold of the distance between the sampling point and the obstacle. The space is divided by the boundary threshold, that is, 0 <p(X rand -X obs )<β1,β1 <p(X rand -X obs )<β2,p(X rand -X obs )>β2 represent collision space, approach space, and free space respectively.

5. The robot arm motion planner path optimization method according to claim 1, characterized in that: S24 is as follows: Either iterate n times or when X new With X goal If the distance is less than the preset value s, the target point is considered to be found, and a collision-free path is obtained from the target point back to the starting point, and a random tree T = {x1, x2, ..., x n }, {x1,x2,...,x n } is a collision-free path point in three-dimensional space, and {x1,x2,...,x n The path point is used as the target position and posture of the end effector of the robot in the motion space. It is substituted into the inverse kinematics equation of the robot to obtain the set of joint group solutions corresponding to the path point, and then the position and posture of the end effector of the robot are obtained.

6. The robot arm motion planner path optimization method according to claim 1, characterized in that: S3 is specifically to fit the generated path points into a smooth path through quintic polynomial interpolation, so that the robot arm moves smoothly along the planned path.

Images