Trajectory Planning of an End-effector for Path with Loop

Planning of smooth trajectories has been a very active area of research, hence a number of scientific reports address the problem. The publications concern both the generation of manipulator end-effector trajectories and designing the motion of tools of machine tools. With regard to manipulators, smooth trajectory planning is mostly imposed by the given tasks (assembly, transport of fragile objects, carrying open containers, gluing and painting) as well as through attempts to reduce the wear of manipulator components (decreased driving torque or limited vibration level caused by a resonance frequency). Regarding machine tools, smooth trajectory planning can facilitate a full utilization of the tools' dynamic capabilities, with high-performance maintenance. Boryga and Grabos [1] presented a planning mode of trajectory for a serial-link manipulator with higher-degree polynomials application. The linear acceleration profiles of end-effectors were planned as polynomials of degree 9 th , 7 th and 5 th. Chettibi [2] proposed a method to plan minimum cost movements for robotic manipulators along prescribed geometric paths while taking into account various kinodynamic constraints. Numerical examples using genetic algorithms are presented to illustrate the effectiveness of the proposed approach. Constantinescu and Croft [3] obtained the smooth trajectory of manipulators by considering torque rate and jerk limits in trajectory planning problem which was solved using flexible tolerance method. Elnagar and Hussein [4] studied acceleration-based optimal piecewise trajectory planning. The acceleration and curvature were used to generate the minimum-energy trajectory. Gasparetto and Zanotto [5] and [6] presented a method for smooth and optimal trajectory planning of robot manipulators. They worked out an objective function containing a term proportional to the integral of the squared jerk and a second term proportional to the total execution time. The algorithm has been tested in simulation, yielding good results. Machmudah et al. [7] described a point-to-point of an arm robot motion planning in complex geometrical obstacle utilizing a 6 th degree polynomial as the joint angle path. A planar robot will be utilized to simulate the proposed approach. Perumaal and Jawahar [8] proposed an approach to generate a synchronized jerk-bounded trigonometric S-curve trajectory for a robotic manipulator. The results of simulations show that the proposed approach is able to generate a synchronized, smooth trajectory with minimum execution time. Tian and Collins [9] formulated a constraint by keeping the end-effector trajectory for a manipulator away from the obstacle. A genetic algorithm using a floating point representation is proposed to search for …


INTRODUCTION
Planning of smooth trajectories has been a very active area of research, hence a number of scientific reports address the problem.The publications concern both the generation of manipulator end-effector trajectories and designing the motion of tools of machine tools.With regard to manipulators, smooth trajectory planning is mostly imposed by the given tasks (assembly, transport of fragile objects, carrying open containers, gluing and painting) as well as through attempts to reduce the wear of manipulator components (decreased driving torque or limited vibration level caused by a resonance frequency).Regarding machine tools, smooth trajectory planning can facilitate a full utilization of the tools' dynamic capabilities, with high-performance maintenance.Boryga and Grabos [1] presented a planning mode of trajectory for a seriallink manipulator with higher-degree polynomials application.The linear acceleration profiles of endeffectors were planned as polynomials of degree 9 th , 7 th and 5 th .Chettibi [2] proposed a method to plan minimum cost movements for robotic manipulators along prescribed geometric paths while taking into account various kinodynamic constraints.Numerical examples using genetic algorithms are presented to illustrate the effectiveness of the proposed approach.Constantinescu and Croft [3] obtained the smooth trajectory of manipulators by considering torque rate and jerk limits in trajectory planning problem which was solved using flexible tolerance method.Elnagar and Hussein [4] studied acceleration-based optimal piecewise trajectory planning.The acceleration and curvature were used to generate the minimumenergy trajectory.Gasparetto and Zanotto [5] and [6] presented a method for smooth and optimal trajectory planning of robot manipulators.They worked out an objective function containing a term proportional to the integral of the squared jerk and a second term proportional to the total execution time.The algorithm has been tested in simulation, yielding good results.Machmudah et al. [7] described a point-to-point of an arm robot motion planning in complex geometrical obstacle utilizing a 6 th degree polynomial as the joint angle path.A planar robot will be utilized to simulate the proposed approach.Perumaal and Jawahar [8] proposed an approach to generate a synchronized jerkbounded trigonometric S-curve trajectory for a robotic manipulator.The results of simulations show that the proposed approach is able to generate a synchronized, smooth trajectory with minimum execution time.Tian and Collins [9] formulated a constraint by keeping the end-effector trajectory for a manipulator away from the obstacle.A genetic algorithm using a floating point representation is proposed to search for the optimal trajectory.In the work of Bu et al. [10], the complicated robotic task is decomposed into two kinds of subprocesses.In the free motion process, the kinematic models of the quasi trapezoidal and quasi triangular waveform are proposed with the dynamic limits of maximum velocities, accelerations and jerks of robotic joints.In the constrained motion process, the mathematical presentation of the task paths is extracted from the CAD models of the work pieces.Chen et al. [11] presented a smooth S-curve feed-rate profiling generation algorithm that produces continuous feed-rate, acceleration, and jerk profiles.The proposed algorithm ensures the automated machinery's motion smoothness.Du et al. [12] presented an adaptive NURBS (Non-Uniform Rational B-Spline) interpolator taking into consideration an acceleration-deceleration control.A real-time flexible acceleration-deceleration control scheme was introduced to solve the sudden feed-rate change around corners with large curvature in their method.Emami and Arezoo [13] introduced look-ahead trajectory generation to determine the deceleration stage according to the fast estimated arc length and the reverse interpolation of each curve at every sampling time.Erkorkmaz and Altintas [14] presented a quintic spline trajectory generation algorithm that produces continuous position, velocity, and acceleration profiles.The proposed trajectory generation algorithm has been tested in machining on a three-axis milling machine.Farouki et al. [15] proposed an approach that takes into account limiting constraints such as the maximal of motor torque and power available to each axis of the machine tool in the calculation of the tool path feed-rate.Nam and Yang [16] developed a recursive trajectory generation method to estimate and determine the final deceleration stage according to the distance left to travel, resulting in exact feed-rate trajectory generation through jerk-limited acceleration profiles for the parametric curves.Stori and Wright [17] proposed an algorithm for convex contours, where an offset tool path is modified so that the engagement is kept constant.Zheng et al. [18] proposed two modified nonlinear tracking differentiators to generate the smooth trajectory for industrial mechatronic system from a rough reference, with bounded velocity and acceleration.Kovacic and Balic [19] proposed an autonomous, intelligent programming system for the cutting device controller based on an evolutionary genetics algorithm.Some research reports discuss the planning of motion, in which a tool moves over the path with a so-called sharp corner.Lloyd and Hayward [20] proposed the use of a 5 th degree polynomial to connect the successive motions.To adjust the spatial shape of the transition curve, the authors defined two parameters that can be set for the various values in the interval (0,1).As an example, they presented a simple algorithm for the trajectory generation.Macfarlane and Croft [21] described a method, which uses a concatenation of quintic polynomials to provide a smooth trajectory between two points.The authors presented the experimental results and simulations on an industrial robot.Erkorkmaz et al. [22] put forward a path planning strategy for maintaining a high positioning accuracy in high speed cornering applications.The authors developed two spline-fitting strategies for smoothing sharp corners (the under-corner and over-corner approach).The obtained cornering accuracy was verified in the experiments.To eliminate jerk constraints and remove discontinuities in the acceleration profile, Dong et al. [23] added an acceleration-continuation procedure to the feed-rate optimization algorithm.In order to verify the effectiveness of this approach, the authors presented some application examples and the research results obtained.Tsai et al. [24] proposed an interpolation algorithm ILD (Integrated Look-ahead Dynamics-based) that considers geometric and servo errors simultaneously.The study results indicate that the proposed approach significantly improves tracking and contour accuracy.Tseng et al. [25], aiming at jerk limitation, suggested a parametric interpolator composed of a look-ahead stage and a real-time sampling stage.The algorithm ensures that chord error as well as maximum acceleration and jerk are within the allowable limits.In the work of Wang et al. [26], aiming to minimize contour errors and feed rate fluctuations, a NURBS (Non-Uniform Rational B-Spline) interpolation algorithm was developed that adjusts adaptively to the machine dynamics and kinematics.The simulation test results confirm the effectiveness of the proposed interpolator for machining curved paths.Grabos and Boryga [27] presented an algorithm PCM (Polynomial Cross Method) for planning motion of a manipulator endeffector, whose path was composed of two rectilinear segments.The paper proposes the algorithm of the method and research results in the form of runs of velocities, acceleration and jerk for the prescribed motion trajectory.
The objective of the present study was to develop a trajectory-planning algorithm that could automatically reconcile two essential requirements, i.e. trajectory smoothing and end-effector moving along the sharp corner.The most commonly used method consists in smoothing the corner.This method prevents a transition of the end-effector through the sharp corner.One solution, which simultaneously provides trajectory smoothing and transition through the corner, is using a loop.The path of the end-effector comprises two rectilinear segments, connected with a loop.The end-effector passes through the corner twice and the time difference is, simultaneously, the time of loop tracking.
The work is organized as follows: Section 1 depicting a trajectory planning technique with the 7 th degree polynomial application; Section 2 presents the algorithm for planning rectilinear-arc trajectory with a loop; Section 3 depicts the numerical example, while the simulation results are given in Section 4. The final section includes the concluding remarks.

TRAJECTORY PLANNING WITH 7 th DEGREE POLYNOMIAL USE
The planning of the robot end-effector trajectory can be accomplished by using higher-degree polynomials that facilitate the acceleration profile development.In a study by Boryga and Grabos [1] to build a polynomial form, the properties of the root's multiplicity were utilized.This approach to polynomial form structure necessitates the determination of only one polynomial coefficient, irrespective of its order.Their work also shows the 5 th , 7 th and 9 th degree polynomials describing the acceleration profile; the lowest values of linear and angular jerks were reported for the 7 th degree polynomial.Therefore, in this paper, the acceleration profile on rectilinear segments is described with the 7 th degree polynomial in the form of: where p is coefficient of polynomial 7 th degree, and t f time of motion end.
The polynomials describing the profiles of velocity and displacement are as following: Sample profiles of acceleration, velocity and displacement are shown in Fig. 1.It was assumed that the velocity profile should realize the following conditions: In order to determine the time of movement t f and polynomial coefficient p for a given path Δs it was set to the maximum polynomial value, which defines the profile of the displacement.Then, a system of equations was created, whose solution is the time of movement t f and polynomial coefficient p.

Algorithm Description
Fig. 2 shows the path of the end-effector's movement with an implemented, local coordinate system ξηζ.

Fig. 2. Path of motion with the introduced local coordinate system
The acceleration profile on a straight BM section is described by a 7 th degree polynomial being used only at the first part of the profile, which represents the start-up phase (a solid line in Fig. 1).In the initial point B and final M, the acceleration profile is tangential to the time axis, which eliminates any undesirable jerk effect in these points.Over the MT 1 segment, the endeffector moves at a constant velocity, just like over the arc connecting the points T 1 and T 2 as well as along the T 2 M segment.The acceleration profile on the ME segment was described, like the BM segment, by a 7 th degree polynomial.The second phase of the profile was used and it is the braking phase (broken lines in Fig. 1).The maximum linear velocity on the BM and ME segments is ν max .This velocity is also the endeffector velocity on the loop.

Calculations of Path Geometry
Flowchart in Fig. 3 presents the calculations of path geometry step by step.
Assume the following: the coordinates of the initial point B, the coordinates of the final point E, the coordinates of the point M, and distance from the point M to the center of a circle Δ.It is also possible to assume the radius R, calculate the angle BME and introduce into the algorithm the distance Δ calculated using sine function of half angle BME.Calculate the distance between the tangency points and point M using the Pythagorean theorem.Multiply the calculated distance value by the unit vector a  and add to the coordinates of the point M to get the coordinates of the point T 1 , then multiply by the unit vector d  and add to the coordinates of the point M to finally obtain the coordinates of point T 2 .
The angle β can be determined by adding to the angle π the angle between vectors a  and d  .
Path increments on the rectilinear segments are calculated using the coordinates of the starts and ends of the appropriate segments.

Calculations for the First Rectilinear Segment
For calculating the time of movement on the BM section it is necessary to use a solution of a system of equations and use substitutions t f = 2t BM and Δs = 2 Δs BM .Finally: where ν max is linear velocity in the motion through the loop, which is simultaneously the maximum velocity on the rectilinear segment BM.
As for the motion along the MT 1 segment, the following dependency should be utilized: The motion time on the BT 1 segment is equal to the sum of t BM and t MT 1 times.
The polynomial coefficient for the BM section can be calculated from the dependency: In order to determine the acceleration, velocity and dislocation profiles on the BM section, it is necessary to use dependencies (Eqs.(1) to (3)) and use substitutions t f = 2t BM and p = p BM .

Calculations for the arc
Motion time over the T 1 T 2 arc is calculated from the formula: The angular position for t t t t ≤ ≤ + is obtained from the following dependency considering the initial angular position β b : where β π β b = −( / ). 2 Coordinates of the position vector S A in the motion on the arc in the basic coordinate system xyz are calculated using the transformation matrix T between the local coordinate system ξηζ and basic system xyz: where ( ), ( ), 1 and Velocities V A and accelerations A A are derived as the successive derivatives of the position vector S A .

Calculations for the Second Rectilinear Segment
The motion time on the T 2 M segment is the same as on the MT 1 segment.In order to obtain motion time over the ME segment, the dependency (Eq.( 7)) should be used with the path increment Δs ME substitution.Motion time on the T 2 E segment is equal to the sum of t T M 2 and t ME times.For the calculation of a polynomial coefficient on a segment, use the Eq. ( 9) and substitute path increment Δs ME and time t ME in the formula.A profile of acceleration, velocity, and position on the ME segment can be derived in an analogous manner as with the BM segment, and each of them needs translation in the time by τ:

Final Calculations
Motion time over the BT 1 T 2 E trajectory is calculated, summing up the motion times on the rectilinear segments and the motion time over the arc The linear path on the BT 1 T 2 E trajectory is calculated as the sum of the path on the rectilinear segments and the path over the arc.

NUMERICAL EXAMPLE
The coordinates of the points for the prescribed endeffector trajectory B, M, E, the center of loop O 1 and tangent points T 1 , T 2 are presented in Table 1.m/s 9 .The motion time through the loop arc t A = 0.146 s and the initial angle value was β p = 1.107 rad.The motion times on the rectilinear segments T 2 M and ME were as follows: t T M 2 =0.072 s and t ME = 0.197 s, while the coefficient of the polynomial profile of acceleration, velocity and position for the segment ME was p ME = 2.65844×10 6 m/s 9 .The translation in time was τ = 0.42 s, whereas the total motion time t e = 0.815 s.The total path along the motion trajectory was Δs = 0.152 m, while on the loop Δs A = 0.036 m.Fig. 4 presents the planned path of movement in a coordinate system xyz.The suggested method of trajectory planning is used to simulate a motion of three manipulators.For each of the manipulators, the following are determined:

Table 1. Coordinates of characteristic points of trajectory
• coordinates of the end-effector in the coordinate system xyz, • configuration coordinates (inverse kinematic task), • velocity and acceleration of links.As a result of a system of Eqs. ( 15) to (17) due to the configuration coordinates is: 2 and E l a = 2 2 .
Courses of links' angular displacements for the generated trajectory are presented in Fig. 9.The course of velocities and accelerations of links is presented on Figs. 10 and 11.   21) to ( 23)), and due to configuration coordinates, the below was obtained: where B s s 2 .The courses of angular accelerations in the kinematic pairs of the SCARA manipulator for a planned trajectory are presented in Fig. 13 (coordinate λ 3 s was skipped because it has a constant value).The velocities and link angular accelerations are presented in Figs. 14 and 15.Fig. 16 shows a manipulator with a Cartesian structure.The coordinates of end-effector position in the xyz coordinate system are: where l c 2 is the length of manipulator's link and l c 2 =0.24 m.
As a result of the equation system solution (Eqs.( 27) to (29)), and due to configuration coordinates, the following is obtained: The courses of displacement of links for the kinematic chain studied for a planned trajectory are presented in Fig. 17    Velocities and linear accelerations of the links are presented in Figs.18 and 19.
For all manipulators, the courses of movements and velocities in kinematic pairs are continuous.The courses of accelerations in kinematic pairs are discontinuous.Discontinuity of acceleration results from centripetal acceleration, which occurs during the motion of end-effector on a curved path.

CONCLUSIONS
In order to obtain high accuracy mapping of a trajectory with concomitant full utilization of machine dynamic capabilities, it is necessary to generate smooth trajectories with minimum jerk constraints, acceleration or velocity.This paper proposes a method for planning a rectilinear-arc trajectory in which two opposite requirements meet, i.e. trajectory smoothing and simultaneous passing through a sharp corner.The simulation tests performed allowed for the formulation of the following final remarks: 1.For the reason that a tool passes through the point M twice, the generated motion trajectory can be used both, as a whole -BT 1 T 2 E or in part -BME.
In the latter case, the on-loop motion MT 1 T 2 M can be treated as the tool output.2. At the characteristic points B, M, E of the trajectory, the acceleration profile is tangential to the time axis which causes zero jerk value at these points.3. The algorithm is effective for calculations.The most laborious prove to be the calculations of trajectory geometry, whereas to obtain a profile of position, velocity and acceleration, it is sufficient to calculate a polynomial coefficient and motion time.4. The simulation results indicate the applicability of the proposed method in the analysis stage as well as the design of manipulators and machine tools.Further studies will be needed to determine the effect of loop size on trajectory mapping accuracy, taking into account the deformability of manipulator kinematic chains.

Fig. 1 .
Fig. 1.The profiles of acceleration, velocity and displacement for the acceleration profile described by a 7 th degree polynomial

Fig. 3 .
Fig. 3. Flowchart for calculation of path geometry Let a denote a vector of the start point B and end point M, while d is a vector starting at the point E and ending at the point M. Unit vectors of these vectors

2 =
The assumed distance between the center of loop O 1 and point M was Δ = 0.02 m.A calculated radius arc circle was R = 8.944×10 -3 m, while the angle β = 4.069 rad.The path increments on the rectilinear segments are as follows: ∆s BM = 0.05 m, = 0.0179 m and ∆s ME = 0.03 m.There was an assumed maximum velocity on the rectilinear segments ν max = 0.25 m/s.The motion times over the rectilinear segments BM and MT 1 are t BM = 0.328 s and t MT 1 = 0.072 s, respectively, whereas for the coefficient of the polynomial profile of acceleration, the velocity and position for the segment BM was p

Fig. 4 .
Fig. 4. The assumed end-effector's path in a coordinate system xyz 4 SIMULATION RESULTS Fig. 5a gives the course of end-effector positions along the axes x and y, whereas Fig. 5b is the course of end-effector positions along the prescribed trajectory.Fig. 6 presents the courses of velocity components of the end-effector toward the axes x and y, and the run of the resultant velocity.During the motion through the loop (on the rectilinear segments MT 1 and T 2 M and arc T 1 T 2 ), the resultant velocity does not change and is equal to the preset one ν max = 0.25 m/s, while on the segments BM and ME it changes in accordance with the assumed profile.The components of velocity along axes x and y are obtained through the projection of the resultant velocity vector onto the directions of the coordinate system xyz.The changes in the components on the segments BM and ME are associated with changes in the resultant velocity, while the change in velocity components on the loop are associated with changes in the direction of a constant vector of resultant velocity.Both the runs of the components and the resultant velocity run are continuous functions considering the value.Fig. 7 presents the course of acceleration components in the x, y direction, and the course of resultant acceleration.A profile of resultant acceleration and acceleration components at points B, M, E, are tangential to the time axis and their value is equal to zero.The acceleration value is also equal to zero on the segments MT 1 and T 2 M. The jump in the acceleration value on the arc T 1 T 2 results from the motion on curved path.Tangential acceleration in motion along an arc equals zero, while centripetal

Fig. 5 .Fig. 6 .
Fig. 5.The courses of end-effector position a) on x and y coordinates, b) along the path

Fig. 7 .
Fig. 7.The courses of acceleration on x, y coordinates, and resultant acceleration Fig. 8 presents a manipulator with an anthropomorphic structure.The coordinates of the end-effector's location in the coordinate system xyz are written below: s c l c l c x

Fig. 10 .Fig. 11 .
Fig. 10.The courses of angular velocity of links for an anthropomorphic manipulator

Fig. 12 .
Fig. 12.The SCARA manipulator Coordinates of the end-effector in the coordinate xyz system are defined by the following dependencies: s l c l c x

Fig. 16 .
Fig. 16.The Cartesian manipulator ( λ 1 c coordinate was skipped because it has a constant value).

Fig. 17
Fig. 17.The courses of linear displacement for the Cartesian manipulator a) link 2, b) link 3

.
Fig. 17.The courses of linear displacement for the Cartesian manipulator a) link 2, b) link 3