Transcript 6 paskaita
Intelektikos pagrindai P176B100 2012 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) Atsidurtų 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 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 7 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 8 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 9 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. 10 Tipinė manipuliatoriaus struktūra Jungtys Sąnariai Griebtuvas Bazė 11 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ą 12 Rankos kinematinis modelis Standžių kūnų grandinė, sujungta lanksčiais sąnariais sąnariai griebtuvas, vykdantis roboto užduotis 13 Tiesioginės kinematikos pavyzdys Roboto rankos struktūros pavyzdys (x, y) 2 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 l1 cos1 l2 cos(1 2 ) (x, y) y l1 sin1 l2 sin(1 2 ) 2 1 15 Atvirkštinė kinematika Norint valdyti robotą reikia rasti kampus 1, 2 r yra atstumas tarp pirmojo sąnario ir rankos galo (x, y): r x y 2 (x, y) 2 r 2 1 16 Atvikštinė kinematika Tuomet lygtis x l1 cos1 l2 cos(1 2 ) y l1 sin1 l2 sin(1 2 ) (x, y) r 2 1 galima išspręsti taip: 2 1 atan2(y, x) 2 2 2 l l r 1 1 2 cos 2l l 1 2 2 2 2 r l l 1 1 2 cos 2l r 1 17 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 18 Du galimi sprendimai Du galimi kinematinės lygties sprendiniai 1 2 19 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! 20 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; 21 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; 22 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; // discriminant // non-existing point z0 = -(float)0.5*(b+sqrt(d))/a; x0 = (a1*z0 + b1)/dnm; y0 = (a2*z0 + b2)/dnm; return 0; } 23 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; } 24 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; } 25 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 26 Laisvės laipsnių skaičius (LLS) 3 padėties koordinatės 3D erdvėje = 6 LL 3 kryptys Robotikoje, LLS = nepriklausomai valdomų sąnarių skaičius Jei LLS valdymo tikslumas skaičiavimų sudėtingumas kaštai 27 Valdomų laisvės laipsnių skaičius (LLS) Dauguma vykdytuvų turi tik 1 LLS Svarbu žinoti Roboto LLS Roboto valdomų LLS Jei kiekvienam LL yra valdymo įtaisas, visi LLS yra valdomi Jei valdomų LLS < visų LLS, robotą valdyti yra sudėtinga 28 Valdomų laisvės laipsnių skaičius (LLS) Automobilis turi 3 laisvės laipsnius: padėtis (x,y) ir kryptis (posūkio kampas). Tik 2 laisvės laipsnius galima valdyti: važiuojant: greičio pedalas ir pavara vairuojant: naudojant vairą. Kadangi laisvės laipsnių skaičius didesnis nei valdomų laisvės laipsnių skaičius, kai kuriuos judesius atlikti sunku arba neįmanoma Pvz., važiuoti į šoną 29 Žmogaus ranka Turi 7 LLS: 3 - petyje, 1 - alkūnėje, 3 - rieše Visi LLS yra valdomi, todėl pasiekti tam tikrą objektą erdvėje žmogaus ranka gali daugeliu būdų 30 Žmogaus plaštaka Žmogaus LLS 23 31 Humanoidiniai HONDA ir SONY robotai • 98 LLS Pakartojamumas ir tikslumas • Pakartojamumas: kaip tiksliai manipuliatorius gali sugrįžti į nurodytą tašką? • Tikslumas: kaip tiksliai manipuliatorius gali pereiti į nurodytą tašką 33 Ratinių robotų judesio programavimas 34 Ratinių robotų judėjimas r o l l x y x y xy z m o t i o n Stabilumui užtikrinti pakanka 3 ratų Differencialinė pavara (TurtleBot) Automobilio (Ackerman’o) pavara Sinchroninė pavara Omni pavaros: Mecanum ratai 35 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 36 Campion* klasifikacija Naudojama skaičių pora (m, s), kur m – mobilumas s - vairuojamumas Tipas (3,0) – omni robotai Tipas (2,0) - balansuojantys robotai Turi 2 valdomus ratus Tipas (2,1) Turi 3 valdomus ratus Gali važiuoti bet kuria kryptimi 2 valdomi ratai, vienas vairavimo ratas Type (1,1) 1 arba daugiau valdomų ratų ant tos pačios ašies 1 vairavimo ratas, esantis ant kitos ašies Type (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. 37 Vienratės sistemos Balansuojantys robotai Labai nestabilūs ir sunkiai programuojami Murata Girl http://www.murata.com Šaltinis: http://www.robotplatform.com 38 Dviratės sistemos Balansuojantys robotai Sudėtingas valdymas, programavimas 2 tipai: Dviračio Lygiagretaus išdėstymo 1 ratas - priekyje, 1 ratas – gale Svorio centras turi būti žemiau ašies Nbot 39 Triratės sistemos Lenvai valdomi ir programuojami Diferencialinė pavara Ackerman‘o pavara Sinchroninė pavara Omni sistemos 40 Ratinių robotų tipai 41 Diferencialinė pavara Pranašumai: Pigus sprendimas Paprasta konstrukcija Paprasta realizuoti Simple design Trūkumai: • Sudėtinga važiuoti į priekį Differencialinės pavaros tipai (1) Du užpakaliniai varomi ratai Vienas priekinis (pasyvus) ratas Pvz., TurtleBot 43 Differencialinės pavaros tipai (2) Du užpakaliniai nepriklausomai varomi ratai Vienas priekinis (pasyvus) ratas 44 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ę 45 Diferencialinė pavara Pictures from “Navigating Mobile Robots: Systems and Techniques” Borenstein, J. D – rato nuvažiuotas kelias 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 47 Diferencialinės pavaros kinematinės lygtys ICC [ x R sin , y R cos ] ICC ( R l / 2) vr ( R l / 2) vl vl l (vl vr ) 2 (vr vl ) vr vl l vr vl v 2 R R (x,y) y vr l/2 x 48 Diferencialinės pavaros kinematinės lygtys ICC x ' cos( t ) sin( t )0 x ICC x IC x y ' sin( t ) cos( t ) 0 y ICC y IC y ' 0 1 0 t R P(t+t) P(t) 49 Diferencialinės pavaros kinematika (R+d) = VL (R-d) = VR y = ( VR - VL ) / 2d R = 2d ( VR + VL ) / ( VR - V Roboto greitis: VL V = R = ( VR + VL ) / 2 x 2d VR ICC R Roboto sukimosi spindulys 50 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ą Re 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.) 52 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 x cot i l xd cot o l i 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ų 54 Ackerman’o pavara L R r = g +d tan(R) g sin(R) = VFR y VFL VFR g VBL d d VBR x r Sukimosi centras 55 Ackerman’o pavara L R y VFL VFR r = g +d tan(R) g sin(R) = VFR g VBL d g sin(L) d VBR = VFL L = tan-1(g / (r + d)) x r (r - d) = VBR (r + d) = VBL IC C 56 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 57 Sinchroninė pavara •Visi ratai sukasi kartu •Tuo pačiu greičiu •Ir išlieka lygiagretūs 58 Sinchroninė pavara ICC at y Vrobot = Vwheels robot = wheels greitis x Vwheels (t) = (t) dt x(t) = Vwheels(t) cos((t)) dt padėtis y(t) = Vwheels(t) sin((t)) dt 59 Sinchropavaros atvirkštinė kinematika Pagrindinė problema: Kaip pasiekti galutinę padėtį, jei žinoma pradinė padėtis? y x VL (t) VR(t) pradinė padėtis galutinė padėtis 60 Sinchropavaros atvirkštinė kinematika Pagrindinė problema: Kaip pasiekti galutinę padėtį, jei žinoma pradinė padėtis? y x VL (t) VR(t) pradinė padėtis galutinė padėtis 61 Sinchropavaros atvirkštinė kinematika Pagrindinė problema: Kaip pasiekti galutinę padėtį, jei žinoma pradinė padėtis? y x VL (t) VR(t) pradinė padėtis galutinė padėtis 62 Sinchropavaros atvirkštinė kinematika Pagrindinė problema: Kaip pasiekti galutinę padėtį, jei žinoma pradinė padėtis? y Reikia išspręsti lygčių sistemą: x = V(t) cos((t)) dt y = V(t) sin((t)) dt x VL (t) = (t) dt VR(t) pradinė padėtis galutinė padėtis = ( VR - VL ) / 2d V = R = ( VR + VL ) / 2 for VL (t) and VR(t) . Daug sprendinių... 63 Sinchropavaros atvirkštinė kinematika Pagrindinė problema: Kaip pasiekti galutinę padėtį, jei žinoma pradinė padėtis? y Sunku rasti geriausią sprendimą... • Greičiausias laikas • mažiausiai energijos • stabiliausias greitis x VL (t) VR(t) pradinė padėtis galutinė padėtis VL (t) VL (t) t 64 Omni ratai (Mecanum ratai) Kuka Omni-Drive vx (v0 v1v2 v3)/4 vy (v0 v1v2 v3)/4 v (v0 v1v2 v3)/4 verror (v0 v1v2 v3)/4 y v1 v1 v0 v1 v0 v2 v3 v2 forward v0 v2 x left v3 turn v3 65 Omni ratai Morevac “Navigating Mobile Robots: Systems and Techniques” Borenstein, J. Pranašumai: • Leidžiami sudėtingi judesiai Trūkumai • Sudėtinga realizuoti 66 Triratis vežimėlis “Navigating Mobile Robots: Systems and Techniques” Borenstein, J. Pranašumai: • ratai neslysta Trūkumai: • ne visomis kryptimis įmanoma važiuoti Judėjimo paklaidos šaltiniai idealus atvejis kelio nelygumai ratų dydžio skirtumai nevienoda ratų trintis 68 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ą 69 Odometrinis modelis • Robotas juda iš • Turima informacija x , y , x ' , yį' , ' . u 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 , rot 1 rot 2 trans x ' , y ' , ' 70 atan2 71 Odometrinio modelio paklaida Įvedama paklaida (triukšmas) ˆrot 1 rot 1 | ro t1 | 2 ˆtrans trans | tra n s| 4 | ro t1 ro t2 | 1 ˆrot 2 rot 2 3 | tra n s| 1 | ro t2 | 2 | tra n s| 72 Vaikščiojantys robotai 73 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 Žmogaus koja turi 30 LL Kelti ir svyruoti Š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 Papildomam skaitymui SSC-32 Manual http://www.lynxmotion.com/images/data/ssc32.pdf Knyga: “The definitive guide to building Java robots” http://books.google.lt/books?isbn=1590595564 2 skyrius, „Serial Communication“ 3 skyrius, „Motion“ 76