VYSOKÉ UČENÍ TECHNICKÉ V BRNĚ BRNO UNIVERSITY OF TECHNOLOGY
FAKULTA STROJNÍHO INŽENÝRSTVÍ ÚSTAV AUTOMATIZACE A INFORMATIKY FACULTY OF MECHANICAL ENGINEERING INSTITUTE OF AUTOMATION AND COMPUTER SCIENCE
RRT PLÁNOVÁNÍ POHYBU ROBOTA APLIKOVANÁ VE 2D PROSTORU RRT METHOD FOR ROBOT MOTION PLANNING APPLIED IN 2D SPACE
BAKALÁŘSKÁ PRÁCE BACHELOR THESIS
AUTOR PRÁCE
ANETA SVIDENSKÁ
AUTHOR
VEDOUCÍ PRÁCE SUPERVISOR
BRNO 2007
ING. RADOMIL MATOUŠEK, PH.D.
Strana 3
ZADÁNÍ ZÁVĚREČNÉ PRÁCE (na místo tohoto listu vložte originál a nebo kopii zadání Vaš práce)
Strana 5
LICENČNÍ SMLOUVA (na místo tohoto listu vložte vyplněný a podepsaný list formuláře licenčního ujednání)
Strana 7
ABSTRAKT Tato bakalářská práce pojednává o problematice plánování pohybu mobilního robota pomocí RRT algoritmu. V teoretické části jsou vysvětleny základní pojmy, přehled metod používaných pro plánování cesty robota a popis pravděpodobnostních stromů. Praktická část popisuje implementaci v prostředí Matlab, ve které je pro plánování cesty robota ve dvourozměrném prostoru použit RRT algoritmus.
ABSTRACT This bachelor thesis deals with robot path planning by means of Rapidly-exploring Random Trees (RRT) algorithm. The theoretic part contains interpretation of basic terms, list of path planning methods and description of RRT algorithm. The practical part describe simplementation in Matlab using RRT for path planning of robot in 2D space.
KLÍČOVÁ SLOVA Plánování cesty, mobilní robot, RRT (Rapidly-Exploring Random Trees).
KEYWORDS Path planning, mobile robot, RRT (Rapidly Exploring Random Trees).
Strana 9
Obsah: Zadání závěrečné práce...................................................................................................3 Licenční smlouva.............................................................................................................5 Abstrakt............................................................................................................................7 1 Úvod................................................................................................................................11 2 Mobilní robot.................................................................................................................13 2.1 Základní struktura robota .............................................................................................13 2.2 Rozdělení mobilních robotů..........................................................................................14 2.2.1 2.2.2 2.2.3 2.2.4 2.2.5 2.2.6
Kolové mobilní roboty................................................................................. .........................14 Diferenciální podvozek............................................................................... ..........................14 Synchronní podvozek............................................................................................. ...............14 Trojkolový podvozek s řízeným předním kolem........................................... ........................15 Ackermanův podvozek................................................................................................ ..........15 Podvozek se všesměrovými koly................................................................................... ........15
3
Reprezentace prostředí robota ....................................................................................17 3.1 Pracovní prostor............................................................................................................17 3.2 Konfigurační prostor.....................................................................................................17 3.3 Metrika..........................................................................................................................17 3.3.1 Euklidovská metrika...................................................................................................... ........18
4
Problematika plánování pohybu..................................................................................19 4.1 Metody rozkladu do buněk............................................................................................19 4.1.1 Exaktní rozklad........................................................................................... ..........................19 4.1.2 Aproximativní rozklad.............................................................................. ............................19
4.2
Mapy cest......................................................................................................................20
4.2.1 Graf viditelnost.................................................................................................................. ....20 4.2.2 Graf tečen.................................................................................................................. ............20 4.2.3 Voroného diagram.......................................................................................... .......................20
4.3 4.4
Pravděpodobnostní mapy cest.......................................................................................21 Potenciálová pole..........................................................................................................21 5 RRT (Rapidly-expolring random trees).......................................................................23 5.1 Princip RRT...................................................................................................................23 5.2 Kontrolní vstupy............................................................................................................23 5.3 Konstrukce RRT............................................................................................................24 5.3.1 Základní algoritmus RRT................................................................................. .....................24 5.3.2 RRT pro kolové mobilní roboty....................................................................................... ......25
5.4
Vlastnosti RRT..............................................................................................................25 6 Implementace.................................................................................................................27 6.1 Popis úlohy....................................................................................................................27 6.2 Popis algoritmu.............................................................................................................27 6.2.1 6.2.2 6.2.3 6.2.4
7
Generování bludiště........................................................................................................... ....27 Zadávání startovní a cílové pozice......................................................................................... 29 Vyhledávání cesty grafem................................................................................................... ...30 Chování algoritmu........................................................................................................ .........33
Závěr...............................................................................................................................37 Seznam použité literatury.............................................................................................39
Strana 11
1
ÚVOD
Představa stroje, který by pracoval za člověka, je velmi stará. Dlouhou dobu však patřila pouze do říše snů. S postupným rozvojem technické úrovně společnosti se však tyto sny začaly pozvolna stávat skutečností. Robotika se začala významněji rozvíjet ve dvacátém století a to hlavně díky elektrotechnice. Poprvé zaznívá slovo robot a objevují se první praktické aplikace z oblasti robotiky. Následuje velmi rychlý vývoj. Dnes je robotika multidisciplinárním oborem, vyžadujícím znalosti mechaniky, elektrotechniky, měřící techniky, umělé inteligence, teorie řízení a mnoha dalších oborů. Plánování pohybu je dynamicky se rozvíjející oblast robotiky, která však také nachází uplatnění například v medicínských robotických systémech pro minimálně invazivní chirurgii, inteligentních zbraňových systémech, průmyslovém designu a nanotechnologiích. Díky značnému zájmu o problematiku plánování pohybu, bylo v posledních letech vyvinuto množství nových, efektivních algoritmů. Jedním ze zástupců nových technik je i RRT algoritmus (Rapidly_exploring Random Trees), kterým se tato práce zabývá. V teoretické části práce je nejprve definován robot, popsána jeho struktura a rozdělení robotů do podskupin podle způsobu pohybu a prostředí ve kterém pracují. Další kapitola je věnována mobilním kolovým robotům a popisu nejběžnějších kolových podvozků. V kapitole třetí jsou zavedeny pojmy umožňující vytvořit model prostředí. Následuje přehled plánovacích metod nejčastěji používaných pro plánování ve dvourozměrném prostoru. Popisu RRT algoritmu se samostatně věnuje kapitola pátá. Praktická aplikace v prostředí Matlab je popsána v kapitole šesté.
Strana 13
2
MOBILNÍ ROBOT
Robotem rozumíme počítačem řízený, integrovaný systém, schopný autonomní a cílově orientované interakce s reálným prostředím v souladu s instrukcemi člověka. (Havel, 1980) Robota můžeme definovat na základě různých hledisek. Z výše citované, klasické, Havlovské definice vyplývá, že robot který by měl zastupovat člověka, musí být schopen v prostředí ve kterém se nachází pohybu a zároveň musí být schopen toto prostředí fyzicky ovlivňovat. Robot obecně je systém skládající se z podsystémů, které mu právě tyto dovednosti umožňují. Druhá kapitola je zaměřena na kolové mobilní roboty a popis nejběžnějších typů kolových podvozků[1].
2.1
Základní struktura robota Robot se obvykle skládá z těchto subsystémů: ● Senzorický systém: Zahrnuje všechna technická zařízení, která snímají a následně zpracovávají údaje o okolním prostředí. Mohou to být například informace o akceleraci a rychlosti robota nebo vzdálenosti jiných objektů. ● Kognitivní systém: Uskutečňuje inteligentní chování robota a skládá se z několika komponent: ○ Systém pro porozumění a interpretaci senzorických dat: Jeho úkolem je vnímat informace o okolním prostředí. Zpracovává a vyhodnocuje informace ze senzorů. Interpretační část: pak podle nově získaných informací aktualizuje model prostředí. ○ Interní model prostředí: Shromažďuje poznatky o uspořádání a vlastnostech prostředí, ve kterém robot vykonává úkoly. ○ Plánovač: Tato část vytváří posloupnosti přípustných akcí, vedoucích od aktuálního stavu robota a prostředí ke stavu cílovému. Plánovač pracuje s informacemi obsaženými ve vnitřním modelu prostředí. ○ Realizátor plánů: Podrobně rozpracovává provedení jednotlivých akcí do podoby vhodné pro řízení aktuátorů. ○ Komunikační systém: Umožňuje styk s nadřazeným operátorem nebo jiným inteligentním systémem. ○ Aktuátory: Umožňují fyzické působení robota na prostředí. Jsou to lokomoční systémy (kola, pásy, nohy) a manipulační systémy (ruce, pracovní nástroje apod.)
Obr. 1 Blokové schéma obecného robota.
Strana 14
2.2
2 Mobilní robot
Rozdělení mobilních robotů
Roboty obecně lze rozlišovat na základě mnoha různých kritérií. Mobilní roboty rozlišujeme podle způsobu činnosti na dvě základní skupiny a to na stroje autonomní a dálkově ovládané. Autonomní robot na základě instrukcí a s použitím prvků takzvané umělé inteligence vykonává zadanou úlohu samostatně. Zároveň je schopen pracovat v neznámém, částečně známém nebo měnícím se prostředí. Oproti tomu dálkově řízené roboty nejsou schopny samostatného rozhodování a pracují výhradně podle instrukcí operátora. Dalším kritériem pro rozdělování je prostředí ve kterém roboti pracují. Mobilní roboti mohou operovat: ● Ve vesmíru. ● Ve vzduchu. ● Ve vodě. ● Na souši. Roboti operující na souši se dále dělí podle způsobu pohybu na: ● Kolové. ● Pásové. ● Kráčivé.
Obr. 2 Robotická ryba. 2.2.1
Kolové mobilní roboty
Kolové podvozky zahrnují širokou skupinu různých modifikací uspořádání. Rozlišujeme je podle typu kol, kdy kola mohou být aktivní nebo pasivní. Dále podle počtu stupňů volnosti kola a v neposlední řadě podle počtu kol a jejich uspořádání. V následujícím přehledu uspořádání kolových podvozků jsou uvedeny nejčastěji používané typy. 2.2.2
Diferenciální podvozek
Jedná se o jednoduchý podvozek s se dvěma aktivními koly s jedním stupněm volnosti, doplněný o stabilizační body. Mezi hlavní klady diferenciálního podvozku patří možnost použití jednoduché, poměrně přesné odometrie , dobrá manévrovatelnost a schopnost rotace na místě.
Obr. 3 Diferenciální podvozek.
2 Mobilní robot 2.2.3
Strana 15 Synchronní podvozek
V typické konfiguraci synchronního podvozku jsou tři kola se dvěma stupni volnosti, uspořádaná do rovnoramenného trojúhelníku. Pohyb kol je synchronizován tak, že se vždy otáčí se stejnou rychlostí stejným směrem a míří vždy na stejnou stranu. Výhodou tohoto podvozku je opět jeho dobrá manévrovatelnost. Stejně jako diferenciální podvozek, ani synchronní není vhodný pro pohyb po nerovném povrchu a k zdolávání překážek.
Obr. 4 Synchronní podvozek. 2.2.4
Trojkolový podvozek s řízeným předním kolem
Trojkolový podvozek má zpravidla dvě zadní kola hnaná a přední motoricky natáčené. Podvozek je hůře manévrovatelný než předchozí dva typy, není schopen otočit se na místě. Jeho výhodami jsou možnost užití ve složitějším terénu a aplikovatelnost jednoduché odometrie.
Obr. 5 Trojkolový podvozek s řízeným předním kolem. 2.2.5
Ackermanův podvozek
Ackermanův podvozek je dobře známým z automobilů. Nejčastějším uspořádáním, užívaným v robotice jsou hnaná zadní kola a přední kola natáčená, každé pod jiným úhlem. Zpravidla je používán u větších robotických aplikací, určených pro pohyb ve venkovním terénu nebo na silnicích.
Obr. 6 Ackermanův podvozek.
Strana 16 2.2.6
2 Mobilní robot
Podvozek se všesměrovými koly
Všesměrová nebo složená kola umožňují pohyb ve dvou osách. Jejich vhodným uspořádáním je možné zkonstruovat všesměrovou mobilní platformu, schopnou pohybovat se libovolným směrem bez předchozího natočení. V praxi se používají dva typy uspořádaní všesměrových kol, čímž vznikají dva typy podvozků. V prvním případě má běžné kolo na svém obvodu umístěny pasivní válečky natočené pod stejným úhlem. U dalšího typu jsou válečky umístěny kolmo na obvod kola a je nutné je uspořádat do kruhu. U obou typů uspořádaní jsou minimálním počtem tři pasivní válečky.
Obr. 7 Uspořádání všesměrového kola.
Strana 17
3
REPREZENTACE PROSTŘEDÍ ROBOTA
Každý reálný objekt, tedy i robot, se pohybuje v nějakém prostředí. Pro robota je určujícím prostor kde plní zadané úkoly. Aby se robot v tomto prostou orientoval, potřebuje mít k dispozici jeho zjednodušený model. Pro usnadnění vytvoření takového modelu bylo zavedeno několik pojmů, které jsou definovány níže.
3.1
Pracovní prostor
Prostor ve kterém se robot pohybuje a vykonává úkoly se nazývá pracovním prostor W. Pracovní prostor lze reprezentovat jako n-rozměrný Euklidovský prostor 3.1 W =ℝ N , kde N =2,3. V pracovním prostoru se mohou vyskytovat neprůchozí objekty, překážky O. Aby bylo možné matematicky popsat pozici a orientaci robota v pracovním prostoru, byl zaveden pojem konfigurační prostor C (configuration space).
3.2
Konfigurační prostor
Pro účely plánování činnosti můžeme robota chápat jako systém, který se vždy nachází v určitém stavu. Konfigurační prostor je pak množina všech přípustných stavů robota. Představme si robota pohybujícího se mezi stacionárními, neprůchozími překážkami O. Hlavní myšlenkou konfiguračního prostoru je reprezentace robota jako bodu a namapování překážek do pracovního prostoru. C obst ={q∈C : Rq ∩O≠0 } 3.2 R(q) je podmnožina pracovního prostoru W obsazená robotem R s konfigurací q. Vektor q jednoznačně určuje pozici a orientaci robota v prostoru. Cobst, oblast překážek nazýváme tu část stavového prostoru, která je obsazena překážkami. C free =C /C obst 3.3 Množina Cfree se nazývá volný konfigurační prostor nebo volný prostor. Je to prostor všech konfigurací, které nekolidují s žádnou z překážek a vyhovují všem omezením robota. Prostřednictvím konfiguračního prostoru redukujeme úlohu plánování robota na plánování pohybu bodu uvnitř robotova volného konfiguračního prostoru.
3.3
Metrika
Nezbytnou částí algoritmu je funkce popisující vzdálenost d dvou konfigurací robota v pracovním prostředí C. Nahrazujeme tak Pythagorovu větu pro nekonečně malý úsek vzdáleností, což nám umožňuje popsat prostor ve kterém se robot pohybuje. Metrika je zobrazení :M ×M ℝ 3.4 které splňuje následující axiomy (pro libovolná x , y , z ∈M ):
1. Axiom totožnosti: x , y=0 ⇔ x= y 2. Axiom symetrie: x , y= y , x 3. Trojúhelníková nerovnost: x , z ≤ x , y y , z
Z axiomů 1 a 3 vyplývá také nezápornost, někdy jmenovaná mezi základními vlastnostmi:
x , y≥0
3.5
Metrika je vždy definována pro určitý prostor. V závislosti na konkrétní aplikaci se volí taková metrika, která vzdálenost dvou konfigurací v pracovním prostoru popisuje nejlépe. Pro euklidovský
Strana 18
3 Reprezentace prostředí robota
prostor je definovaná tzv. euklidovská metrika ρ 3.3.1
Euklidovská metrika
Na množině ℝ N lze definovat takzvanou euklidovskou metriku, která vyjadřuje délku úsečky mezi dvěma body. Tento metrický prostor se nazývá euklidovský prostor dimenze n a označuje se En. Euklidovská metrika je definována následujícím vztahem: Pro X= (x1, x2,…, xn) ∈ Rn a Y = (y1,y2,…, yn) ∈ Rn.
x , y = x − y 2 x − y 2 ⋯ x n − y n 2
3.6
Strana 19
4
PROBLEMATIKA PLÁNOVÁNÍ POHYBU
Důležitým rysem inteligentních systémů je schopnost vytvořit si vnitřní model prostředí a pracovat s ním. Je-li zadán počáteční a cílový stav, má systém za úkol nalézt takovou posloupnost akcí, aby se jejich provedením dostal z počátečního stavu do cílového. Tato posloupnost akcí se nazývá plán. Stavový prostor můžeme reprezentovat orientovaným grafem, ve kterém uzly představují jednotlivé stavy a orientované hrany přechod mezi stavy. Úlohu plánování pohybu tedy můžeme formulovat jako hledání cesty v grafu stavového prostoru. Plánování je rozděleno do dvou kroků. Před uvedením do pohybu se provádí takzvané globální plánování. K němu je potřeba kompletní znalost prostředí. Vlastní pohyb je pak řízen lokálním plánovačem. K nalezení vhodné cesty robota je třeba použít některé z plánovacích metod. V následujícím přehledu [2] je popsáno několik nejčastěji používaných metod pro plánování ve dvourozměrném prostoru. Metody jsou rozděleny podle jejich principu do tří skupin: ● Metody rozkladu do buněk (Cell decomposition). ● Mapy cest (Roadmaps). ● Potenciálová pole (Potential fields).
4.1
Metody rozkladu do buněk
Základní myšlenkou této metody je rozdělení volného konfiguračního prostoru Cfree do množiny jednoduchých buněk. Plánování uvnitř je jednoduché a úkolem je nalezení sekvence buněk s následujícími vlastnostmi: ● První buňka obsahuje startovní pozici. ● Poslední buňka obsahuje cílovou pozici. ● Sousedící buňky v posloupnosti sdílejí společnou hranu v Cfree.. Graf je vytvořen vrcholy reprezentovanými volnými buňkami a hrany mezi dvěma vrcholy přechodem mezi buňkami. Cesta grafem z počátečního uzlu do cílového je pak vyhledána pomocí některého z prohledávacích algoritmů. 4.1.1
Exaktní rozklad
Existuje mnoho variant exaktního rozkladu. Jednotlivé typy se odlišují ve tvaru použitých buněk. Často se volí lichoběžníkový nebo trojúhelníkový tvar. Obecně tato metoda rozděluje volný konfigurační prostor Cfree do konvexních mnohoúhelníků, jejichž vrcholy jsou zároveň vrcholy překážek.
Obr. 8 Princip exaktního rozkladu. Exaktní algoritmus je kompletní, a to v tom smyslu, že pokud existuje řešení, algoritmus jej nalezne, jinak ověří jeho neexistenci. 4.1.2
Aproximativní rozklad
U této metody je svět rozdělen rekurzivně do čtvercových buněk rozdílné velikosti. Každá z buněk obdrží jeden z těchto přívlastků: ● Prázdná – v případě že buňka leží výhradně v Cfree. ● Obsazená – je-li celá obsazená překážkou.
Strana 20
4 Problematika plánování pohybu
● Kombinovaná – pokud je obsazená jen její část. Princip rozkladu je patrný z obrázku č.9. V prvním kroku je prostor rozdělen do čtyř kvadrantů a ty následně označeny přívlastkem. V případě, že buňky jsou pouze prázdné nebo obsazené, není další rozklad nutný. Je-li některý z kvadrantů označen jako kombinovaný, je dále dělen. Dělení kombinovaných buněk pokračuje tak dlouho, dokud není dosaženo dostatečně jemného rozlišení pro nalezení cesty (v případě že existuje).
Obr. 9 Aproximativní rozklad (P-prázdná buňka, K-kombinovaná buňka).
4.2
Mapy cest
Cestovní mapy slouží ke konstrukci sítě cest zachycující spojitost volného konfiguračního prostoru robota. Síť je označována jako cestovní mapa, (roadmap). Pokud je zadaná úloha plánování pohybu, do mapy přibudou startovní a cílová pozice robota, čímž se úloha změní na problém hledání cesty v grafu. 4.2.1
Graf viditelnost
Viditelnostní grafy slouží k popisu známého prostředí. Graf zobrazuje viditelnostní vztah mezi jednotlivými vrcholy scény, reprezentované polygony. Každý uzel nebo vrchol v grafu a každá hrana reprezentují viditelostní spojení, to znamená, že pokud se oba vrcholy mohou vzájemně vidět , tedy neleží mezi nimi žádná překážka, je mezi nimi nakreslena hrana. Bezkolizní cesta je pak určena posloupností hran grafu. Tento typ grafu je možné aplikovat pouze na prostor s polygonálními překážkami. Pokud se ve scéně vyskytují nepolygonální překážky, musejí být nahrazeny polygonem nejblíže odpovídajícím jejich tvaru. Díky tomu však zpravidla značně narůstá počet vrcholů v grafu a následkem toho se prodlužuje doba nutná k nalezení cesty grafem. Tuto nevýhodu řeší dále popsaný tzv. redukovaný graf viditelnosti nebo též graf tečen.
Obr. 10 Graf viditelnosti a redukovaný graf viditelnosti. 4.2.2
Graf tečen Graf tečen je obdobou grafu viditelnosti. Jak jeho název napovídá, hrany grafu jsou
4 Problematika plánování pohybu
Strana 21
zredukovány pouze na tečny. Díky nižšímu počtu hran je vyhledávání výsledné cesty výrazně rychlejší. Další výhodou redukovaného grafu je možnost aplikovat ho i na prostor s nepolygonálními překážkami. 4.2.3
Voroného diagram
Voroného diagramy jsou dlouho známou a podrobně studovanou datovou strukturou. Diagram zprostředkovává informace o rozmístění objektů v rovině. Voroného diagram V(S) je pro euklidovskou metriku definován jako: V S ={V pi ∣ p i∈ S } 4.1 Poskytuje rozdělení prostoru do buněk takových, že bod uvnitř buňky V(pi) je blíž k bodu pi než k libovolnému bodu náležícímu množině S. Získaný graf je tedy určen body Vorového diagramu stejně vzdálenými od dvou nebo více vrcholů překážek. Jak je vidět na následujícím obrázku, úseky road-mapy ve Vorového diagramu mohou být generovány mezi dvěma hranami, mezi dvěma doby a mezi hranou a bodem.
Obr. 11 Tvary segmentu Voroného diagramu.
4.3
Pravděpodobnostní mapy cest
Nechť m je počet milníků a d je maximální spojená vzdálenost. Pravděpodobnostní mapa je pak zkonstruována následovně: Náhodně vybereme m pozic v konfiguračním prostoru. Pak se provede kontrola a vyřadí se ty konfigurace, které paří do oblasti překážek. Výsledná množina je označena jako milníky. Pro každý pár milníků jejichž vzdálenost je menší než d se ověří, jestli jejich spojnice leží výhradně Cfree. Pokud ano, vygeneruje se segment mapy. Postupným opakování se zkonstruuje graf scény. Aby bylo možné použít pravděpodobnostní mapu k plánování pohybu, je potřeba napojit na některé z milníků start a cíl. Zpočátku probíhá pokus o napojení startu na skupinu milníků ležících ve vzdálenosti menší než d. Pokud tento pokus selže, vybere se nejbližší soused startovního bodu a pokus o připojení proběhne znovu. Stejně se postupuje i v případě napojování cíle. Výsledná cesta se pak vyhledává v grafu. Z neúspěšného hledání nelze bezprostředně usuzovat neexistenci cesty. Je možné, že na začátku nebyl vygenerován vhodný počet milníků pro dostatečné rozlišení scény.
Obr. 12 Postup vytváření pravděpodobnostní cestovní mapy.
4.4
Potenciálová pole
Tato metoda pokrývá pracovní prostor potenciálovým polem určeným funkcí V(x,y). Překážky jsou obklopeny odpudivým polem, jehož intenzita se vzdáleností od překážky klesá a zároveň ve scéně působí homogenní přitažlivé pole se silovým vektorem směřujícím k cíli. Hledaná cílová pozice leží v globálním minimu. Robot se pohybuje ve směru silového vektoru určeného součtem působících polí.
Strana 22
4 Problematika plánování pohybu
Nevýhodou tohoto přístupu je, že hledání cesty může uvíznout v lokálním minimu.
Obr. 13 Přitažlivé, odpudivé a kombinované potenciálové pole.
Strana 23
5
RRT (RAPIDLY-EXPOLRING RANDOM TREES)
V předchozí kapitole bylo popsáno několik často používaných plánovacích metod. Mezi ty patří i RRT (A Rapidly exploring Random Tree), který můžeme zařadit do skupiny pravděpodobnostních plánovačů. Podrobněji bude popsán v následujících řádcích.
5.1
Princip RRT
Metoda RRT [2] byla poprvé představena Stevem LaValle v roce 1998. Rychle rostoucí náhodné stromy (RRT) jsou datové struktury a algoritmy pro efektivní prohledávání ndimenzionálního, nekonvexního prostoru. I když je RRT vhodný pro širokou skupinu problémů plánování cesty, navržen byl především se zaměřením na neholonomické plánování, včetně dynamických omezujících podmínek, robotů s vysokým počtem stupňů volnosti. RRT se doporučují jako technika pro generování cest bez cyklů v nelineárních systémech s omezeními na dosažitelné stavy. Lze je intuitivně chápat jako Monte-Carlo verzi Voroného diagramů
Obr. 14 Voroného oblasti odpovídající vrcholům RRT stromu. V podstatě je RRT datová struktura,která se postupně rozrůstá při vytváření nových uzlů stromové struktury ve směru náhodně vybraných bodů. RRT začíná ve startovní konfiguraci qinit a během expanze se snaží nalézt cílovou konfiguraci qgoal. V každém kroku se rozpíná do náhodné konfigurace qrand . V průběhu hledání jsou vrcholy stromu tvořeny takovým způsobem, že všechny uzly náleží Cfree. Z tohoto důvodu musí existovat možnost testovat, zda určitý stav leží v Cobst. Stavy v Cobst mohou mít v závislosti na aplikaci různý význam. Nejčastěji Cobst představuje konfiguraci, kdy je robot v kolizi s překážkou.
5.2
Kontrolní vstupy
Během růstu stromové struktury jsou průběžně aplikovány kontrolní vstup, které ověřují polohu nově vygenerovaných uzlů. Pro správnou funkci algoritmu je potřeba definovat následující parametry. n ● N-dimenzionální stavový prostor X, kde X ={x x 1 , x 1 , , x n }⊂ℝ . ● Konfigurační prostor C. ● Počáteční konfiguraci qinit a požadovanou cílovou konfiguraci qgoal, kde q init ∈C a q goal ⊂C . ● Pohybové rovnice robota ve tvaru x ˙ = f x , y .
Strana 24
5 RRT (Rapidly-expolring random trees) ●
q rand ∉C obst . D : C {TRUE , FALSE }
Funkci D, ověřující polohu náhodné konfigurace
Množinu vstupů U, specifikující případná další omezení (např. maximální úhel natočení kol, nebo maximální poloměr projížděné zatáčky) . ● Inkrementální simulátor, který na základě derivací pohybových rovnic určuje konfiguraci robota v čase t. ● Metriku ρ, kde, : X ×X [0,∞ ] určující vzdálenost dvou konfigurací robota.. ●
5.3
Konstrukce RRT
V prvním kroku je vytvořen základní uzel ve startovní konfiguraci qinit. Vygeneruje se náhodná konfigurace qrand a vyhledá se nejbližší sousední bod existující RRT struktury. Nový bod qnew je generován ve vzdálenosti ∆q od nejbližšího souseda ve směru náhodné konfigurace qrand. Dále jsou na nově vytvořený uzel aplikovány omezující podmínky a jestliže uzel vyhoví všem omezením, (obě nové hrany uzlu z nejbližšího uzlu do nového uzlu leží v Cfree, doplňku Cobst) je přidán do RRT struktury. Jestliže nový uzel omezení nesplňuje, je jednoduše vyřazen a proces rozrůstání RRT pokračuje. RRT struktura se iterativně rozrůstá a algoritmus se snaží spojit uzel RRT struktury s cílovou konfigurací qgoal. Klíčovou výhodou RRT je rychlé rozrůstání k neprozkoumaným oblastem stavového prostoru.
Obr. 15 Rozrůstání RRT stromu. 5.3.1
Základní algoritmus RRT Následující zápis představuje základní verzi algoritmu, zapsanou v [2]. function BUILD_RRT(qinit) T.init(qinit) for k = 1 to K do qrand = RANDOM_CONF( ); EXTEND(T, qrand); return T, function EXTEND(T, qrand) qnear = NEAREST_NEIGHBOR(qrand, T); if NEW_CONF(qrand, qnear, qnew, unew) then T.add_vertex(qnew); T.add_edge(qnear, qnew, unew); if qnew = qrand then return Reached; else return Advanced; return Trapped;
5 RRT (Rapidly-expolring random trees) 5.3.2
Strana 25
RRT pro kolové mobilní roboty Tato verze algoritmu, zapsaná v [3] je upravena pro kolové mobilní roboty. RRT.init(qinit) Repeat for i=1 to CONNECT_CHECK_INTERVAL qrand = random state qclosest = getClosestNode(qrand) qrand = GenerateNewNode(qclosest, qrand) qnew = ApplyRestriction(qclosest ,qnew) if (qnew is OK) RRT.AddNewNode(qclosest, qnew) else RRT.Trapped end if end for RRT.TryConnectTo Goal untilGoalReached
5.4
Vlastnosti RRT
RRT má několik vlastností, které jej činí ideálně vhodným nástrojem pro řešení široké škály praktických problémů plánování cesty. ● Strom má silnou tendenci růst směrem k dosud neprozkoumaným oblastem.. ● RRT je za obecných podmínek pravděpodobnostně kompletní, tedy pokud existuje. cesta, pravděpodobnost že bude nalezena konverguje k jedné jak se počet iterací blíží k nekonečnu. ● RRT algoritmus je poměrně jednoduchý, což ulehčuje provedení vyhodnocení. ● Vrcholy stromu zůstávají vždy propojeny, i když je počet hran minimální. ● RRT může být uvažován jako samostatný plánovací modul, který lze přizpůsobit a začlenit do jiných plánovacích systémů. ● Celý plánovací algoritmus může být konstruován bez potřeby schopnosti řídit systém mezi dvěma určenými stavy, což značně rozšiřuje jeho aplikovatelnost.
Strana 27
6
IMPLEMENTACE
Cílem úlohy plánování pohybu je nalezení bezkolizní cesty z počáteční do cílové pozice ve dvoudimenzionálním prostoru, obsahujícím statické polygonální překážky. Rozměry robota jsou uvažovány. Jedná se o relativně jednoduchý problém, použitý především k ilustraci a přiblížení některých myšlenek a metod které se uplatňují ve složitějších úlohách, jakými jsou například plánování cesty tuhého tělesa s translačním pohybem a rotací nebo neholonomní plánování.
6.1
Popis úlohy
Úkolem je navrhnout algoritmus, který bude akceptovat oblast překážek, počáteční a cílovou konfiguraci. Algoritmus musí vracet bezkolizní cestu po které se robot dostane ze startovní do cílové pozice. W=R2 je dvoudimenzionální pracovní prostor robota, obsahující překážky O. Cfree=W \ O je volný konfigurační prostor robota
6.2
Popis algoritmu
Aplikace je napsána v prostředí Matlab Version7.1.0.246(R14). Ve svém programu jsem použila a pozměnila výukový kód pro jednoduché RRT, dostupný z [4] .Pro usnadnění práce s programem jsem vytvořila grafické rozhraní, které obsahuje panel s deseti tlačítky pro možnost volby bludiště, tlačítko pro zadávání startovní a cílové konfigurace, tlačítko pro zahájení vyhledávání cesty.
Obr. 16 Rozvržení grafického rozhraní. 6.2.1
Generování bludiště
Prostor ve kterém se robot pohybuje je dvourozměrný, s pevným počtem statických překážek. Je možné volit z deseti různých bludišť. Ta jsou realizována prostřednictvím matice znaků a funkcí loadMatrix, která zjistí rozměr matice a počet překážek. Překážky vykreslí v souřadném systému, kdy x-ová souřadnice jednotlivé překážky odpovídá m-tému sloupci matice a y-ová souřadnice n-tému řádku. Pro generování světa o velikosti (100,100) jsem použila matice o rozměru (18,18). Překážky
Strana 28
6 Implementace
jsou čtvercové, jejich rozměr je nastaven pevně a všechny překážky mají rozměry shodné. %matice znaků pro generování bludiště handles.m1 = ['--------0---------0-' '--000---0---000---0-' '--0---------0-------' '------------0-------' '------00--------00--' '-0-----0---------0--' '-00--------00----0--' '-0---------0--------' '----00--------------' '--------0-----------' '--0-----0---000---0-' '------------0-------' '--------------------' '-----000--------0---' '-0-----0---0----00--' '-----------0----0---' '----0------0--------' '----00--------------']; %%funkce pro načtení matice a vykreslen bludiště function loadMatrix(m) cla; SizeMaze = size(m); NumLine = SizeMaze(1); NumColumn = SizeMaze(2); world.NumObstacles = 0; th = [(pi/4):(pi/2):(pi/4)+2*pi]; hold on fill(0*sin(th) + 1, 0*cos(th) + 1,'r'); fill(0*sin(th) + 99, 0*cos(th) + 99,'r'); for y= 1:NumLine for x = 1:NumColumn if (m(y,x)=='0') px = (3.55*sin(th) + x*5); py = 100 - (3.55*cos(th) + y*5); fill(px,py,'b'); end end
end
6 Implementace
Strana 29
Obr. 17 Vykreslení bludiště č.1. 6.2.2
Zadávání startovní a cílové pozice
Počáteční a cílová pozice jsou zadávány poklikem do plochy bludiště pomocí funkce Matlabu ginput.V případě že je start nebo cíl zadán do oblasti obsazené překážkou, není tento bod akceptován a je potřeba jej zadat znovu. Případná kolize zadané konfigurace se ověřuje funkcí collisionObstacle pro každou překážku bludiště zvlášť, nejprve pro start a následně cíl. %fuckce ověřuje zda zadaná konfigurace nekoliduje s překážkou function result = collisionObstacle(m, scx, scy) result = 0; SizeMaze = size(m); NumLine = SizeMaze(1); NumColumn = SizeMaze(2); world.NumObstacles = 0; th = [(pi/4):(pi/2):(pi/4)+2*pi]; hold on fill(0*sin(th) + 1, 0*cos(th) + 1,'r'); fill(0*sin(th) + 99, 0*cos(th) + 99,'r'); for y= 1:NumLine for x = 1:NumColumn if (m(y,x)=='0') px = (3.55*sin(th) + x*5); py = 100 - (3.55*cos(th) + y*5); if (((scx>x*5-2.5) && (scx<x*5+2.5)) ... && (((100-scy)>y*5-2.5) && ((100-scy)
Strana 30
6 Implementace end
end
end
end
%vyhodnocuje se výsledek kontroly kolize, start a cíl se musí zadávat stále %znovu, dokud je návrat funkce collisionObstacle roven jedné while true [scx,scy]= ginput(1); if (collisionObstacle(handles.m, scx, scy)==0) handles.startx = scx; handles.starty = scy; guidata(hObject,handles); break; end end while true [scx,scy]= ginput(1); if (collisionObstacle(handles.m, scx, scy)==0) handles.stopx = scx; handles.stopy = scy; guidata(hObject,handles); break; end end
6.2.3
Vyhledávání cesty grafem
Po načtení startovní a cílové konfigurace a následném umístění vrcholu stromu do startovní konfigurace se zkontroluje, zda není možnost dostat se ze startovního uzlu přímo do cíle [4]. if ( (norm(start_node(1:2)-end_node(1:2))<segmentLength )... &(collision(start_node,end_node,world)==0) ) path = [start_node; end_node]; else numPaths = 0; while numPaths<1, [tree,flag] = extendTree(tree,end_node,segmentLength,world); numPaths = numPaths + flag; end end
Pokud to možné je, spojí se start a cíl, nalezená cesta je vykreslena funkcí plotWorld.. %kontrola zda je možné dostat se přímo ze startovní konfigurace do cíle[4] if ( (norm(start_node(1:2)-end_node(1:2))<segmentLength )... &(collision(start_node,end_node,world)==0) ) path = [start_node; end_node]; else numPaths = 0; while numPaths<1, [tree,flag] = extendTree(tree,end_node,segmentLength,world); numPaths = numPaths + flag; end end path = findMinimumPath(tree,end_node); plotWorld(world,path);
V případě že je cíl přímo ze startu nedosažitelný, je pomocí funkce createWorld vytvořena
6 Implementace
Strana 31
datová struktura světa s překážkami. function world = createWorld(m,NumObstacles, NEcorner, Swcorner); if (NEcorner(1) <= SWcorner(1)) | (NEcorner(2) <= SWcorner(2)), disp('Not valid corner specifications!') world=[]; else world.NEcorner = NEcorner; world.SWcorner = SWcorner; SizeMaze = size(m); NumLine = SizeMaze(1); NumColumn = SizeMaze(2); world.NumObstacles = 0; for y= 1:NumLine for x = 1:NumColumn if (m(y,x)=='0') world.radius(world.NumObstacles + 1)= 3.55; world.cn(world.NumObstacles + 1)= x*5; world.ce(world.NumObstacles + 1)= 100-(y*5); world.NumObstacles = world.NumObstacles + 1; end end end
end
V dalším kroku je vygenerován náhodný bod, funkce collision ověří, jestli nekoliduje s některou z překážek[4]. %% Generování náhodného bodu function node=generateRandomNode(world); % randomly pn = pe = chi = cost = node =
pick configuration (world.NEcorner(1)-world.SWcorner(1))*rand; (world.NEcorner(2)-world.SWcorner(2))*rand; 0; 0; [pn, pe, chi, cost, 0];
% check collision with obstacle while collision(node, node, world), pn = (world.NEcorner(1)-world.SWcorner(1))*rand; pe = (world.NEcorner(2)-world.SWcorner(2))*rand; chi = 0; cost = 0; node = [pn, pe, chi, cost, 0]; end
Kontrola kolize se provede tak, že se z rodičovského uzlu (předchozího přidaného bodu) ve směru náhodného bodu vygeneruje pět kontrolních bodů. Pro každý z nich se pak porovnává vzdálenost jeho x-ové a y-ové souřadnice se souřadnicemi každé překážky. Pokud je vzdálenost souřadnic menší nebo rovna zadanému poloměru překážky, je tento stav vyhodnocen jako kolize. Vyhoví-li podmínce všechny kontrolní body, můžeme strom o náhodný bod rozšířit, protože vzniklý segment cesty nebude kolidovat s žádnou z překážek. Pro započítání rozměru robota jsem v kontrolní podmínce kolize počítala s větším rozměrem překážky, potažmo oblastí překážek. Při poloměru robota dva body jsem zvolila poloměr překážky čtyři.
Strana 32
6 Implementace
%% Kontrola kolize náhodného bodu s překážkou. function collision_flag = collision(node, parent, world); collision_flag = 0; if ((node(1)>world.NEcorner(1))... | (node(1)<world.SWcorner(1))... | (node(2)>world.NEcorner(2))... | (node(2)<world.SWcorner(2))) collision_flag = 1; else plot([node(1),parent(1)],[node(2),parent(2)],'g'); for sigma = 0:.2:1, p = sigma*node(1:2) + (1-sigma)*parent(1:2); % check each obstacle for i=1:world.NumObstacles, if (norm([p(1);p(2)]-[world.cn(i); world.ce(i)])<=4), collision_flag = 1; plot([node(1),parent(1)],[node(2),parent(2)],'m'); break; end end end end
Rozrůstání stromové struktury zajišťuje funkce extendTree. Strom se rozrůstá ve směru náhodně generovaných bodů o pevně danou délku segmentu kroku od nejbližšího souseda nového bodu stromové struktury. Průběžně se kontroluje, zda je možné spojit nově přidaný bod s bodem cílovým. Nalezenou cestu vykresluje funkce plotWorld. Stromová struktura je vykreslena zeleně a segmenty stromu vyhodnocené jako kolizní mají fialovou barvu. Výsledná cesta je zobrazena červeně. Šířka čáry zvýrazňující cestu odpovídá průměru robota.
Obr. 18 Vykreslení cesty bludištěm č.1.
6 Implementace 6.2.4
Strana 33 Chování algoritmu
Na praktické aplikaci bylo možné si názorně ověřit vlastnosti algoritmu. První z nich je výpočetní doba. Doba potřebná pro nalezení cesty bludištěm je v první řadě závislá na členitosti cesty. Prvních šest bludišť má velký počet překážek a členitou strukturu. V bludišti č.2 při pěti pokusech nalezení cesty pro totožný start a cíl byla průměrná doba nalezení cesty 28,2 sekundy. Doba nalezení cesty bludištěm č.2 Pokus č.
Čas (s)
1
19,8668
2
44,8440
3
30,8157
4
13,8392
5
31,6688
Průměrný čas (s)
28,2069
Obr. 19 Průměrná doba nalezení cesty bludištěm č.2
Obr. 20 Doba vyhlednání cesty složitým bludišťem je v řádu desítek sekund.. Oproti tomu u výrazně jednodušších bludišť č.7 až 10 je průměrná doba nalezení cesty o řád nižší. V bludišti č.10 nalezl algoritmus cestu při pěti pokusech s totožným startem a cílem v průměru za 8,16 sekundy.
Strana 34
6 Implementace
Doba nalezení cesty bludištěm č.10 Pokus č.
Čas (s)
1
5,5927
2
15,135
3
6,9731
4
5,8896
5
7,2372
Průměrný čas (s)
8,1655
Obr. 21 Průměrná doba nalezení cesty bludištěm č.10
Obr. 22 Obr. 23 Vykreslení cesty bludištěm č.10. Dále má na rychlost prohledávání vliv velikost délky kroku. Je-li délka kroku delší, prohledávání je výrazně rychlejší, ale hrozí že algoritmus cestu nenajde, nebo je výsledná cesta hrubší a tím i delší. Také může nastat situace že úsek cesty vyhodnotí jako bezkolizní, přestože prochází překážkou. Při menší délce kroku je prohledávání podrobnější, avšak za cenu nárůstu doby. Na obrázcích č.18, 20 a 22 jsem pro vyhledání nastavila délku kroku tři. Na obrázku č.22 je vykreslena cesta s dvojnásobnou délkou kroku.
6 Implementace
Strana 35
Obr. 24 Cesta bludištěm č.1 s délkou kroky šest. Stejně tak je z vykreslení stromu jasně patrná snaha algoritmu rozrůstat se do neprohledaných oblastí. To může být důvodem delší doby prohledání složitých prostředí. Pokud algoritmus často nachází lokální minima, prodlužuje to dobu nalezení cesty. Zároveň je vidět, že vrcholy stromu jsou vždy propojeny a to i v případě prohledávání členitého prostředí s úzkými průchody.
Strana 37
7
ZÁVĚR
Cílem této práce bylo zaprvé vytvořit stručný popis problematiky. Tento cíl je splněn v rozsahu kapitol jedna až pět teoretické části práce, které popisuji strukturu robota, typy mobilních, kolových platforem a problematiku a metody plánování cesty. Přímo RRT algoritmu je věnována kapitola pátá. Dalším cílem bylo vytvořit softwarovou aplikaci RRT metody, včetně názorné vizualizace. Aplikaci jsem vytvořila v prostředí Matlab Version7.1.0.246(R14), pro vizualizaci chování algoritmu slouží grafické uživatelské rozhraní Matlabu s možností volby různých prostředí. RRT algoritmus je efektivním nástrojem pro hledání cesty. Vytvořená aplikace názorně ilustruje základní vlastnosti a princip fungování algoritmu, které lze účelně využívat ve složitějších úlohách, kdy se simuluje pohyb robota s jeho pohybovými omezeními nebo pohyb v mnohodimenzionálním prostoru.
Strana 39
SEZNAM POUŽITÉ LITERATURY [1] Mařík, M., Štěpánková, O., Lažanský, J., a kolektiv: Umělá inteligence (2)2003 [2] LaValle.S.M.: Planning Algorithms, 2006, http://msl.cs.uiuc.edu/~lavalle/ [3] Krejsa, J., Věchet, S.: Rapidly Exploring Random Trees used for mobile robots path planning, 2005 [4] Berard.R.: pathRRT, , http://www.et.byu.edu/groups/eceuavweb/labs/guidance_labs/PathPlanning/index.html
