Infobionika ROBOTIKA. XI. Előadás. Robot manipulátorok III. Differenciális kinematika. Készült a HEFOP P /1.0 projekt keretében

Átírás

1 Infobionika ROBOTIKA XI. Előadás Robot manipulátorok III. Differenciális kinematika Készült a HEFOP P /1.0 projekt keretében

2 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

3 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 P /1.10 3

4 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 P /1.10 4

5 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 P /1.10 5

6 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 P /1.10 6

7 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 P /1.10 7

8 A forgatási mátrix deriváltja Tekintsük a következő példát: 2005 HEFOP P /1.10 8

9 A forgatási mátrix deriváltja 2005 HEFOP P /1.10 9

10 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 P /

11 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 P /

12 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 P /

13 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 P /

14 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 P /

15 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 P /

16 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 P /

17 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 P /

18 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 P /

19 A szegmens szögsebessége Tudjuk, hogy Ebből következik: A végeredmény: 2005 HEFOP P /

20 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 P /

21 Transzlációs csukló A pozíció változása: 2005 HEFOP P /

22 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 P /

23 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 P /

24 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 P /

25 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 P /

26 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 P /

27 A Jacobi-mátrix kiszámítása Összefoglalás: transzlációs csukló esetén rotációs csukló esetén 2005 HEFOP P /

28 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 P /

29 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 P /

30 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 P /

31 A Jacobi-mátrix kiszámítása Ha az u bázis időben nem változik: 2005 HEFOP P /

32 Három szegmensű síkbeli kar 2005 HEFOP P /

33 Három szegmensű síkbeli kar Az egyes szegmensek (csuklók) pozíciói: 2005 HEFOP P /

34 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 P /

35 Három szegmensű síkbeli kar A kiszámított J mátrix: 2005 HEFOP P /

36 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 P /

37 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 HEFOP P /

38 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 HEFOP P /

39 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 P /

40 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 P /

41 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 P /

42 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 P /

43 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 P /

44 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 P /

45 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= HEFOP P /

46 Az algoritmus blokkvázlata 2005 HEFOP P /

47 Bevezetés a manipulátorok dinamikájába 2005 HEFOP P /

48 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 P /

49 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 P /

50 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 P /

51 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 P /

52 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 P /

53 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 P /