Opracowanie i implementacja zdecentralizowanej struktury sterowania zespoªem heterogenicznch robotów mobilnych

Opracowanie i implementacja zdecentralizowanej struktury sterowania zespoªem heterogenicznch robotów mobilnych Wojciech Szynkiewicz, Andrzej Rydzewski, Marek Majchrowski, Piotr Trojanek, Cezary Zieli«ski Sprawozdanie z wykonania 1-ego etapu prac w ramach grantu MEiN nr 3T11C38 29 Raport Instytutu Automatyki i Informatyki Stosowanej nr 6-6 Warszawa, Czerwiec 26 Politechnika Warszawska Instytut Automatyki i Informatyki Stosowanej ul. Nowowiejska 15/19 665 Warszawa tel/fax (48) 22 825 37 19

Spis tre±ci 1 Ogólna struktura ukªadu sterowania zespoªem heterogenicznych robotów mobilnych 3 1.1 Systemy wielorobotowe............................. 3 1.2 Architektury sterowania robotów mobilnych................. 6 1.3 Funkcje sterownika robota mobilnego..................... 7 1.3.1 Opis sterownika robota mobilnego................... 9 2 Adaptacja warstwy sprz towej sterowników robotów do zada«wymagaj cych wspóªpracy robotów 11 3 Opracowanie i implementacja algorytmu planowania ±cie»ek ruchu dla zespoªu robotów mobilnych 15 3.1 Metody i algorytmy unikania kolizji...................... 16 3.2 Algorytm krzywizn i pr dko±ci CVM..................... 17 3.3 Szczegóªowy opis algorytmu.......................... 2 3.3.1 Równanie kwadratowe wzgl dem zmiennej x............. 23 3.3.2 Równanie kwadratowe wzgl dem zmiennej y............. 24 3.4 Szczegóªowy opis dziaªania programu..................... 25 3.5 Wyniki eksperymentalne............................ 28 3.5.1 rodowisko pomiarowe 1........................ 29 3.5.2 rodowisko pomiarowe 2........................ 29 3.6 Wnioski..................................... 29 4 Algorytm wspóªpracy zespoªu robotów w zadaniu wspólnego przemieszczania obiektu 34 4.1 Koncepcja sterowania behawioralnego..................... 34 4.2 Sterowanie behawioralne zespoªem robotów.................. 35 4.3 Przykªad zadania wielorobotowego...................... 35 4.4 Opis zadania.................................. 36 1

SPIS TRE CI 2 5 Opracowanie i implementacja algorytmów sterowania behawioralnego zespoªem heterogenicznych robotów mobilnych 38 5.1 Implementacja.................................. 38 5.2 Realizacja zadania na symulatorze....................... 4 5.3 Eksperymenty.................................. 4

Rozdziaª 1 Ogólna struktura ukªadu sterowania zespoªem heterogenicznych robotów mobilnych 1.1 Systemy wielorobotowe Zagadnienia dotycz ce systemów wielorobotowych zwanych te» wieloagentowymi (tutaj zwanymi agentami upostaciowionymi) jest obecnie w robotyce jednym z intensywnie rozwijanych obszarów badawczych [13,17,21,39,55]. Szczególnie intensywne badania s prowadzone w obszarze robotyki mobilnej [9,1,11,2,22,24,25,32,35,49]. Je±li wiele robotów porusza si w tej samej przestrzeni roboczej ich ruchy musz by koordynowane (harmonizowane, je±li nie ma nadrz dnego koordynatora) w celu unikni cia wzajemnych kolizji i zakleszcze«. Zªo»ono± problemu planowania dziaªa«i sterowania robotami zale»y od bardzo wielu czynników m.in. liczby i typu robotów, stopnia ich wspóªpracy, rodzaju ±rodowiska (statyczne/dynamiczne, znane/nieznane) itd. [2,1,17]. Zastosowanie systemu wielorobotowego rozumianego zazwyczaj jak zespóª wielu wspóªpracuj cych robotów stwarza mo»liwo±ci wykonania zada«, które s niewykonalne dla pojedynczego robota [1, 17]. Jednym z cz ±ciej podawanych takich przykªadów jest zadanie wymagaj ce obecno±ci robota w dwóch oddalonych miejscach w tej samej chwili czasu [18]. W grupach skªadaj cych si z ró»nych robotów mo»na wykorzysta mo»liwo±ci bardziej zaawansowanych robotów np. wyposa»onych w precyzyjne czujniki np. dalmierze laserowe do wspomagania prostszych robotów przy samo-lokalizacji i nawigacji w naturalnym ±rodowisku [9,14,28,42,51]. Wykorzystanie zespoªu robotów do wykonania ró»norodnych zada«powoduje nie tylko ilo±ciowy wzrost wydajno±ci, lecz równie» zamian jako±ciow polegaj c na istotnym poszerzeniu klasy potencjalnych zastosowa«. Odnosi si to w szczególno±ci do wszelkiego rodzaju robotów mobilnych (w tym manipulatorów mobilnych, pojazdów autonomicznych, maszyn krocz cych) wykorzystywanych w zadaniach inspekcyjnych, transportowych, usªugowych, itp. Jest to szczególnie istotne w systemach heterogenicznych, gdzie roboty ró»ni si mi dzy sob nie tylko budow, ale tak»e funkcjami, które mog wykona [35]. W±ród podstawowych powodów tworzenia systemów wielorobotowych wymienia si [2,1,18,21,39,29] : mo»liwo± wykonania zada«zbyt trudnych (lub wr cz niewykonalnych) dla pojedynczego robota oraz znacz cy wzrost wydajno±ci; wykorzystanie ró»nych funkcjonalno±ci (umiej tno±ci) robotów wynikaj cych z ró»- 3

1.1. SYSTEMY WIELOROBOTOWE 4 nej budowy i wyposa»enia (efektory, czujniki); zbudowanie i wykorzystanie kilku prostych robotów mo»e by ªatwiejsze ni» stosowanie jednego uniwersalnego, ale za to bardzo skomplikowanego robota do realizacji ró»norodnych zada«; zwi kszenie odporno±ci i niezawodno±ci (roboty mog nawzajem siebie zast powa, je±li jeden lub wi cej z nich ulegnie awarii); W literaturze podaje si kilka podstawowych grup zada«(zada«testowych) dla wspóªpracuj cych robotów mobilnych [1,17,21]: Kierowanie ruchem: Wiele robotów porusza si we wspólnym ±rodowisku i unika wzajemnych kolizji. Planowanie ruchu mo»e by rozpatrywane jako problem kon- iktu dost pu do zasobu, który mo»e by rozwi zywany przez ustalenie zasad ruchu (kodeks), priorytetów, struktur komunikacyjnych [22]. Pchanie pudªa/kooperatywna manipulacja oraz transportowanie obiektów: Zadanie polega na manipulowaniu du»ymi obiektami wspólnie przez kilka robotów np. pchaniu du»ego pudªa [11,41,58]. Pchanie pudªa zazwyczaj odbywa si na pªaszczy¹nie, podczas gdy transport obiektów wymaga podnoszenia i przenoszenia przedmiotów zespoªowo przez kilka robotów co istotnie zwi ksza trudno± zadania. Wspólne transportowanie przedmiotów jest przykªadem zadania testowego dla takich zagadnie«jak koordynacja ruchów oraz utrzymanie ±cisªego szyku (formacji) robotów [27,31,34,5,56]. Przeszukiwanie i pokrycie obszaru: Grupa robotów poszukuje w ±rodowisku okre±lonych obiektów. Mo»na wymieni wiele przykªadów tego typu zada«m.in. sprz tanie, poszukiwanie min, zbieranie owoców/warzyw [35]. Mo»e by ono wykonywane przez ka»dego robota niezale»nie albo roboty wspóªpracuj przy jego wykonaniu (jest to sprawdzian, czy ewentualna wspóªpraca robotów mo»e przyspieszy realizacj zadania). Pokrycie obszaru wymaga efektywnych sposobów sprawdzenia caªego obszaru przez grup robotów [1,13]. Typowymi przykªadami zadania pokrycia s malowanie du»ych powierzchni, koszenie trawników, od±nie»anie, rozminowanie terenu, itp. Obserwacja wielu celów: Zadanie to wymaga dziaªania grupy robotów w celu wykrycia i ±ledzenia wielu poruszaj cych si obiektów (celów) [8, 15]. Typowymi aplikacjami s tutaj systemy bezpiecze«stwa, w których wymagany jest nadzór i ±ledzenie ruchomych obiektów. Poza grup robotów mobilnych, w tego typu zastosowaniach wykorzystuje si sieci czujników. Zazwyczaj takie czujniki charakteryzuj si ma- ªymi mo»liwo±ciami obliczeniowymi i niewielkim zasi giem i ich wspóªdziaªanie z ruchomymi robotami mo»e istotnie poszerza mo»liwo±ci caªego systemu [14,15]. Piªka no»na robotów: Jest przykªadem bardzo wymagaj cego zadania testowego dla dru»yny robotów, gdzie jest wymagane planowanie i sterowanie ruchem robotów w czasie rzeczywistym, w ±rodowisku zawieraj cym wiele ruchomych przeszkód (innych zawodników). Rozwi zania wymagaj takie problemy podziaª zada«i planowanie dziaªa«grupowych, takich jak strategia gry zespoªu, mo»liwo± peªnienia ró»nych ról przez tego samego zawodnika [5,55].

1.1. SYSTEMY WIELOROBOTOWE 5 Stosunkowo nowym, ale potencjalnie bardzo obiecuj cym, obszarem zastosowa«zespoªów robotów (w szczególno±ci robotów autonomicznych) s szeroko rozumiane zadania usªugowe. Dla szerokiej klasy zada«usªugowych zastosowanie kilku robotów wspóªpracuj cych mo»e przyspieszy wykonanie lub by wr cz niezb dne do realizacji zadania [6]. Roboty usªugowe zazwyczaj dziaªaj w niestrukturalnym ±rodowisku dynamicznym, gdzie oprócz robotów mog by inne ruchome obiekty np. ludzie, których trajektorie ruchu s zazwyczaj nieznane i nieprzewidywalne. W wielu zadaniach usªugowych konieczny jest bezpo±redni kontakt robotów z czªowiekiem. W takiej sytuacji planowanie dziaªa«/ruchu robota czy te» zespoªu robotów jest bardzo zªo»onym problemem, którego rozwi zanie wymaga stosowania zaawansowanych metod i algorytmów. Generalnie, planowanie dziaªa«i sterowanie zespoªem robotów jest znacznie bardziej zªo-»one ni» planowanie ruchu jednego robota i tylko nieliczne metody opracowane dla pojedynczego robota mo»na uogólni na przypadek wielorobotowy. Przykªadowo, dla grupy ±ci±le wspóªpracuj cych robotów (wymagana jest ±cisªa koordynacja dziaªa«/ruchów robotów) ju» znalezienie dopuszczalnych trajektorii ruchu zespoªu robotów jest bardzo trudnym zadaniem. Klasyczne podej±cie z planowaniem trajektorii ruchu i pó¹niejszym jej odtwarzaniem jest dla robotów autonomicznych dziaªaj cych w nieznanym otoczeniu caªkowicie nieprzydatne. Stosowane s za to z pewnym powodzeniem metody maj ce inspiracje biologiczne (sterowanie behawioralne, reaktywne) oraz w zakresie uczenia metody z obszaru sztucznej inteligencji np. systemy klasykuj ce, uczenie ze wzmacnianiem, Q-uczenie, itp. Metody te s równie» stosowane do sterowania grupami robotów. Inspiracj dla tych podej± s przykªady zaczerpni te z przyrody takie jak stada ptaków, ªawice ryb czy te» mrowiska lub ule. Do±wiadczenia eksperymentalne pokazuj,»e wiele prostych robotów o niskim poziomie inteligencji maszynowej mo»e ze sob wspóªpracowa w celu realizacji nawet zªo»onego zadania [2,4,31,35]. Przy planowaniu dziaªa«robotów mobilnych kluczowym zagadnieniem staje si wykorzystanie pochodz cej z ró»norodnych czujników informacji o ±rodowisku, która zwykle jest cz stkowa, obarczona niepewno±ci, a czasami bª dna [8,9,14,43,54]. W zale»no±ci od rodzaju jak i zastosowa«robota mo»e on by wyposa»ony w czujniki dotykowe, zbli»eniowe (indukcyjne i na podczerwie«), siª i momentów, ultrad¹wi kowe, wizyjne, globalnego po- ªo»enia (GPS, DGPS, kompasy,»yroskopy). Efektywne wykorzystanie informacji z wielu ró»norodnych czujników do planowania ruchu i sterowania robotów w czasie rzeczywistym jest otwartym problemem badawczym [17, 21]. Rozwi zania wymagaj m.in. takie problemy jak planowanie ruchu robotów dziaªaj cych we wspólnej przestrzeni roboczej, komunikacja mi dzy robotami, harmonizacja dziaªa«wielu robotów wykonuj cych wspólne zadanie i nadzór na systemem [9,14,17,36]. Jest to szczególnie zªo»ony problem, gdy nie istnieje jednostka nadrz dna koordynator [22,27,41,46]. W literaturze podawane s ró»ne denicje systemów wielorobotowych [1,18,39]. O systemie wielorobotowym mo»emy ju» mówi, je±li mamy do czynienia z co najmniej dwoma robotami i wyst puj interakcje mi dzy nimi wynikaj ce zazwyczaj z dziaªania we wspólnej przestrzeni roboczej. Bardzo cz sto deniuj c system wielorobotowy przyjmuje si,»e jest mo»liwa (przynajmniej potencjalnie) wspóªpraca robotów. O tym czy roboty b d w stanie wykona wspólnie cho by najprostsz czynno± decyduj mo»liwo±ci ich ukªadów sterowania [2,4,22,6].

1.2. ARCHITEKTURY STEROWANIA ROBOTÓW MOBILNYCH 6 1.2 Architektury sterowania robotów mobilnych Mo»na wyró»ni dwie podstawowe cechy, które charakteryzuj ka»dy system wielorobotowy. S to struktura organizacyjna i sposób komunikacji [1,17]. Pierwsza okre±la struktur organizowania wspóªpracy robotów, druga za± sposób wymiany informacji mi dzy robotami. Ze wzgl du na struktur mo»na wyró»ni dwa gªówne podej±cia do problemu planowania ruchu oraz sterowania zespoªem robotów: Systemy scentralizowane o strukturze globalnej, w których zadania do wykonania oraz sposób wspóªpracy jest narzucony robotom przez jednostk nadrz dn (koordynatora) lub operatora [2,58]. Mamy tutaj zatem struktur hierarchiczn. Systemy rozproszone o strukturze lokalnej, w których nie ma»adnej jednostki centralnej, za± mo»liwo±ci wspóªpracy wynikaj z reguª wbudowanych poszczególnym robotom [4,46,49]. Struktura scentralizowana nie zawsze jest odpowiednim rozwi zaniem. Wyznaczenie optymalnych planów jest zazwyczaj bardzo kosztowne ze wzgl du na nakªad oblicze«. Ogranicza to liczebno± zespoªu, dla którego jednostka nadrz dna jest w stanie obliczy taki plan w wymaganym czasie. Warunkiem koniecznym poprawnego dziaªania caªo±ci jest zapewnienie szybkich i pojemnych ª czy komunikacyjnych mi dzy koordynatorem a pozostaªymi robotami. W przypadku utraty ª czno±ci lub awarii koordynatora caªy system przestaje dziaªa. Dla robotów o du»ym stopniu autonomiczno±ci, które dziaªaj w maªo znanym lub nieznanym otoczeniu zazwyczaj lepszym rozwi zaniem jest podej±cie zdecentralizowane. Nie ma tutaj jednostki nadrz dnej, a poszczególne roboty komunikuj si bezpo±rednio mi dzy sob, je±li jest to konieczne. Ka»dy robot dziaªa w du»ym stopniu niezale»nie i planuje swe dziaªania korzystaj c z informacji o lokalnym otoczeniu z wªasnych czujników. Umo»liwia to szybk reakcj na zmiany w otoczeniu i zmniejsza wymagania komunikacyjne z innymi robotami. Uszkodzenie pojedynczego robota nie powoduje awarii caªego systemu. System jako caªo± jest bardziej odporny i znacznie bardziej elastyczny ze wzgl du na modykacje struktury. Podstawow wad podej±cia caªkowicie zdecentralizowanego jest to, i» planowanie na podstawie lokalnej informacji mo»e prowadzi do caªkowicie nieoptymalnych rozwi za«. W rezultacie zadanie mo»e by wykonywane przez zespóª robotów w sposób wysoce nieefektywny lub w skrajnym przypadku mo»e okaza si niewykonalne. Do implementacji struktury zdecentralizowanej najcz ±ciej stosuje si sterowanie behawioralne [2, 4]. Sterowanie jest podzielone na zbiór celowych dziaªa«zachowa«, które mog wykonywa si równolegle [2, 59]. Korzystaj c z selektywnej informacji uzyskiwanej z czujników ka»de takie zachowanie generuje sterowanie dla robota zgodnie z lokaln funkcj celu. Zachowania z ró»nymi i prawdopodobnie niezgodnymi funkcjami celu mog powodowa koniktowe dziaªania (akcje), które pozornie s nie do pogodzenia. Konieczne jest wi c stworzenie efektywnych mechanizmów koordynacji zachowa«. Wykorzystuje si tutaj dwie klasy mechanizmów koordynacji podej±cia bazuj ce na arbitra»u oraz ª czeniu (fuzji) zlece«od ró»nych zachowa«tworz cych sterowanie, które jest wynikiem uzgodnienia np. przez gªosowanie. Wybór odpowiedniej struktury sterowania nadrz dnego dla systemu wielorobotowego zale»y od kilku czynników takich jak: zadanie jakie roboty maj do wykonania, od rodzaju robotów i stopnia ich autonomiczno±ci oraz ±rodowiska w

1.3. FUNKCJE STEROWNIKA ROBOTA MOBILNEGO 7 którym one dziaªaj. W wielu praktycznych rozwi zaniach proponuje si struktury hybrydowe, które próbuj ª czy pozytywne cechy obu powy»szych podej± [2]. Wyró»nia si dwa podstawowe sposoby komunikacji: jawna gdy poszczególne jednostki przesyªaj do siebie informacje o swoim stanie, niejawna nie ma»adnej wymiany bezpo±redniej wymiany informacji, wiedza o innych robotach jest uzyskiwana na podstawie obserwacji otoczenia [31]. W przypadku jawnej komunikacji rozwa»ane s takie problemy jak: rodzaj i struktura komunikacji, zasi g, przepustowo± ª czy, itp. [18, 17, 45]. Obecnie du»e zainteresowanie jest zwi zane z tzw. sieciami robotów i czujników, gdzie komunikacja jest jednym z podstawowych problemów badawczych [1,14]. Jednym z istotnych problemów badawczych jest zapewnienie komunikacji pomi dzy agentami wyposa»onymi w bezprzewodowe urz dzenia sieciowe, bez wykorzystywania zewn trznej infrastruktury sieciowej (poª czenie agentów sieci ad-hoc). Dost pne s dwa rodzaje urz dze«do komunikacji bezprzewodowej: wykorzystuj ce technologi Bluetooth oraz Wi- Fi. Poszczególni agenci poruszaj c si w ±rodowisku wykonuj zadania, komunikuj c si ze sob droga radiow. Rozwa»ony zostanie zarówno wariant wykorzystuj cy zewn trzn infrastruktur sieciow, jak i wariant bez takiej struktury, w zwi zku z tym komunikacja pomi dzy w zªami, które nie znajduj si w bezpo±rednim zasi gu b dzie odbywa si za po±rednictwem innych w zªów. Mobilno± agentów implikuje dynamicznie zmieniaj c si topologi sieci i konieczno± stosowania specjalizowanych mechanizmów komunikacji. Charakterystyka urz dze«nadawczo-odbiorczych zainstalowanych na agentach wymusza zastosowanie algorytmów zarz dzania topologi z rodziny tzw. neighbor-based. Innym intensywnie rozwa»anym problemem jest wykorzystanie Internetu do komunikacji mi dzy robotami oraz zdalnego sterowania zespoªem robotów [2,38]. Innym wa»nym zagadnieniem dotycz cym systemów wielorobotowych jest problem przydziaªu zada«poszczególnym jednostkom [24, 25, 4, 6]. Wyró»nia si dwa podstawowe podej±cia do problemu przydziaªu zada«w systemach rozproszonych bez koordynatora. Pierwsze z nich wykorzystuje prawa rynkowe i polega na negocjacjach roboty negocjuj ze sob i podejmuj kontrakty na wykonanie okre±lonych zada«[6]. Drugie polega na modelowaniu zdolno±ci robotów roboty wykorzystuj modele opisuj ce mo»liwo±ci wykonania zada«przez inne roboty do okre±lenia swoich motywacji do wykonania konkretnych zada«[25]. 1.3 Funkcje sterownika robota mobilnego Podstawowym zaªo»eniem przy projektowaniu ukªadów sterowania tych robotów byªo przyj cie moduªowej struktury zarówno cz ±ci sprz towej jak i programowej sterowników, dzi ki temu, w zale»no±ci od przyszªych potrzeb, ich rozbudowa nie b dzie nastr czaªa wi kszych problemów. Moduªy skªadaj ce si na system powinny mie dobrze zdeniowane interfejsy, metody komunikacji i ograniczenia jakim podlegaj. Architektura sprz towa i programowa sterownika ma hierarchiczn struktur warstwow. Warstwa wykonawcza

1.3. FUNKCJE STEROWNIKA ROBOTA MOBILNEGO 8 realizuje bezpo±redni obsªug sprz tu, w tym sterowania silnikami oraz obsªug czujników. Warstwa decyzyjna realizuje zadania wy»szego poziomu m.in. planowania dziaªa«, nawigacji robotem oraz komunikacji mi dzy robotami. Zaªo»ono,»e ukªad sterowania powinien umo»liwia autonomiczne dziaªanie robota. Przy sterowaniu robotami mobilnymi kluczowym zagadnieniem staje si wykorzystanie pochodz cej z ró»norodnych czujników informacji o ±rodowisku, która zwykle jest cz stkowa, obarczona niepewno±ci, a czasami bª dna. W zale»no±ci od rodzaju jak i zastosowa«robota mo»e on by wyposa»ony w czujniki dotykowe, zbli»eniowe (np. na podczerwie«), ultrad¹wi kowe, wizyjne, globalnego poªo»enia (GPS, DGPS, kompasy,»yroskopy). Efektywne wykorzystanie informacji z wielu ró»norodnych czujników do planowania ruchu i sterowania robotów w czasie rzeczywistym jest otwartym problemem badawczym. Do podstawowych funkcji sterownika zaliczamy realizacj nast puj cych zada«: lokomocj sterowanie nap dami (serwomechanizmy osi); przeliczniki kinematyki (proste i odwrotne zadanie kinematyki), podstawow obsªug czujników pomiarowych (inicjalizacja, odczyt i ewentualnie wst pne przetwarzanie danych pomiarowych); wykrywanie i unikanie kolizji (przy wykorzystaniu danych z czujników dotykowych, zbli»eniowych, itp.); samo-lokalizacj okre±lenie pozycji robota (ró»ne metody wykorzystuj ce dane z czujników odometrycznych, dalmierza laserowego, kamera wizyjnej); planowanie i generacja trajektorii ruchu (w tym rozwi zywanie zada«kinematyki); budow modelu otoczenia robota (mapy); komunikacj wewn trzn (transmisja równolegªa i/lub szeregowa synchroniczna przez specjalizowane ª cza), zewn trzn (radiowa sie Ethernet). Struktury danych dla robotów mobilnych obejmuj : stan robota (pozycja, pr dko±, typ, identykator, itp.) opis ruchu robota (±cie»ka ruchu) stan ±rodowiska (reprezentacja mapy otoczenia) dane z pomiarami czujników (odczyty z skanera laserowego, obrazy w postaci bitmapy, itp.) Ogóln struktur funkcjonaln oprogramowania autonomicznego robota mobilnego pokazano na rys.1.1

1.3. FUNKCJE STEROWNIKA ROBOTA MOBILNEGO 9 Baza Wiedzy Komunikacja WLAN, Bluetooth Polecenia zadania Jednoczesna lokalizacja i mapowanie (SLAM) Jednoczesne planowanie, lokalizacja i mapowanie (SPLAM) Pozycja robota Globalna mapa Poznawanie Planowanie œcie ki Model œrodowiska Lokalna mapa Œcie ka Percepja Ekstrakcja/agregacja informacji Dane z czujników Wyczuwanie Œrodowisko Wykonanie œcie ki Polecenia dla silników Dzia³anie Sterowanie ruchem Rysunek 1.1: Struktura funkcjonalna oprogramowania sterownika robota 1.3.1 Opis sterownika robota mobilnego Do sterowania robotem wykorzystana zostaªa struktura Player/Stage. Ogólny schemat struktury sterowania z wykorzystaniem Playera przedstawia rysunek 1.2. W jego centrum znajduje si wielow tkowy proces Playera. Na dole rysunku znajduj si rzeczywiste urz dzenia, natomiast na samej górze znajduj si klienci. Ka»dy klient poª czony jest z serwerem Playera poprzez gniazda TCP. W przypadku gdy klient jest uruchomiony na tej samej maszynie co Player, wtedy poª czenie jest po interfejsie lokalnym (loopback). W przeciwnym przypadku jest to zyczne poª czenie przez sie. Z drugiej strony Player ª czy si z urz dzeniami, poprzez swoje sterowniki, zazwyczaj poprzez ª cze RS232, cho w przypadku niektórych sterowników (jak festival) poª czenie to jest nawi zywane tak»e poprzez sie TCP. W przypadku sterownika robota mobilnego Electron poª czenie to jest nawi zywane z moduªem j dra Linuxa, który dalej obsªuguje ju» dane» danie. Wewn trz Playera ró»ne w tki komunikuj si wykorzystuj c globaln przestrze«adresow. Ka»de urz dzenie jest powi zane z buforem komend i buforem danych. Dost p do tych buforów wykorzystuje mechanizm wzajemnego wykluczania. W ten sposób zapewniony jest asynchroniczny kanaª komunikacyjny pomi dzy w tkami sterowników urz dze«a w tkami serwera odpowiedzialnymi za odbieranie oraz udost pnianie danych programom klienckim.

1.3. FUNKCJE STEROWNIKA ROBOTA MOBILNEGO 1 Rysunek 1.2: Struktura oprogramownia robota Integracja robota z tym oprogramowaniem wymagaªa stworzenia sterowników (drivers) implementuj cych interfejsy ju» wcze±niej zdeniowane w systemie zapewniaj ce komunikacj z ukªadem nap du (sterowanie pr dko±ciowe i pozycyjne) i czujnikami odlegªo- ±ci, sterowanie serwomechanizmami modelarskimi oraz monitorowanie stanu naªadowania akumulatorów w trakcie pracy robota.

Rozdziaª 2 Adaptacja warstwy sprz towej sterowników robotów do zada«wymagaj cych wspóªpracy robotów Robot mobilny Elektron R1 jest urz dzeniem autonomicznym, zdolnym do samodzielnego poruszania si w nieznanym terenie i do komunikowania si z innymi robotami lub komputerem zewn trznym. W zwi zku z tym jest on wyposa»ony w komputer steruj cy o odpowiednio du»ej mocy obliczeniowej, zdolny do realizacji zada«zwi zanych z odometri, rozpoznawaniem terenu, nawigacj itp. Robot jest przeznaczony do realizacji prac naukowobadawczych oraz do dydaktyki. Jest wyposa»any w ró»nego rodzaju czujniki i urz dzenia do nawigacji (np. dalmierz laserowy, kamera), przy czym wyposa»enie poszczególnych egzemplarzy robota jest ró»ne. Dlatego ukªad sterowania robota ma struktur otwart, jest ªatwy do rozbudowy i modykacji w celu przystosowania go do danej konguracji wyposa»enia robota. Schemat blokowy ukªadu sterowania jest pokazany na rys. 2.1. Podstawowym elementem ukªadu jest gªówny komputer pokªadowy, który zarz dza praca caªego systemu, a tak»e realizuje wszystkie funkcje sterowania wysokiego poziomu. Wykorzystano tu jednopªytkowy komputer PCM9579 EBX rmy Advantech. Jest to komputer z procesorem Celeron lub Pentium, przeznaczony do wbudowania (embedded), wyposa»ony we wszystkie typowe elementy komputera klasy PC (graka, d¹wi k, Ethernet, sterowniki nap dów dyskowych, portu równolegªy i szeregowy RS232, magistrala PCI), a tak»e w elementy dodatkowe, typowe dla komputerów wbudowanych (magistrale PC14 i PC14+, zª cze RS485, zª cze pami ci compact ash pracuj cej w trybie IDE). Magistrale PC14 i PC14+ (standard dla komputerów wbudowanych) umo»liwiaj ªatw rozbudow i modykacj ukªadu sterowania, przez dokªadanie kart rozszerze«zarówno typowych, fabrycznych, jak i prototypowych, wªasnych. Komputer pracuje pod kontrol systemu operacyjnego Linux w dystrybucji Gentoo. W konguracji bazowej robota, do magistrali PC14+ jest przyª czona bezprzewodowa karta sieciowa WIB23 rmy Elcard Oy. Jest to karta zgodna z standardem Ethernet 82.11b i 82.11g. Umo»liwia ona transmisj danych z szybko±ci od 6 do 54 Mb/s (z automatycznym wykrywaniem szybko±ci). Umo»liwia ona dost p do sieci LAN za po±rednictwem punktów dost powych (access point), a tak»e na tworzenie sieci adhoc). Karta jest wykorzystywana do szybkiej komunikacji robota z otoczeniem komputerami zewn trznymi, a tak»e innymi robotami. W ukªadzie sterowania robota wyposa»onego w kamer wysokiej jako±ci, do magistrali PC14+ jest przyª czona karta akwizycji obrazy, a w ukªadach innych robotów mog by wykorzystywane inne specjalizowane karty zgodnie z potrzebami. Robot jest nap - 11

Główny komputer sterujący PCM-9579 EBX (wbudowany, klasy PC) PC-14+ PC-14 Karta akwizycji obrazu PWM A, B pomiarowy binarne 2... Mikrokomputer Mikrokomputer RS-485sterujący pomiarowy Mikrokomputer odległości Czujniki Czujniki...... Wyświetlacz Klawiatura LCD 5V12V24V Zasilanie 22 Przetwornice Przekaźniki akumulatorów Bateria 24V Wzmacniacze mocy Silniki Enkodery (na kołach) Karta WLAN Rysunek 2.1: Struktura ukªadu sterowania dzany dwoma silnikami pr du staªego (o napi ciu nominalnym 24 V i pr dzie max. ok. 6 A) z przekªadni. W wersji podstawowej jest on wyposa»ony w 18 czujników odlegªo±ci (prod. Sharp dziaªaj cych z wykorzystaniem promieniowania podczerwonego): 5 dalekiego zasi gu (1 12 cm) i 13 krótkiego zasi gu (1 3 cm). Dodatkowe wyposa»enie ró»nych egzemplarzy robota ma by ró»ne, zale»ne od potrzeb. Bezpo±rednie sterowanie tych wszystkich elementów wykonawczych robota (silniki, czujniki itp.), a tak»e wykrywanie i obsªuga sytuacji awaryjnych jest realizowana przez specjalizowany mikrokomputer steruj cy, który komunikuje si z gªównym komputerem pokªadowym przez magistral PC14. Wszystkie czujniki s bezpo±rednio obsªugiwane przez specjalizowane mikrokomputery pomiarowe, przyª czone do szeregowej magistrali zbudowanej z wykorzystaniem standardu RS485. Schemat blokowy mikrokomputera steruj cego jest pokazany na rys. 2.2. Centralnym jego elementem jest mikrokomputer jednoukªadowy rmy Atmel T89C51AC2. Jest to 8bitowy mikrokomputer z popularnej rodziny MCS51, wykonany w statycznej technologii CMOS, dzi ki czemu charakteryzuje si niskim zu»yciem energii. Dzi ki wewn trznym blokom funkcjonalnym umo»liwia on programow (bez dodatkowych elementów sprz towych) realizacj niektórych funkcji, np. pomiar napi, wytwarzanie sygnaªów PWM, obsªuga komunikacji szeregowej. Natomiast do zada«sterowania silników nap dowych s wykorzystane scalone regulatory PID ukªady LM629 rmy National Semiconductor. S to specjalizowane ukªady przeznaczone do sterowania silnikiem, które mog realizowa kilka typowych zada«sterowania: regulacja pr dko±ci, realizacja zadanej trajektorii oraz regulacja poªo»enia, wykonuj c cyfrowy algorytm regulacji PID. Ukªad mierzy pozycj i pr dko± wykorzystuj c bezpo±rednio sygnaª z enkodera i wytwarza sterowanie w postaci sygnaªu PWM. Zadanie regulacji (pozycja zadana, pr dko± zadana, przyspieszenie) 12

oraz parametry regulatora (wzmocnienia wszystkich czªonów, cz stotliwo± próbkowania, ograniczenie warto±ci czªonu caªkuj cego) mog by przekazywane programowo. Pozycja i pr dko± (aktualne i zadane) mog by w ka»dej chwili odczytane z ukªadu. Warto±ci zadane pozycji i pr dko±ci s zapisywane do ukªadu (w jednostkach regulatora), a pozycja równie» odczytywana z 32-bitow rozdzielczo±ci. Natomiast warto± pr dko±ci jest odczytywana tylko z rozdzielczo±ci 16-bitow (bardziej znacz ce bity). Przy warto±ciach pr dko±ci wyst puj cych w robocie taka rozdzielczo± jest zdecydowanie za maªa. Z tego powodu pr dko± aktualna b dzie dodatkowo mierzona przez mikrokomputer, przy wykorzystaniu jogo ukªadów czasowolicznikowych. W tym celu do mikrokomputera s doprowadzone sygnaªy z enkodera. 13 Rysunek 2.2: Schemat blokowy mikrokomputera steruj cego Komunikacja mikrokomputera z gªównym komputerem pokªadowym odbywa si za pomoc bloku sprz gaj cego, przyª czonego z jednej strony do magistrali PC14 komputera, a z drugiej strony do magistrali mikrokomputera. Ukªad sprz gaj cy zawiera m.in. dwa 16bitowe rejestry polece«, do zapisu przez komputer gªówny i do odczytu przez mikrokomputer, dwa (16bitowy i 8bitowy) rejestry stanu, do odczytu przez komputer gªówny i do zapisu przez mikrokomputer oraz ukªad sterowania przerwaniami do komputera gªównego oraz do mikrokomputera z rejestrami stanu przerwa«. Przesyªanie danych mi dzy komputerem gªównym a mikrokomputerem jest sterowane przerwaniami. Zapisanie danych do jednego z rejestrów polece«powoduje automatyczne ustawienie przerzutnika zgªoszenia przerwania do mikrokomputera co jest sygnaªem pojawienia si nowego polecenia. Przerzutnik ten jest automatycznie zerowany po odczytaniu przez mikrokomputer wszystkich bajtów polecenia (przy odczycie ostatniego bajtu). Przerzutnik jest jednym z elementów rejestru stanu przerwa«i jego stan mo»e by programowo sprawdzany przez

komputer gªówny. W ten sposób komputer gªówny mo»e sprawdzi przed wysªaniem kolejnego polecenia, czy poprzednio wysªane zostaªo odebrane przez mikrokomputer. Analogicznie, wpisanie danych do jednego z rejestrów stanu powoduje automatyczne ustawienie przerzutnika zgªoszenia przerwania do komputera gªównego, który to przerzutnik jest automatycznie zerowany po odczytaniu tego rejestru przez komputer gªówny, a stan tego przerzutnika (informuj cy o odczytaniu danych) mo»e by sprawdzany przez mikrokomputer. Jednym z podstawowych zada«mikrokomputera steruj cego jest zbieranie danych o stanie czujników od mikrokomputerów pomiarowych, przyª czonych do magistrali szeregowej, zbudowanej na bazie standardu RS-485. Mikrokomputer steruj cy b dzie peªniª w tej strukturze funkcj ukªadu typu master, ukªady pomiarowe b d typu slave, a protokóª komunikacyjny b dzie opieraª si na zasadzie cyklicznego odpytywania mikrokomputerów steruj cych przez mikrokomputer steruj cy. 14 Rysunek 2.3: Schemat blokowy mikrokomputera pomiarowego Zadaniem mikrokomputera pomiarowego jest obsªuga czujników odlegªo±ci rmy Sharp. Sygnaªem wyj±ciowym z czujnika jest napi cie o warto±ci zale»nej od odlegªo±ci przedmiotu od czujnika, przy czym zale»no± ta jest nieliniowa. Schemat blokowy mikrokomputera pomiarowego jest pokazany na rys. 2.3. Jest to bardzo prosty ukªad, zbudowany na bazie mikrokomputera jednoukªadowego PIC16F688 rmy Microchip. Mikrokomputer ten jest wykonany w statycznej technologii CMOS, w 14ko«cówkowej obudowie, a zawiera m.in.: 8kanaªowy, 1bitowy przetwornik analogowo-cyfrowy oraz ukªad transmisji szeregowej UART, Mikrokomputer mo»e obsªu»y osiem czujników. Sygnaªy pomiarowe s przyª - czone bezpo±rednio do wej± analogowych mikrokomputera. Dodatkowo jest mo»liwo± wª czania i wyª czania napi cia zasilania dwóch grup czujników za pomoc klucza elektronicznego.

Rozdziaª 3 Opracowanie i implementacja algorytmu planowania ±cie»ek ruchu dla zespoªu robotów mobilnych Ogólnie, problem planowania ruchu to odpowied¹ na pytanie: Dok d id i jak mam si tam dosta?. Natomiast dokªadniej ujmuj c, polega na poszukiwaniu krzywej geometrycznej przej±cia pomi dzy pocz tkowym i zadanym ko«cowym stanem (pozycj ) robota tak, aby nie wyst powaªy kolizje z przeszkodami oraz planowany ruch nie powodowaª naruszenia ogranicze«kinematycznych i dynamicznych robota (±cie»ka byªa mo»liwa do wykonania). Obecnie typowe roboty przemysªowe wykonuj swoje zadania wchodz c tylko w z góry zaplanowane interakcje z otoczeniem, poniewa» ±rodowisko, w którym pracuj jest statyczne i strukturalne (tzn. uporz dkowane). Natomiast w przypadku robotów mobilnych do interakcji z otoczeniem, mo»e doj± w sposób przypadkowy, niezaplanowany. Wynika to z charakteru ±rodowiska, w którym roboty si poruszaj. W przeciwie«stwie do robotów przemysªowych, ich ±rodowisko jest zazwyczaj dynamiczne i cz sto jest nieuporz dkowane b d¹ cz ±ciowo uporz dkowane. Planowanie ruchu jest zdolno±ci do decydowania jakie dziaªanie nale»y podj w konkretnej sytuacji w celu osi gni cia konguracji ko«cowej. Jest to tak»e rozstrzygni cie wielu decyzji, pocz wszy od: któr ±cie»k wybra a sko«czywszy na: których informacji o ±rodowisku u»y w danej chwili. Planowanie w sytuacji, gdy posiadamy peªn informacji na temat aktualnego stanu ±rodowiska zostaªo ju» szeroko zbadane. Jednak»e w przypadku robotów mobilnych wiedza na temat otoczenia jest zazwyczaj cz ±ciowa i niepewna. W ten sposób problem podejmowania poprawnych decyzji staje si znacznie trudniejszy i wymaga realizacji równolegle wielu zada«(jedne w celu planowania ruchu, inne w celu przetrwania robota omijania przeszkód). Aby planowa ±cie»ki zespoªu robotów, musimy najpierw zapewni,»e»aden z robotów w trakcie ich realizacji nie ulegnie kolizji z obiektami w otaczaj cym go ±rodowisku. Dlatego te» gªówny nacisk poªo»ono na algorytmy, które lokalnie modykuj ±cie»ki ruchu w celu unikni cia kolizji z przeszkodami/obiektami w ±rodowisku, w którym robot si porusza. Wykrywanie lokalnych przeszkód i unikanie z nimi kolizji jest jedn z podstawowych funkcji sterowników robotów mobilnych dziaªaj cych w nieznanym lub cz ±ciowo nieznanym b d¹ dynamicznym otoczeniu. W algorytmach unikania kolizji wykorzystuje si dane pomiarowe z czujników, a tak»e docelow pozycj robota oraz jego aktualn pozycj wzgl - 15

3.1. METODY I ALGORYTMY UNIKANIA KOLIZJI 16 dem docelowej, do lokalnej modykacji trajektorii ruchu robota. Wymaga si, aby algorytmy byªy efektywne obliczeniowo, a generowany ruch byª mo»liwie gªadki oraz by robot pod»aª w kierunku celu przy uwzgl dnieniu zycznych ogranicze«robota [2,47]. 3.1 Metody i algorytmy unikania kolizji Wi kszo± znanych z literatury algorytmów unikania kolizji mo»na zaliczy do jednej z dwóch podstawowych grup: wyznaczania kierunku ruchu [6, 3, 52, 53] oraz wyznaczania pr dko±ci liniowej i k towej robota w wyniku przeszukiwania odpowiednio dobranej przestrzeni pr dko±ci [7,23,37,48]. Jednym z wcze±niejszych algorytmów, zaliczanych do pierwszej grupy, jest podej±cie wykorzystuj ce sztuczne pola potencjaªowe [3]. Robot, traktowany jako cz steczka, porusza si w polu, za± jego ruch jest wypadkow dziaªaj cych na niego siª pochodz cych od przeszkód (siªy odpychaj ce) oraz od celu (siªa przyci gaj ca). W swej pierwotnej wersji metoda pól potecjaªowych miaªa szereg wad, w±ród których najpowa»niejsz byªo wyst powanie minimów lokalnych pola, czyli punktów ró»nych od docelowego, w których siªa wypadkowa jest równa zeru [6,19]. W±ród metod wyznaczania kierunku najwi ksz popularno± zdobyªy kolejne wersje algorytmów z grupy tzw. histogramów pola wektorowego Vector Field Histogram (VFH) [6], VFH+ [52], oraz VHF* [53]. Zbli»one do VFH podej±cie, zaprezentowane w pracy [33], wykorzystuje diagramy blisko±ci (Nearness Diagram (ND)). Dzi ki uwzgl dnieniu geometrycznych, kinematycznych i dynamicznych ogranicze«dla robota, algorytm ten daje lepsze rezultaty ni» VFH+, szczególnie w ±rodowiskach z du» liczb przeszkód i w skimi przej±ciami. Techniki wykorzystuj ce poj cie okna dynamicznego (Dynamic Window) s przykªadem podej±cia polegaj cego na przeszukiwaniu pewnego zbioru pr dko±ci (okna) w celu znalezienia sterowa«(pr dko±ci) dopuszczalnych przy uwzgl dnieniu ogranicze«wynikaj cych z kinematyki robota i uproszczonego modelu dynamiki [7,23,37]. Przestrze«pr dko±ci jest zbiorem wszystkich par (ω, v), skªadaj cych z pr dko±ci k towych ω i liniowych v. W podej±ciu tym zakªada si,»e robot mo»e porusza si tylko po ªukach okr gów reprezentowanych przez par (ω, v). Do grupy podej± bazuj cych na wyznaczaniu pr dko±ci zalicza si tak»e metod tzw. krzywizn i pr dko±ci (Curvature Velocity Method (CVM)) [48]. Zakªada si tutaj, podobnie jak w technikach okna dynamicznego,»e robot porusza si tylko po ªukach okr gów o krzywiznach c = ω. W podej±ciu tym mo»liwe jest uwzgl dnienie ogranicze«na dopuszczalne warto±ci pr dko±ci i przyspieszenia. v Algorytm przedstawiony w niniejszym rozdziale zalicza si do tej grupy i stanowi rozwini cie idei zaproponowanej w [48]. W niniejszej pracy starano si wyeliminowa niedoci - gni cia i bª dy oryginalnej wersji tego algorytmu.

3.2. ALGORYTM KRZYWIZN I PR DKO CI CVM 17 3.2 Algorytm krzywizn i pr dko±ci CVM Algorytm ten jest lokaln metod planowania ruchu bazuj c na wyznaczaniu pr dko±ci liniowej i k towej robota. Problem omijania przeszkód jest opisywany jako zadanie optymalizacji z ograniczeniami w przestrzeni pr dko±ci robota. Przestrze«pr dko±ci jest zbiorem dopuszczalnych pr dko±ci. W zadaniu tym przyjmujemy sterowanie za pomoc niezale»nych pr dko±ci: liniowej v oraz k towej ω. Wybór takich sterowa«jest naturalny dla robotów o nap dzie synchronicznym. Dla naszego robota o nap dzie ró»nicowym bardziej oczywiste wydaje si sterowanie za pomoc niezale»nych pr dko±ci lewego v l oraz prawego v r koªa. Przej±cie pomi dzy tymi pr dko±ciami wyra»a si prostymi zale»no±ciami: ω = v l v r, gdzie d to rozstaw kóª robota, d oraz v = v l+v r. Poniewa» przeliczenie takie jest dokonywane w sterowniku robota, wi c 2 w poni»szych rozwa»aniach b dziemy operowa na pr dko±ciach (ω, v). Po» dane pr dko±ci (ω, v) s uzyskiwane w wyniku maksymalizacji okre±lonej funkcji celu przy speªnieniu ogranicze«. Do zalet tak sformuªowanego zadania nale» mi dzy innymi: ªatwo± dodawania ogranicze«wynikaj cych z dynamiki robota, niezale»ne sterowanie pr dko±ci oraz zmian orientacji robota przez zadawanie pr dko±ci liniowej i k towej (sterowanie pr dko±ci k tow wpªywa tylko i wyª cznie na zmian orientacji, natomiast pr dko± liniowa wpªywa jedynie na pr dko± robota), mo»liwo± wprowadzania dodatkowych czynników decyduj cych o wyborze pr dko- ±ci przez modykacj funkcji celu. W rzeczywisto±ci, z dobrym przybli»eniem, mo»na zaªo»y,»e robot, w ka»dej chwili czasu, porusza si wzdªu» ªuku pewnego okr gu, którego krzywizna wyra»a si wzorem: Fizyczna konstrukcja robota narzuca dwa typy ogranicze«: c = ω v. (3.1) na maksymaln oraz minimaln pr dko± k tow oraz liniow robota: ω min ω ω max, (3.2) v min v v max ; (3.3) na przyspieszenie k towe oraz liniowe. Dla pr dko±ci (ω n, v n ) w chwili n oraz okresu T alg, b d cego czasem jednego wykonania algorytmu, prawdziwe s warunki: ω n ɛ max T alg ω n+1 ω n + ɛ max T alg, (3.4) v n+1 v n + a max T alg, (3.5) gdzie ɛ max oraz a max to odpowiednio maksymalne przyspieszenie k towe oraz maksymalne przyspieszenie liniowe robota.

3.2. ALGORYTM KRZYWIZN I PR DKO CI CVM 18 Najwa»niejszym ¹ródªem ogranicze«na dopuszczalne pr dko±ci (ω, v) s przeszkody znajduj ce si w ±rodowisku otaczaj cym robota. Poniewa» w naturalny sposób przeszkody s opisywane we wspóªrz dnych kartezja«skich, nale»y dokona transformacji ich opisu do przestrzeni pr dko±ci robota. Poni»ej opisano jak jest to realizowane. Rysunek 3.1: Wyznaczanie odlegªo±ci od przeszkody p dla okr gu o krzywi¹nie c i Dla wszystkich krzywizn c nale»y obliczy odlegªo± d c ((, ), p i ) rozumian jako dªugo± drogi, jak musi przeby robot, startuj c z punktu (, ) w ukªadzie lokalnym robota, wzdªu» okr gu o krzywi¹nie c, zanim zderzy si z przeszkod p w punkcie p i. Nast pnie deniujemy funkcj odlegªo±ci robota od przeszkody jako (zapis ten nie jest ±cisªy z matematycznego punktu widzenia): { dc ((, ), p d v (ω, v, p) = i ) dla v dla v =, gdzie c = ω (3.6) v Dla danego zbioru przeszkód P funkcja ta ma posta : D(ω, v, P ) = inf p P d v(ω, v, p). (3.7) Poniewa» zasi g czujników jest ograniczony nale»y ograniczy zakres mo»liwych warto±ci funkcji D do pewnej ustalonej warto±ci L (w naszym przypadku przyj li±my L = 1, 1 m

3.2. ALGORYTM KRZYWIZN I PR DKO CI CVM 19 dla czujników o zasi gu 1, 5 m): D ogr (ω, v, P ) = min (L, D(ω, v, p)). (3.8) Obliczenie odlegªo±ci d c ((, ), p i ) od przeszkody p, o dowolnym ksztaªcie, mo»e by w ogólnym przypadku skomplikowane. Dlatego te», w naszych rozwa»aniach, b dziemy przybli»a przeszkody okr gami opisanymi na nich. Dla robota o orientacji zgodnej z osi OY, dla danego okr gu, o krzywi¹nie c i, który przecina przeszkod w punkcie P i (x i, y i ) otrzymujemy (rys. 3.1): { atan2(yi, x i 1 c θ = i ) dla c i < π atan2(y i, x i 1 c i ) dla c i > { yi dla c i = d ci ((, ), P i ) = 1 c i θ dla ci (3.9) (3.1) Maj c dane ju» ograniczenia wynikaj ce z otoczenia robota, jak i jego mo»liwo±ci, warto±ci pr dko±ci otrzymujemy poszukuj c maksimum funkcji celu. Funkcj celu dobieramy tak, aby speªni nast puj ce wymagania: robot powinien d»y do osi gni cia swojej maksymalnej pr dko±ci liniowej, robot powinien porusza si po krzywych niekoliduj cych z przeszkodami, robot powinien zawsze kierowa si na cel. Powy»sze wymagania mo»na zapisa w postaci liniowej funkcji celu: gdzie: F(ω, v) = α 1 V(v) + α 2 D(ω, v) + α 3 G(ω), (3.11) V(v) = v v max (3.12) D(ω, v) = D ogr(ω, v, P ) L G(ω) = 1 θ cel ω T alg π (3.13) (3.14) Warto±ci poszczególnych skªadników V, D, G funkcji F (3.11) s znormalizowane. Skªadowa V odpowiada za poruszanie si z maksymaln mo»liw pr dko±ci liniow, skªadowa D jest odpowiedzialna ze pod»anie wzdªu» bezkolizyjnych krzywych, natomiast skªadowa G preferuje pod»anie za celem. W ostatnim czªonie θ cel jest orientacj do celu w ukªadzie wspóªrz dnych robota. Warto±ci wspóªczynników α i s wagami powy»szych skªadników. Od doboru tych wspóªczynników zale»y zachowanie robota, tzn. jak b dzie skr caª, jak wcze±nie b dzie reagowaª na przeszkody, itd.

3.3. SZCZEGÓŠOWY OPIS ALGORYTMU 2 3.3 Szczegóªowy opis algorytmu Pomimo uproszczenia, jakim jest przyj cie,»e przeszkody s reprezentowane przez okr gi, obliczenie funkcji D ogr (3.8) dla du»ej liczby przeszkód, jest czasochªonne. Zauwa»my,»e dla danej przeszkody p odlegªo± d c ((, ), p i ) jest niesko«czona na zewn trz krzywych stycznych do danej przeszkody. St d, wystarczy,»e b dziemy rozpatrywa jedynie krzywe le» ce pomi dzy c min a c max (rys. 3.2). Rysunek 3.2: Krzywe styczne do przeszkody p w punktach P min i P max Aby wyznaczy warto±ci c min, c max, P min (x min, y min ) oraz P max (x max, y max ) dla przeszkody p o ±rodku w punkcie P (x, y ) ((x, y ) (, )) i promieniu r nale»y znale¹ takie r, aby poni»szy ukªad równa«, przedstawiaj cy dwa okr gi, miaª dokªadnie jedno rozwi zanie: { (x x ) 2 + (y y ) 2 = r 2 (x r) 2 + y 2 = r 2 (3.15) Powy»szy ukªad równa«nale»y sprowadzi do równania kwadratowego jednej zmiennej i wyznaczy wyró»nik tego równania. O tym, wzgl dem której zmiennej b dziemy rozwi zywa to równanie kwadratowe, b dzie decydowa poªo»enie przeszkody w ukªadzie wspóªrz dnych, czego powodem jest minimalizacja bª dów numerycznych. Wyliczenia znajduj si w 3.3.1 oraz 3.3.2.

3.3. SZCZEGÓŠOWY OPIS ALGORYTMU 21 Maj c dane krzywe styczne do przeszkody, przybli»amy funkcj d v (3.6) funkcj staª, dla wszystkich c z zakresu c [c min, c max ]: { min (dcmin d v (ω, v, p) = ((, ), P min), d cmax ((, ), P max )) dla c min c c max w p. p. (3.16) W ten sposób, dla zbioru przeszkód, otrzymujemy zbiór przedziaªów, ka»dy o staªej odlegªo±ci. Ka»dy z tych przedziaªów mo»na reprezentowa jako struktur postaci: ([c 1, c 2 ], d 1, 2 ), gdzie c 1 i c 2 to krzywizny okr gów wyznaczaj cych ten przedziaª (c 1 c 2 ), a d 1, 2 jest odlegªo±ci do przeszkody wewn trz tego przedziaªu. W dalszych rozwa»aniach przedziaª b dzie rozumiany jako jego reprezentacja w postaci struktury typu: ([c 1, c 2 ], d 1, 2 ) Rysunek 3.3: Przeszkody i graczna interpretacji listy przedziaªów, które z nich powstaªy Zaczynaj c od listy przedziaªów zawieraj cej tylko przedziaª ([, ], L), dla ka»dej przeszkody wyznaczamy przedziaª ([c min, c max ], d) i dodajemy ten przedziaª do listy wedªug zasady: dla ka»dego przedziaªu ([c 1, c 2 ], d 1, 2 ) z listy: sprawd¹ w jakich zale»no±ciach jest ten przedziaª z nowo dodawanym. Je±li: s to przedziaªy rozª czne nic nie rób, przedziaª z listy zawiera si w nowo dodawanym przedziale (c 1 c min i c 2 c max ) ustaw odlegªo± d 1, 2 = min (d, d 1, 2 ), przedziaª z listy zawiera nowo dodawany przedziaª (c 1 c min i c 2 c max ) je±li d < d 1, 2, to podziel istniej cy przedziaª na trzy: ([c 1, c min ], d 1, 2 ), ([c min, c max ], d) oraz ([c max, c 2 ], d 1, 2 ); w przeciwnym przypadku nic nie rób, przedziaªy na siebie zachodz je±li d < d 1, 2, to podziel istniej cy przedziaª na dwa, a wynik uzale»nij od tego, które brzegi przedziaªów na siebie zachodziªy, tzn.

3.3. SZCZEGÓŠOWY OPIS ALGORYTMU 22 dla c max > c 2 podziel na przedziaªy ([c 1, c min ], d 1, 2 ) i ([c min, c 2 ], d), w przeciwnym przypadku na przedziaªy ([c 1, c max ], d) i ([c max, c 2 ], d 1, 2 ); je±li d d 1, 2, to nic nie rób. Nast pnie nale»y poª czy s siaduj ce przedziaªy, które maj t sam odlegªo±. W ten sposób otrzymujemy list rozª cznych przedziaªów w przestrzeni pr dko±ci. Geometrycznie, ka»dy przedziaª w takiej li±cie deniuje par prostych w przestrzeni pr dko±ci (rys. 3.3). Przybli»enie zbioru dªugo±ci krzywych z zakresu [c min, c max ] pojedyncz warto±ci nie daje dobrych wyników (rys. 3.4). W pewnych sytuacjach przybli»enie takie jest zbyt re- Rysunek 3.4: Zbiór krzywych z zakresu [c min, c max ] o ró»nych dªugo±ciach o danej przeszkody p. strykcyjne, a w innych jest zbyt sªabe i mo»e powodowa problemy (sytuacja, w której rzeczywista odlegªo± od przeszkody jest mniejsza ni» ta, wyliczona z denicji (3.16)). Jednym z rozwi za«tego problemu jest podzielenie przedziaªu [c min, c max ] na podprzedziaªy, zastosowanie równania (3.16) dla tych podprzedziaªów, a nast pnie dodanie ich do listy zawieraj cej wszystkie takie przedziaªy w sposób omówiony wy»ej. Rozwi zanie, które przyj to, polega na wyznaczeniu punktu, na okr gu opisuj cym przeszkod, do którego odlegªo± od robota jest najmniejsza, w metryce euklidesowej. Nast pnie okr g ten jest dzielony na k segmentów, które wyznaczaj nowe punkty, (rys. 3.5) pocz wszy od wyznaczonego wcze±niej punktu. Dla ka»dych dwóch s siaduj cych punktów le» cych pomi dzy punktami, wyznaczonymi przez krzywe styczne do przeszkody, tworzony jest przedziaª ([c 1, c 2 ], d 1, 2 ) i dodawany do listy w sposób omówiony wy»ej. c 1 i c 2 to krzywizny okr gów przechodz cych przez te dwa punkty, a jako odlegªo± d 1, 2

3.3. SZCZEGÓŠOWY OPIS ALGORYTMU 23 Rysunek 3.5: Sposób podziaªu okr gu opisuj cego przeszkod na k = 8 cz ±ci brana jest mniejsza z odlegªo±ci liczonych jako droga z równania 3.1, wzdªu» krzywych c 1 i c 2. Dla przykªadu z rysunku 3.5 dodane zostan cztery przedziaªy, zamiast jednego. 3.3.1 Równanie kwadratowe wzgl dem zmiennej x, czyli y > x Wyró»nik równania kwadratowego, które powstaªo po sprowadzeniu ukªadu równa«3.15 do równania kwadratowego zmiennej x, dany jest wzorem: = y 2 ( ) 4x 2 + 4r 2 r 2 + y 2 ( y 2 4x x 2 4x r 2 4x ) r+ + y 2 ( ( ) x 2 2r 2 2y 2 x 2 y 4 + r 2 Rozwi zuj c równanie kwadratowe = wzgl dem zmiennej r otrzymujemy: ( 2y 2 r 2 )) r max = x2 + y 2 r 2 2x 2r r min = x2 + y 2 r 2 2x + 2r (3.17) c min = 1 r max = 2x 2r x 2 + y 2 r 2 c max = 1 r min = 2x + 2r x 2 + y 2 r 2 (3.18) Punkty styczno±ci okr gów P min (x min, y min ) oraz P max (x max, y max ) (podstawiaj c za r odpowiednio r min oraz r max ): { x x = 3 +rr2 rx2 +ry2 r2 x +x y 2 4rx +2r 2 +2x 2 +2y2 dla y y = 2rx+2xx +r 2 (3.19) x2 y2 2y

3.3. SZCZEGÓŠOWY OPIS ALGORYTMU 24 Punkty przeci cia okr gów dla > oraz danego r mo»na wyznaczy ze wzorów: { x1 = x3 +rr2 rx2 +ry2 r2 x +x y 2 4rx +2r 2 +2x 2 +2y2 y 1 = 2rx 1+2x 1 x dla y +r 2 (3.2) x2 y2 2y { x2 = x3 +rr2 rx2 +ry2 r2 x +x y 2 + 4rx +2r 2 +2x 2 +2y2 y 2 = 2rx 2+2x 2 x +r 2 x2 y2 2y dla y (3.21) 3.3.2 Równanie kwadratowe wzgl dem zmiennej y, czyli y x Wyró»nik równania kwadratowego, które powstaªo po sprowadzeniu ukªadu równa«3.15 do równania kwadratowego zmiennej y, dany jest wzorem: = x 6 + 6rx 5 + 2rr 4 x + 2rx y 4 r 2 r 4 + 4r 4 r 2 13r 2 x 4 + 12r 3 x 3 4r 4 x 2 r 2 y 4 + 2r 2 x 4 r 4 x 2 x 2 y 4 2x 4 y 2 8rr 2 x 3 12r 3 r 2 x + 8rx 3 y 2 + 4r 3 x y 2 4rr 2 x y 2 + 14r 2 r 2 x 2 + 2r 2 r 2 y 2 1r 2 x 2 y 2 + 2r 2 x 2 y 2 Rozwi zuj c równanie kwadratowe = wzgl dem zmiennej r otrzymujemy: r max = x2 + y 2 r 2 2x 2r r min = x2 + y 2 r 2 2x + 2r (3.22) c min = 1 r max = 2x 2r x 2 + y 2 r 2 c max = 1 r min = 2x + 2r x 2 + y 2 r 2 (3.23) Punkty styczno±ci okr gów P min (x min, y min ) oraz P max (x max, y max ) (podstawiaj c za r odpowiednio r min oraz r max ): { 2yy x = +r 2 x2 y2 dla x r (3.24) 2r 2x y = 2rx y +y 3+2r2 y r 2y +x 2 y 4rx +2r 2 +2x 2 +2y2 Punkty przeci cia okr gów dla > oraz danego r mo»na wyznaczy ze wzorów: { x1 = 2y 1y +r 2 x2 y2 2r 2x y 1 = 2rx y +y 3+2r2 y r 2y +x 2 y 4rx +2r 2 +2x 2 +2y2 { x2 = 2y 2y +r 2 x2 y2 2r 2x y 2 = 2rx y +y 3+2r2 y r 2y +x 2 y + 4rx +2r 2 +2x 2 +2y2 dla x r (3.25) dla x r (3.26) W przypadku, gdy x = r i y równanie kwadratowe nale»y rozwi zywa wzgl dem zmiennej x. W przypadku, gdy x = r i y = ukªad równa«ma rozwi zanie tylko dla r = r. Wtedy nale»y przyj rozwi zania postaci: { { x1 = x2 = 2r y 1 = y 2 = 2r Przypadku, gdy y = i x = w praktyce nie trzeba rozpatrywa, gdy» ±rodek przeszkody nie mo»e znajdowa si w pocz tku ukªadu zwi zanego z robotem.

3.4. SZCZEGÓŠOWY OPIS DZIAŠANIA PROGRAMU 25 3.4 Szczegóªowy opis dziaªania programu Program implementuj cy algorytm CVM zostaª napisany z wykorzystaniem struktury Player/Stage i dziaªa w nast puj cy sposób: 1. Pobiera dane o aktualnym poªo»eniu robota oraz bie» ce odczyty z czujników (1 razy na sekund ). 2. Na podstawie informacji z odometrii, o przemieszczeniu, przelicza poªo»enie celu wzgl dem lokalnego ukªadu wspóªrz dnych. Je±li w swojej pami ci posiada informacje o przeszkodach, tak»e przelicza poªo»enie tych przeszkód. Dodatkowo, usuwa te przeszkody, po przeliczeniu ich poªo»enia, których wspóªrz dna y < (nie chcemy pami ta o przeszkodach, które min li±my). 3. Dla ka»dego odczytu z czujników odlegªo±ci wykonywane s nast puj ce czynno±ci: (a) Je±li odczyt odlegªo±ci jest wi kszy ni» pewna ustalona warto± (L w algorytmie), to pomija dany odczyt. (b) Sprawdzane jest w pami ci, czy nie ma ju» informacji o przeszkodzie znajduj cej si w podobnym miejscu. Warunkiem,»eby przeszkody zostaªy uznane za identyczne, jest to, aby ich ±rodki znajdowaªy si w kwadracie o boku pos_epsilon. W implementacji przyj ªem pos_epsilon =.5 [m]. Je±li oka»e si,»e istnieje ju» taka przeszkoda, pomija dany odczyt. (c) Wyznacza promie«okr gu opisuj cego dan przeszkod. Promie«zale»y od odlegªo±ci do przeszkody i wyra»a si wzorem: promien = obstacle_radius + increase_radius_f actor gdzie: odleglosc obstacle_radius increase_radius_f actor (odleglosc obstacle_radius) to odlegªo± robota od przeszkody, to promie«wyj±ciowy przeszkody, to wspóªczynnik opisuj cy, jak bardzo nale»y zwi kszy promie«przeszkody w zale»no±ci od odlegªo±ci do niej. W implementacji przyj ªem increase_radius_f actor =.6 (rys 3.6), a warto± obstacle_radius ustaliªem na poziomie.31 [m], gdy» tyle wynosi poªowa przek tnej robota. (d) Przeszkoda dodawana jest do pami ci i zwi kszany jest licznik przeszkód. 4. Sprawdza odlegªo± do celu. Je±li jest ona mniejsza ni» zaªo»ona dokªadno± dist_accuracy, zatrzymuje robota. W przeciwnym przypadku wyznacza nowe pr dko±ci steruj ce. W implementacji przyj ªem dist_accuracy =.6 [m]. 5. Zapami tuje nowo wyznaczone pr dko±ci robota. 6. Je±li opcja remember_obstacles jest wª czona, to z pami ci s usuwane tylko przeszkody zb dne. W przeciwnym przypadku usuwane s wszystkie przeszkody. Przeszkod

3.4. SZCZEGÓŠOWY OPIS DZIAŠANIA PROGRAMU 26 42 Zaleznosc promienia przeszkody od odleglosci do tej przeszkody dla wspolczynnika wzrostu.6 4 Promien przeszkody [cm] 38 36 34 32 3 2 4 6 8 1 12 14 16 18 2 Odleglosc do przeszkody [cm] Rysunek 3.6: Zale»no± promienia przeszkody od odlegªo±ci do tej przeszkody dla increase_radius_f actor =.6 uwa»a si za zb dn, gdy jej ±rodek znajduje si poza prostok tem, o wymiarach xbound ybound, którego ±rodkiem symetrii jest punkt reprezentuj cy robota (rys. 3.7). W implementacji przyj ªem xbound = ybound = 1 [m]. Informacje o przeszkodach program mo»e czerpa z trzech rodzajów czujników: podczerwieni, sonarowych oraz dalmierza laserowego (o wyborze ¹ródªa nale»y zadecydowa w momencie kompilacji programu). Pami o przeszkodach zostaªa wprowadzona do programu z powodu rozmieszczenia czujników podczerwieni w naszym robocie. Bez tej pami ci, robot widz c nawet wcze±niej czarn przeszkod na rysunku 3.8, wjechaªby w ni w pó¹niejszym czasie, poniewa» jest ona niewidoczna dla jego czujników. Informacja o poªo»eniu robota mo»e pochodzi z odometrii robota, b d¹ z globalnej wiedzy o jego poªo»eniu, jak to ma miejsce w symulatorze (o wyborze ¹ródªa nale»y zadecydowa w momencie kompilacji programu). Wyznaczanie nowych pr dko±ci steruj cych odbywa si w nast puj cy sposób: 1. Na podstawie bie» cej informacji o pr dko±ciach, wyznaczane s, z równa«3.2, 3.3, 3.4 oraz 3.5, ograniczenia na dopuszczalne pr dko±ci, które b d brane pod uwag przy wyznaczaniu nowego sterowania. 2. Tworzona jest lista przedziaªów, która pocz tkowo zawiera tylko jeden przedziaª: ([, ], L).

3.4. SZCZEGÓŠOWY OPIS DZIAŠANIA PROGRAMU 27 Rysunek 3.7: Czarne okr gi oznaczaj przeszkody zb dne, czerwone przeszkody, które zostan zapami tane Rysunek 3.8: Wizualizacja wi zki IR czujników podczerwieni w robocie Protonek 3. Dla ka»dej zapami tanej przeszkody wykonywane s nast puj ce czynno±ci: (a) Ze wzoru 3.18 wyznaczane s styczne do przeszkody (c min oraz c max ), natomiast ze wzorów 3.19 oraz 3.24 wyznaczane s punkty styczno±ci (P min oraz P max ). (b) Ze wzoru 3.1 liczona jest odlegªo± (d min oraz d max ) do punktów styczno±ci

3.5. WYNIKI EKSPERYMENTALNE 28 wzdªu» wyznaczonych krzywych. (c) Wyznaczane s segmenty okr gu opisuj cego przeszkod, do których nale» punkty styczno±ci, oraz punkty po±rednie pomi dzy tymi segmentami. (d) Wszystkie przedziaªy ([c 1, c 2 ], d) wynikaj ce z nowo otrzymanych punktów s dodawane do listy przedziaªów w sposób opisany wy»ej. 4. Otrzymana w ten sposób lista przedziaªów poddawana jest kompresji. Š czone s s siaduj ce przedziaªy, mi dzy którymi odlegªo± jest nie wi ksza ni» gdzie: dist max(dist merge_interval_f actor, min_merge_dist) merge_interval_f actor to odlegªo± ostatniego przedziaªu, który nie zostaª poª czony z»adnym innym, to wspóªczynnik odpowiadaj cy za ª czenie przedziaªów, przy maksymalnej odlegªo- ±ci od przeszkody, ª czone s przedziaªy, mi dzy którymi ró»nica odlegªo±ci wynosi: merge_interval_f actor min_merge_dist przedziaªy, mi dzy którymi odlegªo± jest mniejsza ni» merge_interval_f actor zawsze b d ª czone. W implementacji przyj ªem merge_interval_f actor =.5 [m], a min_merge_dist =.5 [m]. 5. Dla ka»dego przedziaªu sprawdzane jest, czy jego odlegªo± jest wi ksza ni» s siednich przedziaªów. Je±li tak jest, to krzywa ograniczaj ca ten przedziaª jest zmniejszana (je±li odlegªo± dla przedziaªu po prawej jest mniejsza) lub zwi kszana (je- ±li odlegªo± dla przedziaªu po lewej jest mniejsza) o pewn warto±, która wynika z powi kszenia promienia okr gu, do którego ta krzywa jest styczna, o warto± saf ety_margin. W implementacji przyj ªem saf ety_margin =.5 [m]. 6. Dla ka»dego przedziaªu wyznaczane jest najlepsze sterowanie uwzgl dniaj c ograniczenia wynikaj ce z kinematyki i dynamiki robota oraz samego przedziaªu (ka»dy przedziaª wprowadza liniowe ograniczenia w przestrzeni pr dko±ci). 7. Z tak wyznaczonych sterowa«wybierane jest to, dla którego funkcja celu 3.11 osi ga maksymaln warto±. 3.5 Wyniki eksperymentalne Wyniki zostaªy przygotowane z wykorzystaniem trójwymiarowego symulatora Gazebo, modeluj cego zachowanie i oddziaªywania mi dzy obiektami. Czas oblicze«algorytmu wynosi szacunkowo 5 µsec dla ±rednio 3 przeszkód, na komputerze klasy Pentium III 9 MHz. Przedstawiony algorytm unikania kolizji zale»y od wielu parametrów, w szczególno±ci od warto±ci wspóªczynników α. Po przeprowadzeniu szeregu do±wiadcze«w ±rodowisku

3.6. WNIOSKI 29 symulacyjnym, a nast pnie ich werykacji w rzeczywisto±ci, dobrano nast puj ce warto±ci parametrów: α 1 =, 3 α 2 =, 6 α 3 =, 1 W obu poni»szych eksperymentach roboty rozpoczynaªy ruch w tej samej chwili. 3.5.1 rodowisko pomiarowe 1 W eksperymencie tym ka»dy z robotów miaª przejecha 4 m przed siebie. Roboty do pomiaru odlegªo±ci wykorzystywaªy jedynie czujniki IR. Pocz tkowa odlegªo± mi dzy robotami wynosiªa 5 m, roboty skierowane byªy do siebie przodem. Na rysunku 3.9 pokazany jest stan pocz tkowy robotów (rys. z lewej), oraz ko«cowy (rys. z prawej). Rysunek 3.1 przedstawia wykres poªo»enia robotów w trakcie wymijania si nawzajem (rys. 3.9). Roboty osi gn ª cel z uchybem mniejszym ni» 6 cm. Na rysunku 3.11 znajduj si wykresy pr dko±ci steruj cych, natomiast na rysunku 3.12 wykresy orientacji robotów w trakcie jazdy. 3.5.2 rodowisko pomiarowe 2 W eksperymencie tym ka»dy z robotów miaª przejecha 4 m przed siebie. Roboty do pomiaru odlegªo±ci wykorzystywaªy dalmierz laserowy SICK LMS 2. Robot stoj cy w linii z obiema okr gªymi przeszkodami, o ±rednicy 1 cm, staª odlegªy o 2 m od pierwszej z nich. Odlegªo± mi dzy przeszkodami wynosiªa 1 m. Drugi z robotów odlegªy byª od przeszkody o 2, 5 m. Na rysunku 3.13 pokazany jest stan pocz tkowy robotów (rys. z lewej), oraz ko«cowy (rys. z prawej). Rysunek 3.1 przedstawia wykres poªo»enia robotów w trakcie wymijania si nawzajem (rys. 3.9). Roboty osi gn ª cel z uchybem mniejszym ni» 6 cm. Na rysunku 3.11 znajduj si wykresy pr dko±ci steruj cych, natomiast na rysunku 3.12 wykresy orientacji robotów w trakcie jazdy. 3.6 Wnioski Wykresy poªo»enia dla zaprezentowanych wyników s gªadkie, co jest istotnym faktem. Dodatkowo w wi kszo±ci testów, roboty nie zaklinowaªy si nigdzie oraz nie zbli»yªy si do przeszkód w stopniu, w którym musiaªyby si zatrzyma i obróci w miejscu. Uzyskane przebiegi pr dko±ci steruj cych nie s gªadkie, co niekorzystnie wpªywa na elementy wykonawcze (silniki w robocie). Jest to spowodowane sposobem doboru pr dko±ci. Algorytm wykorzystuje optymalizacj pewnej funkcji kosztu, od której wªa±nie zale»y charakter sterowania. W celu poprawy efektywno±ci obliczeniowej zastosowane zostaªy funkcje liniowe, które jak wida powoduj oscylacje pr dko±ci k towej.

3.6. WNIOSKI 3 Rysunek 3.9: Wymijanie si robotów nawzajem 2.5 2 Wykres pozycji robotow CVM y [m] 1.5 1.5.5 1 1.5 2 2.5 Tor ruchu robota 1 Punkt startowy robota 1 Punkt koncowy (cel) robota 1 Tor ruchu robota 2 Punkt startowy robota 2 Punkt koncowy (cel) robota 2 5 4 3 2 1 1 x [m] Rysunek 3.1: Wykres poªo»enia robotów w trakcie wymijania si nawzajem

3.6. WNIOSKI 31 Przebieg predkosci katowej ω robot 1.4 Przebieg predkosci liniowej v robot 1.2 ω [rad/s].2.2.4 v [m/s].15.1.5.6 2 4 6 czas [s] Przebieg predkosci katowej ω robot 2.4 2 4 6 czas [s] Przebieg predkosci liniowej v robot 2.2 ω [rad/s].2.2.4 v [m/s].15.1.5.6 2 4 6 czas [s] 2 4 6 czas [s] Rysunek 3.11: Przebiegi pr dko±ci robotów w trakcie wymijania si nawzajem Wykres orientacji robot 1 9 5 12 6 Wykres orientacji robot 2 9 5 12 6 15 25 3 15 25 3 18 18 21 33 21 33 24 27 3 24 27 3 Rysunek 3.12: Wykres orientacji robotów w trakcie wymijania si nawzajem

3.6. WNIOSKI 32 Rysunek 3.13: Omijanie przeszkód przez roboty 1.5 Wykres pozycji robotow CVM 1.5 y [m].5 1 1.5 2 2.5 Tor ruchu robota 1 Punkt startowy robota 1 Punkt koncowy (cel) robota 1 Tor ruchu robota 2 Punkt startowy robota 2 Punkt koncowy (cel) robota 2 Przeszkody 3 2.5 2 1.5 1.5.5 1 1.5 2 x [m] Rysunek 3.14: Wykres poªo»enia robotów w trakcie omijania przeszkód

3.6. WNIOSKI 33 Przebieg predkosci katowej ω robot 1.5 Przebieg predkosci liniowej v robot 1.2.15 ω [rad/s] v [m/s].1.5.5 2 4 6 czas [s] Przebieg predkosci katowej ω robot 2.5 2 4 6 czas [s] Przebieg predkosci liniowej v robot 2.2.15 ω [rad/s] v [m/s].1.5.5 2 4 6 czas [s] 2 4 6 czas [s] Rysunek 3.15: Przebiegi pr dko±ci robotów w trakcie omijania przeszkód Wykres orientacji robot 1 9 5 12 6 Wykres orientacji robot 2 9 5 12 6 15 25 3 15 25 3 18 18 21 33 21 33 24 27 3 24 27 3 Rysunek 3.16: Wykres orientacji robotów w trakcie omijania przeszkód

Rozdziaª 4 Algorytm wspóªpracy zespoªu robotów w zadaniu wspólnego przemieszczania obiektu Sterowanie dziaªaniem robota mobilnego jest procesem znacznie odbiegaj cym od tradycyjnego przetwarzania komputerowego. Odmienno± ta wynika przede wszystkim z faktu,»e nie jest realizowane jedno powtarzalne zadanie (do czego pocz tkowo byªy przewidziane komputery). Zadanie lub inaczej cel dziaªa«mo»e nie by ±ci±le okre±lony, a nawet mo-»e zmienia si podczas pracy robota. Tak»e otoczenie robota mobilnego, w odró»nieniu od manipulatorów przemysªowych cz sto nie jest znane, a za to zmienne oraz trudne do zamodelowania. W ramach prac zostaªo uruchomione ±rodowisko programowania heterogenicznych robotów mobilnych. Opracowane i zaimplementowane zostaªy przykªadowe zachowania robotów, których dziaªanie zostaªo zwerykowane przez realizacje jednego z typowych zada«testowych dla zespoªu robotów mobilnych, jakim jest kooperacyjne transportowanie obiektu. 4.1 Koncepcja sterowania behawioralnego Jednym z pierwszych podej± do sterowania dziaªaniem robota mobilnego byªo kontrolowanie zachowa«jako reakcja na bod¹ce [3] sterowanie reaktywne. Ta koncepcja bierze swoje podstawy w obserwacjach zachowa«prostych organizmów»ywych i jest odwzorowaniem dziaªania instynktu w ±rodowisku naturalnym. Cechuje je przede wszystkim prostota oraz wynikaj ca z niej skuteczno±. Zachowania instynktowne, takie jak unikanie zagro»enia czyli kolizji, przeszkód lub czynników zagra»aj cych funkcjonowaniu robota (wysoka temperatura, niebezpieczne substancje chemiczne) sprawdzaj si doskonale jako metoda przetrwania w nieprzyjaznym ±rodowisku. Korzystaj c z takiego podej±cia do sterowania robotami trudno jednak uzyska realizacj bardziej zªo»onych zada«, które wymagaj przynajmniej cz ±ciowego budowania modelu ±rodowiska. Jednym ze sposobów rozwi zania tego problemu jest koncepcja sterowania behawioralnego, które skrótowo mo»na okre±li jako my±l w sposób w jaki dziaªasz. Tutaj my±lenie traktowane jest jako podej±cie do sterowania i programowania robota jego dziaªanie nie jest opisywane jako reakcje na bod¹ce (czyli konkretne zdarzenia), ale jako zachowania wynikaj ce z obserwacji ±rodowiska. W takim podej±ciu kluczowe staje si nie opracowanie systemu reakcji, a dekompozycja zadania na zachowania sªu» ce do jego realizacji. 34

4.2. STEROWANIE BEHAWIORALNE ZESPOŠEM ROBOTÓW 35 4.2 Sterowanie behawioralne zespoªem robotów heterogenicznych W przypadku sterowania zespoªem robotów z wykorzystaniem zachowa«mo»liwe s dwa podej±cia. W pierwszym ka»dy robot mo»e realizowa wiele zachowa«wtedy s one dynamicznie (tzn. w trakcie realizacji zadania) przydzielane do poszczególnych robotów przez koordynatora. To podej±cie najlepiej stosowa, gdy wszystkie roboty s homogeniczne, czyli dysponuj takimi samymi mo»liwo±ciami, zestawem sensorów i efektorów. W tym przypadku proces przydziaªu ról mo»e uwzgl dnia zmieniaj ce si predyspozycje robotów w trakcie pracy takie jak cho by stan naªadowania akumulatorów, czy awaria jednego z podzespoªów. Drugie podej±cie polega na statycznym przydziale zachowa«dla robotów, które mo»e zosta zrealizowane na etapie okre±lania konguracji zdania. W tym przypadku okre±lenie ról bazowa b dzie na informacji o mo»liwo±ciach ka»dego robota. Dla przykªadu kryterium przydziaªu mo»e by moc obliczeniowa, pozycja pocz tkowa, czy dost pno± okre±lonego typu czujnika. 4.3 Przykªad zadania wielorobotowego Jako przykªad zadania wykorzystuj cego sterowanie zespoªem heterogenicznych robotów mobilnych zostaªo wybrane zadanie pchania pudªa, gdzie sukces mo»liwy jest jedynie przy ±cisªej wspóªpracy wszystkich elementów systemu robotycznego. Zadanie to jest szeroko znane w literaturze, jednak w wi kszo±ci przypadków byªo realizowane w sposób, który nie wymagaª ci gªej i nieprzerwanej kooperacji. Kluczowe w trakcie transportu jest to,»e obiekt jest pchany b d¹ ci gni ty przez robota w po» danym kierunku, który jednocze±nie nie mo»e straci kontaktu z obiektem. Omawiany efekt mo»na uzyska dzi ki m.in. otoczeniu obiektu przez grup robotów tak,»e nie jest mo»liwe jego wysuni cie si w niepo» danym kierunku [44, 57, 12]. Inne sposoby to np. sztywne uchwycenie obiektu przez roboty, czy oplecenie go lin [16]. Metoda, któr zdecydowano si stosowa jest rozwini ciem podej±cia okre±lanego jako pusherwatcher gdzie zadanie jest realizowane przez grup robotów z podziaªem na role [26]. Jeden robot, wyposa»ony w dalmierz laserowy odpowiada za okre±lenie pozycji i orientacji obiektu i na tej podstawie koordynowane jest dziaªanie pozostaªych robotów pchaczy, których dodatkowym zadaniem jest utrzymywanie kontaktu z transportowanym przedmiotem. Wybór tej metody podyktowany byª w du»ej mierze mo»liwo±ciami (tj. zestawem czujników) robotów ELEKTRON, gdy» wymaga ona i jednocze±nie werykuje dziaªanie wielu podzespoªów: dalmierza laserowego, u»ywanego do okre±lenia poªo»enia obiektu, czujników zbli»eniowych IR zamontowanych w zderzaku robotów pchaczy, u»ywanych do utrzymania styczno±ci z transportowanym obiektem, ukªadu jezdnego robotów, moduªu komunikacji WLAN,

4.4. OPIS ZADANIA 36 Rysunek 4.1: Schemat zadania pchania pudªa systemu oprogramowania, który musi pozwala na spójne sterowanie dziaªaniem wielu robotów. Jednocze±nie jest to zadanie, gdzie wyra¹nie wida konieczno± przydziaªu ró»nych ról dla poszczególnych robotów. Dodatkowo, poniewa» okre±lenie globalnej pozycji pudªa jest mo»liwe jedynie przy u»yciu dalmierza laserowego (nie mo»na do tego wykorzysta czujników krótkiego zasi gu), zrealizowanie zadania jest mo»liwe tylko przy jednym, okre±lonym przydziale ról. 4.4 Opis zadania Rozwa»ane jest nast puj ce zadanie: robot O wyposa»ony w dalmierz laserowy okre±la odlegªo± (OA) i orientacj (φ) obiektu. Roboty pchaj ce (L i P) wyposa»one s w m.in. 6 czujników odlegªo±ci daj cych odczyt w zakresie ok. 5... 5cm (rys. 4.1). W trakcie realizacji zadania robot O (prowadz cy) jest sterowany zdalnie przez operatora, natomiast roboty pchaj ce realizuj dwa zachowania: utrzymanie poªo»enia przy kraw dzi pudªa, pchanie z zadan pr dko±ci.

4.4. OPIS ZADANIA 37 Rysunek 4.2: Zachowania steruj ce ruchem robota pchaj cego Pierwsze z nich jest realizowane wyª cznie na bazie informacji z wªasnych czujników odlegªo±ci robota. Z powodu bardzo du»ych szumów pomiarowych oraz ró»nic charaterystyk pomi dzy egzemplarzami czujników nie jest w praktyce mo»liwe dokªadne wyznaczanie poªo»enia robota wzgl dem kraw dzi pudªa, gdy» byªoby ono obarczone du»ym bª dem i ta infomacja byªaby bezu»yteczna. Prost i skuteczn metod, która zostaªa zastosowana jest zliczanie ilo±ci kolejnych odczytów poni»ej ustalonego progu warto±ci (przyj te zostaªo 2cm) zaczyn j c od kraw dzi znajduj cej si bardziej wewn trz pudªa. Na tej podstawie wyznaczana jest skªadowa pr dko±ci obrotowej ruchu robota ω. Jest to zachowanie, które lokalnie generuje skªadow sterowania ruchem robota w oparciu wyª cznie o informacje z czujników danego robota (rys. 4.2). Realizacja drugiego zachowania odbywa si przez taki dobór wzgl dnych pr dko±ci w ruchu post powym w L i w P, aby φ, przy jednoczesnym zapewnieniu, aby przynajmniej jeden robot poruszaª si z maksymaln pr dko±ci liniow w max. Oznacza to tyle,»e aby ustawi pudªo w po» danym kierunku jeden robot musi jecha szybciej, a drugi wolniej. Jest to zachowanie, które generuje skªadow sterowania ruchem robota w opraciu wyª cznie o informacj o globalnym poªo»eniu pudªa, do której robot pchaj cy nie ma bezpo±redniego dost pu (rys. 4.2). Zasadnicz ró»nic w przyj tej metodzie w porównaniu z innymi pracami jest to,»e regulacja orientacji pudªa odbywa si w sposób ci gªy, bez wyró»niania fazy obrotu do zadanego k ta i dalej pchania na wprost. Dzi ki temu mo»liwe jest pªynne przesuwanie obiektu po linii wyznaczanej ruchem robota wodz cego. Rola robota prowadz cego polega na analizie danych z dalmierza laserowego i na ich podstawie okre±lenia poªo»enia i orientacji pudªa. Maj c do dyspozycji te informacje do robotów pchaj cych przesyªana jest zadana skªadowa pr dko±ci w ruchu post powym.

Rozdziaª 5 Opracowanie i implementacja algorytmów sterowania behawioralnego zespoªem heterogenicznych robotów mobilnych 5.1 Implementacja W celu realizacji zadania wielorobotowego zostaª stworzony program klienta Player, który bezpo±rednio ª czy si z serwerami dziaªaj cymi na trzech robotach. Do komunikacji wykorzystywana jest biblioteka C++ playerclient. Do wyznaczania poªo»enia i orientacji pudªa wykorzystywany jest sterownik laserfeaturex bazuj cy na danych z dalmierza laserowego. Wynikiem jego dziaªania jest tablica z lokalizacj we wspóªrz dnych ukªadu zwi zanego z dalmierzem laserowym spójnych obiektów tworz cych odcinki prostych. Ukªad ten pokrywa si z ukªadem zwi zanym ze ±rodkiem robota wodz cego O. Na podstawie wspóªrz dnych pocz tku i ko«ca odcinka wyznaczany jest k t φ oraz odlegªo± OA do pudªa. Przyjmuj c wspóªrz dne punktów B i E w ukªadzie ( x o, y o ) jako: B = (x B, y B ), E = (x E, y E ) dªugo± kraw dzi obiektu mo»na wyrazi jako: BE = (x E x B ) 2 + (y E y B ) 2 Na podstawie dªugo±ci odcinka BE dokonywane jest podstawowe stwierdzenie, czy znaleziony obiekt opisuje kraw d¹ pchanego pudªa o dªugo±ci 18cm: { dla BE 18cm 2cm pudªo dla BE 18cm > 2cm inny obiekt Wspóªrz dne ±rodka kraw dzi pudªa A wyra»aj si wzorem: ( xb + x E A = (x A, y A ) = 2, y B + y E 2 za± odlegªo± ±rodka robota O do ±rodka kraw dzi pudªa: OA = (x A ) 2 + (y A ) 2 38 )

5.1. IMPLEMENTACJA 39 Na podstawie twierdzenia o iloczynie skalarnym wektorów OA i BE: cos OAE = (x B x E ) x A + (y B y E ) y A OA BE i st d k t φ wyra»ony w stopniach: φ = arccos (cos OAE) 18 π 9 Zadanie mo»na traktowa jako ukªad regulacji, gdzie uchybem jest k t φ, natomiast od wyj±cia regulatora u zale»y ró»nica pr dko±ci robotów pchaj cych. Pocz tkowo planowane byªo zastosowanie prostego regulatora proporcjonalnego, jednak w wyniku eksperymentów okazaªo si konieczne wprowadzenie tak»e czªonu caªkuj cego, którego zadaniem jest likwidacja uchybu statycznego powstaj cego w wyniku ró»nic w tarciu o podªo»e lewego i prawgo robota pchaj cego. Zastosowany ukªad regulacji w idealnym przypadku jest ukªadem ci gªym, jednak dla celów implementacji musiaª zosta zrealizowany jako ukªad dyskretny. Czas próbkowania T p wynosi 1ms (czyli cz stotliwo± 1Hz), co odpowiada domy±lnej warto±ci w oprogramowaniu Player/Stage, jednak mo»e by ªatwo modykowane. Przy poruszaniu si robota z maksymaln pr dko±ci w czasie 1ms pokonuje on jedynie ok. 2.5cm, zatem taki czas próbkowania wydaje si wystarczaj cy. Dla celów zapisu formalnego algorytmu wprowad¹my oznaczenia: ω L, ω P pr dko± obrotowa odpowiednio lewego i prawego robota pchaj cego, w L, w P pr dko± liniowa odpowiednio lewego i prawego robota pchaj cego, Prawo sterowania mo»na zapisa w nast puj cy sposób (indeks górny i oznacza i-t iteracj algorytmu dyskretnego): ( ) ( ) u i = u i 1 + K 1 + Tp φ + K Tp 2T t π/2 2T i 1 { w i L = w max (1 u i ) φ π/2 w i P = w max dla φ > { w i L = w max w i P = w max dla φ = { w i L = w max w i P = w max (1 u i ) dla φ < Pr dko±ci obrotowe ω L i ω P s tak ustalane, aby orientowa robota do poªo»enia, w którym kraw d¹ pudªa b dzie znajdowaªa si po±rodku przedniego zderzaka robota. Jako kryterium przyjmowana jest tutaj liczba kolejnych odczytów z czujników pocz wszy od znajduj cego si bli»ej ±rodka pudªa, które nie przekraczaj ustalonego progu 2cm. Wedle tego kryterium pr dko± obrotowa robota jest ustalana na warto± z tablicy eksperymentalnie dobranych warto±ci. W momencie kiedy odlegªo± OA < 1m ruch robotów jest wstrzymywany. Warto podre±li warstwow struktur algorytmu realizacja polega na dekompozycji dzia- ªa«robota przy wykorzystaniu skªadowych pr dko±ci jego ruchu (rys. 5.1):

5.2. REALIZACJA ZADANIA NA SYMULATORZE 4 Nadrzedny regulator PI: w L ϕ uchyb w L, w P sterowania w P ω L ω P ω L, w L δ LL, δ LP ω P, w P δ PL, δ PP Rysunek 5.1: Warstwowa dekompozycja sterowania i zachowa«robotów obrotow, za które odpowiadaj jedynie informacje lokalne dla robota, liniow, za któr odpowiada informacja o globalnym poªo»eniu i orientacji przepychanego pudªa. Konieczno± eksperymentalnego doboru wspóªczynników zwi zanych z regulacj pr dko±ci wynika z faktu,»e podczas pchania tracona jest przyczepno± kóª robota do podªo»a, zatem informacja z enkoderów umieszczonych na osiach kóª staje si w praktyce maªo u»yteczna. Z tego samego powodu opieranie zadania na bardziej rozbudowanym modelu byªoby niecelowe. Komunikacja programu steruj cego z robotami realizowana jest z u»yciem klasy Player- MultiClient biblioteki C++ z pakietu Player/Stage, która jest konieczna do zapewnienia dziaªania p tli gªównej programu jako przesªanie sterowa«w reakcji na przetworzenie danych z czujników wszystkich robotów (tab. 5.1). 5.2 Realizacja zadania na symulatorze Zadanie zostaªo przygotowane z wykorzystaniem trójwymiarowego symulatora Gazebo, modeluj cego zachowanie i oddziaªywania mi dzy obiektami. Zrzut ekranu przedstawia przebieg symulacji widoczne s okna wizualizacji (lewa górna cz ± ), aplikacji rejestruj cej ruch robotów i pudªa (lewa dolna cz ± ), programu playerv do r cznego sterowania robota wodz cego O (prawa górna cz ± ) oraz aplikacji steruj cej ruchem robotów pchaj - cych z zaznaczonym»óªtym kolorem odcinkiem reprezentuj cym poªo»enie pudªa (prawy dolny róg) (rys. 5.2). 5.3 Eksperymenty W trakcie eksperymentów robot wodz cy byª nieruchomy, za± zadaniem robotów pchaj - cych byªa wspóªpraca maj ca na celu minimalizowanie k ta φ pomi dzy prostopadª do pudªa poprowadzon z jego ±rodka a póªprost poprowadzon z tego punktu w kierunku ±rodka robota wodz cego (rys. 4.1). Pchane pudªo miaªo wymiary ok. 18 2 2cm i wag kilkunastu kilogramów (rys. 5.9, 5.1).

5.3. EKSPERYMENTY 41 #include <playerclient.h> // inicjalizacja poª czenia z robotami PlayerClient watcher("pcm1",6665,player_transport_tcp); PlayerClient pusher1("pcm3",6665,player_transport_tcp); PlayerClient pusher2("pcm4",6665,player_transport_tcp); // tworzenie obiektu do zarz dzania komunikacj PlayerMultiClient mc; mc.addclient(&watcher); mc.addclient(&pusher1); mc.addclient(&pusher2); while(true) { // odczytanie stanu wszystkich robotów do { mc.read(); } while(!watcher.fresh!pusher1.fresh!pusher2.fresh); watcher.fresh = pusher1.fresh = pusher2.fresh = false; } // przetworzenie danych z czujników // i~przesªanie sterowa«do robotów //... Tabela 5.1: Schemat programu steruj cego wieloma robotami Poni»ej zamieszczono uzyskane trajektorie transportowanego obiektu (z zaznaczaniem po- ªo»enia co 1 sekund) oraz wykresy odczytów z czujników odlegªo±ci robotów pchaj cych dla dwóch przebiegów na rzeczywistych robotach oraz jednego przebiegu na symulatorze (rys. 5.4, 5.6, 5.8). Pudªo byªo pocz tkowo ªagodnym ªukiem nakierowywane na robota wodz cego, a nast pnie pchane w kierunku celu po praktycznie prostym odcinku. Ostatni fragment (odlegªo± poni»ej 1m) to pchanie przez jednego robota maj ce na celu ustawienie pudªa mo»liwie równolegle do robota wodz cego tak, aby znajdowaªo si w korzystnej pozycji przed rozpocz ciem kolejnego fragmentu ±cie»ki. Maªe warto±ci odczytów z czujników IR odpowiadaj czujnikom zasªoni tym przez pudªo, natomiast du»e to odczyty z czujników, przed którymi znajdowaªa si wolna przestrze«. Odczyty z czujników znajduj cych si od strony ±rodka pudªa nie przekraczaj kilku centymetrów, natomiast odczyty z czujników znajduj cych si po zewn trznej stronie nie s mniejsze ni» kilkadziesi t centymetrów znacznie powy»ej progu ustalonego na 2cm. Kraw d¹ pudªa przez wi kszo± czasu znajdowaªa si przed trzecim czujnikiem (licz c od wewn trznej strony) umieszczonym w przednim zderzaku robota. Warto±ci odczytów dla tego czujnika oscyluj pomi dzy bardzo maªymi i bardzo du»ymi co odzwierciedla naprzemienne zasªanianie i odsªanianie czujnika przez pudªo. W dalszej kolejno±ci umieszczono wykresy k ta φ traktowanego w tym przypadku jako uchyb regulacji (rys. 5.3, 5.5, 5.7). W celu likwidacji staªej ró»nicy k ta φ konieczne

5.3. EKSPERYMENTY 42 Rysunek 5.2: Realizacja pchania pudªa na symulatorze okazaªo si wprowadzenie do równa«stanowi cych w istocie regulator proporcjonalny czªonu caªkuj cego czego konsekwencj jest tak»e widoczne przeregulowanie przebiegu na wykresach. Wykresy pr dko±ci obrotowej robota (ω L i ω R ) przedstawiaj proces regulacji orientacji robota tak, aby kraw d¹ pudªa znajdowaªa si na ±rodku jego zderzaka. Warto±ci pr dko±ci oscyluj pomi dzy ±.2 [rad/sek], co odpowiada skr tom w prawo i lewo koryguj cym ustawienie robota wzgl dem kraw dzi pudªa. Kierunek obracania byª zmieniany co okoªo.7s. Testowana byªa komunikacj mi dzy robotami bazuj ca na protokole TCP oraz UDP, jednak nie zostaªy zaobserwowane»adne ró»nice zwi zane z wydajno±ci czy niezawodno±ci. Komunikacja oparta na protokole TCP w sieciach WLAN mo»e generowa du»e opó¹nienia zwi zane z konieczno±ci retransmisji utraconych pakietów. W niektórych przypadkach mo»e okaza si korzystne zrezygnowanie z niezawodno±ci transmisji dla niedopuszczenia mo»liwo±ci powstania du»ych opó¹nie«. W czasie realizacji tego zadania wymiana danych dla wszystkich robotów nie przekraczaªa ª cznie 1kB/sek. W trakcie realizacji zadania przez rzeczywiste roboty pr dko±ci w i oraz ω i uzyskiwane na podstawie odometrii z enkoderów umieszczonych na osiach kóª obarczone byªy du»ym bª dem co wynikaªo z faktu po±lizgu robota w trakcie pchania ci»kiego pudªa. Zada-