A new method for smooth trajectory planning of robot manipulators

Transcription

1 Mechanism and Machine Theory 42 (2007) Mechanism and Machine Theory A new method for smooth trajectory lanning of robot maniulators A. Gasaretto *, V. Zanotto Diartimento di Ingegneria Elettrica, Gestionale e Meccanica, Università degli Studi di Udine, Via delle Scienze, Udine, Italy Received 25 Aril 2005; received in revised form 3 Aril 2006; acceted 6 Aril 2006 Available online 5 June 2006 Abstract A new method for smooth trajectory lanning of robot maniulators is described in this aer. In order to ensure that the resulting trajectory is smooth enough, an objective function containing a term roortional to the integral of the squared jerk (defined as the derivative of the acceleration) along the trajectory is considered. Moreover, a second term, roortional to the total execution time, is added to the exression of the objective function. In this way it is not necessary to define the total execution time before running the algorithm. Fifth-order B-slines are then used to comose the overall trajectory. With resect to other trajectory otimization techniques, the roosed method enables one to set kinematic constraints on the robot motion, exressed as uer bounds on the absolute values of velocity, acceleration and jerk. The algorithm has been tested in simulation yielding good results, which have also been comared with those rovided by another imortant trajectory lanning technique. Ó 2006 Elsevier Ltd. All rights reserved. Keywords: Trajectory lanning; Smoothness; Jerk; Robot maniulators; Otimization; B-slines 1. Introduction A fundamental roblem in robotics consists in trajectory lanning, which may be defined in this way: find a temoral motion law along a given geometric ath, such as certain requirements set on the trajectory roerties are fulfilled. Trajectory lanning is devoted to generate the reference inuts for the control system of the maniulator, so as to be able to execute the motion. The geometric ath, the kinematic and dynamic constraints are the inuts of the trajectory lanning algorithm, whereas the trajectory of the joints (or of the end effector), exressed as a time sequence of osition, velocity and acceleration values, is the outut. The geometric ath is usually defined in the oerating sace, i.e. with reference to the end effector of the robot, since both the task to erform and the obstacles to avoid can be more naturally described in this sace. * Corresonding author. Tel.: ; fax: address: gasaretto@uniud.it (A. Gasaretto) X/$ - see front matter Ó 2006 Elsevier Ltd. All rights reserved. doi: /j.mechmachtheory

2 456 A. Gasaretto, V. Zanotto / Mechanism and Machine Theory 42 (2007) On the other side, trajectory lanning is normally carried out in the joint sace of the robot, after a kinematic inversion of the given geometric ath. The joint trajectories are then obtained by means of interolating functions which meet the imosed kinematic and dynamic constraints. Planning a trajectory in the joint sace rather than in the oerating sace has a major advantage, namely that the control system acts on the maniulator joints rather than on the end effector, so it would be easier to adjust the trajectory according to the design requirements if working in the joint sace. Moreover, trajectory lanning in the joint sace would allow to avoid the roblems arising with kinematic singularities and maniulator redundancy. The main disadvantage of lanning the trajectory in the joint sace is that, given the lanned trajectory in the joint sace, the motion actually erformed by the robot end effector is not easily foreseeable, due to the non-linearities introduced when transforming the trajectories of the joints into the trajectories of the end effector through direct kinematics. Aart from the articular strategy adoted, the motion laws generated by the trajectory lanner must fulfill the constraints set a riori on the maximum values of the generalized joint torques, and must be such that no mechanical resonance mode is excited. This can be achieved by forcing the trajectory lanner to generate smooth trajectories, i.e. trajectories with good continuity features: in articular, it would be desirable to obtain trajectories with continuous joint accelerations, so that the absolute value of the jerk (i.e. of the derivative of the acceleration) kees bounded. Limiting the jerk is very imortant, because high jerk values can wear out the robot structure, and heavily excite its resonance frequencies. Vibrations induced by nonsmooth trajectories can damage the robot actuators, and introduce large errors while the robot is erforming tasks such as trajectory tracking. Moreover, low-jerk trajectories can be executed more raidly and accurately. Almost every technique found in the scientific literature on the trajectory lanning roblem is based on the otimization of some arameter or some objective function. The most significant otimality criteria are: (1) minimum execution time, (2) minimum energy (or actuator effort), (3) minimum jerk. Besides the aforementioned aroaches, some hybrid otimality criteria have also been roosed (e.g. timeenergy otimal trajectory lanning) Minimum-time trajectory lanning Minimum-time algorithms were the first trajectory lanning techniques roosed in the scientific literature because they were tightly linked to the need of increasing the roductivity in the industrial sector. The first interesting methods of this kind [1,2] are develoed in the osition-velocity hase lane. The main idea is to use the curvilinear abscissa h of the ath as a arameter, in order to exress the dynamic equation of the maniulator in a arametric form. An alternative aroach is roosed in [3,4], where dynamic rogramming techniques are emloyed. However, the aforementioned techniques generate trajectories with discontinuous values of accelerations and joint torques, because the dynamic models used for trajectory comutation assume the robot members as erfectly rigid and neglect the actuator dynamics. This leads to two undesired effects: first, the real actuators of the robot cannot generate discontinuous torques, which causes the joint motion to be always delayed with resect to the reference trajectory. The accuracy in trajectory following is then greatly reduced and the so-called chatter henomenon may eventually occur, consisting in high frequency vibrations that can damage the maniulator structure. Second, the time-otimal control requires saturation of at least one robot actuator at any time instant, so the controller cannot correct the tracking errors arising from disturbances or modeling errors. In order to overcome such roblems, other aroaches [5,6] imose some limits on the actuator jerks, defined as the variation rates of the joint torques. In this way, the generated trajectory will not be exactly time-otimal, albeit close to the otimality value; however, the generated trajectories can be effectively imlemented and more advanced control strategies can be alied.

3 In order to generate trajectories with continuous accelerations, a common strategy is to use smooth trajectories, such as the sline functions, that have been extensively emloyed in the scientific literature on both kinematic and dynamic trajectory lanning. The first formalization of the roblem of finding the otimal curve interolating a sequence of nodes in the joint sace, obtained through kinematic inversion of a discrete set of oints reresenting osition and orientation of the reference frame linked to the end effector of the robot, can be found in [7]. In[8] the same algorithm is resented, but the trajectories are exressed by means of cubic B-slines. In [9] the otimization algorithm roosed in [7] is modified, so that it can deal also with dynamic constraints and with objective functions of a more general tye; however, the reorted simulations still refer to the trajectory lanning roblem with kinematic constraints. In [10] a method is roosed, to comute oint-to-oint minimum-time trajectories using uniform B-slines. The dynamic model of the maniulator is considered, and the roblem of semi-infinite constraints resulting from the algorithm is byassed by samling a certain number of oints. All the algorithms based on the otimization of cubic slines mentioned in the foregoing yield a local otimal solution, which may or may not be the global otimum. So, some algorithms have been develoed, aimed at finding a global otimal solution for the trajectory lanning roblem for robotic maniulators. Examles are [11], where the roosed algorithm is based on the so-called interval analysis, and [12] where a hybrid technique using genetic algorithms is ut forward Minimum-energy trajectory lanning Planning the robot trajectory using energetic criteria rovides several advantages. On one hand, it yields smooth trajectories resulting easier to track and reducing the stresses to the actuators and to the maniulator structure. Moreover, saving energy may be desirable in several alications, such as those with a limited caacity of the energy source (e.g. robots for satial or submarine exloration). Examles of energy otimal trajectory lanning are rovided in [13 15]. In[13,14] oint-to-oint trajectories with minimal energy are considered, with uer bounds on the amlitude of the control signals and the joint velocities. In [15] a trajectory with motion constraints set on the end effector is otimized. The cost function is given by the time integral of the squared joint torques, and the trajectories are exressed by means of cubic B-slines. The resulting motion minimizes the actuators effort. Paer [16] deals with time-energy otimal trajectories, i.e. the cost function has a term linked to the execution time and a term exressing the energy sent. The trade-off between the two needs can be adjusted by changing the resective weights. Other examles of time-energy otimal trajectories are given in [17,18]. In[17] a cubic sline trajectory is considered, subject to kinematic constraints on the maximum values of velocity, acceleration and jerk. In [18] a oint-to-oint trajectory arametrized by means of cubic B-slines is considered Minimum-jerk trajectory lanning A. Gasaretto, V. Zanotto / Mechanism and Machine Theory 42 (2007) The imortance of generating trajectories that do not require abrut torque variations has already been remarked. In [5,6] this has been obtained by setting uer bounds on the rate of torque variations, but in this way the third-order dynamics of the maniulator must be comuted. An indirect method to get the same results is to set an uer limit on the jerk, defined as the time derivative of the acceleration of the maniulator joints. The ositive effects induced by a jerk minimization are: errors during trajectory tracking are reduced, stresses to the actuators and to the structure of the maniulator are reduced, excitation of resonance frequencies of the robot is limited, a very coordinated and natural robot motion is yielded. The last effect aears very interesting; indeed, some studies [19,20] show that the movement of a human arm seem to satisfy an otimization criterion linked to the rate of variation of the joint acceleration. So, it may

4 458 A. Gasaretto, V. Zanotto / Mechanism and Machine Theory 42 (2007) be inferred that minimum-jerk trajectories are an examle of otimization according to hysical criteria imitating the caacity of natural coordination of a human being. In [21] the solutions for two oint-to-oint minimum-jerk trajectories are analytically obtained; the otimization, obtained through the Pontryagin rincile, concerns two objective functions, namely the time integral of the squared jerk and the maximum absolute value of the jerk (minimax aroach). An interesting aroach is described in [22], where the interolation is done through trigonometric slines, that ensure the continuity of the jerk. The interolation through trigonometric slines features a sound locality roerty, i.e. if the value of some node in the inut sequence is changed, only the two slines having that node as their common oint should be comuted again, not the whole trajectory: this roerty can be usefully exloited for instance to byass an obstacle in real time. In [23,24] the so-called interval analysis is used to develo an algorithm that globally minimizes the maximum absolute value of the jerk along a trajectory whose execution time is set a riori: hence, an aroach of the tye minimax is used. The trajectories are exressed by means of cubic slines and the intervals between the via-oints are comuted so that the lowest ossible jerk eak is roduced. A comarison between the simulation results of the algorithm roosed in [23] and those of the method described in [22] is drawn. An imortant remark must be done at the end of this literature overview: in the case of trajectory lanning along a given ath, all jerk-minimization algorithms that could be found consider an execution time set a riori and do not accet any kinematic constraint. On the other hand, the trajectory lanning technique roosed in this aer does not require the execution time to be imosed; moreover, kinematic constraints are taken into account when generating the otimal trajectory. With resect to minimum-jerk otimization techniques that could be found in the scientific literature, the method described in this work enables one to define kinematic constraint on the robot motion before running the algorithm. Such constraints are exressed as uer bounds on the absolute values of velocity, acceleration and jerk for all robot joints, so that any hysical limitation of the real maniulator can be taken into account when lanning its trajectory. The aer is organized as follows: In Section 2 the otimization roblem is formulated, namely the objective function to minimize is defined. Fifth-order B-slines are then chosen to define the trajectory which is to solve the otimization roblem: the objective function and the kinematic constraints are rewritten accordingly. Quintic slines have been roosed for trajectory generation in some scientific aers (see for instance [25 28]). Section 3 describes the whole algorithm, which is based on an iterative minimization rocedure, and Section 4 describes its execution. Section 5 shows the results of some simulation that have been carried out using the same inut data as another imortant algorithm found in the literature, in order to be able to comare the results. 2. The otimization roblem As stated in the foregoing, it is desirable that trajectories generated by lanning algorithms feature sufficient smoothness roerties, so as to avoid to excite mechanical resonance modes of the maniulator structure. This can be achieved by if the acceleration of the lanned trajectory is a continuous function, so that the value of the jerk (its derivative) kees bounded. Limitation of the value of the jerk is an indirect way to bound the variation rate of the joint torques, without any need to keeing into account the dynamic model of the maniulator in the lanning algorithm. The trajectory lanning technique described in this aer assumes that the geometric ath, generated a riori by an uer-level ath lanner, is given in the form of a sequence of via-oints in the oerating sace of the robot, which reresent successive ositions and orientations of the end effector of the maniulator; obstacle avoidance roblems are assumed as already solved by the uer-level ath laner and will not be considered. The roosed technique generates an otimal trajectory for the robot, by associating temoral information to the re-lanned geometric ath, according to an otimality criterion based on the minimization of some objective function that will be defined below, and by taking into account any kinematic constraints, exressed by means of uer bounds on velocity, acceleration and jerk. There are several algorithms where the value of the jerk aears in the objective function. For instance, Simon and Isik [22] minimize the integral of the squared jerk, while Piazzi and Visioli [23] use a minimax aroach to minimize the maximum value of the jerk along the trajectory.

5 A. Gasaretto, V. Zanotto / Mechanism and Machine Theory 42 (2007) However, these techniques consider the execution time as known (and set a riori); moreover, it is not ossible to set any kind of kinematic constraint on the trajectory, because they are not taken into account. On the contrary, the algorithm resented in this aer generates an otimal trajectory such as it is not required to set the execution time a riori, kinematic constraints on the resulting trajectory (i.e. uer bounds on the values of velocity, acceleration and jerk of the robot joints) can be defined before running the algorithm. The objective function adoted in roosed technique is given by the sum of two terms having oosite effects: the first term is roortional to the execution time, the second term is roortional to the integral of the squared jerk. Of course, reducing the value of the first term of the objective function will lead to trajectories featuring large values of the kinematic quantities (velocity, acceleration and jerk), while reducing the second term will lead to a smoother trajectory. The trade-off between these two tendencies can be erformed by suitably adjusting the weights of the two terms of the objective function: a larger weight of the jerk term will lead to smoother but slower trajectories, while a larger weight of the time term will lead to faster but less smooth trajectories. Hence, the otimal trajectory lanning roblem can be formulated as 8 find: min k T N v 1 P Z P h i þ k N tf J ðq v jðtþþ 2 dt j¼1 0 >< subject to: ð1þ j_q j ðtþj 6 VC j ; j ¼ 1;...; N j q j ðtþj 6 WC j ; j ¼ 1;...; N >: jq v jðtþj 6 JC j ; j ¼ 1;...; N The meaning of the symbols aearing in (1) is exlained in Table 1. By solving the otimization roblem (1), the vector of the time intervals h i between any air of consecutive via-oints is comuted. Of course, the trajectory solving the above defined otimization roblem must also meet the interolation conditions for all the via-oints, as well as the initial and final conditions for velocity, acceleration and jerk. Table 1 Meaning of the symbols aearing in Eq. (1) Symbol Meaning N Number of robot joints V Number of via-oints h i Time interval between two via-oints _q j ðtþ Velocity of the jth joint q j ðtþ Acceleration of the jth joint qv j ðtþ Jerk of the jth joint k T Weight of the term roortional to the execution time k J Weight of the term roortional to the jerk t f Total execution time of the trajectory VC j Velocity limit for the jth joint (symmetrical) WC j Acceleration limit for the jth joint (symmetrical) JC j Jerk limit for the jth joint (symmetrical)

6 460 A. Gasaretto, V. Zanotto / Mechanism and Machine Theory 42 (2007) Definition of the trajectory by means of fifth-order B-slines A technique to solve the otimization roblem (1), based on fifth-order B-slines, will be described in the following. We briefly recall here that a B-sline of degree and order k = + 1 is a sline curve B (t) exressed in the so-called B-form, namely as a linear combination of olynomials N i, (t) of degree, called base or blending functions, weighted by some coefficients Q i named control oints. The curve is built on a sequence of nodes t i and the base functions are defined recursively by means of the De Boor formula [29] with B ðtþ ¼ Xnþ1 Q i N i; ðtþ 8 N i; ðtþ ¼ t t i N i; 1 ðtþþ t iþþ1 t >< N iþ1; 1 ðtþ t iþ t i t iþþ1 t iþ1 N i;0 ¼ 1; for t i 6 t < t iþ1 >: 0; elsewhere ð2þ ð3þ where k = + 1 and m = n Table 2 contains the definition of the symbols used in (2) and (3). The sequence of nodes is said to be clamed when the values at the extremities have multilicity k, i.e. are reeated k times, so that the control oints at both ends of the trajectory coincide with the initial and final via-oints, resectively Imosing the interolation and boundary conditions In order to obtain trajectories with no jerk at both ends, two virtual oints have been introduced at the second and the second-last osition of the sequence. Given a sequence, obtained through a kinematic inversion, of v + 2 via-oints in the joint sace of dimension N, m +1=(v +2)+2 = v + 12 nodes are necessary to get a comlete interolation. Being m =(v +12) 1=n + +1=n + 6, the number of control oints needed is given by: n +1=v + 6. So, enough degrees of freedom are available to ensure that the trajectory asses through the via-oints, and to imose the initial and final conditions for velocity, acceleration and jerk. Some remarks about the derivative of a curve reresented through a B-sline are now necessary, in order to roerly define the resolution rocedure of the interolation roblem. The derivative of a B-sline of degree is a B-sline of degree 1. Considering a single joint for sake of simlicity, let CPQ i (i =1,...,n + 1) reresent the control oints of the trajectory; Uq i,(i =1,...,m + 1) the nodes and CPV i (i =1,...,n + 1) the control oints of the trajectory derivative (i.e. the velocity). Table 2 Definition of the symbols used in Eqs. (2) and (3) Symbol k B (t) N i, (t) Q i n + 1 t i m + 1 Definition Degree of the B-sline Order of the B-sline B-sline of degree Base function of degree Control oint of the B-sline Number of control oints Nodes Number of nodes

7 A. Gasaretto, V. Zanotto / Mechanism and Machine Theory 42 (2007) The trajectory is exressed in the form qðtþ ¼ Xnþ1 CPQ i N i; ðtþ ð4þ and the following holds: CPV i ¼ ðcpq Uq iþþ1 Uq iþ1 CPQ i Þ; iþ1 i ¼ 1;...; n ð5þ The velocity can be defined on the sequence of nodes Uv, built by discarding the values at the extremities of Uq, thus obtaining a clamed sequence comatible with the degree 1 of the velocity vðtþ ¼ Xn CPV i N i; 1 ðtþ The boundary conditions on the velocity can then be imosed CPV 1 ¼ ðcpq Uq þ2 Uq 2 CPQ 1 Þ¼ CPQ 2 Uq þ2 Uq 1 þ CPQ 2 Uq þ2 Uq 2 ¼ v in 2 CPV n ¼ ðcpq Uq nþþ1 Uq nþ1 CPQ n Þ nþ1 ¼ CPQ Uq nþþ1 Uq n þ CPQ nþ1 Uq nþþ1 Uq nþ1 ¼ v fin nþ1 ð6þ ð7þ Eq. (7) are to be added to the interolation conditions of the via-oints. The same rocedure is carried out in order to comute the control oints for the acceleration. Let CPA i (i =1,...,n 1) be the control oints of the acceleration curve. The following holds: 1 CPA i ¼ ðcpv iþ1 CPV i Þ; i ¼ 1;...; n 1 ð8þ Uv iþ Uv iþ1 The acceleration can then be defined on the sequence of nodes Ua, built by discarding the values at the extremities of Uv, thus obtaining a clamed sequence comatible with the degree 2 of the acceleration aðtþ ¼ Xn 1 CPA i N i; 2 ðtþ The boundary conditions on the accelerations can then be imosed 1 CPA 1 ¼ ðcpv 2 CPV 1 Þ¼a in Uv þ1 Uv 2 CPA n 1 ¼ 1 Uv nþ 1 Uv n ðcpv n CPV n 1 Þ¼a fin Using the (7), Eqs. (10) can be rewritten as ð9þ ð10þ CPA 1 ¼ k 1 CPQ 1 þ k 2 CPQ 2 þ k 3 CPQ 3 ¼ a in ð11þ CPA n 1 ¼ k n 1 CPQ n 1 þ k n CPQ n þ k nþ1 CPQ nþ1 ¼ a fin Eqs. (11) are also to be added to the interolation conditions of the via-oints. Again, the same rocedure is carried out in order to comute the control oints for the jerk. Let CPJ i (i =1,...,n 2) be the control oints of the jerk curve, where 2 CPJ i ¼ ðcpa iþ1 CPA i Þ; i ¼ 1;...; n 2 ð12þ Ua iþ 1 Ua iþ1

8 462 A. Gasaretto, V. Zanotto / Mechanism and Machine Theory 42 (2007) The jerk can then be defined on the sequence of nodes Uj, built by discarding the values at the extremities of Ua, thus obtaining a clamed sequence comatible with the degree 3 of the jerk jðtþ ¼ Xn 2 CPJ i N i; 3 ðtþ The boundary conditions on the accelerations can then be imosed CPJ 1 ¼ 2 ðcpa 2 CPA 1 Þ¼j Ua Ua in 2 CPJ n 2 ¼ 2 Ua nþ 3 Ua n 1 ðcpa n 1 CPA n 2 Þ¼j fin Using the (7) and the (11), Eqs. (14) can be rewritten as ð13þ ð14þ CPJ 1 ¼ g 1 CPQ 1 þ g 2 CPQ 2 þ g 3 CPQ 3 þ g 4 CPQ 4 ¼ j in ð15þ CPJ n 2 ¼ g n 2 CPQ n 2 þ g n 1 CPQ n 1 þ g n CPQ n þ g nþ1 CPQ nþ1 ¼ j fin Eqs. (15) are also to be added to the interolation conditions of the via-oints. The general form of the linear system solving the interolation roblem using fifth-degree B-slines can now be obtained. Let VPR j,i and CPQ j,i be resectively the ith via-oint and control oint of the trajectory of the jth robot joint, the linear system (16) can be written for each joint j (j =1,...,N): 8 CPQ Uq þ2 Uq j;1 þ CPQ 2 Uq þ2 Uq j;2 ¼ v j;in 2 k 1 CPQ j;1 þ k 2 CPQ j;2 þ k 3 CPQ j;3 ¼ a j;in g 1 CPQ j;1 þ g 2 CPQ j;2 þ g 3 CPQ j;3 þ g 4 CPQ j;4 ¼ j j;in >< vþ6 P N ;k ðs i ÞCPQ j;k ¼ VPR j;i with i ¼ 1;...; v k¼1 ð16þ g n 2 CPQ j;n 2 þ g n 1 CPQ j;n 1 þ g n CPQ j;n þ g nþ1 CPQ j;nþ1 ¼ j j;fin k n 1 CPQ j;n 1 þ k n CPQ j;n þ k nþ1 CPQ j;nþ1 ¼ a j;fin >: CPQ Uq nþþ1 Uq j;n þ CPQ nþ1 Uq nþþ1 Uq j;nþ1 ¼ v j;fin nþ1 The N systems of the tye (16) can be gathered into a comact form of the tye AU j ¼ B j 8j ¼ 1;...; N ð17þ where the unknowns of the linear system (17) are the values of the control oints 2 3 U j ¼ 6 4 CPQ j;1. CPQ j;vþ j ¼ 1;...; N ð18þ The coefficient matrix A, which results non-singular and band-diagonal, is unique for all joints, because it deends only on the intervals h i = t i+1 t i between each air of via-oints. It must be noticed that A is not rovided in analytical form, but requires the evaluation of the base functions at the time instants corresonding to the via-oints; hence, a rocedure for comutation of the base functions must be included in the trajectory otimization algorithm Exression of the kinematic constraints The exression of the kinematic constraints for a B-sline can be conveniently exressed recalling that any B-sline kees within the convex hull associated to its control oints.

9 A. Gasaretto, V. Zanotto / Mechanism and Machine Theory 42 (2007) This can roven thus: let f ðtþ ¼ P nþ1 Q i N i;k ðtþ be a B-sline (reresenting osition, or velocity, or acceleration, or jerk) that is both uer- and lower-bounded (L inf 6 f(t) 6 L su ); the following relations hold: L inf 6 Xnþ1 Q i N i;k ðtþ 6 L su L inf Xnþ1 N i;k ðtþ 6 Xnþ1 Q i N i;k ðtþ 6 L su Xnþ1 N i;k ðtþ X nþ1 L inf N i;k ðtþ 6 Xnþ1 Q i N i;k ðtþ 6 Xnþ1 L su N i;k ðtþ ð19þ A sufficient condition for the validity of the (19) is given by L inf 6 Q i 6 L su 8i ¼ 1;...; n þ 1 ð20þ Hence, it has been roven that, in order to set uer- and lower-bounds on a B-sline (which can reresent osition, velocity, acceleration or jerk), it suffices to ut some constraints on its control oints. The advantage in doing that lies in the fact that the semi-infinite kinematic constraints of the tye ðl inf 6 f ðtþ 6 L su Þ ð21þ imosed on the curves f(t) of velocity, acceleration and jerk, which would have to hold for any value of the time t, are turned into a set of constraints (20) that must hold for just a finite number of oints, namely the control oints of the B-slines. This greatly reduces the comutational comlexity of the roblem. Namely, on the basis of the convex hull roerty and of the exressions for the control oints of the derivative of a B-sline comuted in the foregoing, the kinematic constraints can be easily formulated CPV j;k 6 VCj ; k ¼ 1;...; n CPA j;k 6 WCj ; k ¼ 1;...; n 1 ð22þ 6 JCj ; k ¼ 1;...; n 2 CPJ j;k where VC j,wc j,jc j reresent the (symmetric) constraints for velocity, acceleration and jerk of the jth joint, resectively. It should be remarked that the terms on the left-hand side are linked to the values of the control oints CPQ j,k through the (5), (8) and (12) which in turn are linked to the otimization arameters h i through the (17). It should be remarked again that (20) reresents not a necessary, but just a sufficient condition for the validity of (21). So, if the finite set (20) is used to define the kinematic constraints of the trajectory, instead of the original ones (21), it should be ket in mind that a conservative aroach is emloyed because not all the ossibilities assumed in defining the kinematic model are fully exloited. In other words, the trajectories obtained by running the algorithm may result slower than those theoretically feasible. Another tye of kinematic constraint is due to the fact that the otimization arameters h i are lower bound, because any interval between a air of consecutive via-oints cannot be run at infinite velocity. Hence, for each interval the relation q j;iþ1 q j;i 6 VC j ð23þ h i must hold, so that the h i must be such as jq j;iþ1 q j;i j h i > w i ¼ max > 0 ð24þ j¼1;...;n VC j In other words, the execution time of every trajectory segment should be comuted, corresonding to the maximum allowed velocity for each joint; the largest execution time, corresonding to the most restrictive condition, is then to be icked.

10 464 A. Gasaretto, V. Zanotto / Mechanism and Machine Theory 42 (2007) Finally, it remains to define the exression of the objective function (1) in order to be able to imlement the algorithm and to run the simulations. The generic form of the objective function for a trajectory defined in terms of B-slines is given by vþ1 X X N FOBJ ¼ k T h i þ k J j¼1 Z tf 0 X n 2 k¼1 CPJ j;k N 3;k ðtþ! 2 dt ð25þ However, the exression of the integral of the squared jerk aearing in (25) turns out to be very long and comlex. In order to comute the term R t f P 2 n 2 0 k¼1 CPJ j;k N 3;k ðtþ dt, a first aroach would be to use a numerical integration rocedure (e.g. the quadl function in MatLab), thus avoiding any form of analytical integration. On the other hand, in order to get an exact instead of an aroximate result, the analytical integration of the exression squared jerk has to be comuted. This requires to determine first the analytical exression of the base functions N 3,k of degree 2, by means of the recursive De Boor formula (2). The final exression of the integral, obtained through a very lengthy and tedious analytical integration, would take many ages and will not be reorted here. It must be noticed that running the algorithm using the exact exression of the squared jerk instead of a numerical integration rocedure yields much better results in terms of execution time. 4. Running the algorithm The trajectory lanning algorithm described in the revious section can then be run in whatever simulation environment, by solving the minimization roblem formulated in (25) through some dedicated software routine (usually, based on iteration). The stes for running the algorithm are summarized below: starting from a given ath in the oerating sace, a kinematic inversion is alied, so as to get a sequence of via-oints in the joint sace, the numeric values of kinematic constraints are set, on the basis of the structural constraints of the maniulator, and of design considerations, Table 3 Values of the via-oint of the trajectory Joint Via-oints [deg] Virtual Virtual Table 4 Values of the kinematic limits of the joints Joint Kinematic constraints Velocity [deg/s] Acceleration [deg/s 2 ] Jerk [deg/s 3 ]

11 A. Gasaretto, V. Zanotto / Mechanism and Machine Theory 42 (2007) a suitable initial solution to start the iterative otimization algorithm is chosen, the exression of the objective function (25) and of the kinematic constraints are inut to the otimization algorithm, the solution of the otimization roblem is then obtained using Sequential Quadratic Programming techniques (for instance, the fmincon function of MatLab TM ). As for all iterative routines, a crucial oint is the choice of a suitable initial solution, because a wrong choice would affect the execution time and even the final result of the algorithm. Fig. 1. Planned trajectory for a six joints robot.

12 466 A. Gasaretto, V. Zanotto / Mechanism and Machine Theory 42 (2007) A suitable initial solution can be found by considering the lower bound of the intervals h i making a time scaling of the trajectory. Let H lb be the vector of the lower bound of the v + 1 otimization variables h i comuted using (24). By substitution of H lb into (17) the N joint trajectories corresonding to H lb can be comuted. If a scaled time variable s is defined as: s = Kt; the value of K can be set from the values of the control oints corresonding to the curves of velocity, acceleration and jerk. Namely, if a set of coefficient f K 1 K 2 K 3 g is defined as Fig. 2. Velocity of the six joints for the lanned trajectory.

13 K 1 ¼ max j K 2 ¼ max j K 3 ¼ max j max k max k max k A. Gasaretto, V. Zanotto / Mechanism and Machine Theory 42 (2007) jcpv j;k j VC j jcpa j;k j WC j jcpj j;k j JC j ; j ¼ 1;...; N; k ¼ 1;...; n ; j ¼ 1;...; N; k ¼ 1;...; n 1 ; j ¼ 1;...; N; k ¼ 1;...; n 2 ð26þ Fig. 3. Acceleration of the six joints for the lanned trajectory.

14 468 A. Gasaretto, V. Zanotto / Mechanism and Machine Theory 42 (2007) the scaling factor K can then be obtained by setting K ¼ max 1 K 1 K 1=2 2 K 1=3 3 ð27þ so that a suitable initial solution is H 0 ¼ KH lb ð28þ Fig. 4. Jerk of the six joints for the lanned trajectory.

15 5. Results of the algorithm and comarison A. Gasaretto, V. Zanotto / Mechanism and Machine Theory 42 (2007) The algorithm described in this aer has been tested in simulation for a 6-joint robot, and a comarison with the results obtained from the algorithm roosed by those Authors has been made. The inut data are reorted in the following tables. Table 3 contains the values of the via-oint of the trajectory, while Table 4 contains the values of the kinematic limits of the joints. As already stated in the foregoing, the technique roosed by Simon and Isik [22] takes the total execution time as given, while the algorithm described in this aer oututs the total execution time as a result; its value deending on the weights k T and k J aearing in the exression of the cost function (1). So, in order to be able to comare the results yielded by the two algorithms, the values of the weights k T and k J have been adjusted so that the execution time of the two algorithms would be the same (namely, 9.1 s). The results of the simulations are reorted in Figs. 1 4, showing the trajectories of the six joints and their derivatives (velocity, acceleration and jerk, resectively). Table 5 reorts the maximum kinematic values resulting from the otimization rocedure; such values are comared with those yielded by the technique roosed by Simon and Isik [22]. It can be noticed that the results yielded by the technique described in this aer are well comarable with those rovided by the technique [22] with resect to the maximum values of acceleration and jerk. Table 6 reorts the mean values of velocity, acceleration and jerk for all joints, comared again with those found in [22]. In this case, the technique resented in this work yields lower mean values for acceleration and jerk of almost all the robot joints. The results reorted above show the effectiveness of the algorithm roosed in erforming an otimization of the trajectories. The comarison with one of the best trajectory lanning algorithm found in the scientific literature roves that the kinematic values of the generated trajectories are good and ket under control. Table 5 Maximum kinematic values resulting from the otimization rocedure Algorithm Joint Gasaretto Zanotto V max A max J max Simon Isik [22] V max A max J max Table 6 Mean kinematic values resulting from the otimization rocedure Algorithm Joint Gasaretto Zanotto V med A med J med Simon Isik [22] V med A med J med

16 470 A. Gasaretto, V. Zanotto / Mechanism and Machine Theory 42 (2007) Conclusions A new methodology for otimal trajectory lanning of robotic maniulators has been described in this aer. The technique is based on minimization of an objective function that takes into account both the execution time and the integral of the squared jerk along the whole trajectory. Unlike many other trajectory lanning techniques, the roosed method does not take the execution time as given a riori and takes into account kinematic constraints on the robot motion, exressed as uer bounds on the absolute values of velocity, acceleration and jerk for all robot joints. The algorithm has been alied considering to comose the trajectory by means of fifth-order B-slines connecting airs of consecutive via-oints. The exressions for the objective function and the kinematic constraints have been formulated in this case. Finally, the algorithm has been run in simulation, taking as inut data those found in the work by Simon and Isik [22]. Comarison of the results with those rovided in [22] has shown that the effectiveness of the algorithm is effective in erforming an otimal trajectory lanning. Future work will be devoted to aly the resent technique to trajectories of different kind (like cubic slines, trigonometric slines, etc.), so as to evaluate the alicability the algorithm and its results. Acknowledgements This work was suorted in art by the Italian Ministry of Instruction and University, in the framework of the PRIN 2004 roject. The authors would like to thank Mr. Claudio Mantovani for his imortant contribution to this work. References [1] J.E. Bobrow, S. Dubowsky, J.S. Gibson, Time-otimal control of robotic maniulators along secified aths, International Journal of Robotics Research 4 (3) (1985) [2] K.G. Shin, N.D. McKay, Minimum-time control of robotic maniulators with geometric ath constraints, IEEE Transactions on Automatic Control 30 (6) (1985) [3] K.G. Shin, N.D. McKay, A dynamic rogramming aroach to trajectory lanning of robotic maniulators, IEEE Transactions on Automatic Control 31 (6) (1986) [4] T. Balkan, A dynamic rogramming aroach to otimal control of robotic maniulators, Mechanics Research Communications 25 (2) (1998) [5] D. Constantinescu, E.A. Croft, Smooth and time-otimal trajectory lanning for industrial maniulators along secified aths, Journal of Robotic Systems 17 (5) (2000) [6] D. Costantinescu, Smooth time otimal trajectory lanning for industrial maniulators, PhD thesis, Available from: <htt:// batman.mech.ubc.ca/~ial/ublication/>, The University of British Columbia, [7] C.S. Lin, P.R. Chang, J.Y.S. Luh, Formulation and otimization of cubic olynomial joint trajectories for industrial robots, IEEE Transactions on Automatic Control 28 (12) (1983) [8] C.H. Wang, J.G. Horng, Constrained minimum-time ath lanning for robot maniulators via virtual knots of the cubic B-Sline functions, IEEE Transactions on Automatic Control 35 (35) (1990) [9] E. Jamhour, P.J. André, Planning smooth trajectories along arametric aths, Mathematics and Comuters in Simulation 41 (1996) [10] Y.C. Chen, Solving robot trajectory lanning roblems with uniform cubic B-slines, Otimal Control Alications & Methods 12 (1991) [11] A. Piazzi, A. Visioli, Global minimum-time trajectory lanning of mechanical maniulators using interval analysis, International Journal of Control 71 (4) (1998) [12] C. Guarino Lo Bianco, A. Piazzi, A semi-infinite otimization aroach to otimal sline trajectory lanning of mechanical maniulators, in: M.A. Goberna, M.A. Lóez (Eds.), Semi-infinite Programming: Recent Advances, Sringer-Verlag, Berlin, 2001, [13] O. Von Stryk, M. Schlemmer, Otimal control of the industrial robot Manutec r3, in: R. Bulirsch, D. Kraft (Eds.), Comutational Otimal Control, International Series of Numerical Mathematics, vol. 115, Basel, Birkhäuser, 1994, [14] G. Field, Y. Steanenko, Iterative dynamic rogramming: an aroach to minimum energy trajectory lanning for robotic maniulators, in: Proceedings of the IEEE International Conference on Robotics and Automation, 1996, [15] B.J. Martin, J.E. Bobrow, Minimum effort motions for oen chain maniulators with task-deendent end-effector constraints, International Journal of Robotics Research 18 (2) (1999)

17 A. Gasaretto, V. Zanotto / Mechanism and Machine Theory 42 (2007) [16] Z. Shiller, Time-energy otimal control of articulated systems with geometric ath constraints, Transactions of the ASME Journal of Dynamic Systems, Measurement, and Control 118 (1996) [17] S.F.P. Saramago, V. Steffen Jr., Otimization of the trajectory lanning of robot maniulators tacking into account the dynamics of the system, Mechanism and Machine Theory 33 (7) (1998) [18] S.F.P. Saramago, V. Steffen Jr., Otimal trajectory lanning of robot maniulators in the resence of moving obstacles, Mechanism and Machine Theory 35 (2000) [19] J.E. Bobrow, B. Martin, G. Sohl, E.C. Wang, F.C. Park, J. Kim, Otimal robot motion for hysical criteria, Journal of Robotic Systems 18 (12) (2001) [20] D. Simon, The alication of neural networks to otimal robot trajectory lanning, Robotics and Autonomous Systems 11 (1993) [21] K.J. Kyriakooulos, G.N. Saridis, Minimum jerk ath generation, in: Proceedings of the 1988 IEEE International Conference on Robotics and Automation, Philadelhia, 1988, [22] D. Simon, C. Isik, A trigonometric trajectory generator for robotic arms, International Journal of Control 57 (3) (1993) [23] A. Piazzi, A. Visioli, Global minimum-jerk trajectory lanning of robot maniulators, IEEE Transactions on Industrial Electronics 47 (1) (2000) [24] A. Piazzi, A. Visioli, An interval algorithm for minimum-jerk trajectory lanning of robot maniulators, in: Proceedings of the 36th Conference on Decision and Control, San Diego, California, December 1997, [25] K. Erkorkmaz, Y. Altintas, High seed CNC system design. Part I: jerk limited trajectory generation and quintic sline interolation, International Journal of Machine Tools & Manufacture 41 (2001) [26] M. Tändl, A. Kecskeméthy, An object-oriented framework for incororating sline curves as generalized joints in multibody systems, in: Proceedings of the Annual Scientific Conference of GAMM, Padua, Italy, 2003, [27] M. Tändl, A. Kecskeméthy, An object oriented aroach for robust guided satial motion, in: Proceedings of the IDETC/CIE 2005 (ASME 2005 International Design Engineering Technical Conferences & Comuters and Information in Engineering Conference), Long Beach, California, DETC , [28] D.J. Walton, D.S. Meek, A generalisation of the Pythagorean hodograh quintic siral, Journal of Comutational and Alied Mathematics 172 (2004) [29] C. De Boor, A Practical Guide to Slines, Sringer-Verlag, New York, 1978.