NÁVRH A REALIZACE METODY GLOBÁLNÍ LOKALIZACE VE ZNÁMÉM PROSTŘEDÍ PRO MOBILNÍ ROBOT AUTONOMOUS ROBOT LOCALIZATION FOR KNOWN ENVIRONMENT

VYSOKÉ UČENÍ TECHNICKÉ V BRNĚ BRNO UNIVERSITY OF TECHNOLOGY

FAKULTA STROJNÍHO INŽENÝRSTVÍ ÚSTAV AUTOMATIZACE A INFORMATIKY FACULTY OF MECHANICAL ENGINEERING INSTITUTE OF AUTOMATION AND COMPUTER SCIENCE

NÁVRH A REALIZACE METODY GLOBÁLNÍ LOKALIZACE VE ZNÁMÉM PROSTŘEDÍ PRO MOBILNÍ ROBOT AUTONOMOUS ROBOT LOCALIZATION FOR KNOWN ENVIRONMENT

DIPLOMOVÁ PRÁCE DIPLOMA THESIS

AUTOR PRÁCE

Bc. VOJTĚCH ORGOŇ

AUTHOR

VEDOUCÍ PRÁCE SUPERVISOR

BRNO 2007

Ing. STANISLAV VĚCHET, Ph.D.

Strana 3

ZADÁNÍ DIPLOMOVÉ PRÁCE (Zde je v originále diplomové práce vloženo zadání)

Strana 5

LICENČNÍ SMLOUVA (Zde je v originále diplomové práce vložena licenční smlouva)

Strana 7

ABSTRAKT Tato diplomová práce pojednává o problematice globální lokalizace robotu ve známém prostředí. První část je věnována senzorům používaným pro lokalizaci. V druhé části jsou popsány některé metody lokalizace. V další části je popsána implementace vybrané metody v prostředí C#. V poslední části je popsáno použití vytvořeného simulačního programu pro analýzu citlivosti metody na změnu parametrů lokalizace.

ABSTRACT This thesis deals with autonomous robot localization for known environment. The first part describes senzors from localization point of view. The next one contains description of various localization methods. The third part describes implementation of one localization method in C# language. The final part uses simulation program in localization parameters sensitivity analyse.

KLÍČOVÁ SLOVA Robot, senzor, lokalizace, C#, scan-matching.

KEYVORDS Robot, sensor, localization, C#, scan-matching.

Strana 9

PODĚKOVÁNÍ Děkuji Ing. Stanislavu Věchetovi Ph.D. za odborné vedení, cenné rady a informace potřebné při zpracování diplomové práce.

Strana 11

OBSAH ZADÁNÍ DIPLOMOVÉ PRÁCE..........................................................................................3 LICENČNÍ SMLOUVA ........................................................................................................5 ABSTRAKT ..........................................................................................................................7 PODĚKOVÁNÍ .....................................................................................................................9 OBSAH................................................................................................................................11 1 ÚVOD..........................................................................................................................13 2 PROBLEMATIKA LOKALIZACE ROBOTU ........................................................15 3 SENZORY ...................................................................................................................17 3.1 Informace o okolí robotu - externí senzory .........................................................17 3.1.1 Taktilní senzor .............................................................................................17 3.1.2 Infračervený senzor......................................................................................17 3.1.3 Ultrazvukový sonar......................................................................................18 3.1.4 Laserový skener ...........................................................................................18 3.2 Informace o pohybu robotu – interní senzory......................................................19 3.2.1 Senzory natočení..........................................................................................19 3.2.2 Optické senzory pohybu ..............................................................................20 4 METODY LOKALIZACE ROBOTU VE ZNÁMÉM PROSTŘEDÍ.........................21 4.1 Odometrie ............................................................................................................21 4.1.1 Odvození kinematického modelu ................................................................21 4.1.2 Reprezentace chyby .....................................................................................24 4.2 Techniky lokalizace .............................................................................................25 4.3 Vybrané metody lokalizace .................................................................................26 4.3.1 Metoda trilaterace ........................................................................................26 4.3.2 Markova metoda lokalizace robotu..............................................................27 4.3.3 Metoda lokalizace Monte Carlo...................................................................27 5 POPIS VYBRANÉ METODY ....................................................................................31 5.1 Mapa ....................................................................................................................31 5.2 Testovací pozice ..................................................................................................31 5.3 Sken .....................................................................................................................32 5.4 Hodnotící funkce..................................................................................................33 5.5 Vybrané cesty ......................................................................................................33 6 IMPLEMENTACE VYBRANÉ METODY................................................................35 6.1 Úvod.....................................................................................................................35 6.2 Základní datové struktury ....................................................................................35 6.3 Reprezentace mapy ..............................................................................................36 6.3.1 Rasterizace ...................................................................................................37 6.4 Reprezentace cesty...............................................................................................38 6.5 Model robotu........................................................................................................38 6.6 Model skeneru......................................................................................................39 6.6.1 Sken .............................................................................................................39 6.7 Lokalizace............................................................................................................40 6.7.1 Parametry lokalizace....................................................................................40 6.7.2 Generátor testovacích pozic.........................................................................41 6.7.3 Implementace Scan – matching ...................................................................41 6.7.4 Vyhodnocení historie...................................................................................42

Strana 12

.

6.8 Dávka simulací.....................................................................................................42 6.8.1 Hodnocení lokalizace...................................................................................43 6.9 Definiční soubory.................................................................................................45 6.10 Vizualizace...........................................................................................................46 6.10.1 Grafická reprezentace ..................................................................................46 6.11 Ovládání programu ..............................................................................................47 6.11.1 Význam tlačítek ...........................................................................................48 7 SIMULACE .................................................................................................................53 7.1 Mapy ....................................................................................................................53 7.2 Cesty ....................................................................................................................53 7.3 Analýza citlivosti parametrů ................................................................................54 7.3.1 Parametry DiffX/DiffY/Fi............................................................................54 7.3.2 Parametr Range ............................................................................................55 7.3.3 Parametr NoBestRobotPos...........................................................................56 8 ZÁVĚR ........................................................................................................................59 SEZNAM POUŽITÉ LITERATURY .................................................................................61 SEZNAM PŘÍLOH..............................................................................................................63

Strana 13

1

ÚVOD

Současný technologický svět si nelze představit bez masivního použití robotů. Setkáváme se s nimi téměř ve všech oblastech lidské činnosti a to buď s jejich praktickou aplikací nebo jako předmět výzkumu. Pro dělení robotů můžeme použít, jako jedno z hledisek, schopnost robotu se přemísťovat. Podle tohoto hlediska dělíme roboty do dvou hlavních tříd: stacionární a mobilní. Stacionární roboty, jejichž hlavní oblastí použití je výroba a montáž produktů, se nejčastěji vyskytují jako průmyslové roboty a manipulátory. Škála rozměrů prvků, které mohou být pomocí robotů manipulovány, sahá od řádu 10-4m (např. součástky SMD) až po řád 101m (např. komponenty automobilů). Mobilní roboty dále dělíme podle způsobu řízení na dva základní typy: dálkově ovládané a autonomní. Dálkově ovládané nebo řízené stroje jsou zpravidla roboty bez inteligence či vyšších algoritmů řízení a rozhodování, které pracují čistě podle povelů operátora. Nejdokonalejší formou dálkového ovládání je tzv. teleprezenční řízení. Jedná se o řízení pomocí prvků používaných ve virtuální realitě. Autonomním robotem myslíme zařízení, které na základě instrukcí samostatně vykoná nějakou úlohu. Obvykle se předpokládá, že robot k plnění zadaného úkolu používá prvků umělé inteligence, např. je schopen orientovat se v neznámém nebo pozměněném prostředí, vyhýbat se překážkám, apod. Této kategorii robotů se bude dále věnovat tato práce. V současné době probíhá výzkum v oblasti robotiky zejména na vysokých školách. Např. na ÚAI FSI VUT v Brně je veden výzkum současného mapování a lokalizace mobilního robotu (angl. Simultaneous Location And Mapping - SLAM). Součástí prací je simulační ověření úspěšnosti i praktická aplikace na testovacím robotu [1][2]. Jako příklad ze zahraničí uvádím aktivitu významné americké obranné organizace DARPA, která pořádá soutěž ohledně inteligentních dopravních prostředků „Urban Challenge“. Úkolem soutěže je sestrojit autonomní mobilní robot schopný v simulované vojenské misi bezpečně a efektivně projet běžnou městskou oblastí. Závod se jede na 100 km dlouhé městské trati, dokončení musí být do 6 hodin. Tato akce navazuje na přechozí, 200 km dlouhý, „Grand Challenge“ jehož vítězem se stal tým Standfordské university pod vedení S. Thruna, šéfa standfordské AI laboratoře, který je autorem nebo spoluautorem množství prací zabývajících se problematikou lokalizace. Jedna z nich patří mezi základní literární prameny této práce. Z dostupných informací o vítězném robotu lze zjistit, že základem algoritmů lokalizace jsou různé modifikace pravděpodobnostních modelů. Věnuje se jim i část této práce.

Obr. 1 Vítězný autonomní mobilní robot v soutěži „Grand Challenge“.

Strana 14

.

Společným jmenovatelem uvedených příkladů je lokalizace. Podle všeho, se jedná o základní úlohu autonomního mobilního robotu. První část této práce hledá publikované metody a přístupy k lokalizaci robotu ve známém prostředí. Popisuje základní pojmy, s nimiž lze se při studiu této problematiky setkat. Pozornost je věnována i senzorům pro sbírání informací o vlastním pohybu a o okolním prostředí, jimiž autonomní mobilní robot musí být vybaven. Druhá část popisuje vybranou metodu a její implementaci v prostředí C#. Součástí je i popis návrhu základních datových tříd a metod pro vytvoření modelu robotu a vizualizaci simulace. Ve třetí části jsou popsány provedené simulace lokalizace za různých podmínek prostředí a hledání nastavení parametrů vybrané metody lokalizace. Poslední část hodnotí výsledky lokalizace v závislosti na nastavení parametrů metody lokalizace.

Strana 15

2

PROBLEMATIKA LOKALIZACE ROBOTU

Problém lokalizace robotu se obvykle definuje jako odhad pozice robotu uvnitř známého prostředí (model prostředí existuje v nějaké formě, například topologická mapa, mřížkový popis překážek) založený na informacích ze senzorů. Je známo mnoho algoritmů zabývajících se tímto problémem. Většina z nich spadá do kategorie, kdy je počáteční poloha robotu známa a algoritmus vyrovnává chyby odometrických dat na základě informací ze senzorů zkoumajících okolí. Pokročilé algoritmy, obvykle založené na pravděpodobnostním přístupu, dokáží nalézt i počáteční polohu robotu a mohou řešit úlohu v literatuře běžně označovanou „lost robot problem“. Pro zajištění spolehlivé navigace je potřebné, aby robot věděl, kde se nachází. Proto je odhad pozice na základě informací ze senzorů jeden ze fundamentálních problémů mobilní robotiky. Tento problém může být dále rozdělen na dva podproblémy: • odhad globální pozice - schopnost zjistit pozici robotu ve známé mapě; • sledování pozice - průběžná aktualizace původně známé pozice.

Obr. 2 Nejistota v poloze robotu bez aktualizace dat z odometrie (a) a s aktualizací pomocí měření vztaženému k bodům se známou polohou (b) .

Strana 16

.

Důležitou částí problematiky lokalizace jsou senzory, jimiž je robot vybaven. Podle účelu je rozdělujeme do následujících skupin: • získání informace o okolí - externí senzory - blízkostní senzory, kamery; • sledování pohybu robotu - interní senzory - odometrie. Kombinace informací z obou skupin senzorů, při znalosti kinematického modelu robotu, použijeme k nalezení pravděpodobné polohy.

Strana 17

3

SENZORY

Návrh senzorického subsystému je komplexní proces, při němž je nutné zvážit mnoho faktorů zohledňující nejen vlastnosti senzorů, ale také pracovní prostředí robotu, možnost vzájemné interakce senzorů, nároky na výpočtové možnosti řídícího subsystému atd. [4][9].

3.1

Informace o okolí robotu - externí senzory

Externí senzory slouží k získávání informací o okolí robotu. Podle způsobu měření rozlišujeme dvě základní skupiny senzorů. Jsou to pasivní, vyhodnocují pouze přijaté záření z okolí a aktivní, vyhodnocují vlastní odražené záření. Z hlediska lokalizace jsou důležité pouze senzory sloužící k navigaci. Tu můžeme rozdělit na globální a lokální navigaci. • Lokální navigace slouží pro vyhýbání se překážkám nebo udržování odstupu od nich. • Úkolem globální navigace je zjištění polohy a orientace robotu vůči použitému globálnímu souřadnému systému. Ve většině případů není hodnota naměřená senzorem přímo polohou a je ji nutné teprve vypočítat. V přehledu uvádím vybrané typy senzorů používané pro řízení mobilních robotů ve vnitřním prostředí.

3.1.1 Taktilní senzor Jedná se o nejjednodušší provedení senzoru, nejčastěji realizované kontaktním spínačem. Aktivací spínače – dotykem překážky – dojde ke změně stavu a ta je dále vyhodnocena. Pro malý dosah nemá pro potřeby globální lokalizace větší význam.

3.1.2 Infračervený senzor Slouží k detekování překážek v blízkém okolí robotu, řádově 10-1m, patří do kategorie „near proximity sensor“.

Obr. 3 Princip měření vzdálenosti IR senzorem (a) a typická charakteristika (b).

Strana 18

.

Tyto aktivní senzory vysílají infračervené záření, které je po odrazu detekováno IR fototranzistorem. Vyzařované světlo bývá modulováno a tranzistor je opatřen filtrem propouštějícím světlo použité vlnové délky pro zlepšení odolnosti proti šumu. V novějším provedení senzoru, ve kterém je jednoduchý IR senzor nahrazen lineárním senzorem jehož výstup je závislý na místu dopadu paprsku, je možno pomocí vestavěného jednočipového mikropočítače vypočítat vzdálenost (viz. obr. 3). Tento senzor patří mezí velmi levné senzory a často se používá pro rychlé ověření metody lokalizace [1].

3.1.3 Ultrazvukový sonar Obdobně jako netopýr, zjišťuje senzor polohu objektu měřením doby šíření vyslaného a odraženého ultrazvukového impulzu, která je přímo úměrná vzdálenosti objektu od piezoelektrického měniče. Obvyklá frekvence signálu je 40 kHz. Vzhledem k nízké rychlosti zvuku ve vzduchu je doba mezi vysíláním a příjmem relativně dlouhá. Proto lze dosáhnou vysoké přesnosti s nižšími nároky na vyhodnocovací obvody než u radarových nebo laserových senzorů. Úhlové rozlišení je vzhledem k širokému rozptylu ultrazvukového signálu nízké – kolem 10°. Při použití tohoto senzoru je nutno kompenzovat teplotu kvůli významné závislosti rychlosti šíření zvuku na teplotě. Ta je pro t = 0°C; v = 331 m/s a pro t = 25 °C; v = 343 m/s [4]. Dalším problémem tohoto typu senzoru jsou vícenásobné a křížové odrazy. Novější sonary jsou proto vybaveny výstupem optimalizovaným pro zpracování vícenásobného odrazu umělou neuronovou sítí (ANN). Senzor patří stejně jako předchozí typ do kategorie „near proximity sensor“ a jedná se o cenově dostupné řešení s větším dosahem než předchozí senzor.

Obr. 4 Typická charakteristika senzoru a standardní aproximace pro analýzu.

3.1.4 Laserový skener V principu se tento senzor neliší od předchozího typu, jen místo zvuku se používá světlo laseru, což umožňuje bodové měření. Dále je přístroj vybaven zařízením pro rozmítání paprsku v rovině. Výslednou informací je sada hodnot reprezentujících vzdálenosti překážek. Jednotlivá měření jsou prováděna s úhlovým rozlišením 0.25–1°. Zorné pole bývá obvykle 180° (90° ÷ 270°). Přesnost měření je 10-2 m. Senzor patří do kategorie „proximity sensor“. Tento senzor dnes patří mezi nejvíce používaný typ. Především přesnost a hlavně rychlost s jakou poskytuje informace o okolí v celém zorném poli prostřednictvím jednoho měření, je předpokladem pro spolehlivou navigaci robotu. Jeho výborné vlastnosti jsou vyváženy cenou, která je relativně vysoká.

Strana 19

Obr. 5 Princip laserového skeneru a výrobek používaný při výzkumu robotiky.

3.2

Informace o pohybu robotu – interní senzory

Interní senzory poskytují robotu informace o jeho subsystémech. Pro diagnostické účely je to například stav baterie, monitorování komunikace a kontrola teploty robotu. Pro účely navigace jsou to informace o akčním subsystému, což jsou obvykle poloha a rychlost jednotlivých pohonů nebo výstupních členů. Na základě těchto informací je pak schopen řídící systém pomocí kinematického modelu určit vliv těchto hodnot na pohyb robotu [4]. V následujícím textu jsou uvedeny vybrané typy senzorů používaných pro výpočet odometrie mobilních robotů ve vnitřním prostředí.

3.2.1 Senzory natočení Podle metody měření je rozlišujeme na přírůstkové (inkrementální) a absolutní. Většinou používají digitální bezdotykový princip, kdy kotouč opatřený otvory, pevně spojený s výstupní hřídelí, stíní paprsek světla. Poskytují obvykle informaci o natočení rotačních pohonů. Pro získání informace o smyslu otáčení bývají vybaveny kvadraturními dekodéry. Změnu polohy robotu (odometrie) získáme zahrnutím modelu pohonů do kinematického modelu. Pokud dochází ke skluzu pohonu ve styku s podlahou, je získaná hodnota pouze informativní. Senzor bývá často součástí pohonu – je na společném hřídeli elektromotoru.

Obr. 6 Princip senzoru natočení.

Strana 20

.

Obr. 7 Provedení kotouče enkodéru: Grayův kód (a) a binární kód (b).

3.2.2 Optické senzory pohybu Obdobně jako u počítačové myši je možno použít optický senzor ke snímání pohybu robotu. Snímač vyhodnocuje změny odraženého světla a je citlivý na změnu vzdálenosti od podlahy a optických vlastnostech materiálu. Výstupem je přímo relativní změna polohy v lokální souřadné soustavě robotu (není nutno aplikovat kinematický model podvozku). Při použití jen jednoho senzoru není k dispozici informace o změně orientace robotu. V dostupných pramenech na internetových stránkách je často vidět, že použitý senzor je adaptovaná počítačová myš. V tomto případě se jedná o velmi levný senzor.

Obr. 8 Princip optického snímače pohybu robotu.

Strana 21

4

METODY LOKALIZACE ROBOTU VE ZNÁMÉM PROSTŘEDÍ

4.1

Odometrie

Odometrie (angl. odometry) je proces, který popisuje transformaci dat poskytnutých interními senzory na změnu pozice a orientace robotu. Vlastní slovo odometrie je složeno ze dvou řeckých slov hodos (cestovat, cesta) a metron (měřit). Základem odometrie je znalost kinematického modelu robotu. Tyto modely se liší zejména tím, jakých druhů pohybu jsou roboty schopny. Rozlišujeme následující kinematické modely mobilního robotu (uvedeny jen kolové podvozky): • Diferenciální podvozek – dvě hnaná kola, rovnováha udržována opěrnými body, nebo pasivním kolem (koly); • Synchronní podvozek – často 3 kola, každé se 2 stupni volnosti (může se otáčet i natáčet); • Trojkolový podvozek s řízeným předním kolem – 2 hnaná kola a jedno motoricky natáčené; • „Ackermanův“ podvozek – 4 kola, 2 hnaná a 2 natáčená kola (každé mírně jinak, protože každé při zatáčení opisuje jinou dráhu); tyto podvozky mají běžné automobily; • Trojúhelníkový (všesměrový) podvozek s třemi nezávisle poháněnými koly, jejichž osy procházejí těžištěm a jejichž povrch (složený obvykle z malých koleček, angl. omnidirectional wheel) umožňuje volný skluz ve směru osy.

4.1.1 Odvození kinematického modelu V dalším výkladu bude uveden příklad kinematického modelu diferenciálního podvozku. Pro získání dat se obvykle používají inkrementální senzory natočení pohonů. Data jsou k dispozici ve formě pulsů. Odvodíme posun středu robotu a změnu úhlu natočení na základě známého počtu pulzů přicházejících z enkodérů jednotlivých kol. Vyjádříme si konverzní součinitel ck , který bude odpovídat ujeté vzdálenosti kola na jeden pulz enkodéru: πDk , (1) ck = nC0 kde n je převodový poměr, Dk je průměr hnaného kola a C0 je počet pulzů enkodéru na otáčku pohonu. Pro přírůstek ujeté vzdálenosti levého kola ΔuL,i respektive pravého ΔuR,i v i–tém kroku potom platí:

Δu L ,i = ck N L ,i ,

(2)

Δu R ,i = ck N R ,i ,

(3)

kde NL,i a NR,i jsou počty pulzů enkodérů levého respektive pravého kola.

Strana 22

.

Obr. 9 Kinematický model diferenciálního podvozku. Posun středu robotu Δui v i–tém kroku pak bude:

Δui =

Δu L ,i + Δu R ,i 2

,

(4)

a změna úhlu natočení Δφi v i–tém kroku pak bude:

Δϕ i =

Δu L ,i − Δu R ,i d

,

(5)

kde d je vzdálenost hnaných kol. Nové relativní natočení φi v i–tém kroku pak bude:

ϕ i = ϕ i −1 + Δϕ i ,

(6)

a nová relativní pozice středu xi a yi v i–tém kroku pak bude:

xi = xi −1 + Δui cos(ϕ i ) , yi = yi −1 + Δui sin(ϕ i ) .

(7) (8)

Strana 23

Sledování pozice pomocí jednoduchého akumulování informací z odometrie o její relativní změně časem vede k tomu, že robot předpokládá, že se nachází jinde než ve skutečnosti. Důvodem je akumulace chyby, která není ničím korigována. Pokud máme k dispozici více informací než je definováno v kinematickém modelu, např. „ackermanův“ podvozek se senzory na všech kolech, můžeme použít rozšířenou odometrii s přeurčením parametrů k částečné eliminaci chyby. Chyby, jimiž je zatížena vypočítaná poloha z odometrie, můžeme dělit na chyby systematické: • nestejné průměry kola; • aktuální průměr kola se liší od nominálního průměru kola; • vychýlení kol; • konečná přesnost enkodéru; • konečný vzorkovací kmitočet; a chyby nahodilé: • odvalování po nerovném podkladu; • odvalování po neočekávaných objektech; • skluz kvůli kluzkém podkladu; • skluz kvůli rychlému otočení; • skluz kvůli vnější síle působící na kolo (interakce s externími tělesy); • skluz kvůli vnitřní síle působící na kolo (směrově natáčivé kolo). Určitého vyloučení vlivu chyb lze dosáhnout konstrukčním uspořádáním robotu a volbou podmínek experimentu (materiál podkladu atp.). Vliv chyb na odhad polohy robotu lze orientačně posoudit podle výsledku tzv. „square path“ testu. Princip je vidět na obr. 10.

Obr. 10 Příklad skutečné dráhy a předpokládané dráhy robotu stanovené pomocí odometrie (tzv.“square path“ test).

Strana 24

.

Jednou z možností je každému měření přiřadit očekávanou chybu. Naměřenou hodnotu potom uvádíme jako u ± r a očekáváme, že chyba má gaussovské rozložení se střední hodnotou 0 a rozptylem r. Na základě takto reprezentovaných měřeních se budeme snažit zkonstruovat odhad nám neznámého stavu robotu (nejčastěji x, y, φ) a chybu tohoto odhadu.

7 Obr. 11 Pravděpodobnostní model měření (a) a akumulace chyby (b).

4.1.2 Reprezentace chyby Předpokládejme, že mobilní robot se pohybuje ve 2D prostředí. Jeho pozici (polohu a orientaci) v čase k označíme:

p(k ) = [ x(k ), y (k ),ϕ (k )]T ∈ Q ,

(9)

kde Q ≡ R 2 × (− π ,+π je stavový prostor, který obsahuje všechny možné pozice robotu. Za předpokladu pomalého pohybu robotu a jednoduchého lineárního modelu pohybu, můžeme přijmou následující popis časového vývoje pozice: p ( k + 1) = p ( k ) + u ( k ) + w(k ) ,

( 10 )

kde u (k ) ∈ R 3 představuje změnu pozice robotu určenou vyhodnocením informací z odometrie a w( k ) ∈ R 3 představuje vliv chyb na tuto pozici. Dalším předpokladem je existence mapy, v níž jsou vyznačeny souřadnice orientačních bodů (angl. landmark). Polohu i–tého orientačního bodu označíme:

li = [ xli , yli ]T , i = 1,..., n .

( 11 )

Robot je vybaven senzorem, který poskytuje informaci o vzdálenosti. Senzorová měření popisuje následující rovnice:

ci (k ) = μi ( p(k )) + vi (k ) , i = 1,..., n ;

( 12 )

Strana 25

kde μi ( p (k )) je popis i–tého měření a vi (k ) je popis vlivu chyb na toto měření. Na základě deterministického popisu můžeme přijmout domněnku, že velikost chyby není známa, ale je ohraničená:

wi (k ) ≤ ε iw (k ) , i = 1,2,3 ;

( 13 )

vi (k ) ≤ ε vi (k ) , i = 1,..., m ;

( 14 )

kde ε iw (k ) a ε vi (k ) jsou známé kladné hodnoty.

Výše uvedené vztahy tvoří základní rámec problému lokalizace. Z rovnic je možné odvodit vztah, kde ke každému senzorovému měření ci (k ) lze přiřadit množinu:

Μ i = { p(k ) : ci (k ) − μ i( p(k )) ≤ ε vi (k )} ,

( 15 )

představující všechny polohy p(k) robotu slučitelné s daty poskytovanými senzory mobilního robotu ci(k) za předpokladu ohraničení velikosti chyby ε vi (k ) . Když v čase k provede robot m senzorových měření, budou přípustné pozice robotu patřit do množiny: m

Μ (k ) = I Μ i (k ) .

( 16 )

i =1

Pokud Μ ( k ) = Ø , lze vyvodit, že meze chyb ε iw (k ) a ε vi (k ) nejsou stanoveny správně [5].

4.2

Techniky lokalizace

Samotná lokalizace robotu ve známém prostředí spočívá ve vyhodnocení všech použitelných informací, přicházejících ze senzorů interních i externích, a znalostí o prostředí. V dostupné literatuře je uvedena široká škála různých metod často pojmenovaných po svém autorovi. Většina z nich kombinuje několik základních technik a přístupů, jejichž výčet je zde uveden.

• • •

Techniky lokalizace robotu spadají do tři základní kategorií: na základě chování při interakci s okolím (angl. Behavior–based approaches); pomocí význačných bodů (angl. Landmarks); pomocí shody senzorových pozorování (angl. Dense sensor matching).

Pro poslední kategorii používající senzorová pozorování najdeme mnoho různých přístupů. Pro ilustraci zde uvádím některé z nich: Bayesovy filry; •

Strana 26

• • • • • • • •

„particle“ filtry; Kalmanovy fitry; rozšířené Kalmanovy fitry; Monte Carlo (MCL); adaptivní MCL (A–MCL); smíšená MCL (Mix–MCL); histogramové metody; „scan-matching“ metody.

• •

Další varianty přináší způsob reprezentace mapy: topologická reprezentace; síťová (mřížková) reprezentace.

.

Obor v současné době prožívá překotný vývoj a je značně nepřehledný. Prozatím není dostupná literatura poskytující ucelenou systemizaci. Informace byly čerpány především z internetových stránek.

4.3

Vybrané metody lokalizace

4.3.1 Metoda trilaterace Trilaterace, podobně jako triangulace, je metoda určování relativní polohy objektů pomocí vztahů v trojúhelníku. Na rozdíl od triangulace, při které se pro výpočet polohy objektu využívá změřených úhlů (společně s alespoň jednou známou délkou), se při trilateraci využívá dvou a více bodů se známými souřadnicemi (tzv. referenční body) a změřených délek mezi objektem a referenčními body. K jednoznačnému určení relativní polohy bodu v rovině pomocí trilaterace, jsou obecně potřeba alespoň tři referenční body.

Strana 27

Obr. 12 Princip metody trilaterace.

4.3.2 Markova metoda lokalizace robotu Základní myšlenka této metody spočívá ve stanovení pravděpodobnosti pozice mobilního robotu pro všechny přípustné pozice daného prostředí. Takovouto domněnku o výskytu robotu na pozici l v čase T vyjádříme jako Bel ( LT = l ) . Pozice l představuje polohu x a y v kartézských souřadnicích a φ je orientace robotu. Když počáteční poloha robotu není známa, je hustota pravděpodobnosti Bel (l0 ) rovnoměrně rozdělena. Rozdělení pravděpodobnosti je aktualizováno pokaždé, když robot získá data ze senzorů (obvykle senzory odometrie a blízkostní senzory). Jakmile se robot začne pohybovat a získá data z odometrie provede aktualizaci pravděpodobnosti podle vztahu:

Bel ( LT = l ) = ∑ pa (l l ') Bel ( LT −1 = l ' ) ;

( 17 )

l'

kde pa (l l ') je pravděpodobnost, že se robot přesunul z pozice l‘ do nové pozice l pomocí akce a. Předpokládáme, že chyby v poloze i orientaci mají gaussovské rozložení se střední hodnotou 0 a jsou úměrné délce pohybu. Když robot získá data ze senzorů snímajících okolí sT provede aktualizaci pravděpodobnosti podle vztahu:

Bel ( LT = l ) = α T p( sT l ) Bel ( LT −1 = l ) ;

( 18 )

kde p( sT l ) je pravděpodobnost, že senzorová data sT jsou získána v pozici l a αT je normalizační koeficient, který zabezpečí následující podmínku:

∑ Bel(l ) = 1 .

( 19 )

l

Tato metoda a její testování prostřednictvím jednoduchého robotu vybaveného infračerveným blízkostním senzorem je popsána v [2].

4.3.3 Metoda lokalizace Monte Carlo Charakteristickou vlastností této metody je způsob reprezentace hustoty pravděpodobnosti popisující odhad pozice robotu. Tato metoda využívá množinu náhodně vybraných vzorků: S = {si i = 1,..., n} , ( 20 ) s = {l , p} , l = { x, y ,ϕ } ;

( 21 )

kde n je počet vzorků, x a y je poloha robotu, φ je jeho orientace a p je pravděpodobnost takovéto pozice pro kterou platí následující podmínky:

Strana 28

.

p ≥ 0,

n

∑p i =1

i

= 1.

( 22 )

Počáteční hodnotu pravděpodobnosti p volíme: pi =

1 , n

( 23 )

z čehož plyne, že hustota pravděpodobnosti je rovnoměrně rozdělena. Výhodou reprezentace pomocí vzorků je zejména to, že neklademe žádná omezení na tvar hustoty pravděpodobnosti a jsme tedy bez problémů schopni reprezentovat i multimodální hustoty pravděpodobnosti.

Obr. 13 Aktualizace hustoty pravděpodobnosti pozice l při přechodu z i-tého do i+1 stavu.

• •

Algoritmus má dvě základní fáze: predikce – posun všech vzorků na základě informací o změně pozice robotu např. z odometrie; korekce – úprava vah jednotlivých vzorků na základě shody či neshody naměřených dat s očekáváními, která by odpovídala pozici reprezentované příslušným vzorkem.

Strana 29

Výsledkem predikce je nová sada vzorků reprezentujících upravenou hustotu pravděpodobnosti na základě informací o změně pozice. Následuje krok korekce. Tento krok spočívá v modifikaci vah jednotlivých vzorků. Každý vzorek ohodnotíme podle toho, jak získané měření odpovídá předpokladu pro příslušnou pozici hodnoceného vzorku na základě dat ze senzorů okolí. Neustálým opakováním korekčního kroku dojde časem k určité degeneraci množiny vzorků. Většina vzorků bude mít zanedbatelné váhy a několik málo naopak váhy obrovské. Toto rozložení vah není optimální. Vzorky s malou vahou odpovídají pozicím, kde se robot pravděpodobně nevyskytuje, a proto je zbytečné se jimi zabývat. Naopak pozice se vzorky s vysokou vahou jsou více pravděpodobné. Proto se často využívá ještě fáze třetí – převzorkování. Účelem převzorkování je vyloučit vzorky s hodně malou vahou a vzorky s velkou vahou naopak rozdělit na několik vzorků [9]. Výhodou metody je malá výpočetní náročnost. Proto bývá často použita při experimentech s autonomními roboty, používající pro řízení jednoduché jednočipové mikropočítače. V další části mé práce bude popsána praktická aplikace metody „scan-matching“.

Strana 31

5

POPIS VYBRANÉ METODY

Metoda „scan matching“ je založena na porovnávání skutečných skenů získaných senzory robotu se skeny, které jsou generovány lokalizačním algoritmem v testovacích pozicích.

5.1

Mapa

Algoritmus využívá mřížkové reprezentace mapy ve dvourozměrném prostoru. Mapa je pak matice: M m, n = { aij };

( 24 )

obsahující prvky a s hodnotou dle uvedeného vztahu: ⎧1 pro obsazený prostor aij = ⎨ ; ⎩0 pro nobsazený prostor

( 25 )

kde m a n je velikost matice a pro i a j platí: i = 0,1,..., m − 1 ; j = 0,1,..., n − 1 .

5.2

( 26 )

Testovací pozice

Pozice robotu (poloha x a y v kartézském systému souřadnic a orientace φ) a testovacích bodů může nabývat hodnot: x ∈ { 0,1,...,m − 1} ; y ∈ { 0,1,...,n − 1} ;. ϕ ∈ ℜ .

( 27 )

Stanovíme vzdálenost sousedních pozic dx a dy Testovací pozice rozdělíme rovnoměrně v matici M podle vzahů: xi =

1⎞ n ⎛ m⎛ 1⎞ ⎜ (i − 1) + ⎟ ; y j = ⎜ ( j − 1) + ⎟ ; ϕ = 0 ; dx ⎝ 2⎠ dy ⎝ 2⎠

i = 0,1,..., I ; j = 0,1,..., J ;

kde I a J jsou nebližší nižší celá čísla než hodnoty výrazů

( 28 ) ( 29 )

n m −1 a − 1; dx dy

přičemž musí platit podmínky: a x i y j ∈ M ; a xi y j ≠ 1 . Vnikne tak sada testovacích pozic:

[

p k = xi

yj

ϕ ] ; k = 0,1,..., K ;

kde K je celkový počet testovacích pozic.

( 30 )

Strana 32

5.3

.

Sken

Model skenu si můžeme představit jako sadu jednotlivých měření vzdálenosti přístroje od překážek. Předpokládejme, že robot se nachází na pozici p a o je jednotlivý paprsek senzoru směřující pod úhlem α vztaženému ke směru ve kterém je orientován robot. Naměřenou hodnotu vzdálenosti d jednotlivého měření označíme: d j = g ( p, o j ) ;

( 31 )

j = 0,1,..., n − 1 ;

( 32 )

kde g ( p, o j ) představuje měření ideálního senzoru j–tého paprsku.

Obr. 14 Jednotlivé měření vzdálenosti. Pro n měření jednotlivých paprsků senzoru získáme kompletní sken okolí: d = { d j }.

( 33 )

Obr. 15 Celkový sken okolí.

Strana 33

Vytvoříme množinu dvojic S obsahující skeny získané v m vybraných pozicích mapy:

S=

5.4

{ [p

(i )

d (i )

] };

i = 0,1,..., m − 1 .

( 34 )

Hodnotící funkce

Mějme hodnotící funkci match, která ohodnotí shodu skenů z jednotlivých pozic v množině S se skenem d v pozici p: h ( i ) = match(i, d , S ) ; i = 0,1,..., m − 1 ;

( 35 )

kde h je relativní hodnocení shody pozice i. Hodnotící funkce minimalizuje součet absolutních hodnot rozdílu mezi jednotlivými měřeními d za každý sken postupně pro všechny možné natočení a všechny pozice i podle následujícího vztahu: ⎛ n −1 ⎜ ∑ d j − d k( i()l ) match(i, d , S ) = min⎜ j = 0 l ⎜ n ⎜ ⎝

kde:

⎞ ⎟ ⎟; ⎟ ⎟ ⎠

i = 0,1,..., m − 1 ; j = 0,1,..., n − 1 ; pro j + l ≤ n ⎧j +l k (l ) = ⎨ ; l = 0,1,..., n − 1 . ⎩ j + l − n pro j + l > n

( 36 )

( 37 ) ( 38 )

Pomocí hodnotící funkce match je vytvořen vektor v obsahující s vybraných pozic: v = { p0

p1 ...

ps −1

};

( 39 )

přičemž platí vztah pro jejich hodnocení h:

hi ≤ hi +1 ≤ hs ; i = 0,1,..., s − 1 ; s ≤ m .

( 40 )

Obdobně je vytvořen vektor v pro každou pozici robotu.

5.5

Vybrané cesty Po projetí celé trasy je vytvořen vektor P obsahující všechny vektory v: P = { v ( 0)

v (1)

kde t je počet pozic trasy robotu.

... v ( t −1)

};

( 41 )

Strana 34

.

Mějme funkci, která spočítá vzdálenost pozic na cestě robotu: odo( pi −1 , pi ) = ( xi − xi −1 ) 2 + ( yi − yi −1 ) 2 ,

( 42 )

kde i = 1,..., t − 1 . Sestavením skutečných pozic robotu na cestě vytvoříme vektor cesty c: c = { p0

p1 ...

pt −1

}.

( 43 )

Obdobně prohledáním stromu vzniklého kombinací pozic p po jednotlivých vektorech v vznikne množina pravděpodobných cest C: C = { cb

} ; b = 0,1,... ;

( 44 )

kde jednotlivé cesty c jsou vektory:

cb = { p ( 0) ∈ v ( 0)

p (1) ∈ v (1) ... p (t −1) ∈ v (t −1)

};

( 45 )

a platí podmínka: odo( pi −1 , pi ) − odo( p ( i −1) , p ( i ) ) ≤ ε , kde i = 1,..., t − 1 a ε je horní mez přípustné chyby. Pro každou cestu z množiny C stanovíme průměrnou odchylku polohy bodu od skutečné cesty a medián této odchylky a vyjádříme jejich histogramy. Ty poté slouží pro vyhodnocení lokalizace.

Strana 35

6

IMPLEMENTACE VYBRANÉ METODY

6.1

Úvod

V následujícím textu bude popsána implementace metody „scan – matching“. Tato metoda byla vybrána vzhledem k některým vlastnostem, které umožňují zjednodušit návrh modelu senzoru. Metoda bude použita pro simulaci lokalizace mobilního robotu ve známém prostředí. Prostředí bude omezeno na dva rozměry a bude reprezentováno mapou. Model robotu bude omezen na jeho pozici (poloha x a y v kartézském systému souřadnic a orientace φ). Robot se bude pohybovat po zadané cestě reprezentované posloupností pozic robotu. V každé pozici provede robot svou lokalizaci a vybrané pravděpodobné pozice uloží do historie. Po dokončení cesty provede vyhodnocení a vybere pravděpodobné cesty pro dané parametry lokalizace. Pro platné cesty vypočítá průměrnou odchylku od skutečné polohy robotu a medián odchylky. Potom vytvoří histogramy které uloží do souboru pro další analýzu.

6.2

Základní datové struktury

V příloze této práce je soubor slam.cs obsahující zdrojový text modulu SLAM, který je použit ve vytvořeném simulačním programu. V souboru jsou uvedeny definice tříd popsaných v následujícím textu. Třída Pos tvoří model polohy, který je základem, ze kterého jsou děděním odvozeny další třídy tvořící jádro lokalizace. Má dvě vlastnosti X a Y, které představují velikost souřadnice v příslušné ose. Pro zjednodušení kopírování je implementováno rozhraní ICloneable. Jsou také implementovány logické operátory porovnání == a != a aritmetické operátory + a -. Třída RobotPos představuje model pozice. Své vlastnosti dědí ze třídy Pos, které doplňuje o orientaci Fi. V deklaraci set je vstupní hodnota Fi normována pomocí metody MathEx.DegNorm() na hodnotu v rozsahu 0 až 360. Třída RobotPosMatch rozšiřuje model pozice o příslušné hodnocení a rozdíly od skutečné pozice. Obdobně jako předchozí, dědí své vlastnosti ze třídy RobotPos. Navíc má implementovány vlastnosti: •

Match – hodnocení pozice;

•

PosDiff – vzdálenost od skutečné pozice;

•

FiDiff – rozdíl úhlu orientace normovaný v rozsahu –180 až 180;

•

PathIndex – index skutečné pozice na cestě, ke kterému se vztahuje hodnocení a rozdíly.

Další třída je Area představující model pravoúhlé plochy. Vlasnosti, velikost X a Y a meze XMin, YMin, XMax a YMax mohou být v průběhu života instance měněny, přičemž je zabezpečen vzájemný soulad.

Strana 36

.

Třída Obstacle představuje model překážky a je použita při realizaci modelu mapy. Vlastnostmi jsou počáteční a koncová poloha StartPos a EndPos třídy Pos. Pro usnadnění vývoje a testování programu z příkazového řádku byla při implementaci všech tříd důsledně uplatněna přetížená metoda ToString(), která připraví textový řetězec výstižně popisující okamžitý stav její instance. Později, při vývoji vizualizace, se ukázalo, že metodu lze jednoduše použít pro přehledný vystup pomocí okna generovaného metodou ShowMessage().

6.3

Reprezentace mapy

Reálný robot se pohybuje v prostředí, které lze popsat mnoha různými způsoby. Významnými vlastnostmi popisu jsou přesnost a datová náročnost. Pro účely simulace byla zvolena reprezentace pomocí matice obsazenosti (angl. occupancy grid). Jedná se o kompromis mezi přesností a datovou náročností, přičemž lze tvrdit, že mezi nimi platí přímá úměra. Mapa je tvořena jednotlivými překážkami ve formě úsečky reprezentované krajními body (obstacles) a plochou na které se mohou vyskytovat (world).

Obr. 16 Definiční soubor a výsledná mapa. Základem reprezentace je třída Mapp a třída Obstacle. Tyto třídy obsahují metody pro manipulaci s daty a pro hledání překážky, případně okolí překážky. Implementována je i metoda LoadFromFile(), kterou může volat konstruktor při vytvoření mapy z definičního souboru (viz. obr. 16). Základní datovou strukturou ve třídě Mapp je matice int[,] arrayMapp. Matice je rozměru X x Y a svou velikost odvozuje z vlastností world.X a. world.Y

Strana 37

Prvky matice obsahují buď hodnotu 0, pokud není buňka obsazena překážkou, nebo hodnotu 1 v opačném případě. Matice je vytvořena a naplněna překážkami voláním metody CreateArrayMapp(). Při vytvoření matice se všechny prvky nastaví na hodnotu 0.

6.3.1 Rasterizace Překážka je definována jako úsečka (krajní body). Úkolem rasterizace je krajní body propojit a prvky jimiž spojnice prochází nastavit na hodnotu 1. Pro tuto úlohu je možno použít Bresenhamův nebo DDA algoritmus. V pomocné třídě MathEx je implementována metoda Beam(Pos StartPos, Pos EndPos), která používá modifikovaný algoritmus DDA. Metoda vrací vektor pozic přes které spojnice prochází.

Obr. 17 Rasterizace a), problém s DDA algoritmem b), řešení problému c), d). Na Obr.15 lze vidět rasterizaci dvou překážek. Počáteční body jsou tmavě modré a překážky jsou světle modré. Při následném testování se objevil problém s průhledností překážek pro některé paprsky skeneru (na obr. 17. červeně). Problém mohl být řešen modifikací DDA algoritmu. Jelikož je tato metoda použita také pro generování paprsku skeneru, byla vytvořena další metoda BeamGlue(Pos[] Beam), která modifikuje

Strana 38

.

„zalepí díry“ výsledek předchozí metody (na obr. 17. žlutě). Následně jsou prvky matice arrayMapp obsažené ve vektoru Beam nastaveny na hodnotu 1. Třída Mapp poskytuje další metody o kterých bude zmínka v dalším textu.

6.4

Reprezentace cesty

Cesta skutečného robotu je spojitá křivka, která je množinou pozic jimiž robot projede. Pro zjednodušení nahradíme cestu posloupností pozic robotu. Pozice (poloha x a y v kartézském systému souřadnic a orientace φ) je implementována ve třídě RobotPos. Samotná cesta je implementována ve třídě Path, která obsahuje metodu LoadFromFile(), kterou může volat konstruktor při vytvoření cesty z definičního souboru (viz. obr. 18). Cestu lze vytvořit i prázdnou a pomocí metody Add(RobotPos) do ní na konec přidávat nové pozice.

Obr. 18 Definiční soubor a výsledná cesta na mapě.

6.5

Model robotu

Skutečný robot je obvykle komplexní zařízení provádějící široké spektrum vzájemně provázaných činností. Pro účely této simulace bude vytvořen maximálně zjednodušený model. Model robotu je implementován ve třídě Robot, která obsahuje metody a vlastnosti nutné pro jeho lokalizaci. Konstruktor Robot(RobotParam, Mapp, Path) vytvoří instanci modelu robotu. Předanými parametry jsou mimo parametry robotu odkaz na mapu a cestu, kterou má robot projet. Parametr RobotParam obsahuje odkaz na definici parametrů skeneru robotu.

Strana 39

Po vytvoření instance je volána metoda Init(), která předá robotu první pozici z cesty Path. Zároveň nastaví stavové proměnné (isEndOfPath, isBeginOfPath, isInit) ve vlastnosti RobotStatus na příslušné hodnoty. Metoda Step() posune robota na následující pozici v cestě Path, za předpokladu že ještě nebylo dosaženo poslední pozice. Metoda GetOdometry() vrátí ujetou vzdálenost (odometrii) od poslední pozice cesty. Obdobně metoda GetOdometry(int PastIndex, int ActualIndex) navíc umožňuje vybrat libovolné pozice cesty, za předpokladu ActualIndex je větší nebo roven PastIndex. Metoda GetTrueScan() vrací instanci třídy Scan z aktuální pozice robotu. Další metoda GetScan(RobotPos RobotPos) vytvoří Scan z libovolné pozice předané v parametru RobotPos.

6.6

Model skeneru

Předlohou pro model skeneru v této simulaci, je proximitní laserový skener SICK, často užívaný při pokusech v oblasti mobilní robotiky. Pro popis skeneru postačují tři základní parametry: •

SensorView – úhel ve kterém skener rozmítá paprsek;

•

DiffAngle – úhel mezi sousedními paprsky;

Range – dosah paprsku (maximální vzdálenost překážky). • Odvozený parametr je pak:

•

noOfRays.–.počet paprsků, který je stanoven podle vztahu: ⎧ SensorView ⎪ Range + 1 pro SensorView < 360, ⎪ noOfRays = ⎨ ⎪ SensorView pro SensorView = 360. ⎪⎩ Range

( 46 )

Instance třídy Scanner je objekt vytvořený instancí třídy Robot při její inicializaci. Její metoda GetScan(RobotPos RobotPos, Mapp Mapp) je volána metodou Robot.GetScan() generující instanci třídy Scan. Parametry DiffAngle a Range se rozhodujícím způsobem podílejí na schopnosti robotu se lokalizovat.

6.6.1 Sken Třída Scan implementuje model měření senzoru okolí robotu. Jedná se o jednorozměrné pole hodnot datového typu double o délce noOfRays. Pro každý paprsek je vypočítána koncová pozice. Pomocná třída MatEx má implementovánu metodu Beam(), která vytvoří vektor pozic v matici arrayMapp počínaje počáteční pozicí a konče koncovou pozicí nebo okrajem matice. Matice je prohledávána na vektoru pozic a první nalezená pozice s hodnotou 1 je pozice překážky. Poté je stanovena vzdálenost

Strana 40

.

počáteční a nalezené pozice a ta uložena do pole Scan. Pokud se při prohledávání dojde až na konec vektoru je uložena hodnota rovnající se dvojnásobku dosahu Range.

Obr. 19 Vizualizace skenu s parametry: SensorView=360, DiffAngle=4, Range=50.

6.7

Lokalizace

Třída Localize implementuje metody používané pro aplikaci metody „scanmatching“. Slouží pro přípravu dat (pole History) použitých pro výběr pravděpodobných cest.

6.7.1 Parametry lokalizace Proces lokalizace robotu v daném bodu jeho cesty (instanci třídy RobotPos) můžeme nastavit pomocí parametrů. Parametr NoBestRobotPos stanovuje počet vybraných pozic (instance třídy RobotPosMatch) s nejlepším hodnocením, které jsou uloženy do pole History. Parametr LocalizeMethod určí způsob jakým bude vytvořena množina bodů k hodnocení. Je možno vybrat ze dvou způsobů: •

LocalizeParam.SimpleLevel – pro hodnocení jsou použity všechny pozice vytvořené generátorem podle parametru TestPointParam;

•

LocalizeParam.DualLevel – v první úrovni je vybrán příslušný počet nejlepších pozic podle parametru NoBestRobotPos20 vytvořených generátorem podle parametru TestPointParam. Následně jsou v druhé úrovni v okolí těchto bodů vytvořeny sady nových bodů vytvořených generátorem podle parametru TestPointParam2. Z každé z nich je pak vybrán počet nejlepších pozic určený parametrem NoBestRobotPos21. Pro hodnocení jsou požity všechny takto vybrané pozice v druhé úrovni.

Strana 41

Parametr OdometryTolerance určuje toleranci odometrie, pro proces výběru pravděpodobných cest. Následně parametr MaxProbPath .zavádí omezení počtu těchto cest, pro předčasné ukončení cyklu, v případě příliš rozsáhlého stavového prostoru.

6.7.2 Generátor testovacích pozic Generátor testovacích pozic je implementován jako samostatná třída nazvaná TestPointGenerator. Hlavní metoda GetTestPoints() vygeneruje pole pozic v mapě podle nastavení parametru TestPointParam. Třída TestPointParam obsahuje vlastnosti, popisující způsob generování pozic TestPointMethod, jejich hustotu v ose x DiffX a y DiffY a přednastavenou orientaci Fi. Způsob generování pozic TestPointMethod je možno vybrat ze dvou způsobů: •

TestPointMethod.Ortogonal – pozice jsou rozmístěny pravidelně v mřížce s roztečí podle parametrů DiffX.a.DiffY;

•

TestPointMethod.Random – pozice jsou rozmístěny náhodně v okolí bodů v mřížce s roztečí podle parametrů DiffX.a.DiffY;

Obr. 20 Vizualizace generovaných pozic s parametry: TestPointMethod.Ortogonal, DiffX=6, DiffY=6, Fi=0.

6.7.3 Implementace Scan – matching Součástí třídy Localize jsou metody použité pro samotný proces lokalizace. Je implementována metoda Process(), která podle zvolené metody lokalizace volá metodu

Strana 42

.

ProcessSimpleLevel(), nebo ProcessDualLevel(). V principu jsou obě metody podobné. Po předání příslušných parametrů instanci generátoru, obdrží pole testovacích pozic. Následuje volání metody GetTestScans(), která vytvoří pole příslušných skenů k testovacím pozicím. Metoda robot.GetTrueScan() předá sken ze skutečného okolí robotu. Poté je volána metoda Evaluation(), která ohodnotí všechny testovací pozice a jejich přípustné natočení pomocí metody Match(). Potom je vybráno příslušné množství nejlepších pozic a uloženo do pole History, nebo je v případně metody ProcessDualLevel() provedena druhá úroveň procesu lokalizace v okolí vybraných testovacích pozic.

6.7.4 Vyhodnocení historie Další metoda, EvaluationProbabilityPath() implementovaná ve třídě Lokalize, prohledává stavový prostor možných cest v poli History. Pomocí metody GetOdometryMatch() vybírá vyhovující cesty, jejichž indexy pozic odkazující do pole History, a ty uloží do pole Probability. Použitá modifikace algoritmu prohledávání do hloubky umožňuje přeskočit pozici, pro kterou nebyla nalezena žádná vyhovující pozice v poli History. V tomto případě je do pole Probability uložen odkaz na pozici s nejlepším hodnocením. Následně je spočítána průměrná odchylka a medián této odchylky pro každou vyhovující cestu.

6.8

Dávka simulací

Průběh jednotlivé simulace určuje definiční soubor, obsahující cesty k definičním souborům mapy, cesty robotu a parametrů lokalizace. Po jeho načtení můžeme simulaci spustit a sledovat vizualizaci. Součástí popisu jsou i logovací soubory pro uložení výsledků lokalizace. Této vlastnosti se využívá při spuštění dávky simulací definované ve vlastním souboru. V tomto případě neprobíhá vizualizace ale proces hledání parametru OdometryTolerance (viz. 6.8.1), který má následný průběh: •

robot projde celou cestou a vytvoří pole History;

•

postupně, počínaje hodnotou 0, je hledaný parametr zvětšován o hodnotu 1. Pokaždé je proveden pokus o vytvoření pole Probability. Pokud je vytvořeno, je histogram odchylek spolu s identifikačními daty uložen do logovacího souboru. V opačném případě je uložena informace o neúspěchu. Hledání se zastaví buď dosažením hodnoty parametru OdometryTolerance uvedené v definičním souboru nebo dosažením maximálního počtu cest uvedeném v parametru MaxProbPath.

•

Poté proběhne další simulace až do vyčerpání odkazů definičním souboru. Délka dávky není prakticky omezena. S výhodou lze dávku spustit v době kdy není počítač využíván. Výsledkem dávky jsou textové soubory ve formátu *.csv, které obsahují data, jež lze konvertovat do tabulkového procesoru k dalšímu zpracování. Soubor s názvem uvedeným v definičním souboru simulace jako parametr LogFile obsahuje základní data o průběhu jednotlivé simulace ve struktuře: • BatchFile – jméno definičního souboru simulace;

Strana 43

• • • • • • • • • • • •

MappFile – jméno definičního souboru mapy; MappWorld – rozměry mapy; Obstacles. – počet překážek na mapě; PathFile – jméno definičního souboru cesty; PathPos – počet pozic cesty; SensorView/DiffAngle/Range – parametry senzoru; BestPos – počet vybraných nejlepších pozic; DiffX/Y – parametry generátoru testovacích pozic; OdoTol – tolerance orometrie; NoSkip – počet přeskočených pozic; SkipPos – výčet přeskočených pozic; Valid – počet nalezených cest;

•

MaxValid – hodota parametru MaxProbPath;

• • • • •

ValueMax – střed třídy maximální četností; Ratio – poměr ploch histogramu podle kapitoly 6.8.1; Valuation – hodnocení; Type – typ charakteristiky (medián); 0,00; 2,00; 4,00; 6,00; 8,00; 10,00; 12,00; 14,00; 16,00; 18,00; 20,00; 22,00 – relativní četnost jednotlivých tříd histogramu.

Pokud budou pro různé simulace použity stejné názvy souboru, potom nová data budou přidána na jeho konec.

• • •

Soubor BatchLog.csv obsahuje hodnocení celé dávky simulací ve struktuře: FileName – jméno definičního souboru simulace; Name – popis z definičního souboru simulace; AvgValuation – průměrné hodnocení podle kapitoly 6.8.1.

Soubor je vždy přepisován. Dalším výstupem je grafická prezentace výsledků jednotlivých simulací ve formě bitmap s názvem odvozeným od názvu definičního souboru simulace. Prezentace obsahuje histogramy mediánu odchylky polohy nalezených cest pro jednotlivé hodnoty tolerance odometrie ve formě mapy a graf závislosti počtu nalezených cest pro tyto hodnoty. Zobrazeny jsou parametry lokalizace a hodnocení (obr. 21).

6.8.1 Hodnocení lokalizace Grafická prezentace poskytuje poměrné komplexní hodnocení sady parametrů na proces lokalizace. Svým rozsahem se hodí pro detailní porovnání vybraných simulací. Pro hodnocení celé dávky simulací, často čítající stovky případů, není vhodná. Pro tento účel je použito empirické hodnocení podle níže uvedeného postupu.

Strana 44

.

Pro každý histogram, u něhož je počet nalezených cest menší než mez stanovená parametrem lokalizace MaxProbPath, je provedeno hodnocení h podle vztahu: h=

a b

( 47 )

kde a je střed třídy s maximálním počtem cest a b je podíl plochy histogramu vymezené třídou 0–2 a nejbližší vyšší třídou, na obr. 22 červeně, ku celé ploše histogramu. Následně je pro každou simulaci spočítán průměr z prvních tří hodnocení ve směru vzrůstající hodnoty parametru OdometryTolerance.Takto stanovená hodnota je pak použita jako hodnocení celé simulace.

Obr. 21 Grafická prezentace výsledků simulace.

Strana 45

Obr. 22 Histogram a výpočet hodnocení.

6.9

Definiční soubory Pro řízení běhu programu jsou požity definiční soubory popsané v předchozím

textu: • • • •

definice mapy; definice cesty; definice simulace; definice dávky simulací.

Při psaní definičních souborů je nutno dodržovat tyto zásady: • poznámky je nutno uvádět znakem # na první pozici řádku; • parametry musí začínat na první pozici řádku; • je nutno dodržet pořadí parametrů; více parametrů na řádku oddělujeme čárkou; • • mezi jednotlivými parametry na řádku nesmí být mezera; • jako desetinou tečku požijeme znak tečka. Prázdné řádky jsou při načítání parametrů programem přeskočeny.

Strana 46

.

Obr. 23 Definiční soubor simulace a dávky simulací.

6.10 Vizualizace Vytvoření představy o chování robotu a procesu lokalizace umožňuje modul vizualizace v souboru VisForm.cs, který je uložen v příloze. Modul implementuje pouze jedinou třídu – VisForm. Ve své podstatě se jedná o formulář dědící své vlastnosti a metody ze třídy Form, která je základní součástí prostředí Visual C# 2005.

6.10.1 Grafická reprezentace Při volání konstruktoru VisForm() jsou vytvořeny instance dvou bitmapových objektů o rozměrech klientské plochy formuláře imgPerm a imgTemp. Do prvního z nich jsou pomocí grafických metod vykreslovány grafické objekty, které zůstávají viditelné po celou dobu simulace, a poté je provedeno kopírování jeho obsahu do bitmapy imgTemp. Do ní jsou následně vykresleny grafické objekty dočasného charakteru. Voláním metody OnPaint() mechanismem automatického překreslování obsahu formuláře je provedeno kopírování bitmapy imgTemp do viditelné části formuláře. Tímto je zajištěno zobrazování a následné mazání okamžitých stavových informací do vizualizace probíhající cesty robotu po mapě.

Strana 47

Obr. 24 Příklad vizualizace lokalizace robotu na pozici 12 cesty mající 39 pozic. Pro každý grafický objekt vykreslovaný při vizualizaci je vytvořena zvláštní metoda. Kromě parametrů určujících polohu a doplňkové informace musí být při volání použit parametr výčtového typu mající dvě hodnoty (Layer.TEMP a Layer.PERM), který slouží pro provedení výše uvedeného mechanizmu. Pro transformace souřadných systémů mapy a formuláře byly implementovány metody xWorlToForm() a yWorldToForm().

6.11 Ovládání programu K ovládání programu byl navrhnut kontrolní panel, který se při inicializaci srovná s pravým dolním okrajem obrazovky. Naopak, při spuštění vizualizace, se vytvořený formulář srovná s levým horním okrajem obrazovky. Je tak zajištěno co nejmenší, nebo žádné překrývání formulářů.

Strana 48

.

6.11.1 Význam tlačítek Formulář (obr. 25) je rozdělen na jednotlivé panely, které jsou pro účel popisu významu tlačítek a textových polí číslovány směrem nahoru.

Obr. 25 Panely ovládacího formuláře.

•

Panel I. (Řízení běhu programu) Load : standardní dialog pro otevření definičního souboru simulace. Simulace je načtena, a po potvrzení dialogu zobrazujícího její parametry je vytvořen formulář vizualizace. Je zobrazena mapa, cesta a robot na pozici s indexem 0. Současně jsou aktualizována textová pole ukazující parametry.

Strana 49

•

•

• •

• • • •

Run : spustí běh simulace. Robot postupně prochází všechny pozice na cestě. V každé z nich jsou zobrazeny vybrané pozice uložené do pole History. Po skončení cesty je vyvolán dialog zobrazující statistiku simulace. RunTo : spustí běh simulace. Simulace se zastaví po dosažení pozice s indexem uvedeným v sousedním textovém poli. Znovu lze spustit tlačítkem RunTo nebo Run. Save : standardní dialog pro uložení definičního souboru simulace podle aktuálního stavu parametrů Batch : standardní dialog pro otevření definičního souboru dávky simulací. Po provedení výběru je zobrazen dialog zobrazující obsah dávky. Po potvrzení je dávka spuštěna. Panel II. (Statistiky a vizualizace) Init: zrušena stará vizualizace a vytvořena nová podle aktuálního nastavení parametrů. Evalua.: proveden nový výpočet pravděpodobných cest podle aktuálního nastavení parametrů. Probab.: vytvořen dialog obsahující tabulku pravděpodobných cest.

•

Histogr.: vytvořen dialog obsahující histogram odchylek pravděpodobných cest v tabulkové podobě. Path– : zobrazí předchozí nebo první pravděpodobnou cestu na mapě. (Aktuální zobrazovaná cesta je vykreslena v červené barvě, předchozí změní barvu na žlutou.) Path+ : zobrazí následující nebo první pravděpodobnou cestu na mapě.

•

History: zobrazí dialog obsahující jednotlivé pozice uložené v poli History.

•

Stat.: zobrazí dialog obsahující statistické informace o průchodu robotu cestou.

•

H.Row.: zobrazí pozice z pole History na mapě. Červenou barvou jsou zobrazeny pozice odpovídající nastavenému indexu cesty v sousedním textovém poli. Fialovou pak pozice z předchozího indexu. Současně je zobrazen dialog obsahující podrobný popis těchto pozic.

•

•

Panel III. (Parametry simulace) Mapp: zobrazí dialog popisující objekty aktuální mapy.

•

Param: zobrazí dialog popisující parametry aktuální simulace.

•

Update: provede nastavení parametrů simulace podle aktuálního stavu parametrů uvedených v textových polích. WinSize: šířka klientské části formuláře vizualizace. Přednastavená hodnota je 800 zobrazovacích bodů.

•

Strana 50

•

.

•

Direktory: aktuální adresář. Všechny definiční souboru musí být umístěny v tomto adresáři. DefFile: soubor definice simulace

•

MapFile: soubor definice mapy.

•

PathFile: soubor definice cesty.

•

LogFile: soubor pro uložení výsledků dávky simulací.

•

Panel IV. (Parametry robotu) RobotName: název modelu robotu.

•

ScannerName: název modelu skeneru.

•

SensorView: úhel ve kterém skener rozmítá paprsek (kap. 6.6.1).

•

DiffAngle: úhel mezi sousedními paprsky (kap. 6.6.1).

•

Range: dosah paprsku (maximální vzdálenost měřené překážky, kap. 6.6.1).

•

Panel V. (Parametry lokalizace) LocalizeMethod: výběr metody lokalizace (kap. 6.7.1).

•

NoBestRobotPos: počet vybraných pozic (kap. 6.7.1).

•

•

NoBestRobotPos20: počet vybraných pozic ve druhé úrovni. Pokud není použita příslušná metoda, nemusí být nastaveny. (kap 6.7.1). NoBestRobotPos21: počet vybraných pozic ve druhé úrovni. Pokud není použita příslušná metoda, nemusí být nastaveny. (kap 6.7.1). TestPointMethod: výběr metody generátoru testovacích pozic (kap. 6.7.2).

•

DiffX: rozteč pozic v ose x (kap. 6.7.2).

•

DiffY: rozteč pozic v ose y (kap. 6.7.2).

•

Fi: orientace testovacích pozic (kap. 6.7.2).

•

OdometryTolerance: tolerance pro porovnání odometrie (kap. 6.7.1).

•

MaxProbPath: maximální počet pravděpodobných cest (kap. 6.7.1).

•

Level1: parametry generátoru testovacích pozic pro první úroveň lokalizace.

•

Level2: parametry generátoru testovacích pozic pro druhou úroveň lokalizace. Pokud není použita příslušná metoda, nemusí být nastaveny.

•

Strana 51

Obr. 26 Použitím tlačítek Path+ a path- lze zobrazovat nalezené cesty.

Strana 53

7

SIMULACE

Simulace lokalizace robotu ve známém prostředí chápeme jako proces, který poskytne dostatečně velikou sadu měření pro následné vyhodnocení vlivu změny parametrů na její přesnost.

7.1

Mapy

Vlastní práci zahájíme vytvořením definičních souborů map. Jednotlivé mapy budou obsahovat odlišný charakter překážek. Pro eliminaci vlivu mřížkové reprezentace okolí robotu vytvoříme mapy s překážkami: • rovnoběžnými s osami souřadného systému, • nevnoběžnými s osami souřadného systému, • smíšenými.

Obr. 27 Vzorky map použitých v simulaci.

7.2

Cesty

Dalším krokem je vytvoření definičních souborů cesty robotu. Kvůli omezení velikosti stavového prostoru, který budeme prohledávat, zvolíme hodnotu počtu kroků cesty v rozmezí 8 až 10. Navrhneme rovněž více cest s rozdílným charakterem.

Obr. 28 Cesty použité v simulaci.

Strana 54

7.3

.

Analýza citlivosti parametrů

Parametrů, ovlivňujících lokalizaci, je v použitém modelu mnoho. Počáteční hodnota části parametrů bude zvolena. Pro optimalizovaný parametr bude vytvořena sada hodnot, pro které bude probíhat testování. Kombinací map a cest je vytvořeno devět různých scénářů, které poslouží jako základ pro definiční soubory simulace. Teoretickou chybu lokalizace jedné pozice robotu za nejnepříznivějších podmínek lze stanovit podle jako:

εT =

DiffX 2 + DiffY 2 . 2

( 48 )

Z uvedeného vztahu plyne, že chyba by měla být přímo úměrná hustotě testovacích pozic.

7.3.1 Parametry DiffX/DiffY/Fi Parametr SensorView DiffAngle Range

Parametry skeneru Hodnota volíme hodnotu 360 volíme hodnotu 5 volíme hodnotu 60 Parametry lokalizace

Parametr LocalizeMethod OdometryTolerance MaxProbPath NoBestRobotPos

Hodnota volíme metodu SimpleLevel volíme hodnotu 15 volíme hodnotu 10000 volíme hodnotu 4

Parametr TestPointMethod DiffX/DiffY/Fi

Parametry generátoru testovacích pozic Hodnota volíme metodu Ortogonal hodnotu budeme hledat v sadě 3/3/0, 4/4/0, 5/5/0, 6/6/0, 7/7/0, 8/8/0, 9/9/0, 10/10/0, 11/11/0, 12/12/0, 13/13/0, 14/14/0 a 15/15/0 Dávka simulací

Počet definičních souborů Soubory s výsledky

117 batchlog.csv, test.csv

Výsledný diagram na obr.28 ukazuje, že přesnost je přímo úměrná hustotě sítě testovacích pozic. Pro testovaní dalších parametrů použije pro tento parametr sadu hodnot: 3/3/0, 7/7/0, 11/11/0 a 15/15/0.

Strana 55

Obr. 29 Závislost přesnosti lokalizace na hustotě sítě testovacích pozic

7.3.2 Parametr Range Parametry skeneru

Parametr SensorView DiffAngle Range

Hodnota volíme hodnotu 360 volíme hodnotu 5 hodnotu budeme hledat v sadě 10, 15, 20, 30, 40, 50, 60, 70, 80 a 90

Parametr LocalizeMethod OdometryTolerance MaxProbPath NoBestRobotPos

Parametry lokalizace Hodnota volíme metodu SimpleLevel volíme hodnotu 15 volíme hodnotu 10000 volíme hodnotu 4

Parametr TestPointMethod DiffX/DiffY/Fi

Parametry generátoru testovacích pozic Hodnota volíme metodu Ortogonal použijeme sadu hodnot 3/3/0, 7/7/0, 11/11/0 a 15/15/0 Dávka simulací

Počet definičních souborů Soubory s výsledky

360 batchlog.csv, test.csv

Strana 56

.

Výsledný diagram (obr.29) ukazuje, že přesnost se až do určité hodnoty parametru Range zvětšuje, jeho další zvětšování pak na ni nemá vliv. Pro testovaní dalších parametrů použijeme pro tento parametr hodnotu 60.

Obr. 30 Závislost přesnosti lokalizace na velikosti parametru Range.

7.3.3 Parametr NoBestRobotPos Parametry skeneru

Parametr SensorView DiffAngle Range

Hodnota volíme hodnotu 360 volíme hodnotu 5 volíme hodnotu 60

Parametr LocalizeMethod OdometryTolerance MaxProbPath NoBestRobotPos

Parametry lokalizace Hodnota volíme metodu SimpleLevel volíme hodnotu 15 volíme hodnotu 10000 použijeme sadu hodnot 2, 3, 4, 5, 6, 8, 10, 12 a 14

Parametr TestPointMethod DiffX/DiffY/Fi

Parametry generátoru testovacích pozic Hodnota volíme metodu Ortogonal použijeme sady hodnot 3/3/0, 7/7/0, 11/11/0 a 15/15/0

Strana 57

Počet definičních souborů Soubory s výsledky

Dávka simulací 360 batchlog.csv, test.csv

Výsledný diagram (obr.30) ukazuje, že při zvětšování hodnoty parametru dochází ke zhoršování přesnosti. Malá hodnota však nemusí poskytnou algoritmu lokalizace dostatek použitelných pozic pro stanovení pravděpodobných cest, čímž dochází k přeskakování pozic. Pro další testování volíme hodnotu 4.

Obr. 31 Závislost přesnosti lokalizace na velikosti parametru noBestRobotPos.

Strana 59

8

ZÁVĚR

Cílem této práce bylo implementovat vybranou metodu lokalizace mobilního robotu ve známém prostředí a testování pomocí simulací. Jako vývojový prostředek bylo použito prostředí Visual C# 2005 Express Edition (US) poskytované pro nekomerční účely zdarma. Lze konstatovat, že se použitý programový prostředek jeví jako vhodné řešení pro vytvoření testovací aplikace pro simulaci lokalizace. Jedním z výsledků této práce je aplikace, která umožňuje vizualizaci jednotlivých simulací lokalizace k posouzení vztahu mezi mapou, cestou a parametry lokalizace. Umožňuje také spustit dávku simulací pro statistické vyhodnocení vlivu parametrů na přesnost lokalizace. Testování vybrané metody na přibližně 1000 simulacích ukázalo, že se jedná o poměrně robustní algoritmus. Výběr optimálních parametrů byl testován na sadě úloh pokrývající svým rozsahem lokalizaci robotu pohybujícího se ve vnitřním prostředí v rámci jedné místnosti. Bylo zjištěno, že přesnost lokalizace je úměrná hustotě sítě testovacích bodů. Pro dané podmínky se ukázalo, že dosah skeneru musí být větší nebo roven 40 jednotkám. Jedná se o vzdálenost, ve které je pro většinu testovacích pozic dostupná nějaká překážka. Pro stanovení hodnoty počtu vybraných nejlepších pozic v každém kroku cesty volíme kompromis mezi nepřesností při větším počtu a přeskakováním pozic při menším počtu. Jako použitelná se ukazuje hodnota 4.

Strana 61

SEZNAM POUŽITÉ LITERATURY [1]

VĚCHET, S. Real-time localization for mobile robots. Inženýrská mechanika – Engineering Mechanics, Vol.12, (2005), No.A1, pp.3-12, ISSN 1210-2717, Enginering Academy of the Czech Republic.

[2]

KREJSA, J., VĚCHET, S. Markov Localization for Mobile Robots: simulation and experiment. Engineering Mechanics 2005, pp.177-178, ISBN 80-85918-93-5, (2005), Institute of Thermomechanics, CAS.

[3]

Thrun, Fox D., and Burgard W. Monte carlo localization with mixture proposal distribution. In Proceedings of the AAAI National Conference on Artificial Intelligence, Austin, TX, 2000. AAAI.

[4]

NOVÁK P. Mobilní roboty: pohony, senzory a řízení. 1. vyd. Praha: BEN, 2005. 248 s. ISBN 80-7300-141-1.

[5]

Ceccarelli N., Di Marco M.,Garulli A., Giannitrapani A., and Vicino A. Set membership localization and map building for mobile robots. Dipartimento di Ingegneria dell'Informazione Universit_a di Siena, Italy.

[6]

Sells Ch. C# a WinForms. 1. vyd. Brno: Zoner Press, 2005. 648 s. ISBN 80-8681525-0.

[7]

Virius M. C# hotová řešení. 1. vyd. Brno: Computer Press, 2006. 342 s. ISBN 80251-1084-2.

[8]

Hanák J. C# praktické příklady. 1. vyd. Praha: Grada, 2006. 288 s. ISBN 80-2470988-0.

[9]

http://www.robotika.cz.

[10]

http://www.cs.washington.edu/ai/Mobile_Robotics/mcl/index.html..

[11]

http://imr.felk.cvut.cz//index.html.

[12]

http://www.cs.cmu.edu/afs/cs/project/jair/pub/volume11/fox99a-html/jairlocalize.html

Strana 63

SEZNAM PŘÍLOH CD–ROM: • elektronická podoba této práce v souboru lokalizace_robotu.pdf; • zdrojové soubory slam.cs a visform.cs; • aplikace slam.exe; • simulace test_range; • simulace test_diffxy; • simulace test_nopos.