Solving the Time-Jerk Optimal Trajectory Planning Problem of a Robot Using Augmented Lagrange Constrained Particle Swarm Optimization

The problem of minimum time-jerk trajectory planning for a robot is discussed in this paper. The optimal objective function is composed of two segments along the trajectory, which are the proportional to the total execution time and the proportional to the integral of the squared jerk (which denotes the derivative of the acceleration).The augmented Lagrange constrained particle swarm optimization (ALCPSO) algorithm, which combines the constrained particle swarm optimization (CPSO) with the augmented Lagrange multiplier (ALM)method, is proposed to optimize the objective function. In this algorithm, falling into a local best value can be avoided because a newparticle swarm is generated per initial procedure, and the best value gained from the former generation is saved and delivered to the next generation during the iterative search procedure to enable the best value to be found more easily and more quickly. Finally, the proposed algorithm is tested on a planar 3-degree-of-freedom (DOF) robot; the simulation results show that the algorithm is effective, offering a solution to the time-jerk optimal trajectory planning problem of a robot under nonlinear constraints.


Introduction
Trajectory planning is the foundation of robot trajectory control.The aim of trajectory planning is to generate trajectory points and plan a smooth trajectory.Optimal trajectory planning has been a focus of robot research studies in recent years because its performance is important for the efficiency and motion stability of a robot.The major steps in optimal trajectory planning are as follows.First, several points along a given geometric path are given.Then, the inverse kinematic solutions of the given trajectory points are obtained.Next, interpolating functions are used to perform trajectory planning in the joint space.Finally, an optimal method is used to optimize the trajectory in order to make it smooth and continuous.Boundary condition constraints, such as those on the velocity, acceleration, jerk, and time, need to be considered in the optimization procedure.
Reasonable trajectory planning can reduce vibration and ensure the long joint life and high path tracking accuracy of a robot.The minimum-time trajectory planning approach was first proposed in the literature to improve the efficiency of robots; the study of such planning has become an important field in trajectory optimization.The purpose of minimumtime trajectory planning is to plan the shortest execution time trajectory according to the given points while the boundary constraints are satisfied.Examples of minimum-time trajectory planning are provided in [1][2][3][4][5].In fact, vibration may appear when a robot is in the moving process or when it is stopped, which will negatively impact the tracking accuracy of the robot; therefore, minimum vibration is an important performance index, and many researchers have studied how to utilize minimum-jerk trajectory planning to reduce vibration [6][7][8].The minimum-time and minimum-jerk trajectory planning studies are often considered together to improve the efficiency and smoothness of the trajectory and reduce the vibration of a robot; the combination of these two methods is referred to as time-jerk optimal trajectory planning.Examples of such trajectory planning are provided in [9][10][11][12][13].

Mathematical Problems in Engineering
In the many aforementioned optimal trajectory planning studies, the main research procedure can be summarized as follows.First, the optimal objective function is transformed into a mathematic expression.Second, relational optimization methods are adopted to optimize the mathematic expression.Finally, the optimal results are compared, and a conclusion is reached considering the requirements.Currently, the major interpolated functions used in joint space include cubic splines and fifth-order B-splines, and the major optimal methods include genetic algorithms, PSO, and the sequential quadratic programming (SQP) strategies.The current literature regarding time-jerk optimal trajectory planning suffers from two problems.First, few studies have been performed on this topic, and further in-depth study is required.Second, because optimal trajectory planning is an optimization problem with constraints, the penalty function method is typically used when considering these issues.In the penalty function method, in certain cases, the nonlinear constrained problem can only converge to the optimal solution when the penalty factor becomes infinitely large (according to the exterior penalty function method) or infinitely small (according to the interior penalty function method).Moreover, a large number of tests need to be performed to determine a reasonable initial penalty factor.Therefore, although some optimal algorithms adopted in the prior literature have been improved, it is difficult to achieve a balance in terms of the quality and efficiency of the optimal solution because of the condition restrictions.
This paper proposes a new approach for time-jerk optimal trajectory planning in order to increase the efficiency and reduce the vibration of the robot.The new approach, named augmented Lagrange constrained particle swarm optimization (ALCPSO), combines the constriction factor method from the basic PSO algorithm with the ALM.In contrast to previous research, a new particle swarm is generated for each initial procedure of the PSO; this process can avoid falling into the local best value, and the best value of the prior generation is saved and delivered to the next generation during the iterative search for procedure to allow the best value to be found more easily and quickly.This approach enables the best value to be obtained even for finite penalty factors.The new approach does not require an accurate dynamic model or a continuously differentiable objective function; thus, it has stronger robustness.
The remainder of this paper is organized as follows.The minimum time-jerk trajectory planning problem is described in Section 2. Application of the technique based on the CPSO and ALM methods to solve the nonlinear constrained optimization problem is described in Section 3. Section 4 presents the simulation results of the ALCPSO algorithm.Section 5 presents the conclusions of this study.

The Minimum Time-Jerk Trajectory Planning Problem
The optimal trajectory planning premise of this paper is that the geometric path has been given.Thus, the joint space position points corresponding to the discrete operative space (Cartesian space) can be obtained according to inverse kinematics; subsequently, the joint trajectory of the robot can also be obtained via the polynomial or the cubic splines method by connecting the relevant position points.The use of cubic splines in trajectory planning is ordinary because the generated trajectories have continuous acceleration values.In addition, in contrast to higher-order polynomials, the cubic splines can overcome excessive oscillations and overshoot problems between any pair of reference points.In this paper, we implement the joint trajectory planning by utilizing the cubic splines described in [10,11,14].The trajectory planning performance depends largely on the objective function.The objective function [9] is defined as follows: min In (1),   +   = 1, through adjusting the values of   and   to allow the total execution time and jerk effect to reach the optimum.Because the total execution time and the proportional to the integral of the squared jerk may have a large difference in quantity grade, the elastic coefficient is introduced to balance the effects of them.The meanings of the symbols in (1) are presented in the Symbols.
Therefore, the optimal trajectory planning problem under constraints can be solved by using (1) to search for the optimum solution of time interval ℎ  .

Solving the Nonlinear Constrained Optimization Problem
References [15][16][17] presented the PSO algorithm, which is a theory that explains the origins of the performance of individuals for bird flocks or fish schools about their collective behavior.The ALM method has numerous advantages, such as its numerical stability and calculated efficiency surpassing the penalty function.

Constrained Particle Swarm Optimization.
In [18,19], the constriction factor  of PSO was introduced to ensure the convergence of the PSO as follows: The particle velocity and position of the PSO are updated as follows: In ( 2), researchers commonly define  =

Augmented Lagrange Multiplier (ALM) Method.
The ALM method is similar to the Lagrange multiplier method; however, in contrast to the latter, the former does not require many tests to determine a reasonable initial penalty factor, and the dynamic penalty factor does not tend to infinity; that is, the former surpasses the latter in terms of numerical stability and computational efficiency [20][21][22].For general inequality constraints, the optimization model can be described as min  () Its ALM method can be defined as follows: where  = ( 1 , . . .,   ) denotes the independent variable, () denotes the objective function,   () ≤ 0 ( = 1, . . ., ) denotes the inequality constraints,   denotes the Lagrange multiplier, and   denotes the penalty factor.  is described as follows: The solution of the original constrained problem (4) can be obtained by solving (5).The Lagrange multiplier iterative formula is expressed as where ] denotes the number of times that the Lagrange multiplier is updated.The penalty factor updated formula is expressed as follows: where   denotes the constraint error accuracy.When ] = 1, the termination criterions are as follows: When ] = 2, . . ., V max , the termination criterions are defined as follows: where  denotes the convergence accuracy and V max denotes the update maximal times.
The ALM method can guarantee that the optimal solution can be found when the penalty factor does not reach infinity, which cannot be guaranteed by either the interior or the exterior penalty function method.

Augmented Lagrange Constrained Particle Swarm Optimization (ALCPSO) Algorithm.
Because the CPSO and ALM enjoy their respective advantages, in this paper, we propose an algorithm named ALCPSO algorithm which combines the CPSO with the ALM in order to make use of their respective advantages to provide a new approach for solving nonlinear constrained optimization problems.Time-jerk optimal trajectory planning is such a problem.The ALCPSO algorithm comprises the following steps.
[ALCPSO2].Initialize  ,max particles with randomly chosen positions and velocities and evaluate the corresponding fitness values at each position.
[ALCPSO3].Check the termination criterions (9).If they are satisfied, the algorithm terminates with the best value  * =  best swarm ; if they are not satisfied, the obtained best value  * is saved.Initialize  ,max particles with randomly chosen positions and velocities and set  = 1.
[ALCPSO4].Utilize the obtained best value of former generation to replace one group of particles.
[ALCPSO5].Apply (3) to update the particles, evaluate the corresponding fitness values at each position, and find the best   -best .
[ALCPSO6].Check the updated number of times of the Lagrange multiplier/penalty factor.If it is satisfied, check the termination criterions (10); if the termination criterions (10) are satisfied, the best value  * =  best swarm is obtained; if they are not satisfied, update the Lagrange multipliers and the penalty factors according to (7) and ( 8) and set V = V+1.If the updated number of times of the Lagrange multiplier/penalty factor is not satisfied, proceed with step [ALCPSO5].
In this algorithm, a new particle swarm is freshly generated in each initial procedure to avoid falling into the local best value, and the best value can be found more easily and quickly because the best value of the prior generation is saved and delivered to the next generation during the iterative search for procedure.

Trajectory Planning Simulation and Discussion
Before optimizing the robot's trajectory, its inverse kinematics should be obtained.In [23,24], the configuration control approach is proposed to solve the inverse kinematics of the redundant robot; this approach ensures that the inverse kinematics is unique and enables the robot to perform cyclic motion, which is an essential requirement for repetitive operations.Moreover, this approach requires little computational time, making it particularly favorable for the real-time control of a redundant robot.In this paper, we use the damped least squares (DLS) of the configuration control approach to determine the inverse kinematics of a redundant robot.Its formulation can be written in the following form: where W is a symmetric positive-definite weighting matrix,  = diag[  ,   ], where   and   are symmetric positive-definite weighting matrices in accordance with the basic task and an additional task, respectively; V is a symmetric positive-definite weighting matrix; is a positive scalar constant; is a symmetric positive-definite feedback gain (constant); is error,  =   − .Next, the position solution of inverse kinematics can be obtained by integrating (11).We use the proposed ALCPSO method to perform a trajectory planning simulation of a robot to verify the accuracy and reliability of the method.The program is compiled in Matlab6 R2014.The test object is a space robot under the condition that the position and pose of the associated carrying platform are under control; it is a 3-DOF planar robot shown in Figure 2. The tested mission is tracking a circular path in plane.In Figure 1,  1 ,  2 , and  3 are joint angles.The robot geometric parameters are defined as  1 = 2,080 mm,  2 = 2,080 mm, and  3 = 430 mm.Because the robot has a redundant degree of freedom, the configuration control approach is utilized to determine its inverse kinematics.The initial position of the end effector is (4000, 0) (mm), with corresponding joint angles of (30 ∘ , −49.03 ∘ , −38.27 ∘ ), and the radius  and the center of the track circle are 400 mm and (3600, 0) (mm), respectively.The following procedure is used to obtain the inverse kinematics of the robot: (1) Set the planned trajectory points in the circular path as a function of time , mean (), which is used to plan the desired trajectory points.
(2) Choose Φ =  1 +  2 as the additional task. ( (4) Put the corresponding values into (11) and perform integration to obtain the position solution of inverse kinematics.
In this paper, () indicates the circular central angle; it is divided into uniform acceleration, uniform velocity, and uniform deceleration for a total of three sections along the counterclockwise rotation.The robot starts from an initial position with the angular acceleration from the value of 120/(1/80 × n) 2∘ / 2 ( denotes the number of planned viapoints).When the robot along the circle moves by 60 ∘ , the uniform acceleration section is completed, and it enters the uniform velocity section, moving along the circle by 240 ∘ ; finally, the robot with value of 120/(1/80 × n) 2∘ / 2 enters the  uniform deceleration section and returns to the initial point, thus completing the entire motion.In this paper, set  = 8.Other values are defined as follows: The values of the plan-points of the trajectory are shown in Table 1.As shown in Table 1, the 1-point coincides with the 9-point when the end effector completes a cyclic motion.Thus, there are 9 plan-points and 8 via-points.The kinematic constraints of the velocity, acceleration, and jerk of the joints are 3 deg/s, 3 deg/s 2 , and 5 deg/s 3 , respectively.
We set the robot's execution time constraint  = 60 s, the initial Langrage multiplier  0 = (0, 0, . . ., 0), the initial penalty factor  0 = (1000, 1000, . . ., 1000), the convergence accuracy  = 0.0001, the constraint error accuracy   = 0.0001, the Lagrange multiplier update maximal times V max = 100, the maximum number of iterations in the CPSO  max = 100, the swarm particle number  ,max = 40, and the elastic coefficient  = 100.The weighting coefficients are adjusted as follows:   is 1, 0.8, 0.5, 0.2, and 0, and the corresponding   can be determined via calculation.We performed the proposed optimal method using the five weighting coefficient values, and the corresponding results are shown in Table 2.The execution time gradually increases when   gradually decreases from 1 to 0. The joint trajectories prior to optimization are shown in Figure 2.
The simulation results when   is 1, 0.5, and 0 are shown in Figures 3-5, respectively.When   is 1, the objective function contains only the minimum-time function.The joints' velocities, accelerations, and jerks shown in Figure 3 are generally larger than those in Figure 2; the joint maximum jerk reaches the upper bounds on the absolute value at the initial moment, which corresponds to a rough trajectory, resulting in a comparatively larger vibration and lower path tracking accuracy of the robot during the moving process.If the robot operates in such a condition for an excessive period of time, its lifespan will be reduced.However, the working efficiency of the robot can increase because the execution time is reduced; this condition suits a robot with strict working efficiency requirement.The execution time in Figure 4 is longer than that in Figure 3; however, the jerks on the whole are smaller than that in Figure 3.In Figure 5, the execution time is equal to the set constraint t, and compared to the results in Figure 2, the joints' velocities, accelerations, and jerks are generally smaller than those in Figure 2, indicating that, under the same execution time condition, it is beneficial for a robot during the moving process compared with that before performing optimization because of the lower velocities, accelerations, and jerks.From analyzing Table 2 and Figures 2-5, we can achieve relevant objects by adjusting elastic and weighting coefficients according to different requirements.
When we combine the CPSO with the interior penalty function method, the constrained problem (4) can be expressed as follows: where  denotes the penalty factor.We set the initial penalty factor  0 = 100000 and the convergence accuracy  = 0.001; other corresponding values are referred to in the aforementioned ALCPSO method.The termination criterions are defined as follows: where V denotes the number of iterations.The simulation results are shown in Table 3 for typical values of   and   .After comparing the corresponding results with those of the ALCPSO method, we can find that the ALCPSO method indeed has some advantages, such as the calculated velocity and some obtained best values.In Table 3, when   is set as 0, the obtained time is shorter than that of the ALCPSO method.This implies that when   is set as 0, the CPSO and interior penalty function methods can be used to perform time-jerk optimal trajectory planning because they can get better time.
When we utilize the SQP method (e.g., the fmincon function of Matlab) described in [9,10], the initial input is the same as the aforementioned ALCPSO method to perform time-jerk optimal trajectory planning.The simulation results are shown in Table 4.After comparing the corresponding results with those of the ALCPSO method, we can find that the ALCPSO method generally surpasses the SQP method in the obtained best values.

Conclusion
This paper proposed an ALCPSO algorithm, which is based on the CPSO and the ALM methods, to realize the timejerk optimal trajectory planning of a robot.In this algorithm, a new particle swarm is generated each time in the initial process in order to avoid falling into a local best value.In this manner, the best value can be found more easily and quickly because the best value of the previous generation is saved and delivered to the next generation during the iterative search for procedure.The optimal objective function considers both the total execution time and the integral of the squared jerk along the entire trajectory and then varies the values of two weighting coefficients and the elastic coefficient to achieve such requirements as rapid execution, a smooth trajectory, or both.The algorithm is implemented on a planar 3-DOF robot.
The simulation results illustrate that the time-jerk optimal trajectory that meets both the kinematics and the execution time constraints can be obtained.The proposed algorithm is developed without performing an experiment; thus, future work will be devoted to performing experiments to verify the present algorithm's effectiveness.

Figure 2 :
Figure 2: The joint trajectories prior to optimization.
describes the global best position.Now, the PSO can be referred to as the constrained particle swarm optimization (CPSO) algorithm, which can realize an effective balance between the global search and the local search.

Table 1 :
Input data for trajectory planning.

Table 2 :
Results of trajectory optimization when utilizing the ALCPSO algorithm.

Table 3 :
Results of the trajectory optimization when combining the CPSO with the interior penalty function method.