Time-optimal trajectory optimization of serial robotic manipulator with kinematic and dynamic limits based on improved particle swarm optimization

Effective motion control could achieve the accurate positioning and fast movement of industrial robotics to improve industrial productivity significantly. Time-optimal trajectory optimization (TO) is a great concern in the motion control of robotics, which could improve motion efficiency by providing high-speed and reasonable motion references to motion controllers. In this study, a new general time-optimal TO strategy, the second-order continuous polynomial interpolation function (SCPIF) combined with the particle swarm optimization with cosine-decreasing weight (CDW-PSO) subject to kinematic and dynamic limits, successfully optimizes the movement time of the PUMA 560 serial manipulator. The SCPIF could be used to generate the second-order continuous movement trajectories of six joints in joint space based on the assigned positions and time intervals. The CDW-PSO algorithm could further search for the optimal movement time subject to the limits of the angular displacement, angular velocity, angular acceleration, angular jerk, and joint torque of the manipulator. Two numerical experiments are conducted to illustrate the generalization ability of the CDW-PSO algorithm. The advantage of the CDW would be reflected by comparing with the random weight (RW), the constant weight (CW), and the linearly decreasing weight (LDW), respectively, in each experiment. The experimental results show that the CDW-PSO algorithm would perform better than the RW-PSO, CW-PSO, and LDW-PSO algorithms in terms of the convergence rate and quality of the convergent solution. The proposed time-optimal TO strategy would be applied to all types of manipulators while the optimized trajectories could be incorporated in the motion controllers of the actual manipulators due to considering the kinematic and dynamic limits.


Introduction
The operational stock of industrial robots in factories was in ascending trend over the last few years and achieved 2.7 million in 2019 around the world, an 12% increase compared to 2018 [1]. Robotic manipulators are the most popular industrial robots and are used widely to perform repetitive tasks with high efficiency and precision in tedious, dirty, and dangerous industrial environments. Efficient motion control could improve the execution efficiency and stability of the manipulators significantly, consisting of trajectory planning (TP) and trajectory tracking generally. The TP can shorten the movement time, improve the tracking accuracy, and reduce the wear of the manipulators by providing high-speed and smooth motion references to the motion controllers. The off-line TP is widely adopted when performing repetitive tasks, consisting of path generation (PG) and trajectory optimization (TO). The PG could generate a raw path passing by or avoiding assigned position points in the operating space for specified task requirements. The TO could then convert the raw path to an optimized trajectory to reduce the movement time and avoid manipulators suffering from unnecessary jitter and impact [2].
The conventional TO algorithms only have been used to generate the smooth and continuous movement trajectories in the joint space or the Cartesian space, such as polynomial interpolating function (PIF) [3,4], B-spline curve [5,6], NURBS curve [7,8], and Bezier curve [9,10]. However, the execution efficiency of the manipulators might not be considered in these algorithms. The time-optimal TO algorithms have been researched to find the fastest movement trajectory subject to kinematic or dynamic limits. The dynamic programming (DP) [11,12], quadratic programming (QP) [13,14], and convex optimization (CO) algorithms [2,15] have been used to optimize the movement time. However, the DP and QP algorithms might consume great memory storage and long computation times to find the optimal solution. The approximate conversions of the jerk limits are inevitable in the CO algorithms otherwise the convexity of the CO algorithms is not satisfied. The heuristic search techniques, such as the genetic algorithm [16,17], cuckoo search [18], and particle swarm optimization (PSO) algorithm [5,[19][20][21][22], have been used to directly search for optimal solutions without approximate treatments. It would be more promising to obtain the global optimal solutions by heuristic search techniques due to the population-based strategy [16]. The PSO algorithm has the advantages of fewer parameters, simpler structure, and easier realization than other heuristic algorithms.
However, the above researches based on the PSO algorithms might remain the following shortcomings: 1) many researchers do not consider all kinematic and dynamic limits so that the optimized trajectories based on these algorithms could not be directly applied in the industry [5,[19][20][21][22]; 2) the optimization model is not formulated based on a general condition; 3) the generalization ability of the algorithms is not evaluated since the numerical experiments considering the various number of position points are not conducted [5,[19][20][21][22]; 4) the comparative study of the variants of the PSO algorithm is not provided in some researches [5,19,21,22]. Therefore, a new general time-optimal TO strategy, the second-order continuous PIF (SCPIF) combined with the PSO with cosine-decreasing weight (CDW-PSO), is proposed considering the limits of the angular displacement, angular velocity, angular acceleration, angular jerk, and joint torque of manipulators. The performance of the CDW-PSO algorithm would be evaluated compared with other variants of the PSO algorithm in the previous works. The proposed time-optimal TO strategy could be easily extended to all types of manipulators and implemented efficiently. Moreover, the optimized trajectories could be incorporated in the motion controllers of actual manipulators due to satisfying the kinematic and dynamic limits.
The remainder of this paper is organized as follows. Section 2, the time-optimal TO problem for the PUMA 560 is stated. Section 3, the kinematics and dynamics analysis for the PUMA 560 is presented. Section 4, a new general time-optimal TO strategy is detailed. Section 5, two numerical experiments are conducted. Section 6, the experimental results are discussed. Section 7 concludes this paper.

Problem statement
The PUMA 560 serial manipulator with six degrees of freedom is used to study (Fig. 1). In general, m positions would be assigned in the reachable region of the manipulator in terms of real-world task requirements. The end effector of the manipulator has to pass by the above positions in the Cartesian space. The m angular displacements of each joint at the m assigned positions are then calculated by inverse kinematics and set as the interpolation points in the joint space. Therefore, m − 1 time intervals ( t j1 , t j2 ,...,t j(m−1) ) are obtained from the starting to the end interpolation point of each joint. To reduce the mechanical friction, the movement time of all joints in the jth interval needs to be the same, namely t ji = t i ( j = 1, 2, ..., 6) . The whole movement time of the manipulator corresponds to the addition of these time intervals. The purposes of this study are 1) to generate smooth trajectories with the optimal movement time by the CDW-PSO algorithm and 2) to compare the performance of the CDW-PSO algorithm with other variants of the PSO algorithm in the time-optimal TO problem. First, the major notations used in this paper are listed in Table 1, while the rest of the related parameters and variables are defined below the corresponding equations.

Kinematics and dynamics analysis of PUMA 560
The geometric and inertial parameters of the PUMA 560 are as shown in Table 2 [23]. The simulation model of the PUMA 560 based on the above parameters is built in the MATLAB platform using the Robotics Toolbox. The reachable workspace of the manipulator is then calculated based on the Monte Carlo method [24], as shown in Fig. 2. According to the above kinematic parameters ( i , d i , a i−1 , i−1 ), the corresponding rotation angles of the joints can be calculated using the Kinematics Toolbox in MATLAB when given a fixed position in the Cartesian space [4]. The dynamic equations of the manipulator based on the Lagrangian function are as shown in the formula (1).
where, n is the number of the joints; q i , q (1) i , q (2) i , and i are the angular displacement, angular velocity, angular two functions with the variable iterative number k rand a random value in the range of 0 to 1  i is the transformation matrix of the ith link coordinate system to the (i − 1) th link coordinate system; p is the inertia matrix consisting of the moments and products of inertia; m p is the mass of the pth link; ̄p is the position vector of the center of m p in the pth link coordinate system; T is the acceleration vector due to gravity in the base coordinate system; F fi consists of the Coulomb friction and linear viscous damping; q ir is the relative speed of the two neighbor links of the ith joint ; f c and f vd are the Coulomb force and viscous damping coefficients, respectively.
According to the established PUMA 560 simulation model, the joint torque i in the formula (1) can be directly calculated by the Robotics Toolbox in MATLAB. The limits of the angular displacement , angular velocity v, angular acceleration a, angular jerk J, and joint torque of the PUMA 560 are as shown in Table 3. These limits will be used in the following optimization process.

Time-optimal TO subject to kinematic and dynamic limits
In this section, a new general time-optimal TO strategy namely the SCPIF combined with the CDW-PSO is proposed in the joint space. Let m represent the number of the assigned positions used to generate trajectory. The SCPIF is used to formulate a smooth and second-order continuous trajectory based on m assigned interpolation points and m − 1 time intervals for each joint in the joint space. The CDW-PSO algorithm is used to search for the optimal time intervals subject to the kinematic and dynamic limits of the manipulator. All the joints of the PUMA 560 are considered simultaneously. The detailed descriptions of the procedures will be discussed in Sections 4.1 and 4.2.

SCPIF used in joint space
According to the angular displacements of m interpolation points, the zero angular velocities and the angular accelerations of the starting and the end interpolation points, and the continuities of the angular velocity and the angular acceleration of m − 2 intermediate interpolation points, 4m − 2 constraints must be satisfied to generate a second-order continuous movement trajectory for each joint in the joint space. Therefore, two quartic polynomial functions are built on the time intervals t 1 and t m−1 , respectively, while m − 3 cubic polynomial functions are built on the rest of time intervals ( t 2 , t 3 ,..., t m−2 ), respectively, as shown in the formula (2).
where, a jin is the nth coefficient of the ith polynomial function corresponding to the jth joint; ji (t) is the angular displacement of the ith polynomial function corresponding to the jth joint at time t.
The coefficient matrix consisting of 4m − 2 elements in the formula (2) can be determined by the formula (4) when the matrix is known and non-singular. The detailed solution procedure of the matrix is as shown follows. (2)

SCPIF combined with CDW-PSO used in joint space
In this section, the CDW-PSO algorithm is used to search for the optimal time intervals subject to the kinematic and dynamic limits. The objective function and constraints of the time-optimal TO problem are defined as follows: ji , and ji are the angular displacement, angular velocity, angular acceleration, angular jerk, and joint torque of the jth ( j = 1 , 2,..., 6) joint, respectively, on the ith time interval. j,max , v j,max , a j,max , J j,max , and j,max are the limiting values of the jth joint (Table 3).
The description of the parameters and formulas in the CDW-PSO algorithm is as follows: N and K are the number of the particles and iterations, respectively are the position and speed vectors of the ith particle, respectively, at the k th iteration; t max is the maximum of each time interval; Γ k i is the fitness value of the ith particle at the kth iteration and calculated by t k ; Γ k iL and Γ k G are the historical optimal fitness value of the ith particle and the global optimal fitness (GOF) value of all particles, respectively, during k iterations; k iL and k G are the corresponding position vectors of Γ k iL and Γ k G , respectively; the dynamic inertia weight and learning factors ( c 1 and c 2 ) are expressed in the formulas (6) and (7), respectively. The updating formulas of the speed and position vectors are shown in (8) and (9), respectively.
The dynamic inertia weight has been proven to affect the convergence of the PSO algorithm since it could balance the trade-off between the global and local explorations of the swarm with the iterations [25]. Generally, a large would improve the global exploration ability, while a small one could facilitate the local exploration of the swarm. The value of would be always set to decrease with the iterations [26].
In this study, the CDW is proposed in the formula (6), namely that a cosine function with the variable iterative number k is taken as the decreasing curve of the weight. For the convenience of illustration, is set to 1, 3, 5, and 8, respectively, to generate the curves of the four variants (CDW1, CDW3, CDW5, and CDW8) of the CDW. The initial weight max and final weight min are set to 0.9 and 0.4, respectively. The curves of the CW ( = 0.9 ) and LDW are compared with the above curves, as shown in Fig. 3. Instead of the constant slope of the linear function, the slope of the cosine function in the formula (6) changes with the iterations. Besides, a slower convergence to the final weight could be achieved by setting a smaller . The intersection point of the LDW and CDW curves could be determined by . The iterative number corresponding to the intersection point is denoted as k ip . The CDW curve is above the LDW curve during the period [0, k ip ] otherwise is below the LDW curve. The period could be extended by reducing . Therefore, the CDW could better balance the global and local explorations than the LDW by appropriate adjustment of . The preliminary experiments indicate the good performance would be achieved when = 5 . The theoretical demonstration would be left for a future study.
The procedure of the CDW-PSO algorithm is as follows: Fourth, the coefficients in the formula (2) can be calculated according to the formula (4). Γ i k (i = 1, 2, ..., N) is set to a big large real value ( 100t max ) if the corresponding matrix in the formula (5) is singular. Fifth, the angular displacement, angular velocity, angular acceleration, angular jerk, and joint torque in the trajectories of the joints could be obtained, when the coefficients exist.
if the formula (5) is satisfied, otherwise, Γ i k is set to a big large real value ( 100t max ).
Eighth, the GOF value can be obtained when the iterations terminate.
The stopping criteria for termination of the CDW-PSO algorithm are defined as follows: 1) the GOF of the newest swarm nearly remains unchanged during a given number of iterations ; 2) the algorithm reaches the maximum iterative number.
The flow chart of the CDW-PSO algorithm is as shown in Fig. 4.

Numerical experiments
Two numerical experiments are conducted with m = 4 and m = 10 , respectively, using MATLAB platform in a PC with a processor running at 2.21 GHz and 16 GBytes of RAM. Without loss of generality, four and eight positions are assigned randomly in the reachable region of the PUMA 560 for experiments 1 and 2, respectively. The corresponding angular displacements of the joints are as shown in Table 4.  The purposes of the experiments are 1) to empirically examine the performance of the CW-PSO algorithm in the cases of the various m and 2) to verify the advantages of the CDW compared with the RW, CW, and LDW in different cases. Therefore, three groups of comparisons are performed in each numerical experiment ( m = 4 or m = 10 ), namely the RW-PSO [5] versus CDW-PSO, CW-PSO [22] versus CDW-PSO, and LDW-PSO [21] versus CDW-PSO. The parameters in the algorithms are reasonably set as suggested in [5,21,22], shown in Table 5. For example, when investigating the performance of the CDW-PSO algorithm in comparison with the RW-PSO algorithm, the parameter values in the CDW-PSO algorithm are set as same as those in the RW-PSO algorithm referring to [5], except for . t max is manually set to 5 and as the initial time interval. The initial whole movement time is equal to 5(m − 1) , which has been verified to satisfy the formula (5) and would be further optimized by the above PSO algorithms. The definitions of all parameters have been mentioned in Section 4.2. Finding the best parameter setting would be left for a future study.
For each comparison: 1) 10 swarms are initialized randomly; 2) two competing algorithms are conducted on the same 10 initial swarms; 3) 10 independent runs are conducted per algorithm with the same number of iterations; 4) the average GOF (AGOF) value of 10 runs at each iteration is recorded; 5) the convergent AGOF value is termed as the convergent solution of the algorithm; 6) the corresponding iterative number of the convergent solution is denoted as k con ; 7) the relative standard deviation (RSD) of the GOF values of 10 runs at the k con th iteration is used to measure the robustness of the algorithm.

Comparisons in first experiment ( m = 4)
For the first experiment, three groups of comparisons in terms of the iteration process, namely the RW-PSO versus CDW-PSO, CW-PSO versus CDW-PSO, and LDW-PSO versus CDW-PSO, are presented in Fig. 5, using the AGOF value of 10 runs for each algorithm. The AGOF values for all algorithms would decrease gradually and tend to be stable with the iterations, which would indicate the feasibility of the PSO algorithms solving the time-optimal TO problem. The maximum, minimum, average, and RSD of the GOF  5 5 values and the average CPU time at the k con th iteration for each algorithm are as shown in Table 6.
For the RW-PSO versus CDW-PSO comparison, the RW-PSO and CDW-PSO algorithms would converge to 4.22 and 3.54 at the 44th and 38th iterations, respectively (Fig. 5a). For the RW-PSO and CDW-PSO algorithms, the computational cost in terms of the average CPU time would be about 28s and 23s, respectively. Based on the RW-PSO and CDW-PSO algorithms, the optimized movement time would be reduced by 72% and 77%, respectively, relative to the initial setting time ( 3t max = 15 ). The CDW-PSO algorithm would converge faster and have a smaller convergent solution than the RW-PSO algorithm. For the RW-PSO algorithm, the maximum, minimum, and RSD of the GOF values at the 44th iteration would be 5.31, 3.22, and 0.200, respectively. For the CDW-PSO algorithm, the maximum, minimum, and RSD of the GOF values at the 38th iteration would be 4.83, 2.95, and 0.188, respectively. Two RSDs in both algorithms would be very close, which indicate both algorithms would have similar robustness.
For the CW-PSO versus CDW-PSO comparison, the CW-PSO and CDW-PSO algorithms would converge to 2.62 at the 26th and 22nd iterations, respectively (Fig. 5b). The average CPU time cost of the CW-PSO and CDW-PSO algorithms would be about 112s and 96s, respectively. The movement time of both optimized trajectories would be reduced by about 83%, relative to 15. The CDW-PSO algorithm would converge faster than the CW-PSO algorithm. For the CW-PSO algorithm, the maximum, minimum, and RSD of the GOF values at the 26th iteration would be 2.65, 2.46, and 0.026, respectively. For the CDW-PSO algorithm, the maximum, minimum, and RSD of the GOF values at the 22nd iteration would be 2.66, 2.49, and 0.021, respectively. Both algorithms would have similar robustness based on the above RSDs.
For the LDW-PSO versus CDW-PSO comparison, the LDW-PSO and CDW-PSO algorithms would converge to 2.60 at the 65th and 46th iterations, respectively (Fig. 5c).
The average CPU time cost of the LDW-PSO and CDW-PSO algorithms would be about 111s and 77s, respectively. The movement time of both optimized trajectories would be reduced by 83%, relative to 15. The CDW-PSO algorithm would perform better than the LDW-PSO algorithm in terms of the convergence rate. For the LDW-PSO algorithm, the maximum, minimum, and RSD of the GOF values at the 65th iteration would be 2.78, 2.46, and 0.035, respectively. For the CDW-PSO algorithm, the maximum, minimum, and RSD of the GOF values at the 46th iteration would be 2.71, 2.53, and 0.027, respectively. The above RSDs would also indicate both algorithms have similar robustness.
To further illustrate the effectiveness of the CDW-PSO algorithm, the single run is selected from 10 runs of the CDW-PSO algorithm in the third comparison, which would converge to 2.46 at the 63rd iteration. The final optimized time intervals would be 1.05, 0.53, and 0.88, respectively. The resulting angular displacement, angular velocity, angular acceleration, angular jerk, and joint torque of six joints would be within their limits evidently, as shown in Fig. 6. For the CDW-PSO algorithm, the convergent solutions based on the second and third parameter settings would be very close. Both would be better than the convergent solution based on the first parameter setting. Meanwhile, the CDW-PSO algorithm based on the third parameter setting would converge faster than the second parameter setting. In addition, the CDW-PSO algorithm based on the second parameter setting would have the best robustness.

Comparisons in second experiment ( m = 10)
For the second experiment, the comparison results of the algorithms are as shown in Fig. 7. Similarly, the AGOF values for all algorithms would decrease gradually to the corresponding stable values with the iterations, respectively. The maximum, minimum, average, and RSD of the GOF  Table 6.
For the RW-PSO versus CDW-PSO comparison, the RW-PSO and CDW-PSO algorithms would converge to 17.59 and 15.53 at the 11th and 33th iterations, respectively (Fig. 7a). The average CPU time cost of the RW-PSO and CDW-PSO algorithms would be about 11s and 49s, respectively. Based on the RW-PSO and CDW-PSO algorithms, the optimized movement time would be reduced by 61% and 65%, respectively, relative to the initial setting time ( 9t max = 45 ). Although the RW-PSO algorithm would converge slightly faster than the CDW-PSO algorithm, the convergent solution in the CDW-PSO algorithm would be better than that in the RW-PSO algorithm. For the RW-PSO algorithm, the maximum, minimum, and RSD of the GOF values at the 11th iteration would be 20.59, 14.08, and 0.125, respectively. For the CDW-PSO algorithm, the maximum, minimum, and RSD of the GOF values at the 13th iteration would be 18.87, 13.56, and 0.127, respectively. Both algorithms would have similar robustness based on the close RSDs.
For the CW-PSO versus CDW-PSO comparison, the CW-PSO and CDW-PSO algorithms would converge to 11.20 and 8.77 at the 35th and 25th iterations, respectively (Fig. 7b). For the LDW-PSO versus CDW-PSO comparison, the LDW-PSO and CDW-PSO algorithms would converge to 11.07 and 9.26 at the 162nd and 108th iterations, respectively (Fig. 7c). The average CPU time cost of the LDW-PSO and CDW-PSO algorithms would be about 495s and The single run is selected from 10 runs of the CDW-PSO algorithm in the third comparison, which would converge to 6.01 at the 109th iteration. The final optimized time intervals would be 0.60, 0.96, 0.65, 0.31, 0.42, 0.56, 1.38, 0.29, and 0.84, respectively. The resulting angular displacement, angular velocity, angular acceleration, angular jerk, and joint torque of six joints would be within their limits evidently, as shown in Fig. 8. For the CDW-PSO algorithm, the convergent solution based on the second parameter setting would be better than those on the first and third parameter settings. The CDW-PSO algorithms based on the first and second parameter settings would have better robustness than the third parameter setting. Even though the average CPU time cost of the best CDW-PSO algorithm in the second experiment would be about three times that in the first experiment, it still would be acceptable to the off-line time-optimal TO tasks.

Conclusion
A new general time-optimal TO strategy in the joint space namely the SCPIF combined with the CDW-PSO considering kinematic and dynamic limits, successfully optimizes the movement time of the PUMA 560 robotic manipulator. The SCPIF is used to ensure the second-order continuity of the trajectories of the joints. The CDW-PSO algorithm is used to optimize the movement time compared with the RW-PSO, CW-PSO and LDW-PSO algorithms in two numerical experiments. All variants of the PSO algorithm can well optimize the manually initial setting time in all experiments, which would indicate the feasibility and generalization ability of the PSO algorithms in solving the time-optimal TO problems. Besides, the CDW could better improve the searching ability of the swarm by adjusting than the RW, CW, and LDW. The CDW-PSO algorithm would perform better than the RW-PSO, CW-PSO, and CDW-PSO algorithms in terms of the convergence rate and quality of the convergent solution in most comparisons. The proposed time-optimal TO strategy can be applied to all types of manipulators and be programmed easily. In addition, the optimized trajectories based on the proposed strategy could be incorporated in the motion controllers of the actual manipulators due to considering all kinematic and dynamic limits. Further studies should be as follows: 1) the best setting should be determined by more comprehensive researches to further improve the performance of the CDW-PSO algorithm; 2) more comparative experiments with other nonlinear decreasing weights should be conducted; 3) multi-objective TO problems, such as minimum time-energy and time-jerk, should also be researched for achieving the better motion performance of manipulators.
Funding Information This work is supported in part by the National Natural Science Foundation of China under Grant 61773052.
Availability of data and material The datasets are presented in the present study.
Code availability The algorithm is described in the present study.

Declarations
Ethics approval The article follows the guidelines of the Committee on Publication Ethics (COPE) and involves no studies on human or animal subjects.

Conflicts of interest
The authors have no conflicts of interest to declare that are relevant to the content of this article.