Polynomial interpolation trajectory planning method for robotic arm based on QLPSO algorithm

Abstract

The present invention provides a polynomial interpolation trajectory planning method for a manipulator based on a QLPSO algorithm, comprising: constructing a relationship between a time-impact optimal objective function and an interpolation coefficient and a segmentation time required for 4-5-4 polynomial segmented interpolation trajectory planning of each joint of a multi-joint manipulator; according to the QLPSO algorithm, using a particle Q value to select parameters of a particle swarm algorithm, namely, an inertia weight and a learning factor, using the position of the particle to perform time-impact minimum optimization, and satisfying the kinematic constraints of each joint, namely, position, velocity, acceleration and jerk constraints, so as to obtain a manipulator joint space trajectory with smooth position, continuous velocity and acceleration, and continuous jerk at a path trajectory interpolation point.

Description

Technical Field

[0001] The invention relates to a robot planning technology, in particular to a robot arm polynomial interpolation trajectory planning method based on QLPSO algorithm. Background Art

[0002] Trajectory planning can be divided into two types: trajectory planning in joint space and trajectory planning in Cartesian space. The advantage of joint space trajectory planning is that the amount of calculation is small and it is applicable to many scenarios. Polynomial trajectory planning is a commonly used and easy-to-understand method in joint space trajectory planning algorithms. It uses polynomial curve interpolation for planning, usually using third to seventh order polynomials, and occasionally using ninth order polynomials. However, the higher the degree of the polynomial, the more data is required, the greater the amount of calculation, and the more the interpolation result deviates from the original function. In actual operation tasks, there are different requirements for speed, acceleration, etc. at different stages, so in comparison, piecewise polynomial trajectory planning is more applicable. Some scholars have proposed a "3-5-3" polynomial interpolation trajectory planning algorithm based on an improved cuckoo search algorithm, which achieves time optimization under the premise of satisfying speed constraints. In order to ensure that the excavator maintains a stable trajectory, reduces vibration, and reduces impact during operation, the researchers adopted the "3-3-5-3-3" polynomial interpolation algorithm, which achieves smooth and continuous angular velocity and acceleration curves of each joint of the excavator.

[0003] In actual operation tasks, both time optimization and impact optimization must be considered. The relationship between the two is a relationship of secondary long and mutual elimination. Therefore, an algorithm is needed to compromise between time optimization and impact optimization. On the other hand, whether the robot arm adopts the "3-5-3" polynomial interpolation or the "3-3-5-3-3" polynomial interpolation trajectory planning algorithm, it cannot guarantee the smoothness of the impact force at the midpoint of the trajectory, that is, the robot arm will be subjected to a sudden impact force at the midpoint, thereby affecting the service life of the robot arm. The CN201811135979 patent relates to a QPSO algorithm for robot arm polynomial interpolation trajectory planning. The algorithm has a high population diversity, but due to the randomness of the position, the search efficiency is low, and it cannot always follow the group optimal particle search. As the iteration proceeds, the difference of the particles decreases, the group diversity also decreases, and the particle group parameter selection cannot be corrected in time. At the same time, the acceleration curve is smooth and continuous. Summary of the invention

[0004] The present invention provides a polynomial interpolation trajectory planning method for a multi-joint robot based on a QLPSO algorithm. The method takes into account the comprehensive objectives of time optimization and impact optimization and the problem of maintaining the continuity of jerk at the midpoint of the robot trajectory. A time-impact optimal trajectory planning algorithm for a multi-joint robot is designed using the QLPSO algorithm.

[0005] A polynomial interpolation trajectory planning method for a robotic arm based on a QLPSO algorithm, comprising:

[0006] Step 1: Give the starting position, middle position and target position of the end trajectory of the robot arm, obtain the angular position of each joint according to the inverse dynamics of the robot arm, and give the position, velocity, acceleration and jerk constraint conditions of each joint;

[0007] Step 2, initialize the particle swarm parameters and Q value of the jth joint;

[0008] Step 3, establish the time-impact optimal objective function, update the velocity and position of particle i, and calculate the time-impact objective function value;

[0009] Step 4, calculate the trajectory of the particle, and check whether the position, velocity, acceleration, and jerk of the jth joint meet the constraints; if so, execute step 5; if not, set the fitness value of the particle to infinity, randomly set the position and velocity of the particle, and go to step 3;

[0010] Step 5: Set the individual best P ij and the global optimal G j Compare, if the individual best P ij Less than the global optimal G j , then assign the best individual to the global best G j , complete the update G j , and obtain the minimum time-impact value of joint j in this iteration;

[0011] Step 6: Update the Q value corresponding to each particle, and select the inertia weight w and learning factors c and d corresponding to the maximum Q value as the parameters for the next particle swarm iteration.

[0012] Step 7: If the number of iterations reaches the maximum, go to step 8; if the number of iterations is less than the specified number of iterations, go to step 3;

[0013] Step 8: If all joints have been optimized, calculate the maximum time-impact optimization objective function value, give the time required for each trajectory segment, and calculate the trajectory of each joint based on this time, and calculate the end trajectory of the robot arm based on forward kinematics; if there are still joints that have not been optimized, go to step 2 and plan the trajectory for the unoptimized joints.

[0014] Furthermore, the specific process of step 1 is:

[0015] Step 1.1: Construct the relationship between the coefficients of the 4-5-4 polynomial interpolation function of each joint of the robot and the time taken for each trajectory segment

[0016] θ 1j (t) = a14_j t 4 +a 13_j t 3 +a 12_j t 2 +a 11_j t+a 10_j

[0017] θ 2j (t) = a 25_j t 5 +a 24_j t 4 +a 23_j t 3 +a 22_j t 2 +a 21_j t+a 20_j

[0018] θ 3j (t) = a 34_j t 4 +a 33_j t 3 +a 32_j t 2 +a 31_j t+a 30_j

[0019] In the formula, t represents the time taken for each trajectory, θ ij (t) represents the polynomial interpolation function of the i-th segment of the j-th joint trajectory of the robot arm, i is the number of segments of the trajectory, i∈{1,2,3}, j∈{1,2,…,N}, N represents the total number of joints, a ik_j represents the coefficient of the interpolation polynomial, k∈{1,2,3,4,5};

[0020] Step 1.2, obtain the coefficients in the trajectory and each time t 1j ,t 2j and t 3j The relationship between

[0021] a 10_j =s oj ,a 11_j =0,a 12_j =0

[0022]

[0023]

[0024]

[0025] 24a 14_j t 1j +6a 13_j =6a23_j

[0026] a 20_j =s 1j , a 30_j =s 2j

[0027]

[0028]

[0029]

[0030]

[0031]

[0032]

[0033]

[0034] Furthermore, the time-impact optimal objective function recorded in step 3 is

[0035]

[0036] st

[0037]

[0038] In the formula, λ 1 is the time weight coefficient, λ 2 is the impact weight coefficient, η represents the elastic coefficient, q max 、v max 、a max , jerk max is the maximum constraint set, where q max is the joint angle, v max is the set speed, a max is the set acceleration, jerk max The setting is the acceleration.

[0039] Furthermore, in step 4, a particle swarm algorithm model for trajectory planning of the jth joint is established.

[0040]

[0041] In the formula, w is the inertia weight of the particle, c and d are learning factors, and P ij is the optimal time of the ith particle at the jth joint, G j is the optimal segmentation time for all particles at the jth joint, They represent the time required for each trajectory of the j-th joint particle i and the speed of particle i, respectively. 1 ,r 2 ,r 3 is a random number in the interval [0,1] that satisfies the normal distribution, and k is the number of iterations.

[0042] Furthermore, the Q value updating method in step 6 is:

[0043]

[0044] Where ν∈(0,1) is the learning factor, γ is the discount factor in the Q learning process, and state Action a ij_k+1 =[w ij (k+1),c ij (k+1),d ij (k+1)],w ij (k+1) is the inertia weight selected by the jth joint of the ith particle at the k+1th iteration, c ij (k+1) and d ij (k+1) is the acceleration coefficient selected by the jth joint of the ith particle at the k+1th iteration, Q ij (s ij_k ,a ij_k ) is the cumulative Q value of the state-action; R ij (s ij_k ,a ij_k ) is the instant reward of the state-action at the current iteration number. The rule is that in the current iteration, if the particle objective function value is lower than the objective function value of the previous iteration, then R ij (s ij_k ,a ij_k ) is a positive number, otherwise it is a negative number; It is the Q value of the optimal particle of the objective function in the current iteration environment.

[0045] Furthermore, the immediate reward R ij (s ij_k ,a ij_k ) function is:

[0046]

[0047] In the formula, f(s ij_k ,a ij_k ) is the objective function in the current iteration.

[0048] Furthermore, the time taken by the longest trajectory in each time segment is taken as the running time used by each joint robot arm in that trajectory segment to plan the trajectory of the entire robot arm.

[0049] Compared with the prior art, the present invention has the following advantages: (1) Compared with the QPSO algorithm, the QLPSO optimization algorithm of the present invention uses the Q learning algorithm to guide the parameter selection of the particle swarm, so that the particle swarm can implement adaptive parameter selection according to the population evolution state, and always select the action with the largest Q value for search, thereby ensuring that the population is always moving in a good direction and achieving the goal optimization; at the same time, the "4-5-4" piecewise polynomial interpolation planning is adopted to ensure the continuous and stable operation of the robot arm, while being able to meet the acceleration constraint; (2) Compared with the IWOAPSO, CDWPSO and IPSO algorithms, the QLPSO optimization algorithm of the present invention has the best time-impact effect. The QLPSO algorithm is more suitable for the time-impact optimal trajectory planning of industrial robot arms than other PSO algorithms.

[0050] The present invention will be further described below in conjunction with the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] Figure 1 The figure is a flow chart of the algorithm of the present invention.

[0052] Figure 2 This is a point cloud diagram of the robot arm workspace of the present invention.

[0053] Figure 3 This is a simulation diagram of the robot arm of the present invention passing through four path points.

[0054] Figure 4 It is a schematic diagram of the angle, velocity and acceleration change trajectory of the simulation joint 1 of the present invention.

[0055] Figure 5 It is a schematic diagram of the angle, velocity and acceleration change trajectory of the simulation joint 2 of the present invention.

[0056] Figure 6 It is a schematic diagram of the angle, velocity and acceleration change trajectory of the simulated joint 3 of the present invention.

[0057] Figure 7 It is a schematic diagram of the angle, velocity and acceleration change trajectory of the simulated joint 4 of the present invention.

[0058] Figure 8 It is a schematic diagram of the angle, velocity and acceleration change trajectory of the simulation joint 5 of the present invention.

[0059] Fig. 9 It is a schematic diagram of the angle, velocity and acceleration change trajectory of the simulated joint 6 of the present invention.

[0060] Fig.10 It is a comparison chart of the algorithm of the present invention and other algorithms. DETAILED DESCRIPTION

[0061] Combination Figure 1 , a polynomial interpolation trajectory planning method for a robotic arm based on QLPSO algorithm (Q learning particle swarm algorithm) comprises the following steps:

[0062] Step S100, giving the starting position, intermediate position and target position of the end trajectory of the robot arm; according to the inverse dynamics of the robot arm, the angular position of each joint is obtained, and the position, velocity, acceleration and jerk constraint conditions of each joint are given;

[0063] Step S200, initializing the particle swarm parameters and Q value of the j-th joint (that is, all the numbers in the Q table are 0);

[0064] Step S300, updating the velocity and position of particle i, and calculating the time-impact objective function value;

[0065] Step S400, calculate the trajectory of the particle, detect whether the position, velocity, acceleration, and jerk of the jth joint meet the constraint conditions; if they do, execute step S500; if not, set the fitness value of the particle to infinity, randomly set the position and velocity of the particle, and go to step S300; the fitness value is the value of the objective function, for example, if the objective function is to optimize time, the fitness value is time;

[0066] Step S500: the individual best P ij and the global optimal G j Compare, if the individual best P ij Less than the global optimal G j , then assign the best individual to the global best G j , complete the update G j , and obtain the minimum time-impact value (i.e. fitness value) of joint j in this iteration;

[0067] Step S600, using objective function (3), updates the Q value corresponding to each particle according to formula (5), and selects the inertia weight w and learning factors c and d corresponding to the maximum Q value according to Table 1 as parameters for the next particle swarm iteration.

[0068] Step S700, if the number of iterations reaches the maximum, go to step S800; if the number of iterations is less than the specified number of iterations, go to step S300;

[0069] Step S800, if all joints are optimized, calculate the maximum time-impact optimization objective function value, give the time required for each trajectory segment, and calculate the trajectory of each joint based on this time, and calculate the trajectory of the end of the robot arm based on forward kinematics; if there are still joints that have not been optimized, go to step S200 and plan the trajectory for the unoptimized joints.

[0070] The specific process of step S100 includes:

[0071] Step S101, construct the relationship between the coefficients of the 4-5-4 polynomial interpolation function of each joint of the robot and the time taken for each trajectory segment

[0072]

[0073] In the formula, t represents the time taken for each trajectory, θ ij (t) represents the polynomial interpolation function of the i-th segment of the j-th joint trajectory of the robot arm, i is the number of segments of the trajectory, i∈{1,2,3}, j∈{1,2,…,N}, N represents the total number of joints, a ik_j Represents the coefficient of the interpolation polynomial, k∈{1,2,3,4,5}; "4-5-4" means that the three-segment polynomials are 4th, 5th and 4th order respectively. In order to constrain the jerk, the speed and acceleration values ​​of single-order polynomials such as 5th and 7th order polynomials are larger than those of 4-5-4th order. In order to achieve both minimum impact and high speed (short time), the 4-5-4 segmented polynomial is the best choice;

[0074] Step S102, obtaining the time t used by each joint for three segments of trajectory 1j ,t 2j and t 3j The starting time of each trajectory is recorded as 0, and the angular position of the starting point, the angular positions of the two intermediate points, and the angular position of the target point of each trajectory are recorded as s 0j 、s 1j 、s 2j and 3j , the position, velocity and acceleration at the starting point of the trajectory are all 0, the velocity and acceleration at the target point of the trajectory are all 0, and the position, velocity, acceleration and jerk at the end of each trajectory are the position, velocity, acceleration and jerk at the starting point of the next trajectory, thereby ensuring the position smoothness of the robot trajectory, and the velocity, acceleration and jerk remain continuous; therefore, it can be obtained that the coefficients in the trajectory (1) and each time t 1j ,t 2j and t 3j The relationship between them is:

[0075]

[0076] Step S103: Determine the relationship between the constraint condition, the objective function and the interpolation function

[0077] The time-impact optimal objective function of the j-th robot arm is taken as

[0078]

[0079] In the formula, λ 1is the weight coefficient of time; 2 is the weight coefficient of the impact; η represents the elastic coefficient, which is used to keep the time and weight at the same order of magnitude and is selected based on known experience; q max 、v max 、a max , jerk max is the maximum constraint set, where q max is the joint angle, v max is the set speed, a max is the set acceleration, jerk max The setting is the acceleration.

[0080] Formulas (1) and (2) in step S101 and step S102 can be used to calculate the Time P ij Room 1j ,t 2j and t 3j It can be expressed as: thus converting the objective function (3) into an objective function about time.

[0081] In order to ensure the time consistency of the robot arm, the time used by the longest trajectory in each period is taken as the running time used by each joint robot arm in this section of the trajectory to plan the trajectory of the entire robot arm, that is, the time of each trajectory of the robot arm is The coefficient of each joint trajectory is calculated according to formula (2), and the trajectory of each joint of the robot arm is obtained by formula (1).

[0082] The specific process of step S200 includes: initializing the QLPSO algorithm; the position of the particle represents the time of each segment of the trajectory of each joint, and the fitness function is the time-impact optimal objective function (i.e., formula (3)); the state space of the Q learning algorithm is the particle position and particle velocity, and the action space is the parameters of the particle swarm algorithm - inertia weight w and learning factors c, d.

[0083] In step S400, the particle swarm algorithm model for the j-th joint trajectory planning is established by combining the objective function (3).

[0084]

[0085] In the formula, w is the inertia weight of the particle, c and d are learning factors, and P ij is the optimal time of the ith particle at the jth joint, G j is the optimal segmentation time for all particles at the jth joint, They represent the time required for each trajectory obtained by the j-th joint particle i (hereinafter referred to as the position, the customary name of the particle group) and the speed of particle i, r 1 ,r 2,r 3 is a random number in the interval [0,1] that satisfies the normal distribution, and k is the number of iterations;

[0086] The relationship between the position and velocity values ​​of the jth joint particle i (the two correspond to the state space of Q learning), and the particle swarm parameters such as inertia weight and learning factor (corresponding to the decision space of Q learning) is shown in Table 1. V max Indicates the maximum speed of the particle.

[0087] Table 1 The state-action correspondence table of the jth joint

[0088]

[0089] In step S500, the ratio of the Euclidean distance between the current particle position and the global best particle to the maximum distance between particles is used as the decision plane, which can be divided into three states, namely {near, medium, far}; the relative performance of the difference between the fitness value of the particle and the global best and worst fitness is used as the state of the target plane, which can be divided into {large fitness value, medium fitness value, small fitness value}. Determine the current state of the particle, and the particle should take action in this state.

[0090] In step S600, the Q value is updated, and the update rule is:

[0091]

[0092] Where ν∈(0,1) is the learning factor, γ is the discount factor in the Q learning process, and state Action a ij_k+1 =[w ij (k+1),c ij (k+1),d ij (k+1)],w ij (k+1) is the inertia weight selected by the jth joint of the ith particle at the k+1th iteration, c ij (k+1) and d ij (k+1) is the acceleration coefficient selected by the jth joint of the ith particle at the k+1th iteration, Q ij (s ij_k ,a ij_k ) is the cumulative Q value of the state-action; R ij (s ij_k ,a ij_k ) is the instant reward of the state-action at the current iteration number. The rule is that in the current iteration, if the particle objective function value is lower than the objective function value of the previous iteration, then R ij (s ij_k ,aij_k ) is a positive number, otherwise it is a negative number; It is the Q value of the optimal particle of the objective function in the current iteration environment.

[0093] Instant Rewards ij (s ij_k ,a ij_k ) function is:

[0094]

[0095] In the formula, f(s ij_k ,a ij_k ) is the objective function in the current iteration.

[0096] This embodiment models and simulates a six-degree-of-freedom robotic arm, and the simulation conditions are as follows:

[0097] (1) The object selected for this design is the PUMA560 six-degree-of-freedom robotic arm. Kinematic modeling is performed based on the given DH parameters and simulation modeling is performed in Matlab. The point cloud diagram of its workspace is shown in the figure below. Figure 2 As shown. The robot arm is planned to pass through four path points and complete the planning task with three trajectories, as shown Figure 3 shown.

[0098] (2) By taking the first and second derivatives of equation (1), we can get the velocities and accelerations of the six joints. The angles, velocities, accelerations, and jerks of the six joints are shown in the following figure: Figures 2 to 7 As shown, it can be seen that the velocity and acceleration curves of the six joints of the robot are smooth and continuous, and the trajectory is smooth and continuous.

[0099] (3) The QLPSO algorithm is used to solve the time-impact of the six-degree-of-freedom robot arm. The time and impact of the three trajectories are as follows:

[0100] Table 2 Experimental results

[0101]

[0102] (4) The QLPSO algorithm is used to solve the time-impact of the six-degree-of-freedom manipulator, and compared with the IWOAPSO, IPSO, and CDWPSO algorithms. The number of particles is selected as 50 and the number of iterations is 100. In view of the randomness of the intelligent algorithm, this embodiment adopts the method of taking the average value of multiple measurements. Each algorithm is tested 20 times and the average value is taken for comparison. The comparison results are shown in Table 3, and their comparison diagram is drawn based on the average fitness value as shown in Table 3. Fig.10As shown in the table, whether it is the optimal fitness value or the average and variance, the QLPSO algorithm has a better fitness value performance. Therefore, compared with the general PSO algorithm, the QLPSO algorithm is more suitable for the time-impact optimal trajectory planning of the robot arm.

[0103] Table 3 Comparison of fitness values

[0104]

Claims

1. A polynomial interpolation trajectory planning method for a robotic arm based on the QLPSO algorithm, characterized in that: include: Step 1: Give the starting position, middle position and target position of the end trajectory of the robot arm, obtain the angular position of each joint according to the inverse dynamics of the robot arm, and give the position, velocity, acceleration and jerk constraint conditions of each joint; Step 2, initialize the particle swarm parameters and Q value of the jth joint; Step 3, establish the time-impact optimal objective function, update the velocity and position of particle i, and calculate the time-impact objective function value; Step 4, calculate the trajectory of the particle, and check whether the position, velocity, acceleration, and jerk of the jth joint meet the constraints; if so, execute step 5; if not, set the fitness value of the particle to infinity, randomly set the position and velocity of the particle, and go to step 3; In step 4, a particle swarm algorithm model for trajectory planning of the jth joint is established. In the formula, w is the inertia weight of the particle, c and d are learning factors, and P ij is the optimal time of the ith particle at the jth joint, G j is the optimal segmentation time for all particles at the jth joint, They represent the time required for each trajectory of the j-th joint particle i and the speed of particle i, respectively. r1, r2, r3 are random numbers that satisfy the normal distribution in the interval [0, 1]. k is the number of iterations. Step 5: Set the individual best P ij and the global optimal G j Compare, if the individual best P ij Less than the global optimal G j , then assign the best individual to the global best G j , complete the update G j , and obtain the minimum time-impact value of joint j in this iteration; Step 6: Update the Q value corresponding to each particle, and select the inertia weight w and learning factors c and d corresponding to the maximum Q value as the parameters for the next particle swarm iteration; The Q value update method in step 6 is: Where ν∈(0,1) is the learning factor, γ is the discount factor in the Q learning process, and the state Action a ij_k+1 =[w ij (k+1),c ij (k+1),d ij (k+1)],w ij (k+1) is the inertia weight selected by the jth joint of the ith particle at the k+1th iteration, c ij (k+1) and d ij (k+1) is the acceleration coefficient selected by the jth joint of the ith particle at the k+1th iteration, Q ij (s ij_k ,a ij_k ) is the cumulative Q value of the state-action; R ij (s ij_k ,a ij_k ) is the instant reward of the state-action at the current iteration number. The rule is that in the current iteration, if the particle objective function value is lower than the objective function value of the previous iteration, then R ij (s ij_k ,a ij_k ) is a positive number, otherwise it is a negative number; is the Q value of the optimal particle of the objective function in the current iteration environment; Step 7: If the number of iterations reaches the maximum, go to step 8; if the number of iterations is less than the specified number of iterations, go to step 3; Step 8: If all joints have been optimized, calculate the maximum time-impact optimization objective function value, give the time required for each trajectory segment, and calculate the trajectory of each joint based on this time, and calculate the end trajectory of the robot arm based on forward kinematics; if there are still joints that have not been optimized, go to step 2 and plan the trajectory for the unoptimized joints.

2. The method according to claim 1, characterized in that The specific process of step 1 is: Step 1.1: Construct the relationship between the coefficients of the 4-5-4 polynomial interpolation function of each joint of the robot and the time taken for each trajectory segment In the formula, t represents the time taken for each trajectory, θ ij (t) represents the polynomial interpolation function of the i-th segment of the j-th joint trajectory of the robot arm, i is the number of segments of the trajectory, i∈{1,2,3}, j∈{1,2,…,N}, N represents the total number of joints, a ik_j represents the coefficient of the interpolation polynomial, k∈{1,2,3,4,5}; Step 1.2, obtain the coefficients in the trajectory and each time t 1j ,t 2j and t 3j The relationship between: a 10_j =s oj ,a 11_j =0,a 12_j =0 24a 14_j t 1j +6a 13_j =6a 23_j a 20_j =s 1j ,a 30_j =s 2j 3. The method according to claim 1, characterized in that The time-impact optimal objective function recorded in step 3 is λ1 is the weight coefficient of time, λ2 is the weight coefficient of impact, η is the elastic coefficient, q max 、v max 、a max , jerk max is the maximum constraint set, where q max is the joint angle, v max is the set speed, a max is the set acceleration, jerk max The setting is the acceleration.

4. The method according to claim 1, characterized in that: Instant Rewards ij (s ij_k ,a ij_k ) function is: In the formula, f(s ij_k ,a ij_k ) is the objective function in the current iteration.

5. The method according to claim 2, characterized in that: The time taken by the longest trajectory in each period is taken as the running time used by each joint robot in that trajectory to plan the trajectory of the entire robot.

Images