Laboratorium Podstaw Robotyki ĆWICZENIE 4

Transkrypt

1 Laboratorium Podstaw Robotyki Politechnika Poznańska Katedra Sterowania i Inżynierii Systemów ĆWICZENIE 4 System sterowania robotem mobilnym MiniTracker V3 Celem ćwiczenia jest poznanie sposobów sterowania i komunikacji z nieholonomicznym, dwukołowym robotem mobilnym MiniTracker V3. Podczas ćwiczenia poruszane będą zagadnienia związane z planowaniem trajektorii w przestrzeni zadania oraz jej realizacją w torze otwartym. 1 Model kinematyczny robota MiniTracker V3 Autonomiczny pojazd kołowy posiadający zdolność zautomatyzowanego ruchu związanego z realizacją postawionego mu zadania nazywany jest kołowym robotem mobilnym. Realizacja zadania związana jest ścisle z przestrzenią, w której zadanie to zostało zdefiniowane. W przypadku kołowych robotów mobilnych naturalną przestrzenią zadania jest trójwymiarowa podprzestrzeń kartezjańska, w której robot się porusza. W tym ćwiczeniu rozważania zostaną ograniczone do ruchu robota na płaszczyźnie zorientowanej ortogonalnie do wektora przyspieszenia grawitacyjnego. Do planowania i realizacji trajektorii w przestrzeni zadania niezbędna jest znajomość modelu kinematyki rozważanego robota mobilnego. Uwaga nasza zostanie skupiona na modelu dwukołowego robota mobilnego MiniTracker V3 przedstawionego na rys. 1. Schemat kinematyczny robota Mini- Tracker V3 (w skrócie: MTV3) przedstawia rys. 2. Na rysunku tym zaznaczono lokalny {x L, y L } oraz globalny {x G, y G } układ współrzędnych. Środek układu lokalnego przywiązano na osi kół i w połowie odległości pomiędzy kołami. Pozycję oraz orientację platformy wyrażoną w układzie globalnym oznaczono odpowiednio jako x, y oraz ϕ (zwrot strzałki oznacza dodatni przyrost kąta orientacji). Zaznaczono także prędkości kątowe kół: prawego ω P i lewego ω L oraz prędkości związane z całą platformą: prędkość postępową v początku układu lokalnego (znak + oznacza dodatni zwrot prędkości) oraz prędkość kątową ω (znak + oznacza dodatni kierunek obrotu platformy). Przy założeniu braku poślizgu kół (zarówno poślizgu wzdłużnego jak i poprzecznego), zależności między prędkościami liniową v i kątową ω platformy robota a prędkościami kątowymi koła lewego ω l i prawego ω p są następujące [1]: v = ω p + ω l 2 R, ω = ω p ω l R, (1) b gdzie R jest promieniem kół, a b jest rozstawem między punktami styku kół z podłożem (patrz rys. 2). Przekształcając równania (1) otrzymuje się wyrażenia opisujące prędkości kół robota jako funkcje prędkości liniowej v i kątowej ω całej platformy: ω p (v, ω) = v + b 2 ω R, ω l(v, ω) = v b 2 ω R. (2) Zależności funkcyjne ω p = ω p (v, ω) oraz ω l = ω l (v, ω) mają kluczowe znaczenie z punktu widzenia realizacji trajektorii zdefiniowanej w przestrzeni zadania. Pozwalają one na przeliczenie prędkości 1

2 Laboratorium Podstaw Robotyki 4 2 Rys. 1: Dwukołowy robot mobilny Mini- Tracker V3. Rys. 2: Schemat kinematyczny dwukołowego robota mobilnego w globalnym układzie współrzędnych. platformy do przestrzeni prędkości napędów, które ostatecznie mogą zostać zrealizowane przez pokładowy system sterowania. Model kinematyki w przestrzeni zadania dla dwukołowej platformy mobilnej z rysunku 2 uwzględniający założenie o braku poślizgu kół względem podłoża ma następującą postać [3]: ϕ ẋ = 1 0 ω + 0 cos ϕ v, (3) ẏ 0 sin ϕ gdzie q = [ϕ x y] T jest wektorem stanu platformy moblinej, a u = [ω v] T jest wektorem sterowań w przestrzeni zadania. Korzystając z rysunku 2 można zauważyć kilka użytecznych relacji wiążących poszczególne sygnały związane z kinematyką platformy robota [2]: ẋ v x = v cos ϕ ẏ v y = v sin ϕ } tan ϕ = ẏ ẋ ω = dϕ dt v = v 2 x + v 2 y, (4) ϕ = Atan2 (sgn(v)ẏ, sgn(v)ẋ), (5) ϕ(t) = ϕ(0) + t 0 ω(τ)dτ, (6) przy czym Atan2 (, ) jest czteroćwiartkową funkcją arctan. Parametry geometryczne robota MTV3 występujące w równaniach wiążących prędkości platformy z prędkościami napędów są następujące: R = 0.026[m], b = 0.066[m]. (7) 2 Komunikacja i sterowanie robotem MTV3 Komunikacja nadrzędnego układu sterowania z systemem pokładowym sterownika robota możliwa jest poprzez standardowy interfejs szeregowy RS232 oraz poprzez łącze radiowe pracujące na częstotliwości nośnej 433MHz. Łącze komunikacyjne umożliwia półdupleksową wymianę danych z prędkością [b/s] i służy do przekazywania sterownikowi pokładowemu wartości zadanych prędkości

3 Laboratorium Podstaw Robotyki 4 3 ω pd, ω ld obu kół robota, parametrów pracy pokładowego systemu sterowania oraz służy do odbioru danych pomiarowych. Większość wysyłanych do robota rozkazów powoduje zwrócenie odpowiedzi potwierdzającej. Komunikację można uznać za poprawną dopiero wtedy, gdy otrzymana od robota odpowiedź będzie prawidłowa. Pełną listę rozkazów oraz systemu komunikacyjnego można znaleźć w instrukcji robota [4]. Przekazanie wartości zadanych prędkości poszczególnych kół robota zapewnia rozkaz SetWheelsVelocities, który jednocześnie umożliwia otrzymanie w odpowiedzi następujących danych: aktualnej wartości znacznika czasu pokładowego, aktualnych pozycji kątowych kół, bieżących wartości prądów silników, bieżących wartości prędkości kół oraz aktualnej pozycji i orientacji platformy robota. Prędkości zadane kół ω pd, ω ld przesyła się w jednostkach wewnętrznych sterownika, które są interpretowane jako zadana ilość impulsów enkodera przypadająca na jedną sekundę ([imp/s]). Na pełen obrót koła przypadają 384 impulsy enkodera. Maksymalna prędkość kątowa kół wynosi około ωp/l max 80[rad/s]. 2.1 Podłączyć moduł nadajnika-odbiornika radiowego do portu szeregowego COM1 komputera oraz podłączyć zasilanie. Włączyć zasilanie robota (przełącznik znajduje się w jednym z narożników robota). Uruchomić aplikację TestLink.exe (katalog C:\laboratorium\PR2\cw4) i sprawdzić poprawność połączenia i komunikacji. Poziom błędów nie powinien przekraczać 10%. Jeżeli test wypadnie prawidłowo (można wyłączyć zasilanie robota i przystąpić do realizacji dalszej części ćwiczenia. UWAGA! W przypadku problemów z komunikacją, zmienić ustawienie nadajnika radiowego względem robota. Problemy z komunikacją może powodować również rozładowany akumulator. Gdy robot pozostaje nieużywany, należy wyłączyć zasilanie, aby nie rozładowywać akumulatora. 3 Generator trajektorii zadanej Zadaniem generatora trajektorii jest obliczanie chwilowych wartości sygnałów pozycji x d (t), y d (t) i orientacji ϕ d (t) platformy, które mają zostać zrealizowane w układzie: system sterowania robota robot. Ze względu na konstrukcję samego robota oraz ze względu na strukturę pokład- Rys. 3: Schemat ilustrujący sposób przygotowania sygnałów zadanych do realizacji w systemie sterownia robotem MTV3. Przyjęto oznaczenia: GTZ generator trajektorii zadanej, q d (t) = [ϕ d (t) x d (t) y d (t)] T, Q/V blok konwersji trajektoria/prędkości platformy, V/Ω blok konwersji prędkości platformy/prędkości kół robota. owego systemu sterowania, realizacja trajektorii zadanej wymaga dodatkowo obliczania chwilowych wartości zadanych prędkości platformy v d (t), ω d (t) (wynikających bezpośrednio z zadanej trajektorii) i przeliczania ich na chwilowe prędkości poszczególnych kół robota ω pd (t), ω ld (t). Dopiero

4 Laboratorium Podstaw Robotyki 4 4 wartości zadane prędkości kół mogą zostać przesłane na pokład robota w celu ich realizacji za pomocą pokładowych prędkościowych pętli regulacyjnych typu PI+F (patrz rys. 3). Aby prześledzić sposób generowania sygnałów zadanych, rozważmy równania generatora trajektorii zadanej q d (t) = [ϕ d (t) x d (t) y d (t)] T w przestrzeni zadania na przykładzie trajektorii o kształcie zbliżonym do sinusoidy kreślonej na płaszczyźnie (x, y). Niech zadane współrzędne pozycji platformy opisane będą następującymi równaniami: x d (t) y d (t) = At, = B sin(2πf d t), gdzie A, B > 0 są parametrami projektowymi, a f d jest żądaną częstotliwością zadanej sinusoidy. Aby określić przebieg zadanej orientacji platformy robota ϕ d (t) wzdłuż trajektorii pozycji (8), należy określić przebiegi składowych prędkości v xd (t) oraz v yd (t) wzdłuż tej trajektorii. Różniczkowanie równań (8) względem czasu pozwala zapisać: ẋ d (t) = v xd (t) = A = const, ẏ d (t) = v yd (t) = 2πf d B cos(2πf d t). Na podstawie powyższych składowych prędkości można wyznaczyć przebieg orientacji zadanej ϕ d (t) zgodnie z zależnością (5): gdzie ϕ d (t) = Atan2 (sgn(v d )ẏ d, sgn(v d )ẋ d ) [ π, π), (10) v d (t) = ± vxd 2 (t) + v2 yd (t), (11) a znak prędkości v d (t) determinuje wybór strategii ruchu robota (ruch przodem/ruch tyłem), którą określa projektant trajektorii. Zadana prędkość kątowa platformy wynika z różniczkowania równania (10): ω d (t) = a yd(t)v xd (t) a xd (t)v yd (t) v 2 d (t), (12) gdzie a xd (t) oraz a yd (t) są składowymi sygnałami przyspieszenia zadanego i wynikają z różniczkowania prędkości (9): a xd (t) 0, a yd (t) = (2πf d ) 2 (13) B sin(2πf d t). Korzystając z faktu, że zachodzi ω d (t) = dϕ d(t) dt, przebieg orientacji zadanej możemy także obliczać następująco: ϕ d (t) = ϕ d (0) + t 0 (8) (9) ω d (τ)dτ R, (14) gdzie ω d (t) wynika z (12). Należy zauważyć istotną różnicę między definicjami (10) oraz (14) wynikającą z dziedzin, do których należy sygnał ϕ d (t). Na podstawie wyznaczonych zadanych prędkości: liniowej v d (t) i kątowej ω d (t) platformy i korzystając z zależności (2), można teraz określić funkcje zadanych prędkości ω pd (t) i ω ld (t) dla poszczególnych kół robota, które mogą zostać fizycznie zrealizowane przez pokładowy system sterowania robota. Poza analitycznym wyznaczeniem prędkości (11) oraz (12) istnieje także możliwość numerycznego obliczania tych prędkości w trybie on-line poprzez zastąpienie opercji różniczkowania operacją różnicy skończonej. Dla różnicy wstecz i wybranego elementarnego przyrostu czasu t otrzymujemy następujące wyrażenia: v d (n) = ± vxd 2 (n) + v2 yd (n), gdzie v xd (n) = x d(n) x d (n 1), t v yd (n) = y d(n) y d (n 1) t (15)

5 Laboratorium Podstaw Robotyki 4 5 oraz ω d (n) = ϕ d(n) ϕ d (n 1). (16) t 3.1 W środowisku Microsoft Visual C++ uruchomić przygotowany projekt aplikacji MTV3Controller do komunikacji i sterowania robotem (plik MTV3Controller.dsw w katalogu C:\laboratorium\PR2\cw4\MTV3Controller). W procedurze obsługi timera o nazwie OnTimer (wywoływanej teoretycznie co 40[ms]) przedstawiono sposób wykorzystania funkcji do komunikacji z robotem i odczytu danych z pokładu, miejsce gdzie powinien się znaleźć generator trajektorii zadanej oraz sposób zapisu danych do plików. Wartości sygnałów z generatora trajektorii zapisywane są w pliku generator.txt, natomiast dane odczytywane z pokładu robota w pliku robot.txt a. Kolejność zapisu poszczególnych danych jest następująca: generator.txt: t, x d, y d, ϕ d, ω ld, ω pd, robot.txt: t, x, y, ϕ, ω l, ω p. Do parametyzacji generatora można wykorzystać zmienne pól edycyjnych okna dialogowego aplikacji (zmienne m dp1... m dp6). 3.2 Zmodyfikować procedurę obsługi timera tak, aby możliwe było zrealizowanie następującej sekwencji sygnałów zadanych prędkości kół robota: ω pd = ω ld = +20[rad/s], dla t [0, 2), ω pd = ω ld = 20[rad/s], dla t [2, 4), ω pd = ω ld = 0[rad/s], dla t [4, ) przy czym czas wyrażono w [s]. Powyższe przebiegi prędkości kół odpowiadają trajektorii w postaci odcinka prostoliniowego na płaszczyźnie (x, y) (ruch do przodu/do tyłu ze stałą orientacją platformy). Wykreślić i porównać przebiegi sygnałów zadanych ω pd (t), ω ld (t) oraz sygnałów rzeczywistych ω p (t), ω l (t) odczytanych z pokładu robota (wykorzystać środowisko MATLAB lub Excel). Doświadczenie wykonać dla kół obracających się bez kontaktu z podłożem oraz dla robota umieszczonego na płycie boiska. Badania można powtórzyć dla innych poziomów prędkości ω pd (t), ω ld (t) rejestrując kilka okresów sygnałów zadanych. Wykreślić przebieg odczytanej orientacji ϕ(t) robota. 3.3 Powtórzyć powyższe badania dla zadanych prędkości kół w postaci sygnałów sinusoidalnych: (17) ω pd (t) = ω ld (t) = 10 sin 0.5t. (18) Wykreślić i porównać przebiegi zadanych oraz rzeczywistych prędkości kół robota. Wyznaczyć przebieg orientacji ϕ(t) platformy i porównać go z przebiegiem orientacji otrzymanym dla prędkości z równań (17). Czy rzeczywista orientacja robota w obu przypadkach jest stała? Jakie zjawisko można zaobserwować w przypadku sygnałów (18) i jak można je uzasadnić? Co może być przyczyną dryfu orientacji przy zmianie wartości zadanych prędkości kół robota? a Dane odczytane z robota są zapisywane do pliku tylko wtedy, gdy odebrana ramka jest prawidłowa.

6 Laboratorium Podstaw Robotyki W środowisku MATLAB zaprojektować (w postaci skryptu) generator zadanej trajektorii kołowej o promieniu r k = 0.4[m]. Przyjąć krok czasowy generowania trajektorii t = 40[ms]. Na płaszczyźnie (x, y) wykreślić otrzymaną trajektorię zadaną jako krzywą a y d (t) = x d (t). Wykreślić także czasowe przebiegi sygnałów prędkości ω pd (t) i ω ld (t). Doświadczalnie dobrać parametry trajektorii tak, aby prędkości kątowe kół nie przekraczały wartości 10[rad/s]. Obliczenia generatora wykonywać w jednostkach SI. 3.5 Zasymulowany w środowisku MATLAB generator zaimplementować w aplikacji sterującej robotem. Obliczenia generatora wykonywać w jednostkach SI. Na wspólnych wykresach zamieścić i porównać sygnały zadane z przebiegami otrzymanymi z pokładu robota. Jakie zjawisko można zaobserwować podczas próby realizacji zadanej trajektorii? W jaki sposób można wyeliminować wpływ dryfu orientacji na realizację zadanej trajektorii? 3.6 Powtórzyć poprzednie polecenia implementując (do wyboru): generator trajektorii zadanej w kształcie ósemki (mieszczącej się w prostokącie o bokach 1.2 1[m] lub innej mieszczącej się w wymiarach boiska), generator trajektorii zadanej w postaci sinusoidy o amplitudzie B = 0.2[m] i okresie T = 2π[s], położonej wzdłuż osi x układu globalnego. Zarejestrować wyniki pomiarów i przeanalizować jakość realizacji zadanej trajektorii w przestrzeni zadania. a Krzywa taka nazywana jest zadaną ścieżką geometryczną, gdyż parametr czasowy w tej sytuacji nie występuje w sposób jawny. Literatura [1] K. Kozłowski, J. Majchrzak. Nowe algorytmy sterowania nieholonomicznym robotem mobilnym. XIV Krajowa Konferencja Automatyki, strony , Zielona Góra, [2] M. Michałek, K. Kozłowski. Sterowanie nieholonomicznym robotem mobilnym metodą orientowania pola wektorowego. K. Tchoń, redaktor, Postępy Robotyki, strony Wydawnictwa Komunikacji i Łączności, Warszawa, [3] K. Tchoń, A. Mazur, I. Dulęba, R. Hossa, R. Muszyński. Manipulatory i roboty mobilne. Modele, planowanie ruchu, sterowanie. Akademicka Oficyna Wydawnicza PLJ, Warszawa, [4] Praca zbiorowa. Dwukołowy badawczy robot mobilny Mini-TrackerV3.0. Instrukcja obsługi systemu. Instytut Sterowania i Inżynierii Systemów, Zakład Sterowania i Robotyki, Politechnika Poznańska, Poznań, 2003.