Transcript Kinematika
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