Dokumentacja. Robot mobilny klasy LineFollower

Transkrypt

1 Projekt Sterowniki robotów Dokumentacja Robot mobilny klasy LineFollower Autor: Wojciech Domski Prowadzący: mgr inż. Jan Kędzierski 26 kwietnia 2012

2 Spis treści 1 Opis projektu 2 2 Budżet 2 3 Fotografie 2 4 Schemat ideowy Jednostka centralna Zasilanie Sygnalizacja i sterowanie Zespół układów wykonawczych Sterowanie silnikami Sterowanie turbiną dociskową Czujniki odbiciowe Mechanika Układ jezdny Turbina dociskowa Model 3D Kod źródłowy Inicjalizacja Sterowanie Pomiar Badania Porównanie regulatorów Regulator P Regulator PD Regulator PID Mechanizm docisku Wnioski 14 Literatura 14 1

3 1 Opis projektu Przedmiotem projektu jest robot mobilny klasy LineFollower. Robot został wyposażony w silnik turbinowy, który ma zwiększyć jego przyczepność, a tym samym poprawić trakcję robota na zakrętach. W tym celu został wykorzystany wysokoobrotowy silnik prądu stałego. Projekt miał na celu sprawdzenie przydatności takiego rozwiązania w tego typu konstrukcjach. Podobne konstrukcje można znaleźć w robotach GreenNight, czy Hurricane. Jako algorytm sterowania robotem (systemu trakcji) został wykorzystany algorytm regulatora PID. Natomiast wykorzystywany jest tylko jego człon proporcjonalny. Również został wykorzystany wagowy odczytu danych z czujników co w efekcie usprawnienia reakcję robota na zmienny przebieg trasy. Dopasowanie odpowiednich wag do poszczególnych czujników ma na celu zwiększenia atutu jakim jest rozmieszczenie sensorów na podwoziu. Spowodowało to, że pojazd szybciej reaguje na zmiany trasy w postaci ostrych zakrętów. 2 Budżet W projekcie zostały wykorzystane czujniki odbiciowe QTR, ponieważ posiadają lepszą charakterystykę niż popularnie stosowane CNY70. Jako sterowniki silników prądu stałego zostały wykorzystane układy scalone TB6612, które posiadają maksymalny ciągły prąd obciążenia równy 1A na kanał. Natomiast przy połączeniu równoległym kanałów wydajność prądowa układu się podwaja i wynosi około 2A. Same silnik to Pololu HP 30:1 z metalowymi przekładniami. Tego typu przekładnia zużywa się mniej niż przekładnie wykonane z tworzywa sztucznego i tym samym jest odporniejsza na uszkodzenia mechaniczne. Pojedynczy silnik posiada moment wynoszący około 0.06Nm co jest wystarczające. Wszystkie elementy zostały wyszczególnione w tabeli 1. Element Szacunkowy koszt [PLN] Silniki 80 Sterowniki silników 90 Czujniki odbiciowe 50 Silnik turbinowy 40 Elementy elektroniczne (część zasilająca) 40 Pozostałe elementy elektroniczne 50 Akumulator Li-Pol 40 SUMA 390 Tabela 1: Wyszczególnienie elementów wykorzystanych przy budowie LineFollower a 3 Fotografie W tym rozdziale zostały przedstawione fotografie rzeczywistej konstrukcji. Dodatkowo w podrozdziale 5.3 został przedstawiony model 3D robota wykonany w programie Auto- Desk Inventor

4 Rysunek 1: Linefollower Rysunek 2: Moduł sensorów 3

5 Rysunek 3: Część dolna podwozia 4 Schemat ideowy W tej sekcji zostanie przedstawiony schemat ideowy robota mobilnego. Rozdział ten został podzielony na podrozdziały, które reprezentują bloki funkcjonalne robota. Pojazd można podzielić na następujące bloki funkcjonalne: Jednostka centralna Zasilanie Sygnalizacja i sterowanie Zespół silników Przyssawka 4.1 Jednostka centralna Sercem robota jest mikrokontroler Atmega32A. Na schemacie 4 został pokazany mikrokontroler wraz z elementami i wyprowadzeniami, które zapewniają pełną funkcjonalność. Również na schemacie 4 został umieszczony konektor do złącza programowania (autorskie układ pinów na złączu) oraz wyjście do komunikacji szeregowej po RS232. 4

6 4.2 Zasilanie Rysunek 4: Jednostka centralna Blok zasilania (rys. 5) w głównej mierze składa się z przetwornicy impulsowej o wyjściowych parametrach 5V/2A. Są to wystarczające parametry do zasilania jednostki centralnej jak i układów wykonawczych (silniki oraz przyssawka). W układzie występują dwa napięcia: 11V - napięcie akumulatora Wykorzystywane jest do zasilania silników poprzez sterowniki silników oraz jest wejściowym napięciem dla przetwornicy impulsowej 5V - napięcie zasilania logiki Jest to napięcie wyjściowe otrzymane z przetwornicy impulsowej. Wykorzystywane jest ono do zasilania logiki układu sterującego, sterowników silników (logika), załączanie silnika napędu turbinowego Jako akumulator został wykorzystany pakiet połączonych ze sobą trzech ogniw typu Li-Pol w konfiguracji 3S1P. W związku z wykorzystaniem tego typu ogniwa należy monitorować stan napięcia na akumulatorze. Spadek poniżej napięcia 2V na ogniwo może skutkować nieodwracalnym zniszczeniem pakietu. W związku z tym, aby uniknąć takiej sytuacji w jednostce centralnej została zaimplementowana funkcja pomiaru napięcia. Napięcie akumulatora jest podawane na dzielnik napięcia widoczny na rysunku 5. 5

7 4.3 Sygnalizacja i sterowanie Rysunek 5: Blok zasilania Jak widać na rysunku 6 przedstawia on wyprowadzenie sygnalizacji (wyjść) na dwie diody LED oraz sterowania za pomocą dwóch przycisków (wejścia). Diody sygnalizują miedzy innymi rozpoczęcie procesu kalibracji czujników odbiciowych, czy załączenie napędu tunelowego. Przyciski, które znajdują się na płycie głównej służą do sterowania podstawowymi zachowaniami robota. Są to między innymi załączenie bądź wyłączenie przyssawki, czy odczytanie stanu akumulatora i jego wysłanie przez port szeregowy. Elementem sterującym jest również przycisk resetu, który został zakwalifikowany do jednostki centralnej. Jest on widoczny na rysunku 4 w sekcji 4.1. Rysunek 6: Blok diod LED sygnalizujących i przycisków sterujących 4.4 Zespół układów wykonawczych W skład zespołu układów wykonawczych wchodzi: Sterowanie silnikami Sterowanie turbiną dociskową Sterowanie silnikami Wykorzystywane silniki to Pololu HP 30:1. Ich prąd w czasie jazdy wynosi około 800mA. Sama płyta główna zawiera gniazda (rys. 7), do których są wsadzane sterowniki silników oparte o układ TB6612FNG. Owe układy scalone są w stanie dostarczyć prądu 1A na kanał, a przy równoległym połączeniu (jak ma to miejsce w konstrukcji) prąd ciągły może wynosić do 2A. 6

8 Rysunek 7: Zespół sterowania silnikami Sterowanie turbiną dociskową Jako sterownik turbiny dociskowej, w której skład wchodzi silnik prądu stałego został wykorzystany MOSFET logiczny typu P IRLZ44N. Charakteryzuje się on pełnym otwarciem przy 5V. W czasie testów zostały wykorzystane również inne tranzystory m.in. BD911, TIP122 jednakże najlepsze rezultaty przyniósł zastosowany MOSFET IRLZ44N. Rysunek 8: Włączniki przyssawki 7

9 4.5 Czujniki odbiciowe Na rysunku 9 został przedstawiony schemat przedstawiający moduł czujników odbiciowych, który jednocześnie pełni rolę podwozia robota. Rysunek 9: Moduł czujników odbiciowych wraz ze złączem Schemat złącz jest ukazany na rysunku 10. Znajduje się one na głównej płycie robota. Połączenie między główną płytą, a złączem znajdującym się na podwoziu robota (moduł) odbywa się przez spięcie ich taśmą 9-cio żyłową. 8

10 Rysunek 10: Złącze czujników odbiciowych 5 Mechanika Robot został wykonany z płytek laminatu, które jednocześnie posłużyły za podwozie jak i główną płytę. Obie części zostały ze sobą połączone za pomocą szpilek M3 i śrub M3. W ten sposób otrzymano spójną i sztywną konstrukcję mechaniczną. 5.1 Układ jezdny Jako zespół napędowy zostały wykorzystane silniki firmy Pololu HP 30:1. Posiadają one przekładnię planetarną wykonaną z metalowych zębatek. Do osi przekładni zostały przymocowane koła wyposażone w oponę wykonaną z gumy, która zwiększa przyczepność robota. Silniki zostały przymocowane do podwozia za pomocą specjalnych łóż, które zostały do niego przykręcone. Trzecim punktem podparcia jest tzw. ballcaster. Jest to metalowa kula osadzona w plastikowym łożu. Natomiast łoże zostało przymocowane do spodniej części podwozia za pomocą metalowych śrób M2. Właściwości ślizgowe ballcaster a są wystarczająco dobre. Nie powoduje on nadmiernego tarcia. 5.2 Turbina dociskowa Jako przyssawka został wykorzystany napęd tunelowy firmy GWS. Zasilany jest on napięciem 5V. Przy takich warunkach pracy pobór prądu wynosi około 700mA. W celu przymocowania napędu tunelowego do podwozia należało w nim wywiercić dziurę. Do tak przygotowanego podwozia został przyklejony napęd tunelowy przy użyciu żywicy epoksydowej dwuskładnikowej. Naniesiona została cienka warstwa kleju na krawędzie otworu oraz na zewnętrzną część kołnierza. 5.3 Model 3D W ramach projektu został wykonany model 3D LineFollower a w programie AutoDesk Inventor W projekcie mechaniki zostały zawarte najważniejsze elementy robota mobilnego. Model ten pozwala na pełne odtworzenie części mechanicznej wraz z połączeniami. Zawiera on także prawidłowe skalowanie co pozwala na wykonanie elementów w razie ich zniszczenia. Na rysunku 11 został przedstawiony złożony model w programie AutoDesk Inventor

11 Rysunek 11: Model 3D robota mobilnego klasy LineFollower, widok z góry Rysunek 12: Model 3D robota mobilnego klasy LineFollower, widok od dołu 6 Kod źródłowy Program można podzielić na 3 zasiadnicze bloki funkcjonalne. 10

12 Inicjalizacja Sterowanie Pomiar 6.1 Inicjalizacja W tej części programu wykonywana jest wstępna inicjalizacja wszystkich układów peryferyjnych oraz tych wewnętrznych. W ich skład wchodzą między innymi: liczniki, konwerter ADC, czujniki odbiciowe, komunikacja szeregowa,... Jako część samej inicjalizaci można również zaliczyć kalibrację natężenia poziomu światła jaki jest odczytywany przez czujniki obiciowe. void q t r c a l i b r a t e ( void ) { u i n t 8 t i ; u i n t 1 6 t WartoscOdADC ; // wykonanie k i l k u k o n w e r s j i w c e l u o k r e s l e n i a minimalnego i maksymalnego // progu dla c z u j n i k a w danym srodowisku // i n i c j a l i z a c j a ADC dla c z u j n i k a odbiciowego //w c z a s i e trwania t e j procedury nalezy ustawic na chwile c z u j n i k //nad obszarem czarnym, a n a s t e p n i e nad obszarem bialym f o r ( i = 0 ; i < 2 0 ; ++i ) { // wykonaj konwersje a d c s t a r t c o n v e r s i o n ; // c z e k a j puki konwersja s i e n i e zakonczy while (! a d c i n t e r r u p t f l a g ) ; // wyczyszczenie bitu przez z a p i s a n i e l o g i c z n e j 1 s b i ( ADCSRA, ADIF ) ; WartoscOdADC = adc ( ) ; QTRMinimum = Minimum( WartoscOdADC, QTRMinimum ) ; QTRMaksimum = Maksimum( WartoscOdADC, QTRMaksimum ) ; } delay ms ( ) ; } //po wykonaniu t e j procedury mamy odczyty minimalne i maksymalne //w danym srodowisku Kod źródłowy, który został załączony powyżej odnosi się do kalibracji samych czujników odbiciowych. Odbywa się to w ten sposób, że odczyty pobierane są z wszystkich kanałów, do których podłączone są sensory. W ten sposób pobierane są najmniejsze oraz największe wartości. Dzięki temu wystarczy, że robot (jego część sensoryczna) znajduje się nad linią, a wszystkie odczyty zostaną pobrane automatycznie, bez konieczności przesuwania czujników nad torem. Kolejna procedura, która zawarta jest już w części głównej programu odpowiednio interpretuje uzyskane dane: ProgDlaADC = ( ( QTRMaksimum + QTRMinimum) >> 2) 2 ; 11

13 6.2 Sterowanie Sterowanie odbywa się przez wykonywanie obliczeń w pętli głównej programu: i f ( ZezwolNaPID == 1) { // o p e r a c j e na PID } Sterowanie = pid ( 0, WartoscProcesu, ( s t r u c t PID Dane )(&PID ) ) ; S t e r o w a n i e P r z e l i c z o n e = Sterowanie ; A = S t e r o w a n i e P r z e l i c z o n e + P r z e s u n i e c i e S t e r o w a n i a ; i f ( A < 0) A = 0 ; B = S t e r o w a n i e P r z e l i c z o n e + P r z e s u n i e c i e S t e r o w a n i a ; i f ( B < 0) B = 0 ; SilnikLewyWypelnienie ( A) ; SilnikPrawyWypelnienie ( B) ; // koniec o p e r a c j i na PID ZezwolNaPID = 0 ; Wyliczenie nowego sterowania przy użyciu regulatora PID odbywa się dzięki cyklicznym przerwaniom od licznika timer2. Owy licznik ustawia flagę ZezwolN ap ID dzięki czemu, w głównej pętli programu algorytm sterowania wywoływany jest tylko wtedy, gdy zachodzi taka potrzeba. 6.3 Pomiar Pomiar odczytu z sensorów jest wykonywany przy użyciu wewnętrznego konwertera ADC. Natomiast samo żądanie wykonania pomiaru i jego obróbka jest wykonywana w przerwaniu od licznika timer0: ISR ( TIMER0 OVF vect ) { // t e s t u j wartosc i f ( ProgojWartosc ( ADCWartosc, ProgDlaADC ) ) { //mamy wartosc 1 // dekodowanie numeru kanalu, z ktorego prowadzony j e s t odczyt WartoscProcesu = WagiDlaKanalowADC [ KanalADC ] ; // odczyt j e s t gotowy OdczytGotowy = 1 ; } e l s e { //mamy wartosc 0 // przelaczamy s i e na k o l e j n y kanal ++KanalADC ; } i f ( KanalADC >= 6) { KanalADC = 0 ; } 12

14 } // czyscimy k o n f i g u r a c j e ADC odpowiedzialna za wybor kanalu a d c c b r (ADMUX, (1<<MUX3 1<<MUX2 1<<MUX1 1<<MUX0) ) ; ADMUX = TablicaKanalowADC [ KanalADC ] ; // wlaczamy konwersje ADC a d c s t a r t c o n v e r s i o n ; Jak widać na dołączonym kodzie źródłowym kanały są cyklicznie zmieniane w celu sprawdzenia wszystkich odczytów. Również zastosowano tutaj ważenie odczytów ze względu na odsunięcie czujnika od centrum. Dzięki temu, gdy robot ma do pokonania zakręt zostanie to zauważone na poziomie samego odczytów danych sensorycznych. W ten sposób robot będzie w stanie bardziej efektywnie zareagować na zmianę trasy. Początkowo był stosowany algorytm, który polegał na sumowaniu wag przypisanych do poszczególnych czujników jednakże okazał się on dawać fałszywą informację o stanie faktycznym robota. Załóżmy, że mamy 5 czujników. Numer czujnika Waga Tabela 2: Czujniki wraz z przypisanymi wagami Jeśli linia znajdowała się pomiędzy czujnikiem 1 i 2, tak, że oba dawały odczyty potwierdzający obecność linii pod nimi odczytywana wartość była równa sumie ich wag, czyli 6. Zatem uchyb wynosi 6. Regulator odpowiednio wysterował silniki. Gdy jednak zakręt był coraz ostrzejszy, tak, że linia znajdowała się tylko pod czujnikiem nr 1 odczytywana była wartość 4. Uchyb w tym momencie wynosił 4. Sugerowało by to, że sytuacja uległa poprawie, a tak na prawdę robot był bliski wyjścia z toru. Zastosowanie algorytmu, który był prezentowany na początku tej sekcji wyeliminowuje tą sytuację. Brana jest pod uwagę tylko wartość odczytana z jednego z czujników, którego położenie jest najbardziej skrajne. 7 Badania W ramach projektu należało sprawdzić zachowania regulatora PID dla tego typu konstrukcji oraz użyteczność przyssawki. 7.1 Porównanie regulatorów W ramach projektu zostały zaimplementowane trzy regulatory: proporcjonalny (P), proporcjonalnoróżniczkujący (PD) oraz proporcjonalno-całkująco-różniczkujący (PID). W każdym z podrozdziałów zostały one osobno opisane Regulator P Regulator proporcjonalny spisuje się dobrze w warunkach, gdy trasa składa się z łagodnych łuków i odcinków prostych. Potrafi on przeprowadzić robota przez trasę bez najmniejszych problemów. Kiedy trasa składa się z ostrych zakrętów pojazd ma problemy z pokonaniem takiej trasy. Polegają one na wyjeżdżaniu z łuków, których promień jest zbyt mały Regulator PD Regulator PD został pomyślnie zaimplementowany. W celu doboru nastaw należy odpowiednio wyskalować charakterystykę odpowiedzi skokowej i zastosować pierwszą metodę Ziegler a-nichols a. Sam robot wyposażony został w mechanizmy, które mogą pomóc w zdjęciu charakterystyki odpowiedzi skokowej. Odbywać się to może dzięki komunikacji przez port szeregowy. W każdym takcie, gdy zostanie odczytany nowy stan procesu jest on wysyłany przez port RS232. Posiadając punkty pomiarowe charakterystyka może zostać poddana analizie. Nastawy regulatora można wyliczyć stosując pierwszą metodę Ziegler a-nichols a ([1]). Sam proces przeprowadzania tych czynności został opisany w [2]. 13

15 7.1.3 Regulator PID Podobnie jak regulator PD, algorytm regulatora PID został zaimplementowany w programie. Implementacja algorytmu pozwala na wyłączanie członów regulatora, a tym samym wykorzystanie dowolnego regulatora. 7.2 Mechanizm docisku Jako mechanizm docisku wykorzystywana jest turbina dociskowa, która spełnia swoje zadanie. Przeprowadzona kilka testów i na podstawie obserwacji można stwierdzić poprawę trakcji robota. Została ona rozważona pod dwoma względami: Pokonywanie zakrętów o mniejszym promieniu Szybsza jazda Dla regulatora P przy trasie, która posiada łuki o dużym promieniu robot może jechać szybciej. Jego prędkość, gdy odczyt z czujników sugeruje, że linia znajduje się dokładnie na środku linii sensorów jest większa. Natomiast przy zastosowaniu tego samego regulatora i trasy, o łukach, których promienie są mniejsze niż poprzednio robot jest w stanie utrzymać się na trasie. Jednakże jego prędkość jest mniejsza niż w pierwszym przypadku. Wynika to z tego, że przesuwał się on pchany siłą bezwładności, która powodowała wypadanie LineFollower a z trasy. Związane jest to także po części z czasem reakcji sensorów odbiciowych śledzenie linii. 8 Wnioski Konstrukcja robota została w pełni zrealizowana zgodnie z założeniami projektowymi. Podczas testów zostały zauważone pewne aspekty, które mogą zostać lepiej zrealizowane przy tworzeniu podobnej konstrukcji. Jednym z takich aspektów jest zastosowanie komparatorów pomiędzy czujnikami, a wejściami mikrokontrolera. Zastosowanie tych układów w znacznym stopniu przyspieszyło by odczyt danych z sensorów. Szybszy odczyt mógłby umożliwić zwiększenie częstotliwości wywoływania algorytmu regulacji. W efekcie można by uzyskać lepsze sterowanie układem napędowym. Literatura [1] Karl J. Astrom, Tore Hagglung, PID Controllers, 2nd Edition [2] Wojciech Domski, Zestaw edukacyjny - Regulator PID 14