Basilio Bona ROBOTICA 03CFIOR 1

Transcription

1 Indusrial Robos Kinemaics 1

2 Kinemaics and kinemaic funcions Kinemaics deals wih he sudy of four funcions (called kinemaic funcions or KFs) ha mahemaically ransform join variables ino caresian variables and vice versa 1. Direc Posiion KF: from join space variables o ask space pose 2. Inverse Posiion KF: from ask space pose o join space variables 3. Direc Velociy KF: from join space velociies o ask space velociies 4. InverseVelociyKF: fromaskspace space velociiesojoino space velociies 2

3 Kinemaic funcions Posiions and velociies of wha? I can be he posiion or velociy of any poin of he robo, bu, usually, is he TCP posiion and velociy TCP R P BASE R 0? Wha is he relaion beween hese wo RF? 3

4 Kinemaic funcions The firs sep for defining KFs is o fix a reference frame on each robo arm In general, o move from a RF o he following one, i is necessary o define 6 parameers (hree ranslaions of he RF origin + hree angles of he RF roaion) A number of convenions were inroduced o reduce he number of parameers and o find a common way o describe he relaive posiion of reference frames Denavi Harenberg convenions (1955) were he firs o be inroduced and are widely used in indusry (wih some minor modificaions) 4

5 How o define he robo reference frames? A RF is posiioned on each link/arm, using he so called Denavi Harenberg (DH) convenions The convenion defines dfi only 4 DOF beween wo successive RF, insead of he usual 6 Joins can be prismaic (P) or roaional (R); he convenion is always valid 2 parameers are associaed o a ranslaion, 2 parameers are associaed o a roaion Three ou of four parameers depend only on he robo geomery and herefore are consan in ime One depends on he relaive moion beween wo successive links, and herefore is a funcion of ime. We call his he i h join variable q () i 5

6 DH convenion 1 Assume a conneced mulibody sysem wih n rigid links The links may no be necessarily symmeric Each link is conneced o wo joins, one upsream (oward he base), one downsream (oward he TCP) We wan o locae a RF on his arm link Upsream join Downsream join 6

7 DH convenion 2 Join 5 Join 4 Link 3 Join 6 Join 3 Link 2 Wris Link 1 Join 2 Shoulder b = i g = i arm/link join Link 0 = base Join 1 7

8 DH convenion 3 We use he erm moion axis o include boh revolue and prismaic axes Moion axis g i Moion axis bi g i + 1 b i g Moion axis g i + 2 bi 1 b i+ 2 Base We have his sequence b g b g b g b N TCP 8

9 DH convenion 4 9

10 DH convenion 5 Moion axis Moion axis Moion axis 10

11 DH convenion 6 Moion axis Moion axis Moion axis 11

12 DH convenion 7 Moion axis Moion axis Moion axis 12

13 DH convenion 8 Moion axis Moion axis Moion axis 13

14 DH convenion 9 Moion axis Moion axis Moion axis 14

15 DH convenion 10 Moion axis Moion axis Moion axis 15

16 DH rules 1 16

17 DH rules 2 17

18 DH rules 3 18

19 DH parameers DH parameers define he ransformaion Ri 1 Ri Depending on he join ype g i Prismaic join q () i Roaion join d ( ) q () θ ( ) Join variables i θ, a, α = fixed d, a, α = i i i i i i fissi Geomerical parameers calibraion i i 19

20 DH homogeneous roo ranslaion marix R T T R T 0 1 = T ( R, ) = T T i i cos θ sin θ cos α sin θ sin α a cos θ i i i i i i i sin θ cos θ cos α cos θ sin α a sin θ 1 i i i i i i i = 0 sinα cosα d i i i T T ( I, d ) T ( R, 0) T( I, at ) ( R, 0) i 1 = i k, θ i, α 20

21 From he exbook of Spong... Mark W. Spong, Seh Huchinson, and M. Vidyasagar Robo Modeling and Conrol John Wiley & Sons,

22 From he exbook of Spong... 22

23 From he exbook of Spong... 23

24 From he exbook of Spong... 24

25 From he exbook of Spong... 25

26 Exercise The PUMA robo From above Laeral view 26

27 A procedure o compue he KFs 1 1. Selec and idenify links and joins 2. Se all RFs using he DH convenions 3. Define he consan geomerical parameers 4. Dfi Define he variable ibl join parameers 5. Compue he homogeneous ransformaion beween he base RF and he TCP RF 6. Exrac he direc posiion KF from he homogeneous ransformaion 7. Compueheinverse he posiion KF 8. Inverse velociy KF: analyical or geomerical approach 9. Inverse velociy KF: kinemaic i singulariy i problem 27

28 A procedure o compue he KFs 2 1. Selec and idenify links and joins 2. Se all RFs using DH convenions 3. Dfi Define consan geomerical parameers q () 4 q () 5 q () 2 q () 6 q () 1 R P TCP BASE R 0 0 T P T () q = R R () q () q P P P T

29 Join and caresian variables Join variables Task/caresian variables/pose q 1() x 1() q2() x 2() q3() x 3() q() = q4() p() = α1() q5() α 2() q6() α3() posiion orienaion Direc KF Inverse KF 1 p() = f ( q () ) q() = f ( p() ) 29

30 Direc posiion KF 1 q1() p1() q 2() p2() q ( ()) 3() p3() x q q() = ( ()) = = p q q 4() p4() q ( ()) 5() p5() α q q6() p6() posiion orienaion P P P 1 ( q1) 2 ( q2) 3 ( q3) 4 ( q4) 5 ( q5) 6 ( q6) R T = T T T T T T T P = 1 0 T orienaion posiion R = R ( q () ) = ( q () ) P P P P 30

31 Direc posiion KF 2 T 0 P 0 0 RP P = 0 T 1 Direc caresian posiion KF: easy x x 1 1 x 0 2 P 2 x 3 3 Direc caresian orienaion KF: no so easy o compue, bu no difficul We will solve he problem in he following slides 31

32 Direc posiion KF 3 ( () ) α( q() ) Rq We wan o compue angles from he roaion marix. Bu i is imporan o decide which represenaion o use α ( q() )? Euler angles RPY angles Quaernions Axis angle represenaion 32

33 Inverse posiion KF 1 q () 1 q () ( ) 2 () x q q () 3 pq ( ( )) = = q( ) q () α ( ) 4 () q q () 5 This KF is imporan, since conrol acion are applied o he join moors, while he user wans o work wih q () 6 caresian posiions and orienaions q () 3 q () 1 q () 2 ( () ) x q α( q() ) 33

34 Inverse posiion KF 2 1. The problem is complex and here is no clear recipe o solve i 2. If a spherical wris is presen, hen a soluion is guaraneed, bu we mus find i There are several possibiliies Use brue force or previous soluions found for similar chains Use inverse velociy KF Usesymbolicmanipulaion symbolic manipulaion programs (compuer algebra sysems as Mahemaica, Maple, Maxima,, Lisp) Ieraively compue an approximaed numerical expression for he nonlinear equaion (Newon mehod or ohers) ( ) 1 ( ) { 1 q() f ( p() ) } q f p 1 () = () q() f p() = 0 min () () 34

35 Direc velociy KF 1 Linear and angular direc velociy KF Non redundan robo wih 6 DOF q () p () 1 1 q () p () 2 2 (), () x q q q () p () 3 3 q () = () q () p = p () = 4 4 (), () q q q () p () 5 5 q () p () 6 6 ( ) Linear velociy ( (), () ) vq q = (), () ω q q α ( ) ( ) Angular velociy 35

36 Direc velociy KF 2 A bi brief review of mahemaical i noaions i General rule d pq (()) d d ( () ) x q d = d ( () ) α q d d d f ( q () ) = f ( q (),, q (),, q () ) i i 1 j n d d f i f i f i = q () + + q () + + q () 1 j n q q q 1 j n 36

37 Direc velociy KF 3 q () 1 d f f f ( ()) i i i f q () T q = ( () ) () i = j fi J q q d q q q 1 j n q () n f f f q q q 1 j n q () 1 JACOBIAN d ( ) f f f i i i f q() = q () ( () ) () j d = q q q J q q 1 j n q () n f f f m m m q q q 1 j n 37

38 Furher noes on he Jacobians Velociykinemaics is characerizedby Jacobians There are wo ypes of Jacobians: Geomerical Jacobian Analyical ljacobian J g J a ( ) p () = J q() q () also called Task Jacobian The firs one is relaed o Geomerical Velociies v p x = = ω J q g The second one is relaed o Analyical Velociies p x = = α α J q a 38

39 Geomerical and Analyical velociies Wha is he difference beween hese wo angular velociies? On he conrary, linear velociies do no have his problem: analyical and geomerical velociies are he same vecor, ha can be inegraed o give he caresian posiion 39

40 Furher noes on he Jacobians Moreover i is imporan o remember ha in general no vecor u () exiss ha is he inegral of ω()? u() ω() The exac relaion beween he wo quaniies is: ω () = θ () u () + sin θ() u () + 1 cos θ() S u() u () While i is possible o inegrae p p () ( ) ( ) ( ( τ ) ( ) p ( τ)dτ = dτ ( τ) = ( ) α α x x 40

41 Furher noes on he Jacobians The geomerical Jacobian is adoped every ime a clear physical inerpreaion of he roaion velociy is needed The analyical Jacobian is adoped every ime i is necessary o rea differenial quaniies in he ask space Then, if one wans o implemen recursive formula for he join posiion q ( ) = q ( ) + q ( ) Δ k+ 1 k k he can use or q q J q v Δ 1 ( ) = ( ) + ( ( )) ( ) k + 1 k g k p k q q J q p Δ 1 ( ) = ( ) + ( ( )) ( ) k+ 1 k a k k 41

42 Furher noes on he Jacobians 42

43 Geomeric Jacobian 1 The geomeric Jacobian can be consruced aking ino accoun he following seps a) Every link has a reference frame defined according o DH convenions R i R i b) The posiion ii of he origin ii of is given by: g i g i + 1 R i 1 b i i 1 ri 1, i R i x i 1 x i R 0 x = x + R r = x + r 0 i 1 i i 1 i 1 i 1, i i 1 i 1, i 43

44 Geomeric Jacobian 2 Derivaion wr ime gives x = x + R r + ω R r 0 i 1 0 i 1 i i 1 i 1 i 1, i i 1 i 1 i 1, i = x + v + ω r i 1 i 1, i i 1 i 1, i Linear velociy of R wr R Angular velociy of R i i 1 i 1 Remember: R = S( ω) R = ω R 44

45 Geomeric Jacobian 3 If we derive he composiion of wo roaions, we have: R = R R R R R R R 0 0 i 1 i i 1 i 0 0 i 1 0 i 1 = + i i 1 i i 1 i 0 i 1 0 i 1 = S( ω ) R R + R S( ω ) R i 1 i 1 i i 1 i 1, i i 0 i i 1 = S ( ω ) R R + S ( R ω ) R R i 1 i 1 i i 1 i 1, i i 1 i = ( ) + ( ) S ω S R ω R 0 0 i 1 i 1 i 1, i i Hence: 0 ω = ω +R ω i i 1 i 1 i 1, i angular velociy of RF i in RF 0 angular velociy of RF i wr RF i 1 in RF i 1 angular velociy of RF i 1 in RF 0 45

46 Geomeric Jacobian 4 To compue he Geomeric Jacobian, one can decompose he Jacobian marix as: LINEAR JACOBIAN q 1 q x L,1 L,2 L, n 2 3 ( ) J J J = = = v J q q, J J R g,, L i Ai ω J J J A,1 A,2 An, ANGULAR JACOBIAN q n J indicaes how q conribues o he linear velociy of he TCP J Li, A,1 i n n d x x = q = J q, dq i L i i i= 1 i i= 1 indicaes how q conribues o he angular velociy of he TCP i n = = 46 n ω ω J q i 1, i Ai, i i= 1 i= 1

47 Srucure of he Jacobian LINEAR JACOBIAN ANGULAR JACOBIAN 47

48 Srucure of he Jacobian If he robo is non redundan, he Jacobian marix is square If he robo is redundan, d he Jacobian marix is recangular 48

49 Geomeric Jacobian 5 J If he join is prismaic q = k d = Li, i i 1 i Li, i 1 ω = 0 J = 0 i 1, i Ai, If he join is revolue J R J q = ω r = ( k r ) θ Li, i i 1, i i 1, p i 1 i 1, p i ω = k θ i 1, i i 1 i J = k r k J R i 1 x i 1 1 R 0 Li, i 1 i 1, p = k Ai, i 1 x TCP r i 1,TCP R TCP All vecors are expresses in R 0 r is he vecor ha represens x x in R ( ) i 1, TCP TCP i

50 Geomeric Jacobian 5 In conclusions, in a geomerical Jacobian, is elemens can be compued as follows: Prismaic Revolue J J Li, Ai, k 0 i 1 k r k i 1 i 1, p i 1 50

51 Analyical Jacobian 1 Analyical Jacobian is compued deriving he expression of he TCP pose p (()) p (()) q q 1 1 p d ( ( )) 1 q q q xq 1 n 1 d p = = = d ( ( )) α q p (()) (()) 6 d p q p q q 6 6 n q q 1 n J (()) q a 51

52 Analyical Jacobian 2 The firs hree lines of he analyical Jacobian are equal o he same lines of he geomeric Jacobian The las hree lines are usually differen from he same lines of he geomeric Jacobian These can be compued only when he angle represenaion has been chosen: here we consider only Euler and RPY angles A ransformaion marix relaes he analyical and geomeric velociies, and he wo Jacobians ω = T( ( α ) α I 0 J ( q) = ( ) g a ( ) J q 0 T α 52

53 Relaions beween Jacobians Euler angles α = { φθψ,, } RPY angles α = { θ, θ, θ } x y z 0 cosϕ sinϕsinθ T( α) = 0 sinϕ cosϕsinθ 1 0 cosθ cos θ cos θ sin θ 0 y z z T ( α ) == cos θ sin θ cos θ 0 y z z sin θ 0 1 y The values of α ha zeros he marix T(α) deerminan correspond o a orienaion represenaion singulariy This means ha here are (geomerical) angular velociies ha canno be expressed by 53

54 Inverse velociy KF 1 When he Jacobian is a square full rank marix, he inverse velociy kinemaic funcion is simply obained as: ( ) q J q p 1 () = () () When he Jacobian is a recangular full rank marix (i.e., when he roboic arm is redundan, bu no singular), he inverse velociy kinemaic funcion has infinie soluions, bu he (righ) pseudo inverse can be used o compue one of hem ( ) q () = J + q() p () def + T T 1 J = J ( JJ ) 54

55 Inverse velociy KF 2 If he iniial join vecor q(0) ) is known, inverse velociy can be used o solve he inverse posiion kinemaics as an inegral = + τ τ = + Δ 0 k+ 1 k k q ( ) q (0) q ( )d q ( ) q ( ) q ( ) Coninuous ime Discree ime 55

56 Singulariy 1 A square marix is inverible if ( ) de J q () 0 When ( ) de J q () = 0 S a singulariy exiss a q () S This is called a singular/singulariy configuraion 56

57 Singulariy 2 For an ariculaed/anhropomorphic robo hree singulariy condiions exis A. compleely exended or folded arm B. wris cener on he verical C. wris singulariy When join coordinaes approach singulariy he join velociies become very large for small caresian velociies q = J ( qp ) = Jp Jp dej ε 57

58 Singulariy 3 A. Exended arm The velociies span a dim 2 space (he plane) The velociies span a dim 1 space (he angen line) i.e., singulariy 58

59 Singulariy 4 B. Wris cener on he verical hese velociies canno be obained wih infiniesimal join roaions 59

60 Singulariy 5 C. Wris singulariy Singulariy condiion Two axes are aligned 60

61 Euler Angles (wris) singulariy 3 angles Le us sar from he symbolic marix cc scs cs scc ss φ ψ φ θ ψ φ ψ φ θ ψ φ θ sc + ccs ss + ccc cs φ ψ φ θ ψ φ ψ φ θ ψ φ θ ss cs c ψ θ ψ θ θ cos( φ + ψ ) sin( φ + ψ ) Observe ha if = 1 θ = 0 c θ we have cc ss cs sc 0 φ ψ φ ψ φ ψ φ ψ sc cs cc ss 0 + φ ψ φ ψ φ ψ φ ψ angle only 61

62 Singulariy In pracice, when he joins are near a singulariy configuraion, he ask space velociy may become excessively large, alhough he joins are no exacly in singulariy Near singulariy condiions i is no possible o follow a geomeric pah and a he same ime a given velociy profile; fl i is necessary o reduce he caresian velociy and follow he pah, or o follow he velociy profile, bu follow an approximaed pah In exac singulariy condiions, nohing can be done so avoid singulariy 62

63 Conclusions Kinemaic funcions can be compued once he DH convenions are used Inverse posiion KF is complex Direc velociy KF has he problem of angular velociies: analyical vs geomeric Jacobians Inverse velociy can be compued apar from singulariy poins Avoid singulariy i 63