Simplified Robotics Joint-Space Trajectory Generation with Via Point using a Single Polynomial

Transcription

1 R.L. Willims II, 13, Simplified Robotics Joint-Spce Trjectory Genertion with i Point using Single Polynomil, Hindwi Journl of Robotics, Article ID , 6 pges, Mrch, doi: /13/ Simplified Robotics Joint-Spce Trjectory Genertion with i Point using Single Polynomil Robert L. Willims II Mechnicl Engineering, Ohio University Athens, Ohio, USA willir4@ohio.edu ABSTRACT This rticle presents novel fourth- nd sixth-order polynomils to solve the problem of jointspce trjectory genertion with vi point. These new polynomils use single function rther thn two polynomil functions mtched t the vi point s in previous methods. The problem of infinite spikes in jerk is lso ddressed. KEYWORDS Robot joint-spce trjectory genertion, polynomil, vi point, finite jerk.

2 1. INTRODUCTION Joint-spce trjectory genertion is in common usge in robotics to provide smooth, continuous motion from one set of n joint ngles to nother, for instnce for moving between two distinct Crtesin poses for which the inverse pose solution hs yielded two distinct sets of n joint ngles. The joint-spce trjectory genertion occurs t runtime for ll n joints independently but simultneously. There is n entire body of literture devoted to trjectory genertion (k motion plnning nd pth plnning) t the joint level. Pul nd Zong (1984) were mong the first to suggest the use of polynomils for robot trjectory genertion. A common joint-spce trjectory genertion method (linerly-chnging joint velocity using strting nd ending prbolic blends) is identiclly presented mny uthors (Koivo 1989, Prkin 1991, Scivicco nd Sicilino 1996, Crig 5, Spong et l. 5, nd Jzr 1). Despite the widespred populrity of this method, it suffers from infinite spikes in jerk (the derivtive of ccelertion) nd requires three seprte functions insted of one. An identicl thirdorder polynomil joint-spce trjectory genertion pproch is lso presented by mny uthors (Koivo 1989, Scivicco nd Sicilino 1996, Crig 5, Spong et l. 5, nd Jzr 1). urther, n identicl fifth-order polynomil joint-spce trjectory genertion pproch is presented by mny uthors (Koivo 1989, Scivicco nd Sicilino 1996, Crig 5, nd Jzr 1). rom these uthor lists it my pper tht Koivo ws the first to present these methods, when in fct they were lredy presented in Crig s first edition in u et l. (1987) deprt from these stndrd methods, suggesting initil, intermedite, nd finl polynomils of order 4-3-4, 3-5-3, or 5 third-order polynomils, for single joint motion. These re by fr the most (unnecessrily) complicted methods, nd re presented without justifiction or comprison with simpler methods. or deling with vi point in which the robot need not stop t the vi point (such s for obstcle voidnce), Crig (5) suggests mtching two thirdorder polynomils. Apprently lone mongst ll of the mjor robotics textbook uthors, Angeles (7, Section 6.5) derives seventh-order polynomil to fit two vi points nd ensure finite joint jerk t the strt

3 3 nd end of motion. Angeles (7, Section 6.6) lso presents pproximtion of this seventh-order polynomil with cubic spline nd discusses the ssocited errors. An erly version of the current rticle ws presented by the uthor t conference (Willims, 11). The originl contribution of this rticle is single sixth-order polynomil to provide smooth, continuous joint motion through vi point. Previous pproches hve mtched two third-order polynomils t the vi point. We lso present single fourth-order polynomil to ccomplish the sme purpose. However, this cse should not be used since it leds to discontinuous ccelertions, leding to infinite spikes in jerk, which is uncceptble for relible, smooth, long-life robotic systems. This infinite jerk problem, prevlent in ll of the robotics textbooks, hs lso been pointed out by Mcfrlne nd Croft (3), who present jerk-bounded trjectories nd Gosselin nd Hdj-Messoud (1993) nd Petrinec nd Kovcic (7) who go one step further to ensure continuous, not just bounded, jerk.

4 4. JOINT-SPACE TRAJECTORY GENERATION WITH A IA POINT Stndrd joint-spce trjectory genertion ssumes two sets of n discrete joint prmeters re known (ngles for revolute joints, lengths for prismtic joints) nd it is required to move smoothly in joint spce from one set to the next. or instnce, given required Crtesin poses X S nd X, inverse pose kinemtics clcultes the required n joint vlue sets for chieving ech of these; cll them S nd. Note subscript S stnds for strt (or initil) nd subscript stnds for finish (or finl). The stndrd joint-spce trjectory method then moves smoothly from S nd on ll n joints independently but simultneously. Polynomils re nturl choices for providing smooth, continuous motion, with some level of continuous derivtives. In mny robotics motion plnning problems, the robot must pss through intermedite point(s) between the strt nd finish poses, such s for obstcle voidnce. The joint rtes nd ccelertions need not go to zero t these so-clled vi points, but they must be mtched between functions meeting t the vi point(s)..1 Two Third-Order Polynomils Crig (5) suggested the use of two third-order polynomils meeting t the vi point. This entire Section.1 is summry of Crig s work (with n originl exmple by the current uthor). Two third-order polynomils will provide smooth motion with continuous position nd velocity nd zero velocity t the strt nd end. The two third-order polynomils re: () t t t t 1() t 313t 1t11 () t 6 t () t t t t () t 33t t1 () t 6 t (1) time points: These two polynomils require eight constrints. our constrints come from the initil nd finl

5 1() S () 1 ( t) ( t ) 5 () It is convenient to shift the time xis so tht the (t) polynomil hs zero time strting t the vi time. t 1 is the end time of the first rnge (t 1 = t ) nd t is the reltive end time of the second rnge (t = t t ). We need four more constrints. We force the first polynomil to end t the vi ngle nd the second polynomil to strt t the vi ngle. We lso ensure the velocities nd ccelertions mtch t the vi point (they need not go to zero). This will ensure tht the jerk stys finite during this vi point trnsition between the two polynomils. The four dditionl constrints re: ( t ) 1 1 () 1( t1) () ( t ) () 1 1 (3) The constrints yield eight liner equtions in the eight unknowns (two third-order polynomils, four unknown coefficients ech). Three of the unknown polynomil coefficients re found immeditely, from the initil nd vi time constrints: 1 = S, 11 =, nd =. The simplified 5x5 mtrix/vector eqution to solve for the remining three unknowns is: 1 t1 1 S t 1 1 t t 13 t 1 t 3t 1 t1 3t1 1 6t 1 3 (4) Solve (4) for the remining five unknowns for use in (1). or the specil cse of t 1 t t t T, the nlyticl solution is 1 = S, 11 =, nd =, nd:

6 6 S 1 S T 3 5 4T 3 S 4T 3 3 T 5 3 4T S S 3 3 (5) Exmple Two Third-Order Polynomils with i Point Use two third-order polynomils for smooth joint spce trjectory genertion, plus motion through vi point, for one joint. Given 3, 18, 1, t 1.5, t 3 ( t 1 t 1.5 ) sec, find 1 (t) nd (t): S t t t 1( t) 6.67t 34t 1( t) t34 ( t) ( ) t t t t ( t) t 8t45 ( t) t8 ( t) ( ) (6) deg units re used throughout this exmple. These results re plotted in igure d dd ddd time (sec) igure 1. Two Third-Order Polynomils with i Point

7 Note tht the joint ngle psses through the vi point 18 nd keeps going for brief time this is becuse the velocity is still positive beyond the first 1.5 sec. The mximum ngle is 7 MAX 183.9, occurring t t = 1.68 sec. At MAX the ngulr velocity goes to zero (the slope of the ngle is zero t tht point since the ngle is chnging direction). Also note tht the velocity term in (t), the t coefficient, is non-zero since the velocity does not need to go to zero t this vi point. urther note tht the velocity nd ccelertion hve been successfully mtched t the 1.5 sec vi point trnsition s required. Note tht the jerk is not mtched t the trnsition (the slope of the two polynomil ccelertions re the sme mgnitude but different signs), but jerk remins finite since the ccelertion is mtched. The jerk hs infinite spikes t the strt nd end, which is common in the robotics books, but is uncceptble.. Two ifth-order Polynomils The two third-order polynomils presented bove, mtched t vi point, pper in mny robotics textbooks (e.g. Crig, 5). However, this pproch suffers from discontinuous ccelertion functions t the initil nd finl time, leding to infinite spikes in jerk t these points in time. This is uncceptble for the repeted motions of mechnicl systems, due to uncceptble wer, noise, nd dynmic excittion. To fix this, we could mtch two fifth-order polynomils t the vi point, specifying zero ccelertion t the initil nd finl times (not t the vi time insted mtching velocities nd ccelertions between the two fifth-order polynomil functions t the vi time). However, this will not be presented becuse we cme up with better method, single polynomil vs. two mtched polynomils this originl work is presented in the next two subsections.

8 8.3 Single ourth-order Polynomil The first originl contribution of this rticle is presented in this subsection. We cn chieve the sme gols s the two third-order polynomils meeting t vi point much more simply: let us use only one polynomil, forced to go through the vi point. Here re the constrints for meeting the required ngles with smooth motion (since we use single continuous polynomil, the velocity nd ccelertions re gurnteed to mtch nd be continuous t the vi point): () S () ( ) t ( t ) f ( t ) f (7) Note in this cse since there is only one time rnge, it is convenient to tret ll times s bsolute, rther thn reltive s when we did mtching of two third-order polynomils. With five constrints single fourth-order polynomil is required: () t t t t t ( t) 4 t 3 t t (8) ive liner equtions in the five unknown polynomil coefficients i, i =,1,,3,4 result from the five constrints (7). Two of the unknown polynomil coefficients re found immeditely, from the initil time constrints: = S nd 1 =. The simplified 3x3 mtrix/vector eqution to solve for the remining three unknowns is: 1 t t S t 1 t t 3 S t 3t 4t 4 (9) The single fourth-order polynomil solution for ech joint is:

9 t t t t S S t t t t t St t S 3 3 t t t t 3t t 1 S S 4 3 t t t t (1) or the specil cse of t t, the solution is: t 3 3 t 4 4 t S S 16 7 S S 8 S S (11) Exmple Single ourth-order Polynomil with i Point Use single fourth-order polynomil for smooth joint spce trjectory genertion, plus motion through vi point, for one joint. Given 3, 18, 1, t 1.5, t 3 sec, find (t): S t t t t 3 ( t) 8.96t t t ( t) 48.89t t ( t) t ( ) (1) Agin, deg units re used throughout this exmple. These results re plotted in igure.

10 d dd ddd time (sec) igure. Single ourth-order Polynomil with i Point The (t) shpe for the single fourth-order polynomil is very similr to tht for the two thirdorder polynomils. Agin the joint ngle psses through the vi point 18 nd keeps going briefly, due to the fct tht the velocity is still positive beyond the first 1.5 sec. The pek for this fourth-order cse is slightly greter nd occurs in time slightly fter the pek for the two third-order polynomils exmple. The mximum ngle is 185.4, occurring t t = 1.74 sec. or the fourth-order MAX polynomil, side benefit hs risen: the jerk is now continuous t the vi time, where it ws discontinuous for the two mtched third-order polynomils. The jerk still hs n infinite spike t the strt nd end s we sw with the two third-order polynomils exmple, which is uncceptble. We now improve upon this with single sixth-order polynomil in the next subsection gin, this next subsection is originl work.

11 11.4 Single Sixth-Order Polynomil This section presents nother originl contribution. We cn chieve the sme gols s the single fourth-order polynomil with vi point nd eliminte the infinite spikes in jerk t the strt nd end s follows. To the previous five constrints, dd two more, for zero ccelertion t the strt nd end of the single motion rnge. Here re the seven constrints: () S () () ( ) t ( t ) f ( t ) f ( t ) f (13) With seven constrints single sixth-order polynomil is required: () t t t t t t t ( t) 6 t 5 t 4 t 3 t t ( t) 3 t t 1 t 6 t (14) Seven liner equtions in the seven unknown polynomil coefficients i, i =,1,,3,4,5,6 result from the seven constrints (13). Three of the unknown polynomil coefficients re found immeditely, from the initil time constrints: = S, 1 =, nd =. The simplified 4x4 mtrix/vector eqution to solve for the remining four unknowns is: t t t 3 S t t t t 4 S t 3 3 4t 5t 6t t t 3t 6 (15) The single sixth-order polynomil solution for ech joint is:

12 3 t t t t t t S S t t t t 3 3 t S5t 9tt 5t 3 S t t t t 3 3 t S8t 9tt t 3 S t t t t t t t S S 1t 15t t 5 t 6t 1 (16) or the specil cse of t t, the nlyticl solution is: t 4 4 t 5 5 t 6 6 t S S S S S S 3 S S (17) Exmple Single Sixth-Order Polynomil with i Point Use single sixth-order polynomil for smooth joint spce trjectory genertion, plus motion through vi point, for one joint. Given 3, 18, 1, t 1.5, t 3 sec, find (t): S t t t t t ( t) 55.3t 45.9t 16.6t 846.7t 4 3 ( t) 76.5t 173.7t t t 3 ( t) 116.t t t ( ) (18) Agin, deg units re used throughout this exmple. These results re plotted in igure 3.

13 d dd ddd time (sec) igure 3. Single Sixth-Order Polynomil with i Point Agin the (t) shpe is similr to the previous two exmples. The mximum ngle is MAX 185.6, occurring t t = 1.7 sec. Note the mgnitude of the ngulr velocity, ccelertion, nd jerk re not gretly different for the three cses we hve presented with vi point. Generlly they re lowest for the two third-order polynomils. This single sixth-order polynomil pproch hs eliminted the problem of infinite spikes in jerk t the strt nd end of ech motion. Here the jerk is discontinuous but it remins finite, which obeys the rule of thumb for mechnicl design/motion.

14 14 3. DISCUSSION All joint-spce trjectory genertion methods presented by previous uthors nd the new ones proposed in this rticle cn esily be pplied to multiple joints, simultneously but independently. This min originl contribution of this rticle is to present new joint spce trjectory genertion pproch using single polynomil for motion through given vi point. The robot does not need to stop t the vi point, but the motion must be smooth nd continuous through the vi point. A single fourth-order polynomil ws developed to chieve this smooth motion through vi point, to replce the two three-order polynomils mtched t the vi point in common usge tody. With the new pproch this vi point mtching comes utomticlly nd there is no need for two polynomil functions. An importnt secondry contribution of this rticle is to expose bd prctice in common usge in joint-spce trjectory genertion in robotics tody. We re not the first to notice this (Mcfrlne nd Croft 3, Gosselin nd Hdj-Messoud 1993, nd Petrinec nd Kovcic 7), but judging from the four robotics textbooks in the reference list published within the pst five yers, the messge hs not been widely understood (only Angeles, 7, is concerned with preventing infinite jerk). or ny functions in which the ccelertion is discontinuous, the ssocited jerk (time derivtive of ccelertion) function will hve infinite spikes t those ccelertion discontinuities. Cm design teches tht these infinite spikes in jerk re uncceptble nd must be voided by the designed/motion controller. By extension, in robot joint spce trjectory genertion, ny method which llows discontinuous ccelertion functions re uncceptble nd must not be used. Unfortuntely, the liner velocity with prbolic blends, the third-order polynomil, nd the two third-order polynomils mtched to meet t vi point ll suffer from discontinuity in ccelertion nd thus infinite spikes in jerk. These methods re still very much in use in the stndrd robotics textbooks tody these methods must be discrded. Otherwise, uncceptble noise, wer, stress, reduced life, nd the introduction of bd dynmics my result.

15 15 The novel single fourth-order polynomil introduced in this rticle to go through vi point lso suffers from this discontinuous ccelertion. Therefore, to remedy this we introduced novel sixthorder polynomil to move smoothly nd continuously through vi point, with finite jerk throughout the entire motion, which is cceptble. 4. CONCLUSION or joint-spce trjectory genertion in robotics, the recommendtions of this rticle re simple for ll joint motions without vi point, use the stndrd fifth-order polynomil from Crig (5). or ll joint motions with vi point, use the new single sixth-order polynomil introduced in this rticle. Not only does this new sixth-order polynomil keep the jerk finite, it lso chieves the vi point motion with only one function.

16 16 REERENCES J. Angeles, 7, undmentls of Robotic Mechnicl Systems, 3 rd edition, Springer, New York, NY. J.J. Crig, 5, Introduction to Robotics: Mechnics nd Control, 3 rd edition, Person Prentice Hll, Upper Sddle River, NJ. K.S. u, R.C. Gonzlez, nd C.S.G. Lee, 1987, Robotics: Control, Sensing, ision, nd Intelligence, McGrw-Hill, New York. C.M. Gosselin nd A. Hdj-Messoud, 1993, Automtic plnning of smooth trjectories for pick-nd-plce opertions, Journl of Mechnicl Design. Trns ASME, 115(3): R.N. Jzr, 1, Theory of Applied Robotics: Kinemtics, Dynmics, nd Control, nd edition, Springer, New York, NY. York. A.J. Koivo, 1989, undmentls for Control of Robotic Mnipultors, John Wiley & Sons, New S. Mcfrlne nd E.A. Croft, 3, "Jerk-bounded mnipultor trjectory plnning: Design for rel-time pplictions", IEEE Trnsctions on Robotics nd Automtion, 19(1): 4 5. R.E. Prkin, 1991, Applied Robotic Anlysis, Prentice Hll, Englewood Cliffs, NJ. R.P. Pul nd H. Zhong, 1984, Robot Motion Trjectory Specifiction nd Genertion, Second Interntionl Symposium on Robotics Reserch, Kyoto, Jpn, August. K. Petrinec nd Z. Kovcic, 7, "Trjectory Plnning Algorithm Bsed on the Continuity of Jerk, Proceedings of the 15 th Mediterrnen Conference on Control nd Automtion, IEEE, Athens, Greece, July. L. Scivicco nd B. Sicilino, 8, Robotics: Modeling, Plnning, nd Control, McGrw-Hill, New York. M.W. Spong, S. Hutchinson, nd M. idysgr, 5, Robot Modeling nd Control, John Wiley & Sons, New York. R.L. Willims II, 11, Improved Robotics Joint-Spce Trjectory Genertion with i Point, Proceedings of the ASME IDETC/CIE, Wshington DC, August 8-31, 11, DETC