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