POLITECHNIKA WROCŁAWSKA WYDZIAŁ...

POLITECHNIKA WROCŁAWSKAWYDZIAŁ ELEKTRONIKI

Kierunek: Automatyka i Robotyka (AIR)Specjalność: Robotyka (ARR)

PRACA DYPLOMOWAMAGISTERSKA

Implementacja współdziałania małych robotówmobilnych

Implementation of small mobile robotscooperation

Autor:Robert Kuczaj

Prowadzący pracę:dr inż. Marek Wnuk, I-6

Ocena pracy:

WROCŁAW 2008

Dedykuję rodzicomi narzeczonej

Składam serdeczne podziękowaniaPanu doktorowi Markowi Wnukowiza wskazanie tematu, poświęcony czasi zaangażowanie

Spis treści

1 Wstęp 91.1 Cel i zakres pracy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 91.2 Realizacja . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

2 Wprowadzenie 112.1 Opis problemu . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 112.2 Podstawy teoretyczne . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122.2.1 Model kinematyki . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122.2.2 Regulator PID . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

3 Model formalny współpracy robotów 153.1 Deterministyczny automat skończony . . . . . . . . . . . . . . . . . . . . . 153.2 Złożenie równoległe DFSA . . . . . . . . . . . . . . . . . . . . . . . . . . . 153.3 Dyskretyzacja przestrzeni zadaniowej i ruchu robotów . . . . . . . . . . . . 163.3.1 Zagadnienia unikania kolizji . . . . . . . . . . . . . . . . . . . . . . 16

3.4 Modele DFSA robotów . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 163.5 Koordynacja ruchu robotów . . . . . . . . . . . . . . . . . . . . . . . . . . 20

4 Wirtualny model stanowiska laboratoryjnego 214.1 Modelowanie w środowisku Webots . . . . . . . . . . . . . . . . . . . . . . 214.2 Modele podzespołów robota . . . . . . . . . . . . . . . . . . . . . . . . . . 224.2.1 Układ napędowy . . . . . . . . . . . . . . . . . . . . . . . . . . . . 224.2.2 Wygląd zewnętrzny robota . . . . . . . . . . . . . . . . . . . . . . . 224.2.3 Czujniki podłoża . . . . . . . . . . . . . . . . . . . . . . . . . . . . 234.2.4 Czujniki odległości . . . . . . . . . . . . . . . . . . . . . . . . . . . 234.2.5 Chwytak . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 234.2.6 Moduły komunikacyjne . . . . . . . . . . . . . . . . . . . . . . . . . 23

4.3 Oprogramowanie robotów . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

5 Konstrukcja robotów 295.1 Konstrukcja mechaniczna . . . . . . . . . . . . . . . . . . . . . . . . . . . . 295.1.1 Rama robotów . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 295.1.2 Koła robotów . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29

5.2 Elektronika sterująca . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 315.2.1 Moduły z kontrolerem MC13213 . . . . . . . . . . . . . . . . . . . . 315.2.2 Moduły komunikacyjne xBee . . . . . . . . . . . . . . . . . . . . . . 325.2.3 Układ napędowy . . . . . . . . . . . . . . . . . . . . . . . . . . . . 355.2.4 Czujniki . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 365.2.5 Płyta główna robota i moduł sterujący . . . . . . . . . . . . . . . . 37

2 SPIS TREŚCI

5.3 Arena . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

6 Implementacja i badanie algorytmów sterowania 416.1 Implementacja warstwy sterującej i komunikacyjnej . . . . . . . . . . . . . 416.1.1 Warstwa sterująca . . . . . . . . . . . . . . . . . . . . . . . . . . . 416.1.2 Warstwa komunikacyjna . . . . . . . . . . . . . . . . . . . . . . . . 45

6.2 Implementacja algorytmu współpracy . . . . . . . . . . . . . . . . . . . . . 476.2.1 Regulacja prędkości - regulator PID . . . . . . . . . . . . . . . . . . 476.2.2 Odometria . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 486.2.3 Kalibracja czujników . . . . . . . . . . . . . . . . . . . . . . . . . . 496.2.4 Nawigacja robotów . . . . . . . . . . . . . . . . . . . . . . . . . . . 506.2.5 Bezkolizyjna komunikacja . . . . . . . . . . . . . . . . . . . . . . . 516.2.6 Implementacja zadania . . . . . . . . . . . . . . . . . . . . . . . . . 52

6.3 Eksperymenty i analiza wyników . . . . . . . . . . . . . . . . . . . . . . . 52

7 Podsumowanie 53

Bibliografia 55

A Schematy elektryczne płytek drukowanych I

B Spisy elementów elektronicznych poszczególnych płytek XI

C Rysunki montażowe płytek drukowanych XV

D Rysunki elementów mechanicznych XXI

E Zawartość dołączonej płyty CD XXV

Spis rysunków

1.1 Stanowisko do badań algorytmów współpracy robotów mobilnych . . . . . 10

2.1 Model dwukołowej platformy mobilnej . . . . . . . . . . . . . . . . . . . . 12

3.1 Zdarzenia powodujące zmianę stanu robota (agenta) . . . . . . . . . . . . . 173.2 Możliwe rezerwacje przestrzeni przez robota . . . . . . . . . . . . . . . . . 173.3 Model DFSA robota (źródło [25]) . . . . . . . . . . . . . . . . . . . . . . . 19

4.1 Środowisko Webots-5.10.0 . . . . . . . . . . . . . . . . . . . . . . . . . . . 224.2 Modele robotów w środowisku Webots . . . . . . . . . . . . . . . . . . . . 24

5.1 Robot transportowy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 305.2 Robot zwiadowczy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 305.3 Model koła robota oraz jego odlew z żywicy epoksydowej . . . . . . . . . . 315.4 Moduł z kontrolerem MC13213 . . . . . . . . . . . . . . . . . . . . . . . . 325.5 Moduł xBee z anteną z prętową . . . . . . . . . . . . . . . . . . . . . . . . 335.6 Płytka ewaluacyjna firmy Maxstream dla modułów xBee . . . . . . . . . . 345.7 Płytka ewaluacyjna dla modułów xBee wykonana na potrzeby pracy . . . . 345.8 Moduł USB↔RS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 355.9 Elektronika sterująca silnikiem serwomechanizmu . . . . . . . . . . . . . . 355.10 Czujnik białej linii . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 365.11 Sonar ”PING)))” firmy Parallax . . . . . . . . . . . . . . . . . . . . . . . . 375.12 Czujnik GP2D12 firmy Sharp . . . . . . . . . . . . . . . . . . . . . . . . . 375.13 Moduły z kontrolerem HC9S08GT60A . . . . . . . . . . . . . . . . . . . . 375.14 Zmontowana płytka główna robotów . . . . . . . . . . . . . . . . . . . . . 385.15 Arena wraz z robotami w trakcie eksperymentów . . . . . . . . . . . . . . 39

6.1 Zasada sterowania serwomechanizmem modelarskim . . . . . . . . . . . . . 456.2 Odpowiedź skokowa silnika robota . . . . . . . . . . . . . . . . . . . . . . . 486.3 Wpływ umieszczenia czujnika PSD na pomiar odległości przy podłożu . . . 496.4 Przypadki wymagające korekcji trajektorii . . . . . . . . . . . . . . . . . . 516.5 Sposób poruszania się robotów zwiadowczych . . . . . . . . . . . . . . . . . 52

A.1 Schemat czujnika koloru . . . . . . . . . . . . . . . . . . . . . . . . . . . . IIA.2 Schemat płytki mostka dla silnika serwomechanizmu . . . . . . . . . . . . . IIIA.3 Schemat płytki kodera magnetycznego . . . . . . . . . . . . . . . . . . . . IVA.4 Schemat płytki modułu z układem MC13213 . . . . . . . . . . . . . . . . . VA.5 Schemat modułu konwertera USB↔RS . . . . . . . . . . . . . . . . . . . . VIA.6 Schemat płytki ewaluacyjnej dla modułu xBee . . . . . . . . . . . . . . . . VIIA.7 Schemat modułu z kontrolerem HCS08GT60A . . . . . . . . . . . . . . . . VIIIA.8 Schemat płytki głównej robota . . . . . . . . . . . . . . . . . . . . . . . . . IX

4 SPIS RYSUNKÓW

C.1 Rysunek montażowy czujnika koloru . . . . . . . . . . . . . . . . . . . . . XVC.2 Rysunek montażowy płytki mostka dla silnika serwomechanizmu . . . . . . XVC.3 Rysunek montażowy płytki kodera magnetycznego . . . . . . . . . . . . . . XVIC.4 Rysunek montażowy płytki modułu z układem MC13213 . . . . . . . . . . XVIC.5 Rysunek montażowy modułu konwertera USB↔RS . . . . . . . . . . . . . XVIC.6 Rysunek montażowy płytki ewaluacyjnej dla modułu xBee . . . . . . . . . XVIIC.7 Rysunek montażowy modułu z kontrolerem HCS08GT60A . . . . . . . . . XVIIIC.8 Rysunek montażowy płytki głównej robota . . . . . . . . . . . . . . . . . . XIX

D.1 Rama robota . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . XXID.2 Sposób wygięcia ramy robota . . . . . . . . . . . . . . . . . . . . . . . . . XXIID.3 Pług robotów . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . XXIIID.4 Koła robota . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . XXIII

Spis tabel

6.1 Wykorzystanie podzespołów kontrolera HC9S08GT60A w programie ste-rującym robotami . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42

6.2 Zdarzenia o których roboty informują pozostałych agentów na arenie . . . 476.3 Ramka przesyłana przy komunikacji robotów . . . . . . . . . . . . . . . . . 47

B.1 Spis elementów czujnika koloru. . . . . . . . . . . . . . . . . . . . . . . . . XIB.2 Spis elementów mostka dla silnika serwomechanizmu. . . . . . . . . . . . . XIB.3 Spis elementów płytki kodera magnetycznego. . . . . . . . . . . . . . . . . XIB.4 Spis elementów płytki modułu z układem MC13213 . . . . . . . . . . . . . XIB.5 Spis elementów modułu konwertera USB⇔RS. . . . . . . . . . . . . . . . . XIB.6 Spis elementów płytki ewaluacyjnej dla modułu xBee. . . . . . . . . . . . . XIIB.7 Spis elementów modułu z kontrolerem HCS08GT60A. . . . . . . . . . . . . XIIB.8 Spis elementów płytki głównej robota. . . . . . . . . . . . . . . . . . . . . XII

Spis listingów

4.1 Przykładowy szablon kontrolera w środowisku Webots . . . . . . . . . . . . 254.2 Szablon kontrolera dla robota z czujnikami PSD . . . . . . . . . . . . . . . 276.1 Makra ułatwiające tworzenie funkcji obsługujących przerwania . . . . . . . 436.2 Funkcja obsługi przerwania cyklicznego (od przepełnienia timera 1) . . . . 446.3 Kod obsługi przerwania odpowiedzialnego za odbieranie ramek danych . . 466.4 Implementacja regulatora PID . . . . . . . . . . . . . . . . . . . . . . . . . 476.5 Struktura przechowująca wiersz z tablicy położeń robotów . . . . . . . . . 50

Rozdział 1

Wstęp

Zadanie sterowania grupą robotów mobilnych wiąże ze sobą zagadnienia sterownia plat-formą mobilną oraz współdzielenia jednej przestrzeni roboczej przez wiele robotów. Pro-blemy współdzielenia zasobów i unikania kolizji pociągają za sobą potrzebę tworzenianietypowych algorytmów sterownia pozwalających na modyfikowanie trajektorii ruchuw zależności od położenia innych robotów na scenie [22]. W pracy zaproponowano podej-ście automatowe prezentowane w [25], bazujące na dyskretyzacji przestrzeni zadaniowejco pozwala na opis zadania współpracy jako deterministycznego skończonego automa-tu stanów (DFSA) [1]. Jako przykład do implementacji wybrano zadanie odnalezieniaobiektu na scenie i jego transportu do określonego punktu. Aby w pełni zbadać algorytmwspółpracy robotów, stworzono stanowisko badawczego w postaci grupy małych robotówmobilnych. Przewidziane jest jego wykorzystanie w Laboratorium Robotyki Instytutu In-formatyki Automatyki i Robotyki.

1.1 Cel i zakres pracy

Celem pracy jest stworzenie grupy małych robotów mobilnych, zdolnych do wzajemnej ko-munikacji, i implementacja na nich wybranego algorytmu współdziałania. Roboty służyćbędą jako stanowisko badawcze dla algorytmów współpracy, w tym algorytmów stadnych.Aby zrealizować cel pracy należy:

• Zapoznać się z zagadnieniami sterowania, komunikacji i koordynowania współpracyw grupie robotów mobilnych.

• Opracować małego robota mobilnego umożliwiającego implementowanie algoryt-mów realizujących wybrane cele, oraz wykonać grupę takich robotow.

• Dokonać przeglądu znanych algorytmów stadnych i zaimplementować współpracędla wybranych zadań zespołowych.

• Przeprowadzić eksperymenty dla wybranego sposobu koordynacji.

1.2 Realizacja

W ramach realizacji pracy zapoznano się ze sposobami sterowania grupą robotów i wybra-no jeden z nich, który został zaimplementowany. Aby zrealizować ten cel zaprojektowanoi wykonano grupę 5 robotów mobilnych klasy minisumo [16]. Roboty wyposażone zostały

10 1. Wstęp

w moduły ZigBee, pozwalające na komunikację radiową o zasięgu około 10 m, czujnikizbliżeniowe oraz chwytaki. Dokładny opis konstrukcji robotów znajduje się w rozdziale 5.Kolejnym krokiem było stworzenie oprogramowania dla robotów w języku C, począw-

szy od warstw najniższych, takich jak obsługa peryferiów mikrokontrolera, poprzez pro-cedury obsługi czujników, układy sterowania elementami wykonawczymi (koła, chwytak),a skończywszy na implementacji sposobu komunikacji z wykorzystaniem modułów ZigBee.Ostatnim etapem pracy była implementacja algorytmu współpracy z rozdziału 3 na robo-tach i przeprowadzenie badań. Szczegóły dotyczące oprogramowania robotów oraz wynikieksperymentów przedstawiono w rozdziale 6.Równolegle stworzony został wirtualny model robota w środowisku Webots pozwalają-

cy na modelowanie zachowań robotów na komputerze. Stanowi on uzupełnienie stanowiskabadawczego pozwalające na porównywanie wyników symulacji komputerowej z badaniamina rzeczywistym obiekcie. Opis tworzenia modelu przedstawiono w rozdziale 4.Powstałe w ten sposób stanowisko badawcze przedstawia rysunek 1.1.

Rysunek 1.1 Stanowisko do badań algorytmów współpracy robotów mobilnych

Rozdział 2

Wprowadzenie

Zadanie sterowania grupą robotów we wspólnej, współdzielonej przestrzeni jest jednym zproblemów z jakimi boryka się współczesna robotyka. Najczęściej spotykanym podejściemdo problemu jest planowanie optymalnej trajektorii według narzuconego kryterium przyzałożeniu konieczności unikania kolizji. Wadą takiego sposobu sposobu sterownia jest jegowrażliwość na dużą losowość systemu co nastręcza wiele trudności przy jego praktycznejimplementacji.Innym podejściem jest sterowanie w czasie rzeczywistym z wykorzystaniem nadzorcy

systemu. Podejście to zaproponowano w pracy [24]. System sterwania tego typu daje sięw prosty sposób implementować w wielu językach programowania, jednak bez formalnychprzesłanek dla swej poprawności.W przeciwieństwie do powyższych, praca opisuje podejście do systemu jako determi-

nistycznego automatu skończonego (DFSA), w którym grupa heterogonicznych robotówmobilnych dwóch typów realizuje wspólne zadanie, współdzieląc przestrzeń w jakiej sięporuszają [25].

2.1 Opis problemu

W opisywanym systemie rozważa się grupę dwóch typów robotów, różniących się budowąi możliwościami:

• zwiadowcze wyposażone w czujniki dalekiego zasięgu

• transportujące wyposażone w chwytak,

których zadaniem jest znalezienie i usunięcie z przestrzeni roboczej obiektów (np. piłek).Zarówno roboty zwiadowcze w trakcie poszukiwania obiektów na scenie jak i roboty

transportowe poruszają się po niej w sposób narzucony przez arbitralny sterownik ruchu.Jeśli jakiś robot zwiadowczy odnajdzie obiekt na scenie, jego zadaniem jest okreslenie jegodokładnego położenia oraz przekazane tej informacji najbliższemu robotowi wyposażone-mu w chwytak. Robot transportowy po otrzymaniu informacji o położeniu znalezionegoobiektu ma za zadanie wyznaczyć trajektorię pozwalającą mu dojechać do obiektu, pobraćgo przy pomocy chwytaka i zawieźć w miejsce składowania. Na tym etapie planowaniatrajektorii nie jest jednak możliwe branie pod uwagę pozycji innych robotów na scenie.Aby zapewnić bezkolizyjne poruszanie się robotów dopuszczona jest możliwość modyfi-kowania wyznaczonej trajektorii przez roboty w trakcje poruszania się oraz możliwośćwstrzymania ruchu w celu przepuszczenia innego robota.

12 2. Wprowadzenie

Rysunek 2.1 Model dwukołowej platformy mobilnej

Zadanie sterowania polega na znalezieniu automatu skończonego (DFSA) takiego, któ-ry, poprzez modyfikacje sposobu realizacji wyznaczonej trajektorii, pozwoli w skończonymczasie dostarczyć każdy znaleziony przedmiot na scenie do miejsca docelowego przy zało-żeniu, że dwa roboty nie mogą zajmować tego samego obszaru na scenie.

2.2 Podstawy teoretyczne

Na potrzeby modelowania i skonstruowania grupy małych robotów mobilnych wybranopopularną klasę robotów minisumo [16]. Mają one wymiary 10x10 cm w rzucie z góryoraz masę nieprzekraczającą 500 g. Jako układ robotyczny wybrano popularny i dobrzeopisany w literaturze [29] układ dwukołowy (rys. 2.1) typu unicycle.

2.2.1 Model kinematyki

Model kinematyki robota przedstawia równanie (2.1).

xy

θ

φ1φ2

=−sin(θ) 0cos(θ) 00 1−1r

− lr

1r

− lr

(u1u2

)(2.1)

gdzie: x, y - położenie robota na płaszczyźnie, θ - orientacja robota, r - promień kołarobota, l - odległość koła od środka robota, u1 - prędkość liniowa robota, u2 - prędkośćzmiany orientacji robota a φ1, φ2 - prędkości obrotu kół.Model ten jest bezdryfowym układem sterownia o postaci ogólnej (2.2) opisanym do-

kładniej w [29].

q = G(q)u =m∑i=1

gi(q)u1 (2.2)

2.2. Podstawy teoretyczne 13

Z równania (2.1) można wyznaczyć równania prędkości u1, u2: u1 = r φ2−φ12u2 = −r φ2+φ12l(2.3)

a następnie przekształcając (2.1) korzystając z zależności (2.3) można wyznaczyć zależ-ność pomiędzy prędkością robota a prędkościami kół (2.4). xy

θ

=rsin(θ)2 − rsin(θ)2

− rcos(θ)2rcos(θ)2

− r2l − r2l

( φ1φ2

)(2.4)

2.2.2 Regulator PID

Ponieważ model matematyczny pozwala na sterowanie prędkością robota poprzez za-dawanie prędkości kół, zdecydowano się na zastosowanie regulatorów proporcjonalno-różniczkująco-całkujących (PID), których zadaniem będzie utrzymywanie prędkości ob-rotowej kół. Regulator PID opisany jest równaniem (2.5).

u(t) = KP e(t) +KI∫ t0e(τ)dτ +KD

de(t)dt

(2.5)

gdzie: KP - wzmocnienie członu proporcjonalnego, KI - wzmocnienie członu całkującego,KD - wzmocnienie członu różniczkującego, e(t) - błąd sterowania czyli różnica międzywartością zadaną a rzeczywistą.Sygnał błędu obliczany jest na podstawie zadanej przez sterownik prędkości oraz in-

formacji zwrotnej pochodzącej z koderów umieszczonych na kołach robota. Dla celów im-plementacji regulatora konieczna jest dyskretyzacja algorytmu (2.5). Ze względu na stałyczas próbkowania Tp zarówno przy badaniach symulacyjnych jak i przy implementacjiagorytmu na mikrokontrolerze całkę zastępuje się sumą, a pochodną ilorazem różnico-wym (2.6): ∫ t

0e(τ)dτ ≈

k∑i=0

e(i)Tp,de(t)dt≈ e(k)− e(k − 1)

Tp(2.6)

Rozdział 3

Model formalny współpracy robotów

Aby zrealizować postawione w pracy zadanie, zaproponowano rozważenie systemu jakoRAS (Resource Allocation System) [22], dla którego deterministyczny skończony automatstanów DFSA [1] rozważa się jako złożenie równoległe automatów stanów realizującychpodział wspólnych zasobów, sterowanie ruchem robota oraz realizację wyznaczonego za-dania dla każdego z typów robotów.

3.1 Deterministyczny automat skończony

Deterministyczny automat skończony (DFSA1) opisuje definicja 3.1:

Definicja 3.1 Skończony deterministyczny automat stanu jest piątką2 G = (S,E,Γ, δ, s0)taką, że:

1. S jest zbiorem stanów;

2. E jest zbiorem zdarzeń, przy czym zajście zdarzenia powoduje zmianę stanu w G(tranzycję);

3. Γ : S → 2E jest funkcją wykonalności zdarzenia. Zdarzenie e ∈ E jest wykonalne(tj. może wystąpić ) w stanie s ∈ S jeśli e ∈ Γ(s);

4. δ : S × E → S jest funkcją tranzycji. δ jest funkcją częściową określoną dla par(s, e) takich, że e ∈ Γ(s). s′ = δ(s, e) jest nowym stanem wynikłym ze zdarzeniae w stanie s;

5. s0 jest stanem początkowym.

3.2 Złożenie równoległe DFSA

Jeśli dwa lub więcej systemów DFSA wymaga synchronizacji wspólnych zdarzeń, możnatraktować je jako jeden system definiowany jako złożenie równoległe automatów skończo-nych.

Definicja 3.2 Złożeniem równoległym automatów G1 oraz G2 nazywamy automat G1||G2 =(S1 × S2, E1 ∪ E2,Γ1||2, δ, (s0, s1)), gdzie:1Deterministic Finit State Automaton2Z pominięciem ujetych w [1] stanów markowanych, ze względu na brak markowania konkretnych

stanów w modelu

16 3. Model formalny współpracy robotów

1. Γ1||2 = (Γ1(s1) ∩ Γ2(s2)) ∪ (Γ1(s1)\E2) ∪ (Γ2(s2)\E1)

2. δ((s1, s2), e) =

(δ1(s1, e), δ2(s2, e)) dla e ∈ Γ1(s1)NΓ(s2)(δ1(s1, e), s2) dla e ∈ Γ1(s1)\E2(s1, δ2(s2, e)) dla e ∈ Γ2(s2)\E1niezdefiniowana w innym wypadku

Warto zauważyć, że dla definicji 3.2 prawdziwe są równości (G1||G2)||G3 = G1||(G2||G3)oraz G1||G2 = G2||G1.

3.3 Dyskretyzacja przestrzeni zadaniowej i ruchu ro-botów

Opisywany system składa się z grupy robotów, inaczej agentów, A ∈ A, poruszającychsię w dwuwymiarowej przestrzeni zadaniowej z kartezjańskim układem współrzędnychW ∈ R2. Roboty reprezentowane są jako koła o promieniu r, których konfigurację opisujewektor g = (x, y, θ) ∈ SE2, gdzie x i y to położenie środka koła reprezentującego robotaa θ to kąt pomiędzy wektorem prędkości robota równoległym do jego osi a osią OY układuwspółrzędnych. Model ruchu robota opisuje równanie (3.1).

x = v(t)cos(θ(t))y = v(t)sin(θ(t))v(t) ∈ (0, v)θ(t) ∈ [−ω, ω]

(3.1)

Dyskretyzacja przestrzeni zadaniowej polega na przeskalowaniu układu współrzędnychw taki sposób, aby nowe jednostki były większe niż średnica robotów 2r, i dzieliły prze-strzeń na równe kwadraty. Dzięki temu ruch robota można rozpatrywać jako zmianę zaj-mowanego przez niego kwadratu na scenie.

3.3.1 Zagadnienia unikania kolizji

Unikanie kolizji w systemie odbywa się dzięki rezerwowaniu przez roboty pól, na którychsię znajdują oraz pól, które są w odległości równej co najwyżej średnicy robota. Pozwalato na unikanie możliwości wybrania przez dwa roboty trasy wiodącej w tym samym czasieprzez ten sam kwadrat sceny.

3.4 Modele DFSA robotów

Dla uproszczenia modelu ruchu robotów dopuszcza się jedynie ruch taksówkowy do przo-du. Dodatkowym atutem tego podejścia jest możliwość wprowadzania korekt trajektoriiw przypadku sterowania obiektem rzeczywistym. Wychodząc z modelu opisanego w [25]można zdefiniować zdarzenia powodujące zmianę stanu robota w trakcie ruchu (rysu-nek 3.1):

ax Najazd na linię pionową siatki przodem gdy x > 0

ay Najazd na linię poziomą siatki przodem gdy y > 0

dx Zjazd z linii pionowej siatki tyłem gdy x > 0

3.4. Modele DFSA robotów 17

Rysunek 3.1 Zdarzenia powodujące zmianę stanu robota (agenta)

Rysunek 3.2 Możliwe rezerwacje przestrzeni przez robota


dy Zjazd z linii pionowej siatki tyłem gdy y > 0

ax Najazd na linię pionową siatki przodem gdy x < 0

ay Najazd na linię poziomą siatki przodem gdy y < 0

dx Zjazd z linii pionowej siatki tyłem gdy x < 0

dy Zjazd z linii pionowej siatki tyłem gdy y < 0

Na podstawie znajomości zdarzeń utworzono drzewo rozpinające możliwe sposoby re-zerwacji przestrzeni na arenie (rys.3.2) oraz zdefiniowano model DFSA:

Definicja 3.3 W wielorobotycznym układzie z dyskretną przestrzenią zadaniową W =w[i,j] j∈1..n, i∈1..m pojedynczy robot jest piątką G = (S,E,Γ, δ, s0) taką, że:1. S = S1 ∪ S2 ∪ S3 ∈ 2W jest złożeniem rozdzielnych stanów (reprezentujących zajęteprzez robota obszary sceny) takich, że:S1 reprezentuje pojedynczy obszar s = w[i.j] ∈ 2WS2 reprezentuje zbiór dwóch obszarów s = w[i.j], w[i+ 1, j] ∈ 2WS3 reprezentuje zbiór dwóch obszarów s = w[i.j], w[i, j + 1] ∈ 2W

2. E = Ea ∪ Ed jest zbiorem rozłącznych zdarzeń takich, że Ea = ax, ax, ay, ay,Ed = dx, dx, dy, dy

3. Γ(s) jest funkcją wykonywalności zdarzenia. Zdarzenie e ∈ E może wystąpić w stanies ∈ S, tj. e ∈ Γ(s) jeśli:e = ax i sw[i, j], w[i, j + 1] dla i < n, lube = ax i sw[i, j], w[i, j + 1] dla i > 1, lube = ay i sw[i, j], w[i+ 1, j] dla j < m, lube = ay i sw[i, j], w[i+ 1, j] dla j > 1, lube ∈ dy, dy i s ∈ S2, lube ∈ dx, dx i s ∈ S3

4. Dla pary (s, e) ∈ S × E takiej, że e ∈ Γ(s) następny stan s′ = s ∪∆W jeśli e ∈ Ealub s′ = s\∆W jeśli e ∈ Ed, gdzie ∆W =:w[i+ 1, j] jeśli e = ax, s ∈ S1w[i+ 1, j], w[i+ 1, j + 1] jeśli e = ax, s ∈ S2w[i− 1, j] jeśli e = ax, s ∈ S1w[i− 1, j], w[i− 1, j + 1] jeśli e = ax, s ∈ S2w[i, j + 1] jeśli e = ay, s ∈ S1w[i, j + 1], w[i+ 1, j + 1] jeśli e = ay, s ∈ S3w[i, j − 1] jeśli e = ay, s ∈ S1w[i, j − 1], w[i+ 1, j − 1] jeśli e = ay, s ∈ S3w[i+ 1, j] jeśli e = dx, s ∈ S2w[i, j] jeśli e = dx, s ∈ S2w[i, j + 1] jeśli e = dy, s ∈ S3w[i, j] jeśli e = dy, s ∈ S3

5. s0 ∈ S to stan początkowyModel ten przedstawiono na rysunku 3.3. Podobnie można zdefiniować automat sta-

nów dla robota transportującego piłeczkę do okreslonego punktu. Robot taki porusza siępo z góry znanej ścieżce, co ogranicza dopuszczalne stany do kolejnych położeń wzdłużtrajektorii.

3.4. Modele DFSA robotów 19

Rysunek 3.3 Model DFSA robota (źródło [25])


3.5 Koordynacja ruchu robotów

Unikanie kolizji przez roboty odbywa się poprzez wyłączne korzystanie ze wspólnych za-sobów jakimi są pola na arenie. Model współdzielenia zasobów opisuje definicja:

Definicja 3.4 Niech E ′ =⋃A∈AE

Aa oraz E

′′ =⋃A∈AE

Ad będą zbiorami zdarzeń robotów

A w grupie A oraz niech W będzie zbiorem zasobów areny WS. Automat opisujący WSjest piątką Res = (S,E,Γ, δ, s0) taką, że:

1. S = 2W , gdzie stan s ∈ S reprezentuje wolne zasoby w ∈ W (niezajęte przez robotypola areny)

2. E = E ′′ ∪ E ′′

3. e ∈ Γ(s)⇔ e ∈ E ′ ∧∆W ⊆ s ∨ e ∈ E ′′

4. s′ = δ(s, e) =s\∆W dla e ∈ E ′s ∪∆W dla e ∈ E ′′

5. s0 = W\⋃A∈AsA0

Ponieważ roboty operują na arenie zajmując i zwalniając jej zasoby, automat opisującyzachowanie systemu, definiuje się w następujący sposób:

Definicja 3.5 Automatem DFSA definiującym opisywany system nazywamy złożenie rów-noległe automatów skończonych opisujących sposób poruszania się robotów oraz automatuopisującego współdzielone zasoby:MMRS3 = Res||RobA

3MMRS = Multiple Mobile Robot Systems

Rozdział 4

Wirtualny model stanowiskalaboratoryjnego

W ramach pracy zamodelowano układ opisany w rozdziałach 2 i 3 wykorzystując śro-dowisko Webots [2] firmy Cyberbotics 1. Jest to środowisko symulacyjne, pozwalającena modelowanie robotów mobilnych (choć można wykorzystać je do modelowania innychukładów robotycznych) w środowisku 3D. W zależności od wykorzystanych w projekciemodułów, możliwe jest prowadzenie symulacji na poziomie kinematycznym jak i dyna-micznym. Na potrzeby pracy stworzone zostały wirtualne modele robotów, posiadającewszystkie możliwości swoich rzeczywistych odpowiedników, przy czym szczególny naciskpołożony został na model komunikacji radiowej. Dodatkowo, roboty są wizualnie bardzozbliżone do swoich rzeczywistych odpowiedników.

4.1 Modelowanie w środowisku Webots

Modelowanie w środowisku Webots składa się z czterech etapów:

• Wyboru typu robota i stworzenia jego wizualnego modelu, oraz wyglądu środowiska,w którym będzie się poruszał.

• Zamodelowania ograniczeń wymiarowych robota i jego środowiska

• Zamodelowania własności fizycznych robota i jego środowiska

• Oprogramowania robota w wybranym języku (C, C++, Java)

Środowisko Webots dostarcza gotowe modele najpopularniejszych układów robotycznych,z których jeden (układ unicycle) wykorzystano przy tworzeniu modelu grupy robotów.Ponieważ roboty wymagają zastosowania czujników kilku typów jak również modułówradiowych konieczne było stworzenie modeli tych podzespołów. Model posiada strukturędrzewiastą co znacznie ułatwia wprowadzanie ewentualnych poprawek. Widok okna pro-jektu przedstawiono na rysunku 4.1. Więcej informacji na temat tworzenia projektów wśrodowisku Webots można znaleźć w [3] i [32].

1http://www.cyberbotics.com

http://www.cyberbotics.com

22 4. Wirtualny model stanowiska laboratoryjnego

Rysunek 4.1 Środowisko Webots-5.10.0

4.2 Modele podzespołów robota

Ze względu na sposób definiowania poszczególnych części robota, jego konstrukcje możnapodzielić na kilka części:

4.2.1 Układ napędowy

Jako model układu napędowego wykorzystano dostępny w środowisku Webots pod na-zwą differential wheels układ unicycle. Pozwala on na niezależne sterowanie prędko-ścią obrotową każdego z kół, modeluje kodery umieszczone na ich osiach i stanowi przytym podstawę układu robotycznego, którym środowisko potrafi sterować poprzez programtzw. kontrolera. Do układu dodano dwie bryły w kształcie walców z ich ograniczeniamiw postaci wymiarów, modelujące koła . Jako elementy podrzędne umieszczono pozostałepodzespoły robota łącznie z jego wyglądem zewnętrznym.

4.2.2 Wygląd zewnętrzny robota

Wygląd zewnętrzny zdefiniowany został przez wszystkie podzespoły, które nie wchodząw skład elementów użytkowych (z punktu widzenia symulacji) robota. Są to: rama, słup-ki dystansowe, baterie, serwomechanizmy, pług oraz elektronika sterująca. Elementy teutworzono jako obiekty typu Shape, które nie posiadają właściwości fizycznych takichjak masa, tarcie czy sprężystość. Mogą jedynie posiadać wymiary zewnętrzne. Środo-wisko symulacyjne udostępnia kilka kształtów (sfera, stożek, prostopadłościan, ścianka),z których poprzez odpowiednie translacje i rotacje oraz nadanie odpowiednich kolorówstworzono wizualny wygląd robota. Wygląd umieszczono w drzewie układu robotycznegodzięki czemu porusza się on razem z kołami robota. Dodatkowo, główne elementy odpo-wiadające za wygląd i umieszczone na zewnątrz robota posłużyły na drodze powieleniado stworzenia ograniczeń wymiarowych robota tzw. Bounding Box.

4.3. Oprogramowanie robotów 23

4.2.3 Czujniki podłoża

Model czujnika podłoża został stworzony w oparciu o blok Distance Sensor. Ten typczujnika pozwala na wpisanie własnej charakterystyki czujnika oraz jego typu. Dla zacho-wania zgodności z rzeczywistym projektem wybrano typ czujnika infra-red i zastosowanocharakterystykę układu QRD1114 [15] (wykorzystanego w rzeczywistych robotach). Takstworzony czujnik reaguje nie tylko na zmianę odległości ale również na zmianę kolorupowierzchni, nad która się znajduje. Czujnik dodatkowo posiada wygląd zbliżony do rze-czywistych układów. Po powieleniu czujniki umieszczono pod pługiem robota.

4.2.4 Czujniki odległości

Czujniki odległości zamodelowano w podobny sposób jak czujniki podłoża. Czujniki pod-czerwone stworzono wykorzystując charakterystykę układów GP12D firmy Sharp [27].Posiadają wygląd jak i własności (zasięg, kąt widzenia) zbliżone do swoich pierwowzorów.Układ sonaru zamodelowany został przy pomocy bloku Distance Sensor. Jednak jegotyp zmieniony został na sonar, dostępny jako jedna z opcji w programie, dzięki czemunie jest czuły na kolor przeszkód. Charakterystykę sonaru i jego wygląd zamodelowanona podstawie oferowanego przez firmę Parallax2 układu o nazwie ”PING)))” [20].

4.2.5 Chwytak

Chwytak umieszczony na dwóch z pięciu robotów, jest gotowym układem o nazwie grip-per zbliżonym zasadami modelowania do układu differential wheels. Posiada on układsterujący, do którego dodano kilka elementów stanowiących szczęki chwytaka. Posiadająone ograniczenia typu Bounding Box oraz nieskończone tarcie aby możliwe było podno-szenie chwyconych przedmiotów bez możliwości ich wyślizgnięcia się. Ich kształt natomiastpozwala na uchwycenie kuli wielkości piłeczki pingpongowej.

4.2.6 Moduły komunikacyjne

Ponieważ roboty muszą komunikować się na drodze radiowej dzięki modułom radiowym,stworzono modele modułów komunikacyjnych składające się z nadajnika i odbiornika o ide-alnych parametrach (np. nieskończony zasięg). Model posiada wygląd zbliżony do pier-wowzorów (moduły xBee firmy Digi3, dawniej Maxstream) [14], i oparty został o blokiTransmitter orazReceiver. Wybrany został tryb pracy radiowej (inne możliwe to komu-nikacja szeregowa albo łącze podczerwone), z anteną dookólną o nieskończonym zasięgu.Dodatkowa możliwość ograniczenia prędkości transmisji oraz wbudowane w bloki bufo-ry FIFO zbliżają modele układów do rzeczywistych modułów zgodnych ze standardemZigBee [18].Kompletny model robota posiada dodatkowo zdefiniowane własności fizyczne takie

jak masa (0.5 kg), sprężystość i wiele innych (wybrano domyślne wartości) pozwalającena modelowanie kolizji na scenie. Modele trzech typów robotów na arenie przedstawiarysunek 4.2.


(a) roboty

(b) arena

Rysunek 4.2 Modele robotów w środowisku Webots


Listing 4.1 Przykładowy szablon kontrolera w środowisku Webots/∗∗ F i l e :∗ Date :∗ Des c r i p t i on :∗ Author :∗ Mod i f i c a t i o n s :∗/

#include <device/robot .h>/∗∗ You may need to add i n c l u d e f i l e s l i k e <d e v i c e / d i s t a n c e s e n s o r . h> or∗ <d e v i c e / d i f f e r e n t i a l w h e e l s . h>.∗/

#define TIME STEP 64/∗∗ You may want to add d e f i n e s macro here .∗/

stat ic void r e s e t (void ) ;stat ic int run ( int ) ;/∗∗ You shou l d put t h e p r o t o t y p e s o f t h e f u n c t i o n s you w i l l use in your

∗ program here .∗/

/∗∗ You shou l d d e c l a r e here DeviceTag g l o b a l v a r i a b l e s f o r s t o r i n g p o i n t e r s∗ r o bo t d e v i c e s . You shou l d use t h e s t a t i c keyword when d e c l a r i n g t h e s e∗ v a r i a b l e s .∗/

/∗∗ This i s t h e r e s e t f u n c t i o n c a l l e d once upon i n i t i a l i z a t i o n o f t h e r o bo t .∗/stat ic void r e s e t (void )/∗∗ You shou l d i n s e r t r o b o t g e t d e v i c e ( ) f u n c t i o n s to g e t p o i n t e r s to r o bo t∗ d e v i c e s ( DeviceTag ) and use them in the main c o n t r o l l oop .∗/

return ;/∗∗ This i s t h e main c o n t r o l l oop func t i on , i t i s c a l l e d r e p e a t e d l y by Webots∗/stat ic int run ( int ms)/∗∗ Enter here f u n c t i o n s to read sensor data l i k e∗ d i s t a n c e s e n s o r g e t v a l u e ( ) .∗/

/∗∗ Process s ensor data here .∗/

/∗∗ Enter here f u n c t i o n s to send a c t u a t o r commands ,∗ l i k e d i f f e r e n t i a l w h e e l s s e t s p e e d ( ) .∗/

return TIME STEP; /∗ t h i s i s t h e t ime s t e p va lue , in m i l l i s e c o n d s . ∗/

/∗∗ This i s t h e main program which s e t s up th e r e s e t and run f un c t i o n .∗/int main ( )r o b o t l i v e ( r e s e t ) ; /∗ i n i t i a l i z a t i o n ∗/robot run ( run ) ; /∗ s t a r t s t h e c o n t r o l l e r ∗/

return 0 ;


4.3 Oprogramowanie robotów

W celu oprogramowania części symulacyjnej w środowisku Webots wybrano język C.Sposób tworzenia aplikacji zbliżony jest do tworzenia zwykłej aplikacji w tym języku.Aby stworzyć prosty sterownik należy zdefiniować stałą czasową symulacji oraz zaimple-mentować dwie funkcje wywoływane jedna na początku symulacji a druga cyklicznie zokreślonym wcześniej interwałem. Strukturę pliku kontrolera przedstawiono na listingu4.1. Funkcja main zawiera wywołania dwóch funkcji:

• robot live przyjmującej jako argument wskaźnik do wywoływanej jeden raz funkcjiinicjującej środowisko i robota.

• robot run przyjmującej jako argument wskaźnik do funkcji sterującej wywoływanejcyklicznie.

Domyślnie zaproponowane są w szablonie dwie funkcje przeznaczone odpowiednio do ini-cjalizacji robota oraz jego sterowania. Jak można zauważyć, częstotliwość z jaką wywo-ływana jest funkcja sterująca jest zdefiniowana globalnie i jest zwracana przez funkcjęsterującą dla funkcji nadrzędnej.Dodatkowo szablon zawiera sekcje dla dołączanych plików, zmiennych globalnych i wła-

snych definicji. Ich znaczenie jest takie jak w języku C z drobnym wyjątkiem dotyczącymzmiennych globalnych. Ze względu na sposób wywoływania funkcji inicjującej i sterują-cej wszystkie zmienne deklarujące wskaźniki do urządzeń robota muszą być zdeklarowanejako globalne. Należy również pamiętać o konieczności dołączenia plików nagłówkowychdla poszczególnych podzespołów robota takich jak koła, chwytak, czujniki podczerwoneitp. Przykładowy szablon dla robota wyposażonego w czujniki PSD przedstawiono nalistingu 4.2.

2http://www.parallax.com3http://www.digi.com

http://www.parallax.com

http://www.digi.com


Listing 4.2 Szablon kontrolera dla robota z czujnikami PSD#include <device/robot .h>#include <device/dif ferential wheels .h>#include <device/gripper .h>#include <device/distance sensor .h>

#define TIME STEP 32 // dlugosc kroku = 32 ms

stat ic void r e s e t (void ) ; // i n i c j a l i z a c j a robotastat ic int run ( int ) ; // s te rowanie robotem

stat ic DeviceTag shr ; // praywy sharpstat ic DeviceTag sh l ; // lewy sharpstat ic DeviceTag wlsp ; // prawy czu jn ik l i n n istat ic DeviceTag wl s l ; // lewy czu jn ik l i n i istat ic DeviceTag zigbeeRX ; // odb iorn ik radiowystat ic DeviceTag zigbeeTX ; // nadajnik radiowy

stat ic void r e s e t (void )// Pobranie wskaźników do urządzen przez i ch nazwe

shr = robo t g e t d ev i c e ( ” sharpr ” ) ;sh l = robo t g e t d ev i c e ( ” sharp l ” ) ;w l s l = r obo t g e t d ev i c e ( ” w l s l ” ) ;wlsp = robo t g e t d ev i c e ( ”wlsp” ) ;zigbeeTX = robo t g e t d ev i c e ( ” xbeetx ” ) ;zigbeeRX = robo t g e t d ev i c e ( ” xbeerx ” ) ;

// wlaczen i e urzadzend i s t an c e s e n s o r enab l e ( shr ,TIME STEP) ;d i s t an c e s e n s o r enab l e ( shl ,TIME STEP) ;d i s t an c e s e n s o r enab l e ( wlsp ,TIME STEP) ;d i s t an c e s e n s o r enab l e ( wls l ,TIME STEP) ;d i f f e r e n t i a l wh e e l s e n a b l e e n c o d e r s ( 1 ) ;return ;

stat ic int run ( int ms)unsigned short sh r va l , s h l va l , w l s l v a l , w l sp va l ;unsigned int encl , encp ;

// pobranie war tosc i z czujnikowsh r va l = d i s t a n c e s e n s o r g e t v a l u e ( shr ) ;s h l v a l = d i s t a n c e s e n s o r g e t v a l u e ( sh l ) ;w l s l v a l = d i s t a n c e s e n s o r g e t v a l u e ( w l s l ) ;w l sp va l = d i s t a n c e s e n s o r g e t v a l u e ( wlsp ) ;

// wyswie t lan i e na kon so l i logowr o b o t c o n s o l e p r i n t f ( ” w l s l=%04d , wlsp=%04d\n\ r ” , w l s l v a l , w l sp va l ) ;

// pobranie odczytow z enkoderowenc l = d i f f e r e n t i a l w h e e l s g e t l e f t e n c o d e r ( ) ;encp = d i f f e r e n t i a l w h e e l s g e t r i g h t e n c o d e r ( ) ;

// zadanie predkosc i na kolachd i f f e r e n t i a l w h e e l s s e t s p e e d ( 4 , 4 ) ;

return TIME STEP;

int main ( )r o b o t l i v e ( r e s e t ) ;robot run ( run ) ;return 0 ;

Rozdział 5

Konstrukcja robotów

Opis konstrukcji robota podzielony został na dwie części:

• Część mechaniczną, w której opisano sposób wykonania korpusu robota, chwy-taka oraz mocowania kół

• Część elektroniczną, zawierająca opis i schematy poszczególnych części sterowni-ka oraz modemu radiowego.

5.1 Konstrukcja mechaniczna

Projekt elementów mechanicznych wchodzących w skład robota przewidywał wykorzysta-nie jak największej liczby podzespołów, które są powszechnie dostępne w sklepach elektro-nicznych lub innych tak, aby ich powielenie wymagało zlecania produkcji niewielkiej licz-by specjalizowanych podzespołów. Jedynym elementem wykonanym na zamówienie jestaluminiowa rama robota stanowiąca element mocujący układy napędowe robota, czujni-ki oraz elektronikę sterującą. Dodatkowo niezbędne było wykonanie 10 kół dla robotówtak aby można było je zamontować na serwomechanizmach modelarskich oraz wykonaniechwytaka dla dwóch z pięciu budowanych robotów. Na rysunkach 5.1 i 5.2 przedstawionozłożone roboty dwóch typów.

5.1.1 Rama robotów

Ramę robota zaprojektowano z wykorzystaniem programu Qcad [23], dostępnego na licen-cji GPL dla systemu operacyjnego linux pod warunkiem skompilowania programu we wła-snym zakresie. Rama została wycięta laserem z blachy aluminiowej 1.5 mm oraz wygiętado odpowiedniego kształtu zgodnie z rysunkami technicznymi D.1 i D.2 zamieszczonymiw dodatku D. Następnie ramę pomalowano czarną matową farbą aby nadać jej bardziejestetyczny wygląd.Projekt ramy przewiduje możliwość zastosowania serwomechanizmów modelarskich

w standardowym rozmiarze, jako układów napędowych. Dodatkowo przewidziano otworymontażowe dla montowanego z przodu pługu, umieszczonych pod nim czujników koloru,elektroniki sterującej oraz koszyka na baterie zasilające.

5.1.2 Koła robotów

Ze względu na wybrany rodzaj napędu konieczne było wykonanie kół dla robotów tak abymożliwy był ich montaż na zębatkach serwomechanizmów zamiast dostarczanych z ser-

30 5. Konstrukcja robotów

Rysunek 5.1 Robot transportowy

Rysunek 5.2 Robot zwiadowczy

5.2. Elektronika sterująca 31

(a) model koła (b) odlew

Rysunek 5.3 Model koła robota oraz jego odlew z żywicy epoksydowej

wami orczyków. Przy projektowaniu kół wzorowano się na projekcie firmy Parallax [19],dla podobnego rodzaju robotów. Zdecydowano się na własnoręczne wykonanie kół jakoodlewów, co daje dużą powtarzalność wykonania. Formę odlewniczą wykonano z silikonuBRB Flextosil TLM 722, na modelu wytoczonym według projektu z rysunku D.4. Odlewykół wykonano z żywicy epoksydowej Epidian 5 z dodatkiem kolorowego pigmentu. Widokmodelu oraz odlanego koła przedstawia rysunek 5.3.Projekt koła przewiduje zamontowanie w nim (poprzez wklejenie) orczyka dostarcza-

nego z serwomechanizmem o kształcie koła dzięki czemu możliwe będzie jego montowaniena serwomechanizmach. Wykonane zostały dwie formy odlewnicze. Jedna na modelu bezorczyka oraz druga na modelu z zamocowanym orczykiem. Spowodowane to było obawą,iż elementy zębatki orczyka mogą nie zostać dobrze odwzorowane przez formę odlewni-czą. Wykonane odlewy w obydwu przypadkach okazały się dostatecznie dokładne: jedyniew kilku odlewach brakowało pojedynczych zębów w zębatkach, co nie przeszkadzało w ichmontażu. Wadą tej technologi odlewania jest ograniczona żywotność formy, wynosząca 8-10 odlewów, wynikająca z niszczenia powierzchni sylikonu przez żywicę. Więcej informacjina temat odlewów znajduje się w [17], a niezbędne materiały dostępne są w większościsklepów chemicznych.

5.2 Elektronika sterująca

5.2.1 Moduły z kontrolerem MC13213

W ramach pracy magisterskiej zaprojektowano i wykonano 5 modułów z kontroleremMC13213 [6] firmy Freescale. Jeden z nich przedstawiono na rysunku 5.4. Zastosowa-ny w nich układ składa się z mikrokontrolera HC9S08GT60A oraz modemu radiowegoMC13203 dedykowanego dla sieci ZigBee [18]. Na module oprócz wymienionego ukła-du SIP1 umieszczono układ antenowy wzorowany na przykładzie producenta [7], antenęwykonaną na druku według dokumentacji firmy Texas Instruments [30] oraz konwerterUSB↔RS oparty na układzie FT232RL [10].Próby lutowania układu zakończyły się jednak fiaskiem ze względu na typ obudowy

układu [8] oraz płytki wykonane w złej technologii (wykończenie elektrochemiczne cyną).Z tego względu zdecydowano się na zamówienie obwodów drukowanych dla układu w firmie

1SIP = System In Package


Rysunek 5.4 Moduł z kontrolerem MC13213

Eurocircuits2 w technologii złocenia chemicznego. Dodatkowo dzięki uprzejmości firmyElfin3 i WPT4 możliwy był montaż układów z wykorzystaniem profesjonalnego sprzętutakiego jak precyzyjny dyspenser pasty lutowniczej czy piec lutowniczy. Dzięki temu udałosię poprawnie zmontować 5 kompletnych modułów.Kolejnym krokiem było uruchomienie modułów z wykorzystaniem środowisk BeeKit [9]

oraz oprogramowania CodeWarrior. Dzięki uprzejmości firmy FiberLED5, która wypoży-czyła 4 swoje płytki uruchomieniowe z tym samym kontrolerem, możliwe było sprawdzeniedziałania każdego z modułów. W trakcie testów okazało się, że pomimo poprawnej pracyjednostki centralnej układów (obserwowanej z wykorzystaniem debuggera) układ radiowynie zapewnia poprawnej pracy modułów. W praktyce w czasie testów udało się tylko kilkarazy przesłać dane drogą radiową.Z powodu nasilających się trudności i braku zadowalających efektów projekt modułu

została wstrzymany i wybrano tanie komercyjne rozwiązanie w postaci modułów xBee.Projekt modułu można znaleźć na rysunku A.4 znajdującym się w dodatku.

5.2.2 Moduły komunikacyjne xBee

W ramach pracy magisterskiej wykorzystano moduły przeznaczone dla pasma 2.4 GHz fir-my Digi6. Moduły xBee firmy są modułami przystosowanymi do pracy w sieciach ZigBee.W zależności od wybranego firmwaru mogą pracować jako urządzenia wspierające war-stwę sieciową IEEE 802.15.4, lub jako urządzenia definiowane przez protokół ZigBee [18]:koordynator, ruter lub urządzenie krańcowe. Producent modułów udostępnia darmowenarzędzia służące do konfiguracji modemów, oraz zmiany ich oprogramowania sterujące-go. Dostępna jest również dokumentacja deweloperska, zawierająca schematy elektrycznemodułów. Dodatkowo firma MaxStream udostępnia zmodyfikowane biblioteki dla środo-wiska BeeKit tak, aby możliwa była praca z modułami xBee w środowisku CodeWarrior.Widok modułu w wersji z anteną prętową przedstawia rysunek 5.5.Moduły komunikują się poprzez interfejs RS232 z wykorzystaniem zestawu komend

2http://www.eurocircuits.pl3http://www.elfin-pe.pl/4Wrocławski Park Technologiczny5http://www.fiberled.pl/6http://www.digi.com

http://www.eurocircuits.pl

http://www.elfin-pe.pl/

http://www.fiberled.pl/

http://www.digi.com


Rysunek 5.5 Moduł xBee z anteną z prętową

AT [14]. Pozwalają one na ustawienie prędkości transmisji (domyślnie 9600 8N1), ustawie-nie adresu urządzenia i parametrów sieci. Dodatkowo w wersji oprogramowania dla sieciZigBee można skonfigurować typ urządzenia i jego funkcję w sieci. W pracy wykorzystanojedynie wbudowaną warstwę sieciową protokołu IEEE 802.15.4 wspieraną przez wersjęoprogramowania o oznaczeniu 1084. Funkcje zarządzania komunikacją sieciową umiesz-czone zostały w oprogramowaniu układu sterującego.

Płytka ewaluacyjna dla modułów xBee

Aby możliwie łatwo rozpocząć pracę z modułami xBee zaprojektowano dla nich płytkęewaluacyjną. Projekt powstał w oparciu podobny projekt producenta [13], którego foto-grafię przestawia rysunek 5.6. Schemat płytki ewaluacyjnej przedstawiono na rysunku A.6a samą płytkę na rysunku 5.7. Na płytce umieszczono przyciski i potencjometry podłą-czone do wejść modułu, wskaźnik poziomu sygnału zbudowany w oparciu o układ całkują-cy RC i popularny wzmacniacz LM339, moduł USB↔RS opisany w punkcie 5.2.2, diodystatusowe oraz bufory stanowiące konwerter napięć dla linii interfejsu modułu. Pozwalato podłączać moduł do kontrolerów zasilanych napięciem 5V a dodatkowy tranzystor od-cinający zasilanie zapobiega zwarciom w przypadku podłączenia kilku źródeł zasilania.Ze względu na możliwość przeprogramowania modułu (producent udostępnia instrukcjęoraz biblioteki) na płytce umieszczono również złącze BDM pozwalające na wykorzystaniedowolnego interfejsu tego typu dla kontrolerów z rodziny HCS08 firmy Freescale. Obecnyna płytce moduł⇔RS pozwala na obsługę modemu zarówno za pomocą dowolnego termi-nala jak również z wykorzystaniem dostarczanego przez producenta darmowego programuxCTU. Schemat płytki przedstawiono na rysunku A.6

Konwerter USB↔RS

Interfejs szeregowy w modułach xBee wymaga podłączenia do komputera poprzez układkonwersji napięć. Rozwiązanie to jednak posiada istotne wady jak konieczność doprowa-dzenia dodatkowego zasilania dla tego układu. Coraz rzadziej również producenci kompu-terowych płyt głównych umieszczają na nich port szeregowy. Z tych powodów wybranoukład FT232RL [10] będący konwerterem pomiędzy magistralą USB a łączem szerego-wym. Dla tego układu zaprojektowano małą płytkę z wyprowadzeniami w rastrze 100”(rys. 5.8) pozwalającą na jego wygodne podłączenie do aplikacji testowych z wykorzy-staniem płytek uniwersalnych. Moduł zasilany jest bezpośrednio z magistrali USB i możewspółpracować z układami zasilanymi napięciem 3.3V lub 5V w zależności od konfiguracji


Rysunek 5.6 Płytka ewaluacyjna firmy Maxstream dla modułów xBee

Rysunek 5.7 Płytka ewaluacyjna dla modułów xBee wykonana na potrzeby pracy


Rysunek 5.8 Moduł USB↔RS

(a) oryginalny układ (b) układ użyty w pracy

Rysunek 5.9 Elektronika sterująca silnikiem serwomechanizmu

płytki bazowej. Możliwe jest również zasilanie układów zewnętrznych wybranym napię-ciem (maksymalny dopuszczalny pobór prądu to 500 mA). Dodatkowo moduł posiadawyprowadzony sygnał uśpienia magistrali USB oraz linię zegarową, którą można skonfi-gurować dla wybranej częstotliwości (12/24/36/48 HMz).Schemat modułu przedstawionona rysunku A.5

5.2.3 Układ napędowy

Jako układy napędowe robotów wykorzystane zostały serwomechanizmy modelarskie w stan-dardowym rozmiarze. Aby jednak możliwe było określanie prędkości kół robota koniecznebyło zastosowanie koderów kwadraturowych. Aby możliwe było ich zamontowanie we-wnątrz obudowy konieczna była całkowita wymiana elektroniki sterującej serwomechani-zmów. W jej skład wchodzą: obwód scalonego mostka (L293D) oraz koder magnetyczny(AS4050). Dla układów tych wykonano płytki drukowane w taki sposób aby bez potrzebystosowania dodatkowych elementów, pasowały do obudowy w miejsce usuniętych ukła-dów. Aby zapewnić obrót kół o 360 stopni (oryginalne serwo obraca się jedynie o kąt180 stopni) zastosowano modyfikację zębatek opisaną w [26]. Dodatkowo zmodyfikowanoznajdujące się w serwomechanizmach potencjometry. Zastąpiono w nich układ rezystan-cyjny magnesem dla enkoderów magnetycznych. Na rysunku 5.9(a) pokazano oryginalnyukład elektroniczny a na rysunku 5.9(b) układ zbudowany na potrzeby pracy.


Rysunek 5.10 Czujnik białej linii

5.2.4 Czujniki

Roboty wyposażone zostały w czujniki koloru, służące do wykrywania zmiany koloru pod-łoża, czujniki zbliżeniowe PSD [27] firmy Sharp, o zasięgu 20-80 cm, montowane zamienniez sonarem ultradźwiękowym firmy Parallax [20] oraz czujniki podczerwone krótkiego za-sięgu (do 20 cm), służące do precyzyjnego określania pozycji obiektów na scenie.

Czujniki podłoża

Zaprojektowane czujniki są tak przewidziane aby pracowały przy napięciu zasilania 3.3V oraz5V, co pozwala na ich uniwersalne stosowanie w robotach klasy sumo i minisumo. Do-datkowo umożliwiają wykonywanie pomiarów różnicowych dzięki możliwości sterowaniadiodą nadawczą IR. Odczyt z czujników polega na pomiarze napięcia na wyjściu analogo-wym. Schemat czujników przedstawiono na rysunku A.1 a widok zmontowanego czujnikana rysunku 5.10.

Czujniki zbliżeniowe krótkiego zasięgu

Roboty posiadające chwytaki nie są wyposażone w czujniki zbliżeniowe o dużym zasię-gu. Muszą jednak mieć możliwość wykrywania położenia blisko usytuowanych obiektów(w odległości około 10 cm) aby mogły je uchwycić. Z tego względu na płytkach głów-nych opisanych w punkcie 5.2.5 zamontowano dwa czujniki zbudowane w oparciu o układTSOP34838 [31] popularnie wykorzystywany w pilotach telewizyjnych. Układ ten stanowidetektor modulowanego częstotliwością 38 kHz światła podczerwonego. Jako źródło takie-go sygnału wykorzystano diodę podczerwoną kluczowaną przez multiwibrator astabilnyzbudowany na popularnym układzie NE555 [28]. Schemat układu jest częścią schematupłytki głównej robota (rys. A.8).

Sonar ultradźwiękowy

W jednym z robotów zwiadowczych zastosowano gotowy sonar firmy Parallax (rys. 5.11).Pozwala on na pomiar odległości od obiektu w zakresie 2.5cm - 3.5m. Przystosowany jestdo zasilania napięciem 5V, jednak linia wejścia/wyjścia może współpracować z układamizasilanymi napięciem 3.3V.

Czujniki PSD

Dwa spośród robotów zwiadowczych wykorzystują czujniki GP2D12 firmy Sharp (rys.5.12). Są to czujniki typu PSD7 z interfejsem analogowym, przystosowane do pracy w ukła-

7Position Sensing Device


Rysunek 5.11 Sonar ”PING)))” firmy Parallax

Rysunek 5.12 Czujnik GP2D12 firmy Sharp

dach zasilanych napięciem 5V. Potrafią mierzyć odległość w zakresie 10-80cm. Jednakzgodnie z charakterystyką napięcia wyjściowego umieszczoną w [27] napięcie wyjścioweukładu nie przekracza 2.8V dzięki czemu wyjście układu można bezpośrednio podłączyćdo układów zasilanych 3.3V.

5.2.5 Płyta główna robota i moduł sterujący

Ze względu na niepowodzenia związane z lutowaniem i uruchomieniem modułu opisanegow punkcie 5.2.1 zdecydowano się na wykorzystanie fabrycznego modemu Zigbee. Jednakaby możliwe było wykorzystanie tego modułu w robotach w przypadku jego uruchomieniajako jednostkę sterującą wybrano kontroler z rodziny HCS08 [4], obecną również w ukła-dzie MC13213. Mikrokontroler ten posiada niezbędne do obsługi podzespołów robotaperyferia:

• przetwornik analogowo-cyfrowy

Rysunek 5.13 Moduły z kontrolerem HC9S08GT60A


Rysunek 5.14 Zmontowana płytka główna robotów

• dwa wielokanałowe timery

• dwa porty szeregowe

• interfejs SPI8

• układ PLL

Procesory z tej rodziny wyposażone są w moduł BDM9 pozwalający na wygodne uru-chamianie i testowanie oprogramowania dla kontrolera. Interfejsy wykorzystujące mo-duł BDM dla kontrolerów HCS08 oraz HC12 zostały opisane w [12]. Dla kontroleraHCS08GT60A zaprojektowano moduł uruchomieniowy, wyprowadzający sygnały na igływ rastrze 100”, oraz posiadający kilka podstawowych elementów niezbędnych do urucho-mienia kontrolera, jak kwarc, złącze BDM i przycisk resetu. Dodatkowo od spodu płytkimożliwy jest montaż konwertera USB↔RS wykorzystującego jeden z portów szeregowych.Daje to dodatkową możliwość rozbudowy modułu. Zmontowane moduły przedstawionona rysunku 5.13 a jego schemat na rysunku A.7.W celu podłączenia sygnałów z czujników oraz doprowadzenia sygnałów sterujących

zaprojektowano płytkę główną przedstawioną na schemacie A.8. Stanowi ona dodatkowoelement mechaniczny zapewniający dodatkową sztywność oraz utrzymujący czujniki odle-głości (PSD lub sonar). Płytka montowana jest do ramy przy pomocy śrub M3 na słupkachdystansowych. Wszystkie sygnały z i do płytki doprowadzono przy pomocy kabli zakoń-czonych złączami typu crimp terminal w rastrze 100” z zatrzaskiem. Daje to możliwośćszybkiego demontażu zapewniając jednocześnie niezawodne połączenie. Wyjątek stano-wią taśmy kablowe podłączające kodery magnetyczne. Dla nich zastosowano gniazda IDCw rastrze 2mm ze względu na konieczność zmieszczenia okablowania w obudowie serwo-mechanizmów.

8Serial Peripheral Interface9Background Debug Module

5.3. Arena 39

Rysunek 5.15 Arena wraz z robotami w trakcie eksperymentów

Na płytce oprócz modułu z kontrolerem i modułu xBee zamontowano dodatkowo dwiediody sygnalizacyjne dowolnego przeznaczenia, pamięć szeregową (I2C), mały sygnalizatorelektromagnetyczny oraz opisany wcześniej układ czujników krótkiego zasięgu dla robotówwyposażonych w chwytak.Zmontowaną płytkę przed zamocowaniem na robocie pokazano na rysunku 5.14.

5.3 Arena

Arenę, po której poruszają się roboty wykonano z trzech arkuszy podkładów pod pa-nele podłogowe, dzięki czemu uzyskano prostokąt o wymiarach 120×150 cm. Na płytachumieszczono czarny karton a na nim przyklejono szachownicę, z białego papieru samoprzy-lepnego, o pojedynczym kwadracie 15×15 cm. W ten sposób powstała arena składającesię z 80 kwadratów, ułożonych w konfiguracji 8x10, przedstawiona na rysunku 5.15.

Rozdział 6

Implementacja i badanie algorytmówsterowania

6.1 Implementacja warstwy sterującej i komunika-cyjnej

Na potrzeby implementacji oprogramowania robotów wykonano analizę wykorzystania za-sobów kontrolera. Było to konieczne ze względu na wykorzystanie jego peryferiów do wielucelów jednocześnie. Między innymi dostępne w jednostce GT60A dwa timery zostały wy-korzystane do sterowania silnikami, sterowania serwomechanizmem chwytaka, generowa-nia cyklicznych przerwań dla regulacji PID (opisanego w podsekcji 2.2.2), obsługi sonaruoraz innych pomiarów czasu. Analiza użycia peryferiów przez poszczególne fragmentyalgorytmu sterującego przedstawiona została w tabeli 6.1.

6.1.1 Warstwa sterująca

Warstwę sterującą oprogramowania stanowi część kodu oprogramowania odpowiadającaza konfigurację kontrolera, jego peryferiów jak również podstawowe zadania niezbędnedla poprawnej pracy algorytmu współdziałania. Do zadań tych zalicza się obsługę czuj-ników umieszczonych na robocie, akwizycję danych pomiarowych, realizację podstawo-wych zadań sterownia jak na przykład utrzymywanie zadań prędkości kół, wizualizacjęparametrów robota oraz obsługę komunikacji na poziomie sprzętu. W tej części pracyprzedstawiono wykorzystane przy implementacji rozwiązania wraz z fragmentami kodówźródłowych omawianych części oprogramowania.

Zegar systemowy

Dla zapewnienia dużej mocy obliczeniowej jak również poprawnej komunikacji z wykorzy-staniem portów szeregowych skonfigurowano wbudowany w kontroler moduł generatorazegara ICG1. Układ ten ustawiono w tryb FEE2 podnosząc częstotliwość dostarczonąprzez znajdujący się na module kwarc czterokrotnie. Dzięki temu częstotliwość magistra-li danych w kontrolerze wyniosła 14,7456 MHz co stanowi wielkość podzielną przez 2n,dającą w wyniku standardowe prędkości transmisji interfejsu RS232C.

1Internal Clock Generator (S08ICGV2)2FLL Engaged, External Clock (FEE) Mode - tryb pracy, w którym moduł ICG korzysta z zewnętrz-

nego źródła jakim jest kwarc, rezonator lub zegar zewnętrzny. Pozwala osiągnąć częstotliwość magistralikontrolera w przedziale 4-20 MHz

42 6. Implementacja i badanie algorytmów sterowania

Tabela. 6.1 Wykorzystanie podzespołów kontrolera HC9S08GT60A w programie steru-jącym robotamiPeryferium WykorzystanieSerial Peripheral Interfejs(S08SPIV3)

obsługa koderów magnetycznych

TIMER/PWM 1 (S08TPMV1) obsługa serwomechanizmu chwytaka (kanał 0),sonaru (kanał 1), generowanie cyklicznego (50Hz)przerwania dla regulatora PID

TIMER/PWM 2 (S08TPMV1) sterowanie silnikami napędowymi (kanały 0, 1)Analog-to-Digital Converter(S08ATDV3)

cykliczny pomiar napięcia wyjściowego czujnikówpodłoża, czujników PSD i czujników krótkiego za-sięgu (kanały 0-5)

Inter-Integrated Circuit(S08IICV1)

obsługa pamięci szeregowej

Serial Communications Interfa-ce 1 (S08SCIV1)

komunikacja z PC przez moduł USB↔RS

Serial Communications Interfa-ce 2 (S08SCIV1)

komunikacja z modułem xBee

PORT A sterowanie silnikami napędowymi, brzęczykim,diodami oraz układem nadawczym modulowane-go sygnału IR

Układ przerwań

W środowisku CodeWarrior dostępne są trzy sposoby deklarowania funkcji obsługi prze-rwań [5]. Dwa z nich wymagają modyfikowania pliku konfiguracyjnego linkera. Z tegowzględu wybrano trzecią z dostępnych metod pozwalającą zdeklarować funkcję jako wek-tor obsługi przerwania w jednym pliku poprzez jej deklarator. Aby dodatkowo nie byłokonieczne używanie niewygodnych numerów przerwań zdefiniowano makra odpowiadają-ce nazwom przerwań o wartościach odpowiadających im numerów przerwań. Dodatkowozdefiniowano makro ISR3, które przyjmuje jako argument numer wektora przerwania ageneruje odpowiedni nagłówek i deklarator funkcji obsługi znacznie skracając zapis w ko-dzie źródłowym. Opisana konstrukcja przedstawiona została na listingu 6.1.

Porty szeregowe

Zarówno pierwszy jak i drugi port szeregowy zostały skonfigurowane do pracy z prędko-ścią 115200 bodów4. Jest to największa spośród standardowych prędkość transmisji ob-sługiwana przez moduł xBee. Dla obydwu portów skonfigurowano przerwania od odbiorupozwalające reagować na przychodzące znaki oraz napisano funkcje wysyłające napisyznakowe przez interfejs SCI.

Czujniki koloru i PSD - przetwornik ADT

Do zbierania danych z czujników PSD oraz umieszczonych pod spodem robota czujnikówkoloru wykorzystano przetwornik analogowo cyfrowy. Pracuje on w trybie pojedyncze-go pomiaru wyzwalanego wyborem kanału. Dodatkowo przetwornik generuje przerwanie3Interrupt Service Routine41 bod = 1 bit×s−1

6.1. Implementacja warstwy sterującej i komunikacyjnej 43

Listing 6.1 Makra ułatwiające tworzenie funkcji obsługujących przerwania/∗ Numery wektorow przerwan ∗/#define VNreset 0 // Watchdog, Low voltage detect , RESET pin , i l l e g a l opcode#define VNswi 1 // Software interrupt#define VNirq 2 // IRQ pin#define VNlvd 3 // Low voltage detect#define VNicg 4 // ICG#define VNtpm1ch0 5 // Timer 1 chanel 0#define VNtpm1ch1 6 // Timer 1 chanel 1#define VNtpm1ch2 7 // Timer 1 chanel 2#define VNtpm1ovf 8 // Timer 1 overflow#define VNtpm2ch0 8 // Timer 2 chanel 4#define VNtpm2ch1 10 // Timer 2 chanel 0#define VNtpm2ch2 11 // Timer 2 chanel 2#define VNtpm2ch3 12 // Timer 2 chanel 3#define VNtpm2ch4 13 // Timer 2 chanel 4#define VNtpm2ovf 14 // Timer 2 overflow#define VNspi 15 // SPI#define VNsci1err 16 // SCI1 error#define VNsci1rx 17 // SCI1 receive#define VNsci1tx 18 // SCI1 transmit#define VNsci2err 19 // SCI2 error#define VNsci2rx 20 // SCI2 receive#define VNsci2tx 21 // SCI2 transmit#define VNkeyboard 22 // Keyboard pins#define VNatd 23 // AD conversion complete#define VNiic 24 // IIC control#define VNrti 25 // real−time interrupt

#define ISR( VECTOR) interrupt VECTOR void interrupt ## VECTOR## ();\i n t e r rup t VECTOR void i n t e r rup t ## VECTOR## ()

od zakończenia konwersji, w którym nastepuje odczytanie danych oraz wybór kolejnego ka-nału, co wyzwala kolejny pomiar.W czujnikach koloru dokonywany jest pomiar różnicowypolegający na odjęciu wartości napięcia zmierzonego przy zgaszonej diodzie podczerwonejod wartości napięcia zmierzonego przy diodzie zapalonej. Aby cykl przetwarzania nie byłkrótszy niż czas zgaszenia/zapalenia diody wybrano maksymalny czas konwersji ustawia-jąc preskaler na wartość 15, co daje w efekcie podział częstotliwości magistrali procesoraprzez 32.

Sterowanie silnikami - przerwanie cykliczne

W celu sterowania silnikami robota wykorzystano sygnały PWM 5 generowane przez ka-nały 0 i 1 16-to bitowego Timera 2 oraz 4 linie portu A. Timer ustawiono w trybie pracyz częstotliwością magistrali kontrolera oraz liczenia do wartości 0xffff. Następnie obydwadostępne kanały ustawiono w tryb pracy PWM z wyrównaniem do zbocza.Kolejnym etapem była konfiguracja Timera 1 do generowania cyklicznego przerwania

z częstotliwością równą 50 Hz (aby możliwe było wykorzystanie go do sterowania serwo-mechanizmem modelarskim). Wybrano preskaler równy 4 oraz ustawiono górną granicęlicznika na wartość 0xe38c zgodnie z informacjami zawartymi w [4]. Następnie skonfigu-rowano Timer do generowania przerwania od przepełnienia w którym następuje odczytenkoderów i wywołanie funkcji pid implementującej regulator PID.Sposób implementacji funkcji obsługi przerwania cyklicznego przedstawiono na listin-

gu 6.2. Następuje w nim kolejno odczyt z enkoderów, pomiar przejechanej trasy omówio-ny w punkcie 6.2.2, obliczenie sterowań i zadanie ich na koła, jako wartość wypełnieniaimpulsu dla kanałów Timera 2, z uwzględnieniem kierunku ruchu. Warto zauważyć, żedo funkcji obliczającej sterowanie dla lewego koła prędkość zadawana jest z przeciwnymznakiem. Wynika to z faktu przeciwnego ułożenia mechanizmów napędowych w robo-cie. We wszystkich obliczeniach zakłada się, że prędkość jest dodatnia w kierunku ruchudo przodu.

5Pulse Width Modulation


Listing 6.2 Funkcja obsługi przerwania cyklicznego (od przepełnienia timera 1)ISR(VNtpm1ovf ) // przerwanie od p r z ep e l n i e n i a t imera 1unsigned char s tatus , tmp1 , tmp2 ; // zmienne pomocniczeunsigned short encl , encp ; // odczyt z enkoderówstat ic int sum el=0; // ca lka z uchybu lewego s i l n i k a

stat ic int sum ep=0; // ca lka z uchybu prawego s i l n i k astat ic int o l d e l =0; // uchyb na lewym ko le w poprzednim kroku sterownaia

stat ic int o ld ep=0; // uchyb na lewym ko le w poprzednim kroku sterownaiastat ic unsigned short o l d e n c l =0; // odczyt lewego enkodera w poprzednim krokustat ic unsigned short o ld encp=0; // odczyt prawego enkodera w poprzendim krokuint s te rowanie ;short vl , vp ;

PTCD PTCD6 = 0 ;tmp1 = SPIRxTx(0 x f f ) ;tmp2 = SPIRxTx(0 x f f ) ;PTCD PTCD6 = 1 ;enc l = (tmp1<<9)|(tmp2<<1); // wyrownanie do l ewe j

enc l&=0x f f c 0 ; // u sun i e c i e wskaznikow j a k o s c i z odczytu

PTED PTED2 = 0 ;tmp1 = SPIRxTx(0 x f f ) ;tmp2 = SPIRxTx(0 x f f ) ;PTED PTED2 = 1 ;

encp = (tmp1<<9)|(tmp2<<1); // wyrownanie do l ewe jencp&=0x f f c 0 ; // u sun i e c i e wskaznikow j a k o s c i z odczytu

v l = enc l − o l d e n c l ; // o b l i c z n e i e predkosc i ko lvp = encp − o ld encp ;o l d e n c l = enc l ;o ld encp = encp ;

i f ( ( s t . d i s tp > −300) && ( s t . d i s tp < 300)) // uaktua ln i an i e zadanego przemie szczen ias t . d i s tp = 0 ;

elses t . d i s tp −= (vp ) ;

i f ( ( s t . d i s t l > −300) && ( s t . d i s t l < 300))s t . d i s t l = 0 ;

elses t . d i s t l −= (−v l ) ;

s te rowanie = pid(−speedl , vl ,&sum el ,& o l d e l ) ; // o b l i c z e n i e i zadanie sterowani f ( s te rowanie < 0)PTCD PTCD4 = 0 ;PTCD PTCD5 = 1 ;TPM2C1V = ((unsigned short )(− s te rowanie ))<<1;

elsePTCD PTCD4 = 1 ;PTCD PTCD5 = 0 ;TPM2C1V = ((unsigned short ) ( s te rowanie ))<<1;

s te rowanie = pid ( speedp , vp,&sum ep ,& o ld ep ) ;i f ( s te rowanie < 0)PTAD PTAD4 = 1 ;PTAD PTAD5 = 0 ;TPM2C0V = ((unsigned short )(− s te rowanie ))<<1;

elsePTAD PTAD4 = 0 ;PTAD PTAD5 = 1 ;TPM2C0V = ((unsigned short ) ( s te rowanie ))<<1;

// ga s z en i e f l a g i przerwanias t a tu s = TPM1SC; // read s ta tu sTPM1SC TOF = 0 ; // c l e a r i n t e r rup t f l a g

6.1. Implementacja warstwy sterującej i komunikacyjnej 45

Rysunek 6.1 Zasada sterowania serwomechanizmem modelarskim

Sonar - przechwytywanie wejścia

Do obsługi sonaru wykorzystano kanał 1 timera 1 pracującego z częstotliwością 50 Hz.Daje to okres 20 ms co jest wartością wystarczającą do wykonania pomiaru [20] przypomocy sonaru. Cykl pomiarowy wyzwalany jest stanem wysokim na pinie PTD1 (Kanałtimera jest odłączony od pinu, pin skonfigurowany jest jako wyjście) i odczytem rejestruliczącego timera 1. Następnie kanał timera konfigurowany jest w tryb przechwytywaniawejścia (IC6) reagujący na zbocze opadające. Gdy pojawi się zbocze opadające na wejściutimera, następuje sprzętowe zapamiętanie wartości licznika oraz wyzwalane jest przerwa-nie, w którym wartości te są odejmowane dając w wyniku czas powrotu fali dźwiękowej.Również w przerwaniu nastepuje odłączenie kanału timera od pinu aby możliwe byłowykonanie kolejnego pomiaru.

Serwomechanizm chwytaka - PWM

Serwomechanizm chwytaka sterowany jest sygnałem PWM generowanym z wykorzysta-niem kanału 0 timera 1 (tryb PWM z wyrównaniem do zbocza). Zasadę sterowania ser-womechanizmem modelarskim przedstawia rysunek 6.1. W praktyce jednak serwomecha-nizmy pracują poprawnie dla częstotliwości sygnału sterującego w zakresie 42–58 Hz.Często również posiadają szerszy zakres długości impulsu sterującego jednak mieszczącysię zawsze w przedziale 0.5–2.5 ms z położeniem zerowym dla czasu 1.5 ms.

6.1.2 Warstwa komunikacyjna

Komunikacja kontrolera z modułem xBee

W celu unifikacji obsługi portów szeregowych w kontrolerze zmieniono fabryczne usta-wienia interfejsu szeregowego w module xBee wykorzystując program xCTU. Domyślniemoduły posiadają ustawioną prędkość transmisji 9 600 bodów, co zostało zmienione nawartość 115 200 bodów. Dzięki temu możliwa jest szybsza komunikacja pomiędzy robo-tami oraz jednolita skonfiguracja obu interfejsów szeregowych. Dodatkową zaletą tegoustawienia jest możliwość przekierowania danych z portów w sposób tworzący z kontrole-ra NullModem. Dzięki temu możliwe jest konfigurowanie modułu xBee z wykorzystaniemterminala lub programu xCTU bez konieczności wyjmowania go z płytki głównej robota.

6IC - Input Capture


Listing 6.3 Kod obsługi przerwania odpowiedzialnego za odbieranie ramek danych#define ZIGBEERXFRAMETRANSMITTING 0#define ZIGBEERXBUSCLEAR 1#define ZIGBEERXFRAMERECEIVED 2#define ZB(a ,b) ZIGBEE ## a ## ## b

typedef union // ramka danych protoko luunsigned char frame [ 6 ] ; // ca l a ramkastructunsigned char addr ; // adres nadawcyunionunsigned char sxy ; // po l o z en i e nadawcystructunsigned char sy : 4 ; // po l o z en i e skladowa yunsigned char sx : 4 ; // po l o z en i e skladowa x

; ;unsigned char f code ; // kod zdarzen iaunionunsigned char fxy ; // po l o z en i e zdazen iestructunsigned char fy : 4 ; // po l o z en i e skladowa yunsigned char fx : 4 ; // po l o z en i e skladowa x

; ;unsigned char end [ 2 ] ; // koniec ramki

; frame ;

typedef struct mframe // Struktora obs lugujaca ramkeframe data ; // ramka danychunsigned char dptr ; // wskaznik na aktualny ba j t danych

zframe ;

stat ic zframe RxFrame ;

ISR( VNsci2rx ) // xBeeuchar u8status , u8dummy ;/∗ ensure Rx data b u f f e r i s f u l l ∗/dou8status = SCI2S1 ;

while ( ( u8status & 0x20 ) == 0 ) ;/∗ Check f o r Errors ( Framing , Noise , Pa r i t y ) ∗/i f ( ( u8status & 0x07 ) != 0)u8dummy = SCI2D ;return ;

switch ( State . RXstate )case ZB(RX,BUS CLEAR) :RxFrame . dptr=0;State . RXstate=ZB(RX,FRAME TRANSMITTING) ;RxFrame . data . frame [ RxFrame . dptr++] = SCI2D ;break ;

case ZB(RX,FRAME TRANSMITTING) :RxFrame . data . frame [ RxFrame . dptr++] = SCI2D ;

default :u8dumny=SCI2D ;

break ;i f (RxFrame . dptr==6) State . RXstate=ZB(RX,FRAME RECEIVED) ;

Protokół komunikacji między robotami

Ponieważ wszystkie moduły fabrycznie skonfigurowane są jako konwertery RS⇔ZigBeenadające z wykorzystaniem adresu rozgłoszeniowego, zdecydowano się na wykorzystanietego ustawienia i implementację własnego protokołu komunikacji pomiędzy robotami.Pierwszym założeniem była możliwość śledzenia przesyłanych informacji na terminalu

dzięki wykorzystaniu podłączonego do komputera modułu na płytce ewaluacyjnej. Z te-go względu poszczególne ramki rozdzielane są dwoma znakami CR,LF o kodach 0x0D,0x0A. Dzięki temu są one widoczne na terminalu każda w innej linii bez koniecznościdodatkowej obróbki. Ze względu na nadawanie z adresem rozgłoszeniowym konieczne jestpodanie adresu nadawcy w ramce. Zdecydowano się na rozpoczęcie ramki właśnie od niego.Następnie w ramce danych znajduje się położenie robota, literowe oznaczenie przesyłanejinformacji o działaniu robota oraz dane dotyczące lokalizacji zdarzenia. Możliwe z punk-tu widzenia przesyłania informacji zdarzenia dla robota (zawierają się w nich zdarzenia

6.2. Implementacja algorytmu współpracy 47

Tabela. 6.2 Zdarzenia o których roboty informują pozostałych agentów na arenieZnak ascii znaczenie zakres wartościa rezerwacja pola (0–7, 0–9)b zwolnienie pola (0–7, 0–9)c znalezienie obiektu (0–7, 0–9)d zabranie obiektu (0–7, 0–9)

Tabela. 6.3 Ramka przesyłana przy komunikacji robotówAdres nadawcy Położenie

robota (x,y)Kod zdarzenia Lokalizacja zda-

rzenia (x,y)CR LF

1 bajt 4 bity 4 bity 1 bajt 4 bity 4 bity 1 bajt 1 bajt

z modelu opisanego w rozdziale 3) przedstawiono w tabeli 6.2, natomiast ramkę komuni-kacyjną obrazuje tabela 6.3. Taki sposób implementacji pozwala na prostą interpretacjęprzesyłanych danych w czasie testów pozwalając równocześnie ograniczyć ilość przesyła-nych danych w stosunku do komunikacji w pełni znakowej. Na listingu 6.3 przedstawionofragmenty kodu odpowiadające za odbieranie ramek.

6.2 Implementacja algorytmu współpracy

6.2.1 Regulacja prędkości - regulator PID

Kod funkcji realizującej algorytm regulatora pid przedstawia listing 6.4. Jest to implemen-tacja dyskretnej wersji algorytmu omówionego w części 2.2.2. Dodatkowo w celu wyzna-czenia parametrów regulatora wyznaczono eksperymentalnie odpowiedź silnika na skokjednostkowy. Trzy próbki danych pomiarowych przedstawia wykres na rysunku 6.2. War-to zwrócić uwagę na nietypową jednostkę prędkości obrotowej kół robota. Mimo nieintu-icyjności jest ona wygodna do implementacji w robocie ponieważ zmiana rozdzielczościenkoderów na 16-to bitową (konieczna ze względu na konwersję typów) znosi dzielnik65 535 natomiast częstotliwość pomiarów dokonywanych w przerwaniu cyklicznym znosimnożnik 50. Pozwala to na operowanie różnicą wskazań enkodera w kolejnych cyklachpomiarów jako prędkością koła robota.Korzystając z wykresu wyznaczono zgrubnie stała czasową τ = 0.035 s dla mode-

lu silnika jako obiektu pierwszego rzędu K(s) = 1τs+1 . Korzystając z oprogramowania

Matlab i pakietu Simulink wyznaczono parametry regulatora PID, które następnie zwe-ryfikowano eksperymentalnie. Ostatecznie parametry regulatora wynoszą odpowiednio:KP = 25, KD = 10, KI = 9. Przy takich wartościach robot porusza się wzdłuż linii prostej

Listing 6.4 Implementacja regulatora PIDshort pid ( short sp , int v , int ∗ sum e , int ∗ o ld e , char Kp, char Kd, char Ki )int sterowanie , d , e ;e = (v − sp ) ; // cz lon P∗sum e += e ; // cz lon Ii f ( sp == 0)∗ sum e=0;i f (∗ sum e > 1000) ∗sum e = 1000;i f (∗ sum e < −1000) ∗sum e = −1000;d = e − ∗ o ld e ; // cz lon Dsterowanie = ( e∗Kp + d∗Kd + ∗sum e∗Ki ) ;i f ( s te rowanie > 32760) s te rowanie = 32760;i f ( s te rowanie < −32760) s te rowanie = −32760;∗ o ld e = e ;return s te rowanie ;


0

100

200

300

400

500

600

700

800

0 0.02 0.04 0.06 0.08 0.1 0.12 0.14 0.16 0.18

pred

kosc

[1/6

5 53

5 *

50 d

eg/s

]

czas [s]

probka 1probka 2probka 3

Rysunek 6.2 Odpowiedź skokowa silnika robota

a jego faktyczny zakres prędkości wynosi 30–700 5065535

s.

6.2.2 Odometria

Na listingu 6.2 przedstawiającym funkcję obsługi przerwania cyklicznego, znajduje sięfragment dotyczący pomiaru przejechanej przez roboty trasy. Pomiar dokonywany jestna zasadzie pomiaru zmiany kąta obrotu koła, która jest proporcjonalna do przejechanejtrasy. Koło robota razem z oponą ma średnicę 68 mm a rozdzielczość enkoderów wynosi1024 kroki (mimo wyrównania do lewej w 16 bitowej zmiennej). Stąd obwód koła robotawynosi:

C = πd = π· 68mm = 213.6mm (6.1)

Biorąc pod uwagę (6.1) i rozdzielczość enkodera otrzymujemy proporcję:

C

1024=a

St(6.2)

gdzie: a to długość boku kwadratu (150 mm) na arenie a więc odległość środków kwa-dratów, St to liczba kroków enkodera potrzebna aby tę odległość pokonać. Z (6.2) i faktuwyrównania odczytów enkodera do lewej otrzymujemy:

∆Φ = St· 26 = 719.1· 26 = 46022 (6.3)

Wartość ∆Φ=46022 określa zmianę kąta koła odpowiadającą przejechaniu przez koło trasyo długości 150 mm. Eksperymentalna weryfikacja obliczeń wykazała drobne różnice po-między wartością obliczoną a dobraną doświadczalnie, a wynoszącą 47600. Daje to błądna poziomie 4% i jest wartością dopuszczalną. Takie sterowanie zapewnia prostoliniowyruch robota do przodu.


Rysunek 6.3 Wpływ umieszczenia czujnika PSD na pomiar odległości przy podłożu

Aby możliwa była zmiana orientacji robota (zakłada się możliwość zmiany orientacjitylko o wartość π2 lub π) konieczne było również wykonanie przeliczeń długości trasydla kół poruszających się przeciwbieżnie. Koła robota znajdują się w odległości 90 mmod siebie. Aby więc robot zmienił orientację o π2 każde z kół musi pokonać trasę o długościrównej 14 obwodu koła o promieniu 45mm:

l =14· 2π· r = 1

2π· 45mm = 70.7mm (6.4)

Korzystając z (6.3) oraz (6.4) można zapisać proporcję dla zmiany kąta koła robota od-powiadającej zmianie jego orientacji o π2 :

∆Φa=∆Φrl

(6.5)

a z niej wyznaczyć poszukiwaną zmianę kąta:

∆Φr = ∆Φ·l

a= 46022· 70.7mm

150mm= 21687 (6.6)

Wyznaczona wartość ∆Φr=21687 została zweryfikowana eksperymentalnie i ustalona nawartość 22600. Różnica wartości obliczonej a dobranej wynosi zatem 5% i jest dopusz-czalna rozbieżnością.

6.2.3 Kalibracja czujników

Wszystkie czujniki analogowe zamontowane w robocie wymagają kalibracji. Z tego wzglę-du porównano odczyty czujników koloru oraz czujników PSD w dwóch robotach. Pozwoliłoto eksperymentalnie ustalic próg dla czujników koloru na wartość 80 (większa wartość róż-nicy pomiarów z włączoną i wyłączoną diodą podczerwoną występuje na podłożu białym).Kompensacji wymagają również czujniki PSD umieszczono w robocie w pozycji pionowejoraz przeciwnie do siebie. Wynika to z faktu, że w jednym z czujników układ nadawczy IRznajduje się bliżej podłoża przez co jest ono wykrywane z mniejszego dystansu. Obrazujeto rysunek 6.3. Podobny problem, choć w mniejszym stopniu, występuje przy pomiarzeodległości od przeszkód.


Listing 6.5 Struktura przechowująca wiersz z tablicy położeń robotówtypedef unionunsigned char rows ;structunsigned char row0 : 1 ;unsigned char row1 : 1 ;unsigned char row2 : 1 ;unsigned char row3 : 1 ;unsigned char row4 : 1 ;unsigned char row5 : 1 ;unsigned char row6 : 1 ;unsigned char row7 : 1 ;

; array rows ;

Dodatkowo ograniczono pomiary odległości czujników PSD oraz sonaru do dwóch pól,czyli 30 cm. Wynika to z faktu, że czujniki PSD dla odległości 40 cm zaczynają wykry-wać podłoże, po którym porusza się robot. Odczyt sonaru ograniczono dla zachowaniaspójności w algorytmie mimo, że w jego przypadku problem ten nie występował. Ograni-czenie było również konieczne ze względu na zasięg sonaru wynoszący ponad 3 m. Wszyst-kie wartości kalibracji zostały dobrane eksperymentalnie i są przechowywane w pamięciEEPROM znajdującej się na płytce głównej robota.

6.2.4 Nawigacja robotów

Ponieważ arena została podzielona 80 pól w układzie 8×10 naturalnym sposobem im-plementacji tablicy położeń robotów wydaje się struktura przedstawiona na listingu 6.5.Jest to struktura przechowująca informacje o jednym wierszy tablicy. Została tak za-implementowana aby zajmowała możliwie mało miejsca w pamięci (8 bit) pozwalającodwołać się do każdego ze swoich pól bezpośrednio. Tablica dziesięciu takich elementówwystarcza do zapisania położenia wszystkich robotów na arenie zajmując tylko 10 bajtóww pamięci kontrolera. Gdyby wykorzystano zwykłą tablicę char[8][10] zajęła by ona 80bajtów. Ma to bardzo duże znaczenie przy wykorzystaniu stosunkowo ubogiej platformysprzętowej jaką jest mikrokontroler.Zmiana położenia robotów następuje w momencie przejazdu czujników koloru nad

granicą kwadratów, co pociąga za sobą zdarzenia e ∈ Ea. Zdarzenie e ∈ Eb nastepuje poprzejechaniu przez robota dystansu 9 cm od chwili przekroczenia granicy pól.

Korekcja trajektorii

Ze względu na niedokładności w poruszaniu się robotów, wynikające w szczególności zniedokładnej pozycji początkowej (robot nie stoi równolegle do osi areny) oraz niedokład-nemu obrotowi o 90 konieczne jest poprawiania trajektorii robota w trakcie jego ruchu.Możliwe są dwa momenty, przedstawione na rysunku 6.4 , w których można dokonywaćpoprawek:

1. Robot nie dojeżdża prostopadle do granicy kwadratów (zmiana koloru nie nastepujew krótkim odstępie czasu) - rys. 6.4(b)

2. Robot zjeżdża z kwadratu pod małym kątem (czujniki wskazują różne wartościdłuższy czas) - rys. 6.4(a)

Korekcja orientacji robota odbywa się w inny sposób dla każdego z wymienionychprzypadków, choć obydwa wykrywane są w ten sam sposób. Polega on na sprawdzeniuczasu jaki upływa pomiędzy zmianą wskazań czujników koloru. Jeśli czas ten jest większy


(a) nierówny dojazd do granicy pól (b) zjeżdżanie z pola

Rysunek 6.4 Przypadki wymagające korekcji trajektorii

niż 0.5 s (w praktyce drugi z czujników nie zmienia wskazania w tym czasie) oznaczato, że należy poprawić trajektorię ruchu. Dodatkowo następuje sprawdzenie jaki dystansprzejechał robot od środka pola na jakim się znajdował, i jeśli jest on bliski połowiedługości boku pola (7.5 cm) błąd klasyfikuje się jako błąd pierwszego rodzaju (rys. 6.4(a))Jeśli robot nie przejechał dystansu bliskiego wspomnianej długości przypadek klasyfikujesię jako przypadek drugi (rys. 6.4(b)). Różnica przejechanego dystansu i długości połowyboku pola areny, będąca granica rozstrzygającą przypadki wynosi w pracy 1.5 cm.

Poprawa trajektorii w przypadku pierwszym następuje poprzez zatrzymanie jednegoz kół (po stronie czujnika, który wykrył zmianę pola) i dojechanie drugą stroną robota(drugim czujnikiem) do granicy pól. W przypadku drugim natomiast koryguje się orienta-cję o 10 (na podstawie (6.6) i doświadczeń) co pozwala na uzyskanie pożądanego kierunkuruchu.

6.2.5 Bezkolizyjna komunikacja

W celu zapewnienia bezkolizyjnej komunikacji pomiędzy robotami, w pierwszej wersji sys-temu, zaproponowano komunikację na zasadzie podobnej do protokołu TokenRing. Jednakw związku z problemami związanymi z adresowaniem robotów, kolejnością ich włączeniaoraz możliwością prowadzenia badań na ich mniejszej ilości niż przewidziano ostateczniez niego zrezygnowano. Sposób komunikacji uproszczono do rezerwowania linii radiowejpierwszym wysłanym bajtem i zwalniania jej znacznikiem końca ramki danych. W przy-padku kolizyjnej próby nadawania, tj. gdy dwa roboty rozpoczynają transmisję w tymsamym czasie, następuje odebranie danych przez nadającego robota co sygnalizuje błądi przerywa transmisję. Transmisja jednak nie jest kończona natychmiast. Robot nada-je jeszcze znak powrotu karetki i nowej linii CR,LF. Dzięki temu robot, który próbowałnadawać w czasie rezerwacji linii również odbierze dane w trakcie nadawania co spowoduje,że on również wykryje błąd transmisji. Pozostałe roboty natomiast wychwycą prawidło-wo zakończoną ramkę danych, którą odrzucą ze względu na niepoprawną długość. Tensposób komunikacji sprawdził się doskonale w opisywanym przypadku, głównie ze wzglę-du na mała ilość rzadko przesyłanych danych. Obserwowane przypadki kolizji zdarzałysię bardzo rzadko (na tyle, iż nie było możliwe okreslenie częstości ich pojawiania się).


Rysunek 6.5 Sposób poruszania się robotów zwiadowczych

6.2.6 Implementacja zadania

Zadanie postawione w rozdziale 2 polega na odnalezieniu przez robota zwiadowczegoobiektu, przekazaniu jego położenia do robota transportującego aby ten przewiózł przed-miot w zadane z góry miejsce. Aby roboty zwiadowcze przeszukiwały całą arenę wybranodla nich sposób poruszania przedstawiony na rysunku 6.5. Roboty transportowe natomiastporuszają się na zasadzie losowego wyboru kolejnego pola

6.3 Eksperymenty i analiza wyników

W celach porównawczych przeprowadzono eksperymenty z wykorzystaniem dwóch robo-tów: jednego transportowego i jednego zwiadowczego na dwóch arenach. Jedna miaławymiary 3×4 pola, natomiast druga była areną pełnowymiarową (8×10 pól). Na małejarenie roboty najczęściej z powodzeniem znajdowały i przywoziły obiekt znajdujący sięna jednym z pól. Niestety, nie udało się tego powtórzyć na dużej arenie ze względu na na-kładające się na siebie błędy w nawigacji robotów. Po pokonaniu areny (10 pól) wzdłużjednej osi robot tracił orientację 0 i mimo wprowadzania korekt położenia w trakcie jaz-dy nie był w stanie jej odzyskać. Na mniejszej arenie problem nie wystepował ze względuna szybsze odnajdywanie obiektu. Mimo tych problemów pozostałe aspekty współpracyrobotów, takie jak współdzielenie zasobów w postaci sceny, na której się one znajdują,unikanie kolizji oraz bezkolizyjna komunikacja, udało się w pełni zrealizować.Szerszej dyskusji wymagają błędy powstające w trakcie pomiarów przejechanej przez

roboty trasy. Główną trudność stanowi utrzymanie przez roboty zadanej orientacji. Wyni-ka to z faktu, że odometria z racji zasady pomiaru wprowadza błędy systematyczne, któredodatkowo w trakcie trwania eksperymentów sumowały się wprowadzając coraz większeodchylenia od wartości zadanej. Próby korygowania trajektorii omówione w punkcie 6.2.4nie przyniosły wystarczających rezultatów.Dobrym rozwiązanie tego problemu była by zmiana projektu płyty głównej robotów

polegająca na dodaniu kompasu elektronicznego. Dałoby to możliwość bezwzględnego po-miaru pomiaru orientacji robota. Dzięki temu możliwe było byłoby korygowanie trajektoriiw czasie rzeczywistym. Dodatkową zaletom kompasu jest brak nawarstwiania się błędusystematycznego (bezwzględny pomiar). Przykładem mogą być rozwiązanie prezentowanew [21] oraz [11].

Rozdział 7

Podsumowanie

Wynikiem pracy jest stanowisko przeznaczone do implementacji i badań algorytmówwspółpracy małych robotow mobilnych. Z jego wykorzystaniem można w prosty spo-sób prowadzić badania na obiektach rzeczywistych jak również modelu komputerowym.Dzięki wykorzystaniu środowiska Webots możliwe jest programowanie modeli robotów wjęzykach C, C++ lub Java. Wykorzystany w robotach kontroler z rodziny HCS08 pozwalana korzystanie z darmowego środowiska programistycznego i uruchomieniowego CodeWar-rior. Daje ono możliwość debugowania kodu nie tylko w symulatorze ale również w trakciewykonywania go przez kontroler, co znacznie przyspiesza wykrywanie błędów.Zbudowane w ramach pracy roboty posiadają prosta konstrukcję pozwalającą na dołą-

czanie dodatkowych modułów mechanicznych (jak np, chwytak w pracy) i elektronicznych.Konstrukcja mechaniczna przewiduje łatwość montażu i demontażu robotów (np. wy-korzystanie jednego typu śrub). Daje to możliwość prostej naprawy i łatwej wymianykomponentów pomiędzy robotami. Zastosowane wtyczki posiadają zatrzaski zapobiega-jące rozłączaniu się okablowania w trakcie eksperymentów. Również elektronikę sterującązaprojektowano w sposób modułowy tak aby możliwa była zmiana przeznaczenia robotapoprzez wymianę kilku elementów. Dodatkowo dzięki takiemu podejściu konstrukcyjnemuzmiana jednostki sterującej nie pociąga za sobą konieczności wymiany całej elektronikiw robocie.Stworzony w ramach pracy wirtualny model, będący uzupełnieniem stanowiska labora-

toryjnego, jest wiernym odwzorowaniem rzeczywistych obiektów nie tylko pod względemwizualnym ale również konstrukcyjnym. Oddane zostały w nim wszystkie ważniejsze szcze-góły konstrukcyjne mające wpływ na wyniki badań takie jak wymiary robotów, ich masa,sposób napędu oraz charakterystyki czujników i modułów radiowych. Dodatkowo wier-nie odwzorowano szczegóły wyglądu robotów dzięki czemu model jest bardzo atrakcyjnywizualnie.Wyniki pracy jak również powstałe stanowisko stanowić może podstawę do przygoto-

wania ćwiczeń laboratoryjnych dotyczących: programowania kontrolerów z rodziny HCS08w praktycznych i ciekawych dla studentów aplikacjach oraz zapoznania z programem We-bots jako środowiskiem pozwalającym na modelowania rzeczywistych układów robotycz-nych. W ramach kontynuacji pracy warto zaproponować rozbudowę stanowiska o systemwizyjny co da możliwość implementacji na nim algorytmów z centralnym sterowaniem,stworzenie oprogramowania do pozyskiwania i wizualizacji danych sensorycznych z robo-tów, oraz rozbudowę robotów o kolejne podzespoły (włącznie z dużą jednostką sterującą)zwiększajace ich funkcjonalność.

Bibliografia

[1] C. Cassandras and S. Lafortune. Introduction to discrete event systems. KluwerAcademic Publishers, 1999.

[2] Cyberbotics Ltd. Webots Reference Manual, 5.6.0 edition, 11 2007.

[3] Cyberbotics Ltd. Webots User Guide, 5.6.0 edition, 8 2007.

[4] Freescale Semiconductor. MC9S08GT60 Data Sheet, 1.00 edition, 08 2005.

[5] Freescale Semiconductor. Getting Started with HCS08 and CodeWarrior Using C, 2edition, 07 2006.

[6] Freescale Semiconductor. MC13211/212/213/214 Reference Manual, 1.1 edition, 102006.

[7] Freescale Semiconductor. MC1321x Evaluation Kit (EVK) Reference Manual, 1.1edition, 09 2006.

[8] Freescale Semiconductor. PCB Layout Guidelines for the MC1321x, 0.0 edition, 032006.

[9] Freescale Semiconductor. BeeKit Wireless Connectivity Toolkit User’s Guide, 1.5edition, 07 2008.

[10] Future Technology Devices International Ltd. FT232R USB UART I.C., 1.04 edition,2005.

[11] Honeywe. 1 AND 2-AXIS MAGNETIC SENSORS, a edition, 09 2002.

[12] R. Kuczaj. Interfejsy osbdm i tbdml. Technical report, Koło Naukowe Robotyków,Politechnika Wrocławska, 03 2008.

[13] MaxStream. USB xBee Interface Board, 05 2005.

[14] MaxStream. XBee/XBee-PRO OEM RF Modules, 1.06 edition, 10 2005.

[15] MaxStream. QRD1113,QRD1114 - Reflective Object Sensor, 1.1.0 edition, 1 2008.

[16] Zasady minisumo. http://pl.wikipedia.org/wiki/Minisumo.

[17] Odlewy z żywicy. http://www.old.modelarstwo.org.pl/forum/viewtopic.php?t=21659,05 2005.

[18] Z. S. Organization. Zigbee specification. Technical Report 053474r13, ZigBee Aliance,2006.

56 BIBLIOGRAFIA

[19] Parallax Inc. Boe-Bot Wheel, 11 2006.

[20] Parallax Inc. PING))) Ultrasonic Distance Sensor, 1.5 edition, 02 2008.

[21] Philips Semiconductors. Electronic Compass Design using KMZ51 and KMZ52, 032000.

[22] S. A. Reveliotis. Real-time management of resource allocation systems: A discreteevent systems approach. Springer NY, 2005.

[23] RibbonSoft, GmbH. Qcad Reference Manual, 2006.

[24] E. Roszkowska. DES-based coordination of space-sharing mobile robots. LNCS,4739D:1041–1048, 2007.

[25] E. Roszkowska. Decentralized motion-coordination policy for cooperative mobilerobots. 9th Int. Workshop on Discrete Event Systems, pages 364–369, 2008.

[26] Variable-speed continuous-rotation servo hack. http://www.philbot.com/projects/servo hack/.

[27] SHARP. Analog Output Type Distance Measuring Sensor GP12D120XJ00F Specyfi-cation, 05g061 edition, 03 2005.

[28] ST Microelectronics. NE555 SA555-SE555 General purpose single bipolar timers, 071998.

[29] K. Tchoń, A. Mazur, I. Dulęba, R. Hossa, and R. Muszyński. Manipulatory i robotymobilne. Akademicka Oficyna Wydawnicza PLJ, Warszawa, 2000.

[30] Texas Instruments. Small Size 2.4 GHz PCB antenna, swra117b edition, 06 2007.Application Note AN043.

[31] Vishay. IR Receiver Modules for Remote Control Systems, 5 edition, 07 2003.

[32] Łukasz Małek. Sterowanie robotem minisumo w aplikacji webots. Technical report,Instytut Informatyki, Automatyki i Robotyki, Politechnika Wrocławska, 2007.

Dodatek A

Schematy elektryczne płytekdrukowanych

II A. Schematy elektryczne płytek drukowanych

Rysunek

A.1Schem

atczujnika

koloru

III

RysunekA.2Schematpłytkimostkadlasilnikaserwomechanizmu

IV A. Schematy elektryczne płytek drukowanych

Rysunek

A.3Schem

atpłytki

koderamagnetycznego

V

RysunekA.4SchematpłytkimodułuzukłademMC13213

VI A. Schematy elektryczne płytek drukowanych

Rysunek

A.5Schem

atmodułu

konwertera

USB↔RS

VII

RysunekA.6SchematpłytkiewaluacyjnejdlamodułuxBee

VIII A. Schematy elektryczne płytek drukowanych

Rysunek

A.7Schem

atmodułu

zkontrolerem

HCS08G

T60A

IX

RysunekA.8Schematpłytkigłównejrobota

Dodatek B

Spisy elementów elektronicznychposzczególnych płytek

Tabela. B.1: Spis elementów czujnika koloru.

Symbol Wartość/Nazwa Obudowa Producent Dostawca Kod IlośćC1 100nF 0805 - Maritex 1R1 10kΩ 0805 - Maritex 1R2 220Ω 0805 - Maritex 1R3 1.2kΩ 0805 - Maritex 1Q1 BC807 0805 - Maritex 1OPT1 QRD1114 - Fairchild Semiconductors TME 1P1 nie montowany

Tabela. B.2: Spis elementów mostka dla silnika serwomechanizmu.

Symbol Wartość/Nazwa Obudowa Producent Dostawca Kod IlośćIC1 L293D DIL16 ST TME 1

P1,P2,P3,P4,P5 Taśma kablowaP6 Silnik serwomechanizmu

Tabela. B.3: Spis elementów płytki kodera magnetycznego.

Symbol Wartość/Nazwa Obudowa Producent Dostawca Kod IlośćC1 100nF 0805 - Maritex 1C1 10nF 0805 - Maritex 1U1 AS5040 SSOP16 Austria Microsystems Austria Microsystems 1P1 PCT10 2mm - Maritex 1

Tabela. B.4: Spis elementów płytki modułu z układem MC13213

Symbol Wartość/Nazwa Obudowa Producent Dostawca Kod IlośćC1,C4 100nF 0805 - Maritex 2

Tabela. B.5: Spis elementów modułu konwertera USB⇔RS.

Symbol Wartość/Nazwa Obudowa Producent Dostawca Kod IlośćC1 10nF 0805 - Maritex 1

C3,C4,C6 100nF 0805 - Maritex 3C5 1000nF 0805 - Maritex 1C7 100µF SMDB - Maritex 1D1 LED R 0805 - Maritex 1D2 LED G 0805 - Maritex 1R1 1kΩ 0805 - Maritex 1R2,R3 220Ω 0805 - Maritex 2CON1 USB-B THT - Maritex 1P1,P2 goldpin 8x1 100” - Maritex 2

http://maritex.com.pl





http://tme.pl

http://st.com

http://tme.pl



http://www.austriamicrosystems.com/

http://www.austriamicrosystems.com/













XII B. Spisy elementów elektronicznych poszczególnych płytek

Tabela. B.5 – ciąg dalszySymbol Wartość/Nazwa Obudowa Producent Dostawca Kod IlośćU1 FT232RL SSOP28 FTDI TME 1U2 LM3940 SOT223 National Semiconductors TME 1U3 SI4463 SO8 Vishay TME 1

Tabela. B.6: Spis elementów płytki ewaluacyjnej dla modułu xBee.

Symbol Wartość/Nazwa Obudowa Producent Dostawca Kod IlośćC1,C4 100nF 0805 - Maritex 2C2 1000nF 0805 - Maritex 1C3 100µF SMDB - Maritex 1

D1,D2,D3,D4,D5,D6,D7 LED G 3mm - Maritex 7D8 SCHOTKEY Minimelf - Maritex 1

R1,R2,R3,R4 220Ω 0805 - Maritex 4R5 10kΩ 0805 - Maritex 1R6 1kΩ 0805 - Maritex 1R7 100kΩ 0805 - Maritex 1

R8,R9,R10 470Ω 0805 - Maritex 3R11 68kΩ 0805 - Maritex 1R12,R13 56kΩ 0805 - Maritex 2R14 150kΩ 0805 - Maritex 1

RV1,RV2 10kΩ CA6 - Maritex 2SW1,SW2,SW3 TACT 5mm - Maritex 3P1,P5 golpin 1x10 100” - Maritex 2P2 goldpin 2x5 100” - Maritex 1P3 goldpin 2x5 100” - Maritex 1P4 goldpin 2x3 100” - Maritex 1P6 goldpin 2x5 100” - Maritex 1

M1 Moduł USB⇔RSgniazdo 1x8 100” - Maritex 2

U1 Moduł xBeegniazdo 1x10 2mm - Maritex 2

U4 SI4463 SO8 Vishay TME 1U5 LM3940 SOT223 National Semiconductors TME 1U6 LM339 SO14 - Maritex 1U7,U8 74LVC244 SOL20 - Maritex 1

Tabela. B.7: Spis elementów modułu z kontrolerem HCS08GT60A.

Symbol Wartość/Nazwa Obudowa Producent Dostawca Kod IlośćC1,C2,C3,C4,C5 100nF 0805 - Maritex 5

C6,C7 22pF 0805 - Maritex 2R1,R2 10kΩ 0805 - Maritex 2R3 1kΩ 0805 - Maritex 1FB1 EMI FILTER SMDB - Maritex 1P3 goldpin 2x3 100” - Maritex 1P4,P5 gniazdo 2x10 100” - Maritex 1SW1 TACT 5mm - Maritex 1X1 7.3728MHz HC49S - Maritex 1U1 HC9S08GT60A - Freescale Freescale 1

M1 Moduł USB⇔RSgolpin kątowy SMD 1x8 100” - Maritex 2

Tabela. B.8: Spis elementów płytki głównej robota.

Symbol Wartość/Nazwa Obudowa Producent Dostawca Kod IlośćC1 1000nF 0805 - Maritex 1C2 33µF SMDB - Maritex 1C3,C4 4.7µF 0805 - Maritex 2C5,C6 100nF 0805 - Maritex 1C7 10nF 0805 - Maritex 1

D1,D2,D3 LED G 0805 - Maritex 3D4 IR LED 3mm - Maritex 1

R1,R2,R5 470Ω 0805 - Maritex 3R3,R4,R13,14 10kΩ 0805 - Maritex 4R6,R15 1kΩ 0805 - Maritex 2

http://ftdichip.com

http://tme.pl

http://www.national.com

http://tme.pl

http://www.vishay.com

http://tme.pl
























http://tme.pl


http://tme.pl























XIII

Tabela. B.8 – ciąg dalszySymbol Wartość/Nazwa Obudowa Producent Dostawca Kod IlośćR7,R16 22kΩ 0805 - Maritex 2R8,R9 100Ω 0805 - Maritex 2R10 220Ω 0805 - Maritex 1R12 560Ω 0805 - Maritex 1SP1 BUZZER 9mm - TME 1SW1 SWITCH 100” - Maritex 1Q1,Q2 BC817 SOT23 - Maritex 1U1 LM3940 SOT223 National Semiconductors TME 1U2 24C16 SO8 National Semiconductors TME 1

U3 Moduł xBeegniazdo 1x10 2mm - Maritex 2

U4 Moduł z HC9So8GT60Agoldpin 2x10 100” - Maritex 2

U5,U6 TSOP34838 Vishay TME 2U7 NE555 SO8 National Semiconductors Maritex 1

K1,K3,K4,K6 gniazdo CRIMP 3 100” - Maritex 4K2 PING-:))) 100” Parallax Parallax 1K5 gniazdo CRIMP 2 100” - Maritex 1P1,P2 gniazdo CRIMP 4 100” - Maritex 1P3,P4 gniazdo CRIMP 5 100” - Maritex 1P6,P7 IDC10 kątowe 2mm - Maritex 1





http://tme.pl




http://tme.pl


http://tme.pl




http://tme.pl




http://parallax.com

http://parallax.com





Dodatek C

Rysunki montażowe płytekdrukowanych

(a) strona elementów (b) strona ścieżek

Rysunek C.1 Rysunek montażowy czujnika koloru


Rysunek C.2 Rysunek montażowy płytki mostka dla silnika serwomechanizmu

XVI C. Rysunki montażowe płytek drukowanych


Rysunek C.3 Rysunek montażowy płytki kodera magnetycznego


Rysunek C.4 Rysunek montażowy płytki modułu z układem MC13213


Rysunek C.5 Rysunek montażowy modułu konwertera USB↔RS

XVII

(a) strona elementów

(b) strona ścieżek

Rysunek C.6 Rysunek montażowy płytki ewaluacyjnej dla modułu xBee

XVIII C. Rysunki montażowe płytek drukowanych


Rysunek C.7 Rysunek montażowy modułu z kontrolerem HCS08GT60A

XIX

(a) strona elementów

(b) strona ścieżek

Rysunek C.8 Rysunek montażowy płytki głównej robota

Dodatek D

Rysunki elementów mechanicznych

Rysunek D.1 Rama robota

XXII D. Rysunki elementów mechanicznych

Rysunek D.2 Sposób wygięcia ramy robota

XXIII

Rysunek D.3 Pług robotów

Rysunek D.4 Koła robota

Dodatek E

Zawartość dołączonej płyty CD

Praca dyplomowa

praca dyplomowa.pdf – wersja elektroniczna pracy dyplomowej

Kody źródłowe

Webots – katalog z projektem modelu stanowiska laboratoryjnego w środowisku We-bots 5.10

CodeWarrior – katalog zawierający pliki źródłowe oprogramowania robotów dla śro-dowiska CodeWarrior

Projekty PCB – projekty płytek drukowanych z programu KiCad

wls – projekt czujnika białej linii

servo – projekt zamiennej płytki do serwomechanizmu

servo enkoder – projekt płytki enkodera magnetycznego

mc13213 – projekt płytki modułu z układem MC13213

usbrs – projekt konwertera USB↔RS

xbee usb dev – projekt płytki ewaluacyjnej dla modułu xBee

hcs08gt60mm – projekt moduły z kontrolerem HC9S08GT60A

glownav1 – projekt płytki głównej robota

Rysunki techniczne

rama1.dxf – rysunek wykonawczy ramy robota

rama2.dxf – sposób wygięcia ramy robota

koło.dxf – rysunek koła robota

plug.dxf – rysunek wykonawczy pługu robota

POLITECHNIKA WROCŁAWSKA WYDZIAŁ...

Documents

Transcript of POLITECHNIKA WROCŁAWSKA WYDZIAŁ...