Creating Through Points in Linear Function with Parabolic Blends Path by Optimization Method

The linear segment with parabolic blend (LSPB) trajectory deviates from the specified waypoints. It is restricted to that the acceleration must be sufficiently high. In this work, it is proposed to engage modified LSPB trajectory with particle swarm optimization (PSO) so as to create through points on the trajectory. The assumption of normal LSPB method that parabolic part is centered in time around waypoints is replaced by proposed coefficients for calculating the time duration of the linear part. These coefficients are functions of velocities between through points. The velocities are obtained by PSO so as to force the LSPB trajectory passing exactly through the specified path points. Also, relations for velocity correction and exact velocity solution are derived. Simulation results show that the engagement of modified LSPB trajectory with PSO to work well on the tested cases. This proposed method is very simple which can be used for on-line path planning, and not necessarily to use high acceleration magnitude.


Introduction
Straight line segments is the output from motion planners.This path has velocity discontinuity at waypoints.To achieve an efficient execution on the robot, blends are added to ensure a smooth transition between segments [1].A common trajectory for industrial robots is the linear segment with parabolic blend (LSPB) [2,3].The LSPB needs only the initial and final joint angles, traveling time, and either angular acceleration or angular velocity.Numerous methods and algorithms have been established which generate such trajectories with velocity, acceleration, and jerk limitations [4,5].
In LSPB, it is required to use high acceleration's magnitude to be quite close to the desired waypoint.Time-optimal solution for time durations of LSPB so as to satisfy the constraints velocity and acceleration is presented in [6].This requires calculation a factor for velocity reduction of two neighboring linear segments in order to prevent overlapping of blend phases.Yet, this ethomd can lead to very slow trajectories [7].Rymansaib et al. used a series of time-delayed third-order exponential function to generate an approximation to the trapezoidal velocity profile of LSPB [8].The motion duration is affected by high blending percentage as well as the corresponding accuracy at waypoints.A new technique "envelope of tangents planning" had been developed so as to generate trajectory that reaches waypoints in specified moments of time [9].This is achieved by assigning positions and tangential velocities that the joints must have when the end-effector passes through each of those waypoints.Additionally, the online trajectory generation algorithm was combined with the Reflexxes libraries to make the trajectory reaches waypoint with continuous acceleration and jerk [10,11].
New path planning algorithm was developed for the control of an XY -motion stage using LSPB and minimum time trajectory for an aerosol printing system [12].The algorithm calculates blend times and constant velocity based on given trajectory conditions.But large error in speed appeared for acute angle trajectory.This problem can be considered as multi-segment trajectories without stopping at waypoints.Weber et al. used the tool CorDe (Corner Drive with Defined Speed) to visualize the characteristic of the distance to the corner depending on the speed deviation for transition between path segments [13].But the absolute maximum acceleration is difficult to obtain as a realistic value which is the sole characteristic value.This results in a loss on performance for many transition points.
Classic optimization approaches suffer from many drawbacks, such as high time complexity in high dimensions and trapping in local minima, which make them inefficient in practice.Modern or nontraditional optimization methods such as genetic algorithm and swarm intelligence are widely used in path planning problems [14][15][16].Particle swarm optimization (PSO) is simple and fast because it has few parameters to be adjusted [17].
In the above mentioned works, the suggested methods have limitations and use the same basic equations of LSPB.The LSPB trajectory still deviates from the specified waypoints.In this work, the LSPB trajectory is modified and engaged with PSO.The novelty in this work is to modify LSPB trajectory using two coefficients for calculating the time duration of the linear part in LSPB trajectory.These coefficients are functions of velocities between through points.The velocities are obtained by PSO to force the LSPB trajectory passing exactly through the specified path points.Also, relations for velocity correction and exact velocity solution are derived.

Multisegment Linear Path with Blends
For the case in which there are many waypoints, linear segments with parabolic blends are considered.In LSPB, the segment is divided into three parts: parabolic, linear, and parabolic; respectively (Fig. 1).
Considering the path waypoints which are j, k, and 1.The time duration for blend region of point k is tk.The time duration for linear part between points j and k is tjk.The time duration of the segment which connects points j and k is tdjk.According to Fig. 1, the linear velocity between points j and k is jk θ & , the acceleration at point k is , and the path point position is k θ .The blend times tk is computed from the given: path points k θ , desired time durations tdjk, and the magnitude of acceleration k θ & & .For interior path points, this follows simply the equations [2]: where SGN ( ) returns the sign of the value in the brake.
In the first and last segments there is an entire blend region at one end of the segment.For the first segment [2]: Likewise, for the last segment (the one connecting points n-1 and n), which leads to the solution: In these linear-parabolic-blend splines, when acceleration capability is sufficiently high, the paths will come quite close to the desired waypoint.The manipulator must come to a complete stop if it is desired to pass exactly through a waypoint.The term "through point", as it was mentioned in [2], will be used in the next sections to specify a path point through which the manipulator is forced to pass exactly.

Problem Definition
In the previous section, the LSPB trajectory is constrained to the following [6]: 1-The velocity at the first and last through point must be zero.2-The velocity and acceleration of the trajectory must be: The limitations of this algorithm are: 1-Sufficiently large acceleration is required so as to obtain linear portion in the segment.2-The manipulator's velocity must be zero so as to pass into waypoints.3-The system should generate two pseudo points so as to make the manipulator passes exactly through a path point without stopping.4-The parabolic portion is assumed to be centered equally in time about through point.This later assumption makes the apex of parabolic part to be shifted away from through point as shown in Fig. 2.
For interior path point (equation ( 1)), the apex of the parabolic portion is not equally centered in time about waypoint.This can be easily proved using the kinematic equations for LSPB trajectory as presented below.From equation ( 6), now two coefficients (αj & αk) are obtained for calculating the time duration of the linear portion of the trajectory.These coefficients are obtained from the LSPB kinematic equations.Two coefficients are used to calculate the time's duration of the parabolic portions in the segment's time duration (tdjk).
Given through points (joint's angles), time durations, and assuming accelerations for all through points, the modified LSPB equations are as below: Modified mid segments By using these coefficients, the apex of parabolic portion is positioned exactly under the through point.Although the apex of parabolic part is now positioned exactly under through point, but it stills not passing through it.This is because of replacing a linear part region of the segment by parabolic part.This can be proved as below.

Proof 2:
The position for linear part with constant velocity during a time t is Substituting equations (10) and (11) into equation (12) gives: which means this displacement of parabolic part is less than that of linear part for same time duration.
This problem can be solved by increasing the velocity between through points in presence of acceleration limits.The problem of finding the velocities' value can be solved by optimization methods such as PSO.

Velocity Correction
In the above modified LSPB, the initial velocities are obtained from equation (1).But from proof 2, there is an error due to the parabolic part (equation ( 13)).Therefore, these velocities can be corrected by adding the change of velocity (equation (14.b)) based on the error of the parabolic parts as: In the above equation, half of the blend durations (tj and tk) are accepted as an approximated value.This corrected velocity is used as initial velocity to begin the optimization process.

Exact Solution of Velocity
The LSPB offers exact solution of velocity for path segment when there is acceleration from zero velocity to linear velocity and deceleration to zero velocity.For example, the first and second segments (ij and jk) or the second and third segments (jk and kl) have such situation (Fig. 1).The second and third segments are considered to derive a general solution.Let ts, tl, and te are times' duration for first parabolic part, linear part, and last parabolic part for the path from through point j to through point k.The total displacement is the summation of these three parts as below: Solving the above quadratic equation (equation (15.a)) for the exact velocity to obtain: In the above derivation, absolute values are used to prevent sign confusion.The sign of velocity is introduced after solving the quadratic equation.

Steps of Proposed Method
The proposed modified LSPB is presented here which overcomes the limitations of the normal LSPB.It provides logic sequence for computer programming to generate through points: Step 1: Calculate velocities and accelerations according to equation (1).
Step 2: Use equation ( 16) to solve for exact velocity if there is a change in velocity direction between two following through points.
Step 3: Obtain the time durations for the trajectory using equations (7-9).
Step 4: Apply velocity correction using equation ( 14) to all calculated velocities at step 1 except that obtained at step 2.
Step 5: Use these velocities as initial solution for the optimization process.
This algorithm can be easily converted into computer program to perform optimization process (Fig. 3).The velocities are obtained from optimization process so as to force the LSPB trajectory passing exactly through specified path points.

Particle Swarm Optimization
Recently, modern or nontraditional methods of optimization are widely used for solving different optimization problems.Edward and Kennedy formulated PSO in 1995.The process was inspired by the social behavior of animals, such as bird flocking or fish schooling.Each particle has two characteristics which are: a position and a velocity.It must remember the best position in terms of objective function value.The particles adjust their individual positions and velocities by sharing the received information of the best position [18]: where v is particle's velocity, w is inertia weight, P is particle position or variable, r1 and r2 are uniform distributed random numbers, P local best is best local postion, P global best is best global position, i is particle index, j is dimension of variable, Γ1 is individual learning rate, Γ2 is group learning rate.
Premature and local optimum convergence are the disadvantages of traditional PSO.Modifications to PSO were applied so as not to skip the optimal solution [19].These modifications are happened either on basic components or on swarms itself.Modifications on basic components of PSO are: inertia weight [20,21], velocity constriction [22,23], and velocity clamping [24].Five basic benchmark optimization functions had been tested by using fifteen different inertia weight variants in [25].They concluded that chaotic inertia weight improves the accuracy of the solution.Modifications on the swarms itself are: insertions new swarms [26,27], mutation [28], and swarm initiation [29].These modifications can increase the search diversity.Applying multiple modifications on the basic components of PSO and swarms was suggested as future work in [30].An improved chaotic PSO algorithm based on adaptive inertia weight (AIWCPSO) was proposed in [31].Initially, the positions and velocities of the population are generated by using chaotic mapping.The inertia weight is adjusted according to the values of: iterative number, aggregation degree factor, and improved evolution speed parameter.AIWCPSO algorithm with chaotic swarm initiation and swarm injection were used in [32].This combines modifications to basic components of PSO and swarms.
In this work, improved chaotic PSO algorithm based on adaptive inertia weight (AIWCPSO) is used.Also velocity constriction factor, λ, is included (equation ( 18)).

Steps of AIWCPSO Algorithm
Step 1: Cubic mapping (equation ( 19)) is used to generate double or triple swarm size as chaotic initialization.The cubic mapping is described as following: where p0 is a random number substituted as initial value.These swarms are tested so as to select those of best fitness as initial solution to particle position.Then this initial solution is mapped to the search space range.
Step 2: Chaotic initialization is also applied to generate N initial velocities by cubic mapping (equation ( 19)).
Step 3: The inertia weight is updated by the equations below for a single particle: where k is the current iteration value.wmax and wmin are maximum and minimum values of inertia weight; respectively.iter is the current iteration number and itermax is the maximum number of iteration.The value α and β has the range [0,1].esfi is the improved evolution speed parameter of particle i (i =1, 2,..., N), adf is the aggregation degree factor of swarm, F (pbesti k ) is the best fitness value of particle i at the k th iteration, Fbest is the best fitness obtained from all particles, Favg is the mean fitness of all particles in the swarm at the same iteration.
Step 4: The variance (σ) is calculated for the population's fitness (equation ( 21)).When variance is less than threshold value and the optimal fitness of current iteration is worse than the desired fitness value, chaotic disturbance is applied.
where F( xi) is fitness values of particle i.
At first, the trajectories of normal LSPB (equations (1-3)) and modified LSPB (equations (7-9)) are compared (Fig. 4).The normal LSPB trajectory is shifted while the modified is equally spaced about through points.A comparison of linear part velocities is presented in Table 1 for 50 degrees/second 2 acceleration between normal LSPB, modified LSPB with velocity correction, and optimum modified LSPB (section 4.3).It is clear that modified LSPB with velocity correction is better choice as initial velocity to begin the optimization process.A range between 0.9 to 1.2 of these later calculated velocities is used as initial value for the optimization.This will reduce the number of iterations to reach optimum solution.
PSO is used to generate optimal linear velocities for the linear parts.In fact, that increasing the velocities of the linear portions will compensate the error resulted from inserting parabolic region in the path between two neighboring through points.Figure 5 shows comparison between the normal and modified LSPB trajectories using PSO method.The normal LSPB trajectory deviates from the through points, while the engagement of modified LSPB with PSO (proposed method in section 4.3) passes into them.In the normal LSPB method, it is restricted to use acceleration's value higher than 40 degrees/second 2 [2]. Figure 6 shows results of proposed method for the two acceleration values using PSO.The results of comparison are presented in Table 2  The error at through points is reduced for the cases of proposed method.The error at point 2 is zero because of using the exact velocity solution (equation ( 16)).The error at point 3 is 0.2916•10 - 12 , 0.3588•10 -12 , and 0.0107•10 -12 degree for accelerations 30, 50, and 70 degrees/second 2 ; respectively.Optimal velocities' value are presented in Table 3.The run execution time is found to be about 5.4% of the first parabolic portion (0.2929 seconds) for the 50 degrees/second 2 acceleration.This value is enough to generate trajectory of desired path by using the algorithm of the proposed method for on-line path planning.
Point to point position trajectory is simulated for two-link planar robot manipulator (Fig. 7).It has revolute joints.The masses m1 and m2 are assumed to be concentrated at the distal end of the links which have the lengths l1 and l2; respectively.The robot starts at point (2.95, 0.05) and passes through points (2.45, 0.05) and (2.7, 0.30).Then it stops at point (2.95, 0.05).All coordinates are in meters.The time durations are: 4, 3, 3 seconds.The robot dynamic equation and controller are taken from early published paper [33].The desired through points in joint space are obtained by solving the inverse kinematics equations [2].The trajectory for the two joints is generated by using the proposed method (Fig. 8).The Cartesian trajectory of the two-link manipulator is presented in Fig. 9.The errors in the Cartesian space are: 0.5722•10 -5 , 0.4180•10 -5 , 0.4365•10 -5 , and 0.2551•10 -5 in meters for these through points.The error at the starting point is not zero because of the used controller which is learning on-line.The error at through points is presented in Table 4.
Finally, a comparison of results is presented for path of acute angles from [12].The pattern contains 14 line segments, and beginning at the origin.The segments form angles starting at 125° and decrease linearly to 5°.The peak velocity error shown in Fig. 10 increases as angle decreases.For example, a complete change in direction or a 0° angle results in a 100% error according to the method of [12]; while the error is no more 21% by using the proposed method in this work.In LSPB, the trajectory deviates slightly from the straight path.This deviation is increased as the angle formed by two segments is decreased (Fig. 11).

Conclusions
The normal LSPB is restricted to that the acceleration must be sufficiently high.In this work, modified LSPB is engaged with PSO for generating smooth valid trajectories that passes through specified path points while satisfying velocity and acceleration constraints of physical mechanical robot manipulator.
Increasing the velocity of linear portions compensates the error due to inserting parabolic part.Velocity correction is used to obtain close values to the optimal solution.This reduces the number of iterations to obtain the optimum solution.Also, exact solution of velocity can be used for LSPB path segment when there is acceleration from zero velocity to linear velocity and deceleration to zero velocity.
Simulation results show that the proposed method to work well on the tested cases.The error at through points is almost zero.Advantages of the modified LSPB algorithm are: through points can be created, easily engaged with optimization method, very simple which can be used for online path planning, and not necessarily to use high acceleration's magnitude.The proposed method in this work has no chance to fail to create through points within a reasonable number of iterations.
) P o s i t i o n ( d e g r e e )