Zastosowanie transformacji Hougha do tworzenia mapy i lokalizacji robota mobilnego

Transkrypt

1 Zastosowanie transformacji Hougha do tworzenia mapy i lokalizacji robota mobilnego Barbara Siemiatkowska 1 Streszczenie W niniejszej pracy przedstawiono zastosowanie transformacji Hougha w określeniu zmian położenia robota w środowisku, które nie jest znane. Robot wyposażony w laserowy czujnik odległości porusza się we wnętrzu budynku. Znajdowane są cechy charakterystyczne otoczenia - fragmenty ścian, łuki i obiekty nieregularne. Określenie przemieszczenia robota odbywa się na podstawie określenia zmian położenia pojazdu względem wykrytych obiektów charakterystycznych - znaczników, wykrywanych przy pomocy transformacji Hougha. Zaletą opisywanej metody jest możliwość realizacji w sposób wielorównoległy. W referacie przedstawione będą wyniki eksperymentów prowadzonych w środowisku typu budynek. Metoda może być także stosowana w przestrzeniach otwartych. 1. WSTEP Systemy nawigacyjne robotów mobilnych składają się z trzech podstawowych elementów: określenia położenia otaczających pojazd przeszkód, planowania bezkolizyjnej trasy oraz lokalizacji. Metody lokalizacji dzielone są na dwie podstawowe grupy. W pierwszej grupie znajdują się systemy umożliwiające określenie przemieszczenia robota między kolejnymi punktami pomiarowymi. W drugiej obliczane jest położenie pojazdu w pewnym globalnym układzie współrzędnych. Podstawową metodą określenia przemieszczeń robota jest odometria. Istotną wadą odometrii jest jednak to, że niedokładności pomiarów kumulują się w czasie. Powstanie błędów może być spowodowane wadami urządzenia np. nierównościami kół, ograniczoną rozdzielczością enkoderów lub przypadkowymi zaburzeniami np. poślizgami kół, nierównością powierzchni [1]. Układy odometryczne są wzbogacane o zewnętrzne układ lokalizacji. Najstarsze systemy polegały na badaniu przemieszczenia robota względem sztucznych znaczników aktywnych [5] lub pasywnych (obiektów o określonym kolorze i kształcie [6]). Wadą takich systemów jest to, że układ znaczników musi być znany i ich położenie nie może się zmieniać. Inną dużo bardziej elastyczną metodą jest określenie przemieszczenia na podstawie tworzonych przez robota map lokalnych. W literaturze opisywane są trzy grupy tych metod: Dopasowywanie map[2] - algorytmy umożliwiają określenie przemieszczenia robota z dużą dokładnością, ale są czasochłonne obliczeniowo. 1 Instytut Podstawowych Problemów Techniki PAN, ul. Świętokrzyska 21, Warszawa, bsiem@ippt.gov.pl

2 B. Siemiatkowska Dopasowywanie skanów - do tej grupy należą np. metody określanie kierunków głównych [8][4] - metoda może być stosowana wewnątrz budynków posiadających regularny układ ścian. Wykrywanie i dopasowywanie elementów charakterystycznych - robot znajduje w środowisku znaczniki - elementy charakterystyczne, niezmienne i odporne na przypadkowe zakłócenia [3][10]. Znaczniki są najczęściej opisywane w postaci zbioru cech. Metoda ta jest metodą uniwersalną tzn. może być stosowana zarówno w pomieszczeniach jak i w otwartej przestrzeni. Jej efektywność zależy głównie od sposobu wykrywania i dopasowywania charakterystycznych elementów otoczenia. Przedstawiona w poniższej pracy metoda lokalizacji robota należy do ostatniej z opisywanych grup. W prowadzonych eksperymentach stosowano dalmierz laserowy firmy SICK, a parametry znaczników są obliczane przy pomocy zmodyfikowanej transformacji Hougha. Opisywane w poniższym artykule badania są rozszerzeniem i uzupełnieniem metody przedstawionej w [9]. 2. DALMIERZ LASEROWY Dalmierz laserowy LMS 200 firmy SICK jest urządzeniem pierwszej klasy bezpieczeństwa umożliwiającym dokonywanie pomiarów w pomieszczeniach zamkniętych. Dostarcza ciąg odczytów postaci: (R i,φ i ), gdzie R i jest wskazaną przez laser odległością od przeszkody, a φ i kątem skanowania. Sensor wykrywa przeszkody w odległości nie przekraczającej 30 m. Statystyczny błąd pomiaru wynosi ok. 15mm dla R i < 800cm i ok. 4 cm dla R i > 800cm. LMS umożliwia wykrywanie przeszkód w sektorze 180 o z rozdzielczością kątową 0.5 lub 1. Czas dokonania skanu wynosi 26ms (dla 0.5 ) i 13ms dla rozdzielczości 1. Jeśli przyjmiemy że dalmierz laserowy znajduje się w początku układu współrzędnych, to współrzędne punktów należących do krawędzi przeszkody możemy obliczyć na podstawie równania (1). x i = R i cosφ i y i = R i sinφ i (1) R i jest wskazaną przez laser odległością od przeszkody, a φ i jest kątem skanowania. 3. SCHEMAT ALGORYTMU Algorytm określania przemieszczenia robota między kolejnymi punktami pomiarowymi składa się z następujących etapów: Dokonanie pomiarów przy pomocy dalmierza laserowego. Ciąg odległości jest zapamiętany w jednowymiarowej tablicy. Indeks tablicy w sposób jednoznaczny określa kąt skanowania. Indeksowi o wartości i odpowiada kąt skanowania i ϕ, gdzie ϕ jest rozdzielczością skanera. W prowadzonych eksperymentach przyjęto ϕ = 1.0 0

3 Zastosowanie transformacji Hougha w lokalizacji... Filtracja danych Dane pochodzące ze skanera są często zaszumiane. Błędy odczytów powstają najczęściej w sytuacji, gdy wiązka świetlna pada na granicę dwóch przeszkód. W celu zredukowania występujących szumów dane są filtrowane przy pomocy filtru medianowego. Filtr ten jest realizowany przy pomocy sieci komórkowej. Umożliwia to realizację algorytmu w sposób wielorównoległy. Na rys. 1 przedstawiono wynik filtracji danych. Linią przerywaną zaznaczono odczyty ze skanera, a linią ciągłą wynik zastosowania filtru medianowego. Rys. 1. Dane pochodzące ze skanera laserowego Wyodrębnienie obiektów Obiekty są wyodrębniane prz pomocy transformacji Hougha. Poszukujemy odcinków dłuższych niż zadany próg ( naszym wypadku 20cm). Po wyodrębnieniu długich odcinków analizujemy pozostałe punkty. Przyjmujemy, że dwa kolejne odczyty skanera (R i,φ i ) i (R i+1,φ i+1 ) wskazują na ten sam obiekt, jeśli spełniony jest warunek: R 2 i+1 + R 2 i 2 R i+1 R i cos ϕ R 2 (2) W prowadzonych eksperymentach przyjęto R = 25cm. Na rys. 2 przedstawiono wynik segmentacji danych. Na rys. 3 przedstawiono mapę utworzoną na podstawie odczytów z lasera. Klasyfikacja i określenie cech obiektu. W algorytmach określenia przemieszczenia robota na podstawie wskazań dalmierz laserowego jako cechy charakterystyczne przyjmuje się fragmenty ścian i naroża. W opisywanej w poniższej pracy metodzie zbiór cech rozszerzono o łuki i elementy nieregularne. Postępowanie to umożliwia dokonywanie lokalizacji w środowisku, w którym nie występuje regularny układ ścian.

4 B. Siemiatkowska Rys. 2. Segmentacja danych Określenie zmiany położenia robota Po określeniu parametrów i dopasowaniu znaczników wykrytych w kolejnych punktach pomiarowych, obliczane jest przesunięcie robota oraz zmiana jego orientacji. Metoda obliczania zmian położenia pojazdu na podstawie zaobserwowanych zmian położenia punktów charakterystycznych jest przedstawiona w pracach: [3][7]. 4. TRANSFORMACJA HOUGHA Transformacja Hougha jest od wielu lat stosowana w przetwarzaniu obrazów, zwykle do znajdowania najdłuższych odcinków linii występujących w obrazie rastrowym Równanie prostej można zapisać w postaci normalnej: c = x cosα + y sinα (3) gdzie α jest kątem między normalną do danej prostej a osią OX, c - jest minimalną odległością danej prostej od punktu (0,0). Jeśli przyjmiemy pewną dyskretyzację przestrzeni, to dla każdego punktu płaszczyzny (x, y) możemy wyznaczyć rodzinę prostych do których punkt należy, a więc także rodzinę par (α, c). Jeśli utworzymy tablicę dwuwymiarową i elementowi (α, c) przyporządkowujemy ilość pikseli obrazu leżących na wyznaczonej przez tę parę prostej, to element o największej wartości w sposób jednoznaczny wyznacza najdłuższy odcinek na obrazie. W przypadku, gdy dane pochodzą z lasera równanie (3) może być zastąpione równaniem:

5 Zastosowanie transformacji Hougha w lokalizacji... Rys. 3. Lokalna mapa otoczenia c = R i cos(α φ i ) (4) Z równania (4) wynika, że aby dokonać transformacji Hougha obrazu otrzymanego na podstawie wskazań dalmierza laserowego nie musimy obliczać współrzędnych (x,y), wystarczą dane o odległości od przeszkody i kącie skanowania. Dwa kolejne odczyty (R i,φ i ) i (R i+1,φ i+1 ) należą do tej samej prostej wyznaczonej przez parametry (α,c), jeśli spełnione są warunki (5)-(6). R i cos(α φ i ) R i+1 cos(α φ i+1 ) ε (5) R i cos(α φ i ) c ε (6) Wartość parametru ε jest dopuszczalnym błędem. W eksperymentach przyjęto ε 1.5cm. Analiza powstałego w wyniku transformacji Hougha histogramu umożliwia określenie typu obiektu, który jest obserwowany. W przypadku ścian występuje jedno wyraźne maksimum, w przypadku naroży występują dwa maksima odpowiadające kierunkom ścian. W przypadku obiektów nieregularnych brak jest wyraźnych maksimów. Dla zbioru odczytów współliniowych (R i,φ), należy znaleźć takie wartości parametrów (α, c), dla których błąd średniokwadratowy γ opisany równaniem (7) jest najmniejszy: γ = N i=1 (R i cos(α φ i ) c) 2 (7) gdzie N - jest ilością odczytów współliniowych. Wartości optymalne są obliczane zgodnie ze wzorem (8)

6 B. Siemiatkowska Rys. 4. Określenie zakresu parametrów α α = 1 2 atan( p q ) c = 1 N N i=1 R i cos(φ i α) p = N i=1 N j=1 R i R j sin(φ i + φ j ) + N N i=1 R2 i sin(2 φ i) q = N i=1 N j=1 R i R j cos(φ i + φ j ) + N N i=1 R2 i cos(2 φ i) (8) Naroża są reprezentowane jako punkt (x,y). Punkt ten powstaje w wyniku przecięcia się dwóch ścian. Wyniki metody opisywanej metody są przedstawione w pracy [9]. W prowadzonych obecnie badaniach nie zmieniono koncepcji przedstawionej metody, skoncentrowano się jedynie na stworzeniu algorytmu, który umożliwia przeprowadzenie obliczeń w sposób bardziej efektywny. Wprowadzono następujące modyfikacje: analiza położenia dwóch kolejnych punktów umożliwia określenie zakresu współczynników α dla których warto obliczać wartości parametrów c. Ideę metody przedstawia rys. 4. Czarne koła reprezentują dwa kolejne odczyty dalmierza laserowego. Okręgami zaznaczono obszar błędu odczytu. Zaznaczone proste wyznaczają zakres współczynników α, dla których przeprowadzana będzie transformacja Hougha. W wyniku stosowania opisanej metody następuje kilkukrotne skrócenie czasu obliczeń. Transformacja Hougha może być stosowana także do wykrywania dowolnych krzywych, które mogą być opisywane w sposób parametryczny. W przypadku punktów znajdujących się na okręgu opisanym równaniem (9) musimy wyznaczyć parametry x 0, y 0 i parametr r. (x x 0 ) 2 + (y y 0 ) 2 = r 2 (9) Zwykle stosowanie transformacji Hougha do wykrywania krzywych opisywanych więcej niż dwoma parametrami jest czasochłonne. W tej pracy przedstawiony zostanie algorytm określania parametrów obiektów, które z punktu widzenia sensora wyglądają jak fragmenty okręgu. Okrąg będzie opisywany przez następujące parametry: d - odległość punktu (x 0,y 0 ) od środka układu współrzędnych, α - kąt nachylenia prostej przechodzącej przez (x 0,y 0 ) i (0,0) do osi OX oraz r - promień okręgu. W

7 Zastosowanie transformacji Hougha w lokalizacji... Rys. 5. Parametryczny opis okręgu klasycznej metodzie dla każdego punktu (r i,φ i ) wskazanego przez laser i dla każdego α [0,180], należałoby obliczyć zbiór dopuszczalnych wartości d i r. Równanie (9) możemy zapisać w następujący sposób: (x d cosα) 2 + (y d sinα) 2 = r 2 (10) Ponieważ w opisywanej metodzie spełniony jest warunek (1), to podstawiając wartości x i y do równania (10) otrzymujemy: r 2 i + d 2 2 r i d cos(α φ i ) = r 2 (11) Jeśli zbiór odczytów (r i,ϕ i ) i=0,..,n leży na jednym okręgu to spełnione są następujące warunki: 1. Zbiór r i jest symetryczny względem k = 1+N 2 2. Kąt β = N i=1 ϕ i N jest przybliżoną wartością α. W kolejnym kroku generowane jest przybliżone równanie okręgu, przyjmujemy, że przybliżoną wartością parametru α jest parametr β. Trzy punkty (r i,φ i ),(r k,φ k ),(r j,φ j ) należą do jednego okręgu, którego środek leży na prostej wyznaczonej przez kąt α, jeśli spełniony jest warunek (12). ri 2 r2 r 2 k j = r2 k (12) r i cosφ i r k cosφ k r j cosφ j r k cosφ k Wartości parametrów d i r wyznaczane są zgodnie ze wzorem (13).

8 B. Siemiatkowska r 2 j r2 k r j cosφ j r k cosφ k d = 1 2 r 2 = ri 2 + d2 2 r i d cos(α φ i ) (13) Warunek 12 jest niestabilny numerycznie dla punktów takich, że ϕ i α ϕ k α, dlatego przyjmujemy, że i, j są indeksami odczytów skrajnych, a k indeksem odczytu środkowego. Mając obliczone przybliżone wartości d i α dla każdego punktu (r i,ϕ i ) obliczamy odległość od wyznaczonego środka okręgu. Uznajemy, że zbiór punktów leży na jednym okręgu, jeśli dla każdego z punktów wartość R nie różni się od poprzednio wyznaczonej wartości więcej niż zadany próg. Na rysunku 6a) przedstawiono odczyty z lasera, otrzymane gdy obserwowany jest okrąg o parametrach α = 45 o, d=100cm, R=20cm. Rozdzielczość skanera wynosiła 1 o. Rysunek 6b) przedstawia wyniki pomiarów w układzie biegunowym. Przybliżona wartość kąta wynosi 45 o, a parametru d= Rysunek 7 przedstawia obliczone wartości parametru R dla kolejnych punktów. Kolejny krok polega na wyznaczeniu optymalnych wartości parametrów. W tym przypadku podobnie jak poprzednio szukamy minimum funkcji f (α,d,r) opisanej wzorem 14 f (α,d,r) = N i=1 (d 2 + r 2 i 2 d r i cos(α ϕ i ) R 2 ) 2 (14) a) b) Rys. 6. Odczyty z dalmierza Wyniki przeprowadzonych eksperymentów przedstawia rys. 8. Na wykresie przedstawiono błąd określenia parametru d dla okręgów o promieniu r = 10cm, r = 20cm i r = 30cm. Okręgi były umieszczane w odległościach od 0.5m do 3.0m od

9 Zastosowanie transformacji Hougha w lokalizacji... Rys. 7. Wartości parametru R dalmierza laserowego. Błąd jest tym większy im mniejszy jest obiekt i im dalej znajduje się od robota. Parametr błąd określenia parametru r jest ściśle skorelowany z d i rozkład błędu jest zbliżony do rozkładu błędu dla d. Czas określenia parametrów nie przekroczył 10ms. Obliczenia były prowadzone na komputerze z zegarem 1.2GHz. 5. WNIOSKI W pracy przedstawiono implementację transformacji Hougha umożliwiającą wykrywanie fragmentów ścian i łuków. Metoda umożliwia określenie zmian orientacji i położenia robota wyposażonego w dalmierz laserowy. Przyjęto założenie, że robot porusza się w otoczeniu typu wnętrze. Mapa pomieszczenia nie jest znana. Algorytm jest bardzo efektywny. Opisywana metoda może być stosowana także do określania położenia robota w znanym pomieszczeniu, a także może ułatwiać tworzenie mapy metrycznej otoczenia, które nie jest znane. Czas działania algorytmu nie przekracza 20ms. Wyniki prezentowane w tej pracy zostay zrealizowane w ramach grantu MEiN 3 T11C LITERATURA [1] J. Borenstein, L. Feng. Measurement and Correction of Systematic Odometry Errors in Mobile Robots. Trans. on Robotics and Automation, Vol. 12, No.6,

10 B. Siemiatkowska Rys. 8. Błąd określenia parametru d [2] F. Dellaert, D. Fox, W. Burgard, and S. Thrun. Monte Carlo localization for mobile robots. In IEEE Int. Conf. on Robotics and Automation. s , [3] G. Dissanayake, H. Durrant-Whyte, and T. Bailey. A computationally effcient solution to the simultaneous localisation and map building (SLAM) problem. In: IEEE Int. Conf. on Robotics and Automation. Proceedings. Vol. 2, 2000, s [4] A. Dubrawski, B. Siemiątkowska. A Neural Method for Self-Localization of a Mobile Robot Equipped with a 2-D Scanning Laser Range Finder, In: IEEE Conf. on Robotics and Automation, Leuven. Proceedings. 1998, s [5] G. Giralt, R. Sobek, and R. Chatila. A multi-level planning and navigation system for a mobile robot. A first approach to Hilare. In: Sixth Int. Joint Conf. on Robotics and Automation [6] I. Hallmann, B. Siemiątkowska. Artificial landmark navigation system. In: 9th Int. Symposium Intelligent Robotic Systems. Proceedings. 2001, s [7] E. Mennegatti, M. Zoccarato, E. Pagell, H. Ishiguro. Hierarchical image-based localization for mobile robot with Monte-Carlo Localisation. In: European Conf. on Mobile Robotics Proceedings. 1995, s [8] B. Siemiatkowska, R. Chojecki. Mobile Robot Localization Based on Omnicamera 5th IFAC/EURON Symposium on Intelligent Autonomous Vehicles. Elsevier Proceedings, [9] B. Siemiątkowska, A. Dubrawski. Cellular Neural Networks for a Mobile Robot, Rough Sets and Current Trends in Computing, Springer. s , [10] A. Stevens, M. Stevens, and H.F. Durrant-Whyte. OxNav: Reliable autonomous navigation. In: IEEE Int. Conf. on Robotics and Automation. 1995, s HOUGH TRANSFORM FOR MAP BUILDING AND LOCALIZATION OF A MOBILE ROBOT

11 Zastosowanie transformacji Hougha w lokalizacji... The paper presents the application of Hough transform for mobile robot localization. It is assumed that the robot acts in an unknown environment. It is equipped with the SICK laser range finder. The robot looks for segments, edges and circles, which are features of the environment. Comparing the robot position relatively to the landmarks the displacement of the vehicle is computed. In this paper a certain modification of Hough transform is proposed in order to detect features and to compute their parameters. Experiments show the efficiency of the proposed approach.