Autoři: Jan Krákora,, David Šebek, Quido Herzeq; ČVUT FELK Praha; Dne:

Transkript

1 NÁZEV EXPERIMENTU: NÁVRH, ŘÍZENÍ A PLÁNOVÁNÍ ROBOTU Autoři: Ja Krákora,, David Šeek, Quido Herzeq; ČVUT FELK Praha; De: 6.. Astrakt Optimálí řízeí rootu eí jedoduché, zvlášť pokud o pozici pracoví plochy rootu víme je z orazu z kamery. Pro řešeí prolému ylo třea použít trasformace pro převod z kamerových souřadic do souřadic kartézských, přímé a iversí kiematické úlohy pro popis vlastostí rootu a metod optimálího pláováí pro optimálí přechod z jedoho odu pracoví plochy do druhého. Výsledkem řešeí této úlohy yl simulátor reálého rootu a pláovací procedura jeho optimálího pohyu. Vše ylo řádě ověřeo úspěšými eperimety. Motivace Tato úloha je motivováa alezeím ejoptimálějšího řešeí apláovaé cesty v omezeém pracovím prostoru símaého perspektiví kamerou za pomoci iteligetího rootu. Zadáí Úkolem je apláovat ejoptimálější přesu hmotého odu iteligetím maipulátorem. Podroé zadaí úlohy je v []. Úvod V této úloze se zaýváme řešeím optimálího řízeí rootu símaého perspektiví kamerou. Úkolem je alézt ejvhodější model rootu a apláováí ejoptimálější cesty hmotého odu přesuovaého tímto rootem. Pro řešeí jedotlivých úloh jsme využili metod pro trasformace ze souřadic persp. kamery, metody pro řešeí přímé a iversí kiematické úlohy (dále PKU resp. IKU) a metodu optimálího pláováí A* algoritmu (dále je A*). Výsledkem je simulátor rootu a pláovací procedura pro optimálí cestu. V kapitole je popsá převod z kamerových souřadic do souřadic homogeích kartézských. V kapitole je popis rootu vzhledem k PKU resp. IKU a jeho simulace. V kapitole 3 je pláováí optimálí cesty pomocí A*. Kapitola a jsou dokázáy eperimetem.. Trasformace z perspektiví kamery Součástí řešeí úlohy je převod z orazu získaého z perspektiví kamery do souřadic pracoví plochy roota. Oraz símaý z kamery je a or.. a pracoví prostor rootu je patrý z or... Na pracoví ploše rootu jsou rozmístěy začky, které jsou ozačey číslicemi až 5 a písmey A,B. U začek až 5 záme jejich geometrické uspořádáí v souřadé soustavě roota. Začka A ozačuje počátečí polohu koce maipulátoru. Začka B ozačuje počátečí polohu

2 předmětu, který ude root přeášet do polohy A. Reálé souřadice odů A,B musíme změřit z orazu. Termiologie V této klapitole se používá zejméa tato termiologie Orazové souřadice - ide odu v oraze v souřadicích (u,v) (jedotka piel; viz or..). Reálé souřadice - ide odu v reálých souřadicích (,y,z) (délkové jedotky souřadé soustavy roota; viz or...) Metoda SVD - sigular value decompositio. SVD je rozklad matice A o rozměru m a souči U*D*V T, kde D je diagoálí matice složeá z vlastích čísel matice A, které jsou seřazey sestupě. V je matice vlastích vektorů odpovídajících vlastím číslům [5]. Or... Oraz z kamery Or... pracoví prostor rootu v (u,v, Měřeí v oraze Jedím z úkolů je výpočet trasformačí matice pro přepočítáváí orazu kamery do reálého světa. Dle [] vycházíme ze zámých hodot v reálém světě,y a zámých hodot v oraze kamery u,v. K získáí trasformačí matice A je zapotřeí zalosti souřadic 4 růzých odů. Dosazeím těchto odů do vztahu a ásledou elimiací ui i α i vi = H yi α i získáme homogeí soustavu rovic o 9-ti ezámých: A*h = Matice A osahuje měřeá a zámá data a vektor h osahuje ezámé proměé. Pro splěí uvedeé homogeí soustavy rovic je potřea ajít pravý ulový prostor matice A. Te určíme pomocí SVD rozkladu matice A. Vzhledem k ásoeí vektorem h matice A zprava, je pro ás ejdůležitější matice V.

3 Je potřea ajít takové h, ay v v T : T h : = h Zvoleím ejmešího vlastího čísla matice A za ulu, což lze udělat k získáí výsledku, který eude zatíže ikterak velikou chyou, dostaeme z předchozí rovice souči T v : T v h : = : h Vzhledem k lieárí ezávislosti vlastích vektorů, lze h položit rovo vlastímu vektoru příslušejícímu ejmešímu vlastímu číslu, tedy h = v. Potom souči v T *h je rove ule a je splěa ulovost součiu A*h. Tím jsme získali trasformačí matici, která je dáa vlastím vektorem ejmešího vlastího čísla matice A. Řešeí Do matice A jsme dosadili hodoty odů,,3,4 z orazových souřadic kamery (or... a ta...) Začka Orazové souřadice Reálé souřadice u V y z A 8 8?? B 669 6?? Ta... - Souřadice odů v orazových a v reálých souřadicích K řešeí SVD jsme použili fukci svd(), která je implemetováa v Matlau. [U,D,V]=svd(A); h= V(:,9);

4 Tím jsme získali koeficiety trasformačí matice H. Dále podle (.) spočteme reálé souřadice,y odů A a B. Výsledek je patrý z ta... y z A 3 4 B Ta... zaokrouhleý výsledek výpočtu reálých souřadic odů A a B Zjistili jsme že chya měřeí určíme z maima asolutích odchylek vypočteých reálých souřadic odů,,..,5 od jejich zadaých reálých souřadic. Maimum asolutí chyy hodoty změřeých reálých souřadic mělo hodotu 6.. Simulátor rootu Jelikož eí možé testovat řídící algoritmus optimálího pláováí a reálém rootu, ylo třea vytvořit simulačí program. Pro popis reálého rootu jsme využili teoretických pozatků z rootiky, zejméa části PKU resp. IKU. Při řešeí ylo možé zavést jisté zjedodušující podmíky v případě ehmotosti rame roota, takže eylo uté řešit detekci průchodu rame překážkami. Omezeí při řešeí ylo v olasti mechaické (omezeí akloěí rame), ale i matematické (chováí používaých trigoometrických fukcí ). Na závěr jsme provedli eperimet pro ověřeí správost teoretického řešeí této úlohy. Or... Ukázka reálého rootu Or... Struktura rootu Termiologie V kapitole Simulátor rootu je použita termiologie: Chapadlo mechaická část rootu uzpůsoeá k maipulaci s hmotými předměty Otevřeý kiematický řetězec soustava, kterou lze popsat acyklickým grafem Pracoví prostor možia všech odů, kam je možé astavit chapadlo roota

5 Popis reálého rootu Popis reálého rootu plye ze zadáí []. Pro představu jsou zde uvedey orázky.. a., kde a or.. je ukázka reálého rootu a or.. je jeho struktura. Přímá kiematická úloha Celý otevřeý kiematický řetězec rootu se dá modelovat Deavitovou-Hatemergovou i otací [3]. Výsledkem je soustava trasformačích matic A i ( qi ) o A ( q ) A ( q ) A ( q )... takových, že platí =. (.) Při zalosti koeficietů q (v ašem případě úhly) jsme schopi alézt liovolý od z klouových souřadic rootu v souřadicích kartézských. V případě ašeho rootu se =5. Přímá kiematická úloha roota je v eperimetu použita jako kotrola správosti iversí kiematické úlohy. Iverzí kiematická úloha V případě IKU rootu se jedá o zpětou trasformaci souřadic. Zde platí, že o ( q ) A ( q ) A ( q ) = A... (.) přičemž ezáme koeficiety q. Oecě se jedá o soustavu elieárích (ovykle trigoometrických) rovic, které jsou otížě řešitelé. Aalytické řešeí v ašem případě je sadé, eoť IKU je řešitelá pomocí Siovy a Kosiovy věty []. Or..3. Pohled XY a strukturu rootu Or..4. Pohled YZ a strukturu rootu Řešeí iverzí kiematické úlohy rootu Pro řešeí IKU předpokládejme, že máme od chapadla v odě = ( y z ) rootu, si lze ejlépe popsat a orázcích.3 a.4. Z or. Je patré, že pro úhel α platí že, = π + arctg = arctg α pro r y α pro r y eo (.3a) < a y yr eo (.3). Řešeí

6 = π + arctg α pro r y Z or. 3. Je vidět řešeí úhlu γ podle Kosiovy věty. Platí, γ = π l arccos( + l3 c l l 3 ) < a y < yr (.3c), kde c je vzdáleost odu a r. (.4) Vzhledem k ejedozačosti řešeí, platí pro úhel β, že β = σ + ω resp. β = σ ω kde = π d + arcsi d σ pro z zr eo σ = arcsi pro z < zr a (.5) c c l3 ω = arcsi[si( π γ ) ] (.6) c Z těchto rovic plye řešeí IKU rootu pro úhly α, β a γ. Mechaické omezeí úhlu rame rootu Taulka omezeí úhlů [] yla zakompoováa do simulátoru. Simulátor geeruje chyové hlášeí, v případě, že úhel áklou ramee je mimo uvedeý rozsah. Simulace detekce překážky proimitím čidlem Náraz a hmotého odu a překážku je deteková pomocí průiku přímky s pláštěm překážky. Přímka je dráha mezi současou polohou chapadla a jeho ásledující pozicí. Detekce je patrá z eperimetu pláováí. Eperimetálí ověřeí PKU a IKU Pro eperimetálí ověřeí správosti fukce pro PKU resp IKU jsme použili možiu dat, která simuluje přechod z odu ( ) po přímce k odu ( 3 3 ) = =.Výsledek simulace je patrý z or..5. a.6. je vidět, že průěh vývoje úhlů rame v závislosti a poloze odu odpovídá předpokládaému průěhu chodu reálého roota. z 5 z - 5 y y Or..5. Pohled YZ Or..6. Pohled XYZ

7 3. Pláováí Vzhledem k tomu, že se jedá o iformovaé prohledáváí stavového prostoru, lze použít k apláováí cesty ramee roota apř. A* [6]. Jedotlivé uzly, které se v algoritmu epadují představují vektory souřadic ramee roota. Pro zrychleí celého algoritmu, jsme pracoví prostor rozdělili a elemetárí jedotky, jejichž velikost je dáa miimálím krokem roota ásoeým zvoleou kostatou Bude-li kostata rova jedé, ude se root posuovat po jedom kroku. Zavedeím elemetárích jedotek se zmešuje stavový prostor. V A* algoritmu jsme volili kostatí ohodoceí hra a to jeda. Při hledáí optimálí cesty vycházíme pouze z výpočtu přímé vzdáleosti a vyíráme tu ejkratší. V okamžiku zvoleí kostaty větší ež jeda ( rozděleí prac. prostoru a elemetárí jedotky ), ylo uté zajistit testováí možé překážky ěhem přechodu z jedoho stavu do druhého. Toto přecházeí a ejižší úrovi je založeo opět a A* algoritmu ovšem s jedotkovým krokem, kdy je preferováa pouze přímá cesta. Eperimetálí ověřeí: Vzhledem k edokočeému algoritmu pro pláováí, elze zatím teoretické podklady eperimetálě prokázat. ZÁVĚR Závěrem lze kostatovat, že teoretické pozatky se podařilo úspěšě aplikovat do prae. Výsledkem aší práce je simulátor rootu, který svou IKU věrě popisuje IKU reálého rootu. Do udouca y ylo možé apř. realizovat simulátor, který y euvažoval ehmotost jedotlivých rame. Referece [] T.Pajdla, Iteligetí rootika, materiály k předášce a cvičeí; [] Pultarová, Pultar; Základí matematické vzorce; vydavatelství ČVUT, Praha 998 [3] T.Smutý, Rootika, materiály k předáškám, ČVUT, Praha [4] R.Šára, Oecá teorie systémů, materiály k předáškám, ČVUT, Praha [5] E.Krajík, Maticový počet (str 3), vydavatelství ČVUT, Praha, 998 [6] V.Mařík, O.Štěpáková, J.Lažaský, Umělá Iteligece, Academica, Praha 993