Využití senzorů pro lokalizaci robota v prostoru

Mendelova univerzita v Brně Provozně ekonomická fakulta

Využití senzorů pro lokalizaci robota v prostoru Bakalářská práce

Vedoucí práce: Dr. Ing. Radovan Kukla

Marcel Vytečka

Brno 2011

Rád bych poděkoval vedoucímu této bakalářské práce, panu Dr. Ing. Radovanu Kuklovi, za jeho hodnotné rady a čas, který mi věnoval. Dále bych chtěl poděkovat Bc. Janu Kolomazníkovi, za připomínky a rady, související s problematikou této práce.

Prohlašuji, že jsem tuto bakalářskou práci vypracoval samostatně a v seznamu literatury uvedl veškeré informační zdroje, které jsem použil pro její tvorbu. V Brně dne 23. května 2011

_________________

Abstract Vytečka, M. Utilization of sensors for localization of a mobile robot in space. Bachelor thesis. Brno, 2011. This bachelor thesis deals with the issue of localization of mobile robot in space. Individual methods of location detection are described in the theoretical part. In the same part there are also described the NXT system and sensors used for intelligible elaboration of this thesis. In practical part reliability and suitability of the available sensors are evaluated as a description of the mobile robot construction and designed program. This program serves as a navigation system implemented in mobile robot which participated in international competition Eurobot 2011. Keywords Mobile robot, localization, NXT, Mindstorms.

Abstrakt Vytečka, M. Využití senzorů pro lokalizaci robota v prostoru. Bakalářská práce. Brno, 2011. Bakalářská práce se zabývá problematikou určení polohy mobilního robotu v prostoru. V teoretické části jsou uvedeny jednotlivé způsoby zjišťování polohy. Dále je v této části popsán systém Lego Mindstorms a senzory, využité pro vypracování této práce. V praktické části je zhodnocena vhodnost a spolehlivost dostupných senzorů, jakožto i popis konstrukce robotu a navrženého programu. Navrhovaný program slouží jako systém navigace implementovaný v mobilním robotu, který se účastnil mezinárodní soutěže Eurobot 2011. Klíčová slova Mobilní robot, lokalizace, NXT, Mindstorms.

Obsah

5

Obsah Obsah

5

1

7

2

Úvod a cíl práce 1.1

Úvod ........................................................................................................... 7

1.2

Cíl práce ..................................................................................................... 7

Teoretická část 2.1

8

Stavebnice LEGO Mindstorms ................................................................. 8

2.1.1

Programovatelná kostka ....................................................................9

2.1.2

Senzory............................................................................................. 10

2.1.3

Servomotory..................................................................................... 16

2.2

Možnosti programování .......................................................................... 17

2.2.1

programovací jazyk NXT-G ............................................................. 17

2.2.2

programovací jazyk NXC ................................................................. 17

2.2.3

leJOS NXJ ........................................................................................ 18

2.3

Navigace ................................................................................................... 18

2.3.1

Lokální systém ................................................................................. 18

2.3.2

Globální systém ............................................................................... 19

2.4

Soutěž Eurobot ....................................................................................... 20

2.4.1

Hrací plocha ..................................................................................... 21

3

Metodika

23

4

Vlastní práce

24

4.1

Konstrukce ...............................................................................................24

4.2

Program ................................................................................................... 27

4.2.1

Systémy pro zjištění polohy ............................................................ 28

4.2.2

Lokální navigace ............................................................................. 38

4.3

Navigace robotu .......................................................................................39

5

Diskuze

40

6

Závěr

41

7

Literatura

42

Obsah

6

A

Seznam obrázků

44

B

Seznam tabulek

46

C

Program pro navigaci robotu

47

D

Obrázky finální konstrukce robotu

62

Úvod a cíl práce

7

1 Úvod a cíl práce 1.1

Úvod

Pokud chceme od mobilního robotu, aby vykonával úkony v prostoru, základní úlohou k řešení je právě lokalizace. Je důležité, aby robot dokázal, s určitou přesností, vypočítat svou polohu, vzhledem k okolnímu prostředí. Je třeba rozlišit, zda se jedná o předem známé prostředí s dobře zmapovanými okolními prvky, nebo neznámé prostředí. Prostředím robotu může být hrací plocha, místnost nebo venkovní prostory. K této problematice, můžeme přistoupit více způsoby. Pokud se jedná o předem známé prostředí jako je místnost nebo například hrací plocha, programové vybavení nemusí být příliš složité. Nároky na senzory jsou v tomto případě menší. Takovéto prostředí je obvykle ohraničeno předem známou hranicí, jakou může být stěna bludiště, černá linka na podloží nebo infračervený majáček. Je-li prostředí neznámé, a víme-li, pouze přibližně s jakými překážkami se může robot setkat, je volba senzorů a architektury programu mnohem složitější. Při výběru způsobu navigace robotu musíme také uvažovat výpočetní výkonnost řídicí jednotky. Programovatelná kostka NXT, která je součástí stavebnice Lego Mindstorms, zvládne přibližně sto tisíc operací za sekundu. Díky Širokému spektru dostupných senzorů je možné systém NXT použít ve všech zmiňovaných prostředích.

1.2 Cíl práce Cílem této práce je navrhnout a realizovat systém lokalizace mobilního robotu. Vytvořený program použít jako systém navigace, v mobilním robotu při reprezentaci MENDELU v mezinárodní soutěži Eurobot. Určit vhodnost a spolehlivost jednotlivých senzorů a pohonných jednotek. Stručně popsat konstrukci robotu, která se odvíjí od výběru vhodných senzorů.

Teoretická část

8

2 Teoretická část 2.1 Stavebnice LEGO Mindstorms Robotický systém LEGO Mindstorms NXT 2.0 představuje jednoduchý prostředek k seznámení s odvětvím robotiky. Systém NXT je stavebnice plně kompatibilní se stavebnicemi LEGO system a LEGO technic. Základním prvkem stavebnice je programovatelný mikropočítač. Jako vstupní zařízení jsou v základním balení k dispozici čtyři základní druhy senzorů, které budou popsány dále v textu. Dále existuje možnost pořízení pokročilých senzorů od firem, zabývajících se výrobou produktů speciálně pro systém Mindstorms. Použité senzory od externích firem budou také dále popsány. Výstupní zařízení jsou servomotory se zabudovaným rotačním snímačem. Díky tomu, že celý systém je stavebnice, lze jednoduše sestavit modely, potřebné pro různé aplikace, bez nutnosti pracovat s jakýmkoliv nářadím. LEGO NXT je vhodné pro začátečníky v oblasti robotiky a pro výukové účely. K dispozici je i software pro převedení sestavených modelů do počítačové podoby s možností generování návodů k sestavení. Pokud vyžadujeme větší pevnost modelu, je možné použít stavebnici TETRIX. TETRIX je kovová stavebnice, která je svými rozměry kompatibilní s Lego technic.

Obr. 1

Stavebnice Lego Mindstorms

Zdroj: LEGO Mindstorms [online]. 2000 [cit. 2011-04-30]. NXT log. Dostupné z WWW: .

Teoretická část

2.1.1

9

Programovatelná kostka

Programovatelná kostka je základním prvkem celé stavebnice. Obsahuje v sobě 32 bitový mikroprocesor a 8 bitový koprocesor. K ovládání slouží čtyři tlačítka. Na přední straně je umístěn LCD display. Pro připojení k PC je možné využít USB konektor nebo bezdrátové rozhraní Bluetooth1. Díky Bluetooth mohou kostky mezi sebou komunikovat a je možné vytvořit složité aplikace, vyžadující více senzorů, motorů nebo více výpočetního výkonu. Jako zdroj energie může sloužit akumulátor dodávaný ke stavebnici, nebo šest tužkových baterií. Je vybavena čtyřmi vstupními porty a třemi výstupními. Ke vstupním portům se připojují senzory, díky kterým můžeme zjistit informace o okolí. K výstupním portům se připojují servomotory nebo osvětlující žárovky. Kostku je možné připevnit ke stavebnici pomocí spojek.

Obr. 2



1

LEGO Mindstorms [online]. 2000 [cit. 2011-04-30]. .

NXT

log.

Dostupné

z

WWW:

Teoretická část

2.1.2

10

Senzory

2.1.2.1 základní senzory V základní sestavě jsou čtyři druhy senzorů. Ultrazvukový senzor, dotykový senzor, barevný (NXT 2.0) a zvukový senzor.

2.1.2.1.1 Ultrazvukový senzor Ultrazvukový senzor pracuje na principu odrazu ultrazvukového signálu, obsahuje vysílač i přijímač. Podle času, za který se signál navrátí od překážky, se vypočítá vzdálenost předmětu od senzoru. Senzor měří až do vzdálenosti 255 cm. Přesnost záleží na okolním prostředí, pohybuje se mezi 1 - 3 cm2. Senzor můžeme použít v modu soustavného měření (continuous), nebo jednotlivá měření (ping). Mezi jednotlivými měřeními je třeba vyčkat přibližně 20 ms. Měření vzdálenosti je možné v centimetrech nebo v palcích. Pokud pracujeme s ultrazvukovým senzorem, musíme mít na paměti, že může docházet k rušení při použití více ultrazvukových senzorů a také pokud má překážka nepříznivé vlastnosti pro odraz signálu.

Obr. 3

Ultrazvukový senzor


2.1.2.1.2 Dotykový senzor 2

LEGO MINDSTORMS [ONLINE]. 2000 [CIT . 2011-04-30]. NXT LOG. DOSTUPNÉ Z WWW: .

Teoretická část

11

Dotykový senzor systému NXT udává informaci o jeho stlačení. Je pouze dvoupolohový tzn. stlačený nebo nestlačený, žádné mezi polohy ani úroveň stlačení nelze zjistit. Získáme z něj tedy informaci true nebo false3. Nejčastěji se používá, chceme-li zjistit fyzický kontakt s jiným předmětem. Sílu stlačení potřebnou k aktivaci, je možné upravit připojením konstrukce z Lega.

Obr. 4

Dotykový senzor


2.1.2.1.3 Zvukový senzor Zvukový senzor snímá hlasitost zvuku. Rozsah frekvencí je od 3 do 6 kHz, kdy je lidské ucho nejvíce sensitivní. Úroveň hlasitosti je udávána v dB. Rozsah úrovní zvuku je přibližně 50 - 90 dB4.

3

JAVA FOR LEGO M INDSTORMS [ONLINE]. 1997 [CIT . 2011-04-30]. LEJOS. DOSTUPNÉ Z WWW: . 4

LEGO MINDSTORMS [ONLINE]. 2000 [CIT . 2011-04-30]. NXT LOG. DOSTUPNÉ Z WWW: .

Teoretická část

Obr. 5

12

Zvukový senzor


2.1.2.1.4 Světelný senzor Světelný senzor měří intenzitu světla. Díky zabudovanému osvětlení lze měřit i úroveň odrazu od jednotlivých povrchů. Osvětlení lze softwarově ovládat. Světelného senzoru lze využít jak pro zjištění úrovně osvětlení v okolí, tak například pro jízdu po čáře. Přesnost senzoru je dostačující na přečtení např. čárového kódu apod.

Obr. 6

Světelný senzor


Teoretická část

13

2.1.2.1.5 Barevný senzor Barevný senzor snímá úrovně jednotlivých barevných složek RGB. Každá složka je v rozmezí 0 - 2555. Lze díky němu zjistit barvu předmětu nebo podkladu. Vyžaduje dobré osvětlení snímaného povrchu.

Obr. 7

Barevný senzor


2.1.2.2 Externí senzory Další pokročilé senzory pro systém NXT lze zakoupit od společností Mindsensors a Hitechnic. Senzory použité pro tuto práci představím detailněji.

2.1.2.2.1 Infračervený senzor Infračervený senzor snímá infračervené záření. Pracuje ve dvou režimech. V modulovaném a nemodulovaném. Nemodulovaný signál je například sluneční záření či záření z osvětlení. V případě modulovaného signálu, který je na frekvenci 1200 Hz, dochází k odfiltrování ostatních zdrojů záření a zvýšení přesnosti a spolehlivosti senzoru6.

5

JAVA FOR LEGO M INDSTORMS [ONLINE]. 1997 [CIT . 2011-04-30]. LEJOS. DOSTUPNÉ Z WWW: . 6

HITECHNIC [ONLINE]. 2001 [CIT. 2011-04-30]. ROBOTIC SENSORS. DOSTUPNÉ Z WWW: .

Teoretická část

Obr. 8

14

Infračervený senzor a zobrazení směrů příjmu signálu

Zdroj: Hitechnic [online]. 2001 [cit. 2011-04-30]. Robotic sensors. Dostupné z WWW: .

2.1.2.2.2 Infračervený detektor překážek (NXT sumo eyes) Tento senzor měří překážky ve dvou vzdálenostech a třech směrech. Velká vzdálenost a krátká vzdálenost. Směr střed, vpravo a vlevo. Vyznačuje se vysokou spolehlivostí zjištění překážky. Senzor pracuje na optickém triangulačním principu pro měření vzdálenosti. Využívá infračerveného světla7. Jednotlivé vzdálenosti jsou rovné 15 cm (krátká vzdálenost) a 30 cm (dlouhá vzdálenost) Viz obr. č. 9.

Obr. 9

Infračervený detektor překážek a znázornění detekovaných zón

Zdroj: Mindsensors.com [online]. 2005 [cit. 2011-04-30]. Advanced sensors. Dostupné z WWW: .

7

MINDSENSORS.COM [ONLINE]. 2005 [CIT. 2011-04-30]. ADVANCED SENSORS. DOSTUPNÉ Z WWW: .

Teoretická část

15

2.1.2.2.3 Kamera NXTCam V2 je senzor, který zpracovává obraz v reálném čase. Obsahuje vlastní procesor a rozhraní pro připojení k senzorickému portu NXT kostky. Rozhraní nám poskytuje již zpracované informace z obrazu. Kamera neposílá do NXT kostky obraz ale pouze informace o sledovaných předmětech. Obsahuje také USB port pro připojení k PC a nastavení parametrů snímání. Kamera je schopná sledovat až osm objektů rychlostí třicet snímků za sekundu8. Udává informace o počtu objektů, jejich barvě a souřadnice jednotlivých objektů. Rozlišení kamery je 176 x 144 pixelů.

Obr. 10

Kamera NXTCam V2


2.1.2.2.4 NXT Angle Sensor Tento senzor měří natočení osy a rychlost otáčení. Je schopen pracovat ve třech režimech: • Absolute Angle - měří natočení osy ve stupních od 0 do 360 stupňů s přesností na 1 stupeň. • Accumulated angle - zobrazuje součet stupňů, o který se osa natočila. • Rotation Speed - zobrazuje rychlost osy v otáčkách za minutu.9

8

MINDSENSORS.COM [ONLINE]. 2005 [CIT. 2011-04-30]. ADVANCED SENSORS. DOSTUPNÉ Z WWW: . 9 H ITECHNIC [ONLINE ]. 2001 [CIT. 2011-04-30]. ROBOTIC SENSORS . DOSTUPNÉ Z WWW: .

Teoretická část

Obr. 11

16

Úhlový senzor


2.1.3

Servomotory

2.1.3.1 Servomotory NXT Základní pohonnou jednotkou pro systém NXT jsou interaktivní servomotory. Tyto jednotky mají v sobě rotační snímač, díky kterému je možné měřit rychlost a vzdálenost, kterou motor urazil. Obsahuje v sobě převody10. K NXT kostce je možné připojit až tři tyto motory (A B C). Pro lepší pohyb robota je možné rychlost motorů synchronizovat. Je několik způsobů, jak motory ovládat. Můžeme nastavit rychlost a spustit motor do chodu vpřed nebo vzad. Můžeme také dát příkaz rotovat o určitý úhel s přesností na jeden stupeň. Znaménko u úhlu určuje směr otáčení. Dále je možné zabrzdit motor v určité pozici nebo ho nechat volně dotočit.

Obr. 12

Servomotor dodávaný ke stavebnici NXT Mindstorms


LEGO MINDSTORMS [ONLINE]. 2000 [CIT. 2011-04-30]. NXT LOG. DOSTUPNÉ Z WWW: . 10

Teoretická část

17

2.1.3.2 Modelářské servomotory Vedle základních NXT interaktivních motorů se dají jako manipulátory použít modelářské servomotory. Tyto motory mají vetší přesnost a větší sílu. Společnost Mindsensors distribuuje tyto modelářské servomotory, upravené pro použití v systému NXT, společné s osmi kanálovým kontrolérem. Kontrolér se připojuje k NXT kostce pomocí stejných kabelů jako senzory a podporuje připojení až osmi servomotorů. Vyžaduje externí napájení, jehož velikost je závislá na počtu připojených motorů.

Obr. 13

Modelářský servomotor spolu s multiplexorem pro NXT


2.2 Možnosti programování 2.2.1

programovací jazyk NXT-G

Je to základní dodávaný software ke stavebnici, má grafické prostředí založené na softwaru LabView. Programuje se pomocí přetahování funkčních bloků. Je možné vytvořit podmínky a smyčky. Je velice intuitivní, programování je velice jednoduché. Nicméně, pro pokročilé úlohy není tento software vhodný. 2.2.2

programovací jazyk NXC

Další možnost programování nám poskytuje NXC. Not Exactly C je jazyk podobný jazyku C, postavený na kompilátoru NBC. Díky němu je možné programovat NXT kostku. Pro využití tohoto software není nutné měnit firmware v programovatelné kostce.

Teoretická část

2.2.3

18

leJOS NXJ

Tento software umožňuje programování kostky v jazyku Java. Pro jeho použití je nutné změnit firmware v programovatelné kostce. Nový firmware vytvoří malou Java virtual machine uvnitř kostky. Podporuje mnoho standardních funkcí jazyka Java. K dispozici je rozsáhlá dokumentace všech tříd, které LeJOS obsahuje.

2.3 Navigace Aby mobilní robot mohl plnit funkci, pro kterou byl vytvořen, musí se umět pohybovat v daném prostředí. Pro každý druh robotu může být okolní prostředí jiné. Například, pro mobilní robot, jež, byl vyvinut pro podporu vojáků v boji, je okolní krajina, se všemi jejími prvky, jeho prostředím. U tohoto typu robotu musí být programové vybavení velice pokročilé, protože se může setkat s mnoha nečekanými překážkami. Oproti tomu mobilní robot, sloužící k přepravě zboží ve skladu, má poměrně jasně definovány všechny překážky, se kterými se může setkat. Cesty, po kterých se může pohybovat, bývají označeny na podlaze kontrastní linkou. V hale jsou umístěny aktivní majáky, které ohraničují pracovní prostor apod. Dalším typem prostředí může být hrací plocha s přesně definovanými rozměry a překážkami. Zatímco vojenský robot může pro svou navigaci použít například signál GPS, protože jeho přesnost je pro tak rozlehlý pracovní prostor dostačující, mobilní robot pohybující se na hrací ploše, o velikosti jednotek metrů čtverečních, by tento způsob prakticky nemohl použít. Existují samozřejmě způsoby, jak přesnost signálu GPS zvýšit na jednotky cm, což by ale v tomto případě nebylo moc efektivní. Tato práce se především zabývá posledním zmíněným prostředím a to hrací plochou. Velikost hrací plochy, jakožto i překážky a herní prvky, vycházejí z pravidel soutěže Eurobot. Barevnost podkladů, překážek a umístění lokalizačních majáčků je dáno přesnými údaji. Zpravidla se systém navigace robotu dělí na část globální a na část lokální. Globální má za úkol určit polohu a orientaci mobilního robotu v souřadném systému. Lokální snímá aktuální informace ze senzorů robotu a zabraňuje kolizím s překážkami.11 2.3.1

Lokální systém

Lokální systém navigace je nadřazen globálnímu. Hlavním úkolem lokální navigace je přebírat informace ze senzorů a zabránit tak kolizím s překážkami. Zpracovává informace v omezeném prostoru od robotu, avšak dosah senzorů musí být uzpůsoben

11

NOVÁK , P. MOBILNÍ ROBOTY. PRAHA: BEN, 2007. 248 S., ISBN 80-7300-141-1.

Teoretická část

19

tak, aby mohl robot včas zareagovat na překážku.12 V této práci lokální systém také řeší, zda objekt, který se nachází v dosahu senzorů, je herní prvek tzn. manipulovatelný prvek, či mobilní robot patřící protihráči. 2.3.2

Globální systém

Určuje polohu robotu a jeho orientaci vzhledem k souřadnému systému. Zjištěnou polohu předá řídicímu systému, který poté zvolí vhodnou trajektorii. Globální systém se nejčastěji dělí do dvou kategorií. Relativní a absolutní navigace.13 2.3.2.1 Relativní navigace Tato metoda navigace sčítá ujeté vzdálenosti vzhledem k známému bodu. Tímto bodem může být například startovní pozice robotu, nebo místo, kde byla naposled určena poloha absolutní navigací. Tento typ navigace je zatížen rostoucí chybou, která je způsobena chybami v jednotlivých přírůstcích. Rostoucí chybu je možné zmírnit kombinací s absolutní navigací. Bez kombinace s absolutní navigací je metoda vhodná pouze pro krátké vzdálenosti. Při spojení těchto dvou způsobů je určení polohy poměrně přesné, protože se eliminují nahodilé chyby v jednotlivých systémech. 14

2.3.2.1.1 Odometrie Relativní navigace je v této práci řešena odometrií. Jedná se o způsob měření, kdy jsou přírůstky ujeté vzdálenosti dopočítávány podle pohybů motorů. Program má zadány potřebné rozměry modelu robotu a dopočítává vzdálenost. Nejdůležitější zadávané parametry jsou šířka podvozku a obvod nebo poloměr kol. Jako většina relativních způsobů určení polohy je i odometrie zatížena rostoucí chybou. Chybu je možné zmírnit tím, že zajistíme stálý kontakt měřených kol s podložím a tím minimalizujeme možnost prokluzu jednotlivých pohonných kol.15

2.3.2.1.2 Inertní navigace Jedná se o relativní navigaci, která pracuje na principu dopočítávání polohy podle údajů z akcelerometru nebo gyroskopu. Tento způsob je ale poměrně nepřesný v malých rychlostech a je často ovlivňován vibracemi a nárazy na překážky nebo nerovnosti na podloží. Tato metoda není v této práci prakticky využita.

NOVÁK , P. MOBILNÍ ROBOTY. PRAHA: BEN, 2007. 248 S., ISBN 80-7300-141-1. NOVÁK , P. MOBILNÍ ROBOTY. PRAHA: BEN, 2007. 248 S., ISBN 80-7300-141-1. 14 N OVÁK , P. M OBILNÍ R OBOTY . P RAHA : BEN, 2007. 248 S ., ISBN 80-7300-141-1. 15 N OVÁK , P. M OBILNÍ R OBOTY . P RAHA : BEN, 2007. 248 S ., ISBN 80-7300-141-1. 12 13

Teoretická část

20

2.3.2.2 Absolutní navigace Pro určení absolutní polohy robotu je třeba použít referenční body se známými souřadnicemi. Referenční body mohou být tvořeny přijímači či vysílači. V prvním případě je na robotu umístěn vysílač, který přijímače sledují, vyhodnocují a zasílají polohu řídicímu systému. V druhém případě robot přijímá signál a sám si polohu dopočítává.16 Referenční body mohou být také pasivní, nemusí nutně vysílat ani přijímat. Stačí, že budou mít určitou vlastnost, díky které je může robot identifikovat. Například barvu a tvar či na nich může být nějaký obrazec. Takovéto předměty je možné snímat kamerou.

2.3.2.2.1 Trilaterace O trilateraci hovoříme, pokud měříme vzdálenost mobilního robotu od referenčních bodů. Po zjištění vzdálenosti ke všem referenčním bodům je robot schopen dopočítat své souřadnice, avšak natočení robotu se touto metodou zjistit nedá.17

2.3.2.2.2 Triangulace Pokud měříme tři úhly, pod kterými vidíme jednotlivé body, jedná se o triangulaci. Dopočítání souřadnic metodou triangulace je možné pouze uvnitř pomyslného trojúhelníku, tvořeným spojnicemi mezi referenčními body. Vně tohoto trojúhelníku jsou místa, kde výpočet nelze provést. 18 V této práci je absolutní navigace řešena kombinací těchto dvou metod, z důvodu minimalizace jejich nevýhod.

2.4 Soutěž Eurobot Soutěž Eurobot byla vytvořena v roce 1998. Jedná se o mezinárodní amatérskou robotickou soutěž. Je určená pro týmy mladých lidí, organizovaných v nezávislých klubech nebo ve studentských projektech. Eurobot se koná v Evropě, ale jsou vítány týmy i z jiných kontinentů.

NOVÁK , P. MOBILNÍ ROBOTY. PRAHA: BEN, 2007. 248 S., ISBN 80-7300-141-1. BORENSTEIN, J.; EVERETT, H. R.; FENG, L. WHERE AM I? [ONLINE]. MICHIGAN: UNIVERSITY OF MICHIGAN, 1996 [CIT . 2011-04-30]. DOSTUPNÉ Z WWW: . 18 BORENSTEIN, J.; EVERETT, H. R.; FENG, L. W HERE AM I? [ONLINE ]. M ICHIGAN: U NIVERSITY OF MICHIGAN, 1996 [CIT . 2011-04-30]. DOSTUPNÉ Z WWW: . 16

17

Teoretická část

21

Soutěž se pořádá každý rok. V zemi, kde se přihlásí k účasti více než tři týmy, se koná národní kolo soutěže. Týmy umístěné na prvních třech místech postupují do mezinárodního kola. Národní kolo v České republice pořádají Katedra softwarového inženýrství MFF UK a občanské sdružení Robonika.19

2.4.1

Hrací plocha

Tato práce se zabývá lokalizací mobilního robotu v prostoru, proto uvedu základní informace o hrací ploše, na které se bude robot pohybovat. Veškeré parametry hřiště vycházejí z platných pravidel soutěže Eurobot 2011.

Obr. 14

Obrázek hřiště pro Eurobot 2011

Zdroj: Eurobot [online]. .

1994

[cit.

2011-04-30].

Robotický

den.

Dostupné

z

WWW:

Základní složkou hrací plochy jsou červené a modré čtverce, každý se stranou 35 cm. Čtyři tyto čtverce jsou chráněné a jsou částečně ohrazeny. Tým, který umístí na svou barvu více herních prvků, získá více bodů. Černé kruhy označují bonusová pole. Zelené plochy na stranách hřiště jsou zdrojové oblasti, obsahující herní prvky. Startovací čtverce o straně 40 cm jsou výchozí pozicí robotu. Každý tým má k dispozici tři stoja19

EUROBOT [ONLINE]. 1998 [ CIT. 2011-04-30]. I NTERNATIONAL CONFERENCE EUROBOT. DOSTUPNÉ Z WWW: .

Teoretická část

22

ny na majáky po stranách hřiště. Vzdálenosti mezi stojany a jejich výška jsou jednoznačně dány.20

20

EUROBOT [ONLINE]. 1994 [ CIT. 2011-04-30]. ROBOTICKÝ DEN. DOSTUPNÉ Z WWW: .

Metodika

23

3 Metodika K experimentování s navrženým řešením programu pro lokalizaci robotu, je nutné mít k dispozici zkonstruovaného robota s příslušnými senzory a rozměry, jakožto i prostředí, ve kterém se bude pohybovat. Jednotlivé senzory je možné testovat za použití programovatelné kostky připojené k PC. Jako prostředí pro pohyb robotu bylo využito modelu hrací plochy Eurobot, upravené podle pravidel ročníku 2011. Celý program je realizován v jazyce Java s použitím nástroje LeJOS a prostředí Eclipse. LeJOS obsahuje mnoho předdefinovaných tříd pro práci se systémem Mindstorms. Obsahuje třídy pro všechny základní senzory a v části addons také třídy pro ty pokročilé. Jeho součástí jsou i třídy, které obsahují základní systémy navigace a mapování prostoru. Výhodou použití tohoto nástroje je, že nemusíme každou součást programu vytvářet, stačí použít již hotovou součást. Třídy je možné si upravit podle konkrétní aplikace.

Vlastní práce

24

4 Vlastní práce 4.1 Konstrukce Podvozek je sestaven z dvou diferenčně řízených kol a jedním opěrným. Tato konstrukce se pro pohyb po hrací ploše jevila jako nejlepší. Největší výhodou tohoto typu podvozku je nesporně možnost otáčení prakticky kolem své osy. Tato vlastnost je pro nás důležitá také z důvodu homologace robotu v soutěži Eurobot. Pokud robot při otáčení nezasahuje do okolí a nemusí při něm couvat, logicky nemusí detekovat překážky za sebou ani po stranách. Je tedy menší pravděpodobnost kolize s okolními předměty. Označení d na obr. č. 15 je průměr kola, D rozteč kol.

Obr. 15

Podvozek s dvěma diferenčně řízenými koly a jedním opěrným, někdy nazývaný tribot

Zdroj: Vlastní nákres.

Zadní opěrné kolečko bývá často řešeno tak, že nad bodem jeho upevnění je příčka, na jejímž konci je upevněno kolo. Pohybem do strany je toto kolečko natočeno správným směrem. K jeho otočení je potřeba energie, která může zhoršit přesnost otáčení. V našem případě je jako zadní opěrný bod použito kuličkového kolečka od společnosti Mindsensors. Takto řešené zadní kolečko snižuje velikost energie, která je potřebná pro natočení zadního kolečka při otáčení, tudíž přispívá k větší přesnosti a spolehlivosti při otáčení.

Vlastní práce

Obr. 16

25

Kuličkové kolečko od společnosti Mindsensors


Dalším konstrukčním prvkem, který je umístěn na robotu, je úhlový senzor. Jeho použití je nezbytné pro zpřesnění údajů o ujeté vzdálenosti. Při pohybu robotu v prostoru se může stát, že robot narazí na překážku, a to i přesto, že si okolí snímá svými senzory. V případě kolize s pevnou překážkou je možné, že dojde k prokluzu diferenčně řízených kol. Pokud v tomto případě odečteme hodnotu ze senzoru ujeté vzdálenosti, dostaneme samozřejmě špatná data. Rotační senzor zabudovaný v pohonných motorech nám pouze udá počet otáček, o které se daný motor otočí. Zda přitom opravdu ujel vypočítanou vzdálenost, nevíme. Do konstrukce je proto přidán úhlový senzor. Jeho otáčení je nezávislé na příkazech, které jdou z programovatelné kostky. Pružinou je tlačen na podloží, proto se otáčí pouze při pohybu obou kol vpřed. Přidáním úhlovému senzoru k tomuto typu podvozku velmi zmenšíme pravděpodobnost špatného vypočtení údaje o ujeté vzdálenosti.

Vlastní práce

Obr. 17

26

Pohled na spodní část robotu s připevněným barevným senzorem

Zdroj: Foto: Ing. Jaromír Landa.

V zadní části robotu je dále umístěn barevný senzor za účelem snímání barvy podloží. Při zjišťování polohy hraje barva podloží velikou roli. Hrací plocha je navržená tak, aby lokalizaci usnadňovala. Barevným senzorem především zjistíme startovní pozici a poté jednotlivé barevné čtverce. Také slouží jako mezník pohybu při posunování herních prvků. V potaz přicházel také světelný senzor, který umí odečíst intenzitu odraženého světla a tedy i informaci o barvě. Jeho výhodou je, že disponuje vlastním zdrojem světla oproti barevnému senzoru, který jím vybaven není a je nutné dodat externí osvětlení. Další výhoda nevychází z funkčnosti senzoru, ale z jeho dostupnosti. Světelných senzorů, které jsou v každém balení Lego Mindstorms, bylo k dispozici více, zatímco barevný senzor jen v počtu jednoho kusu. Ukázalo se ale velkou nevýhodou, že světelný senzor nerozezná modrou a černou barvu podloží, proto je robot vybaven barevným senzorem. Světelný senzor slouží pouze jako pomocný pro snímání přechodu barvy podloží v části lokální navigace. Ve vrchní části robotu je umístěna pohyblivá základna, která slouží pro uchycení kamery. Kamera snímá pozice jednotlivých referenčních bodů, proto se musí otáčet. Rozsah otočení je 360 stupňů. Motor pro pohyb kamery je upevněn za programovatelnou kostkou. Ovládáme jej přímo z programovatelné kostky. Kamera je připevněna přímo na ose otáčení motoru, proto se velikost úhlu, o který se natočí kamera rovná úhlu, který zadáme motoru jako parametr otočení. V přední části robotu, jsou připevněna pohyblivá ramena, která slouží k uchycení herního prvku. Díky servomotorům je možné zasunout přečnívající části do konstrukce robotu. Proto pokud robot odveze herní prvek na určené místo, tak pro opuštění prostoru nemusí couvat. Zasune ramena a otočí se kolem své osy jiným směrem a může pokračovat v práci.

Vlastní práce

27

Robot bude také vybaven dotykovými senzory. Celkem je k robotu možné připojit čtyři dotykové senzory díky multiplexoru. Jeden dotykový senzor slouží jako vypínač celého robotu. Po uvolnění se spustí program a při opětovném stisknutí se robot okamžitě zastaví a zanechá veškerých akcí. Tento senzor je umístěn na vrchní straně robotu. Další senzor detekuje, zda robot manipuluje s herním prvkem. Pokud se nachází ve stisknuté poloze, indikuje přítomnost prvku. Další dva senzory slouží jako detektory kolize s robotu s překážkou. Zjišťováním, zda je před robotem nějaká překážka, je pověřen infračervený senzor SumoEye. Tento senzor má oproti ultrazvukovému menší dosah, avšak pro lokální navigaci dostačující. Výhodou je vetší spolehlivost detekce překážky a oproti ultrazvuku malá pravděpodobnost rušení signálu. Jako doplňkové senzory budou použity ultrazvukové snímače využívané převážně v lokálním systému navigace, při posunování herních prvků. Na robotu musí být, podle pravidel, umístěna konstrukce do určité výšky s podstavcem pro maják protivníka. V tomto podstavci je umístěn infračervený senzor, který snímá pozici nepřátelského robotu. Podle informace z tohoto senzoru se program dozví, zda má před sebou nepřátelského robota nebo manipulovatelný prvek. Infračervený senzor pracuje na principu modulovaného signálu, proto nedochází k rušení ostatními zdroji infračerveného signálu. Na konstrukci robotu se podíleli řešitelé soutěže Eurobot z řad studentů MENDELU.

4.2 Program Systém pro lokalizaci a navigaci mobilního robotu je rozdělen na dvě části. Globální a lokální. V globální části jsou sdruženy systémy pro zjištění polohy a globální navigace. V lokální části je implementován systém pro rozeznání překážky a manipulace s herními prvky. Systém pro zjištění polohy obsahuje relativní navigaci a absolutní navigaci. Součástí tohoto systému je také mapa, na které lze určit, v kterém čtverci se robot nachází. Protože na hrací ploše nejsou žádné pevné překážky, a také proto, že se s hracími prvky stále manipuluje, není v mapě uložena žádná informace o stavu herního prostoru. Tyto informace by v tomto případě nebyly užitečné. Navíc se tímto krokem významně zjednoduší plánování tras. Nemusíme prohledávat stavový prostor ani implementovat žádný jiný způsob hledání cest.

Vlastní práce

4.2.1

28

Systémy pro zjištění polohy

4.2.1.1 Souřadný systém Střed soustavy souřadnic je v rohu modrého startovacího čtverce. Nezáleží na startovní pozici, souřadný systém bude pro robot vždy stejný. Startovní natočení robotu musí být také stejné v obou pozicích.

Obr. 18

Zjednodušený nákres hrací plochy s osami x a y


4.2.1.2 Relativní navigace Relativní navigace je řešena odometrií, čili odečítáním informace o ujeté vzdálenosti z kol. Nástroj LeJOS obsahuje třídu s názvem TachoPilot, která toto odečítání řeší. Vstupní parametry vycházejí z konstrukce robotu. Konstruktor vyžaduje obvod kol a vzdálenost mezi středy. Jedná se o typ podvozku s diferenčně řízenými koly. TachoPilot v neupravené podobě dokáže vypočítat ujetou vzdálenost a akumulované natočení robotu. Pro účely lokalizace bylo potřeba třídu upravit a přidat funkci počítání souřadnic. Pro potřeby výpočtu vzdálenosti je nutné znát absolutní natočení ve stupních od počátečního natočení. Metoda getAngle() proto byla uzpůsobena tak, aby takovou hodnotu dodávala. Počáteční nulové natočení je rovnoběžné s osou x směrem od středu souřadnic. Základní TachoPilot pracuje v palcích, proto bylo nutné doimplementovat převod na cm. Přidaná metoda VypocitejPrirustekSouradnic() dopočítává přírůstek souřadnic x a y. Nejprve podle úhlu z metody getAngle() zjistí, do kterého kvadrantu se robot pohybuje. V základu je osm možností kam robot směřuje. Pokud robot

Vlastní práce

29

ujel vzdálenost bez natočení přírůstek x je roven hodnotě ujeté vzdálenosti, přírůstek y je roven nule. Analogicky pokud je úhel roven 180 stupňům, přírůstek x je záporná hodnota ujeté vzdálenosti, přírůstek y rovno nule. Do směrů 90 a 270 stupňů je situace obdobná. Přírůstek x je roven nule. Přírůstek y se rovná délce ujeté vzdálenosti pro 90 stupňů a záporné hodnotě pro 270 stupňů. Jedná se o přírůstky je tudíž nutné je přičíst k výchozím souřadnicím x a y.

Obr. 19

Nákres možných směrů pohybu robotu


Další čtyři výpočty berou v potaz, do kterého kvadrantu robot jede. Metoda počítá jednotlivé přírůstky jako strany pomyslných pravoúhlých trojúhelníků, jak je vidět na obrázku č. 18. Velikost najeté vzdálenosti V počítá z otáček úhlového senzoru, vzorec č. 1.

d

(1)

accAngle je akumulovaný úhel úhlového senzoru. d je průměr volně se otáčejícího kola uprostřed konstrukce. Počet otočení vynásobený obvodem kola nám dá opravdu ujetou vzdálenost. Po každém výpočtu se accAngle resetuje. Vypočítanou hodnotu

Vlastní práce

30

je možné porovnat s hodnotou odečtenou z kol a zjistit tak rozdíl. Pokud známe ujetou vzdálenost a úhel natočení robotu můžeme počítat podle jednotlivých kvadrantů. Pro matematické funkce je použita knihovna java.Math.

4.2.1.2.1 Výpočty v jednotlivých kvadrantech Z pohybu robotu s natočením do 1. kvadrantu vyplívá, že oba přírůstky budou kladné. Protože funkce sinus v knihovně java.Math pracuje s radiány, musíme úhel převést podle vztahu č. 2.

! " #$

(2)

Hodnota přírůstku y je rovna velikosti protilehlé strany pravoúhlého trojúhelníku. Použijeme vzorec č. 3 kde se k rovná ujeté vzdálenosti. y sin )

(3)

Přírůstek x je roven velikosti přilehlé strany, počítáme podle vzorce č. 4. * +) , - . ,

(4)

Nyní má metoda vypočítány oba přírůstky souřadnic. V ostatních kvadrantech je postup stejný, jen se mění znaménka jednotlivých hodnot x a y. • • •

2. kvadrant znamená x záporné a y kladné. 3. kvadrant znamená x záporné a y záporné. 4. kvadrant znamená x kladné a y záporné.

Posledním krokem metody je přičtení přírůstků k absolutním hodnotám absX a absY.

4.2.1.3 Absolutní navigace Absolutní navigace je v této práci založena na snímání barevných prostorů kamerou NXTCamV2. Tato kamera dokáže snímat až osm barevných objektů rychlostí 30 snímků za sekundu. Máme na výběr mezi sledováním linek nebo obdélníků. Pokud vybereme mód sledování obdélníků, program má k dispozici údaje o velikosti stran, souřadnicích x a y, ve snímaném obraze, a barvě jednotlivých objektů. Úhel natočení kamery závisí na motoru, na kterém je kamera umístněna.

Vlastní práce

31

Absolutní poloha je počítána kombinací metod trilaterace a triangulace. Prvkem trilaterace je odečítání vzdálenosti pomocí přečtení obsahu čtverce sledovaného objektu. Princip triangulace je využit při odečítání úhlů, pod kterými jsou referenční body viděny. Pro absolutní navigaci platí stejný souřadný systém jako pro relativní navigaci.

Obr. 20

Znázornění hledaných hodnot vzdáleností a úhlů


Na obrázku je ukázáno jaké hodnoty musí mobilní robot odečíst ze svých senzorů, aby byl schopen dopočítat své souřadnice. Hodnota k je vzdálenost mezi majáky, která je konstantní. Vzdálenosti m a l jsou dopočítány podle obsahu snímaných obdélníků.

Vlastní práce

Obr. 21

32

Program NXTCamView

Zdroj: Vlastní screenshot.

Aby byl systém schopen zjistit vzdálenost od jednotlivých referenčních bodů, musí vědět, jaká je funkce vzdálenosti, pokud víme obsah sledovaného obdélníku. Následující tabulka ukazuje naměřené hodnoty obsahu obdélníku v určité vzdálenosti od objektivu kamery. Měření bylo provedeno pomocí softwaru NXTcam view. Na obrázku č. 20 je znázorněn program, ve kterém se pomocí PC nastavují jednotlivé barevné prostory. V levém horním rohu je snímaný obraz. Na dolním panelu je možnost nastavit, v jakém rozmezí se má barva snímat. V pravém oknu je ukázka spuštěného sledování prostorů včetně údajů, které poskytuje. Na obrázku č. 21 je zobrazen graf, který znázorňuje průběh funkce vzdálenosti.

Vlastní práce Tab. 1

33

Naměřené hodnoty vzdálenosti a obsahu snímaných obdélníků

Zdroj: Vlastní měření.

Obsah v bodech 3654 930 483 240 180 110 90 83 62 50 38 28 15

Vzdálenost v milimetrech 150 300 450 600 750 900 1050 1200 1350 1500 1650 1800 1950

Nameřené hodnoty 2500 Vzdálenost

2000 1500 1000 Nameřené hodnoty

500 0 0

1000

2000

3000

4000

Obsah Obr. 22

Graf naměřených hodnot

Zdroj: Vlastní graf z naměřených bodů.

Pro zjištění hodnoty mezi dvěma body v grafu je použita lineární interpolace. Vzorec č. 5 znázorňuje vztah pro vypočítání neznámé hodnoty vzdálenosti mezi dvěma body

Vlastní práce

34

proloženými přímkou, kde / je vzdálenost prvního bodu a /# je vzdálenost druhého bodu od kamery. Hodnota x udává obsah snímaného čtverce.

/0 / 1

23 425 03 4 05

6x - * 8

(5)

Lineární interpolace je nejjednodušší způsob interpolace. Spočívá v proložení dvou sousedních bodů přímkou21. Na obrázku č. 21 je vidět průběh funkce vzdálenosti po propojení bodů přímkami.

Lineární interpolace 2500

Vzdálenost

2000 1500 1000 Lineární interpolace 500 0 0

1000

2000

3000

4000

Obsah Obr. 23

Graf naměřených hodnot s propojením bodů

Zdroj: Vlastní nákres graf z naměřených hodnot.

Třída Absolutni má dvě hlavní metody. Metoda ZjistiUhly() a metoda VypoctiSouradnice(). Metoda ZjistiUhly() pracuje na principu otáčení motoru a snímání barevných prostorů. Pokud najde hledaný prostor uprostřed obrazu, dá motoru příkaz stop(). Sejme aktuální hodnotu otočení. Dále pokračuje, dokud nenarazí na druhý maják. Při tomto postupu mohou nastat dvě situace, které jsou v programu rozlišeny. Na obrázku č. 22 jsou obě situace ukázány. První případ nastane, pokud je první nalezený maják modrý. V tomto případě program uloží hodnotu prvního úhlu a pokračuje v hledání. Další nalezený maják je červený. Opět si uloží velikost úhlu. V druhém příWikipedia [online]. 2003 [cit. 2011-05-19]. Lineární interpolace. Dostupné z WWW: . 21

Vlastní práce

35

padě postupuje program stejně. V obou případech si program uloží také velikost snímaného čtverce. Výpočet se v jednotlivých případech liší.

Obr. 24

Znázornění postupu snímání úhlů jednotlivých majáků


Hledaný úhel, který svírají majáky, program počítá podle následujícího vztahu. Výpočet platí pro první případ. γ 360 - =

(6)

Úhel natočení k červenému majáku, který je potřeba pro další výpočty, je roven součtu obou úhlů. δ?1=

(7)

Následuje výpočet pro druhý případ. V tomto případě je hledaný úhel γ roven úhlu = . γ = Úhel natočení k červenému majáku δ je roven úhlu ? .

(8)

Vlastní práce

36

δ ?

(9)

Dále zbývá určit vzdálenost od jednotlivých majáků. Využijeme vzorce pro zjištění hodnoty mezi dvěma body proloženými přímkou. Program porovnává zjištěnou velikost snímané plochy s naměřenými hodnotami, dokud nenajde dva body, pro které platí: * @ *A B *#

(10)

Po dosazení hodnot do vztahu č. 5 Dostaneme hledané velikosti stran m a l pomyslného trojúhelníku zobrazeného na obr. č. 19. Po těchto výpočtech má program k dispozici všechny potřebné hodnoty pro přímý výpočet absolutních souřadnic x a y.

Metoda VypoctiSouradnice() provádí výpočty absolutních souřadnic x a y. Označení proměnných je zobrazeno na obrázku č. 23.

Obr. 25

Zobrazení všech proměnných, se kterými počítá metoda VypoctiSouřadnice()


Vlastní práce

37

Nejprve program převádí úhel γ na radiány. γ

C "

(11)

#$

Dále metoda počítá úhel = podle vzorce č. 12, vycházejícího ze sinové věty. = asin6

E! C F

8

(12)

Výsledek je v radiánech, proto převedeme zpět na úhly. =

G #$ "

(13)

Nyní můžeme sečíst uhly = a γ , pomocí nichž vypočítáme hodnotu úhlu ? . ? 180 - 6 = 1 γ8

(14)

* J sin6=8

(15)

Následuje výpočet souřadnice x.

Dalším krokem je výpočet souřadnice y. Vychází s pomyslného pravoúhlého trojúhelníku podle obrázku č. 24. . √J, - * ,

(16)

Posledním výpočtem je určení úhlu natočení, od počátečního stavu. K úhlu, pod kterým je viděn červený maják je přičtena hodnota úhlu ? . Dále je podmínkou zjištěno, zda je tato hodnota menší než 180. Pokud podmínku splňuje, přičte program 180. V opačném případě hodnotu 180 odečte. Výsledné číslo udává hodnotu natočení ve stupních.

4.2.1.3.1 Třída HraciPlocha Tato třída je modelem herní podložky. Je rozdělena na obdélníky, které mají vlastnosti odpovídající jejich pozici, barvě a velikosti. Konstruktor této třídy využívá upravené třídy Rectangle, dostupné v nástroji LeJOS, ke generování obdélníků určujících pozici na podložce.

Vlastní práce

Obr. 26 čtverec

38

Na obrázku je znázorněn model hrací plochy s ukázkou informací, které obsahuje každý


Vygenerované obdélníky se ukládají do jednorozměrného pole. Index pole ve kterém se nachází daný obdélník je roven jeho číslu. Po přístupu k objektu RectangleV2 na tomto indexu mohu zjistit hodnoty jeho atributů a odkazovat se na jeho metody. Důležitou součástí třídy RectangleV2 je metoda contains(). Pokud této metodě předložíme, jako parametry, hodnoty souřadnic x a y, vrátí nám hodnotu pravda, pokud se bod v obdélníku nachází a hodnotu nepravda pokud ne. Tímto jednoduchým způsobem dokáže program zjistit, v jakém obdélníku se robot nachází. Třída HraciPlocha dále obsahuje barevný senzor, který je umístěn na podvozku, a který dokáže určit barvu podloží. Metoda UrciCtverec() provede kontrolu, zda barva podloží odpovídá barvě obdélníku, na kterém se robot, podle výpočtů polohy, nachází.

4.2.2

Lokální navigace

V tomto programu je hlavním úkolem lokální navigace určit typ překážky před sebou. Na hrací ploše Eurobot může robot narazit na čtyři typy překážek. Dvě patří mezi manipulovatelné předměty. Jedná se pěšce a postavy krále a královny. Třetím typem je robot protihráče, a posledním typem je okrajová stěna hřiště. V případě nepřátelského robotu je možnost využít prostoru jeho podstavce a umístit na něj svůj majá-

Vlastní práce

39

ček. V našem případě je maják reprezentován infračerveným zářičem. Infračervené záření je modulováno na 1200 Hz s obdélníkovým průběhem, proto nedochází k rušení vlivem jiných zdrojů. Senzor IRSeeker zaznamenává sílu a směr modulovaného záření. Jako přední detektor překážek slouží senzor SumoEye se třemi směry detekce. Pokud tedy robot narazí na překážku, programu stačí odečíst hodnoty z IR senzoru a určí, zda se jedná o protivníka či o jinou překážku. Podle hodnot souřadnic x a y zjistíme, na jakém obdélníku se robot nachází, a tedy i možnost přiblížení k okraji hřiště. Vylučovací metodou poté program určí, zda se jedná o stěnu nebo o manipulovatelný prvek. Pokud program rozhodne, že překážka je pěšec nebo figura, spustí se podprogram pro posunování hracích prvků.

4.3 Navigace robotu Pevnou překážku symbolizuje pouze okraj hřiště, tato skutečnost je využita pro tvorbu pravidel pro pohyb robotu na hrací ploše. Jak již bylo v této práci výše uvedeno, neprobíhá žádné hledání nejkratší cesty, ani zadávání cíle, kam má robot docestovat. Jakými cestami mobilního robot projede, záleží na volbě herní strategie. Herní strategií může být například pohyb robotu po hranách obdélníků a hledání manipulovatelných prvků. Dalším možný způsob pohybu je po úhlopříčkách. Zjišťování, zda je robot na okraji hřiště, a s tím spojené otočení, probíhá v několika krocích. První akcí je odečtení souřadnic z relativní navigace. Poté se odečte poloha z absolutní navigace a provede se porovnání za pomoci Třídy HraciPlocha. Po zjištění souřadnic x a y proběhne porovnání s nadefinovanými pravidly. Příklad nadefinovaného pravidla je následující. y B 180 LáNOň * B 190

(17)

Pokud je splněna tato podmínka znamená to, že se robot nachází v pravém horním rohu hřiště. Je tedy třeba robot otočit směrem opačným a pokračovat v jízdě podle herní strategie.

Diskuze

40

5 Diskuze Konstrukce podvozku byla při návrhu navigace stěžejní. Koncepce typu tribot, je s přihlédnutím k dostupným pohonům, nejlepší volba s hlediska přesnosti i rychlosti pohybu. V týmu Eurobot byla diskutována také možnost použití všesměrových kol. Velké pevnosti základny celého robotu bylo dosaženo použitím kovové stavebnice Tetrix, kovového plechu a nadstavbou z Lego dílků. Volba odometrie jako způsobu relativní navigace byla logickým krokem ze dvou důvodů. První výhodou bylo to, že motory NXT, které zajišťují pohon mobilního robotu, jsou vybaveny již z výroby rotačním snímačem. Vyhneme se tedy výpočetně složitým operacím odečítání ujeté vzdálenosti. Druhým plusem byla možnost využití existujících tříd v nástroji LeJOS, které částečně splňovaly požadavky na počítání pozice. Problematika, která v řešení této práce zabrala mnohem více experimentálního času, byla absolutní navigace. Prvotní návrh řešení počítal s umístěním tří infračervených zářičů na místa lokalizačních majáčků. Navržené obvody pro modulování odesílaného signálu na obdélníkový průběh s frekvencí 1200 Hz se bohužel neosvědčily pro praktické využití. Pouze jeden infračervený majáček bude umístěn na protivníkova robota. Program jej využije pro detekci jeho přibližné polohy. V řešení tedy bylo využito jiné technologie, a to kamerového snímání barevných prostorů. Hlavním nevýhodou tohoto systému je velká závislost na kvalitě osvětlení. Pokud jsou dobré podmínky osvětlení, je tato technologie poměrně spolehlivá. Jako možnost rozšíření by bylo možné implementovat do systému NXT neuronovou síť (dále jen NN). Experimentálně bylo dokázáno, že naučenou NN dokáže programovatelná kostka použít pro rozhodování. Použití NN by bylo vhodné u systému absolutní navigace. Využita by byla především schopnost generalizace vstupních informací. Pokud by měl být systém navigace implementován tímto způsobem, nutně by muselo věnovat mnoho času sběrem vhodných dat, návrhu a testování NN.

Závěr

41

6 Závěr Cílem práce bylo vytvořit program, který by podle informací ze senzorů byl schopen nalézt aktuální pozici robotu na hrací ploše. Celý program byl realizován v jazyce Java v prostředí Eclipse. Dosažená přesnost lokalizace, na jeden základní obdélník 35 x 35 cm, je pro dané účely dostačující, nicméně pokračováním práce by se dala přesnost i spolehlivost jistě vylepšit. Představené řešení absolutní navigace je jedno z mnoha možných pro daný systém. Existuje mnoho kombinací senzorů, díky kterým je možné lokalizaci robotu realizovat. Přesnost a spolehlivost je však nutné vždy ověřit experimentálně a je tedy těžké říci, která kombinace je nejvhodnější. V případě relativní navigace bylo největší riziko reprezentováno prokluzem kol a s tím spojený velký nárůst chyby. Toto riziko bylo sníženo přidáním úhlového senzoru, který měří ujetou vzdálenost nezávisle na příkazech pro poháněná kola. Absolutní navigace pomocí kamery je velmi citlivá na dobré osvětlení ve chvíli vyhledávání barevných majáků. Bylo experimentováno se svítícími majáky a dalšími způsoby, které by zlepšily citlivost. Pokud by byly světelné podmínky ve chvíli snímání barevných prostorů nevhodné, měření se zopakuje. Pro případ úplného výpadku absolutní navigace byl připraven program, který je schopen navigovat robota pomocí barevného senzoru, umístěného na podvozku. Téma této práce úzce souvisí se soutěží Eurobot 2011. Pravidla soutěže se každoročně mění a s novými pravidly přichází také změna hrací plochy. Výsledný program je ale možné využít pro inspiraci i do dalších ročníků, díky tomu, že rozměry a umístění podstavců pro pomocné majáky se nemění.

Literatura

42

7 Literatura •

Novák, P. Mobilní Roboty. Praha: BEN, 2007. 248 s., ISBN 80-7300-141-1.

•

Java for LEGO Mindstorms [online]. 1997 [cit. 2011-04-30]. LeJOS. Dostupné z WWW: .

•

LEGO Mindstorms [online]. 2000 [cit. 2011-04-30]. NXT log. Dostupné z WWW: .

•

Eurobot [online]. 1998 [cit. 2011-04-30]. International Conference Eurobot. Dostupné z WWW: .

•

Hitechnic [online]. 2001 [cit. 2011-04-30]. Robotic sensors. Dostupné z WWW: .

•

Mindsensors.com [online]. 2005 [cit. 2011-04-30]. Advanced sensors. Dostupné z WWW: .

•

Eurobot [online]. 1994 [cit. 2011-04-30]. Robotický den. Dostupné z WWW: .

•

BORENSTEIN, J.; EVERETT, H. R.; FENG, L. Where am i? [online]. Michigan: University of Michigan, 1996 [cit. 2011-04-30]. Dostupné z WWW: .

•

Wikipedia [online]. 2003 [cit. 2011-05-19]. Lineární interpolace. Dostupné z WWW: .

Přílohy

43

Přílohy

Seznam obrázků

44

A Seznam obrázků Obr. 1

Stavebnice Lego Mindstorms

8

Obr. 2


9

Obr. 3

Ultrazvukový senzor

10

Obr. 4

Dotykový senzor

11

Obr. 5

Zvukový senzor

12

Obr. 6

Světelný senzor

12

Obr. 7

Barevný senzor

13

Obr. 8

Infračervený senzor a zobrazení směrů příjmu signálu

14

Obr. 9 Infračervený detektor překážek a znázornění detekovaných zón. 14 Obr. 10

Kamera NXTCam V2

15

Obr. 11

Úhlový senzor

16

Obr. 12

Servomotor dodávaný ke stavebnici NXT Mindstorms.

16

Obr. 13

Modelářský servomotor spolu s multiplexorem pro NXT. 17

Obr. 14

Obrázek hřiště pro Eurobot 2011

21

Obr. 15 Podvozek s dvěma diferenčně řízenými koly a jedním opěrným, někdy nazývaný tribot.

24

Obr. 16

25

Kuličkové kolečko od společnosti Mindsensors.

Obr. 17 Pohled na spodní část robotu s připevněným barevným senzorem. 26 Obr. 18

Zjednodušený nákres hrací plochy s osami x a y.

28

Obr. 19

Nákres možných směrů pohybu robotu.

29

Obr. 20

Znázornění hledaných hodnot vzdáleností a úhlů.

31

Obr. 21

Program NXTCamView

32

Seznam obrázků

45

Obr. 22

Graf naměřených hodnot

33

Obr. 23

Graf naměřených hodnot s propojením bodů

34

Obr. 24

Znázornění postupu snímání úhlů jednotlivých majáků 35

Obr. 25 Zobrazení všech proměnných, se kterými počítá metoda VypoctiSouřadnice() 36 Obr. 26 Na obrázku je znázorněn model hrací plochy s ukázkou informací, které obsahuje každý čtverec. 38 Obr. 27

Pohled na celkovou konstrukci robotu.

62

Obr. 28

Detailnější pohled na přední senzory.

63

Seznam tabulek

46

B Seznam tabulek Tab. 1 Naměřené hodnoty vzdálenosti a obsahu snímaných obdélníků 33


C Program pro navigaci robotu package navigace; import java.awt.Rectangle; import lejos.nxt.Motor; import lejos.nxt.SensorPort; import lejos.nxt.addon.NXTCam; public class Absolutni { //uhel pod kterym je viditelny cerveny majak private double uhelCerveny; //uhel pod kterym je viditelny modry majak private double uhelZeleny; //uhel ktery sviraji oba majaky z pozice robotu private double uhelMeziMajaky; //uhel natoceni robotu private double uhelNatoceni; //motor ktery otaci kamerou private Motor motor; //kamera private NXTCam kamera; //delka mezi majaky (je to konstanta) private double l; //strana a pomyslneho trojuhelniku private double stranaA; //strana b pomyslneho trojuhelniku private double stranaB; //xova souradnice private double x; //yova souradnice private double y; //pole namerenych hodnot vzdalenosti

47

Program pro navigaci robotu private double namereneHodnoty[] = new double [25]; //konstruktor priradi jednotlive senzory a motory a nastavi promenne public Absolutni(SensorPort port,double delka){ //nastaveni pocatecnich hodnot uhelCerveny = 0; uhelZeleny = 0; stranaA = 0; stranaB = 0; x = 0; y = 0; motor = Motor.B; l = delka; kamera = new NXTCam(port); //zadani vsech namerenych hodnot prvni hodnota x druha y namereneHodnoty[0] = 3654; namereneHodnoty[1] = 150; namereneHodnoty[2] = 930; namereneHodnoty[3] = 300; namereneHodnoty[4] = 483; namereneHodnoty[5] = 450; namereneHodnoty[6] = 240; namereneHodnoty[7] = 600; namereneHodnoty[8] = 180; namereneHodnoty[9] = 750; namereneHodnoty[10] = 110; namereneHodnoty[11] = 900; namereneHodnoty[12] = 90; namereneHodnoty[13] = 1050; namereneHodnoty[14] = 83; namereneHodnoty[15] = 1200; namereneHodnoty[16] = 62; namereneHodnoty[17] = 1350; namereneHodnoty[18] = 50; namereneHodnoty[19] = 1500; namereneHodnoty[20] = 38; namereneHodnoty[21] = 1650; namereneHodnoty[22] = 28; namereneHodnoty[23] = 1800; namereneHodnoty[24] = 15; namereneHodnoty[25] = 1950; }

48

Program pro navigaci robotu //metoda postupne otoci kamerou a zjisti uhel viditelnosti jednotlivych majaku public void ZjistiUhly(){ //promenna pro ctverce Rectangle ctverec1 = new Rectangle(0,0); Rectangle ctverec2 = new Rectangle(0,0); //promenna pro cykleni boolean noPoint = true; //vynulujeme tachometr motoru motor.resetTachoCount(); //prvni zaznamenany uhel double prvniUhel; //druhy zaznamenany uhel double druhyUhel; //ktery majak byl viden jako prvni 1 cerveny 2 modry int majak = 0; //nastavime na kamere aby snimane objekty radila podle velikosti kamera.sortBy(NXTCam.SIZE); //povolime snimani okoli kamera.enableTracking(true); //zacneme motorem pomalu otacet ve smeru hodinovych rucicek motor.setSpeed(50); motor.forward(); while(noPoint){ //jestli kamera vidi nejaky nadefinovany objekt if(kamera.getNumberOfObjects() < 1){ //pokud je objekt ve stredu zorneho pole if(kamera.getRectangle(1).x < 80 || kamera.getRectangle(1).x > 96){ //kamera zaznamenala jeden majak if(kamera.getObjectColor(0) == 0){ //prvni majak je cerveny ulozime do 1 majak = 1; ctverec1 = kamera.getRectangle(1); }else{ //prvni majak je zeleny ulozime do 2

49


50 majak = 2; ctverec2 = kamera.getRectangle(1); }

//zastavime motor motor.stop(); //opustime while noPoint = false; } } Thread.yield(); } //ulozime si prvni uhel a resetujeme tachometr prvniUhel = motor.getTachoCount(); motor.resetTachoCount(); noPoint = true; //zacneme motorem pomalu otacet ve smeru hodinovych rucicek motor.setSpeed(50); motor.forward(); while(noPoint){ //jestli kamera vidi nejaky nadefinovany objekt if(kamera.getNumberOfObjects() < 1){ //pokud je objekt ve stredu zorneho pole if(kamera.getRectangle(1).x < 80 || kamera.getRectangle(1).x > 96){ //kamera zaznamenala jeden majak if(kamera.getObjectColor(0) == 0){ //pokud je barva cervena a prvni byl cerveny ignorujeme jinak ulozime ctverec a koncime if(!(majak==1)){ ctverec2 = kamera.getRectangle(1); //zastavime motor motor.stop(); //opustime while noPoint = false; } }else{ //pokud byla barva prvni modra a je modra ignorujeme jinak ulozime ctverec a koncime


51 if(!(majak==2)){ ctverec1 = kamera.getRectangle(1); //zastavime motor motor.stop(); //opustime cyklus noPoint = false; } }

} } Thread.yield(); } //ziskame druhy uhel druhyUhel = motor.getTachoCount(); //promenne pro vypocitani vzdalenosti od majaku boolean vetsi = true; int index = 0; //zahajime vypocet podle toho, jaky majak byl prvni viden if(majak == 1){ //prvni majak byl cerveny //vypocitani velikosti stran stranaA = ctverec1.width * ctverec1.height; stranaB = ctverec2.width * ctverec2.height; // zjistime mezi kterymi body se hodnota obsahu nachazi while(vetsi){ if(stranaA < namereneHodnoty[index]){ index += 2; }else{ vetsi = false; } } //nyni mame v promenne index ulozen bod x1 v index+1 mame f1 index -2 mame x0 v index -1 mame f0 a muzeme vypocitat hodnotu fx. Strana A stranaA = namereneHodnoty[index-1] + (((namereneHodnoty[index+1]namereneHodnoty[index-1])/(namereneHodnoty[index]-namereneHodnoty[index-2]))*(stranaA - namereneHodnoty[index-2]));


52

vetsi = true; // zjistime mezi kterymi body se hodnota obsahu nachazi while(vetsi){ if(stranaB < namereneHodnoty[index]){ index += 2; }else{ vetsi = false; } } //nyni mame v promenne index ulozen bod x1 v index+1 mame f1 index -2 mame x0 v index -1 mame f0 a muzeme vypocitat hodnotu fx. strana B stranaB = namereneHodnoty[index-1] + (((namereneHodnoty[index+1]namereneHodnoty[index-1])/(namereneHodnoty[index]-namereneHodnoty[index-2]))*(stranaB - namereneHodnoty[index-2])); //uhel mezi majaky je roven druhemu uhlu uhelMeziMajaky = druhyUhel; //uhel natoceni robotu se rovna prvnimu uhlu (tam byl viden cerveny) uhelNatoceni = prvniUhel; }else{ //prvni majak byl modry //vypocitani velikosti stran stranaA = ctverec2.width * ctverec2.height; stranaB = ctverec1.width * ctverec1.height;

// zjistime mezi kterymi body se hodnota obsahu nachazi while(vetsi){ if(stranaA < namereneHodnoty[index]){ index += 2; }else{ vetsi = false; } } //nyni mame v promenne index ulozen bod x1 v index+1 mame f1 index -2 mame x0 v index -1 mame f0 a muzeme vypocitat hodnotu fx. Strana A stranaA = namereneHodnoty[index-1] + (((namereneHodnoty[index+1]namereneHodnoty[index-1])/(namereneHodnoty[index]-namereneHodnoty[index-2]))*(stranaA - namereneHodnoty[index-2]));


53

vetsi = true; // zjistime mezi kterymi body se hodnota obsahu nachazi while(vetsi){ if(stranaB < namereneHodnoty[index]){ index += 2; }else{ vetsi = false; } } //nyni mame v promenne index ulozen bod x1 v index+1 mame f1 index -2 mame x0 v index -1 mame f0 a muzeme vypocitat hodnotu fx. strana B stranaB = namereneHodnoty[index-1] + (((namereneHodnoty[index+1]namereneHodnoty[index-1])/(namereneHodnoty[index]-namereneHodnoty[index-2]))*(stranaB - namereneHodnoty[index-2])); //pokud je modry viden jako druhy tak cesta k cervenemu je zbytek z 360 po odecteni uhlu mezi majaky uhelMeziMajaky = 360 - druhyUhel; //uhel natoceni k cervenemu majaku je dan souctem uhlu ktere urazi kamera od pocatku uhelNatoceni = prvniUhel + druhyUhel; } } //ze zjistenych uhlu zjisti souradnice public void VypoctiSouradnice(){ double uhel = uhelMeziMajaky; //prevedeni uhlu na radiany uhelMeziMajaky = (double) ((uhelMeziMajaky * Math.PI) / 180); //vypocitani uhlu beta (u modreho majaku) uhelZeleny = Math.asin((Math.sin(uhelMeziMajaky) / l) * stranaA); //prevedeni na uhly uhelZeleny = (uhelZeleny * 180) / Math.PI; //sectu dva uhly v trojuhelniku uhel += uhelZeleny;


//prevedeni uhlu na radiany uhelZeleny = (double) ((uhelZeleny * Math.PI) / 180); //vypocteni x a y() x = stranaA * Math.sin(uhelZeleny); y = Math.sqrt((stranaB * stranaB)-(x * x)); //vypocteni natoceni robotu //uhel alfa u cerveneho majaku uhelCerveny = 180 - uhel; //prictu uhel cerveny k velikosti uhlu pod kterym je viden cerveny majak uhelNatoceni += uhelCerveny; //zjistim jestli je spocitana hodnota vetsi nez 180 if(uhelNatoceni < 180){ //hodnota je mensi proto pripocitam 180 uhelNatoceni += 180; }else{ //hodnota je vetsi nebo rovna 180 proto odecitam uhelNatoceni -= 180; } } } //konec tridy absolutni

54


55

package navigace; import lejos.nxt.SensorPort; import lejos.nxt.addon.ColorSensor; /** * * Hraci plocha obsahuje pole obdelniku. Kazdy obdelnik ma souradnice praveho horniho rohu a rozmery. * Oznaceni pozice na stole a barvu. Promenne x a y uchovavaji aktualni pozici robotu. promenne oldx a oldy * obsahuji minulou pozici. * */ public class HraciPlocha { //aktualni pozice robotu //pole s hodnotami souradnic private double souradnice[] = new double [2]; //predchozi pozice robotu //pole s hodnotami souradnic private double souradniceOld[] = new double [2]; private RectangleV2 plocha[] = new RectangleV2[48]; private ColorSensor barevny;

//konstruktor se postara o vytvoreni pole obdelniku s pozadovanymi vlastnostmi public HraciPlocha(int startx, int starty, SensorPort port){ barevny = new ColorSensor(port); //nastaveni aktualni pozice vzhledem k rohu hraciho stolu souradnice[0] = startx; souradnice[1] = starty; //nastaveni souradnic do pole souradniceOld[0] = startx; souradniceOld[1] = starty; //vytvoreni pole obdelniku ... souradnice 0,0 v dolnim rohu modreho startovaciho pole //modry startovaci ctverec x,y,s,v


56

plocha[0] = new RectangleV2(0,40,40,40); plocha[0].color = 1; plocha[0].name = 0; //parametry pro vytvoreni hlavni herni plochy (36 ctvercu cervene a modre barvy) int x = 45; int y = 35; int barva = 2; boolean scitani = true; int jmeno = 1; //cyklus pro vytvoreni hlavni herni plochy for (int i = 1; i <= 6; i++) { //cyklus pro vytvoreni jedne rady (6 ctvercu o konstatni y s menicim se x a barvou stridave) for (int k = 1; k <= 6; k++) { //rozmery jsou konstantni plocha[jmeno] = new RectangleV2(x,y,35,35); plocha[jmeno].color = barva; plocha[jmeno].name = jmeno; jmeno++; //jestli budeme odecitat nebo pricitat xovou souradnici if(scitani){ x += 35; }else{ x -= 35; } //stridave menime barvu modrou a cervenou if(barva == 2){ barva = 1; }else{ barva = 2; } } //pricteme k y 35 tim se posuneme o radu vys y += 35; //korekce jedne hrany aby zustalo konstantni x if(scitani){ x -= 35; }else{ x += 35; } //jestli x pricitame nebo odecitame if(scitani){

Program pro navigaci robotu scitani = false; }else{ scitani = true; } } y = 74; //cyklus pro vytvoreni 5ti ctvercu nad startovni modrou plochou velikost s40 v34 for (int i = 37;i <= 41; i++ ){ plocha[i] = new RectangleV2(0,y,40,34); plocha[i].color = 3; plocha[i].name = i; y += 34; } //cyklus pro vytvoreni 5ti ctvercu nad startovni cervenou plochou velikost s40 v34 y = 74; for (int i = 42;i <= 46; i++ ){ plocha[i] = new RectangleV2(260,y,40,34); plocha[i].color = 3; plocha[i].name = i; y += 34; } //vytvoreni obdelniku nad modrou plochou (cerna hrana podel zasobarny na hraci prvky) plocha[47] = new RectangleV2(40,210,5,210); plocha[47].color = 4; plocha[47].name = 47; //vytvoreni obdelniku nad cervenou plochou (cerna hrana podel zasobarny na hraci prvky) plocha[48] = new RectangleV2(255,210,5,210); plocha[48].color = 4; plocha[48].name = 48; } //metoda pro ulozeni aktualni zjistene pozice public void NastavSouradnice(double[] pole){ //ulozime minulou pozici souradniceOld[0]= souradnice[0]; souradniceOld[1] = souradnice[1]; //ulozime soucasnou pozici souradnice[0]= pole[0]; souradnice[1]= pole[1]; }

57

P rn g a u o p m ticvb

58

//pricteme prirustek souradnic public void Prirustek(double[] pole){ souradnice[0] += pole[0]; souradnice[1] += pole[1]; }; //metoda vraci souradnice public double[] VratSouradnice(){ return souradnice; } //metoda pro vypsani souradnic na disp. public void VypisSouradnice(){ System.out.println(souradnice[0]); System.out.println(souradnice[1]); } //metoda overi barvu ctverce na kterem se nachazi robot public boolean UrciCtverec(double[] pole){ int cisloCtverce = 0; int barvaColor = 0; //projdeme pole ctvercu a najdeme ten v kterem se nachazi robot for (int k = 0; k <= 48; k++){ if(plocha[k].contains(pole[0], pole[1])){ cisloCtverce = k; }; } //precteme hodnotu z barevneho senzoru if(barevny.getBlueComponent()>10){ barvaColor = 1; }else if(barevny.getRedComponent()>10){ barvaColor = 2; }else if(barevny.getGreenComponent()>10){ barvaColor = 3; } return plocha[cisloCtverce].color == barvaColor; } }//konec tridy hraci plocha //vypocti souradnice (metoda jez je vlastni kod ve tride tachoPilot) public void VypocitejPrirustekSouradnic(){


59

double uhelVzdal; //ziskani uhlu natoceni robotu float uhel = getAngle(); //ziskame info o ujete vzdalenosti a prepocitame na cm double vzdal = getTravelDistance() - najeto; najeto =+ getTravelDistance(); vzdal=(float) (vzdal*2.54); //ziskani vzdalenosti z uhloveho senzoru uhelVzdal = uhlovySenzor.getAccAngle(); uhlovySenzor.resetAccAngle(); //prepocitame na vzdalenost uhelVzdal = (uhelVzdal/360) * 9.42; //pokud se najeta vzdalenost nerovna odectu z kol - moznost protoceni vezme se tedy v uvahu skutecna hodnota vzdalenosti if(!(uhelVzdal == vzdal)){ vzdal = uhelVzdal; } //zjistime, do ktereho kvadrantu robot jede if(uhel == 0){ // jede rovne v ose x x = vzdal; y = 0; }else if( uhel == 90){ // jede pravouhle nahoru y = vzdal; x = 0; }else if(uhel == 180){ //jede zpet x = -vzdal; y = 0; }else if (uhel == 270){ //jede rovne dolu y = -vzdal; x = 0; }else if(uhel > 0 && uhel < 90){ //kvadrant 1 oba prirustky jsou kladne //prevedeni uhlu na radiany uhel = (float) ((uhel * Math.PI) / 180); //vypocteni prirustku y (protilehla) y = Math.sin(uhel) * vzdal; //vypocteni prirustku x (prilehla) x = Math.sqrt((vzdal*vzdal)-(y*y));

Program pro navigaci robotu }else if(uhel > 90 && uhel < 180){ //kvadrant 2 y prirustek kladny x prirustek zaporny //prevedeni uhlu na radiany uhel = (float) ((uhel * Math.PI) / 180); //vypocteni prirustku y (protilehla) y = Math.sin(uhel) * vzdal; //vypocteni prirustku x (prilehla) x = -(Math.sqrt((vzdal*vzdal)-(y*y))); }else if(uhel > 180 && uhel < 270){ //kvadrant 3 y prirustek zaporny x prirustek zaporny //prevedeni uhlu na radiany uhel = (float) ((uhel * Math.PI) / 180); //vypocteni prirustku y (protilehla) y = (Math.sin(uhel) * vzdal); //vypocteni prirustku x (prilehla) x = -(Math.sqrt((vzdal*vzdal)-(y*y))); }else if(uhel > 270 && uhel < 360){ //kvadrant 4 y je zaporne x kladne //prevedeni uhlu na radiany uhel = (float) ((uhel * Math.PI) / 180); //vypocteni prirustku y (protilehla) y = (Math.sin(uhel) * vzdal); //vypocteni prirustku x (prilehla) x = (Math.sqrt((vzdal*vzdal)-(y*y))); } xabs += x; yabs += y; } //metoda vraci dvouprvkove pole kde prvek s indexem 0 je x a index 1 je y public double[] VratSouradnice(){ double souradnice[] = new double [2]; souradnice[0] = xabs; souradnice[1] = yabs; return souradnice; } public void VypisPolohu(){

60


//ziskani uhlu / pokud je zaporny prepocitame na urcity kvadrant float uhel = getAngle(); System.out.println("uhel " + uhel); System.out.println("y: "+yabs); System.out.println("x: "+xabs); } }

61


D Obrázky finální konstrukce robotu

Obr. 27

Pohled na celkovou konstrukci robotu


62


Obr. 28

Detailnější pohled na přední senzory


63

Využití senzorů pro lokalizaci robota v prostoru

Recommend Documents