Kinematics and kinematic functions

Transcription

1 ROBOTICS 01PEEQW Basilio Bona DAUIN Poliecnico di Torino

2 Kinemaic funcions

3 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 PosiionKF: from join space variables o ask space pose 2. Inverse PosiionKF: from ask space pose o join space variables 3. Direc VelociyKF: from join space velociies o ask space velociies 4. Inverse VelociyKF: from ask space velociies o join space velociies 3

4 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? How o compue he homogeneous ransformaion beween hese wo RF 4

5 Kinemaic funcions The firs sep 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 convenionswere inroduced o reducehe number of parameers and o find a common way o describe he relaive posiion of reference frames Denavi-Harenberg convenions were inroduced in 1955 and are sill widely used in indusry (wih some minor modificaions) 5

6 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 only 4 parameers beween wo successive RF, insead of he usual 6 2 parameers are associaed o a ranslaion, 2 parameers are associaed o a roaion Joins can be prismaic (P) or revolue (R); he convenion is always valid Three parameers depend on he robo geomery only, and herefore are consan in ime One parameer depends on he relaive moion beween wo successive links, and herefore is a funcion of ime We call his one he i-hjoin variableq () i 6

7 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 oward he base, one oward he TCP We wan o locae a RF on his arm link oward he base oward he TCP 7

8 DH convenion 2 Join 5 Join 4 Link 3 Join 6 Join 3 Link 2 Wris Link 1 Join 2 Shoulder Join 1 Link 0 = base 8

9 DH convenion 3 We use he erm moion axis o include boh revolue and prismaic axes Moion axis Moion axis g i + 1 Moion axis b i g b i i 1 Base g symbols b = i g = b i + 1 arm/link join We have his sequence b g b g b g b i N g i + 2 b i + 2 TCP 9

10 DH convenion 4 10

11 DH convenion 5 Moion axis Moion axis Moion axis 11

12 DH convenion 6 Moion axis Moion axis Moion axis π / 2 π / 2 12

13 DH convenion 7 Moion axis Moion axis Moion axis 13

14 DH convenion 8 Moion axis Moion axis Moion axis 14

15 DH convenion 9 Moion axis Moion axis Moion axis 15

16 DH convenion 10 Moion axis Moion axis Moion axis 16

17 DH rules 1 17

18 DH rules 2 18

19 DH rules 3 19

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

21 DH homogeneous roo-ranslaion marix T TR R 0 1 = (,) = T T i 1 i cosθ sinθcosα sinθsinα acosθ i i i i i i i sinθ cosθcosα cosθsinα a sinθ i i i i i i i = 0 sinα cosα d i i i T TIdTR (, ) (, 0TIaTR ) (, ) (, 0) i 1 = i k, θ i, α 21

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

23 From he exbook of Spong... 23

24 From he exbook of Spong... 24

25 From he exbook of Spong... 25

26 From he exbook of Spong... 26

27 Exercise The PUMA robo From above Laeral view 27

28 A procedure o compue he KFs 1 1. Selec and idenify links and joins 2. Define and place he RFs using DH convenions 3. Define he consan DH parameers 4. Define he variable join DH parameers i 1 i 5. Compue he homogeneous ransformaion and 6. Exrac he direc posiion KF from 7. Compue he inverse posiion KF 0 T TCP T 0 T TCP Inverse velociy KF: analyical or geomerical approach Inverse velociy KF: kinemaic singulariy problem 28

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

30 Join and caresian variables Join variables Task/caresian variables/pose q() q1() q2() q3() q4() = q 5() q6() p() x1() x2() x3() α1() = α2() α 3() posiion orienaion Direc KF ( ) p() = f q() Inverse KF q f p ( ) 1 () = () 30

31 Direc posiion KF 1 posiion q1() p1() q2() p2() q (()) 3() p3() x q q() = (()) q4() pq = p4() = q (()) 5() p5() α q q () p () 6 6 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 31

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

33 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 33

34 Inverse posiion KF 1 q() 1 q() 2 ( () ) x q q() 3 pq ( ()) = () q = q() The inverse KF are imporan, since conrol acion is applied o he join moors, while he ask o be done is defined as caresian posiions and orienaions q() 3 α( ) 4 () q q() 5 q() 6 q() 1 q() 2 x q α (() ) ( q() ) 34

35 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... how? 3. There are several possibiliies Use brue force or previous known soluions found for similar chains Use inverse velociy KF (recursive approach) Use symbolic manipulaion programs (compuer algebra sysems as Mahemaica, Maple, Maxima,, Lisp) NOT SUGGESTED Ieraively compue an approximaed numerical expression for he nonlinear equaion (Newon mehod or ohers) 1 = ( ) 1( ) { 1 q f ( p) } q() f p() q() f p() = 0 min () () 35

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

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

38 Direc velociy KF 3 qɺ () 1 d f f f (()) i i i f q() T q = (() ) () i j fi d = q q q ɺ J q qɺ 1 j n q () ɺn f f f JACOBIAN q q q 1 j n q() ɺ1 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 38

39 Furher noes on he Jacobians Velociy kinemaics is characerized by Jacobians There are wo ypes of Jacobians: Geomerical Jacobian Analyical Jacobian J g J a ( ) pɺ() = J q() qɺ() also called Task Jacobian The firs one is relaed o Geomerical Velociies v p xɺ ɺ = = ω Jqɺ g The second one is relaed o Analyical Velociies pɺ xɺ = = ɺα Jqɺ a 39

40 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 40

41 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ɺ( τ)dτ= dτ = ( τ) () ɺα α x x 41

42 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 1 ( ) = q( ) + J (( q )) v( ) k+ 1 k g k p k q 1 ( ) = ( ) + (( )) ( ) k+ 1 k a k k q J q pɺ 42

43 Furher noes on he Jacobians 43

44 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 of he origin 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 44

45 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 45

46 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 + SR ( ω ) R R i 1 i 1 i i 1 i 1, i i 1 i = ( ) + ( ) Sω SR ω 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 wrrf i-1 in RF i-1 ω angular velociy of RF i-1 in RF 0 46

47 Geomeric Jacobian 4 To compue he Geomeric Jacobian, one can decompose he Jacobian marix as: q LINEAR JACOBIAN ɺ 1 q xɺ L,1 L,2 Ln, 2 3 ( ) J J J ɺ v= = J qqɺ =, g J J R Li, 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 dx xɺ = qɺ = J qɺ dq i i= 1 i i= 1 Li, i indicaes how qɺ conribues o he angular velociy of he TCP i n ω= ω = i 1, i Ai, i i= 1 i= 1 47 n J qɺ

48 Srucure of he Jacobian LINEAR JACOBIAN ANGULAR JACOBIAN 48

49 Srucure of he Jacobian If he robo is non-redundan, he Jacobian marix is square If he robo is redundan, he Jacobian marix is recangular 49

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

51 Geomeric Jacobian 5 In conclusions, he elemens of he geomerical Jacobian can be compued as follows: Prismaic Revolue J J Li, Ai, k 0 i 1 k r k i 1 i 1, TCP i 1 51

52 Analyical Jacobian 1 Analyical Jacobian is compued deriving he expression of he TCP pose p(()) p(()) q q 1 1 p d (()) q 1 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 52

53 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α 53

54 Relaions beween Jacobians Euler angles α= { φθψ,, } RPYangles α= { θ, θ, θ} 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 join velociies 54

55 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-inversecan be usedo compue one of hem ( ) + qɺ() = J q() pɺ() def + T T 1 J = J ( JJ ) 55

56 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 56

57 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 57

58 Singulariy 2 For an ariculaed/anhropomorphic robo hreesingulariy 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 ε 58

59 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 59

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

61 Singulariy 5 C. Wris singulariy Euler wris Two axes are aligned RPY wris Two axes are aligned Two axes are aligned 61

62 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 62

63 Singulariy In pracice, when he joins are neara singular configuraion, o follow a finie caresian velociy he join velociies become excessively large Near singulariy condiions i is no possible o follow a geomeric pah and a he same ime a given velociy profile; 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 63

64 Conclusions Kinemaic funcions can be compued once he DH convenions are applied 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 singulariies 64