Lokalizace pro autonomní systémy

České vysoké učení technické v Praze Fakulta elektrotechnická, Katedra kybernetiky

Lokalizace pro autonomní systémy Ing. Roman Mázl Disertační práce

studijní program: obor studia: školitel:

Elektrotechnika a informatika Umělá inteligence a biokybernetika Ing. Libor Přeučil, CSc.

Gerstnerova laboratoř pro inteligentní rozhodování Skupina Inteligentní mobilní robotiky Praha, únor 2007

Poděkování Výzkum, jímž se zabývají různé části této práce byl v jednotlivých letech částečně přímo či nepřímo podporován z následujících zdrojů (v abecedním pořadí): Fond rozvoje VŠ č. 1043/00 (r. 2000), č. 1900/01 (r. 2001), č. 2156/2002 (r. 2002), Int. grantová soutěž ČVUT č.300108413 (r. 2001) a č. 0209413 (r. 2002), projekt PeLoTe FET-IST 2001-38873 (2002-2005), projekt SyRoTek MŠMT č. 2C06005 (od r. 2006), č. 6840770012 (2005), č. MSM 6840770038 (od r. 2007). Vývoj experimentální robotické platformy G2 byl podporován výzkumným záměrem MŠMT č. MSM 212300013 (1999-2004) a vlastní experimentální části práce byly dále podporovány z následujících zdrojů: Grantová agentura ČR č. GA102/02/0641 (r. 2002-3) a Centrum aplikované kybernetiky č. 1M0567 (od r. 2005).

Na tomto místě bych chtěl poděkovat svým rodičům za jejich obětavost a podporu při mém studiu, přítelkyni Radce za toleranci a trpělivost spojenou s mým zaneprázdněním během dokončování této práce.

Disertační práce navazuje na stávající výzkumné aktivity školícího pracoviště v oblasti mobilní robotiky, zejména na disertační práce RNDr. Miroslava Kulicha, Ph.D. a RNDr. Petra Štěpána, Ph.D. Pro vyhotovení této práce bylo použito existujícího experimentálního vybavení laboratoře Inteligentní mobilní robotiky Katedry kybernetiky na ČVUT FEL. Experimentální část práce se opírá zejména o využití unikátního mobilního robotu G2, na jehož HW a SW vývoji se nejvýznamnější měrou podílel Ing. Jan Chudoba spolu s Ing. Janem Faiglem, který řešil otázky spojené s operačním systémem robotu G2. Výsledky vyvinutých metod byly srovnávány s dřívějšími výsledky RNDr. Miroslava Kulicha, Ph.D., který též poskytl původní senzorická data použitá v jeho disertační práci za účelem dosažení shodných výchozích podmínek pro testovaní nových algoritmů. V práci je pro srovnání výsledků metody stavby modelu prostředí použito též práce RNDr. Petra Štěpána, Ph.D., který poskytl srovnávací obrázek mřížky obsazenosti z mnou dodaných dat. Na sběru experimentálních dat z 3D laserového scanneru a exteriéru se podíleli Ing. Tomáš Krajník a Ing. Jan Chudoba, kteří připravili komunikační interface pro ovládání polohovací jednotky scanneru. Všem zmíněným kolegům tímto děkuji. Mé poděkování patří též školiteli Ing. Liboru Přeučilovi, CSc. a všem ostatním členům skupiny Inteligentní mobilní robotiky Gerstnerovy laboratoře Katedry kybernetiky ČVUT FEL za všestrannou pomoc při práci, vstřícnost, příjemné pracovní prostředí a konstruktivní nápady a připomínky vztahující se k této práci.

V Praze dne 1. března 2007

Obsah Seznam obrázků

v

Seznam tabulek

vi

Matematické značení

viii

Terminologie

ix

1 Úvod

1

2 Autonomní robotické systémy 2.1 Architektura autonomních robotů . . . . . . . . . . . . . . . . . . . . . . . . 2.2 Interakce mobilních robotů a lidí . . . . . . . . . . . . . . . . . . . . . . . .

3 3 5

3 Definice problému 11 3.1 Lokalizace . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12 3.2 Stavba modelu světa . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13 4 Cíle disertace 15 4.1 Definice řešené úlohy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15 4.2 Omezení řešené úlohy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16 5 Senzorické systémy 5.1 Odometrie . . . . . . . . . . . 5.2 Inerciální senzory . . . . . . . 5.3 Sonary . . . . . . . . . . . . . 5.4 Proximitní senzory . . . . . . 5.5 Laserové dálkoměry, scannery 5.6 Kamery . . . . . . . . . . . . 5.7 Další senzorické systémy . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

. . . . . . .

17 17 18 19 20 21 21 23

6 Stav problematiky ve světě 25 6.1 Lokalizace polohy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25 6.1.1 Postupy využívající Kalmanova filtru . . . . . . . . . . . . . . . . . . 26 6.1.2 Markovská lokalizace . . . . . . . . . . . . . . . . . . . . . . . . . . . 28 v

vi

OBSAH . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

32 34 37 38 38 39 42 43 44 45 46 46 48 48 49 52

7 Point-to-Point lokalizace 7.1 Předzpracovaní dat . . . . . . . . . . . . . . . . . . . . 7.2 Segmentace dat . . . . . . . . . . . . . . . . . . . . . . 7.3 2D lokalizace . . . . . . . . . . . . . . . . . . . . . . . 7.3.1 Korekce hrubých odometrických chyb . . . . . 7.3.2 Hledání korespondujících bodových párů . . . . 7.3.3 Odstranění odlehlých bodů, pohyblivé objekty 7.3.4 Výpočet parametrů transformace . . . . . . . . 7.3.5 Validace výsledků lokalizace . . . . . . . . . . . 7.4 Vlastnosti metody Point-To-Point . . . . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

53 54 61 64 65 69 70 72 73 74

6.2

6.3

6.1.3 Monte-Carlo lokalizace . . . . . . . 6.1.4 Scan-matching lokalizace . . . . . 6.1.5 Histogramové přístupy . . . . . . . 6.1.6 Landmarkové lokalizace . . . . . . 6.1.7 Lokalizace z vizuálních landmarků Metody pro stavbu modelu prostředí . . . 6.2.1 Mřížky obsazenosti . . . . . . . . . 6.2.2 Geometrické mapy . . . . . . . . . 6.2.3 Topologické mapy . . . . . . . . . 6.2.4 Symbolické mapy . . . . . . . . . . 6.2.5 Mapy silnic . . . . . . . . . . . . . 6.2.6 Příznakové mapy . . . . . . . . . . Simultánní lokalizace a mapování . . . . . 6.3.1 SLAM a Kalmanův filtr . . . . . . 6.3.2 Algoritmus FastSLAM . . . . . . . 6.3.3 Ostatní přístupy . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

8 Bodově orientovaná mapa 8.1 Vazba lokalizace na mapu . . . . . . . . . . . . . 8.2 Reprezentace dat . . . . . . . . . . . . . . . . . . 8.3 Použití k-D stromu . . . . . . . . . . . . . . . . . 8.3.1 Stavba k-D stromu . . . . . . . . . . . . . 8.3.2 Hledání nejbližšího souseda v k-D stromu 8.4 Stavba bodové mapy . . . . . . . . . . . . . . . . 8.5 Simultánní lokalizace a mapování . . . . . . . . . 8.6 SLAM a kooperativní úlohy s více roboty . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

. . . . . . . .

75 75 76 77 78 79 81 84 86

9 Rozšíření do 3D 9.1 Lokalizace polohy ve 3D . . . . . . . . . . . . 9.1.1 Reprezentace polohy tělesa v prostoru 9.1.2 Quaterniony . . . . . . . . . . . . . . 9.1.3 Modifikace PTP metody pro 3D . . . 9.2 3D model z 2D dat . . . . . . . . . . . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

. . . . .

89 89 90 92 94 97

. . . . .

. . . . .

OBSAH 10 Experimentální výsledky 10.1 Popis experimentální platformy . . . . . . . . . . 10.1.1 Mobilní robot G2 . . . . . . . . . . . . . . 10.1.2 Mobilní robot Pioneer . . . . . . . . . . . 10.1.3 Laserový scanner SICK LMS-200 . . . . . 10.1.4 Zdroje chyb . . . . . . . . . . . . . . . . . 10.2 Popis realizovaných experimentů . . . . . . . . . 10.3 Filtrace mixed-pixelů . . . . . . . . . . . . . . . . 10.4 Lokalizace . . . . . . . . . . . . . . . . . . . . . . 10.4.1 Srovnávací testy lokalizace . . . . . . . . . 10.4.2 Testy přesnosti lokalizace . . . . . . . . . 10.4.3 Lokalizace v projektu PeLoTe . . . . . . . 10.4.4 Lokalizace ve venkovním prostředí . . . . 10.5 Stavba lokalizační mapy . . . . . . . . . . . . . . 10.6 Výsledky aplikačních rozšíření navržených metod 10.6.1 Kooperativní lokalizace . . . . . . . . . . 10.6.2 Registrace 3D scanů . . . . . . . . . . . . 10.6.3 3D model . . . . . . . . . . . . . . . . . .

vii

. . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . .

99 99 99 100 101 102 103 104 108 108 110 116 118 120 126 126 127 130

11 Shrnutí, dosažené cíle disertace 11.1 Shrnutí a přínos práce . . . . . . . . . . . . . . . . . . . 11.1.1 Lokalizace mobilního robotu . . . . . . . . . . . . 11.1.2 Výstavba modelu prostředí a datová reprezentace 11.1.3 Implementace a experimentální ověření algoritmů 11.2 Dosažené cíle disertace, porovnání se stanovenými cíli . 11.3 Otevřené problémy, další rozvoj metod . . . . . . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

133 133 134 135 135 136 137

12 Závěr

. . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . .

139

Literatura 141 Rejstřík . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 152

Seznam obrázků 2.1 2.2 2.3 2.4 2.5 2.6

Struktura mobilního robotu . . . . . . . . . . . . . . Robot Pearl v Longwood Retirement Community . . Dopravní vozíky v nemocnici . . . . . . . . . . . . . Robot RP-7 na vizitě v nemocnici a robot Wakamaru Humanoidní robot HRP-2P . . . . . . . . . . . . . . Armádní robot Sherpa . . . . . . . . . . . . . . . . .

3.1

Vzájemné vazby mezi lokalizací a modelem světa . . . . . . . . . . . . . . 11

5.1 5.2 5.3 5.4 5.5 5.6 5.7 5.8 5.9 5.10 5.11

Vyzařovací charakteristika sonaru . . . . . . . . . . . . . . . Proximitní senzor Sharp GP2Y0A02YK . . . . . . . . . . . Závislost napětí na vzdálenosti u proximitního senzoru . . . Princip měření laserovým dálkoměrem . . . . . . . . . . . . Vnitřní struktura rovinného laserového scanneru . . . . . . . 1D aktivní triangulace . . . . . . . . . . . . . . . . . . . . . 2D aktivní triangulace . . . . . . . . . . . . . . . . . . . . . 3D scanner Minolta Vivid VI-700 . . . . . . . . . . . . . . . 3D kamera SwissRanger CSEM SR 3000 . . . . . . . . . . . Satelit systému NAVSTAR GPS, převzato ze zdroje NASA Dráhy družic systému GPS . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . .

. . . . . . . . . . .

. . . . . . . . . . .

. . . . . . . . . . .

. . . . . . . . . . .

. . . . . . . . . . .

. . . . . . . . . . .

. . . . . . . . . . .

19 20 20 21 21 22 22 23 23 24 24

6.1 6.2 6.3 6.4 6.5 6.6 6.7 6.8 6.9

Myšlenka Markovské lokalizace . . . . . . . . . . . . . . . Vzorkovaná aproximace pravděpodobnosti výskytu robotu Parametry pro prokládání bodů tangenciální přímkou . . . Ilustrace bodové korespondence . . . . . . . . . . . . . . . Způsoby reprezentace map prostředí . . . . . . . . . . . . Reálné prostředí . . . . . . . . . . . . . . . . . . . . . . . . Polygonální grafová reprezentace . . . . . . . . . . . . . . Pozorování objektu a částečná informace o jeho poloze . . Vazby úloh lokalizace, mapování a řízení pohybu . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

. . . . . . . . .

29 34 35 36 41 43 43 47 51

7.1 7.2 7.3

Jev „mixed-pixelÿ u laseru SICK . . . . . . . . . . . . . . . . . . . . . . . 55 Naměřené body ze 140 scanů s mixed-pixel jevem . . . . . . . . . . . . . . 56 Data z jednoho scanu s mixed-pixely . . . . . . . . . . . . . . . . . . . . . 56 viii

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

4 5 6 7 8 9

SEZNAM OBRÁZKŮ

ix

7.4 7.5 7.6 7.7 7.8 7.9 7.10 7.11 7.12 7.13 7.14 7.15 7.16 7.17 7.18 7.19

Množina bodů před rozdělením . . . . . . . . . . . Množina bodů po rozdělení . . . . . . . . . . . . . Postup rekurzivní segmentace úseček . . . . . . . . Vznik okluzní hrany . . . . . . . . . . . . . . . . . Příklady úseček, určené k odstranění . . . . . . . . Obecná rovnice přímky . . . . . . . . . . . . . . . . Kolmé vzdálenosti bodů . . . . . . . . . . . . . . . Ukázka segmentovaných scanů pro korekci natočení Vstupní data pro úhlové histogramy . . . . . . . . Výsledný histogram orientací úseček . . . . . . . . Volba rozptylu pro úsečku složenou ze dvou bodů . Tvorba směrových histogramů . . . . . . . . . . . . Histogram h(k) - referenční a aktuální scan . . . . Korelační funkce . . . . . . . . . . . . . . . . . . . Granularita prostředí pro hledání korespondencí . . Znázornění kritéria pro validaci bodových párů . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . .

61 61 62 62 62 63 63 66 66 66 67 68 68 68 69 71

8.1 8.2 8.3 8.4 8.5 8.6

Rozdělení roviny k-D stromem . . . . . . . . . . . . . . . . Uzly ve vytvořeném k-D stromu . . . . . . . . . . . . . . . Cílový uzel, horní odhad nejbližšího souseda . . . . . . . . Neprohledávaná oblast stromu . . . . . . . . . . . . . . . . Závislost počtu bodů v mapě na normalizační vzdálenosti Vyloučená senzorická data z dalšího zpracování . . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

. . . . . .

79 79 80 80 83 87

9.1 9.2 9.3 9.4 9.5

Znázornění pohybu rovinného senzoru . . . . . . . Popis polohy pevného tělesa v prostoru . . . . . . . Geometrická reprezentace quaternionu . . . . . . . Setříděné vzdálenosti bodových párů a křivka jejich Konfigurace laserových scannerů pro sběr 3D dat .

. . . . . . . . . . . . ve 3D . . . .

. . . . .

. . . . .

. . . . .

. . . . .

89 90 93 95 97

10.1 10.2 10.3 10.4 10.5 10.6 10.7 10.8 10.9 10.10 10.11 10.12 10.13 10.14

Experimentální mobilní robot typu G2 . . . . . . . . . . . . . . . Experimentální mobilní robot typu Pioneer 3AT . . . . . . . . . . Výsledek filtrace mixed-pixelů . . . . . . . . . . . . . . . . . . . . Prostředí pro testy filtrace mixed-pixelů . . . . . . . . . . . . . . Závislost mixed-pixel jevu na vzdál. překážky . . . . . . . . . . . Data po lokalizaci metodou Line-To-Line . . . . . . . . . . . . . . Data po lokalizaci metodou Point-To-Map . . . . . . . . . . . . . Rozdíl poloh mezi dvěma scany, metoda LTL . . . . . . . . . . . Rozdíl poloh mezi dvěma scany, metoda PTM . . . . . . . . . . . Ukázka prostředí s vybranými testovacími pozicemi robotů . . . . Lokalizovaná data . . . . . . . . . . . . . . . . . . . . . . . . . . . Lokalizovaná senzorická mapa chodeb jednoho podlaží budovy . . Výřez z obr. 10.12 zobrazující kumulativní chybu lokalizace . . . Výsledek lokalizace robotu Pioneer ve venkovním prostředí (park)

. . . . . . . . . . . . . .

. . . . . . . . . . . . . .

. . . . . . . . . . . . . .

. . . . . . . . . . . . . .

99 100 106 106 107 108 108 108 108 114 115 117 117 119

. . . . . . . . . . . . . . . diferencí . . . . .

. . . . . . . . . . . . . .

x

SEZNAM OBRÁZKŮ 10.15 10.16 10.17 10.18 10.19 10.20 10.21 10.22 10.23 10.24 10.25 10.26

Závislost počtu bodů na mezibodové vzdálenosti . . . . . . . . . Množina bodů po normalizaci . . . . . . . . . . . . . . . . . . . Výsledná bodová mapa z dávkového zpracování . . . . . . . . . Senzorická mapa ve formě mřížky obsazenosti . . . . . . . . . . Bodové lokalizační mapy . . . . . . . . . . . . . . . . . . . . . . Rozložení mezibodových vzdáleností . . . . . . . . . . . . . . . Konvergence jednotlivých parametrů transformace . . . . . . . 3D scany ve svých výchozích pozicích . . . . . . . . . . . . . . . 3D scany po vzájemné registraci . . . . . . . . . . . . . . . . . . Konfigurace robotu G2 pro sběr dat pro konstrukci 3D modelu Výchozí prostředí pro tvorbu 3D modelu . . . . . . . . . . . . . Vytvořený bodový 3D model prostředí . . . . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

121 122 123 124 125 128 128 129 129 130 131 131

Seznam tabulek 8.1

Položky uzlu k-D stromu

. . . . . . . . . . . . . . . . . . . . . . . . . . . 78

10.1 10.2 10.3 10.4 10.5 10.6 10.7 10.8 10.9 10.10 10.11 10.12

Technické parametry laserového hloubkoměru SICK LMS-200 Výsledky filtrace mixed-pixel bodů v celém prostředí . . . . . Výsledky filtrace mixed-pixel bodů v části prostředí . . . . . . Srovnání metod Line-To-Line a Point-To-Map . . . . . . . . . Střední hodnoty odchylek v testovacích bodech . . . . . . . . Přesnost lokalizace, měření 1-3 . . . . . . . . . . . . . . . . . . Přesnost lokalizace, měření 4-6 . . . . . . . . . . . . . . . . . . Přesnost lokalizace, měření 7-9 . . . . . . . . . . . . . . . . . . Shrnutí parametrů a výsledků měření . . . . . . . . . . . . . Výsledná přesnost lokalizace . . . . . . . . . . . . . . . . . . . Výsledky lokalizace v uzavřeném systému chodeb . . . . . . . Výsledky lokalizace ve venkovním prostředí (parku) . . . . .

xi

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

. . . . . . . . . . . .

101 105 105 109 111 112 113 113 114 115 116 118

Matematické značení |x| x X x˙ x˙ ∗ |pa , pb | x ¯ σ(x) σ 2 (x) min(x) max(x)

... ... ... ... ... ... ... ... ... ... ...

absolutní hodnota x vektor x matice X quaternion x komplexně sdružený quaternion k x euklidovská vzdálenost bodů pa a pb střední hodnota x standardní odchylka veličiny x rozptyl veličiny x hodnota minimálního prvku z pole hodnot x hodnota maximálního prvku z pole hodnot x

xii

Terminologie Vzhledem k tomu, že v práci budou používány ustálené odborné termíny, které by nebylo vhodné překládat do českého jazyka, zde je uveden jejich výčet spolu s příslušným významovým vysvětlením. Laserový scanner - senzor poskytující data o vzdálenostech překážek v prostředí (rozmítaný dálkoměr, rangefinder). Scan - datová struktura (seznam bodů) obsahující informace o vzdálenostech objektů v prostoru, které byly změřeny z jednoho místa jako průsečík laserového paprsku a objektů v prostředí. Scan-matching - metoda pro registraci (sesazení, resp. určení vzájemné relativní polohy) dvou scanů sejmutých ze dvou či více míst v prostoru. Landmark - přirozený nebo uměle vytvořený významný objekt v prostoru, který lze relativně dobře rozpoznat použitými senzory a určit jeho pozici či orientaci v prostoru. Particle - datová struktura reprezentující jeden vzorek aproximace hustoty pravděpodobnosti v Monte-Carlo algoritmech, resp. v algoritmech particle filtrů. Mixed-pixel - bod v laserovém scanu neodpovídající reálné překážce vzniklý chybným měřením vzdálenosti na rozhraní dvou, za sebou ležících překážek, viz. kap. 7.1. 3D TOF kamera - kamera poskytující hloubkovou informaci o scéně, pracující na principu měření doby letu světla či fázového posuvu (time-of-flight, TOF princip).

xiii

Kapitola 1

Úvod Tři zákony robotiky: 1. Robot nesmí ublížit člověku nebo svou nečinností dopustit, aby člověku bylo ublíženo. 2. Robot musí uposlechnout příkazů člověka, kromě případů, kdy tyto příkazy jsou v rozporu s prvním zákonem. 3. Robot musí chránit sám sebe před poškozením, kromě případů, kdy tato ochrana je v rozporu s prvním nebo druhým zákonem. (Isaac Asimov: I Robot, 1950)

Vývoj autonomních systémů je jednou z hlavních oblastí výzkumných aktivit v oboru umělé inteligence. Jako autonomní systémy jsou v této práci chápány kolové, kráčející a popř. i jiné počítačem řízené integrované systémy - roboty, které jsou schopny autonomní, cílově orientované interakce s reálným prostředím, ve kterém se mohou volně pohybovat v kontextu řešené úlohy. Takovéto autonomní systémy jsou často nazývány mobilními roboty. Jejich činnost a interakce s prostředím vždy sleduje určitý globální plán, ovšem reálné provádění akcí je ovlivněno řadou vnějších vlivů, kterým se robot musí být schopen přizpůsobit. Robot tak musí být schopen vnímat a rozpoznávat prostředí, průběžně si vytvářet a přizpůsobovat vnitřní reprezentaci prostředí nazývanou model. Na základě tohoto modelu pak může v souladu s globálním plánem a zadanými cíly rozhodovat o vlastní činnosti. Cílem činnosti robotu je buď přímé ovlivňování stavu prostředí formou manipulace s předměty, nebo i jen pouhý pohyb robotu v prostředí, který umožní například automatizovaný sběr dat z různých částí prostoru. Důležitou schopností autonomních systémů je způsob komunikace s člověkem, která je přizpůsobena druhu řešené úlohy. Komunikace mezi robotem a člověkem může probíhat na základě přenosu strukturovaných dat, pomocí grafických a jiných elektronických či opticko-mechanických rozhraní umožňující i přímou obousměrnou interakci. Na nejvyšší úrovni lze za určitých předpokladů komunikovat i v přirozené řeči. 1

2

KAPITOLA 1. ÚVOD

Vedle autonomních robotických systémů existuje též velká skupina robotů, které nesplňují zcela výše uvedená kritéria a jsou často nazývány manipulátory. Tyto roboty jsou součástí např. automatických montážních linek a jsou zpravidla pevně naprogramované a provádějí předem definovanou činnost s omezenou mírou rozhodování o průběhu své vlastní činnosti. Toto rozhodování se omezuje na detekci množiny kolizních stavů a způsob interakce s prostředím je obvykle pevně zadán. Těmito roboty se však tato práce zabývat nebude. Robotika jako taková je multidisciplinární obor čerpající z mnoha vědních disciplín jako je teorie řízení, počítačové vidění, rozpoznávání, plánování, rozhodování, či umělá inteligence. Tato práce si klade za cíl rozbor a řešení základních problémů v mobilní robotice za definovaných podmínek jako je určování vlastní pozice v prostředí - lokalizace a tvorba interního modelu prostředí, který slouží zejména pro podporu lokalizace. Tato problematika bude řešena zejména v kontextu jednoho robotu, ovšem bude popsána i rozšířená varianta, která je využitelná v kontextu skupiny robotů, které při své činnosti kooperují. Struktura práce má toto členění: následující kapitola obecně shrnuje třídy úloh, které autonomní robotické systémy řeší, zabývá se architekturou robotů spolu s popisem oblastí a aplikací, ve kterých již byly autonomní robotické systémy využity. Kapitola 3 definuje problém lokalizace a mapování, vytyčené cíle řešené v této práci jsou uvedeny v kapitole 4. V kapitole 5 jsou popisovány senzorické systémy, které se používají v oblasti mobilní robotiky. Ve světě používané přístupy a metody pro řešení problematiky lokalizace a vytváření modelů prostředí jsou popsány v kap. 6 spolu s jejich základním popisem a vlastnostmi. Hlavní jádro práce začíná kap. 7. V úvodní části této kapitoly jsou diskutovány vlastnosti používaných postupů a metod v kontextu vytyčených cílů této práce. V další části práce je popsán z nich vycházející námi navržený způsob řešení lokalizace mobilního robotu. Navazující kapitola 8 se zabývá popisem metody pro stavbu modelu prostředí spolu s jeho provázáním s navrženou metodou lokalizace robotu. Kapitola 9 rozšiřuje dvě předešlé kapitoly o trojrozměrný přístup k řešené problematice. Vyvinuté metody byly implementovány a reálně testovány na existujícím robotickém systému. Experimentální ověření všech navržených metod spolu se zhodnocením kvality výsledků je uvedeno v kap. 10. V posledních dvou kapitolách je shrnut přínos této práce a reálné výsledky jsou porovnány s vytyčenými cíly doplněné o potenciální výhled a další vývoj do budoucna.

Kapitola 2

Autonomní robotické systémy Základní otázky, které musí každý autonomní robotický systém řešit jsou: • Kde se nyní nacházím? • Kde se nachází můj aktuální cíl? • Jakým způsobem se do stanoveného cíle fyzicky přemístím? Odpověď na tyto otázky mohou být nalezeny pouze za předpokladu znalosti pracovního prostředí robotu ve vhodné prostorové reprezentaci a poloze robotu v něm. První otázku řeší robotické lokalizační subsystémy, které určují polohu robotu v mapě prostředí. Odpověď na druhou otázku, kde se nachází cíl, určuje buď přímo operátor robotu nebo úlohově orientovaný subsystém, který postupně generuje dílčí podcíle. Tyto podcíle definují, kam se má robot v průběhu vykonávání příslušné úlohy (např. pokrývání pracovního prostoru, průzkum, či doprava zboží z místa na místo) přemístit. Poslední otázka se vztahuje k problematice navigace, která zahrnuje plánování trajektorií, řízení pohybu a vyhýbání se překážkám. Obecně lze shrnout, že navigace je problém nalezení a vykonání cesty z výchozího do cílového bodu tak, aby byla splněna omezení příslušné úlohy a realizovaný pohyb robotu byl bezkolizní.

2.1

Architektura autonomních robotů

Na architekturu autonomních robotů můžeme nahlížet jako na velmi komplexní zpětovazební systém s množstvím úrovní provázaných řídicích smyček. Celý systém robotu lze částečně dekomponovat na několik bloků, které jsou zobrazeny na obr. 2.1. Autor (Havel, 1980) rozděluje jednotlivé řídicí zpětovazební smyčky do tří úrovní na smyčku kognitivní, operační a reflexní. Veškeré poznávací, rozhodovací a řídicí činnosti robotu probíhají v kognitivním systému a jsou tedy součástí nejdelší kognitivní řídicí smyčky, která spolupracuje s podřízeným senzorickým, řídicím a motorickým systémem. Kognitivní systém robotu je zodpovědný za vykonávání zadaných cílů uživatelem. Kognitivními schopnostmi robotu nazýváme aktivity popisující sbírání, ukládání, transformaci a používání znalostí (Matlin, 2004). Jsou to schopnosti, kterými se roboty 3

4

KAPITOLA 2. AUTONOMNÍ ROBOTICKÉ SYSTÉMY Cíl úlohy Motorický systém Řešení úloh a plánování

Realizátor plánů

Akční členy

Řídicí systém Model prostředí

Navigace

Reflexy

Prostředí

Kognitivní systém

Senzorický systém Vnímání a chápání

Míra abstrakce

Zpracování a výběr dat

Senzory

Znalost světa

Obr. 2.1: Struktura mobilního robotu snaží přiblížit lidskému myšlení. Tyto schopnosti zahrnují široké spektrum mentálních procesů od získávání nových informací přes jejich zpracovávání a umísťování do paměti ve vhodném formátu až k jejich využívání v procesu rozhodování či řízení. Kognitivní schopnosti se z velké části opírají o model prostředí, v jehož kontextu se řeší zadaná úloha. Problém, jakým způsobem má být model prostředí reálně uložen a udržován je poměrně komplexní a velmi záleží na typu úlohy, dostupných senzorech a schopnostech robotu. Jedná se zejména o výběr a objem dat, která mají být uchovávána, ale také o jejich přístupnost a modifikovatelnost. O způsobech reprezentace modelů prostředí v podobě různých druhů map dále pojednává kap. 6.2. Operační smyčka robotu je zodpovědná za vykonávání dílčích plánů, které přicházejí z kognitivního systému. Zajišťuje zejména navigaci mobilního robotu v prostředí a zpracovává senzorická data. Na základě senzorických dat se dávají pokyny motorickému systému v souladu s požadovanými plány. Předzpracovaná data se dále předávají kognitivnímu systému ve formě informace popisující aktuální reálný stav řešené úlohy. Na nejnižší úrovni pracuje reflexní smyčka, která je často realizována i na hardwarové úrovni robota. Jejím úkolem je řešení kritických situací, např. odvrácení přímo hrozící nepředvídané kolize, které musí být vyřešeno velmi rychle a robustně. Za bezkolizní vykonávání plánů jsou přirozeně zodpovědné i obě nadřazené smyčky, ovšem pokud v prostředí nastanou nepředpokládané změny nebo se robot dostane do výjimečných situací, které musí být okamžitě řešeny, je toto řešení přenecháno reflexní smyčce. Řešení kolize na této nejnižší úrovni je realizováno nejjednodušším možným způsobem, často pouhým přechodem do zcela bezpečného stavu, např. vypnutím pohonů. Výhodou reflexní smyčky oproti nadřazeným systémům je její přímá vazba na senzorický a motorický systém, potřebné zásahy tudíž mohou být provedeny velmi rychle. V bezpečném stavu vzniklou situaci pak mohou vyhodnotit vyšší řídicí úrovně robota a navrhnout řešení krizové situace s ohledem na dlouhodobé plány činnosti robotu.

2.2. INTERAKCE MOBILNÍCH ROBOTŮ A LIDÍ

5

Rozdělení zpětných vazeb a subsystémy robotu nelze brát příliš striktně, ve skutečnosti jde o velmi provázané hierarchie vzájemně se ovlivňujících smyček různých úrovní. Je přirozeně možné, aby informace ze senzorického systému přímo ovlivnila činnost kognitivního systému a naopak i kognitivní systém může dát přímý pokyn motorickému systému, např. k okamžitému zastavení.

2.2

Interakce mobilních robotů a lidí

Přestože mobilní robotika je poměrně mladá vědní disciplína, lze již najít i první aplikace, kdy mobilní roboty opustily výzkumná pracoviště a jsou využívány v běžném životě. Tímto krokem vzniká nový obor nazývaný servisní robotika. Jednou z prvních aplikací, kde došlo k přímé interakci mobilního autonomního systému a člověka, bylo využití robotu jako automatického průvodce v muzeích (Thrun a kol., 2000). Mobilní robot Rhino (Buhmann a kol., 1995) byl v roce 1997 na šest dnů použit jako atraktivní poutač a zároveň průvodce v Deutsches Museum Bonn. Funkčně podobný robot Minerva o rok později prováděl dva týdny návštěvníky v Smithsonian Museum of Natural History in Washington, D.C. Pozitivní zkušenosti vedly k nasazení dalšího robotického systému Sage do Carnegie Museum of Natural History (Nourbakhsh et al., 1999). Tyto roboty jsou schopné autonomního, předem definovaného pohybu v místnostech s volně se pohybujícími lidmi. Úkolem těchto robotických autonomních systémů je provádění návštěvníků mezi expozicemi, interaktivní komunikace s nimi, představování expozic multimediálním způsobem atd. Z robotického hlediska tyto aplikace představují zejména řešení úlohy lokalizace a navigace ve známém prostoru s náhodnými překážkami (volně se pohybující lidé). Funkčně se řešení opírají o mřížkovou reprezentaci modelu světa (kap. 6.2.1), pro lokalizaci v prostoru se využívá pravděpodobnostních přístupů založených na Markovských a Monte-Carlo metodách (kap. 6.1.2). Roboty nalézají své uplatnění jako osobní asistent pro seniory, což ukázal např. rozsáhlý multidisciplinární projekt Nursebot (Pollack et al., 2002). Hlavní roli zde hraje mobilní robot Pearl, který má dvě hlavní funkce. První rolí je automatický připomínací systém a

Obr. 2.2: Robot Pearl v Longwood Retirement Community (Pollack et al., 2002)

6

KAPITOLA 2. AUTONOMNÍ ROBOTICKÉ SYSTÉMY

Obr. 2.3: Dopravní vozíky v nemocnici, převzato z (NDC, 2006) druhou je průvodce prostředím. Starší nebo jinak postižení lidé mohou mít při pohybu v prostředí potíže se smyslovým vnímáním nebo s motorikou. Mobilní robot vybavený laserovým scannerem a ultrazvukovými senzory má ve známém prostoru schopnost relativně přesné vlastní lokalizace, což robotu umožňuje stát se vhodným průvodcem pro postižené lidi. Při znalosti mapy prostředí navíc robot umožňuje určit polohu i druh pohybu osob v prostředí. Tyto schopnosti jsou využity k implementaci inteligentního osobního asistenta sloužícího pro připomínání rutinních denních činností, s jejichž pravidelným vykonáváním mohou mít starší lidé potíže. Jedná se např. o pravidelné stravovací intervaly, podávání léků, realizaci osobní hygieny a osobních potřeb. Robot se pohybuje v prostředí spolu s osobou, které asistuje, což jednak snižuje pocit její osamělosti, ale také umožňuje sledovat její denní rytmus a k tomu přizpůsobovat i vlastní činnost inteligentního připomínacího systému. V České republice byl na počátku devadesátých let spuštěn unikátní systém autonomních dopravních vozíků v pražské Fakultní nemocnici v Motole (NDC, 2006), (Danaher Motion, 2003). Zde 28 kusů dopravních vozíků během celého dne zajišťuje kompletní rozvoz jídel pacientům, dopravu zdravotnického materiálu, ložního prádla, převoz lůžek atd. Vozíky jsou schopné dálkového ovládání dveří, jízdy výtahem, reagují na případné překážky v cestě. Celý systém pokrývá dopravu mezi 16 distribučními body (kuchyň, prádelna, sklad lůžek, lékárna, zdravotní materiál, atd.) a 400 cílovými uzly. Vozíky během jednoho dne dohromady ujedou kolem 500 km. Celý systém je řízen centralizovaně, lze definovat priority při plánování úkolů, což umožňuje např. upřednostnit rozvoz jídla pacientům před rozvozem ložního prádla. Vozíky jsou navigovány pomocí indukčních kolejnic, ochrana proti kolizím s jinými objekty v prostředí je řešena ultrazvukovými senzory, v blízkosti překážky se robot zastaví a čeká na její odstranění, popř. zašle informaci o nastávající kolizi do velínu. Další prostor pro nasazení mobilních robotů v nemocnicích je ve funkci prostředníka mezi pacientem a lékařem. V současné době na světě pracuje asi 60 robotů typu RP7 (InTouch Health, 2006) firmy InTouch Health’s, které umožňují vzdálenou komunikaci pacienta a jeho ošetřujícího lékaře. Robot se skládá z mobilního podvozku, který je lékařem teleoperovaně ovládán např. joystickem a běžným PC s displayem a kamerou vytvářející

2.2. INTERAKCE MOBILNÍCH ROBOTŮ A LIDÍ

7

komunikační systém. Tímto způsobem je zajištěna plná audiovizuální komunikace pacienta s lékařem, lze takto realizovat vizity nebo na dálku konzultovat zdravotní stav pacienta. Lékař má navíc k dispozici všechny potřebné údaje o aktuálním stavu pacienta, aniž by musel být u pacienta fyzicky přítomen. Dohled či případné konzultace méně zkušených lékařů se specialisty je možné realizovat např. i z jejich domova. Jeden exemplář robotu RP7 byl v červnu 2006 úspěšně testován též v nemocnici na Homolce v Praze (Babička, 2006).

(a)

(b)

(c)

Obr. 2.4: (a) Robot RP-7 (b) na vizitě v nemocnici, převzato z (InTouch Health, 2006), (Babička, 2006) c) Společník Wakamaru (Mitsubishi Heavy Industries, 2006) Pro roli společníka do domácnosti je určen robot Wakamaru (Mitsubishi Heavy Industries, 2006) společnosti Mitsubishi. Tento metr vysoký robot na kolovém podvozku disponuje síťovým připojením s možností zpracovávat informační zdroje, dokáže rozpoznávat tváře dvou majitelů a osmi dalších osob, mluvit a porozumí 10 000 lidským slovům. Tyto schopnosti jej předurčují stát se přátelským členem rodiny, který sám dokáže navázat komunikaci a díky napojení na internetová média se může s lidmi bavit na aktuální témata. V době nepřítomnosti členů domácnosti robot může dům hlídat, v případě potřeby může majiteli zprostředkovat aktuální obrázek z libovolného dostupného místa v domě. V případě neočekávané události může robot poslat mail nebo kontaktovat bezpečnostní agenturu. Robot Wakamaru (obr. 2.4c ) byl podobně jako mnoho jiných robotů vystavován na výstavě Expo 2005. V devadesátých letech vznikl na Carnegie Mellon University systém Navlab 5 (Jochem a kol., 1995), (Jochem, 1996) upravením standardního automobilu Pontiac Trans Sport. Navlab 5 ukázal, že řízení směru jízdy osobního automobilu je možné přenechat automatickému systému založenému na zpracování obrazu z kamery a GPS navigace. Systém je schopen držet se bílé čáry na vozovce a podávat informace o bočních výjezdech ze silnice. Výsledky projektu byly prezentovány v rámci akce „No Hands Across Americaÿ(2331.7.1995), kdy systém Navlab 5 ujel 98.2% cesty mezi městy Pittsburgh a San Diego (cca 2849 mil) bez nutnosti dotýkat se automobilového volantu. Řidič automobilu ovládal pouze akcelerační a brzdový pedál. Do skupiny mobilních robotů jsou zařazovány nejrůznější typy robotů, které mají jednu

8

KAPITOLA 2. AUTONOMNÍ ROBOTICKÉ SYSTÉMY

společnou schopnost, a to přemisťovat se v prostředí za účelem splňování svých cílů. Celková mechanická koncepce těchto robotů je zpravidla odvozována od nejrůznějších kolových podvozků. Ukazuje se však, že v poslední době se část zájmu přesunuje k vývoji kráčejících robotů. V současné době lze považovat, že k vrcholných konstrukcím patří humanoidní typy robotů, tedy dvounohé, kráčející roboty, které svým vzhledem připomínají lidskou fyziognomii. V nedávné době byl firmou Sony veřejnosti představen humanoidní robot ASIMO, který je schopen jednoduché interakce s člověkem. V rámci projektu METI-NEDO Humanoid Robotics vznikl unikátní robot HRP-2 (Hirukawa et al., 2004), který je schopný až 60 minutové chůze rychlostí 1.25 Km/h. Robot byl představen v kooperaci s člověkem v úloze montování dřevěných panelů, dokáže věrně kopírovat složité lidské pohyby, kterými jsou například běh (Nagasaki et al., 2003) či tanec (Nakaoka et al., 2003) atd. Vyspělá motorika tohoto robota jej dokáže znovu bezpečně postavit i při povalení na zem.

(a)

(b)

Obr. 2.5: (a) Robot HRP-2P během zotavovacího manévru po pádu na zem (b) konfigurace jeho kloubů, převzato z (Hirukawa et al., 2004) Dalším robotem s velmi vyspělou motorikou je čtyřnohý armádní robot Sherpa (Lerner, 2006), který je schopen i ve velmi náročném terénu nést zátěž téměř 55 kg. Robot je poháněn benzínovým motorem a hydraulickými aktuátory, které umožní až 500 x za sekundu repozici trojkloubových hliníkových končetin. Orientaci v prostoru zajišťuje stereokamera, laserový scanner, akcelerometry a optické laserové gyroskopy. Firma Toyota vyvinula unikátní typ robotu s vyspělou mechanikou a umělými rty, které robotu umožňují hru na trubku (Toyota, 2007). Tento robot byl v roce 2006 představen i v Praze. Využití humanoidních robotů bylo shledáno významným též při terapii a vzdělávání dětí postižených autismem (Robins a kol., 2005). Autismus je celoživotní vývojová vada mající vliv na sociální verbální i nonverbální komunikační schopnosti a sociální interakce postižených osob s okolím. Zahrnuje potíže s formováním sociálních vztahů a zhoršení porozumění záměrů, pocitů a mentálních stavů okolí. Je znám poměrně kladný vztah au-

2.2. INTERAKCE MOBILNÍCH ROBOTŮ A LIDÍ

9

Obr. 2.6: Armádní robot Sherpa, převzato z (Lerner, 2006) tistických dětí k počítačům, proto byla vyzkoušena i možnost vzdělávat děti za pomoci dálkově řízeného humanoidního robotu, se kterými jsou děti schopny navázat hravou interakci. V rámci projektu Aurora (Dautenhahn et al., 2000) robot pracoval jako sociální zprostředkovatel učitele, se kterým jsou autistické děti schopny navázat lepší kontakt než s neznámou osobou, zaujme je svojí přítomností a snaží se jej napodobovat. Kromě výše uvedených reálných aplikací mobilní robotiky jsou vyvíjeny i významné aktivity v řadě roboticky zaměřených soutěžích. V rámci nich výzkumné skupiny vyvíjejí do jisté míry jednoúčelově zaměřené robotické systémy schopné autonomně vykonávat zadané cíle a to jak individuálně, tak i kooperativně. Jako příklad lze uvést soutěže v robotickém fotbalu (FIRA, 2007), soutěže řady Eurobot (Eurobot, 2007), Robocup Rescue (RoboCupRescue, 2007) a další. V posledních letech se začíná věnovat pozornost i hybridním spolupracujícím týmům, ve kterých vzájemně kooperují mobilní roboty a lidé (Argall et al., 2006), (Veloso et al., 2006), (Kulich a kol., 2004a).

Kapitola 3

Definice problému Mobilní robot je autonomně pracující stroj, který musí být vybaven systémy zajišťujícími bezpečnou a bezkolizní interakci s prostředím, ve kterém se pohybuje. Pokud má robot v prostředí vykonávat smysluplnou činnost, musí být schopen prostředí poznávat, získávat informaci o své vlastní poloze, vytvářet si jeho interní podobu (model) a pohybovat se v něm. Důležitou podmínkou pro řízený pohyb v prostředí je zejména znalost aktuální pozice v pracovním prostoru. Úloha získávání informace o vlastní aktuální pozici v prostoru a vytváření modelu světa bezesporu patří mezi základní problémy v oblasti autonomních systémů (Cox, 1991). Obě úlohy jsou do velké míry vzájemně svázány, proto se často řeší současně. Povaha obou úloh je do jisté míry komplementární, přesný model prostředí nelze vybudovat bez znalosti přesné polohy senzorických systémů a naopak spolehlivost a přesnost určování aktuální polohy je do velké míry podmíněna existencí vhodného modelu prostředí. Vzájemná provázanost problematiky lokalizace a modelu prostření je ilustrována na obr. 3.1.

Aktualizace modelu světa

Odhad pozice Předzpracování dat

Externí senzory Hrubá senzorická data a extrahované příznaky

Fúze dat

Predikce

Model světa

Slepý odhad pozice z pohybu

Odometrická Informace

Obr. 3.1: Vzájemné vazby mezi lokalizací a modelem světa

11

12

3.1

KAPITOLA 3. DEFINICE PROBLÉMU

Lokalizace

Mobilní robot ze svého principu vykonává zadané úlohy v různých místech v prostoru, ve kterém se musí být schopen orientovat. Nezbytnou součástí orientace a porozumění prostředí je lokalizace v něm. Lokalizací je myšlen proces odhadu aktuálních souřadnic (polohy) robotu na základě informací z dostupných senzorů. Vhodnými senzory pro řešení úlohy lokalizace jsou senzory, které umožňují získat informaci buď o vzájemné poloze robotu vůči prostředí nebo získat informaci o vlastním pohybu robotu v prostředí. Pro získání informace o vlastním pohybu v prostředí se používají inerciální a odometrické senzory, které poskytnou přibližný výchozí odhad polohy robotu. Inerciální senzory odvozují polohu robotu od jeho zrychlení a rotace, odometrické senzory měří přímo délku trajektorie, kterou mobilní robot projel. Oba měřící principy jsou však zatíženy vysokou chybou měření. Jako hlavní senzory pro přesné určení pozice jsou nejčastěji používány laserové dálkoměry (scannery), měřící přímou vzdálenost překážek, vůči kterým se robot orientuje. V poslední době se vedle laserových dálkoměrů začínají prosazovat i řešení využívající kamerových systémů. Souřadnice robotu v pracovním prostředí mohou být vztaženy buď k existujícímu modelu prostředí (mapě) nebo ke zvolené vztažné referenci. Referencí často bývá například počáteční poloha robotu. Je-li výchozí pozice robotu známa nebo pokud jsou důležité pouze relativní souřadnice mobilního robotu vzhledem k výchozí pozici, mluvíme o úloze sledování polohy (position tracking). Úloha sledování polohy je někdy nazývaná též úlohou kontinuální lokalizace. Tyto lokalizační úlohy korigují odometrické chyby vznikající během jízdy, čímž zpřesňují aktuální pozice robotu. V opačné situaci, kdy výchozí poloha není známa a potřebujeme znát absolutní souřadnice robotu v modelu světa, mluvíme o úloze globální lokalizace. Úloha globální lokalizace je někdy v literatuře nazývaná též jako „lost robot problemÿ. Jako jeden se základních rozdílů mezi úlohou globální lokalizace a sledování polohy lze považovat požadavek na existenci předem známého modelu světa. Zatímco globální lokalizace model světa potřebuje, pro sledování polohy není model prostředí nezbytně nutný, případně lze využít průběžně vytvářeného modelu během pohybu. Nejobecnější lokalizační úlohou je tzv. „kidnapped robot problemÿ, což je v principu zobecněná úloha sledování polohy a globální lokalizace. V této úloze jde o velmi robustní sledování polohy, kdy robot vedle sledování své pozice musí být schopný rozpoznat situaci, kdy byl „násilněÿ přemístěn do neznámé polohy tak, aniž by to byl schopen jeho senzorický systém přímo podchytit. Metoda pak musí být tedy schopna během kontinuální lokalizace detekovat, že se robot nachází na zcela jiném, než předpokládaném místě a v ten okamžik změnit lokalizační strategii. Tato lokalizační strategie pak využívá globálních lokalizačních algoritmů, které po určité době opět naleznou skutečnou správnou polohu robotu v prostředí. Jakmile je nalezena aktuální poloha robotu v prostředí, lokalizace robotu může pokračovat s využitím méně náročné strategie pro sledování polohy. Pokud uvažujeme pohyb robotu ve vnitřních prostorech (tj. uvnitř budov - haly, kanceláře a jiné obytné místnosti), úloha 2D lokalizace řeší odhad zpravidla tří parametrů, dvě kartézské souřadnice a úhel natočení robotu. V případě, že lokalizujeme robot v terénu, či

3.2. STAVBA MODELU SVĚTA

13

v prostředí, kde pohyb robotu není omezen jednou horizontální rovinou, je nutné odhadovat až šest nezávislých parametrů, které definují polohu pevného tělesa v prostoru, tj. tři kartézské souřadnice (x,y,z) a tři směrové úhly často označované jako „roll/pitch/yawÿ. Tato práce je zaměřena na řešení problému lokalizace z pohledu úlohy sledování polohy s vyžitím laserového dálkoměru rozmítaného v jedné rovině (laserový scanner). Princip činnosti a vlastnosti tohoto senzoru jsou rozebrány v kap. 5.5.

3.2

Stavba modelu světa

Výstavba modelu světa je dalším komplexním problémem nejen v robotice. Existence určité podoby modelu světa je základním předpokladem pro navigaci robotu během realizace nejrůznějších úloh, neboť jejich zadání jsou často přímo či nepřímo svázány s prostorovou konfigurací prostoru a zadáváním poloh dílčích cílů. Model světa pak představuje vztažnou soustavu, vůči které se cíle zadávají. Chceme-li optimálně plánovat a organizovat vybrané akce v neznámém prostředí, je třeba pro toto prostředí nejprve vybudovat model. Průzkum neznámého prostředí a vybudování modelu světa ve formě mapy zvoleného prostoru se tak stává samostatnou robotickou úlohou. Průběžnou tvorbu mapy prostředí nebo dokonce předem vytvořenou mapu vyžadují například i vybrané lokalizační algoritmy. Mapa jako taková je součástí modelu prostředí, který vedle ní může obsahovat informace o dalších vlastnostech a zákonitostech platných v daném prostředí. Pokud mluvíme o modelu prostředí, je třeba uvažovat též jeho vhodnou reprezentaci ve vztahu ke konkrétní řešené úloze. Na problém reprezentace a stavby modelu prostředí z nepřesných senzorických dat je třeba nahlížet z několika hledisek. • Reprezentace. Vnímané prostředí musí být ukládáno ve vhodné datové struktuře, která odpovídá složitosti prostředí (prázdné haly, zařízené místnosti). V případě, že se pracuje s hrubými, nezpracovanými senzorickými daty, je požadována úsporná datová reprezentace. • Nejistota v datech. Pro spolehlivé využití vytvořeného modelu je žádoucí udržovat též popis nejistoty v datech, která vzniká zpracováním zašuměných senzorických informací. Zde lze výhodně použít metod slučování dat pro snižování výsledné nejistoty. Slučovat lze data z různých datových zdrojů nebo z různých časových intervalů během vykonávání úlohy. • Výpočetní a datová náročnost. Použité datové struktury musí být efektivně aktualizovatelné typicky v reálném čase a to i v relativně rozsáhlých prostředích a různých úrovních rozlišení. Téměř vždy je třeba vhodným způsobem vyřešit kompromis mezi efektivitou metody (kvalita modelu) a výpočetní a datovou náročností. • Adaptibilita. Reprezentace modelu prostředí by měla být pokud možno aplikačně nezávislá, což je ovšem velmi náročná úloha. V reálných situacích se reprezentace prostředí často přímo podřizuje úloze, pro kterou má být model prostředí využit

14

KAPITOLA 3. DEFINICE PROBLÉMU tak, aby existence mapy umožnila nebo významně zvýšila efektivitu řešení vybrané úlohy.

Model prostředí může vzniknout mnoha způsoby, od manuálního pořízení dat až po zcela automatické mapovací postupy. Rozdíl v časové efektivitě je však značný. Např. v (Thrun et al., 1998) autor uvádí, že manuální zmapování prostor Deutsches Muzea v Bonnu pro činnost robotického průvodce RHINO (Buhmann a kol., 1995) trvalo přibližně týden. Později, když byly dostupné nástroje pro automatické mapování prostoru robotem, pro vykonání stejné úlohy stačilo pouhých 40 minut. Při stavbě modelu prostředí - mapování je třeba řešit několik základních úloh. Jednou z nich je volba vhodné trajektorie pohybu, která zajistí, že senzorický systém robotu bude mít k dispozici dostatečné množství dat z celého mapovaného prostoru. Tuto problematiku řeší plánovací algoritmy pro průzkum a pokrývání prostoru, které však nejsou náplní této práce. Volbu vhodné trajektorie lze nahradit vzdálenou teleoperací, přičemž operátor sám rozhoduje, do jakých míst v mapovaném prostoru je potřeba robot navést. Další nutnou podmínkou pro konzistentní mapování prostoru je schopnost lokalizace. Jak je zmíněno v kap. 3.1, svou povahou je mapování a globální lokalizace představitelem problému, který lze obrazně popsat jako problém typu „slepice a vejceÿ. Provázaná povaha problému lokalizace a mapování je důsledkem toho, že nepřesnosti v pohybu robotu mají vliv na chyby a přesnost senzorických měření, která se používají jako vstup mapovacích algoritmů. Jakmile se robot pohne, odhad jeho aktuální pozice je zatížen šumem vznikajícím nepřesností pohybu. Vnímaná absolutní poloha objektů vkládaných do mapy je pak ovlivněna jednak vlastním šumem měření, ale také chybou odhadu aktuální polohy robotu. Chyba zanesené polohy do mapy způsobena nepřesnou lokalizací může pak dále snižovat přesnost lokalizačních metod, které jsou na mapě závislé. Tímto způsobem se obě úlohy vzájemně ovlivňují. Obecně lze říci, že chyba odhadnuté trajektorie robotu silně koreluje s chybami vybudované mapy, čili přesná mapa nemůže být vytvořena bez dostatečně přesné, spolehlivé a robustní lokalizace a naopak.

Kapitola 4

Cíle disertace 4.1

Definice řešené úlohy

Cílem této disertační práce je navrhnout vhodnou metodu pro automatickou lokalizaci robotu v neznámém prostředí. Nedílnou součástí lokalizace je výstavba interního modelu prostředí, který lokalizaci podporuje a zvyšuje tak její dlouhodobou robustnost a přesnost. Motivací pro vývoj robustních metod pro simultánní lokalizaci a mapování je implementace základních navigačních schopností mobilního robotu, jejichž korektní funkčnost je předpokládána při řešení řady nadřazených robotických úloh. Model prostředí by měl být rozumně použitelný nejen pro strojové zpracování během navigace robotu, ale též pro člověka, který s robotem v mnoha situacích spolupracuje. Vzhledem k tomu, že výstavba modelu prostředí a lokalizace jsou ve svém principu duální úlohy, bude nutno problematiku řešit dohromady a vytvořit tak komplexní navigační platformu, která robotu umožní autonomně se pohybovat v pracovním prostředí. Cíl disertační práce lze rozdělit do následujících dílčích kroků: 1. Výchozím bodem práce je souhrn stávajících metod pro zpracování senzorických dat a postupů pro lokalizaci a mapování ve světě s rozborem jejich vlastností a oblastí použitelnosti. 2. Zdokonalení stávající ICP metody pro kontinuální lokalizaci robotu v neznámém prostředí. Nově navržená metoda se zaměřuje na zlepšení kritických kroků ICP metody a zejména na provázání lokalizace s průběžně budovaným modelem světa. Hlavním sledovaným cílem je výsledná přesnost nové metody, robustnost a rychlost výpočtu. 3. Nalezení vhodného modelu prostředí, jeho reprezentace a návrh metody na jeho budování pro účely podpory lokalizace robotu. 4. Rozšíření navržené metody lokalizace pro řešení úlohy kooperativní lokalizace skupiny robotů za účelem průzkumu neznámého prostředí ze společného výchozího bodu a rozšíření lokalizační metody do trojrozměrné varianty. Rozšíření tvorby modelu prostředí do 3D pro účely vizualizace a navigace robotu. 15

16

KAPITOLA 4. CÍLE DISERTACE 5. Implementace a integrace navržených metod lokalizace a mapování v rovině do systému experimentálního mobilního robotu ve formě programového modulu. 6. Experimentální ověření navržených postupů a metod včetně srovnání s jinými dostupnými řešeními.

Jádro práce a princip lokalizační metody se bude opírat o využití rovinného laserového scanneru doplněného informacemi o předpokládaném pohybu robotu z odometrických senzorů. Důležitým cílem práce je, aby na algoritmy nebylo kladeno explicitní omezení na tvar pracovního prostředí a předmětů nacházejících se v něm.

4.2

Omezení řešené úlohy

Řešení úlohy simultánní lokalizace a budování podpůrného modelu prostředí pro lokalizaci mobilního robotu je velmi komplexní problém. Z tohoto důvodu je vhodné přijmout určitá omezení na konfiguraci pracovního prostředí robotu, na technické prostředky použité pro řešení úlohy a zejména na účel vytvořeného modelu prostředí. Budování modelu světa je řešeno zejména s ohledem podpory lokalizace, nejsou kladeny další požadavky na přímé využití vytvořeného modelu pro jiné úlohy, např. pro plánování trajektorií robotu. Předpokládá se pohyb robotu zejména v prostředích interiérového charakteru, a tudíž vlastnosti algoritmů by měly být přizpůsobeny pro lokalizaci a vytváření modelů ve vnitřních prostorách budov, jako jsou kanceláře, domácnosti nebo provozní haly. V omezené míře by však algoritmy měly být použitelné (s horšími vlastnostmi) i pro jiná prostředí. Řada omezení na řešenou úlohu vychází z fyzikálně-technických principů senzorů, jímž je vybaven použitý experimentální mobilní robot. Uveďme tedy základní předpoklady kladené na prostředí trochu detailněji: • Mobilní robot se pohybuje v jedné horizontální rovině, a nepřekonává větší terénní nerovnosti. Jsme tedy omezeni na mapování jednoho podlaží, přičemž musí být zajištěn hladký přístup do jednotlivých místností prostoru (tj. vylučujeme existenci schodů, vysokých prahů, zavřených dveří, skloněných podlahových rovin, atd.). • Lokalizační mapa prostředí odpovídá jednomu horizontálnímu řezu prostředí v definované výšce nad podlahou vyplývající z montáže laserového scanneru na základnu robota. Mapa tudíž nemůže zachytit objekty mimo tuto rovinu. • Geometrický tvar objektů a volný prostor v prostředí, který má být reprezentován vytvořenou mapou musí být dostatečně velký vzhledem k fyzickým rozměrům mobilního robotu a rozlišovací schopnosti senzoru. • V prostředí se vyskytují pouze objekty a hrany s nezrcadlivou odrazivostí, předměty v prostředí tudíž musí poskytovat difúzní odraz světla tak, aby byly spolehlivě detekovatelné použitými senzory. • Prostředí je metastatické, během činnosti metod se nemění prostorová konfigurace prostředí a většina objektů v prostředí je nepohyblivá.

Kapitola 5

Senzorické systémy 5.1

Odometrie

Jako základní senzorický systém pro určování polohy je považována odometrie, která umožňuje měřit relativní změnu pozice mezi dvěma časovými okamžiky. Ve své podstatě odometrický polohovací systém převádí měření aktuální polohy a orientace mobilního robotu na měření počtu pulsů generované otáčením kol robotu. Pulsy mohou být generovány přímo z akčních zásahů na motory, nebo z optických, magnetických, či jiných obdobných čidel snímající přímo otáčení kol. První řešení je zpravidla konstrukčně jednodušší, ovšem je zatíženo přídavnou nesystematickou měřicí chybou v případě, že pohyb těla robotu je blokován a pohonná kola se protáčejí na místě. Vlastní poloha robotu je určována na základě znalosti kinematiky daného mobilního robotu, velikosti kola a počtu impulsů na jednu otáčku. Absolutní poloha se obecně získá integrací jednotlivých změn pozice: Z

t1

x1 = x0 + t0

kde

dx dt, dt

dx dt

je změna pozice (a tedy rychlost) a x0 a x1 jsou pozice robotu v čase t0 resp. t1 . Vzhledem k diskrétní povaze výstupů inkrementálních senzorů ve skutečnosti však nepracujeme se spojitou polohou robotu, ale s její diskretizovanou podobou, která je definována v časech tk . Experimentální mobilní roboty jsou zpravidla non-holonického typu, schopné se však otáčet na místě, kde pohon zajišťují dvě nezávisle poháněná kola se společnou osou otáčení. Pro tyto mobilní roboty lze za předpokladu malého rozdílu časů tk a tk−1 polohu robotu přibližně vyjádřit následující soustavou rovnic: ϕ(tk ) = ϕ(tk−1 ) + atan2(4sr − 4sl , l) 4sr + 4sl x(tk ) = x(tk−1 ) + cos(ϕ(tk ) − ϕ(tk−1 )) 2 4sr + 4sl y(tk ) = y(tk−1 ) + sin(ϕ(tk ) − ϕ(tk−1 )), 2 17

18

KAPITOLA 5. SENZORICKÉ SYSTÉMY

kde l je rozchod kol, [xtk , ytk , ϕtk ] a [xtk−1 , ytk−1 , ϕtk−1 ] jsou souřadnice robotu v kartézské soustavě souřadnic a jeho natočení v čase tk a tk−1 . 4sr a 4sl jsou vzdálenosti ujeté pravým resp. levým kolem v časovém intervalu < tk−1 ; tk >. Vzdálenosti 4sr a 4sl je možné vyjádřit jako: 4sr = 4sl =

2πr (nr (tk ) − nr (tk−1 )) P 2πr (nl (tk ) − nl (tk−1 )), P

kde nr a nl jsou počty impulsů v příslušných časech, r označuje poloměr kola robotu a P je počet impulsů na jednu otáčku kola. Se zvyšujícím se rozlišením inkrementálních senzorů měřicí ujetou vzdálenost se může zkracovat délka časového intervalu < tk−1 ; tk > a tím roste i přesnost reprezentace polohy. Délku časového intervalu < tk−1 ; tk > je žádoucí volit vždy co nejmenší s ohledem na rozlišení inkrementálních senzorů, rozchod kol robotu, jeho maximální rychlost a maximální úhlovou rychlost jeho otáčení.

5.2

Inerciální senzory

Mezi inerciální senzory patří gyroskopy a akcelerometry. Oba druhy senzorů slouží k dynamickému měření pohybu, gyroskopy měří úhlovou rychlost rotace, popř. přímo úhel natočení, akcelerometry měří zrychlení. Akcelerometry pracují na dvou základních principech (Lawrence, 1998), jedním je kyvadlový, druhým vibrační. Kyvadlový princip je založen na zpětovazební stabilizaci polohy hmotného tělíska v prostoru. Tělísko, obvykle z feromagnetického materiálu, je volně zavěšeno s možností pohybu ve směru měřené složky zrychlení. Působením zrychlení je tělísko vychylováno z rovnovážné polohy, přičemž změna polohy feromagnetického tělíska způsobí změnu poměru magnetických toků ve dvojici magnetických obvodů diferenciálního transformátoru. Primární vinutí, které je součástí obou magnetických vinutí je buzeno harmonickým signálem, rozdíl indukovaných signálů ve dvojici sekundárních magnetických obvodů je převáděn na měřenou odchylku polohy. Zpětovazební obvod změřenou odchylku polohy převádí na kompenzační sílu působící proti směru pohybu hmotného tělíska a tím jej stále udržuje v poloze odpovídající přibližně nulové odchylce. Vibrační princip měření zrychlení je založen na změně rezonanční frekvence struny nebo proužku kmitající hmoty pokud na ně působí různé podélně napínací síly. Zrychlení je na sílu převáděno spřaženým hmotným tělískem. Klasické mechanické gyroskopy s těžkou rotující hmotou, u nichž se úhlové natočení měřené soustavy odvozuje přímo od úhlového rozdílu mezi závěsem a rotujícím diskem se v robotice používají jen výjimečně. Nejběžnější jsou senzory založené na vibračních principech, kdy se měření rotace převádí na měření polohy kmitající struny, na níž působí

5.3. SONARY

19

Coriolisova síla. V reálných konstrukcích senzorů v mikromechanických křemíkových strukturách se kmitající struna nahrazuje kmitajícím párem laděných vidličkových elementů, přičemž se opět měří výchylky v kolmém směru na rovinu kmitů způsobené Coriolisovou sílou. Nejpřesnější gyroskopy pracují na optickém principu, kdy se využívá inerciálních vlastností světla, konkrétně konstantní rychlosti šíření bez ohledu na relativní rychlost zdroje světla. Úhlová rychlost je u tohoto typu gyroskopů odvozena od Sagnacova interferenčního jevu, který je vyvolán vysíláním světelného paprsku do dvou směrů stočeného optického vlákna. Gyroskopy a akcelerometry se často sdružují do tzv. inerciálních měřících jednotek, ve kterých jsou montovány vždy trojice gyroskopů a trojice akcelerometrů do tří navzájem kolmých os tak, aby umožňovaly měřit otáčení ve vztahu ke všem prostorovým osám a měřit kompletní 3D vektor zrychlení. Zástupcem takového sdruženého senzoru jsou např. produkty firmy Crossbow (6DOF Inertial Measurement Unit - IMU300CC), (CrossBow, 2007). Inerciální senzory se v mobilní robotice používají zejména jako podpůrné senzory pro jiné systémy. Jejich výhodou je relativně rychlá odezva na pohyb vztažného objektu, na druhou stranu, výraznou nevýhodou jsou významné teplotní a časové drifty. Drifty jsou nejvýznamnější zejména u akcelerometrů, neboť pokud požadujeme získání rychlosti objektu, popř. změny polohy, zpracování dat má charakter jednoduché či dvojité integrace. Práce (Mázl a Přeučil, 2003a), (Přeučil a Mázl, 2006) ukázaly, že pokud se neprovádí kontinuální korekce měření, tak v těchto případech chyba měření s časem roste nade všechny meze díky kumulativní propagaci chyby měření. Z tohoto důvodu je pro účely určování pozice v prostoru nutné inerciální senzory vždy svázat s dalšími měřicími principy.

5.3

Sonary

Sonar je relativně jednoduchý senzor poskytující informaci o vzdálenosti překážek v prostoru na základě doby letu zvukové vlny od senzoru k překážce a zpět. Z měřícího principu založeného na šíření zvuku v prostředí vycházejí i hlavní oblasti aplikačního použití a omezení. Rychlost šíření zvuku ve vzduchu je významně závislá na teplotě, proto musí být tento parametr zafixován, nebo příslušným způsobem korigován teplotními senzory. Na šíření zvuku mají vliv i vzdušná proudění, z tohoto důvodu je ve venkovních prostředích nutno

Obr. 5.1: Vyzařovací charakteristika sonaru Polaroid 6500 na frekvenci 50 KHz

20

KAPITOLA 5. SENZORICKÉ SYSTÉMY

počítat se sníženou přesností. V aplikacích, které pracují s rychle pohybujícími se objekty je třeba počítat s Dopplerovým efektem a dalšími degradacemi zvukového signálu vlivem turbulencí apod. Kvalitu měření výrazně ovlivňuje i materiál překážek, přičemž jejich vlivem mohou nastávat dva druhy chyb související se silným pohlcením signálu nebo naopak silným nedifúzním odrazem signálu. První typ chyb, pohlcení signálu, má za následek, že překážka není senzorem detekována vůbec nebo je detekována přilehlá překážka, která se ještě nachází ve vyzařovacím úhlu sonaru. Druhý typ chyby, způsobený zrcadlovým odrazem zvukové vlny má za následek změření mnohem větších vzdáleností, než je reálné. Zvuková vlna se totiž v tomto případě vrátí k senzoru až po několikanásobném odrazu. V poslední době se sonary často nahrazují laserovými dálkoměry, nicméně nízká cena a nezávislost na optických vlastnostech prostředí pro některé aplikace stále předurčuje právě sonary. Hlavní využití sonaru je zejména v systémech vyvarování se kolizím, kdy se využívá relativně širokého vyzařovacího laloku, který je schopen zachytit blízké překážky, jimž se robot musí během svého pohybu bezpečně vyhnout.

5.4

Proximitní senzory

Proximitní senzory nalézají své uplatnění v systémech předcházení kolizí, popř. v jednoduchých úlohách navigace. Jednodušší proximitní senzory pracují na principu detekce odraženého modulovaného světla infračervené diody od překážky, poskytují binární informaci o tom, zda v určitém dosahu je a nebo není překážka. Problémem tohoto měřícího principu je závislost výsledku měření na materiálu povrchu, resp. jeho reflexivitě. Dosah takovýchto čidel jsou přibližně desítky centimetrů.

Obr. 5.2: Proximitní senzor Sharp GP2Y0A02YK

Obr. 5.3: Závislost napětí na vzdálenosti u senzoru Sharp GP2Y0A02YK

Funkce dokonalejších proximitních senzorů (např. obr. 5.2) je založena na triangulačním principu. Tyto senzory mají dosah kolem 1m. Používají řádkový poziční snímač s jednoduchou optikou a pod úhlem směrovaný infračervený paprsek. Vzdálenost detekovaného předmětu, resp. jeho existence v dosahu senzoru ovlivňuje polohu dopadu odraženého paprsku, která určuje výstupní napětí senzoru. Tento měřicí princip umožňuje triangulační

5.5. LASEROVÉ DÁLKOMĚRY, SCANNERY

21

proximitní senzor použít i jako jednoduchý senzor pro měření vzdálenosti překážky, závislost výstupního napětí na vzdálenosti detekovaného předmětu je znázorněno na obr. 5.3.

5.5

Laserové dálkoměry, scannery

Laserové dálkoměry jsou spolu s kamerovými systémy v současné době klíčovými exteroceptivními senzorickými systémy v oblasti mobilní robotiky, neboť umožňují robotu vytváření modelu okolního prostředí a jeho interakci s ním. Podrobný přehled současných technologií pro strojové vnímání okolního světa pro mobilní robotiku lze nalézt v (Hebert, 2000). Laserové dálkoměry umožňují velmi přesně měřit vzdálenost mezi senzorem a překážkami v prostoru. Pracují na principu měření doby letu optického paprsku, popř. na principu měření fázového posuvu mezi přijímaným a vysílaným signálem (viz. obr. 5.4). Levnější konstrukce pracují na triangulačních principech podobně jako proximitní senzory, viz kap. 5.4. Laserový paprsek může být buď statický, nebo rozmítaný rotujícími zrcadly (viz obr. 5.5). Pro účely mobilní robotiky se nejčastěji používá druhý typ, kdy je laserový paprsek rozmítán do jedné, obvykle horizontální, poloroviny. Jeden laserový scan pak popisuje 2D horizontální řez prostředím, který je přirozeně reprezentovaný jako seznam bodů v polárních souřadnicích se středem odpovídající poloze senzoru. Vedle rovinně rozmítaných laserových scannerů existují též scannery rozmítané ve dvou osách. Ty se však používají spíše pro snímání tvarů předmětů, budov apod., pro účely mobilní robotiky jsou příliš drahé a v praxi se nahrazují spíše naklápěným dvojrozměrným scannerem, pokud je žádoucí získávat data z prostoru.

Obr. 5.4: Princip měření laserovým dálko- Obr. 5.5: Vnitřní struktura rovinného laseměrem rového scanneru

5.6

Kamery

Použití kamer v mobilní robotice je orientováno zejména na nepřímé měření vzdáleností povrchů překážek v prostoru. Měřící principy lze rozdělit na aktivní triangulaci a pasivní

22

KAPITOLA 5. SENZORICKÉ SYSTÉMY

stereo (Hebert, 2000). Další přístupy rekonstrukce tvaru prostředí či předmětů využívající kamery se používají spíše v oblasti počítačového vidění než v mobilní robotice, např. Shape from shading (Zhang et al., 1999), prostorové rekonstrukce předmětů snímané z více míst apod. Aktivní triangulace je založena na promítání laserového bodu (obr. 5.6) či proužku (obr. 5.7) na scénu (ležící v x − y rovině), která je snímána kamerou. Poloha obrazu osvětleného bodu či promítnutého proužku na čipu kamery pak určuje vzdálenost z0 mezi kamerou a osvětleným bodem či body ve scéně. Vzdálenost z0 = B/(x0 /f +tan α), kde B je základní linie vzdálenosti laseru a optického středu kamery, f je ohnisková vzdálenost optiky kamery, x0 je poloha měřeného bodu v obrazovém řádku snímacího čipu kamery a α je projekční úhel laserového paprsku vůči ose z. Za účelem jednoduché detekce laserového paprsku je scéna nasvětlována tak, aby laserový proužek byl jejím nejasnějším prvkem. Sejmutí hloubkové informace ze scény v matrici 200x200 bodů a hloubkovém rozsahu 0.6 - 2.5m pomocí 3D scanneru Minolta Vivid VI-700 (obr. 5.8) trvá 0.6 sec (Tišnovský, 2003). Pokud je předmět v minimální možné vzdálenosti 60 cm, pak je rozlišení v x − y rovině 0.35 mm, a hloubkové rozlišení podél osy z 0.11 mm. Scanovací zařízení jsou často doplňována otočným stolkem s možností synchronní rotace předmětu a souběžného laserového měření pro získání plného 3D obrazu předmětu. Na sejmutá prostorová data může být namapována barevná textura.

Obr. 5.6: 1D aktivní triangulace, převzato Obr. 5.7: 2D aktivní triangulace, převzato ze zdroje: (Tišnovský, 2003) ze zdroje: (Tišnovský, 2003) Pasivní stereo patří do kategorie triangulačních technik měření vzdáleností, pracuje s podobnými geometrickými vlastnostmi jako aktivní triangulace. Rozdílné je však získávání poloh bodů, z nichž se triangulace vypočítá. Metoda je založena na podobném principu na jakém pracuje lidský zrakový systém při odhadu vzdálenosti. Místo očí je však použita dvojice kamer. Obrazy z obou kamer představují perspektivní obrazy, ve kterých se na epipolárách kamerové soustavy hledají významné, navzájem si korespondující body. Tyto body, resp. jejich odlišná poloha v obrazech kamer, jsou dále použity pro výpočet vzdálenosti na bázi triangulace. Výsledkem

5.7. DALŠÍ SENZORICKÉ SYSTÉMY

23

Obr. 5.9: 3D time-of-flight kamera Obr. 5.8: 3D scanner Minolta Vivid VI-700 SwissRanger CSEM SR 3000 výpočtu je pak disparitní obraz scény. Pro zvýšení přesnosti či robustnosti se někdy používá místo stereoskopického páru kamer snímačů více vzájemně kalibrovaných snímačů. Přestože pasivní stereovidění je jedním z klíčových výzkumných témat v oblasti počítačového vidění, své využití v mobilní robotice nalézá jen pozvolna zejména kvůli své poměrně značné výpočetní náročnosti a požadavkům mobilní robotiky zpracovávat obrazy v reálném čase s relativně vysokými snímkovými frekvencemi. Literatura zabývající se stereo viděním je velmi rozsáhlá, jako příklad lze uvést např. (Šonka a Hlaváč, 1992), (Haralick a Shapiro., 1992), (Šonka et al., 1998).

5.7

Další senzorické systémy

V mobilní robotice nachází uplatnění řada dalších speciálních senzorů a senzorických systémů. Mezi ně patří i satelitní lokalizační systémy. Prvním satelitním lokalizačním systém je NAVSTAR GPS, což je zkratka pro Navigation Signal Timing and Ranging Global Positioning System. Tento systém je vyvíjený a budovaný od roku 1973 Ministerstvem obrany Spojených států, Rusko vyvíjí od 80.let min. století vlastní systém GLONASS, v současné době vzniká nový evropský systém Galileo, který má v budoucnu přinášet nové kvalitativní vlastnosti. Satelitní lokalizační systémy se skládají z řady družic obíhající kolem zeměkoule po přesně definovaných trajektoriích, pozemních řídicích středisek a vlastních GPS přijímačů. Každá družice (viz obr. 5.10) nese vedle navigačních přístrojů, přijímačů, vysílačů, také velmi přesné (10−13 ) palubní atomové hodiny s cesiovým a rubidiovým oscilátorem. Pozemní střediska sledují aktuální trajektorie družic a v případě potřeby, kdy se družice vychýlí mimo toleranci z požadované trajektorie, iniciují korekční manévry družice. Určení pozice přijímače je založena na kontinuálním příjmu časových informací, které vysílá každá jednotlivá družice spolu s informací o své vlastní trajektorii, tzv. almanachem. Vlastní přijímač pak vypočítává svojí pozici na základě časových rozdílů přijímaných sig-

24

KAPITOLA 5. SENZORICKÉ SYSTÉMY

nálů z družic. V případě systému GPS kolem Země obíhá ve výšce 20200 km celkem 24 družic, na 6 oběžných drahách (viz obr. 5.11) skloněných vždy o 60 stupňů, přičemž průměrně je v našich zeměpisných šířkách nad obzorem viditelných 8 družic. Pro dvojrozměrné určení polohy však postačí dostupnost signálu ze tří družic, pro trojrozměrné určení polohy pak ze čtyř. Základní přesnost určení polohy je v řádu jednotek metru, závisí zejména na podmínkách příjmu (síla a dostupnost signálu z co největšího množství družic, odrazy od budov, atmosférické vlivy, šíření signálu v ionosféře atd.). Přesnost určení polohy lze zvyšovat příjmem a zpracováním diferenčních korekčních signálů a to buď pozemních, nebo družicových. V Evropě lze ke korekci použít signál geostacionární družice systému EGNOS, v USA pracuje podobný systém WAAS. Tímto způsobem lze přesnost zvýšit řádově na desítky cm, dalšího zvýšení přesnosti až na jednotky cm se dosahuje zpracováním pozemních korekčních signálů systémem RTK (Real Time Kinematics). Pro účely mobilní robotiky je však velmi limitujícím faktorem nedostupnost signálu uvnitř budov. Tato situace by se mohla zlepšit s nástupem nového systému Galileo. Satelitní navigační systém GPS je často kombinován s určováním polohy pomocí inerciálních senzorů (Mázl a Přeučil, 2003b), neboť oba navzájem zcela odlišné principy vzájemně kompenzují své nedostatky. Drift inerciálních senzorů eliminuje systém GPS absolutním měřením pozice, naopak krátkodobá nedostupnost satelitního signálu může být překlenuta průběžně kalibrovanými měřeními z inerciálních senzorů.

Obr. 5.10: Satelit systému NAVSTAR GPS, převzato ze zdroje NASA

Obr. 5.11: Dráhy družic systému GPS

Poměrně novým a pravděpodobně velmi perspektivním senzorem pro mobilní robotiku jsou 3D (hloubkové) kamery. Tyto senzory jsou v principu kamerou a dálkoměrem zároveň, poskytují hloubkový obraz scény. Pracují na principu modulovaného osvětlování scény (20 MHz) difúzním infračerveným světlem a měřením doby dopadu světla odraženého od překážek na matrici snímacího čipu, resp. jeho fázového posunu. Výsledkem je pak přiřazení naměřené vzdálenosti ke každému pixelu této kamery. Jako příklad 3D kamery můžeme uvést SwissRanger CSEM SR 3000 (obr. 5.9) mající měřící dosah 7.5 m a rozlišení 176x144 bodů se zorným úhlem 47.5◦ x 39.6◦ a poskytující až 29 scanů za sekundu s rozlišením 2.5 mm (v režimu pro vzdálenosti do 0.3 m).

Kapitola 6

Stav problematiky ve světě 6.1

Lokalizace polohy

Během posledních zhruba patnácti let bylo vyvinuto poměrně velké množství nejrůznějších technik pro určování polohy mobilních robotů. Jednotlivé metody lze podle základního teoretického přístupu rozdělit na metody pracují s neurčitostí v datech i modelech a na metody pracující bez neurčitosti. Tyto metody pak poskytují jen výsledný odhad polohy bez odhadu možných chyb. Oba přístupy mají své významné výhody i nevýhody, které jsou zpravidla vzájemně komplementární. Metody pracující s neurčitostí (Thrun, 2000) jsou obecně více robustní, lépe se dokáží vyrovnat se šumy měření, ovšem jsou často výpočetně náročnější než metody pracující bez ní. Zavedení neurčitosti s sebou přináší nutnost práce s pravděpodobnostním popisem modelu světa i senzorických dat a tím souvisejícím výpočtem složitých posteriorních hustot pravděpodobností, které jsou v idealizovaném teoretickém pojetí spojité a mnohadimenzionální. V praktickém řešení je nutnost přistoupit z důvodů výpočetní složitosti vždy na určitou míru aproximace reálného světa a výpočetních postupů. Pravděpodobnostní algoritmy se mohou zdát méně efektivní, je to ovšem přirozená cena za schopnost práce s nepřesnými modely, nepřesnými daty a možnosti samozotavení se z případných hrubých chyb měření či modelu. Na druhé straně nepravděpodobnostní přístupy mohou dosahovat vyšších přesností, ovšem zpravidla jen za předpokladu splnění přísnějších omezujících podmínek. V případě, že specifikace omezujících podmínek pro danou metodu je porušena, metoda zpravidla zcela selže. Definice omezujících podmínek však umožňuje realizaci nejrůznějších optimalizací pro specifické případy, což významně zvyšuje rychlost algoritmů (Gutmann a kol., 2001). Vedle výše uvedeného základního dělení lokalizačních metod je možné provést jiné dělení do tří základních kategorií podle způsobu přístupu ke zpracování senzorických dat a použití podpůrných modelů světa: • behavioární metody • landmarkové metody • metody využívající přímé registrace senzorických dat. 25

26

KAPITOLA 6. STAV PROBLEMATIKY VE SVĚTĚ

Nejjednoduší behavioární metody jsou založené na specifickém pohybu v prostředí a na interakci s ním. Pro svoji navigaci používají např. pravidlo pravé ruky, traverzují prostředím podél jedné stěny a zpět se vrací po stejné trajektorii (Connel, 1990). Dokonalejší systémy (Atkin, 1990) se učí interní strukturu, kterou následně využívají ke svému pohybu. Behavioární metody mohou být použitelné pro jednoduché typy úloh (Electrolux, 2007), (iRobot, 2007), ovšem jejich schopnosti pro přesnou geometrickou lokalizaci jsou velmi omezené. Lokalizace založené na landmarcích spoléhají na rozpoznání významných objektů v prostoru a následné geometrické určení pozice odkud byly pozorovány. Landmarky mohou být přirozenou součástí prostředí (rohy místností, stěny chodeb, apod.) nebo mohou být do prostředí uměle dodány (E&K Automation, 2007). Do této kategorie lze zařadit i systémy satelitní navigace jako GPS. Poloha landmarků může být pro dané prostředí známá předem, nebo automaticky určena během budování mapy prostředí. Výzkum metod pracujících na principu registrace senzorických dat vytváří v současnosti významný výzkumný směr. Tyto metody využívají nejčastěji sonarových a laserových senzorů, které poskytují velké množství informací o tvaru a konfiguraci okolního prostředí. Zpravidla nevyužívají metody extrakce příznaků z datových souborů, ovšem velmi často pracují s nejrůznějšími typy modelů prostředí, vůči kterým naměřená data registrují. Velkou výhodou těchto metod je, že neselhávají v prostředích, ve kterých je obtížné extrahovat vhodné landmarky. Typickými zástupci těchto metod jsou Markovské lokalizace (Thrun a kol., 2001) a scan-matching techniky (Besl a McKay, 1992), (Lu a Milios, 1994), (Lu a Milios, 1997). Z výzkumu vlastností těchto technik (Gutmann et al., 1998) vyplývá, že se výborně vzájemně doplňují. Markovské lokalizace jsou více robustní i v situacích, kdy není dostatek senzorických dat nebo jsou nespolehlivé, ovšem absolutní přesnost těchto metod obvykle nedosahuje takové kvality, jako u scan-matching technik. V následujících kapitolách jsou podrobněji diskutovány používané lokalizační techniky v oblasti mobilní robotiky.

6.1.1

Postupy využívající Kalmanova filtru

Mezi velmi populární pravděpodobnostní přístupy pro obecné sledování pozice jsou již řadu let používány Kalmanovy filtry. Základní princip byl představen Rudolphem Kalmanem v roce 1960 (Kalman, 1960). Lokalizační metody založené na Kalmanově fitru (Smith a kol., 1990), (Cox, 1991), (Durrant-Whyte a Leonard, 1991) reprezentují odhad pozice robotu pomocí unimodálního gaussovského rozložení. Reprezentace pozice robotu je tedy popsána střední hodnotou této distribuce a jejím rozptylem, což umožňuje pracovat s příslušnou neurčitostí pozice robotu. Proces lokalizace se opírá o aktualizace gaussovského rozložení následujícím způsobem. Jakmile se robot přemístí, gaussovské rozložení je posunuto podle předpokládané velikosti a směru pohybu, která je zpravidla měřena odometrickými senzory, nebo je určena přímo z akčních zásahů do motorů robotu. Současně s tím je upraven rozptyl rozložení podle modelu odometrického subsystému, resp. podle jeho předpokládaných chyb. Následně se do odhadu pozice, tedy do aktualizace gaussovského rozložení, zahrnou informace ze senzorů, které přinášejí informaci o reálné poloze ve vztahu k modelu světa. Využití senzorické

6.1. LOKALIZACE POLOHY

27

informace dříve zvýšenou neurčitost (rozptyl) gaussovského rozložení opět sníží a cyklus se může zopakovat. Formálně lze činnost Kalmanova filtru, tedy aktualizaci parametrů distribuce hustoty rozložení pravděpodobnosti výskytu robotu v příslušné poloze a čase t, popsat následujícími vztahy: 0

µt−1 = µt−1 + But

(6.1)

0

Σt−1 = Σt−1 + Σcontrol 0

T

0

(6.2) T

−1

Kt = Σt−1 C (CΣt−1 C + Σmeasure ) 0

0

µt = µt−1 + Kt (z − Cµt−1 ) 0

Σt = (I − Kt C)Σt−1 ,

(6.3) (6.4) (6.5)

kde µt a Σt jsou momenty distribuce pravděpodobnosti, B je převodní matice řízení, C je převodní matice měření a z je vektor naměřených dat. Předpokládaná přesnost pohybového modelu 6.2 robotu je parametrizována maticí Σcontrol , zatímco důvěra v senzorická data je popisována kovarianční maticí Σmeasure . Kalmanovo zesílení Kt v principu udává jakou měrou bude budoucí odhad stavu (poloha robotu) ovlivněn nově změřenými daty, jeho velikost závisí na odhadované přesnosti minulého odhadu stavu Σt−1 a přesnosti měření Σmeasure . Je nutno uvést, že výše uvedené vztahy platí pouze pro lineární systémy, pro praktické realizace se musí využívat rozšířeného Kalmanova filtru, který celý problém linearizuje vůči aktuální poloze robotu. Existující aplikace Kalmanova fitru se příliš neliší v tom, jak je modelován pohyb robotu. Více rozdílů lze najít ve způsobu aktualizace gaussovského rozložení v reakci na senzorická data. Autoři v (Durrant-Whyte a Leonard, 1991) registrují geometrická primitiva (roviny, válce a rohy) extrahované ze sonarových dat vůči primitivům predikovaným z geometrické mapy prostředí. Pro aktualizaci odhadu polohy autor (Cox, 1991) registruje vzdálenosti změřené infračervenými senzory vůči liniovému popisu prostředí. Pro podporu lokalizace jsou ze sonarových dat často vytvářeny lokální a globální mřížky obsazenosti. Porovnání různých strategií pro sledování pozice na bázi mřížek obsazenosti lze nalézt v (Schiele a Crowley, 1994). Autoři zde porovnávají výkonnost lokalizace v případech, kdy se registrují předzpracované lokální mapy s globální mapou a kdy se pro určení pozice srovnávají přímo naměřená data s extrahovanými primitivy z obou map. Výhoda metod založených na Kalmanově filtraci spočívá v jejich efektivitě a velmi vysoké přesnosti za předpokladu, že je známa výchozí pozice. Bohužel, významnou principiální nevýhodou Kalmanových filtrů jsou omezení kladená na distribuce hustot pravděpodobností. Teoretické základy metody předpokládají, že všechny neurčitosti je možné modelovat procesy s normálním rozložením. Tyto podmínky je v praxi často obtížné splnit, navíc díky reprezentaci polohy robotu jedním gaussovským rozložením nejsou metody tohoto typu schopny řešit úlohy globální lokalizace a nejsou schopny se zotavit z větších pozičních chyb.

28

KAPITOLA 6. STAV PROBLEMATIKY VE SVĚTĚ

Jistou výjimku tvoří systémy pro vícehypotézové sledování pozice založené na vícemodálních reprezentacích posteriorních pravděpodobnostních rozložení sestávající ze směsi několika Gaussiánů (Jensfelt a Kristensen, 1999), (Cox a Leonard, 1994). Toto rozšíření do jisté míry umožňuje robotu schopnost globální lokalizace, ovšem zásadní nevýhoda, podmínka normálních rozložení v neurčitostech zpracovávaných veličin stále zůstává. Lze najít i další řešení na bázi rozšířeného Kalmanova filtru, který během lokalizace pracuje i s neurčitostí vytvářené mapy (Smith a kol., 1990). V tomto případě se však již nejedná o řešení úlohy čisté lokalizace, nýbrž o řešení úlohy simultánní lokalizace a mapování popisované dále v kap. 6.2.6. V současné době lze pozorovat výrazný trend směřující k reprezentaci distribucí hustot pravděpodobnosti pomocí po částech konstantních funkcí a zejména pomocí vzorkovaných rozložení přes celý prostor všech možných stavů. Tyto funkce mnohem lépe reprezentují příslušný tvar komplexních a multimodálních distribucí. Algoritmy využívající těchto přístupů jsou známy pod pojmem Markovská lokalizace.

6.1.2

Markovská lokalizace

Markovská lokalizace patří do skupiny globálních lokalizačních technik, tzn. provádí odhad pozice robotu ve známém prostředí. Název metody je odvozen od společného základního předpokladu, který je využíván též v teorii částečně pozorovatelných Markovských rozhodovacích procesů (POMDP). Markovský rozhodovací proces pracuje se stavovým popisem světa, který je popsán náhodnými veličinami a akcemi v tomto světě, které jsou měřeny mírou své užitečnosti. Základním předpokladem pro rozhodování v tomto světě je znalost aktuálního stavu, resp. jeho odhad, neboť tento stav není typicky měřitelná veličina. Navíc odhad tohoto stavu v každém kroku závisí jen na modelu světa, minulém stavu a aktuálním pozorování reálného světa. Markovská lokalizace je v principu speciálním případem markovského stavového estimátoru, kde stavem světa je aktuální poloha robotu v prostředí. Markovské lokalizace byly realizovány v několika variantách (Fox a kol., 1999a), (Nourbakhsh et al., 1995), (Cassandra et al., 1996) či (Simmons a Koenig, 1995), kde hlavní motivací bylo překlenutí zásadních nevýhod unimodálních lokalizačních technik založených na Kalmanově filtraci. Odhad polohy robotu je v Markovských lokalizačních metodách popsána jako statistická veličina včetně nejistoty určení této polohy. Místo udržování jedné hypotézy o tom, kde se robot může nacházet, Markovská lokalizace používá teorie pravděpodobnosti k udržování odhadů pravděpodobnostního rozložení výskytu robotu v celém prostoru přes celou množinu všech možných poloh robotu. Problém lokalizace robotu je v podstatě převeden na problém aktualizace tohoto rozložení pravděpodobnosti na základě informací o pohybu robotu, dostupných senzorických datech a existující mapy prostředí. Aktuální (nejvěrohodnější) poloha robotu v prostředí je pak určována pomocí maxima v rozložení hustoty pravděpodobnosti. Základní myšlenka Markovské lokalizace je shrnuta na obr. 6.1. Je zde uvažován zjednodušený případ jednorozměrného světa, ve kterém se pohybuje robot bez výchozí znalosti své aktuální pozice. Markovská lokalizace tento stav reprezentuje uniformě rozdělenou hus-

Fox, Burgard & Thrun 6.1. LOKALIZACE POLOHY

29

Fig. 2. The basic idea of Markov localization: A mobile robot during global localization.

Obr. 6.1: Myšlenka Markovské lokalizace - mobilní robot v úloze globální lokalizace, obwhere in the world a robot might be, Markov localization maintains a probability distribution rázek převzat (Fox ofa all kol., over thez space such1999a) hypotheses. The probabilistic representation allows it to weigh

these dierent hypotheses in a mathematically sound way. Before we delve into mathematical detail, let us illustrate the basic concepts with a totou pravděpodobnosti přes the všechny možné pozice (první2.diagram z obr. 6.1). Robot je simple example. Consider environment depicted in Figure For the sake of simplicity, us assume that thezměnu space své of robot positions is one-dimensional, thatúrovni is, therozhodování, robot can schopen let měřit přibližnou polohy a vnímat prostředí na zda only move horizontally (it may not rotate). Now suppose the robot is placed somewhere in stojí před dveřmi nebo před zdí. this environment, but itsenzorická is not told itsdata location. Markov localization represents this state Jakmile robot zpracuje a zjistí, že se nachází před některými dveřmi, of uncertainty by a uniform distribution over all positions, as shown by the graph in the Markovská základě mapy měření rozložení rstlokalizace diagram in upraví Figure 2.naNow let us známé assume the robota senzorického queries its sensors and nds out pravthat it isPravděpodobnost, next to a door. Markov modi es vtheněkteré belief byz raising the probability děpodobnosti. že localization se robot nachází pozic před dveřmi se zvýší, places nextmístech to doors,prostoru and lowering it anywhere This isobr. illustrated in the second naopak vforostatních se sníží (druhýelse. diagram 6.1). Snížení hodnot husdiagram in Figure 2. Notice that the resulting belief is multi-modal, re ecting the fact that měření toty pravděpodobnosti však není nikde až nulovou hodnotu k šumu the available information is insuÆcient for na global localization. Noticevzhledem also that places not a možnosti, že arobot se possess nachází i jinde než před přestože senzorická měření dánext to door still non-zero probability. Thisdveřmi, is because sensor readings are noisy, and a single sight of a door is typically insuÆcient to exclude the possibility of not being vají jiný výsledek. Výsledná hustota pravděpodobnosti upravená na základě senzorického nextmultimodální, to a door. modelu je což reprezentuje neurčitost znalosti aktuální polohy robotu. Now let us přesune assume thedorobot moves a meter forward.známou Markov localization incorporates Robot se posléze pozice o přibližně je v Markovthis information by shifting jiné the belief distribution accordingly, asvzdálenost, visualized in což the third ské lokalizaci posunem celéinherent distribuce směru a vzdádiagramreprezentováno in Figure 2. To account for the noise inpravděpodobnosti robot motion, which ve inevitably leads to a loss ofpředpokládanému information, the new belief is smoother (and(třetí less certain) thanztheobr. previous lenosti odpovídající pohybu robotu diagram 6.1). Možné one. Finally, let us assume the robot senses a second time, and again it nds itself next to aa větším nepřesnosti a nejistoty v pohybu robotu jsou v distribuci zohledněny zploštěním door. Now this observation is multiplied into the current (non-uniform) belief, which leads rozprostřením distribuce (zvýšení rozptylů vzniklých extrémů rozložení). to the nal belief shown at the last diagramdříve in Figure 2. At thislokálních point in time, most ofvthe Tento krok se nazývá aplikace modelu pohybu robotu. probability is centered around a single location. The robot is now quite certain about its position. V další fázi robot získá nová senzorická měření, která odpovídají tomu, že se robot opět nachází před některými dveřmi. Toto pozorování je zkombinováno se současnou mírou 394 důvěry v určité polohy robotu, což vede k získání nového rozložení pravděpodobnosti, ve kterém je již velmi výrazné maximum odpovídající skutečné poloze robotu. Robot je tak lokalizován. V reálné situaci je proces pochopitelně složitější, zejména je třeba více kroků k jednoznačnému určení polohy robotu. Formálně lze metodu popsat následovně. Pozice robotu je chápána jako vektor x = (posx , posy , ϕ) obsahující souřadnice robotu v kartézských souřadnicích spolu s jeho natočením, xt je pak reálná pozice robotu v čase t. Budeme-li s polohou zacházet jako s náhod-

30

KAPITOLA 6. STAV PROBLEMATIKY VE SVĚTĚ

nou veličinou, označíme ji jako Xt . Markovská lokalizace pracuje s pojmem důvěra v pozici robotu v čase t, která je označovaná jako Bel(Xt ), což je v principu distribuce pravděpodobnosti přes prostor všech pozic robotu. Bel(Xt = x) je pak hustota pravděpodobnosti, přiřazující robotu pravděpodobnost, že jeho pozice v čase t je právě x. Důvěra Bel(Xt ) je aktualizována následkem dvou událostí, příchodem senzorických dat zt nebo informací o pohybu ut (odometrie). Robot pak zpracovává sekvenci měření: d = d0 , d1 , ..., dT ,

(6.6)

kde každé dt (s 0 ≤ t ≤ T ) reprezentuje buď jedno senzorické zt nebo odometrické měření ut . Markovská lokalizace odhaduje posteriorní distribuci pravděpodobnosti náhodné veličiny XT , která je podmíněna příchozími daty: P (XT = x | d) = P (XT = x | d0 , d1 , ..., dT ),

(6.7)

V odvození inkrementálních aktualizací posteriorních pravděpodobností se používá markovský předpoklad, který říká, že jakmile známe aktuální polohu xt robotu v čase t, budoucí senzorická měření nejsou ovlivněna dřívějšími měřeními a naopak. Platí tedy: P (dt+1 , dt+2 ... | Xt = x, d0 , ..., dt ) = P (dt+1 , dt+2 ... | Xt = x) ∀ t.

(6.8)

Během aktualizace P (XT = x | d) se rozlišují případy, podle typu aktuálně příchozích dat: Případ 1: Data jsou senzorická měření dT = zT Základní vztah P (XT = x | d) = P (XT = x | d0 , d1 , . . . , zT ) je možné aplikací Bayesova pravidla upravit do podoby

P (XT = x | d) =

P (zT | d0 , . . . , dT −1 , XT = x) · P (XT = x | d0 , . . . , dT −1 ) , P (zT | d0 , . . . , dT −1 )

(6.9)

který může být s využitím markovského předpokladu a zavedením αT za jmenovatele (na XT nezávislým) P (zT | d0 , . . . , dT −1 ) upraven na: P (XT = x | d) = αT · P (zT | XT = x) · P (XT = x | d0 , ..., dT −1 ),

(6.10)

Vzhledem k tomu, že důvěra v danou pozici robotu je ekvivalentní posteriorní pravděpodobnosti Bel(XT = x) = P (XT = x | d0 , . . . , dT ), (6.11) vztah 6.10 lze za předpokladu P (zT | XT = x) ' P (zT | x) (nezávislost na čase) a minulých datech {d0 , . . . , dT −1 } vyjádřit v rekurzivní podobě: Bel(XT = x) = αT · P (zT | x) · Bel(XT −1 = x),

(6.12)

Zde stojí za zmínku důležitost členu P (zT | x), který představuje senzorický model a v praxi popisuje pravděpodobnost daného senzorického měření (předpokládaného podle modelu prostředí), pokud se robot nachází v určité pozici v prostoru.

6.1. LOKALIZACE POLOHY

31

Případ 2: Data jsou informace z odometrie dT = uT V tomto případě budeme počítat P (XT = x | d) na základě teorému o úplné pravděpodobnosti: P (XT = x | d) =

Z

P (XT = x | d, XT −1 = x0 ) · P (XT −1 = x0 | d) dl0

(6.13)

Pro první součinitel na pravé straně opět použijeme markovský předpoklad, čímž dostaneme: P (XT = x | d, XT −1 = x0 ) = P (XT = x | d0 , . . . , dT −1 , uT , XT −1 = x0 ) 0

= P (XT = x | uT , XT −1 = x )

(6.14) (6.15)

Druhý součinitel na pravé straně může být též zjednodušen na základě toho, že uT nepřináší žádnou novou informaci o minulé poloze XT −1 : P (XT −1 = x0 | d) = P (XT −1 = x0 | d0 , . . . , dT −1 , uT ) 0

= P (XT −1 = x | d0 , . . . , dT −1 )

(6.16) (6.17)

Dosadíme-li do výrazu 6.13 součinitele 6.15 a 6.17 získáme výsledek P (XT = x | d) =

Z

P (XT = x | uT , XT −1 = x0 ) · P (LT −1 = x0 | d0 , . . . , dT −1 ) dl0 , (6.18)

který lze dále upravit za použití definované důvěry 6.11 a předpokladu P (x | uT , x0 ) ' P (XT = x | ut , XT −1 = x0 ) (předpokládáme nezávislost na čase) do inkrementální podoby: Z

Bel(XT = x) =

P (x | uT , x0 ) · Bel(XT −1 = x0 )dx0

(6.19)

Zde je důležité rozložení pravděpodobnosti P (x | uT , x0 ), kterému říkáme pohybový model. Pohybový model popisuje, jak se zvýší neurčitost v pozici robotu po aplikaci pohybu popsaného odometrií uT . Lze tedy shrnout, že pro Markovskou lokalizaci jsou podstatné reprezentace důvěry Bel(X), resp. reprezentace distribuce pravděpodobnosti výskytu robotu v daném místě v prostoru a dále distribuce podmíněných pravděpodobností P (x | u, x0 ) (reprezentují pohybový model robotu) a P (z | x) (reprezentující senzorický model robotu, někdy nazývaný též percepční model). Uvážíme-li, že oba uvedené případy se během jízdy robotu střídají, důvěru v příslušnou pozici x robotu v čase t označíme jako Belt (xt ), pak dostaneme rekurzivní vztah popisující princip Markovské lokalizace: Bel(xt ) = αt P (z | xt )

Z

P (xt | uT , xt−1 ) Bel(Xt−1 = x0 )dxt−1 ,

(6.20)

kde αt je normalizační koeficient zajišťující, aby výsledná suma důvěry přes všechny možné pozice robotu byla rovna jedné. Zde je nutno poznamenat, že pohybový a zejména senzorický model reálně závisí na modelu prostředí m. Po zavedení modelu prostředí

32

KAPITOLA 6. STAV PROBLEMATIKY VE SVĚTĚ

m je nutno výše uvedené hustoty pravděpodobnosti označovat jako P (xt | u, xt−1 , m) a P (z | xt , m). Popisy distribucí pravděpodobností reprezentující stavový prostor robotu jsou vícerozměrné funkce, které je v praxi velmi obtížné analyticky popsat, proto se často pracuje s diskretizovanými odhady a aproximacemi (Fox a kol., 1999a), (Thrun a kol., 2000). Stavový prostor robotu je často udržován v pravděpodobnostní mřížkové reprezentaci, kdy je celý pracovní prostor robotu rozdělen do čtvercové sítě o velikosti buňky v řádu jednotek až desítek centimetrů, přičemž každá buňka nese informaci o příslušné pravděpodobnosti. Při aktualizaci těchto pravděpodobností se využívá určitých heuristik, aby se nemuselo vždy pracovat s úplně všemi buňkami. Pro výpočet výsledných pravděpodobností senzorického modelu se z důvodů vysoké výpočetní náročnosti využívá předpřipravených datových struktur. Pokud robot využívá například laserového scanneru, musí vyčíslit stovky hypotéz pro každý z laserových paprsků a to navíc s ohledem na desetitisíce i více možných pozic v prostoru (v rámci mřížkové reprezentace). V případě statického modelu světa se vytvářejí dvojúrovňové vnořené vyhledávací tabulky. Využívá se toho, že senzorický model P (z | x, m) v případě proximitních senzorů závisí pouze na P (z | o, m), kde o je vzdálenost nejbližší překážky ve směru paprsku senzoru. První hledání poskytne informaci o nejbližší překážce v daném směru od příslušné pozice robotu, druhé hledání poskytne přímo P (z | o, m). Pokud robot pracuje v dynamickém prostředí, kde není zaručena dostatečně přesná shodnost modelu se senzorickým pozorováním, je nutné do senzorického modelu zapracovat různé vzdálenostní či entropické filtry. Klíčovými myšlenkami metod odvozených od Markovské lokalizace jsou způsoby výpočtu aproximací distribucí pravděpodobností popisující stavový prostor robotu, jak je popsáno dále v kap. 6.1.3 zabývající se Monte-Carlo lokalizací. Zásadní nevýhodou Markovských metod je vysoká paměťová a výpočetní náročnost pro rozsáhlé prostory, pro které se musí výrazně zvýšit granularita pracovní mřížky. Vedle mřížkových reprezentací existují ovšem i úspornější reprezentace, vycházející např. i z kombinací více Gaussiánů (Gutmann a kol., 1998), které mají ovšem zase jiné nevýhody popsané v kap. 6.1.1.

6.1.3

Monte-Carlo lokalizace

Lokalizace založená na metodě Monte-Carlo (Fox a kol., 1999b) ve svém principu pracuje na shodných teoretických základech jako Markovské lokalizace. Základním rozdílem mezi Markovskou a Monte-Carlo lokalizací (dále jen MCL) je reprezentace distribucí pravděpodobností. Vzorkované reprezenace distribucí pravděpodobnostní jsou poměrně často používaný princip v mnoha různých oborech. V robotické a statistické literatuře jsou Monte-Carlo přístupy známy pod pojmem particle filters (Pitt a Shephard, 1999), (Docet et al., 2001), v oblasti počítačového vidění se vyskytuje pojmem kondenzační algoritmus (Isard a Blake, 1998), v ostatních oborech lze nalézt též názvy bootstrap filters, interacting particle approximations nebo survival of the fittest. Výhody reprezentace problému aproximace odhadu posteriorních pravděpodobností ve formě particle filtrů lze spatřovat v následujících bodech:

6.1. LOKALIZACE POLOHY

33

• Particle filtry jsou univerzálním nástrojem pro aproximaci hustot pravděpodobností, které nekladou omezení na tvar posteriorních hustot pravděpodobnosti. • Particle filtry se mohou přizpůsobit téměř libovolné charakteristice senzorů, dynamice pohybu či rozložení šumu. • Particle filtry rozprostírají výpočetní výkon uvnitř stavového prostoru v poměrech odpovídající příslušným posteriorním pravděpodobnostem, čímž je výkon soustředěn zejména do okolí vysokých pravděpodobností. • Výpočetní náročnost algoritmu může být jednoduše přizpůsobena aktuální výpočetní kapacitě i za běhu přizpůsobením počtu vzorků bez zásahu do samotného algoritmu. • Implementace algoritmu není příliš komplikovaná a může být poměrně snadno paralelizována. Základní myšlenka Monte-Carlo lokalizace je vhodně aproximovat důvěru Bel(x) váženou množinou vzorků (nazývané particly) tak, aby diskrétní distribuce definovaná vzorky skutečně odpovídala výchozí spojité distribuci. Koeficienty vážící jednotné vzorky se nazývají importance faktory. Počáteční rozložení důvěry je reprezentováno uniformním rozdělením množiny vzorků o velikosti n, což znamená, že množina n vzorků uniformně vybraná z prostoru všech možných pozic robotu bude mít přiřazen importance faktor n−1 . MCL algoritmus ve své podstatě implementuje aktualizaci vztahu 6.20 vytvořením nové množiny vzorků ze stávající množiny jako odezvu na příchozí informaci o pohybu robotu ut−1 a senzorické měření zt (pozorování světa) následovně: 1. Náhodně vyber vzorek xt−1 z aktuálního popisu důvěru Belt−1 (xt−1 ) s pravděpodobností danou importance faktory příslušných vzorků důvěry Belt−1 (xt−1 ). 2. Pro tento vybraný vzorek xt−1 odhadni jeho možnou pozici xt podle rozložení pravděpodobnosti P (xt | ut−1 , xt−1 , m) (pohybového modelu), viz. obr. 6.2. 3. Vzorku xt přiřaď výchozí hodnotu importance faktoru podle rozložení P (zt | xt , m) (aplikace senzorických dat) a přidej jej do množiny vzorků reprezentující Belt (xt ). 4. Opakuj kroky 1-3 n krát a nakonec normalizuj importance faktory všech vzorků množiny Belt (xt ), aby jejich součet byl roven jedné. Shrneme-li výše uvedená fakta, MCL pracuje rekurzivně ve dvou základních krocích: • Predikce - posun všech vzorků na základě informací o změně pozice robota např. z odometrie. • Korekce - úprava množiny jednotlivých vzorků a jejich vah 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.

34

KAPITOLA 6. STAV PROBLEMATIKY VE SVĚTĚ

Obr. 6.2: Vzorkovaná aproximace pravděpodobnosti výskytu robotu za předpokladu, že robot měří pouze odometrii. Červená plná čára představuje předpokládaný pohyb robotu a tečky reprezentují rozložení důvěry robotu v různých bodech v čase. V kontextu praktického použití MCL algoritmu je třeba poznamenat, že díky vzorkovaným aproximacím pravděpodobnostních rozložení algoritmus nemůže pracovat s idealizovanými daty (distribuce P (zt | xt , m) mají velmi ostrá maxima) a bezchybným modelem světa. Data musí obsahovat určité množství šumu, které více rozprostře distribuce P (zt | xt , m). Z tohoto důvodu je přímé využití metody pro přesné senzory s nízkým šumem omezeno. Proto byla vyvinuta metoda nazývaná Dual MCL (Thrun a kol., 2001) a následně i kombinace původní MCL a Dual MCL nazývaná Mixture MCL. Princip duality u metody Dual MCL spočívá ve výměně rolí předpokládaných dis(i) tribucí a importance faktorů v MCL. Dual MCL tedy neodhaduje příslušné stavy xt s následným nastavením importance faktorů použitím senzorického měření, ale stavy odhaduje na základě korespondence s aktuálním senzorickým měřením a importance faktory nastaví podle výchozí důvěry Bel(xt−1 ). Důsledkem této modifikace jsou lepší vlastnosti metody pro nízkošumové senzory za cenu horší výkonnosti v případě zašuměných dat. Původní implementace MCL algoritmu pracovaly s modelem světa používající mřížky obsazenosti, práce (Yuen a MacDonald, 2003) nahrazuje tento model polygonálním modelem, kde překážky nejsou reprezentovány formou buněk, ale geometrickými liniemi, což celý algoritmus urychluje.

6.1.4

Scan-matching lokalizace

Lu a Milios (Lu a Milios, 1994), (Lu a Milios, 1997) používají scan-matching techniky pro přesný odhad pozice robotu na základě 2D scanů z laserového scanneru (rangefinderu) a vytvořených modelů prostředí. 2D scan je uspořádaná množina bodů v rovině, jejichž souřadnice odpovídají místům v prostoru, kde byla senzorem detekována překážka. Poloha každého bodu je měřena v sou-

6.1. LOKALIZACE POLOHY

35

řadnicích polární soustavy souřadnic, která je přirozenou souřadnou soustavou pro způsob pořízení dat. Formálně lze scan popsat jako množinu {[ϕ1 , ρ1 ], [ϕ2 , ρ2 ], . . . , [ϕn , ρn ]}, kde ϕ je úhel snímacího paprsku vůči ose scanneru a ρ je laserem naměřená vzdálenost k překážce. Tento přístup spočívá v iterativním zlepšováním registrace scanů aplikací vhodné transformace na souřadnice množiny bodů v rovině, čímž minimalizuje vzdálenost mezi scany. Vzdáleností scanů je myšlena suma čtverců vzdáleností korespondujících si bodů ve dvou scanech sejmutých v různých časech nebo z různých míst. Algoritmus je v podstatě formulován jako iterativní hledání parametrů rotačnětranslační transformace minimalizující zobecněnou vzdálenost mezi všemi scany. Scanmatching lokalizace se vyznačují vysokou přesností za cenu poměrně vysoké citlivosti na počáteční velikost rotace a translace mezi scany. V případě, že počáteční vzdálenost mezi scany je příliš velká, algoritmus nemusí konvergovat ke správnému řešení. Vzhledem k tomu, že tento přístup nevyužívá práce s neurčitostí, v případě selhání algoritmu nelze o dosaženém řešení v podstatě nic říci. Problém citlivosti na počáteční podmínky lze řešit jinou, méně přesnou metodou, která však vykazuje robustnější chování. Autoři (Lu a Milios, 1994) navrhli metodu jak optimalizační problém nalezení vhodné rotačně-translační transformace se třemi stupni volnosti převést na hledání extrému funkce jedné proměnné s vloženou dvojdimenzionální optimalizací. Popsaný postup využívá tangenciálních přímek, které zjednodušují hledání korespondencí bodových párů. Nehledají se tedy přímé korespondence bodů mezi referenčním a aktuálním scanem, nýbrž korespondence mezi body aktuálního scanu a tangenciálními přímkami. Tangenciální přímky jsou vypočítávány pro všechny body P z referenčního scanu proložením n okolních bodů algoritmem nejmenších čtverců tak, aby suma vzdáleností nejbližších bodů kolem bodu P od tečné přímky byla minimální. S ohledem na obrázek 6.3 se tedy minimalizuje chyba: Ef it =

n X

(xi cosφ + yi sinφ − ρ)2 .

(6.21)

i=1

P

ρ φ O

θ

Obr. 6.3: Parametry pro prokládání bodů tangenciální přímkou Určité regiony bodů z referenčního scanu, jejichž proložení neodpovídá dostatečně hladké křivce, nemohou být tangenciálními přímkami korektně proloženy. Tangenciální

36

KAPITOLA 6. STAV PROBLEMATIKY VE SVĚTĚ

přímky v těchto bodech jsou tedy na základě příslušných indikátorů z dalšího zpracování vyloučeny. Tento případ se týká regionů bodů s velkým prostorovým rozptylem, bodů odpovídající hranám překážek čí příliš velké křivosti prostředí. ! ! cos ω − sin ω Tx je rozděHledání optimální rotace Rω = a translace T = sin ω cos ω Ty leno na hledání parametru optimální rotace, které je parametrizováno vhodným odhadem translace T (viz obr. 6.4). Odhad translace T je uskutečněn za předpokladu, že povrch reprezentovaný body ze scanu Sref je hladký. V tomto případě lze aproximovat polohu bodu P2 polohou bodem P ∗ a normálový vektor Rω P1~n∗ tangenciální přímky v bodě P2 aproximovat vektorem ~n∗ . P2

P* Sref

T RωP1

otočený Snew P1 Snew

ω

O

Obr. 6.4: Ilustrace bodové korespondence Jako výsledek těchto aproximací je odvozen lineární vztah pro hledanou translaci T ve formě: C x Tx + C y Ty ≈ D. (6.22) Pro daný bod P1 a rotaci ω lze určit příslušný P ∗ bez použití T , pouze s využitím parametrů Cx ,Cy a D. Tyto parametry se vypočítají z ω a parametrů tangenciálních přímek. Důsledkem výše uvedených faktů lze definovat chybovou funkci, která ohodnocuje ”vzdálenost” mezi dvěma scany Sref a Snew jako funkci ω a T : E(ω, T ) =

np X

(C x Tx + C y Ty − Di )2 .

(6.23)

i=1

Tuto chybovou funkci lze navíc chápat jako kritérium míry správné registrace (vzájemného sesazení) dvou scanů. Při zafixování parametru rotace ω lze navíc určit potřebnou translaci minimalizující vztah 6.23. Do vztahu 6.23 vstupují jen body, které nejsou považovány za odlehlé. Optimální hodnota parametru rotace ω je získána minimalizací vztahu: Ematch (ω) =

1 (min E(ω, T ) + no Hd2 ), np + no T

(6.24)

6.1. LOKALIZACE POLOHY

37

do kterého vstupuje též váha odlehlých bodů, kde np je počet bodů s nalezenými korespondencemi, no je počet odlehlých bodů. Mezní konstantní hodnota Hd2 definuje ”cenu” odlehlých bodů, která je podobně jako parametr α ve vztahu (Rω ~n1 ) · ~n∗ ≥ cos α parametrem celého algoritmu. Vzhledem k tomu, že kritérium 6.24 není hladká funkce, ovšem obsahuje typicky jedno výrazné minimum, je pro finální optimalizaci použita iterativní metoda zlatého řezu. Výsledkem optimalizace je konečná velikost rotace ω, ke které je již dříve vypočítaná optimální translace T použitím vztahu 6.23. Autoři (Gutmann a Schlegel, 1996) vycházejí z práce (Lu a Milios, 1994), registraci scanů ovšem opírají o vytvořený liniový model prostředí a samotnou registraci neprovádí na úrovni bodů, ale na úrovni segmentovaných úseček, což výrazně zvyšuje rychlost metody. Metoda byla úspěšně verifikována reálným nasazením v soutěžích RoboCup, kde autorský tým v roce 1998 vyhrál světový šampionát (Gutmann a kol., 2001). V tomto případě je však model prostředí velmi jednoduchý a jeho liniovou reprezentaci není složité získat. Další prací, ve které je lokalizační metoda postavena na hledání korespondencí segmentovaných úseček je (Kulich, 2003). Autor předpokládá, že v množinách segmentovaných úseček z aktuálního a předposledního scanu bude možné najít neprázdnou podmnožinu alespoň dvou navzájem si korespondujících úseček, z jejichž vzájemné relativní polohy se numerickou optimalizací získá translačně-rotační transformace popisující vzájemnou polohu mezi následnými scany. Pokud metoda neselže na nedostatečném počtu korespondujících si párů úseček, poskytuje relativně velmi přesné výsledky. Přijatelné přesnosti metoda dosahuje právě díky prokládání dat přímkovými úseky, což do značné míry eliminuje šum v datech.

6.1.5

Histogramové přístupy

Další lokalizační technika, která též nepracuje s neurčitostí využívá uložených úhlových histogramů vytvořených ze scanů laserového dálkoměru změřených z různých míst v prostředí (Weiß a kol., 1994), (Weiß a Puttkamer, 1995). Pozice a orientace robotu je vypočítávána přes maximalizaci korelační funkce mezi uloženými histogramy a laserovým scanem získaným během jízdy v prostředí. Zjištěná aktuální pozice spolu s odometrickou informací je následně použita pro zpracování nových příchozích dat. Nevýhoda této metody je, že přesnost algoritmu z velké části závisí na velikosti diskretizace histogramů, přičemž pro jemnější míru diskretizace je již poměrně výpočetně náročná, asymptotická složitost je díky korelačnímu principu činnosti O(n2 ). Podobný přístup k lokalizaci je použit v (Yamauchi, 1996) a (Schultz et al., 1999) s tím rozdílem, že autoři zde pro registraci lokálních sonarových dat do mřížky obsazenosti využívají hill-climbing algoritmus. Lokalizace na bázi histogramů byla přivedena do reálné aplikace autonomního kolečkového křesla „Rollandÿ (Rˇofer, 2002). Systém pracuje s laserovým scannerem, lokalizace se opírá o průběžně budovanou globální mapu. V případě, že je během jízdy detekován návrat do dříve zmapovaného prostoru, tak dochází ke zpětným inkrementálním korekcím modelu tak, aby kumulovaná lokalizační chyba nebyla příliš velká.

38

6.1.6

KAPITOLA 6. STAV PROBLEMATIKY VE SVĚTĚ

Landmarkové lokalizace

Landmarkové lokalizační techniky využívají pro svoji činnost parametrický model světa, který je definován množinou význačných značek v prostředí - landmarků, u kterých je přesně známa jejich poloha. Lokalizace se pak opírá o extrakci příznaků ze senzorů a nalezení korespondujících landmarků v mapě. Rozdíl mezi předpokládanou polohou landmarků v mapě a naměřenou polohou je použit k aktualizaci polohy robotu. Tímto způsoben se úloha sledování vícenásobných cílů (Blackman a Popoli, 1999), převede na úlohu sledování statických cílů, ovšem z pohybujícího se pozorovatele. Klíčovým problémem landmarkových lokalizačních metod je asociace naměřených dat s polohou landmarků v mapě, což je víceméně i nejvýraznější slabinou tohoto přístupu. Špatná asociace mezi naměřenými daty a mapovým popisem může sice lokálně snížit neurčitost v odhadu polohy, ovšem zároveň se tím výrazně zvýší chyba odhadu. Vedle asociace naměřených dat je důležitá i úloha rozpoznání samotného landmarku v prostředí, neboť ve všech situacích nelze používat jednoduše detekovatelné umělé landmarky (RF majáky, odrazky apod.) a využívání přirozených landmarků může být v reálných strukturovaných prostředích velmi obtížné. Jakmile je známa asociace dat vůči mapě, pro odhad aktuální polohy jsou zpravidla použity principy využívající rozšířeného Kalmanova filtru popsaného v kap. 6.1.1. Na druhou stranu, reálné průmyslové aplikace, které využívají aktivních majáků nebo laserových scannerů spolu s přesně umístěnými odrazkami ve výrobních halách dosahují vysoké přesnosti a robustnosti (E&K Automation, 2007). Některé aplikační úlohy využívající lokalizaci landmarkového typu jsou v principu zjednodušeny a převedeny až na úlohu čisté navigace, jako např. řízení automatického kombajnu (Ollis a Stentz, 1997). Řídící systém kamerou sleduje hranici mezi pokoseným a nepokoseným polem, klíčové problémy spočívají zejména v detekci překážek a zajištění robustnosti zejména za ztížených vizuálních podmínek, např. na rozhraní sluncem nasvíceným povrchem a stínem samotného kombajnu.

6.1.7

Lokalizace z vizuálních landmarků

Většina lokalizačních algoritmů je navržena pro zpracování dat ze sonarových dálkoměrů nebo laserových scannerů. Využití obrazu z kamer bylo zpočátku omezeno na nízkoúrovňová zpracovaní obrazu, jako je například segmentace svislých hran v obraze (Castellanos et al., 1999). Segmentaci svislých hran spolu s kombinací informace získané ze sonaru použili též autoři (Kortenkamp a Weymouth, 1994). S nárůstem výpočetní kapacity současných počítačů a zároveň s klesající cenou použitelných kamerových systémů v posledních letech velmi akceleruje výzkum vizuálních lokalizačních technik. Přístupy k vizuální lokalizaci jsou obecně založeny na párování příznaků extrahovaných z obrazu. Jednotlivé metody se mezi sebou liší druhem používaných příznaků, způsobem jejich párování a v neposlední řadě samotným způsobem reprezentace odhadu polohy robotu. (Sim a Dudek, 1999) navrhl metodu využívající naučených přirozených landmarků v prostředí pro určování pozice. Metoda pracuje ve dvou fázích. V první fázi se dávkovým

6.2. METODY PRO STAVBU MODELU PROSTŘEDÍ

39

zpracováním vyberou význačné landmarky v prostředí, které byly z obrazu detekované po určitou delší dobu během jízdy robotu. Ty jsou následně parametrizovány (pozice v obraze, distribuce intenzit a hran v obraze) pro použití během druhé, lokalizační fáze. Ve druhé lokalizační fázi se z aktuálního obrazu opět stejným postupem jako v učící fázi detekují význačné landmarky a pomocí metody PCA (Principle Component Analysis) se provádí párování landmarků uložených v databázi. Po výběru odpovídajících si aktuálních landmarků a landmarků z databáze je určena příslušná aktuální pozice, která odpovídá pozici, ze které byly znovu pozorované landmarky dříve sejmuty. Reálná použitelnost vizuálních lokalizačních technik významnou měrou závisí na schopnosti rozpoznávat landmarky a nalézat jejich vzájemné korespondence. Rozpoznávání landmarků z pohybujícího se robotu v prostředí musí být invariantní vůči translaci, rotaci a měřítku. Toto splňuje SIFT metoda představená v práci (Lowe, 1999). Této metody rozpoznávání příznaků ve scéně ve formě landmarků využili autoři (Se a kol., 2002) pro vývoj lokalizační metody opírající se o trojici kalibrovaných kamer (trinocular stereo). V obrazu se detekují SIFT příznaky, jejichž poloha je sledována Kalmanovým filtrem. Pohyb robotu je pak odhadován metodou nejmenších čtverců vzdálenosti mezi nalezenými korespondujícími příznaky. Další prací využívající SIFT příznaků je (Elinas a Little, 2005), ve které autoři rozvíjejí metodu Monte-Carlo pro lokalizaci robotu v plných šesti stupních volnosti na základě SIFT příznaků a celou lokalizační metodu pak nazývají σMCL. Autor (Thrun, 1998) představil přístup jak detekovat a využívat landmarky užitečné pro lokalizaci mobilního robotu spolu s ohledem na nejistotu v aktuální pozici robota. Těchto zkušeností využili autoři (Dellaert et al., 1999) pro konstrukci lokalizačního systému, kde jako landmarky jsou využita svítidla umístěná na stropě místností. Předem je nasnímána kompletní mozaika obrazu stropů ve formě mapy prostředí a lokalizace je pak formulována jako úloha komplexního odhadu stavu pomocí MCL. Výše popsané techniky využívají buď sofistikovaných technik hledání korespondencí příznaků (feature-matching) nebo spoléhají na jednoduché příznaky jako linie a barvy v prostředí a používají metody pravděpodobnostního odhadu stavu či učící techniky k lokalizování robotu. Tyto přístupy vhodně kombinují autoři (Wolf a kol., 2005), kteří na praktických experimentech ukázali, jak vhodně spojit systém vyhledávání kontextu obrazových příznaků v databázi s principy pravděpodobnostního odhadu stavu pomocí metod Monte-Carlo lokalizace.

6.2

Metody pro stavbu modelu prostředí

Pokud mluvíme o výstavbě modelu světa, mluvíme v podstatě o procesu převádění popisu (tvaru a vlastností) reálného prostředí do datové reprezentace, která je vhodná pro další zpracování. Crowley (Crowley, 1989) problematiku budování a udržování lokálního modelu světa popisuje jako třístupňový proces: • Vytvoření abstraktního popisu nejaktuálnějších dat ze senzorů (senzorický model). • Určení korespondencí a registrace aktuálních dat vůči existujícímu lokálnímu modelu světa.

40

KAPITOLA 6. STAV PROBLEMATIKY VE SVĚTĚ • Modifikace modelu světa s ohledem na rozdíly získané během registrace nových dat vůči původnímu modelu světa.

Způsob reprezentace reálného světa v datové podobě je určena zejména požadavky, pro jaký účel bude model používán. Z hlediska navigace mobilního robotu je třeba rozlišovat mezi modely podporující lokalizaci robotu, plánování a řízení pohybu. V této práci budeme častěji hovořit spíše o mapách prostředí než o komplexních modelech prostředí. Mapy prostředí jsou nedílnou součástí modelu prostředí, popisují zejména statické geometrické či topologické vlastnosti prostředí, ale neobsahují však např. znalosti, jakým způsobem může robot na prostředí zpětně působit a jak se prostředí může měnit. Způsoby reprezentace map prostředí lze podle míry abstrakce rozdělit na následující druhy: • senzorické, • geometrické (segmentové, polygonální, křivkové), • topologické, • symbolické. Znázornění jednotlivých druhů map rozdělených podle míry abstrakce je ilustrováno na obr. 6.5. Senzorické mapy pracují přímo s hrubými senzorickými daty, nebo jen s jejich relativně jednoduchým zpracováním. Hrubá senzorická data se využívají na úrovni reaktivních zpětných vazeb v subsystémech předcházení a řešení kolizí, kdy je potřeba rychlých reakcí (David, 1996), uplatní se tedy zejména na úrovni řízení pohybu. Typickým příkladem senzorické mapy jsou (pravděpodobnostní) mřížky obsazenosti (occupancy grids) (Moravec a Elfes, 1985), (Elfes, 1990). Mřížkou se rozumí dvourozměrné pole buněk, kde každá buňka reprezentuje čtverec reálného světa a určuje pravděpodobnost, s jakou se v dané oblasti vyskytuje překážka. Mřížky obsazenosti (occupancy grids) jsou jedním z využitelných modelů vybranými lokalizačními technikami (Fox a kol., 1999b), (Schiele a Crowley, 1994). Své uplatnění nacházejí však též v oblasti středně a nízkoúrovňového plánování (Connolly a Grupen, 1993) pohybových trajektorií robotu. Mřížky obsazenosti jsou také často využívány pro vizualizace prostředí. Dalším příkladem senzorických map jsou bodové mapy, které jsou vhodné zejména pro podporu lokalizačních technik, neboť jejich interpretace je z hlediska lokalizace poměrně jednoduchá a rychlá. Bodové senzorické mapy lze z určitého úhlu pohledu řadit mezi nejjednodušší geometrické mapy, neboť nesou informace o souřadnicích objektů v prostoru a data v nich obsažená jsou již výrazným způsobem zpracována a filtrována. Bodovými mapami se bude zabývat celá následující kap. 8 v kontextu jejich využití pro lokalizaci mobilního robotu. Velmi efektivní reprezentací prostředí jsou geometrické mapy popisující objekty v prostředí pomocí geometrických primitiv v kartézském souřadném systému. Jako geometrická primitiva jsou brány segmenty (úsečky), polygony, části oblouků, splinů, či jiných křivek. Nad tímto popisem se často vytváří další struktury podporující plánování cesty

6.2. METODY PRO STAVBU MODELU PROSTŘEDÍ

41

3 2 1 0 −1 −2 −3 −4 −5 −6 −6

−4

−2

0

(a) A

E

C

2

G

vede do

rozloha

B

20m

H

je

J

(c)

Místnost 2 má

má

Okno Stůl

K

Místnost 1 má

F

L

8

vede do sousedí

2

N

M

6

Chodba

50m3 velikost

D

4

(b)

je blízko je

Židle

kolik je

3ks

I

Psací

Otočná

Kovová

(d)

Obr. 6.5: Způsoby reprezentace map prostředí a) Senzorická mapa - mřížka obsazenosti b) geometrická mapa - polygonální c) topologická mapa d) symbolická mapa

robotu (např. grafy viditelnosti, obdélníková dekompozice). Geometrické mapy lze použít též pro lokalizaci robotu v prostředí nebo je lze exportovat do CAD nástrojů, ve kterých se následnými korekcemi vytvoří kompletní CAD model prostředí pro jiná než čistě robotická využití. Speciálním případem geometrické mapy je polygonální mapa, která je vyžadována jako vstup vybraných plánovacích algoritmů pro hledání cest v prostoru či pokrývání prostoru (Kulich a kol., 2005). Polygonální mapa je popsána množinou orientovaných polygonů (polygon s dírami), kde orientace polygonu určuje, zda se jedná o tzv. vnější polygon ohraničující pracovní oblast robotu nebo zda se jedná o vnitřní polygon popisující překážku v prostoru. Vysokého stupně abstrakce dosahují topologické mapy (Thrun a Bucken, 1996), které neuchovávají informace o absolutních souřadnicích objektů, ale zaznamenávají topologické vztahy mezi objekty v prostředí (Dudek a kol., 1993), (Lefevre et al., 1994). Pro komunikaci s uživatelem na nejvyšším stupni abstrakce se používají symbolické mapy obsahující informace, které robot zpravidla nemůže přímo zjistit svými senzory. Obsahuje zpravidla abstraktní informace, pomocí kterých lze s robotem do jisté míry komunikovat v přirozeném jazyce. Implementace tohoto typu map bývá prováděna v některém deklarativním jazyku - v Prologu nebo LISPu (Malý, 2005).

42

KAPITOLA 6. STAV PROBLEMATIKY VE SVĚTĚ

Každý z uvedených přístupů k reprezentaci mapy prostředí má svoje klady i zápory. Senzorické reprezentace umožňují pracovat s nepřesnou senzorickou informací, ale jsou naopak velmi náročné na paměť. Topologické reprezentace lze používat pro popis velkých prostorů, neboť jejich paměťová náročnost je malá. Navíc jejich tvorba není na rozdíl od geometrických a senzorických map závislá na přesné znalosti polohy. Výhodou geometrických map je jejich přesnost a že jsou srozumitelné pro člověka, který tak může apriorní informace o objektech v okolí robotu zadávat právě formou geometrické reprezentace. Navíc pro mnoho budov je geometrická mapa hotova ve formě architektonických plánů. Vzhledem k tomu, že každý výše uvedený způsob reprezentace prostředí pracuje s jiným druhem informace, často je velmi obtížné až nemožné jednotlivé reprezentace mezi sebou vzájemně libovolně transformovat. Možné transformace reprezentací jsou zpravidla jen od nižších stupňů abstrakce k vyšším, ale i to je poměrně komplikovaná úloha, neboť často pro transformaci chybí znalost kontextu, neměřitelné atributy apod.

6.2.1

Mřížky obsazenosti

Mřížky obsazenosti (Moravec a Elfes, 1985), (Elfes, 1990) rozdělují popis prostředí do množiny buňek, přičemž každá buňka nese pravděpodobnostní informaci o tom, zda místo, které buňka reprezentuje je volné či obsazené. V reálném světě však překážka v daném místě prostoru buď je a nebo není. Pravděpodobnost obsazení buňky je proto chápáno jako poměr počtu světů, které odpovídají dosavadním měřením a danou buňku mají obsazenou vůči počtu všech světů, které odpovídají dosavadním měřením. Přestože pravděpodobnosti příslušné jednotlivým buňkám nejsou nezávislé, pro praktické výpočty se tato skutečnost zpravidla zanedbává. Výpočet pravděpodobnosti v každé buňce se skládá ze dvou kroků. Nejprve se vytvoří pravděpodobnostní model senzoru, který umožní určit hodnotu pravděpodobnosti v každé buňce pro jednotlivé senzorické měření. Takto získané pravděpodobnosti z každého měření se poté slučují (např. pomocí Bayesova pravděpodobnostního přístupu, věrohodnostního přístupu Dempster-Shaferovy teorie nebo teorie fuzzy množin) do výsledné mřížky obsazenosti. Jeden ze zásadních problémů, který přináší mřížka obsazenosti je skutečnost, že paměťová náročnost na udržování mřížek obsazenosti roste s velikostí prostředí O(n2 ) a ne s jeho složitostí (MacKenzie a Dudek, 1994). S vysokou paměťovou náročností roste též výpočetní čas pro operace s těmito datovými strukturami, což může být pro aplikace pracující v rozsáhlejších prostorech závažný nedostatek. Mřížky obsazenosti však i přesto mají v oblasti robotiky své nezastupitelné místo, lze je použít i jako pomocnou strukturu pro jiné reprezentace, neboť je z nich možné extrahovat geometrický popis prostředí (Kulich, 2003) nebo z nich extrahovat též i informace o topologii prostředí, jak ukazuje autor (Fabrizi a Saotti, 2000). Další velkou výhodou mřížek obsazenosti je, že kromě samotné reprezentace modelu světa jsou zároveň velmi vhodným nástrojem pro fúzi i velmi různorodých senzorických dat. V práci (Štěpán et al., 2005) autor popisuje využití mřížek obsazenosti pro fúzi dat ze sonarů a barevné monokulární kamery. Mřížka obsazenosti je využita jako datová reprezentace pro slučování informací o volném prostoru z dat ze sonaru a segmentovaného

6.2. METODY PRO STAVBU MODELU PROSTŘEDÍ

43

obrazu z kamery, u kterého je volný prostor určován na základě odlišné barvy podlahy a překážek v prostoru.

6.2.2

Geometrické mapy

Geometrické mapy prostředí patří do skupiny modelů se střední mírou abstrakce (Chmelař, 1998). Při popisu světa se předpokládá, že je možné jej aproximovat základními geometrickými primitivy. Na nejnižším stupni se pracuje pouze s vektorizovanými daty, které popisují prostředí pomocí jednoduchých primitiv, tj. pomocí liniových segmentů (Mázl a Přeučil, 2000) bez další přídavné informace o jejich vzájemných relacích. Polygonální popis prostředí vychází z vektorového popisu, avšak prostředí a objekty jsou v něm reprezentovány mnohoúhelníky, které se skládají ze segmentů reprezentující skutečné hrany objektů v prostředí či hranice dosud neprozkoumaných oblastí (Kulich, 2003). Každý liniový segment rozděluje prostor na dvě části, kde jedna část prostoru je tvořena volným prostorem a druhá část obsazeným, popř. neznámým prostorem. Ohodnocení obsazenosti prostoru je zahrnuto v orientaci jednotlivých segmentů, přičemž volný prostor se nachází vždy na levé straně úsečky ve smyslu její orientace. Z orientace segmentů následně vychází i orientace polygonů z nich složených, přičemž levotočivý polygon obvykle ohraničuje pracovní prostředí robotu a pravotočivý polygon popisuje překážky umístěné uvnitř vnějšího, hraničního polygonu (tzv. boundary polygon). Jednotlivé hrany mohou být ohodnoceny mírou spolehlivosti, statistickou významností, či počtem měření potvrzující skutečnou existenci hrany. K jednotlivým mnohoúhelníkům lze přidat i informaci o typu objektu (stěna, dveře, mobilní překážka atd.), čímž se geometrická mapa začne částečně blížit mapě symbolické.

Volný prostor

Volný prostor Objekt 1

Objekt 1

Objekt 2

Objekt 2

Neznámá oblast

Obr. 6.6: Reálné prostředí

Obr. 6.7: Polygonální grafová reprezentace

Pokud mezi hranami jednotlivých objektů zavedeme vzájemné relace, dostáváme polygonální grafový popis prostředí. Celé prostředí je pak možné popsat formou planárního topologického grafu (Přeučil a Štěpán, 1996), (viz obr. 6.7). Topologický graf obsahuje uzly popisující 2D souřadnice hran, které mohou mít různé atributy: • reálná hrana (může být viditelná i neviditelná), • virtuální hrana (je vždy neviditelná).

44

KAPITOLA 6. STAV PROBLEMATIKY VE SVĚTĚ

Reálné hrany vždy reprezentují skutečné překážky, kterými není možné fyzicky projet, atribut viditelnosti určuje, zda překážka byla detekována senzory či nikoliv. Typicky hrany v neznámé oblasti prostoru jsou označeny jako neviditelné. Virtuální hrany jsou určeny pro spojování reálných hran a zajišťují tak spojitost grafu. Virtuální hrany mohou rozdělovat volný prostor na konvexní oblasti, které se následně dají použít jako elementární buňky pro budoucí plánování trajektorií pohybu robotu. Velkou výhodou polygonálního modelu je snadná reprezentace překážek, ovšem výraznou nevýhodou je poměrně velká složitost automatického budování takovéto mapy, neboť během procesu stavby takové mapy je často nutné senzorická měření správně interpretovat v kontextu reálného světa. Inkrementální přístup k budování geometrické mapy byl použit v (Shaffer, 1995), kde se postupně slučují tzv. kontury skládající se z množství krátkých liniových segmentů získaných přímo ze senzorických dat. Slabinou tohoto postupu je hledání korespondencí mezi jednotlivými konturami, pokud se např. díky šumu nenaleznou v mapě již odpovídající kontury, mohou být hrany jedné překážky do mapy vkládány jako duplicitní kontury. Dávkovým způsobem tvorby geometrického popisu prostředí se zabývá práce (Kulich, 2003), která před zahájením tvorby mapy vyžaduje mít k dispozici kompletní množinu senzorických dat, což přináší možnost eliminace šumu v datech. Autor používá dvou postupů, matematické morfologie a metody rostoucího neuronového plynu. První postup ze senzorických dat primárně buduje mřížky obsazenosti, které následně segmentuje a podrobuje operacím založených na matematické morfologii. Tím dosáhne extrakce hranice z mřížky obsazenosti, eliminace šumu a spojení původně nesouvislých objektů do celku. Výsledkem operací matematické morfologie je posloupnost buněk, které odpovídají kostře hranice obsazených částí prostoru. Posloupnost buněk je pak vektorizována do podoby geometrické mapy prostředí. Druhý postup využívající rostoucího neuronového plynu je založen na aproximaci naměřených dat samoorganizující se strukturou, která umožňuje dynamicky měnit počet uzlů struktury. Uzly a hrany neuronové síťové struktury se stávají základem pro následující aproximaci hranic překážek úsečkami.

6.2.3

Topologické mapy

Topologická mapa je ve svém principu protikladem geometrické mapy. U geometrické mapy se snažíme o vyjádření pokud možno co nejpřesnější polohy všech překážek, zatímco vzájemné vazby mezi určitými částmi prostoru nejsou explicitně vyjádřeny. Topologická mapa oproti geometrické mapě reprezentuje prostředí ve formě grafu, kde vzájemné metrické informace popisující prostředí nejsou příliš důležité. Význačné části prostoru popř. význačné objekty jsou v topologické mapě reprezentovány jako uzly grafu. Hrany grafu pak spojují ty uzly v grafu, mezi kterými existuje určitá význačná relace, např. průjezdná cesta, přímá viditelnost, komunikační dosah apod. Své uplatnění nalézají též v oblasti vysokoúrovňového plánování ve velmi rozsáhlých prostorech. Použití topologických map je vhodné též v systémech, které s ohledem na použité senzory neumožňují budovat přesné metrické mapy a z pohledu úlohy je topologická informace o poloze robotu dostačující (Shatkay a Kaelbling, 1997). Pokud je robot vybaven pouze

6.2. METODY PRO STAVBU MODELU PROSTŘEDÍ

45

odometrickými senzory, přesnost odometrie sice neumožní vybudovat globální mapu, ale umožní robotu navigaci mezi sousedními uzly topologické mapy. Topologické mapy mohou být vybudovány buď přímo, nebo mohou být odvozeny z geometrických nebo senzorických map (Thrun a Bucken, 1996) na základě jejich dekompozice a abstrakce. Autoři (Fabrizi a Saotti, 2000) budují topologickou mapu na základě matematické morfologie aplikované na mřížky obsazenosti. Na základě postupně aplikovaných operací eroze jsou získávány soustředné kontury, které v místě svých pomyslných center (místa zániku rozdělených kontur) vytvoří uzly topologického grafu. Hrany jsou pak vloženy mezi uzly původně náležící do společného prostoru ohraničeného jednou konturou, která se však vlivem postupně opakovaných operací erozí rozdělila do dvou sousedních kontur a každý uzel náleží jedné z nich. Možný způsob přímého budování topologické mapy na základě odometrických dat je popsán v (Shatkay a Kaelbling, 1997), kde se využívá Baum-Welch algoritmu, varianty EM algoritmu použitého v kontextu skrytých Markovských modelů. Jiný způsob přímé tvorby topologické mapy použili autoři (Ranganathan et al., 2006) pro budování pravděpodobnostních topologických map, kde reprezentují mapu pomocí vzorkované aproximaci posteriorní hustoty pravděpodobnosti (metodou Markov Chain Monte-Carlo sampling) přes prostor všech topologíí získaných ze senzorických měření. Věrohodnosti senzorických měření jsou odvozovány od vizuálního vzhledu landmarků a prostorové konfigurace detekovaných landmarků na základě odometrie. V projektu ALICE (Lefevre a kol., 1994) je použito neuronových sítí pro vytvoření částečně symbolické topologické reprezentace dynamického prostředí. Topologické mapy nejsou příliš použitelné pro přesnou lokalizaci a zpracování metrických informací o objektech, ovšem na druhou stranu mohou výrazně pomoci při hledání přibližné polohy robotu v rozsáhlém prostoru typu bludiště (Huang a Beevers, 2005). Topologické mapy mohou být základem hierarchických modelů prostředí, kdy topologická mapa popisuje prostředí jako celek a jednotlivé lokální mapy jsou reprezentovány geometrickou či senzorickou mapou.

6.2.4

Symbolické mapy

Symbolické mapy popisují prostředí na nejvyšší možné míře abstrakce. Jejich využití je předurčeno zejména v uživatelských rozhraních pro komunikaci mezi robotem a člověkem. Základní součástí symbolické mapy prostředí jsou záznamy o vlastnostech objektů, vzájemných vazbách a relacích mezi objekty. Jedná se zpravidla o statické vlastnosti a vazby, které nemohou být získány senzorickými měřeními. Vedle těchto vlastností popis prostředí může obsahovat též statická pravidla, která platí v popisovaném reálném, případně abstraktním světě. Mapa vzniká z velké části za vzájemné interakce s člověkem, který poskytuje vhodné interpretace naměřených senzorických informací. Na základě těchto interpretací následně probíhá i komunikace mezi robotem a operátorem, často pomocí přirozeného jazyka (Kazakov et al., 1997). Symbolická mapa vedle naměřených informací umožňuje uchovávat i dodatečné informace, nad kterými je možné provádět odvozování a dokazování nástroji matematické logiky. Možnost provádět odvozovaní či dokazování nad známými

46

KAPITOLA 6. STAV PROBLEMATIKY VE SVĚTĚ

fakty umožňuje určitým způsobem též omezit významové datové duplicity, které jsou odvoditelné z ostatních. Interní reprezentace znalostí uložených v symbolickém popisu prostředí je z tohoto důvodu založena na predikátech, stavebních kamenech matematické logiky, která se stala nepsaným standardem pro použití v umělé inteligenci. Symbolická interpretace dat vyžaduje speciální vyjadřovací prostředky, proto v oblasti umělé inteligence vznikly speciální jazyky jako UML (Müller, 1997), KQML (Mařík a kol., 2001), KIF, Ontolingua a další. Symbolické mapy jsou ve své podstatě znalostní bází komplexních symbolických modelů prostředí, které navíc obsahují i popis způsobu chování robotu a jeho reakce na různé podněty v různých situacích. Symbolický model světa se tak začíná prolínat s plánovacími úlohami, což vede k návrhu zcela nových řídicích struktur a algoritmů pro mobilní roboty (Malý, 2005). Symbolické modely prostředí jsou s ohledem na způsob reprezentace dat implementovány v deklarativních programovacích jazycích jako je Prolog či LISP. V současné době jsou symbolické reprezentace spíše ve stádiu počátečního výzkumu, svá široká uplatnění naleznou pravděpodobně v blízké budoucnosti.

6.2.5

Mapy silnic

Pro úlohu plánování trajektorií se též využívá speciálních druhů topologických map využívající systém mapy silnic (Choset a Nagatani, 2001), (Canny, 1988). Systém map silnic je jednodimenzionální podmnožina volného prostoru robotu, která zachycuje všechny jeho důležité topologické vlastnosti jako jsou: dosažitelnost, konektivita a opustitelnost. Tyto vlastnosti zaručují, že plánovač může naplánovat cestu mezi dvěma body ležící v jedné souvislé komponentě reprezentující volný prostor. Plánovač pak hledá spojnici k mapě silnic (dosažitelnost), dále využije systém silnic do blízkosti cílového bodu (konektivita) a z nejbližšího bodu na mapě silnic k cíli se pak provede doplánování přímo do cílového bodu (opustitelnost). Pokud robot inkrementálně buduje systém silnic, tak může vždy použít stávající systém silnic pro průzkum volného prostoru. Autoři (Choset a Nagatani, 2001) reprezentují volný prostor pomocí zobecněných Voronoi diagramů (GVG) a představují metody pro simultánní lokalizaci a mapování využívající topologii volného prostoru k vlastní lokalizaci v částečně vybudované mapě. Topologie prostředí je obsažena v topologické mapě, která je inkrementálně konstruována jako zobecněný Voronoi diagram. Přestože GVG obsahují též určité metrické informace, neumožňují robotu určit přesně jeho (x, y) pozici, lze pouze částečně propagovat polohovou informaci na základě polohy předem známých bodů. Významná výhoda tohoto přístupu je možnost návrhu nízkoúrovňových řídicích pravidel, které průběžně konstruují hrany a uzly Voronoi diagramů a umožňují tak vytváření následných plánů pro průzkum neznámého prostoru.

6.2.6

Příznakové mapy

Mapy příznaků (často nazývané spíše landmarkové mapy) je možno z určitého úhlu pokládat za kombinaci topologického popisu prostředí v kombinaci s přiřazenými metrickými

6.2. METODY PRO STAVBU MODELU PROSTŘEDÍ

47

údaji a dalšími význačnými vlastnostmi prostředí. Typickou vlastností landmarkových map je práce s neurčitostí, z tohoto důvodu jsou tyto druhy map někdy nazývány jako stochastické mapy (Smith a kol., 1990). Práce s neurčitostí je zásadním rozdílem vůči geometrickým mapám, které přestože reálný svět popisují s definovanou mírou nepřesnosti, explicitně neumožňují pracovat s nejistými daty, které poskytují senzory. Pracují pouze s nejpravděpodobnějším pohledem na změřená data. Vlastní příznaková mapa se skládá s popisů poloh význačných objektů včetně jejich odhadované neurčitosti. Mapa obsahuje též odhady vzájemných prostorových vztahů mezi objekty v mapě včetně jejich neurčitostí na základě všech dostupných informací. Dílčí informace jsou v mapě ukládány ve formě vektorů prostorových proměnných, neurčitost v datech je reprezentována pravděpodobnostními distribucemi. Z praktických důvodů jsou prostorové proměnné popisující vzájemné prostorové vztahy mezi objekty popisovány prvními dvěma momenty jim příslušejících distribucí pravděpodobnosti, tedy střední hodnotou a kovarianční maticí. Mapy jsou budovány na základě senzorických měření inkrementálně, během jízdy robotu v prostředí se nově příchozí informace postupně vkládají do mapy s ohledem na jejich prostorové vazby. Základním nástrojem na udržování obsahu map je Kalmanův filtr, pomocí kterého se aktualizují příslušné odhady poloh změřených příznaků prostředí včetně jejich neurčitostí a vzájemných vztahů. Příznaky prostředí jsou myšleny polohy jednoznačně rozlišitelných primitiv, jako jsou rohy místností, dlouhé zdi nebo jiné snadno segmentovatelné geometrické útvary. Na teoretických základech (Smith a kol., 1990) je postavena práce (Castellanos a Tardós, 1999), která rozvíjí myšlenky symetricko-perturbačních modelů (dále jen SP modely) původně představených v (Tardós, 1992). Symetricko-perturbační modely integrují geometrické informace, které umožňují odhad polohy příznaků nebo objektů z množiny částečných a nejistých pozorování či měření. Senzor měřící polohu

Odhad hrany

Řešení č. 1

Model objektu

Model hrany

Řešení č. 2

Obr. 6.8: Pozorování hrany objektu a částečné informace o jeho poloze (Tardós, 1992) Tato práce pracuje s částečnými reprezentacemi poloh základních geometrických elementů jako je bod, úsečka, rovina. Částečnými reprezentacemi jsou myšleny translační, rotační a cyklické symetrie, jak je znázorněno na obr. 6.8. Význam tohoto popisu spočívá

48

KAPITOLA 6. STAV PROBLEMATIKY VE SVĚTĚ

v tom, že pokud je pozorována jen část objektu, která nedovoluje přesně popsat jeho polohu, umožní nám pracovat se všemi možnými symetrickými reprezentacemi pozorovaných dat. Poloha každého elementu je pak reprezentována jeho přidruženou referenční souřadnou soustavou, nejistota polohy elementu je vztažena k náhodné proměnné s normálním rozdělením, která je nazývána perturbační vektor geometrického elementu. Během párování geometrických elementů pro účely aktualizace mapy rozšířeným Kalmanovým či informačním filtrem se pracuje s maticemi možných vzájemných vazeb mezi skupinami elementů. Tyto matice vazeb umožňuje vyjádřit jeden ze základních geometrických konceptů, vzájemné splývání dvou elementů, např. bodu a přímky. Vazby jsou dále klíčové pro slučování dílčích popisů objektů ze základních geometrických elementů. Koncepce SP modelů částečně zjednodušuje komplexitu globálních systémových kovariančních matic, které svazují vzájemné závislosti mezi dílčími elementy mapy. Autoři (Castellanos a Tardós, 1999) na bázi SP modelů realizují úlohu simultánní lokalizace a mapování, kdy robot na základě svých pozorování aktualizuje informace o polohách a neurčitostech dílčích geometrických elementů, podle kterých pak zároveň určuje svoji pozici. Robot je v principu chápán jako jeden z objektů, jehož poloha se průběžně odhaduje.

6.3

Simultánní lokalizace a mapování

Problematika simultánní lokalizace a mapování byla zpočátku obvykle řešena pod názvem úlohy inkrementální mapování a modelování prostředí (Moutarlier a Chatila, 1989a), (Moutarlier a Chatila, 1989b). Později se vžil termín ”Simultaneous Localization and Mapping” a tím úlohy zkráceně označované jako SLAM. K řešení těchto úloh se přistupuje mnoha různými způsoby, nejčastěji s využitím Kalmanova filtru a particle filtrů s řadou rozšíření. Řešení SLAM úloh se musí vypořádávat s určováním aktuální polohy rozpoznáváním již dříve zmapovaných objektů ve scéně a hledáním jejich korespondencí vůči objektům v pozorovaných datech. V případě, že není nalezena korespondence aktuálních obrazů objektů v datech, je třeba přistoupit k modifikaci stávající mapy.

6.3.1

SLAM a Kalmanův filtr

Přístupy k SLAM úloze založené na rozšířeném Kalmanově filtru trpí dvěma zásadními problémy. První problém představuje kvadratická složitost aktualizace mapy ve smyslu vzrůstajícího počtu landmarků či základních stavebních geometrických elementů v mapě. Kvadratická složitost vychází z faktu, že Kalmanovým filtrem udržovaná kovarianční matice popisující landmarky má O(K 2 ) elementů, kde K je počet landmarků. V případě rozpoznání každého landmarku je nutno celou tuto matici aktualizovat, což limituje maximální počet landmarků, které mohou být tímto způsobem reprezentovány na několik stovek. V reálných prostředích je však často možné rozpoznávat až miliony příznaků. Druhý významný problém EKF-SLAM algoritmů spočívá v přísném předpokladu, že je známa korespondence mezi pozorovanými daty a landmarky v mapě (problém asociace měřených dat). Přiřazení i malého počtu chybných pozorování k nesprávným landmarkům může způsobit, že filtr nebude konvergovat.

6.3. SIMULTÁNNÍ LOKALIZACE A MAPOVÁNÍ

49

Autoři (Thrun et al., 2002) využili empirického pozorování, že v případě udržování vzájemných korelací pouze mezi blízkými landmarky je vhodné převést formalismus kalmanské filtrace do formalismu informačního filtru (místo udržování odhadů přes kovarianční matici Σ se pracuje s informační maticí Σ−1 ). Normalizovaná informační matice obsahuje mnoho prvků blízkých nule, které představují nepříliš těsnou vazbu mezi vzdálenými landmarky. Na základě toho navrhli algoritmus řídkého informačního filtru SEIF (Sparse Extended Information Filter), který je schopen v případě SLAM algoritmu provádět aktualizace v konstantním čase.

6.3.2

Algoritmus FastSLAM

Klíčový problém s velkým počtem landmarků při odhadech posteriorních distribucí pozic robotu a landmarků se autoři (Montemerlo et al., 2002) snaží řešit přesnou faktorizací, která obecný SLAM problém dekomponuje na problém odhadu posteriorních pravděpodobností trajektorií robotu v čase (x1:t ) a problém K odhadů pozicí landmarků, které jsou podmíněny odhady trajektorií robotu. Základní myšlenka tohoto nového přístupu, který je autory nazýván FastSLAM, vychází z prací (Murphy a Russel, 2001), kde je představena technika Rao-Blackwellized particle filtrů pro faktorizaci posteriorních podmíněných distribucí pravděpodobnosti s velmi velkým počtem proměnných. Klíčovou myšlenkou aplikace Rao-Blackwellized particle filtrů pro účely simultánní lokalizace a mapování je následující faktorizace: p(x1:t , θ | z1:t , u1:t , n1:t ) = p(x1:t | z1:t , u1:t , n1:t )

Y

p(θk | x1:t , z1:t , u1:t , n1:t ),

(6.25)

k

kde x1:t značí trajektorii robotu, resp. pozice robotu v časovém intervalu h0; ti, θ jsou pozice landmarků, z1:t jsou senzorická měření, u1:t je odometrie robotu a n1:t značí známé korespondence landmarků (jejich očíslování). FastSLAM algoritmus používá modifikovaný particle filtr pro odhad posteriorních distribucí p(x1:t |z1:t , u0:t−1 , n1:n ) přes odhady trajektorií robotu. Odhady trajektorie lze označit [r] [r] [r] [r] Xt = {x1:n }r = {x1 , x2 , ..., xt }r , (6.26) kde notace [r] značí r-tý particle v množině Xt , která je vypočítávána inkrementálně z předcházející množiny Xt−1 v čase t − 1, odometrie u1:t a měření z1:t . [r] Každý dílčí particle x1:n ∈ Xt , který je použit ke generování pravděpodobnostního odhadu pozice robotu v čase t [r]

[r]

x1:t ∼ p(x1:t | u1:t , xt−1 )

(6.27)

je vzorkován z pravděpodobnostního pohybového modelu robotu. Tento odhad je ná[r] sledně přidán k dočasné množině particlů v rámci trajektorie xt−1 . Za předpokladu, že množina particlů v množině X1:t−1 je distribuována podle p(x1:t−1 |z1:t−1 , u1:t−1 , n1:t−1 ), což je asymptoticky korektní aproximace, nový particle je distribuován podle p(x1:t |z1:t−1 , u1:t , n1:t−1 ).

50

KAPITOLA 6. STAV PROBLEMATIKY VE SVĚTĚ

Po vygenerování R particlů výše uvedeným způsobem je získána nová množina X1:t [r] vzorkováním dočasné množiny particlů. Každý particle x1:t je vybírán s váhovým faktorem r wt , který je roven: [r] p(x1:t | z1:t , u1:t , n1:t ) wtr = . (6.28) [r] p(x1:t | z1:t−1 , u1:t , n1:t−1 ) Hustota pravděpodobnosti p(θk | x1:t , z1:t , u1:t , n1:t ) ve faktorizaci 6.25 představuje trajektoriemi podmíněný odhad pozicí landmarků pomocí K-Kalmanových filtrů. Ke každému particlu reprezentující možnou pozici robotu v čase t přísluší přiřazené Kalmanovy filtry udržující distribuci pozic landmarků. V podstatě lze shrnout, že plnou posteriorní hustotu pravděpodobnosti přes trajektorii robotu a pozice landmarků lze vyjádřit jako množinu vzorků [r] [r] [r] [r] [r] Xt = {x1:n , µ1 , Σ1 , ... , µK , ΣK }r , (6.29) [r]

[r]

kde dvouprvkový vektor µk a matice Σk o dimenzi 2x2 (pro případ planární reprezentace pohybu robotu ve 2D prostředí) jsou střední hodnoty a kovariance Gaussiánů reprezentující odhady k−tého landmarku příslušející k m−tému particlu. Výpočet posteriorních hustot pak závisí na tom, zda nt = k či ne. V případě, že nt = k, pak získáme: p(θk | x1:t , z1:t , u1:t , t1:t ) ≈ p(zt | θk , xt , nt ) p(θk | x1:t−1 , z1:t−1 , u1:t−1 , n1:t−1 ),

(6.30)

v opačném případě nt 6= k zůstanou Gaussiány nezměněny p(θk | x1:t , z1:t , u1:t , t1:t ) = p(θk | x1:t−1 , z1:t−1 , u1:t−1 , n1:t−1 ).

(6.31)

FastSLAM algoritmus implementuje aktualizaci vztahu pomocí rozšířeného Kalmanova filtru, více detailů lze nalézt v (Montemerlo a kol., 2002) nebo v (Smith a kol., 1990). Výrazná výhoda FastSLAM algoritmů v porovnání s klasickým SLAMem (Smith a kol., 1990) je, že se aktualizuje pouze K dvojdimenzionálních Gaussiánů oproti jednomu (2K + 3) dimenzionálnímu Gaussiánu (K landmarků a 3 složky pozice robotu). Přímá implementace algoritmu vede na řešení s časovou složitostí O(M K), kde M je počet particlů v particle filtru a K je počet landmarků. Autoři (Montemerlo a kol., 2002) však navíc vyvinuli stromově orientovanou datovou strukturu, která redukuje časovou složitost na O(M log(K)), což dále výrazně překonává SLAM algoritmy založené na čistém Kalmanově filtru. V další práci se autoři (Montemerlo a Thrun, 2003) zabývají problematikou a neznámého počtu landmarků spolu s neznámou asociací naměřených dat. Metodu založenou na Rao-Blackwellized particle filtrech použili též autoři (Hähnel et al., 2003), ovšem pro reprezentaci modelu prostředí použili místo landmarkové reprezentace mřížkový model. Navíc autoři řeší i fundamentální problém simultánní lokalizace a mapování, kterým je korektní uzavírání cyklických smyček, pokud se robot vrátí do části prostoru, které již dříve mapoval. Základní myšlenka tohoto přístupu je odhad posteriorní pravděpodobnosti p(x1:t |z1:t , u0:t−1 ) potenciálních trajektorií robotu x1:t danou jeho pozorováními z1:t a odometrií u1:t pro výpočet posteriorních pravděpodobností přes všechny mapy a trajektorie robotu: p(x1:t , m | z1:t , u0:t−1 ) = p(m | x1:t , z1:t )p(x1:t | z1:t , u0:t−1 ). (6.32)

6.3. SIMULTÁNNÍ LOKALIZACE A MAPOVÁNÍ

51

Výpočet může být proveden poměrně efektivně, protože hodnoty p(m | x1:t , z1:t , u0:t−1 ) mohou být vypočítány analyticky jakmile x1:t a z1:t jsou známy. Pro odhad posteriorní pravděpodobnosti p(x1:t |z1:t , u0:t−1 ) přes potenciální trajektorie FastSLAM algoritmus používá particle filtr, ve kterém je každému vzorku přiřazena individuální mřížková mapa vybudovaná na základě pozorování z1:t a trajektorie x1:t reprezentovaných příslušným particlem. Během převzorkování, váhové faktory wt každého particlu jsou úměrné hodnotám pravděpodobnosti p(zt | m, xt ) nejaktuálnějšího pozorování dané mapy m přiřazené danému particlu a pozici xt příslušné trajektorie. bt−1 a odhad V kterémkoliv okamžiku, v čase t − 1, je k dispozici odhad pozice robotu x b x b1:t−1 , z1:t−1 ). Jakmile se robot přesune do nové pozice a získá nové příslušné mapy m( měření zt , robot určí nejpravděpodobnější novou pozici jako: b x b1:t−1 , z1:t−1 )) p(xt | ut−1 , x b1:t−1 ).} bt = arg max{p(zt ) | xt , m( x xt

(6.33)

Postup autorů (Hähnel a kol., 2003) pro uzavírání smyček využívá techniky scanmatching, která přímo vychází z práce (Hähnel et al., 2002). Problematika uzavírání smyček vedla autora (Stachniss et al., 2004) k představení nového pojmu aktivní lokalizace, který do problematiky simultánní lokalizace a mapování přidává další dimenzi problému, kterou je řízení pohybu robotu. Tímto vzniká trojice provázaných úloh lokalizace, mapování a řízení pohybu jak ukazuje obr. 6.9. Souběžná lokalizace a mapování

Mapování

Průzkum prostředí

SLAM

Řízení pohybu

Lokalizace

Integrované přístupy

Aktivní lokalizace

Obr. 6.9: Vazby úloh lokalizace, mapování a řízení pohybu podle (Stachniss a kol., 2004) Vedle úlohy SLAM byla tak vhodně vymezena úloha automatického průzkumu prostředí, ve které je aktivně ovlivňováno plánování trajektorií mobilního robotu s ohledem na zmapování všech dosud neznámých částí prostoru. I při řešení této úlohy je třeba udržet stále konzistentní informaci o poloze mobilního robotu. Varianta FastSLAM algoritmus pracující na základě rekurzivního odhadu plných posteriorních hustot pravděpodobnosti pozice robotu a pozic landmarků je popsán v práci (Nieto a kol., 2003). Autoři v rámci FastSLAM algoritmu řeší problém neznámé asociace dat pomocí techniky nejbližších sousedů, s jejíž využitím implementují vícehypotézové sledování nejistoty v datech během jejich asociace.

52

KAPITOLA 6. STAV PROBLEMATIKY VE SVĚTĚ

(Dailey a Parnichkun, 2005) ukazuje, že principy FastSLAM algoritmů (Thrun a kol., 2004), (Montemerlo a Thrun, 2003), které byly původně navrženy jen pro 2D lokalizaci jsou použitelné i pro 3D vizuální SLAM v plných šesti stupních volnosti, kde jsou linie v prostoru použity jako landmarky. Bohužel, autoři (Dailey a Parnichkun, 2005) uvádějí, že současná implementace pracuje příliš pomalu, což přičítají dvojnásobnému počtu stupňů volnosti v porovnání s originálním FastSLAM algoritmem a nepříliš vhodné 3D reprezentaci linií ve vyhledávací struktuře.

6.3.3

Ostatní přístupy

Poměrně netradiční přístup k problému simultánní lokalizace a mapování použil autor (Duckett, 2003). Problém simultánní lokalizace a mapování je definován jako globální optimalizační problém, kde předmětem řešení je prohledávání komplexního stavového prostoru všech možných robotických map. Pro hledání optimálního řešení jsou využity genetické algoritmy se speciální heuristickou hodnotící funkcí, která vyjadřuje míru konzistentnosti a kompaktnosti kandidátských map. Při použití genetických algoritmů je vedle návrhu hodnotící funkce též velmi důležité vhodné zakódování optimalizovaných veličin do tzv. chromozómů a nedefinování operátorů výběru, mutace a křížení. Každý chromozóm je kódován jako řetězec čísel s plovoucí desetinou čárkou reprezentující korekční faktory odometrických dat, kde trajektorie odometrických dat je dělena na třímetrové úseky, které jsou pak dále děleny do segmentů. Pro každý segment k pak chromozóm obsahuje dvojici čísel −δmax ≤ ∆δk ≤ +δmax a −αmax ≤ ∆αk ≤ +αmax , která kóduje faktory αk a δk aplikované pro korekci ujeté vzdálenosti a úhlu natočení. Hodnotící funkce používá zjednodušenou verzi mřížek obsazenosti (Moravec a Elfes, P 1985), je formulována jako F = M C1 + wM C2 , kde M C1 = i min(occi , empi ) vyjadřuje konzistenci obsazených (occi ) a prázdných (empi ) buňek a kde M C2 = (xmax − xmin ) × (ymax − ymin ) vyjadřuje kompaktnost vytvářené mapy, tj. oblast pokrytou obsazenými buňkami. Dalším přístupem využívající k prohledávání stavového prostoru pozic robotu evoluční výpočetní koncepty je práce (Moreno a kol., 2006). Množina možných poloh robotu, nazývaná populace, reprezentuje nejpravděpodobnější oblasti, kde se robot může nacházet v kontextu vnímání vnějšího světa a dostupné informace o jeho vlastním pohybu. Vývoj populace je založen na sledování chyby pohybu odvozené od porovnání pozorovaných a predikovaných dat získaných z pravděpodobnostního senzorického a pohybového modelu.

Kapitola 7

Point-to-Point lokalizace Tato kapitola se bude do hloubky zabývat návrhem, popisem a vlastnostmi lokalizační metody nazvané Point-To-Point (dále již jen zkráceně PTP), která zajistí dlouhodobé přesné určení polohy robotu ve vnitřním prostoru z laserových senzorů. Globální motivací použití této metody je zajištění stabilní a robustní navigace jednotlivých robotů ve společném týmu robotů pro společné plnění zadaných úloh v neznámém prostředí, tj. bez existence předem známé mapy prostředí, což vede na použití metody z kategorie metod pro sledování polohy. Předchozí kapitola 6.1 se zabývala rozborem vlastností existujících metod lokalizace, ze kterých vlastní návrh nové lokalizační metody vychází. Základními požadavky na návrh metody je přesnost, rychlost a robustnost, tedy nezávislost na aktuální konfiguraci prostředí a jiných vnějších podmínek. Existující metody se liší druhem zpracovávaných dat, jejich interní reprezentací a stupněm abstrakce. Reálná použitelnost metod založených na Kalmanových filtrech je limitována na typy prostředí, ze kterých je možná spolehlivá extrakce vhodných příznaků (landmarků) pro následné hledání korespondencí geometrických primitiv. Hledání korespondencí se však stává tím těžším, čím je větší odchylka aktuální pozice od předpokládané pozice. Zásadní nevýhodou těchto algoritmů je jejich selhávání v obecných prostředích, které nesplní požadavky na extrakci primitiv a jejich vzájemných korespondencí. Cestou ke zvýšení robustnosti je zvyšování počtu použitelných landmarků - primitiv. Obecně platí, že pro zjištění změny polohy a orientace robotu v rovině mezi pozicemi A a B stačí znát spolehlivě relativní souřadnice dvou objektů bodů v prostoru vůči poloze A i B robotu. Pro výběr dvou či několika málo význačných bodů je však potřeba vysoký stupeň abstrakce, aby se ze scény vybraly opravdu ty nejrelevantnější body, jejichž polohu lze získat s maximální možnou přesností. Toto s sebou nese požadavky na omezení vhodných konfigurací prostředí. Se snižující se mírou abstrakce a přesností detekce polohy primitiv je třeba zvyšovat jejich počet, aby se kompenzoval a eliminoval vliv potenciálně chybně detekovaných primitiv či jejich korespondencí. V extrémním případě se tento přístup blíží k metodám typu scan-matching, kdy se s daty pracuje na poměrně nízké úrovni abstrakce, ovšem primitivy se stávají buď přímo veškerá senzorická měření (body scanu) nebo jejich jednoduché segmentace. Touto problematikou se do hloubky zabývala úspěšně obhájená disertační práce (Kulich, 2003), kterou v experimentální části bereme jako jistou referenci, 53

54

KAPITOLA 7. POINT-TO-POINT LOKALIZACE

jejíž výsledky se budeme snažit zdokonalit. Metoda pracuje s liniovými primitivy extrahovanými přímo z laserových dat a poskytuje poměrně slibné výsledky. Ovšem je tomu tak pouze ve velmi omezené třídě prostředí, ze kterých lze extrahovat liniové segmenty a spolehlivě nalézt jejich korespondence. Bohužel, pro řadu reálných prostředí tato metoda selhává, neboť ne vždy jsou data ze dvou následujících scanů segmentovatelná na sadu vzájemně si korespondujících podmnožin úseček (Kulich a kol., 2001). Další perspektivní skupinou metod založených na pravděpodobnostních přístupech jsou Markovské lokalizační algoritmy využívající particle filtry. Jejich činnost však principiálně vychází z již existujícího modelu světa, což lze považovat jako daň za to, že algoritmus umožňuje vedle úlohy sledování pozice řešit i úlohu globální lokalizace. Nevýhodou je, že model často neexistuje nebo je velmi nepřesný a tudíž samotný proces lokalizace musí být nutně doprovázen tvorbou modelu, vůči kterému se lokalizace provádí. Metody této třídy přístupů pracují zpravidla s mřížkovým modelem prostředí, což má za následek implementační potíže pro využití této metody pro rozsáhlé prostory. Paměťová složitost roste kvadraticky s poloměrem prostoru, výpočetní složitost se s narůstajícím objemem dat též zvyšuje. Tyto nároky jsou pro některé aplikace založené zejména na embedded systémech již příliš vysoké. Pokud vezmeme v úvahu požadavky na vlastnosti požadované metody kontinuální lokalizace, vychází nám jako výhodný postup založený na přímé registraci (sesazování) scanů. Algoritmus by měl být velmi univerzální, pokud možno nezávislý na konfiguraci pracovního prostředí a proto se budeme snažit celý algoritmus ponechat z hlediska zpracování dat na relativně nízkém stupni abstrakce. Pro tento účel jsou vhodné postupy založené na korelačních principech a ICP algoritmu, který se budeme snažit zdokonalit a propojit s dalšími novými přístupy k problému. Stávající algoritmy umožňují registrovat jednotlivé scany, my provedeme jejich zdokonalení a následně i zaintegrování těchto postupů do procesu tvorby modelu prostředí. Propojení lokalizace s procesem tvorby modelu prostředí bude popsáno v následující kap. 8 a povede k dalšímu zvýšení přesnosti a robustnosti celé metody. Během vývoje nové metody je brán na zřetel požadavek rozšiřitelnosti lokalizační metody do třetí dimenze. To umožní v budoucnu využití výsledků nejen pro roboty pohybující se v rovinném vnitřním prostředí budov, ale též pro venkovní roboty pracující v terénu, kde je nutné robot lokalizovat v šesti stupních volnosti.

7.1

Předzpracovaní dat

Základním krokem před vlastním procesem lokalizace robotu je předzpracování vstupních dat z laserového scanneru. Ukázalo se, že v rámci předzpracování je potřeba řešit dva nejvýznamnější jevy: • filtraci odlehlých měření, • odstranění falešných bodů, které vznikají mezi předmětem v pozadí a hranou překážky v popředí.

7.1. PŘEDZPRACOVANÍ DAT

55

Laserový scanner má přirozeně omezený měřicí dosah, v případě, že je překážka dále než je měřicí dosah, naměřená vzdálenost je obvykle rovna maximu měřicího rozsahu. Z toho vyplývá, že hodnoty, které odpovídají tomuto maximu, musí být z naměřeného scanu vyloučeny. Laserový scan pak tedy neobsahuje rovnoměrně úhlově rozdělené vzdálenosti ve formě vějíře paprsků, vybrané paprsky, resp. jemu odpovídající naměřené body jsou vyloučeny z dalšího zpracování. Druhým, vážnějším problémem, který je potřeba korigovat, je nežádoucí jev nazývaný „mixed-pixelÿ (Tuley et al., 2004), který je přirozenou vlastností laserových scannerů SICK, ale pravděpodobně i dalších senzorů. Tento jev se projevuje chybnými měřeními, které jsou způsobeny neinfinitezimální tloušťkou laserového paprsku ve směru jeho rozmítání. Efektivní řez paprskem připomíná spíše úsečku než bod, což má vliv na stabilitu výsledného měření vzdálenosti z prostoru za hranou. Chyby měření se v praxi projevují nestabilitou odrazu na hranách překážek a následným vznikem falešných bodů, v literatuře nazývanými jako mixed-pixel . Mixedpixely se nacházejí v prostoru mezi pozadím a hranou předmětu stojícím v popředí, situaci dokresluje obrázek 7.1.

Laser

Laser

Obr. 7.1: Jev ”mixed-pixel” u laseru SICK, zeleně jsou označené korektně změřené body, červeně pak falešně naměřené body vlivem jevu mixed-pixel. Příčina tohoto jevu je způsobena tím, že část paprsku se odráží ještě od hrany v popředí a část paprsku se odráží již od překážky v pozadí. Vyhodnocovací elektronika laseru pak naměřenou vzdálenost vyhodnotí jako hodnotu ležící někde mezi předním a zadním odrazem. Výsledek reálného měření je zobrazen na obr. 7.2. Data byla sejmuta během rovnoměrné jízdy robotu podél osy y ze vzdálenosti 5.2 m - 2.5 m od horní linie obrazu prostředí. Překážku v popředí představuje 5 dřevěných hranolů o průřezu 5x3 cm postavených kolmo k rovině podlahy a s orientací větší plochy kolmo na směr pohledu robotu (viz. obr. 10.4). V prostoru mezi pozadím a hranou předmětu v popředí se typicky objeví 1-2 mixedpixely. Více něž jeden bod se objevuje díky pohybu robotu během snímací procedury. Četnost výskytu mixed-pixel jevu je přímo úměrně závislá na vzdálenosti mezi hranou a pozadím, neboť zvyšující se vzdálenost mění poměr mezi intenzitou odrazu od popředí a pozadí. Experimentálně zjištěný vliv zvyšující se vzdálenosti hrany překážky od pozadí je patrný z obr. 10.5 v kap. 10.3.

56

KAPITOLA 7. POINT-TO-POINT LOKALIZACE

Obr. 7.2: Naměřené body ze 140 scanů sejmutých během jízdy robotu podél osy y.

Filtrace mixed-pixelů je velmi důležitá pro korektní činnost lokalizačních i mapovacích algoritmů, přestože autoři tento problém často opomíjejí. Vynechání procesu filtrace má dopad na přesnost lokalizace a následně i na kvalitu vytvářené mapy prostředí. V případě, že by se takto vytvořená mapa použila pro plánovací algoritmy, plánovací algoritmus by též selhával, nevyužíval by veškerého dostupného volného prostoru. Tyto důvody nás vedou k tomu se problematikou podrobně zabývat. Autoři (Tuley a kol., 2004) řešili problematiku filtrování mixed-bodů v kontextu snímání scény naklápěným scannerem pro účely získání 3D popisu scény. Naším cílem bude navrhnout způsob, jak filtrovat body v rámci osamoceného scanu bez ohledu na ostatní scany získané z nepatrně odlišné pozice a v jiném čase. Tato vlastnost je důležitá zejména v procesu lokalizace, kdy je třeba pracovat s každým scanem osamoceně bez ohledu na minulé či budoucí scany. Data jednoho typického laserového scanu obsahující významné množství mixed-pixelů je zobrazen na obr. 7.3.

Obr. 7.3: Data z jednoho scanu s výraznými mixed-pixely

7.1. PŘEDZPRACOVANÍ DAT

57

Filtrace mixed-pixelů je ve své podstatě klasifikátor, který vstupní data (body) zařazuje do dvou tříd, označme si je jako negativní a pozitivní třídu. V ideálním případě by se do pozitivní třídy měly klasifikovat všechny body vzniklé na základě mixed-pixel fenoménu a do negativní třídy by se měly klasifikovat všechny ostatní body, které odpovídají reálné poloze překážek v prostoru. Je zřejmé, že počty prvků příslušejících do příslušných tříd budou velmi nerovnoměrné. Naší přirozenou snahou je klasifikovat co největší počet bodů z reálně volného prostoru do pozitivní třídy a co největší počet bodů odpovídající reálným překážkám do negativní třídy. Zároveň s tím se snažíme minimalizovat chybu klasifikace, což znamená, že do pozitivní třídy bude zařazen bod odpovídající překážce nebo že do negativní třídy bude zařazen bod z volného prostoru. Z formálního hlediska si zaveďme symboliku pro celkový počet klasifikovaných bodů Nc = tpos + f pos + tneg + f neg, kde dílčí počty bodů mají ve vztahu ke klasifikaci následující význam: tpos f pos tneg f neg

(true positive) (false positive) (true negative) (false negative)

... ... ... ...

počet počet počet počet

bodů bodů bodů bodů

z volného prostoru kl. do pozitivní třídy od překážky v prostoru kl. do pozitivní třídy od překážky v prostoru kl. do negativní třídy z volného prostoru kl. do negativní třídy

Z hlediska kvality klasifikátoru se tedy snažíme maximalizovat míru správné pozitivní klasifikace (true positive rate) spolu s minimalizací míry chybné pozitivní klasifikace (false positive rate). Míra správné pozitivní klasifikace tpr představuje procento nalezených mixed-pixelů, jmenovatel je skutečný počet všech bodů ležících mimo reálné překážky, tedy počet bodů, které by klasifikátor měl ideálně zařadit do pozitivní třídy. tpr =

tpos tpos + f neg

(7.1)

Míra chybné pozitivní klasifikace f pr ukazuje, jaká část bodů, které skutečně náleží obrazu překážky, bude klasifikátorem označena jako mixed-pixel. f pr =

f pos f pos + tneg

(7.2)

Výsledná přesnost klasifikace je pak dána mírou acc, nutno však podotknout, že vzhledem k velmi nerovnoměrnému zastoupení počtu bodů z obou tříd jsou mnohem více vypovídající hodnoty tpr a f pr. tpos + tneg acc = (7.3) Nc Návrh samotného klasifikátoru využívá znalosti, za jakých okolností mixed-pixely vznikají, kde se mohou nacházet a jaké vzájemné konfigurace bodů pravděpodobně vylučují výskyt mixed-pixelů. Algoritmus se skládá ze dvou po sobě následujících kroků. Úkolem prvního kroku je maximalizovat tpr i za cenu, že se jako mixed-pixel označí i body, které do pozitivní třídy

58

KAPITOLA 7. POINT-TO-POINT LOKALIZACE

klasifikace nepatří. Tímto získáme všechny body zájmu „podezřeléÿ z pozitivní klasifikace a zároveň zmenšením počtu bodů urychlíme druhý krok algoritmu, který pak použije složitější kritéria pro klasifikaci. Nutnou, nikoliv však postačující podmínkou pro pozitivní klasifikaci bodu je, aby ležel v prostoru mezi pozadím a překážkou v prostoru. To znamená, že daný bod může být pozitivně klasifikován, pokud bude ležet poblíž přímky p procházející středem laserového scanneru a hranou překážky ve vzdálenosti menší než je vzdálenost pozadí a větší než vzdálenost popředí. Dále ve směru této přímky musí být vzdálenost dvou sousedních bodů větší než předem stanovená mez mindist . Pokud však existuje soused ve vzdálenosti menší než mindist , jako soused se uvažuje až další nejbližší bod. Tato dodatečná podmínka zohledňuje skutečnost, že během snímání a současného pohybu robotu ve volném prostoru mohou vzniknout i dva velmi blízké mixed-pixely. První krok metody využívá skutečnosti, že scan obsahuje úhlově uspořádané body v polárních souřadnicích {[ϕ1 , ρ1 ], [ϕ2 , ρ2 ], . . . , [ϕn , ρn ]}, pro které platí ϕ1 < ϕ2 < . . . < ϕn . Uspořádanost bodů zaručuje, že sousední body ve scanu automaticky leží v blízkosti potenciální přímky p. V algoritmu 7.1 pak stačí pracovat se vzdálenostmi ρ1 . . . ρn . Řádky 6-11 algoritmu 7.1 zajišťují, že pokud se ve volném prostoru vyskytuje dvojice blízkých bodů, tak tato dvojice neovlivní budoucí výběr kandidátů podle kritéria překročení min. vzdálenosti mindist . Následující řádky 12-21 algoritmu 7.1 provádějí vlastní výběr kandidátů. Všechny podmínky jsou zdvojeny pro d+ a d− kvůli symetrii průchodu laserových dat po směru a proti směru hodinových ručiček. Parametr algoritmu, mezní vzdálenost bodů mindist , se odvíjí od přesnosti senzoru, je třeba, aby byl větší než je rozlišení a přesnost senzoru, ovšem ne více jak o jeden řád. Se zvyšující se hodnotou mindist klesá detekční schopnost algoritmu, na druhé straně snižující se hodnota mindist způsobuje falešné pozitivní klasifikace bodů. Druhým krokem metody je vyloučení určitých bodů pozitivně klasifikovaných prvním krokem metody. Z principu činnosti prvního kroku algoritmu totiž vyplývá, že do pozitivní třídy jsou klasifikovány i některé body, které by do pozitivní třídy neměly být klasifikovány. Tyto body typicky leží ve větší vzdálenosti od senzoru a odpovídají obrazu části překážek mající směr své hrany přibližně podobný se směrem dopadu laserového paprsku (do 30o ). Podél takovýchto hran se totiž vzdálenosti bodů od senzoru poměrně rychle mění. Do druhého kroku metody vstupují jen pozitivně klasifikované body z prvního kroku spolu s jejich 2(N −1) sousedy ze vstupní sekvence scanu. Sousední body jsou rozděleny tak, že (N − 1) bodů předchází danému pozitivně klasifikovanému bodu zájmu a (N − 1) bodů následuje. Každý pozitivně klasifikovaný bod je validován dvěma kritérii, které pracují s body v lokálním okolí daného bodu. Validace se provádí v posuvném okně přes úhlově uspořádanou posloupnost bodů scanu o velikosti N . Validovaný bod je vždy součástí tohoto klouzavého okna. Dvě validační kritéria jsou: • příslušnost bodu k liniovému segmentu (rozptyl bodů v kolmém průmětu na proklá2 (p )) dající přímku menší než (σlin i 2 (p ). • rozptyl euklidovské vzdálenosti bodů v kartézské souřadné soust. menší než σdist i

7.1. PŘEDZPRACOVANÍ DAT

59

Algoritmus 7.1: Výběr kandidátů pro pozitivní klasifikaci Vstup: posloupnost vzdáleností D = {di | i = h1; ni}, mezní vzdálenost bodů mindist Výstup: množina kandidátů K ve formě indexů do posloupnosti D 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22

K ← [] for i ← 1 to n do d− ← di−1 d+ ← di+1 kandi ← false if |di − d− | < mindist and i > 2 then d− ← di−2 end if |di − d+ | < mindist and i <= n − 2 then d+ ← di+2 end if d− < d+ then if di − d− > mindist and d+ − di > mindist and di < d+ then Přidej i do množiny K end end else if di − d+ > mindist and d− − di > mindist and di < d− then Přidej i do množiny K end end end

Aby bod pi (bod zájmu) mohl zůstat v pozitivní třídě klasifikace z prvního kroku, musí splnit obě výše uvedená kritéria. Příslušnost bodu pi k liniovému segmentu se určuje na základě vyhodnocení rozptylů kolmých vzdáleností bodů od přímky, kterou jsou proloženy body v okolí validovaného bodu. Vypočítávají se vždy rozptyly pro každé vybrané klouzavé okno o velikosti N a jako rozhodující se bere minimální hodnota z N hodnot. Bod pi je prohlášen jako příslušník 2 podle algoritmu liniového segmentu, pokud je minimální rozptyl menší než daná mez σlin 7.2. Kritérium příslušnosti bodu k liniovému segmentu by však samo o sobě z třídy pozitivní klasifikace vyjmulo i řadu bodů, které jsou ve volném prostoru a skutečně odpovídají mixed-pixelům. Bohužel, tyto body často spolu s krajními body od překážky v popředí a body odpovídající pozadí mohou vytvořit falešný liniový segment, který však neodpovídá reálné překážce. Tento segment má však velmi velký rozptyl vzdáleností mezi body, kterým budeme vyjmutí bodu z pozitivní klasifikace podmiňovat.

60

KAPITOLA 7. POINT-TO-POINT LOKALIZACE

Algoritmus 7.2: Příslušnost bodu k liniovému segmentu Vstup: pole bodů P = [p1 , p2 , . . . , pn ], kde pi = [xi , yi ], velikost klouzavého 2 okna N , index i bodu zájmu pi , min. rozptyl σlin Výstup: true/false - bod náleží/nenáleží liniovému segmentu 1 for n ← 1 to N do 2 start ← i − n 3 end ← start + N − 1 4 if start >= 1 and end <= N then 5 cov ← Vypočti kovarianční matici z bodů pstart . . . pend 6 λ ← Vypočti vlastní čísla matice cov 7 λn ← min(λ) 8 end 9 else 2 λn ← σlin 10 11 end 2 ) 12 return (min(λi ) <= σlin 13 end

Z tohoto důvodu jsme navrhli druhé kritérium vyhodnocující rozptyl euklidovské vzdálenosti ve skupině N bodů, které se nacházejí v okolí bodu zájmu pi . Okolím je myšleno (N − 1)/2 bodů předcházejících a (N − 1)/2 bodů následujících po bodu pi v posloupnosti bodů tak, jak byly sejmuty laserovým scannerem. i+(N −1)/2−1

2 σdist (pi ) =

kde |pa , pb | =



i+(N −1)/2−1

2

1 1 |pj , pj+1 | − |pk , pk+1 | (7.4) N − 2 j=i−(N −1)/2 N − 1 k=i−(N −1)/2 X

q

(xa − xb )2 + (ya − yb )2

X

(7.5)

2 (p ) vzdáleností sousedních bodů provedeme podle vztahu 7.4, Výpočet rozptylu σdist i kde N je (lichý) počet bodů, mezi nimiž se vypočítává rozptyl vzdáleností, i je jeho index v bodové posloupnosti scanu, xa , ya xb , yb jsou kartézské souřadnice bodu a resp. bodu b.

Shrneme-li všechny parametry, které definují chování algoritmů pro předzpracování 2 a σ 2 . Parametr min dat, dostáváme celkem čtyři: mindist , N , σlin dist je nutné vhodně dist nastavit pro každý příslušný typ senzoru, měl by odpovídat přibližně dvounásobku až 2 a σ2 trojnásobku přesnosti použitého senzoru. Zbylé parametry N , σlin dist je vhodné určit experimentálně, jejich velikost lze hledat odděleně, nemají mezi sebou příliš velkou vazbu. Způsob jejich nastavení je popsán v kap. 10.3.

7.2. SEGMENTACE DAT

7.2

61

Segmentace dat

Účelem segmentace dat je rozdělení laserových dat (typicky již převedených do kartézské souř. soust.) do skupin bodů, které mohou být s nepříliš velkou chybou aproximovány liniovým segmentem, úsečkou. Tato aproximace je důležitý krok zejména pro část lokalizačního algoritmu popsaného v kap. 7.3.1. K segmentaci bodů v rovině do množiny úseček lze přistupovat v zásadě dvěma způsoby, sekvenčně nebo dávkově. Sekvenční přístupy nepotřebují celou skupinu bodů najednou, body zpracovávají postupně tak, jak přicházejí např. z laserového scanneru. Takto s daty zacházejí např. autoři (Castellanos a Tardós, 1999), kde jsou příchozí body zpracovávány rozšířených informačním filtrem. Postupně body zpracovává i (Yuen a MacDonald, 2003), kde se příchozí body přidávají do lin. segmentu tak dlouho, dokud průměrná chyba čtverců vzdálenosti bodů od prokládající přímky nepřekročí stanovený práh. Podobně pracují algoritmy SEF (Succesive Edge Following) nebo LT (Line Tracking), (Vandorpe a kol., 1996), ovšem využívají přímo naměřených dat v polárních souřadnicích. Segment je ukončen v okamžiku, kdy naměřené vzdálenosti mezi následujícími body překročí definovaný práh. Druhá skupina dávkově pracujících metod využívá ke své činnosti najednou všechna dostupná data, bývají tudíž přesnější a robustnější. Často jsou tyto postupy inspirovány metodami pro zpracování obrazu. Klasickým zástupcem této skupinu metod je metoda IEPF (Iterative End Point Fit), (Duda a Hart, 1973). Tento algoritmus využívá faktu, že data z laserového scanneru jsou přirozeně úhlově uspořádané, pracuje se s nimi však v kartézských souřadnicích. Vstupem algoritmu je posloupnost bodů P = {[x1 , y1 ], . . . , [xn−1 , yn−1 ], [xn , yn ]}, kterou algoritmus rekurzivně rozděluje na množiny P 0 = {[x1 , y1 ], . . . , [xa , ya ]} a P 00 = {[xa , ya ], . . . , [xn , yn ]} (viz. obr. 7.5) pokud není splněno validační kritérium pro posloupnost P. Validační kritériem je maximální vzdálenost da bodu pa = [xa ; ya ] ∈ P od segmentu formovaným posloupností bodů P. Validační kritérium je v podstatě i zastavující podmínkou rekurze, pokud platí da < dmax , na obě vzniklé posloupnosti P 0 i P 00 se aplikuje stejný postup jako na P. Ze základního algoritmu IEPF vycházejí další postupy autorů (Borges a Aldon, 2000), kteří jeho vlastnosti vylepšují přidáním prototypově orientovaných fuzzy slučovacích pravidel pro lepší rozdělování segmentů. pa

P’’

P

P’

Obr. 7.4: Množina bodů před rozdělením

Obr. 7.5: Množina bodů po rozdělení

Principiální problém, kterým trpí metody založené na algoritmu IEPF je jeho neschopnost rozlišit, zda segmentované úsečky skutečně přísluší obrazu reálné hrany v prostředí, nebo volnému prostoru. Na obr. 7.7 je zobrazeno, jak při segmentaci algoritmem IEPF

62

KAPITOLA 7. POINT-TO-POINT LOKALIZACE

(a)

(b)

(c)

Obr. 7.6: Postup rekurzivní segmentace úseček algoritmu Iterative End Point Fit: a) Začátek štěpení b) Štěpení přes nejvzdálenější bod c) Výsledek segmentace

vznikne falešná okluzní hrana spojením krajního bodu překážky s pozadím. Algoritmus IEPF navíc vysegmentuje i další nespolehlivé hrany, které je třeba vyloučit z dalšího zpracování. Laser

Ponechané úsečky Odstraněné úsečky

a) c)

Laser Směr las.paprsku

d)

h) b)

Korektně segmentované hrany Okluze segmentovaná jako falešná hrana

Obr. 7.7: Vznik okluzní hrany

f)

a) e)

g)

Obr. 7.8: Příklady úseček, určené k odstranění

Z tohoto důvodu jsme navrhli proces filtrace nežádoucích hran, který nežádoucí hrany odstraní na základě splnění některého z heuristických pravidel: 1. Nevýznamnost. Segment je tvořen méně než 5 body. 2. Malý zorný úhel. Úhlový rozdíl průvodičů koncových bodů segmentu je menší než 10◦ , typické pro segmenty vzniklé okluzí, které jsou téměř kolineární se směrem laserového paprsku. 3. Mezera mezi body. Rozptyl bodů podél segmentu je příliš velký, v segmentu existují dvojice bodů, které mají vzájemnou vzdálenost výrazně větší než ostatní. Uplatní se pouze pro segmenty kratší než 2 m. 4. Těžiště segmentu. Pro segmenty kratší než 1m se těžiště bodů odchyluje od jejich geometrického středu o více než 10%.

7.2. SEGMENTACE DAT

63

Obr. 7.8 ukazuje možné případy, kdy se výše uvedenými heuristickými kritérii ze zpracování vyloučí příslušné skupiny bodů: a) Nízký počet bodů v segmentu - typický případ okluze. b) Nízký počet bodů v segmentu - důsledek existence překážky bodového typu. c) Těžiště bodů v segmentu je příliš vzdáleno od jeho geometrického středu - segment obsahuje nerovnoměrně rozdělené body. d) Segment má dostatečný počet bodů, i těžiště je blízko geometrického středu, ale mezi dvěma body existuje příliš velká mezera. e) Orientace segmentu je blízká snímacímu úhlu laserového paprsku. f) Segment obsahuje osamocený, tudíž pravděpodobně chybně změřený bod. g) Typický příklad zrcadlového odrazu, odstranění ze stejného důvodu jako u f). h) Segment s velkými vzdálenostmi mezi body není odstraněn, neboť má dostatečnou délku. V posledním kroku segmentace je pro naše účely důležité získat zejména správnou orientaci segmentů, čehož dosáhneme optimalizací proložení zbylých skupin segmentovaných bodů přímkou. Prokládací přímku q lze pomocí parametrů a, b a c vyjádřit její obecnou rovnicí: q : ax + by + c = 0 (7.6)

ρ

di

ϕ

(xi,yi)

O

Obr. 7.9: Obecná rovnice přímky

Obr. 7.10: Kolmé vzdálenosti bodů

Optimalizací proložení je myšlena minimalizace kvadrátů vzdáleností bodů P = {[x1 , y1 ], . . . , [xn−1 , yn−1 ], [xn , yn ]} od prokládající přímky q. Vzdálenost každého bodu [xi , yi ] od přímky p lze vyjádřit jako d=

|axi + byi + c| √ , a2 + b2

(7.7)

kde parametry a, b a c mohou být v kontextu obr. 7.9 substituovány: a = cos(ϕ), b = sin(ϕ) a c = −ρ. Tímto krokem po dosazení a vyloučení multiplikativních konstant získáme

64

KAPITOLA 7. POINT-TO-POINT LOKALIZACE

optimalizační kritérium: min ϕ,ρ

( n X

) 2

(xi cos(ϕ) + yi sin(ϕ) − ρ)

(7.8)

i=1

Kritérium 7.8 je možné minimalizovat analyticky, po provedení parciálních derivací podle ϕ, ρ a následných algebraických úpravách získáme řešení ve tvaru: 1 −2 ∗ Sxy ; arctan 2 S y 2 − S x2 ρ = x cos ϕ + y sin ϕ, kde

ϕ =

x ¯ = y¯ =

7.3

n 1X xi , n i=1 n 1X yi , n i=1

S x2 Sy 2

= =

n X i=1 n X

(xi − x ¯)2 , Sxy = 2

(yi − y¯) ,

(7.9) (7.10)

n X

(xi − x ¯)(yi − y¯)

i=1

i=1

2D lokalizace

Lokalizace robotu rozvíjí myšlenky a přístupy ICP algoritmu (Besl a McKay, 1992), (Lu a Milios, 1994) a (Lu a Milios, 1997), který slouží k nalezení translačně-rotační transformace minimalizující zobecněnou vzdálenost mezi scany. V této kapitole je shrnutý základní popis navržené PTP metody, v následujících kapitolách 7.3.1-7.3.4 jsou podrobně rozebrány jednotlivé dílčí kroky navržené metody. Vzhledem k tomu, že původní ICP algoritmus je poměrně citlivý na chybu ve výchozí orientaci robotu, do PTP metody byl přidán korekční algoritmus založený na popisu scény histogramy a na aplikaci korelačních přístupů (Mázl a Přeučil, 2000) popsaných v kap. 7.3.1. V naší práci je použita základní myšlenka ICP (Iterative Closest Point) algoritmu, která spočívá v iterativním hledání transformace mezi dvěmi datovými množinami na základě určování potenciálních korespondencí mezi elementy obou množin. Pro každou aplikaci tohoto algoritmu je velmi důležité, jakým způsobem se budou vybírat bodové páry. Vhodný výběr kandidátů pro korespondující páry má vliv na počet iterací a na celkovou efektivitu algoritmu. V ideálním případě, kdy by se podařilo ihned v první iteraci nalézt reálně si odpovídající elementy v obou datových množinách, algoritmus by mohl skončit v jednom kroku. Nicméně, spolehlivě nalézt odpovídající si elementy v jednom kroku je prakticky nemožné, proto se iterativně opakuje cyklus: 1. hledání vzájemné korespondujíce elementů v datech, 2. validace korespondujících párů, 3. výpočet parametrů transformace minimalizující sumu kvadrátů vzdálenosti korespondujících párů, 4. aplikace transformace na aktuální data.

7.3. 2D LOKALIZACE

65

Vlastní práce se v této kapitole zaměřuje na nalezení vhodných výchozích podmínek pro ICP algoritmus, následnou rychlou implementaci postupu pro nalézání korespondujících si elementů a efektivní mechanismus pro zamítání falešných korespondencí vznikající zejména z odlehlých měření (outlierů). Lokalizační proces, resp. sledování polohy považuje vždy jeden z předcházejících (dříve sejmutých) scanů jako referenční Sref a aktuální scan Sakt se snaží k referenčnímu scanu registrovat (sesadit jeho polohu vůči referenčnímu scanu). Registrací scanu k předchozím se získá aktuální poloha robotu. Registrovat scany lze nejrůznějšími principy, lze využít pravděpodobnostních přístupů, jak bylo popsáno v kap. 6.1.1 - 6.1.3 nebo pracovat se segmentovanými scany (Kulich, 2003) a hledat korespondence mezi úsečkami. V této práci byl však jako základní element scanu zvolen bod a hledání veškerých korespondencí se váže na n - bodů laserového scanu, a to z důvodů uvedených na začátku kap. 7. Během registrace scanů se minimalizuje kritérium zobecněné meziscanové vzdálenosti

Edist (T, ω) =

n X

|Rω pi + T − p0i |2 ,

i=1

kde translační vektor T a rotační matice Rω určují transformaci mezi referenčním scanem Sref a aktuálním scanem Sakt , P = {pi | pi = [xi , yi ], i = 1, . . . , n} ∈ Sref , P 0 = {pi 0 | pi 0 = [x0i , yi0 ], i = 1, . . . , n} ∈ Sakt . Konečným krokem jedné iterace je aplikace transformace s parametry vypočítanými podle vztahů 7.22 - 7.25 na aktuální scan, čímž se jeho poloha registruje vůči referenčnímu scanu. Vzhledem k tomu, že stabilita iteračního algoritmu je silně závislá na výchozích podmínkách, je vhodné před iterativním hledáním korespondencí a aplikacemi vypočítaných dílčích parametrů transformace provést vhodný výchozí odhad možné polohy a orientace robotu pro následující proces lokalizace.

7.3.1

Korekce hrubých odometrických chyb

Přirozeným výchozím odhadem polohy robotu je informace z odometrie. Odometrie sama o sobě však trpí poměrně významnou chybou, jak je popsáno v kap. 10.1.4. Chyby odometrie jsou zejména rotačního charakteru, což není pro navazující metodu založenou na ICP algoritmu příliš vhodné. Vzhledem k citlivosti ICP na rotační chyby je vhodné do procesu zpracování dat vložit jednoduchý, ale relativně robustní postup pro hrubou korekci odometrických chyb (Mázl a kol., 2001a). Pokud se robot pohybuje v prostředí, ve kterém je možné nalézt dostatečné množství liniových segmentů, provedeme korekce natočení robotu na základě dále popsané histogramového přístupu. V opačném případě se tento krok PTP metody vynechá, což bude mít za následek zvýšený počet iterací v kroku ICP algoritmu. Liniové segmenty odpovídají buď obrazu překážek v prostoru nebo částem zdí v prostředí. Mezi dvěma následujícími scany vlivem pohybu robotu nedojde typicky k nějaké

66

KAPITOLA 7. POINT-TO-POINT LOKALIZACE

výrazné topologické změně senzorických dat. Lze tudíž uvažovat o využití orientace segmentovaných liniových primitiv z referenčního a aktuálního scanu (viz kap. 7.2) pro účely stanovení výchozí změny natočení robotu tak, jak je zobrazeno na obr. 7.11.

Laser

∆ϕ

Obr. 7.11: Ukázka dvojice scanů se extrahovanými segmenty pro korekci natočení. Černé body a červené segmenty odpovídají naměřeným datům z referenčního scanu, fialové body a modré segmenty pak datům z registrovaného scanu. Návrh použité metody vychází z (Weiß a Puttkamer, 1995), kde autoři určují meziscanovou korekci na základě maxima korelace histogramů (obr. 7.13) orientací úseček vytvořených jako přímé spojnice bodů laserového scanu (obr. 7.12). Orientace jednotlivých úseček je však výrazně zatížena šumem měření, což degraduje přesný histogramový popis dominantních úhlů ve scéně. Vzhledem k tomu, že máme k dispozici segmentaci scanu do liniových segmentů optimálně prokládající vstupní data (Mázl a Přeučil, 2000), bylo by jistě škoda této geometrické reprezentace nevyužít. Četnost výskytu [-]

20

15

10

5

Laser

α ∆x

Obr. 7.12: Vstupní data pro úhlové histogramy

∆y 0

-80 -60 -40 -20

0

20 40 60 80 orientace segmentu α[°]

Obr. 7.13: Výsledný histogram orientací úseček

7.3. 2D LOKALIZACE

67

Navrhovaný algoritmus se skládá ze dvou částí. V první části se ze segmentovaných úseček referenčního a aktuálního scanu vytvoří histogramový popis a ve druhé fázi se vypočítají hodnoty korelační funkce obou histogramů. Autor (Weiß a Puttkamer, 1995) má relativně dostačující počet úseček pro přímé vytváření histogramu úhlů z orientace segmentů. My však díky segmentaci z jednoho scanu získáme maximálně jednotky až desítky vhodných úseček, což pro tvorbu histogramu již nestačí. Můžeme však využít dodatečnou informaci, kterou získáme zároveň se segmentací scanu. Touto informací je spolehlivost segmentace úsečky, jejímž hlavním parametrem je rozptyl a počet bodů, ze kterých se příslušný segment skládá. Při vytváření úhlového histogramu nebudeme proto pracovat přímo s orientací příslušného segmentu, ale s gaussovským jádrem, které bude segment zastupovat a popisovat míru důvěry v příslušnou orientaci segmentu. Gaussovské jádro g(x) je popsáno jako funkce proměnné x, která reprezentuje předpokládanou orientaci segmentu: "

1 (x − µ)2 g(x) = √ exp − 2σ 2 σ 2π

#

(7.11)

s parametry µ a σ přičemž µ je roven úhlu orientace segmentovaného liniového segmentu a σ odpovídá spolehlivosti úsečky. Tento parametr σ je třeba vhodně normalizovat tak, aby nejhůře segmentované úsečce odpovídalo Gaussovské jádro s velmi velkým rozptylem a naopak dobře segmentované úsečce odpovídalo poměrně úzké jádro. Jako vhodný ukazatel spolehlivosti segmentu se ukázal počet bodů příslušného segmentu, pro který se určuje jádro. Tento přístup velmi jednoduchým způsobem zohledňuje relativní délku segmentu a tím i spolehlivost určení jeho úhlu. Použití vypočteného bočního rozptylu bodů kolem prokládající přímky se ukázalo jako méně efektivní. Parametr σ lze vyjádřit jako: σ=

π , nb ∗ xdk

(7.12)

kde nb je počet bodů segmentu a xdk je velikost diskretizačního kroku proměnné x pro účely následného numerického výpočtu korelace ve vztahu 7.14. Při volbě parametru σ požadujeme, aby nejméně spolehlivému segmentu (úsečka o dvou bodech) odpovídal Gaussián pokrývající svým 1 σ rozptylem celý rozsah úhlů v intervalu h− π2 ; π2 i (viz obr. 7.14). Tento rozsah též odpovídá max. možnému úhlovému rozdílu orientace úseček v rovině, tedy ±π/2. 0.05

0.05

0.04

0.04

0.03

0.03

0.02

0.02

0.01

0.01

0

-2π -3π/2 -π

-π/2

0

π/2

-π

3π/2 2π α [rad]

0 -π/2

-π/4

0

-π/4

Obr. 7.14: Volba rozptylu pro úsečku složenou ze dvou bodů

α [rad]

π/2

68

KAPITOLA 7. POINT-TO-POINT LOKALIZACE četnost výskytu[-]

5

akt. scan

π/2

Gaussovská jádra pro segm. 1 a 4

π/4

4 3 5

0

2 1 −π/2

referenční scan

∆ϕ

3

ϕ

-1.5

-0.5

0

0.5

1

1.5

ϕ − orientace segmentu [rad]

Obr. 7.16: Histogram h(k) - referenční a aktuální scan

refer. scan

−π/4

∆ϕ − velikost korekce

korelace [-]

Aktuální scan

2

ϕ ... orientace segm.

5

-1

-1.5

-1

-0.5

0

0.5

1

1.5

úhelový posun [rad]

Obr. 7.15: Tvorba směrových histogramů

Obr. 7.17: Korelační funkce

Kompletní popis jednoho scanu scény histogramem h(k) je realizován jako superpozice m - dílčích diskretizovaných gaussovských jader gi (x) příslušející ke každému i-tému liniovému segmentu scanu. 

h(k) =

m  X i=1

δ(x − k xdk ) 1  x

dk



(k+ 12 )xdk

Z



gi (x)dx ,

(7.13)

(k− 12 )xdk

(

1, x = 0 je funkce diskrétního Diracova impulsu (Kronecrovo delta), k je 0, x 6= 0 index vzorku histogramu k ∈ (− n2 ; n2 ), n = π/xdk a xdk je velikost diskretizačního kroku pro následující výpočet korelace. Samotný výpočet úhlové korekce je realizován jako hledání maxima korelační funkce mezi histogramem referenčního scanu href (k) a histogramem aktuálního scanu hakt (k). kde δ(x) =

c(j) =

n X

n n j ∈ (− ; ) 2 2

href (i) ∗ hakt (i + j)

i=1

∆ϕ = arg max(c(j)) ∗ xdk

(7.14) (7.15)

Nalezená hodnota úhlu ∆ϕ se použije jako parametr rotační transformace, která je aplikována na každý bod pi = [xi , yi ] ze scanu Sakt sejmutého z pozice oakt = [ox , oy ], i = 1..n " 0

pi =

cos(∆ϕ) − sin(∆ϕ) sin(∆ϕ) cos(∆ϕ)

#

!

(pi − oakt ) + oakt

(7.16)

Transformované body pi 0 = [xi , yi ] aktuálního scanu Sakt vstupují dále do lokalizační procedury jak bylo uvedeno v kap. 7.3.4 popisující ICP algoritmus. Postup korekce hrubé odometrické chyby na bázi histogramového popisu scanu a následných korelací lze využít i pro korekci chyby translace jak je řešeno v práci (Mázl a

7.3. 2D LOKALIZACE

69

kol., 2001b). Výsledky korekce translace však nejsou tak dobré jako v případě korekce rotace. Navíc použitý způsob hledání korespondujících bodových párů popsaný v následující kapitole není zbytkovou translační chybou příliš degradován.

7.3.2

Hledání korespondujících bodových párů

Jednou z nosných komponent ICP algoritmu je vlastní hledání korespondujících si bodů mezi dvěma scany. K problematice hledání korespondencí lze přistupovat velmi různě. Autoři (Lu a Milios, 1994) ve své práci pracují se strategií výběru korespondujících bodů podle pravidla „Closest Pointÿ nebo „Matching Range Pointÿ. Výhoda pravidla „Matching Range Pointÿ je ve schopnosti ICP algoritmu korigovat větší rotační chyby, na druhé straně u pravidla „Closest Pointÿ byla autory (Besl a McKay, 1992) dokázána konvergence ICP algoritmu. V našem přístupu v předchozí kapitole 7.3.1 korekci větších odometrických chyb vhodně řešíme korelační histogramovou metodou, tudíž komplikovanější pravidlo „Matching Range Pointÿ není třeba dále rozvíjet. Při návrhu našeho přístupu k hledání korespondujících bodových párů vyjdeme z pravidla „Closest Pointÿ, protože u něj lze hledání velmi efektivně implementovat použitím vhodných vyhledávacích struktur. Pro každý bod právě registrovaného aktuálního scanu je třeba nalézt referenční souřadnice bodů odpovídající obrazu korektní polohy tohoto bodu. Tato úloha je v plné obecnosti těžko řešitelná, proto přistupujeme na aproximace. Předpokládáme, že meziscanová chyba polohy registrovaného scanu není větší než velikost průměrné granularity prostředí (velikost a vzdálenost překážek). Za splnění tohoto předpokladu obvykle nenastane situace, kdy se do prostoru mezi reálně korespondující body z referenčního a neregistrovaného aktuálního scanu dostane bod odpovídající jinému předmětu, viz obr. 7.18.

Obr. 7.18: Vliv granularity prostředí na hledání korespondencí Kontura obrazu hran objektů v referenčním i registrovaném scanu by také neměla být příliš silná, aby nedocházelo k chybným přiřazováním korespondencí ze stejných důvodů jako je uvedeno na obr. 7.18. Tato situace však z principu u laserového měření na rozdíl od sonarových nevzniká. Díky přesnosti laserových měření mají změřené body poměrně malý rozptyl, tudíž obraz jedné hrany překážky nevytvoří body „vyplněnouÿ oblast, nýbrž relativně přímou bodovou aproximaci úsečky s nepatrným bočním rozptylem bodů od její podélné osy. Proto postačí, budeme-li hledat korespondující bod na hranici nejbližší překážky. Některé hranice překážek jsou navíc reprezentovatelné liniovými segmenty, jako korespondující bod lze pak tedy vzít průsečík kolmice vedenou z příslušného bodu k nej-

70

KAPITOLA 7. POINT-TO-POINT LOKALIZACE

bližšímu liniovému segmentu. Tento postup má však dvě slabiny, zdaleka ne všechny objekty lze proložit lin. segmenty a hledání nejbližšího bodu k n-přímkám vyžaduje konstrukci zobecněných Voronoidiagramů, což je výpočetně relativně náročné. Požadavky na náš lokalizační algoritmus jsou však takové, aby pokud možno pracoval i v obecných prostředích a navíc co možná nejrychleji. Tyto skutečnosti nás vedou k další úrovni aproximace, kdy korespondující body nebudeme hledat přímo na aproximované hranici objektů z referenčního scanu, ale budeme je hledat přímo výběrem z bodů referenčního scanu jako body euklidovsky nejbližší. Pro hledání euklidovsky nejbližšího bodu z množiny předem daných bodů k libovolnému bodu v prostoru existuje velmi efektivní vyhledávací struktura nazývaná k-D strom, která bude popsána později v rámci kap. 8.3. Z referenčního scanu S ref se proto ještě před první iterací ICP algoritmu nejprve se složitostí O(n log(n)) vytvoří konstantní vyhledávací struktura k-D strom, která je následně využívána v každé iteraci ICP algoritmu pro vyhledání nejbližšího bodu pref ∈ i akt . S ref pro každý jednotlivý bod pakt z aktuálního scanu S j akt Výsledkem tohoto postupu je množina bodových dvojic C = {ci }, kde ci = (pref k , pj ), pro které platí: n

o

ref akt 2 akt 2 , ∀j ∈ h1; ni ∃k ∈ h1; mi : k = arg min (pref kx − pjx ) + (pky − pjy ) k

(7.17)

kde n a m jsou platné počty bodů v aktuálním a referenčním scanu. Tyto nalezené bodové páry nejsou přímo využity jako vstup do minimalizace definované kritériem 7.21 tak, jak tomu je v případě původního pravidla „Closest Pointÿ (u něj se odstraní jen hrubé chyby), ale jsou vstupem námi navrženého postupu pro odstranění bodů z odlehlých měření a chybně vybraných bodových párů.

7.3.3

Odstranění odlehlých bodů, pohyblivé objekty

Nedílnou součástí postupu hledání korespondujících bodů je detekce falešně vzniklých bodových párů. Tato detekce je jednou z důležitých modifikací původního ICP algoritmu přinášející zvýšení robustnosti naší nové PTP metody. Falešné bodové páry mohou vzniknout v principu ve dvou situacích. První situací je nesplnění omezující podmínky na prostředí, konkrétně jeho metastatické povahy. Dojde-li mezi referenčním a aktuálním scanem k pohybu předmětů v prostředí, nalezené bodové páry mezi původní polohou předmětu v referenčním scanu a aktuální polohou předmětu v aktuálním scanu nenesou relevantní informaci o vzájemné relativní pozici obou scanů. Druhý případ nastává v situaci, kdy robot získá viditelnost do oblasti prostoru, která nebyla referenčním scanem pokryta. Aktuální scan pak obsahuje řadu bodů, pro které v referenčním scanu neexistuje vhodný pár. Přesto však základní postup hledání bodových párů založený na hledání nejbližších sousedů nějaký pár nalezne. Tato situace vzniká zejména pokud robot jede tzv. „za rohÿ, projíždí dveřmi do nové místnosti nebo míjí překážku v popředí, která bránila získání prostorových informací o prostředí v pozadí.

7.3. 2D LOKALIZACE

71

V případě, že se výše uvedené situace zanedbají, zahrnutí neplatných bodových párů velmi zásadním způsobem ovlivňuje výsledky lokalizačního procesu. Vede to ke zpomalování konvergence původního ICP algoritmu, zvyšování zbytkové lokalizační chyby či k uvíznutí v lokálním extrému a v konečném důsledku až k selhání celé metody. Základní princip detekce falešných bodových párů z množiny C spočívá ve vzájemném rozložení vzdáleností bodů tvořící páry podle vztahu 7.17. akt Označme si jako CS = {csi } posloupnost bodových párů csi = (pref is ) a (pis ) seřazených podle své vzájemné vzdálenosti, která vznikne přeindexováním bodových párů z množiny C tak, aby platilo: ∀i ∈ h1; n − 1i ∀csi

:

di = |csi | =

|csi | ≤ |csi+1 |

(7.18)

r

ref akt 2 akt 2 (pref isx − pisx ) + (pisy − pisy )

(7.19)

Falešné bodové páry označené jako posloupnost CSf = {cr , . . . , cn−1 , cn } se vyznačují výrazně větší vzdáleností než skutečné bodové páry z posloupnosti CSt = {c1 , . . . , cr−2 , cr−1 }, přičemž pořadí bodových párů v posloupnostech CSf a CSt je urS a pakt čeno vzájemnou vzdáleností di párů bodů pref i . Je tedy stejné jako v C a platí i S S S S C = Ct Cf . Jako rozdělující hranici mezi posloupnostmi nelze vzít určitou pevnou bodovou vzdálenost di . Kritická vzdálenost dr , která rozděluje skutečné a falešné páry se mění během každé iterace a její velikost je řízena samotnými daty. V prvních iteracích jsou vzdálenosti všech bodových párů řádově mnohem větší než v koncových iteracích. B

Vzdálenost bodů v páru akceptované páry zamítnuté páry . dr - práh vzdálenosti

R

A

vhodné páry

vzdálené páry

Obr. 7.19: Znázornění kritéria pro validaci bodových párů Požadavek na volbu kritické vzdálenosti dr je, aby v prvních iteracích byla spíše vyšší a v následujících iteracích se snižovala. Z počátku je žádoucí zamítat pokud možno co nejméně bodů, tedy pouze ty, u kterých je velmi vysoká pravděpodobnost, že skutečně náleží falešným párům. Zbylé páry s velkými vzdálenostmi je nežádoucí zamítat v prvních iteracích, protože nesou nejvýraznější informaci o parametrech hledané transformace (ve

72

KAPITOLA 7. POINT-TO-POINT LOKALIZACE

smyslu kritéria 7.21). Naopak, v závěrečných iteracích je třeba kritickou vzdálenost výrazně snižovat, aby byla vysoká jistota, že všechny vybrané bodové páry přísluší skutečně korektním a reálným párům. Metoda určující kritickou vzdálenost dr využívá faktu, že vzdálenosti bodových párů S Df = {d1 , . . . , dr−2 , dr−1 } v posloupnosti CSf jsou výrazně menší než vzdálenosti párů DSt = {dr , . . . , dn−1 , dn } v posloupnosti CSt , přičemž hodnoty di = |ci | V následujícím kroku sestrojíme diskrétní neklesající distanční funkci pořadí bodových párů: dist(k) = dk , kde dk ∈ DSt

[

DSt , ∀k ∈ h1; n − 1i : dk ≤ dk+1

(7.20)

Tato funkce má velmi charakteristický zlom znázorněný na obr. 7.19, které se nachází právě v oblasti oddělující od sebe falešné bodové páry od reálných bodových párů. Je třeba tedy v každé iteraci ICP algoritmu učit polohu tohoto zlomu a definovat tak kritickou vzdálenost dr . Byly prováděny experimenty s detekcí zlomu využitím principů hledání maxim derivací různých řádů, heuristiky pracující na principu mediánů a středních hodnot, ovšem žádná z těchto technik neposkytovala požadované výsledky zejména s ohledem na robustnost vůči zpracování dat z nejrůznějších typů prostředí a rychlosti jízdy robotu. Jako velmi vhodná technika pro stanovení hodnoty dr se ukázalo použití y-souřadnice bodu R = [r, dist(r)], který je nejvzdálenější od úsečky AB definované krajními body A = [1, dist(1)] a B = [n, min(dist(n), maxdist )]. Zavedení maxdist je potřeba z důvodu, kdy se vyskytnou v bodových párech velmi vysoké vzdálenosti vyplývající z chybných měření nebo z velmi nešťastných konfigurací scény pro nalézání bodových párů. V reálné implementaci se konkrétní hodnota maxdist odvozuje od velikosti granularity překážek v prostředí a průměrné šíře volného prostoru experimentálně tak, aby odpovídala přibližně desetinásobku průměrné vzdálenosti bodových párů v prvních iteracích. V kancelářských prostředích nabývá obvykle jednotek metrů. Velmi výhodná vlastnost popsané techniky zamítání bodových párů je, že s přibývajícími iteracemi, pokud klesá hodnota dist(n) přiměřeně klesá i kritická vzdálenost dr . To znamená, že v prvních iteracích je dr relativně vysoké s ohledem na dist(n) a postupně klesá podle aktuální rychlosti konvergence ICP algoritmu. Pokud y-souřadnice bodu R, tedy dist(r) klesne pod hodnotu odpovídající max. dovolené poziční chybě mezi registrovanými scany maxerr , jako hodnota dr se bere v úvahu maxerr místo dist(r). Pokud by se jako dr použila menší hodnota, mělo by za následek, že by důležité bodové páry obsahující informaci o hledané transformaci byly nežádoucím způsobem vyloučeny z navazující optimalizace.

7.3.4

Výpočet parametrů transformace

Zobecněná vzdálenost Edist mezi scany není veličina, kterou by šlo přímo minimalizovat na základě vztahu 7.11 a příslušných vstupních dat. Její hodnota je výsledkem algoritmického výpočtu, ve kterém se hledají nevhodnější bodové páry mezi body ze dvou scanů Sref a Sakt . Důsledkem toho není možné vztah 7.11 přímo podrobit např. numerické optimalizaci a najít tak parametry optimální transformace mezi scany. Nyní si popišme jednotlivé kroky iteračního lokalizačního cyklu.

7.3. 2D LOKALIZACE

73

Pro jednoduchost předpokládejme, že bodové korespondence mezi body již známe (na základě postupu popsaného v minulé kap.). Označme si vybrané body referenčního scanu P = {[xi , yi ]} a body aktuálního registrovaného scanu P 0 = {[x0i , yi0 ]}, kde i = 1, . . . , n, přičemž vždy i-tý bod z množiny bodů P koresponduje s i-tým bodem z množiny bodů P 0 pro všech n bodů."Vztah 7.11 pak můžeme po dosazení do složek # cos(ω) − sin(ω) vektorů a rozepsání rotační matice R = následně přepsat do podoby sin(ω) cos(ω) Edist (Tx , Ty , ω) = =

n  X



(xi cos(ω) − yi sin(ω) + Tx − x0i )2 + (xi sin(ω) + yi cos(ω) + Ty − yi0 )2 .

(7.21)

i

Vztah 7.21 je možné za předpokladu známých korespondencí bodů P a P 0 analyticky minimalizovat a tím získat uzavřené řešení pro parametry transformace Tx , Ty a ω: Tx = x¯0 − (¯ x cos ω − y¯ sin ω) Ty = y¯0 − (¯ x sin ω + y¯ cos ω) n P

ω = arctan i=1 n P

(xi − x ¯)(yi0 − y¯0 ) − (xi −

i=1

x ¯)(x0i

−

x ¯0 )

−

n P i=1 n P

(7.22) (7.23)

(yi − y¯)(x0i − x ¯0 )

i=1

,

(7.24)

(yi − y¯)(yi0 − y¯0 )

kde x ¯ =

1 n

n P

xi

x ¯0 =

i=1

1 n

n P i=1

x0i (7.25)

y¯ =

1 n

n P

yi

i=1

y¯0

=

1 n

n P i=1

yi0

Jakmile jsou známy parametry transformace, data aktuálního scanu je možné transformovat a zmenšit tak vzdálenost vůči referenčnímu scanu a dále pak pokračovat hledáním nových korespondujících párů pro další iteraci. Kritérium meziscanové vzdálenosti mezi transformovaným a referenčním scanem má pak typicky nižší hodnotu. Celý iterační cyklus je ukončen buď po předem stanoveném maximálním počtu iterací nebo pokud se velikost hodnot korekčních parametrů Tx , Ty a ω v několika následujících iteracích přiblížila nule.

7.3.5

Validace výsledků lokalizace

Pokud nastanou nepříznivé podmínky pro lokalizaci, například díky výraznému pohybu předmětů v popředí nebo extrémní odometrické chybě, může se stát, že lokalizační proces metody PTP selže.

74

KAPITOLA 7. POINT-TO-POINT LOKALIZACE

Selhání metody se projeví tak, že řešení typicky zkonverguje do některého lokálního extrému a nalezené korekční parametry jsou chybné. Pokud k selhání dojde, je nutné jej detekovat a výsledné parametry pro korekci polohy nepoužít. V této situaci je třeba použít jinou metodu odhadu polohy robotu, buď zůstat u apriorní informace z odometrie nebo extrapolovat vývoj pozice z minulých měření. Detekce selhání se opírá o fakt, že v případě nekorektního výsledku je průměrná hodnota skutečných bodových párů výrazně větší než v případě korektní lokalizace a navíc zdánlivý počet falešných bodových párů je relativně vysoký. Poslední faktor mající vliv na rozhodování, zda nedošlo k selhání metody jsou hodnoty velikosti korekce v jednotlivých osách a úhlu natočení. Pokud korekce přesahuje mezní hodnotu, je lokalizační proces též prohlášen za neúspěšný. V souhrnu lze říci, že pokud je některý z příznakových parametrů na hranici spolehlivosti metody, vždy je z globálního hlediska bezpečnější prohlásit výsledek za neúspěšný, neboť v následujícím scanu je šance, že se data získají z nové pozice nebo odezní nežádoucí chybový faktor (průchod člověka před robotem) a robot se podaří v dalším kroku zlokalizovat. V opačném případě bychom do lokalizačního systému vnesli trvalou, neodstranitelnou poziční chybu.

7.4

Vlastnosti metody Point-To-Point

PTP metoda dohromady s unikátním předzpracováním dat představuje poměrně velmi robustní nástroj pro velmi přesnou registraci dvojic scanů, které se příliš neliší pozicí, odkud byly laserovým scannerem sejmuty. Je proto vhodná pro sledování pozice mobilního robotu v neznámém prostředí, popř. k upřesnění výchozí pozice robotu vůči známé mapě za předpokladu, že je přibližně známa pozice robotu v této mapě. Naopak, metoda je zcela nevhodná pro řešení úlohy globální lokalizace ve známém prostředí. Díky způsobu hledání bodových párů a zamítání jejich nevhodných kandidátů se PTP metoda dokáže vyrovnat i s určitým omezeným počtem pohyblivých objektů v prostředí. Množství pohyblivých předmětů a velikost jejich pohybu závisí na vzdálenosti těchto předmětů od robotu. Pohyb vzdálenějších objektů má přirozeně na robustnost metody mnohem menší vliv, než pohyb blízkého předmětu, který robotu zakrývá významnou část jeho zorného úhlu. Pokud v těsné blízkosti robotu dojde k velkému pohybu předmětů a metoda následně selže, je vždy přesnější jako odhad polohy robotu vzít informaci z odometrického subsystému robotu než výsledek PTP metody, který odpovídá některému falešnému extrému minimalizované funkce zobecněné vzdálenosti. Chybný výsledek však není způsoben slabinou navrženého postupu, ale opominutím výchozích definovaných předpokladů a požadavků na vstupní data. Stabilita sledování polohy metodou PTP může být dále výrazně zvýšena využitím kontinuálně budované mapy prostředí, vůči které se aktuální scany následně registrují. Toto další výrazné vylepšení lokalizační metody PTP je popsáno v následující kapitole.

Kapitola 8

Bodově orientovaná mapa 8.1

Vazba lokalizace na mapu

PTP metoda popsaná v předchozí kapitole trpí jednou negativní vlastností, kterou je principiálně ničím neomezený nárůst kumulativní lokalizační chyby během dlouhodobého lokalizačního procesu. Přirozeně, po registraci dvou scanů vždy existuje zbytková nenulová chyba. Pokud však následující n + 1 scan budeme registrovat k n-tému scanu, jednotlivé lokalizační chyby se sčítají a během dlouhodobé lokalizace může chyba neohraničeně růst. Možné řešení, jak lze tomuto předcházet, je nevybírat jako referenční scan poslední scan, ale některý starší, který však pokrývá přibližné stejné prostředí jako aktuální scan. Tímto způsobem byla lokalizace řešena v práci (Saarinen a kol., 2004a), ve které byl jeden scan vždy prohlášen za statický a k němu se registrovaly následující scany. Tento přístup lze považovat jako určité zdokonalení scan-matching metod pracující na principu registrace prostých scanů, ovšem stále zde zůstává problém dlouhodobé přesnosti lokalizace v omezeném prostoru. Klíčový problém se totiž přesouvá k výběru statického scanu, který slouží jako reference. Vybrat vhodný scan z předcházející historie není snadná úloha a často se může stát, že vhodný scan v historii ani existovat nebude. V případě návratu robotu do již navštívené části prostoru totiž senzorický systém robotu může na prostředí nahlížet ze zcela jiného úhlu pohledu než v předchozím průjezdu. Nalézt pak vhodný referenční scan z předcházející historie se stane neřešitelnou úlohou, protože takový scan neexistuje. V praktické realizaci byl v práci (Saarinen a kol., 2004a) jako statický scan vybírán jeden z nepříliš starých scanů, který byl jako referenční scan udržován tak dlouho, dokud nalezený počet bodových párů v scan-matching procesu mezi tímto a aktuálním scanem neklesl pod polovinu párů. V okamžiku, kdy byl počet nalezených párů pro aktuální scan nedostatečný, pro následující registraci scanů se vybral nový registrovaný scan, který předcházel scanu právě sejmutému. Problém kumulativní lokalizační chyby se tímto řešením sice zmírnil, ovšem ne zcela vyřešil. Kumulativní lokalizační chybě se lze zcela vyhnout jen v případě, kdy je předem dostupná kompletní mapa prostředí, vůči které se lokalizace provádí. Tímto bychom se 75

76

KAPITOLA 8. BODOVĚ ORIENTOVANÁ MAPA

však dostali mimo cíle této práce, neboť naším vytyčeným cílem je kontinuální lokalizace v neznámém prostředí. Proto se vydáme jiným směrem. Nebudeme předpokládat apriorní existenci mapy prostředí, ale budeme se snažit vhodnou mapu vytvořit současně s pohybem robotu v prostředí během řešení nadřazené úlohy. V následujících kapitolách se budeme pak zabývat tím, jak takovou mapu vytvořit a v kap. 8.5 je popsána metoda Point-To-Map, která vytvořenou mapu využije pro lokalizaci. Metodu Point-To-Map budeme v následujícím textu označovat jako PTM metodu.

8.2

Reprezentace dat

V kap. 6.2 byly shrnuty v robotice používané typy map spolu s jejich základními vlastnostmi. Při výstavbě mapy, která má podpořit proces lokalizace musíme zvážit jaký druh mapy je pro takovouto úlohu vhodný. Na lokalizační mapu jsou kladeny tyto základní podmínky: • musí být dostatečně přesná, • musí poskytovat možnost rychlého vyhledávaní nejbližších objektů k libovolné souřadnici, • neměla by být závislá na struktuře prostředí, • musí být schopna jednoduché inkrementální aktualizace po každém scanu, • aktualizace musí být velmi rychlá, doba aktualizace musí být kratší než doba mezi dvěma příchozími scany ze senzoru, • musí být paměťově úsporná, velikost by neměla růst s velikostí pracovního prostoru, ale s jeho složitostí. Těmito podmínkami se nám poměrně rychle zúží výběr potenciálně vhodných druhů map. Symbolické a topologické mapy nevyhovují nesplněním hned první podmínky, neboť ze své podstaty v sobě nenesou apriorní informaci o přesném geometrickém popisu prostředí. Z hlediska přesnosti jsou vyhovující geometrické mapy. Způsobů možných reprezentací popisu prostředí v geometrických mapách je velká řada, od nejjednodušších segmentových (úsečkových) reprezentací přes polygonální až po nejobecnější křivkové. Segmentové a polygonální reprezentace velmi dobře popisuje prostředí obsahující předměty s dlouhými, rovnými hranami (Mázl a Přeučil, 2000), (Přeučil et al., 2002), (Shaffer, 1995), (Kulich, 2003). Členité prostředí, jako např. kanceláře s řadou drobným objektů jako jsou nohy stolů a židlí těmito reprezentacemi příliš dobře popsat nelze. Zcela automatická výstavba složitějších geometrických map než jsou segmentové a polygonální je velmi komplikovaná a dosud nám není známo, že by se tato úloha podařila uspokojivě vyřešit. Navíc inkrementální aktualizace geometrických map je velmi složitá, existující algoritmy obvykle stavbu mapy provádí dávkově (Kulich, 2003).

8.3. POUŽITÍ K-D STROMU

77

Výše uvedené úvahy nás vedou k volbě senzorické reprezentace prostředí. Klasickým zástupcem senzorických modelů je mřížka obsazenosti. Tato reprezentace splňuje téměř všechny výše deklarované požadavky, aktualizace je jednoduchá a rychlá, může reprezentovat libovolnou strukturu prostředí, je odolná vůči šumu, míra její přesnosti závisí na zvolené velikosti buňky. Mřížka obsazenosti však při vyšších rozlišeních není příliš paměťově úsporná a bez dalších pomocných struktur není ani příliš vhodná pro vyhledávání informací v ní uložených. Naší základní motivací pro budování modelu světa je podpora lokalizace, která by měla dosahovat až centimetrové přesnosti. Mřížka obsazenosti by pro tento účel musela mít velikost buňky též v řádu jednotek centimetrů a méně. Malá velikost buňky však klade vysoké nároky nejen na paměťový prostor, ale zejména na výpočetní výkon, pokud nad mřížkou provádíme vyhledávací operace, a to je pro nás v případě lokalizace nezbytné. Vhodným řešením se ukázala být reprezentace prostředí ve formě námi navržené bodové mapy, která je vhodnou alternativou k mřížkám obsazenosti nejen pro účely lokalizace. Bodová mapa ve své základní podobě má velmi jednoduchou reprezentaci, je to neuspořádaná množina bodů v kartézské souřadné soustavě. Souřadnice bodů odpovídají poloze hran detekovaných překážek v prostředí, které lze získat přímo z laserového scanneru, pokud jsou lokalizovaná. V této podobě by však bodová mapa příliš mnoho nového oproti mřížce obsazenosti nepřinesla, paměťová náročnost by byla obdobná a vyhledávání v ní by bylo neefektivní. Data do bodové mapy nelze vkládat přímo tak, jak přicházejí ze senzoru. Data je třeba před vkládáním filtrovat s ohledem na již stávající obsah mapy. Nový bod budeme do mapy vkládat pouze za předpokladu, že se v jeho blízkém okolí již nenachází jiný bod. Hledání blízkých bodů je jednou z nejzákladnějších operací, kterou je třeba nad bodovou mapou provádět. Je klíčovou operací nejen při samotném budování mapy, ale i posléze během využívání mapy pro účely lokalizace, kdy se ke každému bodu z nově příchozího nezlokalizovaného scanu bude v mapě hledat nejbližší bod v procesu hledání korespondujících bodů popsaném v kap. 7.3.2. Nad množinou bodů bodové mapy je nutné vybudovat efektivní vyhledávací strukturu, která bude ke každému bodu s libovolnými souřadnicemi schopna vrátit referenci na n nejbližších bodů v stávající mapě. Od této operace je odvozeno rozhodování, zda nově příchozí bod, který je kandidátem pro vložení do mapy již není v mapě reprezentován. Pro účel vyhledávání nejbližších bodů je vhodná datová struktura nazývaná k-D strom, která je popsána v následující kapitole.

8.3

Použití k-D stromu

K-D strom je datová struktura pro ukládání uspořádané konečné množiny bodů z krozměrného prostoru (Moore, 1991). K-D strom je k-rozměrný binární vyhledávací strom, jehož uzly obsahující na každé úrovni dva potomky tvořící též k-D strom a datové položky. Datové položky se skládají z čísla aktuální dělící dimenze prostoru a z vlastních doménových vektorů (bodů vložených do k-D stromu), které jsou buď v roli dělícího pivota n-tého rozměru nebo v roli prostorového bodu ležícího v listu k-D stromu.

78

KAPITOLA 8. BODOVĚ ORIENTOVANÁ MAPA

Binární vyhledávací stromy slouží k vyhledávání v jednorozměrných datech. Každý uzel binárního vyhledávacího stromu (otec) obsahuje data (klíč) a dva podstromy (syny). Struktura uzlů ve stromu a jejich obsah je ilustrován na obr. 8.2. Podstrom může být i prázdný, pokud však obsahuje data, platí, že hodnoty klíčů dat v levém podstromu jsou vždy menší než hodnota klíče otcovského uzlu a naopak hodnoty klíčů pravého podstromu jsou vždy větší než hodnota klíče otcovského uzlu. K-D stromy umožňují navíc oproti klasickému binárnímu vyhledávacímu stromu vyhledávat v k-rozměrném prostoru, díky tomu, že se v každé úrovni k-D stromu pravidelně střídají jednotlivé dimenze klíčů dat. Např. body ve 2-rozměrném prostoru jsou v sudých úrovních stromu rozděleny podle hodnoty souřadnice x a v lichých úrovních pak podle hodnoty souřadnice y.

8.3.1

Stavba k-D stromu

Množina k-rozměrných bodů je reprezentována v k-D stromu množinou uzlů, kde každý uzel reprezentuje jeden bod. Popis položek jednoho uzlu je uveden v tab. 8.1 Položka domain-element split left

Typ položky k-D vektor integer k-D strom

right

k-D strom

Popis Bod z k-rozměrného prostoru Rozdělující dimenze k-D strom reprezentující body nalevo od rozdělující nadroviny k-D strom reprezentující body napravo od rozdělující nadroviny

Tabulka 8.1: Položky uzlu k-D stromu Za předpokladu, že před budováním k-D stromu je k dispozici kompletní datová množina, postup konstrukce je rekurzivní a spočívá ve třech základních operacích: • výběr rozdělující dimenze a vhodného bodu jako pivotu, které definují rozdělující hyperrovinu pro rozdělení vstupních dat, • rozdělení vstupních dat na dva hyperobdélníky, které splňují zvolené kritérium (stejná velikost, rovnoměrné rozložení dat apod.), • rekurzivní vybudování k-D stromu z dat náležících levému a pravému hyperobdélníku. V případě, že do k-D stromu body postupně vkládáme, postupujeme tak, že od kořene strom nejprve procházíme do hloubky. Podle souřadnic vkládaného bodu, aktuální rozdělující dimenze a pivotu se rozhodujeme, zda procházíme na další úrovni levý nebo pravý podstrom, až se dostaneme do listu stromu představující jeden hyperobdélník. Pokud je listový hyperobdélník prázdný, bod do něj pouze vložíme. Pokud již obsahuje bod, z vkládaného bodu nebo stávajícího bodu se stane nový pivot, určíme rozdělující dimenzi, vytvoříme novou rozdělující hyperrovinu kolmou na rozdělující dimenzi a zároveň procházející pivotem. Pravidlo pro určení rozdělující dimenze může být modifikováno podle

8.3. POUŽITÍ K-D STROMU

79 [5,9]

[6,1]

[1,1]

[7,2]

[3,2] [2,3] [1,4]

[8,1]

[4,3]

[1,5]

[9,4]

[8,3]

[6,3]

[3,4]

[3,6]

[9,4]

[2,3]

[8,8]

[7,2]

[1,5]

[1,7]

[4,7] [2,8]

[9,6]

[6,6]

[3,6]

[9,7]

[7,7]

[4,8]

[6,8]

[2,8]

[4,7] [1,1] [3,2]

[1,7] [4,8] [1,4] [4,3]

[6,6] [9,7] [6,3]

[7,7]

[8,1]

[9,6] [6,1] [8,3]

[8,8]

[5,9]

Obr. 8.1: Rozdělení roviny k-D stromem

[3,4]

[6,8]

Obr. 8.2: Uzly ve vytvořeném k-D stromu

aktuálních potřeb a struktury dat. Často se volí přístup, kdy se rozdělující dimenze pravidelně střídají podél hloubky stromu, lze ji však vybírat i podle jiných pravidel. Např. podle délky hran děleného hyperobdélníku tak, aby byl rozdělen na dvě oblasti s poměrem hran podél jednotlivých dimenzí co nejbližšímu jedné. Nakonec vytvoříme dva nové hyperobdélníky, přičemž jeden z nich již obsahuje jeden z dvojice bodů, který se nestal pivotem. Výsledek inkrementálního budování stromu podle výše uvedeného algoritmu je zobrazen na obr. 8.2 a 8.1. Vstupními body v tomto případě byla sekvence 2D bodů [5,9], [9,4], [1,5], [2,3], [3,6], [8,8], [7,2], [9,7], [3,2], [4,7], [4,3], [4,8], [2,8], [1,7], [6,6], [7,7], [9,6], [6,3], [6,1], [8,1], [1,1], [1,4], [6,8], [8,3], [3,4]. Uzel [5,9] rozděluje rovinu podél x = 5, v další úrovni stromu uzly [9,4] a [1,5] dělí poloroviny podle y = 4 resp. y = 5.

8.3.2

Hledání nejbližšího souseda v k-D stromu

Lokalizační metoda PTP, konkrétně její součást ICP algoritmus, vyžaduje efektivní způsob hledání nejbližších sousedů. Zejména k tomuto účelu byla použita pro reprezentaci mapových dat struktura k-D stromu, který umožňuje na předem vytvořené vyhledávací struktuře zodpovídat dotazy na nejbližšího souseda efektivně. Hledání ve stromě probíhá rekurzivním způsobem a to prohledáváním do hloubky s ořezáváním zamítnutých podstromů. Mějme cílový bod X, ke kterému hledáme nejbližšího souseda, na obr. 8.3 a 8.4 je označen křížkem. Nejprve se vykonává klasické prohledávání do hloubky, během kterého je třeba nalézt listový uzel a tím i cílový podprostor (obecně hyperobdélník), do kterého náleží bod X. Tímto krokem je znám i horní odhad vzdálenosti k potenciálnímu nejbližšímu sousedovi, který je na obr. 8.3 označen jako červený bod v kroužku. Tento bod sice není nejbližším sousedem bodu X, nicméně je jisté, že hledaný nejbližší soused leží uvnitř hyperkružnice se středem v bodě X procházející dosud nejbližším známým bodem. Nyní se vrátíme v průchodu stromem do rodičovského uzlu listového uzlu. Na obr. 8.4

80

KAPITOLA 8. BODOVĚ ORIENTOVANÁ MAPA

je označen červenou barvou s kroužkem okolo bodu. Spočítáme, zda je možné, aby druhý potomek (než je ten, ze kterého jsme se vrátili) rodičovského uzlu obsahoval bližší bod k X než dosud nalezené nejlepší řešení v ostatních potomcích. Zde to není možné, protože hyperkružnice z obr. 8.4 prostor horního potomka (označen šedou výplní) červeného uzlu vůbec neprotíná. V případě, že prostor tohoto potomka nemůže obsahovat bližší, než dosud nalezení řešení, vrátíme se ve stromu opět o jednu úroveň výše. V opačném případě bychom museli rekurzivně prohledat prostor příslušného potomka.

[5,1]

[5,1]

[2,2]

[2,2] [7,3]

[7,3] [1,4] [8,4.5]

[4,7]

[1,4]

[8,4.5]

[6,7]

[3,9]

Obr. 8.3: Nalezení cíle uzlu stromu, horní odhad nejbližšího souseda

[4,7]

[6,7]

[3,9]

Obr. 8.4: Oblast stromu, která nemusí být již prohledávána

V našem příkladu na obr. 8.4 je další úroveň stromu kořenový uzel a vzhledem k tomu, že hyperkružnice protíná pravý podprostor, musí být tento podprostor opět do hloubky prohledán, aby byl nakonec nalezen skutečně nejbližší bod. Za určitých okolností je výhodné hledat n nejbližších sousedů. V tomto případě se algoritmus hledání nejbližšího souseda mírně modifikuje tak, že rádius hyperkružnice obsahuje vždy n nejbližších dosud nalezených bodů. Přestože na první pohled se může zdát, že je nakonec prohledán téměř celý prostor stromu, není tomu tak díky zamítání celých podstromů během hledání. Efektivita prohledávání však závisí na struktuře dat a na poloze bodu X vůči poloze bodů v k-D stromu. V neposlední řadě na efektivitu prohledávání mají vliv i strategie výběru pivotu, dělící hyperroviny v procesu budování stromu a s tím i související operace vyvažování stromu. Jedním z nejhorších případů z hlediska efektivity algoritmu je hledání nejbližšího souseda v datové množině, jejíž body leží přibližně na povrchu hyperkoule a dotazovaný cílový bod se nachází v blízkosti středu této koule. Výhoda k-D stromu je, že stejné datové struktury mohou pracovat teoreticky nad libovolným počtem dimenzí prostoru. Ovšem se vzrůstajícím počtem dimenzí velmi rapidně klesá efektivita algoritmu, počet uzlů, které je nutno během prohledávání prostoru o větším počtu dimenzí projít roste exponenciálně. Z tohoto důvodu je praktické využití k-D stromu pro práci s více jak deseti dimenzemi vyhledávacího prostoru minimální.

8.4. STAVBA BODOVÉ MAPY

81

Pro naši práci, kdy pracujeme s vyhledáváním nejbližších bodů v rovině a dále pak v třírozměrném prostoru, je však struktura k-D strom ideální.

8.4

Stavba bodové mapy

Námi navrhovaná reprezentace modelu světa ve formě bodové mapy prostředí patří do kategorie senzorických map podobně jako mřížky obsazenosti. Bodovou mapu lze chápat jako alternativu k mřížkám obsazenosti. Naším cílem je u bodové mapy zachovat pozitivní vlastnosti mřížek obsazenosti a zmírnit jejich nevýhody. V první řadě jde o zachování možnosti filtrace šumu při vkládání dat do mapy. Během měření laserovým scannerem se může vyskytnout situace, kdy naměřená vzdálenost neodpovídá realitě a to zejména kvůli vícenásobnému odrazu nebo vzhledem k jevu mixed-pixel (kap. 7.1). Tato chybná měření jsou v datech náhodně rozptýlena, návrh výstavby bodové mapy počítá s jejich filtrací. Mřížky obsazenosti dovolují definovat velikost granularity reprezentace prostředí, v bodové mapě je tato vlastnost zachována v podobě možnosti volby mezibodových vzdáleností. Nepříjemná vlastnost mřížek obsazenosti je jejich paměťová náročnost, která je závislá na půdorysné velikosti prostoru bez ohledu na jeho zaplnění. Bodová mapa tuto vlastnost nemá, data popisují jen oblasti, kde se skutečně nalézají překážky. Z hlediska lokalizace je však pro nás nejdůležitější vlastnost bodových map, která nám umožňuje v mapě efektivně vyhledávat polohy nejbližších objektů k libovolné zvolené souřadnici. Tuto vlastnost zajišťuje vybudovaná vyhledávací struktura k-D strom, kterou mřížky obsazenosti ve své základní podobě postrádají. Podobnou informaci z nich sice lze získat, ale méně efektivně. Stavba bodové mapy ze sady naměřených laserových scanů se principiálně skládá ze třech základních kroků: 1. normalizace vzdáleností mezi naměřenými body, 2. filtrace šumu a málo významných bodů, 3. určení souřadnic vlastních mapových bodů. První dva kroky budování mapy spolu přímo souvisí, řeší problematiku filtrace dat vkládaných do mapy a omezují množství dat, které vstupuje do třetího kroku algoritmu. Během jízdy se mobilní robot opakovaně dostává do stejných míst v prostoru, popř. na jednom místě se pohybuje delší dobu než na jiných. To má za následek, že z těchto míst je k dispozici mnohem větší množství dat než z míst, kudy robot projel pouze jedenkrát. Velké rozdíly v hustotě dat by činily potíže v druhém kroku algoritmu, kdy se provádí filtrace dat na základě počtu sousedních bodů ve zvolené vzdálenosti. Proto je nutné přistoupit k normalizaci vstupních dat, která spočívá v omezení počtu bodů, vstupujících do druhého kroku algoritmu tak, aby žádné dva body neležely ve vzdálenosti menší než zvolená hodnota. Tato hodnota musí být o několik řádů menší než je rozlišení použitého senzoru. Algoritmus 8.1 pro normalizaci minimálních mezibodových vzdáleností pracuje následovně. Nejprve se nad vstupními daty vybuduje vyhledávací struktura k-D strom popsaná v kap. 8.3.1. Následně se v cyklu procházejí všechny body, ke kterým se

82

KAPITOLA 8. BODOVĚ ORIENTOVANÁ MAPA

Algoritmus 8.1: Normalizace vzdáleností bodů v mapě Vstup: množina bodů P = {pi | i ∈ h1; ni}, kde pi = [xi , yi ], minimální bodová vzdálenost dnorm min Výstup: množina bodů P = {pi | i ∈ h1; mi}, kde pi = [xi , yi ] 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38

for i ← 1 to n do wi ← 1, vi ← 1 end Vytvoř k-D strom ze vstupních bodů p1 . . . pn . kDtree ← kDtree make(P) k←2 repeat for i ← 1 to n do if vi = 1 then Vyhledej v k-D stromu nejbližšího souseda k bodu pi . {(idxj , distj )}kj=1 ← kDtree search(kDtree, pi , k) soused ← idx2 if distk < dnorm min then {(idxj , distj )}kj=1 ← kDtree search(kDtree, psoused , k) soused ← idx2 if soused = i then xi ← wi ∗ xi + xsoused /(wi + 1) yi ← wi ∗ yi + ysoused /(wi + 1) wi ← wi + wsoused vi ← 0 end end end end celkem bodu ← n, zdroj ← 0, cil ← 0 while zdroj ≤ celkem bodu do if vzdroj = 1 then pcil ← pzdroj , wcil ← wzdroj vcil ← 1 cil ← cil + 1 end else n←n−1 end zdroj ← zdroj + 1 end until celkem bodu 6= n m ← celkem bodu

8.4. STAVBA BODOVÉ MAPY

83

6

2.5

x 10

pocet bodu [−]

2

1.5

1

0.5

0 −5 10

−4

10

−3

10 limit vzdálenosti bodu [m]

−2

10

−1

10

Obr. 8.5: Závislost počtu bodů v mapě na zvolené normalizační vzdálenosti pro různá měření získaná ze stejného prostředí vyhledáváním ve stromu hledá nejbližší soused. Pokud je sousední bod nalezen v maximální definované vzdálenosti dnorm min , ověří se, zda i pro nalezeného souseda je výchozí bod, ze kterého se iniciovalo hledání též jeho nejbližším sousedem. Pokud ano, poloha výchozího bodu se upraví tak, že se posune do váženého těžiště obou bodů a nalezený nejbližší sousední bod se zruší. Celý cyklus, který prochází body a provádí jejich průměrování je potřeba iterativně opakovat tak dlouho, dokud se pro žádný bod nenalezne žádný soused ve vzdálenosti menší než dnorm min . Výsledkem tohoto postupu je, že žádné dva body nebudou mít mezi sebou menší vzdálenost než definovaná mez dnorm min . Tuto mez je třeba vhodně zvolit tak, aby v celém prostředí byla data rozprostřena s přibližně stejnou hustotou a zároveň nedocházelo k významnému úbytku počtu bodů, což by následně degradovalo kvalitu výsledné mapy. Obr. 8.5 znázorňuje závislost celkového počtu bodů, které zůstanou po normalizaci v datovém souboru v závislosti na zvolené hodnotě dnorm min . Jednotlivé křivky odpovídají různým měřením shodného prostředí. Správnou velikost dnorm min je třeba volit z oblasti horního zlomu křivky. Druhým krokem algoritmu stavby mapy je filtrace bodů, které v datech vznikly jako chybná měření. Tyto body se vyznačují tím, že v jejich bezprostředním okolí se nachází mnohem menší počet sousedů než u bodů, které odpovídají korektně změřené překážce v prostředí. Princip filtrace popsaný algoritmem 8.2 spočívá v jednorázovém průchodu všech bodů, které jsou výsledkem prvního kroku algoritmu 8.1. Pro každý bod se nalezne jeho k-nejbližších sousedů a pokud se k-tý nejbližší soused nachází ve větší vzdálenosti než rf lt , tak se zvolený bod z datového souboru vypustí. Posledním krokem je vytvoření vlastní výsledné mapy. Pro tento krok se využije zcela shodný algoritmus 8.1 jako pro normalizaci mezibodových vzdáleností, pouze se spustí

84

KAPITOLA 8. BODOVĚ ORIENTOVANÁ MAPA

Algoritmus 8.2: Filtrace osamocených bodů v mapě Vstup: množina bodů P = {pi | i ∈ h1; ni}, kde pi = [xi , yi ], filtrační poloměr rf lt , váha filtru k Výstup: množina bodů Q = {qi | i ∈ h1; mi}, kde qi = [xi , yi ] 1 2 3 4 5 6 7 8 9 10

Q←[] Vytvoř k-D strom ze vstupních bodů p1 . . . pn . kDtree ← kDtree make(P) for i ← 1 to n do Vyhledej v k-D stromu k-nejbližších sousedů pro bod pi . {(idxj , distj )}kj=1 ← kDtree search(kDtree, pi , k) if maxj=1..k (distj ) < rf lt then Přidej pi do množiny Q end end

se zcela jinou hodnotou parametru dnorm min . Velikost tohoto parametru definuje výslednou granularitu mapy, obvykle se nastavuje přibližně na velikost odpovídající rozlišení senzoru, případně o něco vyšší, typicky však ne o více než jeden řád. V opačném případě se pak již zcela zbytečně degradovala kvalita vytvářené mapy.

8.5

Simultánní lokalizace a mapování

V úvodní kapitole 8.1 byly popsány základní důvody k tomu, proč je pro lokalizaci přínosné využití mapy prostředí. Jakmile je k dispozici mapa prostředí, metodu PTP lze relativně snadno přizpůsobit pro použití s mapou. V metodě PTP se každý příchozí scan registruje do stávajícího souřadného systému robotu na základě minulého scanu. V první fázi lokalizace se využívá informace z odometrického systému robotu, který určí přibližnou změnu polohy robotu od přecházejícího scanu. Informace o natočení robotu se ještě upřesňuje histogramovou metodou pro korekci výrazných odometrických chyb (viz. kap. 7.3.1). Dále se v rámci druhé fáze PTP metody hledají korespondující bodové páry mezi referenčním a aktuálním scanem. Tato druhá fáze lokalizace je v PTM (Point-To-Map) metodě výrazně modifikována. Dostupná mapa prostředí umožňuje nahradit výpočet relativní meziscanové transformace pro registraci scanu daleko přesnějším výpočtem absolutní transformace pro registraci scanu v kontextu celé dostupné mapy. Výpočet absolutní transformace se opírá o stejný mechanismus hledání a validace bodových párů jako v případě PTP metody (kap. 7.3.2 7.3.4), ovšem korespondující body k bodům aktuálního registrovaného scanu nejsou hledány v referenčním scanu, nýbrž v existující bodové mapě. Hledání korespondujících bodů v mapě lze provést velmi efektivně, neboť všechny body mapy jsou uloženy ve vyhledávací struktuře k-D strom. V předchozí kapitole byl popsán postup budování bodové mapy, pokud máme k dispo-

8.5. SIMULTÁNNÍ LOKALIZACE A MAPOVÁNÍ

85

zici všechna dostupná měření, ze kterých mapu stavíme. Tento předpoklad však v případě lokalizace robotu v neznámém prostředí, tedy při řešení úlohy třídy SLAM, neplatí. Bodovou mapu je nutné budovat inkrementálně a pro lokalizaci použít její aktuální podobu, která zahrnuje všechny dosud získané informace o konfiguraci prostředí. Inkrementální budování mapy je založeno na přidávání vybraných bodů z nově příchozích laserových scanů do stávající mapy. Do mapy nelze přidávat všechny změřené body, je třeba provádět určitý druh filtrace tak, aby se počet bodů příliš nezvyšoval, pokud se robot pohybuje v již zmapovaném prostředí. Veškeré body, které tvoří mapu jsou uchovávány ve vyhledávací struktuře k-D strom, která je používána jak při samotné tvorbě mapy, tak i v lokalizaci. Algoritmus 8.3: Inkrementální budování bodové mapy Vstup: množina platných bodů z aktuálního scanu S = {si | i ∈ h1; ni}, kde si = [xi , yi ] a n je počet platných bodů ve scanu, stávající bodová mapa M ve formě k-D stromu, min vzdálenost bodů v mapě dmap min Výstup: aktualizovaná bodová mapa M ve formě k-D stromu 1 2 3 4 5 6 7 8

k←1 for i ← 1 to n do Vyhledej v k-D stromu nejbližšího souseda k bodu si . (idx, dist) ← kDtree search(M, si , k) if dist > dmap min then Přidej si do mapy M end end

Algoritmus inkrementálního budování mapy je inicializován vložením bodů z aktuálního scanu, další body z následujících scanů jsou pak vkládány podle algoritmu 8.3. Pro každý bod z aktuálního scanu je ve stávající mapě hledán euklidovsky nejbližší bod, pokud žádný bod v definovaném okolí (v max. vzdálenosti pointdist ) není nalezen, je nový bod vložen do stávající mapy. Tento postup je cyklicky opakován pro každý scan, přičemž registrace následujících scanů se již provádí vůči aktualizované mapě. Tento postup narozdíl od dávkové tvorby mapy, vede i na přidání případného šumu v datech, je to ovšem daň za možnost rychlé aktualizace mapy. Navíc, jen tímto způsobem zajistíme, aby se do mapy okamžitě přidaly všechny důležité znaky prostředí, které mohou být rozhodující pro spolehlivou lokalizaci. Pokud je žádoucí používat bodovou mapu pro jiný účel než k lokalizaci, je výhodnější použít dávkový způsob tvorby mapy popsaný v předchozí kapitole 8.4.

86

8.6

KAPITOLA 8. BODOVĚ ORIENTOVANÁ MAPA

SLAM a kooperativní úlohy s více roboty

Doposud jsme se zabývali pouze úlohou simultánní lokalizace a mapování pro jednoho robota. Rozšíření této úlohy pro více kooperujících robotů přináší nové problémy, které je potřeba řešit. Má-li skupina robotů vzájemně kooperovat na řešení týmové úlohy, je bezpodmínečně nutné, aby všechny roboty pracovaly ve společném souřadném systému nebo aby při vzájemné komunikaci měly možnost své pozice transformovat do souřadných systémů svých partnerů. Druhým klíčovým problémem, který má vliv na kvalitu lokalizace je existence pohybujících se partnerských robotů. Během řešení lokalizace každého z robotů může nastat situace, kdy např. dva roboty budou souběžně vykonávat trajektorie, které se nacházejí v těsné blízkosti. V tuto chvíli každý z nich má částečně zakryto zorné pole svého laserového scanneru a z tohoto důvodu může být lokalizace založená na registraci scanů vůči pevnému modelu prostředí komplikovaná. Vedle toho, výše popsaný algoritmus inkrementální tvorby mapy pro úlohy s více roboty není zcela vhodný, neboť pohybující se roboty by způsobily přidávání bodů do mapy prostředí, které neodpovídají pevným překážkám, ale pohybujícím se robotům. Řešení výše uvedených problémů se neobejde bez spolupráce s distribuovaným (popř. centralizovaným) multirobotickým řídicím systémem pro skupinu robotů (Chudoba et al., 2006). Z hlediska lokalizace a mapování se jedná zejména o možnost sdílení aktuálních pozic robotů v týmu a sdílení vytvářených mapových struktur. Pokud se roboty před začátkem řešení kooperativní úlohy nacházejí ve společné části prostoru tak, aby jejich senzorické systémy poskytovaly každému robotu informace o přibližně shodné části prostoru, je nutná na začátku mise stanovit společnou souřadnou soustavu. Společnou souřadnou soustavu lze definovat na základě znalosti vzájemných relativních poloh robotů v týmu. Jeden z robotů, který např. jako první získá senzorický scan prostředí, rozdistribuje tento výchozí referenční scan prostřednictvím multirobotického řídicího systému mezi ostatní roboty v týmu. Každý z robotů pak tento scan přijme jako referenční a metodou PTP vypočítá svoji aktuální relativní pozici vůči tomuto scanu. Získanou relativní pozici na základě znalosti polohy robotu, který poskytl referenční scan, přepočte na svoji absolutní pozici v rámci celé multirobotické komunity. Tímto způsobem se zajistí výchozí shoda lokálních souřadných systémů všech robotů. V tuto chvíli jsou splněny základní předpoklady, aby každý z robotů mohl začít řešit jádro nadřazené kooperativní úlohy. Nicméně, původní metodu PTM je nutné mírně modifikovat, aby si jednotlivé roboty stále udržely konzistentní souřadnou soustavu. Vlivem zbytkových kumulovaných lokalizačních chyb by se souřadné soustavy robotů s přibývajícím časem neustále více od sebe odchylovaly. Za tímto účelem je nutné metodu PTM provázat se strukturou sdílené mapy, kterou bude pro účely lokalizace používat a aktualizovat každý člen multirobotického týmu. Navržená struktura bodové mapy dovoluje přímou implementaci její sdílené varianty. Každý robot si udržuje vlastní interní podobu mapy prostředí ve vyhledávací struktuře k-D strom. Před aktualizací své mapy však robot zamkne přístup ke sdílené mapě pro ostatní členy

8.6. SLAM A KOOPERATIVNÍ ÚLOHY S VÍCE ROBOTY

87

týmu, vyžádá si její aktuální podobu, resp. její změny od své poslední aktualizační žádosti. Následně provede aktualizaci své mapy v kontextu aktuální sdílené mapy a zapracuje do ní svá senzorická měření. Tyto slučovací operace jsou velmi jednoduché a efektivní, použije se dříve popsaného algoritmu 8.3. Poté je obsah (rozdíl) lokální aktualizované mapy zapsán ve formě množiny bodů zpět do sdílené mapové struktury a povolen přístup ostatních robotů. Tento mechanismus zaručí, že každý z robotů se během svého pohybu lokalizuje vůči globálně konzistentní mapě a jedné souřadné soustavě. Druhý klíčový problém, kterým jsou pohybující se partnerské roboty ve společném pracovním prostředí, lze řešit na základě eliminace zpracování senzorických dat, které odpovídají odrazu od ostatních robotů. Přímé rozpoznání pohybujících se robotů v senzorických datech však není typicky potřebné, neboť multirobotický řídicí systém (Chudoba a kol., 2006) umožňuje sdílet informaci o aktuální poloze kooperujících robotů. Pro účely lokalizace a stavby mapy tedy stačí příslušná vybraná senzorická data vyloučit z dalšího zpracování. Tato data se nacházejí uvnitř kružnic o průměru odpovídající velikosti příslušného robotu se středem v jeho aktuální známé pozici. Na obr. 8.6 jsou tyto data označena křížkem. V tomto případě, kdy z laserového scanu je vyloučen určitý počet bodů má za následek, že musí být příslušným způsobem upravena pravidla v metodě PTP pro validaci výsledků lokalizace. Konkrétně to znamená, snížit min. počet korektně nalezených bodových párů o počet bodů, které byly vyloučeny ze zpracování.

Obr. 8.6: Vyloučená senzorická data z dalšího zpracování Problematika kooperativní lokalizace a mapování je velice široká, zejména pokud kooperující roboty explicitně neznají své vzájemné pozice a pracují na lokálních souřadných systémech. Tato témata vyžadují explicitní rozpoznávání robotů v senzorických datech, vzájemná vyjednávání mezi roboty, vykonávání předem dohodnutých manévrů apod. a souvisí již s jinými tématy, zejména s multiagentními systémy (Šišlák et al., 2005). V experimentální části práce jsou rozebrány výsledky základních navržených rozšíření PTM metody pro prostředí s více roboty.

Kapitola 9

Rozšíření do 3D 9.1

Lokalizace polohy ve 3D

V předchozích dvou kapitolách jsme se zabývali lokalizací robotu v rovině. Pro řešení této úlohy (kap. 7) se stačilo omezit na určení polohy robotu v planárním prostředí využívající senzorická hloubková data sbíraná pouze v jedné rovině. Lokalizace založená na registraci 3D scanů nachází své uplatnění v systémech, kde lokalizace pomocí horizontálního laserového scanneru selhává, což může nastat například pokud se robot pohybuje v členitém terénu. Opustí-li mobilní robot prostředí interiérového typu, kde se typicky pohybuje pouze v rovině podlahy, musíme se zabývat problémem lokalizace v prostoru v 6-ti stupních volnosti. Určujeme tedy polohu v podobě tří souřadnic [x,y,z ] a úhlová natočení kolem třech lineárně nezávislých os jak je znázorněno na obr. 9.2. Úhlová natočení α, β a γ se obvykle vztahují k osám kartézské soustavy souřadnic a často se nazývají též roll, pitch a yaw.

Obr. 9.1: Znázornění pohybu rovinného senzoru pro sběr prostorových dat V tomto případě pro určení všech šesti parametrů již nemůžeme vystačit s pevným horizontálně montovaným scannerem. Místo rovinného senzoru je třeba použít senzor poskytující prostorovou informaci z prostředí. Nejlepší možná varianta z hlediska vysoké přesnosti získaných dat pro lokalizaci, trojrozměrný scanner, se v mobilní robotice příliš 89

90

KAPITOLA 9. ROZŠÍŘENÍ DO 3D

nepoužívá z důvodu relativně dlouhé doby potřebné na získání jednoho 3D scanu. V rámci naší práce jsme pro získávání prostorových dat použili servomotorem úhlově polohovaný (naklápěný) laserový scanner, který nám umožní získat prostorová data ze svazku rovin mající uzel v bodě otáčení laserového scanneru (viz. obr. 9.1). Důležitý předpoklad pro tento způsob lokalizace robotu v prostoru je podmínka stacionárního sběru dat, mobilní robot se tudíž v době snímání dat z prostoru nesmí pohybovat, jinak by data byla nekonzistentní. Vlastní základní princip navržené lokalizace zůstává ve 3D v základních rysech obdobný jako v dříve popsané metodě PTP. Zásadní rozdíl je však v postupu minimalizace kritéria 7.11 a v postupu zamítání nalezených bodových párů. Při rozepsání kritéria 7.11 potřebujeme analyticky minimalizovat celkem šest parametrů, z tohoto důvodu je nutné použít při analytickém odvození teorie quaternionů, pro naše účely stručně popsané v kap. 9.1.2. Vlastní popis PTP metody pro použití ve 3D je součástí navazující kapitoly 9.1.3.

9.1.1

Reprezentace polohy tělesa v prostoru

Pozice těleso v prostoru je popsána šesti parametry, popis pozice je snadný, je to uspořádaná trojice [x, y, z]. Složitější situace nastává s popisem orientace. Ta se udává třemi úhly, ovšem jejich interpretace bývá často různorodá. Základem popisu bývá vždy velikost úhlu rotace kolem jednotlivých tří souřadných os. Liší se však pořadím rotačních os a tím, zda se postupně aplikované rotace provádějí vůči pevné souřadné soustavě, nebo vůči souřadné soustavě otáčeného tělesa. V druhém případě se popis druhé a třetí rotace provádí kolem již transformovaných os vzniklých na základě předchozích rotací. Této reprezentaci se říká Eulerovy úhly. Bohužel, literatura (Lawrence, 1998), (Luethi a Moser, 2000), (Stengel, 2004) a další se neshodují v jednotných konvencích, v jakém pořadí a podle kterých os má těleso rotovat. My použijeme konvenci, která popisuje pozici a rotaci tělesa v následujícím pořadí a označení (viz. obr. 9.2): 1. translace mezi souřadnými soustavami [x0 , y0 , z0 ] → [x, y, z], 2. rotace o úhel γ kolem osy z (yaw), 3. rotace o úhel β kolem osy y 0 (pitch), 4. rotace o úhel α kolem osy x00 (roll). Každou dílčí rotační transformaci můžeme popsat pomocí rotační matice jako 







x0 x  0     y  = R y , z0 z

(9.1)

kam se za R dosadí rotační matice vztažené k rotaci kolem příslušných os ve tvaru:  

1

0

0

 

 Rx =   0 cos (α) − sin (α)  0 sin (α) cos (α)

9.1. LOKALIZACE POLOHY VE 3D

91

Obr. 9.2: Popis polohy pevného tělesa v prostoru, podle (Luethi a Moser, 2000) 

cos (β)



0

Ry =  

0 sin (β)



1

  

0

− sin (β) 0 cos (β)



cos (γ) − sin (γ) 0



Rz =   sin (γ) 0

cos (γ) 0

 

0   1

Celkovou rotaci tělesa lze pak popsat jednou transformační maticí R jako: R = Rz Ry Rx = 

cos γ cos β − sin γ cos α + cos γ sin β sin α

  sin γ cos β 

− sin β

cos γ cos α + sin γ sin β sin α cos β sin α

sin γ sin α + cos γ sin β cos α

 

− cos γ sin α + sin γ sin β cos α  

(9.2)

cos β cos α

Reprezentace natočení tělesa v prostoru pomocí Eulerových úhlů není však pro naše účely příliš použitelná, obsahuje singularity a pro další operace tato reprezentace není vhodná. Další možností, jak je možné reprezentovat orientaci tělese v prostoru je pomocí jednoho daného směrového vektoru v prostoru a velikosti rotace kolem něho. Pro tyto účely je vhodný matematický aparát využívající quaternionové formalizace. Zavedení quaternionů spolu s jejich základními vlastnostmi, se bude věnovat následující kapitola. Popis je záměrně zaměřen jen na vlastnosti, které budeme dále využívat a je veden bez důkladného matematického odvození a důkazů, které jsou dohledatelné v literatuře (Horn, 1987).

92

9.1.2

KAPITOLA 9. ROZŠÍŘENÍ DO 3D

Quaterniony

Quaterniony jsou algebraickou strukturou ve formě čtyřsložkového vektoru. Lze je chápat jako skalár a trojrozměrný směrový vektor nebo jako rozšířené komplexní číslo s jednou reálnou a třemi imaginárními složkami. V dalším textu budeme pro quaterniony používat symbolů s tečkou nad svým označením. Každý quaternion lze zapsat jako q˙ = q0 + i qx + j qy + k qz ,

(9.3)

kde q0 je reálná část quaternionu a qx , qy , qz jsou imaginární části. Podobně jako u komplexních čísel, pro symboly imaginárních částí i, j a k jsou definovány operace násobení následovně: i2 = −1, j 2 = −1, k 2 = −1, ij = k, jk = i, ki = j, (9.4) ji = −k, kj = −i, ik = −j, Z toho pak vyplývá, jak se násobí dva quaterniony. Máme-li druhý quaternion r˙ = r0 + i rx + j ry + k rz ,

(9.5)

pak sčítání a násobení dvou quaternionů r˙ a q˙ vypadá následovně: r˙ + q˙ = r0 + q0 + i(rx + qx ) + j(ry + qy ) + k(rz + qz ) r˙ q˙ = (r0 q0 − rx qx − ry qy − rz qz ) + i (r0 qx + rx q0 + ry qz − rz qy ) + j (r0 qy − rx qz + ry q0 + rz qx ) + k (r0 qz + rx qy − ry qx + rz q0 ). Výsledek násobení q˙r˙ vypadá podobně, pouze jsou změny ve znaménkách. Pro quaterniony obecně platí, že násobení není komutativní, r˙ q˙ 6= q˙r. ˙ Násobení quaternionů lze reprezentovat též jako násobení ortogonální matice 4x4 čtyřsložkovým sloupcovým vektorem následovně: 

r0 −rx −ry −rz rx r0 −rz ry ry r z r0 −rx rz r y rx r0







r0 −rx −ry −rz    r r0 rz −ry      ¯ q. r˙ q˙ =  ˙ q˙r˙ =  x ˙  q˙ = Rq,  q˙ = R    ry −rz r0 rx  rz ry −rx r0 (9.6) ¯ se liší jen transpozicí pravé dolní 3x3 submatice, důležité však je, že Matice R a R ¯ shodný a je zároveň roven skalárnímu součet kvadrátů ve všech sloupcích je pro R i R 2 součinu dvojice shodných quaternionů r˙ · r˙ = r0 + rx2 + ry2 + rz2 . Skalární součin dvou quaternionů r˙ a q˙ je pak roven r˙ · q˙ = p0 q0 + px qx + py qy + pz qz .

(9.7)

Pro další práci je třeba zavést pojem konjungovaný quaternion q˙∗ , který má opačná znaménka imaginárních částí (9.8) r˙ ∗ = r0 − i rx − j ry − k rz .

9.1. LOKALIZACE POLOHY VE 3D

93

Pokud vynásobíme quaternion s jemu příslušejícím konjungovaným quaternionem, dostaneme reálné číslo: q˙ · q˙∗ = (q02 + qx2 + qy2 + qz2 ) = q˙ · q. ˙ (9.9) Mezi další vlastnosti quaternionů vzhledem k ortogonalitě matic Q patří následující ekvivalence (Horn, 1987): (q˙p) ˙ · (q˙r) ˙ = (Qp) ˙ · (Qr) ˙ = (Qp) ˙ T (Qr) ˙ = p˙T QT Qr˙ = p˙T (q˙ · q)I ˙ r, ˙

(9.10)

ze kterých lze odvodit: (q˙p) ˙ · (q˙r) ˙ = (q˙ · p)( ˙ q˙ · r) ˙

(9.11)

(q˙p) ˙ · r˙ = p˙ · (r˙ · q˙∗ )

(9.12)

Dále platí: Quaternion lze vedle reprezentace r˙ = r0 + i rx + j ry + k rz reprezentovat též jako θ θ θ θ + v1 sin i + v2 sin j + v3 sin k. 2 2 2 2 

r˙ = cos











(9.13)

Toto vyjádření má i geometrickou reprezentaci a to takovou, že quaternion lze principiálně vyjádřit jako reprezentaci 3D rotaci kolem osy tvořenou vektorem v = (v1 , v2 , v3 ) o velikosti θ. V dalším textu budeme pracovat s jednotkovým quaternionem q, pro které platí q˙ · q˙ = (q02 + qx2 + qy2 + qz2 ) = 1. Geometrická reprezentace rotace reprezentovaná quaternionem je zobrazena na obr. 9.3. V tomto kontextu může být quaternion vyjádřen jako dvojice r˙ = (a, v), přičemž pro násobení dvou quaternionů r˙1 r˙2 platí, že a = a1 a2 − v1 · v2 a v = a1 v2 + a2 v1 − v1 × v2 . Reprezentaci 3D rotace pomocí jednotkového quaternionu q˙ lze popsat rovnicí r˙ 0 = q˙r˙ q˙∗ ,

(9.14)

která každému čistě imaginárnímu quaternionu r˙ přiřadí též čistě imaginální quaternionu ¯ T (Qr) ¯ T Q)r, ¯ r˙ 0 , což je důsledkem ekvivalencí p˙r˙ q˙∗ = (Qr) ˙ q˙∗ = Q ˙ = (Q ˙ kde matice Q a Q odpovídají quaternionu q˙ podle vztahu 9.6:  

¯T Q =  Q  

q˙ · q˙ 0 0 0 0 qx2 + q02 − qz2 − qy2 2 qx qy − 2 q0 qz 2 qx qz + 2 q0 qy 0 2 qx qy + 2 q0 qz qy2 − qz2 + q02 − qx2 2 qy qz − 2 q0 qx 0 2 qx qz − 2 q0 qy 2 qy qz + 2 q0 qx qz2 − qy2 − qx2 + q02

    

2π-θ θ

v

-v

Obr. 9.3: Dvě ekvivalentní reprezentace 3D rotace quaternionem

(9.15)

94

KAPITOLA 9. ROZŠÍŘENÍ DO 3D

V případě, že budeme pracovat s jednotkovým quaternionem q˙ a čistě imaginárním quaternionem r˙ = (a, v), lze na základě vztahu 9.14 - 9.15 napsat: v0 = Rv, 

2(q02 + qx2 ) − 1

kde

(9.16) 

2(qx qy − q0 qz ) 2(qx qz + q0 qy )  2(q02 + qy2 ) − 1 2(qy qz − q0 qx )  2 2 2(qx qz − q0 qy ) 2(qy qz + q0 qx ) 2(q0 + qz ) − 1

 R =  2(qx qy + q0 qz )

(9.17)

¯ T Q, která je navíc ortonormální díky ortonormálje pravá dolní 3x3 podmatice matice Q ¯ T Q a jednotkovému q, nosti výchozí matice Q ˙ pro který platí q˙ · q˙ = 1. Důležitou vlastností matice R je, že lze ukázat její ekvivalentnost s rotační maticí ze vztahu 9.2. Pro popis modifikace PTP algoritmu do 3D je důležité, že souřadnice libovolného prostorového bodu Pi = [xi , yi , zi ] mohou být vyjádřeny čistě imaginárním quaternionem p˙ = 0 + i xi + j yi + k zi . Ze vztahu 9.14 platí, že p˙0 = q˙r˙ q˙∗ , kde imaginární složky quaternionu p˙0 = 0 + i x0i + j yi0 + k zi0 určují souřadnice bodu Pi0 = [x0i , yi0 , zi0 ]. Souřadnice bod Pi0 jsou tedy definovány souřadnicemi bodu Pi po aplikaci rotační transformace, která je určena quaternionem q. ˙

9.1.3

Modifikace PTP metody pro 3D

Lokalizační metoda PTP pro použití ve 3D vyžaduje upravit strategii výběru korespondujících bodů, což je jeden z kritických kroků, na němž závisí úspěšnost celé metody. Na rozdíl od 2D varianty se pracuje s mnohem větším počtem bodů, mezi nimiž je nutné najít korespondence. Výchozím krokem je vytvoření vyhledávací struktury k-D strom, která bude v každé iteraci využívána pro hledání nejbližších sousedů. Vyhledávací struktura je vytvořena nad všemi body z referenčního scanu. Dála se spustí iterační cyklus, jehož ukončovací podmínkou je pokles velikosti korekčních parametrů (posun ve třech osách a rotace kolem tří os) pod stanovenou mez. V každé iteraci se hledají nejbližší body ke každému bodu z registrovaného scanu ve vyhledávací struktuře za účelem stanovení kandidátů bodových párů. Registrovaný scan pochopitelně bude obsahovat množství bodů, které v referenčním bodu nemohou nalézt žádný korespondující bod, nebo korespondence mezi registrovaným a referenčním scanem nebudou stanoveny správně. Většinu těchto párů je třeba před minimalizační fází ICP algoritmu ze zpracování vyloučit. Bohužel, kritérium navržené v kap. 7.3.3 v prostoru nepracuje správně. Důvodem je mnohem méně výrazný zlom v posloupnosti bodových vzdáleností (viz. obr. 7.19). Mezibodové vzdálenosti v 3D rostou v nižších hodnotách téměř lineárně (viz obr. 9.4), výrazný zlom, který by zde určoval kritickou vzdálenost dr není možné jednoznačně určit tak, jako v dvojrozměrném případě. Přirozeně, bodové páry mající vzájemnou vzdálenost větší než je povolená mez dmax z dalšího zpracování automaticky vyřazujeme. Pro lokalizaci nás zajímají zejména kandidáti s menší vzdáleností než dmax , ale i u těchto zbylých musíme rozhodnout o velikosti kritické vzdálenosti dr . Pro určení kritické vzdálenosti dr , která přímo rozhoduje o tom, které korespondující páry bodů postoupí do minimalizačního kritéria, jsme navrhli následující postup. Seřazené vzdálenosti bodových párů vykazují vzrůstají vzájemné rozdíly. Spočítáme-li průběh

9.1. LOKALIZACE POLOHY VE 3D

95

rozdíl vzdáleností −→

křivky rozdílů vzdáleností (horní graf z obr. 9.4), lze ukázat, že zde již existuje znatelný nárůst rozdílů bodových vzdáleností. Pozice zvýšeného nárůstu diferencí bude určovat kritickou mezibodovou vzdálenost dr .

vzdálenost −→

pořadí bodového páru (po setřídění podle vzdálenosti) −→

dmax dr pořadí bodového páru (po setřídění podle vzdálenosti) −→

Obr. 9.4: Setříděné vzdálenosti bodových párů a křivka jejich diferencí ve 3D, barevně jsou odlišeny křivky vzniklé v různých iterací, výchozí iterace má modrou barvou, červená je během činnosti a černá barva znázorňuje vzdálenosti párů v konečném stavu. Křivka diferencí pro výpočet však musí být vyhlazena (obr. je již po vyhlazení), neboť přímé diference vykazují příliš vysoký šum, ve kterém se zlomy křivky diferencí ztrácejí. Vyhlazení jsme provedli klouzavým průměrováním v posuvném okně o velikosti řádu stovek bodových párů. Na křivce diferencí je nalezena velikost maximální diference maxdif f z oblasti zahrnující validních bodových párů, maxdif f je tedy počítána z bodových párů mající vzájemnou vzdálenost menší než dmax . Na základě hodnoty maxdif f se určí bodový pár s minimální mezibodovou vzdáleností dr , jehož následovník v setříděné posloupnosti má mezibodovou vzdálenost větší než dr + 2/3 · maxdif f . Všechny bodové páry mající mezibodovou vzdálenost menší než dr jsou prohlášeny za nalezené vzájemně korespondující páry bodů. Jakmile známe korespondence bodů, je podobně jako v kap. 7.3.4 nutné nalézt transformaci od referenčního scanu k aktuálnímu scanu, která bude minimalizovat kritérium vzájemné mezibodové vzdálenosti: Edist (T, ω) =

n X

|Rω pi + T − p0i |2 ,

(9.18)

i=1

Určení velikosti translace T mezi body vychází z výpočtu rozdílů polohy těžišť bodových množin, výpočet 3D rotační matice R je však o něco komplikovanější, pro její určení je použito quaternionové reprezentace.

96

KAPITOLA 9. ROZŠÍŘENÍ DO 3D

Mějme množinu quaternionů r˙i = i xi + j yi + k zi a r˙i0 = i x0i + j yi0 + k zi0 , jejichž imaginární složky odpovídají souřadnicím referenčních, resp. aktuálně registrovaných bodů. Každý čistě imaginární quaternion představuje vektor v 3D, přičemž naším cílem je nalézt takovou rotační transformaci reprezentovanou jednotkovým quaternionem q, ˙ že rozdíl 0 směrů všech dvojic vektorů r˙i a r˙i bude minimální. Tuto vlastnost splňuje maximalizace skalárního součinu dvojice quaternionů. Jeden quaternion představuje registrovaný bod v prostoru, druhý optimálně transformovaný referenční bod. Naší snahou tedy bude na základě vztahu 9.14 najít takový jednotkový q, ˙ který maximalizuje max q˙

n X

(q˙r˙i q˙∗ ) · r˙i0 .

(9.19)

i=1

Tento vztah může být použitím vztahu 9.12 upraven na: max q˙

n X

(q˙r˙i ) · (r˙i0 q). ˙

(9.20)

i=1

Pokud vztah 9.20 přepíšeme do maticové podoby pomocí ekvivalencí    

r˙ q˙ = 

0 −xi −yi −zi xi 0 −zi yi y i zi 0 −xi zi −yi xi 0





  ˙  q˙ = Ri q, 

  

r˙ 0 q˙ = 

0 −x0i −yi0 −zi0 x0i 0 zi0 −yi0   ¯ 0 q,  q˙ = R i˙ yi0 −zi0 0 x0i  zi0 yi0 −x0i 0 (9.21) 

dostaneme dalšími úpravami: n X

¯ i q) (R ˙ · (Ri q) ˙ =

n X

T ¯ 0T q˙ R i Ri q˙ = q˙ T

! T ¯ 0T R i Ri q˙ = q˙

i=1

i=1

i=1

n X

n X

!

Ni q. ˙

(9.22)

i=1

¯ T Ri je symetrická, pokud položíme N = ni=1 Ni , matice N bude Každá z matic Ni = R i též symetrická, jejíž prvky jsou sumy součinů příslušných souřadnic bodů z referenčního a registrovaného scanu. P

   

N =

Sxx + Syy + Szz Syz − Szy −Sxz + Szx Sxy − Syx Syz − Szy Sxx − Szz − Syy Sxy + Syx Sxz + Szx −Sxz + Szx Sxy + Syx Syy − Szz − Sxx Syz + Szy Sxy − Syx Sxz + Szx Syz + Szy Szz − Syy − Sxx

   , 

kde Sxx =

n X i=1

xi x0i ,

Sxy =

n X

xi yi0 ,

Sxz =

i=1

n X i=1

xi zi0 ,

Szz =

n X

zi zi0 , atd.

i=1

Po vyjádření podoby matice N je třeba nalézt q, ˙ který maximalizuje výraz q˙T N q. ˙

(9.23)

Tímto q˙ je vlastní vektor odpovídající největšímu kladnému vlastnímu číslu matice N. Jakmile vypočítáme složky tohoto vektoru, hledanou rotační transformační matici R získáme ze vztahu 9.17.

9.2. 3D MODEL Z 2D DAT

9.2

97

3D model z 2D dat

V kapitole 8 jsme se zabývali vytvářením dvojrozměrného modelu prostředí zejména pro účely lokalizačních algoritmů. Dvojrozměrný model je často použitelný i pro vzdálenou navigaci robotu člověkem, ovšem chybějící prostorová informace může být často zásadní. Pro případ vzdálené teleoperace robotu člověkem je vhodné předem vybudovat virtuální trojrozměrný model, ve kterém se člověk jednak lépe orientuje a přirozeně obsahuje i více informací. S tímto modelem mohou též pracovat systémy plánování trajektorie nebo systémy automatického předcházení kolizím. I přesto, že i nadále bude pohyb robotu plánován ve 2D, lze díky modelu prostředí vzít v úvahu i překážky, které omezují pohyb robotu, ale nejsou viditelné rovinným senzorem. Nejjednodušším 3D modelem je bodový model se stejnými vlastnostmi jako 2D model popisovaný v kap. 8, pouze rozšířený o třetí souřadnici. Bodový model lze získat složením lokalizovaných 3D scanů, které získáme např. z vertikálně polohovaného laserového scanneru tak, jak byl použit v kap. 9.1. Získávat 3D model tímto způsobem je sice možné, ale robot musí být v době snímání celého scanu zastaven, jinak by sejmutý scan nebyl konzistentní. Druhou nevýhodou je přirozená struktura prostorového rozložení bodů, prostor v blízkosti otočné osy laserového scanneru je vzorkován s mnohem větší hustotou než ostatní části prostoru.

Obr. 9.5: Konfigurace laserových scannerů pro sběr 3D dat Pro účely vytváření trojrozměrného modelu světa současně během jízdy robotu jsme navrhli vhodnou konfiguraci senzorů a zpracování získaných dat. Použití dále popsaného postupu je však omezeno podmínkou pohybu robotu v jedné horizontální rovině. Využijeme přesné 2D lokalizace v rovině z horizontálního laseru, která nám umožní vytvořit trojrozměrného modelu prostředí na základě jednoduché superpozice dat z laserového scanneru, který je umístěný kolmo k podlaze směrem vzhůru. Vytvořený bodový 3D model se skládá z neuspořádané množiny prostorových bodů a je základním vstupem řady algoritmů pro rekonstrukce povrchů a geometrické modelování prostředí. Geometrické modely nalézají svá uplatnění v systémech virtuální reality, ale též i pro vlastní navigaci mobilního robotu např. během teleoperace.

Kapitola 10

Experimentální výsledky 10.1

Popis experimentální platformy

Experimentální výsledky byly získány použitím dvou typů mobilních robotů. Data z vnitřních prostředí byla převážně získávána použitím robotu G2, experimentální část pracující s daty z exteriéru byla realizována využitím robotu Pioneer 3AT. V následujících dvou kapitolách následuje popis obou robotů a hlavního použitého senzoru SICK LMS-200.

10.1.1

Mobilní robot G2

Experimentální mobilní robot G2 je robot non-holonického typu, který byl vyvinut ve skupině Inteligentní mobilní robotiky Gerstnerovy laboratoře na Katedře kybernetiky Elektrotechnické fakulty ČVUT. Základní rám robotu tvoří pravoúhlé hliníkové profily, pohonový subsystém robotu (krokové motory, převody, kola a řídicí jednotka) vychází z mobilního robotu ER1 firmy Evolution Robotics a dosahuje maximální rychlosti 0.5 m/s. Rozchod kol 38 cm robotu umožňuje maximální rychlost otáčení se na místě 3.75 rad/s.

Obr. 10.1: Experimentální mobilní robot typu G2 Robot je vybaven vestavěným PC pracujícím na hodinové frekvenci 677 MHz, vzdálená komunikace je řešena pomocí WiFi připojení typu IEEE802.11b (11Mbps). Vlastní řídicí 99

100

KAPITOLA 10. EXPERIMENTÁLNÍ VÝSLEDKY

systém robotu je implementován v jazyce Java a je spouštěn na OS Linux s verzí jádra 2.6, distribuce založena na Gentoo Linux. Jednotlivé funkční bloky, senzory a periferie jsou hvězdicovou architekturou propojeny sériovými komunikačními linkami na bázi rozhraní RS232 a USB. Mezi základní senzorické vybavení patří laserový scanner SICK LMS 200 a inkrementální odometrie, která je integrována přímo do řídicí jednotky krokových motorů. Odometrické impulsy jsou odvozovány přímo od akčních zásahů za motory. Volitelně je možné robot vybavit sadou krátkodosahových infračervených distančních senzorů Sharp GP2D02, inerciální měřící jednotkou Crossbow IMU-300C (obsahující trojosý akcelerometr s rozsahem do ±2g a trojosý vibrační gyroskop s rozsahem ±100◦ /sec.) či WEB kamerou s polohovacím zařízením se dvěma stupni volnosti. Robot má omezenou užitečnou nosnost, zátěž by neměla přesáhnout 4 kg, což však ještě umožňuje zvýšit palubní výpočetní výkon např. připevněním přídavného notebooku přímo na robot.

10.1.2

Mobilní robot Pioneer

Mobilní robot Pioneer 3AT je čtyřkolový robot s diferenciálním pohonem synchronně poháněné dvojice kol. Kinematika tohoto robotu je blízká pásovému vozidlu, zatáčí smykem, pohonový systém je předurčen zejména pro jízdu ve venkovním prostředí. Výrobcem robotu vyobrazeném na obr. 10.2 je firma ActiveMedia Robotic.

Obr. 10.2: Experimentální mobilní robot typu Pioneer 3AT Robot je široce konfigurovatelný, mezi jeho standardní a doplňkovou výbavu patří odometrie, gyroskop, inklinometry, mechanické nárazníky, sonarový věnec, kamery, mechanická paže s pěti stupni volnosti a úchopovým mechanismem. Řízení pohonů robotu spolu s HW rozhraním k senzorům zajišťuje systém ARCOS založený na mikroprocesoru Hitashi H8S. Uživatel robotu pracuje s embedded PC systémem Intel Pentium III 850MHz s 256 MB RAM a 20GB v OS Linux, na kterém je možné spouštět uživatelské úlohy pracující s HW robotu přes SW interface ARIA zpřístupňující používané senzory.

10.1. POPIS EXPERIMENTÁLNÍ PLATFORMY

101

Na rozdíl od robotu G2 výrobce deklaruje užitečnou zátěž až 35 kg, což umožňuje montáž množství přídavných systémů. V praxi však robot není možné zatěžovat plnou zátěží, neboť pak má robot kvůli zvýšenému tření mezi koly a podkladem potíže při smykovém zatáčení. Jiný způsob zatáčení kinematika tohoto robotu neumožňuje.

10.1.3

Laserový scanner SICK LMS-200

V experimentální části této práce bude používán laserový scanner SICK LMS-200 jako hlavní zdroj dat, proto se zde budeme podrobněji zabývat jeho vlastnostmi. SICK LMS-200 je senzor průmyslového typu dodávaný ve velmi robustním provedení. S nadřazeným systémem komunikuje buď po sériové lince RS232 nebo RS485. Senzor umožňuje měřit nejen vzdálenost překážky, ale též i sílu odrazu laserového paprsku. Podle síly odrazu je možné usuzovat na druh materiálu či barvu příslušné detekované překážky. Laserový puls je senzorem emitován v příslušném směru a difúzním odrazem se vrací zpět k senzoru. Doba mezi vysláním laserového pulsu, odražením se od překážky a jeho následnou detekcí je použita k výpočtu vzdálenosti příslušné překážky. Směr emise laserového paprsku je ovlivňována rotujícím zrcadlem, které rozmítá měření vzdálenosti do jedné poloroviny, jak je vidět na obr. 5.4 a 5.5. Rychlost otáčení zrcadla je 4500 ot./min. Přirozené úhlové rozlišení senzoru je 1◦ , je však možné jej přepnout do režimu 2 nebo 4 prokládaných scanů, a získat tím tak až 0.25◦ rozlišení. Senzor dále umožňuje měřit v 80 m rozsahu nebo 8 m s vyšším milimetrovým rozlišením, které je však již pod ±15 mm chybou měření. Chyba měření mírně driftuje s časem, ustálí se přibližně po 3 hod. provozu, kdy dojde k ustálení tepelných poměrů uvnitř senzoru. Další technické parametry shrnuje tab. 10.1. Tabulka 10.1: Technické parametry laserového hloubkoměru SICK LMS-200 Úhel záběru 100/180◦ Úhlové rozlišení 0.25 / 0.5 / 1◦ Minimální detekovatelná vzdálenost 0.1 m Maximální detekovatelná vzdálenost 8.191 / 81.91 m Šířka vysílaného signálu 0◦ Přesnost měření ±15 mm s std.odch. 5mm (v 8 m modu) Doba měření 10 ms Max.frekvence scanů 3 / 75 Hz Komunikační rychlost 9.6 / 19.2 / 38.4 / 500 Kbaud Většina realizovaných experimentů pracovala se senzorem SICK LMS-200 v 80 m módu s 0.5◦ úhlovým rozlišením. Navržené metody pracují i jinými laserovými scannery, experimenty popisované v kap. 10.4.1 a 10.4.3 byly provedeny se scannerem SICK PLS 100. Po drobnějších úpravách souvisejících zejména s nastavením příslušných parametrů by metody měly být adaptovatelné i pro další typy senzorů, jako jsou např. scannery Hokuyo PBS-03JN, PB9-12, PB9-22, URG-04LX atd.

102

KAPITOLA 10. EXPERIMENTÁLNÍ VÝSLEDKY

10.1.4

Zdroje chyb

Na experimentální výsledky popisované v této práce mají vliv tři základní zdroje chyb. Je to chyba způsobená danou mechanickou konstrukcí robota, jež má vliv na výsledné přesnosti určování polohy (odometrie), chyby měření vyplývající z vlastností laserového hloubkoměru a chyby časové synchronizace dat z různých senzorů způsobené komunikačními zpožděními. Chyby způsobené mobilním robotem Pohyb robota je realizován dvěma nezávisle poháněnými koly. Vzhledem k tomu, že odometrická informace o ujeté dráze kopíruje akční zásahy na motory, není možné na úrovni odometrie detekovat situaci, kdy se kola robotu protáčejí namístě. Do měření tedy nepříznivě vstupují vlivy související s adhezí mezi koly a podložkou. Při měření ujeté dráhy a změny polohového úhlu nejvýraznější chybu způsobují: • Nerovnost podložky, kdy může dojít ke změně efektivního obvodu kol např. díky boření kol. • Příliš hladká podložka (např. navoskovaná podlahová krytina PVC), kdy dochází k prokluzování pohonných kol, zejména při prudších manévrech, jako je prudký rozjezd, razantní změna směru jízdy apod. • Konečná šířka pláště kol, která při otáčení způsobuje, že není přesně definován bod styku pláště a podložky. V důsledku toho se mění efektivní rozchod kol podvozku, který ovlivňuje měření úhlového natočení. Experimentálně bylo zjištěno, že za příznivých podmínek se odometrická chyba polohy na vzdálenosti 1 m pohybuje v řádu centimetrů a chyba úhlového natočení nepřesáhne 3◦ . Tyto chyby se mohou ovšem při velmi nepříznivých podmínkách (značné prokluzování kol na hladké podlaze, pohyb po hrbolaté či měkké podložce, velký schod apod.) podstatně a nepředvídatelně zvětšit. Bohužel negativní vliv prostředí na odometrické měření polohy nelze nijak předpokládat, ani přímo korigovat (Chmelař, 1998). Vzhledem k tomu, že měření polohy a úhlového natočení má integrační charakter, při dlouhých trajektoriích absolutní chyba měření může dosáhnout vysokých hodnot. Chyby měření způsobené laserovým hloubkoměrem Chyby měření způsobené vlastním hloubkoměrem mají přímý vliv na výslednou kvalitu výsledků algoritmů jak lokalizace, tak mapování. Můžeme je rozdělit do těchto skupin: • Vliv kvality prostředí - princip činnosti hloubkoměru je založen na difúzním odrazu světelného paprsku od překážky, proto pokud dojde v prostředí k zrcadlovému odrazu, dochází ve většině případů ke ztrátě signálu, popř. změření vzdálenosti vyplývající z několikanásobného odrazu. Tyto případy naštěstí nejsou příliš časté, problémy tohoto charakteru se projevují zejména u sonarového hloubkoměru.

10.2. POPIS REALIZOVANÝCH EXPERIMENTŮ

103

• Absolutní chyba měření hloubkoměru ( ±1 − 10 mm ) a rozlišovací schopnost v horizontální rovině je 0.5−1◦ podle použitého měřícího režimu. Velikost chyby je v celém 180◦ rozsahu zorného pole konstantní. • Systematická chyba způsobená měřením v jediné horizontální rovině ve výšce přibližně 20 cm nad podlahou. Důsledkem toho je, že překážky ležící pod nebo nad touto horizontální rovinou nejsou vůbec registrovány. • Detekce falešných objektů způsobených nestabilitou odrazu na hranách překážek, jak bylo popsáno v kap. 10.1.3. Chyby závislé na komunikačních zpožděních Lokalizační algoritmy využívají dvou základních senzorů, odometrie a laserového scanneru. Odometrie je implementována přímo v řídící jednotce pohonu robota, která s řídicím PC komunikuje přes USB rozhraní. Laserový scanner SICK LMS-200 je k PC připojen rozhraním RS232. Rozdílnost komunikačních rozhraní a softwarových komunikačních protokolů, které v sobě navíc neobsahují jednoznačnou časovou značku bohužel vedou k tomu, že je prakticky nemožné zaručit časovou synchronnost dat přicházejících z různých senzorů. Vedle statického časového offsetu, který lze identifikovat a korigovat, rozhraní obsahují též nepředvídatelně proměnnou složku zpoždění, které způsobí, že příchozí data z odometrie odpovídají jinému časovému okamžiku než data z laserového scanneru. Tato velmi obtížně korigovatelná zpoždění dosahují typicky hodnot několika desítek milisekund. Tato zpoždění mají významný vliv zejména pokud robot vykonává rychlý rotační manévr, kdy odometrie může být vůči reálné pozici laserového scanneru úhlově posunuta o více než 10◦ (při větším časovém offsetu než 50 ms). Chyba v translaci při časovém offsetu 50 ms a maximální rychlosti pohybu je 2.5 cm, což je hodnota řádově srovnatelná s absolutní chybou měření laserového scanneru a tudíž není tak kritická. Z výše uvedených faktů vyplývá, že nesynchronnost mezi odometrickými a laserovými daty je nejvýraznější právě v rotační složce pohybu. Lokalizační algoritmus musí být schopen následně korigovat i tuto chybu.

10.2

Popis realizovaných experimentů

Realizované experimenty byly rozděleny do několika skupin. V první části se zabýváme experimentálním ohodnocením vlastností navržené metody pro filtraci chybných měření vzniklých na rozhraních mezi překážkou a pozadím. Nejrozsáhlejší část experimentů se věnuje ověření vlastností, zejména přesnosti navržené lokalizační metody PTM a to jednak srovnáním s existující metodou, tak i nezávislými testy v různých prostředích. Testy byly prováděny v kancelářském prostředí, v prostředí s dlouhými cyklickými chodbami a též ve venkovním prostředí. S testy lokalizace souvisí i sekce zabývající se vyhodnocením námi navrženého postupu pro budování mapy a jeho srovnáním s jinou reprezentací modelu světa. Ukázány jsou výsledky dávkové a inkrementální tvorby bodové mapy, která slouží k podpoře lokalizace.

104

KAPITOLA 10. EXPERIMENTÁLNÍ VÝSLEDKY

Poslední část experimentů se věnuje výsledkům rozšířených základních metod lokalizace a stavby bodového modelu prostředí. Jsou ukázány výsledky trojrozměrné varianty PTP metody a stavby 3D modelu prostředí. Dále jsou konstatovány a prezentovány zkušenosti s využitím PTM metody pro účely řešení úlohy kooperativního průzkumu prostředí s více roboty spolu s realizovanými modifikacemi základní podoby PTM metody s ohledem na výrazné zvýšení robustnosti metody v prostředí kooperujících robotů.

10.3

Filtrace mixed-pixelů

Pro kvalitativní vyhodnocení úspěšnosti filtrace mixed-pixelů je třeba nahlížet v kontextu dalšího zpracování dat. Strategie filtrace mohou být s ohledem na nastavení parametrů buď opatrné, či agresivní. Při opatrné strategii se algoritmus snaží filtrovat spíše méně bodů, ovšem s vysokou mírou jistoty, že zamítnutý bod je skutečně chybný. Naopak při agresivní strategii je cílem vyfiltrovat pokud možno všechny falešné body i za cenu toho, že filtrem neprojdou některé korektně naměřené body. Přirozeně se snažíme maximalizovat obě kritéria, nicméně volbou parametrů filtrace se můžeme přiklonit k jedné či druhé strategii. Pro experimenty s laserovým scannerem SICK LMS-200 zaměřené na filtraci mixedpixelů jsme zvolili kompromisní nastavení filtru s následujícími parametry: limitní rozdíl naměřené vzdálenosti sousedních bodů pro zařazení bodu do množiny kandidátů filtrace byl mindist = 5 cm, celkový počet bodů prokládaných pomocnou přímkou byl nastaven na N = 5, povolená směrodatná odchylka kolmé vzdálenosti bodů od prokládající přímky byla σlin = 3 cm, povolená směrodatná odchylka vzdálenosti rozložení bodů podél prokládající přímky byla σdist = 7 cm. Experimentální prostředí je zobrazeno na obr. 10.4. Provedení experimentů zaměřených na přesnost filtrace vyžaduje mít k dispozici srovnávací, pokud možno správně klasifikovanou množinu naměřených bodů, které náleží buď do skupiny reálných bodů nebo skupiny bodů vzniklých jevem mixed-pixel. Správnou klasifikaci nelze provést jinak než specifikací prostoru, kde se naměřené body mohou reálně vyskytovat a kde nikoliv. Pro tyto účely byl v naměřených datech stanoven volný prostor ve formě polygonu s dírami. Ideálně by všechny body, které náleží vnitřnímu prostoru polygonu měly být klasifikované jako mixed-pixel a naopak. Polygon ohraničující volný prostor byl pro každou konfiguraci prostředí získán manuální aproximací střední linie senzorických dat podél hranic překážek. V polygonu se však nemohou nacházet body odpovídající šumu měření při měření překážek, proto byla na polygon aplikována operace kontrakce o 3 cm. Kontrakce polygonu zajistí, že téměř všechny body odpovídající naměřeným obrazům skutečných překážek budou skutečně vně polygonu. Každému bodu, který laserový scanner změřil jako validní, byl následně přiřazen příznak, zda náleží volnému prostoru s ohledem na příslušnost k výše definovaného polygonu. Pokud změřený bod náleží polygonu byl zařazen do množiny rP os (vzorová reálná pozitivní klasifikace), pokud bod do polygonu nenáleží, byl zařazen do množiny rN eg (vzorová reálná negativní klasifikace). Testovatý filtrační algoritmus pak každý změřený bod klasifikoval do pozitivní či ne-

10.3. FILTRACE MIXED-PIXELŮ

počet scanů [-] rPos [-] rNeg [-] tpos [-] tneg [-] fpos [-] fneg [-] tpr [%] fpr [%] acc [%]

105

Vzdálenost překážek od pozadí 0.5 m 0.75 m 1m 1.25 m 1.5 m 1503 1514 1374 1676 1256 8186 8872 8589 7188 5644 535113 538393 488143 598469 448420 6907 7829 7307 5730 4428 527450 531205 482510 592690 443334 7663 7188 5633 5779 5086 1279 1043 1282 1458 1216 84.38 88.24 85.07 79.72 78.45 1.43 1.34 1.15 0.97 1.13 98.35 98.5 98.61 98.81 98.61

Tabulka 10.2: Výsledky filtrace mixed-pixel bodů v celém rozsahu prostředí

počet scanů [-] rPos [-] rNeg [-] tpos [-] tneg [-] fpos [-] fneg [-] tpr [%] fpr [%] acc [%]

Vzdálenost překážek od pozadí 0.5 m 0.75 m 1m 1.25 m 1.5 m 1503 1514 1374 1676 1256 5713 7306 6357 3889 3794 253342 225807 144215 142596 129751 5267 6769 5673 3397 2992 251478 224345 142821 141742 128072 1864 1462 1394 854 1679 446 537 684 492 802 92.19 92.65 89.24 87.35 78.86 0.74 0.65 0.97 0.60 1.29 99.11 99.14 98.62 99.08 98.14

Tabulka 10.3: Výsledky filtrace mixed-pixel bodů ve vybrané části prostředí gativní třídy vůči mixed-pixel jevu a s ohledem na vzorovou klasifikaci rP os a rN eg body rozčlenil do tříd tpos, f pos, tneg a f neg tak jak byla uvedeno v kap. 7.1. Z těchto hodnot byla následně podle vztahů 7.1 a 7.2 vypočítána míra správné pozitivní klasifikace tpr, kterou se algoritmem snažíme maximalizovat a míra minimalizované chybné pozitivní klasifikace f pr. Celková výsledná přesnost algoritmu acc je vypočítaná podle vztahu 7.3. Algoritmus byl testován pro pět různých vzdáleností mezi překážkou a pozadím od 0.5 do 1.5 m. Ukázalo se, že čím je překážka více vzdálena od pozadí, tím méně četný je výskyt mixed-pixel jevu. Vliv četnosti výskytu falešných bodů v laserových datech na vzdálenosti překážky od pozadí je znázorněn na obr. 10.5. Tabulka 10.2 shrnuje naměřené a vypočtené hodnoty z celého testovaného prostředí, jak je zobrazeno na obr. 10.5 f). Tabulka 10.3 je vytvořena ze shodných dat, pro výpočet jsou však použita pouze data se souřadnicí x > 3.5 m. Motivací pro přidání této druhé tabulky je zahrnutí pouze oblasti, ve které primárně vznikají body určené k filtraci. Je to navíc oblast, která byla zvolenou trajektorií intenzivně měřena.

106

KAPITOLA 10. EXPERIMENTÁLNÍ VÝSLEDKY

Výše uvedený experiment pracuje částečně se syntetickými daty, pro které je však reálné stanovit vzorové klasifikace. U reálného prostředí tato možnost není, výsledek filtrace pak může být vyhodnocena víceméně jen subjektivně na základě znalosti reálného tvaru scény a naměřených vyfiltrovaných dat. Výsledek filtrace laserových dat v kancelářském prostředí je zobrazen na obr. 10.3. Tento typ prostředí v laserových datech generuje velké množství falešných bodů, které se snažíme filtrovat. Použitá data jsou součástí množiny měření, která byla snímána pro účely stanovení přesnosti lokalizace v rámci kap. 10.4.2. Červenou barvou jsou v obrázku zobrazeny body odstraněné filtrem, modrou barvou pak body, které filtrem procházejí k dalšímu zpracování.

Obr. 10.3: Výsledek filtrace mixed-pixelů z dat naměřených v kanc. prostředí s velkým počtem nohou od židlí a stolů, viz obr. 10.10 c). Červené body byly odfiltrovány.

Obr. 10.4: Prostředí pro testy filtrace mixed-pixelů, datová reprezentace prostředí viz. obr. 10.5 f)

10.3. FILTRACE MIXED-PIXELŮ

107

(a)

(b)

(c)

(d)

(e)

(f)

Obr. 10.5: Závislost jevu mixed-pixel na vzdálenosti mezi překážkou a pozadím se zobrazením výsledků filtrace. Červenou barvou jsou zobrazeny body, které byly vyfiltrovány, černou barvu mají body určené pro další zpracovaní. Vzdálenost překážky a pozadí je a) 0.50 m, b) 0.75 m, c) 1.00 m, d) 1.25 m, e) 1.50 m. Na obr. f) je vyobrazeno celé testovací prostředí včetně označení trajektorie snímacího laseru (zelenou barvou).

108

KAPITOLA 10. EXPERIMENTÁLNÍ VÝSLEDKY

10.4

Lokalizace

10.4.1

Srovnávací testy lokalizace

V úspěšně obhájené disertační práci (Kulich, 2003) byla řešena obdobná problematika jako v této práci, která jistým způsobem navazuje na dříve publikované výsledky. Naším přirozeným cílem byl kvalitativní posun lokalizační techniky, proto se zde budeme zabývat srovnáním nově navržené metody PTM s dříve publikovanou metodou Line-To-Line, dále zkráceně jen LTL.

Obr. 10.6: Data z laserového scanneru v kan- Obr. 10.7: Data z laserového scanneru v kancelářském prostředí po lokalizaci metodou celářském prostředí po lokalizaci novou metodou Point-To-Map Line-To-Line


10

x x φ

dx dy dphi 5

0 rozdíl polohy [cm/°]

    ◦ 



−

−5

−10

−10

−


−15

−20






150




 

250



350



−20

0

50

100

150

200 scan [−]

250

300

350

400

Obr. 10.8: Rozdíl poloh mezi dvěma scany Obr. 10.9: Rozdíl poloh mezi dvěma scany po lokalizaci metodou LTL po lokalizaci novou metodou PTM Z porovnání grafických výsledků lokalizace na obr. 10.6 a 10.7, je vidět rozdíl kvality lokalizace. Rozdíl v přesnosti je patrný i bez numerického vyhodnocení, čím je lokalizace přesnější, superpozice dílčích scanů je prostorově kompaktnější. Na obrázcích se to pak projeví jako ostřejší zobrazení hranice jednotlivých překážek v prostředí.

10.4. LOKALIZACE

109

Předchozí obrázky 10.6 - 10.9 zobrazují zpracování shodných zdrojových dat naměřených v rámci práce (Kulich, 2003). Tato data obsahovala pouze hrubá data z laserového scanneru typu SICK PLS-100, součástí těchto dat nebyla odometrie. SICK PLS-100 má podobné vlastnosti jako jeho nástupce SICK LMS-200, který byl popsán v kap. 10.1.3, dosahuje však zhruba pětkrát nižší přesnosti a rozlišení. Je nutno podotknout, že pro srovnání jsme vybrali typ prostředí, v kterém starší LTL metoda poskytovala nejhorší, nicméně použitelné výsledky. Nová metoda PTM má vůči starší metodě LTL výraznou výhodu v tom, že si během lokalizace vytváří navíc model prostředí, který umožňuje výrazně snížit negativní vliv kumulované lokalizační chyby. Objektivní číselné zhodnocení výsledků dosažených oběma metodami je shrnuto v tab. 10.4.

Nscans Metoda l[m] tT [s] ts [ms] Npairs Npoints ∆xy[cm] ∆xy[%] ∆ϕ [◦ ]

LT L 16.40 139.0 359.2 16.36 14.80 0.902 3.00

sada 1 387 PTM 14.85 8.4 21.7 917 0.21 0.014 -0.16

LT L 11.96 143.4 455.2 19.17 18.10 1.513 7.70

sada 2 315 PTM 10.88 6.8 21.6 920 0.30 0.027 -0.11

LT L 10.39 118.0 450.4 19.74 17.40 1.675 4.80

sada 3 262 PTM 8.71 5.7 21.7 867 0.35 0.040 -0.11

LT L 9.95 115.7 411.7 18.88 13.40 1.347 -6.00

sada 4 281 PTM 9.03 6.2 22.1 892 0.87 0.096 0.32

LT L 9.40 123.6 414.8 19.49 15.90 1.692 2.00

sada 5 298 PTM 8.82 6.5 21.8 899 0.42 0.048 1.00

Tabulka 10.4: Srovnání vlastností a přesnosti metod Line-To-Line a Point-To-Map lokalizace v běžném kancelářském prostředí. Záhlaví tabulky má následující význam: Nscans je celkový počet zpracovaných scanů, l je délka projeté trajektorie, tT je celková doba běhu algoritmu, ts je průměrná doba na zpracování jednoho scanu, Npairs je průměrný počet použitých párů úseček pro metodu LTL, Npoints je počet bodů ve vytvořené mapě metodou PTM, ∆xy je chyba polohy a ∆ϕ je chyba natočení robotu. Relativní přesnost je vztažena k délce trajektorie. Vyčíslení dosažených přesností bylo provedeno shodným postupem jak bylo popsáno ve výchozí práci (Kulich, 2003). V kancelářském prostředí byla vykonána trajektorie, u které se výchozí a koncový bod nachází v prostředí na totožném místě. Tento použitý postup sice není z hlediska stanovení přesnosti optimální, jako přesnost lokalizace je považována naměřená odchylka počátečního a koncového bodu trajektorie, nezohledňuje se přesnost lokalizace v bodech podél trajektorie. Tento postup pracuje pouze vyhodnocením celkové kumulativní chyby. V následující kap. 10.4.2 jsme pro stanovení přesnosti metody navrhli dokonalejší postup, nicméně pro srovnávací účely byl v této kapitole použit původní postup. Oba experimenty byly provedeny na stejně výkonných PC (Pentium IV 2.8GHz). Metoda Line-To-Line dosahuje výrazně horší časové výkonnosti zejména díky implementaci v prostředí MATLAB. Metoda PTM je implementována v jazyce Java a časově kritické

110

KAPITOLA 10. EXPERIMENTÁLNÍ VÝSLEDKY

operace jsou implementovány v jazyce C. Z výsledků v tabulce 10.4 a z porovnání obr. 10.6 a 10.7 je zřejmé, že nová metoda dosahuje mnohem vyšší přesnosti a to až o dva řády. Původní metoda LTL pro svoji korektní činnost nutně vyžaduje, aby v referenčním a registrovaném laserovém scanu bylo možné spolehlivě vysegmentovat alespoň 2-3 významné liniové segmenty s výrazně odlišnou směrovou orientací. Tato vlastnost u nové metody PTM není zapotřebí. Ta s orientací segmentů pracuje pouze na úrovni korekce hrubých odometrických chyb. Pokud se tedy v datech nevyskytnou významné odometrické chyby a relativní odchylka poloh dvou scanů nepřesáhne kritické hodnoty, správná činnost metody není ohrožena ani ve vysoce strukturovaných prostředích, ve kterých je minimální šance na úspěšné segmentace dat do liniových úseků. Jako výrazný přínos nové PTM metody lze považovat průběžné budování mapy, která výrazně zvýší jednak přesnost lokalizace při návratu do výchozí pozice, ale též výrazně omezí velikost kumulativní chyby lokalizace.

10.4.2

Testy přesnosti lokalizace

Použitý postup pro stanovení přesnosti lokalizace realizovaný v předchozí kapitole není zcela správný. Použili jsme jej jen pro zachování stejných podmínek při srovnávacích testech. Přestože také nemáme k dispozici přesný referenční systém pro stanovení polohy robotu v prostoru, pokusíme se navrhnout lepší metodiku, než jak byla stanovena ve starší práci (Kulich, 2003), ve které se pro zhodnocení kvality lokalizace nebrala v úvahu přesnost trajektorie pohybu a počet měření byl uměle navyšován opakovaným spuštěním experimentu na omezeném počtu reálných měření s různým zašuměním dat. Naše zhodnocení přesnosti lokalizačního procesu bude vycházet ze vzájemných rozdílů naměřených poloh během opakovaných měření. Závěry se budou opírat výhradně o naměřená data, ne o dílčí simulace nebo uměle zašuměná data. Jako testovací prostředí jsme zvolili tři vzájemně propojené místnosti kancelářského typu s přilehlou chodbou. Prostředí tedy obsahuje čtveřici dveří spojující jednotlivé místnosti, vyskytuje se v něm i cyklická struktura. Celková struktura prostředí je patrná např. z obr. 10.11 ilustrující zároveň i grafickou podobu přesnosti lokalizace. V prostředí o celkové rozloze cca 180 m2 bylo stanoveno celkem 14 testovacích pozic, do kterých budeme opakovaně robot navádět a zaznamenávat souřadnice, které byly vypočítány lokalizační metodou Point-To-Map. Pozicí v prostředí jsou myšleny souřadnice robotu spolu s jeho orientací. Pozice byly v prostředí rovnoměrně rozvrženy tak, aby jejich vzájemná vzdálenost byla v rozsahu 2.5 - 5 m a měly rozdílnou orientaci. Ukázky některých testovacích pozic spolu s jejich přilehlým prostředím jsou zobrazeny na obr. 10.10. Pro zjištění přesnosti lokalizační metody by přirozeně bylo nejlepší porovnat naměřené hodnoty s referenčními absolutními souřadnicemi testovacích pozic. Tyto referenční souřadnice však bohužel nemáme k dispozici, pro účely našich experimentů je nahradíme střední hodnotou vypočtených souřadnic ve vybraných konkrétních pozicích. Přesnost lokalizační metody bude tedy vycházet z naměřených odchylek od takto stanovené reference. Předpokládáme, že takto stanovená reference trpí určitou chybou, neboť může být ovlivněna způsobem pohybu robotu v prostředí, pořadím návštěv jednotlivých pozic nebo

10.4. LOKALIZACE

111

rozložením překážek v prostoru. Zmíněný nežádoucí vliv se snažíme snížit tím, že prostředí projíždíme po různých trajektoriích definovaných různým pořadím testovacích pozic. Navíc mezi určitými měřením byly provedeny též drobné změny v prostorové konfiguraci překážek v prostředí, jak je patrné z vybraných vytvořených map prostředí na obr. 10.19. Celkem bylo provedeno devět nezávislých experimentálních jízd s robotem G2 se shodnou výchozí pozicí. Před každým experimentem jsme stanovili pořadí návštěv jednotlivých pozic, do kterých jsme robot následně naváděli. Zvolená trajektorie byla interpretována řídicím systémem robotu, který na základě informací z lokalizačního modulu zajistil autonomní přesun robotu do blízkosti zvolené pozice. Přesné fyzické umístění robotu do jedné ze 14ti žádaných pozic bylo zajištěno jeho manuálním přesunem do žádané pozice trvale označené na zemi. Z této pozice byly PTM metodou vypočítány aktuální souřadnice, které se zaznamenaly pro další zpracování. Pro jednotlivá měření bylo velmi důležité zajistit, aby se počátek souřadných soustav pro všechna měření nacházel na fyzicky stejném místě. Za tímto účelem byl před započetím experimentů pořízen referenční scan, ke kterému se registroval vždy první scan z aktuálního měření. Střední hodnoty naměřených pozic jsou uvedeny v tab. 10.5. spolu s maximálními odchylkami vypočtených hodnot z různých měření v jednotlivých pozicích. Tato tabulka je ještě doplněna vypočtenými standardními odchylkami, ty ovšem vzhledem k malému počtu měření (pro každý bod pouze 9) mají jen velmi malou vypovídací hodnotu. Na obr. 10.11 je graficky zobrazen výsledek lokalizace ve formě kumulovaných laserových scanů. Každý naměřený scan byl umístěn do polohy určené lokalizační metodou Point-To-Map. Chyby lokalizace jsou shrnuty v tabulkách 10.6 - 10.8. Pro každou z devíti měřicích sad jsou v každém ze 14 měřicích bodů zaznamenány odchylky natočení robotu

Bod 1. 2. 3. 4. 5. 6. 7. 8. 9. 10. 11. 12. 13. poč.

Střední poloha x ¯[cm] y¯[cm] 99.8 -298.1 103.2 -694.7 206.3 299.2 643.9 304.4 -294.0 -100.6 -300.1 -493.9 -249.2 -894.9 -606.5 -852.2 -542.7 -447.8 -579.4 -60.3 -300.2 240.1 -649.2 300.6 102.0 10.9 0.2 2.9

bodu ϕ[ ¯ ◦] -89.2 -125.8 179.7 -47.8 -177.7 -90.4 -88.3 -0.7 -177.0 2.6 90.0 -178.9 -87.7 0.2

Maximální odchylky ∆x[cm] ∆y[cm] ∆ϕ[◦ ] 2.579 5.082 0.861 5.178 4.529 1.078 3.645 3.784 0.457 1.658 2.002 1.362 1.299 1.638 0.957 4.334 2.344 0.847 5.153 1.917 0.390 5.633 3.964 0.759 2.094 4.460 0.685 2.753 2.659 0.579 1.018 2.606 0.426 2.459 2.912 0.862 0.935 4.492 1.336 1.476 2.466 1.132

Standardní odchylka δx[cm] δy[cm] δϕ[◦ ] 1.331 2.029 0.466 2.556 1.995 0.449 1.539 1.509 0.283 1.138 0.992 0.736 0.658 1.066 0.532 1.917 1.192 0.498 2.636 1.297 0.235 2.820 2.071 0.444 1.165 2.353 0.405 1.827 1.533 0.378 0.570 1.139 0.277 1.191 1.987 0.626 0.633 1.797 0.670 0.698 1.094 0.531

Tabulka 10.5: Střední hodnoty odchylek v testovacích bodech metody Point-To-Map

112

Bod 1. 2. 3. 4. 5. 6. 7. 8. 9. 10. 11. 12. 13. poč. ∆max

KAPITOLA 10. EXPERIMENTÁLNÍ VÝSLEDKY Měření č. 1 Odch.polohy,úhlu [cm,◦ ] ∆x ∆y ∆l ∆ϕ -0.57 1.01 1.17 0.33 0.51 1.57 1.65 1.08 0.43 0.24 0.49 -0.22 0.50 2.00 2.06 0.62 0.61 1.27 1.41 -0.27 0.04 1.76 1.76 -0.82 -1.17 1.62 2.00 0.09 -0.72 2.10 2.22 -0.54 0.05 3.28 3.28 -0.25 2.75 1.98 3.39 -0.58 0.39 0.13 0.41 -0.11 0.17 -1.26 1.28 -0.58 -0.15 1.02 1.03 0.18 0.41 0.14 0.44 0.21 2.75 3.28 3.39 1.08

Měření č. 2 Odch.polohy,úhlu [cm,◦ ] ∆x ∆y ∆l ∆ϕ -0.71 0.93 1.18 0.02 -0.19 0.21 0.28 -0.45 -0.78 1.05 1.31 -0.31 1.55 0.22 1.57 -0.44 -0.42 0.15 0.44 0.14 -1.00 -0.13 1.00 -0.22 -1.03 -0.38 1.10 0.39 -1.14 -0.34 1.19 -0.34 -0.69 0.37 0.78 -0.53 0.37 -0.03 0.37 -0.01 1.02 0.80 1.29 -0.24 1.86 2.27 2.93 -0.84 0.94 -0.04 0.94 -0.16 0.68 1.33 1.50 -0.75 1.86 2.27 2.93 0.84

Měření č. 3 Odch.polohy,úhlu [cm,◦ ] ∆x ∆y ∆l ∆ϕ -0.14 1.15 1.16 -0.59 2.52 1.37 2.87 -0.21 -1.45 1.20 1.88 -0.12 -0.88 -0.16 0.89 -0.12 0.72 -0.59 0.94 0.25 1.19 -0.45 1.28 0.13 2.06 -0.31 2.08 -0.03 2.31 0.91 2.48 -0.25 1.24 0.92 1.54 0.37 0.89 -1.24 1.53 0.42 0.13 -0.25 0.28 -0.22 -0.32 0.57 0.65 -0.13 0.65 0.68 0.94 -0.29 -0.03 0.53 0.53 1.13 2.52 1.37 2.87 1.13

Tabulka 10.6: Výsledky přesnosti lokalizace metodou Point-To-Point v kombinovaném prostředí kanceláří a chodby, měření 1-3. ϕ a odchylky poloh v ose x i y vůči středním hodnotám veličin (vypočtených přes všechna měření) příslušejících k danému bodu ze všech měření. Dále je pak uvedena vzdálenost l mezi vypočtenou hodnotou pozice a bodem reprezentující střední polohy ze všech měření. Z uvedené sady měření vyplývá, že chyba určení polohy se v tomto prostředí pohybuje v řádu jednotek centimetrů, což vzhledem v průměrné délce trajektorie kolem 100 m představuje průměrnou relativní chybu polohy v řádu setin až desetin procent. Hovořit však o relativní chybě u takto výše definovaného experimentu není zcela přesné, ovšem bez přesného referenčního systému pro stanovení absolutní polohy jinou možnost nemáme. Celkové zprůměrování chyb přes všechny pozice a všechny experimenty je uvedeno v tab. 10.10. Řádová přesnost lokalizační metody odpovídá přesnosti použitého senzoru, dosažení řádově vyšších přesností s použitým senzorem lze jen stěží předpokládat. Další důležitý parametr, efektivita a rychlost algoritmu je plně dostačující, neboť doba zpracování jednoho scanu je více než 10x kratší než doba mezi příchodem dvou následujících scanů ze senzoru. Z hlediska kritéria robustnosti metody lze konstatovat, že mezi všemi realizovanými měřeními se nevyskytla žádná sada, během které by metoda selhala.

10.4. LOKALIZACE

Bod 1. 2. 3. 4. 5. 6. 7. 8. 9. 10. 11. 12. 13. poč. ∆max

Měření č. 4 Odch.polohy,úhlu [cm,◦ ] ∆x ∆y ∆l ∆ϕ -1.58 0.60 1.69 0.01 -2.18 0.49 2.24 -0.11 -1.57 0.02 1.57 -0.12 -0.17 -0.82 0.84 -0.20 0.05 0.72 0.72 0.59 0.66 1.00 1.20 -0.31 1.28 1.46 1.94 0.10 1.63 0.34 1.66 -0.15 -0.04 1.25 1.25 -0.21 -2.11 0.61 2.19 -0.01 -0.11 -0.71 0.72 -0.15 0.06 2.15 2.15 0.59 -0.39 1.32 1.38 1.34 -1.48 0.11 1.48 -0.03 2.18 2.15 2.24 1.34

113 Měření č. 5 Odch.polohy,úhlu [cm,◦ ] ∆x ∆y ∆l ∆ϕ -0.73 1.14 1.36 0.34 1.49 1.82 2.35 -0.34 -0.13 0.18 0.22 0.46 0.96 0.13 0.97 0.30 0.01 1.28 1.28 0.30 0.47 1.03 1.13 0.13 0.07 0.60 0.61 -0.29 -0.76 2.41 2.53 -0.36 -0.39 2.20 2.23 0.07 1.39 2.66 3.00 -0.30 -0.96 2.61 2.78 0.12 -0.20 1.87 1.88 0.14 -0.58 0.92 1.09 0.16 -0.26 0.05 0.26 -0.34 1.49 2.66 3.00 0.46

Měření č. 6 Odch.polohy,úhlu [cm,◦ ] ∆x ∆y ∆l ∆ϕ 1.74 -0.37 1.78 -0.86 -0.89 -0.40 0.97 -0.15 -0.46 0.66 0.81 -0.05 1.36 -1.17 1.79 0.06 -1.30 -0.85 1.55 0.61 -4.33 -0.62 4.38 0.38 -2.71 -1.39 3.05 0.19 -3.41 -2.23 4.08 0.15 -1.45 -0.54 1.55 0.25 -1.70 -0.48 1.76 -0.44 -0.38 -0.09 0.40 0.25 -2.46 0.14 2.46 0.56 -0.70 1.09 1.29 -0.32 0.45 1.02 1.11 0.23 4.33 2.23 4.38 0.86

Tabulka 10.7: Výsledky přesnosti lokalizace metodou Point-To-Point v kombinovaném prostředí kanceláří a chodby, měření 4-6.

Bod 1. 2. 3. 4. 5. 6. 7. 8. 9. 10. 11. 12. 13. poč. ∆max

Měření č. 7 Měření č. 8 Měření č. 9 Odch.polohy,úhlu [cm,◦ ] Odch.polohy,úhlu [cm,◦ ] Odch.polohy,úhlu [cm,◦ ] ∆x ∆y ∆l ∆ϕ ∆x ∆y ∆l ∆ϕ ∆x ∆y ∆l ∆ϕ 0.18 1.28 1.29 -0.13 2.58 -0.67 2.66 0.55 -0.75 -5.08 5.14 0.33 0.66 0.99 1.19 -0.08 3.26 -1.51 3.59 0.09 -5.18 -4.53 6.88 0.18 0.29 -0.39 0.48 0.36 0.02 0.83 0.83 0.26 3.64 -3.78 5.25 -0.25 -0.85 0.43 0.95 -0.30 -0.82 0.51 0.96 -1.29 -1.66 -1.13 2.01 1.36 -0.56 0.70 0.90 -0.96 0.38 -1.64 1.68 -0.62 0.49 -1.03 1.14 -0.05 0.81 0.09 0.81 0.29 2.59 -2.34 3.49 0.85 -0.44 -0.33 0.55 -0.44 0.01 1.26 1.26 0.04 5.15 -1.92 5.50 -0.14 -3.66 -0.96 3.78 -0.35 -0.40 -0.62 0.74 0.58 5.63 -3.96 6.89 0.76 -3.11 1.41 3.42 0.15 -1.38 -0.61 1.51 0.69 2.09 -4.46 4.93 0.10 0.55 -2.38 2.45 -0.48 -1.49 -0.26 1.51 0.48 1.77 -2.05 2.71 0.12 -1.89 -1.18 2.23 0.31 -0.08 -0.86 0.86 0.29 -0.39 -1.20 1.27 -0.36 0.39 -0.40 0.56 0.43 -0.54 0.01 0.54 0.57 0.29 -2.91 2.93 0.56 1.14 -2.82 3.04 -0.86 -0.43 0.10 0.44 -0.52 0.83 -0.60 1.03 0.58 -0.15 -4.49 4.49 -0.97 -0.59 -0.59 0.84 0.14 0.67 -0.13 0.68 -0.28 0.14 -2.47 2.47 -0.30 1.49 1.53 1.61 0.96 5.63 4.46 6.89 1.29 5.18 5.08 6.88 1.38

Tabulka 10.8: Výsledky přesnosti lokalizace metodou Point-To-Point v kombinovaném prostředí kanceláří a chodby, měření 7-9.

114

KAPITOLA 10. EXPERIMENTÁLNÍ VÝSLEDKY

(a)

(b)

(c)

(d)

Obr. 10.10: Ukázka prostředí s vybranými referenčními body a) bod č. 1, b) bod č. 5, c) bod č. 8, d) bod č. 12

1. 2. 3. 4. 5. 6. 7. 8. 9.

Počet scanů 11341 7092 6819 5992 5241 6887 5313 5597 5709

Délka dráhy 102.9 m 101.2 m 100.8 m 101.6 m 96.6 m 106.1 m 90.6 m 100.1 m 91.9 m

Doba jízdy 2441 s 1532 s 1476 s 1297 s 1133 s 1487 s 1153 s 1216 s 1242 s

Prům.čas zprac./scan 14.20 ms 15.37 ms 15.25 ms 15.82 ms 15.14 ms 15.02 ms 16.08 ms 16.54 ms 15.43 ms

Bodů v mapě 3858 3583 3783 3635 3478 3441 3528 3479 4036

∆lmax 3.39 2.93 2.87 2.24 3.00 4.38 1.61 6.89 6.88

cm cm cm cm cm cm cm cm cm

Tabulka 10.9: Shrnutí parametrů a výsledků měření

∆ϕmax 1.08◦ 0.84◦ 1.13◦ 1.34◦ 0.46◦ 0.86◦ 0.96◦ 1.29◦ 1.38◦

10.4. LOKALIZACE

115

Obr. 10.11: Lokalizovaná kumulovaná laserová data z měření č. 7. Červenou barvou jsou v obrázku zobrazeny body odstraněné filtrem falešných bodů (mixed-pixelů), modrou barvou jsou zobrazeny body, které byly použity pro lokalizaci metodou Point-To-Map.

|∆x| |∆y| |∆l| |∆ϕ|

Max. hodnota 5.63 cm 5.08 cm 6.89 cm 1.39 ◦

Střední hodnota 1.098 cm 1.146 cm 1.771 cm 0.361 ◦

Standardní odchylka 1.107 cm 1.032 cm 1.295 cm 0.285 ◦

Tabulka 10.10: Výsledná přesnost lokalizace na základě výpočtu v 126 pozicích

116

10.4.3

KAPITOLA 10. EXPERIMENTÁLNÍ VÝSLEDKY

Lokalizace v projektu PeLoTe

Nejrozsáhlejší experiment z hlediska délky vykonané trajektorie mobilním robotem byl uskutečněn v říjnu 2004 v rámci závěrečných testů systému PeLoTe zaměřených zejména na robotický subsystém. Evropský projekt FET-IST 2001-38873 PeLoTe (Kulich a kol., 2004a), (Kulich a kol., 2004b) řešený mezi lety 2002-2005 byl zaměřen na ověření možných principů spolupráce hybridních typů entit (roboty a lidé) v jednom týmu řešící úlohu kooperativní záchranné mise. V této misi bylo hlavním úkolem prohledat určitý prostor, nalézt všechny možné oběti a dopravit je do bezpečí. V systému byly použity dva roboty a jeden člověk, přičemž pokud oběť byla nalezena robotem, byla její poloha nahlášena do systému a ten se postaral o navedení záchranáře přímo k oběti. Systém z hlediska sdílení dat (Mázl a kol., 2005) přistupoval podobným způsobem k oběma druhům entit v systému, pro člověka byly použity stejné algoritmy pro plánování trajektorie jako pro roboty, výraznější rozdíly byly zejména jen v komunikačním rozhraní. Roboty i lidé byli vybaveni lokalizačním systémem (Saarinen a kol., 2004b), což vzdálenému vedoucímu mise (teleoperátorovi) umožňovalo mít dokonalý přehled nad stavem celého systému a v případě potřeby provést i určité korekční zásahy např. z hlediska priorit akcí. Vyhodnocení výsledků projektu PeLoTe ukázalo (Driewer a kol., 2005), že schopnost robotů lokalizovat polohu obětí (na základě znalosti své vlastní pozice) je nejdůležitější vlastností robotického subsystému. Na tomto závěru se shodli dobrovolní i profesionální hasiči, kteří systém testovali. Pro účely této práce jsme použili výsledky jednoho z experimentů, během kterého jeden robot typu G2 (obr. 10.1) provedl osamoceně průzkum neznámého prostředí. Tímto prostředím byl systém chodeb jednoho podlaží budovy fyzikálních věd na univerzitě v německém Wüzburgu. Půdorysný rozměr prozkoumaného prostoru je 130x85 m, trajektorie o celkové délce 598 m byla projeta za 29 min a 7 sec s průměrnou rychlostí robotu 0.34 m/s. Nejdůležitějším závěrem pro účely této práce je však zhodnocení kvality lokalizace. Bohužel nebylo možné porovnat vytvořenou mapu budovy s reálnými plány, vyhodRozměry prostoru Parametry trajektorie robotu dálka trajektorie doba jízdy průměrná rychlost Přesnost lokalizace Posun v ose x Posun v ose y rozdíl natočení

130x85 m 598 m 1447 sec 0.34 m/s 0.60 m 0.15 m 3◦

Tabulka 10.11: Výsledky lokalizace v uzavřeném systému chodeb

10.4. LOKALIZACE

Obr. 10.12: Lokalizovaná senzorická mapa chodeb jednoho podlaží budovy

Obr. 10.13: Výřez z obr. 10.12 zobrazující kumulativní chybu lokalizace

117

118

KAPITOLA 10. EXPERIMENTÁLNÍ VÝSLEDKY

nocení přesnosti lokalizace tudíž provedeme na základě srovnání polohy senzorických dat na počátku a konci trajektorie. Vyhodnocení můžeme provést tímto způsobem díky tomu, že prostředí a zvolená trajektorie má převážně cyklický charakter a v rámci chodeb není zaručena trvalá viditelnost mezi všemi místy, která by díky souběžně vytvářené lokalizační mapě výrazně zvyšovala přesnost lokalizace. Výsledky lokalizační Point-To-Map metody opírající se o průběžně budovanou bodovou lokalizační mapu jsou shrnuty v tab. 10.11. Výsledná senzorická mapa vzniklá superpozicí všech platných měření laserového scanneru je zobrazena na obr. 10.12. Pro lepší znázornění hodnocených výsledků lokalizace je na obr. 10.13 zvětšen výřez mapy v okolí počátku a zároveň konce trajektorie. Trajektorie má svůj počátek v bodě [0,0], který je zvýrazněn tečkou. Robot postupně projel celou trajektorii proti směru hodinových ručiček a vrátil se zpět, vykonal pohyb ještě podél části počátku trajektorie, aby se v senzorických datech zvýraznila vzájemná poloha na počátku a na konci trajektorie. Přesnost lokalizace je určena na základě rozdílu polohy senzorických měření u korespondujících částí prostoru. Celková relativní chyba polohy mobilního robotu vztažená k celkové délce trajektorie je 0.104%, což lze považovat za velmi dobrý výsledek. Přesnosti však bylo dosaženo na základě dodatečného zpracování původně naměřených dat, neboť v době provádění experimentu ještě nebyla kompletně vyvinuta lokalizační metoda PTM s vazbou na průběžně vytvářenou mapu. K dispozici byla pouze metoda PTP, která v takto komplexním prostředí neposkytovala uspokojivé výsledky.

10.4.4

Lokalizace ve venkovním prostředí

Lokalizace ve venkovním prostření je náročnější úkol než lokalizace uvnitř budov. Pro tyto účely byl použit mobilní robot Pioneer 3AT, vybavený odometrickým subsystémem, inerciální měřicí jednotkou Crossbow IMU300CC. Jako klíčový senzor je opět použit laserový scanner SICK LMS-200, ovšem montovaný na polohovacím naklápěcím zařízení viz. obr. 9.1. V těchto experimentech nebylo využíváno satelitního navigačního systému GPS (kap. 5.7) Polohovací zařízení bylo použito za účelem udržení stálé horizontální orientace laserového dálkoměru nezávisle na aktuálním naklonění těla robotu. Řízení aktuálního relativního náklonu laseru bylo odvozováno od aktuálního směru vektoru gravitačního zrychlení měřeného trojosým akcelerometrem. Výsledky lokalizace dosahují nižší přesnosti (tab. 10.12), než v případě pohybu robotu ve vnitřních prostorech, je však třeba si uvědomit, že vlastnosti prostředí jsou zcela jiné. Vedle toho kinematika robotu Pioneer 3AT je odlišná od robotu G2. Robot Pioneer přirozeně zatáčí smykem, přičemž výchozí odometrická informace má mnohem nižší přesnost než u robotu G2. Lokalizační metoda se s daty sice do jisté míry vypořádá, ovšem díky přirozeně nižší míře možnosti najít korespondující množiny bodů mezi následnými naměřenými scany (meziscanový překryv), jsou výsledky lokalizace horší. Celkově lze shrnout, že námi prezentovaná metoda je do určité míry použitelná i pro venkovní prostředí, ovšem její hlavní doména použitelnosti jsou vnitřní prostory budov, kde dosahuje vysoké přesnosti. Výsledek lokalizace v podobě lokalizovaných laserových scanů je zobrazen na obr. 10.14,

10.4. LOKALIZACE

119

Obr. 10.14: Výsledek lokalizace robotu Pioneer ve venkovním prostředí (park) číselné vyjádření přesnosti lokalizace je uvedeno v tab. 10.12. Přesnost určení polohy byla stanovena od posunu pozice významného objektu ve vizualizovaných datech (byl jím osamocený kmen stromu, v obrázku 10.14 je označen šipkami) na počátku experimentu a na jeho konci, kdy se robot vrátil přibližně do výchozí pozice. Přesnost stanovení úhlu natočení robotu je odvozeno od vzájemného úhlu dvou přímek (zobrazených též na obrázku 10.14), kterými byla proložena vybraná data z počátku a konce jízdy robotu. Vyhodnotíme-li vlastnosti lokalizační chyby, lze dojít k závěru, že poziční chyba v řádu jednotek metrů byla způsobena zejména kumulativní chybou určení natočení robotu podél celé trajektorie jízdy robotu. Jedním ze základních negativních faktorů ovlivňující kvalitu lokalizace je vysoká míra šumu v datech, která je dána velkou různorodostí prostředí a použitými principy sběru dat. Při pořízení horizontálního laserového scanu ve venkovním prostředí (parku) získáme nejčastěji odraz laserového paprsku od volně pohybujících se větviček stromů, listů, v lepším případě kmenů stromů. To vše má za následek poměrně vysokou míru variability dat

120

KAPITOLA 10. EXPERIMENTÁLNÍ VÝSLEDKY Rozměry prostoru Parametry trajektorie robotu dálka trajektorie doba jízdy průměrná rychlost Přesnost lokalizace Posun v ose x Posun v ose y rozdíl natočení

130x110 m 319 m 877 sec 0.36 m/s 6.8 m 4,2 m 10 ◦

Tabulka 10.12: Výsledky lokalizace ve venkovním prostředí (parku) získaných i ze dvou bezprostředně po sobě následujících scanech. Na tomto místě je však nutno poznamenat, že cíle disertace nezahrnovaly vývoj lokalizační metody, která by byla zcela vhodná i pro venkovní typ prostředí.

10.5

Stavba lokalizační mapy

Stavba lokalizační mapy je nedílnou součástí lokalizace mobilního robotu. Přestože pro účely lokalizace je nutné mapu budovat inkrementálně, nejprve se zde budeme zabývat zejména výsledky dávkové tvorby mapy, kdy jsou k dispozici najednou všechna měření a výsledné mapy jsou kvalitnější, s eliminovaným šumem. Prvním krokem během tvorby mapy je normalizace mezibodových vzdáleností. Na základě normalizace mezibodových vzdáleností na minimální vzdálenost 5.10−4 m je následně možné provést filtraci osamocených bodů v mapě, což má za následek omezení vkládání šumu do mapy. Obr. 10.16 zobrazuje výsledky této filtrace, během které vypustíme část bodů. Vypuštěné body, které ve svém okolí (ve vzdálenosti 7 cm) nemají alespoň dva sousedy, jsou na obrázku znázorněny červenou barvou. Posledním důležitým parametrem pro výslednou tvorbu mapy je její granularita, tedy zadaná finální minimální mezibodová vzdálenost. Na obr. 10.15 je vidět, jak se snižuje počet bodů reprezentující reálné prostředí v mapě při zvyšování minimální mezibodové vzdálenosti. Na levém grafu je tato závislost vidět v širokém rozsahu mezibodových vzdáleností, včetně i velmi malých. Pro výstavbu bodové mapy je však rozumné uvažovat o mezibodových vzdálenostech přibližně od jednotek centimetrů. Proto je v pravém grafu detailněji zobrazena závislost počtu bodů v mapě v rozsahu centimetrových mezibodových vzdáleností. Na obr. 10.17 je vyobrazena výsledná bodová mapa s mezibodovou vzdáleností 5 cm a celkovým počtem bodů 2508. Důležitým experimentálním závěrem je srovnání mapy s jinými dostupnými řešeními. Námi vytvořená bodová mapa patří do kategorie senzorických map, bude tedy nejvhodnější naši mapu srovnat s reprezentací pomocí mřížek obsazenosti. Pro srovnávací účel byly použity výsledky práce (Štěpán, 2001). Na obr. 10.18 je vyobrazena mřížka obsazenosti, která byla vybudována na základě shodných dat jako bodová mapa z obr. 10.17.

10.5. STAVBA LOKALIZAČNÍ MAPY

121

5

10

4

x 10

3.5 3 pocet bodu [−]

8 pocet bodu [−]

x 10

6 4

2.5 2 1.5 1

2 0.5

0 −3 10

−2

−1

10 10 limit vzdálenosti bodu [m]

(a)

0

10

0 −2 10

−1

10 limit vzdálenosti bodu [m]

0

10

(b)

Obr. 10.15: Závislost počtu bodů ve vytvořené mapě na minimální mezibodové vzdálenosti a) Celkový pohled b) Detail na oblast vhodných bodových vzdáleností pro bodové mapy

Srovnáme-li výsledky bodové mapy a mřížky obsazenosti, lze konstatovat, že jedna z důležitých vlastností mřížek obsazenosti, kterou je filtrace dat, je zachována i u bodové mapy. Způsob konstrukce mřížky obsazenosti neobsahoval filtraci mixed-pixelů, proto v mřížce zůstává řada artefaktů, které jsou v bodové mapě díky filtraci eliminovány. Jistou nevýhodou bodové mapy oproti mřížce obsazenosti je nemožnost z principu odlišit volný a obsazený prostor. Budeme-li hodnotit paměťovou náročnost obou reprezentací, mřížka obsazenosti v tomto případě zabírá s velikosti buňky 2.5 cm na prostoru 16x16 m celkem 409600 byte. Oproti tomu bodová mapa z obr. 10.17 s min. mezibodovou vzdáleností 5 cm a celkovým počtem 2508 bodů zabírá jen 20064 byte, což je 20x méně. Pokud necháme vygenerovat bodovou mapu se stejným rozlišením, jako má mřížka obsazenosti tedy s mezibodovou vzdáleností 2.5 cm, získáme bodovou mapu s 6832 body, která v paměti zabírá 54656 byte, což je stále přibližně 7.5x méně než v případě mřížky. Na obr. 10.19 jsou zobrazeny mapy, které byly vytvořeny on-line během jízdy robotu ve vybraných experimentech orientovaných na testování přesnosti lokalizace (viz. kap. 10.4.2). Při tvorbě těchto map se neprováděla normalizace a filtrace osamocených bodů, což je rozdíl od předchozích výsledků, kdy mapy byly vytvářeny až dodatečně z kompletní množiny dat včetně filtrací. Na mapách je tudíž zřetelný vliv náhodně se vyskytujících bodů, neboť on-line strategie výstavby mapy se snaží do mapy vždy zahrnout maximum bodů i za cenu jejich nepříliš vysoké důvěryhodnosti. Ze stejného důvodu se v mapách místy objevují bodové linie v místech volného prostoru. Tyto body vznikly na základě dat ze scanneru, který měl krátkodobě skloněnou snímací rovinu směrem k podlaze vlivem náklonu celého robotu během přejezdu prahů mezi místnostmi.

122

KAPITOLA 10. EXPERIMENTÁLNÍ VÝSLEDKY

Obr. 10.16: Množina bodů po normalizaci mezibodové vzdálenosti získaná z měření č. 7 (viz kap. 10.4.2). Vstupem algoritmu budování bodové mapy jsou všechny body, které prošly mixed-pixel filtrem, tedy body, které jsou na obr. 10.11 zobrazeny modrou barvou. Na tomto obrázku jsou dvě množiny bodů, černé body jsou významné body algoritmem 8.8.2 označené jako neosamocené a červené body jsou ty, které byly procesem filtrace vyloučeny z dalšího zpracování.

10.5. STAVBA LOKALIZAČNÍ MAPY

123

Obr. 10.17: Výsledná bodová mapa pro měření č.7, která vznikla dávkovým zpracováním všech bodů algoritmem pro dávkové budování bodové mapy popsaného v kap. 8.4. V algoritmus byl spuštěn s následujícími parametry: normalizační vzdálenost bodů −4 m, počet sousedů pro filtraci osamocených bodů knn = 3 v poloměru dnorm min = 5.10 filtrace rf lt = 7 cm, vzdálenost bodů ve výsledné mapě dmap min = 5 cm.

124

KAPITOLA 10. EXPERIMENTÁLNÍ VÝSLEDKY

Obr. 10.18: Senzorická mapa ve formě mřížky obsazenosti (viz kap. 6.2.1) získaná z měření č. 7. aplikací postupů z práce (Štěpán, 2001). Velikost buňky mřížky byla nastavena na 2.5 cm. Světlé plochy v mřížce značí volný prostor, černé oblasti definují hranice překážek a šedý prostor je neznámá oblast. Světlé paprskové průniky směrem k okrajům mřížky jsou způsobeny chybnými měřeními, které byly během tvorby mřížky nahrazeny nekonečnou vzdáleností. Tyto měření odpovídají situaci, kdy laserovým scannerem nebyl detekován návrat vyslaného paprsku.

10.5. STAVBA LOKALIZAČNÍ MAPY

125

Obr. 10.19: Bodové lokalizační mapy vytvořené on-line během vybraných experimentů (měř. č. 1, 2, 5, 6, 7, 9) popsaných v kap. 10.4.2 s mezibodovou vzdáleností min. 10 cm.

126

KAPITOLA 10. EXPERIMENTÁLNÍ VÝSLEDKY

10.6

Výsledky aplikačních rozšíření navržených metod

10.6.1

Kooperativní lokalizace

Schopnosti lokalizace v prostředí kooperujících robotů byly ověřeny během vývoje a hlavně reálného testování implementace úlohy kooperativní explorace neznámého prostředí. V této úloze je cíl definován jako co možná nejefektivnější zmapování neznámého prostředí skupinou kooperujících mobilních robotů. To znamená, že po dokončení řešení úlohy je k dispozici mapa prostředí obsahující kompletní popis prostředí, který je robotu fyzicky dostupný. Alespoň jeden robot tedy musí navštívit řadu míst v prostoru tak, aby z nich byly rozmítaným laserovým scannerem viditelné všechny jeho dostupné části. Pro řešení této úlohy je použit multirobotický řídicí systém (Chudoba a kol., 2006) v kombinaci se systémem agentových technologií A-globe (Šišlák a kol., 2005). Multirobotický řídicí systém umožňuje přístup vyšších softwarových vrstev k hardwaru robota a umožňuje vykonávání elementárních akcí jako např. jízdu z bodu do bodu. Systém postavený na platform A-globe zajišťuje vzájemnou komunikaci robotů, výměnu informací, generování a rozdělování lokálních cílů mezi jednotlivé mobilní roboty tak, aby byla zadaná úloha vyřešena v co nejkratším čase. Lokálními cíli jsou dílčí body v prostoru, které je ještě potřeba alespoň jedním robotem navštívit, aby do sdílené mapové struktury mohly být postupně zahrnovány všechny dostupné informace o prostorové konfiguraci roboty zkoumaného prostředí. Sdílená mapová struktura ve formě mřížek obsazenosti obsahuje informaci o obsazených, resp. volných oblastech již prohledaných částí prostoru. Vedle ní roboty sdílí též informaci o hranicích mezi prozkoumaným a neprozkoumaným prostorem (frontiers). Tyto hranice vznikají typicky v oblastech prostorových okluzí, viz. obr. 7.7 a jsou základem pro generování lokálních cílů. Efektivita přidělení lokálního cíle konkrétnímu robotu je v rámci skupiny mobilních robotů vyhodnocována na základě délky trajektorie mezi cílem a příslušným robotem, jeho aktuálním stavem, pozicí atd. Experiment byl prováděn se čtyřmi roboty, které se na počátku nacházely na přibližně známých souřadnicích. Jako první krok před zahájením samotného průzkumu prostředí je nutné přesné určení vzájemných poloh mezi roboty, aby všechny roboty mohly pracovat nad jednou sdílenou mapovou strukturou. Jeden z robotů poskytne svůj aktuální scan prostředí ostatním robotům, kteří pak metodou PTP určí svoji pozici vůči němu. Následuje pak již rozdělování cílů a z hlediska lokalizace kontinuální budování sdílené mapy a sledování pozice každého robotu zvlášť vůči mapě. Tento experiment byl z hlediska lokalizace důležitý pro ověření schopnosti metody PTM vyrovnat se s pohybujícími se objekty za předpokladu, že je známa jejich pozice. Během průzkumu prostředí se v prostoru pohybuje několik robotů, každý z nich si metodou PTM určuje svoji pozici, kterou dává k dispozici ostatním. Každý robot má tedy informaci o poloze partnerských robotů v týmu. Experimenty zaměřené na kooperativní lokalizaci byly prováděny před aplikací postupů uvedených v kap. 8.6 a po jejich zaintegrování do systému. V prvním případě se ukázalo, že jakmile se dva roboty dostaly do vzájemné těsné blízkosti (blíže než 1 m), tak téměř zhruba v polovině případů lokalizační proces jednoho nebo

10.6. VÝSLEDKY APLIKAČNÍCH ROZŠÍŘENÍ NAVRŽENÝCH METOD

127

druhého robotu selhal z důvodu nedostatečného volného zorného úhlu na okolní statické prostředí, o které se lokalizační metoda opírá. Druhým problémem způsobující selhání bylo, že do lokalizační mapy bylo přidáváno velké množství bodů změřených jako odraz od robotu. V extrémním případě, kdy se určitý robot pohyboval delší dobu v zorném poli jiného robotu (a to i větší vzdálenosti), lokalizační bodová mapa se začala zcela zaplňovat odrazy od ostatních robotů, což mělo za následek též selhání lokalizace. Důvodem tohoto druhu selhání metody PTM bylo, že metoda začala vyhodnocovat korespondence vůči nekorektně vloženým bodům v mapě, které neodpovídají reálnému prostředí. Během opakovaných experimentů se ukázalo, že vyloučení všech bodů z laserového scanu, které odpovídají odrazu od těl robotů na známých pozicích podle popisu v kap. 8.6 skutečně výrazně zvýšilo robustnost lokalizace. Na rozdíl od předchozího případu, kdy kompletní úloha kooperativní explorace byla úspěšně dokončena zhruba v 25% případů kvůli selhání lokalizace, po zdokonalení výše uvedeným postupem se spolehlivost zvýšila přibližně na 90%. Zbývající případy, kdy lokalizace nebyla schopna poskytnout aktuální informaci o poloze zejména vlivem nedostatečného množství platných bodů v laserovém scanu (vyloučené body jako odrazy od robotů) jsme vyřešili aktivním přístupem k lokalizaci. Aktivní přístup k lokalizaci spočíval v tom, že jakmile metoda PTM neposkytla spolehlivou informaci o poloze, lokalizační modul dočasně převzal řízení pohonů robotu a provedl pomocný manévr s cílem podpořit lokalizaci získáním vhodnějších dat. Pomocný pohybový manévr spočíval ve zpětném pohybu robotu o 30 cm následovaný rotací na místě o ±π/2. Tímto pohybem se robot zpravidla dostal do vhodnějšího postavení a dříve či později lokalizační modul opět začal poskytovat korektní výsledky. Celkový systém kooperativní lokalizace po zaintegrování všech výše uvedených doplňků úlohu kooperativní lokalizace v prostoru přibližně 15x15 m (viz prostory z obr. 10.17) úspěšně dokončí ve více než 95% případů. Neúspěšné případy jsou zpravidla způsobeny výrazným náhodným pohybem osob nebo jiných předmětů v prostředí.

10.6.2

Registrace 3D scanů

Výsledky registrace 3D scanů představíme na základě registrace dvou scanů sejmutých ze dvou míst v prostoru. Pozice naklápěného laseru se od sebe liší ve všech třech kartézských souřadnicích a výchozí orientace laseru je též odlišná ve všech třech rotačních osách. Referenční scan je na obr. 10.22 zobrazen v perspektivní projekci a v pohledech rovnoběžnými s jednotlivými souřadnými osami. Výsledky registrace dvojice 3D scanů metodou PTP rozšířenou do třetího rozměru jsou zobrazeny na obr. 10.23. Rozložení mezibodových vzdáleností seřazených podle své velikosti během jednotlivých iterací je zobrazeno na obr. 10.20. Na obrázku je vidět, jak se v průběhu jednotlivých iterací zvýrazňuje ostrost ohybu průběhu seřazených mezibodových vzdáleností. Na obr. 10.21 je pak vidět závislost velikosti parametrů dílčích meziscanových korekčních transformací. Výrazná korekční výchylka ve 32. iteraci je způsobena výraznou změnou počtu bodových párů, ke které došlo díky výrazně zvětšenému lokálnímu maximu v rozdílech mezibodových vzdáleností (viz. horní graf na obr. 10.20). Tento graf v principu ukazuje rychlost konvergence algoritmu, který zkonvergoval ve 49. iteraci. Počet platných bodů v referenčním scanu bylo 81616,

128

KAPITOLA 10. EXPERIMENTÁLNÍ VÝSLEDKY

v registrovaném scanu pak 84364. Algoritmus byl implementován ve vývojovém prostředí Matlab, s externím voláním knihovny implementující vyhledávací strukturu k-D strom v jazyce C++. Celková doba běhu algoritmu byla 75 sec. 0.025 9 14 20 27 34 48

rozd´ıl [cm]

0.02 0.015 0.01 0.005 0

5.00

5.25

5.50

5.75

6.00

6.25

6.50

6.75

7.00

7.25

7.50

poˇrad´ı p´ aru [-]

x 104

vzd´ alenost [cm]

100 9 14 20 27 34 48

80 60 40 20 0

5.00

5.25

5.50

5.75

6.00

6.25

6.50

6.75

7.00

7.25

7.50

poˇrad´ı p´ aru [-]

x 104

Obr. 10.20: Rozložení mezibodových vzdáleností v jednotlivých iteracích 15 ∆x ∆y ∆z

posun / rotace [cm/0.1◦ ]

10

∆roll ∆pitch ∆yaw

5

0

−5

−10

−15 5

10

15

20

25

30

35

40

45

iterace [-]

Obr. 10.21: Konvergence jednotlivých parametrů transformace

50

10.6. VÝSLEDKY APLIKAČNÍCH ROZŠÍŘENÍ NAVRŽENÝCH METOD

Obr. 10.22: 3D scany ve svých výchozích pozicích

Obr. 10.23: 3D scany po vzájemné registraci

129

130

KAPITOLA 10. EXPERIMENTÁLNÍ VÝSLEDKY

10.6.3

3D model

Tvorba bodového 3D modelu může být chápána jako jedna z aplikací nové přesné lokalizační metody PTM. Pro tvorbu 3D modelu prostředí jsme použili speciální konfigurace robotu G2 s kolmou montáží dvou laserů tak jak je popsáno v kap. 9.2. Tato konfigurace je znázorněna na obr. 10.24. Pro vytváření trojrozměrného bodového modelu jsme použili kancelářské prostředí, ve kterém jsme PTM metodou schopni velmi přesně stanovit pozici, odkud byla v každém okamžiku snímána prostorová data, což je velmi důležité pro konzistenci prostorových dat. Prostředí kancelářského typu, pro které jsme vytvářeli trojrozměrný bodový model je zobrazeno na obr. 10.25. Na obr. 10.26 jsou dva druhy bodů, černé a červené. Červené body pochází z horizontálního scanneru, který zajišťuje lokalizaci robotu. Ve svém principu zobrazují půdorys prostředí odpovídající horizontální snímací rovině laserového scanneru. Černé body tvoří vlastní bodový model prostředí, který byl vytvořený postupem popsaný v kap. 9.2, tedy ze svislého laserového scanneru. Pro přehlednější zobrazení byly z obrázku vynechány body mají z-souřadnici větší než 3 m, čímž jsme zabránili zobrazení roviny stropu.

Obr. 10.24: Konfigurace robotu G2 pro sběr dat pro konstrukci 3D modelu Takto zpracovaná výsledná data byla dále použita jako vstup pro tvorbu trojrozměrného geometrického modelu prostředí v práci (Koutník a kol., 2006). Výsledky geometrického modelování vytvořené a nadále vyvíjené za spolupráce s dalšími autory však již nejsou součástí náplně této práce, která se zabývá zejména lokalizací a modely prostředí, které lokalizaci podporují. Numerické vyčíslení přesnosti takto zkonstruovaného modelu prostředí je ještě obtížnější než v případě dvojrozměrných modelů. Zde nezbývá než konstatovat, že přesnost takto vytvořeného modelu prostředí závisí zejména na přesnosti dvojrozměrné lokalizace, jejíž vlastnosti byly experimentálně zhodnoceny v předchozích sekcích 10.4.1 - 10.4.4. Výsledky experimentů doplněné o prezentační videa lze nalézt též na síti Internet na adrese: http://labe.felk.cvut.cz/~mazl/disertace.

10.6. VÝSLEDKY APLIKAČNÍCH ROZŠÍŘENÍ NAVRŽENÝCH METOD

Obr. 10.25: Výchozí prostředí pro tvorbu 3D modelu

Obr. 10.26: Vytvořený bodový 3D model prostředí z obr. 10.25

131

Kapitola 11

Shrnutí, dosažené cíle disertace 11.1

Shrnutí a přínos práce

V této práci je do hloubky řešena problematika lokalizace mobilních robotů v neznámém prostředí spolu se simultánním automatickým vytvářením mapy prostředí, ve kterém se robot pohybuje. Úvodní části práce se zabývají aplikacemi mobilní robotiky, řídicími systémy a senzorickými systémy, které jsou vhodné pro použití v mobilní robotice. Následuje přehled používaných metod pro lokalizaci a mapování včetně jejich rozboru a popisu jejich principů. Přestože rozbor problematiky lokalizace a mapování může působit dojmem, že úlohy z této oblasti jsou již vyřešeny, neustále zvyšující se odborný zájem o tuto oblast je důkazem opaku. Úlohy jsou často řešeny za nejrůznějších omezujících předpokladů, na základě nichž se úlohu podaří vyřešit. V této práci jsme se snažili minimalizovat počet výchozích předpokladů a byl kladen důraz na zpracování dat v reálném čase, nezávislost algoritmů na tvaru pracovního prostředí a na celkovou robustnost řešení. Základním přínosem práce je návrh a implementace kontinuální lokalizační metody Point-To-Map. Výstupem této metody je vedle lokalizace robotu též údržba modelu prostředí ve formě bodové mapy, která efektivním způsobem dokáže podpořit spolehlivost a dlouhodobou přesnost dříve navržené lokalizační metody Point-To-Point vycházející z principů ICP algoritmu a korelačních principů. Začleněním automatické stavby mapy prostředí do procesu lokalizace a jejich vzájemnou provázaností vznikl komplexní balík metod náležící do kategorie metod pro simultánní lokalizaci a mapování (Simultaneous Localization and Mapping - SLAM). S ohledem na možnost využití robotu jako nástroje pro vzdálenou inspekci prostředí dále vznikly nástroje umožňující tvorbu trojrozměrného modelu prostředí spolu s možností jeho vizualizace pro uživatele. Funkčnost a otevřenost celého systému již byla prověřena implementací řešení úlohy kooperativního prohledávání neznámého prostoru skupinou robotů. Dílčí výsledky a přínosy jednotlivých částí této práce jsou shrnuty v následujících podkapitolách. 133

134

KAPITOLA 11. SHRNUTÍ, DOSAŽENÉ CÍLE DISERTACE

11.1.1

Lokalizace mobilního robotu

S využitím znalostí současného stavu problematiky ve světě byly zhodnoceny vlastnosti stávajících lokalizačních metod a principů. Pro danou definici úlohy se nám podařilo vyvinout metodu kontinuální lokalizace v neznámém prostředí nazvanou Point-to-Point. Tato metoda byla dále rozšířena a zdokonalena vazbou na průběžně budovaný model prostředí, čímž vznikla metoda nazvaná Point-To-Map. Vstupem navržených algoritmů jsou kontinuálně přicházející data (scany) z rovinného laserového scanneru a odometrická informace přicházející od pohonových kol robotu. Postupně přicházející scany jsou registrovány vůči poloze předcházejících dat. Na základě registrace posledního scanu je určována poloha robotu v prostoru. Výstupem metody jsou souřadnice polohy mobilního robotu v prostředí ve formě trojice hodnot [x, y, ϕ], kde první dvě jsou souřadnice polohy robotu vztažené k referenčnímu bodu, třetí je relativní aktuální natočení robotu ve vztahu k jeho natočení v referenčním bodě. Jednotlivé dílčí přínosy práce jsou shrnuty v následujícím výčtu: • Byl navržen postup pro filtraci chybně změřených bodů v laserovém scanu. Laserový dálkoměr trpí systematickou chybou měření, kterou je nutné pro následující zpracování dat eliminovat. Filtrace využívá znalostí charakteristických vlastností chybně naměřených dat a příčin jejich vzniku. Rozpoznání a filtrace chybných měření je předpokladem pro správnou funkci navazujících algoritmů po lokalizaci robotu a tvorbu modelu světa. • Byla navržena metoda pro korekci hrubých odometrických chyb pracující na principu hledání maxima korelace histogramového popisu mezi dvěma po sobě následujícími scany. Byl navržen způsob výběru relevantních liniových segmentů segmentovaných z laserového scanu a jejich konverze do histogramového popisu scanu. • Byla vytvořena Point-To-Point metoda pro kontinuální lokalizaci robotu v neznámém prostředí založená na stávající metodě ICP. Hlavní myšlenka zdokonalující metodou ICP spočívá ve vylepšení kritického kroku metody ICP, kterým je určování a validace korespondujících párů. Hledání korespondujících párů je založeno na definici nového kritéria pro validaci potenciálních bodových párů. Nová metoda určování korespondujících párů přináší odolnost metody vůči odlehlým měřením a částečně i vůči pohybujícím se objektům. • Metoda Point-To-Point byla rozšířena o spolupráci s průběžně vytvářeným modelem prostředí, čímž vznikla lokalizační metoda Point-To-Map spadající do kategorie SLAM algoritmů. Tato metoda na rozdíl od metody Point-To-Point zajišťuje dlouhodobou stabilitu lokalizace a minimalizuje vznik a růst kumulativní lokalizační chyby, kterou z principu trpí metoda Point-To-Point. Metoda Point-To-Map umožňuje lokalizaci robotu i v předem vytvořené mapě, popřípadě je schopna přidávat do stávající mapy nové informace.

11.1. SHRNUTÍ A PŘÍNOS PRÁCE

135

• Lokalizační metoda PTP byla modifikována do trojrozměrné varianty úpravou kritéria validace bodových párů a změnou způsobu minimalizace kritéria popisující zobecněnou meziscanovou vzdálenost. Použití této metody je jednou z možností, jak lokalizovat polohu robotu v terénu v plných šesti stupních volnosti. • Řešení úloh v rámci skupiny kooperujících robotů si vyžádalo návrh dalších rozšíření Point-to-Map metody pro účely kooperativní lokalizace tak, aby celá skupina mobilních robotů pracovala v konzistentním souřadném systému a aby lokalizace každého z robotů nebyla ovlivněna pohybem blízkých partnerských robotů.

11.1.2

Výstavba modelu prostředí a datová reprezentace

Druhá část práce se zabývá tvorbou modelu prostředí, jehož budování má za cíl zvýšit kvalitu a robustnost lokalizačního algoritmu a umožnit přechod od metody Point-To-Point k metodě Point-To-Map. Jako ideální se ukázala bodová reprezentace modelu světa, která umožňuje modifikovat výchozí Point-To-Point metodu tak, že jako referenční data pro registraci příchozích scanů není používán pouze jeden předcházející scan, nýbrž průběžně vytvářený model prostředí, který obsahuje vyfiltrovanou konzistentní informaci ze všech předcházejících scanů. • Implementace inkrementálně budované bodové mapy v reprezentaci datovou strukturou k-D strom umožnilo efektivní hledání korespondujících bodových párů pro metodu Point-to-Map, což výrazným způsobem zkracuje potřebný čas pro výpočet polohy jednoho scanu. • Byl vytvořen postup pro dávkové vybudování bodové mapy, která má lepší vlastnosti než inkrementálně budovaná mapa z hlediska možnosti omezení vnášení šumu měření do mapy. Pro omezení šumu měření byl navržen filtr odstraňující osamocená senzorická měření. Takto vytvořená mapa je ideální pro sledování polohy robotu metodou PTM v prostředí, ze kterého byla již dříve robotem nasnímána senzorická data. • Na základě přesné 2D lokalizace metodou Point-to-Map byl implementován rychlý způsob tvorby bodového 3D modelu prostředí a jeho vizualizace pro účely uživatelského rozhraní.

11.1.3

Implementace a experimentální ověření algoritmů

Během vývoje algoritmů byla provedena velká řada experimentů. Na základě získaných zkušeností byly postupně zdokonalovány vlastnosti příslušných algoritmů. Výrazným přínosem této práce je též i to, že navržené postupy byly vedle laboratorních prototypů úspěšně implementovány ve formě znovu použitelného modulu do multirobotického řídicího systému. Předpokladem integrace algoritmů do reálného systému byla řada implementačních optimalizací, aby byly algoritmy schopné běhu v reálném čase. Realizací lokalizačních a mapovacích metod byl završen vývoj druhé generace experimentálního robotického systému založeného na platformě robotů G2.

136

11.2

KAPITOLA 11. SHRNUTÍ, DOSAŽENÉ CÍLE DISERTACE

Dosažené cíle disertace, porovnání se stanovenými cíli

V této části porovnáme stanovené cíle disertace, tak jak byly po jednotlivých bodech uvedeny v kap. 4, s reálně dosaženými cíli. 1. Souhrn stávajících metod. V kapitole 5 je uveden souhrn senzorických systémů, které se v mobilní robotice již používají nebo jsou velmi perspektivní pro masivní používání v budoucích letech. Byly popsány jejich vlastnosti, principy, oblasti použitelnosti a naznačeny problematické oblasti, se kterými se můžeme při používaní daných senzorů setkat. Kapitola 6.1 se zabývá popisem existujících metod lokalizace mobilních robotů a to jak v případě, že je k dispozici mapa prostředí, tak i bez ní. Navazující kapitola 6.2 se zabývá rozborem a vlastnostmi možných metod pro reprezentaci modelu prostředí. Existující kombinace přístupů k lokalizaci a mapování jsou rozebrány v kap. 6.3. 2. Zdokonalení lokalizační metody. Kapitola 7 popisuje návrh základní podoby lokalizační metody Point-To-Point, včetně nutných postupů pro předzpracování vstupních dat zahrnující filtraci a redukci extrémních odometrických chyb. Point-To-Point metoda byla v rámci kapitoly 8.5 dále zdokonalena a vznikla Point-To-Map metoda, která se řadí do třídy SLAM algoritmů. Důležitým cílem bylo zdokonalení stávajících lokalizačních metod, měli jsme možnost přímého srovnání výsledků s prací (Kulich, 2003), na kterou jsme tematicky částečně navazovali. Praktické experimenty v kap. 10.4.1 ukázaly, že při srovnání se starší metodou Line-To-Line dosahujeme použitím metody Point-To-Map téměř až o dva řády lepší přesnosti . 3. Reprezentace modelu prostředí. Kapitola 8 se zabývá vhodnou reprezentací modelu prostředí, která umožní zvýšit robustnost lokalizační metody a zároveň snížit její kumulativní chybu. Tohoto cíle se dosáhlo zavedením průběžné výstavby modelu světa, ve formě bodové mapy prostředí, kterou využívá navržená metoda Point-To-Map. Pro reprezentaci modelu prostředí byla použita struktura k-D strom, která má schopnost efektivně zodpovídat četné dotazy ohledně sousedství dílčích bodů. Tato vlastnost je plně využívána jednak při konstrukci samotné mapy, ale též v algoritmu Point-To-Map. 4. Aplikační rozšíření lokalizace a rozšíření do 3D. Požadavky kladené na lokalizační systém multirobotickými aplikacemi jsou řešeny v podkapitole 8.6, kde je popsáno rozšíření PTP a PTM metod pro prostředí kooperujících robotů. Zde jsme se zabývali vzájemnou lokalizací robotů a řešením kritických situací, které vzniknou během lokalizace ve skupině robotů. Princip PTP metody umožňuje její rozšíření do 3D, jehož popis je popsán v kap. 9.1. Vysoká dosažená přesnost PTM metody umožnila realizaci sběru prostorových dat z přídavného laserového scanneru (viz. kap. 9.2), ze kterých lze vybudovat 3D model prostředí. Na základě znalosti přesné polohy přídavného laseru v každém okamžiku se kompletní sada měření sloučí do bodového modelu, který může být vytvořenou externí apli-

11.3. OTEVŘENÉ PROBLÉMY, DALŠÍ ROZVOJ METOD

137

kací dále vizualizován. Tuto aplikaci lze využít pro navigaci robotu v prostředí, neboť uživateli umožňuje zobrazit semiprostorový obraz prostředí z libovolného místa v prostoru v libovolném úhlu pohledu. 5. Implementace lokalizačního a mapovacího modulu. Do stávajícího řídicího systému robotu G2 byl zaintegrován modul implementující metodu Point-To-Map lokalizující robot při příchodu každého laserového scanu. 6. Experimentální ověření. Vlastnosti vyvinutých metod byly experimentálně ověřeny na třech zcela odlišných typech prostředí, byly provedeny též srovnávací testy vůči stávající metodě kontinuální lokalizace LTL. Výsledky experimentů shrnuté v kap. 10 dokládají, že se úspěšně podařilo vyvinout robustní lokalizační modul, který je schopen v reálném čase lokalizovat mobilní robot v rovinném prostředí. Navržená metoda PTM pracuje výborně ve vnitřních prostorech budov jako jsou kanceláře či chodby. Nad rámec vytyčených cílů je metoda PTM schopna v omezené míře lokalizovat mobilní robot i v exteriérových prostředích s přibližně rovinným povrchem. Žádoucích výsledků bylo dosaženo zejména integrací výstavby mapy prostředí do procesu lokalizace.

11.3

Otevřené problémy, další rozvoj metod

Předložená práce ukazuje, že lokalizace mobilního robotu na základě odometrie a horizontálního laserového scanneru je pro práci ve vnitřních prostorech prakticky vyřešena s přesností, která vyhoví téměř všem potenciálním aplikacím. Další rozvoj lze spatřovat zejména ve zdokonalování vlastností lokalizace robotu v prostředích exteriérové povahy, ve kterém navíc nemusí být pohyb robotu omezen na jednu horizontální rovinu tak, jak jsme si stanovili my v cílech své práce. Při pohybu robotu v šesti stupních volnosti se již nevystačí s jedním pevným horizontálním laserovým scannerem, pro lokalizaci je třeba použít buď principů založených na postupech uvedených v kap. 9.1.3 nebo použít zcela jiných měřicích principů a metod. Velký potenciál pro lokalizaci v šesti stupních volnosti lze spatřovat ve využití metod zpracování obrazu, popř. v kombinaci využití dat z laserového scanneru a kamery. Další možnosti pravděpodobně přinesou též nové perspektivní druhy senzorů na bázi 3D kamer. V exteriérových prostředích lze navíc počítat s dostupností signálů satelitních navigačních sítí, algoritmy se proto mohou opírat i o sdružování polohových informací z různých senzorických zdrojů. Vedle lokalizace robotu v prostoru existuje spousta otevřených problémů v oblasti aktivní kooperativní lokalizace, kdy jednotlivé roboty mají mezi sebou omezený komunikační kanál či nemají výchozí znalost polohy svých kolegů. Další varianty úloh spočívají v upřesňování pozice jednoho robotu s ohledem na polohy ostatních robotů v týmu ve smyslu globálních optimalizací vedoucích na minimalizaci celkové lokalizační chyby. Tato úloha získá na zajímavosti, pokud se jednotlivé roboty budou pohybovat a lokalizovat bez nutnosti trvalé komunikace se svými partnery. S touto problematikou souvisí i oblast

138

KAPITOLA 11. SHRNUTÍ, DOSAŽENÉ CÍLE DISERTACE

aktivní lokalizace či explorace prostředí, kdy vykonávaná trajektorie robotu je přizpůsobována potřebám lokalizačních algoritmů. Z hlediska algoritmů pro vytváření mapy jsme se soustředili na takovou reprezentaci, která by byla dobře použitelná pro podporu lokalizace. Bodová reprezentace je pro tento účel plně vyhovující, ovšem pro použití např. v algoritmech plánování pohybu či trajektorií již příliš vhodná není. Pro tento účel jsou nejvýhodnější geometrické mapy. Automatická tvorba geometrické mapy spolu s inkrementálními aktualizacemi jsou z velké části otevřené problémy. Dimenze problému inkrementální stavby geometrické mapy roste též v kontextu multirobotických úloh, kdy musí být zajištěna konzistence vytvářené mapy více roboty najednou, řešeny nejednoznačnosti v datech apod. Námi vytvořená bodová mapa může být použita jako vhodný základ pro budování geometrické mapy. Shlukování liniových skupin bodů lze prohlásit za výchozí krok pro tvorbu liniových segmentů. Návaznost dílčích skupin bodů by mohla být řešitelná např. aproximačními algoritmy řešící úlohu obchodního cestujícího nebo by mohla být formulována jako globální optimalizační úloha. Vytvořený prostorový bodový model je opět základem pro rozvoj řady metod pro aproximaci prostorových povrchů z roztroušených prostorových bodů. Tímto směrem se již vydala práce (Koutník a kol., 2006) a lze očekávat, že aktivity zaměřené na rekonstrukce povrchů z roboticky nasnímaných prostorových dat budou v následujících obdobích pokračovat. Jejich využití se předpokládá zejména v systémech virtuální reality či realistické teleoperace různorodých entit ve známém prostředí. V současné době existuje řada prací (Šára a Bajcy, 1998), (Amenta a kol., 2001), (Liua a kol., 2006) řešící problematiku rekonstrukce povrchů těles, ovšem roboticky získaná data námi popsaným způsobem mají zpravidla zcela jinou povahu i účel využití, než prostorová mračna bodů získaných z přesných 3D scannerů. Základním rozdílem je, že nejde o uzavřené objekty, ale naopak o částečné pohledy na scénu s mnoha objekty a s velmi odlišnou mírou užitečných detailů a potřebnými stupni abstrakce. Z robotického pohledu zpravidla není vhodné modelovat např. nerovnosti na zdi, je vhodné zeď modelovat celistvou plochou, ovšem na druhou stranu je třeba získat pokud možno přesný obraz blízkých překážek, které mohou ovlivňovat vykonávání příslušných činností robotu. Tyto důvody vedou na poněkud odlišný přístup problému, což bude pravděpodobně součástí dalšího budoucího výzkumu.

Kapitola 12

Závěr Předkládaná práce se zabývá hlavním tématem lokalizace mobilního robotu v neznámém prostředí spolu s budováním modelu světa, který podporuje činnost lokalizačního procesu. Vedle základní varianty metod pro práci ve dvojrozměrném prostředí jsou představena i rozšíření těchto metod pro práci s třetím rozměrem. Budování modelu světa je s lokalizací velmi úzce spjato, proto jsou oba problémy řešeny společně. Práce určitým způsobem logicky navazuje na disertační práce RNDr. P. Štěpána, Ph.D. a RNDr. M. Kulicha, Ph.D., jejichž výsledky byly použity pro srovnání. Ukázalo se, že námi navržená metoda PTM dosahuje z hlediska lokalizace mobilního robotu výrazně lepších výsledků a to jak z hlediska přesnosti, tak i z hlediska rychlosti výpočtu. Vyvinuté metody mají do budoucna potenciál širokého využití i dalšího rozvoje, experimentální výsledky potvrzují, že dosažené přesnosti jsou plně dostačující pro většinu potenciálních aplikací, jako jsou např. automatické zásobovací systémy, autonomní čistící stroje nebo i aplikace pro speciální sběr dat s vazbou na přesnou polohu atd. Vytvořený lokalizační modul tvoří nedílnou součást experimentální robotické platformy G2 a bude sloužit pro podporu budoucí výzkumné práce členů skupiny Inteligentní mobilní robotiky na Katedře kybernetiky ČVUT FEL v Praze. Experimentální robotická platforma již teď slouží jako experimentální nástroj pro experimentální ověřování výsledků aktivit v rámci Centra aplikované kybernetiky v Praze a budou jej používat studenti v rámci nově vzniklého předmětu Mobilní robotika na Katedře kybernetiky ČVUT FEL v Praze.

139

Literatura Amenta, N., Choi, S. a Kolluri, R. K. (2001). The power crust. In: Proc. of the sixth ACM Symposium on Solid Modeling and Applications. pp. 249–266. Argall, B., Browning, B., Gu, Y. a Veloso, M. M. (2006). The first segway soccer experience: Towards peer-to-peer human-robot teams. In: Proc. of the 2006 Conference on Human-Robot Interaction. Salt Lake City, Utah, USA. Atkin, R. C. (1990). Integrating behavioral, perceptual and world knowledge in reactive navigation. Robotic and Autonomous System 6(2), 105–122. Babička, L. (2006). Robot na vizitě, http://www.prazskapetka.cz/node/4691. Pražská pětka - měsíčník Městské části Praha 5. Besl, P.J. a McKay, N.D. (1992). A method for registration of 3-d shapes. IEEE Transactions on Pattern Analysis and Machine Intelligence 2(14), 239–256. Blackman, S.S. a Popoli, R. (1999). Design and Analysis of Modern Tracking Systems. Artech House Radar Library. Borges, G., A. a Aldon, M., J. (2000). A split-and-merge segmentation algorithm for line extraction in 2-d range images. In: 15th International Conference on Pattern Recognition (ICPRVision-based perception for an automated harvester.’00). Vol. 1. p. 1441–1445. Buhmann, J. M., Burgard, W., Cremers, A. B., Fox, D., Hofmann, T., Schneider, F. E., Strikos, J. a Thrun, S. (1995). The mobile robot RHINO. AI Magazine 16(2), 31–38. Canny, J.F. (1988). Constructing roadmaps of semi-algebraic sets i: Completeness,. Artificial Intelligence 37, 203–222. Cassandra, A. R., Kaelbling, L. P. a Kurien, J. A. (1996). Acting under uncertainty: Discrete bayesian models for mobile robot navigation. In: Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems. Castellanos, J. a Tardós, J. D. (1999). Mobile Robot Localization and Map Building. Kluwer Academic Publishers. London. 141

142

LITERATURA

Castellanos, J., Montiel, J., Neira, J. a Tardos, J. (1999). Sensor influence in the performance of simultaneous mobile robot localization and map building. In: P. Corke and J. Trevelyan, editors, Experimental Robotics IV. Springer-Verlag. p. 287–296. Chmelař, B. (1998). Interpretace hloubkové informace pro využití v mobilní robotice. Diplomová práce. ČVUT - FEL. Praha. Chmelař, B. (1998). Aktualizace polohy robota. Research Report GLČ. 20/98. Czech Technical University in Prague. Choset, H. a Nagatani, K. (2001). Topological simultaneos localization and mapping(slam):toward exact localisation without explicit localization. IEEE Transaction on Robotics and Automation 17(2), 125–137. Chudoba, J., Mázl, R. a Přeučil, L. (2006). A control system for multi-robotic communities. In: Proc. of the 11th IEEE International Conference on Emerging Technologies and Factory Automation, ETFA’06. Prague. pp. 827–832. Connel, J. (1990). Minimalist Mobile Robotics: A Colony-style Architecture for an Artificial Creature. Academic Press. Connolly, C. a Grupen, C. (1993). The application of harmonic functions to robotics. Journal of Robotic Systems 7(10), 931–946. Cox, I. J. (1991). Blanche - an experiment in guidance and navigation of an autonomous robot vehicle. IEEE Transaction on Robotics and Automation 7(2), 193–204. Cox, I.J. a Leonard, J.J. (1994). Modeling a dynamic environment using a bayesian multiple hypothesis approach. Artificial Intelligence 66(2), 311–344. CrossBow (2007). Crossbow inertial systems. http://www.xbow.com. Crowley, J. L. (1989). World modeling and position estimation for a mobile robot using ultrasonic ranging. In: IEEE International Conference on Robotics and Automation. Vol. 2. Scottsdale, AZ, USA. p. 674–680. Dailey, M. N. a Parnichkun, M. (2005). Landmark-based simultaneous localization and mapping with stereo vision. In: Proceedings of the 2005 Asian Conference on Industrial Automation and Robotics (ACIAR ’05). Danaher Motion (2003). Case studies – motol hospital, http://www.danahermotion. se/solutions-evs-agv-case_studies-motol.htm, http://www.lazerway.com/ cases/motol.html. Dautenhahn, K., Davis, M. a Robins, B. (2000). AuRoRa project, http://www. aurora-project.com. Dellaert, F., Burgard, W., Fox, D. a Thrun, S. (1999). Using the condensation algorithm for robust, vision-based mobile robot localization. IEEE Computer Society Conference on Computer Vision and Pattern Recognition (CVPR’99).

LITERATURA

143

Docet, A., Freitas, N. a Gordan, N. (2001). Sequential Monte Carlo Methods in Practice. Springer-Verlag. New York. Driewer, F., Baier, H. a Schilling, K. (2005). Robot–Human Rescue Teams: a User Requirements Analysis. Advanced Robotics 19(8), 819–838. Duckett, T. (2003). A genetic algorithm for simultaneous localization and mapping. In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA’2003). Taipei, Taiwan. p. 434–439. Duda, R., O. a Hart, P., E. (1973). Pattern Classification and Scene Analysis. John Wiley & Sons. New York, USA. Dudek, G., Freedman, P. a Hadjres, S. (1993). Using local information in a non-local way for mapping graph-like worlds. In: Proceedings of Intl. Joint Conference of Artifical Intelligence. Chambery, France. Durrant-Whyte, H. F. a Leonard, J. J. (1991). Simultaneous map building and localisation for an autonomous robot. In: IEEE/RSJ International Workshop on Intelligent Robots and Systems IROS ’91. IEEE. pp. 1442–1447. E&K Automation (2007). Indumat - laser navigation. http://www.ek-automation. com/uploads/media/Siemens_kurz_3_01.mpg, http://www.ek-automation.com/ uploads/media/Brochure_Indumat_EN.pdf. Electrolux (2007). Trilobite vacuum cleaner robot. http://trilobite.electrolux.com. Elfes, A. (1990). Occupancy grids: A stochastic spatial representation for active robot perception. In: Proceedings of the Sixth Conference on Uncertainty in AI. Morgan Kaufmann Publishers, Inc. . 2929 Campus Drive, San Mateo, CA 94403. Elinas, Pantelis a Little, James J. (2005). σMCL: Monte-Carlo localization for mobile robots with stereo vision. In: Proceedings of Robotics: Science and Systems. Cambridge, USA. Eurobot (2007). Robotická soutež Eurobot. http://www.eurobot.org. Fabrizi, E. a Saotti, A. (2000). Extracting topology-based maps from gridmaps. In: IEEE Intl. Conf. on Robotics and Automation (ICRA). San Francisco, CA. p. 2972–2978. FIRA (2007). Federation of International Robosoccer Association. http://www.fira.net. Fox, D., Burgard, W. a Thrun, S. (1999a). Markov localization for mobile robots in dynamic environments. Journal of Artificial Intelligence Research. Fox, D., Burgard, W., Dellaert, F. a Thrun, S. (1999b). Monte carlo localization: Efficient position estimation for mobile robots. In: AAAI/IAAI. pp. 343–349.

144

LITERATURA

Gutmann, J. a Schlegel, C. (1996). Amos: Comparison of scan matching approaches for selflocalization in indoor environments. In: Proceedings of the 1st Euromicro Workshop on Advanced Mobile Robots. IEEE Computer Society Press. Gutmann, J. S., Burgard, W., Fox, D. a Konolige, K. (1998). An experimental comparison of localization methods. In: Proc. of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS’98), 1998. Gutmann, J., Weigel, T. a Nebel, B. (2001). A fast, accurate, and robust method for selflocalization in polygonial environments using laser-range-finders. Advanced Robotics 14(8), 651–668. Hähnel, D., Fox, D., Burgard, W. a Thrun, S. (2003). A highly efficient FastSLAM algorithm for generating cyclic maps of large-scale environments from raw laser range measurements. In: Proceedings of the Conference on Intelligent Robots and Systems (IROS). Hähnel, D., Schulz, D. a Burgard, W. (2002). Map building with mobile robots in populated environments. In: Proc. of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Haralick, R. M. a Shapiro., L. G. (1992). Computer and Robot Vision. Addison-Wesley. New York. Havel, I. (1980). Robotika - úvod do teorie kognitivních robotů. SNTL. Praha. Hebert, M. (2000). Active and passive range sensing for robotics. In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA ’00). Vol. 1. pp. 102 – 110. Hirukawa, H., Kanehiro, F., Kaneko, K., Kajita, S., Fujiwara, K., Kawai, Y., Tomita, F., Hirai, S., Tanie, K. a et al., T. Isozumi (2004). Humanoid robotics platforms developed in HRP. Robotics and Autonomous Systems 48(4), 165–175. Horn, B. K. P. (1987). Closed-form solution of absolute orientation using unit quaternions. Journal of the Optical Society of America 4(A), 629–633. Huang, W. H. a Beevers, K. R. (2005). Topological map merging. International Journal of Robotics Research 24(8), 601–613. InTouch Health (2006). Rp-7 system, http://www.intouchhealth.com/products-RP7. html. iRobot (2007). Roomba - vacuum cleaning robot. http://www.irobot.com. Isard, M. a Blake, A. (1998). Condensation — conditional density propagation for visual tracking. International Journal of Computer Vision 29(1), 5–28.

LITERATURA

145

Jensfelt, P. a Kristensen, S. (1999). Active global localisation for a mobile robot using multiple hypothesis tracking. In: In Proc. of the IJCAI-99 Workshop on Reasoning with Uncertainty in Robot Navigation. Jochem, T. (1996). Vision Based Tactical Driving. PhD thesis. Robotics Institute, Carnegie Mellon University. Pittsburgh, PA. Jochem, T., Pomerleau, D., Kumar, B. a Armstrong, J. (1995). Pans: A portable navigatin platform. In: IEEE Symposium on Intelligent Vehicles. IEEE. Kalman, R. E. (1960). A new approach to linear filtering and prediction problems. Transactions of the ASME–Journal of Basic Engineering 82(Series D), 35–45. Kazakov, D., Jelínek, L., Malý, K. a Štěpánková, O. (1997). Komunikace s robotem v přirozeném jazyce. Technická zpráva GLČ 07/97. ČVUT FEL, Katedra řídicí techniky. Praha. Kortenkamp, D. a Weymouth, T. (1994). Topological mapping for mobile robots using a combination of sonar and vision sensing. In: Proceedings of the Twelfth National Conference on Artificial Intelligence (AAAI-94). Koutník, J., Mázl, R. a Kulich, M. (2006). Building of 3d environment models for mobile robotics using self-organization. In: Proc, of The 9th International Conference on Parallel Problem Solving From Nature - PPSN-IX. Springer. pp. 721–730. Kulich, M., Faigl, J. a Přeučil, L. (2005). Cooperative planning for heterogeneous teams in rescue operations. In: IEEE International Workshop on Safety, Security and Rescue Robotics. IEEE. Piscataway. pp. CD–ROM. Kulich, M., Kout, J., Přeučil, L., Mázl, R., Chudoba, J., Saarinen, J., Suomela, J., Halme, A., Driewer, F., Baier, H., Schilling, K., Ruangpayoongsak, N. a Roth, H. (2004a). PeLoTe – a heterogeneous telematic system for cooperative search and rescue missions. In: Urban search and rescue: from Robocup to real world applications, in conjunction with the 2004 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Sendai, Japan. Kulich, M., Mázl, R. a Přeučil, L. (2001). Self-localization methods for autonomous mobile robots based on range scan-matching. In: In: Proc. of the International Conference on Field and Service Robotics (Aarne Halme, Raja Chatila a Erwin Prassler, Eds.). Helsinky, Finland. pp. 373–378. Kulich, M., Pavlíček, J., Chudoba, J., Mázl, R. a Přeučil, L. (2004b). Hybrid telematic systems for rescue missions. In: 17th European Meeting on Cybernetics and System Research (EMCSR2004). Austria, Vienna. pp. 731–736. Kulich, Miroslav (2003). Lokalizace a tvorba modelu prostředí v inteligentní robotice. Disertační práce. ČVUT, Elektrotechnická fakulta, katedra kybernetiky. Praha.

146

LITERATURA

Lawrence, A. (1998). Modern Inertial Technology: Navigation, Guidance, and Control. Mechanical Engineering. 2nd ed. . Springer. Lefevre, P., Prüß, A. a Zimmer, U. R. (1994). ALICE - topographic exploration, cartography and adaptive navigation on a simple mobile robot. In: TSRPC’94. Leeuwenhorst, The Netherlands. p. 5. Lerner, Preston (2006). The army’s robot sherpa, http://www.livescience.com/ technology/060409_robot_sherpa.html. Popular Science. Liua, H., Yana, J. a Zhang, D. (2006). Three-dimensional surface registration: A neural network strategy. Neurocomputing 70, 597–602. Lowe, D. G (1999). Object recognition from local scaleinvariant features. In: Proc. of the Seventh Int. Conf. on Computer Vision (ICCV’99). Kerkyra, Greece. p. 1150–1157. Lu, F. a Milios, E. (1994). Robot pose estimation in unknown environments by matching 2D range scans. In: CVPR94. pp. 935–938. Lu, F. a Milios, E. (1997). Globally consistent range scan alignment for environment mapping. Autonomous Robots 4, 333–349. Luethi, P. a Moser, T. (2000). Low cost inertial navigation system - design and characterization of a strapdown inertial navigation system based on low cost sensors. Students project. Electronics Laboratory of the Swiss Federal Institute of Technology. Zurich, Switzerland. MacKenzie, P. a Dudek, G. (1994). Precise positioning using model-based maps. In: IEEE Int. Conf. on Robotics and Automation. p. 1615–1621. Mařík, V., Štěpánková, O. a Lažanský, J. (2001). Umělá inteligence 3. Academia. Malý, K. (2005). Vytvoření symbolického modelu světa pro řízení robota. Disertační práce. ČVUT FEL, Fakulta elektrotechnická, Katedra kybernetiky. Matlin, M. W. (2004). Cognition. 6 ed. . John Wiley & Sons. Mázl, R. a Přeučil, L. (2000). Building a 2D environment map from laser range-finder data. In: Proc. of the IEEE Intelligent Vehicles Symposium 2000. IEEE. Dearborn, Michigan, USA. pp. 290–295. Mázl, R. a Přeučil, L. (2003a). Sensor data fusion for inertial navigation of trains in GPS-dark areas. In: Proceeding of the IEEE Intelligent Vehicles Symposium 2003. Ohio,USA. pp. 345–350. Mázl, R. a Přeučil, L. (2003b). Vehicle localization using inertial sensors and GPS. In: Proc. of The 4th International Conference on Field and Service Robotics. University of Tsukuba. Japan. pp. 195–200.

LITERATURA

147

Mázl, R., Kulich, M. a Přeučil, L. (2001a). Range-based localization methods for mobile robots in comlex environments. In: 2001 IEEE Intelligent Transportation Systems Proceedings. Oakland CA, USA. pp. 280–285. Mázl, R., Kulich, M. a Přeučil, L. (2001b). Statistical and feature-based methods for mobile robot position localization. In: Database and Expert Systems Applications. Vol. 1. Heidelberg : Springer. pp. 518–526. Mázl, R., Pavlíček, J. a Přeučil, L. (2005). Structures for data sharing in hybrid rescue teams. In: Proc. of the IEEE International Workshop on Safety, Security and Rescue Robotics, SSRR’05. Kobe. pp. 236–241. Mitsubishi Heavy Industries (2006). Wakamaru, http://www.mhi.co.jp/kobe/wakamaru, http://www.linuxexpres.cz/linuxovi-roboti. Montemerlo, M. a Thrun, S. (2003). Simultaneous localization and mapping with unknown data association using FastSLAM. In: Proceedings of the 2003 IEEE International Conference on Robotics and Automation, ICRA 2003. IEEE. pp. 1985–1991. Montemerlo, M., Thrun, S., Koller, D. a Wegbreit, B. (2002). Fastslam: A factored solution to the simultaneous localization and mapping problem. Moore, A. (1991). An introductory tutorial on kd-trees. Technical Report Technical Report No. 209, Computer Laboratory, University of Cambridge. Robotics Institute, Carnegie Mellon University. Pittsburgh, PA. Available from http://www.cs.cmu.edu/simawm/papers.html. Moravec, H. a Elfes, A. (1985). High resolution map from wide-angle sonar. In: Proceedings of the IEEE International Conference on Robotics and Automation. pp. 116–121. Moreno, L., Garrido, S. a Mu˜ noz, M.L. (2006). Evolutionary filter for robust mobile robot global localization. Robotics and Autonomous Systems 54(7), 590–600. Moutarlier, P. a Chatila, R. (1989a). An experimental system for incremental environment modelling by an autonomous mobile robot. In: 1st International Symposium on Experimental Robotics (ISER 1989). Springer-Verlag. London, UK. p. 327–346. Moutarlier, P. a Chatila, R. (1989b). Stochastic multisensory data fusion for mobile robot location and environment modelling. In: Fifth International Symposium on Robotics Research. Tokyo, Japan. p. 85–94. Müller, P. A. (1997). Instant UML, anglický překlad. Wrox Press Ltd. . Canada. Murphy, K. a Russel, S. (2001). Sequential Monte Carlo Methods in Practise. Chap. RaoBlackwellised Particle Filtering for Dynamic Bayesian Networks. Springer. Nagasaki, T., Kajita, S., Yokoi, K., Kaneko, K. a Tanie, K. (2003). Running pattern generation and its evaluation using a realistic humanoid model. In: International Conference on Robotics & Automation, ICRA 2003. Taipei, Taiwan. p. 1336–1342.

148

LITERATURA

Nakaoka, S., Nakazawa, A., Yokoi, K., Hirukawa, H. a K.Ikeuchi (2003). Generating whole body motions for a biped humanoid robot from captured human dances. In: International Conference on Robotics & Automation, ICRA 2003. Taipei, Taiwan. NDC (2006). The new driving forces behind automation, http://www.ndc.se/ pressroom/pdf/nr13/news13_eng.pdf, http://www.lazerway.com/cases/motol. html. NDC News - Information from Netzler & Dahlgren Co AB 1(13), 4–5. Nieto, J., Guivant, J. E., Nebot, E. M. a Thrun, S. (2003). Real time data association for fastslam. In: Proceedings of the 2003 IEEE International Conference on Robotics and Automation, ICRA 2003. pp. 412–418. Nourbakhsh, I., Bobenage, J., Grange, S., Lutz, R., Meyer, R. a Soto, A. (1999). An affective mobile educator with a full-time job. Artificial Intelligence 114(1 - 2), 95 – 124. Nourbakhsh, I., Powers, R. a Birchfield, S. (1995). DERVISH an office-navigating robot. AI Magazine 16(2), 53–60. Ollis, M. a Stentz, A. (1997). Vision-based perception for an automated harvester. In: Proceedings of IROS’97. Vol. 3. Grenoble. p. 1838–1844. Pitt, M. K. a Shephard, N. (1999). Filtering via simulation: Auxiliary particle filters. Journal of the American Statistical Association 94(446), 590–631. Pollack, M., Engberg, S., Matthews, J.T., Thrun, S., Brown, L., Colbry, D., Orosz, C., Peintner, B., Ramakrishnan, S., Dunbar-Jacob, J., McCarthy, C., Montemerlo, Michael, Pineau, Joelle a Roy, Nicholas (2002). Pearl: A mobile robotic assistant for the elderly, http://www.cs.cmu.edu/~nursebot. In: Workshop on Automation as Caregiver: the Role of Intelligent Technology in Elder Care (AAAI). Přeučil, L. a Štěpán, P. (1996). Intelligent self guided vehicles: Common methods, requirements and optimal design. Technical report. École Nationale Supérieure des Télécommunications. Paris. Přeučil, L. a Mázl, R. (2006). Field and Service Robotics: Recent Advances in Research and Applications. Chap. Vehicle Localization Using Inertial Sensors and GPS, pp. 135– 144. Vol. 24 of Springer Tracts in Advanced Robotics. Springer Berlin. Přeučil, L., Mázl, R., Štěpán, P. a Kulich, M. (2002). Towards environment modeling by autonomous mobile system. In: Proc. of the Knowledge and Technology Integration in Production and Service - Balancing Knowlege and Technology in Product and Service Life Cycle (BASYS 2002) (Vladimír Marík, Luis M. Camarinha-Matos a Hamideh Afsarmanesh, Eds.). Kluwer. Cancun, Mexico. Ranganathan, A., Menegatti, E. a Dellaert, F. (2006). Bayesian inference in the space of topological maps. IEEE Transactions on Robotics 22(1), 92–107.

LITERATURA

149

Robins, B., Dautenhahn, K., Boekhorst, R., Te a Billard, A. (2005). Robotic assistants in therapy and education of children with autism: Can a small humanoid robot help encourage social interaction skills?. In: Access in the Information Society (UAIS). Vol. 4. Springer-Verlag. School of Computer Science, The University of Hertfordshire, College Lane, Hat.eld, Hertfordshire, AL10 9AB, UK. p. 105–120. RoboCupRescue (2007). Projekty řady robocup. http://www.rescuesystem.org/ robocuprescue. Rˇofer, T. (2002). Using histogram correlation to create consistent laser scan maps. In: In the IEEE International Conference on Robotics Systems (IROS-2002). EPFL, Lausanne, Switzerland. p. 625–630. Saarinen, J., Mázl, R., Ernest, P., Suomela, J. a Preucil, L. (2004a). Sensors and methods for human dead reckoning. In: Proc. of the 8th Conference of Intelligent Autonomous Systems, IAS-8 (Frans Groen, Nancy Amato, Andrea Bonarini, Eiichi Yoshida a Ben Kröse, Eds.). Amsterdam. p. 8. Saarinen, J., Mázl, R., Kulich, M., Suomela, J., Preucil, L. a Halme, A. (2004b). Methods for personal localization and mapping. In: Proceedings of the 5th IFAC/EURON symposium on Intelligent Autonomous Vehicles, IAV’04. Lisboa, Portugal. Schiele, B. a Crowley, J. L. (1994). A comparison of position estimation techniques using occupancy grids. In: In Proceedings of the 1994 IEEE International Conference on Robotics and Automation. San Diego, CA. p. 1628–1634. Schultz, A., Adams, W. a Yamauchi, B. (1999). Integrating exploration, localization, navigation and planning with a common representation. Autonomous Robots 6(3), 293–308. Se, S., Lowe, D. a Little, J. (2002). Mobile robot localization and mapping with uncertainty using scale-invariant visual landmarks. International Journal of Robotics Research 21(8), 735–758. Shaffer, G. (1995). Two-Dimensional Mapping of Expansive Unknown Areas. Disertační práce. Robotics Institute, Carnegie Mellon University. Pittsburgh, PA. Shatkay, H. a Kaelbling, L. P. (1997). Learning topological maps with weak local odometric information. In: IJCAI (2). pp. 920–929. Sim, R. a Dudek, G. (1999). Learning and evaluating visual features for pose estimation. In: Proceedings of the Seventh International Conference on Computer Vision (ICCV’99). Vol. 2. IEEE Computer Society. Washington, DC, USA. p. 1217–1222. Simmons, R. a Koenig, S. (1995). Probabilistic robot navigation in partially observable environments. In: Proceedings of the International Joint Conference on Artificial Intelligence. p. 1080–1087.

150

LITERATURA

Smith, R., Self, M. a Cheeseman, P. (1990). Autonomous Robot Vehicles. Chap. Estimating Uncertain Spatial Relationships in Robotics. Springer Verlag. Stachniss, C., Haehnel, D. a Burgard, W. (2004). Exploration with active loop-closing for FastSLAM. In: Proc. of the IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS). p. 7. Stengel, R., F. (2004). Flight Dynamics. Princeton University Press. Šára, R. a Bajcy, Ruzena (1998). Fish-scales: Reprezenting fuzzy manifolds. In: Proc. of the Int. Conference on Machine Vision ICCV’98. pp. 811–817. Šišlák, D., Rehák, M., Pěchouček, M., Rollo, M. a Pavlíček, D. (2005). A-globe: Agent development platform with inaccessibility and mobility support. In: Software Agent-Based Applications, Platforms and Development Kits (Rainer Unland, Matthias Klusch a Monique Calisti, Eds.). Birkhauser Verlag. Berlin. p. 21–46. Šonka, M. a Hlaváč, V. (1992). Počítačové vidění. Grada. Praha. Šonka, M., Hlaváč, V. a Boyle, R. (1998). Image Processing, Analysis and Machine Vision. PWS. Boston. Štěpán, P. (2001). Vnitřní reprezentace prostředí pro autonomní mobilní roboty. Disertační práce. ČVUT, Elektrotechnická fakulta, katedra kybernetiky. Praha. Štěpán, P., Kulich, M. a Přeučil, L. (2005). Robust data fusion with occupancy grid. IEEE Transactions on Systems, Man, and Cybernetics: Part C 35(1), 106–115. Tardós, J. D. (1992). Representing partial and uncertain sensorial information using the theory of symmetries. In: Proc. IEEE Int. Conf. Robotics and Automation. Nice, France. pp. 1799–1804. Thrun, S. (1998). Bayesian landmark learning for mobile robot localization. Machine Learning 33(1), 41–76. Thrun, S. (2000). Probabilistic algorithms in robotics. AI Magazine 21(4), 93–109. Thrun, S. a Bucken, A. (1996). Integrating grid-based and topological maps for mobile robot navigation. In: AAAI/IAAI, Vol. 2. pp. 944–950. Thrun, S., Burgard, W. a Fox, D. (1998). A probabilistic approach to concurrent mapping and localization for mobile robots. Machine Learning 31(1-3), 29–53. Thrun, S., Fox, D., Burgard, W. a Dellaert, F. (2001). Robust monte carlo localization for mobile robots. Artificial Intelligence 128(1-2), 99–141. Thrun, S., Koller, D., Ghahramani, Z., Durrant-Whyte, H. a Ng, A. (2002). Simultaneous mapping and localization with sparse extended information filters: Theory and initial results. Research Report CMU-CS-02-112. School of Computer Science, Carnegie Mellon University. Pittsburgh, PA 15213.

LITERATURA

151

Thrun, S., Montemerlo, M., Koller, D., Wegbreit, B., Nieto, J. a Nebot, E. (2004). Fastslam: An efficient aolution to the simultaneous localization and mapping problem with unknown data association. Journal of Machine Learning Research (to appear). Thrun, S., Schulte, J. a Rosenberg, C. R. (2000). Robots with humanoid features in public places: A case study. IEEE Intelligent Systems 15(4), 7–11. Tišnovský, P. (2003). Bezkontaktní digitalizace předmětů pomocí 3D scanneru minolta vivid vi-700. Elektrorevue. Toyota (2007). Toyota partner robot. http://www.toyota.co.jp/en/special/robot/. Tuley, J., Vandapel, N. a Hebert, M. (2004). Analysis and removal of artifacts in 3-d ladar data. Technical Report CMU-RI-TR-04-44. Robotics Institute, Carnegie Mellon University. Pittsburgh, PA. Vandorpe, J., Brussel, H., V. a Xu, H. (1996). Exact dynamic map muilding for a mobile robot using geometrical primitives produced by a 2D range finder. In: Proceedings of the IEEE International Conference on Robotics and Automation. p. 901–908. Veloso, M. M., Rybski, Paul E. a von Hundelshausen, Felix (2006). FOCUS: a generalized method for object discovery for robots that observe and interact with humans. In: Proc. of the 2006 Conference on Human-Robot Interaction. Salt Lake City, Utah, USA. Weiß, G. a Puttkamer, E. (1995). A map based on laserscans without geometric interpretation. In: U. Rembold et al. (Eds.) Intelligent Autonomous systems. IOS Press. p. 403–407. Weiß, G., Wetzler, C. a Puttkamer, E. (1994). Keeping track of position and orientation of moving indoor systems by correlation of range-finder scans. In: Proc. of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Munich, Germany. p. 595–601. Wolf, J., Burgard, W. a Burkhardt, H. (2005). Robust vision-based localization by combining an image retrieval system with monte carlo localization. IEEE Transactions on Robotics 21(2), 208–216. Yamauchi, B. (1996). Mobile robot localization in dynamic environments using dead reckoning and evidence grids. In: Proc. of the 1996 IEEE International Conference on Robotics and Automation. Minneapolis. p. 1401–1406. Yuen, D. C.K. a MacDonald, B. A. (2003). Line-based SMC SLAM method in environments with polygonal obstacles. In: Proceedings of the Australasian Conference on Robotics and Automation. CSIRO, Brisbane, Australia. pp. 434–439. Zhang, R., Tsai, P. S., Cryer, J. a Shah, M. (1999). Shape from shading: A survey. IEEE Transactions on Pattern Analysis and Machine Intelligence 21(8), 690–706.

152

LITERATURA

Rejstřík 3D kamera, 24 Akcelerometry, 18 Algoritmus FastSLAM, 52 ICP, 64 IEPF, 61 Bootstrap filtry, 32 Dual MCL, 34 Eulerovy úhly, 90 GPS systém, 23 Gyroskopy, 18 K-D strom, 77 Kamerové systémy, 21 Laserové dálkoměry, 21

Mřížky obsazenosti, 42 Mapy geometrické, 43 senzorické, 40 silnic, 46 symbolické, 41, 45 symetrickoperturbační, 46 topologické, 44 Markovská lokalizace, 28 Matice rotační 2D, 73 rotační 3D, 91 Metoda SIFT, 39 LTL, 108 PTM, 76 PTP, 53 Mixed-pixel, 55

Mixture MCL, 34 Model pohybu, 29, 31 senzorický, 29, 30 Model pohybu, 31 Monte-Carlo, 32 Particle filtry, 32 Quaterniony, 92 Scan-matching, 34 Senzory inerciálnıi, 18 odometrické, 17 proximitní, 20 Sonary, 19 Trinocular stereo, 39