Dla moich Rodziców oraz najwspanialszej ze wszystkich mojej Żony

Transkrypt

1 POLITECHNIKA WARSZAWSKA Wydział Elektroniki i Technik Informacyjnych Instytut Automatyki i Informatyki Stosowanej Zespół Sterowania i Programowania Robotów Marek Majchrowski Algorytm unikania kolizji przez robota mobilnego bazujący na przeszukiwaniu przestrzeni prędkości Obstacle avoidance method by mobile robot based on velocity space Praca magisterska wykonana pod kierunkiem dra Wojciecha Szynkiewicza Warszawa 2006

2

3 Dla moich Rodziców oraz najwspanialszej ze wszystkich mojej Żony

4

5 Pragnę podziękować: mojemu promotorowi dr Wojciechowi Szynkiewiczowi za opiekę, wyrozumiałość i miła współpracę, mgr Tomaszowi Winiarskiemu za zainteresowanie mnie robotyka, oraz Piotrowi Trojankowi, koledze i przyjacielowi w boju, za cenne wskazówki i nieoceniona pomoc.

6

7 Streszczenie Głównym celem tej pracy było zapoznanie się z najistotniejszymi i zarazem podstawowymi rozwiązaniami problemu planowaniu ruchu robotów mobilnych. Szczególny nacisk położony został na algorytmy unikania kolizji, jako że są one niezbędne do poruszania się robota w nieznanym lub częściowo nieznanym bądź dynamicznym otoczeniu. Szczegółowo omówione zostały dwa algorytmy: metoda tzw. krzywizn i prędkości (Curvature Velocity Method (CVM)) należąca do grupy podejść bazujących na wyznaczaniu prędkości, oraz algorytm z grupy tzw. histogramów pola wektorowego Vector Field Histogram Plus (VFH+) należący do metod planowania ruchu bazujących na wyznaczaniu kierunku. Działanie obu algorytmów zostało porównane dla robotów nieholonomicznych, w szczególności dla robotów o napędzie różnicowym. Opisany został także sieciowy serwer Player, służący do sterowania robotów mobilnych, oraz sterownik, który powstał do sterowania robotem mobilnym Protonek z wykorzystaniem ww systemu. Przedstawiono problematykę akwizycji danych pomiarowych z czujników oraz ich sposób zbierania w systemie Player. Opisany został sterownik, który powstał do akwizycji danych z czujników IR z wykorzystaniem ww systemu. Algorytm unikania kolizji i

8 STRESZCZENIE Marek Majchrowski ii Algorytm unikania kolizji

9 Abstract This thesis is concerned with the development and implementation of local obstacle avoidance algorithms. These algorithms are essential for the movement of the robot in unkown or partly-unknown environment (workspace). Two algorithms have been described, namely: Curvature Velocity Method (CVM) stands for the group of trials based on indicating the velocity. This method for local obstacle avoidance by indoor mobile robots formulates the problem as one of constrained optimization in velocity space. Constraints that stem from physical limitations (velocities and accelerations) and the environment (the configuration of obstacles) are placed on the translational and rotational velocities of the robot. Vector Field Histogram Plus (VFH+) which stands for the methods of planning the movement based on indicating the direction. This method permits the detection of unknown obstacles and avoids collisions while simultaneously steering the mobile robot toward the target. Both algorithms were tested and compared on the non-holonomic robots, especially those with the differetial drive. Secondly, the network server Player has been described, which is used for controlling the mobile robots. Thirdly, the driver that was made to contol the mobile robot Protonek by using the above mentioned system. Finally, the issue of acquisition the measuring data of the sensors and their way of collecting in the Player system have been presented. The driver for the data acquisition from the IR sensors which used the above mentioned system has been described. Algorytm unikania kolizji iii

10 ABSTRACT Marek Majchrowski iv Algorytm unikania kolizji

11 Spis treści Streszczenie Abstract i iii 1 Wstęp Cel pracy Zakres pracy Zawartość pracy Metody i algorytmy unikania kolizji Planowanie ścieżki ruchu Globalne metody planowania ruchu Mapy drogowe (Road map) Podział na komórki dekompozycja komórkowa (cell decomposition) Sztuczne pola potencjałowe (artificial potential field) Lokalne metody planowania ruchu Algorytm BUG VFH oraz jego odmiany Dynamiczne okna Diagramy sąsiedztwa Algorytm CVM Algorytm krzywizn i prędkości CVM 17 4 Algorytm VFH oraz jego odmiany VFH Tworzenie siatki reprezentującej dwuwymiarowy histogram Tworzenie histogramu biegunowego Wyznaczanie kierunku ruchu VFH Opis stanowiska badawczego Robot mobilny Protonek Trójwymiarowy symulator Gazebo v

12 SPIS TREŚCI Marek Majchrowski 5.3 Oprogramowanie robotów mobilnych Player Cechy Playera Ogólna struktura sterowania z wykorzystaniem Playera Interfejsy Plik konfiguracyjny Sterowniki Sterownik Protonek Dostęp do urządzeń Format przesyłanych danych Programy klienckie Łaty Akwizycja danych pomiarowych Czujniki podczerwieni IR Odometria Szczegółowy opis algorytmu Szczegółowy opis algorytmu Równanie kwadratowe względem zmiennej x, czyli y 0 > x Równanie kwadratowe względem zmiennej y, czyli y 0 x Szczegółowy opis działania programu Wyniki eksperymentalne Wyniki symulacyjne Wyniki eksperymentów z rzeczywistym robotem Wyniki algorytmu CVM przy wykorzystaniu czujników IR dalekiego zasięgu Wyniki algorytmu CVM przy wykorzystaniu dalmierza laserowego Wnioski Podsumowanie 93 Dodatek A 94 Dodatek B 97 vi Algorytm unikania kolizji

13 Spis rysunków 2.1 Przykładowy graf widoczności [24] Przykładowy diagram Woronoia [12] Przykładowa dokładna dekompozycja komórkowa przy podziale trapezoidalnym, gdzie CB i to przeszkody [23] Przykładowy przybliżony podział na komórki o stałej szerokości [23] Przykładowy przybliżony podział na komórki o zmiennej szerokości [21] Sztuczne pole potencjałowe Przykładowy potencjał przyciągający Przykładowy potencjał odpychający Przykład działania algorytmu BUG1 [12] Przykład działania algorytmu BUG2 [12] Wyznaczanie odległości od przeszkody p dla okręgu o krzywiźnie c i Tworzenie siatki, która opisuje dwuwymiarowy histogram Wygładzony histogram biegunowy dla przeszkód A, B oraz C Wygładzony histogram biegunowy dla przeszkód A, B oraz C Robot mobilny Protonek Struktura sterownika robota mobilnego Protonek Model robota mobilnego Protonek w symulatorze 3D wraz z głowicą do skanowania trójwymiarowego oraz czujnikami IR Architektura Playera [27] Czujniki zbliżeniowe na podczerwień: a) GP2D120, b) GP2Y0A Pomiar odległości metodą triangulacji [26] Napięcie w funkcji odległości dla czujnika GP2D120 [26] Odległość w funkcji odczytów z przetwornika A/D dla czujnika GP2D Napięcie w funkcji odległości dla czujnika GP2Y0A02 [26] Odległość w funkcji odczytów z przetwornika A/D dla czujnika GP2Y0A Dwukołowy robot mobilny o napędzie różnicowym podstawowe oznaczenia Orientacja robota wyznaczona na podstawie odometrii w trakcie jazdy po korytarzu Orientacja robota wyznaczona na podstawie odometrii w trakcie omijania przeszkody vii

14 SPIS RYSUNKÓW Marek Majchrowski 5.14 Orientacja oraz droga przebyta przez robota wyznaczona na podstawie odometrii oraz algorytmu LODO w trakcie jazdy po okręgu Orientacja oraz droga przebyta przez robota wyznaczona na podstawie odometrii oraz algorytmu LODO w trakcie jazdy po ósemce Krzywe styczne do przeszkody p w punktach P min i P max Przeszkody i graficzna interpretacji listy przedziałów, które z nich powstały Zbiór krzywych z zakresu [c min, c max ] o różnych długościach o danej przeszkody p Sposób podziału okręgu opisującego przeszkodę na k = 8 części Zależność promienia przeszkody od odległości do tej przeszkody dla increase_radius_ f actor = Czarne okręgi oznaczają przeszkody zbędne, czerwone przeszkody, które zostaną zapamiętane Wizualizacja wiązki IR czujników podczerwieni w robocie Protonek Omijanie pojedynczej przeszkody w środowisku symulacyjnym Wykres położenia robota w trakcie omijania pojedynczej przeszkody w środowisku symulacyjnym Przebiegi prędkości robota w trakcie omijania pojedynczej przeszkody w środowisku symulacyjnym Wykres orientacji robota w trakcie omijania pojedynczej przeszkody w środowisku symulacyjnym Omijanie trzech przeszkód w środowisku symulacyjnym Wykres położenia robota w trakcie omijania trzech przeszkód w środowisku symulacyjnym Przebiegi prędkości robota w trakcie omijania trzech przeszkód w środowisku symulacyjnym Wykres orientacji robota w trakcie omijania trzech przeszkód w środowisku symulacyjnym Omijanie wielu przeszkód w środowisku symulacyjnym Wykres położenia robota w trakcie omijania wielu przeszkód w środowisku symulacyjnym Przebiegi prędkości robota w trakcie omijania wielu przeszkód w środowisku symulacyjnym Wykres orientacji robota w trakcie omijania wielu przeszkód w środowisku symulacyjnym Omijanie ściany w środowisku symulacyjnym Wykres położenia robota w trakcie omijania ściany w środowisku symulacyjnym Przebiegi prędkości robota w trakcie omijania ściany w środowisku symulacyjnym Wykres orientacji robota w trakcie omijania ściany w środowisku symulacyjnym Omijanie dwóch ścian w środowisku symulacyjnym viii Algorytm unikania kolizji

15 Marek Majchrowski SPIS RYSUNKÓW 7.18 Wykres położenia robota w trakcie omijania dwóch ścian w środowisku symulacyjnym Przebiegi prędkości robota w trakcie omijania dwóch ścian w środowisku symulacyjnym Wykres orientacji robota w trakcie omijania dwóch ścian w środowisku symulacyjnym Omijanie trzech ścian w środowisku symulacyjnym Wykres położenia robota w trakcie omijania trzech ścian w środowisku symulacyjnym Przebiegi prędkości robota w trakcie omijania trzech ścian w środowisku symulacyjnym Wykres orientacji robota w trakcie omijania trzech ścian w środowisku symulacyjnym Omijanie trzech przeszkód przez rzeczywistego robota Wykres położenia robota w trakcie omijania trzech przeszkód przez rzeczywistego robota Wykres orientacji robota w trakcie omijania trzech przeszkód przez rzeczywistego robota Omijanie dwóch przeszkód przez rzeczywistego robota Wykres położenia robota w trakcie omijania dwóch przeszkód przez rzeczywistego robota Przebiegi prędkości robota w trakcie omijania dwóch przeszkód przez rzeczywistego robota Wykres orientacji robota w trakcie omijania dwóch przeszkód przez rzeczywistego robota Omijanie ławki przez rzeczywistego robota Wykres położenia robota w trakcie omijania ławki przez rzeczywistego robota Przebiegi prędkości robota w trakcie omijania ławki przez rzeczywistego robota Wykres orientacji robota w trakcie omijania ławki przez rzeczywistego robota Omijanie wielu przeszkód w wąskim korytarzu przez rzeczywistego robota z ograniczoną prędkością liniową Wykres położenia robota w trakcie omijania wielu przeszkód w wąskim korytarzu przez rzeczywistego robota z ograniczoną prędkością liniową Przebiegi prędkości robota w trakcie omijania wielu przeszkód w wąskim korytarzu przez rzeczywistego robota z ograniczoną prędkością liniową Wykres orientacji robota w trakcie omijania wielu przeszkód w wąskim korytarzu przez rzeczywistego robota z ograniczoną prędkością liniową Omijanie wielu przeszkód w wąskim korytarzu przez rzeczywistego robota z maksymalną prędkością liniową Wykres położenia robota w trakcie omijania wielu przeszkód w wąskim korytarzu przez rzeczywistego robota z maksymalną prędkością liniową Algorytm unikania kolizji ix

16 SPIS RYSUNKÓW Marek Majchrowski 7.42 Przebiegi prędkości robota w trakcie omijania wielu przeszkód w wąskim korytarzu przez rzeczywistego robota z maksymalną prędkością liniową Wykres orientacji robota w trakcie omijania wielu przeszkód w wąskim korytarzu przez rzeczywistego robota z maksymalną prędkością liniową x Algorytm unikania kolizji

17 Rozdział 1 Wstęp Ogólnie, problem planowania ruchu to odpowiedź na pytanie: Dokad idę i jak mam się tam dostać?. Natomiast dokładniej ujmując, polega on na obliczeniu oddziaływań przeprowadzających układ od konfiguracji początkowej do zadanej konfiguracji końcowej. Obecnie typowe roboty przemysłowe wykonują swoje zadania wchodząc tylko w z góry zaplanowane interakcje z otoczeniem, ponieważ środowisko, w którym pracują jest statyczne i strukturalne (tzn. uporządkowane). Natomiast w przypadku robotów mobilnych do interakcji z otoczeniem, może dojść w sposób przypadkowy, niezaplanowany. Wynika to z charakteru środowiska, w którym roboty się poruszają. W przeciwieństwie do robotów przemysłowych, ich środowisko jest zazwyczaj dynamiczne i często jest nieuporządkowane bądź częściowo uporządkowane. Planowanie ruchu jest zdolnością do decydowania jakie działanie należy podjąć w konkretnej sytuacji w celu osiągnięcia konfiguracji końcowej. Jest to także rozstrzygnięcie wielu decyzji, począwszy od: która ścieżkę wybrać a skończywszy na: których informacji o środowisku użyć w danej chwili. Planowanie w sytuacji, gdy posiadamy pełną informacji na temat aktualnego stanu środowiska zostało już szeroko zbadane. Jednakże w przypadku robotów mobilnych wiedza na temat otoczenia jest zazwyczaj częściowa i niepewna. W ten sposób problem podejmowania poprawnych decyzji staje się znacznie trudniejszy i wymaga realizacji równolegle wielu zadań (jedne w celu planowania ruchu, inne w celu przetrwania robota omijania przeszkód). Wykrywanie lokalnych przeszkód i unikanie z nimi kolizji jest jedną z podstawowych funkcji sterowników robotów mobilnych działających w nieznanym lub częściowo nieznanym bądź dynamicznym otoczeniu. W algorytmach unikania kolizji wykorzystuje się dane pomiarowe z czujników, a także docelową pozycję robota oraz jego aktualną pozycję względem docelowej, do lokalnej modyfikacji trajektorii ruchu robota. Wymaga się, aby algorytmy były efektywne obliczeniowo, a generowany ruch był możliwie gładki oraz by robot podążał w kierunku celu przy uwzględnieniu fizycznych ograniczeń robota [1, 12]. Algorytm unikania kolizji 1

18 ROZDZIAŁ 1. WSTEP Marek Majchrowski 1.1 Cel pracy Głównym celem tej pracy było opracowanie, implementacja i eksperymentalna weryfikacja algorytmu unikania kolizji wykorzystującego tzw. metodę krzywizn i prędkości (Curvature Velocity Method (CVM)). Ze względu na wybór pakietu Player/Stage jako podstawowego środowiska programowania robotów należało zapoznać się z jego budową i funkcjonalnością. Następnie zaadaptować to środowisko do konkretnego sprzętu i napisać sterownik urządzeń, w tym sterownika robota mobilnego Protonek i czujników zbliżeniowych na podczerwień (IR). Należało także opracować metodę akwizycji i przetwarzania danych z czujników w systemie Player. W tym celu konieczne było napisanie sterownika dla systemu Player zbierającego i przetwarzającego dane z czujników IR. Realizacja podstawowego celu pracy wymagała przeglądu stanu wiedzy dotyczącej zagadnień wykrywania i unikania kolizji. 1.2 Zakres pracy Zakres niniejszej pracy obejmował następujące czynności: 1. Opracowanie algorytmu CVM, 2. Implementacja algorytmu CVM w środowisku Player/Stage, 3. Przygotowanie stanowiska badawczego, czyli sprzęt i jego oprogramowanie, umożliwiającego implementację i testowanie algorytmów unikania kolizji, 4. Przeprowadzenie badań eksperymentalnych działania algorytmów CVM i VFH+. 5. Porównanie wyników działania tych algorytmów dla wybranych środowisk. 1.3 Zawartość pracy W rozdziale drugim wyszczególniono najczęściej spotykane w literaturze rozwiązania typowych problemów związanych z tematem pracy. Liczba takich opracowań jest bardzo duża i są one zróżnicowane, dlatego bliżej przedstawiono jedynie typowe z nich. W rozdziałach trzecim i czwartym przedstawiono odpowiednio algorytm krzywych i prędkości CVM oraz metodę VFH wraz z jej modyfikacjami. Rozdział piąty poświecony jest szczegółowemu opisowi stanowiska badawczego, czyli serwera urządzeń Player i budowie robota mobilnego Protonek. Został w nim omówiony proces integracji sensorów i efektorów z oprogramowaniem sterującym. W rozdziale szóstym znajduje się szczegółowy opis algorytmu CVM wraz z programem. Ostatni rozdział zawiera wyniki i podsumowanie eksperymentów przeprowadzonych w środowisku symulacyjnym i na rzeczywistym robocie. 2 Algorytm unikania kolizji

19 Rozdział 2 Metody i algorytmy unikania kolizji Większość znanych z literatury algorytmów unikania kolizji można zaliczyć do jednej z dwóch podstawowych grup: wyznaczania kierunku ruchu [2, 6, 16, 17] oraz wyznaczania prędkości liniowej i kątowej robota w wyniku przeszukiwania odpowiednio dobranej przestrzeni prędkości [3, 5, 11, 13]. Jednym z wcześniejszych algorytmów, zaliczanych do pierwszej grupy, jest podejście wykorzystujące sztuczne pola potencjałowe [6]. Robot, traktowany jako cząsteczka, porusza się w polu, zaś jego ruch jest wypadkową działających na niego sił pochodzących od przeszkód (siły odpychające) oraz od celu (siła przyciągająca). W swej pierwotnej wersji metoda pól potecjałowych miała szereg wad, wśród których najpoważniejszą było występowanie minimów lokalnych pola, czyli punktów różnych od docelowego, w których siła wypadkowa jest równa zeru [2, 4]. Wśród metod wyznaczania kierunku największą popularność zdobyły kolejne wersje algorytmów z grupy tzw. histogramów pola wektorowego Vector Field Histogram (VFH) [2], VFH+ [16], oraz VHF* [17]. Zbliżone do VFH podejście, zaprezentowane w pracy [9], wykorzystuje diagramy bliskości (Nearness Diagram (ND)). Dzięki uwzględnieniu geometrycznych, kinematycznych i dynamicznych ograniczeń dla robota, algorytm ten daje lepsze rezultaty niż VFH+, szczególnie w środowiskach z dużą liczbą przeszkód i wąskimi przejściami. Techniki wykorzystujące pojęcie okna dynamicznego (Dynamic Window) są przykładem podejścia polegającego na przeszukiwaniu pewnego zbioru prędkości (okna) w celu znalezienia sterowań (prędkości) dopuszczalnych przy uwzględnieniu ograniczeń wynikających z kinematyki robota i uproszczonego modelu dynamiki [3, 5, 11]. Przestrzeń prędkości jest zbiorem wszystkich par (ω, v), składających z prędkości kątowych ω i liniowych v. W podejściu tym zakłada się, że robot może poruszać się tylko po łukach okręgów reprezentowanych przez parę (ω,v). Do grupy podejść bazujących na wyznaczaniu prędkości zalicza się także metodę tzw. krzywizn i prędkości (Curvature Velocity Method (CVM)) [13]. Zakłada się tutaj, podobnie jak w technikach okna dynamicznego, że robot porusza się tylko po łukach okręgów o krzywiznach c = ω v. W podejściu tym możliwe jest uwzględnienie ograniczeń na dopuszczalne wartości prędkości i przyspieszenia. Algorytm unikania kolizji 3

20 ROZDZIAŁ 2. METODY I ALGORYTMY UNIKANIA KOLIZJI Marek Majchrowski Algorytm przedstawiony w niniejszej pracy zalicza się do tej grupy i stanowi rozwinięcie idei zaproponowanej w [13]. W niniejszej pracy starano się wyeliminować niedociągnięcia i błędy oryginalnej wersji tego algorytmu. 2.1 Planowanie ścieżki ruchu Podstawowy problem planowania ścieżki polega na poszukiwaniu krzywej geometrycznej przejścia pomiędzy początkową i zadaną końcową pozycją robota tak, aby nie występowały kolizje z przeszkodami oraz planowany ruch nie powodował naruszenia ograniczeń kinematycznych i dynamicznych robota (ścieżka była możliwa do wykonania). Ze względu na możliwości ruchowe wyróżnia się dwa podstawowe rodzaje robotów mobilnych [14]: roboty holonomiczne ograniczenia prędkościowe (fazowe) można sprowadzić do ograniczeń konfiguracyjnych. Bardzo często robot jest modelowany jako punkt materialny poruszający się w przestrzeni o przeszkodach powiększonych o promień okręgu opisanego na robocie. Planowanie ruchu robota holonomicznego można podzielić na trzy etapy: wyznaczenie geometrycznej ścieżki ruchu między stanem początkowym i docelowym, planowania trajektorii polegające na parametryzacji czasowej ścieżki geometrycznej czyli określeniu prędkości ruchu wzdłuż ścieżki, śledzenie zdanej trajektorii wykonywane przez układ sterowania. roboty nieholonomiczne ograniczeń prędkościowych nie można usunąć, tzn. charakterystyk pozycyjnych nie można rozpatrywać oddzielnie od prędkościowych. Dwa pierwsze etapy planowania muszą odbywać się jednocześnie. Robot jest modelowany jako układ sterowania. Sterowanie robotami nieholonomicznymi jest znacznie trudniejsze niż holonomicznymi, ze względu na mniejszą liczbę sterowań niż stopni swobody robota. Metody planowania ruchu robotów mobilnych są specyficzne dla rodzaju robota. W przypadku rzeczywistych robotów mobilnych najczęstszym podejściem do problemu planowania ścieżki jest przyjęcie, że robot, z którym mamy do czynienia jest holonomiczny. Dzieje się tak szczególnie często dla robotów z napędem różnicowym, ponieważ mogą one obracać się prawie w miejscu, zatem holonomiczna ścieżka łatwo może zostać naśladowana, jeśli orientacja robota nie jest krytyczna. Dodatkowo, jak już wspomniano, przyjmuje się, że robot jest modelowany jako punkt materialny poruszający się w przestrzeni o przeszkodach powiększonych o promień okręgu opisanego na robocie. Generalnie metody planowania ścieżki możemy podzielić na [14]: 4 Algorytm unikania kolizji

21 Marek Majchrowski ROZDZIAŁ 2. METODY I ALGORYTMY UNIKANIA KOLIZJI globalne w których kompletna ścieżka jest obliczana w procesie iteracyjnym. Działanie metod globalnych polega zazwyczaj na minimalizacji pewnej funkcji błędu. Wykorzystuje się w nich wyidealizowany model robota i jego otoczenia (zakłada się znajomość rozkładu wszystkich przeszkód w środowisku robota). Cechuje je duża złożoność obliczeniowa, mała odporność na zmianę warunków początkowych (np. pojawienie się ruchomych przeszkód). Metody globalne ze względu na dużą obliczeniochłonność są metodami off-line (tzw. wstępnego planowania ruchu), czyli cała ścieżka jest zaplanowana, zanim zostanie przesłana do wykonania przez układ sterowania. lokalne unikanie przeszkód, podążanie do celu, zakładają dostępność informacji o przeszkodach w najbliższym otoczeniu robota. Metody lokalne są zazwyczaj metodami on-line czyli planowanie odbywa się w czasie rzeczywistym, ścieżka tworzona jest przyrostowo. Charakteryzuje je szybkość działania, stosunkowo duża odporność na zmiany otoczenia (dynamiczne środowisko); Algorytm jest zupełny, gdy znajdzie rozwiązanie, jeśli ono tylko istnieje. Istnieją słabsze postacie zupełności: zupełność przy zadanej rozdzielczości (dyskretyzacji przestrzeni poszukiwań) i zupełność probabilistyczna (prawdopodobieństwo znalezienia rozwiązania dąży do 1, gdy czas działania algorytmu dąży do nieskończoność. Ze względu na zupełność algorytmów planowania wyróżniamy: podejścia dokładne (deterministyczne) zupełne; podejścia przybliżone zupełność przy zadanej rozdzielczości; podejścia losowe zupełność probabilistyczna; podejścia heurystyczne niezupełne. 2.2 Globalne metody planowania ruchu Środowisko, w którym poruszają się roboty, może być modelowane w postaci map m. in. geometrycznych bądź topologicznych. Pierwszym krokiem jakiegokolwiek algorytmu planowania ruchu jest odwzorowanie tego środowiska w mapę odpowiednią dla wybranego algorytmu. Planery ruchu różnią się pod względem tworzenia tej mapy. Można rozróżnić trzy główne strategie tworzenia map [12]: 1. mapy drogowe (z ang. road map) w efekcie otrzymujemy opis otoczenia w postaci grafu określającego sieć możliwych ścieżek, czyli mamy dyskretną przestrzeń stanu 2. dekompozycja (podział) komórkowy (z ang. cell decomposition) dzielimy przestrzeń roboczą na komórki, otrzymujemy dyskretną przestrzeń stanu 3. sztuczne pola potencjałowe (z ang. artificial potential field) poprzez określenie funkcji matematycznej nad przestrzenią roboczą, otrzymujemy ciągłą przestrzeń stanu Algorytm unikania kolizji 5

22 ROZDZIAŁ 2. METODY I ALGORYTMY UNIKANIA KOLIZJI Marek Majchrowski Mapy drogowe (Road map) W metodzie tej łączone są ze sobą wolne obszary wokół robota za pomocą krzywych bądź prostych, zwanych drogami. W efekcie otrzymujemy opis otoczenia w postaci grafu określającego sieć możliwych ścieżek. W momencie, gdy ten graf zostanie skonstruowany, zostanie wykorzystany jako sieć dróg (ścieżek) pomiędzy obszarami, w których robot może się poruszać. W ten sposób planowanie ruchu robota mobilnego ogranicza się do dołączenia punktu początkowego i końcowego do powstałej wcześniej sieci dróg, a następnie znalezienie odpowiedniej ścieżki pomiędzy wspomnianymi punktami. Opis przestrzeni roboczej robota za pomocą map drogowych zależy od geometrii przeszkód. Trudnością (lub wyzwaniem) w tym podejściu jest taka konstrukcja zbioru dróg, aby robot poruszając się po nich mógł dostać się do któregokolwiek miejsca w jego wolnej przestrzeni roboczej, minimalizując przy tym liczbę wszystkich dróg w całym zbiorze. Poniżej przedstawiono dwa algorytmy bazujące na metodzie map drogowych, tj. graf widoczności oraz diagram Woronoia. Graf widoczności Graf widoczności dla przestrzeni konfiguracyjnej C zawiera krawędzie powstałe z połączenia wszystkich par wierzchołków nawzajem widocznych (włączając w to punkt startowy jak i docelowy jako wierzchołki). Linie proste (drogi) łączące te węzły stanowią najkrótsze odległości między nimi. Zadaniem planera jest znalezienie najkrótszej ścieżki przejścia z punktu początkowego do docelowego wzdłuż wyznaczonych dróg za pomocą grafu widoczności (rysunek 2.1). Algorytm ten jest powszechnie stosowany, ponieważ jego implementacja jest niezwykle prosta. Wystarczy aby reprezentacja środowiska opisywała wszystkie obiekty w nim znajdujące się jako wieloboki (wielościany). Jednakże algorytm ten ma dwie poważne wady: Rysunek 2.1: Przykładowy graf widoczności [24] 1. Liczba krawędzi i węzłów rośnie wraz ze wzrostem liczby wieloboków (wielościanów) opisujących przeszkody. Zatem metoda jest bardzo szybka i wydajna w rzadkich (z nie- 6 Algorytm unikania kolizji

23 Marek Majchrowski ROZDZIAŁ 2. METODY I ALGORYTMY UNIKANIA KOLIZJI wielką liczbą przeszkód) środowiskach, jednak może być bardzo wolna i niewydajna, w porównaniu do innych metod, w środowisku o dużej liczbie przeszkód. 2. Znaleziona ścieżka ruchu może stykać się z przeszkodami (zwykle w wierzchołkach lecz niekiedy także wzdłuż krawędzi). Można temu zapobiec przez poszerzenie przeszkód o promień okręgu opisanego na robocie. Diagram Woronoia Porównując z grafem widoczności, metoda diagramu Woronoia służy do planowanie skrajnie bezpiecznych ścieżek. W podejściu tym jest maksymalizowana odległość pomiędzy robotem a przeszkodami na mapie. Na mapie środowiska nanosi się krzywe równoodległe od przeszkód (rysunek 2.2). Kształt krzywych zależy od metryki przyjętej do wyznaczania odległości. Na przecięciach krzywych są węzły tworzonego grafu. Dodatkowe węzły to punkt początkowy i docelowy oraz dwa węzły w grafie położone w miejscach najbliższych od wierzchołka początkowego i docelowego. Łukom grafu są przypisywane wagi równe długościom krzywej między węzłami (odległość ta jest zazwyczaj większa od odległości euklidesowej między węzłami). Jeśli przeszkody w przestrzeni konfiguracyjnej są wielobokami, to diagram Woronoia składa się jedynie z odcinków prostych (metryka L1) lub odcinków prostych i fragmentów parabol (metryka L2). Algorytm, który szuka najkrótszej ścieżki w diagramie Woronoia jest taki sam jak w przypadku grafu widoczności, ponieważ istnienie ścieżki w wolnym obszarze (graf widoczności) implikuje istnienie takiej na diagramie Woronoia (obie metody są zupełne). Jednakże ścieżka wyznaczona za pomocą tego algorytmu jest daleka od optymalnej (jeśli kryterium stanowi długość ścieżki). Rysunek 2.2: Przykładowy diagram Woronoia [12] Algorytm ten ma istotną słabość w przypadku robotów, w których stosuje się tylko czujniki odległości krótkiego zasięgu. Ponieważ algorytm ten maksymalizuje dystans pomiędzy robotem a obiektami, każdy czujnik krótkiego zasięgu na tym robocie może pracować na granicy Algorytm unikania kolizji 7

24 ROZDZIAŁ 2. METODY I ALGORYTMY UNIKANIA KOLIZJI Marek Majchrowski poprawności odczytów. W takim przypadku może wystąpić trudność w wyznaczeniu poprawnej ścieżki ruchu, ponieważ przeszkody mogą znajdować się poza zasięgiem tych czujników i niemożliwa będzie maksymalizacja odległości od nich (przeszkód) Podział na komórki dekompozycja komórkowa (cell decomposition) Idea kryjąca się za dekompozycja komórkowa to podział obszaru na komórki wolne i zajęte przez przeszkody. Podstawowy algorytm planowaniu ruchu opierający się na dekompozycji komórek można opisać w następujący sposób: Podziel obszar na proste, spójne obszary zwane komórkami. Określ, które puste komórki są sąsiadujące i skonstruuj graf spójności między nimi. Znajdź komórki zawierające punkt startowy oraz docelowy, a następnie znajdź połączenie między nimi w grafie spójności. Wyznacz ścieżkę przechodzącą przez każdą komórkę w znalezionym połączeniu (np. przechodzącą przez środek każdej komórki). W metodzie tej ważne jest położenie krawędzi pomiędzy komórkami. Jeżeli granice te wynikają ze struktury środowiska, wtedy dekompozycja ta jest bezstratna i określana mianem dokładnego podziału na komórki (z ang. exact cell decomposition). Jeśli jednak granice te nie zależą od struktury środowiska, metodę tę nazywa się przybliżonym podziałem na komórki (z ang. approximate cell decomposition). Dokładny podział na komórki (exact cell decomposition) Rysunek 2.3 przedstawia dokładny podział na komórki, w przypadku gdy granice zostały wyznaczone za pomocą trapezoidalnego podziału. W podziale tym trzeba wyznaczyć nie przecinające się odcinki przez poprowadzenie przez wszystkie wierzchołki przeszkód pionowych linii prostych. Linie prowadzone są do pierwszej przeszkody. Na powstałych odcinkach (oczywiście trzeba wyeliminować wszystkie te, które znajdują się w przestrzeni zabronionej) zaznaczamy punkty dzielące je na dwie równe części. Wyznaczamy również środki powstałych w ten sposób trapezów. Środki odcinków i środki trapezów będą punktami składowymi powstałej mapy, na której rozpina się graf (w przestrzeni dozwolonej) zawierający bezkolizyjną ścieżkę robota. Do wyznaczenia granic pomiędzy komórkami można zastosować także inne metody. Idea: podziel obszar F na zbiór K rozdzielnych komórek, których suma (w sensie mnogościowym) dokładnie odwzoruje obszar F. Własności komórek: Geometria komórek powinna być na tyle prosta, aby w łatwy sposób można było wyznaczyć ścieżkę pomiędzy dwoma dowolnymi komórkami. 8 Algorytm unikania kolizji

25 Marek Majchrowski ROZDZIAŁ 2. METODY I ALGORYTMY UNIKANIA KOLIZJI Rysunek 2.3: Przykładowa dokładna dekompozycja komórkowa przy podziale trapezoidalnym, gdzie CB i to przeszkody [23] Powinno się dać łatwo sprawdzić, czy dowolne dwie komórki mają wspólną krawędź (bok). Powinno się dać łatwo znaleźć fizyczną ścieżkę przechodzącą przez granicę dzieląca dwie sąsiadujące komórki. Komórki, które otrzymamy stosując powyższe zasady są albo puste, albo zajęte. Zatem znalezienie ścieżki z punktu początkowego do punktu docelowego, tak jak w przypadku algorytmów opierających się na mapach drogowych, jest niczym innym jak znalezieniem ścieżki w grafie. Kolejną wadą tego algorytmu (jak i w wielu innych) jest fakt, że efektywność planowania ruchu zależy od gęstości (liczba) oraz złożoności obiektów w środowisku, tak jak w przypadku algorytmów opierających się na mapach drogowych. Zaleta w przypadku tego algorytmu polega na tym samym co wada: w niezwykle rzadkim otoczeniu liczba komórek będzie bardzo mała, niezależnie od geometrycznej wielkości środowiska. W rzeczywistości, z powodu złożoności implementacji, dokładny podział na komórki stosuje się stosunkowo rzadko w robotach mobilnych. Przybliżony podział na komórki (approximate cell decomposition) W przeciwieństwie do dokładnej dekompozycji komórkowej przybliżony podział ma komórki jest jedną najczęściej stosowanych technik w zagadnieniu planowania ruchu robotów mobilnych. Częściowo dzieje się tak, z powodu popularności reprezentacji środowiska w postaci siatki zajętości. Komórki tej siatki mogą mieć stałą bądź zmienną szerokość. Idea: podziel obszar F na zbiór K rozdzielnych się komórek, których suma (w sensie mnogościowym) w przybliżony sposób odwzoruje obszar F, ponieważ komórki częściowo zajęte są traktowane jako zajęte. Algorytm unikania kolizji 9

26 ROZDZIAŁ 2. METODY I ALGORYTMY UNIKANIA KOLIZJI Marek Majchrowski Rysunek 2.4: Przykładowy przybliżony podział na komórki o stałej szerokości [23] Własności komórek: Komórki powinny mieć prosty kształt (np. kwadrat, sześciokąt foremny), tak aby łatwo można było dokonać dekompozycji (inaczej niż w dokładnej dekompozycji komórek) oraz łatwo można było wyznaczyć ścieżkę pomiędzy dwoma dowolnymi komórkami (tak samo jak dla dokładnej dekompozycji komórek). Powinno się dać łatwo sprawdzić, czy dowolne dwie komórki mają wspólną krawędź (tak samo jak dla dokładnej dekompozycji komórek). Powinno się dać łatwo znaleźć fizyczną ścieżkę przechodzącą przez granicę współdzieloną przez dwie sąsiadujące komórki (tak samo jak dla dokładnej dekompozycji komórek). Na rysunku 2.4 pokazano przykładowy podział na komórki o stałej szerokości. Szerokość komórek może zależeć od kształtu i rozmiarów przeszkód, jednak jest stała dla całej siatki zajętości. Na rysunku 2.5 pokazano natomiast przykładowy podział na komórki o zmiennej szerokości. Zaletą podziału na komórki o stałej szerokości jest stosunkowo mała złożoność obliczeniowa planowania ścieżki (wykorzystując np.: algorytmy propagacji fali NF1, BFS, DFS czy algorytm Dijkstry lub A*) oczywiście zależy od rozmiarów mapy. Jest to algorytm pamięciochłonny. Dla dużych środowisk (nawet rzadkich) siatka zajętości musi być reprezentowana w całości. W rzeczywistości, w związku ze spadającymi cenami pamięci operacyjnej, ta niedogodność została w ostatnich latach złagodzona Sztuczne pola potencjałowe (artificial potential field) Metoda sztucznego pola potencjałowego tworzy pole, które kieruje robota do celu poprzez wiele pośrednich pozycji ([7]). To podejście oryginalnie zostało zaadaptowane w celu planowania ruchu manipulatorów, ale jest często używane w, wielu odmianach, dla robotów mobilnych. 10 Algorytm unikania kolizji

27 Marek Majchrowski ROZDZIAŁ 2. METODY I ALGORYTMY UNIKANIA KOLIZJI Rysunek 2.5: Przykładowy przybliżony podział na komórki o zmiennej szerokości [21] Podstawową ideą we wszystkich metodach opierających się na zasadzie sztucznego pola potencjałowego, jest to, że robot jest przyciągany do celu, w tym samym czasie będąc odpychanym od przeszkód. W przypadku pojawienia się nowej przeszkody wystarczy zaktualizować pole sztucznego potencjału. W metodzie sztucznego pola potencjałowego traktuje się robota jako punkt materialny poruszający się pod wpływem sztucznego pola potencjałowego U(q). Robot porusza się w polu sztucznych sił, tak jakby piłka staczała się z góry. Cel (minimum globalne pola w całej przestrzeni) działa jako siła przyciągająca robota, natomiast przeszkody jako siła odpychająca. Superpozycja wszystkich sił oddziałuje na robota, który w większości przypadków jest reprezentowany jako punkt w przestrzeni konfiguracyjnej. Takie sztuczne pole potencjałowe gładko prowadzi robota do celu, jednocześnie zapewniając mu bezpieczeństwo (unikanie przeszkód). Podstawowym problemem jest unikanie lokalnych minimów sztucznego pola potencjałowego. Wypadkowa siła może mieć wartość zero poza punktem docelowym. W najprostszym przypadku, robot jest punktem, jego orientacja Θ jest zaniedbywana, a wynikowe pole potencjałowe jest dwuwymiarowe (x,y). Mając różniczkowalną funkcję U(q) sztucznego pola potencjałowego, możemy wyznaczyć sztuczną siłę działająca w pozycji q = (x,y) F(q) = U(q) = [ U(q) x U(q) y gdzie U(q) oznacza wektor gradientu U w pozycji q. Sztuczne pole potencjałowe (rysunek 2.6) U(q) jest sumą pól: przyciągającego i odpychające- Algorytm unikania kolizji 11 ]

28 ROZDZIAŁ 2. METODY I ALGORYTMY UNIKANIA KOLIZJI Marek Majchrowski Rysunek 2.6: Sztuczne pole potencjałowe go: U(q) = U att (q) +U rep (q). Podobnie można rozdzielić siłę na część przyciągającą i odpychającą: F(q) = F att (q) F rep (q) = U att (q) + U rep (q) Potencjał przyciagaj acy Rysunek 2.7: Przykładowy potencjał przyciągający Potencjał przyciagaj acy (rysunek 2.7) może, na przykład, być zdefiniowany jako funkcja kwadratowa U att (q) = 1 2 k att ρ 2 goal (q) gdzie k att jest dodatnim współczynnikiem skalującym, a ρ goal (q) oznacza euklidesową odległość q q goal. Tak zdefiniowany potencjał przyciagaj acy jest różniczkowalny, zatem 12 Algorytm unikania kolizji

29 Marek Majchrowski ROZDZIAŁ 2. METODY I ALGORYTMY UNIKANIA KOLIZJI można określić siłę przyciągającą F att (q) = U att (q) = k att ρ goal (q) ρ goal (q) = k att (q q goal ), która liniowo zbiega do 0 w momencie gdy robot osiągnie cel. Potencjał odpychajacy Rysunek 2.8: Przykładowy potencjał odpychający Ideą potencjału odpychajacego (rysunek 2.8) jest generowanie siły odpychającej od wszystkich znanych przeszkód. Potencjał ten powinien być bardzo silny, kiedy robot jest blisko obiektu, ale jednocześnie nie powinien wpływać na ruch robota, gdy ten jest wystarczająco daleko od danego obiektu. Poniższa funkcja spełnia te warunki U rep (q) = ( ) k rep 1 ρ(q) ρ 1 0 dla ρ(q) ρ 0 0 dla ρ(q) > ρ 0 gdzie k rep jest dodatnim współczynnikiem skalującym, ρ(q) jest odległością od q do obiektu, a ρ 0 jest odległością, powyżej której pole nie ma wpływu na obiekty. Tak zdefiniowany potencjał odpychajacy jest dodatni lub zerowy i dąży do nieskończoności w miarę zbliżania się q do przeszkody. Potencjał ten jest także różniczkowalny, jeśli brzeg obiektu jest wypukły i opisany funkcjami przedziałami gładkimi. Zatem { ( ) k rep 1 F rep (q) = U rep (q) = ρ(q) ρ 1 1 q q obstacle 0 ρ 2 (q) ρ(q) dla ρ(q) ρ 0 0 dla ρ(q) > ρ Lokalne metody planowania ruchu Lokalne metody planowania ruchu opierają się zazwyczaj na modyfikacji zadanej trajektorii bazując na bieżących odczytach z czujników. Algorytm unikania kolizji 13

30 ROZDZIAŁ 2. METODY I ALGORYTMY UNIKANIA KOLIZJI Marek Majchrowski Algorytm BUG Algorytm BUG jest jest chyba najprostszym algorytmem omijania przeszkód. Robot podążając do celu, gdy napotka przeszkodę podąża wzdłuż jej brzegu. Istnieje wiele odmian algorytmu BUG. Robot, wykorzystujący algorytm BUG1, najpierw objeżdża całą przeszkodę, a dopiero podczas drugiego okrążenie decyduje, w którym miejscu przestać podążać wzdłuż brzegów przeszkody. Obrazuje to rysunek 2.9. Rysunek 2.9: Przykład działania algorytmu BUG1 [12] Jest to bardzo niewydajny algorytm, ale gwarantuje, że robot dotrze do celu, jeśli tylko ten jest osiągalny. W przypadku algorytmu BUG2 robot podąża wzdłuż brzegów przeszkody, podobnie jak w przypadku BUG1, ale tylko do momentu, gdy jest w stanie bezpośrednio podążyć do wyznaczonego celu, co przedstawia rysunek Rysunek 2.10: Przykład działania algorytmu BUG2 [12] W ogólności BUG2 będzie zapewniał krótsza ścieżkę ruchu, ale nadal nie będzie ona optymalna (jeśli kryterium stanowi długość ścieżki) VFH oraz jego odmiany Algorytmy te należą do grupy metod planowania ruchu bazujących na wyznaczaniu kierunku. Dokładniejszy opis tych metod znajduje się w rozdziale Algorytm unikania kolizji

31 Marek Majchrowski ROZDZIAŁ 2. METODY I ALGORYTMY UNIKANIA KOLIZJI Dynamiczne okna Metoda dynamicznych okien, jest techniką unikania kolizji uwzględniającą ograniczenia kinematyczne robota. W literaturze zostały przedstawione dwa podejścia: metoda dynamicznych okien (podejście lokalne) [5] zaproponowana przez Foxa, Burgarda i Thruna oraz metoda dynamicznych okien (podejście globalne) [3] podana przez Brocka i Khatiba. Podejście lokalne W algorytmie dynamicznych okien (podejście lokalne) wartości sterujące robota (prędkość liniowa v oraz kątowa ω) otrzymuje się w wyniku przeszukiwania przestrzeni prędkości. Przestrzeń ta jest zbiorem wszystkich możliwych wartości (v, ω), gdzie v to prędkość liniowa robota, natomiast ω to prędkość kątowa. W algorytmie tym zakładamy, że robot porusza się po torze będącym łukiem okręgu. Mając daną aktualną prędkość wyznaczamy najpierw wszystkie dynamiczne okna dla pary (v, ω), które mogą być osiągnięte w następnej chwili czasowej (uwzględniając dynamikę robota). Następnie zbiór uprzednio wyznaczonych okien jest redukowany do takich, które zapewniają bezpieczne zatrzymanie się robota przed przeszkodą. Wybrane w ten sposób pary prędkości (v, ω) nazywamy dopuszczalnymi. Nowy kierunek i prędkość jest wyznaczana przez minimalizację wskaźnika jakości dobieranego jako liniowa zależność (z odpowiednimi wagami) pomiędzy: odległością od przeszkody, która wynika z danej pary (v, ω), orientacją względem celu (też wynikającym z danej pary (v,ω)) oraz prędkością, z jaką będzie możliwy ruch (preferuje się szybki ruch na wprost). Podejście globalne Podejście globalne polega na rozszerzeniu wcześniej omawianego wskaźnika o składnik odzwierciedlający odległość do celu. Część ta wyznaczana jest w oparciu o algorytm propagacji fali NF1, który przeszukuje siatkę zajętości analizowanego obszaru. W praktyce podejście to okazuje się bardzo wydajnym rozwiązaniem, pozwalającym na osiągnięcie czasu wyznaczania ścieżki rzędu kilkunastu milisekund Diagramy sasiedztwa Metoda diagramów sąsiedztwa (Nearness Diagram ND [8]) wydobywa informacje o środowisku otaczającym robota w trzech etapach: 1. Na podstawie dostępnych informacji z sensorów budowane są dwa diagramy sąsiedztwa: PND diagram sąsiedztwa widziany względem punktu centralnego, RND diagram sąsiedztwa widziany względem robota. Punkt centralny w praktyce jest środkiem geometrycznym robota. Diagram PND reprezentuje sąsiedztwo przeszkód do tego punktu. Natomiast diagram RND reprezentuje sąsiedztwo przeszkód do granic korpusu robota. Algorytm unikania kolizji 15

32 ROZDZIAŁ 2. METODY I ALGORYTMY UNIKANIA KOLIZJI Marek Majchrowski 2. Diagram PND jest analizowany pod kątem wolnych obszarów. Następnie wybierany jest jeden z tych obszarów. 3. Diagram RND jest analizowany w celu oceny bezpieczeństwa sytuacji, w której robot się znajduje. Uzyskane w trakcie tych analiz informacje są wykorzystywane do podjęcia decyzji o następnym ruchu, na podstawie pięciu praw, które przekładają się na pięć ocen bezpieczeństwa sytuacji, w których robot może się znaleźć. Szerszy opis algorytmu znajduje się w [8]. Podejście to zostało rozszerzone o cechy globalnych metod planowania ruchu (Global Nearness Diagram GND) i zostało opisane w [10]. Rozszerzenie to jest analogiczne jak w przypadku globalnego podejścia w metodzie dynamicznych okien Algorytm CVM Do grupy podejść bazujących na wyznaczaniu prędkości zalicza się także metodę tzw. krzywizn i prędkości (Curvature Velocity Method (CVM)) [13]. Zakłada się tutaj podobnie jak w technikach okna dynamicznego, że robot porusza się tylko po łukach okręgów o krzywiznach c = ω v. W podejściu tym możliwe jest uwzględnienie ograniczeń na dopuszczalne prędkości i przyspieszenia. Dokładniejszy opis tych metod znajduje się w rozdziale Algorytm unikania kolizji

33 Rozdział 3 Algorytm krzywizn i prędkości CVM Algorytm ten jest lokalną metodą planowania ruchu bazującą na wyznaczaniu prędkości liniowej i kątowej robota. Problem omijania przeszkód jest opisywany jako zadanie optymalizacji z ograniczeniami w przestrzeni prędkości robota. Przestrzeń prędkości jest zbiorem dopuszczalnych prędkości. W zadaniu tym przyjmujemy sterowanie za pomocą niezależnych prędkości: liniowej v oraz kątowej ω. Wybór takich sterowań jest naturalny dla robotów o napędzie synchronicznym. Dla naszego robota o napędzie różnicowym bardziej oczywiste wydaje się sterowanie za pomocą niezależnych prędkości lewego v l oraz prawego v r koła. Przejście pomiędzy tymi prędkościami wyraża się prostymi zależnościami: ω = v l v r d, gdzie d to rozstaw kół robota, oraz v = v l+v r 2. Ponieważ przeliczenie takie jest dokonywane w sterowniku robota, więc w poniższych rozważaniach będziemy operować na prędkościach (ω, v). Pożądane prędkości (ω, v) są uzyskiwane w wyniku maksymalizacji określonej funkcji celu przy spełnieniu ograniczeń. Do zalet tak sformułowanego zadania należą między innymi: łatwość dodawania ograniczeń wynikających z dynamiki robota, niezależne sterowanie prędkością oraz zmianą orientacji robota przez zadawanie prędkości liniowej i kątowej (sterowanie prędkością kątową wpływa tylko i wyłącznie na zmianę orientacji, natomiast prędkość liniowa wpływa jedynie na prędkość robota), możliwość wprowadzania dodatkowych czynników decydujących o wyborze prędkości przez modyfikację funkcji celu. W rzeczywistości, z dobrym przybliżeniem, można założyć, że robot, w każdej chwili czasu, porusza się wzdłuż łuku pewnego okręgu, którego krzywizna wyraża się wzorem: Fizyczna konstrukcja robota narzuca dwa typy ograniczeń: c = ω v. (3.1) Algorytm unikania kolizji 17

34 ROZDZIAŁ 3. ALGORYTM KRZYWIZN I PREDKOŚCI CVM Marek Majchrowski na maksymalną oraz minimalną prędkość kątową oraz liniową robota: ω min ω ω max, (3.2) v min v v max ; (3.3) na przyspieszenie kątowe oraz liniowe. Dla prędkości (ω n, v n ) w chwili n oraz okresu T alg, będącego czasem jednego wykonania algorytmu, prawdziwe są warunki: ω n ε max T alg ω n+1 ω n + ε max T alg, (3.4) v n+1 v n + a max T alg, (3.5) gdzie ε max oraz a max to odpowiednio maksymalne przyspieszenie kątowe oraz maksymalne przyspieszenie liniowe robota. Najważniejszym źródłem ograniczeń na dopuszczalne prędkości (ω, v) są przeszkody znajdujące się w środowisku otaczającym robota. Ponieważ w naturalny sposób przeszkody są opisywane we współrzędnych kartezjańskich, należy dokonać transformacji ich opisu do przestrzeni prędkości robota. Poniżej opisano jak jest to realizowane. Dla wszystkich krzywizn c należy obliczyć odległość d c ((0, 0), p i ) rozumianą jako długość drogi, jaką musi przebyć robot, startując z punktu (0, 0) w układzie lokalnym robota, wzdłuż okręgu o krzywiźnie c, zanim zderzy się z przeszkodą p w punkcie p i. Następnie definiujemy funkcję odległości robota od przeszkody jako (zapis ten nie jest ścisły z matematycznego punktu widzenia): { dc ((0, 0), p d v (ω, v, p) = i ) dla v 0 dla v = 0, gdzie c = ω (3.6) v Dla danego zbioru przeszkód P funkcja ta ma postać: D(ω, v, P) = inf p P d v(ω, v, p). (3.7) Ponieważ zasięg czujników jest ograniczony należy ograniczyć zakres możliwych wartości funkcji D do pewnej ustalonej wartości L (w naszym przypadku przyjęliśmy L = 1, 1 m dla czujników o zasięgu 1,5 m): D ogr (ω, v, P) = min (L, D(ω, v, p)). (3.8) Obliczenie odległości d c ((0, 0), p i ) od przeszkody p, o dowolnym kształcie, może być w ogólnym przypadku skomplikowane. Dlatego też, w naszych rozważaniach, będziemy przybliżać przeszkody okręgami opisanymi na nich. Dla robota o orientacji zgodnej z osią OY, dla danego okręgu, o krzywiźnie c i, który przecina przeszkodę w punkcie P i (x i, y i ) otrzymujemy (rys. 3.1): d ci ((0, 0), P i ) = { atan2(yi, x i 1 θ = c i ) dla c i < 0 π atan2(y i, x i c 1 i ) dla c i > 0 { yi dla c i = 0 c 1 i θ dla ci 0 (3.9) (3.10) 18 Algorytm unikania kolizji

35 Marek Majchrowski ROZDZIAŁ 3. ALGORYTM KRZYWIZN I PREDKOŚCI CVM Rysunek 3.1: Wyznaczanie odległości od przeszkody p dla okręgu o krzywiźnie c i Mając dane już ograniczenia wynikające z otoczenia robota, jak i jego możliwości, wartości prędkości otrzymujemy poszukując maksimum funkcji celu. Funkcję celu dobieramy tak, aby spełnić następujące wymagania: robot powinien dążyć do osiągnięcia swojej maksymalnej prędkości liniowej, robot powinien poruszać się po krzywych niekolidujących z przeszkodami, robot powinien zawsze kierować się na cel. Powyższe wymagania można zapisać w postaci liniowej funkcji celu: F (ω, v) = α 1 V (v) + α 2 D(ω, v) + α 3 G(ω), (3.11) Algorytm unikania kolizji 19

36 ROZDZIAŁ 3. ALGORYTM KRZYWIZN I PREDKOŚCI CVM Marek Majchrowski gdzie: V (v) = v (3.12) v max D(ω,v) = D ogr(ω, v, P) (3.13) L θ cel ω T alg G(ω) = 1 (3.14) π Wartości poszczególnych składników V, D, G funkcji F (3.11) są znormalizowane. Składowa V odpowiada za poruszanie się z maksymalną możliwą prędkością liniową, składowa D jest odpowiedzialna ze podążanie wzdłuż bezkolizyjnych krzywych, natomiast składowa G preferuje podążanie za celem. W ostatnim członie θ cel jest orientacją do celu w układzie współrzędnych robota. Wartości współczynników α i są wagami powyższych składników. Od doboru tych współczynników zależy zachowanie robota, tzn. jak będzie skręcał, jak wcześnie będzie reagował na przeszkody, itd. 20 Algorytm unikania kolizji

37 Rozdział 4 Algorytm VFH oraz jego odmiany Algorytmy te należą do lokalnych metod planowania ruchu bazujących na wyznaczaniu kierunku i wykorzystują tzw. histogramy pola wektorowego. 4.1 VFH Algorytm VFH wykorzystuje siatkę, w kartezjańskim układzie współrzędnych, reprezentującą dwuwymiarowy histogram, jako model otoczenia (lokalną mapę otoczenia robota). Model ten jest aktualizowany, w sposób ciągły, za pomocą czujników odległości. Wykorzystując później dwuetapowy proces upraszczania tego modelu otoczenia, wyznaczane są sterowania dla robota. W pierwszej fazie siatka, opisująca dwuwymiarowy histogram, redukowana jest do jednowymiarowego histogramu biegunowego, który jest tworzony wokół aktualnej pozycji robota. Do każdego wycinka tego histogramu biegunowego przypisana jest wartość reprezentująca prawdopodobieństwo wystąpienia przeszkody w danym kierunku. W drugiej fazie zostaje wybrany jeden wycinek, z całego histogramu biegunowego, o najmniejszym prawdopodobieństwie wystąpienia przeszkody. Następnie wyznaczane są parametry sterowania zapewniające ruch w kierunku reprezentowanym przez ten wycinek. W algorytmie tym możemy wyróżnić trzy abstrakcyjne poziomy reprezentacji danych: Najwyższy poziom przechowuje szczegółowy opis otoczenia robota w postaci siatki C, w kartezjańskim układzie współrzędnych, reprezentującej dwuwymiarowy histogram. Sposób tworzenia tej siatki został opisany w Na pośrednim poziomie konstruowany jest jednowymiarowy histogram biegunowy H wokół aktualnej pozycji robota. Histogram biegunowy H składa się z n wycinków, każdy o kącie wierzchołkowym równym α. Sposób transformacji aktywnego obszaru C do histogramu biegunowego H został opisany w Najniższy poziom danych reprezentuje wyjście algorytmu: wartości zadane dla układów sterowania robota. Algorytm unikania kolizji 21

38 ROZDZIAŁ 4. ALGORYTM VFH ORAZ JEGO ODMIANY Marek Majchrowski Tworzenie siatki reprezentujacej dwuwymiarowy histogram Metoda VFH wykorzystuje lokalną mapę w postaci siatki C, która opisuje dwuwymiarowy histogram. Każdej komórce (i, j) tej siatki, przypisana jest pewna wartość, która jest miarą ufności, że w danym miejscu znajduje się przeszkoda. Przy każdym odczycie z czujnika odległości aktualizowana jest tylko wartość jednej komórki, która odpowiada zmierzonej odległości d i leży w kierunku wyznaczonym przez dany czujnik (rys. 4.1). W wyniku otrzymujemy rozkład prawdopodobieństwa, w którym du- Rysunek 4.1: Tworzenie siatki, która opisuje dwuwymiarowy histogram że prawdopodobieństwo w komórce oznacza najbardziej prawdopodobną i aktualną pozycję przeszkody Tworzenie histogramu biegunowego Odwzorowanie aktywnego obszaru C dwuwymiarowego histogramu C w histogram biegunowy H wykonuje się w sposób opisany poniżej. Aktywny obszar C porusza się wraz z robotem pokrywając kwadratowy obszar siatki o wymiarach w s w s (rys. 4.2). Dla każdej aktywnej komórki, w siatce reprezentującej dwuwymiarowy histogram, wyznaczany jest wektor o początku w środku robota, a końcu w aktualnie rozpatrywanej komórce. Wektor taki ma kierunek β: β i, j = arctan y i y 0 x i x 0 (4.1) 22 Algorytm unikania kolizji

39 Marek Majchrowski ROZDZIAŁ 4. ALGORYTM VFH ORAZ JEGO ODMIANY oraz długość: m i, j = ( c ) 2 ( ) i, j a b di, j (4.2) gdzie: a, b dodatnie stałe dobrane tak, aby a b d max = 0, gdzie d max = 2 w s 1 2, c i, j miara ufności aktywnej komórki c i, j, d i, j odległość pomiędzy aktywną komórką c i, j a środkiem robota, m i, j długość wektora dla komórki c i, j, x 0, y 0 aktualne współrzędne środka robota x i, y j współrzędne aktywnej komórki c i, j β i, j kierunek wyznaczony przez aktywną komórkę c i, j i środek robota. Histogram biegunowy H ma dowolną rozdzielczość kątową taką aby n = 360 α było liczbą całkowitą. Każdy wycinek k tego histogramu odpowiada pewnemu skwantowanemu kątowi φ, który jest wielokrotnością kąta α ( φ = k α, dla k = 1,...,n). Zależność pomiędzy c i, j a wycinkiem k wyraża się wzorem: ( ) βi, j k = floor, (4.3) α gdzie floor to funkcja, która zaokrągla swój argument w dół do najbliższej liczby całkowitej. Dla każdego wycinka k histogramu biegunowego miara ufności wyraża się wzorem: h k = m i, j (4.4) i, j Każda aktywna komórka jest powiązana z pewnym sektorem za pomocą równań 4.1 i 4.3. Na rysunku 4.2, przedstawiającym odwzorowanie aktywnego obszaru C w histogram biegunowy H, wszystkie aktywne komórki powiązane z wycinkiem k zostały wytłuszczone (rysunek wykonano dla parametrów α = 10 i n = 36). Aby powstały w ten sposób histogram biegunowy nie był poszarpany, stosuję się funkcję wygładzającą postaci: h k = h k l + 2h k l lh k + + 2h k+l 1 + h k+l 2l + 1 (4.5) gdzie h k jest wygładzon a miar a ufności (smoothed polar obstacle density POD) histogramu biegunowego. Funkcja ta pełni rolę filtru dolnoprzepustowego, gdzie jakość wygładzenia zależy od parametru l Wyznaczanie kierunku ruchu Na rysunku 4.3, przedstawiającym wygładzony histogram biegunowy, widać lokalne maksima (szczyty) oraz minima (doliny) odpowiadające odpowiednio wysokiej oraz niskiej wartości POD. Wszystkie doliny zawierające wycinki histogramu o wartościach POD poniżej ustalonej wartości (wartości progowej) są nazywane dolinami kandydujacymi. W przypadku istnienia więcej niż jednej takiej doliny kandydującej, wybierana jest ta, której kierunek jest najbliższy kierunkowi do celu k targ. W momencie gdy już dokonaliśmy wyboru doliny, wybieramy Algorytm unikania kolizji 23

40 ROZDZIAŁ 4. ALGORYTM VFH ORAZ JEGO ODMIANY Marek Majchrowski Rysunek 4.2: Wygładzony histogram biegunowy dla przeszkód A, B oraz C odpowiedni wycinek histogramu należący do tej doliny. Na początku, należy określić szerokość wybranej doliny, rozumianą jako liczbę kolejnych wycinków histogramu o wartości POD poniżej wartości progowej. W ten sposób można rozróżnić dwa typy dolin: szerokie ich szerokość jest większa niż s max (s max wynosi zazwyczaj 18 dla α = 5 i n = 72). Wycinek histogramu, którego kierunek jest najbliższy kierunkowi k targ oznaczmy k n. Wycinek k n reprezentuje bliską granicę doliny, natomiast wycinek k f odległą i wyraża się wzorem k f = k n + s max. Pożądanym kierunkiem ruchu, w takim przypadku, jest kierunek θ = k n+k f 2. waskie ich szerokość jest mniejsza niż s max i jest zazwyczaj spowodowana jazdą pomiędzy przeszkodami, znajdującymi się blisko robota. W tym przypadku także pożądanym kierunkiem ruch jest kierunek θ = k n+k f 2, gdzie k n i k f są granicznymi wycinkami tej doliny w histogramie biegunowym. 4.2 VFH+ W algorytmie VHF+ przy wyznaczaniu toru ruchu brane są pod uwagę wymiary robota (uproszczony model robota w postaci okręgu) i możliwość realizacji danej trajektorii przy ograniczeniach wynikających z kinematyki robota [16]. Wybór kierunku polega na poszukiwaniu 24 Algorytm unikania kolizji

41 Marek Majchrowski ROZDZIAŁ 4. ALGORYTM VFH ORAZ JEGO ODMIANY Rysunek 4.3: Wygładzony histogram biegunowy dla przeszkód A, B oraz C Algorytm unikania kolizji 25

42 ROZDZIAŁ 4. ALGORYTM VFH ORAZ JEGO ODMIANY Marek Majchrowski minimum wartości pewnej funkcji kosztu. Zakłada się także, że robot porusza się po odcinkach prostych i łukach okręgów. Natomiast idea całego algorytmu pozostaje niezmieniona w stosunku do podstawowej wersji VFH. 26 Algorytm unikania kolizji

43 Rozdział 5 Opis stanowiska badawczego W skład stanowiska badawczego, które było wykorzystywane przy testowaniu algorytmów unikania kolizji, wchodzi: robot mobilny Protonek wraz z czujnikami, trójwymiarowy symulator Gazebo ([27]), oprogramowanie do sterowania robotem oraz symulatorem. W ramach przygotowania stanowiska badawczego, należało: oprogramować robota mobilnego Protonek, oprogramować czujniki podczerwieni robota mobilnego Protonek. 5.1 Robot mobilny Protonek Robot mobilny Protonek (5.1) został zaprojektowany i zbudowany w Instytucie Automatyki i Robotyki oraz Instytucie Automatyki i Informatyki Stosowanej Politechniki Warszawskiej. Jest to autonomiczna platforma laboratoryjna przygotowana do badań nad systemami sterowania i nawigacji robotów mobilnych. Baza jezdna robota jest sześciokołową platformą mobilną z napędem na wszystkie koła. Robot ma budowę modułową i w jej skład wchodzą moduły napędowe, moduł centralny rama, moduł sterowania, moduł stopnia mocy oraz wymienne moduły wykonawczo sensoryczne. Bazowe podwozie ma wymiary mm. Kadłub robota wykonany został z aluminium i całkowicie osłania wszystkie elementy układu sterowania i zasilania. W konstrukcji ramy wykorzystano prostokątne profile, dzięki czemu otrzymano lekką i sztywną konstrukcję nośną. Zastosowany w robocie układ napędowy jest typu czołgowego: skręcanie odbywa się przez różnicowanie prędkości kół po prawej i lewej stronie robota. Do napędu pojazdu zastosowano dwa silniki prądu stałego o nominalnym napięciu zasilania 24 V, ze zintegrowanymi reduktorami. Każdy z silników napędza trzy koła, odpowiednio po lewej i prawej stronie pojazdu. Napęd z silników przekazywany jest na koła za pośrednictwem systemu przekładni z paskami zębatymi. Przekładnie zapewniają Algorytm unikania kolizji 27

44 ROZDZIAŁ 5. OPIS STANOWISKA BADAWCZEGO Marek Majchrowski Rysunek 5.1: Robot mobilny Protonek bezluzową i cichą pracę napędów. W pojeździe zastosowano koła o jednakowej średnicy wynoszącej 100 mm. Między modułami napędowymi znajdują się dwa 12 V akumulatory, każdy o pojemności 7 Ah. Komputer pokładowy wraz z modułami sterującymi został umieszczony nad układem napędowym. Robot Protonek wyposażony jest w kilka niezależnych układów czujników. Do podstawowych należą: układ pomiaru odometrycznego oraz układ dalmierzy optycznych rozmieszczonych dookoła korpusu robota. Układ pomiaru odometrycznego składa się z dwóch przetworników obrotowo-impulsowych (enkoderów optycznych) umieszczonych w module napędowym. Zastosowano dwufazowe enkodery o rozdzielczości 1000 impulsów na obrót, dzięki czemu jest możliwy pomiar kąta obrotu kół z rozdzielczością 1/4000 pełnego obrotu. Pomiar prędkości obrotowej odbywa się bezpośrednio na osiach kół. Układ dalmierzy optoelektronicznych służy do wykrywania przeszkód znajdujących się w bezpośrednim otoczeniu robota. Zastosowano dwa typy dalmierzy, krótkiego i dalekiego zasięgu. Na robocie rozmieszczono osiemnaście czujników: pięć dalekiego zasięgu i trzynaście krótkiego. W przedniej części robota umieszczono 11 dalmierzy, po dwa na każdej burcie pojazdu oraz trzy w tylnej części. Aby uniknąć wzajemnego zakłócania czujniki dalekiego i krótkiego zasięgu umieszczone są naprzemiennie. Dalmierze są obsługiwane przez jednoukładowe mikrokomputery podłączone do komputera nadrzędnego przez łącza RS 485. Robot jest przystosowany do współpracy z dodatkowym wyposażeniem. Otwarta struktu- 28 Algorytm unikania kolizji

45 Marek Majchrowski ROZDZIAŁ 5. OPIS STANOWISKA BADAWCZEGO ra układu sterowania oraz specjalnie zaprojektowana konstrukcja mechaniczna pozwalają na łatwy montaż dodatkowych modułów. Jednym z takich modułów jest głowica do skanowania trójwymiarowego. Moduł składa się ze skanera laserowego SICK LMS 200 umieszczonego na obrotowej głowicy. Głowica może obrać skaner wokół osi poprzecznej w zakresie kątowym 105. Jednak, dla potrzeb tej pracy, wykorzystywany będzie skaner laserowy SICK LMS 200 w położeniu, w którym jego wiązka jest równoległa do podłoża. PC-104+ Główny komputer sterujący PCM-9579 EBX (wbudowany, klasy PC) Karta akwizycji obrazu Karta WLAN Zasilanie PC-104 5V 12V 24V Mikrokomputer sterujący PWM A, B 2 2 Wyświetlacz LCD Klawiatura Przetwornice Przekaźniki 24V Bateria akumulatorów RS-485 Wzmacniacze mocy Mikrokomputer pomiarowy... Mikrokomputer pomiarowy... Czujniki odległości... Czujniki binarne 2 2 Silniki Enkodery (na kołach) Rysunek 5.2: Struktura sterownika robota mobilnego Protonek Schemat blokowy układu sterowania jest pokazany na rys Podstawowym elementem układu jest główny komputer pokładowy, który zarządza pracą całego systemu i realizuje wszystkie funkcje sterowania wysokiego poziomu. Wykorzystano tu jednopłytkowy komputer PCM-9579 EBX firmy Advantech. Jest to komputer z procesorem Pentium z zegarem o częstotliwości 900 MHz, przeznaczony do zastosowań wbudowanych, wyposażony we wszystkie typowe elementy komputera klasy PC (grafika, dźwięk, Ethernet, sterowniki napędów dyskowych, port równoległy i szeregowy RS-232, magistrala PCI), a także w elementy dodatkowe, typowe dla komputerów wbudowanych (magistrale PC-104 i PC-104+, złącze RS-485, złącze pamięci compact flash pracującej w trybie IDE). W konfiguracji bazowej robota, do magistrali PC-104+ jest przyłączona bezprzewodowa karta sieciowa zgodna z standardem Ethernet g wykorzystywana do do szybkiej (54 Mb/s) komunikacji robota z otoczeniem. Bezpośrednie sterowanie wszystkich elementów wykonawczych robota (silniki, czujniki, itp.), a także wykrywanie i obsługa sytuacji awaryjnych są realizowane przez specjalizowany Algorytm unikania kolizji 29

46 mikrokomputer sterujący. Jest on zbudowany na bazie mikrokomputera jednoukładowego firmy Atmel T89C51AC2 8-bitowego układu rodziny MCS-51. Komunikacja mikrokomputera z głównym komputerem sterującym odbywa się za pomocą bloku sprzęgającego, przyłączonego z jednej strony do magistrali PC-104 komputera, a z drugiej strony do magistrali mikrokomputera. Jedna z funkcji mikrokomputera sterującego jest sterowanie silnikami napędowymi robota (regulacja prędkości lub położenia). Do tego celu wykorzystano scalone regulatory PID układy LM629 firmy National Semiconductor. W celu zapewnienia elastyczności konfiguracji, prostoty rozbudowy oraz zmniejszenia liczby przewodów dochodzących do mikrokomputera sterującego, przyjęto że wszystkie czujniki będą bezpośrednio obsługiwane przez specjalizowane mikrokomputery pomiarowe, pracujące w sieci lokalnej. Do budowy sieci mikrokomputerów pomiarowych przyjęto standard RS Trójwymiarowy symulator Gazebo W symulatorze trójwymiarowym Gazebo możliwe jest nie tylko modelowanie robotów połączone z wizualizacją, ale także uwzględniana jest dynamika ruchu obiektów (a zatem także tarcie) oraz możliwe jest oddziaływanie robota na środowisko (takie jak przesuwanie czy podnoszenie obiektów). Rysunek 5.3: Model robota mobilnego Protonek w symulatorze 3D wraz z głowicą do skanowania trójwymiarowego oraz czujnikami IR Silnikiem symulacji jest biblioteka ODE (Open Dynamics Engine [25]), zaś roboty są modelowane jako zestaw brył połączonych przegubami. W przypadku robota Protonek (rys. 5.3) model stanowią prostopadłościenne bryły korpusu, do których przegubami obrotowymi dołączone zostały koła. Tak jak w rzeczywistym robocie napęd przenoszony jest z osi silnika na 3 koła za pomocą paska, w modelu koła otrzymują taką samą prędkość obrotową. 30 Algorytm unikania kolizji

47 Marek Majchrowski ROZDZIAŁ 5. OPIS STANOWISKA BADAWCZEGO 5.3 Oprogramowanie robotów mobilnych W poniższym podrozdziale przedstawiono system Player służący do sterowania robotów mobilnych. Opisany zostanie sterownik, który powstał do sterowania robotem mobilnym Protonek z wykorzystaniem ww. systemu Player Player [27] jest sieciowym serwerem do sterowania robotów. Uruchomiony na robocie udostępnia poprzez sieć IP proste i przejrzyste interfejsy do sensorów znajdujących się na robocie, jak i do jego efektorów. Program kliencki komunikuje się z Playerem poprzez gniazda TCP. Może w ten sposób odczytywać dane z czujników, wysyłać komendy do efektorów oraz konfigurować wszelkie urządzenia w czasie pracy. Player wspiera różnorodne platformy robotów. Oryginalnie Player powstał dla robotów typu Pioneer 2 firmy ActivMedia. Jednakże w niedługim czasie programiści dopisali wsparcie do innych robotów oraz sensorów. Stało się tak, dzięki modularnej budowie Playera, która zapewnia łatwość tworzenia nowych sterowników dla urządzeń. Player uruchamia się na systemach Linux, Solaris i *BSD Cechy Playera Player został zaprojektowany tak, aby był niezależny zarówno od języka programowania jak i od platformy sprzętowej. Programy klienckie mogą być uruchamiane na jakiejkolwiek maszynie, która jest wyposażona w połączenie sieciowe z robotem, jak również aplikacje te mogą być napisane w jakimkolwiek języku wspierającym gniazda TCP. W chwili obecnej wraz z Playerem są dystrybuowane narzędzie klienckie napisane w takich językach jak: C++, Tcl, Java oraz Python. Player nie narzuca żadnej struktury programów do sterowania robotów. Programy klienckie sterujące robotami mogą być zarówno wielowątkowymi aplikacjami jak i zarówno prostymi programami implementującymi pętle S-P-A (Wyczuwaj Planuj Działaj Sense Plan Act). Player dopuszcza aby wiele urządzeń używało tego samego interfejsu. Jest to szczególnie użyteczne w połączeniu z symulatorem Stage. Programy napisane pod symulator często działają niezmienione na rzeczywistym sprzęcie. Player został również zaprojektowany aby wspierać dowolną liczbę klientów. Dowolny klient może podłączyć się i zbierać odczyty z sensorów z dowolnego robota, na którym uruchomiony jest serwer Playera. W takim sam sposób mogą one wydawać polecenia dla silników robota. Zachowanie serwera może również być konfigurowane w trakcie jego pracy. Player jest darmowym oprogramowanie wydawanym pod licencją GNU Public License. Wszystkie materiały dotyczą Playera w wersji Algorytm unikania kolizji 31

48 ROZDZIAŁ 5. OPIS STANOWISKA BADAWCZEGO Marek Majchrowski Ogólna struktura sterowania z wykorzystaniem Playera Ogólny schemat struktury sterowania z wykorzystaniem Playera przedstawia rysunek 5.4. W jego centrum znajduje się wielowątkowy proces Playera. Z lewej strony znajdują się rzeczywiste urządzenia, natomiast po prawej znajdują się klienci. Każdy klient połączony jest z serwerem Playera poprzez gniazda TCP. W przypadku gdy klient jest uruchomiony na tej samej maszynie co Player, wtedy połączenie jest po interfejsie lokalnym (loopback). W przeciwnym przypadku jest to fizyczne połączenie przez sieć. Z drugiej strony Player łączy się z urządzeniami, poprzez swoje sterowniki, zazwyczaj poprzez łącze RS232, choć w przypadku niektórych sterowników (jak festival) połączenie to jest nawiązywane także poprzez sieć TCP. W przypadku sterownika robota mobilnego Protonek połączenie to jest nawiązywane z modułem jądra Linuxa, który dalej obsługuje już dane żądanie. Wewnątrz Playera różne wątki komunikują się wykorzystując globalną przestrzeń adresową. Jak przedstawiono na rysunku 5.4 każde urządzenie jest powiązane z buforem komend i buforem danych. Dostęp do tych buforów wykorzystuje mechanizm wzajemnego wykluczania. W ten sposób zapewniony jest asynchroniczny kanał komunikacyjny pomiędzy wątkami sterowników urządzeń a wątkami serwera odpowiedzialnymi za odbieranie oraz udostępnianie danych programom klienckim. Rysunek 5.4: Architektura Playera [27] 32 Algorytm unikania kolizji

49 Marek Majchrowski ROZDZIAŁ 5. OPIS STANOWISKA BADAWCZEGO Interfejsy Player udostępnia m.in. następujące interfejsy (opisano te, które mogą być używane w laboratorium dla robota mobilnego Protonek): player reprezentuje serwer Playera i służy do jego konfiguracji. Jest tylko jedno urządzenie (z indeksem 0), które udostępnia ten interfejs, i jest zawsze otwarte. position służy do sterowania robotem mobilnym w przestrzeni dwuwymiarowej. Zostanie on opisany dokładniej przy opisywaniu sterownika do robota mobilnego Protonek. sonar interfejs ten zapewnia dostęp do zestawu czujników sonarowych. Nie przyjmuje on żadnych komend. Umożliwia odpytanie serwera o pozycję czujników (rozmieszczenie czujników na korpusie robota). Nie można konfigurować rozmieszczenia czujników w trakcie pracy. Daje możliwość sterowania zasilaniem czujników oraz odbierania odczytów z tych czujników. ir interfejs ten zapewnia dostęp do zestawu czujników podczerwieni. Nie przyjmuje on żadnych komend. Umożliwia odpytanie serwera o pozycję czujników (rozmieszczenie czujników na korpusie robota). Nie można konfigurować rozmieszczenia czujników w trakcie pracy. Daje możliwość sterowania zasilaniem czujników oraz odbierania odczytów z tych czujników. laser interfejs ten zapewnia dostęp do pojedynczego dalmierza laserowego, jakim jest sensor SICK LMS 200. Nie przyjmuje on żadnych komend. Urządzenia wspierające ten interfejs mogą zostać skonfigurowane do skanowania w różnych zakresach i z różną rozdzielczością. Z tego powodu dane zwracane przez ten interfejs mogą mieć różną formę. Aby zapewnić ich odpowiednią interpretację dane przesyłane zawierają kilką dodatkowych pół (przed aktualnymi odczytami) opisujące odpowiednio: początkowy oraz końcowy kąt odczytów ze skanera, rozdzielczość kątową skanów oraz ich liczbę. Odczyty przebiegają zgodnie z ruchem wskazówek względem lasera. Kąt 0 jest na wprost lasera. blobfinder interfejs ten zapewnia dostęp do sterownika, który wykrywa plamy na obrazach. Zwraca on listę wykrytych obiektów. Każdy obiekt opisany jest przez strukturę: id numer obiektu, color jego kolor zapisany jako 32-bitowa wartość RGB (tj. 0x00RRGGBB), area jego pole (w pikslach), x współrzędna x jego środka na obrazie, y współrzędna y jego środka na obrazie, left współrzędna x lewego boku prostokąta opisanego na tym obiekcie, right współrzędna x prawego boku prostokąta opisanego na tym obiekcie, Algorytm unikania kolizji 33

50 ROZDZIAŁ 5. OPIS STANOWISKA BADAWCZEGO Marek Majchrowski top współrzędna y górnego boku prostokąta opisanego na tym obiekcie, bottom współrzędna y dolnego boku prostokąta opisanego na tym obiekcie, range odległość w milimetrach do środka plamy. Interfejs ten daje możliwość śledzenia obiektów tylko pewnego koloru, oraz przekazania pewnych parametrów obrazu (np. kontrast, jasność) do sterownika udostępniającego dany interfejs. camera interfejs ten jest używany aby zobaczyć co widzi kamera. Nie przyjmuje on żadnych komend, oraz nie udostępnia żadnej konfiguracji. Został on stworzony raczej w celu wykorzystania po stronie serwera do transmisji obrazu z jednego sterownika do drugiego, niż do transmisji z serwera do programów klienckich. Obraz może być w wielu formatach (PLAYER_CAMERA_FORMAT_MONO8, PLAYER_CAMERA_FORMAT- _MONO16, PLAYER_CAMERA_FORMAT_RGB565, PLAYER_CAMERA_FORMAT- _RGB888), ale zawsze jest dane są wyrównywane do wielkości bajtu. speech interfejs ten zapewnia dostęp do systemów syntezy mowy. Nie zwraca żadnych danych. bumper interfejs ten zapewnia dostęp do zestawu zderzaków. Nie przyjmuje on żadnych komend. Umożliwia odpytanie serwera o pozycję odbijaczy/zderzaków. Daje możliwość odbierania odczytów z tych czujników. wifi interfejs ten zapewnia dostęp do informacji o stanie sieci bezprzewodowej. Nie przyjmuje on żadnych komend. Pozwala na odczyt wielu informacji związanych z łączami bezprzewodowymi jak: siła sygnału, poziom szumów, numer kanału na jakim dokonywana jest transmisja etc. localize interfejs ten udostępnia informacje o pozycji robota. Nie przyjmuje on żadnych komend. Umożliwia ustawienie nowej pozycji, odczytania domniemanych pozycji (niekoniecznie jednej). Każda pozycja jest opisana także kowariancją estymowanego położenia. map interfejs ten zapewnia dostęp do informacji o mapie w postaci siatki zajętości. Nie zwraca żadnych danych oraz nie przyjmuje on żadnych komend. mcom interfejs ten został zaprojektowany w celu wymiany informacji pomiędzy klientami serwera. Jeden klient wysyła wiadomość określonego typu na określony kanał. Sterownik zachowuje tę wiadomość na stosie przeznaczonym dla danego kanału. Następnie drugi klient może pobrać informacje o określonym typie z określonego kanału. Operacje takie jak: Push, Pop, Read oraz Clear zostały zdefiniowane, ale ich semantyka zależy od konkretnego sterownika. speech_recognition interfejs ten zapewnia dostęp do systemów rozpoznawania mowy. 34 Algorytm unikania kolizji

51 Marek Majchrowski ROZDZIAŁ 5. OPIS STANOWISKA BADAWCZEGO ptz (pan/tilt/zoom) interfejs ten umożliwia kontrolo wonie serwomechanizmów takich jak ogniskowa z kamerze, czy jednostek, które pozwalają na sterowanie głowicami obrotowymi i obiektywami typu moto-zoom Plik konfiguracyjny Plik konfiguracyjny składa sie z listy sterowników, które mogą być używane przez dany serwer Playera. Każda z sekcji opisującej sterownik może zawierać kilka z poniższych opcji: port (string) nazwa sterownika do załadowania. Opcja ta jest wymagana w każdej sekcji. provides (string tuple) nazwy interfejsów, które dany sterownik udostępnia. Opcja ta jest wymagana w każdej sekcji. requires (string tuple) sterowniki (interfejsy), od których dany sterownik zależy (np. będzie czytał lub pisał z/do niego). Domyślna wartość [] oznacza brak zależności. plugin (string) w przypadku, gdy dany sterownik jest w postaci wtyczki (plugin) jest to nazwa pliku do załadowania. Brak domyślnej wartości. alwayson (integer) w przypadku wartości niezerowej, sterownik ten zostanie uruchomiony w momencie uruchomienia serwera, nie zaś w przypadku podłączenia pierwszego klienta. Domyślna wartość 0. Przykład konfiguracji: driver ( name "camera1394" provides ["camera:0"] ) driver ( name "cameracompress" provides ["camera:1"] requires ["camera:0"] ) Sterowniki Player udostępnia m.in. następujące sterowniki (opisano te, które mogą być używane w laboratorium do robota mobilnego Protonek): protonek sterownik do robota mobilnego Protonek. Został on opisany w kolejnej sekcji. protonek_ir sterownik czujników podczerwieni dla robota mobilnego Protonek. Został on opisany w rozdziale 5.4. Algorytm unikania kolizji 35

52 ROZDZIAŁ 5. OPIS STANOWISKA BADAWCZEGO Marek Majchrowski camerav4l sterownik ten przechwytuje obraz z kamer video podłączonych do framegrabberów kompatybilnych ze standardem V4L (Video4Linux). Sterownik ten dostarcza interfejs camera. Pozwala na podanie następujących opcji w pliku konfiguracyjnym: port (string) urządzenie do odczytu obrazu video, domyślna wartość /dev/video0. source (integer) numer wejścia na karcie, domyślnie wejście nr 3. norm (string) standard przechwytywanego obrazu. Domyślnie NTSC. Możliwe wartości NTSC lub PAL. size (integer tuple) wielkość obrazu w pikslach. Domyślna wartość zależy od standardu obrazu. mode (string) tryb przechwytywania obrazu. Standard V4L przewiduje następujące możliwości: GREY 8-bitowy monochromatyczny obraz, RGB bitowy kolorowy obraz w standardzie RGB, RGB24 24-bitowy kolorowy obraz w standardzie RGB, RGB32 32-bitowy kolorowy obraz w standardzie RGB, YUV420P 8-bitowy monochromatyczny obraz. Tryb ten zależy od framegrabbera, nie każdy tryb musi być wspierany. Domyślny tryb to RGB24. Nie wszystkie tryby są wspierane przez wewnętrzny format serwera Player. Stąd np. tryb RGB32 będzie transformowany do formatu RGB888. size (integer) ustawiona na 1 powoduje zapis każdej ramki przechwyconego obrazu. Domyślnie: 0. Przykład konfiguracji: driver ( name "camerav4l" provides ["camera:0"] port "/dev/video0" source 2 size [ ] mode "RGB32" ) W celu poprawnego uruchomienia kamery w standardzie PAL trzeba było zmienić maksymalną dopuszczalną ramkę przez serwer Player. W dołączonej płycie CD zamieściłem łatkę (patch.player pal.diff) poprawiającą ten problem dla Playera w wersji Wadą tego sterownika jest to, że zabiera dużo czasu procesora. Wynika to stąd, iż musi on zmieniać kolejność bajtów dla każdego piksla odczytanego obrazu. Obraz z framegrabbera ma kolejność bajtów BGR, natomiast wewnętrzne formaty Playera jak i te specyfikowane przez standard V4L mają odwrotną kolejność bajtów, tj. RGB. 36 Algorytm unikania kolizji

53 Marek Majchrowski ROZDZIAŁ 5. OPIS STANOWISKA BADAWCZEGO cameracompress sterownik ten kompresuje obraz z innego sterownika udostępniającego interfejs camera i udostępnia go pod nowym interfejsem. Wykorzystuje on kompresję JPEG. Przykład konfiguracji: driver ( name "cameracompress" provides ["camera:1"] requires ["camera:0"] ) cmvision CMVision (Color Machine Vision [22]) jest biblioteką, która udostępnia szybką segmentację obrazu na 32 kolory z wykorzystaniem tylko dwóch operatorów bitowych AND. CMVision został napisany przez Jima Bruce a z CMU i jest darmowo rozpowszechniany na licencji GNU GPL. Sterownik ten wykorzystuje bibliotekę CMVision udostępniając już posegmentowany obraz w postaci interfejsu blobfindera każdy segment obrazu jest traktowany jako inna plama na obrazie. Wymaga podania jednego dodatkowego parametru w pliku konfiguracyjnym: colorfile (string) plik konfiguracyjny opisujący kolory dla biblioteki CMVision Przykład konfiguracji: driver ( name "cmvision" provides ["blobfinder:0"] requires ["camera:0"] colorfile "colors.txt" ) Na dołączonej płycie CD zamieściłem łatkę (patch.cmvision v4l2.diff) na programy użytkowe udostępniane wraz z biblioteką CMVision. Naprawia ona obsługę framegrabbera w standardzie V4L2 i powoduje, że programy te się kompilują pomyślnie. Jeden z tych programów pozwala utworzyć plik, z definicjami kolorów dla biblioteki CMVision, wymagany przez sterownik cmvision. festival sterownik ten zapewnia dostęp do systemu syntezy mowy Festival [20]. Festival jest dostępny jako osobny pakiet, który należy wcześniej zainstalować. Sterownik ten udostępnia interfejs speech. W przeciwieństwie do większości sterowników, ten kolejkuje przychodzące komendy, a nowe są ignorowane dopiero po przepełnieniu kolejki. System Festival należy zainstalować ręcznie, natomiast jego uruchomieniem oraz zatrzymywaniem zajmie się już serwer Player. Pozwala na podanie następujących opcji w pliku konfiguracyjnym: port (integer) port TCP, na którym ma nasłuchiwać odpalony serwer Festival. Domyślnie: libdir (string) ścieżka do katalogu zawierającego pliki Festivala. Domyślnie: "/usr/local/festival/lib". Algorytm unikania kolizji 37

54 ROZDZIAŁ 5. OPIS STANOWISKA BADAWCZEGO Marek Majchrowski queuelen (integer) rozmiar kolejki. Domyślnie: 4. Przykład konfiguracji: driver ( name "festival" provides ["speech:0"] libdir "/usr/share/festival" # language "polish" ) Na dołączonej płycie CD zamieściłem łatkę (patch.player festival.diff), która naprawia sterownik festival. Powoduje ona, że sterownik ten nie obciąża tak procesora, poprawnie czeka na zamknięcie procesów serwera Festival oraz dodaje obsługę dodatkowej opcji w pliku konfiguracyjnym: language (string) nazwa języka w jakim Festival ma generować mowę (przekazywana jako parametr language do serwera Festival). sphinx2 sterownik ten zapewnia dostęp do systemu rozpoznawania mowy Sphinx2 [19]. Sphinx2 jest dostępny jako osobny pakiet, który należy wcześniej zainstalować. Sterownik ten udostępnia interfejs speech_recognition. Pozwala na podanie następujących opcji w pliku konfiguracyjnym (wszystkie opcje odnoszą się do katalogów/plików wymaganych w celu uruchomienia systemu Sphinx2): hmm_dir (string) domyślnie: "/usr/local/share/sphinx2/model/hmm/6k", map_file (string) domyślnie: "/usr/local/share/sphinx2/model/hmm/6k/map", phone_file (string) domyślnie: "/usr/local/share/sphinx2/model/hmm/6k/phone", noise_file (string) domyślnie: "/usr/local/share/sphinx2/model/hmm/6k/noisedict", sendump_file (string) domyślnie: "/usr/local/share/sphinx2/model/hmm/6k/sendump", task_dir (string) domyślnie: "/usr/local/share/sphinx2/model/lm/turtle", lm_file (string) domyślnie: "/usr/local/share/sphinx2/model/lm/turtle/turtle.lm", dict_file (string) domyślnie: "/usr/local/share/sphinx2/model/lm/turtle/turtle.dict". Na dołączonej płycie CD zamieściłem łatkę (patch.player sphinx2.diff), która naprawia sterownik sphinx2: dodaje kolejne dwa parametry wymagane w celu uruchomienia systemu Sphinx2. cep_dir domyślnie: "/usr/local/share/sphinx2/model/lm/turtle/ctl" data_dir domyślnie: "/usr/local/share/sphinx2/model/lm/turtle/ctl" Przykład konfiguracji: 38 Algorytm unikania kolizji

55 Marek Majchrowski ROZDZIAŁ 5. OPIS STANOWISKA BADAWCZEGO driver ( name "sphinx2" provides ["speech_recognition:0"] hmm_dir "/usr/share/sphinx2/model/hmm/6k" map_file "/usr/share/sphinx2/model/hmm/6k/map" phone_file "/usr/share/sphinx2/model/hmm/6k/phone" noise_file "/usr/share/sphinx2/model/hmm/6k/noisedict" sendump_file "/usr/share/sphinx2/model/hmm/6k/sendump" task_dir "/usr/share/sphinx2/model/lm/turtle" lm_file "/usr/share/sphinx2/model/lm/turtle/turtle.lm" dict_file "/usr/share/sphinx2/model/lm/turtle/turtle.dic" cep_dir "/usr/share/sphinx2/model/lm/turtle/ctl" data_dir "/usr/share/sphinx2/model/lm/turtle/ctl" ) sicklms200 sterownik ten kontroluje skaner laserowy SICK LMS 200. Umożliwia zmiany konfiguracji w trakcie pracy, udostępnia interfejs laser. Pozwala na odczyt położenia lasera i odczyt jego konfiguracji. Można podać następujące opcje w pliku konfiguracyjnym: port (string) port szeregowy, pod który jest podłączony dany laser. Domyślnie: "/dev/ttys1". rate (integer) prędkość transmisji danych z lasera. Domyślnie: Dozwolone wartości: 9600, (dla RS232 i RS422) oraz (tylko dla RS422). delay (integer) opóźnienie pomiędzy uruchomieniem sterownika a inicjalizacją lasera. Domyślnie: 0. resolution (integer) rozdzielczość kątowa skanowania. Domyślnie: 50. Dozwolone wartości: 50 odczyt co 0,5 stopnia, w sumie 361 odczytów z częstotliwością 5Hz (dla prędkości transmisji 38400) albo z częstotliwością 32Hz (dla prędkości transmisji ). 100 odczyt co 1 stopień, w sumie 181 odczytów z częstotliwością 10Hz (dla prędkości transmisji 38400) albo z częstotliwością 75Hz (dla prędkości transmisji ). range_res (integer) zakres skanowania. Domyślnie 1. Dozwolone wartości: 1 dokładność 1mm, maksymalny zasięg 8,192m. 10 dokładność 10mm, maksymalny zasięg 81,92m. 100 dokładność 100mm, maksymalny zasięg 819,2m. invert (integer) czy odczyty należy odwrócić (bo np. skaner stoi do góry nogami). Domyślnie: 0, czyli nie. pose (length tuple) pozycja skanera względem robota. Domyślnie: [ ]. Przykład konfiguracji: Algorytm unikania kolizji 39

56 ROZDZIAŁ 5. OPIS STANOWISKA BADAWCZEGO Marek Majchrowski driver ( name "sicklms200" provides ["laser:0"] port "/dev/ttys0" resolution 100 range_res 10 ) Na dołączonej płycie CD zamieściłem łatkę (patch.player sicklms200.diff), która poprawia sterownik pod względem niezawodności oraz komunikacji przez port szeregowy. linuxwifi sterownik ten zapewnia dostęp do informacji o karcie bezprzewodowej w systemie Linux. Bazuje na informacjach z /proc/net/wireless. Udostępnia interfejs wifi. iwspy sterownik ten udostępnia informacje o sile sygnału z bezprzewodowej karty. Udostępnia interfejs wifi. Wymaga podania pewnych opcji w pliku konfiguracyjnym. lifomcom sterownik ten udostępnia kolejki LIFO, w celu wymiany informacji pomiędzy klientami serwera Player. Implementuje interfejs mcom. amcl sterownik ten implementuje algorytm adaptacyjnej lokalizacji Monte-Carlo opisanej przez Dietera Foxa. Implementuje interfejsy localize oraz position. vfh sterownik ten implementuje lokalną metodę nawigacji VFH+ (Vector Field Histogram Plus) zaproponowaną przez Ulricha i Borensteina Sterownik Protonek Sterownik ten ma formę wtyczki do serwera Player, udostępnia interfejs position dla robota mobilnego Protonek. Został on opracowany przeze mnie. Sterownik ten komunikuje się z modułem jądra Linuxa napisanym przez Piotra Trojanka [15]. Do komunikacji wykorzystuje funkcję ioctl. Protokół komunikacyjny opracowany przez nas zamieszczam w dodatku tej pracy. Sterownik ten pozwala na: ustawienie prędkościowego trybu sterowania robotem, ustawienie pozycyjnego trybu sterowania robotem, odczyt odometrii, wyzerowanie odczytów odometrii, ustawienie pozycji robota poprzez podmianę odczytów odometrii, odczytanie rozmiarów robota. 40 Algorytm unikania kolizji

57 Marek Majchrowski ROZDZIAŁ 5. OPIS STANOWISKA BADAWCZEGO Plik konfiguracyjny oprócz 3 standardowych linii powinien zawierać definicję następujących parametrów: max_speed maksymalna prędkość liniowa robota w [m/s], domyślna wartość ticks_per_rev ilość impulsów enkodera na jeden obrót koła, domyślna wartość max_enc_ticks maksymalna prędkość jaką może uzyskać silnik, w wewnętrznych jednostkach regulatora scalonego LM629, domyślna wartość robot_length długość robota w mm, domyślna wartość 500. robot_width szerokość robota w mm, domyślna wartość 360. wheel_diam średnica koła robota w m, domyślna wartość robot_axle_length długość osi napędowej robota w mm, domyślna wartość 330. driver ( name "protonek" plugin "protonekdriver.so" provides ["position:0"] max_speed ticks_per_rev 4000 max_enc_ticks robot_length 500 robot_width 360 wheel_diam 0.10 robot_axle_length 330 ) Kod sterownika (plik protonekdriver.tar.gz) znajduje się na dołączonej do pracy płycie CD, w katalogu protonek Dostęp do urzadzeń Kiedy klient czyta z urządzenia otrzymuje od niego aktualne dane. Format tych danych zależy od interfejsu jaki udostępnia dany sterownik. Domyślnie każdy klient będzie otrzymywał nowe dane z każdego urządzenia, do którego jest podłączony, z częstotliwością 10Hz. Ponieważ wartość ta może nie być odpowiednia dla wszystkich klientów, możliwa jest zmiana tej częstotliwości, jak również możliwe jest przełączenie serwera w tryb Request/Reply. Należy pamiętać, że serwer nie przechowuje wszystkich danych, jedynie tę najświeższą. Częstotliwość, z jaką klient chce obierać dane od serwera, jest definiowana dla każdego klienta z osobna. Są dostępne 4 tryby przesyłania danych: Algorytm unikania kolizji 41

58 ROZDZIAŁ 5. OPIS STANOWISKA BADAWCZEGO Marek Majchrowski PLAYER_DATAMODE_PUSH_ALL dane są wysyłane okresowo do klienta z wszystkich sterowników aktualnie otwartych przez klienta. PLAYER_DATAMODE_PULL_ALL dane są wysyłane na żądanie klienta z wszystkich sterowników aktualnie otwartych przez klienta. PLAYER_DATAMODE_PUSH_NEW dane są wysyłane okresowo do klienta jedynie z tych sterowników, które są aktualnie otwarte przez klient i które wygenerowały nowe dane w porównaniu z poprzednim odczytem. PLAYER_DATAMODE_PULL_NEW dane są wysyłane na żądanie klienta jedynie z tych sterowników, które są aktualnie otwarte przez klient i które wygenerowały nowe dane z porównaniu z poprzednim odczytem. Domyślnym trybem jest aktualnie PLAYER_DATAMODE_PUSH_NEW. Jest oczywiście możliwe, że dane generowane będą szybciej przez urządzenie niż program kliencki będzie je odczytywał z serwera. W szczególności nie ma żadnego sposobu, aby sterownik poinformował klienta, że nowe dane zostały wygenerowane. Kiedy klient zapisuje dane do urządzenia, wysyła do niego nową komendę. Format tych komend zależy od interfejsu jaki udostępnia dany sterownik. Nie ma żadnej gwarancji, że komenda wysłana przez klienta trafi rzeczywiście do urządzenia. Player nie zapewnia żadnych mechanizmów synchronizacji w przypadku, gdy wielu klientów zechce równolegle pisać do tego samego urządzenia. Nie ma żadnego kolejkowania komend, a każda nowa komenda nadpisze starą. W przypadku, gdy chcemy zapewnić jeszcze jakąś funkcjonalność dla urządzenia, a nie podpada ona pod wysyłanie komend oraz odbieranie danych, możemy to zrobić przez mechanizm konfiguracji. W przeciwieństwie do wysyłania komend i odbierania danych, które są jednokierunkowym oraz asynchronicznym mechanizmem komunikacji, mechanizm konfiguracji w Playerze zapewnia synchroniczną oraz dwukierunkową komunikację, opartą na zasadzie żądania i odpowiedzi, pomiędzy klientem a urządzeniem. W takim przypadku mamy zagwarantowane, że wszystkie wiadomości wysłane od klienta, będą obsłużone przez sterownik urządzenia. W ogólności wszystkie interakcje z urządzeniem mogą zostać zaimplementowane wykorzystując mechanizm konfiguracji Format przesyłanych danych Klienci oraz serwer komunikują się wykorzystując prosty symetryczny protokół. Każda wiadomość składa się z dwóch części: nagłówka oraz danych. Poniżej przedstawiony został format nagłówka oraz danych dla 4 różnych typów wiadomości. Nagłówek Nagłówek składa się z 32 bajtów opisujących w jaki sposób należy interpretować dane znajdujące się za nagłówkiem. Poniżej przedstawione są kolejne pola nagłówka w kolejności od 42 Algorytm unikania kolizji

59 Marek Majchrowski ROZDZIAŁ 5. OPIS STANOWISKA BADAWCZEGO bajtu zerowego do trzydziestego pierwszego. STX 2 bajty. Specjalny symbol oznaczający początek wiadomości. Zawsze ma wartość: 0x5878. type 2 bajty. Opisuje tym wiadomości. Jest 7 typów wiadomości w protokole Playera. Dopuszczalne wartości: 0x0001 wiadomość zawierająca dane, 0x0002 wiadomość zawierająca komendę, 0x0003 wiadomość zawierająca konfigurację, 0x0004 potwierdzenie otrzymania wiadomości, 0x0005 wiadomość synchronizacyjna, 0x0006 negatywne potwierdzenie otrzymania wiadomości, 0x0007 błąd otrzymania wiadomości. device 2 bajty. Opisuje rodzaj interfejsu, do którego dana wiadomość się odnosi. index - 2 bajty. Opisuje konkretne urządzenie, do którego dana wiadomość się odnosi. Przydatne, gdy robot wyposażony jest w dwa urządzenia udostępniające ten sam interfejs. Wtedy identyfikacji następuje po indeksie. (t_sec, t_usec) 8 bajtów ustawianych wyłącznie przez serwer. Znacznik czasu serwera odpowiadający temu ze struktury danych struct timeval. (ts_sec, ts_usec) 8 bajtów ustawianych wyłącznie przez serwer. Znacznik czasu danych odpowiadający temu ze struktury danych struct timeval. Może być interpretowany, jako czas, w którym sensor dokonał odczytu danych. reserved 4 bajty. Nie używane. size 4 bajty. Opisuje wielkość danych dołączonych za nagłówkiem Programy klienckie Tworzenie programów klienckich wykorzystujących Playera jest bardzo proste. Można wykorzystać jedną z poniższych bibliotek: C client library podstawowa biblioteka w C, nie powinna być bezpośrednio używana, C++ client library biblioteka języka C++ udostępniająca pełną funkcjonalność, libplayerc biblioteka języka C udostępniająca pełną funkcjonalność, libplayerc_py biblioteka wiążąca Pythona z biblioteką libplayerc. Algorytm unikania kolizji 43

60 ROZDZIAŁ 5. OPIS STANOWISKA BADAWCZEGO Marek Majchrowski C++ client library Biblioteka ta jest generalnie najbardziej rozbudowana, ponieważ jest używana do testowania nowych funkcji, które są implementowane w serwerze. Jest także najszerzej stosowaną biblioteką. Biblioteka ta oparta jest na obiektach typu Proxy. W modelu tym klient tworzy sobie lokalne obiekty, które są pośrednikami do odległych urządzeń. Są dwa rodzaje takich lokalnych obiektów: specjalny obiekt PlayerClient opisujący sam serwer, oraz wiele obiektów proxy dla poszczególnych interfejsów. Każdy obiekt proxy jest zaimplementowany jako osobna klasa. Użytkownik powinien najpierw stworzyć obiekt typu PlayerClient, a następnie użyć go do nawiązania połączenie z serwerem Player. Potem odpowiednie obiekty proxy dla konkretnych interfejsów powinny zostać utworzone, a następnie zainicjalizowane wykorzystując istniejący już obiekt PlayerClient. Poniżej przedstawię przykładowy program wykorzystujący tę bibliotekę. Dla jasności kodu pominięte zostało sprawdzanie błędów. #include <playerclient.h> #include <stdlib.h> int main(int argc, char *argv[]) { PlayerClient robot("localhost"); SonarProxy sp(&robot, 0, r ); PositionProxy pp(&robot, 0, w ); double newturnrate, newspeed; for (int i=0; i < 1000; i++) { if (robot.read()) exit(1); // print out sonars for fun sp.print(); // do simple collision avoidance if((sp.ranges[0] + sp.ranges[1]) < (sp.ranges[6] + sp.ranges[7])) newturnrate = DTOR(-20); // turn 20 degrees per second else newturnrate = DTOR(20); if(sp.ranges[3] < 0.500) newspeed = 0; else newspeed = 0.100; } } // command the motors pp.setspeed(newspeed,newturnrate); 44 Algorytm unikania kolizji

61 Program ten przedstawia, zaczerpnięty z dokumentacji Playera, przykład jak w prosty (ale zły jak piszą jego autorzy) sposób można unikać kolizji wykorzystując jedynie informacje z czujników sonarowych. Na początku jest tworzony obiekt typu PlayerClient wykorzystując domyślny konstruktor do połączenia się na serwerem Playera na lokalnej maszynie na porcie Następnie tworzone są odpowiednio obiekty proxy dla czujników sonarowych (SonarProxy) oraz do kontrolowania silników robota (PositionProxy). Konstruktory dla tych obiektów wykorzystują istniejący już obiekt PlayerClient w celu nawiązania połączenia typu read (odczyt z czujników) oraz typu write (zapis do efektorów) odpowiednio dla sterownika czujników sonarowych oraz dla sterownika silników robota. Oba te urządzenia są dostępne pod zerowym indeksem dla odpowiadającego im interfejsu. Następnie jest krótka pętla, w której czytamy nowy stan czujników sonarowych i wysyłamy odpowiednie komendy do silników. Dokładnego opis wszystkich bibliotek oraz przykładowych programów wykorzystujących te biblioteki zawiera dokumentacja Playera [27] Łaty Wszystkie łaty znajdują się w katalogu player-patch na dołączonej do pracy płycie CD. Dodatkowo znajduje się tam jedna duża łata (patch.player diff), która przenosi naniesione poprawki z Playera w wersji na wersję Akwizycja danych pomiarowych W poniższym podrozdziale przedstawiono problematykę akwizycji danych pomiarowych z czujników w systemie Player [27], Opisano sterownik, który powstał do akwizycji danych z wykorzystaniem ww systemu Czujniki podczerwieni IR W posiadanych przez nas robotach jednymi z głównych wykorzystywanych czujników są czujniki podczerwieni. Czujniki tego typu nie są zbyt precyzyjne. Ich głównym zastosowaniem jest dostarczenie informacji o istnieniu w otoczeniu robota jakiegoś obiektu. Z tą wiedzą robot będzie mógł zmodyfikować tak swoją trajektorię ruchu, aby ominąć ten obiekt. Spośród dostępnych na rynku czujników firmy Sharp [26] w naszym robocie zastosowano dwa modele (rys. 5.5): GP2D120 jest to cyfrowy czujnik odległości z wyjściem analogowym. Czujnik ma (według producenta) zakres pomiarowy od 4 cm do 30 cm, a błąd pomiaru wynosi 0,5 cm. GP2Y0A02 jest to także cyfrowy czujnik odległości podobny do czujnika GP2D120 tylko z zakresem pomiarowym od 20 cm do 150 cm. Algorytm unikania kolizji 45

62 ROZDZIAŁ 5. OPIS STANOWISKA BADAWCZEGO Marek Majchrowski a) b) Rysunek 5.5: Czujniki zbliżeniowe na podczerwień: a) GP2D120, b) GP2Y0A02 Zasada działania czujników IR firmy Sharp Wraz z pojawieniem się czujników firmy Sharp z linii GP2DXX, wprowadzono technikę pozwalającą wykryć obiekt w większej odległości niż było to osiągalne wcześniej. Możliwy jest także pomiar odległości do tego obiektu. Czujniki te oferują także lepszą odporność na warunki oświetleniowe. Nowe czujniki wykorzystują metodę triangulacji (rys. 5.6) oraz małą matrycę CCD w celu obliczenia odległości od obiektu i/lub stwierdzeniu obecności tego obiektu w polu widzenia tego czujnika. Zasada działania jest prosta: nadajnik wysyła wiązkę światła podczerwonego. Wiązka ta przebywa przestrzeń przed czujnikiem i albo odbija się od przeszkody, albo podąża dalej. W przypadku, gdy nie ma obiektu w zasięgu pracy czujnika wiązka ta nigdy nie zostanie odbita i odczyty nie wskażą żadnego obiektu. Natomiast w przypadku, gdy wiązka ta odbije się od przeszkody, powróci do detektora i utworzy trójkąt pomiędzy punktem odbicia (point of reflection), emiterem i detektorem. Rysunek 5.6: Pomiar odległości metodą triangulacji [26] 46 Algorytm unikania kolizji

63 Marek Majchrowski ROZDZIAŁ 5. OPIS STANOWISKA BADAWCZEGO Kąty w tym trójkącie zależą od odległości od obiektu. Częścią odbiornika w tych czujnikach jest precyzyjnie wykonaną soczewką, która przepuszcza odbitą wiązkę na odpowiedni punkt matrycy CCD właśnie w zależności od kąta odbicia. Na podstawie odczytu z matrycy CCD wyznaczana jest właściwa odległość od przedmiotu. Metoda ta jest niemalże całkowicie odporna na zmienne warunki oświetleniowe oraz jest niewrażliwa na kolor obiektu. Wykorzystując trygonometrię można wykazać, że odległość, pomiędzy czujnikiem a przeszkodą, nie jest funkcją liniową. Ma ona raczej charakter logarytmiczny, dlatego warto znormalizować wyjścia tych czujników za pomocą LUTa lub wyznaczyć przybliżony wzór takiej funkcji. W przyjętym przeze mnie rozwiązaniu postanowiłem wyznaczyć przybliżony wzór funkcji. W tym celu zebrałem charakterystykę kilku czujników jednego rodzaju, za pomocą skonstruowanego przez siebie stanowiska pomiarowego, a następnie aproksymowałem je wielomianem odpowiedniego stopnia. Charakterystyka czujników GP2D120 Dla czujnika krótkiego zasięgu wykonano 50 pomiarów co 1 cm, począwszy od odległości 1 cm od przeszkody, aż do odległości 50 cm. Poniżej zamieszczam charakterystykę przedstawioną przez producenta. Rysunek 5.7: Napięcie w funkcji odległości dla czujnika GP2D120 [26] Pominąwszy fakt, że informacją użyteczną jest funkcją odwrotną do zamieszczonej na powyższym wykresie (interesuje nas charakterystyka odległości w funkcji napięcia), widać Algorytm unikania kolizji 47

64 ROZDZIAŁ 5. OPIS STANOWISKA BADAWCZEGO Marek Majchrowski że czujniki te mają jedną wadę: otóż przeszkody bliższe niż 4 cm będą rozpoznawane jako przedmioty znajdujące się dalej niż są w rzeczywistości. Poniżej zamieszczam charakterystykę wyznaczoną podczas pomiarów Odleglosc od przeszkody Wartosci z przetwornika A/D Rysunek 5.8: Odległość w funkcji odczytów z przetwornika A/D dla czujnika GP2D120 Na rysunku tym znajduje się 5 zdjętych charakterystyk z 5 różnych czujników oraz aproksymowana charakterystyka (ta krzywa, dla której występują odległości większe niż 50 cm). Funkcja aproksymująca ma postać wielomianu 7 stopnia: f (x) = 7 i=0 x i gp2d120 i, gdzie gp2d120 i to współczynniki wielomianu podane poniżej w formie pliku nagłówkowego języka C. #ifndef GP2D120_H #define GP2D120_H #define gp2d120_ e-13 #define gp2d120_ e-10 #define gp2d120_ e-07 #define gp2d120_ e-05 #define gp2d120_ e-03 #define gp2d120_ e-01 #define gp2d120_ e+00 #define gp2d120_ e+01 #endif /* GP2D120_H */ 48 Algorytm unikania kolizji

65 Marek Majchrowski ROZDZIAŁ 5. OPIS STANOWISKA BADAWCZEGO Jak widać z wykresu efektywny zasięg tych czujników wynosi od 5cm do 50cm. Dla bezpieczeństwa sczytane wartości powyżej 140 będą ignorowane. Charakterystyka czujników GP2Y0A02 Dla czujnika dalekiego zasięgu wykonano 45 pomiarów co 4 cm, począwszy od odległości 20 cm od przeszkody, aż do odległości 200 cm. Poniżej zamieszczam charakterystykę przedstawioną przez producenta. Rysunek 5.9: Napięcie w funkcji odległości dla czujnika GP2Y0A02 [26] Tu także widać, podobnie jak w przypadku czujnika typu GP2D120, że czujniki te mają podobną wadę: otóż przeszkody bliższe niż 20 cm będą rozpoznawane jako przedmioty znajdujące się dalej niż są w rzeczywistości. Na rysunku 5.10 zamieszczam charakterystykę wyznaczoną podczas pomiarów. Na rysunku tym znajduje się 5 zdjętych charakterystyk z 5 różnych czujników, oraz aproksymowana charakterystyka (ta krzywa, dla której występują odległości większe niż 200 cm). Funkcja aproksymująca ma postać wielomianu 7 stopnia: f (x) = 7 i=0 x i gp2y0a02 i, gdzie gp2y0a02 i to współczynniki wielomianu podane poniżej w formie pliku nagłówkowego języka C. Algorytm unikania kolizji 49

66 ROZDZIAŁ 5. OPIS STANOWISKA BADAWCZEGO Marek Majchrowski Odleglosc od przeszkody Wartosci z przetwornika A/D Rysunek 5.10: Odległość w funkcji odczytów z przetwornika A/D dla czujnika GP2Y0A02 #ifndef GP2Y0A02_H #define GP2Y0A02_H #define gp2y0a02_ e-11 #define gp2y0a02_ e-08 #define gp2y0a02_ e-06 #define gp2y0a02_ e-04 #define gp2y0a02_ #define gp2y0a02_ #define gp2y0a02_ #define gp2y0a02_ e+02 #endif /* GP2Y0A02_H */ Jak widać z wykresu efektywny zasięg tych czujników wynosi od 30cm do 150cm, przy czym charakterystyka odległości dla obiektów znajdujących się powyżej 100cm jest mocno zależna od posiadanego przez nas egzemplarza. Także i w tym przypadku dla bezpieczeństwa sczytane wartości powyżej 140 będą ignorowane. Sposób pobierania danych Każdy z robotów może być wyposażony maksymalnie w 24 czujniki analogowe. Każdy z czujników jest podłączony do 1 z 4 mikrokomputerów PIC16F688. Mikrokontrolery te są połączone magistralą RS 485 z urządzeniem nadrzędnym oraz posiadają wbudowany przetwornik A/D. 50 Algorytm unikania kolizji

67 Marek Majchrowski ROZDZIAŁ 5. OPIS STANOWISKA BADAWCZEGO Komunikacja po magistrali RS 485 jest 9 bitowa. Pakiet z rozkazem wysłany do mikrokomputerów PIC16F688 musi mieć ustawiony owy 9 bit, natomiast pakiety zwrotne zgaszony. W ten sposób inne mikrokomputery rozróżnią pakiet z rozkazem od pakietu z danymi. Aby mikrokontrolery mogły rozróżniać, które pakiety są do nich, dwa najstarsze bity z pozostałych 8 bitów oznaczają numer układu (tj układ, 01 2 układ, 10 3 układ, 11 4 układ). Pozostałe 6 bitów jest przeznaczone na informację, które odczyty z podłączonych 6 czujników nas interesują. W ten sposób w jednym pakiecie możemy zapytać się zarówno o 1 czujnik jak i o wszystkie 6 czujników podłączonych do danego układu. W momencie rozpoznania komendy przez konkretny mikrokontroler wysyła on informacje zwrotną, którą są kolejne bajty. Są one odczytami z czujników, o które pytano. Odpowiedź jest udzielana począwszy od czujnika o najmniejszym numerze. Czujnik ten jest reprezentowany jako najmłodszy bit w pakiecie nadawczym do mikrokomputerów PIC16F688. Mikrokomputer PIC16F688 zbiera w pętli odczyty z podłączonych (bądź nie) czujników. W celu oszczędzania energii wprowadzono następujące założenia: jeśli o dany czujnik nie przyjdzie zapytanie z układu nadrzędnego przez co najmniej 1 sekundę dany kanał zostanie wyłączony (aby nie dokonywać zbędnych odczytów). Ponowne włączenie kanału nastąpi po pierwszym nowym zapytaniu o niego. W ten sposób, żaden z układów nie musi wiedzieć, do których wejść analogowych są podłączone czujniki, a do których nie. Wystarczy, że układ nadrzędny będzie o tym wiedział i nie będzie się pytał o wejścia, do których nic nie jest podłączone. W każdym z robotów magistrala RS 485 nie jest połączona bezpośrednio do komputera pokładowego klasy PC. Układy pomiarowe są tą magistralą podłączone z układem nadrzędnym, którym jest sterownik silników w standardzie PC104 wyposażony w mikrokontroler klasy Dopiero sterownik ten komunikuje się z komputerem pokładowym robota. Układ ten działa na identycznych zasadach co mikrokomputer PIC16F688 tzn. na samym początku odpytuje wszystkie czujniki co 40 ms. Jeśli w ciągu 25 kolejnych odczytów nie przyjdzie zapytanie z komputera pokładowego o dany czujnik, sterownik przestaje o niego pytać układ podrzędny, który po kolejnej sekundzie wyłącza dany przetwornik. System Player a czujniki podczerwieni System Player udostępnia interfejs ir, który zapewnia dostęp do zestawu czujników podczerwieni. Nie przyjmuje on żadnych komend. Umożliwia odpytanie serwera o pozycję czujników. Daje możliwość sterowania zasilaniem czujników oraz odbierania odczytów z tych czujników. Domyślnie, sterowanie zasilaniem czujników IR dotyczy wszystkich czujników danej klasy. Wydawało się rozsądne, aby wprowadzić pewną modyfikację. Używając tej samej struktury (tj. dwóch bajtów: bajt 1. opisuje typ danego żądania tu musi być zawsze wpisane PLAYER_IR_POWER_REQ natomiast bajt 2. domyślnie oznacza: 0 wyłącz a 1 włącz) zmodyfikowaliśmy znaczenie 2-go bajtu. W zaimplementowanym przeze mnie sterowniku do systemu Player, obsługującym dane czujniki IR, 2. bajt oznacza włączenie, bądź wyłączenie jednego (konkretnego) z czujników. Młodsze 4 bity tego bajtu są przeznaczone na numer czujnika zgodny z konfiguracją w pliku konfiguracyjnym dla danego sterownika. Natomiast 4 starsze bity będąc samymi jedynkami oznaczają włączenie danego czujnika, będąc samymi Algorytm unikania kolizji 51

68 ROZDZIAŁ 5. OPIS STANOWISKA BADAWCZEGO Marek Majchrowski zerami wyłączenie danego czujnika. Plik konfiguracyjny oprócz 3 standardowych linii powinien zawierać definicję czujników, na którą składa się: sensors oznacza liczbę czujników, sensorxx oznacza definicję czujnika XX, przy czym czujnikowi numer 1 odpowiada wpis sensor01 a nie sensor1. driver ( name "protonek_ir" plugin "protonekdriver_ir.so" provides ["ir:0"] sensors 18 # sensorsxx [ x[mm] y[mm] t[deg] PIC[1-4] io[1-6] state[0 - off, 1 - on] # kind[0 - GP2D120, 1 - GP2Y0A02] ] sensor01 [ ] sensor02 [ ] sensor03 [ ] sensor04 [ ] sensor05 [ ] sensor06 [ ] sensor07 [ ] sensor08 [ ] sensor09 [ ] sensor10 [ ] sensor11 [ ] sensor12 [ ] sensor13 [ ] sensor14 [ ] sensor15 [ ] sensor16 [ ] sensor17 [ ] sensor18 [ ] ) Każda linia odpowiadająca definicji czujnika XX składa się z sześciu wartości liczbowych oznaczających kolejno: położenie czujnika wzdłuż osi OX względem środka robota wyrażone w milimetrach, położenie czujnika wzdłuż osi OY względem środka robota wyrażone w milimetrach, orientacja czujnika względem orientacji robota wyrażona w stopniach, numer układu PIC16F688, do którego dany czujnik jest podłączony, wartość z zakresu 1 4, numer wejścia w układzie PIC16F688, do którego dany czujnik jest podłączony, wartość z zakresu 1 6, stan początkowy czujnika, 1 czujnik włączony, 0 czujnik wyłączony. rodzaj podłączonego czujnika: 1 GP2D120, 0 GP2Y0A Algorytm unikania kolizji

69 Marek Majchrowski ROZDZIAŁ 5. OPIS STANOWISKA BADAWCZEGO W momencie, gdy dany czujnik jest wyłączony, driver nie będzie się pytał układu podrzędnego (sterownika silników) o odczyty z tego czujnika, mimo, że takie żądania mogą wystąpić, aż do momentu włączenia programowego tego czujnika w strukturze Player. W ten sposób spośród trzech urządzeń (driver, sterownik, układy pomiarowe) biorących udział w pozyskiwaniu informacji z czujników podczerwieni, tylko sterownik systemu Player musi zdawać sobie sprawę z położenia czujników (fizycznego jak i ich podłączeń pod odpowiednie układy). Pozostałe dwa układy działają bez tej wiedzy, opierając się jedynie na czasie, w którym nie było pytania o dany czujnik. W taki sposób, po 2 sekundach wszystkie czujniki zostaną wyłączone (nie będą pobierać prądu), jeśli nikt ich nie zechce używać ze struktury Player. Kod sterownika (plik protonekdriver_ir.tar.gz) znajduje się, na dołączonej do pracy płycie CD, w katalogu protonek Odometria Układ odometrii przyrostowej znajdujący się w naszych robotach wykorzystuje impulsy z przetworników obrotowo impulsowych (enkoderów) sprzężonych mechanicznie z kołami robota. Przetworniki te umożliwiają generowanie impulsów, których względna liczba jest miarą położenia kątowego każdego z kół. Do wyznaczenia położenia i orientacji robota, na podstawie tych impulsów, konieczna jest znajomość równań kinematyki dwukołowego robota mobilnego (rysunek 5.11), dla którego prędkość liniowa v i kątowa ω wynosi: Rysunek 5.11: Dwukołowy robot mobilny o napędzie różnicowym podstawowe oznaczenia Algorytm unikania kolizji 53

70 ROZDZIAŁ 5. OPIS STANOWISKA BADAWCZEGO Marek Majchrowski v = v l + v p 2 ω = v p v l d gdzie v l i v p oznaczają odpowiednio liniową prędkość lewego i prawego koła, d oznacza rozstaw między kołami. Środek układu lokalnego robota znajduje się na osi kół w połowie odległości pomiędzy kołami. Na podstawie prędkości v oraz ω można zapisać prędkości robota w układzie bazowym: ω = θ v x = ẋ = v cos(θ) v y = ẏ = v sin(θ) gdzie v x i v y oznaczają składowe prędkości wyznaczone jako rzuty na poszczególne osie układu bazowego, a θ oznacza orientację robota. Zatem orientację i położenie robota w układzie bazowym można wyznaczyć w następujący sposób: Z θ = ωdt = Z vp v l dt d Z Z x = v x dt = v cos(θ)dt Z y = Z v y dt = v sin(θ)dt Należy pamiętać, że odometria jest najczęściej implementowana na układach cyfrowych. W związku z tym aby powyższe równania można było zastosować w praktyce należy zamienić je z równań ciągłych na równania różnicowe. Odometria w praktyce Metoda ta ma jednak podstawową wadę w przypadku choćby małych poślizgów czy nierówności powstające błędy orientacji bardzo szybko się kumulują. I tak na rysunkach 5.12 oraz 5.13 znajdują się wykresy orientacji robota w trakcie jazdy, zamieszczone na wykresie biegunowym (czas upływa im wykres bardziej się oddala od jego środka). Każdy z tych wykresów powinien się zakończyć orientacją końcową robota zgodną z początkową (w tym przypadku 0 stopni). W obu przypadkach czas jazdy wynosił zaledwie kilkadziesiąt sekund. Wykorzystując surową odometrię w trakcie tak krótkiej jazdy po laboratorium otrzymaliśmy ogromny błąd orientacji. 54 Algorytm unikania kolizji

71 Marek Majchrowski ROZDZIAŁ 5. OPIS STANOWISKA BADAWCZEGO 120 Wykres orientacji Rysunek 5.12: Orientacja robota wyznaczona na podstawie odometrii w trakcie jazdy po korytarzu 120 Wykres orientacji Rysunek 5.13: Orientacja robota wyznaczona na podstawie odometrii w trakcie omijania przeszkody Algorytm unikania kolizji 55

72 ROZDZIAŁ 5. OPIS STANOWISKA BADAWCZEGO Marek Majchrowski LODO Biblioteka LODO dostarcza funkcje do laserowej stabilizacji odometrii [18]. Jako dane wejściowe pobiera surowe odczyty z odometrii oraz skany z dalmierza laserowego SICK LMS 200. Na wyjściu otrzymujemy poprawione dane odometryczne. Biblioteka LODO bazuje na algorytmie inkrementacyjnego SLAMu wykorzystywanego do poprawiania danych odometrycznych. Dokładniejszy opis tej metody znajduje się w [18]. Na rysunkach 5.14 oraz 5.15 przedstawione są odpowiednio wykresy orientacji (wyznaczonej z danych odometrycznych pochodzących bezpośrednio od robota jak i tych stabilizowanych za pomocą czujnika laserowego) oraz ścieżki ruchu (wyznaczonej tak jak orientacji) w dwóch przypadkach. Wykres orientacji w trakcie jazdy robota Trajektoria ruchu robota Stabilizowana odometria LODO Surowa odometria y [m] Stabilizowana odometria LODO Surowa odometria x [m] Rysunek 5.14: Orientacja oraz droga przebyta przez robota wyznaczona na podstawie odometrii oraz algorytmu LODO w trakcie jazdy po okręgu Wykres orientacji w trakcie jazdy robota Trajektoria ruchu robota y [m] Stabilizowana odometria LODO Surowa odometria 270 Stabilizowana odometria LODO Surowa odometria x [m] Rysunek 5.15: Orientacja oraz droga przebyta przez robota wyznaczona na podstawie odometrii oraz algorytmu LODO w trakcie jazdy po ósemce W pierwszym jak i w drugim przypadku orientacja końcowa powinna się pokrywać z 56 Algorytm unikania kolizji

73 Marek Majchrowski ROZDZIAŁ 5. OPIS STANOWISKA BADAWCZEGO orientacją początkową robota. Wykorzystując algorytm LODO otrzymujemy prawie idealną zgodność orientacji. Natomiast w przypadku wykresu trajektorii ruchu robota niewielki błąd pozostaje pomimo wykorzystania algorytmu LODO. Widać to wyraźnie na drugim wykresie, gdzie robot powinien wykonać ruch po "ósemce" i zatrzymać się blisko pozycji startowej (różnica powinna wynosić niecałe 10 cm wzdłuż każdej z osi układu odniesienia). Na wykresie natomiast widzimy prawie idealną pozycję wzdłuż osi OX natomiast wzdłuż osi OY występuje błąd rzędu 30 cm. Algorytm unikania kolizji 57

74 ROZDZIAŁ 5. OPIS STANOWISKA BADAWCZEGO Marek Majchrowski 58 Algorytm unikania kolizji

75 Rozdział 6 Szczegółowy opis algorytmu unikania kolizji CVM 6.1 Szczegółowy opis algorytmu Pomimo uproszczenia, jakim jest przyjęcie, że przeszkody są reprezentowane przez okręgi, obliczenie funkcji D ogr (3.8) dla dużej liczby przeszkód, jest czasochłonne. Zauważmy, że dla danej przeszkody p odległość d c ((0, 0), p i ) jest nieskończona na zewnątrz krzywych stycznych do danej przeszkody. Stąd, wystarczy, że będziemy rozpatrywać jedynie krzywe leżące pomiędzy c min a c max (rys. 6.1). Aby wyznaczyć wartości c min, c max, P min (x min, y min ) oraz P max (x max, y max ) dla przeszkody p o środku w punkcie P(x 0, y 0 ) ((x 0, y 0 ) (0, 0)) i promieniu r 0 należy znaleźć takie r, aby poniższy układ równań, przedstawiający dwa okręgi, miał dokładnie jedno rozwiązanie: { (x x 0 ) 2 + (y y 0 ) 2 = r 2 0 (x r) 2 + y 2 = r 2 (6.1) Powyższy układ równań należy sprowadzić do równania kwadratowego jednej zmiennej i wyznaczyć wyróżnik tego równania. O tym, względem której zmiennej będziemy rozwiązywać to równanie kwadratowe, będzie decydować położenie przeszkody w układzie współrzędnych, czego powodem jest minimalizacja błędów numerycznych. Wyliczenia znajdują się w oraz Mając dane krzywe styczne do przeszkody, przybliżamy funkcję d v (3.6) funkcją stałą, dla wszystkich c z zakresu c [c min, c max ]: d v (ω, v, p) = { min(dcmin ((0, 0), P min ),d cmax ((0, 0), P max )) dla c min c c max w p. p. (6.2) W ten sposób, dla zbioru przeszkód, otrzymujemy zbiór przedziałów, każdy o stałej odległości. Każdy z tych przedziałów można reprezentować jako strukturę postaci: ([c 1, c 2 ],d 1, 2 ), gdzie c 1 i c 2 to krzywizny okręgów wyznaczających ten przedział (c 1 c 2 ), a d 1, 2 jest odległością do przeszkody wewnątrz tego przedziału. W dalszych rozważaniach przedział będzie rozumiany jako jego reprezentacja w postaci struktury typu: ([c 1, c 2 ],d 1, 2 ) Algorytm unikania kolizji 59

76 ROZDZIAŁ 6. SZCZEGÓŁOWY OPIS ALGORYTMU Marek Majchrowski Rysunek 6.1: Krzywe styczne do przeszkody p w punktach P min i P max Zaczynając od listy przedziałów zawierającej tylko przedział ([, ], L), dla każdej przeszkody wyznaczamy przedział ([c min, c max ],d) i dodajemy ten przedział do listy według zasady: dla każdego przedziału ([c 1, c 2 ],d 1, 2 ) z listy: sprawdź w jakich zależnościach jest ten przedział z nowo dodawanym. Jeśli: są to przedziały rozłączne nic nie rób, przedział z listy zawiera się w nowo dodawanym przedziale (c 1 c min i c 2 c max ) ustaw odległość d 1, 2 = min(d,d 1, 2 ), przedział z listy zawiera nowo dodawany przedział (c 1 c min i c 2 c max ) jeśli d < d 1, 2, to podziel istniejący przedział na trzy: ([c 1, c min ],d 1, 2 ), ([c min, c max ],d) oraz ([c max, c 2 ],d 1, 2 ); w przeciwnym przypadku nic nie rób, przedziały na siebie zachodzą jeśli d < d 1, 2, to podziel istniejący przedział na dwa, a wynik uzależnij od tego, które brzegi przedziałów na siebie zachodziły, tzn. dla c max > c 2 podziel na przedziały ([c 1, c min ],d 1, 2 ) i ([c min, c 2 ],d), w przeciwnym przypadku na przedziały ([c 1, c max ],d) i ([c max, c 2 ],d 1, 2 ); jeśli d d 1, 2, to nic nie rób. 60 Algorytm unikania kolizji

77 Marek Majchrowski ROZDZIAŁ 6. SZCZEGÓŁOWY OPIS ALGORYTMU Rysunek 6.2: Przeszkody i graficzna interpretacji listy przedziałów, które z nich powstały Następnie należy połączyć sąsiadujące przedziały, które mają tę samą odległość. W ten sposób otrzymujemy listę rozłącznych przedziałów w przestrzeni prędkości. Geometrycznie, każdy przedział w takiej liście definiuje parę prostych w przestrzeni prędkości (rys. 6.2). Przybliżenie zbioru długości krzywych z zakresu [c min, c max ] pojedynczą wartością nie daje dobrych wyników (rys. 6.3). W pewnych sytuacjach przybliżenie takie jest zbyt restrykcyjne, a w innych jest zbyt słabe i może powodować problemy (sytuacja, w której rzeczywista odległość od przeszkody jest mniejsza niż ta, wyliczona z definicji (6.2)). Jednym z rozwiązań tego problemu jest podzielenie przedziału [c min, c max ] na podprzedziały, zastosowanie równania (6.2) dla tych podprzedziałów, a następnie dodanie ich do listy zawierającej wszystkie takie przedziały w sposób omówiony wyżej. Rozwiązanie, które przyjęto, polega na wyznaczeniu punktu, na okręgu opisującym przeszkodę, do którego odległość od robota jest najmniejsza, w metryce euklidesowej. Następnie okrąg ten jest dzielony na k segmentów, które wyznaczają nowe punkty, (rys. 6.4) począwszy od wyznaczonego wcześniej punktu. Dla każdych dwóch sąsiadujących punktów leżących pomiędzy punktami, wyznaczonymi przez krzywe styczne do przeszkody, tworzony jest przedział ([c 1, c 2 ],d 1, 2 ) i dodawany do listy w sposób omówiony wyżej. c 1 i c 2 to krzywizny okręgów przechodzących przez te dwa punkty, a jako odległość d 1, 2 brana jest mniejsza z odległości liczonych jako droga z równania 3.10, wzdłuż krzywych c 1 i c 2. Dla przykładu z rysunku 6.4 dodane zostaną cztery przedziały, zamiast jednego. Algorytm unikania kolizji 61

78 ROZDZIAŁ 6. SZCZEGÓŁOWY OPIS ALGORYTMU Marek Majchrowski Rysunek 6.3: Zbiór krzywych z zakresu [c min, c max ] o różnych długościach o danej przeszkody p Równanie kwadratowe względem zmiennej x, czyli y 0 > x 0 Wyróżnik równania kwadratowego, które powstało po sprowadzeniu układu równań 6.1 do równania kwadratowego zmiennej x, dany jest wzorem: = y 2 ( 0 4x r0) 2 r 2 + y 2 ( 0 y 2 0 4x 0 x0 2 4x 0 r04x 2 ) 0 r+ + y 2 ( ( 0 x 2 0 2r 2 0 2y 2 0 x0) 2 y r0 2 ( 2y 2 0 r0 2 )) Rozwiązując równanie kwadratowe = 0 względem zmiennej r otrzymujemy: r max = x2 0 + y2 0 r2 0 2x 0 2r 0 r min = x2 0 + y2 0 r2 0 2x 0 + 2r 0 (6.3) c min = 1 r max = 2x 0 2r 0 x y2 0 r2 0 c max = 1 r min = 2x 0 + 2r 0 x y2 0 r2 0 (6.4) Punkty styczności okręgów P min (x min, y min ) oraz P max (x max, y max ) (podstawiając za r odpowiednio r min oraz r max ): x = x3 0 +rr2 0 rx2 0 +ry2 0 r2 0 x 0+x 0 y 2 0 4rx 0 +2r 2 +2x0 2+2y2 0 dla y y = 2rx+2xx 0+r (6.5) x2 0 y2 0 2y 0 62 Algorytm unikania kolizji

79 Marek Majchrowski ROZDZIAŁ 6. SZCZEGÓŁOWY OPIS ALGORYTMU Rysunek 6.4: Sposób podziału okręgu opisującego przeszkodę na k = 8 części Punkty przecięcia okręgów dla > 0 oraz danego r można wyznaczyć ze wzorów: x 1 = x3 0 +rr2 0 rx2 0 +ry2 0 r2 0 x 0+x 0 y 2 0 4rx 0 +2r 2 +2x y2 0 y 1 = 2rx 1+2x 1 x 0 +r 2 0 x2 0 y2 0 2y 0 dla y 0 0 (6.6) x 2 = x3 0 +rr2 0 rx2 0 +ry2 0 r2 0 x 0+x 0 y rx 0 +2r 2 +2x y2 0 y 2 = 2rx 2+2x 2 x 0 +r 2 0 x2 0 y2 0 2y 0 dla y 0 0 (6.7) Równanie kwadratowe względem zmiennej y, czyli y 0 x 0 Wyróżnik równania kwadratowego, które powstało po sprowadzeniu układu równań 6.1 do równania kwadratowego zmiennej y, dany jest wzorem: = x rx rr4 0x 0 + 2rx 0 y 4 0 r 2 r r 4 r r 2 x r 3 x 3 0 4r 4 x 2 0 r 2 y r 2 0x 4 0 r 4 0x 2 0 x 2 0y 4 0 2x 4 0y 2 0 8rr 2 0x r 3 r 2 0x 0 + 8rx 3 0 y r 3 x 0 y 2 0 4rr 2 0x 0 y r 2 r 2 0x r 2 r 2 0y r 2 x 2 0y r 2 0x 2 0y 2 0 Algorytm unikania kolizji 63

80 ROZDZIAŁ 6. SZCZEGÓŁOWY OPIS ALGORYTMU Marek Majchrowski Rozwiązując równanie kwadratowe = 0 względem zmiennej r otrzymujemy: r max = x2 0 + y2 0 r2 0 2x 0 2r 0 r min = x2 0 + y2 0 r2 0 2x 0 + 2r 0 (6.8) c min = 1 r max = 2x 0 2r 0 x y2 0 r2 0 c max = 1 r min = 2x 0 + 2r 0 x y2 0 r2 0 (6.9) Punkty styczności okręgów P min (x min, y min ) oraz P max (x max, y max ) (podstawiając za r odpowiednio r min oraz r max ): x = 2yy 0+r0 2 x2 0 y2 0 2r 2x 0 y = 2rx 0y 0 +y r2 y 0 r0 2y 0+x0 2y 0 4rx 0 +2r 2 +2x0 2+2y2 0 dla x 0 r (6.10) Punkty przecięcia okręgów dla > 0 oraz danego r można wyznaczyć ze wzorów: x 1 = 2y 1y 0 +r 2 0 x2 0 y2 0 2r 2x 0 y 1 = 2rx 0y 0 +y r2 y 0 r 2 0 y 0+x 2 0 y 0 4rx 0 +2r 2 +2x y2 0 x 2 = 2y 2y 0 +r 2 0 x2 0 y2 0 2r 2x 0 y 2 = 2rx 0y 0 +y r2 y 0 r 2 0 y 0+x 2 0 y 0+ 4rx 0 +2r 2 +2x y2 0 dla x 0 r (6.11) dla x 0 r (6.12) W przypadku, gdy x 0 = r i y 0 0 równanie kwadratowe należy rozwiązywać względem zmiennej x. W przypadku, gdy x 0 = r i y 0 = 0 układ równań ma rozwiązanie tylko dla r = r 0. Wtedy należy przyjąć rozwiązania postaci: { x1 = 0 { x2 = 2r 0 y 1 = 0 y 2 = 2r 0 Przypadku, gdy y 0 = 0 i x 0 = 0 w praktyce nie trzeba rozpatrywać, gdyż środek przeszkody nie może znajdować się w początku układu związanego z robotem. 6.2 Szczegółowy opis działania programu Program implementujący algorytm CVM został napisany z wykorzystaniem struktury Player/Stage i działa w następujący sposób: 1. Pobiera dane o aktualnym położeniu robota oraz bieżące odczyty z czujników (10 razy na sekundę). 2. Na podstawie informacji z odometrii, o przemieszczeniu, przelicza położenie celu względem lokalnego układu współrzędnych. Jeśli w swojej pamięci posiada informacje o przeszkodach, także przelicza położenie tych przeszkód. Dodatkowo, usuwa te przeszkody, po przeliczeniu ich położenia, których współrzędna y < 0 (nie chcemy pamiętać o przeszkodach, które minęliśmy). 64 Algorytm unikania kolizji

81 Marek Majchrowski ROZDZIAŁ 6. SZCZEGÓŁOWY OPIS ALGORYTMU 3. Dla każdego odczytu z czujników odległości wykonywane są następujące czynności: (a) Jeśli odczyt odległości jest większy niż pewna ustalona wartość (L w algorytmie), to pomija dany odczyt. (b) Sprawdzane jest w pamięci, czy nie ma już informacji o przeszkodzie znajdującej się w podobnym miejscu. Warunkiem, żeby przeszkody zostały uznane za identyczne, jest to, aby ich środki znajdowały się w kwadracie o boku pos_epsilon. W implementacji przyjąłem pos_epsilon = 0.05 [m]. Jeśli okaże się, że istnieje już taka przeszkoda, pomija dany odczyt. (c) Wyznacza promień okręgu opisującego daną przeszkodę. Promień zależy od odległości do przeszkody i wyraża się wzorem: promien = obstacle_radius+increase_radius_ f actor (odleglosc obstacle_radius) gdzie: odleglosc to odległość robota od przeszkody, obstacle_radius to promień wyjściowy przeszkody, increase_radius_ f actor to współczynnik opisujący, jak bardzo należy zwiększyć promień przeszkody w zależności od odległości do niej. W implementacji przyjąłem increase_radius_ f actor = 0.06 (rys 6.5), a wartość 42 Zaleznosc promienia przeszkody od odleglosci do tej przeszkody dla wspolczynnika wzrostu Promien przeszkody [cm] Odleglosc do przeszkody [cm] Rysunek 6.5: Zależność promienia przeszkody od odległości do tej przeszkody dla increase_radius_ f actor = 0.06 obstacle_radius ustaliłem na poziomie 0.31 [m], gdyż tyle wynosi połowa przekątnej robota. (d) Przeszkoda dodawana jest do pamięci i zwiększany jest licznik przeszkód. Algorytm unikania kolizji 65

82 ROZDZIAŁ 6. SZCZEGÓŁOWY OPIS ALGORYTMU Marek Majchrowski 4. Sprawdza odległość do celu. Jeśli jest ona mniejsza niż założona dokładność dist_accuracy, zatrzymuje robota. W przeciwnym przypadku wyznacza nowe prędkości sterujące. W implementacji przyjąłem dist_accuracy = 0.06 [m]. 5. Zapamiętuje nowo wyznaczone prędkości robota. 6. Jeśli opcja remember_obstacles jest włączona, to z pamięci są usuwane tylko przeszkody zbędne. W przeciwnym przypadku usuwane są wszystkie przeszkody. Przeszkodę Rysunek 6.6: Czarne okręgi oznaczają przeszkody zbędne, czerwone przeszkody, które zostaną zapamiętane uważa się za zbędna, gdy jej środek znajduje się poza prostokątem, o wymiarach xbound ybound, którego środkiem symetrii jest punkt reprezentujący robota (rys. 6.6). W implementacji przyjąłem xbound = ybound = 1 [m]. Informacje o przeszkodach program może czerpać z trzech rodzajów czujników: podczerwieni, sonarowych oraz dalmierza laserowego (o wyborze źródła należy zadecydować w momencie kompilacji programu). Pamięć o przeszkodach została wprowadzona do programu z powodu rozmieszczenia czujników podczerwieni w naszym robocie. Bez tej pamięci, robot widząc nawet wcześniej czarną przeszkodę na rysunku 6.7, wjechałby w nią w późniejszym czasie, ponieważ jest ona niewidoczna dla jego czujników. Informacja o położeniu robota może pochodzić z odometrii robota, bądź z globalnej wiedzy o jego położeniu, jak to ma miejsce w symulatorze (o wyborze źródła należy zadecydować w momencie kompilacji programu). Wyznaczanie nowych prędkości sterujących odbywa się w następujący sposób: 66 Algorytm unikania kolizji

83 Marek Majchrowski ROZDZIAŁ 6. SZCZEGÓŁOWY OPIS ALGORYTMU Rysunek 6.7: Wizualizacja wiązki IR czujników podczerwieni w robocie Protonek 1. Na podstawie bieżącej informacji o prędkościach, wyznaczane są, z równań 3.2, 3.3, 3.4 oraz 3.5, ograniczenia na dopuszczalne prędkości, które będą brane pod uwagę przy wyznaczaniu nowego sterowania. 2. Tworzona jest lista przedziałów, która początkowo zawiera tylko jeden przedział: ([, ],L). 3. Dla każdej zapamiętanej przeszkody wykonywane są następujące czynności: (a) Ze wzoru 6.4 wyznaczane są styczne do przeszkody (c min oraz c max ), natomiast ze wzorów 6.5 oraz 6.10 wyznaczane są punkty styczności (P min oraz P max ). (b) Ze wzoru 3.10 liczona jest odległość (d min oraz d max ) do punktów styczności wzdłuż wyznaczonych krzywych. (c) Wyznaczane są segmenty okręgu opisującego przeszkodę, do których należą punkty styczności, oraz punkty pośrednie pomiędzy tymi segmentami. (d) Wszystkie przedziały ([c 1, c 2 ],d) wynikające z nowo otrzymanych punktów są dodawane do listy przedziałów w sposób opisany wyżej. 4. Otrzymana w ten sposób lista przedziałów poddawana jest kompresji. Łączone są sąsiadujące przedziały, między którymi odległość jest nie większa niż gdzie: max(dist merge_interval_ f actor, min_merge_dist) Algorytm unikania kolizji 67

84 ROZDZIAŁ 6. SZCZEGÓŁOWY OPIS ALGORYTMU Marek Majchrowski dist to odległość ostatniego przedziału, który nie został połączony z żadnym innym, merge_interval_ f actor to współczynnik odpowiadający za łączenie przedziałów, przy maksymalnej odległości od przeszkody, łączone są przedziały, między którymi różnica odległości wynosi: merge_interval_ f actor min_merge_dist przedziały, między którymi odległość jest mniejsza niż merge_interval_ f actor zawsze będą łączone. W implementacji przyjąłem merge_interval_ f actor = 0.05 [m], a min_merge_dist = 0.05 [m]. 5. Dla każdego przedziału sprawdzane jest, czy jego odległość jest większa niż sąsiednich przedziałów. Jeśli tak jest, to krzywa ograniczająca ten przedział jest zmniejszana (jeśli odległość dla przedziału po prawej jest mniejsza) lub zwiększana (jeśli odległość dla przedziału po lewej jest mniejsza) o pewną wartość, która wynika z powiększenia promienia okręgu, do którego ta krzywa jest styczna, o wartość sa f ety_margin. W implementacji przyjąłem sa f ety_margin = 0.05 [m]. 6. Dla każdego przedziału wyznaczane jest najlepsze sterowanie uwzględniając ograniczenia wynikające z kinematyki i dynamiki robota oraz samego przedziału (każdy przedział wprowadza liniowe ograniczenia w przestrzeni prędkości). 7. Z tak wyznaczonych sterowań wybierane jest to, dla którego funkcja celu 3.11 osiąga maksymalną wartość. 68 Algorytm unikania kolizji

85 Rozdział 7 Wyniki eksperymentalne Zaimplementowany przeze mnie algorytm będzie porównywany z algorytmem VFH+, ponieważ jest to najczęściej stosowany algorytm z grupy metod bazujących na wyznaczaniu kierunku. W tym celu wykorzystałem moduł vfh systemu Player, który implementuje algorytm VFH+. Ponieważ moduł vfh, jako czujnik odległości wykorzystuje dalmierz laserowy, oba algorytmy, zarówno na symulatorze, jak i w środowisku rzeczywistym, będę porównywał przy wykorzystaniu skanera laserowego SICK LMS 200 jako czujnika odległości. Dodatkowo, dla algorytmu CVM, przedstawię wyniki przy wykorzystaniu czujników IR dalekiego zasięgu (patrz 5.4.1) dla środowiska rzeczywistego. W celu przeprowadzenia dokładnych pomiarów położenia robota w środowisku rzeczywistym wykorzystano algorytm LODO opisany w Czas obliczeń algorytmu wynosi szacunkowo 500 µsec dla średnio 30 przeszkód, na komputerze klasy Pentium III 900 MHz. Przedstawiony algorytm unikania kolizji zależy od wielu parametrów, w szczególności od wartości współczynników α. Po przeprowadzeniu szeregu doświadczeń w środowisku symulacyjnym, a następnie ich weryfikacji w rzeczywistości, dobrano następujące wartości parametrów: α 1 = 0,3 α 2 = 0,6 α 3 = 0,1 7.1 Wyniki symulacyjne Środowisko pomiarowe 1 W eksperymencie tym cel znajdował się w odległości 3 m przed robotem. W trakcie jazdy, robot musiał ominąć okrągłą przeszkodę (o średnicy 30 cm) stojącą na drodze do celu (rys. 7.1). Algorytm unikania kolizji 69

86 ROZDZIAŁ 7. WYNIKI EKSPERYMENTALNE Marek Majchrowski Rysunek 7.1: Omijanie pojedynczej przeszkody w środowisku symulacyjnym Rysunek 7.2 przedstawia wykres położenia robota w trakcie omijania przeszkody (rys. 7.1). Robot osiągnął cel z uchybem mniejszym niż 6 cm. Na rysunku 7.3 znajduje się wykres prędkości sterujących, natomiast na rysunku 7.4 wykres orientacji robota w trakcie jazdy. Wykres pozycji robota CVM y [m] 1 0 Tor ruchu robota Punkt startowy Punkt koncowy (cel) Przeszkoda 1 y [m] x [m] Wykres pozycji robota VFH+ Tor ruchu robota Punkt startowy Punkt koncowy (cel) Przeszkoda x [m] Rysunek 7.2: Wykres położenia robota w trakcie omijania pojedynczej przeszkody w środowisku symulacyjnym 70 Algorytm unikania kolizji

87 Marek Majchrowski ROZDZIAŁ 7. WYNIKI EKSPERYMENTALNE 0.5 Przebieg predkosci katowej ω CVM 1 Przebieg predkosci liniowej v CVM ω [rad/s] 0 v [m/s] czas [s] Przebieg predkosci katowej ω VFH czas [s] Przebieg predkosci liniowej v VFH ω [rad/s] 0 v [m/s] czas [s] czas [s] Rysunek 7.3: Przebiegi prędkości robota w trakcie omijania pojedynczej przeszkody w środowisku symulacyjnym Wykres orientacji robota CVM Wykres orientacji robota VFH Rysunek 7.4: Wykres orientacji robota w trakcie omijania pojedynczej przeszkody w środowisku symulacyjnym Algorytm unikania kolizji 71

88 ROZDZIAŁ 7. WYNIKI EKSPERYMENTALNE Marek Majchrowski Środowisko pomiarowe 2 W eksperymencie tym cel znajdował się w odległości 9 m przed robotem. W trakcie jazdy, robot musiał ominąć trzy przeszkody: dwie okrągłe (każda o średnicy 30 cm) oraz jedną prostokątną (o wymiarach podstawy 20 cm 60 cm), stojące na drodze do celu (rys. 7.5). Rysunek 7.5: Omijanie trzech przeszkód w środowisku symulacyjnym Rysunek 7.6 przedstawia wykres położenia robota w trakcie omijania przeszkód (rys. 7.5). Robot osiągnął cel z uchybem mniejszym niż 6 cm w przypadku algorytmu CVM. Dla algorytmu VFH+ robot zatrzymał się w odległości mniejszej niż 15 cm. Na rysunku 7.7 znajduje się wykres prędkości sterujących, natomiast na rysunku 7.8 wykres orientacji robota w trakcie jazdy. Wykres pozycji robota CVM Wykres pozycji robota VFH Tor ruchu robota Punkt startowy Punkt koncowy (cel) Przeszkody 5 4 Tor ruchu robota Punkt startowy Punkt koncowy (cel) Przeszkody y [m] 3 y [m] x [m] x [m] Rysunek 7.6: Wykres położenia robota w trakcie omijania trzech przeszkód w środowisku symulacyjnym 72 Algorytm unikania kolizji

89 Marek Majchrowski ROZDZIAŁ 7. WYNIKI EKSPERYMENTALNE Przebieg predkosci katowej ω CVM 0.6 Przebieg predkosci liniowej v CVM 0.3 ω [rad/s] v [m/s] czas [s] Przebieg predkosci katowej ω VFH czas [s] Przebieg predkosci liniowej v VFH+ 0.3 ω [rad/s] v [m/s] czas [s] czas [s] Rysunek 7.7: Przebiegi prędkości robota w trakcie omijania trzech przeszkód w środowisku symulacyjnym Wykres orientacji CVM Wykres orientacji VFH Rysunek 7.8: Wykres orientacji robota w trakcie omijania trzech przeszkód w środowisku symulacyjnym Algorytm unikania kolizji 73

90 ROZDZIAŁ 7. WYNIKI EKSPERYMENTALNE Marek Majchrowski Środowisko pomiarowe 3 W eksperymencie tym cel znajdował się w odległości 6 m przed robotem. W trakcie jazdy, robot musiał przejechać pomiędzy słupami, każdy o średnicy 30 cm. Odległość między środkami słupów w rzędzie wynosiła 1 m, odstęp między rzędami też wynosił 1 m. Rzędy były przesunięte względem siebie o 0.5 m (rys. 7.9). Rysunek 7.9: Omijanie wielu przeszkód w środowisku symulacyjnym Rysunek 7.10 przedstawia wykres położenia robota w trakcie omijania przeszkód (rys. 7.9). Robot osiągnął cel z uchybem mniejszym niż 6 cm w przypadku algorytmu CVM. Dla algorytmu VFH+ robot zatrzymał się w odległości mniejszej niż 10 cm. Na rysunku 7.11 znajduje się wykres prędkości sterujących, natomiast na rysunku 7.12 wykres orientacji robota w trakcie jazdy. Wykres pozycji robota CVM y [m] 4 2 Tor ruchu robota Punkt startowy Punkt koncowy (cel) Przeszkody x [m] Wykres pozycji robota VFH+ y [m] 4 2 Tor ruchu robota Punkt startowy Punkt koncowy (cel) Przeszkody x [m] Rysunek 7.10: Wykres położenia robota w trakcie omijania wielu przeszkód w środowisku symulacyjnym 74 Algorytm unikania kolizji

91 Marek Majchrowski ROZDZIAŁ 7. WYNIKI EKSPERYMENTALNE 0.5 Przebieg predkosci katowej ω CVM 1 Przebieg predkosci liniowej v CVM ω [rad/s] 0 v [m/s] czas [s] Przebieg predkosci katowej ω VFH czas [s] Przebieg predkosci liniowej v VFH ω [rad/s] 0 v [m/s] czas [s] czas [s] Rysunek 7.11: Przebiegi prędkości robota w trakcie omijania wielu przeszkód w środowisku symulacyjnym Wykres orientacji CVM Wykres orientacji VFH Rysunek 7.12: Wykres orientacji robota w trakcie omijania wielu przeszkód w środowisku symulacyjnym Algorytm unikania kolizji 75

92 ROZDZIAŁ 7. WYNIKI EKSPERYMENTALNE Marek Majchrowski Środowisko pomiarowe 4 W eksperymencie tym cel znajdował się w odległości 3 m przed robotem. W trakcie jazdy, robot musiał ominąć ścianę, o wymiarach podstawy 20 cm 160 cm, stojącą na drodze do celu (rys. 7.13). Rysunek 7.13: Omijanie ściany w środowisku symulacyjnym Rysunek 7.14 przedstawia wykres położenia robota w trakcie omijania przeszkód (rys. 7.13). Robot osiągnął cel z uchybem mniejszym niż 6 cm w przypadku obu algorytmów. Na rysunku 7.15 znajduje się wykres prędkości sterujących, natomiast na rysunku 7.16 wykres orientacji robota w trakcie jazdy. Wykres pozycji robota CVM y [m] 1 0 Tor ruchu robota Punkt startowy Punkt koncowy (cel) Przeszkody 1 y [m] x [m] Wykres pozycji robota VFH+ Tor ruchu robota Punkt startowy Punkt koncowy (cel) Przeszkody x [m] Rysunek 7.14: Wykres położenia robota w trakcie omijania ściany w środowisku symulacyjnym 76 Algorytm unikania kolizji

93 Marek Majchrowski ROZDZIAŁ 7. WYNIKI EKSPERYMENTALNE Przebieg predkosci katowej ω CVM 1 Przebieg predkosci liniowej v CVM 0.3 ω [rad/s] v [m/s] czas [s] Przebieg predkosci katowej ω VFH czas [s] Przebieg predkosci liniowej v VFH ω [rad/s] 0 v [m/s] czas [s] czas [s] Rysunek 7.15: Przebiegi prędkości robota w trakcie omijania ściany w środowisku symulacyjnym Wykres orientacji CVM Wykres orientacji VFH Rysunek 7.16: Wykres orientacji robota w trakcie omijania ściany w środowisku symulacyjnym Algorytm unikania kolizji 77

94 ROZDZIAŁ 7. WYNIKI EKSPERYMENTALNE Marek Majchrowski Środowisko pomiarowe 5 W eksperymencie tym cel znajdował się w odległości 4 m przed robotem. W trakcie jazdy, robot musiał ominąć dwie ściany, każda o wymiarach podstawy 20 cm 160 cm, stojące na drodze do celu (rys. 7.17). Ściany były przesunięte względem siebie o wektor [ 1 m,1.5 m]. Rysunek 7.17: Omijanie dwóch ścian w środowisku symulacyjnym Rysunek 7.18 przedstawia wykres położenia robota w trakcie omijania przeszkód (rys. 7.17). Robot osiągnął cel z uchybem mniejszym niż 6 cm w przypadku algorytmu VFH+. Dla algorytmu CVM robot zatrzymał się w odległości mniejszej niż 10 cm. Na rysunku 7.19 znajduje się wykres prędkości sterujących, natomiast na rysunku 7.20 wykres orientacji robota w trakcie jazdy. Wykres pozycji robota CVM y [m] Tor ruchu robota Punkt startowy Punkt koncowy (cel) Przeszkody y [m] x [m] Wykres pozycji robota VFH+ Tor ruchu robota Punkt startowy Punkt koncowy (cel) Przeszkody x [m] Rysunek 7.18: Wykres położenia robota w trakcie omijania dwóch ścian w środowisku symulacyjnym 78 Algorytm unikania kolizji

95 Marek Majchrowski ROZDZIAŁ 7. WYNIKI EKSPERYMENTALNE 0.5 Przebieg predkosci katowej ω CVM 1 Przebieg predkosci liniowej v CVM ω [rad/s] 0 v [m/s] czas [s] Przebieg predkosci katowej ω VFH czas [s] Przebieg predkosci liniowej v VFH ω [rad/s] 0 v [m/s] czas [s] czas [s] Rysunek 7.19: Przebiegi prędkości robota w trakcie omijania dwóch ścian w środowisku symulacyjnym Wykres orientacji CVM Wykres orientacji VFH Rysunek 7.20: Wykres orientacji robota w trakcie omijania dwóch ścian w środowisku symulacyjnym Algorytm unikania kolizji 79

96 ROZDZIAŁ 7. WYNIKI EKSPERYMENTALNE Marek Majchrowski Środowisko pomiarowe 6 W eksperymencie tym cel znajdował się w odległości 5.5 m przed robotem. W trakcie jazdy, robot musiał ominąć trzy ściany, każda o wymiarach podstawy 20 cm 160 cm, stojące na drodze do celu (rys. 7.21). Dwie ściany, bliższe położenia początkowego robota, były przesunięte względem siebie o wektor [ 0.6, 1.5] [m], natomiast trzecia ze ścian, była przesunięta o wektor [0,6,3] [m] względem ściany najbliższej położenia początkowego robota. Rysunek 7.21: Omijanie trzech ścian w środowisku symulacyjnym Rysunek 7.22 przedstawia wykres położenia robota w trakcie omijania przeszkód (rys. 7.21). Robot osiągnął cel z uchybem mniejszym niż 6 cm w przypadku obu algorytmów. Na rysunku 7.23 znajduje się wykres prędkości sterujących, natomiast na rysunku 7.24 wykres orientacji robota w trakcie jazdy. y [m] y [m] Wykres pozycji robota CVM Tor ruchu robota Punkt startowy Punkt koncowy (cel) Przeszkody x [m] Wykres pozycji robota VFH+ Tor ruchu robota Punkt startowy Punkt koncowy (cel) Przeszkody x [m] Rysunek 7.22: Wykres położenia robota w trakcie omijania trzech ścian w środowisku symulacyjnym 80 Algorytm unikania kolizji