Trajectory Planning and Feedforward Design for High Performance Motion Systems

Transcription

1 Trajectory Planning and Feedforward Deign for High Performance Motion Sytem Paul Lambrecht, Matthij Boerlage, Maarten Steinbuch Faculty of Mechanical Engineering, Control Sytem Technology Group Eindhoven Univerity of Technology P.O. Box 53, 56 MB Eindhoven, The Netherland Abtract Thi paper give an algorithm for fourth order trajectory planning with contrained dynamic for ingle axi motion control. A model-baed feedforward controller i derived that make full ue of thee trajectorie. Application to indutrial high-preciion electromechanical motion ytem i motivated. Iue like time-optimality, implementation and digitization are conidered. Simulation reult how uperior effectivene in comparion with rigid-body feedforward. I. INTRODUCTION Feedforward control i commonly applied to high performance indutrial motion control ytem like robot and pick-and-place unit. Thee ytem are often embedded in a factory automation cheme, which provide deired motion tak. Such motion tak are then tranferred to computer hardware dedicated to the control of the ytem, leaving the detail of planning and execution of the motion to thi dedicated motion controller. For implicity, the trajectory planning and feedforward control are uually done for each actuating device eparately, relying on ytem compenation and feedback control to deal with interaction and non-linearitie. Each actuating device i then conidered to be acting on a ingle ma moving along a ingle degree of freedom. The feedforward control problem i then to generate the force required for acceleration of the ma over the deired trajectory. Converely, the trajectory hould be uch thahe force i allowable and can be generated by the actuating device. Thi approach i referred to a ma feedforward or rigid-body feedforward. The diadvantage of thi approach i it dependence on ytem compenation and feedback control to deal with unmodelled behavior. The reulting problem formulation can be plit in two. ) During the trajectory, poition error and feedback control action can be large, reulting in unallowable velocitie and/or acceleration (hence: actuator force). ) When arriving ahe endpoint, the poition error i large and it i neceary to introduce a ettling time before ubequent action or motion are allowed. To improve on thi, many academic and practical approache are poible. Thee can be categorized in three. ) Smoothing or haping the trajectory and/or application of force. The reult of thi can be good, but it may alo lead to a coniderable increae in execution time of the trajectory. Variou example can be found in [], [5], [6], [7], [], []. ) Feedforward control baed on plant inverion, either by uing a more detailed model or by learning it behavior baed on meaurement. Thi doe not provide an approach for deigning a trajectory. Variou example can be found in [], [3], [8], [9], [], [3], [], [5]. 3) Feedback control and/or ytem compenation improvement. Obviouly, any feedback control deign method can be ued for thi, burajectory deign i again not conidered. Some reference given above alo dicu the effect of feedback control on trajectory following; e.g. ee [9], [], [5]. Thi paper will provide a method for fourth order trajectory planning and feedforward control that can be ued in addition to all of thee approache. After a review of rigidbody feedforward in ection II, Fourth order feedforward will be preented in ection III. An accurate planning algorithm i given in ection IV. The effect of dicrete time implementation will be conidered in ection V. Finally, ome imulation reult are given in ection VI, followed by concluion in ection VII. II. RIGID-BODY FEEDFORWARD The pecific of planning a trajectory and calculating a feedforward ignal baed on rigid-body feedforward are fairly imple and can be found in many commercially available motion control ytem. In thi ection a hort review i given a an introduction to a tandardized approach to higher order feedforward calculation. Conider the configuration of figure with m denoting the ma of the motion ytem, F the force upplied by the actuating device, x the poition and k a vicou damping term. Now uppoe we have a given bound on acceleration ā (i.e. a bound on F ), and we wano perform a motion over a ditance denoted a x. Then the horteime within which the motion can be performed i calculated a: x = x āt tā = ā t x = tā () with tā denoting the contant acceleration phae duration and t x denoting the total trajectory execution time. Hence,

2 N.? N Fig... Simple motion ytem: a ingle ma. the trajectory conit of a contant maximal acceleration phae followed directly by a contant maximal deceleration phae. Clearly if a bound on velocity, denoted a v, i taken into account, t x can only become larger. We can tet whether the velocity bound v i violated by calculating the maximal velocity obtained uing the minimal time trajectory: ˆv := ā tā () Now if ˆv <= v we are finihed: t x = tā and no contant velocity phae i required. If ˆv > v we calculate: tā = v ā x ā := āt ā < x (3) and the contant velocity phae duration t v i calculated a: t v = ( x x ā) () v reulting in: t x = tā + t v. Thi procedure can now be given a a imple trajectory planning algorithm: ) calculate tā from equation, ) calculate maximal velocity ˆv from equation, if ˆv > v: recalculate tā from equation 3, 3) calculate xā from equation 3, ) calculate t v from equation, and 5) finihed: x = ātā + vt v and t x = tā + t v. Note tha v automatically revert to zero if the velocity bound i not obtained. Contruction of the acceleration profile a from tā and t v i traightforward. From thi, the deired trajectory can be determined by integrating it once to obtain the velocity profile v, and integrating iwice to obtain the poition profile x; ee figure. A the poition profile thu etablihe the trajectory a a equence of polynomial in time with a degree of at mowo, rigid-body feedforward i alo referred to a econd order feedforward. Note thahe feedforward force F i imply calculated from the profile in figure a: F = ma + kv (5) III. HIGHER ORDER FEEDFORWARD PSfrag replacement Compared with the econd order trajectory conidered in the previou ection, higher order trajectorie inherently have the advantage of moothing. Thi implie a lower energy content at higher frequencie, which reult in a lower high frequency content of the error ignal, which in turn enable the feedback controller to be more effective. a [m/ ] v [m/] x [m] 5 tā t v tā Second order trajectory profile Fig.. Second order trajectory determination. Furthermore thi reduce the chance of demanding a motion which i phyically impoible to perform by the given motion ytem: e.g. mot power amplifier exhibit a rie time effect, uch that it i impoible to produce a teplike change in force. The reult i a decreae of poition error during execution of the trajectory and a reduced ettling time. Becaue of thi, many high performance motion ytem are already equipped with a third order trajectory planner; in thi ection it will be determined that a fourth order trajectory planner may give a ignificant further improvement. The main argument for thi i that an electromechanical motion control ytem will uually have ome compliance between actuator and load, and that both actuator and load will have a relevant ma. For thi reaon it i natural to extend the ingle ma model of figure to the double ma model of figure 3. Here m denote the ma of the actuator, m the ma of the load, F the force upplied by the actuating device, x the actuator poition, x the load poition, c the tiffne between the two mae, k the vicou damping between the two mae, k the vicou damping of the actuator toward ground and k the vicou damping of the load toward ground. The equation of Fig. 3. N Extended motion ytem: double ma. motion for thi configuration are: { m ẍ = k ẋ c(x x ) k (ẋ ẋ ) + F m ẍ = k ẋ + c(x x ) + k (ẋ ẋ ) (6)

3 Laplace tranformation and ubtitution then reult in: F = q + q 3 + q 3 + q x (7) k + c q = m m q = (m + m )k + m k + m k q 3 = (m + m )c + k k + (k + k )k q = (k + k )c Thi implie that if we have planned ome fourth order trajectory for x, from which we can derive the correponding profile for velocity v, acceleration a, jerk j and derivative of jerk d, the feedforward force F can be calculated a: F = k + c {q d + q j + q 3 a + q v} (8) PSfrag replacement An implementation of thi feedforward cheme i given in figure. Analogou to rigid-body feedforward, all required Derivative of jerk profile generator d Rigid Body Feedforward q q q3 d [m/ ] j [m/ 3 ] a [m/ ] v [m/] x [m] Fig. 5. Fourth order trajectory profile 8 9 t Fourth order trajectory planning. t Jerk Acceleration Velocity Poition Fig.. q PD Controller k.+c F Motion Sytem th order model Fourth order feedforward implementation. profile can be obtained by integration of the derivative of jerk profile d. Note that rigid-body feedforward i implicit in thi cheme: imply etting m = k = k = make equation 8 equal to equation 5. The remaining difference i that boundedne of jerk and derivative of jerk will reult in a mooth trajectory in comparion with figure. Thi i illutrated in figure 5, which give an example of a ymmetrical fourth order trajectory for a point-to-point move baed on the contruction of a derivative of jerk profile d. Thi profile i completely determined by the value of the given bound and the witching time intance t t 5. An algorithm for obtaining thee witching time intance will be the ubject of the next ection. IV. FOURTH ORDER TRAJECTORY PLANNING Planner for econd and third order trajectorie are fairly well known in indutry and academia and there are many approache for obtaining a valid olution. Extenion to fourth order trajectory planning i however norivial. In thi ection an approach i given that can be een a a direct extenion of the rigid-body algorithm given in ection II. Aume thahe poition diplacement x and bound on all derivative of the trajectory up to the derivative of jerk d x are given (indicated a v, ā, j and ). Furthermore, aume that all derivative are equal to zero ahe tart and end poition. The propoed fourth order trajectory planning algorithm i then given in the following tep. ) Temporarily dicard v, ā and j. From figure 5 follow that d conit of 8 period with value or. ) Determine t : the horteime of contant d (alway firt period) uch thahe total diplacement i x: t = x 8 3) Calculate maximal value of velocity ˆv: if ˆv > v: recalculate t baed on v: (9) ˆv = t 3 () t = 3 v ) Calculate maximal value of acceleration â: if â > ā: recalculate t baed on ā: ā () â = t () t = 5) Calculate maximal value of jerk ĵ: if ĵ > j: recalculate t baed on j: (3) ĵ = t () t = j (5)

4 The reulting t will not be changed anymore. 6) Temporarily dicard v and ā, but not j: extend the trajectory ymmetrically with period of contant j whenever j reache the value j or j. 7) Determine t j uch thahe total diplacement i x. Thi i the poitive real olution of the third order polynomial equation: t 3 j + (5t )t j + (8t )t j + (t 3 x t ) = (6) 8) Calculate maximal value of velocity ˆv: ˆv = t t t j + t t j (7) if ˆv > v: recalculate t j baed on v; thi i the poitive real olution of the econd order polynomial equation: t j + 3t t j + t vt = (8) 9) Calculate maximal value of acceleration â: if â > ā: recalculate t j baed on ā: â = t + t t j (9) t j = ā j t () The reulting t j will not be changed anymore. ) Temporarily dicard v but not ā: extend the trajectory ymmetrically with period of contant a whenever a reache the value ā or ā. ) Determine tā uch thahe total diplacement i x. Thi i the poitive real olution of the econd order polynomial equation: {t + t t j } t ā + {6t 3 + 9t t j + 3t t j } tā+ {8t + 6t 3t j + t t j + t t 3 j } x = ) Calculate maximal value of velocity ˆv: () ˆv = t t t j + t t j + t tā + t t j tā () if ˆv > v: recalculate tā baed on v: tā = v t 3 3 t t j t t j t + t t j (3) 3) Calculate total diplacement a if no contant velocity phae i required: xā = {8t + 6t 3t j + t t j + t t 3 j + t t ā + t t j tā + 6t 3tā + 9t t j tā + 3t t j () tā} ) Calculate contant peed phae duration t v uch that the total diplacement i x: t v = x x ā v (5) 5) Finihed:, t, t j, tā and t v completely determine the trajectory. The trajectorie reulting from thi algorithm have two important inherent propertie: none of the given bound i violated, in cae there i a contant velocity phae (t v > ), the trajectory i time-optimal. Furthermore, although obviouly more complex than the rigid-body approach, the algorithm conit of traightforward calculation that can relatively eaily be implemented in tate-of-the-art motion control hardware. V. IMPLEMENTATION ASPECTS Thi ection give ome conideration on the trajectory planning algorithm and the feedforward control cheme of figure for implementation in digital hardware. A. Switching time When conidering dicrete time implementation, the witching time intance of the planned trajectory mut be ynchronized with the ampling time intance. Thi implie thahe time interval t v, tā, etc. mut be roundedoff toward a multiple of the ampling time interval T. To remain within the given bound, but ahe ame time approximate them a cloely a poible, thi rounding off mut be done to the next higher multiple. The uggeted approach i to do thi immediately after each calculation of a time interval, after which the maximal value mut be recalculated accordingly. A an example conider the firt calculation of t (equation 9). The rounded-off value for t i: ( ) t t = ceil T (6) with ceil(.) denoting the rounding off toward the next higher integer. From equation 9 we can then calculate a new value for : = x (7) T 8t Note that with t t we mut have. It can be verified thahi ame approach i valid for the calculation or recalculation of all time interval. Note that with each new calculation of it value mut reduce. Thi guarantee that none of the bound that were checked in earlier tep of the algorithm will be violated. B. Synchronization of profile The dicrete time implementation of the integrator in figure can be done by replacing the continuou time integrator by forward Euler dicrete time integrator a in figure 6. Clearly, all required profile are now calculated Derivative of jerk profile generator Fig. 6. z- Jerk j z- Acceleration a z- Velocity v z- Poition Dicrete time planner uing forward Euler integrator. with ampling time interval T. However, due to the zero d x

5 order hold effect, each of the four integrator introduce a pecific delay time. Thi can be een in figure 7a, in which the dicrete time profile are compared with the correponding continuou time profile. Note that T =.5, which i choen large in relation to the required trajectory to how the dicretization effect more clearly. To fix thi effect, the higher order profile can be delayed individually uch thahe ymmetry of the complete et of profile i retored. Figure 7b how thi: the dicrete time profile are now perfectly ynchronized with the T delayed continuou time profile. poition, velocity, acceleration, jerk, derivative of jerk Normalized dicrete time fourth order profile, compared with continuou time profile a: not ynchronized b: ynchronized, delayed by Fig. 7. Dicrete time fourth order profile uing forward Euler integrator, compared with equivalent continuou time profile. Note thahe derivative of jerk profile mut be delayed with T, the jerk profile with T, the acceleration profile with T and finally the velocity profile with T. To obtain a delay of T when ampling with T the average value i taken from the current and previou amplitude of the conidered profile. Thi operation appear to work very well, although the aociated moothing effect i undeirable. C. Implementation of firt order filter All required profile for calculation of the feedforward ignal are now available. The multiplication with factor q to q followed by ummation a indicated in figure i traightforward. The firt order filtering i le trivial, a it mut alo be tranferred to dicrete time. A poible implementation that prevent problem with unwanted time delay and give good reult i to make ue of the trapezoidal integration method a hown in figure 8. u ZOH / (*k+c*) z -K- -K- (*k-c*) / (*k+c*) Fig. 8. Dicrete implementation of firt order filter uing the trapezoidal integration method. y D. Calculation of reference trajectory A final point on ynchronization mut be made with repeco the calculation of the reference trajectory that i ued for feedback control. When applying the ynchronized feedforward ignal a given above, the actual plant repone will be cloe to the ideal continuou time repone with a delay of T. However, in order to compare thi repone with the reference trajectory it mut alo be ampled with T, leading to an additional delay of T. Hence, it i neceary to alo delay the reference trajectory with thi ame value. The reult of thi i thahe control error will not be affected by ampling. The controller will only act on the effect of diturbance and on dicrepancie between the actual plant and the modelled fourth order behavior. Obviouly, thi i only true if the ampling frequency i ufficiently high: otherwie the momentary control error may deviate ignificantly from the average value. If thi i the cae, an increae in ampling frequency mut be conidered. Uually however, the ampling frequency i more ignificantly determined by the demand on tability and performance of the (digital) feedback controller. VI. SIMULATION RESULTS The effect of parameter variation and dicretization on the performance of fourth order feedforward control are conidered. For thi, ome imulation are performed uing the configuration of figure. The motion ytem parameter and their variation are given in table I. The trajectory Parameter Value Unit Variation m Kg m {5 5}, m Kg m = 3 m k N/m k {5 5}, k N/m k = k c 6 5 N/m ±33% k 5 N/m ±% TABLE I SIMULATION PARAMETERS i calculated for a diplacement x = m, with bound: = m/, j = 5 m/ 3, ā = 5 m/, and v =.5 m/ (ee figure 5). The main concern with model-baed feedforward i the dicrepancy between the behavior of the actual motion ytem and the ued model. Figure 9 how the performance of fourth order feedforward for a fourth order motion ytem model with perturbation according to table I. For comparion, the repone of the nominal motion ytem model with optimal rigid-body feedforward i given (by applying equation 5 to the acceleration and velocity profile of figure 5). Note that in pite of the ignificant plant variation, fourth order feedforward perform at leawice a good. Figure how thahe ervo error repone will not ignificantly deteriorate if fourth order feedforward i implemented in dicrete time. A an example, the motion

6 Poition error [m] Open loop fourth order feedforward repone of ervo error with plant variation 8 x 5 Dahed line give nominal rigid body FF repone Dahed: nominal Rigid Body FF repone : m max, m min 6 : m min, m max 3: k max, k min : k min, k max 5: c max, 6: c min 6 7: k max, 8: k min Fig. 9. Open loop imulation reult of fourth order feedforward controller with plant variation, in comparion with optimally tuned rigid-body feedforward. Poition error [m] 3 x 5 Open Loop Cloed Loop Digital ff repone: th order, minimal c, =5e 3 x Fig.. Simulation reult of open loop and cloed loop dicrete time fourth order feedforward controller with minimal tiffne plant. Thick line: dicrete, thin line: continuou time. ytem with minimal pring-tiffne i conidered. Both open loop and cloed loop reult are given: the feedback controller i tuned for a bandwidth of about Hz, wherea the motion ytem firt reonance mode i at 5 Hz. The digital feedforward controller i combined with a dicrete time feedback controller, both with a ampling rate of Hz. Note thahi i a low ampling rate for a high performance ervo ytem: thi i choen to demontrate the dicretization effect more clearly. VII. CONCLUSIONS For high performance motion control the uefulne of feedforward i well known. Thi paper how that rigidbody feedforward can be extended to fourth order feedforward with uperior performance for an important cla of motion ytem. An algorithm i given to calculate fourth order trajectorie for point-to-point move with important propertie like time-optimality, actuator effort limitation, reliability, implementability and accuracy. Other motion command, like peed change operation, can be derived from thi. Further implementation iue, like dicrete time calculation and ynchronization, are addreed. It i hown that deterioration of the continuou time reult due to ampling are mall when applying a ufficient ampling rate. REFERENCES [] Devaia S. (). Robut inverion-baed feedforward controller for outpuracking under plant uncertainty. Proc. Am. Contr. Conf., [] Dijktra B.G., Rambaratingh N.J., Scherer C., Bogra O.H., Steinbuch M., & Keremaker S. (). Input deign for optimal dicrete-time point-to-point motion of an indutrial XY poitioning table. Proc. 39th IEEE Conf. on Dec. and Contr., [3] Hunt L., & Meyer G. (996). Noncaual invere for linear ytem. IEEE Tran. on Aut. Contr. (), [] Meckl P.H. (999). Dicuion on: comparion of filtering method for reducing reidual vibration. Eur. J. of Contr. 5, 9-. [5] Meckl P.H., Aretide P.B., & Wood M.C. (998). Optimized S- curve motion profile for minimum reidual vibration. Proc. Am. Contr. Conf. 998, [6] Murphy B.R., & Watanaabe I. (99). Digital haping filter for reducing machine vibration. IEEE Tran. on Robotic and Automation 8(), [7] Paganini F., & Giuto A. (997). Robut ynthei of dynamic prefilter. Proc. Am. Contr. Conf. 997, [8] Park H.S., Chang P.H., & Lee D.Y. (). Concurrent deign of continuou zero phae error tracking controller and inuoidal trajectory for improved tracking control. J. Dyn. Sy., Mea., and Contr. 5, [9] Roover D. (997). Motion control for a wafer tage. Delft Univerity Pre, The Netherland. [] Roover D., & Sperling F. (997). Point-to-point control of a high accuracy poitioning mechanim. Proc. Am. Contr. Conf. 997, [] Singer N., Singhoe W., & Seering W. (999). Comparion of filtering method for reducing reidual vibration. Eur. J. of Contr. 5, 8-8. [] Steinbuch M., & Norg M.L. (998). Advanced motion control: an indutrial perpective. Eur. J. of Contr., [3] Tomizuka M. (987). Zero phae error tracking algorithm for digital control. J. Dyn. Sy., Mea., and Contr. 9, [] Torf D.E., Swever J., & De Schutter J. (99). Quai-perfect tracking control of non-minimal phae ytem. Proc. 3th Conf. on Dec. and Contr., -. [5] Torf D.E., Vuerinckx R., Swever J., & Schouken J. (998). Comparion of two feedforward deign method aiming at accurate trajectory tracking of the end point of a flexible robot arm. IEEE Tran. on Contr. Sy. Tech. 6(), -.