Robotszerkezetek animációja

Robotszerkezetek animációja Kovács Zoltán 1. Bevezetés A számítógépi animáció megvalósításakor valamely virtuális világbeli adatot időfüggően adunk meg. Pédául egy felfúvódó léggömb esetében a (gömbbel modelezett) léggömb sugara egy időben változó adat, de lehet a léggömb színét megadó (r, g, b) adatokat is időtől függővé tenni (ahogyan nagyobb lesz a léggömb úgy halványul), ha a szél is fúj, akkor már a léggömb középpontjának (x, y, z) koordinátái is időben változnak. Az adatok időbeli megadása jellemzően függvényvezérlő-pontokkal történik, azaz (t i, f i ), (i = 1,..., N) koordinátapárokkal, amelyekre valamilyen interpolációs módszerrel illesztjük az f függvényt (f(t i ) = f i ). Úgy is fogalmazhatunk, hogy a szóban forgó adat értékét csak bizonyos kulcsfázisokban adjuk meg, minden más fázisban az adat értékét valamilyen interpolációs módszerrel számítjuk. (Ld. 1. ábra.) A karakter animáció egy gyakori megoldása, hogy csontokból és ízületekből felépítjük a karakter csontvázát. Ehhez a csontvázhoz rendeljük hozzá a karakter háromszöghálóját. (A számítógépi animációban ez utóbbi folyamatot bőrözésnek nevezik.) Az ízületek biztosítják a csontok elfordulását. Az ízület állapota tehát (a legegyszerűbb esetben, amikor csak egy tengely körüli forgást biztosít minden ízület) egy szöggel adható meg. Ha minden ízülethez 1 http://www.huro-cbc.eu

f(t) t 0 t 1 t 2 t 3 t 1. ábra. megadunk egy szöget, akkor egy pózt kapunk. A szögek időfüggő megadása biztosítja a csontváz, és a bőrözésen keresztül a karakter animációját. Mint azt az előbb említettük, csak vezérlő pózokkal dolgozunk, a közbenső fázisokban a csontváz helyzetét interpolációs módszerekkel automatikusan generáljuk. Erre a módszerre az egyik első példa volt a YouTube-on elérhető, 1985- ből származó Tony de Peltrie c. film (rendezte P. Lachapelle, P. Bergeron, P. Robidoux). Bergeron a film karakterét úgy jellemezte, hogy... énekel, beszél, mozog, mint egy igazi ember. Bár az arca karikatúra-szerű, a kifejezései természetesek, érzéseket és érzelmeket fejeznek ki. Ebben a jegyzetben a csontváz animációjának matematikai alapjaival foglalkozunk. A gyakorlatban egy karakter-animáció készítéséhez természetesen különböző programokat használunk. Népszerű a GPL felhasználói szabadsággal rendelkező Blender program. A 2. ábra egy Blenderben modellezett karakter csontvázát mutatja. A számítógépi geometria matematikai alapjait az olvasó pl. a [2] jegyzetből ismételheti át. Jelen jegyzet feldolgozásához szükség van a homogén koordináták használatára, a geometriai transzformációk, különösen az eltolások és forgások mátrixos megadására. Ezen kívül az elemi koordinátageometria és mátrixszámítás ismeretét tételezzük fel. Az inverz kinematikai feladat megoldása során használjuk a vektor vektor függvények differenciálszámításának alapjait, így a többváltozós függvények parciális deriváltjának, skalármező gradiensének fogalmát. A jegyzetben alkalmazott numerikus módszerek pontos matematikai tárgyalása túlmenne a jegyzet keretein, ezért ismertetésük szemléletes. Így például nem tárgyaljuk a módszerek konvergenciájának kérdését. A közölt pszeudokódok olvasásához felhívom a figyelmet arra, hogy az 2

2. ábra. Blenderben modellezett karakter csontváza összetartozó programrészek behúzással vannak jelölve, tehát end utasítást nem használok. 2. Robotkarok reprezentációja, robotkarok direkt kinematikai feladata Az ipari robotkarok egymáshoz csuklókkal csatlakozó merev szegmensekből és végberendezésből állnak. A csuklók lehetnek rotációs csuklók, ezek a szegmensek forgását biztosítják, továbbá lehetnek prizmatikus (vagy teleszkópikus) csuklók, ezek a szegmensvégek közötti távolságot tudják változtatni. Az ipari robotkarok sokkal bonyolultabbak lehetnek a számítógépi grafikában leggyakran használt robotkaroknál. Ebben a jegyzetben olyan robotkarokkal foglalkozunk, amelyek csak rotációs csuklót tartalmaznak és szerkezetük 1. csukló 1. szegmens 2. csukló 2. szegmens... N. csukló N. szegmens végberendezés típusú, és egy csukló forgástengelye merőleges a csuklóhoz tartozó szegmens(ek)re. (Egy szegmenshez csatlakozó két csukló forgástengelye nem feltétlenül párhuzamos.) A legelső csukló helyzete fix, legyen ez az origó, és megállapodás szerint a hozzá tartozó forgástengely a Descartesféle koordináta-rendszer z-tengelye. (Ld. 3. ábra.) Az olyan robotkaroknál, amelyek csak az xy síkban tudnak mozogni, minden tengely párhuzamos a z-tengellyel. A szegmensek (a 1, a 2,..., a N ) hosszával és az egymást köve- 3

3. ábra. Két rotációs csuklóval rendelkező robotkar két állapota. A csuklókhoz tartozó forgástengelyek (az ábrán pirossal színezve) nem párhuzamosak, szögük azonban a mozgás során változatlan. Az ábra a Pov-Ray modellező programmal készült. 4

tő forgástengelyek (α 1, α 2,..., α N 1 ) irányított szögével a robotkar típusát megadtuk, ezek a robotkar kötött paraméterei, melyek a robotkar mozgása során nem változnak. (Az utolsó szegmenshez csatlakozó végberendezéshez nem tartozik forgástengely.) Egy szegmens hossza lehet nulla is, így minden irányban mozgó ízületeket is tudunk modellezni. Síkbeli robotkarnál a forgástengelyek egymáshoz viszonyított szögének megadására természetesen nincs szükség. Lényeges a szegmensek hierarchikus viszonya, a legelső szegmenstől eltekintve minden szegmens az előtte levő szegmens leszármazottja. Eszerint egy csuklónál szülő-gyerek viszony van a szegmensek között: a k-adik csuklónál (N k 1) a (k 1)-edik szegmens a szülő, a k-adik szegmens pedig a gyerek. A szegmensek hossza kötött, a robotkar mozgását a csuklók biztosítják. Minden csuklóhoz (a végberendezéstől eltekintve) egy törésszög tartozik, a k. csuklóhoz a θ k -val jelölt szög. A legelső csuklóhoz tartozó szög az első szegmens x-tengellyel bezárt irányított szöge. A többi csuklónál a törésszög olyan irányított szög, melynek egyik szára a csuklóba befutó szülő meghosszabbítása, a másik szára a csuklóhoz csatlakozó gyerek. A törésszögek sorozatából képzett (θ 1, θ 2,..., θ N ) vektor a robotkar állapotvektora. 1. Definíció. Az (a 1, a 2,..., a N ), (α 1, α 2,..., α N 1 ), (θ 1, θ 2,..., θ N ) paramétereket a robotkar Denavit Hartenberg paramétereinek nevezzük. A definíciót korlátozottabban használjuk, mint az ipari robotoknál, ahol fellép egy más típusú paramétervektor is. 2. Definíció. A robotkar munkatere mindazon pontok halmaza, amelyet a végberendezés elérhet. Egy robotkar direkt kinematikai feladata a végberendezés helyzetének meghatározása az állapotvektor ismeretében. A robotkar inverz kinematikai feladata a végberendezés egy rögzített, munkatérben elhelyezkedő helyzetéhez tartozó valamely állapotvektor meghatározása. A grafikában természetesen nem csak a végberendezés helyzete a fontos, hanem minden csukló helyzete is, hiszen az egész kart le kell rajzolnunk. 1. Feladat. Adjunk meg egy síkbeli robotkart a szegmensek (a 1,..., a N ) hosszával és a (θ 1,..., θ N ) állapotvektorral. Valamely általunk választott nyelven készítsük el a robotkar rajzát! Teszteljük programunkat az egyenlő hosszúságú szegmensekkel és (60, 120, 120) állapotvektorral adott robotkarral, ahol az állapotvektor szögeit fokokban adtuk meg! 5

θ 2 a 2 a 1 θ 1 x 4. ábra. Rotációs csuklókkal rendelkező robotkar állapotvektorának értelmezése. Az ábrán két rotációs csuklóból, két szegmensből és végberendezésből álló síkbeli robotkar látható. (θ 1, θ 2 ) a robotkar állapotvektora. Az ilyen robotkart RRrobotkarnak is nevezik. Megoldás. Minden szegmenst egy hozzá rögzített Descartes-féle koordinátarendszerben rajzolunk meg. Ezt a Descartes-féle koordináta-rendszert úgy kapjuk, hogy origója a szegmens kezdőpontja, az x-tengelye a szegmenst tartalmazza, mégpedig úgy, hogy a szegmens végpontja a pozitív x-féltengelyen legyen. Így, ha a szegmens hossza a k, akkor a szegmens rajzakor az origót kell összekötni az (a k, 0) ponttal. A koordináta-rendszert a szegmenshez a koordináta-rendszer transzformációival igazítjuk. (Ld. 5. ábra!) A feladat megoldásának algoritmusa a következő (ld. 1. pszeudokód). Válasszuk ki a k. csuklót (k 2). Legyen az Descartes-féle koordináta-rendszer origója a csuklóban rögzítve, x tengelye pedig olyan irányú, mint a csuklóhoz tartozó szülő. Ha θ k szöggel elforgatjuk a koordinátarendszert, akkor a csuklóhoz tartozó gyerek végpontjának (azaz a következő csuklónak) a koordinátái (a k, 0). A legelső szegmensre az elmondottak ugyanúgy érvényesek, ha az első csuklónál a szülő a pozitív x-tengely, a gyerek pedig maga a szegmens. A pszeudokódban rot(α) jelenti azt, hogy az új koordináta-rendszer a régiből annak origója körüli α irányított szögű elforgatásával származik, 6

θ k gyerek szülő 5. ábra. A szegmenshez igazított Descartes-féle koordináta-rendszer. Zöld: a szülő koordinátarendszere, eltolva a k. csuklóhoz. Piros: a gyerek koordinátarendszere a k. csuklónál, ebben rajzoljuk meg a k + 1. csuklót. Kék: a gyerek koordinátarendszere a k + 1. csuklóhoz eltolva. T(v) pedig a koordináta-rendszer origójának v vektorú eltolására utal. A-val a rajzlap Descartes-féle koordináta-rendszerét jelöltük. A grafikus állapot tárolására és előhívására azért van szükség, hogy a robotkar rajza után a Descartes-féle koordinátarendszer visszaálljon az eredeti helyzetébe, így a rajzot tovább tudjuk folytatni. 1. DrawChain Input: (θ 1,..., θ N ), (a 1,..., a N ), 1: A grafikus állapot tárolása 2: for i = 1 to N do 3: A rot(θ i )A 4: Vonal húzása (0, 0)-tól (a i, 0)-hoz 5: A T(a i, 0)A 6: A grafikus állapot előhívása A feladat PostScript nyelven írt megvalósítását ld. az 1. programleírásban. (Vigyázzunk az indexek eltolására, a vektorok első komponensének indexe 0.) 7

6. ábra. Az 1. feladat megoldása: 1, 2, 2, 1 hosszúságú szegmensekkel és (60, 30, 20, 75) állapotvektorral rendelkező síkbeli robotkar. Az állapotvektor szögei fokokban vannak megadva. A kész rajz a 6. ábrán látható. A feladatban megadott teszt robotkar egy szabályos háromszöget formáz. 2. Feladat. Adjunk meg egy síkbeli robotkart a szegmensek (a 1,..., a N ) hosszával és a (θ 1,..., θ N ) állapotvektorral. Számítsuk ki a végberendezés koordinátáit. Konkrét számítási feladatként legyen a szegmensek hossza rendre 1, 2, 2, 1 és az állapotvektor (60, 30, 20, 75). Megoldás. A megoldásban ismét az 1. algoritmust használjuk. Ha az algoritmust lefuttajuk, akkor az utolsó koordinátarendszerben a végberendezés koordinátái [0, 0, 1], a kérdés az, hogy a rajzlap eredeti Descartes-féle koordináta-rendszerében ez milyen koordinátákat jelent. A koordinátarendszer transzformációjának mátrixát kell kiszámítani. Az algoritmus 3. sorának megfelelő mátrix cos θ i sin θ i 0 A i = sin θ i cos θ i 0, 0 0 1 míg az 5. sornak megfelelő mátrix 1 0 a i T i = 0 1 0. 0 0 1 Homogén koordinátákat használunk. 8

Listing 1. Az 1. feladat PostScript megoldása /C [ 0 0 ] def %kezdo c s u k l o /a [ 1 2 2 1 ] def %szegmensek hossza / theta [ 6 0 30 20 75] def %a l l a p o t v e k t o r /n a length def gsave C aload pop translate 0 1 n 1 sub{ / i exch def theta i get rotate newpath 0 0 moveto a i get 0 lineto stroke a i get 0 translate } for grestore 9

Listing 2. wxmaxima kód részlete a 2. feladathoz A: m a k e l i s t ( matrix ( [ cos ( theta [ i ]), s i n ( theta [ i ] ), 0 ], [ s i n ( theta [ i ] ), cos ( theta [ i ] ), 0 ], [ 0, 0, 1 ] ), i, 1, n ) A koordináta-rendszer transzformációja pedig M = A 1 T 1 A N T N. M (0, 0, 1) t adja az utolsó csukló koordinátáit. Descartes-koordinátákkal a megoldás (M 1,3, M 2,3 ). A wxmaxima programban a forgatások mátrixainak sorozatát a 2. kódrészlet szerint adjuk meg. 3. Feladat. Adott egy térbeli robotkar a szegmensek hosszával és az egymást követő forgástengelyek szögével: (a 1,..., a N ), (α 1,..., α N 1 ). Számítsuk ki a csuklók és a végberendezés helyzetét a (θ 1,..., θ N ) állapotvektorban! Konkrét számítási feladatként legyen a = (1, 1), α 1 = π/2 Θ = (π/2, π/2). Megoldás. A csuklók és a végberendezés helyzetét a 2. algoritmus szerint kapjuk meg, ahol rot(e, α) jelenti az e tengely körüli α szögű forgást. A számítás elvégzésekor szükségünk van az origón áthaladó, általános tengely körüli térbeli elforgatás kiszámítására. Ha a tengely irányvektora az e egységvektor, az elforgatás szöge pedig α, akkor ennek képlete rot(α, e)p = cos α p + (1 cos α) e, p e + sin α e p, p R 3. (1) Az (1) formulában a vektoriális szorzás,.,. pedig a skaláris szorzás jele. 4. Feladat. Az (1) formula alapján számítsuk ki a koordinátatengelyek körüli elforgatások mátrixát! 10

2. DrawChain3D Input: (θ 1,..., θ N ), (a 1,..., a N ), (α 1,..., α N 1 ) Output: (J 1,..., J N, E) 1: J 1 = (0, 0, 0), e 1 = (1, 0, 0), e 3 = (0, 0, 1) 2: e 1 rot(θ 1, e 3 )e 1 3: for i = 2 to N do 4: J i = J i 1 + a i 1 e 1 5: e 3 rot(α i 1, e 1 )e 3 6: e 1 rot(θ i, e 3 )e 1 7: E = J N + a N e 1 Megoldás. Példaként tekintsük az x-tengely körüli elforgatást! Ekkor e = (1, 0, 0) és legyen p = (x, y, z). e, p = x, e p = (0, z, y), így x = x y = cos α y sin α z z = sin α y + cos α z. A keresett mátrix tehát 1 0 0 0 cos α sin α. 0 sin α cos α 3. Robotkar inverz kinematikai feladata Jelölje a robotkar végberendezésének koordinátáit E R n, a robotkar állapotvektora Θ R N. Az állapotvektor meghatározza a végberendezés helyzetét, a függvénykapcsolat: f : R N R n, E = f(θ). A robotkar inverz kinematikai feladatához meg kell E-hez határozni egy lehetséges Θ állapotvektort (ha van ilyen), azaz meg kell adnunk f 1 (Θ) (azaz 11

Θ teljes inverz képének) egy elemét. A probléma nehézségének megérzéséhez vegyük figyelembe, hogy f trigonometrikus függvényeket tartalmazó vektor-vektor leképezés, és még a legegyszerűb N = n = 2 esetben sem invertálható. (RR-típusú síkbeli robotkarnál az állapottér pontjaihoz általában két állapotvektor is tartozik jóllehet ez az egyszerű eset elemi geometria eszközökkel explicite megoldható.) Mindebből következik, hogy hatékony megoldást általában numerikusan kell keresni. A megoldás egyik lehetséges ötlete a probléma linearizációja: E = f(θ) = E df(θ) Θ, (2) ahol f 1 Θ 1... f 2 df(θ) R n N Θ, df(θ) = 1..... f n Θ 1... az f Jacobi mátrixa Θ-ban. Rögzített Θ mellett a f 1 Θ N f 2 Θ N. f n Θ N df(θ) Θ = E (3) egyenletre mint egy inhomogén lineáris egyenletrendszerre kell tekintenünk, melynek alapmátrixa df(θ), az ismeretlenek vektora pedig Θ. Így a megoldás során az inhomogén lineáris egyenletrendszerek approximációs módszereit lehet használni. Ettől a probléma azonban nehezebb, mert az alapmátrix maga is tartalmazza Θ-t, így numerikusan csak iteratív megoldások jöhetnek szóba: egy iterációs lépésben a (Θ, E) párhoz meghatározzuk (3) alapján Θ-t, majd (Θ + Θ, E )-vel folytatjuk mindaddig, míg a kívánt pontosságon belül a célt megtaláltuk. ( E meghatározásával később még foglalkozunk.) 5. Feladat. A 3. feladattól függetlenül, elemi geometriai eszközökkel adjuk meg egy RR síkbeli robotkar állapotfüggvényét! Oldjuk meg elemi módszerrel az inverz kinematikai feladatot! Megoldás. A szegmensek hossza legyen a és b, az állapotvektor (θ 1, θ 2 ). A 7. ábra lapján a végberendezés első koordinátája OP 1 +P E 1 -el egyenlő (irányított szakaszok előjeles hosszával számolva). Az OP P 1 és P EE 1 derékszögű háromszögekben az θ 1 és θ 1 + θ 2 szögek szögfüggvényeit használva ez a kifejezés az alábbiak szerint számolható OP 1 + P E 1 = a cos θ 1 + b cos(θ 1 + θ 2 ). 12 Θ

y P E θ 1 + θ 1 2 b a E θ 1 O P 1 x 7. ábra. RR robotkar állapotfüggvénye elemi geometriai eszközökkel. Az ábrán θ 1 + θ 2 negatív előjelű! A második koordináta P 1 P + E 1 E-vel egyenlő, szintén irányított szakaszok előjeles hosszával számolva. Ezeket a szakaszokat az OP P 1 és P EE 1 derékszögű háromszögekben kiszámítva: P 1 P + E 1 E = a sin θ 1 + b sin(θ 1 + θ 2 ). Így a végberendezés függése az (θ 1, θ 2 ) állapotvektortól: f(θ 1, θ 2 ) = (a cos θ 1 + b cos(θ 1 + θ 2 ), a sin θ 1 + b sin(θ 1 + θ 2 )). (4) Az inverz feladat megoldásakor az OEP háromszögben ismertek az oldalak, lényegében a szögeket kell kiszámítani. Ezt a koszinusz-tételt alkalmazva könnyen megtehetjük. Ha a cél koordinátáit (x, y) jelöli, θ 2 = arccos a2 + b 2 x 2 y 2 2ab π Miután θ 2 -t meghatároztuk, behelyettesíthetünk (4)-be: (a + b cos θ 2 ) cos θ 1 + ( b sin θ 2 ) sin θ 1 = x ( b sin θ 2 ) cos θ 1 + (a + b cos θ 2 ) sin θ 1 = y. 13

cos θ 1 -re a Cramer-szabály szerint megoldva: cos θ 1 = x(a + b cos θ 2) + yb sin θ 2 (a + b cos θ 2 ) 2 b 2 sin 2 θ 2, ahonnan θ 1 arkusz koszinusz függvénnyel meghatározható. Megjegyezzük még, hogy 0 < OE < a + b esetén az OE tengelyre szimmetrikusan két megoldás is lehetséges, míg OE > a + b esetén a cél nem elérhető. 3.1. Invertálható Jacobi-mátrixú probléma A legegyszerűbb eset, amikor n = N és a df(θ) mátrix invertálható, ekkor Θ df(θ) 1 E. (5) A probléma iterációval történő megoldása az (5) egyenlet alapján a következő. Legyen a végberendezés kezdő helyzete S, véghelyzete (az elérendő cél) T, k természetes szám (ennyi lépésben iterálunk). Egy iterációs lépésben a végberendezés helyzetének megkövetelt változása E = T S. k (5) alapján kapjuk az állapotvektor változását, azaz Θ i+1 = Θ i + df(θ i ) 1 E. (6) A végberendezés új helyzete f(θ i+1 ), a tényleges elmozdulás f(θ i+1 ) f(θ i ), így az iterációs lépés hibája f(θ i+1 ) f(θ i ) E. Az algoritmust a 3. pszeudokódban foglaltuk össze. A módszer hibáját a 8. ábrán tanulmányozhatjuk kis iterációs lépésszám mellett. A következőkben az inverz Jacobi-mátrix módszer hibájának javításával foglalkozunk. Erre elterjedt módszer, hogy E-t az eljárás során folyamatosan változtatjuk, a végberendezés aktuális állapotának megfelelően jelöljük ki, tehát az i + 1-edik lépésben E = T f(θ i ) irányú lesz. Azt hogy ezt a vektort hányszorosára (hanyadára) skálázzuk, úgy állapítjuk meg, hogy egy próba helyettesítést végzünk. Ha a végberendezés elmozdulása egy hibahatárt meghalad az elvárt E-hez képest, akkor 14

3. InverseJacobi Input: Θ 0, T Output: Θ, melyre f(θ) = T Θ Θ 0 E = (T f(θ 0 ))/k for i = 1 to k do Θ Θ + df(θ) 1 E return Θ 2.5 Target Start 2 y 1.5 1 0.5 0.5 1 1.5 2 2.5 x 8. ábra. Az inverz Jacobi-mátrix módszer 10 iterációs lépésben. Két szegmensből álló síkbeli RR-robotkar, wxmaxima eredmény. 15

E/2-vel újra próbálkozunk, stb. Ezt a módszert a közbenső cél kijelölés módszerének nevezzük. Tekintsük át az algoritmust még egyszer a 4. pszeudokód alapján! A pszeudokódban a H változó a cél megtalálásának hibaha- 4. TargetCorrection Input: Θ 0, T, H, δ Output: Θ, melyre f(θ) = T 1: Θ Θ 0 2: E = T f(θ) 3: while T f(θ) > H do 4: test Θ + df(θ) 1 E 5: if f(test) f(θ) E < δ then 6: Θ Θ + df(θ) 1 E 7: E = T f(θ) 8: else 9: E ( E)/2 10: return Θ tára, azaz akkor fogadjuk el a végberendezés helyzetét, ha a végberendezés és a cél távolsága ettől nem nagyobb. δ egy iterációs lépés hibahatára, azaz akkor fogadunk el egy elvárt E elmozdulást, ha a tényleges elmozdulás és az elvárt elmozdulás különbségének normája δ-t nem haladja meg. 1. A kiindulási helyzettel foglalkozunk, ez lesz az állapotvektor első értéke. 2. Az elvárt elmozdulás a cél és a kiindulási helyzet különbségvektora. 3. Ellenőrizzük, hogy a végberendezés a hibahatáron belül tartózkodik-e a célhoz képest. Ha igen, akkor az állapotvektort elfogadjuk (10. sor). Ha nem, akkor... 4.... ellenőrizzük, hogy a végberendezés elmozdulása az iterációs lépésben a hibahatáron belül megközelítené-e az elvárt elmozdulást. Ha igen, akkor iterálunk (6. sor), majd módosítjuk az elvárt elmozdulást a végberendezés új helyzetének megfelelően (7. sor), majd vissza a 3. sorhoz. Ha nem, akkor felére skálázzuk az elvárt elmozdulást (9. sor) és vissza a 3. sorhoz. 16

2.5 Target Start 2 y 1.5 1 0.5 0.5 1 1.5 2 2.5 x 9. ábra. Inverz Jacobi-mátrix módszer a közbenső cél kijelölésének módszerével, wxmaxima eredmény. δ = 0, 25, H = 0, 01. A program tényleges megírásakor be kell építenünk az iterációs lépések számának figyelését, ha ez egy értéket meghalad, a program álljon le. Erre szükség van pl. elérhetetlen cél esetében. Egy futást látunk a 9. ábrán. A robotkar, a start pozíció és a cél ugyanaz, mint a 8. ábránál. A pszeudokód 9. sora szerinti skálázásra összesen 8-szor került sor a futás során. H = 0, 0001 hibahatárnál mindössze eggyel több iterációra volt szükség. Ha N > n (pl. síkbeli a robotkar, de kettőnél több szegmens van), akkor ún. másodlagos kritériumok beiktatásával lehet az állapottér dimenzióját növelni, azaz nemcsak a végberendezés helyzetét figyeljük, hanem a robotkar törésszögeire további feltételeket írunk elő. Pl. egy síkbeli RRR-robotkarnál (3 szegmens, 3 rotációs csukló) ilyen feltétel lehet, hogy Θ 1 Θ 2 = 0, tehát a két utolsó törésszög legyen egyenlő. Így f harmadik komponens függvénye f 3 = Θ 1 Θ 2, és a Jacobi-mátrix már 3 3 típusú. 6. Feladat. Tanulmányozzuk az elkészített wxmaxima munkalapon az invertálható Jacobi-mátrixú problémákat: 1. síkbeli RR robotkar 17

2. síkbeli RRR robotkar pótlólagos feltétellel 3. térbeli RRR robotkar. 3.2. Nem invertálható Jacobi-mátrixú probléma: mátrixok Moore Penrose pszeudo inverze Ha a (3) egyenletben df(θ) nem invertálható négyzetes mátrix, vagy nem is négyzetes mátrix, akkor az (5) egyenlet nem működik, mert az invertálást nem lehet elvégezni. A Moore Penrose pszeudo inverz módszer egy általános eljárás az F : R n R, x Ax b, A R m n, b R m (7) függvény minimumhelyének keresésére. Úgy is fogalmazhatunk, hogy a b vektort (a legkisebb négyzetek elve szerint) legjobban közelítő Ax vektort keressük. Ha az Ax = b lineáris egyenletrendszernek van megoldása, akkor a megoldás vektorra F (x) = 0, tehát ebben az esetben a módszer az egyenletrendszer egy megoldását adja. A (7) minimum probléma megoldását a módszer x = A b formában adja meg, ahol A az A mátrix ún. Moore Penrose inverze. Általánosított inverz mátrixon olyan mátrixot kell érteni, amely az inverz mátrix néhány (de nem feltétlenül mindegyik) tulajdonságával rendelkezik. Legyen A R m n, és tekintsük az alábbi mátrix egyenleteket az X R n m mátrixra: AXA = A (8) XAX = X (9) AX = (AX) t (10) XA = (XA) t. (11) Világos, hogy ha A invertálható, akkor X = A 1 mind a négy tulajdonsággal rendelkezik. Ha A jobbról (balról) invertálható, akkor a jobb inverzre (bal inverzre) (8) és (9) teljesül, de a másik két tulajdonság már nem feltétlenül. Ha X = A t, akkor (10) és (11) teljesül, de (8) és (9) már nem feltétlenül. 3. Definíció. Az A R m n mátrixnak az A R n m mátrix Moore Penrose pszeudo inverze, ha (8)-(11) mindegyikét teljesíti. 18

y b Im A p(b) A b x H Ker A 10. ábra. A Moore Penrose megoldás geometriai konstrukciója a 7. feladat adataival. 1. Tétel. Minden A R m n mátrixnak egyértelműen létezik Moore Penrose pszeudo inverze és x R n : Ax b 2 = Ax AA b 2 + AA b b 2. (12) Ha A invertálható, akkor a Moore Penrose pszeudo inverz megegyezik az inverz mátrixszal: A = A 1. b R m -re A b geometriai konstrukciója a következő (ld. a 10. ábrát). Vetítsük b-t A képterére merőlegesen, a merőleges vetület legyen p(b). Mindazon vektorok, melyre Ax = p(b) lineáris sokaságot alkotnak R n -ben, jelöljük ezt H-val. (Mivel p(b) az A mátrix képterében van, ezért az Ax = p(b) lineáris egyenletrendszer biztosan megoldható, a megoldás A magterének az eltoltja.) H legrövidebb eleme lesz A b. A (12) egyenlet nyilvánvaló következménye, hogy x R n : Ax b AA b b, azaz AA b a lehető legközelebb van b-hez A képteréből, másképpen AA b a legjobban közelíti b-t A képterében (a különbséget a szokásos euklideszi normával mérve). Ha az Ax = b lineáris egyenletrendszernek van megoldása, akkor A b az (euklideszi norma szerint) legrövidebb megoldásvektort adja. 19

7. Feladat. Legyen ( ) 1 2 A = 1 2, b = (1, 2). Határozzuk meg A b-t! Milyen hibával közelíti AA b a b vektort? Bizonyítás. A képtere az y = x egyenes, így az Ax = b lineáris egyenletrendszer nem megoldható, mert b nincs benne az A képterében. Először meghatározzuk b merőleges vetületét az y = x egyenesre. Ha a vetület p(b) = (s, s), akkor b p(b) = (1 s, 2 s), és ez a vektor merőleges az (1, 1) vektorra: 1 s + 2 s = 0 = s = 3 2, A merőleges vetület tehát p(b) = (3/2, 3/2). Második lépésként megoldjuk az Ax = p(b) lineáris egyenletrendszert. A második egyenlet megegyezik az elsővel, így elhagyjuk, tehát az x + 2y = 3 2 egyenletet kell megoldani. A megoldástér parametrizálva: hossza x = 2t + 3, y = t, t R. 2 Utoljára ki kell választanunk a legrövidebb megoldást. A megoldásvektor f(t) = ( 2t + 3 ) 2 + t 2 = 5t 2 6t + 9 2 4. f minimumhelyét a derivált zérushelye adja: Így A b = (3/10, 3/5). f (t) = 10t 6 = 0 = t = 3 5, A közelítés hibája (1 ) 2 AA b b = + 2 ( ) 2 1 0, 7. 2 A feladatot wxmaximával megoldva először be kell olvasnunk a linearalgebra csomagot, amely tartalmazza a Moore Penrose inverz számítását (ld. 3 kódrészlet). 20

Listing 3. wxmaxima kód részlete a 7. feladathoz load (" l i n e a r a l g e b r a " ) ; A: matrix ( [ 1, 2 ], [ 1, 2 ] ) ; moore_penrose_pseudoinverse (A) ; A Moore Penrose pszeudo inverz meghatározása általánosan hosszadalmas, vannak viszont egyszerűen kiszámítható esetek. Ha A t A invertálható mátrix, akkor egyszerű behelyettesítéssel látható, hogy x = (A t A) 1 A t b az Ax = b lineáris egyenletrendszernek megoldása és A = (A t A) 1 A t. 8. Feladat. Határozzuk meg a következő mátrix Moore-Penrose pszeudo inverzét! 1 2 A = 2 1 2 1 Megoldás. Mivel A két oszlopa nem arányos, így a mátrix rangja 2, ebből következően A t A invertálható 2 2 típusú mátrix, azaz az előző megjegyzés alapján lehet számítani a Moore Penrose pszeudo inverzet. Lépésenként: ( ) 9 6 A t A = 6 6 ( ) (A t A) 1 = 1 6 6 18 6 9 ( ) A = 1 2 2 2. 6 4 1 1 9. Feladat. Tanulmányozzuk az elkészített wxmaxima munkalapon kinematikailag határozatlan problémák megoldását a Moore Penrose pszeudo inverz módszerrel. Megoldás. Kinematikailag határozatlan robotkarokra a 4. algoritmusban df(θ) 1 helyett mindenütt df(θ) -ot kell venni. (Ld. 11. ábra!) 21

1.5 Target Start 1 0.5 y 0-0.5-1 -1.5 0 0.5 1 1.5 2 2.5 3 x 11. ábra. Inverz kinematikai feladat megoldása Moore Penrose pszeudo inverz módszerrel. Három szegmensből álló síkbeli RRR-robotkar, wxmaxima eredmény. δ = 0, 05, H = 0, 01. 22

3.3. A Jacobi-mátrix számítása a mozgó-tengely formulával Térjünk vissza az (1) egyenletre, az általános tengely körüli forgatás képletére. A képletben szereplő szögfüggvényeket közelítsük az első Taylorpolinomukkal, azaz helyettesítsünk a következőek szerint: sin α α, cos α 1. Azt kapjuk, hogy rot(α, k)x x + α k x. (13) A (13) képletet az infinitezimális forgatás képletének nevezzük, ez közelítését adja a forgatásnak. Most vegyük a robotkar egy Θ állapotvektorhoz tartozó helyzetét melyben a forgástengelyek irányvektora rendre (t 1,..., t N ), és mozdítsuk ki kicsit a robotkart, azaz változtassuk az állapotát meg a ( θ 1,..., θ N ) vektorral! J 1 rögzített, helyzete nem változik. J 2 helyzete az infinitézimális forgatás képletét használva Hasonlóan, J 2 J 2 + θ 1 t 1 (J 2 J 1 ) = J 2 θ 1 t 1 (J 2 J 1 ). J 3 θ 1 t 1 (J 3 J 1 ) + θ 2 t 2 (J 3 J 2 ). Tovább folytatva, a végberendezésre n E θ i t i (E J i ). i=1 Ez azt jelenti, hogy a Jacobi-mátrixra egy olyan közelítő formulát kaptunk, melyhez nincs szükség deriválásra: df(θ) (t 1 (E J 1 ), t 2 (E J 2 ),..., t N (E J N )). A Jacobi-mátrixnak ezt a kiszámítását nevezik mozgó-tengely formulának. 10. Feladat. Írjunk wxmaxima munkalapot az inverz kinematikai probléma megoldására, ahol a Jacobi-mátrixot a mozgótengely-formula alapján számoljuk. Megoldás. A célfüggvény kiszámítása során a 2. algoritmusban a forgástengelyek helyzetét nem őriztük meg, azonban a mozgótengely-formulához erre is szükség van, így a forgástengelyeket egy vektorban össze kell gyűjteni. Az 5. pszeudokód írja le a módosított algoritmust. 23

5. DrawChain3D Input: (θ 1,..., θ N ), (a 1,..., a N ), (α 1,..., α N 1 ), Output: (J 1,..., J N, E) 1: J 1 = (0, 0, 0), e 1 = (1, 0, 0), e 31 = (0, 0, 1) 2: e 1 rot(θ 1, e 31 )e 1 3: for i = 2 to N do 4: J i = J i 1 + a i 1 e 1 5: e 3,i rot(α i 1, e 1 )e 3,i 1 6: e 1 rot(θ i, e 3,i )e 1 7: E = J N + a N e 1 3.4. A minimum módszer az inverz kinematikai feladat megoldására Legyen a robotkar célfüggvénye f : R N R n, azaz a végberendezés helyzete az állapottér egy Θ R N vektorában E = f(θ). Jelölje D : R N R, D(Θ) = E(Θ) T 2 a végberendezés és a T R n cél távolságának négyzetét. A vektoranalízis nyelvén D egy skalármező. A végberendezés akkor és csakis akkor van a célban, ha D(Θ) = 0, azaz Θ a D függvény abszolút minimum helye. (D definíciójában azért nem a távolsággal számolunk, mert el akarjuk kerülni a gyökvonást.) A probléma tehát egy többváltozós függvény szélsőérték feladata. Egyszerű (de nem mindig célravezető) numerikus megoldás a legmeredekebb lejtő módszere. Ez a módszer szemléletesen a következő. Induljunk ki az egyszerűség kedvéért 2-dimenziós állapottérből, D grafikonja itt egy felület, azaz szemléletesen egy hegység (12. ábra). A legmélyebb völgy legalsó pontja a feladat megoldása. A hegység egy pontján állunk, a következő lépést arra tegyük, amerre a hegyoldal legjobban lejt. Mivel egy skalármező gradiens vektora a legnagyobb növekedés irányába mutat, a D skalármező gradiense jelöli ki a legmeredekebb lejtő irányát. A Θ k állapotból a Θ k+1 állapotba tehát a következőek szerint juthatunk: Θ k+1 = Θ k h grad D(Θ k ) = (14) ( ( ) ( ) ) D D = θ 1,k h,..., θ N,k h. θ 1 Θ k θ N Θ k 24

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 -2-1 0 1 2 3-3 u 12. ábra. Az iterációt addig kell folytatni, míg a kívánt pontossággal meg nem közelítettük a célt. Nem tárgyaljuk a h lépésköz megválasztásának és a konvergenciának a kérdését. 11. Feladat. Írjuk föl D-t egy síkbeli RR-robotkarra, ha T = (x 0, y 0 ), valamint határozzuk meg grad D-t! Bizonyítás. Az 5. feladat alapján, ha a szögeket x és y jelöli, D = (a cos x + b cos(x y) x 0 ) 2 + (a sin x + b sin(x + y) y 0 ) 2. Az x változó szerinti parciális derivált: D x = 2 (b cos (y + x) + a cos x) ( y 0 + b sin (y + x) + +a sin x) + + 2 (a cos (y x) x 0 + a cos x) (a sin (y x) a sin x). Hasonló módon határozzuk meg az y változó szerinti parciális deriváltat! 12. Feladat. Oldjuk meg egy RRR síkbeli robotkar inverz kinematikai feladatát a minimum módszerrel. 25

4. Összefoglalás A három dimenziós számítógépi animációknál népszerű az a megoldás, amikor a szintetikus színészeket strukturált csuklós szerkezetekként építik fel. A karakter animáció egy gyakori megoldása, hogy csontokból és ízületekből felépítjük a karakter csontvázát. Ehhez a csontvázhoz rendeljük hozzá a karakter háromszöghálóját. (A számítógépi animációban ez utóbbi folyamatot bőrözésnek is nevezik.) Az ízületek biztosítják a csontok elfordulását. Az ízület állapota különböző paraméterekkel, az ún. Denavit Hartenberg paraméterekkel adható meg. Ha minden ízülethez megadjuk a DH-paramétereket, akkor egy pózt kapunk. A paraméterek időfüggő megadása biztosítja a csontváz, és a bőrözésen keresztül a karakter animációját. Csak vezérlő pózokkal dolgozunk, a közbenső fázisokban a csontváz helyzetét interpolációs módszerekkel automatikusan generáljuk. Erre a módszerre az egyik első példa volt a YouTube-on is elérhető, 1985- ből származó Tony de Peltrie c. film (rendezte P. Lachapelle, P. Bergeron, P. Robidoux). Bergeron a film karakterét úgy jellemezte, hogy... énekel, beszél, mozog, mint egy igazi ember. Bár az arca karikatúra-szerű, a kifejezései természetesek, érzéseket és érzelmeket fejeznek ki. Ebben a jegyzetben a csontváz animációjának matematikai alapjaival foglalkozunk. Tárgyaljuk a csuklós szerkezetek reprezentációját, a robotkar kinematikai és inverz kinematikai feladatát. Kitérünk mindezek matematikai alapjára, így ismertetjük a Jacobi-mátrix számítását és iterálását, a mozgó tengely formulát, mátrixok Moore Penrose pszeudo inverzét és az ún. minimum módszert az inverz kinematikai feladat megoldására. Hivatkozások [1] H. Dym. Linear algebra in action, volume 78 of Graduate Studies in Mathematics, chapter 11. Pseudoinverses. American Mathematical Society, Providence, RI, 2007. [2] Z. Kovács. Számítógépi geometria. Kelet-Magyarországi Informatika Tananyag Tárház, 2011. Online elérhető: zeus.nyf.hu/ kovacsz. [3] A. Watt and M. Watt. Advanced Animation and Rendering Techniques, chapter 16. Animating articulated structures. Addison-Wesley, 1992. 26

Tartalomjegyzék 1. Bevezetés 1 2. Robotkarok reprezentációja, robotkarok direkt kinematikai feladata 3 3. Robotkar inverz kinematikai feladata 11 3.1. Invertálható Jacobi-mátrixú probléma.............. 14 3.2. Nem invertálható Jacobi-mátrixú probléma: mátrixok Moore Penrose pszeudo inverze..................... 18 3.3. A Jacobi-mátrix számítása a mozgó-tengely formulával.... 23 3.4. A minimum módszer az inverz kinematikai feladat megoldására 24 4. Összefoglalás 26 27