Robotų programavimo technologijos

T125B114

2014 m.

Robertas Damaševičius [email protected]

1

Robotų kinematika

2

Turinys

  Robotinės rankos kinematika   Tiesioginė kinematika Atvirkštinė kinematika Robotinio vežimėlio judėjimo teorija  Pavaros  Odometrinis modelis 3

Kas yra kinematika?

    Mokslas apie judėjimą, geometrines judėjimo savybes Kinematinė roboto būsena yra kiekvieno jo judančio elemento (vežimėlio rato / rankos sąnario) padėtis Tiesioginė kinematika  Žinomos visų sąnarių padėtys (pasukimo kampai)  Rasti griebtuvo koordinates po judesio Atvirkštinė kinematika   Žinomos griebtuvo koordinatės Rasti sąnarių padėtis, kad griebtuvas atsidurtų norimoje vietoje 4

Robotinė ranka

 Mechaninė ranka    Pagal atliekamas funkcijas panaši į žmogaus ranką Gali būti programuojama Manipuliatorius juda taip, kad  jo galūnė (griebtuvai, pirštai) a tsidurtų norimoje 3D padėtyje ir būtų nukreipta tam tikra kryptimi 5

Lynxmotion robotinė ranka

 Turi:  besisukantį pagrindą,     petį, alkūnę, riešą, griebtuvą   SSC-32 Servo valdiklis Maitinama iš baterijų arba naudojant elektros tinklo srovės adapterį 6

Tipinė manipuliatoriaus struktūra

Jungtys Sąnariai Griebtuvas Bazė 7

Rankos kinematinis modelis

Standžių kūnų grandinė, sujungta lanksčiais sąnariais sąnariai griebtuvas, vykdantis roboto užduotis 8

 

Manipuliatoriaus judėjimo problemos

Manipuliatorių užduotys:  

sugriebti objektą,

perkelti objektą .

Tai turi būti padaryta

efektyviai

ir

saugiai

:  Ranka turi judėti taip, kad nepažeistų nustatytų apribojimų sąnariams  Neatsitrenktų į save arba į kitas aplinkos kliūtis 9

Sudėtingos manipuliavimo ranka užduotys

 Griebimo jėgos valdymas  Trapių objektų ėmimas  Kontakto su objekto paviršiumi palaikymas  Dinaminis manipuliavimas :  žongliravimas  mėtymas,  gaudymas 10

Manipuliavimas – Nuotolinis valdymas

    Autonominio manipuliatoriaus valdymas yra sudėtinga problema Pirmosios robotinės rankos buvo operatoriaus valdomos nuotoliniu būdu dirbant su pavojingomis medžiagomis Tai buvo žmogaus rankų imitacijos Operatoriams buvo sunku išmokti valdyti tokias rankas 11

Kodėl sudėtinga valdyti manipuliatorius?

   Nėra tiesioginio ryšio tarp to, ką:   Robotinė ranka turi padaryti realiame pasaulyje, ir Ką daro rankos sąnarių vykdytuvai (varikliai).

Tokį ryšį nagrinėja

kinematika

Reikia žinoti  1. kas ir kaip yra sujungta,   2. kiek yra jungčių (sąnarių), 3. kiekvieno sąnario laisvės laipsnių skaičių,  ir t.t.

12

Kinematikos problemos

    Kaip patikimai perkelti griebtuvą į norimą padėtį Nekliūdyti kliūčių Judėti tolygiai  Neviršijant greičio ir pagreičio apribojimų Prisitaikyti prie kintančių sąlygų,  pvz. pasikeitusios apkrovos pakėlus objektą 13

Tiesioginės kinematikos pavyzdys

 Roboto rankos struktūros pavyzdys  2 (x, y)  1 14

Tiesioginės kinematikos pavyzdys

 Ryšys tarp roboto rankos galo pozicijos (x, y) ir sąnarių kampų  1 ,  2 , aprašomos tokiomis lygtimis:

x y

 

l

1 cos  1

l

1 sin  1  

l

2

l

2 cos(  1 sin(  1   2 )   2 )  2 (x, y)  1 15

Tiesioginės kinematikos pavyzdys

16

Tiesioginės kinematikos pavyzdys

 Raudoni ta škai – visos galimos roboto manipuliatoriaus griebtuvo pad ėtys erdvėje 17

Atvirkštinė kinematika

 

r

Norint valdyti robotą reikia rasti kampus  1 ,  2 yra atstumas tarp pirmojo sąnario ir rankos galo (x, y): (x, y)

r



x

2 

y

2 r  2  1 18

Atvikštinė kinematika

 1  atan2(

y

,

x

)   (x, y)  Tuomet lygtis

x y

 

l

1

l

1 cos sin  1  1  

l

2

l

2 cos(  1 sin(  1    2 )  2 )  galima išspręsti taip:  2     r  1  2   cos  1  

l

1 2 

l

2 2 2

l

1

l

2 

r

2     cos  1  

r

2 

l

1 2 2

l

1

r



l

2 2   19

Atvirkštinės kinematikos lygčių sprendinių charakteristikos

   Gali būti 0, 1 arba daug sprendinių Jei taškas (x, y) nepasiekiamas, sprendinių nėra Jei taškas (x, y) gali būti pasiekiamas keliais skirtingais būdas, bus keli sprendiniai 20

Du galimi sprendimai

1

Du galimi kinematinės lygties sprendiniai

2

21

Geriausias sprendinys

 Jei yra keli galimi sprendiniai, tuomet geriausiu laikomas tas, kuriam pasiekti reikia nueiti trumpiausią kelią nuo prieš tai buvusios manipuliatoriaus būsenos 2 sprendiniai!

22

Sprendimas 3 sąnarių atveju (1)

// robot geometry const float e = 115.0; // end effector const float f = 457.3; // base const float re = 232.0; const float rf = 112.0; // trigonometric constants const float sqrt3 = sqrt(3.0); const float pi = 3.141592653; // PI const float sin120 = sqrt3/2.0; const float cos120 = -0.5; const float tan60 = sqrt3; const float sin30 = 0.5; const float tan30 = 1/sqrt3; 23

Sprendimas 3 sąnarių atveju (2)

// forward kinematics: (theta1, theta2, theta3) -> (x0, y0, z0) // returned status: 0=OK, -1=non-existing position int delta_calcForward(float theta1, float theta2, float theta3, float &x0, float &y0, float &z0) { float t = (f-e)*tan30/2; float dtr = pi/(float)180.0; theta1 *= dtr; theta2 *= dtr; theta3 *= dtr; float y1 = -(t + rf*cos(theta1)); float z1 = -rf*sin(theta1); float y2 = (t + rf*cos(theta2))*sin30; float x2 = y2*tan60; float z2 = -rf*sin(theta2); float y3 = (t + rf*cos(theta3))*sin30; float x3 = -y3*tan60; float z3 = -rf*sin(theta3); float dnm = (y2-y1)*x3-(y3-y1)*x2; 24

Sprendimas 3 sąnarių atveju (3)

// ... Continued forward kinematics float w1 = y1*y1 + z1*z1; float w2 = x2*x2 + y2*y2 + z2*z2; float w3 = x3*x3 + y3*y3 + z3*z3; float a1 = (z2-z1)*(y3-y1)-(z3-z1)*(y2-y1); float b1 = -((w2-w1)*(y3-y1)-(w3-w1)*(y2-y1))/2.0; float a2 = -(z2-z1)*x3+(z3-z1)*x2; float b2 = ((w2-w1)*x3 - (w3-w1)*x2)/2.0; float a = a1*a1 + a2*a2 + dnm*dnm; float b = 2*(a1*b1 + a2*(b2-y1*dnm) - z1*dnm*dnm); float c = (b2-y1*dnm)*(b2-y1*dnm) + b1*b1 + dnm*dnm*(z1*z1 - re*re); float d = b*b - (float)4.0*a*c; if (d < 0) return -1; } z0 = -(float)0.5*(b+sqrt(d))/a; x0 = (a1*z0 + b1)/dnm; y0 = (a2*z0 + b2)/dnm; return 0; // discriminant // non-existing point 25

Sprendimas 3 sąnarių atveju (4)

// inverse kinematics // helper functions, calculates angle theta1 (for YZ-pane) } int delta_calcAngleYZ(float x0, float y0, float z0, float &theta) { float y1 = -0.5 * 0.57735 * f; // f/2 * tg 30 y0 -= 0.5 * 0.57735 * e; // shift center to edge // z = a + b*y float a = (x0*x0 + y0*y0 + z0*z0 +rf*rf - re*re - y1*y1)/(2*z0); float b = (y1-y0)/z0; // discriminant float d = -(a+b*y1)*(a+b*y1)+rf*(b*b*rf+rf); if (d < 0) return -1; // non-existing point float yj = (y1 - a*b - sqrt(d))/(b*b + 1); // choosing outer point float zj = a + b*yj; theta = 180.0*atan(-zj/(y1 - yj))/pi + ((yj>y1)?180.0:0.0); return 0; 26

Sprendimas 3 sąnarių atveju (5)

// inverse kinematics: (x0, y0, z0) -> (theta1, theta2, theta3) // returned status: 0=OK, -1=non-existing position int delta_calcInverse(float x0, float y0, float z0, float &theta1, float &theta2, float &theta3) { theta1 = theta2 = theta3 = 0; int status = delta_calcAngleYZ(x0, y0, z0, theta1); // rotate coords to +120 deg if (status == 0) status = delta_calcAngleYZ(x0*cos120 + y0*sin120, y0*cos120-x0*sin120, z0, theta2); // rotate coords to -120 deg if (status == 0) status = delta_calcAngleYZ(x0*cos120 - y0*sin120, y0*cos120+x0*sin120, z0, theta3); return status; } 27

Kaip naudoti atvirkštinę kinematiką

  Naudojant atvirkštinės kinematikos metodą rastas sprendimas negarantuoja, kad ranka judės nuspėjamai, t.y. tiesiai tarp pradinio ir galinio taško Rekomenduojama kiekvieną žymesnį judesį skaidyti į atskirus žingsnelius ir po kiekvieno žingsnio perskaičiuoti kampus 28

Pakartojamumas ir tikslumas

• • Pakartojamumas: manipuliatorius gali sugrįžti į nurodytą tašką?

kaip tiksliai Tikslumas: kaip tiksliai manipuliatorius gali pereiti į nurodytą tašką 29

Ratinių robotų judesio programavimas

30

Ratinių robotų tipai

 Pagal ratų skaičių      1 ratės sistemos 2 ratės sistemos 3 ratės sistemos 4 ratės sistemos Daugiaratės sistemos 31

Campion* klasifikacija

      Naudojama skaičių pora (

m, s

), kur m – mobilumas (valdomų ratų skaičius) s vairuojamumas (vairavimo ratų skaičius) Tipas (3,0) – omni robotai  Turi 3 valdomus ratus  Gali važiuoti bet kuria kryptimi Tipas (2,0) - balansuojantys robotai  Turi 2 valdomus ratus Tipas (2,1)  2 valdomi ratai, vienas vairavimo ratas Tipas (1,1)  1 arba daugiau valdomų ratų ant tos pačios ašies  1 vairavimo ratas, esantis ant kitos ašies Tipas (1,2)  Mažiausiai du vairavimo ratai

Šaltinis

: G. Campion, W. Chung, Springer Handbook of Robotics, Springer Verlag, 2008, Ch. Wheeled robots, pp. 391 –410.

32

Ratinių robotų judėjimas

r l x y o n      Stabilumui užtikrinti pakanka 3 ratų Differen cialinė pavara (TurtleBot) Automobilio (Ackerman ’o) pavara Sinchroninė pavara Omni pavaros: Mecanum ratai 33

Vienratės sistemos

  Balansuojantys robotai Labai nestabilūs ir sunkiai programuojami  Murata Girl http://www.murata.com

Šaltinis:

http://www.robotplatform.com

34

Dviratės sistemos

    Balansuojantys robotai Sudėtingas valdymas, programavimas 2 tipai:  Dviračio    1 ratas - priekyje, 1 ratas – gale Lygiagretaus išdėstymo  Svorio centras turi būti žemiau ašies Nbot  www.youtube.com/watch?v=uaaNUMwI3xg 35

Triratės sistemos

    Lenvai valdomi ir programuojami Diferencialinė pavara Ackerman ‘o pavara Sinchroninė pavara  Omni sistemos 36

Diferencialinė pavara

Pranašumai:  Pigus sprendimas  Paprasta konstrukcija  Paprasta realizuoti Trūkumai: • Sudėtinga važiuoti į priekį

Differencialinės pavaros tipai (1)

   Du užpakaliniai varomi ratai Vienas priekinis (pasyvus) ratas Pvz., TurtleBot 38

Differencialinės pavaros tipai (2)

  Du užpakaliniai

nepriklausomai

varomi ratai Vienas priekinis (pasyvus) ratas 39

   

Diferencialinės pavaros valdymo principai

Abu ratai sukasi vienodu greičiu ir vienoda kryptimi – robotas važiuoja tiesiai Ratai sukasi vienodu greičiu, bet skirtingomis kryptimis – robotas sukasi vietoje Vienas ratas nejuda, kitas sukasi – robotas apsisuks Vienas ratas sukasi greičiau nei kitas – robotas sukasi į vieną pusę 40

Diferencialinės pavaros valdymas

  

Du atskirai valdomi varomieji ratai Robotas gali suktis vienoje vietoje Roboto nuvažiuotas kelias D ir greitis V: , , Kairiojo rato kelias ir greitis Dešiniojo rato kelias ir greitis = kairiojo taško atstumas iki sukimosi taško

41

y Diferencialinės pavaros kinematinės lygtys ICC  [

x



R

sin  ,

y



R

cos  ] 

ICC

R v l (x,y)  v r  (

R

 (

R



l

/ 2 ) 

l

/ 2 )  

v r v l R



v

 

l

2 (

v

(

v r l



v r

) 

v l

)

v r



v l



v r



l v l

2

l

/2 x 42

Diferencialinės pavaros kinematinės lygtys

ICC

R    

t

   0 0 P(t+  t)       

y

       P(t) 43

Diferencialinės pavaros kinematika y ( R+d) = V L ( R-d) = V R  Roboto greitis:  = ( V R - V L ) / 2d R = 2d ( V R + V L ) / ( V R - V V =  R = ( V R + V L ) / 2 V L x 2d V R

ICC

R Roboto sukimosi spindulys 44

Problema: blogai pripūstos padangos

“Navigating Mobile Robots: Systems and Techniques” Borenstein, J.

Pasikeitęs ratų diametras įneša paklaidą skaičiuojant nuvažiuotą atstumą R e sunku įvertinti

Ackerman’o pavara

    Priekinis varomas ratas(-ai) Du pasyvūs ratai Panašiai valdomas, kaip priekiniais ratais varomi automobiliai Pvz., Neptune (Carnegie Mel. Univ.) 46

Ackerman’o pavara

Pranašumai: • Lengva realizuoti Trūkumai: • Kai kuriuos judesius sunku suplanuoti ir įgyvendinti • Ribotas ratų sukimosi kampas • Negalima apsisukti vietoje “Navigating Mobile Robots: Systems and Techniques” Borenstein, J.

Ackerman ’o pavara



o



i l

d x 

i



o

x = atstumas nuo vidinio rato iki sukimosi centro d = atstumas tarp rat ų l = atstumas tarp priekini ų ir galinių ratų cot  cot 

o i



x



l x



d l



o



i

48

Ackerman ’o pavara

V FL  L y  R V FR g V BL d d V BR x r r =  g sin(  R ) = g tan(  R ) + d V FR Sukimosi centras 49

Ackerman ’o pavara

V FL  L y  R V FR g V BL d d V BR x r IC C r =  g sin(  R ) = g tan(  R ) + d V FR determines   g sin(  L ) = V FL  L = tan -1 (g / (r + d))  (r - d) = V BR  (r + d) = V BL 50

Sinchroninė pavara

• Visus ratus sinchroniškai valdo vienas variklis ➔ Apsprendžia roboto važiavimo greitį • Važiavimo kryptį valdo antras variklis • Trūkumai: • Sudėtinga konstrukcija ir realizacija 51

Sinchroninė pavara

•Visi ratai sukasi kartu •Tuo pačiu greičiu •Ir išlieka lygiagretūs 52

Sinchroninė pavara

ICC at  y V wheels   x V robot = V wheels  robot =  wheels greitis  (t) =   (t) dt x(t) =  y(t) =  V wheels (t) cos(  (t)) dt V wheels (t) sin(  (t)) dt padėtis 53

Sinchropavaros atvirkštinė kinematika Pagrindinė problema: y Kaip pasiekti galutinę padėtį, jei žinoma pradinė padėtis?

V L (t) V R (t) pradinė padėtis x galutinė padėtis 54

Sinchropavaros atvirkštinė kinematika Pagrindinė problema: y Kaip pasiekti galutinę padėtį, jei žinoma pradinė padėtis?

V L (t) V R (t) pradinė padėtis x galutinė padėtis 55

Sinchropavaros atvirkštinė kinematika Pagrindinė problema: y Kaip pasiekti galutinę padėtį, jei žinoma pradinė padėtis?

V L (t) V R (t) pradinė padėtis x galutinė padėtis 56

Sinchropavaros atvirkštinė kinematika Pagrindinė problema: y V L (t) V R (t) pradinė padėtis Kaip pasiekti galutinę padėtį, jei žinoma pradinė padėtis?

x galutinė padėtis Reikia išspręsti lygčių sistemą: x =  V(t) cos(  (t)) dt y =  V(t) sin(  (t)) dt  =   (t) dt  = ( V R - V L ) / 2d V =  R = ( V R + V L ) / 2 for V L (t) and V R (t) .

Daug sprendinių...

57

Sinchropavaros atvirkštinė kinematika Pagrindinė problema: y Kaip pasiekti galutinę padėtį, jei žinoma pradinė padėtis?

Sunku rasti geriausią sprendimą ...

V L (t) V R (t) pradinė padėtis x galutinė padėtis • Greičiausias laikas • mažiausiai energijos • stabiliausias greitis V L (t) V L (t) t 58

Triratis vežimėlis

Pranašumai: • ratai neslysta “Navigating Mobile Robots: Systems and Techniques ” Borenstein, J.

Trūkumai: • ne visomis kryptimis įmanoma važiuoti

y Omni ratai (Mecanum ratai) Kuka Omni-Drive

v x v y

 (

v

0 

v

1 

v

2 

v

3 ) / 4  (

v

0 

v

1 

v

2 

v

3 ) / 4

v

  (

v

0 

v

1 

v

2 

v

3 ) / 4

v



error

(

v

0 

v

1 

v

2 

v

3 ) / 4 v 1 v 2 v 0 v 3 forward x v 1 v 2 left v 3 v 0 v 1 v 2 turn v 3 v 0 60

Omni ratai

Pranašumai: • Leidžiami sudėtingi judesiai Trūkumai • Sudėtinga realizuoti Morevac “Navigating Mobile Robots: Systems and Techniques ” Borenstein, J.

61

Judėjimo paklaidos šaltiniai

idealus atvejis ratų dydžio skirtumai kelio nelygumai nevienoda ratų trintis 62

Judesio modeliai

 

Odometriniai

 Robotas pats registruoja savo nueitą kelią

Greičiu grįsti

 Roboto padėtis apskaičiuojama pagal žinomą roboto greitį ir praėjusį laiką 63

Odometrinis modelis

• Robotas juda iš • Turima informacija

x

,

y

, 

u

 į

x

' ,

y

' ,  '  .

rot

1 , 

rot

2 , 

trans



trans

 (

x

' 

x

) 2  (

y

' 

y

) 2 

rot

1  atan2 (

y

' 

y

,

x

' 

x

)   

rot

2   '    

rot

1

x

' ,

y

' ,  '

x

,

y

,  

rot

1 

trans



rot

2 64

atan2

65

Odometrinio modelio paklaida

 Įvedama paklaida (triukšmas)  ˆ

rot

1  ˆ

trans

 

rot

1    1 | 

rot

1 |   2 | 

trans

|  

trans

   3 | 

trans

|   4 | 

rot

1  

rot

2 |  ˆ

rot

2  

rot

2    1 | 

rot

2 |   2 | 

trans

| 66

Vaikščiojantys robotai

67

Vaikščiojantys robotai

    Vienakojai (šokinėjantys) robotai  https://www.youtube.com/watch?v=K92tohX_ZV4 Dvikojai robotai  https://www.youtube.com/watch?v=8PHfdpq3yl8 Keturkojai robotai  https://www.youtube.com/watch?v=MbnHmzGGkrI Šešiakojai robotai  https://www.youtube.com/watch?v=VY_ACJ2Rjk0 68

Kodėl kojos?

    Sveria mažiau nei ratai Geriau tinka, kai nėra gero pagrindo (kelio) Geresnis manevravimas Nereikia nuolatinio sąlyčio su pagrindu, todėl robotas gali judėti „trūkioje“ aplinkoje

Kojos LLS

   Koja turi turėti mažiausiai 2 LL  Kelti ir svyruoti Žmogaus koja turi 30 LL     Šlaunies = 3 LLS Kelio sąnarys= 2 LLS Kulkšnis = 1 LLS Pėda - 24 LL Robotų kojos dažniausiai turi 3 LL  Esant didesniam skaičiui valdymas labai sudėtingas

Programinė įranga

 Modeliavimui:    MATLAB Robotix Toolbox http://www.petercorke.com/Robotics_Toolbox.html

 Python versija: http://code.google.com/p/robotics-toolbox-python 71

Papildomam skaitymui

 SSC-32 Manual http://www.lynxmotion.com/images/data/ssc 32.pdf



Knyga: “The definitive guide to building Java robots ”

http://books.google.lt/books?isbn=1590595564  

2 skyrius, „Serial Communication“ 3 skyrius, „Motion“

72