Infobionika ROBOTIKA XI. Előadás Robot manipulátorok III. Differenciális kinematika Készült a HEFOP-3.3.1-P.-2004-06-0018/1.0 projekt keretében
Tartalom A forgatási mátrix időbeli deriváltja A geometriai Jacobi-mátrix Kinematikai szingularitások Differenciális kinematikai inverzió Redundáns manipulátorok
A forgatási mátrix deriváltja tekintsünk egy időfüggő forgatási mátrixot: R=R t ortogonalitási feltételből tudjuk: a szorzat deriváltja: R t R T t =I Ṙ t R T t R t R T t =0 vezessük be a következő jelölést: S t =Ṙ t R T t S ferdén szimmetrikus, azaz: S t S T t =0 R deriváltja kifejezhető önmaga függvényeként: Ṙ t =S t R t 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 3
Ismétlés: a vektoriális szorzat legyen ekkor a=[a x a y a z] és a b=[a y b z a z b y a z b x a x b z a x b y a y b x] x b=[b b z] y b a b = a b sin 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 4
A vektoriális szorzat fizikai jelentése Tekintsünk egy origó körül ω szögsebességel forgó merev testet: Kérdés: mi lesz a p pont v sebességvektora? v nagysága: v = = r sin v merőleges r-re és ω-ra : v= r 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 5
A forgatási mátrix deriváltja Fizikai jelentés: legyen p t =R t p' derivált: azaz: ṗ t =Ṙ t p' ṗ t =S t R t p' Ha ω(t) jelöli az R(t) bázis tengelyeinek alap bázishoz képesti szögsebességeit, akkor tudjuk, hogy ṗ t = t R t p' 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 6
A forgatási mátrix deriváltja Legyen: Ekkor S a következő: Jelölhetjük tehát így: t =[ x y ] T z 0 z y S t =[ ] z 0 x y x 0 S t =S t Bebizonyítható a következő összefüggés: R S R T =S R 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 7
A forgatási mátrix deriváltja Tekintsük a következő példát: 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 8
A forgatási mátrix deriváltja 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 9
A geometriai Jacobi-mátrix Tekintsünk egy n szabadságfokú manipulátort. A direkt kinematikai egyenlet a következő: Ahol a csuklóváltozók vektora: 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 10
A geometriai Jacobi-mátrix A differenciális kinematika célja: megtalálni az összefüggést a csuklóváltozók sebessége és a végberendezés sebessége (pozíció, orientáció változása) között a következő formában: Vagy kompakt jelöléssel: amely a manipulátor differenciális kinematikai egyenlete 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 11
A geometriai Jacobi-mátrix a 6 x n-es méretű J mátrix a manipulátor geometriai Jacobi-mátrixa amely a csuklóváltozók általános függvénye 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 12
A forgatási mátrix deriváltja Tekintsük a P pont 1. bázis és 0. bázis közötti koordináta-transzformációját: 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 13
A forgatási mátrix deriváltja a kifejezést idő szerint differenciálva kapjuk: felhasználva a következő összefüggést: az eredmény: 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 14
A forgatási mátrix deriváltja Vezessük be a következő jelölést: = Ekkor: Ha p 1 állandó, akkor mivel ebben az esetben 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 15
Manipulátor-struktúra i. szegmense p i : az i. bázis origója p i-1 : az i-1. bázis origója r i-1,i : az i. bázis origójának i-1. bázisra vonatkozó koordinátája 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 16
Az i. szegmens pozíciójának változása Ismétlés: (1) Az i+1. szegmens origójának pozíciója: Az (1) egyenletet differenciálva: Ezt alkalmazva kapjuk: 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 17
A szegmens szögsebessége kiindulás: felhasználva, hogy (1) időbeli deriváltja a következő: ahol az jelöli az i. bázis i-1. bázishoz képesti szögsebességét az i-1. bázisban kifejezve összefüggésből adódik: 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 18
A szegmens szögsebessége Tudjuk, hogy Ebből következik: A végeredmény: 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 19
Transzlációs csukló Az i. bázis i-1. bázishoz képesti orientációja az i. csukló mozgatásával nem változik, tehát: A pozíció változása: ahol z i-1 az i. csukló mozgástengelyén fekvő egységvektor A szögsebesség a következőképp számolható: 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 20
Transzlációs csukló A pozíció változása: 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 21
Rotációs csukló A szögsebesség: A lineáris sebesség: Az i. bázis i-1. bázishoz képesti forgása miatt 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 22
Rotációs csukló A szögsebesség és lineáris sebesség általános formulái: Rotációs csuklónál a következő alakúak lesznek: 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 23
A Jacobi-mátrix kiszámítása A Jacobi mátrix: Partícionáljuk 3 x 1-es oszlopvektorokra a következőképp: az i. csukló hozzájárulása a végberendezés lineáris sebességéhez: az i. csukló hozzájárulása a végberendezés szögsebességéhez: 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 24
A Jacobi-mátrix kiszámítása A szögsebességhez való hozzájárulás: Ha az i. csukló transzlációs Ha az i. csukló rotációs 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 25
A Jacobi-mátrix kiszámítása A lineáris sebességhez való hozzájárulás: Ha az i. csukló transzlációs: Ha az i. csukló rotációs (a végberendezés origójának sebességéhez való hozzájárulást számoljuk): 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 26
A Jacobi-mátrix kiszámítása Összefoglalás: transzlációs csukló esetén rotációs csukló esetén 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 27
A Jacobi-mátrix kiszámítása A z i-1, p és p i-1 vektorok a csuklóváltozók függvényei 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 28
A Jacobi-mátrix kiszámítása z i-1 : forgatási mátrix harmadik oszlopa: ahol p a mátrix utolsó oszlopának első három eleme p homogén formában: ahol: p i-1 homogén alakban: 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 29
A Jacobi-mátrix kiszámítása Az előzőekben a Jacobi-mátrixot az alap-bázisban írtuk fel. Ha az u bázisra vonatkozóan szeretnénk felírni, akkor elég ismerni az R u forgatási mátrixot. A két bázisban felírt sebességek kapcsolata: Behelyettesítve a összefüggésbe 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 30
A Jacobi-mátrix kiszámítása Ha az u bázis időben nem változik: 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 31
Három szegmensű síkbeli kar 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 32
Három szegmensű síkbeli kar Az egyes szegmensek (csuklók) pozíciói: 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 33
Három szegmensű síkbeli kar A rotációs csuklók mozgástengelyeihez tartozó egységvektorok: J mátrix felbontása (volt): 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 34
Három szegmensű síkbeli kar A kiszámított J mátrix: 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 35
Kinematikai szingularitások Két szegmensű síkbeli kar a szingularitás határán A differenciális kinematika alapegyenlete (q-tól függő lineáris leképezés): ahol a csuklósebességek vektora, és a végberendezés sebessége 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 36
Kinematikai szingularitások Kinematikai szingularitások: azok a konfigurációk, ahol a J mátrix nem teljes rangú A kinematikai szingularitások jelentősége: A szingularitások olyan konfigurációkhoz tartoznak, ahol a szerkezet mobilitása lecsökken, azaz nem lehet a végberendezéssel tetszőleges mozgást végeztetni. Ha a szerkezet szinguláris konfigurációban van, akkor az inverz kinematikai problémára végtelen megoldás is létezhet. Szingularitás környezetében a műveleti tér kis sebességeihez nagy sebességek tartozhatnak a csuklóváltozók terében. 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 37
Kinematikai szingularitások A kinematikai szingularitások csoportosítása: Határ-szingularitások: akkor jelennek meg, ha a manipulátor teljesen kihúzott vagy összecsukott állapotban van. Elkerülhetők, ha a manipulátort nem vezéreljük ki az elérhető munkatér határáig. Belső szingularitások: az elérhető munkatér belsejében vannak, és általában a mozgástengelyek elrendezése vagy bizonyos speciális végberendezéshelyzetek okozzák őket. Súlyos problémát jelenthetnek bizonyos trajektóriák követésénél. 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 38
Differenciális kinematikai inverzió Probléma: adott egy kívánt v(t) mozgás trajektória és a pozíció ill. orientáció kezdeti értékei. Határozzunk meg egy megvalósítható q t, q t trajektóriát a csuklóváltozók terében, amelynek eredménye v(t). a v=j q q egyenletből kiindulva n=r esetén a megoldás: q t =J 1 q t v t azaz egy elsőrendű nemlineáris közönséges differenciálegyenlet-rendszer 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 39
Redundáns manipulátorok Ha a manipulátor redundáns (r<n), akkor a Jacobimátrixnak több oszlopa van, mint sora, és a v=j q q egyenletre végtelen sok megoldás létezik valahogy ki kellene választani egyet. Válasszuk ki azt, amely minimalizálja a következő célfüggvényt: g q = 1 2 q T W q ahol W egy megfelelő n x n-es szimmetrikus pozitív definit súlyozómátrix 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 40
Redundáns manipulátorok A megoldás (levezetés nélkül): q=w 1 J T JW 1 J T 1 v Speciális eset: ha W egységmátrix, akkor: q=j T JJ T 1 v ahol J T JJ T 1 a J mátrix jobb oldali pszeudoinverze 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 41
Geometriai és analitikus Jacobi-mátrix geometriai Jacobi-mátrix: v=[ ṗ ] =J q q analitikus Jacobi-mátrix: a végberendezés pozícióját és orientációját a műveleti tér változóinak segítségével fejezzük ki (pl. pozíció és Euler-szögek) x=[ ] p =k q ẋ= k q q q=[ J p q J q ] J A q q 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 42
Geometriai és analitikus Jacobi-mátrix Példa: két szegmensű síkbeli kar Kinematikai függvény Az analitikus Jacobi-mátrix 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 43
Inverz kinematikai algoritmus Az inverz differenciálkinematikai egyenlet diszkretizálása időben: Probléma: a diszkrét idejű egyenlet megoldása az integráció pontatlansága miatt eltér a folytonos idejű megoldástól a kiszámított csuklóváltozókhoz tartozó végberendezés-pozíció és -orientáció eltér az előírtaktól (drift-jelenség). Megoldás: vegyük az előírt (x d ) és tényleges (x) végberendezés-pozíció és -orientáció közötti különbséget: e=x d x=x d k q 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 44
Inverz kinematikai algoritmus Vegyük a hiba időbeli deriváltját: ė= x d ẋ= x d J A q q Válasszuk meg q deriváltját a következőképpen: q=j 1 A q x d K e ahol K sajátértékeinek valós része negatív. Ekkor az aszimptotikusan stabil (nullához tartó) hibadinamika a következő: ė Ke=0 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 45
Az algoritmus blokkvázlata 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 46
Bevezetés a manipulátorok dinamikájába 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 47
A dinamikai feladat Manipulátorok dinamikus egyenletei: leírják, hogyan mozog a manipulátor, ha adottak az aktuátorok nyomatékai és az esetleges külső erők Két fontos alapprobléma: inverz dinamika: adott a végrehajtandó mozgás, kiszámítandók az általánosított erők (nyomatékok), mint bemenetek direkt dinamika: adott nyomatékok és erők esetén meg kell oldani a mozgásegyenleteket 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 48
A dinamikus egyenletek általános alakja Egy n csuklóval rendelkező manipulátor egyenleteinek általános alakja: q q q C M F G M q q C q, q q F q G q = csuklókoordináták csuklósebességek csuklógyorsulások Coriolis és centripetális erőhatások manipulátor inerciatenzora viszkózus és Coulomb súrlódás (általában nem modellezik) gravitációs hatás a q általánosított koordinátákhoz (csuklóváltozókhoz) tartozó általánosított erőhatások A robot szerkezetéből és fizikai paramétereiből (szegmensméretek, -tömegek) algoritmikusan számolható (pl. szimbolikus algebrai szoftverek segítségével) 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 49
A dinamikus egyenletek általános alakja A dinamikus egyenletek irányítási célra legjobban használható tömör formája: H q q h q, q = ahol H(q) pozitív definit, szimmetrikus mátrix 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 50
Trajektóriakövetési feladat linearizálással Feladat: a csuklókoordináták megadott időfüggvényének (q ref (t)) követése visszacsatolással. Linearizáló visszacsatolás: =H q u h q, q ahol u az új referenciabemenet. A megadott bemenettel a rendszer egyenletei lineárisak és irányíthatók lesznek az új referenciabemenettel: q=u 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 51
Trajektóriakövetési feladat linearizálással Jelölés: z= q Az állapottér-modell alakja ekkor: q=z ż=u A követési hibák (n csukló esetén): e i =q i q i, ref, f i = q i q i, ref =ė i, A követési hibadinamika: ė i = f i i=1,..., n i=1,..., n ḟ i =u i q i,ref, i=1,...,n 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 52
Trajektóriakövetési feladat linearizálással A referenciakövető visszacsatolás: u i = q i, ref k i1 e i k i2 f i Ahol a k konstansokat úgy kell megválasztani, hogy az (e i, f i ) állapotváltozókkal rendelkező lineáris rendszer stabil legyen 2005 HEFOP 3.3.1-P.-2004-06-18/1.10 53