Podpůrný software pro výuku mobilní robotiky

Transkript

ČESKÉ VYSOKÉ UČENÍ TECHNICKÉ V PRAZE
Fakulta elektrotechnická
BAKALÁŘSKÁ PRÁCE
Hana Szücsová
Katedra kybernetiky
Vedoucí bakalářské práce: Ing. Jan Faigl
Zadání
Prohlášení
Prohlašuji, že jsem svou bakalářskou práci vypracoval samostatně a použil jsem pouze
podklady (literaturu, projekty, SW atd.) uvedené v přiloženém seznamu.
Poděkování
Chtěla bych především poděkovat svému vedoucímu bakalářské práce Janu Faiglovi
za jeho trpělivost, čas a stále dobrou náladu. Dále všem lidem z Gerstnerovi laboratoře,
kteří mi byli kdykoliv ochotni podat pomocnou ruku. Také chci poděkovat svému příteli
Vladimírovi a členům rodiny za psychickou podporu.
Abstrakt
Tato práce se zabývá vývojem podpůrného softwaru pro výuku předmětu
mobilní robotika, která probíhá nejen formou teoretického výkladu, ale
také jako praktická cvičení s reálnými roboty. Pro potřeby cvičení byla
vyvinuta robotická platforma Morbot. Součástí podpory výuky je konfigurace palubního počítače Morbotu skládající se z nastavení parametrů jeho periférií, ale především přípravy prostředí křížové kompilace
systému Player, jehož součástí je implementovaný modul pro přístup
k Morbotu. Hlavní výhodou použití prostředí Player je snadný přenos
odladěné aplikace v simulátoru na reálný robot. Součástí práce je popis
implementace modulu Morbotu spolu s ukázkami přenosu aplikace ze
simulátoru na reálný robot. V práci jsou dále diskutována zadání úloh
řešených na předmětu mobilní robotika. Ukázková řešení jsou studijními
materiály, které přibližují řešenou problematiku a principy Playeru studentům, kteří tak mohou řešit pokročilé úlohy.
Abstract
The goal of this thesis is a creation of supporting software for teaching of
mobile robotics. Labs of the course are completed by hands-on trainings
with real robots. A robotic platform called Morbot was specially developed for the mobile robotics course. A part of support is configuration
of the main computer of the robotic platform, which is not only settings
of peripheral devices but mainly setup of cross-compilation environment
for compiling the Player framework with implemented Morbot accessing
module. An advantage of using the Player environment is simple application migration from a robot simulator to the real robot platform
Morbot. The thesis includes a description of implementation of Morbot
module. The main part of the thesis is dedicated to discussion of the
tasks definition solved at trainings. Example solutions are presented as
well, which can be used by students as study materials.
OBSAH
Obsah
1 Úvod
1
2 Morbot
3
2.1
Palubní počítač . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3 Player/Stage
4
7
Používání Playeru . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
8
3.1.1
Používání Stage . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
9
3.2
Modely zasílání zpráv . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
13
3.3
Implementace ovladače . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
14
3.3.1
Začlenění nového ovladače do Playeru . . . . . . . . . . . . . . . . .
15
3.3.2
Implementace ovladače pro robot Morbot . . . . . . . . . . . . . . .
16
3.1
4 Úlohy na X33MOR
20
4.1
Verzovací systém SVN . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
22
4.2
Implementace klientské aplikace . . . . . . . . . . . . . . . . . . . . . . . .
23
4.3
Řízení pohybu robotu . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
26
4.3.1
Motivace . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
26
4.3.2
Zadání . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
26
4.3.3
Rozbor zadání . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
26
4.3.4
Implementace regulátoru . . . . . . . . . . . . . . . . . . . . . . . .
26
4.3.5
Spuštění aplikace . . . . . . . . . . . . . . . . . . . . . . . . . . . .
29
Detekce překážek . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
31
4.4.1
Motivace . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
31
4.4.2
Zadání . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
31
4.4.3
Vlastnosti dálkoměrných senzorů . . . . . . . . . . . . . . . . . . .
31
4.4.4
Vytvoření modelu senzorů v simulátoru Stage . . . . . . . . . . . .
33
4.4.5
Implementace úlohy v simulátoru . . . . . . . . . . . . . . . . . . .
36
4.4.6
Přizpůsobení aplikace pro Morbot . . . . . . . . . . . . . . . . . . .
39
Algoritmy třídy BUG . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
40
4.5.1
40
4.4
4.5
Motivace . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
i
OBSAH
4.6
4.7
4.5.2
Zadání . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
41
4.5.3
Algoritmy třídy Bug . . . . . . . . . . . . . . . . . . . . . . . . . .
41
4.5.4
Implementace algoritmu třídy Bug v simulátoru . . . . . . . . . . .
45
Metody vizuální navigace . . . . . . . . . . . . . . . . . . . . . . . . . . . .
48
4.6.1
Motivace . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
48
4.6.2
Zadání . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
48
4.6.3
Rozhraní BlobfinderProxy v simulátoru Stage . . . . . . . . . . . .
49
4.6.4
Implementace algoritmu pro vizuální navigaci na základě dat z inteligentní kamery . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
50
Tvorba topologické mapy . . . . . . . . . . . . . . . . . . . . . . . . . . . .
54
4.7.1
Motivace . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
54
4.7.2
Zadání . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
54
4.7.3
Základní druhy modelů prostředí . . . . . . . . . . . . . . . . . . .
54
4.7.4
Rozbor úlohy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
55
4.7.5
Popis implementace . . . . . . . . . . . . . . . . . . . . . . . . . . .
56
5 Závěr
58
ii
SEZNAM OBRÁZKŮ
Seznam obrázků
1
Robotická platforma Morbot využívaná při výuce mobilní robotiky. . . . .
2
2
Architektura Morbotu. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
3
3
Základní deska Verdex (a) a netwifimicroSD modul (b), převzato z [2].
. .
4
4
Architektura Playeru. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
7
5
Ukázky prostředí robotických simulátorů, převzato z [5]. . . . . . . . . . .
8
6
Robot specifikovaný v konfiguračním souboru morbot.inc. . . . . . . . . . .
11
7
Hierarchie tříd v ovladači pro robot Morbot. . . . . . . . . . . . . . . . . .
17
8
Část adresářové struktury šablony. . . . . . . . . . . . . . . . . . . . . . .
21
9
Princip SVN. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
22
10
Souřadný systém robotu. . . . . . . . . . . . . . . . . . . . . . . . . . . . .
27
11
Průběh první úlohy v simulátoru. . . . . . . . . . . . . . . . . . . . . . . .
29
12
Princip IR dálkoměrného senzoru. . . . . . . . . . . . . . . . . . . . . . . .
31
13
Převodní charakteristika IR senzorů, převzato z [10]. . . . . . . . . . . . .
32
14
Původ falešných ech sonaru. . . . . . . . . . . . . . . . . . . . . . . . . . .
32
15
Vyzařovací charakteristika sonaru SRF10, převzato z [11]. . . . . . . . . .
33
16
Možný průběh druhé úlohy. . . . . . . . . . . . . . . . . . . . . . . . . . .
37
17
Naměřená převodní charakteristika IR senzorů.
. . . . . . . . . . . . . . .
40
18
Trajektorie nalezená algoritmem Bug1. . . . . . . . . . . . . . . . . . . . .
42
19
Bug1 - nedosažitelný cíl. . . . . . . . . . . . . . . . . . . . . . . . . . . . .
43
20
Trajektorie generované algoritmem Bug2. . . . . . . . . . . . . . . . . . . .
44
21
Vývojový diagram algoritmu reaktivního řízení. . . . . . . . . . . . . . . .
46
22
Průběh třetí úlohy v simulátoru. . . . . . . . . . . . . . . . . . . . . . . . .
49
23
Simulace pohledu kamery v prostředí simulátoru Stage. . . . . . . . . . . .
50
24
Příklad prostředí a jeho reprezentace topologickou mapou. . . . . . . . . .
55
iii
SEZNAM TABULEK
Seznam tabulek
1
Základní rozhraní Playeru. . . . . . . . . . . . . . . . . . . . . . . . . . . .
9
2
Základní funkce třídy PlayerClient. . . . . . . . . . . . . . . . . . . . . . .
24
3
Přehled základních rozhraní proxy. . . . . . . . . . . . . . . . . . . . . . .
24
4
Základní funkce třídy Position2DProxy. . . . . . . . . . . . . . . . . . . . .
24
5
Základní funkce rozhraní BlobfinderProxy . . . . . . . . . . . . . . . . . . .
25
6
Položky datové struktury typu playerc blobfinder blob t. . . . . . . . . . .
51
7
Obsah přiloženého CD. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
60
iv
SEZNAM VÝPISŮ KÓDU
Seznam výpisů kódu
1
Konfigurace wifi. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5
2
Připojení microSD na příkazové řádce. . . . . . . . . . . . . . . . . . . . .
6
3
Připojení microSD úpravou souboru /etc/fstab. . . . . . . . . . . . . . . .
6
4
Konfigurační soubor - morbot.cfg. . . . . . . . . . . . . . . . . . . . . . . .
8
5
Definice modelu Morbotu - morbot.inc. . . . . . . . . . . . . . . . . . . . .
9
6
Definice vlastností senzorů (součást morbot.inc). . . . . . . . . . . . . . . .
10
7
Definice světa - task1.world. . . . . . . . . . . . . . . . . . . . . . . . . . .
11
8
Definice vlastností překážek ve světě. . . . . . . . . . . . . . . . . . . . . .
12
9
Konfigurační soubor Playeru pro spuštění simulátoru Stage - task1.cfg. . .
13
10
Funkce Morbot Register(DriverTable* table). . . . . . . . . . . . . . . . . .
15
11
Funkce Morbot Init(ConfigFile* cf, int section). . . . . . . . . . . . . . . .
15
12
Test konfiguračního souboru na přítomnost rozhraní position2d. . . . . . .
15
13
Načtení proměnné port z konfiguračního souboru. . . . . . . . . . . . . . .
16
14
Funkce Setup. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
17
15
Funkce Main. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
17
16
Funkce MatchMessage. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
19
17
Funkce Shutdown. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
19
18
Konstruktor třídy CMorbot. . . . . . . . . . . . . . . . . . . . . . . . . . .
25
19
Použití instancí tříd Proxy pro načtení dat. . . . . . . . . . . . . . . . . . .
25
20
Funkce moveTo. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
27
21
Funkce printStatus. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
28
22
Konfigurační soubor morbot.cfg - vytvoření rozhraní position2d. . . . . . .
30
23
Ukázka funkce main. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
30
24
Definice modelu IR senzorů. . . . . . . . . . . . . . . . . . . . . . . . . . .
33
25
Definice modelu sonarů. . . . . . . . . . . . . . . . . . . . . . . . . . . . .
34
26
Konfigurační soubor pro vytvoření dvou rozhraní sonarProxy. . . . . . . . .
34
27
Výpis Playeru - kontrola vytvoření dvou rozhraní pro dálkoměrné senzory.
35
28
Vytvoření instancí třídy CRangeFinder pro vyčítání dat ze dvou modelů
dálkoměrných senzorů. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
36
Funkce moveTo v druhé úloze. . . . . . . . . . . . . . . . . . . . . . . . . .
38
29
v
SEZNAM VÝPISŮ KÓDU
30
Natočení robotu bokem k překážce. . . . . . . . . . . . . . . . . . . . . . .
46
31
Objezd překážky. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
47
32
Definice barevných kanálů algoritmu blobfinder. . . . . . . . . . . . . . . .
50
33
Ukázka funkcí třídy CCamera. . . . . . . . . . . . . . . . . . . . . . . . . .
52
34
Regulátor pro dojezd ke krabici známé barvy, funkce gotoColor. . . . . . .
53
vi
1
Úvod
Mobilní robotika se zabývá řešením problémů týkající se pohybujících se robotů, mezi
které patří lokalizace robotu v prostředí, mapování nebo plánování trajektorie robotu. Pro
řešení těchto komplexních úloh je nutná řada dílčích algoritmů, řešící jednotlivé problémy
od vyčítání dat senzorů přes zpracování dat, vlastní plánování a rozhodování až po generování akčních zásahů, které realizují interakci robotu s prostředím. Student mobilní
robotiky musí mít relativně široké teoretické znalosti, které mu umožní porozumět základním architekturám řízení mobilních robotů, jejichž chování bychom označili za inteligentní.
Na druhé straně je vhodné teoretické znalosti podpořit prakticky získanými zkušenostmi,
neboť v reálném světě je nutné zpravidla řešit nepřesnosti, způsobené šumem.
Výuka mobilní robotiky může probíhat formou teoretických výkladů doplněných o praktická cvičení. Při výuce je vhodné využít robotických simulátorů, neboť ty umožňují soustředit se na řešení konkrétního problému (algoritmů) Ostatní nezbytné komponenty systému interakce robot↔prostředí jsou součástí simulačního prostředí. Přestože v prostředí
robotického simulátoru mohou být senzory modelovány včetně chyb měření, vždy se jedná
pouze o modely a jejich chování může být reálným senzorům více či méně vzdálené. Výhodou ideálního chování senzorů je názornost demonstrace algoritmů používaných v mobilní
robotice. Pokud je však studentům umožněno pracovat také s reálnými roboty, získávají
studenti cenné zkušenosti a cvičení se stanou mnohem názornějšími, ale také náročnějšími.
Právě náročnost praktické realizace některé robotické úlohy na reálném robotu je hlavní
motivací pro tuto bakalářskou práci. Vytvořením podpůrného software, který mohou studenti využít při řešení praktických úloh, jim může pomoci překonat především počáteční
náročnou fázi, než se robot začne opravdu alespoň trochu smysluplně pohybovat. Ačkoliv tato úloha může znít triviálně, při experimentování s reálným robotem je nutné čelit
řadě problémů, které v simulaci nenastávají, počínaje omezenou kapacitou akumulátorů,
rušením rádiového spojení s robotem až po nepřesnosti v řízení a chyby ve vyčítaných
senzorický datech.
Základní úvod do problematiky mobilní robotiky na ČVUT v Praze poskytuje kurz
Mobilní robotika vyučovaný katedrou kybernetiky. Výuka probíhá formou teoretických
výkladů, které se zabývají především rozborem typických úloh z oblasti mobilní robotiky,
jako je pohyb robotu, zpracování senzorických dat, lokalizace nebo mapování. Výklad je
doplněn cvičeními, na kterých je postupně programováno pět na sebe navazujících úloh, jejichž výsledkem by mělo být vytvoření topologické mapy prostředí a bezkolizní autonomní
pohyb robotu v tomto prostředí. Jednotlivé úlohy jsou vždy řešeny nejdříve v robotickém
simulátoru a poté jsou přeneseny na reálný robot. Pro potřeby cvičení byla vyvinuta robotická platforma Morbot, jejíž senzorické vybavení je uzpůsobeno tomu, aby bylo možné
všech pět úloh na platformě realizovat. Platforma je zobrazena na obrázku 1.
První část této práce se zabývá základním softwarovým vybavením palubního počítače
Morbotu a konfigurací periférií palubního počítače. Druhá část popisuje obecný postup
vývoje softwarového modulu pro integraci nepodporovaného hardware do systému Pla1/60
Obrázek 1: Robotická platforma Morbot využívaná při výuce mobilní robotiky.
yer a také konkrétní implementaci modulu pro robot Morbot. Player poskytuje abstrakci
přístupu k hardware robotu založenou na unifikovaném rozhraní, které implementují jednotlivé moduly Playeru. Je poskytován jednotný přístup k modelu robotu v simulátoru i
reálnému robotu, proto značně zjednodušují přenos aplikace odladěné v robotickém simulátoru na reálný robot. V druhé části budou také popsány základní dovednosti, které jsou
pro práci s Playerem důležité. Hlavní část práce je popisem pěti úloh, řešených v rámci
semestrální práce na cvičení. Zabývá se motivací studentů pro řešení úloh a jejich zadáním,
ale také rozborem zadání, tématickým pozadím úloh, návrhy řešení a jejich ukázkami.
2/60
2
Morbot
Morbot je označení pro robotickou platformu vyvinutou pro potřeby předmětu Mobilní
robotika, X33MOR. Jedná se o mobilní robot rozměrů 24x22x20cm, který je navržen tak,
aby mohl být využíván studenty předmětu mobilní robotika.
Hlavní rám robotu je postaven z pevných hliníkových profilů. Tvar robotu byl uzpůsoben
tomu, aby rám odolal i velmi prudkým nárazům. Ke standardnímu senzorickému vybavení
patří enkodéry a sedm taktilních nárazníků detekujících kolize. Robot může být vybaven i
dalšími senzory, které mohou studenti volit dle zadané úlohy:
• čtyři infračervené dálkoměrné senzory,
• dva ultrazvukové dálkoměrné senzory (sonary) a
• inteligentní kamera.
Umístění dálkoměrných senzorů je variabilní, jejich poloha může být měněna v závislosti
na řešeném problému.
Obrázek 2: Architektura Morbotu.
Základní architektura Morbotu je znázorněna na obrázku 2. Z hlediska přístupu studenta k Morbotu je nejdůležitější palubní počítač komunikující převážně s řídicí deskou
3/60
2.1 Palubní počítač
prostřednictvím sériového rozhraní RS232. Řídicí deska sbírá data ze všech senzorů kromě
inteligentní kamery, obstarává komunikaci s řídicí jednotkou motorů a udržuje aktuální
informaci o poloze robotu počítanou z odometrických dat.
V následujícím oddíle této kapitoly se seznámíme s palubním počítačem, konkrétně
s jeho základním softwarovým vybavením a nastavením jeho periférií. Tato znalost je důležitá při řešení případných problémů s palubním počítačem, které se mohou vyskytnout
při řešení úloh na předmětu X33MOR. Podrobnější informace o elektronických částech a
konstrukci Morbotu lze najít v práci [1].
2.1
Palubní počítač
Palubní počítač Morbotu je platforma Verdex XL6P vyvíjená firmou Gumstix [2]. Jeho
hlavní součástí je procesor páté generace architektury ARM, PXA270 [3]. Architektura
ARM je založena na RISCové instrukční sadě, instrukce jsou prováděny přímo hardwarem,
nikoli mikrokódem. Výhodou jsou malé rozměry a nízká spotřeba energie. Procesor využívá
32MB programové paměti typu Flash a operační paměť RAM 128MB.
Základní deska s procesorem je rozšířena dvěma periferními deskami:
• upravenou verzí desky console vx a
• deskou netwifimicroSD EU.
Upravená verze desky console vx obsahuje rozhraní tří sériových portů vyvedených na
pinové konektory a konektor pro miniUSB, oproti standardní verzi je rozšířena o konektor
pro připojení I2 C sběrnice. NetwifimicroSD poskytuje Ethernet 100Mb/s, WiFi 802.11g
modul pro bezdrátové připojení a slot pro microSD kartu. Rozšířující modul a základní
deska jsou na obrázku 3.
(a)
(b)
Obrázek 3: Základní deska Verdex (a) a netwifimicroSD modul (b), převzato z [2].
4/60
Základní softwarové vybavení
Z hlediska softwarového vybavení palubního počítače je důležitý obsah paměti Flash,
která je rozdělena do tří sektorů. První sektor obsahuje zavaděč U-boot, který slouží pro
načtení a spuštění operačního systému. Obsahuje funkce přístupu k periferním zařízením
pro aktualizaci operačního systému. V druhém sektoru jsou ukládány proměnné prostředí
zavaděče. Tyto dva sektory jsou softwarově chráněny proti přepsání z důvodu zamezení
náhodnému vymazání zavaděče z paměti.
Třetí sektor slouží k uložení souborového systému JFFS2 (Journaling Flash File System
version 2), který je kořenovým adresářem, obsahuje vlastní jádro operačního systému Linux
a základní uživatelské prostředí. JFFS2 je souborový systém určený pro paměti typu Flash.
Soubory z něj nejsou vymazávány, jsou pouze přidávány novější verze. Jakmile je paměť
zaplněna, jsou starší verze souborů vymazány a provádí se defragmentace [4].
Konfigurace periférií
Základní deska palubního počítače je rozšířena o periferní obvody, které nám poskytují
rozhraní WiFi, Ethernet, slot pro microSD kartu, sériová rozhraní a miniUSB konektor.
Použitý obraz souborového systému obsahuje základní konfiguraci těchto rozhraní, proto
je zapotřebí nastavit pouze parametry některých zařízení.
Pro nastavení WiFi sítě je zapotřebí provést sekvenci příkazů:
• nastavení klíče,
• výběr módu připojení a
• specifikace přístupového bodu.
Sekvence příkazů je uvedena ve výpisu 1.
iwconfig
iwconfig
iwconfig
iwconfig
wlan0
wlan0
wlan0
wlan0
key on
key *****
mode managed/ad-hoc
essid *****
Výpis 1: Konfigurace wifi.
Experimentálně bylo ověřeno, že příkazy je nutné provést v uvedeném pořadí. Sekvenci
je možné napsat ve formě skriptu a přidat jej do souboru /etc/rc.5. Soubory /etc/rc.*
jsou spouštěny při zavádění operačního systému a provádějí jeho inicializaci. Číslo v názvu
souboru označuje tzv. runlevel, uživatelem mohou být vetšinou konfigurovány úrovně čtyři
a pět.
5/60
Při konfiguraci Ethernet rozhraní si můžeme vybrat buď nastavení pevné IP adresy nebo
přiřazovaní dynamické IP adresy serverem DHCP. Nastavení se provádí v konfiguračním
souboru /etc/interfaces/network.
Rozšiřující paměť, karta microSD, musí být v základní konfiguraci operačního systému
palubního počítače naformátována souborovým systémem FAT32. Její připojení provedeme
příkazem ve výpisu 2. Druhou možností je přidání jednoho řádku z výpisu 3 do souboru
/etc/fstab.
mount -t vfat /memory/card /dev/mmcblk1p0
Výpis 2: Připojení microSD na příkazové řádce.
/dev/mmcblk1p0
/opt
vfat
rw,fmask=0022,dmask=0022,uid=500
0
0
Výpis 3: Připojení microSD úpravou souboru /etc/fstab.
Sériové porty ani miniUSB nepotřebují žádná zvláštní nastavení.
6/60
3
Player/Stage
Player [5] je framework, který realizuje abstrakci přístupu k hardware robotu. Abstrakce
je založena na unifikovaném rozhraní, které implementují rozšiřující moduly komunikující
s konkrétní hardwarovou částí robotu. Moduly existují jak pro samostatné senzory, jako
jsou laserové dálkoměry firmy SICK nebo inteligentní kamery CMUcam2, tak pro celé
roboty, např. Pioneer nebo Amigobot. Moduly mohou také realizovat různé algoritmy
používané v mobilní robotice, jako je lokalizační algoritmus Monte Carlo nebo Blobfinder
pro vyhledávání souvislých barevných oblastí v obraze.
Moduly můžeme rozdělit do dvou skupin. První skupinou jsou staticky linkované moduly, které jsou nedílnou součástí hlavní aplikace. Jejich povolení nebo zákaz je proveden
při konfiguraci instalace Playeru. Takové moduly budeme dále nazývat ovladače. Do druhé
skupiny řadíme dynamicky linkované moduly, které budeme nazývat pluginy.
Architektura zapouzdřující aplikace je založena na modelu server/klient, schematicky
je znázorněna na obrázku 4. Tento přístup nám umožňuje vybrat si pro implementaci
klientských aplikací ze široké škály programovacích jazyků, např.: C++, Javu nebo Python.
O aplikaci reprezentující serverovou část budeme dále mluvit jako o Playeru.
Obrázek 4: Architektura Playeru.
Jedním z rozšiřujících pluginů Playeru je Stage, robotický simulátor pro mobilní roboty. Poskytuje modely různých senzorů, jako jsou sonary, nárazníky nebo infračervené
dálkoměry. Z hlediska Playeru mají tyto modelované senzory stejné rozhraní jako ovladače
skutečného hardware. Klientská aplikace tak může být vyvíjena v simulátoru a přenesena
na reálný robot s žádnými nebo malými změnami. Virtuální roboty se pohybují a sbírají
senzorická data ve dvourozměrném světě, který je reprezentován bitmapovým obrázkem.
Roboty i světy jsou specifikovány v konfiguračních souborech, které umožňují definovat
kromě základních vlastností také nepřesnosti, např. chybu odometrie.
Obdobou Stage je plugin Gazebo. Jedná se také o simulátor, ale tentokrát venkovních
prostředí v třídimenzionálním světě. Dokáže věrněji vytvářet zpětnou vazbu senzorů a
fyzikálně věrohodnější interakci mezi objekty než simulátor Stage. Gazebo také využívá
7/60
3.1 Používání Playeru
standardního rozhraní pro Player, proto mohou být moduly napsané pro Stage použity i
pro Gazebo. Grafická prostředí simulátorů Stage a Gazebo jsou ukázány na obrázku 5.
(a) Stage
(b) Gazebo
Obrázek 5: Ukázky prostředí robotických simulátorů, převzato z [5].
V následujících třech částech budou popsány některé základní dovednosti, které mohou
být uplatněny při používání Playeru a simulátoru Stage. V první části je ukázáno, jakým
způsobem je možné implementovat konfigurační soubory pro definici požadovaných rozhraní při spuštění Playeru a Stage a pro specifikaci modelu světa a modelu robotu. V další
části si vysvětlíme možné způsoby komunikace mezi serverem a klientskou aplikací a rozdíly
mezi nimi. V poslední části je popsána implementace nového ovladače hardware, nejdříve
obecně a poté konkrétní implementace ovladače pro robot Morbot.
3.1
Používání Playeru
Nedílnou součástí Playeru je velké množství modulů. Při každém spuštění serveru je
nutné specifikovat požadavek na konkrétní moduly konfiguračním souborem. Player se
spustí s parametrem souboru příkazem player configFile.cfg.
Na příkladu konfiguračního soubor morbot.cfg uvedeném ve výpisu 4 bude ukázána
základní syntax konfiguračních souborů Playeru. Tento soubor zajistí vytvoření rozhraní
pro odometrii a ultrazvukové dálkoměry
driver
(
name "morbot"
provides ["position2d:0"
"sonar:0"]
port "/dev/ttyS2"
)
Výpis 4: Konfigurační soubor - morbot.cfg.
8/60
V každém konfiguračním souboru může být více sekcí ohraničených kulatými závorkami
a uvozených klíčovým slovem driver. V takovém případě je využíváno více modulů najednou. Mezi závorkami se může nacházet několik druhů proměnných. Mezi základní patří:
• name,
• provides a
• requires.
Proměnná name udává název načítaného modulu, ovladače nebo pluginu. Hodnotou
proměnné provides jsou poskytovaná rozhraní, seznam často používaných rozhraní je uveden v tabulce 1. Requires je proměnnou obsahující seznam nutných rozhraní pro správnou
funkčnost modulu.
Proměnná port je zavedena v ovladači pro robot Morbot. Hodnotou je označení sériového
portu využívaného na palubním počítači. Proměnná je načítána při inicializaci ovladače
pro Morbot, pokud není nalezena, je nahrazena definovanou konstantou.
Název rozhraní
blobfinder
bumper
ir
laser
limb
planner
position2d
simulation
sonar
Popis
visuální vyhledávání barevných oblastí v obraze
nárazníky
IR dálkoměry
laserové dálkoměry
ovládání robotického ramene s více klouby
planární plánovač cesty robotu
odometrie
robotický simulátor
ultrazvukový dálkoměr
Tabulka 1: Základní rozhraní Playeru.
3.1.1
Používání Stage
Jedním z pluginů Playeru je robotický simulátor Stage. Pokud se rozhodneme tento simulátor použít, je potřeba předem definovat model robotu (soubor *.inc) a model světa
(soubor *.world). Jako příklad uvedeme konfiguraci simulačního prostředí, které je využíváno studenty předmětu X33MOR. Ve výpisu 5 jsou specifikovány vlastnosti i vzhled
robotu.
define morbot position
(
# definice lokalizace odometrií
9/60
localization "odom"
# specifikace chyby odometrie
odom_error [0.0 0.01 0.0 0.01 0.0 0.01]
# barva robotu
color "orange"
# velikost robotu
size [0.25 0.25]
#offset od středu otáčení
origin [-0.0 0.0 0]
# podoba přední části robotu
gui_nose 1
# odhad váhy v kg
mass 2.0
# definice maximální rychlosti
max_speed 0.1
# vytvoření přibližného tvaru robotu polygony
polygons 3
# uveden první polygon, další dva se tvoří obdobným způsobem
polygon[0].points 4
polygon[0].point[0] [ 0.050 0.124 ]
polygon[0].point[1] [ 0.050 0.085 ]
polygon[0].point[2] [ -0.050 0.085 ]
polygon[0].point[3] [ -0.050 0.124 ]
...
# diferenciální řízení
drive "diff"
# použití definice sharpů a kamery (uvedeno níže)
morbot_sharp()
morbot_camera()
).
Výpis 5: Definice modelu Morbotu - morbot.inc.
Definice senzorů, infračervených dálkoměrů a kamery, je uvedena ve výpisu 6, který je
součástí souboru morbot.inc.
# definice vlastností infračervených dálkoměrů
define morbot_sharp ranger (
# počet senzorů
scount 2
# poloha senzorů na robotu [xpos ypos heading]
spose[0] [ 0.12 0.00 00 ]
spose[1] [ 0.00 0.08 90 ]
# vlastnosti senzorů [range_min range_max view_angle]
10/60
sview [0.15 0.8 1]
# velikost jednotivých senzorů [xsize ysize] in meters
ssize [0.01 0.03]
)
# vlastnosti kamery
define morbot_camera (
ptz (
# poloha kamery na robotu
pose [0.05 0 0]
# specifikace barev pro blobfinder
blobfinder(
channel_count 1
channels ["red"]
)
)
)
Výpis 6: Definice vlastností senzorů (součást morbot.inc).
Součástí simulačního prostředí je také vizualizace, ukázka grafického prostředí je na
obrázku 5. Na obrázku 6 je zobrazen robot, který byl specifikován v souboru morbot.inc,
ve výpisu 5.
Obrázek 6: Robot specifikovaný v konfiguračním souboru morbot.inc.
Svět, ve kterém se robot pohybuje a plní zadané úlohy je definován v souboru *.world.
Syntax si ukážeme na definici světa task1.world, který je uveden ve výpisu 7. Tento svět
se od ostatních používaných na X33MOR liší načítaným bitmapovým obrázkem, který
představuje podobu pracovního prostředí robotu.
include "morbot.inc"
include "map.inc"
# velikost světa v metrech
size [10 10]
# nastavení rozlišení v metrech
resolution 0.05
# interval pro update simulátoru
gui_interval 20
11/60
# definice okna grafického prostředí
window (
# velikost
size [ 600.000 600.000 ]
# střed
center [-.0 -.0]
# mřížka
scale 0.010
)
#načtení mapy světa
map (
bitmap "bitmaps/task1.png"
size [5 5]
name "task1map"
)
# vytvoření robotu dle nadefinovaného modelu
morbot (
name "robot1"
#počáteční pozice robotu ve světě [xpos ypos angle]
pose [-1 -2 90]
)
Výpis 7: Definice světa - task1.world.
Ve světě se mohou nacházet překážky, které mohou být buď součástí připravených bitmapových obrázků nebo specifikovány v konfiguračním souboru světa. Ve druhém případě je
možné s překážkami pohybovat i v průběhu simulace. Definice překážky je uvedena ve
výpisu 8.
#definice modelu překážky
define puck model(
size [ 0.8 0.8 ]
gui_movemask 3
gui_nose 0
fiducial_return 10
)
#umístění překážky a její barva
puck( pose [0 -1 0] color "red" )
puck( pose [0 1 0] color "blue" )
Výpis 8: Definice vlastností překážek ve světě.
12/60
3.2 Modely zasílání zpráv
Pro spuštění stage je nutné vytvořit konfigurační soubor specifikující načítaný model
světa a model robotu, který se předává Playeru při spuštění. Takový konfigurační soubor
je uveden ve výpisu 9. Oproti konfiguračnímu souboru z výpisu 4, obsahuje nové proměnné:
• plugin,
• worldfile a
• model.
driver
(
name "stage"
provides ["simulation:0" ]
plugin "libstageplugin"
worldfile "task1.world"
)
driver
(
name "stage"
provides ["position2d:0"]
model "robot1"
)
Výpis 9: Konfigurační soubor Playeru pro spuštění simulátoru Stage - task1.cfg.
Hodnota proměnné plugin je jménem použitého modulu, v tomto případě libstageplugin.
Jedná se o dynamicky linkovanou knihovnu, která se používá pro simulaci robotů nejen
jako modul Playeru, ale také jako knihovna v jazyce C. Knihovna může být umístěna:
• v adresáři, ve kterém je uložen konfigurační soubor Playeru nebo
• v adresáři /usr/local/lib, do kterého je knihovna ukládána při instalaci Stage.
Hodnota proměnné worldfile je název načítaného modelu světa a hodnota proměnné
model je název modelu robotu.
3.2
Modely zasílání zpráv
Architektura Playeru je založena na modelu server/klient. Mezi serverem a klientskou
aplikací existuje komunikace založená na výměně zpráv.
V rámci Playeru existují dva standardní módy, které jsou používány pro výměnu zpráv,
PULL a PUSH mód. Způsoby zasílání zpráv ovlivňují pouze frontu příchozích zpráv na
13/60
3.3 Implementace ovladače
straně klienta, nikoliv zasílání zpráv ve směru k serveru. Standardně je v Playeru nastaven
PUSH mód.
Při použití PUSH módu jsou zprávy ze serveru zasílány ve chvíli, kdy jsou vygenerovány.
Je na klientovi, aby dokázal zprávy přijímat dostatečně rychle. Předpokládá se, že operační
systém na straně klienta dokáže udržovat vyrovnávací paměť, ze které předává zprávy
klientovi při každém volání funkce Read(). Pokud jsou zprávy vyčítány příliš pomalu,
dostává klientská aplikace neaktuální informace a navíc velikost vyrovnávací paměti nemusí
stačit.
Takový případ nenastane při použití PULL módu. Fronta zpráv je udržována na straně
serveru a zprávy jsou zasílány pouze na žádost klienta. Navíc mohou být nadefinována
pravidla pro přepisování zpráv uvnitř fronty funkcí AddReplaceRule().
V případě Morbotu může být ponecháno implicitní nastavení PUSH módu. Toto řešení
je jednodušší, protože nenastává problém s přetečením vyrovnávací paměti nebo s neaktuálností zpráv, neboť mezi serverem a klientem jich není posíláno příliš mnoho.
3.3
Implementace ovladače (verze Playeru 2.0.3)
Pro ovládání nepodporovaného hardware rozhraním Playeru je nutná implementace nového modulu. Rozhodli jsme se pro řešení implementace ovladačem, tedy staticky linkovaným modulem. Každý ovladač Playeru je odvozen od třídy Driver, která zajišťuje výměnu
zpráv mezi serverem a klientskou aplikací a povolování jednotlivých ovladačů. Dokumentace je dostupná na [6].
Každý ovladač musí implementovat dvě funkce pro jeho registraci:
• Register(Drivertable* table) a
• Init(ConfigFile* cf, int section).
Kromě nich je nezbytné implementovat tři základní funkce, které zajišťují základní funkcionalitu ovladače:
• Setup(),
• Main() a
• Shutdown().
Funkce Setup je první spouštěnou funkcí vlastního ovladače a slouží k jeho inicializaci,
Main je hlavním vláknem ovladače a Shutdown toto vlákno ukončuje.
V následujícím textu bude nejdříve popsáno začlenění ovladače do serverové aplikace
Playeru a poté implementace vlastního ovladače pro robot Morbot.
14/60
3.3.1
Začlenění nového ovladače do Playeru
Pro začlenění ovladače do Playeru je nezbytná implementace funkcí pro registraci a
inicializaci ovladače. V následujícím textu budou názvy těchto funkcí uváděny tak, jak
jsou implementovány v ovladači pro robot Morbot:
• Morbot Register a
• Morbot Init,
kde první část názvů funkcí je jméno ovladače.
Funkce Morbot Register uvedená ve výpisu 10 je registrační funkcí ovladače. Prototyp této funkce void Morbot Register(DriverTable* table); není v hlavičkovém souboru
ovladače, ale v souboru player/server/libplayerdrivers/driverregistry.cc, který
obsahuje registrace všech ovladačů.
/* registrační funkce ovladače morbot */
void Morbot_Register(DriverTable* table) {
table->AddDriver("morbot", Morbot_Init);
}
Výpis 10: Funkce Morbot Register(DriverTable* table).
Registrační funkce spouští inicializaci ovladače, výpis 11. Vzniká nová instance třídy a
je načítán konfigurační soubor specifikovaný při spuštění Playeru.
/* inicializace ovladače, načtení konfiguračního souboru */
Driver* Morbot_Init( ConfigFile* cf, int section) {
return((Driver*)(new Morbot(cf, section)));
}
Výpis 11: Funkce Morbot Init(ConfigFile* cf, int section).
Konfigurační soubor se dále zpracovává v konstruktoru třídy ovladače, ve kterém je
ověřována přítomnost definic rozhraní. Vytvořena jsou pouze rozhraní specifikovaná v konfiguračním souboru. Jako příklad je uvedena část konstruktoru ovladače ve výpisu 12.
V konstruktoru jsou také načítány hodnoty speciálních proměnných ovladače, v našem případě se jedná o hodnotu proměnné port. Pokud není hodnota proměnné v konfiguračním
souboru uvedena, je použita hodnota konstanty AVR DEFAULT PORT, příklad je uveden
ve výpisu 13.
/* Test na přítomnost požadavku v konfiguračním souboru */
/* na vytvoření rozhraní pro vyčítání odometrie. */
if (cf->ReadDeviceAddr(&(this->position_id),
section,
15/60
"provides",
PLAYER_POSITION2D_CODE,
-1,
NULL) == 0) {
if(this->AddInterface(this->position_id) != 0) {
this->SetError(-1);
return;
}
}
Výpis 12: Test konfiguračního souboru na přítomnost rozhraní position2d.
/* načtení hodnoty proměnné port z konfiguračnního souboru */
serial_port = cf->ReadString(section, "port", AVR_DEFAULT_PORT);
Výpis 13: Načtení proměnné port z konfiguračního souboru.
Pro sestavení serverové aplikace Player s nově implementovaným ovladačem je nutné
zahrnout všechny zdrojové soubory ovladače do adresářové struktury Playeru. V případě
ovladače pro robot Morbot se jedná o adresář player/server/drivers/mixed/morbot.
Dále je nutné upravit všechny soubory Makefile.am a Makefile.in, které obsahují data
potřebná pro sestavení Playeru.
3.3.2
Implementace ovladače pro robot Morbot
Ovladač pro Morbot implementuje standardní rozhraní Playeru pro přístup k hardware.
Hlavním úkolem ovladače je komunikace s řídicí deskou a úprava vyčtených dat tak, aby
je bylo možné předat klientské aplikaci. Samotný server je spuštěn na palubním počítači
Morbotu, klientská aplikace pak studenty na jejich pracovní stanici. Komunikace mezi
serverem a klientem probíhá po síti, použitý protokol je TCP/IP.
Ovladač Morbot se skládá ze tří tříd:
1. SerialPort poskytuje rozhraní pro komunikaci palubního počítače s řídicí deskou po
sériové lince. Obsahuje funkce pro nastavení portu, čtení a zápis.
2. AVR obsahuje jednoduchý protokol pro komunikaci na základě modelu žádost/odpověď.
3. Morbot je vlastní implementací ovladače. Kromě pěti povinných funkcí obsahuje také
funkci ProcessMessages zajišťující zpracování zpráv, které jsou přijímány od klientské
aplikace.
Hierarchie tříd je ukázána na obrázku 7.
16/60
Obrázek 7: Hierarchie tříd v ovladači pro robot Morbot.
Výpis 14 obsahuje zdrojový kód funkce Setup. Funkce je využita pro otevření sériového
portu a komunikaci s řídicí deskou. Vlákno ovladače, funkce Main, je spuštěno voláním
funkce StartThread.
int Morbot::Setup() {
/* avr - instance třídy avr.cc */
/* Otevření a nastavení sériového portu. */
avr.openSerial(serial_port);
/* Vypnutí motorů z důvodu bezpečnosti. */
avr.setMotorsOff();
motors_enabled = false;
/* Vytvoření vlákna a spuštění funlce Main. */
StartThread();
return 0;
}
Výpis 14: Funkce Setup.
Funkce Main, která je obsažena ve výpis 15, obsahuje nekonečnou smyčku, ve které jsou
aktivně vyčítána data z řídicí desky. Tato data jsou ukládána do nadefinovaných vnitřních
struktur Playeru a následně předávána klientské aplikaci. Pro přehlednost je uvedeno pouze
vyčítání odometrických dat a dat ze sonarů.
Z důvodu nedokonalé simulace infračervených dálkoměrných senzorů v Playeru používané verze 2.0.3 jsou tyto senzory poskytovány prostřednictvím rozhraní pro sonary, neboť
věrnější simulace infračervených dálkoměrů je možné dosáhnout konfigurací modelu sonarů. Při simulaci jsou proto infračervené dálkoměry vyčítané rozhraním sonarů a tedy i
reálný robot poskytuje toto rozhraní pro snadnější přenos aplikace z prostředí simulátoru.
Ve zdrojovém kódu ovladače je připravena implementace příslušného rozhraní pro vyčítání
infračervených dálkoměrných senzorů, ale není používáno.
void Morbot::Main() {
/* Inicializace. */
player_position2d_data_t pos_data;
player_sonar_data_t sonar_data;
17/60
sonar_data.ranges_count = 2;
/* Spuštění motorů. */
avr.setMotorsOn();
motors_enabled = true;
for(;;) {
/* Test na běh vlákna. */
pthread_testcancel();
/* Zpracování zpráv. */
ProcessMessages();
/* Vyčtení odometrických dat */
/* a jejich odeslání klientské aplikaci. */
avr.getPosition(&pos_data.pos.px, &pos_data.pos.py,
&pos_data.pos.pa);
this->Publish(position_id,
NULL,
PLAYER_MSGTYPE_DATA,
PLAYER_POSITION2D_DATA_STATE,
(void*)&(pos_data),
sizeof(player_position2d_data_t),
NULL);
/* Vyčtení dat ze sonarů */
/* a jejich odeslání klientské aplikaci. */
avr.getIRs(&sonar_data.ranges[0],
&sonar_data.ranges[1],
&sonar_data.ranges[2],
&sonar_data.ranges[3]);
avr.getSonars(&sonar_data.ranges[4],
&sonar_data.ranges[5]);
this->Publish(sonar_id,
NULL,
PLAYER_MSGTYPE_DATA,
PLAYER_SONAR_DATA_RANGES,
(void*)&sonar_data,
sizeof(sonar_data),
NULL);
}
}
Výpis 15: Funkce Main.
Funkce ProcessMessages zajišťuje zpracování zpráv, které jsou přijímány od klientské
aplikace. Obsluha zpráv je založena na jednoduchém principu porovnání hlavičky přijaté
18/60
zprávy s hlavičkami obsluhovaných zpráv funkcí MatchMessage. Ve výpisu 16 je uvedena
obsluha žádosti o zaslání skutečné velikosti robotu.
if (Message::MatchMessage(hdr,
PLAYER_MSGTYPE_REQ,
PLAYER_POSITION2D_REQ_GET_GEOM,
position_id)) {
memset(&pos_geom, 0, sizeof(pos_geom));
/* Offset robotu
pos_geom.pose.px
pos_geom.pose.py
pos_geom.pose.pa
od středu otáčení. */
= 0;
= 0;
= 0;
/* Velikost robotu. */
pos_geom.size.sw = 0.24;
pos_geom.size.sl = 0.23;
Publish(position_id,
resp_queue,
PLAYER_MSGTYPE_RESP_ACK,
PLAYER_POSITION2D_REQ_GET_GEOM,
(void*)&pos_geom,
sizeof(pos_geom),
NULL);
return 0;
}
Výpis 16: Funkce MatchMessage.
Voláním funkce Shutdown, výpis 17, je zajištěno korektní ukončení vlákna ovladače.
int Morbot::Shutdown() {
/* Ukončení vlákna ovladače.
StopThread();
return 0;
}
*/
Výpis 17: Funkce Shutdown.
19/60
4
Úlohy řešené v rámci předmětu Mobilní robotika
Cílem předmětu Mobilní robotika, X33MOR, je seznámit studenty s typickými úlohami
řešenými v mobilní robotice, se základní strukturou mobilních robotů a jejich senzorickým
vybavením. Cvičení probíhají v laboratoři nejen v robotickém simulátoru, ale také na
reálných robotech. V rámci cvičení jsou studenti rozděleni do malých skupin z důvodu
časové náročnosti řešení jednotlivých úloh a získání praktických zkušeností s prací v malém
týmu. Spolupráce jednotlivých členů týmů je podpořena systémem pro verzování souborů
Subversion (dále SVN). Každá skupina řeší pět na sebe navazujících úloh:
• řízení pohybu a lokalizace robotu podle odometrických dat,
• využití dálkoměrných senzorů pro detekci překážek,
• implementace algoritmu třídy Bug pro vyhýbání se překážkám,
• řízení pohybu podle dat z inteligentní kamery a
• tvorba topologické mapy.
Pro řešení jednotlivých úloh potřebují studenti několik základních dovedností, kterými
jsou
• práce s prostředím Player/Stage,
• práce s SVN a
• tvorba klientské aplikace.
První z těchto dovedností již byla popsána v kapitole Player/Stage. Způsob práce s SVN
a tvorba klientské aplikace v Playeru bude ukázána v následujících částech této kapitoly.
Studenti mají k dispozici šablonu úloh. Šablona je připravena ke stažení z repozitáře na
začátku semestru a můžeme ji považovat za osnovu práce na předmětu. Její součástí jsou:
• sestavitelný zdrojový kód klientské aplikace s prázdnými funkcemi, jejichž komentáře
jsou specifikací funkcionality funkce v každé úloze,
• soubory pro sestavení aplikace,
• konfigurační soubory Playeru,
• definice modelu světů pro jednotlivé úlohy,
• definice modelu robotu a
20/60
• bitmapové obrázky reprezentující podobu světů pro každou z úloh.
Adresářová struktura šablony je poměrně rozsáhlá, proto se v jejím kořenovém adresáři
nacházejí dva podadresáře:
• Adresář gB, kde B je název skupiny studentů. Tento adresář obsahuje zdrojové kódy
úloh, příslušné soubory Makefile pro sestavení aplikace a po provedení prvního sestavení také spustitelný soubor. Adresářová struktura je zobrazena na obrázku 8.
• Adresář worlds obsahující konfigurační soubory a podadresář maps, ve kterém jsou
uloženy bitmapové obrázky prostředí.
Obrázek 8: Část adresářové struktury šablony.
V následujícím oddíle je ukázán princip SVN a základy práce s ním. Poté je popsáno
vytvoření klientské aplikace a vyčítání dat z robotu, a to jak z modelu robotu v simulátoru,
tak z Morbotu. V následujících oddílech jsou popsány příklady řešení úloh, které jsou
organizovány následujícím způsobem:
21/60
4.1 Verzovací systém SVN
1. motivace pro vypracování úlohy,
2. zadání předkládané studentům,
3. rozbor zadání a tematické pozadí úlohy,
4. popis implementace a
5. přenos aplikace ze simulátoru na Morbot, který je podrobněji popsán v prvních úlohách, v ostatních se již příliš neliší.
4.1
Verzovací systém SVN
SVN [7] je nástroj, který se využívá pro správu verzí projektu (jeho zdrojových kódů)
na kterém pracuje více uživatelů najednou. S výhodou jej také může využít jediný uživatel
pracující na více počítačích, například doma a ve škole. Verze souborů jsou udržovány
v centrálním repozitáři. Uživatelé přistupují k verzím souborů po síti. Každý uživatel si
na svém počítači udržuje pracovní kopii, kterou modifikuje a synchronizuje s repozitářem.
Tento princip je zobrazena na obrázku 9.
Obrázek 9: Princip SVN.
Pro práci s SVN existuje několik základních příkazů:
• svn checkout slouží pro stažení celého repozitáře. Je prvním krokem pro začátek
používání již vytvořeného repozitáře.
• Příkaz svn update je určen pro aktualizaci verzí pracovní kopie z repozitáře.
• Příkazem svn commit jsou nahrány změny provedené na pracovní kopii do repozitáře.
Každý commmit je vhodné komentovat a doplnit tak provedené změny jejich popisem.
• svn add fileName přidá nový soubor nebo adresář do repozitáře. Přidání proběhne
až po provedení příkazu svn commit.
22/60
4.2 Implementace klientské aplikace Podpůrný software pro výuku mobilní robotiky
• svn log slouží pro zobrazení změn v repozitáři. Zobrazovány jsou uživatelská jména
těch, kteří provedli změny v souborech, a také zprávy zadané při nahrání změn do
repozitáře příkazem commit.
Uvedené příkazy lze použít přímo z příkazového interpretu. Také je možné používat některou z grafických nadstaveb, ve kterých bývají jednotlivé úkony pojmenovány stejně.
Pokud více uživatelů změní stejnou část souborů, musí být vyřešen konflikt. Správce
verzí nenabízí řešení těchto konfliktů, ale poskytuje nástroje pro jejich řešení. Odstranění
konfliktu je třeba explicitně označit příkazem svn resolved.
Nespornou výhodou používání SVN je uživatelské pohodlí. Uživatel nemusí vytvářet
několik verzí jednoho souboru nebo projektu, ale může mít na svém počítači pouze jedinou
verzi pracovní kopie a ostatní verze v repozitáři.
4.2
Implementace klientské aplikace
Neodmyslitelnou součástí klientské aplikace je zajištění komunikace se serverem. Pro
tyto účely existují v Playeru dvě třídy, PlayerClient a ClientProxy.
Třída PlayerClient [8] obsahuje funkce nejen pro řízení komunikace mezi serverem a
klientem, její součástí jsou také funkce určené k získání a modifikaci parametrů připojení
k serveru a jeho přerušení. Při tvorbě instance třídy PlayerClient předáváme konstruktoru
dva parametry:
1. const char *host - řetězec s adresou pro připojení k serveru po síti, nebo řetězec
”localhost” pro připojení k serveru spuštěném na stejném počítači jako klientská
aplikace. Pokud tento parametr ponecháme prázdný, připojení neproběhne. V takovém případě je možné se připojit k serveru později voláním funkce Connect.
2. const int port - číslo portu, se kterým je spuštěn server, standardně je nastavena
hodnota 6665.
V tabulce 2 jsou popsány základní funkce třídy PlayerClient.
Pro následující implementaci úloh je nejdůležitější funkcí Read, která přijímá zprávy ze
serveru a přijatá data ukládá do nadefinovaných datových struktur Playeru. Data přijatá od
různých zařízení jsou ukládána do odlišných struktur, ke kterým klient přistupuje instancí
příslušné třídy odvozené od třídy ClientProxy. Jména odvozených tříd korespondují se
jmény rozhraní Playeru. Přehled základních tříd je uveden v tabulce 3. Dokumentace třídy
je dostupná na webových stránkách [9].
Přístup k odometrickým datům získáme vytvořením instance třídy Position2DProxy
s parametry:
1. PlayerClient *pc - ukazatel na instanci třídy PlayerClient,
23/60
2. unsigned short index - index určující zařízení, se kterým se má pracovat. Většinou se
jedná o nulu, ta označuje první zařízení příslušného typu.
Základní funkce třídy Position2DProxy jsou popsány v tabulce 4.
Název funkce
Read
Peek
SetFrequency
SetDataMode
Funkcionalita
Příjem zpráv ze serveru. Funkce blokuje další činnost
klientské aplikace, dokud není přečtena alespoň jedna
zpráva.
Zjištění přítomnosti zprávy na serveru. Návratovou hodnotou je 0, pokud nejsou k dispozici žádná data, 1 při
přítomnosti dat, nebo -1, pokud byla zjištěna chyba.
Nastavení frekvence pro příjem dat v módu PUSH.
Nastavení modelu komunikace mezi serverem a klientem.
Tabulka 2: Základní funkce třídy PlayerClient.
Název třídy
BumperProxy
IRProxy
LaserProxy
MotorProxy
Position2DProxy
Obsluhované zařízení
nárazníky
infračervené dálkoměrné senzory
laserový dálkoměr
stav motorů
odometrie
Tabulka 3: Přehled základních rozhraní proxy.
Název funkce
SetSpeed
Funkcionalita
Nastavení rychlosti robotu. Funkce může být použita pro řízení holonomních robotů s parametry double
xspeed, double yspeed a double yawspeed, stejně jako pro
řízení neholonomních robotů a parametry double speed,
double turnrate.
SetOdometry
Nastavení nové hodnoty odometrie uživatelem, parametry jsou hodnoty x, y a úhel natočení robotu.
ResetOdometry Reset odometrie, tzn. nastavení hodnot pozice x, y a
úhlu na nulu.
GetXPos
Načtení souřadnice x pozice robotu.
GetYPos
Načtení souřadnice y pozice robotu.
GetYaw
Načtení natočení robotu.
Tabulka 4: Základní funkce třídy Position2DProxy.
24/60
Název funkce
GetCount
GetBlob
Funkcionalita
Získání počtu detekovaných jednobarevných oblastí.
Uložení informací o jedné oblasti do proměnné typu playerc blobfinder blob t.
Tabulka 5: Základní funkce rozhraní BlobfinderProxy
SonarProxy je používána pro přístup k datovým strukturám sonarů. Vytvoření instance
třídy SonarProxy je shodné s vytvořením instance třídy Position2DProxy. Používanou
funkcí na cvičeních je funkce GetScan(index), která vyčítá poslední naměřená data sonary.
Dalším používaným rozhraním je rozhraní BlobfinderProxy, které je využíváno při vyhledávání spojitých jednobarevných oblastí v obraze pořízeném kamerou. Základní funkce,
které budou využívány v řešených úlohách, jsou vypsány v tabulce 5.
Pro zjednodušení přístupu k datovým strukturám Playeru je v šabloně pro cvičení provedeno zapouzdření používaných rozhraní. Zapouzdření je vytvořeno pro usnadnění práce
na prvních z hlediska množství nových informací poměrně náročných cvičeních. Studenti
se nemusejí hned na začátku práce probírat dokumentací k jednotlivým třídám odvozených
od ClientProxy, ale mohou si projít pouze hlavičkové soubory zapouzdřujících tříd a začít
s implementací úloh. Pro řešení náročnějších úloh již bude dokumentace zapotřebí, avšak
v té době budou mít studenti osvojeny základní principy Playeru a budou mít s prací
v tomto prostředí jisté zkušenosti. Z těchto důvodů bude orientace v dokumentaci a její
použití značně jednodušší.
Příkladem použití rozhraní pro vyčítání dat je výpis 18, ve kterém je uveden způsob
vytvoření instancí tříd zapouzdřujících výše uvedená rozhraní. Použití těchto instancí pro
získání dat z datových struktur Playeru je uveden ve výpisu 19.
CMorbot::CMorbot(int port) {
robot = new PlayerClient("localhost", port);
drive = new CDrive(robot);
ranger = new CRangeFinder(robot, 0);
}
Výpis 18: Konstruktor třídy CMorbot.
/* Načtení odometrických dat. */
double positionX = drive->getX();
double positionY = drive->getY();
double yaw = drive->getYaw();
/* Načtení naměřených dat prvního sonaru. */
double range = ranger->getData(0);
Výpis 19: Použití instancí tříd Proxy pro načtení dat.
25/60
4.3 Řízení pohybu robotu
4.3
4.3.1
Úloha I - Řízení pohybu robotu
Motivace
Studenti se seznámí s principem řízení robotu a jeho lokalizací na základě odometrických
dat. Řešením úlohy je jednoduchý regulátor rychlostí robotu, po úspěšné implementaci bude
robot schopen prvních autonomních pohybů v prostředí bez překážek.
4.3.2
Zadání
Implementujte regulátor dopředné a úhlové rychlosti robotu pro jeho přemístění na
předem určenou polohu. Souřadnice požadované polohy robotu budou zadávány relativně
vzhledem k jeho počáteční poloze.
4.3.3
Rozbor zadání
Aktuální poloha robotu i požadovaná konečná poloha robotu je udávána v kartézském
souřadném systému (x,y). Regulátor dopředné a úhlové rychlosti je popsán rovnicemi (1).
Souřadný systém robotu, stejně jako význam použitých symbolů v rovnicích (1), je zřejmý
z obrázku 10. Uvedený regulátor je proporcionální a je založen na záporné zpětné vazbě.
Konstanta REG FORWARD RATE respektive REG TURN RATE je konstantou regulátoru pro dopřednou respektive úhlovou rychlost.
0
Px
0
Py
v
ω
4.3.4
=
=
=
=
(Px − Rx ) · cos(ϕ) + (Py − Ry ) · sin(ϕ)
−(Px − Rx ) · sin(ϕ) + (Py − Ry ) · cos(ϕ)
0
REG FORWARD RATE · Px
0
REG TURN RATE · Py
(1)
Implementace regulátoru
V kódu šablony je potřeba upravit tři funkce:
• moveTo(double positionX, double PositionY),
• printStatus() a
• funkci main(int argc, char* argv[]).
První dvě funkce jsou součástí třídy CMorbot, která je v adresářové struktuře šablony
umístěná v adresáři src/device. Funkce main je implementována v souboru src/main/
morbot.cpp.
26/60
Obrázek 10: Souřadný systém robotu.
Funkce moveTo je v této úloze klíčovou. Jejími parametry jsou relativní cílové souřadnice vzhledem k počáteční poloze robotu. Jako počáteční poloha robotu je v simulátoru
považována poloha robotu při spuštění klientské aplikace. U reálného robotu je počáteční
polohou aktuální informace o poloze načtená z řídicí desky při spuštění aplikace. Poloha
může být resetována buď voláním funkce resetOdometry() nebo hardwarově, případně nastavena funkcí setOdometry(positionX, positionY).
Funkce moveTo obsahuje cyklus, který vyčítá data z robotu a počítá akční veličinu regulátoru. Podmínkou ukončení smyčky je rovnost požadované a aktuální polohy robotu.
Jakmile se tyto parametry rovnají, vrací funkce hodnotu true. Příklad je uveden ve výpisu 20.
/* Tolerance
const double
/* Konstanty
const double
const double
dosažení cíle v metrech. */
TARGET_REACH_TOLERATION = 0.03;
regulátoru. */
REG_FORWARD_RATE = 0.5;
REG_TURN_RATE = 1.5;
/*
* Proporcionální regulátor rychlosti pro
* pohyb robotu na zadané souřadnice positionX, positionY.
*/
bool CMorbot::moveTo(double positionX,double positionY) {
robot->Read();
positionX += drive->getX();
27/60
positionY += drive->getY();
bool stop = false;
while (stop==false) {
robot->Read();
/* Vzdálenost aktuální polohy robotu a cíle. */
float dx = positionX - drive->getX();
float dy = positionY - drive->getY();
/* Natočení robotu.*/
float yaw = drive->getYaw();
/* Výpočet regulátoru. */
float px = dx*cos(yaw) + dy*sin(yaw);
float py = -dx*sin(yaw) + dy*cos(yaw);
/* Nastavení rychlostí robotu. */
drive->setSpeeds(REG_FORWARD_RATE*px,REG_TURN_RATE*py);
printStatus();
/* Pokud je Euklidovská vzdálenost do cíle menší než tolerance, */
/* robot dosáhl cíle. */
if ((dx*dx + dy*dy)
< TARGET_REACH_TOLERATION*TARGET_REACH_TOLERATION){
stop = true;
}
}
drive->setSpeeds(0.0, 0.0);
return stop;
}
Výpis 20: Funkce moveTo.
Požadovaná i aktuální poloha robotu je ukládána v plovoucí desetinné čárce, dle standardu IEEE 754. Při porovnávání operátorem == jsou uvažována všechna desetinná místa,
proto je potřeba rozmyslet toleranci dosažení požadované polohy. V uvedeném příkladu
funkce moveTo, výpis 20, se jedná o tři centimetry.
Další modifikovanou funkcí v první úloze je funkce printStatus, příklad je uveden ve
výpisu 21. Tato funkce je používána pro výpis aktuálního stavu robotu a ladění programu.
V první úloze se jedná o výpis aktuální polohy robotu, souřadnic x, y a úhlu natočení.
/*
* Výpis aktuální pozice (x,y) robotu.
*/
void CMorbot::printStatus() {
std::cout << "x: " << drive->getX() << ’\n’;
28/60
std::cout << "y: " << drive->getY() << ’\n’;
std::cout << "uhel: " << drive->getYaw() << ’\n’;
}
Výpis 21: Funkce printStatus.
Ve funkci main je realizováno volání funkce moveTo, které jsou předány relativní cílové
souřadnice robotu.
Možný průběh první úlohy v simulátoru je ukázán na obrázku 11.
Obrázek 11: Průběh první úlohy v simulátoru.
4.3.5
Spuštění aplikace
Pro spuštění Playeru si vybereme konfigurační soubor task1.cfg z připravené šablony,
soubor je uveden ve výpisu 9. Konfigurační soubor vytváří grafické prostředí simulátoru a
rozhraní position2d pro práci s odometrickými daty. Klientskou aplikaci sestavíme příkazem
gmake v adresáři src. Z adresáře bin spustíme klientskou aplikaci příkazem ./morbot.
Pro spuštění Playeru na Morbotu použijeme odlišný konfigurační soubor, je potřeba
použít ovladač, který byl pro robot implementován. Takový konfigurační soubor je uveden
ve výpisu 22. Serverovou část Playeru spustíme na palubním počítači Morbotu a klientskou
aplikaci na studentské pracovní stanici.
29/60
driver
(
name "morbot"
provides ["position2d:0"]
port "/dev/ttyS2"
)
Výpis 22: Konfigurační soubor morbot.cfg - vytvoření rozhraní position2d.
Také je nutné zaměnit hodnotu ”localhost”, která je parametrem pro vytvoření instance třídy playerClient, za adresu robotu ve formě textového řetězce. Z implementačního
hlediska je vhodné zadávat hodnoty port a host jako parametry při spouštění klientské
aplikace. Pokud se rozhodneme pro spuštění klientské aplikace příkazem ./morbot 6665
10.10.20.51, kde prvním argumentem je port a druhým host, je možné řešení ukázáno ve
výpisu 23.
using namespace PlayerCc;
int main(int argc,char* argv[]) {
/* Načtení parametrů programu. */
int port = (argc > 1)? atoi(argv[1]) : 6665;
const char *host;
host = (argc > 2)? argv[2] : "localhost";
/* Vytvoření instance třídy CMorbot a současně */
/* připojení klienta k serveru. */
CMorbot morbot(host, port);
morbot.moveTo(1.0,3.0);
return 0;
}
Výpis 23: Ukázka funkce main.
Reálný robot má jiné dynamické vlastnosti než model robotu v simulátoru. Experimentálně bylo ověřeno, že při přenosu aplikace na Morbot je vhodné zvýšit poměr mezi
konstantou pro nastavení úhlové rychlosti a konstantou pro nastavení dopředné rychlosti.
Konkrétní hodnotou konstanty pro nastavení úhlové rychlosti REG TURN RATE je 1,5,
konstanta pro nastavení dopředné rychlosti REG FORWARD RATE má hodnotu 0,5.
30/60
4.4 Detekce překážek
4.4
4.4.1
Úloha II - Detekce překážek dálkoměrnými senzory
Motivace
Studenti se seznámí s použitím dálkoměrných senzorů, s jejich principy a vlastnostmi.
Pro řešení úlohy bude použit již implementovaný regulátor pohybu robotu dle odometrických dat a přidána nová funkcionalita, bezdotyková detekce překážek.
4.4.2
Zadání
Modifikujte regulátor z první úlohy tak, aby byl robot schopen detekovat překážku, která
mu byla umístěna do cesty. Umístění, počet a velikost překážek je libovolný, definován je
pouze minimální rozměr překážky - 5x5x5cm. K dispozici máte dva druhy dálkoměrných
senzorů:
• infračervené dálkoměrné senzory (dále IR senzory) a
• ultrazvukové dálkoměrné senzory (dále sonary).
4.4.3
Vlastnosti dálkoměrných senzorů
Infračervené dálkoměrné senzory jsou založeny na principu triangulace. Jejich součástí
je LED (Light Emitting Diode), která emituje úzký světelný paprsek v infračerveném spektru. Paprsek se odráží od reflexních povrchů a následně je senzorem detekován. Odražené
paprsky dopadají na řádkový senzor CCD (Charge-Coupled Device). Úměrně úhlu dopadajícího paprsku je generována hodnota napětí. Úhel je úměrný vzdálenosti mezi senzorem
a detekovanou překážkou. Tento princip je zobrazen na obrázku 12.
Obrázek 12: Princip IR dálkoměrného senzoru.
Použité IR senzory Sharp GP2D120XJ00F mají nelineární převodní charakteristiku uvedenou na obrázku 13. Pracovní oblastí senzorů je přibližně vzdálenost mezi třemi a čtyřiceti
31/60
Obrázek 13: Převodní charakteristika IR senzorů, převzato z [10].
centimetry od překážky. S tím souvisí umístění senzorů na robotu. Pokud je senzor umístěn po obvodu robotu, je nutné počítat s tím, že vzdálenosti menší než tři centimetry není
možné změřit správně. Technická specifikace senzorů je dostupná na [10].
Sonary fungují na principu měření času mezi momentem vyslání akustického impulsu a
momentem detekce zvuku odraženého od překážky. Známá rychlost zvuku ve vzduchu a
změřený čas letu zvuku umožní vypočítat vzdálenost k překážce. Vzdálenost je zjišťována
přímo senzorem a data jsou z něj vyčítána jako vzdálenost k překážce v jednotkách centimetrů nebo palců. Alternativně je možné vyčítání doby letu v jednotkách mikrosekund.
Tento způsob má tu výhodu, že vlastní vzdálenost je vypočtena uživatelem senzoru, který
může upravit hodnotu rychlosti zvuku ve vzduchu podle teploty okolního vzduchu a tím
získat přesnější měření.
Obrázek 14: Původ falešných ech sonaru.
Jednou z vlastností sonarů je šířka vyzařovacího laloku a vznik falešných ech (vysvětlení
pojmu na obrázku 14), které vedou k obtížné interpretaci dat. Sonary typu SRF10, které
patří k senzorickému vybavení Morbotu, mají vyzařovací charakteristiku širokou přibližně
72◦ . Vyzařovací lalok tak sice pokryje větší prostor, ale zároveň je potřeba počítat s větším
32/60
množstvím falešných ech. Vyzařovací charakteristika je ukázána na obrázku 15. Technická
specifikace používaných sonarů je ke zhlédnutí na webových stránkách [11].
Obrázek 15: Vyzařovací charakteristika sonaru SRF10, převzato z [11].
4.4.4
Vytvoření modelu senzorů v simulátoru Stage
Z důvodu nedostatečné podpory simulace IR senzorů ve verzi používaného Stage (2.0.3)
jsou oba dva druhy senzorů vyčítány a simulovány jako sonary. Rozdíl mezi senzory je
specifikován při tvorbě modelu robotu, v našem případě v souboru morbot.inc - příklad
uveden ve výpisu 5. Definice IR senzorů (výpis 24) se od definice sonarů (výpis 25) liší
hlavně pracovní oblastí a vyzařovacím úhlem senzorů.
define morbot_sharp ranger
(
# počet definovaných senzorů
scount 4
# pozice
spose[0]
spose[1]
spose[2]
spose[3]
jednotlivých senzorů na robotu [xpos ypos heading]
[ 0.00 0.08 45 ]
[ 0.12 0.1 00 ]
[ 0.12 -0.1 00 ]
[0.00 -0.08 -45 ]
# minimální a maxmální možná měřená vzdálenost,
# zorný úhel senzorů [range_min range_max view_angle]
sview [0.03 0.4 1]
# velikost senzorů [xsize ysize] v metrech
ssize [0.01 0.03]
)
Výpis 24: Definice modelu IR senzorů.
33/60
define morbot_sonar ranger
(
# počet sonarů
scount 2
# pozice sonarů na robotu [xpos ypos heading]
spose[0] [ 0.1 0.08 00 ]
spose[1] [0.1 -0.08 00 ]
# definice vlastností senzorů
sview [0.05 6 72]
ssize [0.01 0.03]
)
Výpis 25: Definice modelu sonarů.
Pro vytvoření dvou rozhraní modelů senzorů v Playeru je potřeba pozměnit konfigurační
soubor Playeru tak, jak je ukázáno ve výpisu 26.
driver
(
name "stage"
provides ["simulation:0" ]
plugin "libstageplugin"
worldfile "task2.world"
)
driver
(
name "stage"
# vytvoření rozhraní pro práci s odometrií a dvěma modely
# dálkoměrných senzorů
provides ["position2d:0" "sonar:0" "sonar:1"]
model "robot1"
)
Výpis 26: Konfigurační soubor pro vytvoření dvou rozhraní sonarProxy.
Jako kontrola správného vytvoření dvou rozhraní může sloužit výpis Playeru na standardní výstup po spuštění, který je uveden ve výpisu 27. Tři po sobě jdoucí červeně označené řetězce znamenají:
1. vytvoření modelu robotu s názvem robot1 v simulátoru Stage,
34/60
2. vytvoření rozhraní sonarProxy pod názvem ranger s indexy 0 a 1. Podle těchto indexů
jsou jednotlivé modely senzorů rozlišovány v klientské aplikaci.
zirafka@elemnia:~/Pracovni/morbot_ulohy/task2/worlds$ player task2.cfg
* Part of the Player/Stage/Gazebo Project [http://playerstage.sourceforge.
net].
* Copyright (C) 2000 - 2006 Brian Gerkey, Richard Vaughan, Andrew Howard,
* Nate Koenig, and contributors. Released under the GNU General Public
License.
* Player comes with ABSOLUTELY NO WARRANTY. This is free software, and
you
* are welcome to redistribute it under certain conditions; see COPYING
* for details.
trying to load /home/zirafka/Pracovni/morbot_ulohy/task2/worlds/./
libstageplugin...
trying to load /usr/local/lib/libstageplugin...
success
invoking player_driver_init()...
Stage driver plugin init
** Stage plugin v2.0.3 **
* Part of the Player/Stage Project [http://playerstage.sourceforge.net]
* Copyright 2000-2006 Richard Vaughan, Andrew Howard, Brian Gerkey
* and contributors. Released under the GNU General Public License v2.
success
Stage driver creating 1 device
6665.31.0 is a Stage world [Loading ./task2.world][Include morbot.inc][
Include map.inc]
Stage driver creating 3 devices
6665.4.0 is "robot1"
6665.5.0 is "robot1.ranger:0"
6665.5.1 is "robot1.ranger:1"
Listening on ports: 6665
Stage: Request close window.
Stage: Confirmed
Quitting.
Výpis 27: Výpis Playeru - kontrola vytvoření dvou rozhraní pro dálkoměrné senzory.
Pro vyčítání dat z obou definovaných modelů je nutné v konstruktoru třídy CMorbot implementovat vytvoření dvou různých instancí třídy CRangeFinder, která obsahuje rozhraní
35/60
SonarProxy. Část tohoto konstruktoru je uvedena ve výpisu 28.
ranger = new CRangeFinder(robot,0);
sonar = new CRangeFinder(robot,1);
Výpis 28: Vytvoření instancí třídy CRangeFinder pro vyčítání dat ze dvou modelů
dálkoměrných senzorů.
4.4.5
Implementace úlohy v simulátoru
V rámci druhé úlohy, detekce překážek z dálkoměrných senzorů, je potřeba upravit dvě
funkce:
• moveTo(double positionX, double positionY) a
• printStatus().
Obě funkce se nacházejí v souboru CMorbot.cpp v adresáři src/devices v adresářové
struktuře šablony.
Pro změnu funkcionality funkce moveTo může být použit jeden z následujících přístupů:
1. Při detekci překážky, která brání robotu v další cestě, je robot zastaven, návratovou
hodnotou funkce je false a program končí neúspěchem.
2. Po detekci překážky je rychlost robotu nastavena na nulu až do momentu odstranění
překážky. Jakmile se naměřené hodnoty senzory blíží k jejich maximálnímu rozsahu,
překážka byla odstraněna a robot může pokračovat k cíly. Po dojezdu do cíle je
návratovou hodnotou funkce true.
Implementace druhé možnosti řešení je názorná při demonstraci funkčnosti senzorů, proto
se jí zde budeme zabývat. Průběh algoritmu dle druhého přístupu je zobrazen v prostředí
simulátoru Stage na obrázku 16.
36/60
(a) Zastavení robotu před překážkou.
(b) Dojezd robotu do cíle po odstranění překážky.
Obrázek 16: Možný průběh druhé úlohy.
37/60
Modifikací funkce moveTo oproti předchozí úloze je přidání detekce překážky. Jsou kontrolovány vzdálenosti naměřené dálkoměrnými senzory. Jakmile je alespoň jedna naměřená
vzdálenost k překážce menší než předem určený práh, je detekována překážka a robot
musí co nejrychleji zastavit. Jakmile se naměřené hodnoty přiblíží k maximálnímu rozsahu
senzorů, překážka byla odstraněna a robot může pokračovat v jízdě. Modifikovaná funkce
moveTo je ukázána ve výpisu 29.
/* Tolerance dosažení cíle v metrech. */
const double TARGET_REACH_TOLERATION = 0.3;
/* Konstanty regulátoru. */
const double REG_FORWARD_RATE = 0.5;
const double REG_TURN_RATE = 1.5;
/* Práh měřených hodnot dálkoměrných senzorů, */
/* při jeho překročení detekce překážky. */
const double THRESHOLD = 0.2;
/* Indexy senzorů předávané SonarProxy pro načítání jejich hodnot. */
/* Číselné hodnoty indexů jsou závislé na pořadí specifikace */
/* senzorů v modelu, respektive jejich zapojení na Morbotu.*/
/* Boční levý IR senzor. */
const int IR_LEFT = 0;
/* Přední levý. */
const int IR_FRONT_LEFT = 1;
/* Přední pravý. */
const int IR_FRONT_RIGHT = 2;
/* Boční pravý. */
const int IR_RIGHT = 3;
/* Sonar vpravo. */
const int SONAR_RIGHT = 0;
/* Sonar vlevo. */
const int SONAR_LEFT = 1;
/*
* Regulátor rychlosti robotu.
*/
bool CMorbot::moveTo(double positionX,double positionY)
{
robot->Read();
positionX += drive->getX();
positionY += drive->getY();
bool stop = false;
while (!stop) {
38/60
robot->Read();
float r1 = ranger->getData(IR_LEFT);
float r2 = ranger->getData(IR_FRONT_LEFT);
float r3 = ranger->getData(IR_FRONT_RIGHT);
float r4 = ranger->getData(IR_RIGHT);
float s1 = sonar->getData(SONAR_LEFT);
float s2 = sonar->getData(SONAR_RIGHT);
/* Je detekována překážka? */
if(r1 < THRESHOLD ||
r2 < THRESHOLD ||
r3 < THRESHOLD ||
r4 < THRESHOLD ||
s1 < THRESHOLD ||
s2 < THRESHOLD) {
std::cout << "Prekazka!!" << ’\n’;
/* Pokud není detekována překážka, pokračuj v cestě. */
} else {
float yaw = drive->getYaw();
float dx = positionX - drive->getX();
float dy = positionY - drive->getY();
float px = dx*cos(yaw) + dy*sin(yaw);
float py = -dx*sin(yaw) + dy*cos(yaw);
drive->setSpeeds(REG_FORWARD_RATE*px, REG_TURN_RATE*py);
printStatus();
/* Bylo dosaženo cíle?*/
if((dx*dx + dy*dy)
< TARGET_REACH_TOLERATION*TARGET_REACH_TOLERATION) {
stop = true;
}
}
}
return stop;
}
Výpis 29: Funkce moveTo v druhé úloze.
4.4.6
Přizpůsobení aplikace pro Morbot
Při přenosu klientské aplikace odladěné v simulátoru na Morbot je potřeba respektovat vlastnosti reálných senzorů. Zatímco modely IR senzorů v simulátoru měří vzdálenost
39/60
4.5 Algoritmy třídy BUG
k překážce v metrech, hodnota měřená reálnými IR senzory je hodnotou napětí z AD převodníků a není dále upravována. Z důvodu této odlišnosti je nutné změřit a implementovat
převodní charakteristiku senzorů.
Řešením problému implementace může být vyhledávací tabulka nebo aproximační vztah
zjištěný z měření. Na obrázku 17 je zobrazena naměřená převodní charakteristika. Červený
průběh je spočítán podle aproximačního vztahu (2) sestaveného na základě měření, kde l
znamená vzdálenost k překážce v metrech a AD je vyčtená hodnota z AD převodníku.
l=
14
AD − 2.5
(2)
Obrázek 17: Naměřená převodní charakteristika IR senzorů (modrý průběh) a vypočtená
charakteristika aproximačním vztahem (červený průběh).
4.5
4.5.1
Úloha III - Algoritmy třídy Bug pro vyhýbání se překážkám
Motivace
Před vlastní implementací úlohy se studenti seznámí s algoritmy třídy Bug [12] pro
reaktivní řízení robotu, které mohou být použity jako inspirace při řešení třetí úlohy. Třetí
úloha kombinuje algoritmy implementované v předchozích dvou úlohách a přidává novou
funkcionalitu, objíždění překážek. Výsledkem implementace by měl být algoritmus, který
zajistí autonomní pohyb robotu v prostředí s překážkami.
40/60
4.5.2
Zadání
Implementujte algoritmus pro reaktivní řízení robotu třídy Bug, kterým docílíte dojezdu
robotu na předem určenou polohu v prostředí s libovolným množstvím překážek. Robot
by měl všechny překážky detekovat a pokusit se je objet, nedostačující je pouhé čekání na
odstranění překážky. Při řešení můžete opět využít oba druhy dálkoměrných senzorů.
4.5.3
Algoritmy třídy Bug
Algoritmy třídy BUG jsou určeny pro plánovaní cesty robotu. Cesta je posloupnost
bodů spojující počáteční a cílový bod v prostředí. Robot, který sleduje cestu, se bez kolizí
pohybuje prostředím k cíly. Podmínkou použití algoritmu je planární prostředí s libovolným
množstvím navzájem se nedotýkajících překážek, které mohou být libovolného tvaru a
velikosti. Jedná se o předem neznámé prostředí, jedinými informacemi o něm jsou:
• aktuální poloha robotu získaná lokalizací na základě odometrických dat,
• požadovaná cílová poloha a
• senzorická měření.
Tyto informaci se vztahují pouze k nejbližšímu okolí robotu, algoritmy třídy Bug tedy
spadají do skupiny algoritmů plánování pohybu s neúplnou informací o prostředí, neboli
algoritmů reaktivního plánování.
Algoritmy pro reaktivní plánovaní cesty můžeme rozdělit do dvou tříd:
Algoritmy třídy 1 jsou takové, ve kterých robot nejdříve objede celou detekovanou překážku a teprve poté se může vydat k cíly, respektive k další překážce.
Algoritmy třídy 2 nepožadují objetí celé detekované překážky, robot ji může opustit
dříve a pokračovat v cestě k cíly, respektive k další překážce.
Na první pohled se algoritmy třídy 1 mohou zdát nevýhodnými, protože generují delší
trajektorii. Tento přístup je však vhodný v případech složitějších prostředí, ve kterých by
algoritmy třídy dva mohli najít mnohem delší cestu nebo by ji nemuseli najít vůbec.
Mezi základní algoritmy patří Bug 1, respektive Bug 2, který patří mezi algoritmy
třídy 1, respektive třídy 2. V následujícím textu si ukážeme jak fungují, ale před tím je
důležité vysvětit si význam proměnných a registrů, které se v souvislosti s těmito algoritmy
používají:
• S - start, souřadnice počátečního bodu.
• T - target, souřadnice cílového bodu.
41/60
• H - hit point, neboli souřadnice bodu, ve kterém robot narazil na překážku.
• L - leave point, bod, ve kterém se robot při objezdu překážky od ní odpoutá.
• Q - bod, který náleží hranici překážky a zároveň je jeho vzdálenost k cíly nejmenší.
• R1 - registr, ve kterém jsou uloženy souřadnice nejbližšího bodu k cíly na hranici
překážky. Tento bod je zjišťován v průběhu objezdu překážky.
• R2 - délka hranice překážky počítaná od bodu H.
• R3 - délka hranice překážky počítaná od bodu Q.
Bug1
Algoritmus hledá cestu z počátečního do cílového bodu. Jedná se o algoritmus třídy 1,
tzn. že pokud robot na své cestě k cíly narazí na překážku, musí ji před pokračováním k cíly
celou objet. Trajektorie nalezená tímto algoritmem v jednoduchém prostředí je zobrazena
na obrázku 18.
Obrázek 18: Trajektorie nalezená algoritmem Bug1.
Průběh algoritmu definují tři pravidla:
1. Pohyb po spojnici bodů S a T,
- dokud není dosaženo bodu T, pak algoritmus končí.
- V případě detekce překážky:
- zapamatování souřadnic bodu H a
- aktivace druhého pravidla.
2. Sledování hranice překážky
- V případě dojezdu do bodu H:
- nalezení bodu L a
42/60
- aktivace pravidla tři.
3. Výběr směru objíždění překážky na základě informace uložené v registrech R2 a R3 ,
jízda kolem překážky vybraným směrem do bodu L. Algoritmus končí buď dosažením
cíle nebo detekcí nedosažitelného cíle.
Test na dosažení cílového bodu je založen na faktu, že algoritmus Bug1 najde na každé
detekované překážce pouze jeden bod H a jeden bod L. Bod L je bodem, který se nachází nejblíže k cíly (vzdálenost brána bez ohledu na překážky). Pokud je při opuštění
překážky z bodu L směrem k cíly detekována překážka v tomto bodě, znamená to, že cíl
je nedosažitelný. Příklad takového prostředí je na obrázku 19.
Obrázek 19: Bug1 - nedosažitelný cíl.
Bug2
Algoritmus Bug2 na rozdíl od Bug 1 neobjíždí celou detekovanou překážku, ale je mnohem více vázán na spojnici bodů S a T. Pokud robot detekuje překážku a začne ji objíždět,
snaží se v co nejdříve na tuto spojnici opět napojit. Platí, že robot může narazit pouze na
překážky, které protínají spojnici S-T a také všechny body L a H jí náleží. U algoritmu
Bug2 neplatí pravidlo, že robot narazí na překážku pouze jednou. Ke každé překážce může
existovat více bodů L a H.
Směr objíždění překážky není vybírán dle registrů R2 a R3 , ale je zvolen při implementaci algoritmu. Směr objíždění překážky může výrazně ovlivnit délku nalezené cesty. Dva
případy cest nalezené algoritmem Bug2 pro různé směry objíždění překážky jsou zobrazeny
na obrázku 20.
Následující pravidla definují průběh algoritmu:
1. Pohyb po úsečce spojující body S a T,
43/60
Obrázek 20: Trajektorie generované algoritmem Bug2.
- V případě detekce překážky:
- označení bodu H a
- aktivace druhého pravidla.
2. Objezd překážky zvoleným směrem,
- V případě dosažení spojnice S-T, pokud je vzdálenost k cíly z tohoto bodu kratší
než z bodu H a pokud v tomto bodě nestojí v cestě k cíly překážka,
- pak označení tohoto bodu jako L,
- jízda směrem k bodu T,
- a aktivace prvního pravidla.
Jinak pokračování v objezdu.
- V případě dosažení bodu H aktuální překážky a tím uzavření křivky okolo překážky je cíl nedosažitelný.
Bug12
Oba základní algoritmy, Bug1 a Bug2, mají své výhody i nevýhody. Zatímco robot
algoritmem Bug1 vždy objede celou překážku, algoritmus Bug2 je vázán na spojnici ST. Bug1 nikdy nezpůsobí lokální cykly algoritmu, ale na druhou stranu, nenalezne kratší
cestu než je obvod překážky. Algoritmus Bug2 může nalézt mnohem kratší cestu, hlavně
v jednoduchých prostředích, ale ve složitých případech také mnohem delší nebo žádnou.
44/60
Problémem druhého algoritmu jsou lokální cykly, ve kterých se robot více než dvakrát vrátí
do již navštívených bodů H a L.
Algoritmus Bug12 je kombinací základních algoritmů, snaží se využívat předností obou
najednou. Jeho průběh je nejprve shodný s Bug2. Pokud však nastane problémová situace,
tzn. že nalezený bod L je od cíle dále než příslušný bod H, nebo vznikne cyklus, pokračuje
algoritmus jako Bug1 a bod L bude definován jako bod náležící hranici překážky, který je
nejblíže k bodu T.
4.5.4
Implementace algoritmu třídy Bug v simulátoru
Implementace spočívá v kombinaci předchozích dvou úloh, tedy jízdy podle odometrických dat a detekce překážek, které doplníme o objíždění překážek. Před začátkem vlastní
implementace algoritmu je vhodné rozmyslet si rozmístění senzorů na robotu, které může
ovlivnit navrhovaný algoritmus. Senzory by měli překážku nejen detekovat, ale podle jejich
měření by robot měl být schopen udržovat konstantní vzdálenost od překážky na pravé
nebo levé straně robotu. Z tohoto důvodu je vhodné zvolit umístění takové, že kromě senzorů detekujících překážky před robotem budou dva senzory na jedné ze stran robotu.
V dalším textu budou uvažovány dva senzory vpředu a dva na levé straně robotu.
Algoritmus pro reaktivní řízení robotu může být založen na jednoduchých principech.
Vývojový diagram, který je zobrazen na obrázku 21, představuje následující pravidla:
1. Jízda robotu přímo k cíly po spojnici aktuální poloha-cíl.
- Pokud není detekována překážka, dopředná i úhlová rychlost je nastavena regulátorem, stejně jako v předchozích úlohách. Jakmile robot dosáhne cíle, program
je ukončen.
- Pokud je detekována překážka, pokračování pravidlem 2.
2. Natočení robotu levým bokem k překážce (senzory jsou na levé straně robotu) a
aktivace třetího pravidla.
3. Pokud již překážka není detekována, aktivace prvního pravidla. Jinak nastavení rychlosti podle porovnání měření z dálkoměrných senzorů pro sledování překážky.
V rámci této úlohy jsou obměněny opět funkce moveTo a printStatus. Změnou funkce
moveTo je volání funkce pro objezd překážky jakmile je překážka detekována dálkoměrnými
senzory. Do funkce printStatus přidáme výpis aktuální činnosti robotu, tzn. výpis odlišných
zpráv v případě jízdy k cíly a jízdy kolem překážky.
Kromě zmíněných funkcí je implementována nová funkce pro objezd překážky, funkce
moveAround. Jejími parametry budou souřadnice x a y bodu T. Funkci můžeme rozdělit
na dva samostatné cykly. V prvním cyklu, který je ukázán ve výpisu 30 proběhne natočení
robotu k překážce.
45/60
Obrázek 21: Vývojový diagram algoritmu reaktivního řízení.
const
const
const
const
const
const
double IR_MAX_RANGE = 0.39;
double THRESHOLD = 0.2;
int IR_SIDE_BACK = 0;
int IR_SIDE_FRONT = 1;
int IR_FRONT_LEFT = 2;
int IR_FRONT_RIGHT = 3;
/* Dopředná rychlost. */
double v = 0.0;
/* Naměřené hodnoty senzorů. */
float r1 = 0.0;
float r2 = 0.0;
float r3 = 0.0;
float r4 = 0.0;
do {
robot->Read();
r1 = ranger->getData(IR_SIDE_BACK);
r2 = ranger->getData(IR_SIDE_FRONT);
r3 = ranger->getData(IR_FRONT_LEFT);
r4 = ranger->getData(IR_FRONT_RIGHT);
printStatus();
/* Výběr nižší hodnoty předních IR senzorů. */
46/60
v = (r3 < r4) ? r3 : r4;
/* Pokud je detekovaná překážka příliš blízko, */
/* nastavena záporná rychlost - couvání. */
v = (v < THRESHOLD) ? (v - 0.5) : 0.0;
/* Nastavení dopředné rychlosti podle výsledku měření */
/* a konstantní úhlové rychlosti. */
drive->setSpeeds(v, -0.5);
/* Pokud není před robotem detekována překážka a hodnoty bočních */
/* senzorů se téměř rovnají, robot je natočen. */
} while ((r2 > IR_MAX_RANGE && r1 > IR_MAX_RANGE) &&
((r2-r1 > 0.1) || (r2-r1 < -0.1)));
Výpis 30: Natočení robotu bokem k překážce.
V druhém cyklu již probíhá samotná jízda kolem překážky. Funkcionalita je založena
na porovnávání naměřených hodnot předními i bočními senzory. Cyklus je ukázán ve výpisu 31.
const
const
const
const
const
const
double IR_MAX_RANGE = 0.39;
double TARGET_REACH_TOLERATION = 0.3;
int IR_SIDE_BACK = 0;
int IR_SIDE_FRONT = 1;
int IR_FRONT_LEFT = 2;
int IR_FRONT_RIGHT = 3;
while (avoid) {
robot->Read();
float dx = x - drive->getX();
float dy = y - drive->getY();
/* Test na dosažení cíle. */
if((dx*dx + dy*dy) <
TARGET_REACH_TOLERATION*TARGET_REACH_TOLERATION) {
avoid = false;
}
r1 = ranger->getData(IR_SIDE_BACK);
r2 = ranger->getData(IR_SIDE_FRONT);
r3 = ranger->getData(IR_FRONT_LEFT);
r4 = ranger->getData(IR_FRONT_RIGHT);
printStatus();
/* Žádná překážka nebyla detekována.*/
if (r1 > IR_MAX_RANGE &&
r2 > IR_MAX_RANGE &&
r3 > IR_MAX_RANGE &&
r4 > IR_MAX_RANGE) {
47/60
4.6 Metody vizuální navigace
avoid = false;
}
/* Detekce překážky před robotem. */
if(r3 < IR_MAX_RANGE || r4 < IR_MAX_RANGE) {
if (r1 < r2) {
drive->setSpeeds(0.0, -0.3);
} else {
drive->setSpeeds(-0.1, -0.1);
}
} else {
if (r1 < r2) {
drive->setSpeeds(0.3, -0.3);
} else {
}
}
}
Výpis 31: Objezd překážky.
Průběh algoritmu v prostředí simulátoru Stage je zobrazen na obrázku 22.
V uvedeném algoritmu chybí mechanizmus pro testování dosažitelnosti cíle. Pokud se
bude cíl nacházet uvnitř překážky nebo pod ní, bude se robot snažit překážku objet dokola,
ale program se neukončí. Z tohoto důvodu je nutnou podmínkou pro správnou funkčnost
algoritmu dosažitelnost cíle.
4.6
4.6.1
Úloha IV - Metody vizuální navigace
Motivace
Studenti se poprvé na cvičení setkají s inteligentní kamerou CMUcam3, zvídaví se mohou podívat na webové stránky [13], kde naleznou technickou specifikaci kamery. Studenti
se blíže seznámí s metodou vizuální navigace, která využívá vyhledávání souvislých jednobarevných oblastí v obraze pořízeném touto inteligentní kamerou.
4.6.2
Zadání
Implementujte reaktivní algoritmus pro navigaci robotu k objektu zvolené barvy. K senzorickému vybavení robotu přidejte inteligentní kameru a použijte ji ke snímání okolí robotu. Pro vyhledávání souvislých oblastí určité barvy v obraze pořízeném kamerou využijte
modul Blobfinder, který je součástí Playeru. Prostředí robotu pro tuto úlohu je tvořeno
48/60
Obrázek 22: Průběh třetí úlohy v simulátoru.
krabicemi výrazné barvy (červená, modrá, zelená, fialová), které využijete pro účely vizuální navigace, a krabice nevýrazné barvy sloužící jako překážky. Všechny krabice budou
rozměrů větších než 40x40x40cm.
4.6.3
Rozhraní BlobfinderProxy v simulátoru Stage
Základem algoritmu vizuální navigace je vyhledávání souvislých jednobarevných oblastí
v obraze, který je pořízen kamerou, pro které může být použito rozhraní Blobfinder Modelování pohledu kamery v simulátoru Stage je zachyceno na obrázku 23.
Modul je využíván pro vyhledávání souvislých oblastí více barev, přičemž pro každou
hledanou barvu je potřeba definovat jeden kanál. V konfiguračním souboru robotu morbot.inc, který je součástí šablony, je definován pouze kanál pro červenou barvu. Rozšíření
barevného spektra je zajištěno drobnou úpravou tohoto souboru, totiž připsáním dalších
barev tak, jak je uvedeno ve výpisu 32. Druhým upravovaným konfiguračním souborem je
task4.cfg, ve kterém je nutné doplnit ”blobfinder:0” mezi poskytovaná rozhraní Playeru.
49/60
Obrázek 23: Simulace pohledu kamery v prostředí simulátoru Stage.
# Definice vlastností kamery.
define morbot_camera
ptz
(
pose [0.05 0 0]
blobfinder
(
# Počet barevných kanálů.
channel_count 4
# Detekované barvy.
channels ["red" "blue" "green" "violet"]
)
)
Výpis 32: Definice barevných kanálů algoritmu blobfinder.
Rozhraní BlobFinderProxy je využíváno pro kontrolovaný přístup k datovým strukturám
Playeru, do kterých jsou ukládány informace o detekovaných souvislých jednobarevných
oblastech. Položky datové struktury playerc blobifndet blob t jsou uvedeny v tabulce 6.
4.6.4
Implementace algoritmu pro vizuální navigaci na základě dat z inteligentní kamery
Implementace algoritmu spočívá v navržení jednoduchého regulátoru, který je založen
na vyčtených datech z kamery. Dopředná rychlost je úměrná vzdálenosti robotu od krabice
a úhlová rychlost je úměrná vychýlení krabice ze středu pořízeného obrázku.
50/60
Proměnná a její datový typ
Význam
unsigned int color
barva oblasti v barevném modelu RGB
unsigned int area
obsah oblasti v jednotkách pixelů
unsigned short x, y
souřadnice středu oblasti, udáváno v pixelech
unsigned short left, right, top, bottom souřadnice hranice oblasti uvedené v pixelech
double range
vzdálenost ke středu oblasti v jednotkách metrů
Tabulka 6: Položky datové struktury typu playerc blobfinder blob t.
Algoritmus vizuální navigace je možné shrnout do několika kroků:
1. Nalezení první barevné krabice a zapamatování si její barvy, nebo zadání hledané
barvy a nalezení příslušné krabice v prostředí.
2. Načtení dat o detekované krabici, konkrétně barvy, vzdálenosti k ní range a souřadnice x středu oblasti v obraze.
3. Pokud je vzdálenost ke krabici menší než předem určený práh, robot se zastaví a
algoritmus končí. V opačném případě je nastavena dopředná a úhlová rychlost dle
přepočtených dat, návrat do druhého kroku.
Vztahy pro výpočet akční veličiny dopředné rychlosti (3) a úhlové rychlosti robotu (4)
z parametrů detekované oblasti byly získány experimentálně.
v =
range
,
1000
(3)
ω=
40 − x
,
4
(4)
kde
v je dopředná rychlost robotu,
range je vzdálenost ke středu detekované oblasti v pixelech,
ω je úhlová rychlost robotu,
x je souřadnice detekovaného objektu (rozsah nula až osmdesát).
Vzdálenost ke krabici range získaná z obrázku je pouze odhadem skutečné. Pro kontrolu
vzdálenosti od překážky je vhodné použít také dálkoměrných senzorů.
51/60
Ukázka implementace
V rámci této úlohy budeme upravovat dvě třídy:
• CCamera a
• CMorbot.
Třída CCamera je zapouzdřující třídou rozhraní BlobfinderProxy. Je využívána pro
načítání dat o detekovaných objektech a úpravě těchto dat. Ve výpisu 33 jsou ukázány
dvě funkce této třídy, jedná se o getBlobPosition, která je využívána pro získání dat, a
ConvertBlobData obsahující převodní vztahy pro přepočet získaných dat.
/*
* Získání dat o detekovaném objektu známé barvy.
*/
TBlobData CCamera::getBlobPosition(unsigned int color) {
/* Datový typ, který reprezentuje datovou strukturu */
/* obsahující vzdálenost k objektu, směr k němu, barvu */
/* booleovskou hodnotu detekce.*/
TBlobData res;
res.detected = false;
/* Postupné načtení dat o všech detekovaných objektech. */
for(unsigned int i = 0; i < blobfinderProxy->GetCount(); i++) {
playerc_blobfinder_blob_t blobData = blobfinderProxy->GetBlob(i);
if (color == blobData.color){
return convertBlobData(blobData);
} else {
return res;
}
}
}
/*
* Funkce pro přepočet získaných dat a jejich uložení
* do struktury TBlobData.
*/
TBlobData CCamera::convertBlobData(playerc_blobfinder_blob_t blobData) {
TBlobData res;
res.detected = true;
/* Úprava dat, výpočet akční veličiny pro řízení */
/* dopředné rychlosti. */
res.distance = blobData.range/1000;
52/60
/* Vychýlení obzázku ze středu, na levo záporné hodnoty, */
/* napravo kladné. Akční veličina pro regulaci úhlové rychlosti. */
res.angle = (40 - blobData.x)/10;
res.color = blobData.color;
return res;
}
Výpis 33: Ukázka funkcí třídy CCamera.
Ve třídě CMorbot je nutné vytvořit instanci třídy CCamera a implementovat regulátor
pro řízení pohybu robotu. Pro samotnou jízdu k překážce budou využívána data z kamery,
ukázka implementace je uvedena ve výpisu 34. Pro detekci překážek v podobě šedých
krabic a jejich objíždění mohou být použita senzorická měření dálkoměrnými senzory a
samozřejmě algoritmy z minulých úloh. Podmínkou správného běhu algoritmu je neustálá
detekovatelnost sledované krabice.
const double THRESHOLD = 0.2;
/* Přední
const int
/* Přední
const int
senzor nalevo. */
IR_FRONT_LEFT = 2;
senzor napravo. */
IR_FRONT_RIGHT = 3;
/*
* Navigace robotu k barevnému objektu.
*/
bool CMorbot::gotoColor(int color) {
robot->Read();
TBlobData blobData = camera->getBlobPosition(color);
while(blobData.distance > THRESHOLD &&
(ranger->getData(IR_FRONT_LEFT) > THRESHOLD ||
ranger->getData(IR_FRONT_RIGHT) > THRESHOLD)) {
robot->Read();
blobData = camera->getBlobPosition(color);
drive->setSpeeds(blobData.distance, blobData.angle);
}
drive->setSpeeds(0.0,0.0);
robot->Read();
return true;
}
Výpis 34: Regulátor pro dojezd ke krabici známé barvy, funkce gotoColor.
53/60
4.7 Tvorba topologické mapy
4.7
4.7.1
Úloha V - Tvorba topologické mapy prostředí
Motivace
Studenti se seznámí se základními druhy vnitřních modelů prostředí robotu a jejich
reprezentací. Vyzkouší si implementaci komplexnější úlohy a uplatní všechny dosud získané
dovednosti.
4.7.2
Zadání
Implementujte algoritmus pro průzkum neznámého prostředí, tvorbu topologické mapy
a plánování cesty ve zmapovaném prostředí. V operačním prostoru robotu se nacházejí dva
druhy krabic:
• šedé krabice reprezentují překážky a
• krabice výrazných barev sloužící jako orientační body.
Použijte inteligentní kameru pro pořizování obrazů okolí a rozhraní Playeru blobfinder pro
detekci souvislých barevných oblastí v pořízeném obraze. Po průzkumu prostředí by měl
být robot schopen autonomní jízdy k zadané krabici s minimálním počtem přesunů mezi
nimi.
4.7.3
Základní druhy modelů prostředí
Mezi základní úlohy, které jsou řešeny v oblasti mobilní robotiky, patří plánování. Pro
naplánování cesty robotu je nutné znát model prostředí, nebo-li mapu. Mapy prostředí
mohou být robotu k dispozici předem, ale většinou neexistují ve vhodném formátu pro
další zpracování. Z tohoto důvodu může být mapa vytvořena v průběhu průzkumu prostředí
robotem. Mapy lze rozdělit do tří základních skupin:
Senzorické mapy jsou reprezentací prostředí s nejnižším stupněm abstrakce. Jedná se
o vhodně uchovávaná data získaná senzorickým měřením. Tvorba takové mapy je
velmi jednoduchá, ale na druhou stranu je mapa paměťově náročná a nemusí být
příliš vhodná pro další zpracování. Příkladem senzorické mapy je mřížka obsazenosti.
Mřížkou rozděluje prozkoumávané prostředí na menší celky. Každá buňka mřížky
obsahuje míru pravděpodobnosti přítomnosti překážky.
Geometrická mapa reprezentuje prostředí vhodně zvolenými geometrickými entitami,
jako jsou úsečky nebo kružnice. Tyto mapy jsou výpočetně náročnější než předchozí,
ale díky větší míře abstrakce jsou méně paměťově náročné.
54/60
Topologické mapy mohou být definovány například jako grafy, jejichž uzly jsou významnými místy v okolním prostředí a hrany definují přechody mezi těmito místy. Mapy
nemusí obsahovat žádné geometrické údaje. Topologické mapy jsou výhodné pro plánování nad velkým množstvím dat. Příklad mapovaného prostředí a jeho reprezentace
topologickou mapou jsou uvedeny na obrázku 24.
(a) Prostředí.
(b)
Topologická
mapa.
Obrázek 24: Příklad prostředí a jeho reprezentace topologickou mapou.
Kromě těchto tří základních modelů existuje také symbolická mapa, jejíž tvorba je založena na znalosti topologické mapy. Symbolická mapa se skládá ze jmen objektů nacházejících se v prostředí, jako je stůl nebo židle, vztahů mezi nimi, například židle je vedle stolu
v kuchyni, a některých vlastností objektů. Symbolická mapa by měla umožňovat zadávat
dotazy na vytvořený symbolický svět, například dotaz na cestu z chodby do kuchyně.
4.7.4
Rozbor úlohy
Zadání páté úlohy poskytuje studentům prostor pro její dodefinování. Umožňuje přidání
definice podmínek fungování algoritmu, kterými si studenti mohou sami upravit složitost
úlohy. Pro algoritmus popsaný v následujícím oddíle platí pro rozestavení krabic v prostoru
tyto omezující podmínky:
• V prostředí robotu neexistuje krabice nedetekovatelná při objíždění jiné krabice.
• V prostředí může existovat více krabic stejné barvy, za předpokladu že
- dvě krabice stejné barvy nejsou detekovatelné při objezdu jedné krabice a
- od krabice jedné barvy není detekovatelná krabice stejné barvy.
• Všechny krabice musí být možné zcela objet.
• Z počáteční pozice robotu je možné detekovat alespoň jednu krabici.
55/60
4.7.5
Popis implementace
Implementaci páté úlohy rozdělíme na dva samostatné problémy:
1. průzkum prostředí a stavba mapy,
2. plánování cesty na již sestavené mapě tak, aby algoritmus plánování našel vždy tu
nejkratší cestu. Nejkratší cestou je myšlena cesta s nejnižším počtem přesunů mezi
krabicemi.
Průzkum a stavba mapy spočívá v prohledání okolí robotu a sestavení grafu. Uzly grafu
představují důležitá místa v operačním prostoru robotu, v tomto případě barevné krabice.
Každý uzel má unikátní číselné označení a svou barvu, kromě toho obsahuje seznam všech
sousedních uzlů. Hrany grafu reprezentují spojnice mezi uzly a v tomto případě nejsou
ohodnoceny. Ohodnocení jednotlivých hran, neboli vzdálenosti mezi krabicemi, je možné
na základě odhadu vzdálenosti mezi krabicemi. Ohodnocení může být použito při plánování
nejkratší cesty, kde délka cesty je počítána v metrech. Takový přístup však komplikuje
řešení úlohy. Pro jednoduchost a názornost jej zde nebudeme uvažovat.
Průzkum prostředí může probíhat náhodně i deterministicky, vždy je ale potřeba zajistit
úplné prohledání prostoru. Pravidla pro průzkum prostředí mohou vypadat takto:
1. Otočení robotu kolem své osy o 360◦ , výběr nejbližší krabice podle odhadu vzdálenosti
získaného z rozhraní Blobfinder a jízda k ní. Vytvoření prvního uzlu grafu a aktivace
druhého pravidla. Pokud nebyla detekována žádná krabice, program končí.
2. Objezd zvolené krabice dokola a přidání detekovaných krabic do seznamu sousedů,
pokud:
- barva detekované krabice není rovna barvě objížděné krabice a
- detekovaná krabice ještě není v seznamu sousedů.
Pokud se detekovaná krabice dosud nenachází v mapě, je vytvořen nový uzel grafu.
Objetá krabice je označena za prozkoumanou a je aktivováno třetí pravidlo.
3. Výběr krabice pro další průzkum.
- Pokud existuje alespoň jeden neprozkoumaný soused právě objeté krabice, je
vybrán libovolný soused.
- Pokud neexistuje žádný takový soused, ale je nalezen alespoň jeden dosud neprozkoumaný uzel grafu, je vybrán uzel, ke kterému vede cesta s nejnižším počtem
přejezdů mezi krabicemi.
Pokud nebyl nalezen žádný dosud neprozkoumaný uzel, algoritmus končí. V opačném
případě pokračuje robot jízdou k vybranému uzlu a je aktivováno druhé pravidlo.
56/60
Druhou částí implementace je plánování cesty na sestavené mapě. Pro tyto účely mohou
být použity algoritmy pro prohledávání stavového prostoru, kde každý uzel vytvořeného
grafu je považován za jeden stav.
Pro průzkum prostředí byl nutný jeden senzor, inteligentní kamera. Pohyb robotu podle
naplánované cesty mezi krabicemi vyžaduje použití dálkoměrných senzorů. Kromě vizuální
navigace pro jízdu od krabice ke krabici využijeme také dálkoměrné senzory pro detekci
šedých krabic a vyhýbání se jim.
57/60
5
Závěr
Cílem této práce bylo vytvořit softwarovou podporu pro výuky mobilní robotiky. Nejprve
byl nakonfigurován palubní počítač robotické platformy Morbot, která byla vytvořena pro
účely výuky. Samotná konfigurace nespočívá jen v nastavení parametrů periférií, ale také
v nezbytné přípravě prostředí křížové kompilace pro sestavení serverové části Playeru pro
procesor architektury ARM.
Mezi nejdůležitější výstupy bakalářské práce patří softwarový modul zajišťující integraci
robotické platformy Morbot do systému Player. Tento modul implementuje unifikované
rozhraní Playeru poskytující abstrakci přístupu k hardware. To umožňuje jednoduchý přenos aplikace implementované v robotickém simulátoru na reálný robot, především přístup
k senzorům. Přístup k nim je stejný jak pro robot modelovaný v simulátoru tak pro reálný
robot.
Hlavní část vlastní bakalářské práce je věnována popisu úloh řešených v rámci předmětu Mobilní robotika spolu s ukázkovými řešeními. Řešení úloh slouží nejen k odladění
implementovaného modulu Playeru, ale umožňuje studentům mobilní robotiky rychlejší
proniknutí do problematiky a pochopení principů Playeru.
V průběhu příštích let bude jistě zajímavé sledovat postup studentů při řešení úloh
předmětu, pokud budou mít k dispozici vytvořený materiál. Studenti mohou využít ucelený zdroj informací a řešení úloh pro ně může být zpočátku jednodušší, proto by mohli
zvládnout řešení náročnějších úloh.
58/60
REFERENCE
Reference
[1] Grimmer Vladimír. Platforma pro výuku mobilní robotiky. Technical report, ČVUT
v Praze, Fakulta Elektrotechnická, 2008. Bakalářská práce.
[2] Gumstix - dream, design, deliver. http://www.gumstix.com.
[3] PXA270 Electrical, Mechanical and Thermal Specification. http://www.phytec.com/
pdf/datasheets/PXA270_DS.pdf.
[4] The Journaling Flash File System, version 2. http://sourceware.org/jffs2.
[5] The Player Project. http://playerstage.sourceforge.net.
[6] Driver Class Reference. http://playerstage.sourceforge.net/doc/Player-2.0.
0/player/classDriver.html.
[7] Version Control with Subversion. http://svnbook.red-bean.com.
[8] PlayerClient Class Reference.
http://playerstage.sourceforge.net/doc/
Player-1.6.5/player-html/classPlayerClient.php.
[9] Client Proxy Class Reference.
http://playerstage.sourceforge.net/doc/
Player-1.6.5/player-html/classClientProxy.php.
[10] GP2D120, Opto Electronic Device.
americas/en/part/G2D120XJ00F/.
http://www.sharpsma.com/Page.aspx/
[11] Srf10 Ultrasonic range finder, Technical Specification.
robot-electronics.co.uk/htm/srf10tech.htm.
http://www.
[12] Vladimir J. Lumelsky. Sensing, Intelligence, Motion: How Robots and Humans Move
in an Unstructured World. John Wiley and Sons, Inc., 2004.
[13] CMUcam3: Open Source Programmable Embedded Color Vision Platform. http:
//www.cmucam.org.
59/60
Příloha
Obsah přiloženého CD
V tabulce 7 jsou uvedena jména všech kořenových adresářů přiloženého CD s popisem
obsahu.
Jméno adresáře Popis obsahu
bp
bakalářská práce ve formátu pdf.
úlohy
zdrojové kódy k úlohám řešeným na X33MOR.
driver
zdrojové kódy modulu pro integraci robotu Morbot do
Playeru.
Tabulka 7: Obsah přiloženého CD.

Podpůrný software pro výuku mobilní robotiky

Transkript

Podobné dokumenty

Untitled - Brno Alligators

Tecnicall 4/2009

práci - Intelligent and Mobile Robotics Group

BAKALÁˇRSKÁ PRÁCE

Petrov MRBT - Ústav automatizace a měřicí techniky

Videokamery Systému Video Camcorder