2010 sz, Debreceni Egyetem
Csuklós szerkezetek animációja (Kép 1985-b l: Tony de Peltrie)
Csontváz-modellek
Csuklós szerkezet (robotkar) A robotkar részei: csuklók (joints) rotációs prizmatikus (transzlációs) szegmensek (links) végberendezés (end eector) csukló végberendezés szegmens robotkar rotációs csuklókkal
A robotkar leírása Állapotvektor Független paraméterek vektora, amely a csuklók és a végberendezés helyzetét egyértelm en megadja. Az állapotvektor komponenseinek száma a szabadsági fokok száma (DOF). Az összes lehetséges állapotvektor az állapotteret alkotja. Kinematika vs. inverz kinematika direkt probléma (kinematika): az állapotvektorból határozzuk meg a végberendezés helyzetét: E = f (Θ) inverz probléma (inverz kinematika): a végberendezés helyzetéb l következtetünk az állapotvektorra: E Θ
2 szabadsági fokú 2D robotkar leírása az állapotvektorral Prizmatikus csukló nélkül: Θ = (θ 1, θ 2 ) R 2. a 2 θ 2 a 1 θ 1 start 1. lépés 2. lépés J 1 = (0, 0) - - J 2 = (a 1, 0) J 2 rot J1 (θ 1 )J 2 - E = (a 1 + a 2, 0) E rot J1 (θ 1 )E E rot J2 (θ 2 )E E = (a 2 cos (θ 2 + θ 1 ) + a 1 cos θ 1, a 2 sin (θ 2 + θ 1 ) + a 1 sin θ 1 )
3D robotkar leírása: DenavitHartenberg (DH) paraméterek x i a (z i, z i+1 ) normáltranszverzálisa, a csukló a z tengely körül forog. a i = d(z i, z i+1 ) α i = (z i, z i+1 ) d i+1 = d(x i, x i+1 ) θ i+1 = (x i, x i+1 ) z i+1 x i+1 link i z i d i+1 x i a i
3D robotkar megadása az állapotvektorral start 1. lépés n. lépés (n = 2,... N) N + 1 J 0 J 1 = J 0 J n = J n 1 + a n 1e 1 E = J N + a N e 1 e 3 = (0, 0, 1) e 3 rot(e 1, α 0)e 3 e 3 rot(e 1, α n 1)e 3 e 1 = (1, 0, 0) e 1 rot(e 3, θ 1)e 1 e 1 rot(e 3, θ n)e 1 J n J n + d ne 3
Forgatás a térben Origón áthaladó tengely körüli elforgatás A p pont elforgatása n irányvektorú ( n = 1) tengely körül θ szöggel: R(p) = cos(θ) p + (1 cos(θ)) n n, p + sin(θ) n p Innitezimális elforgatás R kifejezésében a szögfüggvényeket 0 körül Taylor sorba fejtjük és az els Taylor polinomjukkal helyettesítjük: sin θ θ, cos θ 1: R inf. (p) = p + θ n p
Csuklós szerkezet animációja Csuklós szerkezet animációja a végberendezés meghatározása az állapotvektorból: E = f (Θ) R n görbe az állapottérben: c : [a, b] R N, c(t) = (θ 1 (t),..., θ N (t)) animáció: E : [a, b] R n, E(t) = f (Θ(t)) = f (θ 1 (t),..., θ N (t)). Az állapottér görbéje kulcs-fázisok: t 1 = a < t 2 < < t m = b, az i-edik kulcsfázis c i = (θ i,1,..., θ i,n ). a kulcs-fázisok összekötése interpolációs görbével (pl. harmadfokú szplájnnal): θ j (t): [a, b] R, θ j (t i ) = θ i,j
Inverz kinematika f : R N R n, E = f (Θ), E Θ? Ötlet f nemlineáris (sin, cos függvények) analitikus megoldás általában nem lehetséges b megoldáshalmaz (redundancia, ugyanazt a térbeli helyzetet a végberendezés az állapottér több pontján is felveheti) üres megoldástér (elérhetetlen cél) numerikus megoldásra van szükség Linearizáció!
Inverz kinematika: Jacobi-mátrix módszer f : R N R n, E = f (Θ) = de = df (Θ)dΘ, df (Θ) M n N Jacobi-mátrix: lineáris approximáció f -re df (Θ) = f 1 f 1 θ 1 f 2 f 2 θ 1. f n θ 1 θ 2... θ 2.... f n θ 2... f 1 θ N f 2 θ N. f n θ N Θ
3D csuklós szerkezet Jacobi mátrixa Mozgó tengely formula J i = i θ j n j (J i J j 1 ) = j=1 df (Θ) = n 1 (P i P 0 ) n 2 (P i P 1 ). n i (P i Pi 1)
Inverz kinematika: Jacobi-mátrix módszer Elemzés ha n = N és df (Θ) invertálható, akkor dθ = df (Θ) 1 de : iteratív módszer alkalmazható df (Θ) invertálhatósága az állapotvektortól függ az iterációs lépés hibája df (Θ)dΘ de) ha N > n, akkor a rendszer kinematikailag redundáns, általában végtelen sok megoldása létezik általánosított invertálás eljárások adnak megoldást: dθ = df (Θ) de (Pl. Moore Penrose pszeudoinverz) másodlagos kritérium beiktatása
Inverz kinematika: invertálható Jacobi-mátrix Pszeudokód Input: Θ 0, T Output: Θ, melyre f (Θ) = T Θ Θ 0 de = (T f (Θ 0 ))/k for i = 1 to k do Θ Θ + df (Θ) 1 de return Θ
A Jacobi-módszer hibája Piros: az optimális pálya, piros háromszög: a cél Egy iterációs lépés hibája (tracking error): df (dθ) de
A tracking error javítása Az iterációs lépés hibájának gyelése (közbens cél kijelölése) és a célirány folyamatos korrekciója: Pszeudokód Input: Θ 0, T,, δ Output: Θ, melyre f (Θ) = T Θ Θ 0 de = T f (Θ) while T f (Θ) > do if df (df (Θ) 1 de) de < δ then Θ Θ + df (Θ) 1 de de = T f (Θ) else de (de)/2 return Θ
Redundáns/szinguláris esetek Általánosított inverz X a J általánosított inverze, ha az alábbi tulajdonságok valamelyike teljesül: JXJ = J XJX = X (XJ) t = XJ (JX ) t = JX X a J mátrix pseudoinverze, ha mind a négy tulajdonság teljesül. Jelölés: X = J. + a pszeudoinverz egyértelm - (relatíve) lassú a kiszámolása - szingularitásoknál instabil J t általánosított inverz és jó eredményt ad!
Inverz kinematika: minimum módszer f : R N R n, E = f (Θ); D : R N R, D(Θ) = E(Θ) T 2 A végberendezés a célban van D(Θ) = 0, azaz Θ a D függvény abszolút minimumhelye. Function 3.5 3 2.5 2 1.5 1 3.5 3 2.5 2 1.5 1 0.5 0 0.5 0 3 2 v 1 0-1 -2-3 -3-2 -1 0 1 2 3 u
Inverz kinematika: minimum módszer Numerikus megoldás a minimumra: a legmeredekebb lejt módszere (gradiens módszer) Θ k+1 = Θ k hdd(θ k )
Feladatok Feladat 2D csuklós szerkezet megrajzolása az állapotvektor ismeretében. A szegmensek hosszát a program input adatként kérje. 2D csuklós szerkezet animációjának megvalósítása az állapovektor paraméterezésével. (Kulcsfázisokból.) 3D csukló megrajzolása (pl. ortogonális axonometriában) az állapotvektor ismeretében. A program tartalmazza a gömbcsukló lehet ségét is. Inverz kinematika (2D): egérrel kijelölt cél megtalálása. Inverz kinematika (2D): egeret követ csuklós szerkezet. Inverz kinematika (2D): Animáció, melyben egy csuklós szerkezet végberendezése egy adott görbén mozog. Inverz kinematika: a Jacobi-mátrix módszer és a minimum módszer hibájának összehasonlítása. Inverz kinematika: két különböz általánosított inverz módszer összehasonlítása.