TECHNICKÁ UNIVERZITA V LIBERCI
Fakulta mechatroniky, informatiky a mezioborových studií
DIPLOMOVÁ PRÁCE
Liberec 2013 Martin Durchánek
Poděkování
Chtěl bych poděkovat panu Ing. Lubomíru Slavíkovi za zapůjčení robota a panu Ing.
Miroslavu Holadovi, Ph.D. za obětavé vedení mé práce.
Abstrakt
Hlavním cílem této práce bylo navrhnout pokročilý interaktivní ovládací systém, který by využíval v co největší míře možnosti robota Hexor II. Práce se zabývá zejména návrhem softwaru pro řídící počítač a softwaru mikroporcesoru pro ovládání robota. Robot využívá pro své řízení mikroprocesor ATmega128 s důrazem na maximální možnou efektivitu využití jeho vnitřních komponent a výpočetního výkonu i s ohledem na možné budoucí rozšíření hardwaru robota, popřípadě využití navržených algoritmů pro jiné aplikace. Pro zpracování obrazu využívá robot distribuovaný výpočetní výkon osobního počítače. Aby bylo možné efektivně a v reálném čase výpočetní výkon osobního počítače využívat, bylo nutné sestrojit nový rychlý komunikační kanál mezi robotem a počítačem. Veškeré použité postupy a algoritmy
realizované na robotovi Hexor II byly navrženy tak, aby je bylo možné používat v reálných podmínkách předváděcích akcí a v reálném čase, a jsou limitovány hlavně hardwarem robota, zejména jeho nekvalitní kamerou.
Klíčová slova: mobilní robot, Hexor II, ATmega128, handtracking,
The main objective of this Diploma Thesis was to design an advanced interactive control system, which would use in the greatest degree the possibilities of Robot Hexor II. The Diploma Thesis is primarily engaged in designing software for managing computer and creating software for a microprocessor to control the robot. The Robot uses ATmega128 microcontroller for its own management with an emphasis on maximum efficiency of utilization of its internal components and also computing power with regard to the possible future expansion of the robot hardware or using the proposed algorithms for other
applications. For image processing the robot uses a distributed computing power of personal
computers. In order to use efficiently and in real -time the computing power of personal
computer, it was necessary to construct a new fast communication channel between the Robot
and the computer. All the procedures and algorithms implemented in the Robot Hexor II were
designed so that they could be used in the real world demonstration events in real time, and
these are limited mainly by the Robot hardware, especially the poor camera.
Obsah
1. Úvod ...1
1.1 Problematika řízení mobilních robotů ...1
1.2 Kinematika mobilních robotů (1) ...1
2. Robot Hexor II ...3
2.1 Popis robota ...3
2.2 Režimy použití ...6
3. Použité technologie ...7
3.1 Použité technologie pro programování ...7
3.2 Robot Hexor II ...8
4. Program pro řízení robota ... 11
4.1 Handtracking ... 11
4.2 Zpracování naměřených dat a synchronizace ... 18
4.3 Hlavní okno programu pro ovládání robota ... 20
4.4 Hlavní algoritmus ... 21
4.5 Povely a běh programu v reálném prostředí ... 22
5. Software robota ... 26
5.1 Popis řešení dle výrobce robota ... 26
5.2 Návrh nové architektury SW robota ... 27
5.3 Realizace SW robota ... 29
6. Komunikace... 33
6.1 Původní řešení ... 33
6.2 Nový komunikační kanál ... 34
6.3 Nový formát komunikace ... 35
6.4 Nastavení procesoru ATmega128 (11) ... 37
6.5 Výsledky testování komunikačního kanálu v reálném provozu ... 39
7. Závěr ... 40
7.1 Překážky při tvorbě této práce ... 40
7.2 Dosažené výsledky ... 42
8. Reference... 42
9. Seznam obrázků ... 44
10. Seznam příloh ... 45
1 1. Úvod
1.1 Problematika řízení mobilních robotů
Mobilní roboty jsou elektromechanické stroje, které jsou schopné autonomního nebo řízeného pohybu v reálném světě a obvykle bývají schopni okolní svět i omezeným způsobem měnit.
Míra autonomie mobilních robotů může být různá. Teleoperovaný robot striktně vykonává přímé rozkazy od obsluhy robota. U takového robota není žádoucí, aby reagoval samostatně na okolní svět. Semiautonomní robot je také řízen obsluhou, avšak dokáže reagovat na předem definované situace, například pokud zjistí robot v dráze pohybu překážku, tak se zastaví, aby se o ni nepoškodil. Plně autonomní robot vykonává všechny činnosti bez vnějších zásahů obsluhy, ta robotovi může pouze zadat cíle, kterých má dosáhnout.
Robot Hexor II může být řízen s jakoukoliv výše popsanou mírou autonomie. V mé předchozí práci (magisterský projekt), byl pro robota vytvořen program pro plně autonomní řízení, kdy se robot byl schopen pohybovat bez kontaktu s překážkami podle předem definovaných algoritmů, s využitím pouze svých vlastních zdrojů výpočetního výkonu i snímání okolního světa a zpracování těchto naměřených hodnot přímo mikroprocesorem robota. Dále byl vyvinut SW pro plně teleoperované řízení robota, kdy obsluha pomocí bezdrátové komunikace řídí pohyb robota. Tato práce se zabývá řídícím systémem semiautonomním, kdy obsluha pomocí gest ruky zadává robotovi příkazy k pohybu, ale robot může v krajním případě odmítnout rozkaz vyplnit a to v případě, že by hrozila kolize s překážkou.
1.2 Kinematika mobilních robotů (1)
Základní vlastností všech mobilních robotů je tedy schopnost pohybu. Mobilní roboty lze podle možností pohybu rozdělit na roboty pohybující se po pevné podložce ve dvourozměrném prostoru a roboty pohybující se v prostoru trojrozměrném. Tato práce se zabývá pouze variantou první, tedy roboty pohybující se ve dvourozměrném prostředí.
Takové roboty je dále možné rozdělit na roboty kolové (pásové) a kráčející. Kolové
roboty mají jednoznačně nejlepší poměr vydané energie oproti vykonanému pohybu. Jsou
schopné vykonávat poměrně rychlé pohyby s možností plynulé regulace rychlosti směru i
vzdálenosti pohybu. Kolový robot může být „holonomický“, což znamená, že má řiditelný
2
stejný nebo vyšší počet stupňů volnosti, než je počet souřadnic nutných k popisu jeho polohy, což je například vozík se všemi řiditelnými koly. Takový robot je schopen měnit směr pohybu, aniž by výrazně měnil svou polohu. Oproti tomu robot „neholonomický“ může směr pohybu měnit pouze v kombinaci s jiným (např. dopředným) pohybem. Typickým příkladem takového pohybu je pohyb automobilu. Kolový robot je značně omezen v pohybu při nerovném terénu, tento nedostatek je možné částečně eliminovat požitím pásů.
Kráčející roboty mohou být holonomické i neholonomické, většinou jsou však holonomické, protože pro pohyb pomocí kráčejících nohou jsou nutné alespoň dva stupně volnosti pohybu (například zvednutí a pootočení). Kráčející roboty mohou využívat pohybu statického i dynamického. Statický pohyb znamená, že robot je neustále v kontaktu s podložkou alespoň třemi body, proto robot využívající k pohybu statickou chůzi musí mít nejméně čtyři nohy. Dynamická chůze (např. běh, skákání) může být oproti statické výrazně rychlejší, avšak je podstatně náročnější na řízení. Robot je při dynamické chůzi ohrožen pádem nebo ztrátou koordinace pohybu, a to jak vlivem vnitřním, například chybou programu, tak vlivem vnějším, například podklouznutí nebo nečekaná překážka. U dynamického pohybu může nastat situace, kdy robot není vůbec v kontaktu s podložkou, setrvačnost takového pohybu navíc způsobuje, že se robot stává neholonomický. Výhodou kráčejících robotů může být lepší prostupnost terénem, zejména u robotů specializovaných, nevýhodou je však neefektivní využití energie a nutnost precizního řízení pohybu a synchronizace pohybu jednotlivých kráčejících nohou robota.
Robot Hexor II je šestinohý kráčející robot, který používá chůzi se dvěma stupni volnosti. Prostřední noha robota nadzvedne a krajní nohy způsobí jeho pootočení. Pohyb robota Hexor II je tedy holonomický a statický, protože se robot může otáčet na místě a je neustále minimálně třemi nohami v kontaktu s podložkou.
3 2. Robot Hexor II
Robot Hexor II byl vyvinut na Silesian University of Technology ve městě Gliwice v Polsku, ve spolupráci s polskou firmou Stenzel. Robot je dodáván se softwarem, ale pouze přímo v souboru typu .HEX, takže tento software není možné modifikovat. Na přiloženém CD je alespoň podrobný popis softwaru, ze kterého je možné odvodit většinu fyzického zapojení jednotlivých komponent robota, protože žádný podrobný popis fyzického zapojení robota neexistuje. Z těchto důvodů je vývoj softwaru a hardwaru robota velice problematický a zdlouhavý.
2.1 Popis robota
Software robota navržený výrobcem má nevhodnou architekturu a jeho nová podoba je předmětem této práce. Při vývoji nového ovládacího softwaru bylo jednáno o spolupráci s výrobcem robota, konkrétně s Dr. Ing. Meciej Sajkowski, který se spolupodílel na výrobě robota Hexor II. I přes veškerou snahu však tato spolupráce nebyla příliš úspěšná, spíše kontraproduktivní, protože slíbená řešení problémů buď nebyla funkční, nebo nakonec žádná, ač byla přislíbena. Takové (i několikatýdenní) čekání od práce spíše zdržoval, protože veškeré problémy musely nakonec stejně být vyřešeny svépomocí, popřípadě s podporou vedoucího této práce panem Ing. Miroslavem Holadou, Ph.D. Slíbený popis dostupných vstupů a výstupů pro možnosti rozšíření a programování robota nakonec nebyl vytvořen i přes několik urgencí vůbec, chyba v komunikačním modulu sice byla odhalena, ale její oprava vyřadila
Obr. 1: Rozložení periferií na robotovi
4
modul z provozu úplně. Z tohoto důvodu musel být vytvořen komunikační modul úplně nový a bez jakékoliv dokumentace musel plně nahradit komunikační modul původní.
Procesor ATmega128 (2) (3):
Procesor ATmega128 je 8 bitový mikročip typu RISC a harvardskou architekturou.
Atmega128 je sice mikročip typu RISC, ale počtem 133 instrukcí se blíží procesorům typu CISC. Naprostá většina těchto instrukcí je jednotaktová. Může pracovat rychlostí až 16MIPS při 16MHz. V případě robota Hexor II je procesor taktován externím oscilátorem o frekvenci 14,7456MHz. Pouzdro tohoto procesoru má 64 vývodů, z toho 53 je možné použít jako vstupní nebo výstupní.
Atmega128 má tři vnitřní paměti.
a) 128KB flash paměti pro program: Tato paměť je energeticky nezávislá a výrobce udává, že při teplotě 25°C udrží program 100 let. V případě potřeby je možné ještě paměť pro program rozšířit o 64KB externí paměti. Flash paměť je možné přepsat 10000x.
b) 4KB paměti typu EEPROM: slouží pro uložení vnitřní konfigurace procesoru, například údaje o oscilátoru, způsob připojení a typ programovacího rozhraní. Jsou v ní uloženy data přijatá přes sběrnici I2C atd.
c) 4KB paměti typu SRAM: slouží jako operační paměť procesoru. Tato paměť je samozřejmě energeticky závislá.
Obr. 2: Vstupy a výstupy procesoru ATmega128 (3)
5
Procesor je poměrně dobře vybaven pro univerzální použití. Nejzajímavějším vybavením jsou 4x časovač s různou vybaveností, 2x UART pro komunikaci s počítačem, případně dalším procesorem, podpora sběrnic I2C a SPI, 8x AD převodník, rozhranní JTAG které podporuje debug přímo na procesoru, 8x PWM s různými parametry, vnitřní oscilátor, podpora 8 externích přerušení atd. Kompletní specifikace je k dispozici v datsheetu procesoru.
Hardware robota:
Robot Hexor II je od výrobce osazen infrasenzory, hmatovými senzory s mikrospínači pro detekci překážek při chůzi, ultrazvukovým senzorem pro měření vzdálenosti překážek, pohyblivou kamerou pro přenos obrazu do počítače a modulem pro radiovou komunikaci na frekvenci 433MHz (4).
Infrasenzory používají pro svůj provoz vlastní obvod řízený mikroprocesorem ATmega8, který obsahuje čtyři emitory a tři senzory. Emitory vysílají záření o frekvenci 38kHz s čtvercovým průběhem. Senzory snímají odrazy od překážek ve vzdálenosti cca 20 cm, což je dostačující pro to, aby do nich robot nenaboural, a zároveň nesnímají příliš vzdálené překážky, které by zbytečně omezovaly pohyb robota. Jejich řešení však není dostatečně robustní a jejich ladění je značně problematické. Bylo nutné během snímání přidat dodatečná zpoždění, protože docházelo k snímání neexistujících překážek. Infrasenzory není možné určit vzdálenost překážky, pouze její detekci.
Hmatové senzory (dále tykadla) při nárazu na překážku aktivují mikrospínač, který je připojen na vstup mikroprocesoru ATmega128. Tykadla pomáhají zachytit překážky, které není možné detekovat předním infrasenzorem. Nevýhodou je, že není možné rozlišit mezi nárazem tykadla na překážku zepředu a zezadu (například při otáčení robota).
Měření vzdálenosti je realizováno ultrazvukovým modulem SRF05, který je sériově vyráběn firmou Devantech a je běžně komerčně dostupný. Sonar emituje zvukový signál o frekvenci 40Khz (4) a měří dobu jeho návratu zpět po odrazu od překážky. Senzor i emitor je samostatně pohyblivý horizontálně i vertikálně v rozsahu +-90°. Měří vzdálenost v rozsahu 3cm až 3m s přesností 1cm. V praxi však přesnost sonaru bude pravděpodobně větší a je předmětem dalšího testování.
CCD kamera s mikrofonem není součástí systému řízeného mikroprocesorem, ten řídí
pouze její pohyb. Komunikace kamery s počítačem funguje samostatně na frekvenci 2,4GHz
(4). K přenosu videa do počítače bylo nutné zakoupit USB převaděč z S-Video výstupu
přijímače. Součástí balení robota byla pouze PCI karta, která je však pro mobilní použití
nepoužitelná.
6
Rádiový modul umožňuje komunikaci robota s počítačem. Modul pracuje na frekvenci 433Mhz. Rádiová komunikace probíhá přes virtuální sériový port, který je zapojen do USB počítače. Tento modul byl vyvinut úplně nový, původní komunikační modul byl nevyhovující.
Robot je napájen gelovo-olověným akumulátorem o kapacitě 3,2 Ah a napětí 6V. Tato baterie umožňuje 1,5 až 3 hodiny provozu. Výrobce udává (4), že akumulátor je plně nabit za 1 hodinu, avšak v praxi je doba nabíjení přiloženou nabíječkou podstatně delší, což vychází i z maximálního proudu 1,2A, který může přiložená nabíječka dodat. Při delších prezentacích je rychlost nabíjení podstatná a z toho důvodu byla vyvinuta rychlejší nabíječka, využívající pulsní zdroj z nabíječky notebooku, který dokáže dodat při plném vybití baterie nabíjecí proud až 3 - 4A.
2.2 Režimy použití
Robot pracuje ve dvou základních režimech. Samostatný bez vnějších zásahů do běhu programu, nebo je ovládaný přes rádiovou komunikaci počítačem. Pokud robot pracuje samostatně, je limitován svým hardwarem. Mikroprocesor ATmega 128 není schopen zpracovávat ani ukládat mnoho dat. Z toho důvodu není možné aplikovat složitější algoritmy, například zpracování obrazu nebo zvuku a využití těchto dat pro následné řízení programu.
Velkou výhodou je jednoduchá obsluha, kdy stačí robota zapnout a ten se již sám začne řízeně pohybovat, navíc není nutný k robotovi žádný další hardware. Rozšíření softwaru pro autonomní pohyb robota bylo předmětem mého magisterského projektu.
Pokud však potřebujeme větší výpočetní výkon nebo velkokapacitní paměť, je možné
odeslat data pro zpracování do počítače, který je nezávisle zpracuje a odešle robotovi pouze
příkaz, jak se má zachovat. Typickým příkladem je zpracování obrazu pořízeného kamerou
robota počítačem, který poté zasílá robotovi příkazy. Toto spojení je možné snadno využívat i
pro jiná zařízení, než je robot Hexor II. Levné zařízení řízené procesorem o ceně v řádu
jednotek stokorun tak získá velikou výpočetní sílu univerzálního počítače, který i v podobě
mobilního notebooku již většina lidí vlastní. Další výhodou tohoto distribuovaného výkonu je
malá energetická náročnost robota proti robotům využívajícím vlastní výpočetní výkon. Toto
snížení energetické náročnosti robota se netýká pouze snížení spotřeby energie řídícím
modulem, ale tím pádem nutností možností využití menšího zdroje energie, s čímž souvisí
nižší hmotnost celého robota. S nižší hmotností robota není potřeba tak masivní tělo robota a
7
nejsou potřeba tak silné pohonné jednotky robota, čímž se hmotnost, energetická náročnost i náročnost výroby robota snižuje ještě více.
V rámci této práce byl vyvinut univerzální software, který příkladně využívá právě distribuovaného výkonu osobního počítače softwarem za pomoci rychlé komunikace a výkonu vlastního mikroprocesoru robota. Z ekonomického hlediska je velice efektivní vyrábět právě takové roboty, kde se využívá výpočetní výkon univerzálního osobního počítače.
Takový robot je levný a jednoduchý na výrobu, a aniž by uživatele (a tím pádem i potencionálního zákazníka) nutil investovat další prostředky na nějaké drahé obslužné zařízení, tak může vykonávat složité úlohy. Osobní mobilní počítač je dnes tak rozšířenou věcí, že není nutné drahý a energeticky náročný výpočetní výkon mobilním robotům přímo implementovat. Nutnost vlastnit počítač pro provoz takových zařízení tedy není překážkou.
Osbobní počítače (popř. chytré telefony, tablety atd.) jsou uživatelsky přívětivé a nabízejí oproti případnému vlastnímu ovládacímu modulu dobré grafické prostředí a rozmanité ovládací prvky (případně externí jako je např. joystick).
Tento model s distribuovaným výpočetním výkonem zatím není příliš rozšířen a výrobci se snaží vymýšlet vlastní autonomní zařízení, avšak s rozvojem výkonných a levných mikroprocesorů podobných jako je řada AVR od firmy Atmel a dnešní přívětivosti jejich programování, je možné předpokládat, že tento trend se bude rozšiřovat. Potom by bylo běžné například koupit si RC model auta či vrtulníku, a ovládat ho pomocí joysticku, připojeném k osobnímu počítači, nebo pomocí chytrého telefonu. Takovým způsobem by bylo možné ovládat i například průmyslové stroje či domácí spotřebiče. Zařízením s centrálním řízením a distribuovaným výpočetním výkonem by se otevřely nové možnosti řízení a plánování, zatímco jejich cena a složitost výroby a vývoje by se vlastně snížila.
3. Použité technologie
3.1 Použité technologie pro programování
Robot je programován v jazyce C++ a o převod programu do strojového kódu
procesoru ATmega128 se stará aktuálně nejnovější překladač Atmel Studio 6.0. Aby bylo
možné používat tento překladač, bylo nutné zakoupit nový programátor AVR Dragon USB,
namísto původního IsoJTAG ISP, který pracuje na principu sériové linky. Nové překladače od
firmy Atmel tento systém již nepodporují. Hlavním důvodem přechodu na nový překladač je
možnost nahlížet přes grafické rozhraní přímo do procesoru. Laděním programu se tím stává
podstatně přehlednější.
8
Program pro vzdálené řízení robota je vytvořen ve Visual Studiu 2010 a je v jazyce C++.
3.2 Robot Hexor II Pohyb robota:
O pohyb robota se starají modelářská analogová serva od výrobce Hitec. Výrobce v manuálu robota neuvádí fyzické zapojení ovládacích vodičů jednotlivých servomotorů k procesoru. Jednotlivá serva jsou připojena:
levé otočné nohy – pine.3 pravé otočné nohy – pinb.5 středové nohy – pine.4 kamera horizontálně – pine.5 kamera vertikálně – pinb.5
Obr. 3: Rozsah pohybu sonaru a kamery (4)
Ovládání a čtení infraportů:
Příslušné bity portu A nastavíme jako výstupní a nastavíme je na jedničku. Na Portg.3 přivedeme signál trvající 1µs, tím se aktivuje emitor a vyzáří signál o frekvenci 38kHz. O tvorbu tohoto 38kHz signálu se stará modul infransenzorů řízený procesorem ATmega8. Poté aktivujeme Portg.4 a výstupní port A nastavíme na vstupní pin A a čteme jeho příslušný bit.
Po uplynutí 1µs opět Portg.4 deaktivujeme. Měření na jednotlivých cenzorech je prováděno
postupně s odstupem 300µs.
9 Fyzické zapojení jednotlivých senzorů k portu/pinu A:
pina.1 - přední pina.3 – levý zadní
pina.0 – středolevý, středopravý zadní pina.2 – pravý zadní
Středolevý a středopravý zadní senzor je sice připojen na stejný port, měří se ale v jiném čase [Obr. 5], proto je také možné je jednoznačně identifikovat.
Obr. 4: Fyzické propojení infrsenzorů s mikroprocesorem (5)
Obr. 5: Postup čtení zadních infrasenzorů (5)
10 Sonar:
Čtvrtý bit portu C je připojen ke vstupu modulu sonaru SRF05, sedmý bit portu E je připojen na výstup modulu sonaru. Na vstup sonaru odešleme 15µs impuls, tím se inicializuje emitor sonaru, který odešle osm period ultrazvuku o frekvenci 40Khz. O modulaci ultrazvuku se stará modul SRF05 sám. Poté se vyvolá zpoždění 1ms, což je doba náběhu měření sonaru.
Toto zpoždění je pro přehlednost realizováno pouze 1ms trvajícím for cyklem. V této době se sedmý bit pinu E překlopí do jedničky a zůstane v ní, dokud modul SRF05 nezachytí ozvěnu, nebo nevyprší časový limit. Poté se výstup z modulu STF05 překlopí zpět na nulu. Doba, po kterou je Porte.7 v jedničce, je pak přímo úměrná vzdálenosti měřené překážky, nebo maximálně 15ms.
Obr. 6: Zapojení sonaru (5)
Obr. 7: Funkce sonaru(5)
11 Hmatové senzory:
Čtvrtý bit portu G se nastaví na jedničku, čímž aktivujeme měřící obvod. Poté je testován šestý bit pinu A pro pravé tykadlo a sedmý bit pinu A pro levé tykadlo. Pokud je na těchto vstupech přečtena jednička, robot narazil tykadlem na překážku. Robot není schopen rozeznat směr nárazu tykadla na překážku.
Obr. 8: Fyzické propojení hmatových senzorů s mikroprocesorem(5)
Obr. 9: Funkce hmatových senzorů (4)
4. Program pro řízení robota
4.1 Handtracking Fyzické řešení:
Původní záměr byl realizace handtrackingu s detekcí lidské ruky. To je však velice
obtížné, zejména kvůli mobilnímu použití robota s nestálými podmínkami osvětlení a
okolního prostředí. Barva ruky se s rozdílnou světelnou intenzitou mění od černé po bílou
díky přímým odrazům, vysokému kontrastu a nekvalitní kameře robota. Tyto nepříjemné jevy
jsou částečně eliminovány použitím červené rukavice a realizací vlastního osvětlení přímo u
pohyblivé kamery robota vysoce svítivou LED diodou s regulací výkonu.
12 Softwarové řešení:
Inspirací pro algoritmus segmentace ruky od zbytku obrazu byl článek (6). Z tohoto postupu bylo vybráno:
a) Použití barevného prostoru YCbCr [Obr. 10], kvůli lepší detekci červené barvy rukavice.
b) použití Gaussova rozložení pro vyhlazení prahovaného obrazu a interpolaci výsledné kontury ruky.
Navíc byl tento algoritmus doplněn o adaptabilní vícestupňové prahování pro dosažení větší robustnosti vůči změnám osvětlení a prostředí. Pro další zpracování obrazu byl inspirací článek (7), který se zabývá trojrozměrným zobrazením ruky, ale i pro 3D zobrazení je potřeba nejdříve zpracovat 2D obraz. Z tohoto článku je převzato použití:
a) Convex hull [Obr. 17]
b) Convexity defects [Obr. 18].
Algoritmus pro další zpracování obrazu, tzn. detekce natočení ruky a jednotlivých prstů, stejně jako adaptabilní vícestupňové prahování je vlastním dílem autora této práce.
Obr. 10: Cr složka obrazu
Obraz je nejprve prahován metodou Thresh to zero [Chyba! Chybný odkaz na záložku.],
která odstraní nevýrazná místa a zvýrazní světlá místa, a poté klasickým binárním prahem
[Obr. 12]. Hodnota prahu se přizpůsobuje aktuálním světelným podmínkám a barvě pozadí
(bude vysvětleno níže).
13
Obr. 11: Thresh to zero (vlevo)
Obr. 12: Binary threshold (vpravo)
Pomocí eroze a dilatace [Obr. 14] je z obrazu odstraněn případný jemný šum. Pokud by malých objektů bylo příliš mnoho, mohlo by to program zpomalovat, protože každý objekt bude dále samostatně zpracováván. Ostré a nepravidelné hrany obrazu jsou vyhlazeny pomocí lineární konvoluce s Gaussovým rozložením (Gaussian kernel), čímž se výrazně sníží náročnost pro pozdější aproximaci kontury obrazu.
Pro Gaussovo rozložení platí (8):
V tomto případě byla použita matice velikosti 9x9, to znamená x,y = 9. Hodnota σ je směrodatná odchylka rozdělení a její hodnota je vypočtena pomocí (9):
Kde n = 9, to znamená v tomto případě je hodnota σ = 1,85. Tato hodnota je doporučena v manuálu knihovny openCV pro dosažení maximální efektivity výpočtu algoritmu.
Obr. 13 :Ukázka Gaussova rozložení (9)
14
Hrany obrazu se vyhlazením lehce rozmažou, proto je nutné provést znovu binární prahování [Obr. 15].
Obr. 14: Eroze a dilatace (vlevo)
Obr. 15: Vyhlazení hran pomocí Gaussova rozložení (vpravo)
V obraze se všechny objekty označí konturou [Obr. 16] a spočte se jejich velikost v pixelech. Pokud jsou objekty menší než 5800 pixelů (což je přibližně prostor 100 x 100 pixelů), obarví se černou barvou (malý objekt vedle palce na [Obr. 16]) a dále se ignorují.
Pokud je nalezen objekt o větší velikosti než 5800 pixelů, obarví se na zeleno a předá se dalšímu zpracování. Pokud je nalezen objekt o větší velikosti než 5800 pixelů, obarví se na zeleno a předá se dalšímu zpracování.
Kontura velkého objektu je aproximována metodou Douglas-Peucker, která vynechá méně významné body křivky, ale zároveň ty významné zachovává, takže výsledný tvar ruky příliš nedeformuje, což je patrné z [Obr. 17], ale výrazně zjednodušuje její reprezentaci a tím zrychluje běh programu. Dále bude „velký objekt“ nazýván „rukou“
Do obrazu [Obr. 17] je vykreslen Convex hull (vnější obrys nejvzdálenějších bodů ruky.)
15
Obr. 16: Vykreslení kontury Obr. 17: Convex hull
Metodou Convexity defects jsou nalezeny lokální maxima neboli „start point“
(červený bod), což jsou body, kde se kontura ruky dotýká Convex hull křivky a lokální minima (modrý bod) neboli „depth points“, což jsou nejhlubší body mezi dvěma maximy [Obr. 18]. Tyto body jsou pospojovány úsečkami. Podle úhlů, které svírají úsečky mezi sebou ve start pointech (červená čísla na [Obr. 19]), je možné jednoznačně identifikovat natočení ruky. Pokud je nalezen úhel větší než 60° v deseti snímcích po sobě bez přerušení (velký červený bod na [Obr. 20]), je jednoznačně identifikována poloha ruky, a tím pádem i poloha palce ruky. Úhel, který má palec vůči těžišti ruky, se nastaví jako nulový, a tím je dokončena fáze učení programu. V praxi se toto řešení osvědčilo.
Obr. 18: Convexity defect
Obr. 19: Pomocné úsečky, kružnice a úhly
V každém dalším snímku se ukládá nová poloha palce vůči těžišti ruky, a tím se
poloha ruky aktualizuje. Výhodou tohoto řešení je nezávislost na vzdálenosti (tzn. velikosti)
16
ruky a i při velmi rychlém pohybu, při alespoň přibližném zachování natočení ruky, jsou prsty opět okamžitě identifikovány. Nevýhodou je, že pokud se ztratí ruka z obrazu, musí se vrátit v podobném úhlu, jinak program selhává. To je ošetřeno tím, že ve chvíli, kdy se v obraze objeví dva palce (což se nikdy nemůže stát, pokud je identifikován opravdu palec, protože ostatní body jsou vzdálené téměř 90° na každou stranu), tak se znovu spustí metoda pro určení polohy ruky.
Algoritmus pro adaptabilní prahování je založen na bázi kontur. Program odečte hodnotu 15 od mezní hodnoty prahování a uloží si aktuální hodnotu prahu, velikost velkého objektu, počet velkých objektů a počet malých objektů. Mezní hodnotu prahu zvýší o 1 a proces opakuje 30krát. Z naměřených hodnot vybere takovou hodnotu, která má maximální velikost velkého objektu, minimální počet malých objektů a zároveň nemá více než jeden velký objekt. Tuto proceduru provede program vždy po startu a pokaždé je ukončena novým výpočtem polohy ruky.
Pokud je nalezen více než jeden velký objekt, není nalezen žádný objekt nebo pokud je nalezeno příliš mnoho malých objektů v průběhu snímání, inkrementuje se čítač pro testování kvality obrazu. Pokud je obraz v pořádku (jeden velký objekt a prsty nalezeny), čítač se dekrementuje. Pokud program identifikuje 10 špatných snímků za sebou, což zapříčiní změna pozadí nebo osvětlení, spustí se znovu algoritmus pro výpočet mezní hodnoty prahování a polohu ruky.
Jednoznačná identifikace prstů je demonstrována na [Obr. 21]. Červená čísla u konců prstů jsou úhly kružnice se středem v těžišti a nulovým bodem v oblasti palce (žlutý kroužek).
Program zároveň zjišťuje počet prstů a natočení ruky, což je úhel protínající prostor přibližně mezi ukazováčkem a prsteníčkem.
Obr. 20: Identifikace prsů Obr. 21: Reálný obraz
17
Při psaní programu byl kladen velký důraz na jeho robustnost, protože není jeho cílem pouhá demonstrace funkčnosti v rámci této práce, ale i reálné použití na předváděcích akcích FM TUL. Adaptabilní prahování obrazu pomocí kontur se v praxi ukazuje jako velmi výhodné řešení. Stejný důraz byl i kladen na maximální efektivitu programu a podařilo se jeho náročnost snížit natolik, že obraz snímaný rychlostí 25fps při rozlišení 640x480 pixelů běží i na méně výkonném počítači naprosto plynule a nedochází ke zpoždění obrazu či jeho zasekávání. Toho bylo docíleno maximálním využitím možností OpenCV knihovny a optimalizací kódu, jednotlivých křivek a objektů.
Testování v reálných podmínkách:
V rámci testování v reálných podmínkách se jako nejproblematičtější článek řetězce pro zpracování obrazu ukázala nekvalitní kamera použitá na robotovi. Tato kamera velmi špatně snímá barevné rozdíly, navíc robot snímá obraz od země proti zdrojům světla, což způsobuje tak velký kontrast, že obraz úplně ztratí barevnost, která je pro tento program nezbytná. Tento nedostatek je částečně vyřešen přidáním silnějšího světelného zdroje, není však vhodné robota provozovat proti silným zdrojům světla. Ukázka zpracování obrazu při mírném osvětlení je na [Obr. 22], kde je jasně patrný obrys ruky včetně všech detailů. Obraz je v tomto případě snímán plynule a reakce robota na ruku jsou rychlé a bezchybné.
Obr. 22: Detekce ruky kamerou robota při nižším osvětlení (60W žárovka ve stropním svítidle)
18
Pokud je kamera namířena přímo proti zdroji světla [Obr. 23], tak je díky adaptabilnímu prahování stále detekce ruky možná, ale je nutné počítat s vyšší chybovostí programu. Je také nutné provádět pohyby rukou pomaleji, protože rychlý pohyb obraz více rozmazává. Důvod zhoršení detekce je dán nutností programu zvýšit práh pro detekci ruky (na výše uvedených snímcích je to hodnota „actual thresh“), protože jsou snímány další světlé objekty a navíc se sníží červená složka v části obrazu, kde se ruka opravdu nachází.
Obr. 23: Detekce ruky kamerou robota přímo proti zdroji světla (60W žárovka ve stropním svítidle)
Obraz lze jen obtížně zpracovávat venku proti dennímu světlu a obloze a problémy mohou způsobit i silné zdroje světla v místnosti (např. řada zářivek). Menší zdroje světla (jedna žárovka) je program schopen odfiltrovat, ale stoupá jeho chybovost, protože obraz ruky není ostrý, a objevují se nechtěné artefakty.
4.2 Zpracování naměřených dat a synchronizace
Program zpracovává obraz v reálném čase. Není možné zpracovávat data z každého
snímku a brát každý výsledek měření jako směrodatný. Z tohoto důvodu jsou výsledky
nejdříve průměrovány pomocí FIFO zásobníku. Výsledek je vložen na konec fronty a
s každým snímkem je posouván dále. Pokud se zásobník s pevnou délkou naplní celý
naměřenými hodnotami, všechny jeho hodnoty se zprůměrují a tento průměr je teprve brán za
naměřenou hodnotu. To je také patrné na [Obr. 22] a [Obr. 23], kde je jiná hodnota úhlu ruky
19
v TextBoxu vespod obrázku a přímo v obraze. V obraze je hodnota pro aktuální snímek a v TextBoxu je hodnota průměrovaná.
Filtrování naměřených hodnot:
Zásobník pro výpočet úhlu ruky má 20 prvků a zpracovává údaje z každého snímku, takže výsledek měření úhlu ruky je k dispozici se zpožděním necelou jednu vteřinu po spuštění handtrackingu, a je plynule s malým zpožděním dopočítáván. Výsledek tohoto měření přímo uděluje rozkazy pro pohyb robota, proto je zde kladen menší důraz na rychlost a větší důraz na přesnost. Robotovi trvá 600ms, než udělá další krok. Každý předchozí krok musí dokončit. Proto zde malé zpoždění nehraje velkou roli, ale případné chybné vyhodnocení úhlu ruky by vyvolalo samovolný pohyb robota. Samovolný nechtěný pohyb robota je věc nežádoucí a v celém algoritmu řízení robota je nejvyšší prioritou se tomuto jevu nejlépe úplně vyhnout, i za cenu zpoždění při jeho ovládání. Toto zpoždění se však podařilo snížit na velmi přijatelnou úroveň. Stejný počet prvků (20) má i zásobník pro změřený počet prstů, protože počet prstů je také směrodatný pro rozkazy posílané do robota.
Hodnota těžistě nalezeného objektu (na [Obr. 22] a [Obr. 23] znázorněno černým x a hodnotou X a Y dole v TextBoxech) byla původně průměrována pouze z posledních dvou obrázků. Tato hodnota slouží pro úpravu směru kamery při pohybu robota a ruky. U této hodnoty je velice důležitá rychlost zpracování aktuálních dat a díky průměrování se stávalo, že se robot snažil zaměřit ruku na místech, kde se již nenachází. Z tohoto důvodu bylo od průměrování těžiště ustoupeno a pro hlídání těžiště ruky jsou použita aktuální data z posledního jednoho snímku. Chybně změřená data a následný chybný příkaz jsou sice také nežádoucím jevem, ale následující rozkaz je schopen předchozí chybu opravit a nedochází zde běžně k fatálním chybám, které by například mohly způsobit ztrátu ruky v obraze. Program do robota posílá aktuální údaje o poloze ruky s periodou 34ms, což umožňuje téměř plynulý pohyb, a tato rychlost je na samé hranici technických možností robota, protože kamera snímá obraz rychlostí 30 snímků (tedy každých 33ms) a vyhodnocovat data z jednoho snímku vícekrát je nežádoucí. Procesor ATmega128 a nový komunikační modul zvládá i vyšší rychlosti přenosu (úspěšně testováno i pro rychlost 9ms). Při těchto teoretických rychlostech použitelných například pro vysokorychlostní kameru již zpracování přijatých dat ohrožuje plynulý běh programu robota. Rychlost 34ms je navíc zvolena tak, aby co nejméně kolidovalo přijetí a zpracování rámce s ostatními procesy robota a navíc v různých místech programu.
Pokud by byla zvolena rychlost např. 40ms, narušovalo by přijetí rámce každou druhou
periodu pro obsluhu servomotorů, a to ve stejném okamžiku dané periody. Při testování se
20
tato teorie potvrdila, dokonce kromě občasných chyb a nepravidelností při pohybu kamery se občas i resetoval mikroprocesor. Pohyb servomotorů však i tak byl plynulý a bezchybný.
Sonar robota provádí měření stále, s periodou 0,8s. Každý výsledek měření posílá do počítače. Tento výsledek je také průměrován v zásobníku o patnácti prvcích. Takto velký zásobník sice znamená zpoždění při startu programu, ale je nežádoucí, aby jediné špatné měření způsobilo zastavení programu (bude popsáno níže). Následný opětovný start programu by způsobil zpoždění cca 1,5 – 3s. Chybné měření sonaru bývá způsobeno rychlým pohybem ruky, kdy sonar změří vzdálenost přesně v okamžiku, kdy se ještě poloha kamery nestihla zaktualizovat podle nové polohy ruky. Dvě či tři špatná měření bývají v reálném provozu eliminována a nezpůsobují zastavení programu. Zároveň je zpoždění pro start programu přijatelné.
4.3 Hlavní okno programu pro ovládání robota
Po spuštění robota se zobrazí hlavní okno programu. Začneme tím, že vybereme sériový port komunikačního modulu (v našem případě COM5) a přenosovou rychlost 9600Bd, poté potvrdíme tlačítkem připojit. [Obr. 24]
Obr. 24: Hlavní okno programu po spuštění
Pokud se program úspěšně připojí k sériovému portu, tak se aktivují ostatní ovládací
prvky, poté je možné robotem ručně pohybovat pomocí tlačítek pro ovládání směru chůze a
pohyb kamery [Obr.25].
21
Obr. 25: Hlavní okno programu po připojení sériového portu pro komunikaci
4.4 Hlavní algoritmus
K aktivaci handtrackingu je potřeba stlačit tlačítko „handtrack start“. Tím zezelená první indikátor a program začne měřit pomocí údajů ze sonaru robota, jestli se nachází ve vzdálenosti 40 – 100cm nějaký objekt. Pokud se v uvedené vzdálenosti a přímém směru od kamery nic nenachází, program čeká. Pokud sonar změří přítomnost objektu, spustí se zpracování obrazu pomocí handtrackingu a zezelená druhý indikátor. Handtracking začne prohledávat obraz v rozsahu adaptabilního prahování a hledá právě jeden červený objekt.
Pokud ho najde, změří pomocí algoritmu popsaném v kapitole 4.2 Handtracking, jestli má nalezený objekt tvar ruky. Pokud tvar objektu odpovídá kritériím, zezelená i třetí indikátor a spustí se komunikace mezi robotem a počítačem.
Pokud jsou splněny všechny podmínky pro běh handtrackingu, program odesílá každých 29ms do robota údaj o přesné poloze těžistě ruky. Podle těchto údajů si robot v reálném čase udržuje i při pohybu ruku v zorném poli. Zároveň je odesílán s periodou 400ms rozkaz ke směru chůze robota podle gest ruky a jejího natočení. Jeden krok robota trvá cca 0,5s a po každém kroku je směr chůze nulován. Toto opatření zabezpečuje, že se robot nebude pohybovat, pokud není jasně detekováno gesto ruky s příslušným rozkazem. Každý krok tedy musí být podložen aktuálním rozkazem z počítače.
V případě že kterékoliv ze tří výše uvedených kritérií pro běh programu (tlačítko
handtrack start, měření sonaru, ruka identifikovaná v obraze) přestane být aktuální, příslušný
indikátor opět zčervená a program se zastaví. V tomto případě program čeká, dokud příslušná
podmínka pro jeho spuštění je opět splněna. Po zastavení programu z kteréhokoliv důvodu je
robotovi odeslán údaj o chybě měření, který vrátí polohu kamery do počátečního stavu.
22
V hlavním programu jsou vynulovány všechny zásobníky a měření všech údajů začíná od začátku.
4.5 Povely a běh programu v reálném prostředí
Robot (resp. počítač) rozpoznává a používá ke svému řízení šest různých povelů a polohu těžiště ruky. Jednotlivé povely jsou: doprava, doleva, rovně, stůj a dozadu. Těžiště ruky robot zpracovává s každým snímkem a slouží pro udržení ruky v zorném úhlu kamery robota. Robot se při pohybu naklání a provádí škubavé pohyby, bez rychlého vyrovnávání těchto pohybů kamerou by detekce ruky v obraze robota nebyla možná vůbec.
Pro demonstraci robustnosti programu jsou screenshoty pořízeny v běžném, tedy spíše nevhodném prostředí - za běžného denního světla (cca 15:00) a zároveň v některých případech i proti oknu. Navíc je na screenschotech patrné odlišení ruky proti složitému pozadí, například osvětlený obličej nebo roh skříně, který také obsahuje červenou barvu.
Mohlo by se zdát že barevný rozdíl mezi rohem skříně a červenou rukavicí je veliký, ale při snímání nekvalitní kamerou robota a ještě k tomu proti zdroji světla jsou rozdíly v barevnosti opravdu minimální, přesto je však program pro handtracking dokáže využít s poměrně vysokou spolehlivostí.
Program měří dle výše uvedených pravidel hodnoty ukázané na [Obr. 26]. Hodnota Sonar je vzdálenost ruky v centimetrech. Pro spuštění handtrackingu musí být tato hodnota v rozmezí 40-100cm. Ruka v této vzdálenosti je detekovatelná, navíc je tímto opatřením jasné, že robot nebude sledovat nějaký červený vzdálený objekt např. na stropě. Další údaj udává počet prstů. Tento údaj nemusí souhlasit s hodnotou „Finger count“ uvedenou přímo v obraze, protože u hodnoty „Finger count“ se jedná o hodnotu v konkrétním snímku, zatímco u hodnoty Prsty se jedná o hodnotu průměrovanou. To samé platí u hodnot „hand angle“ a Uhel. Hodnoty X a Y udávají polohu těžiště ruky a hodnota size of hand udává počet pixelů v objektu ruky v aktuálním snímku.
Obr. 26: Detail naměřených údajů při spuštěném handtrackingu
Povel doleva je odeslán v případě, že ruka je natočena v úhlu větším než 40° a
zároveň je detekováno 4 a více prstů. Hodnota „actual thresh“ v obraze udává aktuální
hodnotu prahu pro detekci ruky. Ze screenshotů je patrné, že tato hodnota se při běhu
23
programu mění dle výše popsaných algoritmů v závislosti na aktuálních světelných podmínkách.
Obr. 27: Povel doleva (obraz z kamery je kvůli otočení o 180° zrcadlově otočen)
Povel doprava je odeslán v případě, že ruka je natočena v úhlu menším než -40° a zároveň je detekováno 4 a více prstů. Na obrázku [Obr. 28] je patrné roztřesení obrazu proti obličeji a zbytku ruky. Tento jev je způsoben malými rozdíly v červené složce obrazu kvůli vysokému kontrastu proti oknu za denního světla. Je zde také patrné, že jsou detekovány pouze 4 prsty, i když ruka jich ukazuje pět. V tomto případě však program ještě dokáže gesta ruky poměrně spolehlivě detekovat.
Obr. 28: Povel doprava (obraz z kamery je kvůli otočení o 180° zrcadlově otočen)
24
Povel rovně je odeslán v případě, že ruka je natočena v úhlu menším než 40°, větším než -40° a zároveň je detekováno 4 a více prstů. Na obrázku [Obr. 29] je dobře vidět v horní části černé artefakty. Takto jsou označena objekty, které jsou příliš velké na to, aby se odfiltrovaly, ale příliš malé na to, aby mohly být rukou. Počet těchto černých objektů a jejich velikost jsou použity pro nastavení hodnoty prahu v automatickém adaptabilním prahování.
Pokud by tento černý objekt byl příliš velký, program by hodnotu prahu snížil. (Podrobně popsáno v kapitole 4.2 Handtracking.)
Obr. 29: Povel jdi rovně
Povel stůj se provede, pokud ruka ukazuje právě tři prsty. Tento počet prstů je vybrán z toho důvodu, že jej lze snadno detekovat i při zhoršeném obraze, protože prsty jsou daleko od sebe.
Obr. 30: Povel stůj
25
Povel jdi dozadu je odeslán v případě, že ruka ukazuje jeden nebo dva prsty, ale zároveň je její úhel větší než -25° a menší než 25°. Hodnota úhlu ruky je k dispozici, pouze pokud je ruka správně zaměřena.
Obr. 31: Povel jdi dozadu
26 5. Software robota
5.1 Popis řešení dle výrobce robota
Robot Hexor je dodáván s návodem na tvorbu SW, který definuje jeho architekturu.
Tento návod byl použit v předchozí práci autora a pro použití v režimu samostatné chůze byl dostačující. Největším problémem původního řešení byla nutnost čekání procesoru na dokončení pohybu každého servomotoru. Servomotory se ovládají signálem popsaným na [Obr. 32]. Ovládací elektronika servomotoru měří dobu, kdy je na signálovém ovládacím vstupu napětí 5V. Pokud chceme natočit servomotor do polohy 90 stupňů, pošleme ovládací vstup signál 5V o délce trvání 1,5ms a opakujeme jej periodicky po 20ms. Toto bylo docíleno použitím dvou časovačů a jejich Output compare registrů (OCR).
Obr. 32: Ovládání natočení servomotoru
Procesor porovnával obsah OCR registru s hodnotou časovače [
Obr. 33] a v případě shody TCNT counteru a hodnoty OCR registru vyvolá přerušení, ve kterém výstup pro servomotor vynuluje. Tím se tvořil výstup pro ovládání servomotorů.
Nevýhodou tohoto řešení však bylo, že nebylo možné řídit efektivní využití procesoru a zároveň synchronizovat pohyb několika servomotorů.
Bylo nutné použití dvou časovačů z toho důvodu, že časovač 1 (Timer Counter 1 – TCR1) procesoru ATmega128 má k dispozici dva OCR registry a časovač 3 (TCR3) má k dispozici tři OCR registry. Ostatní dva časovače procesoru OCR registry nemají. Tím se tedy vyčerpají všechny OCR registry procesoru.
Oba časovače čítaly stejnou hodnotu a měřily stejný časový úsek, tedy periodu 20ms
pro řízení servomotorů. Toto řešení způsobovalo neefektivitu v rámci využití zdrojů
27
procesoru. Dva časovače čítající totéž a zároveň obsazení všech OCR registrů, které má ATmega128 k dispozici.
Problém se synchronizací pohybu nohou: Původní řešení problém synchronizace chůze řešilo nic nedělajícím for cyklem, který na dobu cca 0,5s zaneprázdnil procesor (v podstatě zastavil vykonávání dalšího kódu). V tuto dobu nebylo možné provést žádný další úkol a bylo nutné vždy čekat na dokončení kroku. Jediný využitelný čas procesoru byl okamžik mezi dvěma kroky. Robot tedy nebyl schopen ovládat pohyb kamery, snímat hodnoty na senzorech a sonaru během chůze.
Obr. 33: Schéma Output Compare registru (2)
5.2 Návrh nové architektury SW robota
Cílem nového řídícího SW robota je především optimalizovat zdroje nutné pro běh programu, ale zároveň běh programu zpřehlednit zrychlit a umožnit lépe synchronizovat jednotlivé procesy.
Nový software využívá pro obsluhu servomotorů pouze jeden časovač TCR1. a jeden
OCR registr. Architektura hlavní smyčky programu je znázorněna na [Obr. 34]. Časovač
TCR1 počítá od 0 do 36863 a tuto hodnotu ukládá do registru TCNT1. Pokud je hodnota
TCNT1 v rozmezí 1200 – 4000, probíhá obsluha servomotorů. Pokud je však v rozmezí 0-
1199 a 4001-36863, processor je volně k dispozici a může zpracovávat jakýkoliv kód. Z toho
vyplývá, že obsluha servomotorů zabere pouze cca 7,5% času mikroprocesoru a navíc by jich
procesor mohl v tomto časovém úseku obsloužit i více než 5. Je možné zbylé časové kvantum
rozdělit na libovolně velké bloky pro obsluhu jednotlivých částí programu, čímž vlastně
28
vytvoříme jednoduchou formu prioritního multitaskingu. Přetečení časovače TCR1 se opakuje s periodou 20ms.
Pokud by však vykonání algoritmu v hlavní smyčce trvalo déle než 20ms, je zde pro jistotu zavedeno přerušení při hodnotě TCNT1 = 1200 realizované pomocí OCR1A registru.
Toto přerušení zajišťuje spolehlivost obsluhy servomotorů i při velkém zatížení mikroprocesoru. Po vyvolání tohoto přerušení se odebere cca 2ms času procesoru pro obsluhu všech servomotorů. Po vykonání obsluhy servomotorů se program vrátí zpět a dokončí předchozí úkol.
V případě robota Hexora je servomotorů pět, v případě obecném by bylo možné procesorem ATmega128 takto řídit servomotorů desítky najednou. Tímto řešením tedy bylo docíleno ušetření zdrojů procesoru (jednoho časovače a čtyř OCR registrů) a zároveň zjednodušení synchronizace chůze.
Synchronizace jednotlivých procesů je řešena čítáním přetečení časovače (každé přetečení časovače je 20ms). Při chůzi robota je nutné vždy počkat na dokončení pohybu.
Toho je docíleno tím, že před vykonáním dalšího kroku program čeká, než 30x přeteče časovač TCR1. Takto je možné časovat i jakýkoliv jiný proces, který je závislý na dokončení nějakého děje. Synchronizace pohybů tímto způsobem je přehledná a efektivní. Pokud by bylo nutné měřit i jiné časové úseky, má ATmega128 k dispozici další tři volné časovače.
Obr. 34: Zjednodušený vývojový diagram hlavní smyčky nového programu
29
Obr. 35: Zjednodušený časový diagram obsluhy servomotorů
5.3 Realizace SW robota Nastavení hlavního časovače:
Časovač TCR1 řídí přepínání procesů a synchronizaci programu. U časovače se nastavuje zejména Waveform Generation mode (WGM) – režim průběhu čítání časovače a Compare Output mode (COM) – způsob jakým časovač ovlivňuje výstupy procesoru. COM pro nás v tomto případě není zajímavý, protože procesor nastavuje jednotlivé výstupní piny.
V původním SW robota se tímto nastavoval průběh výstupu po přerušení OCR.
WGM je nastaveno na non-inverted, fast PWM. Toto nastavení určuje, že výstup z časovače bude 1 do dosažení hodnoty OCR a poté 0 (non-inverted) a bude generován pulsním generátorem o vysoké frekvenci, což v případě využití přímého výstupu z časovače snižuje zákmit servomotorů. Pro tuto práci je však důležité, že čítač v tomto nastavení bude čítat do hodnoty ICR1 (nastavena pevná hodnota) a poté se vynuluje a čítá od začátku.
Bity ICNC1 (Input Capture Noise Canceler) a ICES1 (Input Capture Edge Select) jsou pro nastavení filtrování a charakteristiky vstupu pro přerušení časovače. V tomto řešení jsou nepodstatné, protože vstup do časovače používaný není.
Bity CS1x (Clock Select) jsou pro nastavení předděličky (prescaler) časovače.
V našem případě bude nastaven bit CS11, čítač tedy bude čítat každý osmý tik hodin
procesoru.
30
Obr. 36: Registry pro nastavení časovače TCR1 (2)
Potřebujeme, aby časovač čítal rychlostí 50Hz (neboli 20ms). Po dosazení do jednoduchého vzorce frekvence procesoru / frekvence časovače * předdělička – dostaneme hodnotu ICR = 36863, což jak z výše uvedeného vyplývá, je také maximální hodnota TCNT1.
Po dosažení této hodnoty bude časovač čítat znovu od nuly a toto bude opakovat 50x za vteřinu, čímž je jeho účel splněn. Dále je ještě nutné povolit přerušení vyvolané OCR1B a přetečení časovače, které umožníme nastavením bitu OCIE1B a OCIE1A v TIMSK (Timer/Counter Interrupt Mask Register) registru pro nastavení povolení přerušení časovače.
Tvorba signálu pro řízení servomotorů:
Pro řízení servomotorů nejsou využity přímo výstupy časovačů, ale jsou řízeny programově přímým nastavováním a resetováním výstupů, které jsou určeny pro ovládání servomotorů.
Po vyvolání přerušení při dosažení hodnoty TCNT1 = ICR1 (tedy v tomto případě 36863) jsou hodnoty všech výstupů pro řízení servomotorů nastaveny najednou na 1.
Jak již bylo zmíněno výše, časovač při dosažení hodnoty TCNT1 = 1200 vyvolá přerušení. Toto přerušení zavolá metodu pro obsluhu servomotorů, která běží v „nekonečné“
smyčce do dosažení hodnoty TCNT1 < 4200. Tímto je zabezpečeno, že obsluze servomotorů
bude přidělen právě tento čas. V této „nekonečné“ smyčce se testuje, zda nebyla dosažena
hodnota přidělená jednotlivým servomotorům. Pokud ano, je přes jednoduchou podmínku
nastavena hodnota jednotlivého výstupu na 0. Toto řešení umožňuje v reálném čase
s minimálními nároky na procesor řídit desítky servomotorů i při využití levnějších a
jednodušších procesorů, než je poměrně dobře vybavená ATmega128.
31
Obr. 37: Tvorba signálu pro řízení servomotorů
Sonar:
Měření doby návratu zvukového signálu bylo výrobcem robota realizováno for cyklem, který inkrementoval proměnnou. Toto řešení opět znamenalo zamrznutí robota na poměrně dlouhou dobu a nemožnost vykonávání ostatních procesů během měření sonarem.
Tato nepříjemná vlastnost byla znásobena při častém měření (několikrát za vteřinu) na neúnosnou míru. Naštěstí výrobce robota připojil výstup modulu SRF05 k Pine.7, který zároveň umožňuje vyvolat přerušení INT7 (10), synchronizované s časovačem TCR3.
V původním kódu však tohoto velmi výhodného zapojení nebylo možné využít, protože jak je uvedeno výše, časovač TCR3 byl zaneprázdněn obsluhou servomotorů.
V novém řešení inicializujeme časovač TCR3 1000µs po vyslání impulzu na Portc.4.
Časovač TCR3 začne čítat do registru TCNT3 až do doby, než se na vstupu Pine.7 objeví
sestupná hrana, která vyvolá přerušení INT7. Po vyvolání přerušení je uložena hodnota
TCNT3 do globální proměnné a časovač TCR3 je resetován. Nastavení časovače TCNT3 je
podobné nastavení časovače TCR1, pouze je zde navíc ošetřeno přerušení INT7 a mezní
hodnota je nastavena pomocí ICR3 na 25ms. Pokud by došlo k tomu, že se časovač
nevynuluje dříve než za 25ms (obvykle do maximálně 15ms), vyvolá se přerušení, které
ohlásí chybu měření sonaru. V praxi tento případ zatím nikdy nenastal. Zpracování výsledku
měření může být poté libovolně zpracováno v hlavní smyčce.
32
Obr. 38: Časový diagram měření sonaru
Přerušení INT7 je nutné před použitím povolit a nastavit. Toho docílíme nastavením bitu 7 v registru EIMSK, který spravuje globální povolení jednotlivých externích přerušení.
V registru ETIMSK, který je součástí časovače TCR3 je nutné nastavit bit 5 „Timer/Counter3, Input Capture Interrupt Enable“, který povolí přerušení ze vstupu pro časovač TCR3.
Nakonec v registru EICRB nastavíme bit 7 „External Interrupt Sense Control Bit“, kterým je definováno, že přerušení INT7 má být vyvoláno po detekci sestupné hrany.
Obr. 39: Registry pro povolení a nastavení přerušení INT7 (2)
Toto řešení se v praxi osvědčilo. Nyní je možné měřit sonarem mnohokrát za vteřinu
v jakékoliv části programu, a přesto měření neovlivňuje běh zbytku programu. Přesnost
měření je v řádu milimetrů, je srovnatelná s předchozím řešením a je limitována spíše
modulem SRF05 a odrazovou plochou překážky, než způsobem měření času procesorem a
změnou architektury programu. Program převádí naměřené hodnoty na centimetry, maximální
33
rozsah měření je 25cm – 200cm. Měření probíhá při běhu programu stále s periodou 0,8s.
Tato perioda je měřena pomocí časovače TCNT1, který je použit pro obsluhu servomotorů.
Při každém jeho přetečení (20ms) se inkrementuje čítač a po dosažení hodnoty 40 se čítač vynuluje a provede měření. Každé měření je odesláno ke zpracování do počítače.
6. Komunikace
6.1 Původní řešení
Robot byl vybaven komunikačním modulem, který byl vytvořen firmou Stenzel (výrobce robota). Tento komunikační kanál umožňoval bezdrátovou (433MHz) sériovou komunikace přes UART mezi procesorem a počítačem. Komunikační obvod jak na straně robota, tak na straně počítače byl realizován pomocí integrovaného obvodu CC1010 od výrobce Texas Instruments. Výrobce implementoval do komunikačního kanálu kontrolu integrity dat. Data v obou směrech musela mít přesně specifikovaný formát [Obr. 40]. Každý příkaz odeslaný počítačem robotovi musel mít 6 nebo 7 bytů. Pokud měl 6 bytů, pak obsahoval: počet bytů datagramu, druhý byte byl rezervován, třetí byl adresa odesílatele, čtvrtý adresa příjemce, pátý obsahoval příkaz a poslední byl kontrolní součet CRC, který kontroloval integritu celého datagramu. Pokud bylo třeba k příkazu použít i nějaké argumenty, vložil se mezi příkaz a CRC ještě jeden byte s argumenty (například naměřená vzdálenost).
Obr. 40: Datagram pro komunikaci přes původní komunikační modul
Pevnou strukturu datagramu hlídal přímo komunikační modul, a pokud nebyla přesně
dodržena, přijatá data prostě zahodil. Další a ještě nepříjemnější skutečností bylo, že i když
komunikace probíhala rychlostí 9600Bd, tak komunikační modul byl schopen přijmout data
pouze v případě, že mezi jednotlivými rámci byla časová prodleva cca dvě vteřiny. Pokud tato
prodleva nebyla dodržena, rámec se nepřenesl, avšak o další dvě vteřiny oddálil možnost další
komunikace. Výrobce robota po upozornění na tento velmi nepříjemný nedostatek přiznal, že
34
o této chybě ví, avšak velmi zdlouhavý pokus o nápravu vedlo pouze k tomu, že komunikační modul přestal pracovat úplně a výrobce robota přestal raději komunikovat také. Modul nebylo možné svépomocí opravit a nijak jinak modifikovat, protože výrobce robota dodával program komunikačního čipu CC1010 pouze ve formátu .HEX, který modifikaci ani náhled do zdrojového kódu neumožňuje. Nebylo možné ani vytvořit pro tento modul program nový, protože výrobce robota odmítl poskytnout schéma zapojení komunikačního modulu.
Z těchto důvodů bylo nutné vytvořit úplně nový komunikační kanál, nezávislý na původním řešení.
6.2 Nový komunikační kanál
Návrh nové komunikace komplikovala chybějící dokumentace k robotovi. Bylo nutné zjistit, které vývody patří k RX a TX pinům UARTu 0 procesoru robota, a zajistit potřebné napájení pro nový komunikační modul.
Nový komunikační kanál byl vyvinut ve spolupráci s vedoucím této práce panem Ing.
Miroslavem Holadou Ph.D. a kolegou Bc. Jakubem Štěpánkem.
Základem nového komunikačního kanálu je bezdrátový komunikační modul HM- TR/TTL.
Obr. 41: Komunikační modul HM-TR/TTL (12)
Protože se jedná o verzi TTL, nebylo nutné používat další obvody pro připojení
komunikačního modulu k robotovi. Největším problémem bylo najít aktivní vstup/výstup RX
a TX na základní desce robota. Sledovat konkrétní dráhy bylo nemožné, protože výstupy pro
komunikační modul jsou poměrně vzdálené od procesoru robota.
35
Na straně počítače bylo nutné nejprve převést signál z USB portu na UART. K tomuto účelu výborně posloužil integrovaný obvod MCP2200, který je přímo určen k tomuto účelu.
Schéma zapojení komunikačního modulu k počítači je na přiloženém CD – Příloha 1.
Rychlost komunikace, přenosovou frekvenci a nastavení parametrů sériové linky je možné nastavit konfiguračními utilitami přímo od výrobců čipu a komunikačního modulu.
Obr. 42: Ukázka konkrétního nastavení čipu MCP2200m a komunikačního modulu HM-TR/TTL
Rychlost přenosu nového komunikačního kanálu je stejná jako u původního, ale mezi jednotlivými rámci není nutná žádná prodleva. Komunikace má dosah dle výrobce 300m otevřeným prostorem, což vzhledem k použití robota je více než dostatečné, spolehlivost komunikace je také dostatečná. O spolehlivost komunikace se stará řídící SW robota, popřípadě hlavní ovládací program počítače.
6.3 Nový formát komunikace
V původním řešení komunikace bylo nutné dodržet přesný formát komunikace podle výrobce robota. Pro tuto práci byl však tento formát nevyhovující, protože vyžadoval příliš velkou režii, zejména potom počítání kontrolního součtu pro každý přenesený rámec.
Vzhledem k tomu, že každou vteřinu je posíláno téměř 30 rámců oběma směry, tak tato režie
není zanedbatelná zejména na straně mikroprocesoru robota, který vykonává spoustu dalších
činností, a zbytečně by zatěžovala i komunikační kanál. Spolehlivost komunikace další
kontrolu v podstatě nepotřebuje. Pro tuto práci je spolehlivost nového komunikačního modulu
36
naprosto dostatečná a není nutné více data kontrolovat. Pokud by se přeci jenom stalo, že data dojdou jakkoliv poškozena, robot s největší pravděpodobností nerozezná příkaz, a proto ho neprovede. Jednotlivé příkazy jsou posílány s pevnou periodou znovu, proto by byla případná jednotlivá chyba rychle opravena a neohrozila by chod programu.
Nový formát komunikace je optimalizován pro tuto konkrétní práci. Kontrola integrity dat je omezena na identifikaci prvního a posledního bytu rámce. V těch je pevná hodnota 255 resp. 254. Tyto hodnoty jednoznačně identifikují začátek a konec komunikace, nemohou být v žádné jiné části rámce.
Pro komunikaci ve směru od robota k počítači [Obr. 43] má rámec 4 byty. Tento rámec posílá pouze typ rozkazu (například že se jedná o hodnotu ze sonaru a jeden parametr), což by v tomto případě byla konkrétní vzdálenost překážky. Robot nepotřebuje odesílat složitější rámce, tento rámec je dostatečný pro zpracování dat ze všech jeho periferií.
Obr. 43: Datagram komunikace ve směru robot > počítač
Pro komunikaci ve směru z počítače k robotovi [Obr. 44] je rámec delší, a to čistě z důvodu nutnosti řídit pohyb kamery v reálném čase v závislosti na pohybu ruky při spuštěném handtrackingu. Počátek rámce je identifikován stejně jako v předchozím případě hodnotou 255. Další byte je rezervován pro určení konkrétního příkazu, například pohyb kamery nebo změna směru chůze. V bytu nazvaném Rozkaz 1 je například konkrétní směr pohybu kamery v horizontálním směru a v bytu Parametr 1 bude rychlost pohybu kamery v tomto směru. Pro byty Rozkaz 2 a Parametr 2 platí to samé, pouze ve směru vertikálním.
Obr. 44: Datagram komunikace ve směru počítač > robot
