POLITECHNIKA WROCŁAWSKA WYDZIAŁ ELEKTRONIKI PRACA DYPLOMOWA MAGISTERSKA

Transkrypt

1 POLITECHNIKA WROCŁAWSKA WYDZIAŁ ELEKTRONIKI Kierunek: Specjalność: Automatyka i Robotyka (AIR) Robotyka (ARR) PRACA DYPLOMOWA MAGISTERSKA Implementacja wykonywania wybranych zadań w robocie autonomicznym Implementation of selected tasks execution in an autonomous robot Autor: Paweł Michałkiewicz Prowadzący pracę: dr inż. Marek Wnuk PWr, I-6 Ocena pracy: WROCŁAW 2011

2

3 Spis treści 1 Wstęp Geneza Ogólna koncepcja pracy Zachowania autonomiczne Architektura Subsumption Architektura Motor Schema Linefollower Automatyzacja samochodów Makieta Informacje o makiecie Charakterystyka konstrukcji mechanicznej Opis konstrukcji elektronicznej Algorytmy zachowania robota mobilnego Jazda w kolumnie Znajdywanie miejsca do parkowania Parkowanie równoległe Inne zachowania Badania Sensory PSD Przyspieszenie Jazda do przodu Maksymalny promień skrętu Implementacja algorytmów zachowania Zachowywanie bezpiecznej odległości Znajdowanie miejsca do parkowania Podsumowanie 49 A Opis użytych sensorów 51 A.1 Czujniki optyczne PSD A.2 Kodery

4

5 Rozdział 1 Wstęp 1.1 Geneza Wraz ze wzrostem dobrobytu zarówno w Polsce jak i na świecie, dostęp do pojazdów mechanicznych, takich jak samochody, motocykle, stał się bardzo łatwy. Rosnąca liczba prowadzi do szeregu utrudnień w komunikacji, jak jazda w korku czy znalezienie miejsca do parkowania itp. Koncerny samochodowe od dłuższego czasu starają się opracowywać systemy wspomagające prowadzenie pojazdów oraz zwiększające bezpieczeństwo na drogach. Burkhard Huhnke, dyrektor laboratorium Volkswagena zajmującego się elektroniką widzi przyszłość motoryzacji w autonomicznych pojazdach. Swoje twierdzenie argumentuje dużą liczbą śmiertelnych wypadków spowodowanych przez kierowców [6]: Przez dziesięć lat zabiera nam się 50 tys ofiar, to szaleństwo. Porównując to do wypadków lotniczych - czy zaakceptowalibyśmy sytuację w której codziennie rozbijają się dwa samoloty? Dzień w dzień, przez cały rok? Nie, nigdy. Ale ewidentnie akceptujemy taką sytuację na drogach. Zauważa również, że autonomiczne pojazdy poprawią komfort jazdy (np. przez eliminacje korków ulicznych) oraz pozwolą na optymalizacje zużycia energii w tych samochodach. Volkswagen intensywnie współpracuje z Uniwersytetem Stanforda w dziedzinie samochodów mobilnych. Wspólnie brali udział w zawodach DARPA Urban Challenge, oraz opracowali wspólnie autonomiczne AudiTT mające pokonać trasę jednego z najtrudniejszych wyścigów samochodowych Pikes Peak International Hill Climb [7] Nie tylko koncern Volkswagena prowadzi badania nad autonomicznymi pojazdami. Podczas wakacji w roku 2010 Google testowało w Californii swoją flotę tego typu pojazdów. Były to samochody GoogleStreet wyposażone w systemy autonomiczne pozwalające na samodzielną jazdę [8]. Innym przykładem są 4 samochody VIAC skonstruowane przez zespół VisLab z Uniwersytetu w Parmie. Przejechały one drogę z Włoch do Chin (13 tys. km) prawie bez żadnej ingerencji człowieka [9]. Wspomniane badania Uniwersytetu Stanforda oraz koncernów samochodowych zainspirowały mnie do napisania pracy magisterskiej na ten temat. Pozwoliły mi również na wyróżnienie kilku podstawowych warunków, jakie musi spełniać autonomiczny pojazd, aby był użyteczny.

6 Wstęp Ogólna koncepcja pracy Praca dyplomowa została podzielona na trzy rozdziały. W pierwszym rozdziale przedstawiono koncepcje zachowań autonomicznych. Skupiono się głównie na podejściu behawioralnym. W kolejnej części opisywana została konstrukcja mechaniczna oraz elektroniczna platformy mobilnej rys. 1.1, przeznaczonej do implementacji różnego typu zachowań autonomicznych. Jest ona makietą w skali 1 do 10, wyposażoną w napęd różnicowy na przednie koła. W następnych rozdziałach opisano kilka możliwych do implementacji zachowań, przeprowadzone wymagane badania potrzebne przy wdrążaniu zadań. Pod koniec przedstawiono kilka zadań, które zostały oprogramowane na platformie. Rysunek 1.1 Zdjęcie platformy mobilnej

7 Rozdział 2 Zachowania autonomiczne Istnieje wiele systemów pozwalających na realizacje zachowań autonomicznych. Jednym z możliwych rozwiązań są systemy reaktywne (behawioralne), oparte na inteligencji niskiego poziomu. R.C. Arkin w swojej książce [1] opisał ten typ podejścia do sterowania następująco: Simply put, reactive control is a technique for tightly coupling perception and action, typically in the contex of motor behaviors, to produce timely robotic response in dynamic and unstructured worlds. Nawiązując do tych słów, można wyróżnić następujące cechy takich systemów: szybkość w działaniu, niezależność od dokładności i zupełności modelu świata, nieskomplikowane obliczenia, proste zależności między sygnałem z czujników a reakcją. Podejście reaktywne można rozpatrywać w kilku aspektach. Robot jest umiejscowiony w rzeczywistym świecie (Situatedness). Nie operuje on w abstrakcyjnej jego reprezentacji, raczej w nim samym. Zajmuje fizyczną przestrzeń, czego konsekwencją jest oddziaływanie ze światem, czego nie można wiernie zasymulować (Embodiment). Inteligencja powstaje z interakcji agenta z otoczeniem. Nie jest ona właściwością tego robota czy też środowiska, a raczej rezultatem wzajemnych oddziaływań między nimi (Emergence). Źródła podejść behawioralnych można często upatrywać w przyrodzie. Przykładem mogą być sieci neuronowe, których genezą są badania w dziedzinie neurologii, bazującej na budowie neuronu (stąd jej nazwa). Podobieństwa te można zobaczyć na rys. 2.1a oraz rys. 2.1b. Wejścia w sieci neuronowej odpowiadają dendrytom, a wyjście aksonowi. Cechami charakterystycznymi systemów behawioralnych są: zachowania służące jako podstawowe bloki do budowy akcji robota, nieużywanie abstrakcyjnych reprezentacji do celu generacji reakcji, ich podstawę stanowią modele zachowań pochodzące z obserwacji zwierząt, możliwość łatwej rozbudowy poprzez dodawanie nowych zachowań.

8 6 2. Zachowania autonomiczne a). b). Rysunek 2.1 a). Schemat neuronu, b). Schemat pojedynczego neuronu w sztucznej sieci neuronowej Kryteria oceny systemów behawioralnych zostały wymienione poniżej: Wsparcie dla zrównoleglenia (support for parallelism): określa, w jakim stopniu architektura wspiera zrównoleglenie. Odwzorowanie w sprzęcie (hardware targetability): definiuje, jak dobrze system może być odwzorowany w sprzęcie, może też mówić o stopniu wsparcia dla implementacji na poziomie wyspecjalizowanych układów scalonych. Nakierowanie na nisze (niche targetability): wyznacza, jak dobrze robot pasuje do środowiska, w którym pracuje. Wsparcie dla modularności (support for modularity): mówi, jakie metody dostarcza architektura do realizacji modularności. Trwałość (robustness): Jak dobrze system spisuje się w sytuacji awarii któregoś z jego elementu, np. sensora. Stosowalność (timeliness in development): odpowiada na pytanie, czy architektura jest tylko podejściem filozoficznym, czy też dostarcza elementów, umożliwiających jej rzeczywistą implementacje. Elastyczność w czasie pracy(run time flexibility): Jak łatwo system może być rekonfigurowany i dostosowywany w trakcie jego pracy. Efektywność pracy (performance effectiveness): Jak dobrze robot wykonuje swoje zadanie. Na podstawie zebranych w [1] opisów przedstawiane zostały wybrane architektury. 2.1 Architektura Subsumption W latach 80. XXw. Rodeny Brooks zaproponował architekturę subsumpion. Była ona całkowicie reaktywnym systemem opartym o metodykę behawioralną, mało popularną w tamtym okresie. Brooks, argumentując sens podejścia patrz-myśl-działaj (sense-planact) twierdził m.in., że podejście wykorzystujące modele świata oraz formalny opis wiedzy, stanowi przeszkodę w szybkich odpowiedziach robotów oraz prowadzą badania robotyczne w złym kierunku. Alternatywą miała być zaproponowana przez niego architektura, której głównymi założeniami były:

9 2.1. Architektura Subsumption 7 złożone zachowania nie musza być produktem pełnego systemu sterowania, inteligencja wynika z obserwacji, świat jest sam dla siebie najlepszym modelem, prostota jest zaletą, roboty powinny być tanie, niezawodność w środowisku narażonym na zakłócenia oraz niesprawnych sensorów jest celem projektów, planowanie jest unikaniem odpowiedzi na pytanie: Co dalej? obliczenia na pokładzie robota są najważniejsze, systemy powinny być budowane narastająco, brak reprezentacji, brak kalibracji, brak złożonych obliczeń, brak szybkiej komunikacji. Jak pokazuje rys. 2.2, automat stanu w tej architekturze składa się z modułu behawioralnego, definiującego reakcje na podstawie danych sensorycznych, z sygnału resetu przywracającego wspomniany moduł do stanu początkowego. Dodatkowo linie wyjścia i wejścia wyposażono w inhibitor (inhibitor) oraz supresor (suppressor), umożliwiające opóźnienie lub zdławienie reakcji przez inny aktywny moduł behawioralny. Opóźniacz uniemożliwia Rysunek 2.2 Schemat modułu behawioralnego sygnałowi transmitowanemu wzdłuż linii automatu dotarcie do urządzeń wykonawczych. Dławik natomiast zapobiega transmisji sygnału prądowego i zastępuje go wiadomością nadrzędną. Na rys. 2.3 pokazano natomiast wygląd całego systemu w architekturze subsumption dla której poszczególne zachowania ułożono w system wzajemnie blokujących się reakcji. Przykładowe implementacje: Allen - konstrukcja R.Brooks a z 1986r, pierwszy robot wykorzystujący architekturę subsumption. Do nawigacji użyto sonaru i odometrii. Tom i Jerry - małe samochody zabawki wyposażone w podczerwone czujniki zbliżeniowe skonstruowane przez zespół R. Brooks a w 1990r.

10 8 2. Zachowania autonomiczne Rysunek 2.3 Schemat systemu w architekturze subsumption SQUIRT - robot reagujący na hałas. W miarę nasilania się hałasu robot oddala się w kierunku przeciwnym od źródła, powrót następuje po zaniknięciu sygnału. Konstrukcja A.M Flynn a z 1989r. Polly - robot-przewodnik po laboratorium AI uniwersytetu MIT skonstruowany przez Horswill a w 1993 roku. COG - robot humanoidalny z 1994 roku zaprojektowany przez Brooks a i Stein a w celu poznania procesów postrzegania przez ludzi. 2.2 Architektura Motor Schema Teoria dotycząca architektury Motor Schema została zaprezentowana w 1974 r. na narodowym spotkaniu North American Society for Psychology of Sport and Physical Activity. Była ona spadkobiercą teorii opisanej przez J. A. Adamsa, w 1971 r., definiującej programowanie napędów [3]. Do sterowania naprawdę szybkimi akcjami systemy opierające się na odpowiedzi produkowanej z pętli sprzężenia lub informacji sensorycznej, były zbyt wolne, aby można było na nie liczyć. Dlatego też architektura Motor Schema zakłada, iż szybkie wykonanie akcji wymaga zaplanowania jej z wyprzedzeniem oraz odpowiednie jej zaprezentowanie w pamięci. W Motor Schema do sterowania używany jest zbiór dyskretnych akcji. Stosując więc odpowiednią organizację pamięci można osiągnąć niezliczoną liczbę kombinacji akcji. Do definiowania systemu behawioralnego Motor Schema używa następujących możliwości: zachowania napędów zależą od konkurujących między sobą sterowań różnych zachowań, przechowywane są w pamięci zarówno informacje o tym jak reagować jak również w jaki sposób reakcje są osiągane, Motor Schema jest rozproszonym modelem obliczeń, dostarczany język opisu pozwala na łączenie akcji z obserwacją, poziomy aktywacji są związane z schematem, który determinuje ich gotowość lub stosowalność do reakcji. Motor Schema dostarcza metod uczenia poprzez przejęcie i dostrajanie schematu.

11 2.2. Architektura Motor Schema 9 Na rys. 2.4 przedstawiono schemat zależności obserwacja - akcja. Schematy obserwacyjne (Perceptual Schema) dostarczają informacji z otoczenia, charakterystycznej dla konkretnego zachowania. Są one rekurencyjne; mogą używać pod schematów do wyciągania dokładniejszych informacji z innych schematów obserwacyjnych. Przykładem może być rozpoznawanie osoby przy użyciu kilku czujników, np. czujnika podczerwieni, dającego informacje o temperaturze oraz kamery wykrywającej twarz osoby. Wyjście każdego Motor Schema składa się z wektora akcji, który opisuje zarówno orientacje jak i wartość tego wektora. Z kolei wektor akcji definiuje sposób, w jaki robot powinien się poruszyć, aby odpowiedzieć na odebrany bodziec. Rysunek 2.4 Schemat zależności obserwacji i reakcji w architekturze Motor Schema Istnieje wiele juz zdefiniowanych Motor Schemas, oto kilka z nich: move-to-goal - porusza się w stronę wykrytego celu: V magnitude = stała wartość wzmocnienia. V direction = w kierunku spostrzeżonego celu. avoid-static-obstacle - odjeżdżanie od pasywnej przeszkody, 0 d > S, S d V magnitude = G R < d <= S, S R gdzie d <= R S = sfera wpływu, R = promień przeszkody, G = wzmocnienie, d = odległość robota od centrum przeszkody V direction = radialny wzdłuż linii robot centrum przeszkody o zwrocie skierowanym od przeszkody. noise - poruszanie się w losowym kierunku. V magnitude = stała wartość wzmocnienia. V direction = losowo zmieniany kierunek co stały interwał czasowy. Konstrukcje robotyczne wykorzystujące architekturę Motor Schema:

12 10 2. Zachowania autonomiczne Ganymede, Calisto, Io z uniwersytetu Gorgia z 1993r. Ich pierwotnym zadaniem było oczyszczenie pomieszczenia z różnego rodzaju śmieci. W 1994 roku wygrał turniej Office Cleanup Event podczas AAAI Mobile Robot Competition. Ren oraz Stimpy para robotów potrafiących uciekać, unikać, szukać. George pierwszy robot potrafiący wykonywać dokowanie przy użyciu architektury behawioralnej (1990r.). 2.3 Linefollower To jedna z najprostszych konstrukcji robotycznych. Jest to pojazd najczęściej dwukołowy używający swoich sensorów, aby w pełni autonomiczny sposób poruszać się wzdłuż wyznaczonej linii. Najczęściej linie wyznaczane są kolorem, a do ich rozpoznawania używane są czujniki optyczne odbiciowe. Fala świetlna odbijając się od powierzchni, steruje fototranzystorem wytwarzającym odpowiednie napięcie. Zależy ono między innymi od barwy powierzchni, od której się odbija. Oprócz prostych konstrukcji składających się z napędów, czujników optycznych oraz układu sterującego, spotykane są też konstrukcje wykorzystujące obraz z kamery. Jest on przekształcany przy pomocy różnych algorytmów dzięki czemu możliwe jest uzyskanie informacji o położeniu linii na obrazie. Są one dostarczane do układu sterującego, który przekształca je na sygnały użyteczne napędów. Przykładowym Linefollowerem wykorzystującym informacje z kamery jest ChumbyRoller, konstrukcja oparta o zestaw rozwojowy Chumby One [10]. Diagram blokowy przedstawiono na rys. 2.6 Jednostką centralną zestawu jest układ Freescale imx233 ARM926EJ-S o częstotliwości taktowania 454Mhz, wyposażony w 64MB pamięci RAM oraz w 2GB kartę SD. Jako urządzenie wyświetlające służy 3,5 kolorowy ekran LCD, wyposażony w touchscreen, który umożliwia obsługę specjalnie dostosowanej do układu dystrybucji Linuksa. Dodatkowo na potrzeby tej płytki opracowano bibliotekę ChumbyCV z funkcją prostej obróbki wideo. ChumbyRoller jest robotem klasy (2,0) wyposażonym w zestaw rozwojowy Chumby One oraz kamerę na USB firmy Logictech. Jego zadaniem jest poruszanie się po czarnej planszy wzdłuż ciągłej, białej linii. Do wykonywania tej czynności wykorzystywana jest wspomniana kamera, z której obraz jest przetwarzany przy użyciu biblioteki ChumbyCV. Na rys. 2.5 przedstawiono wygląd robota. Rysunek 2.5 Robot ChumbyRoller

13 2.4. Automatyzacja samochodów 11 Rysunek 2.6 Schemat blokowy zestawu Chumby One 2.4 Automatyzacja samochodów Od lat na rynku motoryzacyjnym pojawiają się systemy wspomagające kierowców oraz optymalizujące zużycie energii przez samochody. Jednym z najpopularniejszych są systemy do wspomagania parkowania. Występują one w różnych formach. Systemy do samodzielnego montażu informują (podczas jazdy tyłem) o zbliżającej się przeszkodzie. Systemy wspomagane przez obraz z kamery, z zaznaczonym obrysem tylnej części pojazdu. Autonomiczne rozwiązania wskazujące konkretne miejsce do parkowania, wykonują także samodzielnie parkowanie równoległe.

14

15 Rozdział 3 Makieta 3.1 Informacje o makiecie Makieta wykorzystywana w pracy magisterskiej jest rozbudowaną konstrukcją wykonaną przez Arkadiusza Materka w ramach jego badań [2]. Została ona wykonana z samochodu RC z napędem różnicowym na 4 koła. Aktualna wersja posiada napęd różnicowy przeniesiony tylko na przednie koła. Tylne pozostały wolne i wyposażono je w kodery. Koła są zamontowane na elastycznym zawieszeniu. Wymiary: rozstaw kół 152mm odległość między kolami tylnymi a przednimi 265mm promień koła 32,315mm długość podwozia 380mm szerokość nadwozia 123mm wysokość nadwozia Charakterystyka konstrukcji mechanicznej Pojazd użyty w pracy magisterskiej jest samochodem modelarskim typu RC na podwoziu TT-01, wykonanym w skali 1:10. Koła przednie służą zarówno do zadawania kierunku jak i zwrotu wektora prędkości poruszania się pojazdu. Do zadawania kierunku służy serwomechanizm firmy Hitec. Serwo modelarskie Hitec HS-325HB zasilane jest napięciem 5V. Sterowanie mechanizmem wykonuje się poprzez zadawanie sygnału PWM o określonej długości impulsu. Zakres długości impulsu znajduje się w przedziale od 1ms do 2ms, przy czym 1,5ms jest określone jako położenie neutralne. Sterowanie powtarzane jest co 10-20ms. Zaletą serwomechanizmu są wytrzymałe karbonowe tryby, natomiast największą wadą brak możliwości odczytu dokładnego obrotu orczyka. Elementem zadającym zwrot i wartość wektora prędkości makiety jest silnik modelarski klasy 540. Do sterowania silnikiem służy regulator obrotów TEU-101BK. Pozwala on regulować obroty w dwóch kierunkach, możliwe jest również hamowanie silnikiem przy pomocy wspomnianego układu. Aby ustawić bieg wsteczny należy wyhamować pojazd, wyzerować sterownik i ustawić tryb jazdy wstecz. Sterowanie regulatorem wygląda podobnie jak w

16 14 3. Makieta przypadku serwomechanizmów, do sterowania używa się odpowiednio spreparowanych sygnałów PWM. Źródłem zasilania modelu jest pakiet litowo-polimerowy firmy 3E Model - Drivers, złożony z 2 baterii połączonych szeregowo o łącznym napięciu 7,4V oraz pojemności 3,8Ah. Wydajność prądowa układu wynosi 25C, co daje maksymalne możliwe natężenie prądu równe 95A. Pakiet nie jest wyposażony w żaden układ zapobiegający nadmiernemu rozładowywaniu ogniw. Do celów sterowania przyjęto kinematykę modelu taką samą jak dla samochodu kinematycznego o napędzie różnicowym na przednich kołach. Do celu sterowania wykorzystano informację z koderów umieszczonych w tylnych kołach. Do sterowania użyto 2 sygnałów sterujących prędkości liniowej kół przednich oraz zmia- Vp α X Y0 Y Vt Ѳ d y l x X0 Rysunek 3.1 Schemat kinematyki ny kąta skrętu tych kół. Natomiast dzięki koderom w kołach tylnych możemy odczytać prędkości ich obrotu, z których można wyznaczyć prędkość liniową kół tylnich υ t oraz prędkość obrotową pojazdu ω poj. Wzory (3.1) oraz (3.2) opisują transformację z prędkości obrotowych, otrzymanych z koderów dla koła lewego ω tl oraz prawego ω tp na υ t oraz ω poj. υ t = r ω tp + ω tl 2 (3.1) ω poj = r ω tp ω tl (3.2) l Z równań (3.3) opisujących kinematykę robota klasy (1,1) z początkiem układu lokalnego robota umieszczonym na środku tylnej osi kół samochodu oraz równań kinematyki (3.4) przy pomocy υ t i ω poj, można wyprowadzić równania przedstawiające prędkość liniową pojazdu oraz prędkość zmiany kąta kół przednich. ẋ cos (α) cos (θ) 0 ẏ [ ] θ = cos (α) sin (θ) 0 v sin (α) 0 (3.3) d γ γ 0 1

17 3.1. Informacje o makiecie 15 ẋ ẏ θ = cos (θ) 0 sin (θ) [ υt ω poj ] (3.4) Wiedząc, że obie kinematyki są opisane w jednym układzie odniesienia, można na podstawie równań (3.3) i (3.4) wyznaczyć sterowania v oraz γ. Należy wykonać podstawienie (3.5), cos (α) cos (θ)v = cos (θ)υ t cos (α) sin (θ)v = sin (θ)υ t (3.5) sin (α) v d = ω poj aby otrzymać równania (3.6) - (3.9) opisujące sterowanie. v = υ 2 t + d 2 ω 2 poj (3.6) cos (α) = υ t v sin (α) = dω poj v γ = α (3.7) (3.8) (3.9) Po kolejnych prostych podstawieniach oraz wykorzystaniu wzorów (3.1), (3.2) uzyskano końcową postać równań (3.10) - (3.13). v = r ( ) ( ωtp 2 + ωtl 2 d 2 l + 1 ) Opis konstrukcji elektronicznej ( ) 1 + ω tp ω tl 2 2d2 l 2 γ = arccos r (ω tp + ω tl ) 2v γ = arcsin dr (ω tp ω tl ) lv γ = arctan 2d(ω tp ω tl ) l(ω tp + ω tl ) (3.10) (3.11) (3.12) (3.13) Na rys. 3.2 przedstawiono ideowy schemat konstrukcji elektronicznej. Centralnym elementem układu jest mikroprocesor firmy freescale MC9S12A64. Należy on do 16 bitowych procesorów z rodziny HCS12. Posiada on szereg różnego typu peryferiów, z których wykorzystano: przetworniki 16 bit analogowo-cyfrowe użyte do obsługi czujników odbiciowych PSD (Position Sensitive Device) firmy Scharp. pary wejść IC (Input Capture) zastosowano do odczytu sygnału kwadraturowego z koderów magnetycznych AS5040, generatory sygnału PWM (Pulse-Width Modulation) posłużyły jako sterowanie regulatora obrotów silnika napędowego oraz serwomechanizmu zadającego skręt kół, moduł SCI (Serial Communication Interface) odpowiada za komunikację z modułem radiowym CC1000PP.

18 16 3. Makieta PSD1 AS5040 koder magnetyczny MC9S12A64 PSD2 Pin3-A Pin4-B IOC7/PT7 IOC6/PT6 PAD0/AN0 PAD1/AN1 V0 V0 PSD3 AS5040 Koder magnetyczny Pin3-A Pin4-B IOC5/PT5 IOC4/PT4 PAD2/AN2 PAD3/AN3 V0 V0 PSD4 Moduł radiowy CC1000PP RxD TxD CTS RxD0 TxD0 RXD1 PAD4/AN4 PWM0/PP0 PWM1/PP1 V0 PSD5 RTS TxD1 Serwomechanizm Sterownik Obrotów TEU-101BK Silnik Rysunek 3.2 Schemat konstrukcji elektronicznej Jak wspomniano, w konstrukcji użyto czujników PSD firmy Scharp. Są to czujniki analogowe, których działanie przedstawiono w dodatku A.1. Wykorzystane modele to: 2Y0A21 - zasięg od 100mm do 800mm, 2Y0A02 - zasięg od 200mm do 1500mm, 2D120X - zasięg od 40mm do 300mm, 2Y0A710 - zasięg od 1000m do 5500mm. Opis użytych koderów znajduje się w dodatku A.2. Informacje na temat użytych podzespołów zaczerpnięto z publikacji [2], [4], [5] a także z dokumentacji technicznych producentów. Oznaczenia ENC4 -koder magnetyczny zamontowany na prawym kole, ENC3 -koder magnetyczny zamontowany na lewym kole, SERV1 -sterownik silnika, SERV2 -serwomechanizm.

19 3.1. Informacje o makiecie Rysunek 3.3 Widok platformy mobilnej z oznaczonymi czujnikami Rysunek 3.4 Podłączenia zasilania oraz sygnałów do płytki drukowanej 17

20

21 Rozdział 4 Algorytmy zachowania robota mobilnego 4.1 Jazda w kolumnie Jednym z podstawowych zadań stawianych kierowcom pojazdów poruszającym się w ruchu drogowym jest umiejętność jazdy w kolumnie. Polega ona na utrzymywaniu odpowiedniej odległości od pojazdu poprzedzającego w zależności od prędkości z jaką porusza się pojazd. Jest to szczególnie ważne w sytuacji gdy pojazd porusza się z dużą prędkością w otoczeniu dużej liczby samochodów, np. na autostradach. Niezachowanie odpowiedniej odległości jest jedną z częstszych przyczyn wypadków. W czasie gdy pojazd poprzedzający gwałtownie hamuje kierowca potrzebuje zarówno czasu na wyhamowanie swojego pojazdu jak i również musi pozostawić sobie czas na reakcje na bodziec. W pracy przyjęto, iż jedynymi informacjami z zewnątrz są pomiar prędkości poruszania się pojazdu oraz zmierzona odległość od pojazdu poprzedzającego naszą makietę. Mając z góry przyjętą zależność odległości od drogi hamowania można wyznaczyć minimalną odległość jakiej potrzebuje pojazd aby się zatrzymać. Odległość taką należy zwiększyć o margines bezpieczeństwa zmniejszający ryzyko zderzenia wywołane przez zakłócenia. Poniżej wypunktowano kroki algorytmu: pomiar prędkości pojazdu, wyliczenie drogi hamowania i minimalnej odległości, zwiększenie drogi o margines bezpieczeństwa, pomiar odległości od pojazdu, zwiększenie/ zmniejszenie prędkości w zależności od wyliczonej oraz zmierzonej odległości przy ograniczeniu prędkości maksymalnej, powtórzyć powyższe kroki do momentu gdy różnica odległości zmierzonej od wyliczonej będzie zbiegała do 0 + i będzie dostatecznie mała.

22 20 4. Algorytmy zachowania robota mobilnego Pomiar prędkości pojazdu Pomiar odległości od pojazdu Wyliczenie drogi hamowania i minimalnej odległości Dodanie marginesu bezpieczeństwa do drogi Zmniejsz /zwiększ prędkość pojazdu tak Czy zmiana odległości jest gwałtowna i czy jest ujemna? nie HAMOWANIE AWARYJNIE Rysunek 4.1 Schemat blokowy algorytmu jazdy w kolumnie Jako zależność drogi hamowania od prędkości można przyjąć wzór (4.1): gdzie: s = v2 + d, (4.1) 2a s oznacza drogę potrzebną do wyhamowania pojazdu, v prędkość z jaką porusza się pojazd, d stały margines błędu, a przyspieszenie (opóźnienie) związane z hamowaniem. 4.2 Znajdywanie miejsca do parkowania Kolejną istotną funkcją pojazdu autonomicznego poruszającego się w terenie zabudowanym jest umiejętność znalezienia miejsca do zaparkowania. Robot poruszający się z punktu A do B po terenie przeznaczonym do ruchu nie może się po prostu zatrzymać gdy dojedzie do wyznaczonego celu. Musi on zjechać z obszaru, po którym poruszają

23 4.3. Parkowanie równoległe 21 się inne pojazdy aby nie powodować zagrożenia oraz nie blokować przejazdu. Jednakże aby opuścić strefę ruchu musi znaleźć odpowiednie miejsce, w którym mógłby się zatrzymać. Miejsce takie musi być przeznaczone do postoju oraz swoimi wymiarami zapewnić swobodny wjazd i wyjazd pojazdu. W ruchu ulicznym prawostronnym, obowiązującym w naszym kraju, miejscem takim jest między innymi prawa strona jezdni. Dlatego też należy określić czy z prawej strony pojazdu znajduje się przestrzeń wolna i czy jest ona dość duża aby umożliwić manewr parkowania. Do tego celu zostały wykorzystane dwa pomiary. Pierwszym jest pomiar miejsca wolnego z prawej strony, a drugim pomiar przebytej drogi przez robota. Mając wyróżnione wskazania można zastosować prosty algorytm określający czy przestrzeń wolna z prawej strony pojazdu nadaje się do zaparkowania naszego pojazdu. Algorytm znajdowania miejsca parkowania wygląda następująco: zmierz wolną przestrzeń z prawej strony robota, jeżeli powyższy pomiar wykazuje dostateczną ilość wolnego miejsca uruchom zliczanie przejechanej drogi, jeżeli wskazanie mówi nam o braku wolnej przestrzeni to jedź dalej aż wskazanie zwróci wynik pozytywny, kontynuuj zliczanie przejechanej drogi aż napotkasz przestrzeń zajętą, po przerwaniu zliczania drogi należy sprawdzić czy przejechana droga odpowiada długości minimalnej potrzebnej na zaparkowanie, jeżeli powyższy warunek zwrócił wartość PRAWDA można zakończyć działanie algorytmu i wskazać to miejsce jako dozwolone do parkowania, w innym wypadku należy jechać dalej postępując zgodnie z algorytmem za każdym razem zerując licznik drogi gdy przestrzeń jest niewystarczająca, dodatkowym warunkiem stopu może być określenie drogi minimalnej po przejechaniu której pojazd wskaże to miejsce jako dozwolone do parkowania bez konieczności napotkania przestrzeni zajętej. W algorytmie przyjęto, że z prawej strony jezdni znajduje się pas do parkowania a pojazd porusza się tylko do przodu środkiem tego pasa. Algorytm przyjmuje jeden sposób parkowania dlatego przestrzeń wolna jest sprawdzana pod katem jednej wartości tak samo jak minimalna przestrzeń pozwalająca na swobodne parkowanie. Dodatkowo przyjęto, iż wszystkie parkujące samochody są ustawione w jednej linii, równolegle do krawędzi jezdni. 4.3 Parkowanie równoległe Mając znalezione miejsce do zaparkowania należy przystąpić do procedury parkowania. Istniej wiele różnych rodzajów parkowania, tej pracy skupiono się na jednej nazywanej parkowaniem równoległym. Polega ona na zaparkowaniu samochodu równolegle do krawężnika. Można to osiągnąć poprzez ustawienie się równolegle do samochodu za którym będzie się parkować. Jak wspomniano należy się ustawić tak aby ten samochód mieć z prawej strony, następnie poruszając się do tyłu najpierw skręcić kierownicę w prawo aby zmienić kąt między pojazdami a następnie w odpowiednim momencie przestroić kierownicę na ustawienie przeciwne, względem centralnej pozycji, tak aby wjechać tyłem

24 22 4. Algorytmy zachowania robota mobilnego Zmierz wolna przestrzeń z prawej strony Tak Czy wolna? Nie Jedź dalej Zliczaj drogę Tak Czy przestrzeń wolna? Nie Zeruj licznik drogi Jedź dalej Nie Czy droga wystarcza do zaparkowania Tak Przejechana droga, Średnia szerokość wolnej przestrzeni KONIEC Rysunek 4.2 Schemat blokowy algorytmu znajdywania miejsca do zaparkowania w miejsce do parkowania. W czasie parkowania bardzo ważna rolę odgrywają chwile w których zmieniana jest pozycja kierownicy. odpowiednio określając je można zaparkować samochód wykonując tylko dwie zmiany sterowania. Użyty algorytm parkowania wygląda następująco: Aby zaparkować należy wyznaczyć trzy punkty oznaczone na rys. 4.5 kolejno A, B i C, procedurę parkowanie rozpoczynamy na ustawieniu punktu B w linii prostej do tylnej części sąsiedniego pojazdu, następnie skręcamy pod takim kątem aby w chwili gdy pojazd przesunie się w kierunku prostopadłym do drogi o połowę odległości od pojazdu sąsiedniego w sumie z połową szerokości pojazdu punkt A znajdował się w linii prostej z tylnią częścią sąsiedniego pojazdu, w tym momencie następuje przesterowanie kąta na przeciwny, po zmianie kontynuujemy jazdę do momentu gdy wyrównamy do zewnętrznej linii parkowania. Na rys. 4.6 przedstawiono algorytm parkowania w kolejnych krokach. Natomiast rys. 4.7 przedstawiono zmianę zmiennych sterowanych podczas wykonywania manewru parkowania. Dla uproszczenia przyjęto, iż pojazd porusza się z tak małą prędkością, że można przyjąć czas zmiany kąta γ za pomijalnie mały. Objaśnienia do rys. 4.5: R a -promień dla łuku zataczanego przez punkt A, R t -promień dla łuku zataczanego przez środek tylnej osi kół, R p -promień dla łuku zataczanego przez środek przedniej osi kół,

25 4.3. Parkowanie równoległe 23 Wolna przestrzeń większa równa minimalnej pierwszy raz więc wyzwala zliczanie drogi Wolna przestrzeń większa równa minimalnej jedź dalej zliczaj drogę Wolna przestrzeń mniejsza niż minimalna i równa m, zakończ zliczanie drogi i sprawdź czy wystarczająco miejsca do parkowania Rysunek 4.3 Etapy znajdowania miejsca do parkowania w-szerokość samochodu, z p -odległość od początku pojazdu do osi przednich kół, z t -odległość osi tylnich kół od końca samochodu, d-odległość między osiami, α-kąt skrętu przednich kół, l-rozstaw tylnych kół. Objaśnienia do rys. 4.6: m-odległość od samochodu sąsiedniego h-długość drogi parkowania, µ-odległość bezpieczeństwa i uprzejmości od samochodu przed którym się zaparkuje, Na rys. 4.7 t 1 oraz t 2 są nieporównywalnie mniejsze od czasów jazdy ze stałą prędkością różną od zera.

26 24 4. Algorytmy zachowania robota mobilnego Jedź dalej Czy punkt B samochodu jest w jednej linii z tyłem samochodu sąsiedniego ( o ile jest)? Skręć kola w prawo i rozpocznij jazdę do tyłu Jedź dalej Czy przejechano połowę drogi przeznaczonej na zaparkowanie Zmień ustawienie kół na przeciwne Jedź dalej Czy przejechano drugą połowę odległości od pojazdu sąsiedniego w sumie z połową szerokości pojazdu? Zatrzymaj pojazd Rysunek 4.4 Schemat blokowy algorytmu parkowania

27 4.3. Parkowanie równoległe 25 A C B α Ra Rp Rt 90,0 α d w zp zt Rysunek 4.5 Rozmieszczenie punktów A, B i C

28 26 4. Algorytmy zachowania robota mobilnego w m A B C Rp Ra Rt α h (m+w)/2 β Rt µ Rysunek 4.6 Etapy parkowania: a ustawienie się w pozycji wyjściowej, b zmiana kierunku na kierownicy, c zatrzymanie się pojazdu.

29 4.4. Inne zachowania 27 Zmian kąta kierwnicy α 0 -α Prędkość liniowa pojazdu 0 Vn Δt1 Δt2 czas Rysunek 4.7 czasu Wykres zmiany prędkości pojazdu oraz kąta ustawienia kół przednich od 4.4 Inne zachowania W trakcie pracy nad algorytmami przyjęto szereg uproszeń. Niektóre z nich można zastąpić odpowiednio skonstruowanymi algorytmami. Do takich problemów należy jazda pojazdu w poszukiwaniu wolnego miejsca parkingowego, można by było zaproponować algorytm utrzymujący stałą odległość od środkowej linii trasy dzięki czemu wyszukiwanie przestrzeni parkingowej byłoby możliwe nie tylko dla prostych tras. Dodatkowo badając głębokość miejsca parkingowego można byłoby przyjąć inny typ parkowania, prostopadły do kierunku jezdni. Innymi algorytmami możliwymi do użycia jest algorytm umożliwiający unikanie przeszkód dzięki czemu można by pozwolić na nieregularności w sposobie parkowania pojazdów sąsiadujących. Algorytmy te nie zostały rozważone ze względu na zdolność obliczeniową użytego mikrokontrolera oraz z powodu ograniczeń wynikających z wykorzystanych czujników i ich rozmieszczenia. Rozbudowa makiety w większości wypadków wymaga rozbudowania struktury sterownika o dodatkowe zachowania, co jednakże w przypadku architektur behawioralnych nie sprawia dużych kłopotów.

30

31 Rozdział 5 Badania 5.1 Sensory PSD Aby skalibrować czujniki PSD wykorzystano dwie miarki milimetrowe ustawione równolegle na posadzce oraz duży biały ekran pionowy. Aktualnie badany czujnik pojazdu umiejscowiono tak aby początek jego zakresu znajdował się na równo z linią 0 wyznaczoną przez metry. Przesuwając ekran wzdłuż miarek i odczytując wyniki z czujników wykonano pomiary. Pomiary dla odpowiednich czujników zebrano w tabelach Tabela 5.1: Pomiary czujnika [mm]-czujnik numer 1 odległość[mm] wartość odczytana z czujnika , , , , , , , , , , , , , , , , , , , ,8

32 30 5. Badania Tabela 5.1: Pomiary czujnika [mm]-czujnik numer 1 odległość[mm] wartość odczytana z czujnika , , , , , , , , , ,6 Tabela 5.2: Pomiary czujnika [mm]-czujnik numer 2 odległość[mm] wartość odczytana z czujnika , , , , , , , , , , , , , , , , , , , , , , ,2

33 5.1. Sensory PSD 31 Tabela 5.2: Pomiary czujnika [mm]-czujnik numer 2 odległość[mm] wartość odczytana z czujnika , ,8 Tabela 5.3: Pomiary czujnika [mm]-czujnik numer 3 odległość[mm] wartość odczytana z czujnika , , , , , , , , , , , , , , , , , , ,4 Tabela 5.4: Pomiary czujnika [mm]-czujnik bok 1 odległość[mm] wartość odczytana z czujnika , , , , , , , , ,8

34 32 5. Badania , , , , , , , , , , , , ,4 Tabela 5.5: Pomiary czujnika [mm]-czujnik bok 2 odległość[mm] wartość odczytana z czujnika , , , , , , , , , , , , , , , , , , , , , , , , , ,2

35 5.1. Sensory PSD 33 Pomiary wykonywano od początku zakresu czujników, należało więc najpierw sprowadzić je do jednego układu współrzędnych. Poprzez dodanie do odczytów odpowiedniej wartości odpowiadającej odległości pomiędzy początkiem zakresu sensora a wybranym punktem. Jako punkt odniesienia przyjęto środek tylnej osi kół pojazdu. Następnie dopasowując do wzoru funkcji (A.3) przy programu GNUPLOT, uzyskano parametry niezbędne do odczytu odległości z sensorów. Parametry zostały zebrane w tabeli 5.7. Tabela 5.6: Parametry uzyskane dzięki funkcji fit programu Gnuplot Czujnik a 00 a 10 a 20 a 0 a 1 a 2 PSD-5000mm , PSD-1500mm , PSD-800mm PSd-1-300mm PSD-2-300mm Jednakże po sprowadzeniu do punktu odniesienia, wyliczeniu parametrów funkcji przy pomocy GNUPLOT i przeprowadzeniu testów odległości zauważono, że podczas sprowadzania do środka układu popełniono błąd i dodano za duże wartości. Aby go wyeliminować należy od wyliczonej odległości (d) ze wzoru (5.1) (przekształcony wzór (A.3)) odjąć odpowiednią wartość przedstawioną w tabeli d = a 0a 1 v a 2 (5.1) Tabela 5.7: Poprawka wymagana do odległości d przy użyciu wzoru (A.3) Czujnik Wartość sprowadzająca do środka układu poprawka [mm] PSD-5000mm PSD-1500mm PSD-800mm PSd-1-300mm PSD-2-300mm Rysunki przedstawiają dopasowanie pomiarów zebranych w tabelach do funkcji (A.3) dla poszczególnych czujników. Punkty oznaczają pomiary a linia przebieg wyznaczonej funkcji.

36 34 5. Badania Rysunek 5.1 Dopasowanie dla czujnika PSD nr 1 o maksymalym zakresie 5000mm

37 5.1. Sensory PSD 35 Rysunek 5.2 Dopasowanie dla czujnika PSD nr 2 o maksymalym zakresie 1500mm

38 36 5. Badania Rysunek 5.3 Dopasowanie dla czujnika PSD nr 3o maksymalym zakresie 800mm

39 5.1. Sensory PSD 37 Rysunek 5.4 Dopasowanie dla czujnika PSD-bok1 o maksymalym zakresie 300mm

40 38 5. Badania Rysunek 5.5 Dopasowanie dla czujnika PSD-bok2 o maksymalym zakresie 300mm

41 5.2. Przyspieszenie Przyspieszenie Aby móc poprawnie określić drogę hamowania należało posiadać informacje o przyspieszeniu pojazdu. Aby wyliczyć przyspieszenie jakie jest potrzebne do swobodnego wyhamowania pojazdu można skorzystać ze wzoru (5.2). Po prostym przekształceniu otrzymać można wzór (5.4). gdzie a = v t, (5.2) v = s t, (5.3) a = s t 2, (5.4) a-przyspieszenie, v-prędkość, s-droga, t-czas. Wykorzystując opisaną poniżej funkcje jazda kolumna(speed), w której ustawiono drogę hamowania przed przeszkodą jako stałą wartość, mierząc odległość od przeszkody po zatrzymaniu i znając odległość przy której rozpoczynało się hamowanie można wyliczyć drogę jaka przebył pojazd w czasie hamowania. W międzyczasie, wysyłającej wartość z koderów (przez moduł radiowy) od momentu gdy wypełnienie sygnału PWM silnika przyjęło wartość zerową (288), do momentu gdy koła przestały się obracać. Wiedząc, że wartość ta wysyłana była co 1,024ms znamy czas jaki potrzebny był od rozpoczęcia hamowania do wyhamowania pojazdu. Posiadając te informacje można wyliczyć przyspieszenie. Aby skutecznie określić przyspieszenie wykonano 4 testy dla PWM=279, których wyniki przedstawiono w tabeli 5.8. Dla wypełnienia PWM dla silnika równego 279 wyznaczono przyspieszenie [ mm s 2 ]. Tabela 5.8 Wyniki testów określających przyspieszenie droga hamowania[mm] czas[s] przyspieszenie a[ mm ] s , , , , , , , , Jazda do przodu Jednym z testów jakie należało przeprowadzić było znalezienie wypełnienia sygnału PWM dla serwomechanizmu gwarantującego pozycje zerową kół. Do przeprowadzenia tego doświadczenia wykorzystano prostą linię narysowaną na podłodze, na której ułożono pojazd

42 40 5. Badania tak aby krańcem prawych kół dotykał jej krawędzi. Następnie ustawiając wypełnienie sygnału, zaprogramowano jazdę do przodu przez 30 sekund. Po jego zatrzymaniu wizualnie określano kierunek odchylenia od linii i modyfikowano wypełnienie. Najlepszą pozycję gwarantuje wypełnienie PWM=285. Jednakże podczas pomiarów zauważono, iż pozycja początkowa ma bardzo duży wpływ na odchylenie. Dlatego aby z tą wartością pojazd poruszał się prosto (z małym odchyleniem) powinien mieć już tę wartość ustawioną zanim dotknie podłoża. Jednakże takie rozwiązanie nie spełnia w pełni swojej funkcji. W normalnych warunkach, dlatego opracowano procedurę umożliwiającą jazdę do przodu po linii prostej. if(oo[0]-oo[1]>0) //różnica zmian położenia koła prawe i lewego PWMDTY23=PWMDTY23-1; // gdy prawe szybciej sie obraca skręca w prawo else if(oo[0]-oo[1]!=0) //sprawdza czy się równo obracają PWMDTY23=PWMDTY23+1; //jak się nierówno obracają i // lewe szybciej to skręca w lewo else if(oo[1]!=0) // żeby nie zatrzymał się zaraz po uruchomieniu PWMDTY01=288; //zatrzymuje się jak równo się obracają Ten fragment kodu stara się doprowadzić do sytuacji takiej by zmiana pozycji lewego i prawego koła była taka sama, co dawałoby realizacje jazdy po prostej. Trudność w dobraniu odpowiedniego wypełnienia wynika z luzów na kołach, których nie można zniwelować ze względu na niewystarczającą moc serwomechanizmu przy zbyt ciasnym spasowaniu elementów pozycjonujących koła. Przy okazji przeprowadzono test identyfikujący PWM silnika. Pozycja neutralna to PWM=288. Od wypełnienia równego 279 w dół silnik zadaje kierunek poruszania pojazdu do przodu. Im mniejsze wypełnienie tym szybciej. Natomiast od PWM=300 w górę pojazd porusza się do tyłu, im wyższa wartość tym większa prędkość poruszania się. Dodatkowo aby pojazd po jeździe do przodu poruszał się do tyłu należy po zadaniu kierunku odczekać pewien czas (w testach przyjęto czas 1.024ms pochodzący od przerwania cyklicznego) i ponownie zadać aby pojazd wykonał polecenie. Wiąże się to z właściwością sterownika silnika który przed zmianą na kierunek wsteczny musi wyhamować silnik. 5.4 Maksymalny promień skrętu Pierwszym etapem wyznaczenia maksymalnego skrętu kół było wyznaczenie wartości wypełnienia, dla której serwomechanizm nie osiąga fizycznych ograniczeń skrętu kół. Dobrym sposobem jest manualne zmienianie sygnału PWM i określenie wartości, dla której serwomechanizm wydaje charakterystyczny dźwięk. Pojawia się on gdy siła serwomechanizmu nie może pokonać ograniczeń. Należy wtedy przyjąć wypełnienie mniejsze dla prawego koła/większe dla lewego o 1. Dla odchylenia w prawo PWM wynosi 228 a dla pozycji przeciwnej 350. Aby wyliczyć kąt można skorzystać ze wzoru (3.13) oraz odczytu zmiany pozycji obu kół. W kierunku lewym równe 29,17 w kierunku prawym 29,22.

43 Rozdział 6 Implementacja algorytmów zachowania Zanim przedstawione zostaną zaimplementowane algorytmy należy wspomnieć o niezbędnych częściach kodu sterownika pojazdu zapewniających jego funkcjonalność. Na początek przedstawione zostaną przerwania obsługujące kodery magnetyczne (do obsługi koderów AS5040 użyto trybu kwadraturowego). Przykład dla prawego koła (obsługa lewego na taka sama, na przerwaniach isrvtimch4, isrvtimch5): interrupt void isrvtimch7(void) { /* kanał A: Przerwanie zwiększające/zmniejszające wartość impulsu w momencie dowolnego zbocza */ if (PTIT_PTIT6!=PTIT_PTIT7) { // gdy na drugim kanale różny poziomie enc[0]++; if (enc[0]==1024) // gdy przyjmie wartość 1024 to dać 0 //bo wartość na kole o = 1024 impulsy enc[0]=0; } else { // taki sam enc[0]--; //podobnie jak wcześniej impulsy od 0 do 1023 if (enc[0]==-1) enc[0]=1023; } TFLG1=TFLG1_C7F_MASK; } interrupt void isrvtimch6(void) { /* Kanał B: Przerwanie zwiększające/zmniejszające wartość impulsu w momencie dowolnego zbocza */ if (PTIT_PTIT6==PTIT_PTIT7){ // gdy takie same enc[0]++; if (enc[0]==1024) { enc[0]=0; liczba_obr[0]++; }

44 42 6. Implementacja algorytmów zachowania } else { //gdy różne enc[0]--; if (enc[0]==-1) enc[0]=1023; } TFLG1=TFLG1_C6F_MASK; } Silnik jak i serwomechanizm są sterowane sygnałem PWM. Aby zadać wypełnienie sygnały dla silnika należy przypisać wartość zmiennej PWMDTY01, a dla serwomechanizmu PWMDTY23. Do wyboru kierunku, w którym mierzona jest odległości wykorzystywana jest funkcja: float pomiar_odleglosc(int kierunek) /* Kierunek: 0-przód; 1-tył 2-bok1; 3-bok2; Zwraca odległość w mm. */ { switch(kierunek){ case 0:{ return przod(); // mierzy odległość z przodu pojazdu break; } case 1:{ return tyl(); //mierzy odległość z tyłu break; } case 2:{ return bok(1); //dla 1 PSD z boku break; } case 3:{ return bok(2); //dla 2 PSD z boku break; } default: { return -1; //dla nieokreślonego kierunku break; } } }

45 6.1. Zachowywanie bezpiecznej odległości 43 Funkcje przod(), tyl(), bok() wyliczają odległość na podstawie odczytu z PSD(v) według wzoru (6.1). d = a 0a 1 a 2 (6.1) v Wartości a 0, a 1, a 2 to odpowiednio a, b, c wyznaczone dla każdego czujnika PSD w rozdziale (5.1). Funkcja znak(liczba) zwraca wartość bezwględną LICZBY. Użytymi zmiennymi globalnymi są: int enc[2]; //przechowuje informacje o pozycji z kodera 0-kolo prawe 1-lewe int oo[2]; //przechowuje informacje zmianie na kole w ciągu 1,024ms //0-kolo prawe 1-lewe [impulsy] const float r=32; //promień koła [mm] const float d=260; //odleglość miedzy osiami kół [mm] const float l=155; // długośc tylniej osi[mm] const float PI=3.1416; //użyta dokładność liczby PI 6.1 Zachowywanie bezpiecznej odległości Do zachowania bezpiecznej odległości wykorzystano 2 czujniki PSD firmy Sharp, o numerach 1 oraz 2. Zostały one tak dobrane aby łącznie pokrywały odległość 5m przed pojazdem. Pierwszy z nich o zasięgu mm został tak umieszczony na pojeździe aby jego zasięg rozpoczynał się na krańcu płyty pojazdu. Natomiast drugi ustawiono tak aby jego zasięg mm przedłużał zasięg wcześniejszego czujnika. Fragment kodu realizujący działanie tych czujników został przedstawiony poniżej. float przod(void) { /* Funkcja ta zwraca odległość odczytaną z jednego z dwóch czujników w [mm] */ //dane wyliczone w rozdziale (5.1) float a1= ; float b1= ; float c1= ; float a2= ; float b2= ; float c2= ; // sprawdzenie czy wartość znajduje się // w zasięgu czujnika [mm] if((atd0dr3>4428)&&(atd0dr3<46618)) { return ((-a2*b2)/atd0dr3)-c2-185; } else { // sprawdzenie czy wartość znajduje się // w zasięgu czujnika [mm] if((atd0dr2>24985)&&(atd0dr2<49677)) { return ((-a1*b1)/atd0dr2)-c1-15; } else { // jak nie znajduje się w zasięgu żadnego z nich przypisać wartość -1

46 44 6. Implementacja algorytmów zachowania } } return -1; } Jak można zaobserwować odległość obliczana jest według wzoru (5.1) tylko dla wartości z zasięgu czujników. Dla odczytów spoza tych przedziałów zwracana jest wartość -1. Tę wartość przyjęto dlatego, iż przestrzeń poza zasięgiem sensorów jest nieokreślona. Dla takiego przypadku lepiej przyjąć najgorszy przypadek niż liczyć na sprzyjającą sytuacje. Znając już odległość przed pojazdem potrzebna jest też odległość potrzebna do wyhamowania pojazdu. Można ją obliczyć używając wzoru (4.1). Przyspieszenie zostało obliczone według schematu pokazanego w rozdziale 5.2. Natomiast prędkość wyliczana jest w następujący sposób: float pomiar_v(void){ /* funkcja ta zwraca prędkość z jaką porusza się pojazd w [mm/s] */ float wt[2]; //tablica przechowująca prędkość kątową float v_t=0; //prędkość kół tylnich float w_poj=0; // orientacja pojazdu //oo[1]; //zmiana dla lewego koła (wyliczana w przerwaniu rti) //oo[0]; //zmiana dla prawego koła(wyliczana w przerwaniu rti) if(znak(oo[1])>10) //ograniczenie zmiany wt[1]=59.912; else wt[1]=znak(oo[1])*5.9912; if(znak(oo[0])>10) //ograniczenie zmiany wt[0]=59.912; else wt[0]=znak(oo[0])*5.9912; // wartość stała wyliczona na podstawie //wzoru 2*PI/1024/0, rad/s} v_t=r*(wt[1]+wt[0])/2; // wyliczenie prędkości kół tylnich w_poj=r*(wt[0]-wt[1])/l; //wyliczenie orientacji pojazdu return sqrt(pow(v_t,2)+pow(d*w_poj,2)); } Znając zmianę pozycji kół (odczyt z koderów) oraz czas w jakim ta zmiana nastąpiła można w łatwy sposób obliczyć prędkość kątową. Następnie stosując wzór (3.10) obliczyć można prędkość poruszania się. Ograniczenia służą pominięciu sytuacji gdy pojazd przy uruchamianiu zamiast poruszać się do przodu lekko się cofnie i zostanie odczytany nagły duży przyrost obrotu kół. Wartość dobrano tak, aby nie wpływa na algorytm gdyż droga potrzebna dla wyhamowania znajduje się poza zakresem czujników. Poniżej przedstawiono kod realizujący jazdę z funkcją zwalniania gdy przeszkoda znajdzie się w odległości niewiększej od drogi potrzebnej do wyhamowania pojazdu. void jazda_kolumna(int speed) { /* Funkcja ta zmniejsza prędkość lub całkowicie hamuje w zależności od odległości i aktua Zmienna speed przechowuje aktualną prędkość poruszania się

47 6.2. Znajdowanie miejsca do parkowania 45 */ float droga_h=0; //droga hamowania float odleglosc=0; //odległość przeszkody od pojazdu const int dd=100;// odległość bezpieczeństwa 100[mm] const float a=780.58; //przyspieszenie wyliczone w rozdziale (5.2) odleglosc=pomiar_odleglosc(0); //wyliczenie odległości droga_h=(pow(pomiar_v(),2)/(2*a))+dd; //wyliczenie drogi hamowania if(1500<droga_h) //zabezpieczenie przeciw zakłóceniom droga_h=1500; if(odleglosc<droga_h) { //gdy odległość mniejsza od drogi zwolnij if((1.5*odleglosc)<droga_h) //gdy odległość gwałtownie mniejsza hamuj zmniejsz_v(1); // hamowanie awaryjne else zmniejsz_v(0); //zwalnianie } else PWMDTY01=speed; //utrzymanie aktualnej odległości } Zabezpieczenie przed zakłóceniami zostało wdrożone po dłuższych testach, w czasie których pojazd w wyniku zakłóceń zatrzymywał się w losowych momentach. Wartość drogi hamowania rosła w tamtych momentach do bardzo dużych wartości. Funkcja realizująca zwalnianie została przedstawiona poniżej: void zmniejsz_v(int tryb) { /* zwiększa wypełnienie sygnału PWM wartość zerowa dla silnika to 288 */ if(1==tryb) PWMDTY01=288;//procedura hamowania natychmiostowego else if(288!=pwmdty01) // swobodne zwalnianie dopóki nie PWM<288 PWMDTY01=PWMDTY01+1; } 6.2 Znajdowanie miejsca do parkowania Do wyliczania szerokości przestrzeni z boku pojazdu służy czujnik PSD mm- bok 2. Fragment kodu wyliczający tą szerokość to: float bok(int numer) { /* Funkcja zwracająca odległość przeszkody z prawego boku pojazdu dla jednego z dwóch czujników PSD. */ //drugi czujnik PSD // dane potrzebne do wyliczenia odległości float a= ; float b= ; float c= ; int pomiar=0; pomiar=atd0dr1;