Metoda LCM do unikania kolizji w nawigacji robotów mobilnych w
Transkrypt
Metoda LCM do unikania kolizji w nawigacji robotów mobilnych w
POLITECHNIKA WARSZAWSKA Rok akademicki 2010/2011 WYDZIAŁ ELEKTRONIKI I TECHNIK INFORMACYJNYCH INSTYTUT AUTOMATYKI I INFORMATYKI STOSOWANEJ PRACA DYPLOMOWA INŻYNIERSKA Tomasz Borkowski Metoda LCM do unikania kolizji w nawigacji robotów mobilnych w pakiecie Player/Stage Opiekun pracy: dr inż. Tomasz Winiarski Ocena pracy: . . . . . . . . . . . . . . . . . . . . . . . . . . . . ......................................... Data i podpis Przewodniczącego Komisji Egzaminu Dyplomowego Streszczenie Tytuł: Metoda LCM do unikania kolizji w nawigacji robotów mobilnych w pakiecie Player/Stage. Celem niniejszej pracy inżynierskiej było zaimplementowanie i przetestowanie algorytmu Lane-Curvature Method do unikania kolizji w nawigacji robotów mobilnych w pakiecie Player/Stage. Algorytm jest połączeniem metody kierunkowej wyznaczającej pożądany kierunek ruchu oraz metody krzywizn i prędkości odpowiedzialnej za wyznaczaie prędkości postępowej i obrotowej do sterowania robotem. Stworzono wtyczkę do powszechnie używanego w świecie robotyki pakietu Player/Stage, będącą sterownikiem, który implementuje standardowy interfejs algorytmu wyznaczającego trajektorię robota. Pozwala to na użycie zaimplementowanego algorytmu do każdego typu nieholonomicznego robota mobilnego używającego do sterowania programu Player. Badania zachowania algorytmu wykazały jego wysoką efektywność w ciasnym otoczeniu. Pomimo znajomości tylko bliskiego otoczenia skutecznie i płynnie dąży do wyznaczonego celu, efektywnie wykorzystując możliwości dynamiczne robota. Abstract Title: The Lane-Curvature Method for collision avoidance navigation in Player/Stage package. The aim of this BSc dissertation was to implement and test the Lane-Curvature Method for collision avoidance in Player/Stage. The algorithm combines the directional method, which calculates the right directions, and the curvature-velocity method, being responsible for defining translational and rotational velocity to steer the robot. A plugin was created for Player/Stage, a commonly used project in the world of robotics. The plugin is a driver implementing the standard interface of the planner algorithm which defines the robot’s trajectory. That makes it possible to use the implemented algorithm to every type of nonholonomic mobile robot which is steered by Player programe. The algorithm, as proven by the tests of its behaviour, is highly effective in narrow spaces. Although it knows only the closest surroundings, the algorithm reaches the destination effectively and smoothly while making efficient use of vehicle dynamics. 2 Spis treści 1 Wstęp 5 1.1 Cel pracy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5 1.2 Układ pracy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5 2 Nawigacja robotów mobilnych 7 2.1 Tworzenie mapy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7 2.2 Samolokalizacja . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8 2.3 Wyznaczanie trajektorii . . . . . . . . . . . . . . . . . . . . . . . . . . 9 3 Algorytm lokalnego planowania ścieżki LCM 11 3.1 Zarys działania . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11 3.2 Metoda Pasów Ruchu (The Lane Method) . . . . . . . . . . . . . . . . 12 3.2.1 Podział widoku na pasy ruchu . . . . . . . . . . . . . . . . . . . 12 3.2.2 Wybór pasa ruchu . . . . . . . . . . . . . . . . . . . . . . . . . 14 3.2.3 Wybór lokalnego celu pośredniego . . . . . . . . . . . . . . . . . 16 Metoda krzywizn i prędkości . . . . . . . . . . . . . . . . . . . . . . . . 17 3.3 4 Projekt Player/Stage 21 4.1 Geneza i historia projektu . . . . . . . . . . . . . . . . . . . . . . . . . 21 4.2 Architektura . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22 4.3 Player . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23 4.3.1 Interfejsy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24 4.3.2 Drivery . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24 4.3.3 Urządzenia . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25 Stage . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26 4.4.1 Modelowanie obiektów . . . . . . . . . . . . . . . . . . . . . . . 27 Uzasadnienie wyboru PS . . . . . . . . . . . . . . . . . . . . . . . . . . 28 4.4 4.5 3 5 Implementacja oprogramowania 30 5.1 Architektura . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30 5.2 Implementacja algorytmu LCM . . . . . . . . . . . . . . . . . . . . . . 31 5.2.1 Mapa otoczenia dla drivera LCM . . . . . . . . . . . . . . . . . 38 Program kliencki . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39 5.3 6 Badania i wnioski 40 6.1 Środowisko badawcze . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40 6.2 Strojenie parametrów algorytmu LCM . . . . . . . . . . . . . . . . . . 40 6.2.1 Dopasowanie parametrów metody pasów ruchu . . . . . . . . . . 40 6.2.2 Dopasowanie parametrów CVM . . . . . . . . . . . . . . . . . . 45 6.3 Porównanie metody LCM z CVM . . . . . . . . . . . . . . . . . . . . . 47 6.4 Wady algorytmu . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53 7 Podsumowanie 7.1 54 Możliwości rozwoju . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54 A Sposób obliczania dystansu do przeszkody 55 B Podział pola widzenia na pasy ruchu 58 4 Rozdział 1 Wstęp Planowanie ruchu jest jednym z kluczowych zagadnień w sterowaniu robotami mobilnymi. Kwestią szczególnej wagi w nawigacji jest omijanie przeszkód, gdyż oprócz samego unikania kolizji ważne jest też, aby robot sprawnie dążył do celu. Ponadto, aby robot mógł poruszać się szybko, gładko i bez oscylacji, metoda planowania ruchu powinna efektywnie działać w czasie rzeczywistym i uwzględniać ograniczenia dynamiczne i kinematyczne robota. W pracy tej zaprezentowano metodę lokalnego planowania trajektorii ruchu robota mobilnego, która umożliwia realizację postawionych wyżej wymagań. Jest to metoda LCM (Lane Curvature Method). 1.1 Cel pracy Celem pracy była implementacja algorytmu LCM w programie Player/Stage, a także testy symulacyjne pokazujące wynik działania stworzonego oprogramowania. 1.2 Układ pracy W skład pracy wchodzą trzy rozdziały teoretyczne oraz trzy rozdziały opisujące wykonaną pracę. Rozdział drugi wprowadza w tematykę nawigacji robotów mobilnych oraz nakreśla zadanie jakie zostało wykonane w tej pracy. Rozdział trzeci opisuje zasadę działania algorytmu LCM będącego głównym tematem niniejszej pracy. Kolejny rozdział (czwarty) przedstawia środowisko z jakim współpracuje stworzone oprogramowanie, oraz symulator zintegrowany z tym projektem. 5 Rozdziały piąty i szósty opisują stworzoną implementację algorytmu LCM oraz opisują jego działanie. Rozdział siódmy przedstawia krótkie podsumowanie wykonanej pracy. Na końcu pracy znajdują się dwa dodatki (A i B) precyzujące mechanizmy funkcjonowania niektórych funkcji zastosowanych w implementacji algorytmu. 6 Rozdział 2 Nawigacja robotów mobilnych Nawigacja (łac. navigatio - żegluga) w pierwotnym znaczeniu była nauką traktującą o procesie prowadzenia statku wodnego albo latajacego określoną trasą do punktu przeznaczenia, jak również o metodach okreslania jego pozycji [1]. Pojęcie to rozszerzyło swój zakres i odnosi się dziś także do pojazdów mechanicznych a w szczególnym przypadku robotów mobilnych. Na nawigację robotów składają się następujące elementy: • tworzenie mapy, • samolokalizacja, • wyznaczanie trajektorii. Każdy z wymienonych elementów może być wykonywany równolegle. Relacje między nimi mogą być różne w zależności od środowiska w jakim pracuje robot i sposobu realizacji poszczególnych zadań. Mapa może być tworzona na podstawie metod samolokalizacji, w innym podejściu porównanie mapy utworzonej przez robota ze znaną mapą może posłużyć do lokalizaji robota. Poniżej opisano pokrótce każdy z tych elementów z wyróżnieniem ostatniego, gdyż niniejsza praca traktuje o algorytmie wyznaczania trajektorii. 2.1 Tworzenie mapy Mapa w robotyce oznacza pewną wewnętrzną znaną robotowi reprezentację środowiska zewnętrznego. Jest to model środowiska tworzony na podstawie pomiarow z różnego rodzaju czujnikow. Mapy rożnią się metodą reprezentacji środowiska [10]. 7 Znajomość mapy otoczenia, w którym porusza się robot jest istotnym elementem nawigacji. Dzięki mapie robot jest w stanie efektywnie planować trajektorię ruchu. Może być także pomocna przy samolokalizacji robota. Poniżej przedstawiono wymagania, jakie stawia się mapie wykorzystywanej do nawigacji robota mobilnego: • możliwości dodawania nowych obiektów do modelu otoczenia, • posiadanie informacji i procedur umożliwiających lokalizację robota, • wspieranie planowania trasy. Wszystkie powyższe punkty są istotne z punktu widzenia przeznaczenia budowanej mapy. Mają one także wpływ na konstrukcję robota, budującego lub/i korzystającego z mapy, dlatego powinny być rozpatrzone na etapie projektowania robota. Dokładność mapy powinna być na tyle duża, aby jej stosowanie niosło korzyści, jednak nie może być zbyt dokładna, gdyż jej przetwarzanie może stać się obliczeniowo za trudne. Należy wyposażyć robota w odpowiednie czujniki umożlwiające zbieranie z otoczenia danych wykorzstywanych przy budowaniu mapy. Ostatnim omawianym aspektem jest sposób reprezentacji otoczenia. Wyróżniamy m.in. mapy ciągłe metryczne, dyskretne i topologiczne [10]. 2.2 Samolokalizacja Samolokalizacja robota to okreslenie jego pozycji w danym układzie odniesienia. Informacje o położeniu robota mogą być odczytywane między innymi na podstawie mapy oraz obserwacji z czujników - estymowana jest aktualna pozycja robota, na podstawie położenia względem pewnych punktów charakterystycznych (tzw. latarni), czy też z obliczeń modułu globalnej lokalizacji GPS. Można wyróżnić następujące zagadnienia dotyczące problemu samolokalizacji [5]: • Samolokalizacja lokalna i globalna. Metody lokalne mogą określić pozycję robota na podstawie poprzedniej pozycji. Są efektywne tylko na ograniczonym obszarze, ponieważ na większym obszarze tracą dokładność z powodu sumowania się błędów. Wymagana jest znajomość poyzcji początkowej, czego nie wymagają metody globalne, które wyznaczają położenie robota w globalnym układzie odniesienia. Umożliwiają też ponowną lokaliację robota po „zgubieniu się”. Wadą metod globalnych jest duża złożoność obliczeniowa. 8 • Środowisko statyczne i dynamiczne. Jeśli obiekty w otoczeniu robota nie mogą się przemieszczać mamy do czynienia ze środowiskiem statycznym. W przypadku środowiska dynamicznego możlwe są przemieszczenia elementów w otoczeniu robota. Mogą to być otwierane drzwi czy poruszające się przeszkody czy ludzie. • Metody bierne i aktywne. Metody aktywne to takie, które wymagają jakiejś akcji robota aby określić swą pozycję. Jeśli zaś dla potrzeb lokalizacji robot nie wykonuje dodatkowych ruchów oraz nie steruje czujnikami, mowa jest o lokalizacji biernej. Metody pomiaru pozycji robota można podzielić na dwie grupy: • względne – metody odometryczne - pomiar przemieszczenia się pojazdu (np. obrót kół napędowych), – metody inercyjne - oparte na pomiarze sił działających na robota - odczytach z akcelerometrów i żyroskopów. • bezwzględne – aktywne latarnie kierunkowe, – rozpoznawanie sztucznych znaczników, – rozpoznawanie naturalnych znaczników. 2.3 Wyznaczanie trajektorii Kolejną ważną częścią systemu nawigacji robotów moblinych jest metoda planowania trajektorii ruchu. Ma ona za zadanie unikanie kolizji z obiektami znajdującymi się na drodze do celu, a przede wszystkim jej zadaniem jest doprowadzenie sterowanego pojazdu do wyznaczonego celu. Wybór odpowiedniej metody zależy od rodzaju zadań postawionych robotowi. Wyznaczenie trajektorii przy pomocy mapy otoczenia i lokalizacji robota zazwyczaj odbywa się poprzez ustalenie całkowitej trasy od początku do końca. Takie metody można podzielić na dwie główne odmiany: 9 • metody deterministyczne - takie, które działają według ściśle określonego, deterministycznego algorytmu. Oznacza to, że dla wyznaczonego punktu startowego i docelowego algorytm zwróci zawsze dokładnie ten sam wynik, • metody niedeterministyczne - są to metody wprowadzające do swego działania element losowości, przez co dla takiego samego zadania mogą wygenerować różne trajektorie. Szczególnym rodzajem plannerów trajektorii są metody, które nie mają dostępu do mapy otoczenia i polegają jedynie na aktualnie dostępnej wiedzy o najbliższym otoczeniu. Zazwyczaj takie metody nie dają gwarancji odnalezienia punktu docelowego i nie są w stanie stwierdzić, czy to zadanie jest w ogóle wykonalne póki kierowany pojazd nie znajdzie się w otoczeniu miejsca docelowego. 10 Rozdział 3 Algorytm lokalnego planowania ścieżki LCM 3.1 Zarys działania Metoda LCM (Lane Curvature Method) [4] składa się z dwóch części. Pierwszą z nich jest metoda pasów ruchu (Lane Method), która proponuje lokalny kierunek tak, by omijać widziane w danej chwili przeszkody i zarazem poruszać się w stronę punktu docelowego. Wyznaczony kierunek lokalny służy kolejnej części algorytmu, którą jest metoda krzywych i prędkości (Curvature Velocity Method) do wyznaczenia prędkości postępowej i obrotowej robota do następnego kroku algorytmu. Wybór prędkości przez CVM zależy od takich kwestii jak pożądany kierunek, ograniczenia dynamiczne oraz unikanie kolizji z przeszkodami. Podejście LCM zabezpiecza się przed kolizjami w dwojaki sposób. Najpierw poprzez metodę pasów ruchu, a następnie przez CVM. Metoda pasów ruchu rozważa omijanie przeszkód zakładając, że trajektoria robota składa się z prostych odcinków, zaś CVM bierze pod uwagę fakt, że robot porusza się po odcinkach łuków. Połączenie tych dwóch podejść daje w rezultacie bardziej bezpieczny ruch niż w przypadku użycia tylko metody CVM. Na rysunku 3.1 można zobaczyć zarys współpracy dwóch głównych części algorytmu. 11 Pożądany docelowy kierunek Metoda Pasów Ruchu 1. Podział horyzontu na pasy 2. Wybór pasa ruchu 3. Wybór lokalnego punktu docelowego Informacje o przeszkodach Lokalny kierunek docelowy Metoda Krzywizn i Prędkości Prędkość obrotowa i postępowa Rysunek 3.1: Zarys działania LCM. 3.2 3.2.1 Metoda Pasów Ruchu (The Lane Method) Podział widoku na pasy ruchu Pierwszym krokiem algorytmu LCM jest podział przestrzeni otaczającej robota na pasy ruchu wzdłuż kierunku bezpośrednio prowadzącego do celu. Każdy pas jest określony przez odległość, jaką można wzdłuż niego przebyć bezkolizyjnie. Pasy o podobnej odległości do przeszkody są scalane. Aby uprościć wyznaczanie pasów i późniejszą implementację, reprezentacja przeszkód jest aproksymowana do kształtu okręgu. Dlatego przeszkoda przedstawiona jest jako okrąg lub kilka zachodzących na siebie okręgów o konkretnym środku i promieniu. Promienie przeszkód są powiększone o promień koła, w które można wpisać kształt robota. Dzięki temu otrzymuje się zdefiniowaną przestrzeń, w której w każdym punkcie nie będącym w okręgu może znaleźć się środek robota. Parametrami opisującymi każdy pas ruchu są: • szerokość - w(k) - (width) • bezkolizyjny dystans - d(k) - (distance) • kąt widzenia - va(k) - (view angle) 12 kierunek docelowy pasy: 0 1 2 3 4 5 d(4) w(4) va(0) Aktualna pozycja ba zasięg sonarów Rysunek 3.2: Pasy i opisujące je parametry. Bezkolizyjną odległość dla pasa k-tego, d(k) definiuje się jako odległość, jaką robot może przebyć przez linię k-tą zanim uderzy w przeszkodę, zaczynając od linii startowej. „Kąt widzenia” opisujący każdy pas ruchu jest minimalnym kątem pomiędzy linią kierunku docelowego, a linią prostą prowadzącą bezkolizyjnie od aktualnej pozycji do opisywanego pasa ruchu. Parametry te przedstawiono graficznie na rys. 3.2. Na rysunku 3.2 liczba linii NL wynosi sześć. Przestrzeń robocza wyznaczona jest przez maksymalny zasięg czujników odległościowych. W wyznaczaniu pasów brane są pod uwagę tylko te przeszkody, które w dążeniu do zadanego celu są „przed” robotem, a ignorowane te, które są „za” robotem i nie mieszczą się w polu widzenia ograniczonym przez ustalony kąt ba (blocking angle). Pasy o bardzo małej szerokości są scalane z sąsiednim pasem w taki sposób, że: jeżeli w(h) ≤ wmin i d(h) > M in{d(h−1), d(h+1)} dla 1 ≤ h ≤ NL −2, wtedy scalany jest hty pas z pasem sąsiednim, który ma krótszy dystans bezkolizyjny. Pojęcie pasów „bardzo małej szerokości” jest pojęciem względnym, dlatego parametr wmin ustala się w taki sposób, by był odpowiedni dla danego otoczenia i robota. Scaleniu ulegają również pasy sąsiednie, które w małym stopniu różnią się od siebie odległością bezkolizyjną. Określone zostało to zasadą: Jeżeli |d(h) − d(h + 1)| ≤ ∆dmin dla 0 ≤ h ≤ NL − 2, to 13 pas o dłuższym dystansie bezkolizyjnym zostaje złączony z pasem sąsiednim. 3.2.2 Wybór pasa ruchu Po konstrukcji pasów i wyznaczeniu ich parametrów algorytm wybiera najlepszy w danym momencie pas ruchu dla efektywnej i bezkolizyjnej trajektorii. Aby ruch był bezpieczny i bezkolizyjny, należałoby poruszać się po pasie, który jest zarazem szeroki i długi. Zaś aby uzyskać efektywne sterowanie pożądane jest to, aby kierunek lokalny nie zmieniał się znacznie. Dzięki temu warunkowi można również uniknąć gwałtownych zmian kierunku pomimo szumów w odczycie z czujników odległościowych. Kolejnym zaleceniem do wyboru pasa ruchu jest, aby kierunek lokalny był zbliżony do aktualnej orientacji robota or , dzięki czemu robot może uzyskać lepszą i szybszą trajektorię ruchu. Aby uwzględnić te wszystkie warunki używa się następującej funkcji liniowej fs (k) jako funkcji selekcji pasa ruchu. fs (k) = β1 ∗ d(k) + β2 ∗ w(k) − β3 ∗ adva,c − β4 ∗ adva,o (k) (3.1) d(k) = M in{d(k), Dlimit }/Dlimit (3.2) w(k) = M in{w(k), Wlimit }/Wlimit (3.3) adva,c (k) = M in{|va(k) − cp | , Climit }/Climit (3.4) adva,o (k) = M in{|va(k) − or | , Olimit }/Olimit (3.5) cp : kierunek lokalny wyznaczony w poprzedniej iteracji algorytmu or : aktualna orientacja robota Znajdując takie k, które daje maksymalną wartość funkcji fs (k), uzyskuje się szeroki i bezkolizyjny pas ruchu, zapewniający efektywne poruszanie się. Ponieważ va(k) jest najmniejszym możliwym odchyleniem od kierunku docelowego, które zapewnia bezkolizyjny ruch, parametr używany jest jako kierunek przewodni dla każdego z pasów ruchu w funkcji je wartościującej (3.1). Każdy składnik funkcji fs (k) jest znormalizowany i ograniczony przez odpowiednio ustalone maksymalne wartości, Dlimit , Wlimit , Climit , Olimit . Odległość do przeszkody 14 jest ograniczona przez dystans Dlimit . Jeżeli d(k) jest większe niż Dlimit , pas ruchu jest uznawany za zupełnie bezkolizyjny, a dłuższy dystans nie gwarantuje bezpieczniejszej trajektorii. Podobnie jest z szerokością pasa: jeżeli w(k) jest większa niż Wlimit , pas ruchu jest wystarczająco szeroki, aby bezpiecznie się po nim poruszać, a większa szerokość pasa nie gwarantuje bezpieczniejszego ruchu. W równaniu (3.4), różnica między kątem widzenia a ostatnim wybranym kierunkiem jest ograniczona do Climit , zaś większe różnice niż maksymalna zwracają tę samą (maksymalną) wartość. Podobnie jest w równaniu (3.5), gdzie różnica między kątem widzenia i orientacją robota jest ograniczona przez Olimit . W funkcji celu fs (k) składniki adva,c (k) oraz adva,o (k) służą do redukcji wpływu szumów z odczytu czujników odległościowych, a także do płynnego poruszania się robota. Wiele ultradźwiękowych czujników odległościowych jest podatnych na błędy w odczycie spowodowane przez szumy czy wielokrotne odbicia fal dźwiękowych. Błędne odczyty mogą zostać zinterpretowane jako przeszkody, co zakłóca działanie plannera trajektorii, w konsekwencji może spowodować gwałtowne zmiany kierunku robota. Dzięki funkcji adva,c (k) algorytm preferuje opcję o mniejszej zmianie kierunku docelowego, zaś funkcja adva,o (k) - dąży do kierunku zbliżonego do aktualnego kierunku poruszania się. Dzięki tym dwóm składnikom funkcji celu, algorytm dąży do jak najmniejszych zmian w kierowaniu się do celu, a dzięki temu zmniejszony jest wpływ krótkotrwałych błędów w odczycie z czujników. Wiąże się to z faktem, że częściej odbierane są sygnały zbliżone do tych, które rzeczywiście odwzorowują otoczenie niż takie, które są błędne i mylące. Inaczej urządzenia nie byłyby wiarygodne i nie miałoby sensu używanie ich do celów pomiarowych. Użyte w równaniu (3.1) wartości β są wagami, jakie przyporządkowuje się każdej składowej i każda z nich ma wartość dodatnią. Te parametry należy ustalić samemu i odpowiednio je dobrać do właściwości robota, zaś standardowo zostały one ustalone tak: β1 : β2 : β3 : β4 = 6 : 1 : 6 : 1. β1 i β2 są wagami na odpowiednio - bezkolizyjny dystans oraz szerokość pasa. β3 i β4 są wagami dla odchylenia od poprzedniego wyznaczonego kierunku oraz aktualnej orientacji robota. Jeżeli parametry β1 , β2 ustalone są na wyższe, to algorytm wybiera ścieżki szersze o dłuższym bezkolizyjnym dystansie do przeszkód. Jeżeli zaś β3 , β4 będą wyższe, trajektoria robota będzie płynniejsza i możliwie pozbawiona gwałtownych zmian kierunku ruchu. Jednakże zbyt wysokie wartości β3 , β4 nie pozwalają robotowi na zmianę ruchu odpowiednio szybko, gdy przed robotem pojawiają się przeszkody. W odwrotnym przypadku, gdy wartości są zbyt niskie, 15 algorytm uwrażliwia robota na odczyty z sonarów, a w wyniku tego powoduje oscylacje w trajektorii. Przy błędach odczytu z sensorów może to również skutkować zbędnym ruchem wymijającym przeszkody, które w rzeczywistości nie istnieją. 3.2.3 Wybór lokalnego celu pośredniego Kolejnym etapem algorytmu pasów ruchu jest ustalenie lokalnego celu trasy, czyli konkretnego punktu w przestrzeni, do którego robot ma dążyć. kierunek docelowy pas ns va(ns ) hc ba or Aktualna pozycja Rysunek 3.3: Wyznaczenie pożądanego lokalnego kierunku. Jeżeli robot znajduje się aktualnie na pasie wybranym jako najlepszy, końcowy punkt docelowy jest przesyłany do kolejnego kroku, czyli CVM, a wtedy zostaje użyty do obliczenia sterowania prędkością obrotową i postępową. W innym przypadku algorytm wyznacza lokalny kierunek tak, aby robot zmierzał do najlepszego pasa. W trakcie rozpatrywania sytuacji w której robot nie znajduje się na wybranym pasie, można założyć że w iteracji „s” pas ruchu ns został wybrany jako pas najlepszy. 16 Ponieważ kąt va(ns ) jest najmniejszym kątem, który wyznacza linię bezkolizyjnego dotarcia do pasa ns , pożądany lokalny kierunek przedstawiany w odchyleniu od kierunku docelowego - hc (heading command) powinien spełniać warunek: |va(ns )| ≤ |hc|. Lokalny kierunek powinien również zależeć od wspomnianego wcześniej parametru ba tak, aby |hc| ≤ |ba|. Tak więc lokalny kierunek jest wyznaczany za pomocą wzoru: gdy nr = ns hc = gd (3.6) hc = va(ns ) + σ ∗ (ba − va(ns )) (3.7) gdy nr 6= ns gdzie, nr jest numerem pasa, na którym robot się znajduje, oraz 0.0 ≤ σ ≤ 1.0. Wartość σ wyznacza jak odległy powinien być lokalny kierunek od kąta widzenia wybranego pasa ruchu. Jeżeli σ = 0.0, lokalny kierunek będzie równy kątowi widzenia, co nie daje pewności bezpiecznej trajektorii. Jeżeli zaś σ = 1.0, lokalny kierunek będzie zawsze wyznaczany skrajnie na lewo bądź na prawo od kierunku docelowego. Dość bezpiecznym ustawieniem jest σ = 0.5. Jako wynik działania jednej iteracji metody pasów ruchu uzyskuje się aktualny kierunek ruchu, który możliwie bezkolizyjnie prowadzi bliżej punktu docelowego. 3.3 Metoda krzywizn i prędkości Kolejnym etapem postępowania w algorytmie LCM jest metoda krzywizn i prędkości (CVM - Curvature Velocity Method) [9]. Ponieważ roboty mobilne o nieholonomicznych zdolnościach ruchu nie mają możliwości natychmiastowej zmiany kierunku poruszania się, sam pożądany kierunek jazdy nie może zostać użyty jako sygnał sterujący robotem. Aby poruszać się w pożądanym kierunku płynnie, została zastosowana metoda CVM, która jako wynik swojego działania podaje prędkość postępową i obrotową, z jaką powinien poruszać się robot w danym momencie. CVM uwzględnia również omijanie przeszkód zakładając, że trajektoria robota składa się z krzywizn, w tym przypadku łuków. Aby zoptymalizować prędkości robota, CVM wymaga jako informacji wejściowej pożądanego kierunku poruszania się. Dlatego lokalny kierunek wyznaczony przez metodę pasów ruchu zostaje przekazany jako wejście sterujące. 17 Metoda CVM formułuje zadanie omijania przeszkód jako problem optymalizacyjny w przestrzeni prędkości robota. Określa pożądaną prędkość postępową tv (translational velocity) oraz prędkość obrotową rv (rotational velocity) maksymalizując funkcję celu f (tv, rv) : f (tv, rv) = α1 ∗ dist(tv, rv) + α2 ∗ head(rv) + α3 ∗ speed(tv) (3.8) dist(tv, rv) = d(tv, rv, OBS)/L (3.9) head(rv) = 1 − |θc − rv ∗ Tc | π (3.10) tv speed(tv) = tvmax (3.11) Gdzie d(tv, rv, OBS) jest długością łuku, po którym robot może podróżować wzdłuż rv krzywizny c = zanim uderzy w jedną z przeszkód spośród OBS (obstacles). Dłutv gość łuku d(tv, rv, OBS) jest znormalizowana do postaci dist(tv, rv) poprzez ustalony maksymalny dystans L. Składowa head(rv) jest znormalizowanym odchyłem od pożądanego kierunku. Określa się ją jako znormalizowaną różnicę pomiędzy kierunkiem wyznaczonym przez metodę pasów ruchu θc a kierunkiem jaki uzyska robot, jeżeli będzie poruszał się z prędkością obrotową rv w ciągu pewnego ustalonego czasu Tc (gdzie Tc jest okresem pomiędzy iteracjami CVM). Funkcja speed(tv) ma za zadanie utrzymywać prędkość postępową na wysokim poziomie. Ogólnie rzecz biorąc funkcja celu w równaniu (3.8) dąży do sytuacji, w której robot porusza się w kierunku zbliżonym do pożądanego z możliwie dużą prędkością postępową oraz z jak najdłuższą bezkolizyjną ścieżką. W równaniu (3.8), α1 jest wagą bezkolizyjnego dystansu, α2 jest wagą dla sterowania bliżej pożądanego kierunku, zaś α3 parametrem określającym ważność wysokiej prędkości w wyznaczaniu sterowania robotem. Gdy zwiększy się α1 , robot zwraca większą uwagę na omijanie przeszkód, lecz porusza się wolniej i bardziej odchyla się od pożądanego kierunku. Jeżeli α2 będzie wyższe, robot z większym wysiłkiem porusza się w wyznaczonym kierunku, ale przemieszcza się wolniej i zwiększa się prawdopodobieństwo trafienia przez niego w jakąś przeszkodę. Jeżeli zaś α3 będzie większe, robot będzie 18 poruszał się szybciej, a prawdopodobieństwo kolizji czy zboczenia z pożądanego kierunku wzrośnie. Wartości wag α metody CVM najlepiej określić doświadczalnie serią testowych przejazdów, które zagwarantują bezpieczną, gładką i efektywną trajektorię ruchu robota. Przeglądanie przestrzeni prędkości w celu znalezienia punktu o największej wartości funkcji celu ma miejsce tylko na przestrzeni dozwolonych rozwiązań. Ustanowiono następujące ograniczenia obszaru dozwolonych rozwiązań dostosowane do fizycznych i dynamicznych ograniczeń robota mobilnego: 0 ≤ tv ≤ tvmax − rvmax ≤ rv ≤ rvmax rv ≥ rvcur − (ramax ∗ Taccel ) (3.12) rv ≤ rvcur + (ramax ∗ Taccel ) tv ≥ tvcur − (tamax ∗ Taccel ) tv ≤ tvcur + (tamax ∗ Taccel ) Powyższe nierówności poprzez wartości tvmax , rvmax , tamax , ramax ograniczają prędkość postępową i obrotową, oraz przyspieszenie robota. Ograniczenie 0 ≤ tv zapobiega przed cofaniem się robota. Użyta stała Taccel jest okresem pomiędzy kolejnym uruchomieniem algorytmu CVM i ma tę samą wartość co Tc z równania (3.10). Na rysunku 3.4 przedstawiono graficzny przykład działania algorytmu CVM. Krzywa C(0) obrazuje trajektorię, jaką poruszałby się robot, gdyby utrzymał aktualny stan prędkości postępowej VT i prędkości obrotowej VR . Inne krzywe przedstawiają kilka możliwych kombinacji prędkości robota. Funkcja oceny dąży do znaleziania prędkości postępowej i obrotowej, przy których ruch po krzywej trajektorii jest jak najdłuższy (uwzględniając limit długości L), ruch kierował robota w stronę punktu docelowego oraz aby prędkość postępowa był jak największa. Na przedstawionym przykładzie można zauważyć krzywą C(2) złożoną z kropek, która w tej sytuacji zostałaby wybrana z powodu najdłuższej bezkolizyjnej trajektorii, najbardziej zbliżonego kierunku do celu oraz większej prędkości postępowej. Podsumowując, metoda CVM znajduje punkt w przestrzeni dozwolonych prędkości poprzez maksymalizację funkcji celu (3.8). Dzięki temu otrzymuje się aktualne sterowanie robota tak, aby omijał przeszkody zbliżając się do wyznaczonego celu, a zarazem poruszał się tak szybko jak to możliwe uwzględniając możliwości poruszania się. Choć 19 VT VR Rysunek 3.4: Przykład wyboru trajektorii. CVM nie uwzględnia w pełni ograniczeń dynamicznych, to zapobiega nagłym zmianom kierunku ruchu i czyni ruch płynnym. 20 Rozdział 4 Projekt Player/Stage Projekt Player/Stage (w skrócie PS) [3] to zaawansowane środowisko do tworzenia systemów wielorobotowych, pozwalające na łatwą komunikację z urządzeniami zainstalowanymi na pokładzie robota oraz symulację działania robota lub grupy robotów. PS jest projektem typu „Open Source”, co oznacza między innymi, że jest całkowicie darmowy i posiada otwarty dostęp do swego kodu źródłowego. 4.1 Geneza i historia projektu Nazwa „Player/Stage” wzięła się od wersów: „All the world’s a stage, And all the men and women merely players” komedii Williama Szekspira As You Like It [3]. Początki systemu nazwanego „Project Player” (oznaczającego aktualnie zestaw trzech aplikacji: Player, Stage oraz Gazebo) sięgają 1998 roku. Prace nad tym oprogramowaniem rozpoczęto na Uniwersytecie Południowej Kalifornii w Los Angeles. Celem pierwotnym było stworzenie systemu pozwalającego na sterowanie grupą robotów mobilnych Pioneer 2 firmy ActivMedia [2], będących na wyposażeniu tamtejszego laboratorium robotyki. Projekt rozpoczęli Brian Gerkey i Richard Vaughan, później dołączyli do nich Andrew Howard oraz Kasper Stoy. W ciągu prac narodziła się idea stworzenia uniwersalnego zestawu aplikacji umożliwiającego kontrolę nad robotem lub grupą robotów niezależnie od zastosowanego w tych robotach sprzętu, zgodnie współpracującego z symulatorem oraz powszechnie dostępnego za darmo. W taki sposób powstały aplikacje Player i Stage. Player miał dostarczać narzędzie do sterowania podzespołami 21 robota, zaś Stage stał się prostym dwuwymiarowym symulatorem. Oprogramowanie PS znalazło szerokie zastosowanie na całym świecie w uczelnianych laboratoriach robotyki, jak również pośród indywidualnych użytkowników. W 2002 roku zapoczątkowano prace nad nowym symulatorem, nazywanym Gazebo, który tworzy trójwymiarowe środowisko symulacyjne. Można w nim modelować zachowania robotów z uwzględnieniem oddziaływań fizycznych między obiektami. Cały projekt jest w fazie ciągłego rozwijania przez deweloperów, jak i aktywną społeczność internautów. Oprogramowanie działa na systemach Linux, *BSD, Solaris, Windows oraz Mac OSX. 4.2 Architektura Komunikacja z Playerem odbywa się poprzez protokół sieciowy TCP/IP (rys. 4.1) w strukturze klient-serwer, gdzie Player odgrywa rolę serwera. Dzięki takiej architekturze możliwe jest pisanie aplikacji sterujących robotami w jednym z wielu języków programowania, takich jak C, C++, Java, Python, Ruby, Ada czy Octave. Zapewnia to również wieloplatformowość, dzięki czemu PS zyskuje coraz większą popularność. Robot może być sterowany przez program kliencki komunikujący się z serwerem Player przez TCP/IP. Aplikacja kliencka TCP/IP Player lub Robot Stage Rysunek 4.1: Schemat komunikacji w środowisku Player/Stage 22 4.3 Player Player jest serwerem sieciowym dostarczającym gotowe interfejsy do sprawowania kontroli nad receptorami i efektorami, w które wyposażony jest robot, za pośrednictwem protokołu TCP/IP. Schemat działania Playera został przedstawiony na rysunku 4.2. Serwer ten zawiera zbiór obiektów proxy, które odpowiadają za komunikację z programem klienckim, oraz zbiór driverów (sterowników) do urządzeń bądź zaimplementowanych algorytmów. Kod programu ROBOT Proxy Serwer Player Drivery Urządzenia Rysunek 4.2: Schemat działania Playera sterującego robotem Do kodu projektu zostały dołączone biblioteki klienckie dla języków programowania C, Python, C++, dzięki którym z poziomu własnego kodu użytkownik może wydawać polecenia robotowi w przystępnej dla niego formie, np. ustawić pożądaną prędkość postępową robota w [ ms ] lub obrotową w [ rad ]. Ułatwione jest również odbieranie infors macji z czujników, np. odczyt z dalmierzy laserowych w [m]. W przypadku, gdy do konstrukcji robota użyto urządzeń, do których sterowniki zostały napisane, można oddzielić się zupełnie od warstwy sprzętowej oprogramowania. Playera zaprojektowano początkowo do obsługi robotów z rodziny Pioneer 2 firmy ActivMedia, później poszerzono go o obsługę wielu innych robotów, urządzeń i algorytmów. Na liście obsługiwanych robotów znajduje się trzynaście pozycji, a wśród nich tak znane jak „Roomba” - domowy, autonomiczny robot - odkurzacz firmy iRobot, czy „Segway” [3]. 23 4.3.1 Interfejsy Cała komunikacja wewnątrz Playera odbywa się poprzez interfejsy, które specyfikują składnię oraz semantykę przesyłanych wiadomości. Interfejsy wyznaczają standard definiowania driverów, dzięki czemu różne urządzenia pełniące te same funkcje mogą być używane w ten sam, zunifikowany sposób, a użytkownik nie musi znać specyfikacji urządzenia. Oto kilka podstawowych interfejsów, jakie wyznacza Player: • position2d - służy do kontroli baz jezdnych robotów mobilnych w 2D, w tym obsługę odometrii, • ranger - obsługuje różnego rodzaju czujniki odległościowe, • map - obsługuje mapy otoczenia, • planner - wyznacza ścieżkę do postawionego celu. Interfejsy umożliwiają komunikację między driverami, a co za tym idzie - ich współpracę. W PS łączenie możliwości driverów jest bardzo proste i odbywa się za pomocą plików konfiguracyjnych. Zastosowanie komunikacji poprzez ustalone interfejsy pozwala na przezroczystą współpracę ze sprzętem robota lub zamiennie z symulatorem Stage czy Gazebo. Dzięki tej możliwości ten sam program napisany w Playerze może być uruchomiony na fizycznym robocie lub na wirtualnym obiekcie wymodelowanym w środowisku symulacyjnym. 4.3.2 Drivery Drivery, po polsku nazywane „sterownikami”, są to części kodu odpowiadające za komunikację z urządzeniami. Uwzględniają one rodzaj użytej komunikacji i odpowiednio interpretują surowe informacje otrzymywane ze sprzętu. Drivery służą do agregacji i przekazywania informacji zebranych z urządzenia do bardziej abstrakcyjnej i przystępnej dla użytkownika formiy. Każde urządzenie ma swoje określone właściwości i możliwości, dlatego do każdego modelu urządzenia stosuje się odpowiedni driver. To stwierdzenie można łatwo zobrazować. Za przykład niech posłuży urządzenie wysyłające sygnały sterujące do silników, które odbiera od drivera zadaną wartość mocy w procentach (0% do 100%) i wysyła ilość obrotów silnika od poprzedniego czasu wysłania tej informacji. Zadaniem takiego 24 sterownika, któremu zostało polecone uzyskanie odpowiedniej prędkości przez robota wyrażonej w [ ms ] jest uwzględnienie średnicy kół robota i wysterowanie mocą silników tak, aby koła poruszały się z odpowiednią prędkością obrotową zgodnie ze wzorem: v ω= . r W Playerze jako drivery przedstawiono również implementacje różnych algorytmów stosowanych w robotyce. Można wyróżnić takie algorytmy jak: • Vector Field Histogram (VFH+) - zmierza do wyznaczonego celu i omija przeszkody, • Adaptive Monte Carlo Localization (AMCL) - wyznacza na podstawie posiadanej mapy i odczytu z czujników probabilistyczną lokalizację robota, • Nearness Diagram (ND) Navigation - inny rodzaj nawigacji szczególnie odpowiadający robotom nieholonomicznym w ciasnych przestrzeniach. Algorytmy i niektóre urządzenia wymagają współpracy z innymi driverami poprzez odpowiednie interfejsy. Każdy driver udostępnia wynik swojego działania poprzez jeden lub wiele interfejsów. Przykładem jest driver AMCL, który wymaga: odometrii przez interfejs position2d, odczytów z dalmierzy laserowych przez interfejs laser oraz mapy poprzez map, zaś udostępnia interfejsy: localize, który zapewnia reprezentację hipotetycznego miejsca na mapie, oraz position2d, który otrzymuje najbardziej prawdopodobną pozycję, którą można traktować jako wynik działania idealnego systemu odometrii. 4.3.3 Urządzenia Aktualnie na liście obsługiwanych przez Player/Stage sensorów i efektorów są między innymi [3]: • LMS 200 i inne modele - dalmierze laserowe firmy SICK, • VS-C4 - kamera cyfrowa firmy Canon, • Geko 201 - odbiornik GPS firmy Garmin, • różnego typu sprzęt dźwiękowy. 25 Kompletna lista obsługiwanych urządzeń znajduje się na stronie projektu, obecnie jest ich około trzydziestu. Swój rozwój PS zawdzięcza również indywidualnym użytkownikom, którzy zaimplementowali część sterowników do używanych przez siebie urządzeń. 4.4 Stage Rysunek 4.3: Przykładowy zrzut obrazu z działania programu Stage Stage jest prostym dwuwymiarowym symulatorem umożliwiającym modelowanie dużych skupisk robotów mobilnych. Grafika wyświetlana jest w trzech wymiarach, lecz symulacja dotyczy tylko dwóch. Zrezygnowano w nim ze szczegółów modeli na rzecz wydajności obliczeniowej. Reprezentacja symulowanych obiektów ogranicza się do modeli złożonych z prostopadłościennych bloków o różnych kolorach, a posiadane przez nie cechy fizyczne to tylko rozmiar, prędkość i masa. Stage nie uwzględnia praw fizyki tarcia, pędu, czy nawet możliwości dynamicznych robota. Jednak by przybliżyć działanie algorytmu wyznaczania trajektorii jest zupełnie wystarczający. Samo środowisko ma duże możliwości konfiguracyjne, pozwala na sprawne testowanie stworzonych aplikacji sterujących bez konieczności operowania na prawdziwym robocie i bez obaw o uszkodzenie prawdziwego sprzętu czy o inne problemy, takie jak rozładowanie akumulatorów. 26 Kod programu Proxy Serwer Player Drivery Stage Symulacja Rysunek 4.4: Schemat współpracy Playera z symulatorem Stage Stage współpracuje z Playerem udostępniając odpowiednie interfejsy. Rysunek 4.4 przedstawia schemat współpracy, w którym Stage zastępuje urządzenia robota w sposób niezauważalny dla programu sterującego. 4.4.1 Modelowanie obiektów Modelowanie obiektu w symulatorze Stage jest bardzo uproszczone, gdyż ogranicza się do opisania tylko kilku jego cech fizycznych i umieszczonych na nim urządzeń. Całe modelowanie odbywa się poprzez tekstowe pliki konfiguracyjne. Ich składnia jest opisana w dokumentacji projektu i dołączonej do niej instrukcji [6]. Do testów przeprowadzonych na potrzeby niniejszej pracy dyplomowej, stworzono model robota mobilnego „Zephyr” będący przybliżeniem kształtów robota „Ryś” skonstruowanego przez Dawida Seredyńskiego na Wydziale Elektroniki i Technik Informacyjnych [8]. Model „Zephyr” jest modelem robota o bazie jezdnej składającej się z dwóch kół niezależnie napędzanych, umieszczonych na jednej osi, oraz punktu podparcia z tyłu robota (tzw. „ogonka”), który zapewnia robotowi stabilność statyczną. Jak można zauważyć w części pliku konfiguracyjnego (4.6), informacji potrzebnych do opisania modelu robota jest niewiele. Przed głównym opisem ciała robota znajdują się opisy zainstalowanych na nim urządzeń. W tym przypadku są to sonary rozmieszczone w przedniej części kadłuba razem z ich charakterystyką. W kolejnej części opisywane są ogólne cechy robota, takie jak: rozmiar, oś obrotu, masa, baza jezdna oraz informacje o tym, czy obiekt może uderzyć w przeszkody lub czy odbija fale dźwiękowe. 27 Rysunek 4.5: Model robota „Zephyr” w symulatorze Stage W zamieszczonym kodzie (4.6) został przedstawiony tylko jeden blok opisujący kształt robota, jak również umiejscowienie tylko jednego sensora, gdyż dalsza część kodu nie wnosi więcej informacji o sposobie modelowania obiektów. Całkowity kod znajduje się w pliku „zephyr.inc” załączonym do pracy. 4.5 Uzasadnienie wyboru PS Do wykonania niniejszej pracy dyplomowej wybrano Projekt Player/Stage, ponieważ charakteryzuje się on dużymi możliwościami rozwoju i łatwością we współpracy z użytkownikiem. Duża popularność projektu świadczy o jego jakości oraz zwiększa szanse na dalszy rozwój. Ważną zaletą jest możliwość uruchomienia programu sterującego w symulatorze, a także na fizycznym robocie, bez konieczności zmian kodu programu. 28 define zephyrs_sonars ranger #Zephyr's sonars ( scount 9 # number of sonars spose[0] [0.20 0.16 40] # define the pose of each transducer [xpos ypos heading] ... # define the field of view of each transducer sview [0.05 5.0 1 ] #[range_min range_max view_angle] ... # define the size of each transducer ssize [0.01 0.01] # [xsize ysize] in meters ) define zephyr position #Zehpyr's basic info ( size [ 0.60 0.61 0.60 ] # actual size origin [0 0 0] # center of rotation mass 11.0 # estimated mass in KG drive "diff" # type of driving base (differential) zephyrs_sonars() # use the sonar array defined above obstacle_return 1 # can hit things ranger_return 1 # reflects sonar beams # approximation of ZEPHYR's shape block # trunk ( points 4 point[0] [ 0.15 0.22 ] point[1] [ -0.18 0.22 ] point[2] [ -0.18 -0.22 ] point[3] [ 0.15 -0.22 ] z [ 0.16 0.4 ] ) ... ) Rysunek 4.6: Część kodu pliku konfiguracyjnego opisującego model robota „Zephyr” 29 Rozdział 5 Implementacja oprogramowania 5.1 Architektura Do zaprezentowania metody LCM służącej do wyznaczania trajektorii robota mobilnego stworzono oprogramowanie współpracujące z projektem Player/Stage. Kod projektu został napisany w językach C i C++, dlatego konieczne było użycie języka C++ do implementacji algorytmu LCM jako drivera będącego częścią serwera Player. Na rysunku 5.1 ukazano schemat funkcjonowania oprogramowania użytego w niniejszej pracy. Powyższy schemat przedstawia współpracę użytej konfiguracji Playera z aplikacją kliencką oraz ze Stage’em. Widoczny jest również zarys komunikacji pomiędzy driverami (pkt. 4.3.2) wewnątrz serwera Player. Do uruchomienia symulacji wykorzystano drivery ze Stage’a, które dostarczyły następujących interfejsów (pkt. 4.3.1): • position2d - który umożliwił sterowanie silnikami wirtualnego robota w symulatorze oraz odbieranie informacji o odometrii (położeniu na mapie oraz aktualnej prędkości postępowej i obrotowej), • simulation - interfejs umożliwił ustawianie symulowanych obiektów na mapie w pożądanej pozycji (np. robota w wybranej pozycji startowej). Do zaprezentowania metody LCM stworzono driver do programu Player, który jest główną częścią opisanej pracy dyplomowej. Driver LCM spełnia rolę plannera, co oznacza że zapewnia autonomiczne podążanie do celu wyznaczonego przez program kliencki. 30 Aplikacja kliencka TCP proxy Player planner (LCM) simulation position2d Stage Program symulatora Rysunek 5.1: Schemat działania użytego oprogramowania. Posługując się interfejsem „position2d”, LCM przejął kontrolę nad ruchem robota. Robot zyskał możliwość otrzymywania bardziej abstrakcyjnych poleceń, których wykonanie odbywa się bez udziału klienta. 5.2 Implementacja algorytmu LCM Diagram klas (rys. 5.2) przedstawia wszystkie klasy obiektów, jakie użyto do implementacji drivera LCM. Na przedstawionym schemacie większość atrybutów klas nie została ukazana, gdyż każda z klas LcmAlgorithm oraz Cvm posiada dużą liczbę parametrów. Ich obecność na diagramie znacznie zmniejszyłaby jego czytelność. W kolejnych punktach przedstawiono bardziej szczegółowo każdą z klas. Klasa LcmDriver Aby wprowadzić do Playera własny driver [7] stworzono nową klasę LcmDriver dziedziczącą po klasie bazowej ThreadedDriver, której definicja znajduje się w kodzie źródłowym rdzenia Playera. Klasa LcmDriver tworzy podstawę dla implementacji 31 ThreadedDriver +ProcessMessage() : int #Main() : void #MainSetup() : int #MainQuit() : void LcmDriver 1 -lcm_ : LcmAlgorithm +LcmDriver() -SetupOdom() : int -ShutdownOdom() : int -PutCommand() : void -ProcessOdom() : void -doOneStep() : void 1 0..* LcmAlgorithm 1..* -localObstacles_ : vector<obstacle> -lanes_ : vector<lcmlane> -sim_ : Simulation -cvm_ : Cvm +LcmAlgorithm() +readObstacles() +getBa() +getSightRange() +getSightWidth() +setActPosition() +setGoalPosition() +nextStep() +divideSightIntoLanes() +setCvmLocalGoal() +chooseLane() +localHeading() +readConfigFile() 1 1 1 <<struct>> Obstacle +x_ : double +y_ : double +r_ : double 1 1 Simulation 0..* -obstacles_ : vector<obstacles> +Simulation() +readLocalObstacles() : int +loadMapFromFile() : int 1 1 <<struct>> LcmLane +nr_ : int +lBorder_ : double +rBorder_ : double +dist_ : double +viewAngle_ : double Cvm +Cvm() +update() : void +updateGoal() : void +calculateSteering() : void +readConfigFile() : int -objectiveFunction() : double -head() : double -speed() : double -distance() : double -distToObstacle() : double Rysunek 5.2: Diagram klas użytych w driverze LCM. algorytmu LCM. Odpowiada za odbieranie i wysyłanie wiadomości od i do innych driverów, oraz ich odpowiednią interpretację. Aby wykorzystać stworzoną klasę w programie Player należało przeciążyć następujące metody: • LcmDriver() - konstruktor odpowiadający za interpretację sekcji pliku konfiguracyjnego dotyczącej danego drivera. Metoda ta sprawdza poprawność wyznaczonych do współpracy driverów oraz dostępność zapewnianych przez nie interfejsów. W wypadku błędu, zatrzymuje działanie programu. Odczytuje również numer zestawu konfiguracyjnego, z którego mają być pobrane parametry dla algorytmu LCM; • Main() - główna pętla uruchamiana w oddzielnym wątku, podtrzymująca dzia32 łanie całego drivera. Na rysunku 5.3 przedstawiono schemat postępowania tej funkcji; • MainSetup() - metoda odpowiadająca za alokację wszystkich potrzebnych zasobów i inicjalizację potrzebnych do uruchomienia algorytmu zmiennych; • MainQuit() - metoda uwalniająca zasoby, lecz nie rozwiązująca drivera; • ProcessMessage() - funkcja rozpoznaje i sprawdza poprawność przychodzących wiadomości, a następnie przekierowuje je do odpowiednich metod. Klasa posiada również prywatne metody: • PutCommand(double, double) - która wysyła sterowanie do drivera „stage” przez interfejs position2d, • ProcessCommand() - przetwarza otrzymane polecenie dotarcia do celu, • ProcessOdom() - przetwarza otrzymane informacje o aktualnej pozycji i prędkości robota. W swych atrybutach klasa przechowuje informacje o pozycji i prędkości robota, aktualnym celu, powiązanych urządzeniach, a także posiada wskaźnik do obiektu klasy LcmAlgorithm. 33 START Czekaj na polecenia lub informacje o odometrii Czy zakończyć wątek? TAK STOP NIE NIE Przetwórz przychodzące wiadomości Czy robot dotarł do celu? Wyślij obliczone prędkości do position2d Zmień stan aktualnego celu na nieaktywny TAK Wykonaj krok algorytmu LCM NIE Czy wyznaczony cel jest nadal aktywny? TAK Prześlij aktualną pozycję robota do algorytmu LCM Rysunek 5.3: Algorytm postępowania głównej pętli drivera LCM. Klasa Simulation Klasa Simulation odpowiada za symulowanie przeszkód w wirtualnym świecie. Posiada zbiór wszystkich przeszkód znajdujących się na mapie w postaci wektora obiektów typu Obstacle. Klasa posługuje się tylko dwiema metodami: • loadMapFromFile() - wczytuje mapę ze wskazanego pliku tekstowego; • readLocalObstacles() - metoda za argumenty przyjmuje: szerokość i zasięg widzianego pola, aktualną pozycję globalną (w globalnym układzie odniesienia symulacji), aktualny punkt docelowy oraz wskaźnik na wektor przeszkód, do którego wstawia lokalne przeszkody. Funkcja dla każdej przeszkody w posiadanym zbiorze wykonuje następujące operacje. Przekształca współrzędne środka przeszkody z układu globalnego na układ lokalny robota, taki że aktualna pozycja robota jest środkiem układu współrzędnych, a jego oś Y jest skierowana na punkt docelowy. Następnie dla ustalonej liczby punktów znajdujących się na obwodzie przeszkody funkcja sprawdza, czy któryś z nich znajduje się w polu widzenia robota. Jeżeli którykolwiek ze sprawdzanych punktów znajduje się w określonym 34 otoczeniu robota przeszkoda jest dodawana do wektora wskazanego przez jeden z argumentów funkcji. Klasa LcmAlgorithm Klasa ta stanowi główną część kodu implementującego algorytm Lane Curvature Method opisany w rozdziale 3. Wśród jej atrybutów znajdują się parametry warunkujące postępowanie LCM. Są to stałe w postaci liczb zmiennoprzecinkowych typu double: • ba_ - ba - (pkt. 3.2.1 [rad], • minLaneWidth_ - wmin - minimalna szerokość pasa ruchu [m], • minLaneDistDelta_ - ∆dmin - minimalna różnica bezkolizyjnego dystansu dla sąsiednich pasów [m], • beta1_ - β1 - waga dystansu w wyborze linii, • beta2_ - β2 - waga szerokości pasa ruchu, • beta3_ - β3 - waga odchylenia od poprzednio wyznaczonego kierunku, • beta4_ - β4 - waga odchylenia od aktualnego kierunku robota, • dLimit_ - Dlimit - limit długości pasa [m], • wLimit_ - Wlimit - limit szerokości pasa [m], • cLimit_ - Climit - limit odchylenia od poprzednio wyznaczonego kierunku [rad], • oLimit_ - Olimit - limit odchylenia od aktualnego kierunku robota [rad], • sigma_ - σ - parametr, od którego zależy jak szeroko robot będzie próbował omijać przeszkody, • term_ - Tlcm - okres między iteracjami metody pasów ruchu [s]. Najważniejszymi metodami klasy LcmAlgorithm są: • nextStep() - wykonuje jeden krok iteracji algorytmu. Jego postępowanie przedstawiono na rysunku 5.4; 35 • divideSightIntoLanes() - wyznacza na płaszczyźnie przed robotem pasy ruchu, oraz wyznacza parametry opisujące każdy pas. Sposób podziału został przedstawiony w dodatku B; • chooseLane() - określa wartość każdego z wyznaczonych wcześniej pasów poprzez funkcję opisaną w punkcie 3.2.2; • localHeading() - na podstawie wybranego pasa wyznacza lokalny kierunek poruszania się jako punkt pośredni trasy robota; • readConfigFile() - wczytuje ze wskazanego pliku parametry konfiguracyjne. START Odczytaj lokalne przeszkody Minął czas na kolejną iterację metody „pasów ruchu” Informacje o przeszkodach TAK KONIEC NIE Odbierz wyznaczone przez CVM sterowanie i prześlij do drivera Uruchom metodę CVM Symulacja Podziel przestrzeń lokalną na pasy ruchu Wybierz najlepszy pas ruchu Wyznacz i prześlij pożądany kierunek poruszania się robota do metody CVM Rysunek 5.4: Schemat blokowy zaimplementowanego algorytmu LCM. Klasa Cvm Klasa implementuje algorytm Curvature Velocity Metho.d Jest to druga część metody LCM, która odpowiada za wyznaczenie sterowania prędkością postępową i rotacyjną robota. Wyznaczenie prędkości opiera się na założeniu, że ruch mobilnego robota nieholonomicznego składa się z łuków. Na podstawie informacji o aktualnym stanie, pozycji i punkcie docelowym oraz możliwościach jezdnych robota algorytm sprawdza możliwe dla danej iteracji trajektorie ruchu (przedstawiane jako łuki). Następnie wybiera najbardziej odpowiadającą założeniom i wyznacza prędkość postępową i obrotową, które zrealizują tę trajektorię. W programie znajduje się jeden obiekt tej klasy, który jest przechowywany jako pole klasy LcmAlgorithm. 36 Algorytm CVM wymaga określenia parametrów charakteryzujących możliwości dynamiczne robota oraz parametrów decydujących o wyborze trajektorii. Wspomniane parametry są wczytywane ze wskazanego pliku poprzez metodę readConfigFile(). Poniżej opisano stałe używane do określenia możliwości ruchowych robota: • maxTransVel_ - maksymalna prędkość postępowa w [ ms ], • maxTransAcc_ - maksymalne przyspieszenie prędkości postępowej [ sm2 ], ], • maxRotVel_ - maksymalna prędkość obrotowa w [ rad s • maxRotAcc_ - maksymalne przyspieszenie obrotowe w [ rad ]. s2 Parametry charakteryzujące optymalizację trajektorii: • alpha1_ - α1 - waga składowej funkcji celu opisującej bezkolizyjny dystans na badanej trajektorii, • alpha2_ - α2 - waga funkcji opisującej zgodność kierunku jazdy robota z kierunkiem pożądanym, • alpha3_ - α3 - waga funkcji określającej prędkość postępową robota, • timeStep_ - Tc - okres między kolejnymi iteracjami algorytmu CVM w [s], • distLimit_ - L - maksymalna rozpatrywana długość bezkolizyjnej trajektorii [m]. Główną metodą w klasie Cvm jest calculateSteering(). Jej zadanie polega na znalezieniu punktu z przestrzeni dozwolonych na daną chwilę prędkości, który będzie miał największą wartość funkcji oceny trajektorii. Znaleziona kombinacja prędkości postępowej i prędkości obrotowej robota jest wynikiem końcowym pełnej iteracji całego algorytmu LCM. Kolejną metodą jest objectiveFunction(), która dla podanych jako argumenty prędkości oblicza wartość funkcji oceny wywołując poniższe trzy składowe metody: • distance() - oblicza wartość funkcji oceny trajektorii robota dla podanych wartości prędkości. Sposób obliczania tej oceny został przedstawiony w dodatku A; 37 • head() - oblicza kierunek jaki uzyska robot w kolejnej iteracji algorytmu przy podanej prędkości obrotowej i na tej podstawie wyznacza wartość funkcji oceny kierunku; • speed() - zwraca stopień zbliżenia wartości prędkości postępowej do maksymalnej możliwej. 5.2.1 Mapa otoczenia dla drivera LCM Głównym celem stworzonego drivera LCM było zaprezentowanie możliwości zachowania metody do wyznaczania trajektorii Lane Curvature Method, dlatego towarzyszący temu zadaniu problem budowania mapy został pominięty. Wykorzystana metoda postrzega przeszkody w postaci zbioru okręgów, w związku z czym na potrzeby tej pracy stworzono odwzorowanie każdej z testowanych map otoczenia w postaci zbioru okręgów zapisanych w pliku tekstowym. Każdy okrąg został zapisany w postaci trzech liczb (globalnych współrzędnych x i y, oraz promienia r). Promień każdego okręgu został powiększony o promień okręgu, w który można wpisać kształt robota. Dzięki temu otrzymano przestrzeń na mapie, w której robot może się poruszać bez zahaczania o przeszkody. Rysunek 5.5: Przykład mapy widzianej w Stage’u i jej odwzorowanie w postaci okręgów. 38 5.3 Program kliencki Kod programu (rys. 5.6) sterującego robotem ukazuje wygodę i prostotę użycia bibliotek klienckich projektu Player. Na początku działania program inicjuje połączenie z serwerem Player uruchomionym na lokalnej maszynie poprzez port nr 6665. Aby uprościć testowanie drivera LCM zaimplementowano funkcję openCfgFile(), która na podstawie pliku konfiguracyjnego odczytuje pożądaną pozycję startową i cel trasy. Program kliencki poprzez SimulationProxy przekazuje polecenia do symulacji o ustawieniu robota w pozycji startowej. Następnie zadaje punkt docelowy dla drivera LCM poprzez obiekt PlannerProxy. using namespace PlayerCc; int main(int argc, char *argv[]) { PlayerClient robot("localhost", 6665); PlannerProxy planner(&robot, 0); SimulationProxy sim(&robot, 0); Position2dProxy position(&robot, 1); openCfgFile("test_lcm.cfg"); printf("From - x: %d, y: %d, a: %d \t to X: %d, Y: %d\n", from_x, from_y, from_a, goal_x, goal_y); sim.SetPose2d("zephyr1", from_x, from_y, from_a); usleep(500000); planner.SetGoalPose( goal_x, goal_y, 0.0); int i = 60; while (i-->0) { sleep(1); } return 0; } Rysunek 5.6: Kod programu klienckiego. 39 Rozdział 6 Badania i wnioski 6.1 Środowisko badawcze Stworzone oprogramowanie uruchomiono na systemie operacyjnym Ubuntu w wersji 9.04. Poprawne uruchomienie drivera LCM i aplikacji klienckiej wymaga programów opisanych w rozdziale numer 4 w następujących wersjach: • Player w wersji 3.0.1 • Stage w wersji 3.2.2 6.2 Strojenie parametrów algorytmu LCM Poszukiwanie wartości parametrów używanych w LCM, przy których trajektoria robota jest bezkolizyjna, efektywna i umożliwia płynny ruch jest trudnym zadaniem wielowymiarowej optymalizacji. Każdy z dziesięciu parametrów, który opisuje działanie algorytmu LCM (w tym części CVM) ma swój wpływ na kształt trajektorii robota. Do rozwiązania tak skomplikowanego zadania nie wystarczy użyć metody „brute force” na komputerze klasy PC (pięć różnych wartości dla każdej zmiennej oznacza 510 ≈ 107 kombinacji). Użyto zatem metody „dziel i zdobywaj” aby przybliżyć się do satysfakcjonującego rozwiązania. Podzielono zadanie strojenia parametrów na dwa podzadania rozpatrujące najpierw parametry metody pasów ruchu, a następnie parametry CVM. 6.2.1 Dopasowanie parametrów metody pasów ruchu Parametry β1 , β2 , β3 , β4 , σ wymagają doboru odpowiednich proporcji w zależności od typu otoczenia w jakim robot ma się poruszać. Twórcy algorytmu [4] proponują 40 proporcje - β1 : β2 : β3 : β4 = 6 : 1 : 6 : 1. Jednak jeżeli zadania robota odbywałyby się w rozległych przestrzeniach z przeszkodami, parametry β3 , β4 mogłyby zostać zmodyfikowane w taki sposób, by algorytm mniej chętnie zmieniał kierunek jazdy, skutkiem czego byłby płynniejszy ruch robota. Podczas tych badań szczególną uwagę zwrócono na zachowanie LCM w warunkach ograniczonych przestrzeni, a w niniejszej pracy w szczególności korytarzy gdyż jest to dość powszechne i wymagające otoczenie. Aby znaleźć efektywną konfigurację parametrów β oraz σ zbadano zbiór przypadków wartości będący iloczynem kartezjańskim zbiorów: • β1 ∈ {4, 5, 6, 7, 8}; • β2 ∈ {0.7, 0.8, 0.9, 1.0, 1.1, 1.2, 1.3}; • β3 ∈ {4, 5, 6, 7, 8}; • β4 ∈ {0.7, 0.8, 0.9, 1.0, 1.1, 1.2, 1.3}; • σ ∈ {0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8}; co daje w wyniku zbiór 8575 różnych kombinacji wartości parametrów. Dla każdego elementu ze wspomnianego zbioru konfiguracji uruchomiono symulację (rys. 6.1) ograniczając maksymalną długość trwania do tmax = 50[s]. Jeżeli do tego czasu robot dotarł do celu następował zapis informacji o symulacji do pliku, w przeciwnym wypadku symulacja uznawana była za zakończoną fiaskiem. Aby zrealizować taką ilość symulacji stworzono prosty system symulacji ruchu robota (podobny do używanego w Stage’u), który obliczał zmianę położenia robota przy zadanej przez LCM prędkości postępowej i obrotowej. Poniżej przedstawiono metodę obliczeń: xn+1 = xn + vT ∗ cos(an ) ∗ Ts yn+1 = yn + vT ∗ sin(an ) ∗ Ts an+1 = an + vR ∗ Ts przyjmując oznaczenia: xn , yn , an - współrzędne x i y oraz orientacja robota w iteracji n. Natomiast vT , vR - prędkość postępowa i obrotowa, Ts - czas między krokami symulacji. Badania wykazały niemal identyczne wyniki dla symulatora Stage i użytego systemu symulacji, dlatego rezultaty sumulacji uznano za wiarygodne do badań poglądowych. 41 Pozycja startowa 3m 1m 3m Punkt docelowy Rysunek 6.1: Przypadek testowy do badań parametrów LCM. Do badania wpływu parametrów metody pasów ruchu użyto wartości wag dla CVM: α1 = 0.1, α2 = 1.0, α3 = 0.02, tvmax = 0.4, L = 2.0. Te ustawienia stanowią dość bezpieczną konfigurację do testowania właściwości samego algorytmu pasów ruchu. Duża waga kierunku jazdy i mniejsza waga prędkości poruszania się sprawia, że algorytm bardziej stara się trzymać wyznaczonego kursu. Niska prędkość maksymalna powoduje szybszą reakcję na zmianę pożądanego kierunku, zaś krótki horyzont rozpatrywanej trajektorii skutkuje zmniejszeniem reakcji na odleglejsze przeszkody. Przedstawione parametry mają na celu zwiększenie wpływu metody kierunkowej Lane Method na trajektorię robota. Po przeprowadzeniu długotrwałej serii symulacji otrzymano zbiorcze informacje o 1906 przypadkach zakończonych dotarciem symulowanego robota do celu. z każdego przypadku otrzymano informacje o wartości użytych parametrów, czas symulacji oraz przebytą w tym czasie drogę. Na rysunku numer 6.2 przedstawiono wykres wzrostu czasu i przebytej drogi dla otrzymanych wyników. Jak można zauważyć na wykresie, dla 1700 najkrótszych symulacji czas wynosi poniżej 27 sekund, a w kolejnych uzyskany czas jazdy szybko wzrasta. Podobnie jest z długością przebytej drogi - dla 1800 przypadków długość trasy jest mniejsza niż 16.5 metra. Ponieważ czas symulacji wiąże się 42 z długością przebytej drogi, a długość przebytej drogi wiąże się ze zbliżeniem ścieżki do rozwiązania optymalnego, do dalszej analizy pominięto wyniki których czas był dłuższy od 27 sekund, gdyż te dane nie pomogłyby w znalezieniu efektywnego rozwiązania. 45 czas dojazdu do celu przebyta trasa 40 [s] lub [m] 35 30 25 20 15 0 200 400 600 800 1000 1200 1400 1600 1800 Liczba przypadków testowych Rysunek 6.2: Wykres najkrótszych czasów i przebytych dróg w uzyskanych wynikach. Analizując otrzymane dane zauważono pewne prawidłowości. Histogram wystąpień wartości parametru σ wskazuje tylko dwie na wysokim poziomie: • σ = 0.2 - 616 wystąpień, • σ = 0.3 - 1080 wystąpień. Występowanie reszty badanych wartości σ jest pomijalnie małe. Można przyjąć że średnia ważona tych wartości będzie odpowiednia do zastosowania w kolejnych badaniach zachowania algorytmu, zatem przyjęto że σ = 0.26. 43 400 beta2 beta1 350 wystapienia 300 250 200 150 100 50 0 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 Rysunek 6.3: Histogram wystąpień wartości β1 /β, β2 /β 350 beta3 beta4 300 wystapienia 250 200 150 100 50 0 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 Rysunek 6.4: Histogram wystąpień wartości β3 /β, β4 /β Analiza histogramów wystąpień wartości poszczególnych parametrów β nie przynosi wielu informacji, ponieważ słupki histogramów wskazują zbliżone ilości wystąpień. Dlatego zmodyfikowano wartości parametrów w ten sposób, by zawierały również korelację z innymi parametrami. Dla każdej wartości wyznaczono jaką jest częścią sumy wszystkich użytych wartości parametrów (oznaczona jako β). Z histogramów (rys. 6.3, 6.4) można zauważyć, że dla parametrów β2 , β4 w otocze44 niu wartości 0.07 gromadzi się zdecydowana większość rozpatrywanych przypadków. Tak więc uznano, że będą to odpowiednie wartości do dalszego badania zachowania algorytmu. Uznając, że suma wszystkich wag β będzie wynosiła 10, ustalono wartości parametrów: β2 = 0.7, β4 = 0.7. Histogramy przedstawiające wagi β1 , β3 nie pozwalają jednoznacznie określić jakie wartości doprowadzą do zbliżenia trajektorii do optymalnej, ponieważ mają zbyt równomierny rozkład. Kolejna analiza danych doprowadziła do wyizolowania 100 przypadków o najkrótszym czasie przejazdu robota. Poprzez te dane można było zauważyć, że szybsze przejazdy generowały kombinacje parametrów w których β3 < β1 , dlatego wykonano kilka symulacji w których uwzględniono ten fakt. Badania metodą prób i błędów polegające na manipulacji parametrami β1 , β3 wykazały, że robot porusza się zwinniej pomiędzy przeszkodami gdy: β3 /β = 1/3, a β1 /β = 1/2, a zarazem zachowuje płynność ruchu. Tak więc przyjęto wartości: • β1 = 5, • β2 = 0.7, • β3 = 3.3, • β4 = 0.7, • σ = 0.26; 6.2.2 Dopasowanie parametrów CVM Podobnie jak w poprzednim punkcie uruchomiono serię symulacji dla różnych wartości parametrów. Zbiór zestawów parametrów obejmował 6800 różnych kombinacji parametrów α1 , α2 , α3 , vmax . Zbiór obejmował parametry α w zakresie: • α1 ∈ [0.02, 0.18] co 0.02 • α2 ∈ [0.4, 1.4] co 0.1 • α3 ∈ [0.02, 0.2] co 0.02 oraz maksymalną prędkość robota w zakresie vmax ∈ [0.4, 1.0] co 0.1[ ms ]. 45 Z tych 6800 prób robot dojechał do punktu docelowego w mniej niż 50 sekund tylko 503 razy. Tak mały procent sukcesów oznacza, że algorytm CVM jest bardzo wrażliwy na zmiany parametrów α. Następnie z tych 503 wybrano 350 przypadków, które osiągnęły najkrótsze trasy. Dzięki temu odizolowano przypadki w których robot zapętlał trasę, czy wykonywał zbędne ruchy. Rysunek 6.5: Histogramy wystąpień parametrów α1 , α2 , α3 . Analiza histogramów (rys. 6.5) wystąpień parametrów α jednoznacznie wskazuje tendencje preferowanych parametrów. Pomimo tego, że dane dotyczą różnych prędkości maksymalnych, wykazują wspólne cechy. Trajektorie wygenerowane przez CVM z parametrem α3 = 0.02 (będącym wagą funkcji prędkości robota) dominują wśród rozpatrywanych przypadków testowych. Zauważalna jest również tendencja parametrów α1 , α2 . Narzuca się tutaj spostrzeżenie, iż krótsze trasy uzyskują konfiguracje z parametrami, które bardziej stawiają na pożądany kierunek jazdy niż na długość trasy bez kolizji. 46 6.3 Porównanie metody LCM z CVM Dla porównania zaimplementowanej metody LCM zmodyfikowano driver LCM tak, aby można było uruchomić samą metodę CVM jako planner trajektorii. Poniżej przedstawiono cztery mapy, które pokazują różnice pomiędzy zachowaniami algorytmów, jak i ukazują zalety metody LCM. Na tych mapach widoczne są ścieżki, które przebył robot z różnymi ustawieniami maksymalnej możliwej prędkości. Na rysunkach (6.6, 6.7) przedstawiono sytuację, w której robot miał za zadanie przejechać z szerszego korytarza do węższego. Widoczne różnice w trasach obu metod można wytłumaczyć tym, że metoda CVM nie zmienia kierunku trasy od razu gdy pojawią się przed nim przeszkody, ponieważ funkcja kierunku ruchu przytrzymuje robota w obranym kursie prosto na cel. Dopiero gdy jest bliżej, zmienia kierunek ruchu aby uniknąć kolizji. Metoda LCM wcześniej reaguje na przeszkodę przez co możliwa jest mniejsza zmiana kierunku jazdy. Obie metody na tej trasie obierają maksymalne możliwe prędkości. W tej sytuacji metoda LCM wykazuje krótszą trasę i mniejsze zmiany kierunku jazdy. Większa prędkość maksymalna w przypadku LCM zwiększa płynność ruchu, zaś w przypadkach metody CVM zwiększa promień łuku, który musi pokonać robot po ominięciu rogu ściany. Kolejna mapa (rys. 6.8, 6.9) przedstawia korytarz o szerokości 3[m] (podobna szerokość do korytarzy wydziału EiTI) z różnymi przeszkodami stojącymi na drodze. W tym przypadku lepszą trajektorię ruchu osiągnął robot sterowany metodą CVM, lecz możliwe to było dopiero przy prędkości wyższej niż 0.7[ ms ], ponieważ przy mniejszej prędkości algorytm nie mógł wytworzyć trajektorii o wystarczająco długim łuku, aby pokonać widoczne na rysunku 6.8 przeszkody w takim ustawieniu. Metoda LCM preferuje ścieżki prowadzące przez szerszy pas. Z tego względu metoda LCM wybiera bezpieczniejsze ścieżki niż metoda CVM, a zarazem dąży do mniej gwałtownych zmian kierunku jazdy, co pozwala na płynniejszą i szybszą jazdę - widoczne jest to na wykresie (rys. 6.9). 47 Rysunek 6.6: Mapa 1 - trasa. 1 LCM 0.4 m/s LCM 0.7 m/s LCM 1.0 m/s 0.9 0.8 speed [m/s] 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0 0 5 10 15 20 25 30 time [s] Rysunek 6.7: Mapa 1 - prędkości. 48 35 Rysunek 6.8: Mapa 2 - trasa. 1 CVM 0.8 m/s LCM 0.4 m/s LCM 0.7 m/s LCM 1.0 m/s 0.9 0.8 speed [m/s] 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0 0 5 10 15 20 25 30 time [s] Rysunek 6.9: Mapa 2 - prędkości. 49 35 Mapa 3 przedstawia trasę prowadzącą przez prostopadły korytarz i wąskie przejście (np. skrzydło drzwi). Trasa ta w wielu sprawdzanych konfiguracjach algorytmu CVM nie została przez niego ukończona. Algorytm LCM w tej sytuacji dominuje, gdyż wyznacza trasę bezpieczną i dociera do wyznaczonego celu. W zachowaniu metody LCM przy większej prędkości widoczne są lekkie oscylacje spowodowane dużą zmianą kierunku, przez co robot osiąga dużą prędkość obrotową i pozostaje przy tej prędkości, aż do momentu przekroczenia linii wyznaczonego kierunku. Problem ten nie pojawia się przy niższych prędkościach postępowych. Na wykresie prędkości (6.11) widoczne są spadki prędkości w czasie, kiedy robot przekracza wąskie przejście, co polepsza możliwości manewrowania. Kolejna mapa (rys. 6.12) okazała się zbyt trudna do przebycia przez robota sterowanego algorytmem CVM. Rozpoczynając jazdę z punktu widocznego na rysunku, robot prowadzony przez CVM nakierowywał się na cel, przez co zyskiwał prędkość obrotową w jego lewą stronę. Stąd jego możliwe trajektorie w każdym kolejnym kroku prowadziły na ścianę, a najdłuższe bezkolizyjne trajektorie w takiej sytuacji można było uzyskać tylko skręcając mocniej w lewo. Dlatego robot chwilę po starcie kierował się poza mapę i przy żadnych z badanych konfiguracji nie dotarł do wyznaczonego celu. Algorytm LCM w tym przypadku bardzo dobrze radził sobie w tak trudnym otoczeniu. Zwiększenie prędkości maksymalnej podobnie jak w mapie 6.10 spowodowało pojawienie się oscylacji. Trajektoria wyznaczona przez metodę LCM, jak widać na rysunku (6.12), jest bardzo płynna i pozwala robotowi na osiągnięcie i utrzymanie maksymalnej możliwej prędkości przez cały czas ruchu do celu (rys. 6.13). 50 Rysunek 6.10: Mapa nr 3 - trasa. 1 LCM 0.4 m/s LCM 0.7 m/s LCM 1.0 m/s 0.9 0.8 speed [m/s] 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0 0 5 10 15 20 25 30 35 time [s] Rysunek 6.11: Mapa nr 3 - prędkości. 51 40 Rysunek 6.12: Mapa nr 4 - trasa. 1 LCM 0.4 m/s LCM 0.7 m/s LCM 1.0 m/s 0.9 0.8 speed [m/s] 0.7 0.6 0.5 0.4 0.3 0.2 0.1 0 0 5 10 15 20 25 30 35 time [s] Rysunek 6.13: Mapa nr 4 - prędkości. 52 40 6.4 Wady algorytmu Algorytm LCM nie przewiduje cofania się robota, jak również nie rozpatruje przeszkód, które względem kierunku docelowego znajdują się za robotem. Skutkuje to tym, że robot kierowany tą metodą może wjechać w zaułek, z którego sam nie wyjedzie. Napotykając taką przeszkodę, robot nie tylko blokuje się i sam nie potrafi się wydostać z takiej sytuacji, ale wręcz udeża w przeszkodę znajdującą się przed nim. Spowodowane jest to zastosowaniem mniejszej wartości parametru α1 niż w przypadku użycia tylko metody CVM, który odpowiada za wybór trajektorii o dłuższej bezkolizyjnej trasie. 53 Rozdział 7 Podsumowanie Stworzono implementację algorytmu Lane-Curvature Method służącego do wyznaczania trajektorii ruchu robotów mobilnych. Oprogramowanie będące wtyczką do popularnego i używanego na całym świecie programu do sterowania robotami mobilnymi Player spełnia zaplanowane zadanie z wysoką skutecznością i zwraca spodziewane rezultaty. Algorytm LCM okazuje się być skutecznym algorytmem planowania trajektorii. Szczególnie dobrze radzi sobie w środowisku ciasnych przestrzeni, w których inne metody (jak Curvature Velocity Method) napotykają problemy. Algorytm pozwala na efektywne omijanie przeszkód i zachowuje płynność ruchu sterowanego robota. Wykorzystuje zalety metody kierunkowej (Lane Method) i metody opartej na sterowaniu robotem w przestrzeni możliwych prędkości. Pozwala to na uwzględnienie ograniczeń dynamicznych robota, a dzięki temu lepsze wykorzystywanie jego możliwości jezdnych. 7.1 Możliwości rozwoju Implementację drivera LcmDriver można by zintegrować z modułami tworzącymi mapę otoczenia, wzbogacić go o zachowania wykrywające sytuacje w której robot się blokuje i przechodzi w inny tryb poszukiwania celu (np. prosty algorytm Bug okrążający przeszkodę). Implementacja ze zintegrowanym tworzeniem mapy tworzyłaby oprogramowanie, które możnaby zastosować w praktyce na robotach „Ryś” i „Elektron” w wydziałowym laboratorium. Również mogłoby zostać udostępnione jako driver Playera użytkownikom projektu Player/Stage na całym świecie do zastosowania w swoich pracach. Przyczyniłoby się to choć w niewielkim stopniu do rozwoju robotyki na świecie. 54 Dodatek A Sposób obliczania dystansu do przeszkody Aby obliczyć wartość funkcji distance() użytej w implementacji algorytmu LCM należało wyznaczyć długość łuku, po jakim bezkolizyjnie poruszałby się robot przy podanej prędkości postępowej i obrotowej. Metoda sprawdza, czy badana trajektoria przecina się z którąś z lokalnych przeszkód, a jeżeli ma to miejsce sprawdza, z którą przeszkodą trajektoria przetnie się najwcześniej. W tym celu opracowano metodę obliczania długości trajektorii do przeszkody wykorzystującą fakt, że przeszkody przedstawiane są w postaci okręgów oraz, że trajektoria ruchu robota przy stałej prędkości postępowej i obrotowej ma kształt koła. Zakładając że robot obraca się w około własnej osi z prędkością obrotową ω oraz porusza się z prędkością postępową V , pełny obrót robot wykonałby w okresie: 2π T = ω (A.1) a zatem pokonałby dystans s, który wynosi: V ∗ 2π s= ω (A.2) co oznacza, że przebyta droga s jest równa długości obwodu okręgu po którym poruszałby się robot. Promień okręgu trajektorii oznaczony jako rT wynosi zatem: V rT = (A.3) ω Przyjmując postawione założenia należy obliczyć punkt przecięcia się obydwu okręgów (przeszkody i trajektorii robota). Przecięcie się tych okręgów zachodzi, jeżeli z odcinków rT , rP , d, gdzie rP jest promieniem przeszkody, a d jest odległością między 55 środkami rozpatrywanych okręgów, można skonstruować trójkąt. Oznacza to, że żaden z odcinków nie jest dłuższy niż suma pozostałych dwóch. K1 rT rP P d K2 T R y al x bl Rysunek A.1: Graficzne przedstawienie oznaczeń. Jeżeli spełniony jest warunek przecięcia się okręgów, to można przyjąć, że punkty przecięcia znajdują się na prostej y = al x+bl , która spełnia układ równań obu okręgów: (x − Px )2 + (y − Py )2 = r2 P (A.4) (x − Tx )2 + (y − Ty )2 = r2 T gdzie P jest środkiem przeszkody, a T jest środkiem trajektorii robota. Przekształcając układ równań (A.4) doprowadzono go do postaci: y = al x + b l Px − Tx al = Ty − Py Tx2 − Px2 + Ty2 − Py2 + rP2 − rT2 b l = 2(Ty − Py ) (A.5) Następnie podstawiono równanie prostej y = al x + bl do równania okręgu przeszkody. Po podstawieniu al x + bl za y równanie doprowadzono do ogólnej postaci równania kwadratowego: 56 ak x 2 + b k x + c k = 0 ak = 1 + a2 l (A.6) bk = 2(al bl − al Py − Px ) c = P 2 + (b − P )2 − r2 k l y x P Rozwiązując równanie kwadratowe (A.6), otrzymano współrzędne X obu punktów przecięcia się okręgów. Podstawiając je do równania prostej y = al x + bl otrzymano oba punkty przecięcia się okręgów. Kolejnym krokiem jest określenie szerokości kąta γ, którą można uzyskać dzięki obliczeniu wartości funkcji trygonometrycznej arctan dla wektorów o początkach w punkcie T i końcach w punktach R, K1 , K2 . Mając wartość kąta γ długość trajektorii bezkolizyjnej D dla rozpatrywanej trajektorii wynosi: D = γ ∗ rR 57 (A.7) Dodatek B Podział pola widzenia na pasy ruchu Do podziału pola widzenia na LcmAlgorithm::divideSightIntoLanes(). pasy Spośród ruchu służy otaczających metoda robota prze- szkód stworzony algorytm wybiera najpierw te, w których istnieje punkt P w lokalnym układzie współrzędnych spełniający nierówności (B.1). Lokalny układ współrzędnych zdefiniowany jest tu w taki sposób, że początkiem układu jest aktualna lokalizacja robota, a oś OY jest skierowana do punktu docelowego. Przyjmując oznaczenia: sw - szerokość pola widzenia, sr - zasięg pola widzenia, ba - kąt blokujący (pkt. 3.2.1). − s2w ≤ Px ≤ s2w 0 ≤ P y ≤ s r (B.1) Py ≥ tan(ba)Px P ≥ tan(−ba)P y x Pasy ruchu w stworzonej implementacji algorytmu LCM są równoległe do kierunku docelowego i przedstawiono je za pomocą wartości: • lBorder_ (lb) - lewej granicy [m], • rBorder_ (rb) - prawej granicy [m], • dist_ (d) - bezkolizyjnego dystansu [m], • va_ (va) - kąta widzenia (view angle), który ma wartość najmniejszego odchylenia od kierunku do celu, które trzeba uzyskać, by robot mógł bezkolizyjnie przemieścić się na określony pas ruchu [rad]. 58 Metoda sortuje zbiór wybranych przeszkód w taki sposób, by lista zaczynała się od tych najbardziej wysuniętych w lewą stronę pola widzenia robota, następnie dla każdej przeszkody tworzy pas ruchu o wartościach: • lb = x − r, • rb = x + r, • d = y − r, • va = 0. przyjmując, że x, y, r to współrzędne środka i promień przeszkody. Następnie wydzielone pasy porządkowane i doprowadzane są do stanu, w którym żaden z pasów nie nachodzi na siebie, pasy wypełniają całe pole widzenia robota oraz pasy mające większy bezkolizyjny dystans są przysłaniane tymi krótszymi. Aby tak się stało, sporządzono algorytm porównujący pasy zapisane w wektorze zaczynając od tych po lewej stronie. Ponieważ pasy ustawione są w kolejności od tych zaczynających się najbardziej po lewo, istnieje pięć możliwych konfiguracji sąsiadujących pasów (rys. B.1), które wymagają zmiany ustawienia: 1. lbn ≤ rbn < lbn+1 ≤ rbn+1 - pasy są rozłączne - algorytm dodaje pas wolny pomiędzy rozpatrywanymi; 2. lbn ≤ lbn+1 < rbn ≤ rbn+1 - pasy częściowo zachodzą na siebie: (a) pas po lewej jest krótszy, (b) pas po prawej jest krótszy, W obu przypadkach dłuższy pas jest obcinany tak, by nie nachodził na krótszy; 3. lbn < lbn+1 < rbn+1 < rbn - jeden pas przysłania całkowicie drugi, (a) szerszy pas jest dłuższy - węższy pas pozostaje bez zmian, a szerszy jest dzielony na dwie części tak, aby pasy nie zachodziły na siebie, (b) węższy pas jest dłuższy - szerszy pas pozostaje bez zmian, a węższy jest usuwany. Po każdej zmianie lewej granicy któregoś z pasów lub dodaniu kolejnego zbiór pasów ruchu jest sortowany. 59 1 2a 2b 3a 3b Rysunek B.1: Możliwe kombinacje ustawienia sąsiednich pasów. Następnie funkcja scala pasy sąsiednie, których różnica bezkolizyjnej długości jest mniejsza niż ∆dmin = 2.5[cm], oraz każdy pas, który jest węższy niż wmin = 2[cm] z sąsiednim krótszym pasem. kierunek docelowy 0 1 2 3 4 5 Aktualna pozycja Rysunek B.2: Sposób obliczania kąta va dla pasów ruchu. Ostatnim krokiem funkcji jest obliczenie wartości va dla każdego pasa. Dla każdego pasa oblicza się wartość jak przedstawiono na rys. B.2. Ostatecznie danemu pasowi przypisuje się wartość największą spośród pasów znajdujących się pomiędzy nim, a pasem środkowym (tym o wartości va = 0). W ten sposób funkcja przygotowuje pole widzenia robota do kolejnego etapu algorytmu. 60 Bibliografia [1] Encyklopedia PWN. http://encyklopedia.pwn.pl/. [2] B. Gerkey, R. Vaughan, K. Støy, A. Howard, GS Sukhatme, and MJ Matarić. Player history. [3] B. Gerkey, R. Vaughan, K. Støy, A. Howard, GS Sukhatme, and MJ Matarić. The player project. [4] N.Y. Ko, R.G. Simmons, and K.S. Kim. A lane based obstacle avoidance method for mobile robot navigation. Journal of Mechanical Science and Technology, 17(11):1693–1703, 2003. [5] Tomasz Winiarski Maciej Staniak. Nawigacja robotów mobilnych. Praca magisterska, WEiTI, 2002. [6] J. Owen. How to Use Player. Stage. On Player/Stage website–http://playerstage. sourceforge. net, 2009. [7] B. Petersen and J. Fonseca. Player/Stage. 2006. [8] Dawid Seredyński. Platforma sprzętowa i programowa dwukołowego robota mobilnego. Praca dyplomowa inżynierska, WEiTI, 2011. [9] R. Simmons. The curvature-velocity method for local obstacle avoidance. In Robotics and Automation, 1996. Proceedings., 1996 IEEE International Conference on, volume 4, pages 3375–3382. IEEE, 2002. [10] Wojciech Szynkiewicz. Wstęp do robotyki, wykład, 2010. 61