Robottechnika Differenciális kinematika és dinamika Magyar Attila Pannon Egyetem Műszaki Informatika Kar Villamosmérnöki és Információs Rendszerek Tanszék amagyar@almos.vein.hu 2009 október 8.
Áttekintés Differenciális kinematika 1 Differenciális kinematika Jacobi számítása 2 Dinamika Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 2 / 21
Differenciális kinematika Differenciális kinematika A direkt- és inverz kinematika a csuklóváltozók és a végszerszám pozíciója/orientációja között teremt kapcsolatot A differenciális kinematika a csuklósebességek és a végszerszám sebesség/szögsebesség között Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 3 / 21
Differenciális kinematika Geometriai Jacobi mátrix Eml: direkt kinematikai modell R(q) p(q) T (q) = 0 T 1, q = [q 1,..., q n] T Cél: a csuklósebességek és a végszerszám sebesség/szögsebesség közti kapcsolat felírása a végszerszám egyenes vonalú sebességének (ṗ) felírása a csuklósebesség ( q) segítségével a következő alakban ṗ = J P (q) q a végszerszám szögsebességének (ω) felírása a csuklósebesség ( q) segítségével a következő alakban ω = J O (q) q Kompakt alakban [ ṗ v = ω ] = J(q) q, ahol J = [ JP J O ] R 6 n Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 4 / 21
Jacobi számítása Differenciális kinematika Jacobi számítása Összegezve [ jpi j Oi ] = [ zi 1 0 ] [ zi 1 (p p i 1 ) z i 1 ] csúszó ízület esetén forgó ízület esetén Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 5 / 21
Áttekintés Dinamika 1 Differenciális kinematika 2 Dinamika Mozgási energia Helyzeti energia Mozgásegyenletek Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 6 / 21
Dinamikus modell Dinamika Beavatkozó nyomatékok és a manipulátor mozgása közt teremt kapcsolatot Mozgásszimuláció, irányítási algoritmusok tervezése Ízületek, átalakítók, beavatkozók méretezése (erők, nyomatékok kiszámíthatók) Mozgásegyenletek felírása a csuklóváltozók terében Lagrange formalizmus Newton-Euler formalizmus Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 7 / 21
Lagrange-formalizmus Dinamika Szisztematikusan vezetjük le a mozgásegyenleteket Általánosított koordináták: λ 1,..., λ n Lagrange függvény (L) L = T U, Lagrange egyenletek ahol d L L = ξ dt λ i, i λ i T kinetikus energia U potenciális energia i = 1,..., n ξ i a λ i -hez tartozó általánosított erő (nyomaték) Nyílt kinematikai lánc esetén az általánosított koordináták legyenek a csuklóváltozók λ = q Kapcsolatot teremt a manipulátorra ható nyomatékok és a csuklópozíciók, -sebességek, és -gyorsulások között Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 8 / 21
Dinamika Példa - Meghajtott inga τ nyomatékkal forgatjuk viszkózus súrlódás a nyomatékot egy motor adja, aminek az áttétele k r > 1, tehetetlenségi nyomatéka I m ϑ szögelfordulás (ϑ = 0, ha lefelé áll) Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 9 / 21
Dinamika Példa - Meghajtott inga Általánosított koordináta - ϑ Mozgási energia T = 1 2 I ϑ 2 + 1 2 I mk 2 r ϑ 2 Helyzeti energia U = mgl(1 cos(ϑ)) Lagrange függvény L = 1 2 I ϑ 2 + 1 2 I mkr 2 ϑ 2 mgl(1 cos(ϑ)) Behelyettesítve a Lagrange egyenletbe (I + I m kr 2 ) ϑ + F ϑ + mgl sin(ϑ) = τ Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 10 / 21
Dinamika Mozgási (kinetikus) energia Mozgási energia n merev szegmensből álló manipulátor Teljes kinetikus energia az egyes szegmensek illetve a rájuk erősített motorok mozgási energiájából áll össze n T = (T li + T mi ) i=1 Az i. szegmens mozgási energiája T li = 1 ṗ T ṗ 2 i i ρdv V li ṗ i : egyenesvonalú sebesség V li : az i. szegmens térfogata Az i. szegmens tömegközéppontja p li = 1 p m i ρdv li V li A szegmens egy pontjának sebessége ṗ i = ṗ li + ω i r i = ṗ li + S(ω i ) r i Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 11 / 21
Dinamika Szegmens mozgási energiája Mozgási energia Visszahelyettesítve a kinetikus energiába (T li ) 3 tag adódik: Egyenes vonalú 1 ṗ T ṗ 2 l V i li ρdv = 1 li 2 m l i ṗ T ṗ l i li Kölcsönös 2 1 2 V li ṗ T S(ω l i )r i ρdv = 2 1 i 2 ṗt S(ω l i ) i V li (p i p li )ρdv = 0 Forgási 1 r T i S T (ω 2 i )S(ω i )r i ρdv = 1 V li 2 ωt i S T (r i )S(r i )ρdv ω i = 1 V li 2 ωt i I ω li i kihasználtuk, hogy S(ω i )r i = S(r i )ω i Tehetetlenségi nyomaték tenzor: (r 2 iy + riz 2 )ρdv r ix r iy ρdv r ix r iz ρdv I li = (r 2 ix + riz 2 )ρdv r iy r iz ρdv (r 2 ix + riy 2 )ρdv Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 12 / 21
Dinamika Szegmens mozgási energiája Mozgási energia Összegezve, az i. szegmens mozgási energiája T li = 1 2 m l i ṗ T l i ṗ li + 1 2 ωt i I li ω i A mozgási energiát a csuklóváltozók függvényeként kellene felírni, ehhez a Jacobi formalizmus haszálható ṗ li = j (l i ) P1 q 1 +... + j (l i ) Pi q i = J (l i ) P q ω i = j (l i ) O1 q 1 +... + j (l i ) Oi q i = J (l i ) O [ ] q ahol J (l i ) P = j (l i ) P1... j(l i ) Pi 0... 0 J (l i ) O = [ ] j (l i ) O1... j(l i ) Oi 0... 0 Az oszlopvektorok számítása { j (l i ) zj 1 csúszó ízület esetén Pj = z j 1 (p li p j 1 ) forgó ízület esetén { j (l i ) 0 csúszó ízület esetén Oj = z j 1 forgó ízület esetén (Az i. szegmens kinetikus energiája) T li = 1 2 m l i q T J (l i )T J (l i ) q + 1 P P 2 qt J (l i )T I J (l i ) q O l i P Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 13 / 21
Motor mozgási energiája Dinamika Mozgási energia Szegmenséhez hasonlóan számolható Forgó elektromos motorokat tételezünk fel (mindkét ízülettípus meghajtására alkalmas) Az állórész az i 1. szegmenshez van rögzítve, így annak mozgási energiáját már kiszámoltuk A forgórész mozgási energiája a kérdés Az i. rotor kinetikus energiája T mi = 1 2 mm i ṗt m i ṗ mi + 1 2 ωt m i I mi ω mi ahol m mi a rotor tömege a rotor tömegközéppontjának ṗ mi egyenes vonalú sebessége I tehetetlenségi nyomaték mi a rotor szögsebessége ω mi Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 14 / 21
Motor mozgási energiája Dinamika Mozgási energia Jelölje ϑ mi a rotor szögelfordulását. Merev áttételt feltételezve: Az i. edik rotor eredő szögsebessége k ri q i = ϑ mi ω mi = ω i 1 + k ri q i z mi ahol ω i 1 az i 1. szegmens szögsebessége, z mi pedig a rotor tengelyének irányába mutató egységvektor A kinetikus energiát most is a csuklóváltozók függvényeként kellene felírni. A rotor tömegközéppontjának egyenes vonalú sebessége Jacobi mátrix j (m i ) Pj = J (m i ) P ṗ mi = J (m i ) q P [ ] = j (m i ) P1... j (m i ) P,i 1 0... 0, ahol { zj 1 csúszó ízület esetén z j 1 (p mi p j 1 ) forgó ízület esetén p j 1 pedig a j 1. koordinátarendszer origójába mutató vektor, j (m i ) Pi pedig azért nullvektor, mert a rotor tömegközéppontja a forgástengelyen helyezkedik el Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 15 / 21
Motor mozgási energiája Dinamika Mozgási energia A rotor szögsebessége is kifejezhető csuklóváltozókkal ω mi = J (m i ) q O [ ] Jacobi mátrix J (m i ) = j (m i ) O O1... j (m i ) O,i 1 j(m i ) Oi 0... 0, ahol { j (m i ) j (l i ) Oj = Oj j = 1,..., i 1 k ri z mi j = i a rotor forgástengelyének irányába mutató egységvektor z mi (Az i. rotor kinetikus energiája) T mi = 1 2 mm i qt J (m i )T P J (m i ) P q + 1 2 qt J (m i )T O I J (m i ) q mi O Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 16 / 21
A teljes mozgási energia Dinamika Mozgási energia Összegezve az egyes szegmensek és motorok mozgási energiáit magkapjuk a manipulátor mozgási energiáját: T (q, q) = 1 2 n n b ij (q) q i q j = 1 2 qt B(q) q i=1 j=1 B(q) az n n-es tehetetlenségi nyomaték mátrix: B(q) = n ( i=1 m li J (l i )T P szimmetrikus pozitív definit konfiguráció-függő J (l i ) + P J(l i )T I J (l i )T O l i O + m mi J (m i )T P J (m i ) P + J (m i )T O ) I J (m i )T mi O Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 17 / 21
Dinamika Helyzeti (potenciális) energia Helyzeti energia A mozgási energiához hasonlóan a helyzeti energiát is az egyes szegmensek és rotorok helyzeti energiájának összegeként írjuk fel n U = (U li + U mi ) i=1 Merev szegmenseket feltételezve a szegmensek helyzeti energiája a gravitációs erőkből származik U li = g T p i ρdv = m l i g T p li, ahol g T = [0, 0, g] V li Az i. rotor helyzeti energiája hasonlóan U mi = m mi g T p mi A teljes potenciális energia n ) U(q) = (m li g T p li + m mi g T p mi i=1 p li és p mi vektorokon keresztül a potenciális energia csak a csuklóváltozóktól függ, azok idő szerinti derváltjától (sebességétől) nem Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 18 / 21
Mozgásegyenletek Dinamika Mozgásegyenletek Eml: Lagrange függvény: L(q, q) = T (q, q) U(q) = 1 n n n ( ) b ij (q) q i q j + m li g T p 2 li (q) + m mi g T p mi (q) i=1 j=1 i=1 Eml: Lagrange egyenletek: d L L = ξ i, i = 1,..., n dt q i q i A derivált elemei: d dt U n = q i j=1 ( ) L = d q i dt ( m lj g T p lj q i ( ) T q i n n db ij (q) n n n b ij (q) = b ij (q) q j + q j = b ij (q) q j + q k q j dt q j=1 j=1 j=1 j=1 k=1 k T = 1 n n b jk (q) q k q j q i 2 q j=1 k=1 i p ) + m mj g T mj n ( = m lj g T j (l j ) Pi (q) + m mj g T j (m j ) Pi q i j=1 ) (q) = g i (q) Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 19 / 21
Mozgásegyenletek Dinamika Mozgásegyenletek (A mozgásegyenletek) n n n b ij (q) q j + h ijk (q) q k q j + g i (q) = ξ i, j=1 j=1 k=1 i = 1,..., n ahol h ijk = b ij 1 b jk q k 2 q i Gyorsulások a b ij együttható adja meg a j. ízület gyorsulásának hatását az i. ízületre Négyzetes sebességek: h ijj q j 2 reprezentálja a j. ízület sebessége által az i. ízületre gyakorolt centrifugális hatást h ijk q j q k reprezentálja a j. és a k. ízületek sebességei által az i. ízületre gyakorolt Coriolis hatást Csuklóváltozó függő tagok: g i reprezentálja a gravitáció hatására az i. ízületnél ébredő nyomatékot Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 20 / 21
Mozgásegyenletek Dinamika Mozgásegyenletek Általánosított erők, nemkonzervatív erők Beavatkozó nyomatékok τ Súrlódási nyomatékok viszkózus súrlódási nyomaték: F v q statikus súrlódási nyomaték: f s (q, q), vagy egyszerűbben a Coulomb súrlódási nyomaték: F s sgn( q) A környezettel való érintkezésből (végszerszám) adódó kontakt-erők: J T (q)h Összegezve, a csuklóváltozókra vonatkozó dinamikus modell: B(q) q + C(q, q) q + F v q + F s sgn( q) + g(q) = τ J T (q)h ahol a C R n n mátrix c ij elemei kielégítik a következő egyenletet: n n n c ij q j = h ijk q k q j j=1 j=1 k=1 Magyar Attila (Pannon Egyetem) Robottechnika 2009 október 21 / 21