Roboics I April 11, 017 Exercise 1 he kinemaics of a 3R spaial robo is specified by he Denavi-Harenberg parameers in ab 1 i α i d i a i θ i 1 π/ L 1 0 1 0 0 L 3 0 0 L 3 3 able 1: able of DH parameers of a 3R spaial robo Given a posiion p R 3 of he origin of he end-effecor frame, provide he analyic expression of he soluion o he inverse kinemaics problem For L 1 = 1 [m] and L = L 3 = 15 [m], deermine all inverse kinemaics soluions in numerical form associaed o he end-effecor posiion p = 1 1 15 [m] Exercise A robo join should move in minimum ime beween an iniial value a and a final value b, wih an iniial velociy a and a final velociy b, under he bounds and A Provide he analyic expression of he minimum feasible moion ime when = b a > 0 and he iniial and final velociies are arbirary in sign and magniude bu boh saisfy he velociy bound, ie, a and b Using he daa a = 90, b = 30, a = 45 /s, b = 45 /s, = 90 /s, A = 00 /s, deermine he numerical value of he minimum feasible moion ime and draw he velociy and acceleraion profiles of he join moion [180 minues, open books bu no compuer or smarphone] 1
Exercise 1 Soluion April 11, 017 From he direc kinemaics, using ab 1, we obain for he posiion of he origin of he end-effecor frame p 0 p H = = 0 A 1 1 A A 3 3 1 1 L cos + L 3 cos + 3 cos 1 p = L cos + L 3 cos + 3 sin 1 L 1 + L sin + L 3 sin + 3 1 he analyic inversion of e 1 for p = p d = p dx p dy p dz proceeds as follows Afer moving L 1 o he lef-hand side of he hird euaion, suaring and adding he hree euaions yields he numeric value c 3 for cos 3 c 3 = p dx + p dy + p dz L 1 L L 3 L L 3 he desired end-effecor posiion will belong o he robo workspace if and only if c 3 [ 1, 1] Noe ha his condiion holds no maer if L and L 3 are eual or differen Under such premises, we compue s 3 = 1 c 3 3 and {+} 3 = AAN {s 3, c 3 }, { } 3 = AAN { s 3, c 3 }, 4 yielding by definiion wo opposie values { } 3 = {+} 3 If c 3 = ±1, he robo is in a kinemaic singulariy: he forearm is eiher sreched or folded, in boh cases on he boundary of he workspace In paricular, when c 3 = 1, {+} 3 and { } 3 are boh eual o 0; when c 3 = 1, he wo soluions will be aken 1 eual o π Insead, when c 3 [ 1, 1], he inverse kinemaics algorihm should oupu a warning message desired posiion is ou of workspace and exi When p dx + p dy > 0, from he firs wo euaions in 1 we can furher compue p dx + p dy = L cos + L 3 cos + 3 cos 1 = and hus p dx, sin 1 = ± p dx + p dy p dy ± p dx + p dy {+} 1 = AAN {p dy, p dx }, { } 1 = AAN { p dy, p dx } 5 hese wo values belong o π, π] and will always differ by π Insead, when p dx = p dy = 0, he firs join angle 1 remains undefined and he robo will be in a kinemaic singulariy wih he end-effecor placed along he axis of join 1 he soluion algorihm should oupu a warning message singular case: angle 1 is undefined, possibly se a flag sing 1 = ON, bu coninue 1 Remember ha we use as convenional range π, π], for all angles hus, if he oupu of a generic compuaion is π, we always replace i wih +π,
A his sage, we can rewrie a suiable combinaion of he firs wo euaions in 1 as well as he hird euaion in he following way: and cos 1 p dx + sin 1 p dy = L cos + L 3 cos + 3 = L + L 3 cos 3 cos L 3 sin 3 sin p dz L 1 = L sin + L 3 sin + 3 = L 3 sin 3 cos + L + L 3 cos 3 sin Plugging he muliple values found so far for 1 and 3, we obain four similar linear sysems in he rigonomeric unknowns c = cos and s = sin : L + L 3 c 3 L 3 s {+, } 3 c cos {+, } 1 p dx + sin {+, } 1 p dy = A {+, } x = b {+, } L 3 s {+, } 3 L + L 3 c 3 s p dz L 1 6 In 6, we should use and he values from 4 and 5 his gives rise o four possible combinaions for he marix/vecor pair A {+, }, b {+, }, which will evenually lead o four soluions for ha are in general disinc hese will be labeled as {f,u} {f,d} {b,u} {b,d} {f,u} {f,d} {b,u} {b,d} depending on wheher he robo is facing f of backing b he desired posiion uadran due o he choice of 1, and on wheher he elbow is up u or down d due o he combined choice of 1 and 3 If he common deerminan of he coefficien marix is differen from zero, ie, using e, de A {+, } = L + L 3 c 3 + L 3 s {{f,b},{u,d}} s {+, } 3 = L + L 3 + L L 3 c 3 = p dx + p dy + p dz L 1 > 0, he soluion for of each of he above four cases is uniuely deermined from {{f,b},{u,d}} c = L + L 3 c 3 cos {+, } 1 p dx + sin {+, } 1 p dy L + L 3 c 3 p dz L 1 L 3 s {+, } 3 and henceforh + L 3 s {+, } 3 p dz L 1 cos {+, } 1 p dx + sin {+, } 1 p dy { } {{f,b},{u,d}} = AAN s {{f,b},{u,d}}, c {{f,b},{u,d}} 7 Insead, when p dx = p dy = 0 and p dy = L 1, he robo will be in a double kinemaic singulariy, wih he arm folded and he end-effecor placed along he axis of join 1 Noe ha his siuaion can only occur in case he robo has L = L 3 oherwise he singular Caresian poin would be ou of he robo workspace he soluion algorihm should oupu a warning message singular case: angle is undefined, possibly se a second flag sing = ON, and hen exi In his case, only a single value 3 = π for he hird join angle will be defined Moving nex o he reuesed numerical case wih L 1 = 1, L = 15, and L 3 = 15 [m], and for he desired posiion 1 p d = 1 [m], 15 A special case arises when he join angle 1 remains undefined a singulariy wih flag sing 1 = ON he firs componen of he known vecor b in 6 will vanish p dx = p dy = 0 and only wo soluions would be lef for he case in which hese wo well-defined soluions collapse ino a single value is lef o he reader s analysis, 3
we can see ha p d belongs o he robo workspace and ha his is no a singular case since c 3 = 05 [ 1, 1], p dx + p dy = > 0 We noe ha he desired posiion is in he second uadran x < 0, y > 0 hus, he four inverse kinemaics soluions obained from 4, 5 and 7 are: 356 3π/4 13500 {f,u} = 13870 = 13870 [rad] = 7947 0944 π/3 1000 {f,d} = {b,u} = {b,d} = 356 07074 0944 07854 17546 = = 0944 07854 434 0944 = 3π/4 356 π/3 π/4 17546 [rad] = π/3 π/4 434 π/3 [rad] = [rad] = 13500 4053 1000 4500 10053 1000 4500 13947 1000 As a double-check of correcness, i is always highly recommended o evaluae he direc kinemaics wih he obained soluions 8 In reurn, one should ge every ime he desired posiion p d Exercise his exercise is a generalizaion of he minimum-ime rajecory planning problem for a single join under velociy and acceleraion bounds, wih zero iniial and final velociy res-o-res as boundary condiions I is useful o recap firs he soluion o he res-o-res problem he minimum-ime moion is given by a rapezoidal velociy profile or a bang-coas-bang profile in acceleraion, wih minimum moion ime and symmeric iniial and final acceleraion/deceleraion phases of duraion s given by = + A > s, s = > 0 9 A his soluion is only valid when he disance o ravel in absolue value and he limi velociy and acceleraion values > 0 and A > 0 saisfy he ineualiy 8 A, 10 namely, when he disance is sufficienly long wih respec o he raio of he suared velociy limi o he acceleraion limi When he eualiy holds in 10, he maximum velociy is reached only a he single insan / = s, when half of he moion has been compleed Insead, when 10 is violaed, he minimum-ime moion is given by a bang-bang acceleraion profile ie, wih a riangular velociy profile having only he acceleraion/deceleraion phases, each of duraion s = A = s 11 4
he crusing phase wih maximum velociy is no reached in his case For all he above cases, when < 0 he opimal velociy and acceleraion profiles are simply changed of sign flipped over he ime axis a b a a > 0, b > 0 a b b a > 0, b < 0 b a a b c a < 0, b > 0 d a < 0, b < 0 Figure 1: Qualiaive asymmeric velociy profiles of he rapezoidal ype for he four combinaions of signs of he iniial and final velociy a and b I is assumed ha > 0, and ha his disance is sufficienly long so as o have a non-vanishing cruising inerval a maximum velociy = Consider now he problem of moving in minimum ime he join by a disance = b a > 0, bu wih generic non-zero boundary condiions 0 = a and = b on he iniial and final velociy he reuiremen ha a and b is obviously mandaory in order o have a feasible soluion Wih reference o he ualiaive rapezoidal velociy profiles skeched in Fig 1, we see ha non-zero iniial and final velociies may help in reducing he moion ime or work agains i In paricular, when boh a and b are posiive case a i is clear ha less ime will be needed o ramp up from a > 0 o, raher han from 0 o he same is rue for slowing down from o b > 0, raher han down o 0 On he conrary, when boh a and b are negaive case d, an exra ime will be spen for reversing moion from a < 0 o 0 in his ime inerval, he join will coninue o move in he opposie direcion o he desired one, unil i sops, when finally a posiive velociy can be achieved, and, similarly, anoher exra ime will be spen oward he end of he rajecory for bringing he velociy from 0 o b < 0 also in his second inerval, he join will move in he opposie direcion o he desired one Cases b and c in Fig 1 are inermediae siuaions beween a and d, and can be analyzed in a similar way 5
As a resul: in general, he acceleraion/deceleraion phases will have differen duraions 0 raher han he single s 0 of he res-o-res case; 0 and he original reuired disance o ravel > 0 will become in pracice longer, since we need o counerbalance he negaive displacemens inroduced during hose inervals where he velociy is negaive; since we need o minimize he oal moion ime, inervals wih negaive velociy should be raversed in he leas possible ime, hus wih maximum posiive or negaive acceleraion = ±A Wih he above general consideraions in mind, we perform now uaniaive calculaions In he posiive acceleraion and negaive deceleraion phases, we have = a A, = b A 1 We noe ha boh hese ime inervals will be shorer han s = /A for a posiive boundary velociy and longer han s for a negaive one he area wih sign underlying he velociy profile should provide, over he oal moion ime > 0, he reuired disance > 0 We compue his area as he sum of hree conribuions, using he rapezoidal rule for he wo inervals where he velociy is changing linearly over ime: a + + + + b = 13 Subsiuing 1 in 13 and rearranging erms gives + a a + A A + a + b + + b b = 14 A A Solving for he moion ime, we obain finally he opimal value = + a + b 15 A his is he generalizaion for > 0 of he minimum moion ime formula 9 of he res-o-res case which we recover by seing a = b = 0 his soluion is only valid when he disance o ravel > 0, he velociy and acceleraion limi values > 0 and A > 0, and he boundary velociies a and b saisfy he ineualiy a + b A 0, 16 which is again he generalizaion of condiion 10 his ineualiy is obained by imposing ha he sum of he firs and hird erm in he lef-hand side of 14, ie, he space raveled during he acceleraion and deceleraion phases, does no exceed a cruising phase a maximum speed > 0 would no longer be necessary I is ineresing o noe ha, for a given riple,, and A, he ineualiy 16 would be easier o enforce as soon as a 0 and/or b 0, independenly from heir signs he physical reason, however, is slighly differen for a posiive or negaive boundary velociy, say of a When a > 0, less ime is needed in order o reach he maximum velociy > 0; hus, i is more likely ha 6
he same problem daa will imply a cruising velociy phase Insead, when a < 0, a negaive displacemen will occur in he iniial phase, which needs o be recovered; hus, i is more likely ha a cruising phase a maximum velociy will be needed laer Finally, we poin ou ha: when ineualiy 16 is violaed, or for special values of a or b eg, a =, a number of sub-cases arise; heir complee analysis is ou of he presen scope and is lef as an exercise for he reader; for < 0, i is easy o show ha he formulas corresponding o 1, 15, and 16 are = + a A, = + b A, = + + a + + b, A a + b A Indeed, he velociy profiles in Fig 1 will use he value as cruising velociy [ /s]! = 90! a = 45 b = - 45 = 05 = 0675 [s]! [ /s ]! A = 00! * = 18958 = 05 cruise = 0996 = 0675 [s]! - 00! Figure : ime-opimal velociy and acceleraion profiles for he numerical problem in Exercise Moving o he given numerical problem, from = b a = 30 90 = 10 > 0, a = 45 /s, b = 45 /s, = 90 /s, and A = 00 /s, we evaluae firs he ineualiy 16 and verify ha 10 > 90 45 + 45 = 30375, 00 so ha he general formula 15 applies his yields while from 1 we obain = 05 [s], = 18958 [s], = 0675 [s], wih an inerval of duraion cruise = = 09958 [s] in which he join is cruising a = 90 /s he associaed ime-opimal velociy and acceleraion profiles are repored in Fig 7