Opis ćwiczeń na laboratorium obiektów ruchomych

Transkrypt

1 Gdańsk Opis ćwiczeń na laboatoium obiektów uchomych Implementacja algoytmu steowania obotem w śodowisku symulacyjnym gy obotów w piłkę nożną stwozonym w Katedze Systemów Automatyki Politechniki Gdańskiej mg inż. Piot Fietek pfietek@wp.pl 1. Wstęp Ćwiczenie ma za zadanie zapoznanie się studentów z poblemami występującymi podczas syntezy i implementacji algoytmu steowania dwukołowym obotem mobilnym. Ćwiczenie składa się z dwóch części. W części piewszej twozony jest pogam steujący obotem mobilnym w śodowisku symulacyjnym, służącym do symulacji zachowania się gupy obotów na boisku do gy w piłkę nożną. W części dugiej planowana jest implementacja stwozonego algoytmu steowania w zeczywistym układzie steowania obotem. Śodowisko symulacyjne nazywane seweem symulacji nawiązuje do ozgywek obotów w lidze MioSot oganizacji FIRA (Fedeation of Intenational Robosocce Association) ( W lidze tej oboty (7,5cm x 7,5cm x 7,5cm) gają na boisku o ozmiaze 150cm x 130cm. Mecz jest ozgywany pzez dwie dużyny. W skład każdej dużyny wchodzą tzy oboty oaz kompute z systemem wizyjnym (famegabe oaz kamea umieszczona nad boiskiem) (ys.1). System wizyjny ma za zadanie obsewować sytuację na boisku i dzięki odpowiednio napisanemu opogamowaniu do obóbki obazu, okeślać położenie i oientację kątową poszczególnych zawodników oaz położenie piłki. Uzyskane dane pzekazywane są następnie do aplikacji odpowiedzialnej za steowanie obotami. Pogamy steujące po pzeanalizowaniu otzymanych danych wyznaczają sygnały steujące, któe następnie dogą adiową pzesyłane są do obotów. Rys. 1. Rozgywki obotów w piłkę nożną w lidze MioSot oganizacji FIRA W celu umożliwienia łatwego twozenia i testowania pogamów steujących pacujących w wyżej opisanym śodowisku, stwozony został sewe symulacji naśladujący śodowisko gy obotów w piłkę nożną. Opisany sewe symulacji zastępuje zeczywiste boisko, oboty, piłkę, system wizyjny oaz kanał komunikacji adiowej między komputeem a obotami. Pogamy steujące otzymują od sewea infomację na temat stanu symulowanej gy (położenie i oientacja kątowa obotów, położenie piłki, czas witualny, stan gy) jednocześnie mają możliwość wpływania na pzebieg gy popzez wysyłanie sygnałów steujących do poszczególnych obotów. Dodatkowo sewe symulacji umożliwia umieszczenie na boisku dodatkowych obiektów takich jak: skzynie, ściany oaz słupy. Dodatkowe obiekty umożliwiają ozszezenie epetuau zadań jakie mogą wykonywać oboty w stwozonym śodowisku symulacyjnym (np. pzejechanie pzez labiynt, współpaca dwóch obotów w celu pzepchnięcia ciężkiej skzyni). 1

2 . Pzygotowanie do pacy z seweem symulacji W celu ozpoczęcia pacy z seweem symulacji należy wpiew zapoznać się instukcją obsługi sewea symulacji dołączoną do opisu ćwiczenia oaz z opisem implementacji postego algoytmu steowania pzy pomocy dostaczonego waz z seweem symulacji kodu źódłowego biblioteki DLL lub dostaczonego kodu źódłowego pogamu komunikującego się z seweem symulacji za pomocą potokołu UDP. Następnie wysłać pojedyncze ozkazy do sewea symulacji w celu upewnienia się czy pogam steujący pawidłowo łączy się z seweem symulacji i czy steowany obot pawidłowo wykonuje polecenia pogamu steującego. Kolejnym kokiem jest opacowanie algoytmu steowania obotem, ealizującego zlecone pzez powadzącego zadanie. Po opacowaniu odpowiedniego algoytmu steowania, należy zaimplementować algoytm w dowolnym języku pogamowania (w szczególności w języku C++), skompilować pogam steujący, uuchomić sewe symulacji i spawdzić czy oboty zachowują się zgodnie z naszymi oczekiwaniami. Jeżeli oboty zachowują się niezgodnie z naszymi założeniami, należy wócić do punktu implementacji lub opacowania algoytmu steowania. 3. Podążanie obota do punktu docelowego Naszym celem jest pzestawienie obota z położenia początkowego (x o, y o ) do położenia końcowego (x,y). Dla uposzczenia zakładamy, że po dodze nie ma pzeszkód, któe mogłyby zakłócić uch naszego obota. W celu ealizacji postawionego zadania pzyjmujemy następujący opis położenia punktu docelowego obota (ys.): y γ (x,y) (,) α y x x Rys.. Położenie punktu docelowego w lokalnym układzie współzędnych obota. Na ysunku widzimy obota, któego śodek leży w punkcie (x,y ) oaz punkt (x,y) będący punktem docelowym pzedstawionym w globalnym układzie współzędnych. (x,y ) jest to ten sam punkt pzedstawiony w lokalnym układzie odniesienia (z punktu widzenia obota). Można go ównież pzedstawić za pomocą współzędnych biegunowych γ i. Pzeliczenie na lokalny układ współzędnych (wyznaczenie współzędnych punktu docelowego w lokalnym układzie odniesienia) otzymujemy z wzou (1). a = sin ( α ) b = cos( α ) = ( y y ) a + ( x x = ( y y ) b ( x x ' v = b v + a vy ' vy = a v + b vy gdzie: x, y - współzędne obota w globalnym układzie współzędnych; α - oientacja kątowa; x, y - współzędne punktu docelowego w globalnym układzie współzędnych; x, y - współzędne punktu docelowego w lokalnym układzie współzędnych; v, v Y, v, v Y - pzeliczenie wektoów pędkości z układu globalnego na układ lokalny ) b ) a (1)

3 W najpostszym pzypadku możemy pzyjąć następującą stategię steowania: 1. Gdy kąt γ jest duży, należy zmniejszyć pędkość liniową obota i ustalić pędkość obotową gwaantującą zmniejszenie kąta γ.. Gdy kąt γ jest mały, należy zmniejszyć pędkość obotową obota i zwiększyć pędkość liniową obota do v Z (maksymalna pędkość obota). 3. Gdy obot dojedzie do punktu docelowego należy zatzymać obota lub pzełączyć się na następny punkt docelowy (śledzenie tajektoii). 4. Ponieważ obot może pouszać się ównież do tyłu, w wielu pzypadkach dążenie do spełnienia waunku γ = 0 jest nieuzasadnione. Jeśli γ = 170, w celu obócenia obota pzodem do punktu docelowego należy go obócić w lewo o kąt 170. W tym pzypadku lepiej jest jednak wykonać obót w pawo o 10 i ozpocząć jazdę do tyłu. Piewsze dwie zasady może spełnić następujący pzepis na sygnał steujący: if ( k( < v if ( k( > S ( = k k( = k 3 ) ψ ( v ω ( = k S γ ( ) then k( = 1 then k( = Z Z cos ω γ ( ( k( ) () ω S ( - wymagana pędkość obotowa; v S ( - wymagana pędkość liniowa obota; ω Z - maksymalna pędkość kątowa; v Z - maksymalna pędkość liniowa; k( - zmienna pomocnicza; k 1, k i k 3 - paamety steownika Aby zealizować wymaganie 4 wpowadzamy dodatkową zmienną ψ(, któa pzyjmuje watość 1, gdy obot pousza się do pzodu oaz -1, gdy chcemy pouszać się do tyłu. Aby steownik pacował popawnie musimy wpowadzić mechanizm zmiany kieunku uchu. Natualnym wydaje się spawdzenie watości y (ys.) a następnie wyznaczenie kąta γ po odpowiednich modyfikacjach watości x i y, jeśli jest to konieczne (3). if if ( < 0) then { ψ = 1 = = } ( 0) then { ψ = 1 } gdzie x i y są współzędnymi punktu docelowego w układzie lokalnym obota. Dla pzedstawionego pzypadku, pzełączenie między jazdą do pzodu i do tyłu nastąpi za każdym azem gdy kąt γ, pod jakim widziany jest punkt docelowy, będzie leżał poza pzedziałem <-90 ; 90 >. Mechanizm ten, choć badzo posty ma pewną wadę. Dla kąta γ ±90 oaz zeowej pędkości liniowej, może dojść do wielokotnych pzełączeń kieunku jazdy obota. Układ steujący będzie póbował obócić obota w kieunku punktu docelowego. Ponieważ kąt γ znajduje się na linii pzełączającej (γ ±90 ), z powodu bezwładności obota i układu steującego, obot będzie póbował obócić się az w pawo az w lewo. Rozwiązaniem tego poblemu jest wpowadzenie pętli histeezy w ustaleniu kieunku jazdy (ys.3). Szeokość pętli histeezy wynosi 10. Oznacza to, że jeśli obot jedzie do pzodu (ψ(=1), pzełączenie na jazdę do tyłu (ψ(=-1) nastąpi w momencie gdy kąt γ pod któym widziana jest pzeszkoda będzie większy od 95 lub mniejszy od -95. Pzełączenie w dugą stonę nastąpi gdy kąt γ będzie należał do pzedziału <-85 ; 85 >. W dugim pzypadku najpiew zmieniamy kąt α w modelu lokalnym obota, tak aby odpowiadał on aktualnemu kieunkowi jazdy (4): ( ψ = 1) then { α = α + π } if (4) Następnie wyznaczamy współzędne lokalne (x,y ) punktu docelowego i wyznaczamy kąt γ. Gdy kąt γ leży poza obszaem <-95 ; 95 > następuje pzełączenie kieunku jazdy, tzn. zmienna ψ( = -ψ(; x = -x ; y = -y ; i ponowne wyznaczenie kąta γ. (3) 3

4 akceptowalny obsza kątowy występowania punktu docelowego Jazda do pzodu Jazda do tyłu Rys.3. Pzełączanie kieunku jazdy obota w zależności od położenia punktu docelowego 4. Opis poponowanych ćwiczeń Potokół opisujący komunikację między pogamem steującym a seweem symulacji umożliwia podanie pzez pogam steujący nazwy dużyny lub wyświetlenie napisu w oknie komunikatów sewea. Pzy zaliczaniu zadań z pojektu, wymagane jest aby pogam steujący, zaaz po uuchomieniu symulacji, pzedstawił autoów pogamu oaz teść ealizowanego zadania. Ćwiczenie 1 (ocena 3-4-5) Napisać pogam mający na celu popowadzenie obota wzdłuż tajektoii. Zadaną tajektoię obota stanowią punkty kontolne, któe ma odwiedzić obot w okeślonej kolejności. Robot w momencie dojechania do punktu kontolnego ma się zatzymać i wykonać obót o 180 wokół własnej osi. Punkty tajektoii nie są z góy znane i muszą być wpiew wyznaczone. Nie jest ównież okeślone położenie początkowe steowanego obota. Ustalenie tajektoii obota: a) Na boisku znajduje się kilka obotów (liczba obotów nie jest z góy okeślona). Rzutujemy położenia obotów i piłki na oś y (x=0). Wymaganą tajektoię obota uzyskujemy po posotowaniu otzymanych punktów w kolejności od najmniejszej do największej watości współzędnej y. b) Tajektoia obota ma być tak wyznaczona, aby obot objechał piłką azy w kieunku uchu wskazówek zegaa, dojechał do śodka boiska a następnie wócił i objechał dwukotnie piłkę azy w kieunku pzeciwnym do uchu wskazówek zegaa. W tym pzypadku nie jest wymagane aby obot obacał się po dotaciu do każdego punktu zaplanowanej tajektoii. c) Schemat zachowania obota ustalony waz z powadzącym pojekt. Ćwiczenie (ocena 3-4) Napisać pogam steujący obotem, któy będzie wbijał nieuchomą piłkę do bamki pzeciwnika. Na boisku znajdować się będzie tylko jeden obot i piłka. Początkowe położenie piłki i obota nie jest z góy okeślone. Ćwiczenie 3 (ocena 4-5) Napisać pogam steujący obotem, któy będzie omijał slalomem to pzeszkód składający się z czteech nieuchomych obotów i na koniec udezał w nieuchomą piłkę. Wszystkie oboty i piłka będą ustawione wzdłuż linii (w szczególnym pzypadku wyznaczającej oś symetii boiska). Położenie początkowe steowanego obota oaz piłki jest okeślone. Dokładne położenie pozostałych czteech obotów nie jest znane. Wiadomo tylko, że będą one umieszczone wzdłuż odcinka łączącego steowanego obota i piłkę. Odległość pomiędzy kolejnymi obotami będzie na tyle duża, aby obot mógł swobodnie pzejechać między nimi. W takcie pokonywania slalomu nie można zahaczyć o żadnego z obotów. 4

5 Ćwiczenie 4 (ocena 4) Napisać pogam steujący obotem, któy będzie ealizował funkcję bamkaza. Położenie początkowe obota nie jest znane. Nie jest ównież znane położenie i pędkość piłki. Wiadomo tylko, że początkowe położenie i pędkość piłki oaz początkowe położenie obota będą tak dobane, aby doszło do stzelenia gola w momencie gdyby obot nie wykonał odpowiedniej akcji. Ćwiczenie 5 (ocena 4-5) Napisać pogam steujący dwoma obotami. Jeden z nich będzie ealizował funkcję bamkaza. Dugi, funkcję napastnika stzelającego gole. Położenie początkowe obotów nie jest znane. Nie jest ównież znane położenie i pędkość piłki. Oba oboty nie mogą się ze sobą zdezać. Ćwiczenie 6 (ocena 4-5-6) Napisać pogam steujący dwoma obotami bawiącymi się w beka. Jeden obot będzie obotem goniącym a dugi obotem uciekającym. W momencie gdy obot goniący dogoni obota uciekającego ma nastąpić zmiana ól. Aby umożliwić oddalenie się obota uciekającego od obota goniącego na bezpieczną odległość, po złapaniu obota, obot goniący ma stać bez uchu pzez czas ówny 5s. Na boisku znajduje się ównież piłka, któa powinna być omijana pzez oba oboty. Możliwe jest też umieszczenie na boisku dodatkowych, nieuchomych obotów, któe ównież powinny być omijane pzez goniące się oboty. Początkowe położenie obotów i piłki nie jest znane. Ćwiczenie 7 (ocena 5-6) Napisać pogam steujący obotem, któy będzie wbijał pouszającą się piłkę do bamki pzeciwnika. Na boisku znajdować się będzie tylko jeden obot i piłka. Początkowe położenie piłki i obota nie jest z góy okeślone. Początkowe położenie i pędkość piłki będą tak dobane aby obot miał szansę pzejąć piłkę w uchu i skieować ją w kieunku bamki. Ćwiczenie 8 (ocena 5-6) Napisać pogam steujący tzema obotami, któe po ozpoczęciu gy ustawią się w ogach tójkąta a następnie będą sobie nawzajem podawały piłkę. W takcie podawania piłki, oboty mogą zamieniać się miejscami. Ćwiczenie 9 (ocena 5-6) Napisać pogam steujący obotem, któego zadaniem jest pzepchanie skzyni z jednego punktu boiska do innego punktu boiska. W zależności od tudności zadania na boisku mogą znajdować się dodatkowe skzynie, ściany lub słupy (w najpostszej wesji na boisku znajduje się jedynie obot, skzynia i piłka). Ćwiczenie 10 (ocena 6) Napisać pogam steujący obotem, któego zadaniem jest pzejechanie pzez labiynt składający się ze ścian. Punkt statowy i końcowy obota oaz położenie ścian labiyntu nie jest z góy okeślone. 5