ROBOTICA. Basilio Bona DAUIN Politecnico di Torino. Basilio Bona ROBOTICA 03CFIOR 1

Transcription

1 ROBOTICA 03CFIOR Basilio Bona DAUIN Poliecnico di Torino 1

2 Conrol Par 1

3 Inroducion o robo conrol The oion conrol proble consiss in he design of conrol algorihs for he robo acuaors In paricular i consiss in generaing he ie funcions of he generalized acuaing orques, such ha he TCP oion follows a specified ask in he caresian space, fulfilling he specificaions on ransien and seady-sae response requireens 3

4 Tasks Two ypes of asks can be defined: 1. Tasksha do no require an ineracion wih he environen (free space oion); he anipulaor oves is TCP following caresian rajecories, wih consrain on posiions, velociies and acceleraions due o he anipulaor iself or he ask requireens Soeies i is sufficien o ove he joins fro a specified value q ( ) i 0 o anoher specified value q( ) i f wihou following a specific geoeric pah 2. Tasksha require and ineracionwih he environen, i.e., where he TCP shall ove in soe caresian subspace while subjec o forces or orques fro he environen We will consider only he firs ype of ask 4

5 Moion conrol In paricular he oion conrol proble consiss in generaing he ie funcions of he generalized acuaing orques, such ha he TCP oion follows a specified ask in he caresian space, fulfilling he specificaions on ransien and seady-sae response requireens Conrol schees can be developed for: Join space conrol Task space conrol considering ha he ask descripion is usually specified in he ask space, while conrol acionsare defined in he join space 5

6 Join space conrol The Inverse Kineaics block ransfors he desired ask space posiions and velociies ino desired join space reference values. The Transducereasures he value of he join quaniies (angles, displaceens) and copares he wih he desired ones, obained, if necessary, fro he desired caresian quaniies. The Conrolleruses he error o generae a (low power) signal for he Acuaor ha ransfors i in a (high power) orque (via he Gearbox) ha oves he robo joins 6

7 Task space conrol pd Conroller Acuaor Gearbox Robo p Transducer In his case, he Transducerus easure he ask space quaniies in order o copare he wih desired ones. Usually his is no an easy ask, since i requires environen-aware sensors; he os used ones are digial caera sensors (vision-based conrol) or oher ypes of exerocepive sensors (infra-red, ulra-sonic,...). Oherwise one uses he direc kineaics o esiae he ask space pose Torques are always applied o he joins, so Inverse Kineaics is hidden inside he Conroller block 7

8 Join space conrol archiecures Two ain join space conrol archiecures are possible Decenralized conrol or independen join conrol: each i-hjoin oor has a local conroller ha akes ino accoun only local variables, i.e., he join posiion q () and velociy qɺ() i i The conrol is of SISO ype, usually based on a P, PD or PID archiecure The conroller is designed considering only an approxiaed odel of he i- hjoin. This schee is very coon in indusrial robos, due o is sipliciy, odulariy and robusness The classical PUMA robo archiecure is shown in he following slide Cenralized conrol: here is only one MIMO conroller ha generaes a coand vecor for each join oor; i is based on he coplee odel of he anipulaor and akes ino accoun he enire vecor of easured posiions and velociies 8

9 Decenralized conrol Decenralized Join Conrol q() 1 join 1 reference conroller 1 Task space Join space join 2 reference conroller 2 q() 2 join 6 q() reference 6 conroller 6 9

10 Decenralized conrol Terinal Disk Teach pendan Oher PUMA Conrol µg D/A Aplifier Moor 1 DLV-11J T=0.875 s Encoder EPROM RAM Inerface T=28 s Reference angles CPU µg D/A Aplifier Moor 6 T=0.875 s Encoder COMPUTER ROBOT CONTROL 10

11 Moor and gearbox odel (rigid body assupions) N r= N Gearbox = Gearrain Gearbox Fricion N;, τ r Fricion Ineria τ Robo Ineria N;, τ r Moor τ Inpu power τ r τ r Oupu power 11

12 Moor and gearbox odel ia τ = τ + τ r β b R L a a N Γb p τ v a i e E τ Γ N τ p β τ = τ τ r p τ p 12

13 Losses in geared oor Moor side Join side v i a a Araure circui Eia Moor ineria τ r Gearbox τ r Join ineria τ d L i + R i d a a a a volage drop d Γ + β d τ p η gearbox efficiency d Γ + β d b b τ p 13

14 Gearbox odel τ r Join side Moor side τ r ρ θ θ ρ ρθ = ρθ ρ = ρ Power in τ r r Ideal gearbox: η = 1 GEARBOX ρ N = = ρ N r τ = rτ r r = r Power ou τ = η τ r 14

15 CC oor equaions 1 MOTOR SIDE d L i = v Ri E a a a a a d φ = K i φ e E = kφ = K τ = k φi = K i a τ a τ i = a K τ k k K K τ = Γ θɺɺ + βθɺ = Γ ɺ + β τ = τ τ p r p τ 15

16 CC oor equaions 2 MOTOR SIDE 1 τ = τ r r r 1 = ( τ + τ p ) r 1 = b b r τ + Γɺ + β = τ + Γ β b ɺ + b r r r 1 1 = τ + 2 ( Γɺ + β b b ) r r JOINT SIDE τ = rτ r r = r( τ τ ) p = r τ Γ β ɺ = r τ Γ ( r ) β ( r ) ɺ = rτ r 2 ( Γ ɺ + β ) 16

17 Conrol equaions T Hqq ( ) ɺɺ + Cqqq (, ɺ) ɺ + Bqq ( ) ɺ + gq ( ) + J F = τ e coponen-wise join orques n n n H ( q) qɺɺ + h ( q) q qɺ + β qɺ + g( q) + τ = τ j= 1 j= 1 k= 1 ij j ijk j k bi i i fi ri H i n n n ( q) qɺɺ + H ( q) qɺɺ + h ( q ) qɺɺ q + β qɺ + g() q + τ = τ i r i ij j ijk j k bi i i fi j i j= 1 k= 1 i Inerial orques Coriolis & cenripeal orques Fricion, graviy & exernal orques 17

18 Conrol equaions n n n H ( q) qɺɺ + H ( q) qɺɺ + h ( q) qɺɺ q + β qɺ + g( q) + τ = τ ii i ij j ijk j k bi i i fi j i j= 1 k= 1 ri H ( qɺɺ ) q + β qɺ + τ + τ + τ + τ = τ ii i bi i Mi ci gi fi r i Modelled orques Disurbance orques 18

19 Fro single oor odel o robo conrol equaion q θ = q = ɺ ɺ qɺɺ = i i i i i i r r r i i i Gearbox ransforaion n + τ = H q ɺɺ + β q ɺ + H q ɺɺ + τ + τ + τ + τ ri ii i bi i ij j Mi ci gi fi j i = = Γ Γ ɺ + β + τ + τ + τ + τ i i bi bi Mi ci gi fi r r i i ɺ + β + τ i i bi bi di r r i i Equaion seen a he join side Srucured disurbance 19

20 Fro single oor odel o robo conrol equaion Equaion seen a he oor side since and τ = τ = ( Γ ɺ + β ) + τ r r r ri ri 2 bi i bi i di i i i ( β ) τ = τ τ = τ Γ ɺ + ri i pi i i i i i we obain τ = τ + τ = Γ Γ β β + ɺ τ r r r i ri pi 2 bi i i 2 bi i i di i i i Toal ineria Toal fricion 20

21 Fro single oor odel o robo conrol equaion τ = τ + τ = Γ ɺ + β + τ i pi di i i i i di τ = τ + τ = Γ ɺ + B + τ p d d Moor side Γ B = bi i r Γ + Γ 2 i 1 bi i r β + β 2 i 21

22 Fro single oor odel o robo conrol equaion τ = τ + τ = Γ ɺ + β + τ i pi di i i i i di τ = τ + τ = Γ ɺ + B + τ p d d Join side Γ = + + B ( 2 Γ + rγ ) ( 2 β + rβ ) bi i i bi i i 22

23 Block diagra of open-loop CC oor (oor side) For sipliciy we drop he prie sybol for he oor side quaniies, and we consider he generic i-h oor Taking he Laplace ransfor of he involved variables, we have ( sγ + β) () s = τ () s τ() s τ d = K i τ a τ d θ 1 () s = () s s v a + E R a 1 + sl a ia τ τ r 1 1 K τ + β + sγ s θ K 23

24 Block diagra of open-loop CC oor (oor side) The araure circui inducance is sall and usually can be negleced L i K 0 v R = a a a a i a = τ K τ τ v R = K a a Kτ τ = τ + Γɺ + β d R R R v τ = Γ s + K + β a a a a d K K K τ τ τ 24

25 Block diagra of open-loop CC oor (oor side) Rβ = + a K K K Kτ since Rβ a K τ R i a a araure losses K β K K i τ a fricion orque orque τ araure e..f. 25

26 Block diagra of open-loop CC oor (oor side) RΓ a 1 Ra s 1 + v K K = K K K a d τ τ K τ d d τ where RΓ a T = K K τ Ra K = d K K τ va G ( s) + 1 s θ ( ) G s 1 = K + ( 1 st) 26

27 Marix forulaion (join side) Lagrange Equaion where R τ T Hqq ( ) ɺɺ+ Cqqq (, ɺ) ɺ + Bqɺ + gq ( ) + J ( qf ) = τ τ = R ( τ τ ) r p = RKv + RKqɺ 2 a a e τ R R qɺɺ Bqɺ R qɺɺ Bqɺ 2 τ = ( Γ + ) = ( Γ + ) p and Moor side Join side K K K i i i R = 0 r 0; 0 τ 0; 0 τ 0 i K = a R K = R ai a 0 0 i

28 Marix forulaion (join side) Then, we have Mass arix Mq ( ) Fricion arix F ( 2 ) ( 2 ( ) (, ) ( )) Hq + RΓ qɺɺ + Cqqɺ + B + R K + B qɺ + Graviy T + gq ( ) + J ( qf ) e = RKv a a Ofen we use his sybol o indicae he velociy dependen ers ( ) hqq (, ɺ) = Cqq (, ɺ) + F qɺ Ineracion u c Coand inpu 28

29 Marix forulaion (join side) Graviy and ineracion No ineracion T Mqq ( ) ɺɺ+ hqq (, ɺ) + gq ( ) + J ( qf ) = u No graviy, no ineracion Mqq ( ) ɺɺ+ hqq (, ɺ) + gq ( ) = uc Mqq ( ) ɺɺ+ hqq (, ɺ) = uc e c Conrol Design Proble u =...? c 29

30 Decenralized join conrol Assuing, for sipliciy Mqq ( ) ɺɺ+ hqq (, ɺ) = uc If ( 2 ) 2 ( ) Γ RΓ diagonal R I Hq + R Cqqq (, ɺ) ɺ sall Then Γ disurbance 2 qɺɺ + Fqɺ + τ = u wih Γ =RΓ d c ( Γ ) s + F = u c The odel is diagonal, i.e., naurally decoupled Each join can be conrolled by local conrollers 30

31 Decenralized join conrol This is he proporional velociy conroller or velociy copensaor τ d reference volage K d vr + e K D va + G ( s ) 1 s θ ( ) K s Transducer T.F. (achieric sensor) ( ) K s K 31

32 Open loop vsclosed loop () s 1 = G() s = v() s K (1 + st) a () s αk D = G () s = v() s K (1 + sαt) r () s T = G() s = K G() s = d d τ () s Γ (1 + st ) d () s αk K d d αt = G () s = = G () s = d τ() s K (1 + sαt) K Γ(1 + sαt) d D α K = < K + K K D 1 32

33 The closed-loop syse τ d K d K d τ d v a + G ( s ) 1 s θ Open loop K D vr G ( s) + 1 s Closed loop θ 33

34 The closed-loop syse Tie consan is reduced α T < T Disurbance gain is reduced K d K K d D τ d Design paraeer whenk = 1 K d vr + e K D va + G ( s ) 1 s θ 34

35 Posiion copensaor Conroller 1 τ d K d θ r + e K P + K D va + G ( s ) 1 s θ K K 1 K θ K θ 1 35

36 Posiion copensaor θ τ θ θ r () s () K = G() s = s s sαt K () s 1 = G() s = 2 2 () s d Γ( s + sαt + K) where K K K = = TK K K K D P D P R Γ a τ Configuraion dependen Second-order TF wih ζ 1 K K τ 1 = = 2αT K 2α R K K K Γ a D P τ n = K = K K K D R P a τ 1 Γ 36

37 Design paraeers The daping coefficien and he naural frequency are inversely proporional o he square roo of he ineria oen, ha ay vary in ie when he angles vary 1 Γ = Γ Γ + 2 b r Γ = H q() b ii ( ) Since he daping coefficien and he naural frequency are ofen used as conrol specificaions, we can design a conroller copuing he axiu ineria oen Γ and adjusing he wo gains K,ax, K P D in such a way ha he daping raio ζ is saisfacory, e.g., no overshoo appears in he sep response 37

38 An alernaive τ d Conroller 2 K d θr + e K + sk P D va + G ( s ) 1 s θ v() = K e() + K eɺ() a P D θ () s K K s + K / K D τ P D = G() s = 3 2 θ() s RΓ r a s + sαt + K K RΓ P τ a θ () s 1 1 = G() s = 4 2 τ() s Γ d s sαt K K RΓ + + P τ a A zero appears 38

39 Anoher alernaive τ d Conroller 3 K d θr + e K + sk P D + K D v a + G ( s ) 1 s θ 39

40 Coparison Conroller 1 v() = K K e() K () a D P D Conroller 2 v() = K e() + K () K () a P D r D Conroller 3 ( ) v() = K K e() + K K () K K 1+ 1 K () a D P D D r D D D 40

41 Pracical Issues 1. Sauraing acuaors 2. Elasiciy of he srucure 3. Nonlinear fricion a joins 4. Sensors or aplifiers wih finie band 41

42 Sauraing Acuaors I is a nonlinear effec, difficul o be considered a-priori y() sauraion u() y() u() sauraion Lineariy y IF u() > u ax ax y() = ku() IF u u() u in y IF u() < u in in 42 ax

43 Elasiciy of he srucure Alhough we have considered rigid bodies, he elasiciy is a phenoenon ha liis he closed loop band We canno design conrollers ha are oo fas wihou aking explicily ino consideraion soe sor of elasic odel. Recall ha when we use a siplified odel Γθɺɺ () + kθ () = 0 he proper srucural resonance (or naural) frequency is r = k Γ 43

44 Elasiciy of he srucure 44

45 Nonlinear fricion A nonlinear effec beween velociy and fricion force f() oal v() sicion viscous coulob f() v() 45

46 Nonlinear fricion Nonlinear saic fricion odels include: Coulob + viscous F(v) = F sign(v) +βv c Saic odel ha includes sicionand Sribeckeffec : fricion decreases wih increasing velociy for v < v s (Sribeck velociy) δ s v ( ) v s F(v) = F + F F e sign(v) + βv c s c 46

47 Finie pass-band in sensors and aplifiers Sensors and aplifiers are ofen odelled as siple gains, while in he real world hey have a finie pass-band, nonlineariies, sauraions, ec. These effecs us be aken ino accoun when he siulaed and real behaviours differ. Forunaely, very ofen he band of sensors and aplifiers is uch wider han he final closed loop band of he conrolled syse. Basilio Bona ROBOTICA 03CFIOR 47