A path optimization method for robotic arm motion planner

Abstract

This invention relates to a path optimization method for a robotic arm motion planner, comprising the following steps: performing kinematic modeling of the robotic arm to obtain inverse kinematic equations; improving the RRT* robotic arm path planning algorithm by adopting a target bias sampling strategy; introducing an artificial potential field method to construct a potential force field formed by the target gravitational field and obstacle repulsion; adaptively adjusting the weights of random expansion performance and target bias performance based on obstacles near the target point, increasing the target bias weight in areas far from obstacles; reducing the target bias weight and increasing the random expansion weight as the obstacle density near the target point increases, in order to achieve rapid obstacle avoidance; adjusting the expansion step size according to the distance of the sampling point from the repulsion field range; iteratively solving for a collision-free path to obtain the joint set solution corresponding to the path point, thereby obtaining the position and orientation of the robotic arm end effector; and designing a trajectory interpolation function.

Description

Technical Field

[0001] This invention relates to a path optimization method for a robotic arm motion planner. Background Technology

[0002] While commonly used motion planners for robotic arms offer good performance, their path planning time is long, and the paths are not always optimal. Taking the Rapid Exploratory Random Tree (RRT) path planner as an example, the use of spatial random sampling makes the controller path planning problem difficult to solve through interpolation fitting due to issues such as blind sampling, disorder, and poor convergence. It is necessary to establish a problem description model based on performance indicators, transforming it into a path optimization problem, and then using a path planning algorithm with fast convergence and asymptotic optimization to solve it. The Rapid Exploratory Random Tree (RRT*) algorithm was proposed in 2010 by Sertac Karaman and Emilio Frazzoli (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 improves upon RRT by introducing cost functions for parent node reselection and node reconnection. In each iteration, with the new node as the origin, optimal parent node reselection and node reconnection are performed within a certain range, resulting in an asymptotically optimal path.

[0003] The Rapidly Explored Random Tree Star (RRT*) algorithm possesses global search and expansion capabilities, and is widely applied to global path planning in high-dimensional spaces. Building upon RRT, it introduces a parent node reselection mechanism and a node cost function reconnection mechanism, resulting in asymptotically optimal planned paths and shortened path lengths. The main problems with the RRT* algorithm are the blind expansion of random tree nodes and its low exploration efficiency. The RRT* algorithm's exploration of useless spaces is primarily attributed to the lack of target bias in node expansion. Summary of the Invention

[0004] This invention proposes a robotic arm path planning method based on the fusion of rapid exploration of random tree stars and artificial potential fields, overcoming 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 employs a target bias sampling method to improve sampling efficiency. Secondly, it constructs a potential force field by integrating the target's gravitational field and the obstacle's repulsive field, enhancing the target bias of the expanded nodes. It also adaptively adjusts the target bias and random expansion of the expanded nodes, constructing an adaptive dynamic step size function based on the obstacle repulsive field to avoid invalid expansion and obstacle avoidance failures during the generation of new nodes, thus improving the global path planning efficiency. The technical solution is as follows:

[0005] A path optimization method for a robotic arm motion planner includes the following steps:

[0006] S1: Perform kinematic modeling of the robotic arm and obtain the inverse kinematic equations;

[0007] S2: An improvement to the RRT* robotic arm path planning algorithm is as follows:

[0008] S21: Adopt a target bias sampling strategy;

[0009] S22: Introducing the artificial potential field method, a potential force is formed by constructing the target's gravitational field and the obstacle's repulsive force. Based on the obstacles near the target point, the weights of random expansion performance and target bias performance are adaptively adjusted. In the range far from the obstacles, the target bias weight is enhanced; the higher the obstacle density near the target point, the lower the target bias weight and the higher the random expansion weight, so as to achieve rapid avoidance of obstacles.

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

[0011] S24: Iteratively solve for the collision-free path to obtain the joint set of solution points corresponding to the path points, and then obtain the position and orientation of the end effector of the robotic arm;

[0012] S3: Design the trajectory interpolation function.

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

[0014]

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

[0016] The function is randomly generated; X goal The target point.

[0017] Furthermore, S22 includes:

[0018] Establish a target gravitational field model:

[0019]

[0020] Establish an obstacle repulsive force field model:

[0021]

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

[0023]

[0024] Where F aat F repu F represents the gravitational force of the target, the repulsive force of the obstacle, and the potential force, respectively; k a k rep σ and σ represent the gravitational factor, repulsive factor, and obstacle influence factor, respectively, and λ is the weighting coefficient.

[0025] Furthermore, S23 is detailed below:

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

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

[0028] β i＝1,2,3 The boundary threshold representing the distance of the sampling point from the obstacle is used to divide the space, i.e., 0. <p(X rand -X obs )<β1,β1 <p(X rand -X obs )<β2, p(X) rand -X obs )>β2 represent the collision space, proximity space, and free space, respectively.

[0029] Furthermore, S24 is detailed below:

[0030] Or iterate n times or when X... new With X goalIf the distance is less than a preset value s, then the target point is considered found. A collision-free path is then traced back from the target point to the starting point, and a random tree T = {x1, x2, ..., x...} is generated. n}, {x1,x2,...,x n Let {x1, x2, ..., x} be a collision-free path point in three-dimensional space. n The path point is used as the target position and orientation of the end effector of the robotic arm in the motion space. Substituting it into the inverse kinematics equation of the robotic arm, the solution set of the joint group corresponding to the path point is obtained, and then the position and orientation of the end effector of the robotic arm is obtained.

[0031] Furthermore, S3 specifically involves fitting the generated path points into a smooth path using fifth-order polynomial interpolation, enabling the robotic arm to move smoothly along the planned path.

[0032] This invention first performs kinematic modeling on a six-axis robotic arm, then designs an inverse kinematics solver, and utilizes an improved RRT* algorithm with a target bias sampling method to improve sampling efficiency. It constructs a potential force field by combining the target's gravitational field and the obstacle's repulsive field, enhancing the target bias of the extended nodes. Furthermore, it adaptively adjusts the target bias and random expansion of the extended nodes, constructing an adaptive dynamic step size function based on the obstacle repulsive field. This avoids invalid expansion and obstacle avoidance issues during the generation of new nodes, improving 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 using the inverse kinematics solver, and then fits the path points using trajectory interpolation, resulting in a smoother robotic arm trajectory. Specifically, the beneficial effects are as follows:

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

[0034] 2. By adjusting the random expansion and target bias performance through adjustment factors, an effective path can be found quickly in narrow channels.

[0035] 3. Based on the range of the obstacle's repulsive force, an adaptive dynamic step size method is adopted to accelerate the convergence speed. Attached Figure Description

[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 the improved RRT* path planning algorithm

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

[0041] Figure 6 The robotic arm moves from the starting point to the ending point, with the 6-axis joints moving along a trajectory over time. Detailed Implementation

[0042] The present invention will now be described in conjunction with the accompanying drawings and embodiments.

[0043] Step 1: Generalized kinematic modeling of a six-axis robotic arm:

[0044] A six-axis robotic arm is a multi-axis control system composed of links and joints. To design a controller, the mapping relationship between joint space and Cartesian space must first be established, thereby creating a reasonable and accurate kinematic model. Figure 2 In this model, the attitude of the end effector of the robotic arm is determined by the rotation angles of each joint, and its position is described by (x, y, z). A coordinate system is established on each link of the robot using the DH modeling method. Homogeneous coordinate transformation is used to achieve coordinate transformation between adjacent link coordinate systems, thus establishing the kinematic model of the six-axis robotic arm as follows:

[0045] Represents coordinate system T i-1 , to coordinate system T i The homogeneous transformation matrix, where θ i a represents the joint rotation angle between adjacent links. i d represents the length of the link. i Let α be the distance between the two links. i Given the link twist angle, the joint space variable of the robotic arm is q = (θ1θ2θ3θ4θ5θ6). T The description matrix of the robot arm's end-effector pose can be obtained:

[0046]

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

[0048] Because robotic arm end effectors have specific task configurations, it is usually necessary to infer a specific configuration space from the position space. This process is called inverse kinematics. Joint angles can be calculated from the position and orientation of the end effector, thereby enabling precise motion control of the joints. The inverse kinematics equations are:

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

[0050] Where J(θ) is the Jacobian matrix, x d The desired position and orientation of the robotic arm's end effector are given by x(θ), where x(θ) represents the currently calculated position and orientation of the robotic arm's end effector.

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

[0052] 1) RRT* Robotic Arm Path Planning Algorithm

[0053] The RRT* algorithm builds upon RRT by introducing node reconnection and parent node reselection mechanisms. First, it initializes the search count, starting point, and target point location, establishing a tree T containing only the starting point as the root node. Then, it randomly generates a point X from the map. rand And this point is not inside the obstacle. Next, find the leaf node closest to the sampling point, from X... near To X rand By extending the direction with a fixed step size d, a new node X is obtained. new If the path for generating a new node does not collide, then the new node X needs to be... new A new parent node is selected, and then the random tree is reconnected within a certain range, entering the next cyclic sampling and growth process. After n iterations, a feasible path is obtained by backtracking from the target point to the starting point.

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

[0055]

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

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

[0058]

[0059] Set a threshold α. Each time the random tree T samples in three-dimensional space, it generates a probability P using the uniform probability method `random_sample()`. If P is less than α, then the sampling point X is not selected. rand For the target point, the random tree T samples the target point as the target direction. If P is greater than α, X randThe sample is randomly generated by the random_sample() function. The function then queries the leaf node X in the random tree T that is closest to the sampled point. near Used to generate new node X later. new .

[0060] In the new node expansion strategy, an artificial potential field method is introduced to construct a potential force by combining the target gravitational field and the obstacle repulsive force. The weights of random expansion performance and target bias performance are adaptively adjusted. When the target is far away from the obstacle, the target bias weight is increased. When there are many obstacles near the target point, the target bias weight is decreased and the random expansion weight is increased to quickly bypass the obstacles.

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

[0062]

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

[0064]

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

[0066]

[0067] Where F aat F repu F represents the gravitational force of the target, the repulsive force of the obstacle, and the potential force, respectively; k a k rep σ and σ represent the gravitational factor, repulsive factor, and obstacle influence factor, respectively, and d is the expansion step size. near The nearest node; p(X) near ,X goal ), p(X rand -X obs ) represent the distances between the nearest node and the target point, and the distances between the random node and the obstacle, respectively; λ is the weighting coefficient.

[0068] Because the traditional RRT* algorithm uses a fixed expansion step size d, it results in long search time and slow convergence. This method adopts an adaptive step size method to construct an obstacle repulsion field and adjust the expansion step size d according to the distance of the sampling point from the repulsion field range. The expansion step size d is calculated as shown in Formula 9.

[0069]

[0070] β i＝1,2,3 The boundary threshold representing the distance of the sampling point from the obstacle is used to divide the space, i.e., 0. <p(X rand -X obs )<β1,β1 <p(Xrand -X obs )<β2, p(X) rand -X obs )>β2 represent respectively Figure 5 The collision space, proximity space, and free space shown in Formula 9 are the same as those in Formula 8.

[0071] Either iterate n times, or when X new With X goal If the distance is less than a preset value s, the target point is considered found. A collision-free path is then traced back from the target point to the starting point, and a random tree T = {x1, x2, ..., x...} is generated. n}, {x1,x2,...,x n Let {x1, x2, ..., x} be a collision-free path point in three-dimensional space. n The path points, representing the target positions and orientations of the robotic arm's end effector in the motion space, are substituted into the inverse kinematics solution formula (3) for the robotic arm in step one to obtain the joint set of solutions corresponding to the path points:

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

[0073] The joint group solution X will be calculated. i ={θ1,θ2,θ3,θ4,θ5,θ6} i Substituting into formula (2) from step one, we obtain the position and orientation of the robotic arm's end effector:

[0074] When P 3*1 =x i If so, it is assumed that the planned path points conform to the kinematic constraints of the robotic arm.

[0075] Step 3: Design the trajectory interpolation function

[0076] In the motion planner of the six-axis robotic arm, the path planning algorithm generates a series of collision-free path points. To make the robotic arm move smoothly along these path points, the joint angles corresponding to each point are first calculated by the inverse motion solver in step (1). Then, the generated path points are fitted into a smooth path by fifth-order polynomial interpolation, so that the robotic arm 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] In the formula, θ0, Given the joint position, joint velocity, and joint acceleration of the starting point of a known path segment, θ represents... f , Let θ(t) be the joint position, joint velocity, and joint acceleration at the end of the path segment, respectively, and let θ(t) be a function of the joint angle changing with time.

Claims

1. A path optimization method for a robotic arm motion planner, comprising the following steps: S1: Perform kinematic modeling of the robotic arm and obtain the inverse kinematic equations; S2: An improvement to the RRT* robotic arm path planning algorithm is as follows: S21: Adopt a target bias sampling strategy: (5) Random tree with a preset threshold Each time sampling is performed in three-dimensional space, according to uniform probability Generation probability If less than Then the sampling point For the target point, a random tree Sample in the direction of the target, if Greater than , Depend on The function is generated randomly; For the target point; S22: An artificial potential field method is introduced to construct a potential force field formed by the gravitational field of the target and the repulsive force of obstacles. Based on the obstacles near the target point, the weights of random expansion performance and target bias performance are adaptively adjusted. In areas far from obstacles, the target bias weight is increased; the higher the obstacle density near the target point, the lower the target bias weight and the higher the random expansion weight, to achieve rapid obstacle avoidance. The method is as follows: Establish a target gravitational field model: (6) Establish an obstacle repulsive force field model: (7) New node The updated formula is as follows: (8) in , , These are, respectively, the gravitational force of the target, the repulsive force of the obstacle, and the potential force; , , These are the gravitational factor, repulsive factor, and obstacle influence factor, respectively. These are the weighting coefficients; S23: Adjust the expansion step size according to the distance of the sampling point from the repulsive field range, as follows: set up To extend the step size; These represent the distance between the nearest node and the target point, and the distance between the random node and the obstacle, respectively. Adjust the expansion step size according to the distance of the sampling point from the repulsive field range. Expand step size The calculation formula is as follows: (9) The boundary threshold representing the distance of the sampling point from the obstacle is used to divide the space. , , These represent collision space, proximity space, and free space, respectively. S24: Iteratively solve for the collision-free path to obtain the joint set of solution points corresponding to the path points, and then obtain the position and orientation of the end effector of the robotic arm; S3: Design the trajectory interpolation function.

2. The path optimization method for a robotic arm motion planner according to claim 1, characterized in that, S24 is as follows: Or iterate n times or when and The distance is less than the preset value If the target point is found, a collision-free path is obtained by backtracking from the target point to the starting point, and a random tree is generated. , For collision-free path points in 3D space, The path point serves as the target position and orientation of the robotic arm's end effector in the motion space. By substituting it into the inverse kinematics equations of the robotic arm, the solution set of the joint group corresponding to the path point is obtained, and thus the position and orientation of the robotic arm's end effector are obtained.

3. The path optimization method for a robotic arm motion planner according to claim 1, characterized in that, Specifically, S3 involves fitting the generated path points into a smooth path using fifth-order polynomial interpolation, enabling the robotic arm to move smoothly along the planned path.

Images