Dipartimento di Elettronica Informazione e Bioingegneria Robotics

Transcription

1 Diprimeno di Eleronic Inormzione e Bioingegneri Roboics From moion plnning o 015

2 robo clssiicions Robos cn be described by Applicion(seelesson1) Geomery (see lesson mechnics) Precision (see lesson perormnce) Trjecory Conrol Auonomy

3 Trjecory: Sop-o-Sop A B cuor In his exmple: pneumic joins, sop-o-sop open loop cuor NO COMPUTATION REQUIRED

4 Trjecory: Poin-o-Poin join 4 1 rge poins Posiions ken rom n exmple o he pplicion Robo mnully moved (joysick, eleconrol ) join 1 join Posiions in join spce sved nd repeed bck join

5 Memory poin-o-poin Posiion 1 1 Posiion Posiion 1000 { θ ϕ β B 1 { θ ϕ β B { θ ϕ β B 1014 join ngles join vlues in consecuive memory locions In ply bck he seuence o poins is sen o he cuors using some inerpolion o obin smooh rjecory memory ddress

6 Trjecory: Conrolled Compued (possibly in rel-ime) To hve smooh movemen To ollow given geomeric speciicions To pss hrough given poins To void obscles? Usully hs o-line preprion needs compuing power

7 Trjecory: Coninuous Used in pining, welding, ec 4 1 poin-o-poin - ew poins re sored; he conroller pplies simple compuion o genere he inermedie rjecories Thousnd o poins sored coninuous rjecory - he poins re sored he conroller re

8 rjecories Sop-o-sop Poin-opoin Conrolled Coninuous Simple conroller Limied versiliy Pick-nd-plce Simple progrmming Low in memory use Unpredicble rjecory beween poins Cn genere ny rjecory shpe Reuires compuion nd more complex conroller Cn use sensors Simple progrmming (on ield). Simple conroller Huge memory reuiremens No dpion o exernl sensors

9 Why, where nd how o compue conrolled rjecories?

10 Plnning hierrchy Tsks Tsk Pln Acion Pln Ph Pln Trjecory Pln Moion plnning Plnning is hierrchy o reinemens The robo conroller usully receives edbck rom inernl sensors The rjecory plnner should receive eedbck rom exernl sensors Conroller Robo Sensor

11 Moion plnning Find collision ree ph rom sr o gol Speciic ph (liner, opiml ec...) Possible obscles O line plnning execuion No obscles Preprion execuion No speciic ph Posiion conrol Upde obscles Posiion conrol

12 Ph nd rjecory Ph (geomeric speciicion) Geomery vi poins (lso used o bypss obscles) geomeric shpe (or insnce circumerence, ) minimum lengh does no cre bou robo dynmics Trjecory (emporl speciicion ) uncion o ime o genere se poins Needed by he regulor Cn genere posiion, velociy, ccelerion

13 Invlid rjecories obscle A (θ 0, 0 ) Move rom A o B wys We cn choose B (θ, ) The shores The one h voids obscles 1 link robo

14 Invlid rjecories - disconinuiy Joins sr ogeher he sme velociy gol sr disconinuiy join1 sops when here joins move ogeher o he sme moun gol sr 0 θ 1 mkes 50 θ mkes 70 Joins move seuenilly join1, hen join join hen join 1 gol gol sr sr

15 Trjecory plnning obscles Geomeric speciicions Trjecory plnner joins... θ() θ () θ () Cresin p(), v(), () Do no consider obscles

16 Trjecory preprion - join spce Given sr nd gol in Cresin spce vi poins hrough IK rnsorm he poins in join spce mke rjecory inerpolion in join spce: ll he joins move ogeher nd rech he poins he sme ime insn he ime sr, vi poins, nd gol is he sme or ll he joins In he join spce he rjecory is deined s expressions or bles compile ime i he world is known Ech join hs is own rjecory, he uncion h i ()

17 run-ime generion o se poins uncion h() is used o genere posiion, velociy, (ccelerion) or he nex ime sep or given join Ech join hs his h() uncion 0 e re he sme or ll he joins 0; loop: wi or nex ime sep Δ; h(), h. () join posiion nd velociy ime ; i hen end else go loop; end:

18 in Cresin spce Use he uncion H() o produce he hnd rjecory. Needs IK in he loop 0; loop: wi or nex ime sep Δ; H() hnd ; Θ(H()) join soluion or H(); i hen end else go loop; end:

19 run-ime generion o se poins A ech ime sep compue θ() rom IK compue velociy hrough numericl diereniion θ. () (θ() - θ(-d))/d he sme (i needed) or ccelerion

20 Simple uncion in join spce θ() coninuiy in posiion nd velociy join θ θ 0 0 ime

21 Cubic polynomils 1. Robo res (ime 0). Move join rom A o B in ime. Sop he robo (ime ) 4 consrins θ( ) 0 0 θ θ ( 0) 0 In A null posiion nd velociy o θ 0 θ( ) θ θ ( ) 0 In B null posiion nd velociy o θ Deermine he 4 coeiciens θ( ) 0 1

22 Deermining he coeiciens θ() 0 1 irs derivive 1 (second derivive 6 ) 4 euions o solve o ind he 4 coeiciens θ0 0 θ θ ( θ θ ) 0 ( θ θ ) 0

23 Trjecories or cubic uncion grd grd/sec grd/sec p 0 0 sec 0 sec sec Posiion, velociy, ccelerion proiles

24 Vi poins Add vi poins (o void obscles) Do no sop in vi poins join vi poin vi θ vi θ( ) B A vi1 vi ime

25 Do no sop in vi poin I we know he join velociy in he vi poin Use he bsic mehod, jus in he consrin euion use he given velociy insed o 0 For ech piece o he rjecory: θ (0) θ 0 θ ( ) θ θ. (0) θ. 0 θ. ( ) θ.

26 coeiciens The coeicien or ech piece re: 0 θ 0 1 θ. 0 / (θ -θ 0 ) - / θ. 0-1/ θ -/ (θ -θ 0 ) 1/ (θ. θ. 0 ) Imporn: ech piece is compued rom ime 0

27 How o speciy velociy in vi poin 1. The user speciies he velociy (or insnce rom he Cresin speciicion o liner nd ngulr velociy). This mehod is compuionlly hevy (Jcobin) nd hs o mnge he singulr poins. The sysem uses heurisics o deine he velociy. The sysem chooses he velociy in such wy o minin coninuous ccelerion in he vi poin

28 Cse coninuiy in ccelerion poins or he join θ 0 θ v θ cubics θ 1 () rom θ0 o θv θ () 0 1 rom θv o θ 0 o 1 or he irs cubic 0 o or he second cubic

29 consrins Posiion θ0 10 θv θv 0 θ 0 1 Velociy Eul ccelerion in vi poin euions, 8 unknown

30 coeiciens 0 θ θ θ 10 θ v θ θ θ v θ θ θ v θ θ θ v θ θ θ

31 Adding consrins on ccelerion ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) i i i i i i i i i i i i i i i i i i i i i 5 grde polynomils We cn impose lso iniil nd inl ccelerions This soluion hs numericl insbiliies no used

32 Oher mehods in join spce Liner uncions wih prbolic connecions Liner uncions in he join posiions reuires ininie ccelerions sr nd gol Add prbolic pr Obin dieren proiles θ Rccordo gol vi poins re no reched wih precision gol liner sr Rccordo 1 Sr

33 Trpeziodl proile o velociy i m m c m c m c m c m c c c m c c m c Pr consn velociy c m c m c d c o c c c 0 cos. posiion ccelerion For indusril robos

34 Trjecory or pick nd plce 1. Iniil posiion. Iniil velociy. Iniil ccelerion 4. Deproching posiion 5. Coninuiy in posiion 1 6. Coninuiy in velociy 1 7. Coninuiìy in ccelerion 1 8. Appochig posiion 9. Coninuiy in posiion 10. Coninuiy in velociy 11. Coninuiy in ccelerion 1. Finl posiion 1. Finl velociy 14. Finl ccelerion Polinomil uncions o grde 1-h! No ccepble Find more polynomil o low degree: 4--4 or he pr o he rjecory

35 In prcice The progrmmer deines vi poins o void obscles Trjecories re Compued run ime, upded Ph Upde Re PUMA 6Hz, Phnom 800Hz Or precompued compile ime nd sored in bles no use o sensors Only velociy nd ccelerion re considered

36 rjecories in Cresin spce The generion o rjecories in Cresin spce my presen problems 1. Inermedie poins ou o workspce. Singulr poins. Sr nd gol cn be reched only wih dieren conigurions

37 Liner rjecory in Cresin spce 1. some poins could be ouside he working spce. some poins could be singulr (reuire ininie velociy) Soluion genere he liner rjecory wih sep by sep pproximion Deine he sep lengh

38 Tylor lgorihm 1. mke IK or sr nd gol; compue rjecory in join spce;. Compue he poin in he middle in join spce; using DK rnsorm in Cresin spce. I in Cresin spce error> ε hen. dd kno poin in he middle o he Cresin ph b. spli he ph in (sr - kno, kno - gol; c. cll recursively; oherwise end; 4. reurn he seuence o kno poins nd execue he rjecories in join spce moo oenuo moo voluo 0 livelli di ricorsione 1 livello di ricorsione livelli di ricorsione livelli di ricorsione

39 Exmple - RR plnr liner ph rom (185, 67) o (7, 16) Using IK ind θ 1 nd θ in sr: (10, 0 ) nd gol: (60, 90 ). Middle poin in join spce: (5,55 ). Using DK rnsorm in join spce; Compue middle poin in Cresin spce Compre: I oo r, ke he middle poin in Cresin spce s kno, nd recursively pply on he irs nd second pr o he rjecory liner rjecory pproximed liner rjecory join rjecory or liner rjecory in Cresin spce join velociies join ccelerions

40 Which spce? Join spce Trjecories in join spce hve no mening No need o exensive IK compuion How o mke obscle voidnce? Cresin spce Trjecories in Cresin spce hve mening Hevy compuion or IK Problems or singulr or degenerion poins Obscle voidnce s geomeric resoning

41 Collision checking z0 x0 yv xc zv xv yc IDEA: Conver he robo o poin? In which spce? Obscles cn collide wih ny pr o he robo, no only he end eecor A collision ree ph hs o consider ll he links nd ll he objecs in working re Compuionlly expensive

42 C spce Objec conigurion speciy he posiion o every poin o he objec ( 1,,, n ). n ( 1,,, n ) C-spce he spce o ll he conigurions 1 Any conigurion is poin in C- spce Lozno-Perez 79

43 mobile robo in -D Cresin spce robo direcion y θ reerence -prmeers: (x, y, θ ), θ [0, π). D Cresin spce, D conigurion spce x

44 C-spce or RR θ 80 x y In he working spce i is esy o represen he hnd posiion, no esy o represen he chin posiion Consider he spce o he join vribles N dimensionl spce, where ech dimension ccouns or join A poin in his spce compleely describes he chin 0 θ1

45 Conigurion spce Tsk spce he rel physicl spce h conins robo nd objecs Conigurion spce (Cspce) se o ll he conigurions Free spce (Cree) se o ll he conigurions ouside obscle Obscle spce (Cobscle) se o ll he conigurions no inble (inside obscles)

46 C-spce represenion The blue obscle is represened in he blue re

47 Workspce vs C-Spce A possible ph

48 Workspce vs C-Spce No ph possible

49 Creing C-ree represenion Move link ime, blocking he preceden ones nd ignoring he nex ones Sr rom he bse deine unum o movemen Sr n 1 WHILE n<mxnum _link DO (FOR ech vlue in he rnge o joins 1 o n-1 compue he movemen using he discree number o uniied vlues) The resul cn be sored in ree where only ree nodes re expnded in he ollowing levels. The ree cn be used or ph plnning.

50 Trnsormion o c-spce In C-spce: robo is poin obscles re compliced volumes Cresin spce: objecs hve heir geomeric shpe robo is complex volume Conversion beween he spces is no esy Memory occupion cn be concern

51 Ph nd rjecory Join spce CON: rjecories in Cresin spce depend on he robo srucure No obscle considered PRO: esy compuion possible in rel ime Cresin spce PRO: rjecories hve mening in Cresin spce Uncomplee considerion o obscles CON: hevy compuion run ime problem or singulriy Cspce PRO: Robo hs simple represenio n( poin) Cn void obscles CON: Obscles hve complex represenio n Hevy compuion