IADENIE MOBILNÝCH OBOOV Průběžná lokalzace a torba map pomocí smkem řízeného robotu omáš Neužl, Frantšek Buran Abstrakt V článku je ueden prncp algortmu pro lokalzac a torbu map pomocí moblního robotu. Jedná se o algortmus, který pro lokalzac užíá odhad poloh orentačních bodů a měření získaná pomocí laseroého promtního snímače. ato data jsou zpracoáána pomocí rozšířeného Kalmanoa fltru. Klíčoá sloa robot řízený smkem, rozšířený Kalmanů fltr, průběžná lokalzace a torba map, SLAM Úod V oblast moblní robotk je současné době kladen soký důraz na schopnost autonomního proozu moblního robotu. Autonome moblního robotu je defnoána jako schopnost robotu konáat čnnost bez nutnost zásahu ldské obsluh nestrukturoaném prostředí. Pro to, ab moblní robot blo možno označt za autonomní, musí mít následující lastnost [,,3]: schopnost dlouhodobé samostatné čnnost bez nutnost ldského zásahu, schopnost samostatně se pohboat praconím prostředím, schopnost hnout se stuacím, které b mohl ést ke zranění ldské obsluh nebo znčení robotu, schopnost získáat nformace o praconím prostředí. Základním předpokladem pro splnění ýše uedených podmínek je, ab robot znal každém okamžku soj polohu. Polohu robotu lze získat mnoha způsob. Mez nejznámnější patří například lokalzace pomocí odometre, nercálního nagačního sstému nebo nagace podle orentačních bodů (do této skupn patří například nagační sstém GPS). V článku je uedena metoda lokalzace moblního robotu pomocí orentačních bodů prostoru, které jsou získáán pomocí laseroého promtního snímače, který je umístěn na robotu s podozkem řízeným smkem (skd steer). Lokalzační algortmus pracuje na prncpu rozšířeného Kalmanoa fltru, jehož pomocí je zpracoáán odhad poloh moblního robotu získaný z matematckého modelu a poloh orentačních bodů získaných zpracoáním měření z laseroého snímače. Lokalzace pomocí rozšířeného Kalmanoa fltru Pro určení poloh moblního robotu jsou užíána měření ze snímačů. ato data jsou současně užíána pro táření map, e které se robot lokalzuje, což ční moblní robot autonomním (e smslu schopnost samostatného pohbu prostorem). [,,4] Výhodou tohoto přístupu k lokalzac a torbě map je skutečnost, že moblní robot je schopen pohbu předem neznámém prostředí. Pro orentac prostředí nepotřebuje zásah čloěka a toření umělých orentačních bodů. Prncp lokalzačního algortmu založeného na rozšířeném Kalmanoě fltru je ueden na Obr.. Pro predkc poloh moblního robotu a predkc poloh orentačních bodů je užto matematckých modelů robotu a snímače, které jsou defnoán následujících kaptolách. Pomocí staoého popsu lze robot popsat jako: f [ h[, u, w, ] ] Kde funkce f popsuje nelneární matematcký model moblního robotu, ϑ je šumoý šumoý sgnál, který sobě zahrnuje nepřesnost a zjednodušení proedená př táření matematckého modelu. ento šumoý sgnál má nuloou střední hodnotu a koaranc Q. Funkce h defnuje matematcký model snímače, který úloze lokalzace a torb map předstauje transformac mez lokální a globální mapou. aké popsu snímače stupuje šumoý sgnál, který má obdobné lastnost, jako sgnál stupujcí modelu robotu a koaranční matc. lokální mapa souřadný sstém map je spojen se souřadným sstémem moblního robotu (počátek souřadného sstému je e středu laseroého snímače, globální mapa počátek souřadného sstému je 0,, ϕ umístěn bodě se souřadncem 0 0. Mapa obsahuje polohu orentačních bodů, které slouží k určení poloh moblního robotu. V popsoaném případě jsou orentační bod tořen roh prostoru, e kterých se robot pohbuje. Algortmus založený na rozšířeném Kalmanoě fltru (EKF) umožňuje určení nejlepšího odhadu poloh (stau) robotu. () 4
IADENIE MOBILNÝCH OBOOV moblního robotu. Pro stanoení průběhu rchlost pohbu těžště robotu jsou použt pouze rchlost jednotlých kol. Matematcký model robotu nezohledňuje síl, které na robot př pohbu působí. V případě robotu UA se jedná zláště o smkoé síl, které působí na kola robotu př zatáčení. Idea náhrad jednotlých druhů podozků je zobrazena na Obr.. Obr. Algortmus Kalmanoa fltru Fg. Kalman flter algorthm Odhad staoého ektoru a koarance odhadu je defnoána jako: ) ( ) f [ k u k + k ( ),,0] () P( ) f P( ) f k + fv Q f + V Hodnota staoého ektoru po proedení měření kroce k + je určena pomocí áhoé matce W a noačního ektoru. ˆ ( ) ˆ ( ) + W + + + υ P( ) P( ) W + + + S υυ ( k + k ) W kde υ h[ ( k ),0] (4) je noační ektor, jehož koaranční matce je určena podle: S υυ h P( ) hk + hw( k + ) f w (5) Kalmanou áhoou matc lze pak určt podle: W( k + ) P( + ) h S υυ (6) ato matce umožňuje nastat, zda algortmus dáá ětší áhu ýstupu z modelu robotu (snímače) nebo reálnému měření ze snímačů umístěných na robotu. Matematcké model moblního robotu a laseroého snímače jsou ueden následujcích kaptolách. Moblní robot Pro úlohu mapoání a lokalzace je užíán robot UA, který je íjen na Ústau automatzace a měřcí technk od roku 999. Jedná se o čtřkoloý moblní robot, který je poháněn děma stejnosměrným motor. Kola na každé straně robotu jsou spojena řetězem, robot nemá otočnou náprau, proto př otáčení robotu dochází ke smku. Hlaní ýhodou této konstrukce moblního podozku je jeho jednoduchá mechancká konstrukce a elká robustnost, což umožňuje pohb nejen e ntřních prostorách, ale e nějším prostředí. [3,5] Pro účel lokalzačně mapoacího algortmu bl tořen jednoduchý matematcký model umožňující stanoení staoých elčn popsujících polohu robotu. Matematcký model je založený na knematckém popsu pohbu (3) Obr. Náhrada smkem řízeného robotu tpu dfferental dre Fg. Skd steer moble platform replacement wth dfeferental dre Pro pops modelu robotu pomocí staoých elčn platí: f ϕ + cosϕ + snϕ ϕ + ω Změnu rchlost pohbu moblního robotu lze jádřt jako: ω Ld + r Pd Ld Pd dff + ϑ + ϑ ϕ Prk ϑ a ϑ ϕ jsou složk šumoého ektoru. ento ektor reprezentuje šum, který je do matematckého modelu nášen nedokonalostí matematckého modelu. Pro usnadnění odhadu elkost jednotlých složek šumoého ektoru bl proeden eperment, př němž bl na robot umístěn barené štítk, jejchž poloha bla určoána pomocí barené kamer, která bla umístěna nad plochou, po které se robot pohboal. Př tomto epermentu bla pro různé kombnace rchlostí otáčení kol robotu měřena poloha jednotlých kol robotu a poloha těžště. Př epermentu bla ukládána nejen poloha barených štítků umístěných na robotu, ale také data čítaná z řídcí jednotk robotu. ato data bla užta pro stanoení parametrů sloužících pro přepočet řídcích sgnálů z řídcí jednotk (elkost sgnálu se pohboala rozmezí hodnot [-7 7]) na rchlost otáčení kol robotu. Na Obr. 3 je zobrazen ýsledek epermentu. V praém horním rohu grafu jsou patrné bod, které leží mmo (6) (7) 5
IADENIE MOBILNÝCH OBOOV předpokládanou kružnc, po které se robot pohboal. ato skutečnost je způsobena nestejným optckým lastnostm podkladu, po kterém se robot pohboal. V těcto místech hodnocoací algortmus proedl špatnou nterpretac bare štítků umístěných na robotu, z čehož plnulo špatné určení poloh bareného štítku. Modrou barou je grafu zobrazen ýsledek měření poloh těžště smkem řízeného robotu. Bara čerená odpoídá ýstupu z matematckého modelu robotu, který odpoídá dferencálnímu podozku. uložena do akumulátoru H ( α, r). Každá buňka akumulátoru obsahuje počet bodů, které leží na přímce popsána normáloém taru: p cosα + snα Jejíž parametr jsou defnoán souřadncem buňk. Úloha hledání přímek obraze tímto přeedena na hledání lokálních mam akumulátoru H ( α, r). Příklad akumulátoru je zobrazen na Obr. 4. (9) Obr. 4 Akumulátor adono transformace Fg. 4 adon transform accumulator Obr. 3 Poloha těžstě robotu řízeného smkem získaná měřením a ýstup z matematckého modelu Fg. 3 Skd steer centre of grat poston wth model smulaton Z tohoto grafu je patrné, že pohb smkem řízeného robotu lze popsat pomocí matematckého modelu dferencálního podozku. Použtí tohoto modelu je omezeno na konkrétní druh odoroného porchu, pro nějž bla měřením získána konstanta pro určení poměru mez rozchodem kol jednotlých druhů moblních podozků. Model snímače Pro získáání nformací o praconím prostředí moblního robotu a torbu map prostředí je užíán planární laseroý snímač SICK LMS 00. [6,7,8,9,0] Prostředí, e kterém se robot je popsáno geometrckým prmt, která jsou hledána obraze ( ) O, získaném pomocí laseroého promtního snímače. to geometrcké prk reprezentují zd místností, e kterých probíhá úloha lokalzace. Jako orentační bod jsou užíán průsečík přímek, které předstaují roh místnost. Poloha průsečíků přímek obraze, je určoána pomocí adono transformace, která je defnoána jako: ( r, α ) O(, ) δ ( r cosα snα )dd f + je transformo- r, α ( 0, π. Obrazoá funkce O (, ), (, ) ána na funkc f ( r,α ), Bnární obraz ( ) (8) O, je každém kroku algortmu pootočen kolem sého středu a pro každý řádek pootočeného obrazu je určen počet bodů. ato hodnota je Mez hlaní ýhod adono transformace patří skutečnost, že není ctlá na rostoucí zdálenost mez jednotlým bod obrazu (z Obr. 5). Další ýhodou je, že chbějící bod obrazoé funkc O needou ke špatné nterpretac nalezených přímek. adonou tranformac lze použít nejen k hledání přímek obraze, ale také k hledání parametrů lboolně zolených křek. Algortmus adono transformace je šak poměrně ýpočetně náročný a jeho ýsledkem jsou ronce přímek, nkol úsečk, které b přímo odpoídal zdem ohrančujícím prostor. adonoa transformace elm úzce sousí s transformací Houghoou, kterou je také možné užít pro určení parametrů hledaných přímek. adonoa transformace - pro každý prek z prostoru α r je hledán počet bodů b- parametrů přímek H (, ) nárního obrazu ( ) O,, který houje zolenému popsu (ronce přímk), Houghoa transformace - pro každý bod z bnárního O, jsou hledán prk z prostoru para- obrazu ( ) metrů H ( α, r), které hoují zolenému popsu (ronce přímk). Jestlže se průsečík přímek nachází blízkém okolí naměřených bodů naměřených snímačem (eukldoská zdálenost), je označen jako roh místnost. V opačném případě se jedno průsečík, který neodpoídá reálnému orentačnímu bodu. Příklad lokální geometrcké map s přímkam, které bl určen za použtí adono transformace je ueden na Obr. 5. Modrou barou jsou zobrazen bod, které bl získán měřením pomocí laseroého snímače, barou čerenou jsou reprezentoán přímk nalezené pomocí adono transformace a orentační bod odpoídající rohům místnost. Barou zelenou jsou označen bod odpoídající průsečíkům přímek, které neodpoídají reálným orentačním bodům. 6
IADENIE MOBILNÝCH OBOOV [cm] 600 500 400 300 00 00 0 0 00 00 300 400 500 600 700 800 [cm] Obr. 5 Lokální senzorcká mapa s nalezeným přímkam a orentačním bod Fg. 5 Local sensorc map wth lnes and landmarks Pro model (transformační matc) promtního snímače lze psát: r h ϕ Kde ( ) ( ) tg ϕ w + w r ϕ (0) r, ϕ je poloha orentačního bodu defnoaná polárních souřadncích.,, ϕ jsou prk stao- ého ektoru popsující polohu moblního robotu. Souřadnce orentačního bodu lokální mapě jsou označen,. w r, w Šumoý sgnál, jehož složk ϕ odpoídají přesnost měřené zdálenost překážk a přesnost nastaení úhlu měřcího paprsku. Obr. 6 SLAM algortmus založený na EKF Fg. 6 EKF SLAM algorthm Pomocí algortmu NEES (normalzoaný kadrát odchlek odhadu) bla hodnocena kalta získané map. [,]. Pomocí ronce lze počítat kadratckou odchlku mez odhadem poloh získaným pomocí narženého algortmu a skutečnou polohou moblního robotu získanou přesným měřením. SLAM průběžná lokalzace a mapoání Na základě algortmu EKF, popsaného prní kaptole bl naržen algortmus pro průběžnou lokalzac a torbu map. Prncp tohoto algortmu je zobrazen na Obr.6. Pro odhad poloh moblního robotu a poloh orentačních bodů jsou užt model robotu a snímače defnoané předchozích kaptolách. ento algortmus bl otestoán na reálných datech získaných měřením e ntřních prostorách VU Brně. Výsledek je ueden na Obr. 7. ato hodnota umožňuje stanot, nakolk je odhad poloh posunutý oprot skutečné poloze robotu. ε ( ˆ ) ( ˆ ) P( k k ) (), ˆ označují staoý ektor (polohu robotu) a jeho odhad. Pro N kroků SLAM algortmu lze normalzoanou kadratckou odchlku určt jako: N ε ε () N Pro proedený pokus bla hodnota ε stanoena na 3,76, což odpoídá 9. percentlu. Z tohot ýsledku plne skutečnost, že hodnota E( ˆ ) 0 a odhad staoého ektoru je posunutý. 7
IADENIE MOBILNÝCH OBOOV Posun mez odhadem staoého ektoru a reálnou pozcí může případě mapoání rozlehlých prostor ést k nedostatečné přesnost odhadu poloh robotu. Nárůstu kadratcké chb odhadu lze zabránt zýšením počtu orentačních bodů. Jako orentační bod pro lokalzac lze užít například geometrcká prmta popsující zkoumaný prostor, statstcký pops zkoumaného prostoru (elkost, smetre, hodně zolené matematcké moment obrazoé funkce ( ) Záěr O, ), apod. V článku je popsán algortmus pro průběžnou lokalzac a mapoání (SLAM) ntřních prostor užíající rozšířený Kalmanů estmátor staů. Jsou zde ueden jednotlé prk nutné pro spránou funkc algortmu (matematcké model moblního robotu a snímače). V druhé část článku je uedena metoda pro hledání orentačních bodů e zkoumaném prostoru, která je založena na adonoě transformac. V záěru je uedena mapa zkoumaného prostoru získaná pomocí narženého algortmu a proedeno zhodnocení dosažených ýsledků. Naržený algortmus je hodný pro použtí e ntřních prostorách budo. éto skutečnost odpoídá naržený matematcký model robotu a metoda pro hledání orentačních bodů. V případě užtí e nějším prostředí, b blo nutné sstém doplnt o možnost měření neronost porchu. aké algortmus pro hledání orentačních bodů b musel bt nahrazen (doplněn) metodou hodnou pro nější prostředí, např. Orentační bod defnoané pomocí GPS souřadnc. [4] CSOBA, M., Smultaneous Localsaton and Mappng, Ph.D. thess, Oford, 997 [5] NEUŽIL,., Smultaneous Mappng and Nagaton for Skd Steered Moble obot, WSEAS Press, 008, ISBN 978-960-6766-44 [6] NEUŽIL,., JEŽ, O., Data processng for mappng n moble robotcs, IEA 007 [7] <www.sck.cz> [ct. 4. 9. 007] [8] NGUYE, V. MAINELLI, A., OMAIS, N., SIEG- WA,., A Comparson of ne Etracton Algorthms usng D Laser angefnder for Idoor Moble obotcs, IOS'005, ISBN 0-7803-89-3, 005 [9] NIEO, J., Detaled Enronment epresentaton for the SLAM Problem, Ph.D. hess, Unerst of Sdne, 005 [0] GINKEL, M., HENDIKS, L., VLIE, L. J., A Short Introductonto the adon and Hough transforms and how the relate to each other, Deft Unerst of echnolog, <www.ph.tn.tudelft.nl/~mchael/> [ct. 5.. 008] [] CASELLANOS, J., A., NEIA, J., ADÓS, J., D., mts to the Consstenc of EKF Based SLAM, IFAC Smposum on Intellgent Autonomous Vehcles, 004, <webds.unzar.es/~jdtardos> [ct. 0.. 008] [] BAILEY,., NIEO, J., GUIVAN, J., SEVENS, M., NEBO, E., Consstenc of the EKF-SLAM Algorthm, <www-personal.acfr.usd.edu.au/tbale> [ct. 0.. 008] Poděkoání ento článek znkl za podpor projektu MSM0063059 Intelgentní sstém automatzac a projektu MŠM Č - M0567. teratura [] HUN, S., BUGAD, W., FOX, D., Probablstc robotcs, he MI Press, 005, ISBN 0-6-06-3 [] HUN, S., obotc Mappng: A Sure, Eplorng artfcal ntellgence n the new mllenum, Morgan Kaufman Publshers Inc., 003, ISBN: -55860-8-7 [3] SIEGWA,., NOUBAKSH, I.,., Introducton to Autonomous Moble obots, A Bradford Book, MI Press, 004, ISBN 0-6-950-X Abstract he paper presents etended Kalman flter based algorthm for smultaneous localsaton and mappng. hs algorthm was desgned for use wth skd steer moble robot platform. Models of skd steerng moble robot and laser promt sensor are presented. Qualt of the proposed SLAM algorthm s ealuated at the end of the artcle. Ing. omáš Neužl Vsoké učení techncké Brně Fakulta elektrotechnk a komunkačních technologí Ústa automatzace a měřcí technk Kolejní 4, Brno 6 00 Emal: neuzl@feec.utbr.cz 8