Z E S Z Y T Y N A U K O W E PO LITECH N IK I ŚLĄSKIEJ Seria: A U T O M A T Y K A z. 119
_______ 1996 N r kol. 1339
A ndrzej FO R T U N A , W itold SILEIK1S P olitechnika Śląska
A L G O R Y T M N A W IG A C JI R O B O T A M O B IL N E G O W P R Z E S T R Z E N I 2D
S treszczen ie. Tematem referatu jest synteza i analiza algorytm u nawigacji autonom icznego robota mobilnego, poruszającego się w ograniczonej przestrzeni dw uw ym iarow ej, w której m ogą znajdować się proste przeszkody, pom ieszczenia, korytarze itp. System sensoryczny robota w ykorzystuje dalm ierz do pozyskiw ania inform acji o stanie otoczenia. Przedstawiono algorytm tw orzenia lokalnej mapy o toczenia na podstaw ie informacji sensorycznej pozwalający na rozw iązyw anie zadań zw iązanych z naw igacją w tym otoczeniu, wyszukiwaniem bezkolizyjnej trajektorii, o raz sposób syntezy hierarchicznej mapy otoczenia.
A L G O R IT H M O F M O B IL E R O B O T N A V IG A T IO N IN 2D S P A C E
S u m m a ry . Subject o f this paper is synthesis and analysis o f navigation algorithm fo r autonom ous mobile robot, which could navigate in two dimensional space, filled w ith simply obstacles, rooms, corridors etc. Robots sensor system is using range-finder to gain inform ation about environment. There is presented algorithm o f building local environm ent map, which enable solving problems o f navigation in this environm ent, searching fo r trajectory free from the possibility o f collisions and m ethod o f synthesis o f hierarchical environm ent map.
1. W stę p
A utonom iczny R obot Mobilny (A RM ) jest robotem potrafiącym sam odzielnie poruszać się w otoczeniu o znanym lub nieznanym modelu, umiejącym reagow ać na dynam iczne zm iany tego otoczenia, posiadającym m ożliwość omijania przeszkód lokalnych, sam odzielnego tw orzenia obrazu (m odelu w ew nętrznego, mapy) otoczenia i rozw iązyw ania zadań zw iązanych z naw igacją w tym otoczeniu.
Jednym z w ażniejszych podsystemów systemu sterow ania robotem mobilnym jest podsystem naw igacji. Podsystem nawigacji w celu spełnienia w ym agań przedstaw ionych pow yżej pow inien u m o żliw iać:
• w yznaczenie w szystkich podstawow ych param etrów określających położenie i orientację A R M w otoczeniu,
• lokalizację obiektów otoczenia oraz ich współrzędnych,
1&2. A. Fortuna, W. Siieikis
• tw orzenie i m odyfikow anie obrazu ( mapy) otoczenia,
• w yszukiw anie bezkolizyjnej trajektorii w celu realizacji zadań,
• omijanie przeszkód lokalnych.
Schem at blokow y systemu nawigacji oraz opis podstaw ow ych jeg o elem entów przedstaw iono w [12],
Realizacja zadania przem ieszczenia robota do zadanego położenia końcow ego, sform ułow ana przez system planowania zadań, wymaga wstępnej dekom pozycji zadania na zadania cząstkow e i alternatywne. K ażde zadanie cząstkowe jest przez system planow ania trajektorii lokalnej (na podstaw ie modelu topologicznego otoczenia) przekazyw ane do u kładów w ykonaw czych robota, które realizują w yznaczoną trajektorię. T ę część system u m ożna nazw ać częścią w ykonaw czą dla przemieszczeń ARM w otoczeniu.
System sensoryczny robota mobilnego nie je st częścią systemu nawigacji, dostarcza jed n ak niezm iernie istotnych danych dla tego systemu. Dla systemu naw igacyjnego potrzebne są dw a rodzaje informacji :
1. dane o zbliżaniu się do lokalnych przeszkód, nie będących elementami stałym i otoczenia, w celu zapew nienia możliwości ich ominięcia lub podjęcia przez system sterow ania innych decyzji m ogących zmienić realizację zadań. W tym celu stosuje się proste czujniki zbliżeniow e,np. ultradźw iękow e [7],[1],
2. dane o położeniu robota w otoczeniu oraz dane o położeniu innych obiektów , niezbędne do lokalizacji robota oraz do tw orzenia modelu otoczenia. W projekcie badaw czym H IL A R E [7] w ykorzystano do tego celu dalmierz laserowy um ieszczony na obrotow ej głow icy o raz kam erę wizyjną.
D ane drugiego rodzaju są w ykorzystywane przez system tw orzenia modelu otoczenia, który na podstaw ie w stępnie przetworzonych danych systemu sensorycznego buduje i m odyfikuje m odel topologiczny. Model topologiczny jest podstaw ą syntezy modelu strukturalnego otoczenia. W arunkiem koniecznym popraw ności m odelu je s t w iarygodne określenie w spółrzędnych robota w otoczeniu, za co odpow iedzialny je st system lokalizacji robota.
W opracow aniu wykorzystywane są pojęcia mapy lokalnej i globalnej (hierarchicznej).
M ap a lokalna jest to opis otoczenia A RM w układzie bazow ym , uw zględniający elem enty topologiczne (ściany, przeszkody, w olne obszary) oraz relacje pom iędzy nimi i ARM . N atom iast m apa globalna stanow i opis otoczenia ARM w układzie bazowym, uw zględniający grupy elem entów topologicznych (pomieszczenia, korytarze) oraz relacje pom iędzy nimi.
Algorytm nawigacji robota mobilnego .
Jj53.
Celem badań je st sform ułow anie algoiytmu, który na podstaw ie informacji z dalm ierza o raz system u nawigacji inercyjnej pozwoli utw orzyć mapę lokalną otoczenia ARM.
2. A lg o ry tm b u d o w y m ap y lokalnej A R M
S chem at blokow y algorytmu nawigacji i budowy mapy lokalnej przedstaw iono na ry s .l.
Id ea algorytm u jest oparta na wyodrębnieniu z informacji o odległości do najbliższych przedm iotów otoczenia stanowiących ograniczenie przestrzeni wolnej i dostępnej do celów naw igacji A R M charakterystycznych punktów , jak np. punkty załam ania ścian, kraw ędzie przeszkód, i zbudow anie w oparciu o te punkty zbioru w ielokątów w ypukłych obejm ujących w idzianą i w o ln ą przestrzeń. Zbiór w ielokątów i relacje pomiędzy nimi stanow ią reprezentację otoczenia i m o g ą być traktow ane jako mapa lokalna. Zbiór w ielokątów musi być rozłączny i musi pokryw ać c a łą w idoczną przestrzeń. M apa lokalna zbudow ana na podstaw ie tej zasady pozw ala na stosunkow o proste w yznaczenie położenia dow olnego punktu otoczenia w yrażonego w e w spółrzędnych układu bazow ego i wyznaczenie do niego trajektorii, obejm ującej kolejne sąsiadujące ze sobą wielokąty.
B udow anie m apy lokalnej otoczenia przez ARM jest realizow ane w trakcie w ykonyw ania zadań zlecanych przez system planowania zadań naw igacyjnych. System dekom pozycji zadań w yznacza zadane położenie i orientację robota w odniesieniu do układu bazow ego otoczenia. Jeżeli zadany punkt końcow y lub dowolny punkt pośredni trajektorii należy d o części otoczenia nieznanej dla robota, to w trakcie realizacji tego zadania A RM ma m ożliw ość poznania tej przestrzeni i uaktualnienia mapy lokalnej i globalnej.
A lgorytm budow y m apy lokalnej i wyznaczenia trajektorii zostanie szczegółow o przedstaw iony na podstaw ie przykładu w punkcie 3.
3. P rz y k ła d d z ia ła n ia a lg o ry tm u
Z ałóżm y, że A RM znajduje się w otoczeniu przedstaw ionym na rys.2, gdzie zaznaczono p oczątkow e położenie robota R(x,,yr), układ pom ieszczeń i przeszkód, bazow y układ odniesienia xyz oraz zadane położenie końcow e G(xg,yg). Załóżm y, że m apa otoczenia jest pusta, tzn. ro b o t nie zna otoczenia, w którym ma w ykonać zadanie. W trakcie je g o realizacji ro b o t musi zbudow ać mapę tej części przestrzeni, przez k tó rą w iodła trajektoria.
W spółrzędne punktu docelow ego zostały w yznaczone przez system planow ania i dekom pozycji zadań.
1S<, A, F ortuna, W. Sileikis
Yea
♦
Rys. 1. Algorytm nawigacji i budowy m apy lokalnej Fig. 1.N avigation algorithm and building local environm ent m ap
A lgorytm naw igacji ro b o ta mobilnego 165
z X
Rys.2. Schem at przykładow ego otoczenia robota Fig.2. Schem a o f sample robot environment
Pierw szym krokiem algorytmu jest sprawdzenie, czy punkt docelow y G leży w przestrzeni CKj, czyli w przestrzeni leżącej poza m apą lokalną otoczenia. Z godnie z powyższymi założeniam i w arunek ten jest spełniony.
K olejny punkt algorytmu to wyznaczenie trajektorii do punktu Tk(xv,yx), który leży w przestrzeni znanej dla robota i leżącego najbliżej punktu docelow ego. N a tym etapie rozw iązania zadania taki punkt nie istnieje i robot w celu rozpoczęcia budow y m apy lokalnej musi w ykonać skanow anie otoczenia.
P ro ces skanow ania otoczenia dalmierzem polega na w yznaczeniu odległości do najbliższej przeszkody w pełnym zakresie obrotu głowicy dalmierza od 0 do 36 0 stopni z zadanym skokiem . Efektem skanow ania jest wyznaczenie dyskretnej funkcji r¿ = u ( c t), gdzie a przyjm uje dyskretne w artości od 0 do 360, natom iast r,¡(a) jest odległością do przeszkody dla określonego położenia głow icy dalmierza a .
P o odfiltrow aniu szum ów i aproksymacji odcinkowej funkcji u = tj ( a ) m eto d ą najm niejszych kw adratów uzyskuje się funkcję r = r(a ) dla ciągłej w artości argum entu a .
W ykres tej funkcji je st przedstaw iony na rys.3.
A naliza punktów nieciągłości tej funkcji oraz jej pierwszej i drugiej pochodnej pozw ala na w yodrębnienie charakterystycznych punktów otoczenia, pokazanych n a rys. 4.
W spółrzędne każdego z tych punktów m ożna wyznaczyć na podstaw ie położenia i orientacji robota w układzie bazow ym oraz wartości a i r ( a ) [12], które są znane.
A. Fortuna, W. Sileikis
W yznaczone punkty charakterystyczne zw ane dalej w ierzchołkam i m ożna podzielić na trzy klasy w zależności od w artości r ( a ) w otoczeniu a .
• W ierzchołek W¡ należy do klasy I ("kąt"), jeżeli r(aj-) = r(cti+). W ierzchołki tej klasy zaznaczone są na rysunkach jako X.
• W ierzchołek W¡ należy do klasy II ("przysłaniający"), jeżeli r(a¡-) < r(a¡+).
W ierzchołki tej klasy zaznaczone są na rysunkach jak o O .
• W ierzchołek Wi należy do klasy III ("przysłonięty"), jeżeli r(oti-) > r(oti+).
W ierzchołki tej klasy zaznaczone są na rysunkach jak o v.
R ys.3. W ykres funkcji r = r ( a ) dla punktu R z rys.2 Fig.3. G raph o f the fiinction r = r ( a ) for point R from fig.2
Rys.4. A naliza punktów charakterystycznych skanow ania Fig.4. Analysis o f scaning characteristic points
A lgorytm nawigacji robota m obilnego . 167
P ołączenie kolejnych punktów pozwala na określenie obszaru w idocznego i niezajętego przez jakiekolw iek przeszkody (rys.4).
W yznaczone i przyporządkow ane do odpow iednich klas w ierzchołki obszaru w idocznego pozw alają na wyznaczenie zbioru w ielokątów w ypukłych, pokryw ających ten obszar. W pierwszym kroku należy wyznaczyć takie wielokąty, których w szystkie w ierzchołki należą do klasy I lub II, poniew aż stanow ią one niezmienne elementy mapy lokalnej. T akie w ielokąty nazyw am y kom órkam i klasy I. Pozostały obszar zostaje pokryty kom órkam i klasy II, tzn. takim i wielokątam i, których przynajmniej jeden w ierzchołek należy do klasy III. Tej klasy kom órki nie stanow ią niezmiennej części mapy otoczenia i przy innym ustawieniu robota w otoczeniu m o g ą zostać zmienione.
M apę lokalną zbudow aną na podstawie kom órek klasy I i II w ygodnie je st przedstaw ić w postaci grafu, w którym w ierzchołki odpowiadają poszczególnym kom órkom , a łuki odpow iadają m ożliwości przem ieszczenia się robota pomiędzy tymi kom órkam i. W każdej k om órce m ożna w yróżnić dow olnie w ybrany punkt (np. środek ciężkości) w celu w yznaczania odległości pom iędzy nimi.
M apę lokalną w postaci zbioru kom órek klasy I i II oraz odpow iadającego im grafu połączeń dla skanow ania z punktu R przedstawia rys.5.
W ęzły grafu oznaczone jako X odpow iadają kom órkom klasy I, natom iast w ęzły oznaczone ja k o p odpow iadają kom órkom klasy II.
P o zakończeniu tego kroku algorytmu możliwe jest wyznaczenie trajektorii zbliżającej ro b o ta do punktu docelow ego. Realizowane jest to za pom ocą algorytm u A*, pozw alającego znaleźć ścieżkę w grafie minimalizującą funkcję kosztów
F = Fz + Fn
gdzie:
Uk-1
Fz = ^ d, itl - długość trajektorii, obejmującej komórki kl. I i II i=l
Fn = C - ^ ( x k - x s )2 + (y k - y g): - najkrótsza odległość od ostatniego znanego punktu trajektorii do punktu końcow ego (w g m iary Euklidesow ej) lub
Fn = C- (|y k - y s |+ |x k - x g|) - jw. (w g miary now ojorskiej).
W wyniku analizy mapy lokalnej i zdefiniowanej funkcji kosztów kolejnym krokiem algorytm u w tym przykładzie jest przem ieszczenie robora do kom órki Cs. Poniew aż cel nie został osiągnięty, a m apa lokalna nie zaw iera informacji o otoczeniu z kom órki Cg w kierunku
168 A. Fortuna, W. Sileikis
punktu docelow ego proces poznaw czy jest realizowany ponow nie począw szy od skanow ania otoczenia z now ego położenia.
K olejne rysunki 6 i 7 przedstawiają kolejne fazy budow y m apy lokalnej i przem ieszczenia ro b o ta do punktu docelow ego, natom iast rys.8 przedstaw ia pełny o braz otoczenia robota, który został zsyntezowany w wyniku realizacji wielu zadań naw igacyjnych.
*)
K o m ó rk a k l a t y Q
K o m ó rk a k la ty ę
Rys.5. M apa lokalna otoczenia -faza 1 F ig.5. Local environment map - phase 1
I v j Komórka k laty q
B S 5 B 8 I Komórka k laty
Rys. 6. B udow a mapy lokalnej - faza 2 Fig.6. Local map building - phase 2
Algorytm naw igacji robota mobilnego 169
[. i- / *1 Konłórlci klaay C ,
F x frf& sM K om ófka k la iy C ,
Rys.7. Budow a mapy lokalnej - faza 3 Fig.7. Local map building - phase 3
. ’ i " j / V ' •...V - .- ,. • - • ■
w . . f A»
-■-
• v;= “ C 'W*tV*sA>
W rS,>j«S
Rys. 8. Budow a mapy lokalnej - otoczenie poznane Fig.8. Local map building - known environment
170 A. Fortuna, W. Sileikis
N a rys.9 przedstaw iono g ra f mapy lokalnej caiego poznanego otoczenia.
Rys.9. G raf m apy lokalnej - otoczenie poznane Fig.9. G raph o f local map - know n environm ent
T ak w yznaczony g ra f mapy lokalnej dla otoczenia całkow icie poznanego przez ro b o ta pozw ala na syntezę m apy globalnej za pom ocą agregacji części grafu, w których w ystępuje zam knięta pętla. Ideę takiej agregacji przedstaw ia rys. 10.
Synteza m apy globalnej pozwala na znaczne uproszczenie w yznaczania trajektorii realizującej określone zadanie nawigacyjne.
D la przykładu prezentow anego powyżej analiza mapy globalnej pozw ala stw ierdzić, że R (xr,yt) e Pi
G (xf,yg) e P 4
W ynika z tego na podstawie grafu mapy globalnej trajektoria P I P 2 => P6 => P4, która da się prosto zdekom ponow ać na zadania proste, realizow ane na podstaw ie m apy lokalnej.
Algorytm naw igacji robota mobilnego ...______________________________________________ j 7]
Rys. 10. Mapa globalna i jej g raf Fig. 10. Global map and its graph
4. W n io sk i
P rzedstaw iony w pracy algorytm pozwala na zbudow anie mapy lokalnej otoczenia przez ro b o ta m obilnego, w yposażonego w dalmierz i w ykonującego zadania naw igacyjne w tym otoczeniu. O toczenie m oże być całkowicie nieznane dla robota. N a podstaw ie informacji z dalm ierza i system u nawigacji inercyjnej względem układu bazow ego robot m oże utw orzyć w łasny o b raz świata, w którym pracuje.
Synteza m apy globalnej otoczenia pozwala na znaczne uproszczenie rozw iązyw ania zadań naw igacyjnych, ale nastręcza duże kłopoty w określeniu reguł agregacji grafit m apy lokalnej.
L IT E R A T U R A
1. C ox I.J.: A utonom ous R obot Vehicles. Springer-Verlag, 1990.
2. B orkow ski A. et in.: Grid-based mapping for autonom ous mobile robot. R obotics and A utonom ous Systems, V olum e 11, Number 1, M ay 1993.
3. C raig 1.1.: Introduction to Robotics. M echanics and Control. A ddison-W essley Publ., 1986.
172 A. F ortuna, W. Sileikis
4. Szkodny T.: M anipulatory robotów przemysłowych. M odele m atem atyczne. Skrypt Pol.
ŚI. n r 1530, G liw ice 1990.
5. G reen Cordell C.: Application o f Theorem Proving to Problem Solving. In W ebber B .L., N ilsson N .J."R eadings in Artificial Intelligence", M organ Kaufmann Publishers, 1981.
6. Fikes R ., H art P., Nilsson N.: Learning and Executing G eneralized R o b o t Plans. In W ebber B .L., Nilsson N.J. "Readings in Artificial Intelligence", M organ K aufm ann Publishers, 1981.
7. G iralt G.: M obile Robot. In Brady M., Gerhardt L., Davidson H. "R obotics and Artificial Intelligence", Springer V erlag, 1984.
8. K ow alski R.: L ogika w rozwiązywaniu zadań. W N T,W arszawa 1989.
9. B oissier L.: M odélisation de lE nvironnem ent et Localisation du R obot M obile H ILA R E par Telem etrie Laser. Laboratoire d’Automatique et d'Analyse des Systèmes, T oulose-1985, Praca doktorska.
10. M ichie D .: On M achine Intelligence. Ellis H orw ood Limited, 1986.
11. Fu K .S, G onzales R .C ., Lee C.S.G.: Robotics: Control, Sensing, Vision and Intelligence.
M cG raw -H ill B o o k Com pany, 1989.
12. F ortuna A.: M etoda lokalizacji robota mobilnego w otoczeniu w zględem punktów charakterystycznych. Zeszyty N aukow e Pol. Śl. z. 115, Gliwice 1994.
13. F ortuna A.: Algorytm nawigacji robota mobilnego w przestrzeni 2D . Praca nie publikow ana, zrealizow ana w ram ach badań własnych, Gliwice 1995.
Recenzent: Prof.dr hab.inż. A ntoni W oźniak W płynęło do Redakcji do 30.06.1996 r.
A b s tr a c t
Subject o f this paper is synthesis and analysis o f navigation algorithm fo r autonom ous m obile robot, w hich could navigate in tw o dimensional space, filled w ith simply obstacles, room s, corridors etc. R obots sensor system is using range-finder to gain inform ation about environm ent. T here is presented algorithm o f building local environm ent map, w hich enable solving problem s o f navigation in this environment, searching for trajectory free from the possibility o f collisions and m ethod o f synthesis o f hierarchical environm ent map.
T h e idea o f presented algorithm is based on separating characteristic points, like walls, edges o f obstacles w hich enclose free space to navigation, from inform ation a b o u t distance to nearest elem ents o f environm ent and defining set o f convex polygons, w hich cover this free space. This set o f convex polygons and relations betw een them determ ine representation o f the environm ent and is treated as local map. The set o f polygons m ust b e disjoint and m ust cover all visible space. T h e local map, constructed this way, allow to calculate coordinates o f any point in base coordinate system and calculate trajectory composed o f consecutive and adjacent polygons.
T h e local map is constructed during execution o f tasks determ ined by planning system.
T he task decom position system determ ine position and orientation o f robot in base coordinate system. I f goal o r any point o f trajectory belongs to unknow n part o f environm ent, robot during execution this trajectory is able to recognize this space and m odernize the local map.
T he algorithm o f local map constructing and determining trajectory to goal is presented in details in point 3 o f this paper.
