SROVNÁNÍ LOKALIZAČNÍCH METOD PRO MOBILNÍ ROBOTIKU COMPARISON OF LOCALIZATION METHODS FOR MOBILE ROBOTICS

Transkript

1 SROVNÁNÍ LOKALIZAČNÍCH METOD PRO MOBILNÍ ROBOTIKU COMPARISON OF LOCALIZATION METHODS FOR MOBILE ROBOTICS BAKALÁŘSKÁ PRÁCE BACHELOR S THESIS AUTOR PRÁCE AUTHOR VEDOUCÍ PRÁCE SUPERVISOR LUBOMÍR VESPALEC ING. TOMÁŠ MARADA, PH.D. BRNO 2007

2 Strana 2 1. Úvod

3 2.1. Architektura autonomních robotů Strana 3 Zadání bakalářské práce

4 Strana 4 1. Úvod

5 2.1. Architektura autonomních robotů Strana 5 Licenční smlouva

6 Strana 6 1. Úvod

7 2.1. Architektura autonomních robotů Strana 7 Abstrakt Tato bakalářská práce v úvodu obecně shrnuje třídy úloh, které autonomní robotické systémy řeší a seznámí Vás s architekturou robotu. Dále pak vysvětlí navigační a senzorické systémy, které rozdělí podle vztahu k robotu, podle použití a uvede dva nejpoužívanější senzory. Práce také vysvětlí zpracování dat ze senzorů a jejich chyby v měření. Hlavním tématem práce je seznámení se s existujícími lokalizačními metodami, vysvětlení jejich algoritmů a popsání výhod a nevýhod vůči ostatním metodám. Systematické srovnání těchto metod se nachází v závěru práce. Abstract In the beginning of this bachelor's thesis, groups of tasks in general are summarized which address autonomous robotic systems and introduce the architecture of a robot. Further, navigational and sensor systems are explained and two most used sensors are mentioned. The systems are divided according to their relationships to the robot as well as according to usage. This work also explains the data processing from the sensors and their measurement errors. The main subject of this work is the familiarization with the existing localization methods, explanation of their algorithms and description of their advantages and disadvantages compared to other methods. Systematic comparison of these methods can be found in the final part of this work. Klíčová slova Lokalizační metody, Kalmanův filtr, Monte Carlo lokalizace, Markovská lokalizace, SLAM, Scan matching, Histogramové metody. Keywords Localization method s, Kalman filter, Monte Carlo localization, Markov localization, SLAM, Scan matching, Histogram methods.

8 Strana 8 1. Úvod

9 2.1. Architektura autonomních robotů Strana 9 Poděkování Touto cestou bych chtěl poděkovat vedoucímu bakalářské práce Ing. Tomášovi Maradovi, Ph.D. za věcné připomínky při vypracování této bakalářské práce. Dále pak za důležité informace v oblasti lokalizačních metod Ing. Romanu Mázlovi, Ph.D. z katedry kybernetiky na ČVUT v Praze. Poděkování také směřuje k mé rodině, kteří mě v mém studiu řádně podporují.

10 Strana Úvod

11 2.1. Architektura autonomních robotů Strana 11 Obsah Zadání bakalářské práce...3 Licenční smlouva...5 Abstrakt...7 Poděkování Úvod Autonomní robotické systémy Architektura autonomních robotů Definice problému Lokalizace Mapování Navigace Navigační systém Globální navigační systém Relativní navigace Absolutní navigace Využití globálních informací Senzorické systémy Odometrie Rozdělení senzorů Optické snímače polohy Optické absolutní snímače polohy Optické inkrementální snímače polohy Laserové dálkoměry, scannery Zpracování dat Filtrace dat Konstantní hodnota vstupních dat Průměr Pomalu se měnící hodnota vstupních dat Plovoucí průměr Odhad plovoucího průměru Rychle se měnící hodnota vstupních hodnot Reprezentace chyby Lokalizační metody Lokalizace polohy Landmarkové lokalizace Scan matching lokalizace Histogramové přístupy Monte Carlo lokalizace Algoritmus MCL Bayesovské metody Kalmanův filtr Základní princip Kalmanova filtru Více-dimenzionální (maticová) podoba Kalmanova filtru Particle filtr Markovská lokalizace Skrytý Markovský model MCML Simultání lokalizace a mapování...53

12 Strana Úvod SLAM a Kalmanův filtr Algoritmus FastSLAM Výsledné srovnání metod Závěr...59 Seznam použité literatury...61

13 2.1. Architektura autonomních robotů Strana 13 1 Úvod Problém lokalizace robotu je klíčový problém ve výrobě autonomních robotů. Jestliže robot neví kde se nachází v určitý časový okamžik, může to být pro něj tak obtížné, že nedovede určit co dělat dál. Nejjednodušší metodou lokalizace je lokalizace na základě informací o změnách vnitřního stavu robotu (dead reckoning). Nejrozšířenější je určování polohy robotu z odometrie, tedy integrace polohy na základě otáčení kol robotu. Všechny metody založené na dead reckoning jsou zatíženy kumulativní chybou. Malé chyby v senzorických datech se časem sčítají až dosáhnou takové hodnoty, že je výsledná poloha nepoužitelná. Přesto se tato lokalizace používá často, avšak většinou jako součást složitější lokalizace a informace jsou skládány s dalšími informacemi od jiných senzorů. Další možnou metodou je lokalizace využívající aktivních prvků (landmarků) dodaných do prostředí, ve kterém se robot pohybuje. Taková lokalizace je i lokalizace prostřednictvím GPS, která využívá družic rozmístěných na oběžné dráze. Ve vnitřních prostorách se využívá landmarků vysílajících ultrazvukové nebo světelné signály. Poloha robotu je určena vzhledem k jednotlivým landmarkům a není třeba mít mapu prostředí. Lokalizace může probíhat vůči existující mapě prostředí. Protože mapu obvykle nemáme k dispozici, řeší se problém současné lokalizace a mapování (SLAM). V tomto případě se inkrementálně rozšiřuje známá mapa a robot se lokalizuje vůči této částečné mapě. Při SLAMu se často využívá toho, že změna polohy robotu je malá mezi jednotlivými kroky rozšiřování mapy. Pokud máme známou mapu prostředí a nevíme počáteční polohu robotu, pak můžeme využít metod lokalizace sledujícími více hypotéz o poloze robotu. Struktura práce má toto členění: následující kapitola obecně shrnuje třídy úloh, které autonomní robotické systémy řeší a také se zabývá architekturou robotu. Kapitola 3 definuje problém lokalizace a mapování. Kapitola 4 rozdělí a vysvětlí navigační systémy. Popisy a rozdělení senzorických systémů, které se používají v oblasti mobilní robotiky, jsou uvedeny v kapitole 5. Kapitola 6 přiblíží zpracování dat ze senzorů a vysvětlí výskyt chyb při měření. Hlavní téma této práce, a to lokalizační metody, jsou popsány v kapitole 7. Seznámíme se z nejznámějšími, ale i méně známými metodami, které nám zjišťují aktuální pozici robotu, u každé metody také uvede výhody a nevýhody při lokalizaci a vůči ostatním metodám. Systematické srovnání pak provedeme v závěru práce.

14 Strana Úvod

15 2.1. Architektura autonomních robotů Strana 15 2 Autonomní robotické systémy Každý mobilní robot řeší základní otázky, Kde jsem?, Kam se chci dostat? a Jak se tam dostanu?. Aby byl robot schopen si odpovědět na tyto otázky musí být schopen vnímat svoje prostředí, lokalizovat se v něm a naplánovat cestu. Otázka o aktuální poloze robotu v mapě prostředí řeší robotické lokalizační subsystémy. Odpověď na druhou otázku, kam se chci dostat nebo 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 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í [14]. 2.1 Architektura autonomních robotů Na architekturu autonomních robotu můžeme nahlížet jako na velmi komplexní zpětnovazební 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.1 z [14]. Autor [14], který přebírá teorii z [15], rozděluje jednotlivé řídicí zpětnovazební 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ílu uživatelem. Kognitivními schopnostmi robotu nazýváme aktivity popisující sbírání, ukládání, transformaci a používání znalostí. Jsou to schopnosti, kterými se roboty 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 Obr. 1: Struktura mobilního robotu.

16 Strana Autonomní robotické systémy 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. 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 robotu. 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ánu 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 pohonu. 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ě robotu a navrhnout řešení krizové situace s ohledem na dlouhodobé plány činnosti robotu. 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 muže dát přímý pokyn motorickému systému, např. k okamžitému zastavení [14].

17 3.1. Lokalizace Strana 17 3 Definice problému Autonomní mobilní robot musí být vybaven systémy zajištujícími bezpečnou a bezkolizní implementaci v prostředí, 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ů. Obě úlohy jsou do velké míry vzájemně svázány a 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. 2 z [14]. Obr. 2: Vzájemné vazby mezi lokalizací a modelem světa. 3.1 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 snímače IRC a 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ěru začínají prosazovat i řešení využívající kamerových systému. 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.

18 Strana Definice problému 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 (positron 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ílu 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 muž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 robota v terénu, či v prostředí, kde pohyb robotu není omezen jednou horizontální rovinou, je nutné odhadovat až šest nezávislých parametru, které definují polohu pevného tělesa v prostoru, tj. tri kartézské souřadnice (x,y,z) a tri směrové úhly často označované jako roll/pitch/yaw [14]. 3.2 Mapování Mapování 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í muž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 [14].

19 3.2. Mapování Strana 19 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 intervalu 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 tak, aby existence mapy umožnila nebo významně zvýšila efektivitu řešení vybrané úlohy. Model prostředí muž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 [14,16] autor uvádí, že manuální zmapování prostoru Deutsches Muzea v Bonnu pro činnost robotického průvodce RHINO [6] 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. 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 robota 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 algoritmu. Jakmile se robot pohne, odhad jeho aktuální pozice je zatížen šumem vznikajícím nepřesností pohybu. Vnímaná absolutní poloha objektu 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í muž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 [14].

20 Strana 20 0.

21 4.1. Navigační systém Strana 21 4 Navigace 4.1 Navigační systém Pokud po mobilním robotu požadujeme byť i nejjednodušší schopnost autonomního chování např. pohyb zadaným směrem, musí být robot vybaven navigačním subsystémem. Mezi typické úkoly navigace patří poskytnutí informací potřebných k přemístění se z výchozího (aktuálního) do cílového bodu. Úkol je omezen řadou podmínek, z nichž základní je vyhnutí se kolizím s překážkami. Vlastní navigační systém se obvykle dělí na dvě úrovně [2]: Globální má za úkol dopravit robotu z výchozího do cílového bodu, a to např. s ohledem k dosažení optimální trajektorie. Pojem globální souřadný systém není nezbytně nutné chápat jako celosvětový, ale jako souřadný systém zvolený tak, aby obsahoval celý pracovní prostor robotu, např. místnost. Pro zjednodušení je často srovnáván s geografickým severem. Lokální navigace je nadřazena globální a jejím úkolem je zabránit kolizím s okolními objekty a tyto kolize řešit. Lokální navigační systém pracuje obvykle v souřadném systému robotu a zpracovává informace o překážkách v omezené vzdálenosti od robotu. Lze říci, že tato vzdálenost musí být taková, aby poskytla řídícímu systému dostatek času pro spolehlivou identifikaci překážek a určení nekolizní trajektorie s ohledem na fyzické možnosti robotu. Z výše uvedeného je patrné, že podstatný vliv na volbu senzorů nemají pouze jejich vlastnosti, ale také parametry robotu a charakter jeho pracovního okolí Globální navigační systém Problém globální navigace spočívá v určení polohy a orientace robotu vůči globálnímu souřadnému systému. Po určení této polohy přebírá úkol řídící systém, který ze získaných hodnot určí optimální trajektorii pohybu. K určení polohy robotu jsou využívány různé metody, které lze podle typu globálního souřadného systému rozdělit na dva typy [2]: Relativní navigace využívá k určení polohy parametry měřitelné na robotu a bez přímého vztahu k okolí. Počátek globálního souřadného systému je obvykle shodný s výchozím bodem (pozice robotu). Řídicí systém pomocí těchto parametrů určuje změnu polohy vůči výchozímu bodu. Problém těchto způsobů navigace spočívá v tom, že většinou bývají zatíženy rostoucí chybou polohy, proto jsou, pokud je to možné, obvykle kombinovány s absolutním systémem navigace. Absolutní navigace určuje polohu robotu vůči globálnímu souřadnému systému. K tomu jsou využívány referenční body se známou polohou v globálním souřadném systému. Tyto metody pak určují polohu vůči těmto bodům Relativní navigace Tento způsob navigace pracuje na principu měření přírůstků změny polohy a orientace robotu. Změna je vztažena vůči startovnímu bodu, nebo bodu, v němž byla naposledy určena absolutní poloha robotu. Nevýhodou této metody je neustálý růst chyby

22 Strana Navigace polohy způsobený přítomností chyby v jednotlivých přírůstcích. Proto je tato metoda samostatně použitelná pouze pro relativně krátké trajektorie v závislosti na přesnosti měření. Ve většině případů je v praxi kombinována se systémy absolutní navigace, sloužícími k omezení této chyby. Výhodou této kombinace je to, že umožňuje relativně velkou nezávislost robotu na absolutním systému navigace a eliminaci náhodných chyb absolutní navigace. Tak lze výrazně zvýšit spolehlivost a přesnost navigace [2] Absolutní navigace Základním úkolem absolutní navigace je jednoznačné určení polohy robotu vůči referenčním bodům. K řešení tomuto problému se nejčastěji využívají dvě metody. Metoda trilaterace, definující polohu robotu pomocí vzdálenosti od referenčních bodů. Druhou metodou je triangulace, umožňující polohu robotu pomocí měření tří úhlů λ 1,2,3 mezi referenčními body S 1,2,3 a robotem. Ze znalostí polohy těchto bodů v globálním souřadném systému je mobilní robot schopen pomocí naměřených úhlů vypočíst svou polohu. Obr. 3: Princip triangulace. Tato měření mohou být realizována různými prostředky. Nejčastěji jde o laserové paprsky, elektromagnetické záření, nebo akustické vlny. Triangulační metody mohou pracovat buď s jedním vysílačem umístěným na robotu a několika přijímači umístěnými v pracovním prostředí robotu, nebo naopak a to že přijímač je umístěn na robotu a aktivní prvky (vysílače) slouží jako majáky. Výhodou druhého způsobu je, že je bezproblémově použitelná pro větší počet robotů, kteří se v daném prostoru pohybují. Druhým úkolem je stanovení orientace robotu. K jejímu měření mohou být využívány systémy sloužící k určení polohy, kombinace těchto systémů s relativní navigací nebo použití samotného systému pro měření orientace. První přístup je použitelný u metody triangulace díky principu jejího měření. Druhý přístup je charakteristický pro trilateraci, ta je totiž schopna určit pouze polohu robotu. Princip měření orientace touto metodou spočívá v tom, že se robot přemístí pomocí metody relativní navigace po dráze, jejíž délka zohledňuje chybu obou systémů navigace. Z rozdílu mezi změnou polohy, určenou těmito systémy navigace, lze stanovit orientaci robotu.

23 4.2. Využití globálních informací Strana 23 Zásadní nevýhodou této metody je sčítání chyb obou metod a nutnost přemístění robotu, po němž je schopen robot určit orientaci. Proto jsou často využívány speciální systémy k měření orientace. Nejrozšířenější je použít kompas, který je schopen měřit orientaci robotu vůči magnetickému poli. Výhodou tohoto systému je dostupnost prakticky na celé planetě, velká spolehlivost a minimální pořizovací náklady. Nevýhodou je samozřejmě citlivost na okolní magnetické pole, které mohou znehodnotit měření. Pro stanovení orientace lze ve vnějším prostředí využít také slunce. Robot potřebuje informace o aktuálním čase a datu, na základě těchto informací je schopen pomocí matematického modelu určit svou orientaci [2]. 4.2 Využití globálních informací Představte si, že se pohybujete po ulici. Ulice je plná překážek, jako jsou domy, stromy a lidé. Chcete jít do supermarketu, ale máte zavřené oči. Nejspíše budete dost obtížně hledat svojí cestu, i když ji dobře znáte. Naneštěstí, většina z nás má oči, které nám pomohou v hledání této cesty. V každém pohybu, který uděláme, nám oči říkají, kde se nacházíme. Neustále opravují nedokonalosti našich pohybů. Neuvědomíte si nepřesnost pohybu, který uděláte, do té doby, než si zkusíte jít rovně právě po slepu. Třebaže si myslíte že jdete rovně krok za krokem, pravděpodobně rychle budete cítit, jak se posouváte pryč z dané rovné cesty. Pravděpodobně vás dokonce opustí i rovnováha. S otevřenýma očima vám ale tato cesta problémy dělat nebude. Z informací, kterou vám dají oči a další smysly, neustále získáváte představu kde se nacházíte. Tyto představy používáte k tomu, aby jste se rozhodli co budete dělat dál, například jestli s této rovné cesty odbočíte někam jinam. Tyto informace, ale vstřebáváte podvědomě a určitě vás nebude zajímat vzdálenost, kterou jste urazili na milimetr přesně a ani všechny směry, kterými jste se pohybovali. Co vás ale zajímá je, kde se nacházíte relativně vůči vašemu okolí. Například když jdete do zmiňovaného supermarketu, zajímá vás, že jste v obchodě a nikoli že jste urazili 127,285 m rovně vpřed, pak se otočili o 28 atd. To, že se nacházíte na požadovaném místě, zjistíte porovnáním aktuálních vjemů s informacemi, které si pamatujete. Jinými slovy porovnáním aktuálních údajů ze senzorů mobilního robota ( oči robotu ) se zapamatovanou mapou prostředí. Příklad si můžeme ukázat na obr.4 jednodimenzionálního prostoru, kde se snažíme zjistit například vzdálenost ke stěně sonarem, či laserem. Zahrnutím měření tohoto druhu do našeho odhadu pozice můžeme dosáhnout korekce akumulované chyby [1]. Obr. 4: Měření vzdálenosti.

24 Strana Navigace

25 5.1. Odometrie Strana 25 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 [14]. Jde o nejběžnější metodu patřící do relativní navigace. Spočívá v tom, že řídící systém obsahuje kinematický model robotu a pomocí tohoto modelu je schopen určit změnu polohy robotu v závislosti na změně polohy akčních členů (typicky kola, které snímá enkodér). Tato metoda je často využívána pro kolové roboty u nichž lze zajistit stálý kontakt s podlahou bez prokluzu. V případě že tomu tak není je použití této metody zatíženo chybou, což prakticky znemožňuje její použití, nebo omezuje použití na kratší vzdálenosti. V případě kolových robotů představuje jeden z velmi výhodných systémů navigace, a to zejména díky jeho jednoduchosti a nízké pořizovací ceně. Nejčastěji se k tomuto účelu používají inkrementální snímače [2]. Přesnost této metody závisí při zajištění kontaktu s podlahou zejména na přesnosti kinematického modelu. Jeho přesnost je limitována zejména přesností stanovení rozměrů jednotlivých kol, a to v závislosti na použitém typu podvozku. Většinu mobilních robotů můžeme rozdělit do těchto tří kategorií [1]: Kolové, zahrnují roboty, kteří se nedokáží otáčet na místě, protože změny orientace mohou dosáhnout pouze příslušným natočení kol či nápravy následovaným pohybem vpřed a vzad (obr.5). Tomuto pohybu se často říká Ackermanovo řízení a zahrnuje většinu dnešních automobilů. Obr. 5: Model tříkolky. Pásové, zahrnují všechny roboty, co jsou schopni se otáčet na místě kolem své osy, ale pohybují se pouze vpřed či vzad (obr.6). Často je nazýváme diferenciálně řízenými, protože změna orientace závisí na rozdílu rychlosti levého a pravého kola či pásu. Všesměroví, zde patří roboti, jak již název napovídá, kteří se dokáží pohybovat všemi směry bez ohledu na aktuální orientaci, včetně otáčení na místě (obr.7). S touto kategorií robotů se nesetkáme příliš často, i když poslední dobou začíná být populární v robotickém fotbalu. Často je nalezneme pod označením omnidrive.

26 Strana Senzorické systémy Obr. 6: Model diferenciálního robotu. Obr. 7: Model všesměrového robotu. 5.2 Rozdělení senzorů Snímače lze rozdělit podle vztahu k robotu na: Interní měřící parametry robotu Externí měřící parametry okolí robotu Použité senzory mohou plnit mnoho funkcí, ovšem s pohledu vlastního robotu jsou významné pouze snímače sloužící k navigaci a diagnostice robotu. Těmito snímači by měl být vybaven každý mobilní robot. Mezi ty nejjednodušší lze zařadit snímače potřebné pro detekci překážek reprezentovanými buď dotykovými (taktilní senzory), případně bezdotykovými snímači (typicky IR senzory a sonary). Jejich úkolem je zabránění kolizí s objekty v okolí robotu, případně udržování požadované vzdálenosti od těchto objektů. Jde o úkol lokální navigace, která je při pohybu robotu nadřazena globální. Senzory poskytují v tomto případě řídicímu systému informace o tvaru a rozmístění objektu v okolí robotu [2].

27 5.2. Rozdělení senzorů Strana 27 Snímače mechanických veličin se tedy rozdělují dle několika kritérií [3]: Podle druhu měřené fyzikální veličiny o polohy o rychlosti a zrychlení o kmitavého pohybu o mechanického napětí o síly atd. Podle průběhu výstupního signálu o spojité o limitní o číslicové Podle principu činnosti o mechanické o odporové o magnetické o indukční o kapacitní o optické o ultrazvukové atd. Podle způsobu odměřování o absolutní o přírůstkové (inkrementální) o smíšené Ostatní typy snímačů jsou specifikovány podle požadavků konkrétního využití robotu (např. měření teploty, analýza plynů, atd.) Optické snímače polohy Princip optických snímačů polohy spočívá v modifikaci světelného toku mezi vysílačem a přijímačem polohou snímaného předmětu a následném převodu na elektrickou veličinu. Jak už jsme uvedli v rozdělení v minulé kapitole, snímače se podle způsobu odměřování dělí na [2]: Absolutní poloha je měřena vzhledem k referenčnímu bodu Inkrementální poloha je měřena vůči předchozímu bodu Limitní poloha je vyhodnocována dvouhodnotově Optické absolutní snímače polohy Tento typ senzoru využívá kompletní typ kódování a vyžaduje větší počet snímacích prvků. Předností tohoto senzoru je to, že výstupní hodnota ze senzoru udává absolutní velikost natočení v rozsahu 0 až 360. Pro větší počet otáček je vybaven čítačem inkrementujícím počet otáček kódového kotouče. Obsah tohoto čítače pak spolu s kódem aktuální pozice kódového kotouče součástí tvoří absolutní údaj o poloze natočení. Princip kódování spočívá v tom, že je svazek optických paprsků kódován optickým kotoučem

28 Strana Senzorické systémy a senzory zaznamenávající tyto paprsky jsou rozmístěny tak, že jejich výstupem je přímo digitální informace o poloze v binární hodnotě [2]. Obr. 8: Provedení absolutního senzoru natočení, převzato z [2]. Vlastní kódovací kotouč existuje v několika provedeních (viz. obr.9). Jejich funkce je totožná, levá varianta (obr.9a) ovšem místo klasického binárního kódování (obr.9b), využívá Grayův (zrcadlový) kód. Jeho výhodou je větší odolnost vůči chybám, protože kód sousedního čísla se vždy liší v maximálně jednom bitu. Obr. 9: Kódové kotouče absolutního senzoru natočení Optické inkrementální snímače polohy Inkrementální senzory jsou typicky používané ve zpětnovazebních systémech řízení polohy, rychlosti a případně zrychlení v rozsahu aplikací od periferií počítačů, přes průmyslovou robotiku až po zdravotnickou techniku. Inkrementální snímače jsou charakteristické svou vysokou rozlišovací schopností, malými rozměry a nízkou hmotností. Název inkrementální, je vzat z principu činnosti, založeném na otáčivém mezikruží (R) s pravidelně se střídajícími průhlednými a neprůhlednými ryskami, které při otáčení přerušují emitované světlo (P) LED diody (S) umístěné na jedné straně tohoto mezikruží viz. obr.10. Toto světlo je detekováno fototranzistorem (E1,E2), umístěným na druhé straně mezikruží naproti LED diodě. Do optické cesty mezi zdrojem a přijímačem světla je u většiny snímačů zařazen ještě nepohyblivý maskovací kotouč s ryskami o stejné rozteči, jako má kotouč pohyblivý.

29 5.3. Laserové dálkoměry, scannery Strana 29 Obr. 10: Princip optického inkrementálního snímače polohy. Světlo ze zdroje prochází přes průhledné rysky pohyblivého kotouče. Jsou-li v zákrytu průhledné rysky pohyblivého kotouče a průhledné rysky pevného maskovacího kotouče, dopadá na fotodiodu maximální světelný tok. V případě, že jsou v zákrytu průhledné rysky pohyblivého kotouče a neprůhledné rysky segmentu nepohyblivého kotouče, světlo neprochází a světelný tok na fotodiodě je minimální. Mezi těmito polohami se světelný tok mění přímo úměrně posunutí obou kotoučů. Výstupní signál fotodiody má periodu nepřímo úměrnou počtu rysek na otáčku a rychlosti otáčení pohyblivého kotouče. Počet impulsů nese informaci o poloze a sekvence signálů UA, UB z obr.11 nese informaci o směru pohybu [3]. Obr. 11: Výstupní kvadratický signál s fotodiody. 5.3 Laserové dálkoměry, scannery Laserové dálkoměry (viz. obr. 13) 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.

30 Strana Senzorické systémy 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. 12). Levnější konstrukce pracují na triangulačních principech podobně jako proximitní senzory. Laserový paprsek muže být bud statický, nebo rozmítaný rotujícími zrcadly. 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í tvaru předmětu, 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 [14]. Obr. 12: Princip měření laserovým dálkoměrem, převzato z [14]. Obr. 13: Rovinný laserový scanner.

31 6.1. Filtrace dat Strana 31 6 Zpracování dat 6.1 Filtrace dat Základním principem fungování senzorů je jejich schopnost převádět měření různých fyzikálních vlastností či skutečností na elektrické napětí. Toto napětí je na vstupu do výpočetní jednotky převedeno na číselnou hodnotu. Posloupnost takových měření je tedy zaznamenána jako posloupnost číselných hodnot. Každé měření jako takové je zatížené určitou chybou, protože je to vlastně pouze náš (nedokonalý) pokus o zjištění oné pravdivé hodnoty. Velikost a charakter této chyby jsou ovlivněny mnoha faktory, které nemáme, a typicky ani nemůžeme, mít pod kontrolou. Zdrojem chyb může být cokoli od vlastního senzoru až po převod napětí na číselnou hodnotu v počítači. Pro robotu je tedy velmi důležité umět všechna měření správně interpretovat. Každé další měření nám dodává novou informaci o hodnotě měřené veličiny, a proto jejich vhodnou kombinací můžeme získat jak kvalitnější odhad příslušné veličiny, tak i informaci o kvalitě tohoto odhadu [1] Konstantní hodnota vstupních dat Nejjednodušší situace nastává, když víme, že veličina, která nás zajímá, má konstantní hodnotu (tj. s časem se nemění) [1] Průměr Kombinace měření jedné veličiny pomocí průměrování asi nebude pro nikoho překvapením [1]: sum = 0.0; nebo také for ( i= 0; < n; i+ + ) sum+ = y[] i; avg = sum 1 x = n n n x i i= 0 Popsaný způsob výpočtu můžeme použít, pokud máme v jednom okamžiku k dispozici všechna měření y[i]. Často se nám ale stává, že potřebuje průměr postupně zpřesňovat tak, jak nám přibývají měření. Počítat v tomto případě celý průměr znovu by bylo neefektivní a vyžadovalo by to, abychom si pamatovali všechna doposud obdržená data. Průměr můžeme počítat i postupně: Tento zápis je ale ekvivalentní ( avg ( n ) y[ n] ) n avg = 1 + (3) ( avg + n ( y[ n] ) avg) avg = 1 (4) (1) (2)

32 Strana Zpracování dat který nám názorněji popisuje výpočet průměru v kroku i, jako úpravu průměru z kroku předcházejícího určitým korekčním členem skládajícím se ze dvou částí: 1. váhy (v tomto případě 1/n) 2. rozdílu nového měření a původního průměru Odchylka nového měření od původního průměru je tedy základem, pro získání průměru nového. Čím více měření již máme v průměru integrováno, tím menší má tato odchylka vliv (tím více našemu průměru věříme ve srovnání s novým měřením). Rekurzivním výpočtem průměru jsme se sice zbavili nutnosti pamatovat si všechna doposud získaná data a můžeme postupně integrovat nová měření, ale stále odhadujeme pouze konstantní veličinu. Znázorníme-li graficky tento postup, vidíme, že se vlastně snažíme naměřenými daty proložit vodorovnou přímku [1]. Obr. 14: Průměr Pomalu se měnící hodnota vstupních dat Pokud se měřená veličina mění, musíme zvolit jiné řešení (proložení vodorovné přímky všemi daty by nám v tomto případě moc nepomohlo). Co můžeme udělat, je proložit onu přímku pouze několika posledními hodnotami [1] Plovoucí průměr Plovoucí průměr, je průměr z několika posledních hodnot [1]: sum = 0.0; for ( i = n k; i < n; i + + ) sum+ = y[] i ; avg = sum k; (5) Plovoucí průměr můžeme počítat rekurzivně a složitost výpočtu tedy nemusí záviset na délce k průměrovacího okna: ( avg k y[ n k] y[ n] ) k avg = + (6)

33 6.1. Filtrace dat Strana 33 V tomto vztahu reprezentuje člen avg k součet posledních k měření. V dalším kroku od tohoto součtu tedy odečteme nejstarší měření x[n-k] a naopak přičteme nové měření x[n], čímž získáme součet posledních k měření. Vydělením hodnotou k tedy získáme průměr z posledních k měření. Rekurzivní výpočet plovoucího průměru si můžeme přepsat jako součet posledního průměru a určité korekce: ( y[ n] y[ n k] ) avg = avg + 1 k (7) Váha určující vliv korekční složky je nyní konstantní a odpovídá převrácené hodnotě délky průměrujícího okna. Velikost korekce je dána rozdílem nejnovějšího a nejstaršího měření. Plovoucí průměr tedy prokládá vodorovnou přímku posledními k měřeními (viz. obr.15). Rekurzivní definicí výpočtu jsme opět získali konstantní složitost (tj. složitost nezávislou na délce průměrovacího okna). Stále si ale musíme pamatovat všech posledních k měření, která jsou zahrnuta do plovoucího průměru [1]. Obr. 15: Plovoucí průměr pro k= Odhad plovoucího průměru Pokud si nechceme nebo nemůžeme pamatovat všechna měření z průměrovacího okna, musíme se pokusit tato data něčím nahradit, nějak je odhadnout. Nejlepší odhad nejstaršího měření, které bychom chtěli z výpočtů odstranit, je poslední hodnota průměru. Místo vlastního měření, tedy odečteme poslední průměr [1]: Při zápisu pomocí korekce pak: ( avg k avg y[ n] ) k avg = + (8) ( y[ n] avg) avg = avg + 1 k (9) Touto úpravou získáme jednoduché, rychlé a praktické řešení, které bude ve většině případů dostačující. Popisované řešení se někdy nazývá průměrování s exponenciálním zapomínáním, protože výsledná hodnota obsahuje např. i naše první měření y[0]. Jeho váha je však ((k-1)/k)^n a tedy exponenciálně klesá.

34 Strana Zpracování dat Rychle se měnící hodnota vstupních hodnot Aplikace plovoucího průměru v tomto případě má ten nepříjemný důsledek, že průměrovaná hodnota jakoby plave za skutečnou s určitým zpožděním a navíc zaobluje špičky (viz. obr.16 a 17). Obr. 16: Plovoucí průměr pro k=16. Obr. 17: Sinus bez šumu, k=8. Toto chování je způsobeno jedním předpokladem, který je v plovoucím průměru schován tj. že hodnota je v rámci průměrovacího okna konstantní. Pokud ale máme signál, který se v rámci průměrovacího okna výrazně mění (a tyto změny nejsou způsobeny šumem), dojde ke zmiňované degradaci (ztrátě informace). Klíčem k řešení je použití složitějšího modelu chování a to například tak, že místo konstantní hodnoty budeme očekávat hodnotu lineárně se měnící místo jednoho parametru budeme tedy odhadovat parametry dva, a a b ( y = a x + b ) [1].

35 6.1. Filtrace dat Strana Reprezentace chyby Jednou z možností je ke každému měření přiřadit očekávanou chybu. Naměřenou vzdálenost potom uvádíme jako d ± r a očekáváme, že chyba má gaussovskou distribuci se střední hodnotou 0 a rozptylem r (obr.18). Na základě takto reprezentovaných měřeních se budeme snažit zkonstruovat odhad nám neznámého stavu robotu (nejčastěji x, y, úhel) a chybu tohoto odhadu [1]. Obr. 18: Pravděpodobnostní model měření. Řekněme si pro jednoduchost, že stav robotu je reprezentován pouze jedním číslem (vzdáleností na ose x). Informace z enkodérů či akceleromerů odpovídají informaci o posunu na této ose vlevo či vpravo. Jaký vliv na náš odhad pozice bude mít, zahrnutí dvou informací o posunu vpravo vidíte na (obr.19). Jednoduchý náhled na znázorněnou situaci nám říká, že když nevíme přesně, kde se nacházíme, a máme informaci pouze o posunu v před (také nepřesnou), tak se nám nepřesnosti kombinují a náš nový odhad je ještě nepřesnější. Obr. 19: Akumulace chyby. V úvodu do lokalizace v [1] je uveden příklad ze života. Jak přesně dokážete odhadnout jeden metr? Většina z nás to asi zvládne relativně dobře. Jak dobře potom zvládnete odhadnout 100 metrů? Tj metr? Je velmi pravděpodobné, že bez nějaké vnější reference (tj. známého okolního prostředí stačí chodit s páskou na očích) dokonce každý z vašich pokusů dopadne úplně jinak v závislosti na tom, jak dobře se vám bude dařit odhadnout vzdálenost jednoho metru. Pokud bychom tento pokus (odhad 100 metrů) zopakovali vícekrát a výsledek si zobrazili v grafu, kde osa x odpovídá skutečné vzdálenosti a osa y počtu, kolikrát jsme tuto informaci naměřili (tj. vytvoříme histogram), bude jeho tvar připomínat obr.18, kde střední hodnota d by měla být blízko 100 metrů (vzdálenost od hodnoty 100 metrů nám říká, jak dobrý máte odhad 1 metru) a rozptyl r nám říká, jak moc se vám tento odhad v průběhu pokusu měnil.

36 Strana Zpracování dat

37 7.1. Lokalizace polohy Strana 37 7 Lokalizační metody 7.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í 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ůvodu 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 algoritmu [14]. 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 modelu světa [6]: behavioární metody landmarkové metody metody využívající přímé registrace senzorických dat 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. Dokonalejší systémy 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, 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 objektu 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. Do této kategorie lze zařadit i systémy satelitní navigace jako GPS. Poloha landmarku muž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 senzoru, které poskytují velké množství informací o tvaru a konfiguraci okolního prostředí. Zpravidla nevyužívají metody extrakce příznaku z datových souboru,

38 Strana Lokalizační metody 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 [13] a scan-matching techniky [6]. Z výzkumu vlastností těchto technik 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 [14]. V následujících kapitolách jsou podrobněji diskutovány používané lokalizační techniky v oblasti mobilní robotiky 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ílu, převede na úlohu sledování statických cílu, 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 muž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ů muž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 Scan matching lokalizace Jak píší autoři v [14,17], scan matching (překládání nebo porovnávání snímků) je metoda, která pro získání přesného odhadu pozice robotu používá, pomocí laserového scanneru, 2D scanny. 2D scan je uspořádaná množina bodů v rovině, jejichž souřadnice odpovídají místům v prostoru, kde byla, v různých vzdálenostech od robotu, senzorem detekována překážka. Poloha každého bodu je měřena v souřadnicích polární soustavy souřadnic, která je přirozenou souřadnou soustavou pro způsob pořízení dat [14]. Jednotlivé snímky se potom scanují v různých časech a z různých míst. Tím dostáváme přesné obrysy prostředí. Jednoduše řečeno pomocí metody scan matching získáváme informace v podobě celých snímků prostředí. Jednotlivé snímky pak dává na sebe a otáčí tak, aby mezi sebou maximálně překrývali údaje ze snímače a dřívěji ukázanou mapou. Hlavní nevýhodou této metody je, že musí být známa počáteční pozice robotu, která je obvykle odvozena z odometrických informací. Pozice robotu a jeho aktualizace ze scan matchingu je modelována jako jednotlivá Gaussova rozložení. Výhodou scan matchingu je tedy vysoce precizní vypočítaná pozice robotu. V dalším kroku této účinné metody pak může být použito Kalmanovo filtrování, které potom může být ještě přesnější.

39 7.1. Lokalizace polohy Strana 39 Scan matching má následující vlastnosti [6]: Robot poměrně dobře zlokalizuje vstupní informace a v lineárním případě z toho vyplyne optimální odhad umístění. Robot se nedokáže vzpamatovat z katastrofálních chyb, jako např. nedokáže se zlokalizovat pokud se ztratí Při hledání pozice robotu jsou scenny (snímky) snímače omezeny pouze malými odchylkami, proto je metoda výpočtově účinná. Na obr. 20 je ukázaný jeden s experimentů provedený v [17]. Na schématu vlevo (a) jsou znázorněny dva snímky z různých pozic s různým natočením (B, B ). Jejich rozdíl nám ukazuje chybu v pozici robotu (A) a lépe zlokalizuje prostředí. Na druhém schématu (b) je ukázaná výsledná korekce pozicové chyby po seřazení dvou snímků. Všimněte si, že mapa pozice je ve druhém schématu lépe zřetelná, i když na obrázku není vidět posun bodu A, v reálném experimentu činil rozdíl pozice po korekci cm. Obr. 20: Příklad experimentu lokalizace pomocí metody scan matching v [17] 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 scanu laserového dálkoměru změřených z různých míst v prostředí [12, 14]. 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 histogramu, 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(n 2 ) [14]. Lokalizace na bázi histogramu byla přivedena do reálné aplikace autonomního kolečkového křesla Rolland [18]. 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á [14].

40 Strana Lokalizační metody Monte Carlo lokalizace Metoda je založena na výpočtu rozložení pravděpodobnosti výskytu robotu, podmíněné danými senzorickými daty. Na začátku je pravděpodobnost výskytu robotu stejná ve všech místech prostoru. Čím déle se robot pohybuje prostředím, tím více se profiluje místo s nejvyšší pravděpodobností. Reprezentace této hustoty pravděpodobnosti popisující odhad pozice robotu je pro tuto metodu velice charakteristická. Oproti střední hodnotě a rozptylu používané Kalmanovým filtrem, využívá tato metoda množinu tzv. vzorků (nebo také složky particles). Jejich váhy a rozložení ve stavovém prostoru určuje, jak je která pozice pravděpodobná [1]. Monte Carlo lokalizace (MCL) na rozdíl od Kalmanova filtru velice účinným způsobem řeší globální problém lokalizace. Rozšířená forma Monta Carlo lokalizace, nazvaná Mixture MCL (smíšený MCL) a kterou se budeme zabývat níže, dokonce umí řešit i zmiňovaný problém katastrofální ztráty pozice. Výhodou reprezentace pomocí vzorků je zejména to, že neklademe žádná omezení na tvar hustoty pravděpodobnosti a jsme tedy bez problémů schopni reprezentovat i multimodální hustoty pravděpodobnosti, tedy takové, které mají více než jeden vrchol (nejpravděpodobnější pozice) [1]. 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 a bezchybným modelem světa. Data musí obsahovat určité množství šumu. 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 [5] a následně i kombinace původní MCL a Dual MCL nazývaná Mixture MCL [14], která základním způsobem převrací vzorkovací proces. Zatímco standardní MCL nejprve odhaduje novou pozici pomocí odometrických informací a potom používá snímač k měření, aby zhodnotil důležitost tohoto vzorku, Dual MCL odhaduje pozici použitím posledních měření ze snímače, a pak užívá odometrických informací, aby zhodnotil shodu těchto odhadů s předešlými důvěryhodnostními a odometrickýmy daty. 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 [14]. Obr. 21: Odhad distribuční funkce.

41 7.1. Lokalizace polohy Strana Algoritmus MCL Vlastní algoritmus MCL má dvě základní fáze: predikce - posun všech vzorků na základě informací o změně pozice robota korekce - úprava vah jednotlivých vzorků na základě shody či neshody naměřených dat s očekáváními, která by odpovídala pozici reprezentované příslušným vzorkem Výsledkem predikce je nová sada vzorků reprezentujících upravenou hustotu pravděpodobnosti na základě informací o změně pozice. Využití těchto informací by mohlo spočívat v prostém posunu každého vzorku o příslušnou hodnotu. Výsledkem by byla v podstatě totožná hustota pravděpodobnosti jako ta výchozí, pouze příslušně posunutá. Nicméně, takto by situace vypadala pouze v případě, že bychom si informací o změně pozice byli stoprocentně jisti. V situaci, kdy i tato informace je svým způsobem nejistá, potřebujeme toto zohlednit. Potřebujeme tedy nějaký ekvivalent sčítání rozptylů, ovšem pro reprezentaci hustoty pravděpodobnosti pomocí vzorků. Toho docílíme tak, že neposuneme všechny vzorky o stejnou hodnotu. Ke každému z posunů přidáme navíc nějaký náhodný šum, jehož míra bude odpovídat míře nedůvěry, kterou k této informaci chováme. Tím získáme hustotu pravděpodobnosti, která bude svým způsobem trošku rovnoměrnější. V této fázi chceme aplikovat krok korekce. Tento krok spočívá v modifikaci vah jednotlivých vzorků. Každý vzorek ohodnotíme podle toho, jak získané měření odpovídá předpokladu pro příslušnou pozici hodnoceného vzorku. Pokud chceme například využít informaci o barvě podlahy pod robotem, zvýšíme váhu všem takovým vzorkům, které se v mapě nachází na pozici, kde je pod robotem barva shodná s barvou naměřenou senzory a naopak snížíme, kde se neshoduje. Tyto fáze opakujeme pro každé měření, které chceme využít k odhadu pozice. Neustálým opakováním korekčního kroku, ale časem dojde k určité degeneraci naší množiny vzorků. Většina vzorků bude mít zanedbatelné váhy a několik málo naopak váhy obrovské. Toto rozložení vah není optimální z hlediska rozložení výpočetní síly. Vzorky s malou vahou odpovídají pozicím, kde se robot pravděpodobně nevyskytuje, a proto je zbytečné se jimi zabývat. Naopak pozice s vzorky s vysokou vahou jsou více pravděpodobné, a proto bychom tam chtěli soustředit většinu pozornosti. Proto se často využívá ještě fáze třetí převzorkování, anglicky resampling. Účelem převzorkování je v podstatě zahodit vzorky s hodně malou vahou a vzorky s vahou velkou naopak rozdělit na vzorků několik. Je vhodné, aby se hustota pravděpodobnosti tímto krokem změnila co nejméně, protože se na převzorkování můžeme dívat pouze jako na implementační detail, který do celého výpočtu jen vnáší určitý šum [1].

42 Strana Lokalizační metody Obr. 22: Příklad vzorkování založeného na aproximaci poziční důvěry pro robotu. Plná čára zobrazuje předpokládaný pohyb robotu a vzorky (tečky) představují důvěryhodnost robotu v čase. Obrázek 23 ukazuje MCL v akci. První schéma ukazuje distribuci důvěry (particle množina) na začátku experimentu, kdy robot ještě neví kde se nachází. Každý bod je trojrozměrný vzorek x-y umístění robotu spolu s jeho směřujícím směrem θ. Druhé schéma ukazuje důvěru v pozici po krátkém pohybu robotu, po zařazení několika snímačů či sonarů. V tomto bodu se většina vzorků koncentruje do dvou lokalit, tzn. že souměrnost prostoru znemožňuje robotu se zlokalizovat. Poslední schéma na obr. 23 ukazuje důvěru v pozici po pohybu robotu do jedné z místností, tím se zruší souměrnost a umístění má pak vysokou důvěru.

43 7.1. Lokalizace polohy Strana 43 Obr. 23: Globální lokalizace mobilního robotu pomocí MCL ( vzorků): (a) rovnoměrně rozdělená počáteční particle množina (plánovano ve 2D). (b) vzorky (složky) po přibližně 2 metrech pohybu robotu. Kvůli symetrickému prostředí se většina složek centruje kolem dvou lokalit. (c) particle množina po posunu do místnosti, tím se rozbije souměrnost a ukáže se pozice robotu. Tyto experimenty byli uskutečněny s robotem RWI B21 v [5].

44 Strana Lokalizační metody 7.2 Bayesovské metody Bayesovské metody (BM) spadají do všeobecného formalismu pojmenovaného Bayesovské programování (BP), které je popsané v [9]. BM představují jeden ze základních přístupů teoreticko-pravděpodobnostního myšlení i matematicko-statických vyhodnocovacích metod. Vychází se z předpokladu, že naše informace (apriorní znalost, zkušenost) o hodnotě neznámého parametru může být vyjádřena pomocí pravděpodobnostního rozdělení, tj. neznámý parametr můžeme považovat za náhodnou veličinu. K závěrům o hodnotě neznámého parametru využijeme jednak apriorní informaci o hodnotě parametru, jednak experimentální výsledky (nezávislé na této apriorní informaci). Na bayesovské metody však můžeme hledět jako na metody, které nám poskytují jisté řešení statických problémů [19]. Obr. 24: Pravděpodobnostní znázornění formalismu a jeho neúplná organizace s ohledem na nadřazenost a specifikaci. Základním principem Bayesovský metod je, že se v nich neznámé stavy, parametry či struktury považují za náhodné veličiny. Naší subjektivní představu o rozložení pravděpodobnosti těchto náhodných veličin objektivizujeme daty změřenými na identifikovaném objektu [8]. Předpokládejme, že pozorujeme (měříme) vstup u(τ) a výstup y(τ) v čase τ =1,,k-1 a naše znalost stavu systému založená na množině dat { u( 1 ), y( 1 ),..., u( k 1 ), y( k 1) } 1 D k = (10) je popsána podmíněnou hustotou pravděpodobnosti k 1 ( x( k ) D ) p (11) Problém je, jak aktualizovat naši znalost stavu popsanou podmíněnou hustotou k 1 p x k D na pravděpodobnostní derivační funkci (p.d.f.) pravděpodobnosti (p.h.p.) ( ) ( )

45 7.2. Bayesovské metody Strana 45 k ( x( k + ) D ) poté, co jsou změřena nová vstupní a výstupní data { ( k ) y( k) } p 1 u,. Při tom známe pravděpodobnostní model systému, který určuje pravděpodobnostní vlastnosti výstupu systému p y( k) x( k), u( k) (12) ( ) a pravděpodobnostní vývoj stavu systému ( x( k 1) x( k), u( k) y( k) ) p +, (13) Řešení tohoto problému se skládá ze dvou kroků. Datový, (filtrační, nebo objektivní) krok je založený na vstupních a výstupních datech. Vycházíme tedy ze k 1 p x k D. Povšimněme si, že toto p.h.p. je založeno na časech až do k-1. známé p.h.p. ( ) ( ) ( ) Užitím modelu výstupu určeným p.h.p. y( k) x( k) u( k) výstupu a stavu psát ve tvaru p k 1 ( y( k), x( k) D, u( k) ) = p y( k) x( k), u( k), = p x = k 1 k 1 ( D ) p( x( k) D, u( k) ) k D k 1 k 1 ( k) y( k), u( k), D p y( k) u( k), D, u( k) p, můžeme vzájemnou p.h.p. ( ) Z předchozího vztahu užitím tzv. přirozených podmínek řízení k ( x( k ) D ( )) 1, u k = p x( k) k 1 ( D ) (14) p (15) Plyne vztah pro aktualizaci p.h.p. pomocí nově získaných dat p k ( x( k ) D ) p y( k) x( k ), u( k) k 1 k (, D ) p( x( k ) D, u( k )) 1 1 = α (16) kde normalizační konstanta α je nezávislá na datech a je rovna p k 1 k 1 ( y( k) D, u( k) ) = p y( k ), x( k) D, u( k ) ( ) dx( k) (17) Povšimněte si, že až na normalizační konstantu je datový krok součinu k 1 k 1 y k x k, u k, D p x k D, u k založené na ( ) ( ) věrohodnostní funkce p ( ) ( ) ( ) a p.h.p. ( ) ( ) starých datech. Předchozím vztahem je ukončen datový krok algoritmu. Časový (predikční nebo subjektivní) krok je založen na pravděpodobnostním modelu systému, který je určen známou p.h.p. (13). Využitím vztahu pro marginální hustotu pravděpodobnosti můžeme psát prediktivní hustotu pravděpodobnosti ve tvaru

46 Strana Lokalizační metody p k ( x( k + ) D ) = p x( k 1), x( k) k ( D ) dx( k ) 1 + (18) Použitím vztahu pro podmiňování dostaneme konečný vztah pro prediktivní p.h.p. stavu p k k ( x( k + ) D ) = p x( k 1) D, x( k) k ( ) p( x( k) D ) dx( k ) 1 + (19) Kalmanův filtr Kalmanův filtr (KF) je pojmenovaný po Rudolfu E. Kalmanovi, ačkoli Thorvald Nicolai Thiele a Peter Swerling ve skutečnosti vyvinuli podobný algoritmus již dříve. První realizací KF byl pověřen Stanley Schmidt. Během Kalmanovi návštěvy v Americkém výzkumném středisku NASA, to byl on co viděl použitelnost Kalmanovi teorie v problému odhadu trajektorie pro program Apollo. To mělo za důsledek, že byl KF začleněn do navigace v počítači na Apollu. Filtr byl dále rozvíjen Swerlingem (1958), Kalmanem (1960) a Kalmanem a Bucym (1961). Široká škála Kalmanových filtrů byla rozvinuta, z Kalmanovy originální formulace. Nyní jsou nazývány jednoduchý Kalman filtr, Schmidtův rozšířený filtr, informační filtr a různorodý odmocňovací filtr vyvinutý Biermenem, Thortonem a mnohými jinými [4]. Kalmanův filtr je jeden z nejznámějších a nejpoužívanějších způsobů, ve kterém implementujeme systém popsaný v kap 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 motoru 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é informace dříve zvýšenou neurčitost (rozptyl) gaussovského rozložení opět sníží a cyklus se muže zopakovat [14]. Vztahy, kterými popíšeme Kalmanův filtr, platí pouze pro lineární systémy, pro praktické realizace se musí využívat rozšířeného Kalmanova filtru (EKF Extended Kalman filter), který celý problém linearizuje vůči aktuální poloze robotu. EKF během lokalizace pracuje i s neurčitostí vytvářené mapy. 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 Rozšířený Kalman filtr má slabý výkon na vysoce nelineárních funkcích. Unscented Kalman filtr (UKF) používá deterministickou techniku vzorkování známé jako unscented transformace pro výběr minimální kolekce vzorků bodů (nazvaných sigma body). Tyto sigma body jsou pak propagovány skrz nelineární funkce a kovariance odhadu (kovariance je střední hodnota součinu odchylek obou náhodných veličin od jejich středních hodnot) je pak obnovena. Výsledek je potom filtr, který přesněji zachycuje pravdivý průměr a kovarianci [4, 20]. 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 [14].

47 7.2. Bayesovské metody Strana Základní princip Kalmanova filtru Hodně zjednodušeně by se dalo říci, že Kalmanův filtr je takový vylepšený odhad plovoucího průměru. Toto vylepšení spočívá v rozdělení algoritmu do dvou kroků: predikce nového stavu korekce integrací nového měření V případě odhadu plovoucího průměru se predikce stavu neměnila (avg = avg). Navíc budeme chtít nějakým způsobem reprezentovat zapomenutí nejstaršího měření. Toho dosáhneme tak, že si budeme pamatovat, jak moc našemu aktuálnímu odhadu (ne)věříme. S každou predikcí P jakoby snížíme důvěryhodnost (nebo spíše zvýšíme nedůvěryhodnost Q) našeho odhadu: P = P + Q (20) Korekci nově predikovaného stavu provedeme pomocí nového měření: ( y[ n] avg) avg = avg + K (21) kde K je určitá váha (u plovoucího průměru jsme používali převrácenou hodnotu délky průměrovacího okna). Kalmanův filtr definuje K, Kalmanovo zesílení (Kalman gain) jako: ( P R) K = P + (22) kde R je míra nedůvěryhodnosti měření (nebo také množství šumu). Kalmanovo zesílení je vždy menší než 1. Pokud by R bylo rovno 0, znamená to, že měření má naši 100%-ní důvěru, a proto aplikujeme celou korekci. Jinak aplikujeme vždy pouze její část, která je dána poměrem nedůvěryhodnosti odhadu stavu a měření. Po korekci vlastního stavu bychom rádi opravili i jeho aktuální míru nedůvěryhodnosti. Tato korekce je definována jako: ( K ) P P = 1 (23) Tento vztah lze interpretovat tak, že pokud jsme použili jednu desetinu z korekce stavu, zmenšila se nedůvěryhodnost našeho odhadu na devět desetin. Pokud zvolíme P=0,9, Q = 0,1 a R = 9, degeneruje náš příklad na odhad plovoucího průměru pro délku okna 10.

48 Strana Lokalizační metody Obr. 25: 1D Kalmanův filtr odpovídající k=4. Na příkladu jsme si ukázali, že pro konkrétní hodnoty P, Q a R může Kalmanův filtr počítat odhad plovoucího průměru. Je to tak ale i v obecném případě? Při výpočtu průměru jsme předpokládali, že jednotlivá měření pro nás mají stejnou váhu, a že se tedy na výsledku podílí stejnou měrou. Dále definice rekurzivního výpočtu průměru je vlastně vážený průměr dvou čísel, kde jedno má váhu n-1 a druhé 1, protože to první reprezentuje průměr n-1 hodnot. Podobně jsme uvažovali i při odhadu plovoucího průměru s tím rozdílem, že obě váhy byly konstantní k-1 a 1. Kalmanův filtr ale není definován pomocí vah, ale pomocí rozptylů, tzn. nikoli pomocí důvěryhodnosti hodnot, ale naopak jejich nedůvěryhodnosti. Jak by asi tak mohl vypadat vhodný převodní vztah mezi vahou a rozptylem? Zkusme se více zamyslet nad jednotlivými kroky výpočtu. První krok kalmanova filtru se zabývá predikcí stavu pro následující krok a jeho nového rozptylu. Nový rozptyl nabývá hodnoty P = P + Q. Tento krok odpovídá snížení váhy průměru z hodnoty k na hodnotu k-1. Druhý krok integruje nové měření a snižuje rozptyl podle P = ( 1 K ) P, kde K = P ( P + R). V našem odhadu průměru to odpovídá opětovnému zvýšení váhy na hodnotu k. Co kdyby rozptyl byl vlastně převrácenou hodnotou naší váhy? Museli bychom ukázat, že platí: 1 k 1 = 1 k + Q K 1 ( ) = ( 1 ( k 1) ) ( 1 ( k 1) + R) k = ( 1 K ) 1 ( k 1) Q. Předpokládáme-li nadále, že R = 1, což můžeme, protože záleží pouze na poměru hodnot R a Q, a nikoli na jejich absolutní hodnotě, získáme zjednodušením rovnice druhé vztah K = 1 / k. Dosazením do rovnice třetí ověříme, že náš původní předpoklad o rovnosti rozptylu a převrácené hodnoty váhy, byl pravdivý [1]. Postupnými úpravami první rovnice snadno nahlédneme, že = 1 k ( k 1) (24)

49 7.2. Bayesovské metody Strana Více-dimenzionální (maticová) podoba Kalmanova filtru Obecná definice Kalmanova lineárního filtru pro více dimenzí vypadá takto [1]: Predikce x k +1 = Ax k (25) Pk +1 = Pk + Q (26) Korekce x x + K z Hx (27) k + 1( k ) T ( HP H ) 1 k + 1 = k + 1 k 1 K T k + 1 = Pk + 1H k R (28) P ( ) k + 1 = I KkH Pk + 1 (29) Přestože se tento maticový zápis může zdát na první pohled dosti neprůhledný, princip zůstává stále stejný, jako při odhadu plovoucího průměru. Obr. 26: Ilustrace Kalmanova filtru: (a) počáteční důvěra, (b) měření (tučná křivka) s přidruženou nejistotou, (c) důvěra po integrování měření do důvěry používající algoritmu Kalmanova filtru, (d) důvěra po pohybu doprava (představuje ji nejistota), (e) nové měření s přidruženou nejistotou a (f) vyplívající důvěra.

50 Strana Lokalizační metody Particle filtr Různé metody byly v minulosti použity, aby odhadovaly a redukovali nejistotu pohybujícího se robotu. Jeden přístup, který později získal popularitu pod kategorií Monte Carlovy simulace a je známý pod různými názvy v různých oborech, byl představen jako Particle filtr (částicový filtr) od N. J. Gorgona. Particle filtr (PF) je tedy také známý jako sekvenční Monte Carlo metoda (SMC). Obvykle je také používán pro odhadování Bayesianových modelů, které jsou sekvencovitou analogií pro Monte Carlo Markovových řetězců (MCML). Jestliže je PF dobře navržený může být mnohem rychlejší než zmiňované MCML. Často jsou také alternativou k rozšířenému Kalmanovu filtru (EKF) nebo Unscented Kalmanovu filtru (UKF). Výhoda této alternativy je, že s dostatečnými vzorky se PF přiblíží k Bayesianovu optimálnímu odhadu a tím může být přesnější než EKF nebo UKF [7]. Výhody reprezentace problému aproximace odhadu posteriorních pravděpodobností ve formě particle filtru lze spatřovat v následujících bodech [14]: 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 filty 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 na 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 paralyzována. Algoritmus PF je totožný s algoritmem MCL uvedený v kapitole , s tím že PF používá opakovatelné převzorkování Markovská lokalizace V souvislosti s lokalizací mobilního robotu jsou Bayesovi filtry také známé jako Markovská lokalizace (ML). Specifický tvar těchto pravděpodobností závisí na odometrii robotu a typech snímačů pro lokalizaci. 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 procesu (POMDP). Markovský rozhodovací proces (MDP) 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. Základní pojmy Markovské lokalizace představím v jednoduchém příkladu z [13]. Uvažujme nad prostředím na obr. 27. Kvůli jednoduchosti předpokládejme,

51 7.2. Bayesovské metody Strana 51 že prostor pozic robotu je jednorozměrný, tzn. robot se může pohybovat jen vodorovně. Teď předpokládejme, že je robot umístěn někde v tomto prostoru, ale neznáme jeho počáteční polohu. Markovská lokalizace představuje tento stav nejistoty jako rovnoměrné rozložení přes všechny pozice (první schéma obr. 27). Nyní předpokládejme dotaz robotu jeho snímače a zjistíme, že se nachází vedle dveří. Markovská lokalizace modifikuje důvěru, zvyšováním pravděpodobnosti pro místo vedle dveří a snižováním pravděpodobnosti kdekoli jinde (druhé schéma obr. 27). Všimněte si, že vyplívající důvěra je vícevidová (máme více míst s vyšší důvěrou), protože dostupná informace je nedostatečná pro globální lokalizaci. Na třetím schématu se robot pohnul o nějaký úsek vpřed. Markovská lokalizace připojí tuto informaci posunutím distribuce důvěry. Pro vysvětlení, základní šum v pohybu robotu by nevyhnutelně vedl ke ztrátě informace, proto se zavádí nová důvěra, která je hladší (a tím ale i méně jistá) než předchozí. Nakonec opět předpokládejme umístění robotu a on se znovu nalézá, ale teď už vedle dveří. Nyní je toto pozorování násobené stávající (nerovnoměrnou) důvěrou, která vede ke konečné důvěře v pozici robotu, jak je ukázané na posledním schématu obrázku. V tomto bodě v čase je většina pravděpodobností centrováno kolem jednoho umístění. Nyní si je tedy robot přibližně jistý, kde se nachází. Obr. 27: Základní podstata Markovské lokalizace. Mobilní robot během globální lokalizace.

52 Strana Lokalizační metody Skrytý Markovský model Skrytý Markovský model (HMM hidden Markov model) jsou velmi populární specializace Bayesianova filtrování. HMM je model stochastického procesu založený na pravděpodobnostním konečném automatu, který v diskrétních časových okamžicích generuje náhodnou posloupnost pozorování (posloupnost vektorů pozorování). V každém časovém kroku změní model svůj stav, a to podle souboru předem daných pravděpodobností přechodu. Stav, do kterého model přejde, vygeneruje jedno pozorování (jeden vektor pozorování) podle rozdělení výstupní pravděpodobnosti příslušné k tomuto stavu. Každý model má přitom dva typy rozdílných stavů. Vstupní stav, který je stavem modelu před začátkem pozorování procesu, a výstupní stav, do kterého model přejde, jakmile proces generování skončil, jsou stavy, které negenerují žádná pozorování a nemají tedy žádné k nim příslušné rozdělení výstupní pravděpodobnosti - jsou to tzv. neemitující stavy. Vstupní a výstupní stav tvoří skupinu prvního typu, do skupiny druhého typu patří všechny zbylé - a to emitující stavy, které generují všechna pozorování. Příklad skrytého Markovova modelu generujícího posloupnost vektorů pozorování O ukazuje obr. 28 z [11]. Obr. 28: Markovský model generující posloupnost pozorování O = {o 1, o 2, o 3, o 4, o 5 } MCML Monte Carlo Markovská lokalizace (MCMC Markov chain Monte Carlo) je třída algoritmů umožňující simulovat složité stochastické systémy. Když chceme generovat z nějakého pravděpodobnostního rozdělení, tak zkonstruujeme markovský řetězec, jehož stacionární rozdělení je požadované rozdělení. Simulujeme markovský řetězec a po dostatečném velkém počtu kroků dostaneme přibližně výběr z daného rozdělení, pokud jsou splněny jisté předpoklady na řetězec, které zaručí, že limitní rozdělení existuje a splývá se stacionárním. Kolik je dostatečně velký počet kroků a jak zkonstruovat takový markovský řetězec? Konstrukce markovského řetězce s daným stacionárním rozdělením není těžká, existuje řada postupů. Těžší je určit, po kolika krocích řetězec zkonverguje k limitnímu rozdělení s rozumnou chybou. Existující MCMC algoritmy, které dají přesný výběr z limitního rozdělení (tzv. perfektní simulace), a to v konečném čase, který je ovšem náhodný. Navíc je to za cenu dodatečných výpočtů [10].

53 7.3. Simultánní lokalizace a mapování Strana Simultánní lokalizace a mapování Simultánní lokalizace a mapování se často v literatuře vyskytuje pod zkratkou SLAM a to z termínu Simultaneous Localization and Mapping. 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 filtru 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 objektu 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 [14] 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 elementu 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říznaku. Druhý významný problém EKF-SLAM algoritmu 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 [14] Algoritmus FastSLAM Všeobecný pohled na algoritmus FastSLAMu popisuje autor ve [14]. Klíčový problém s velkým počtem landmarků při odhadech posteriorních distribucí pozic robotu a landmarků se autoři [21] snaží řešit přesnou faktorizací, která obecný SLAM problém dekomponuje na problém odhadu posteriorních pravděpodobností trajektorií robotu v čase (x 1:t ) a problém K odhadu 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í [22], kde je představena technika Rao-Blackwellized particle filtru 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 filtru pro účely simultánní lokalizace a mapování je následující faktorizace: ( x1 : t, z1: t, u1: t, n1: t ) = p( x1: t z1: t, u1: t, n1: t ) p( θ k x1: t, z1: t, u1: t, n1 t ) p : k θ (30) kde x 1:t značí trajektorii robotu, resp. pozice robotu v časovém intervalu 0 ; t, θ jsou pozice landmarků, z 1:t jsou senzorická měření, u 1:t je odometrie robotu a n 1:t značí známé korespondence landmarků (jejich očíslování). FastSLAM algoritmus používá modifikovaný particle filtr pro odhad posteriorních x z, u n přes odhady trajektorií robotu. Odhady trajektorie lze označit p 1 : t 1: t 1: t 1, 1: t distribucí ( ) [ r ] [ r ] [ r ] [ r ] X t = { x1 : n } = { x1, x2,..., xt }r (31) r

54 Strana Lokalizační metody kde notace [r] značí r-tý particle v množině X t, která je vypočítávána inkrementálně z předcházející množiny X t-1 v čase t-1, odometrie u 1:t a měření z 1:t. [ r ] Každý dílčí particle x1 : n X t, který je použit ke generování pravděpodobnostního odhadu pozice robotu v čase t [ r ] [ r ] ~ p x u x (32) ( ) x1 : n 1: t 1: t, t 1 je vzorkován z pravděpodobnostního pohybového modelu robotu. Tento odhad je následně [ r ] přidán k k dočasné množině particlů v rámci trajektorie xt 1. Za předpokladu, že množina p x z u n, což je 1 : t 1 1: t 1, 1: t 1, 1: t 1 particlů v množině X 1:t-1 je distribuována podle ( ) asymptoticky korektní aproximace, nový particle je distribuován podle p x z u n. ( ) 1 : t 1: t 1, 1: t, 1: t 1 Pro vygenerování R particlů výše uvedeným způsobem je získána nová množina [ r ] X 1:t vzorkováním dočasné množiny particlů. Každý particle x 1 :n je vybírán s váhovým r faktorem w t, který je roven: [ r] p( x1: t z1: t, u1: t, n1: t) r w t = [ r] (33) px z, u, n ( ) 1: t 1: t 1 1: t 1: t 1 Hustota pravděpodobnosti ( x, z, u n ) p k 1 : t 1: t 1: t, 1: t θ ve faktorizaci (30) 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ů kde dvouprvkový vektor [ r ] [ r ] [ r ] [ r ] [ r ] { x1 : n, 1, 1,..., µ K K } r X t =, [ r ] 1 µ (34) µ a matice r 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 n t =k či ne. V případě, že n t =k, pak získáme: ( x z, u, t ) p( z θ, x, n ) p( θ x, z, u n ) p θ (35) v opačném případě k 1 : t, 1: t 1: t 1: t t k t t k 1: t 1 1: t 1 1: t 1, 1: t 1 n t k zůstanou Gaussiány nezměněny ( x z, u, t ) p( θ x, z, u n ) p θ (36) k 1 : t, 1: t 1: t 1: t = k 1: t 1 1: t 1 1: t 1, 1: t 1 FastSLAM algoritmus implementuje aktualizaci vztahu pomocí rozšířeného Kalmanova filtru, více detailů lze nalézt v [21]. Výrazná výhoda FastSLAM algoritmu v porovnání s klasickým SLAMem je, že se aktualizuje pouze K dvojdimenzionálních Gaussiánů oproti jednomu (2K + 3) dimenzionálnímu Gaussiánu (K landmarku a 3 složky pozice robotu).

55 7.3. Simultánní lokalizace a mapování Strana 55 Přímá implementace algoritmu vede na řešení s časovou složitostí O(MK), kde M je počet particlů v particle filtru a K je počet landmarků. Autoři ve [21] 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. Metodu založenou na Rao-Blackwellized particle filtrech použili též autoři [23], 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 x 1:t danou jeho pozorováními z 1:t a odometrií u 1:t pro výpočet posteriorních pravděpodobností přes všechny mapy a trajektorie robotu: p x m z, u p m x, z p x z u (37) ( ) ( ) ( ) 1 : t, 1: t 0: t 1 = 1: t 1: t 1: t 1: t, 0: t 1 Výpočet muže být proveden poměrně efektivně, protože hodnoty p ( m x z u ) 1 : t, 1: t, 0: t 1 mohou být vypočítány analyticky jakmile x 1:t a z 1:t jsou známy. Pro odhad posteriorní p x z u přes potenciální trajektorie FastSLAM algoritmus pravděpodobnosti ( ) 1 : t 1: t, 0: t 1 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í z 1:t a trajektorie x 1: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 p z m, nejaktuálnějšího pozorování dané mapy m přiřazené danému pravděpodobnosti ( ) t x t particlu a pozici xt příslušné trajektorie. V kterémkoliv okamžiku, v čase t 1, je k dispozici odhad pozice robotu x ˆ t 1 a odhad příslušné mapy m ˆ ( x ˆ, t 1 z1: t 1 ). Jakmile se robot přesune do nové pozice a získá nové měření zt, robot určí nejpravděpodobnější novou pozici jako: xˆ t = arg max xt { p( z ) x, mˆ ( xˆ, z )) p( x u, x )} t t 1: t 1 1: t ˆ 1 t t 1 1: t 1 Postup autorů [23] pro uzavírání smyček využívá techniky scan matching. Problematika uzavírání smyček vedla autora [24] 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. 29. (38)

56 Strana Lokalizační metody Obr. 29: Vazby úloh lokalizace, mapování a řízení pohybu podle [24], převzato z [14]. 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 [25]. 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. V [26] ukazuje, že principy FastSLAM algoritmu [27, 28], 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 [26] 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 [14].