Lokalizace pro autonomni systemy - České vysoké učení technické v

Transkript

Č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 . . . . . . . . . . . . . . . . . .
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
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í
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
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-
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
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
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
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
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
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
modies 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,
reecting
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
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.
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
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:
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
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-
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
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)
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ÿ (Rǒfer, 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
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
• 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
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
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
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
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
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
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
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
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
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)
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
(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
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
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
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
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
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
(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
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
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
č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
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
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 ω)
(7.22)
Ty = ȳ 0 − (x̄ sin ω + ȳ cos ω)
(7.23)
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
(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
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
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
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
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
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-
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
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
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 




ṙq̇ = 
q̇ ṙ =  x
 q̇ = Rq̇,
 q̇ = R̄q̇.


 ry −rz
r0
rx 
rz ry −rx r0
(9.6)
Matice R a R̄ se liší jen transpozicí pravé dolní 3x3 submatice, důležité však je, že
součet kvadrátů ve všech sloupcích je pro R i R̄ shodný a je zároveň roven skalárnímu
součinu dvojice shodných quaternionů ṙ · ṙ = r02 + 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 .
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
ṙ0 , což je důsledkem ekvivalencí ṗṙq̇ ∗ = (Qṙ)q̇ ∗ = Q̄T (Qṙ) = (Q̄T Q)ṙ, kde matice Q a Q̄
odpovídají quaternionu q̇ podle vztahu 9.6:




Q̄T 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
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)
je pravá dolní 3x3 podmatice matice Q̄T Q, která je navíc ortonormální díky ortonormálnosti výchozí matice Q̄T Q a jednotkovému 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
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
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
směrů všech dvojic vektorů ṙi a ṙi0 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 

 q̇ = R̄0i q̇,
yi0 −zi0
0
x0i 
zi0 yi0 −x0i 0
(9.21)

dostaneme dalšími úpravami:
n
X
(R̄i q̇) · (Ri q̇) =
n
X
q̇
T
R̄0T
i Ri q̇
= q̇
n
X
!
R̄0T
i Ri
q̇ = q̇
T
i=1
i=1
i=1
T
n
X
!
Ni q̇.
(9.22)
i=1
Každá z matic Ni = R̄Ti Ri je symetrická, pokud položíme N = ni=1 Ni , matice N bude
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
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
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
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
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
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
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
∆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
∆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
∆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
∆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
∆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
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
114
(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
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
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
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
(a)
0
10
0
−2
10
−1
10
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
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í.
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
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.
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
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
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
pořadı́ páru [-]
x 104
vzdálenost [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
pořadı́ páru [-]
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
Obr. 10.22: 3D scany ve svých výchozích pozicích
Obr. 10.23: 3D scany po vzájemné registraci
129
130
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.
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
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
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 Muñoz, 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.
Rǒfer, 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

Lokalizace pro autonomni systemy - České vysoké učení technické v

Transkript

Podobné dokumenty