prof. dr hab. inĪ. Krzysztof Kozáowski dr inĪ. Jarosáaw Majchrzak
dr inĪ. Maciej Michaáek dr inĪ. Dariusz Pazderski Politechnika PoznaĔska,
Katedra Sterowania i InĪynierii Systemów
SYSTEM PROGRAMOWANIA I STEROWANIA
MODUàOWEGO ROBOTA MOBILNEGO DO ZASTOSOWAē
TRANS-PORTOWYCH W POMIESZCZENIACH
Praca naukowa finansowana ze Ğrodków na nauk Ċ w latach 2007–2011 jako projekt ba-dawczy rozwojowy R02 009 02.
Praca prezentuje koncepcjĊ oraz realizacjĊ wybranych elementów moduáowego robota mobilnego do zastosowaĔ transportowych w pomieszczeniach. Zaprezen-towano konstrukcjĊ, elementy systemu sterowania oraz sposób programowania zadaĔ dla robota. W projekcie uwzglĊdniono wymogi bezpieczeĔstwa ludzi oraz zaáoĪenie przyjaznego systemu programowania.
PROGRAMMING AND CONTROL SYSTEM OF MODULAR MOBILE ROBOT TO TRANSPORTATION TASKS IN CLOSED ENVIRONMENTS This paper presents an idea and application of selected elements of modular mo-bile robot used for transportation tasks in closed environments. Some details with respect to robot’s mechanical structure, its control system and programming lan-guage are described. Additionally, requirements for safety system are formulated and assumptions of user-friendly system for robot programming are discussed. 1. ROBOT MOBILNY DO ZADAē USàUGOWYCH
Przedmiotem tej pracy jest opis konstrukcji robota m obilnego, który zosta á zaprojektowany i wykonany w Katedrze Sterowania i InĪynierii Systemów na Wydziale Informatyki Politech-niki PoznaĔskiej. Na etapie zaáoĪeĔ projektowych przyjĊto, Īe celem robota bĊdzie realizacja usáug, gáównie o charakterze transportowym . Robot jest przeznaczony do transportu áadun-ków o ciĊĪarze do 120kg, co determ inuje jego gabaryty, m oc zainstalowanych napĊdów, po-jemnoĞü pokáadowego Ĩródáa energii. Zdefiniowany zakres wykorzystania robota determinuje architekturĊ systemu sterowania, której postawą jest komputer pokáadowy klasy PC z otocze-niem sieciowym i sterownikami motoryki i wieloprocesowym oprogramowaotocze-niem. Takie roz-wiązanie poprawia efektywno Ğü tworzenia oprogram owania. Za áoĪenia obejm ują tak Īe wprowadzenie jĊzyka programowania robota oraz interfejsu przyjaznego uĪytkownikowi.
Rozpatrując róĪne struktury kinem atyczne robotów ko áowych i ich szczegó áowe wa-rianty ostatecznie wybrano do wykonania klasyczny pojazd dwuko áowy z nap Ċdem bezpo-Ğrednim w uk áadzie ró Īnicowym oraz pasywnym sam onastawnym ko áem podporowym . Struktura ta pozwala uzyska ü duĪą mobilnoĞü (np. m oĪliwoĞü zmiany orientacji robota bez zmiany pozycji) i zadowalaj ącą stateczno Ğü m echaniczną konstrukcji. System sterowania i sensoryki zostaá zaprojektowany w sposób m oduáowy i wraz z m echanizmami i elementami wykonawczymi robota um oĪliwia holowanie innego pojazdu pasywnego, np. przyczepy. UmoĪliwia to odpowiednio przygotowane sterowane urz ądzenie zaczepowe. W wyniku wy-korzystania obrotowego po áączenia zaczepowego powstaje pojazd dwusegm entowy, którego
sterowanie oparte jest na kontroli pojazdu dwuko áowego. W wyniku wykorzystania po áącze-nia zaczepowego ustalanego pojazd podstawowy stanowi bazĊ zestawu zespolonego rotujące-go razem z nim.
Na konstrukcjĊ moduáowego robota skáada siĊ (rys. 1): A. korpus wraz z wewnĊtrznymi elementami noĞnymi,
B. mechanizmy zawieszeĔ robota z wahaczam i i uk áadami sprĊĪynującymi, oraz m oco-waniami hamulców,
C. system m echanicznych zderzaków, um ieszczonych wokó á robota z prze áącznikami stanu.
D. ukáad podnoszenia i opuszczania zaczepu z m oĪliwoĞcią blokowania sprz Ċgu z przy-czepą,
E. ukáad pozycjonowania (kąta nachylenia) skanera pomiaru odlegáoĞci.
Na etapie projektowania konstrukcja by áa analizowana pod k ątem optymalizacji wagi i wy-miarów zewn Ċtrznych. Dzi Ċki zastosowaniu konstrukcji pow áokowej, która zast ąpiáa kon-strukcjĊ ram ową wcze Ğniejszych wersji projektu robota uzyskano redukcj Ċ ci ĊĪaru o ok. 50 %.
Rys. 1. Robot mobilny zatwierdzony do realizacji o roboczej nawie LEOnardi
2. SYSTEM PROGRAMOWANIA ROBOTA
Warto zwróciü uwagĊ, Īe odróĪnieniu od klasycznych m anipulatorów przemysáowych do tej pory nie powsta áy standardy program owania robotów m obilnych. W projekcie za áoĪono, Īe system programowania robota oparty jest o klasyczną metodĊ uczenia ĞcieĪki przejazdu (ana-logicznie jak realizuje siĊ to dla uĪytkowych robotów manipulacyjnych). W wyniku tego pro-cesu powstaje uporządkowany zestaw punktów reprezentujących ĞcieĪkĊ opisaną przez pozy-cjĊ i orientapozy-cjĊ robota w ka Īdym z nich. Podczas procesu program owania robota nast Ċpuje zapis punktów charakterystycznych przez program ator, którym jest aplikacja konsoli opera-torskiej (rys. 2), funkcjonująca w systemie operacyjnym Windows. Jest to moĪliwe dziĊki áączeniom komunikacyjnym (rys. 3) pom iĊdzy komputerem programatora i kom puterem po-káadowym robota, sterownikiem motoryki robota i sterownikam i osi oraz odpowiednio przy-gotowanym oprogram owaniem pracuj ącym na ka Īdym z tych urz ądzeĔ. W ynikiem
progra-Skaner bezpieczeĔstwa
Hak do holowania
Stóá unoszony
Zderzak Zderzak boczny
Tabela 1. Przykáad instrukcji w jĊzyku LPL Nr Instrukcja 1 maxWheelVelocity(80) 2 maxRobotVelocity(500) 3 motorOn() 4 wheelVelocity(50, -50) 5 sleep(5, 500000000) 6 wheelVelocity(0, 0) 7 sleep(0, 100000000) 8 wheelVelocityPercent(100, 100) 9 sleep(5, 0) 10 robotVelocity(200) 11 ptp(1000, 1000, 90, 50) 12 ptpRelative(-1000, 1000, 0, 50) 13 motorOff()
mowania robota jest zestaw instrukcji zapisany w postaci pliku na dysku konsoli operator-skiej.
Rys. 2. Widok okna konsoli operatorskiej a) w trybie pracy rĊcznej, b) w trybie pracy automatycznej Konsola ta um oĪliwia sterowanie robotem w trybie r Ċcznym i zapisywanie punktów ĞcieĪki robota w procesie uczenia oraz odtwarzanie jej w trybie autom atycznym jako zlecenie do wy-konania dla Ğrodowiska uruchomieniowego i generatora ĞcieĪki pracującego na kom puterze pokáadowym robota.
2.1. JĊzyk programowania robota
Zapis programu do wykonania przez generator ĞcieĪki opiera si Ċ na j Ċzyku programowania robota. Tryb pracy autom atycznej ma sáuĪyü samoczynnemu sterowaniu robotem , w sposób powtarzalny, tak aby m óc weryfikowa ü ró Īne algorytm y zaim plementowane w sterowniku pokáadowym. Sterowanie to m a byü realizowane przez sterownik pokáadowy, a komputer po-káadowy ma nadzorowaü ten proces, wysy áając kolejne polecenia. W prowadzony jĊzyk pro-stych polece Ĕ o nazwie LeoOS Programming Language (w skrócie LPL) [Fel2008] zosta á uproszczony do tego stopnia, iĪ jedynymi dopuszczalnymi konstrukcjami są wywoáania funk-cji. Skáadnia w tym aspekcie wzorowana jest na skáadni jĊzyka C. Zdefiniowano dwa termina-le: term inal identyfikator reprezentuje prawid áową instrukcj Ċ wyst Ċpującą w j Ċzyku i moĪe sáuĪyü jako nazwa funkcji lub teĪ nazwa zmiennej oraz
terminal liczba, który reprezentuje liczb Ċ caákowitą z opcjonalnym znakiem . Dopuszczalne znaki zdefinio-wane są nastĊpująco:
identyfikator = [A-Za-z_][A-Za-z_0-9]*,
liczba = "-"{0,1}[0-9]+.
Bezkontekstowa gramatyka jĊzyka skáada siĊ z nastĊ-pujących produkcji:
args o (liczba ("," liczba)*)*
funkcja o identyfikator "(" args ")" ";" linia o funkcja*
Produkcja args definiuje dopuszczalne argum enty, ja-kie moĪe przyjmowaü funkcja. Produkcja funkcja
starto-wą definiująca jedną liniĊ kodu. Gram atyka umoĪliwia zdefiniowanie polece Ĕ dostĊpnych w jĊzyku LPL. Prawidáowo skonstruowany program zgodny z gram atyką, skáada siĊ z wielu li-nii kodu, zgodnych z produkcją startową. Program wykorzystujący polecenia prezentuje tabe-la 1. W tabeli 2 przedstawiono wybrane polecenia konfiguracyjne i ruchowe oraz funkcje z áo-Īone o charakterze makroinstrukcji.
Tabela 2. Zestawienie wybranych poleceĔ
Lp. Naz wa polecenia Opis Parametry
Polecenia konfiguracyjne
1 m otorOn(); Programowe wáączenie silników robota - 2 m otorOff(); Programowe wyáączenie silników robota -
3 m axWheelVelocity Ustawienie maksymalnej prkóá robota ĊdkoĞci dla 1: prĊdkoĞü wyraĪona w [obr/min] 4 m axRobotVelocity Ustawienie maksymalnej prĊdkoĞci robo-ta 1: prĊdkoĞü wyraĪona w [mm/s]
Polecenia wykonywania ruchu
5 wheelVelocity Zadawanie bezpoĞrednich prĊdkoĞci kóá robota 1: prĊdkoĞü lewego koáa w [obr/min], 2: prĊdkoĞü prawego koáa w [obr/min] 6 wheelVelocityPercent Zadawanie bezpoĞrednich prĊdkoĞci kóá robota w procentach w stosunku do
prĊd-koĞci maksymalnej robota
1: prĊdkoĞü lewego koáa, 2: prĊdkoĞü prawego koáa; prĊdkoĞci wyraĪone są w procentach prĊdkoĞci maksymalnej 7 ptp Wykonanie zadania ruchu do punktu
1: wspóárzĊdna X punktu w [mm], 2: wspóárzĊdna Y punktu w [mm], 3: orientacja robota w stopniach, 4: promieĔ sąsiedztwa wokóá punktu w [mm]
8 ptpRelative Wykonanie zadania ruchu do punktu wzglĊdem aktualnego poáoĪenia
parametry: 1 — przesuniecie robota w osi X w [mm], 2 — przesuniecie robo-ta w osi Y w [mm], 3 — zmiana orien-tacji robota w stopniach, 4 — promieĔ sąsiedztwa punktu w [mm]
9 sleep Usypianie procesu sterowania, sáuĪy do przedáuĪania poprzedniego polecenia
za-dania prĊdkoĞci na koáa 1: czas w [sek], 2: czas w [nsek] Polecenia funkcji záoĪonych
10 onAskPlace Wáącz poszukiwanie oznaczenia miejsca 11 offAskPlace Wyáącz poszukiwanie oznaczenia miejsca 12 goGetPower Dokowanie do Ĩródáa zasilania
13 goLockTrailer Dojazd i pojecie przyczepy 14 unLockTrailer Odczepienie przyczepy
2.2. Architektura systemu programowania
System programowania robota sk áada siĊ z oprogram owania konsoli operatorskiej, oprogra-mowania komputera pokáadowego oraz sterownika m otoryki i sterowników osi. Architektura pokáadowego systemu operacyjnego wynika z opisu Ğrodowiska otaczającego komputer po-káadowy czyli sterownik nadrzĊdny, który funkcjonowaü musi jako poĞrednik pomiĊdzy ope-ratorem i sterownikiem motoryki. Realizacja tego zadania narzuca zastosowanie rozwi ązania kaskadowej hierarhii typu klient-serwer. Operator korzystaj ący z konsoli operatorskiej jest klientem, który zleca zadania dla robota pe ániącego rolĊ usáugodawcy. Sterownik nadrz Ċdny (komputer pok áadowy) pe áni role po Ğrednika i t áumacza, i dla operatora jest on serwerem ,
z kolei dla sterownika pokáadowego (motoryki) jest on klientem. ArchitekturĊ komunikacyjną systemu programowania robota przedstawia rys. 3.
Rys. 3. Architektura klient-serwer — schemat blokowy
Komputer pokáadowy w przypadku program owania robota m a wykonywaü kilka podstawo-wych zada Ĕ. Pierwsze zadanie to zapewnienie kom unikacji pom iĊdzy oprogram owaniem komputera pokáadowego – sterownika nadrz Ċdnego a operatorem robota do sterowania nim z poziomu aplikacji pracującej jako konsola operatorska (KO). Z tego powodu oprogram owa-nie wchodz ące w sk áad system u sterowania udost Ċpnia bezprzewodowy dost Ċp dla konsoli w oparciu o protokóá TCP/IP. DziĊki temu operator korzystając z komputera lub PDA, wypo-saĪonych w karty w standardzie IEEE802.11b/g, b Ċdzie mógá sterowaü robotem bez wyko-rzystywania okablowania.
Drugim podstawowym zadaniem jest zapewnienie komunikacji pomiĊdzy sterownikiem motoryki (MC) a kom puterem pok áadowym (PC). Kom unikacja ta przebiega w oparciu o przewodową przemysáową magistralĊ komunikacyjną CAN, pracuj ącą w czasie rzeczywi-stym. Kolejne zadania peániące waĪne role w procesie programowania robota, to:
zdalne sterowanie rĊczne, realizowane przez operatora,
programowanie robota w oparciu o uczenie wykorzystujące sterowanie rĊczne z obecnoĞcią operatora przy robocie,
realizacja zaprogramowanych zadaĔ sterowania (np. zadanie generowania i wyko-nywania ĞcieĪki),
nadzorowanie pracy robota i system u sterowania w celu reagowania na stany wyj ąt-kowe.
W wyniku zaprogramowania funkcji ruchowych robota w ka Īdym trybie realizuje je sterow-nik motoryki (MC), który za pom ocą magistrali CAN kom usterow-nikuje siĊ ze sterowsterow-nikam i osi. Sterownik motoryki realizuje w áaĞciwy algorytm sterowania dla wyró Īnionego typu pojazdu nieholonomicznego. Oprogramowanie tego sterownika zawiera wbudowane praktyczne roz-wiązania sterowania znane z literatury przedmiotu (omówione w rozdz. 4.2), takie jak: stabil-ny i bezpieczstabil-ny dojazd do zadanego punktu, odtwarzanie ĞcieĪki záoĪonej z uporządkowanej liczby punktów, realizacja om ijania przeszkód zidentyfikowanych przez oprogram owanie komputera pokáadowego i zintegrowane z nim sensory, itp.
3. KONCEPCJA SYSTEMU STEROWANIA I JEJ REALIZACJA 3.1. ZaáoĪenia
Zadania, do których robot zosta á przeznaczony s ą zadaniami programowego odtwarzania za-planowanej ĞcieĪki ruchu w otoczeniu przeszkód. Aby by áo to m oĪliwe, konieczna jest inte-gracja wielu system ów sensorycznych z system em sterowania na pok áadzie robota. Sprz Ċto-wym integratorem wszystkich funkcji robota jest jego kom puter pokáadowy, natomiast pro-gramowym jest system zadaniowo - operacyjny oraz zaprojektowane i uruchom ione w nim aplikacje. Na kom puterze pokáadowym pracuje system zadaniowo – operacyjny, który s áuĪy jako platform a do realizacji z áoĪonych m oduáów zadaniowych, takich jak: program owanie i odtwarzanie funkcji ruchowych robota, sterowanie autom atyczne, przetwarzanie i integracja
danych sensorycznych z ró Īnych sensorów i system ów sensorycznych, lokalizacja wzgl Ċdna i bezwzglĊdna, sterowanie dokowaniem w oznaczonych m iejscach przeznaczonych do sam o-czynnego poboru energii oraz zaczepiania i wyczepienia przyczep, kontrol Ċ przeszkód w ob-szarze najbliĪszego otoczenia robota i wykrywanie kierunków i obszarów kolizyjnych z prze-szkodami podczas wykonywania zada Ĕ ruchowych. Aplikacje w postaci procesów podzielo-nych na wątki są wykonawcami zaprojektowapodzielo-nych moduáów zadaniowych.
ZdolnoĞü do realizacji zada Ĕ przez robota wynika z odpowiednio przygotowanej kon-cepcji zadaniowo – operacyjnego system u robota oraz jej realizacji. System ten jest urucho-miony na komputerze pokáadowym i korzysta z zasobów klasycznego system u operacyjnego Linux. KaĪde zadanie realizowane w tym systemie operacyjnym jest podporządkowane regu-áom budowy systemu zadaniowego jako caáoĞci.
Podstawą system u s ą wzorce procesów i kolejek kom unikatów, które s ą atrybutam i wykonywalnych procesów. Powstaje w ten sposób system wielozadaniowy, wieloprocesowy i wielowątkowy. Uruchomienie kolejnego moduáu zadaniowego w systemie opiera siĊ na wy-korzystaniu wzorca procesu oraz jednego lub kilku wzorców kolejek kom unikacyjnych, sáu-Īących do wymiany danych pomiĊdzy procesami, i dalej, zapisanego w j Ċzyku programowa-nia wáaĞciwego kodu zadaprogramowa-nia, które moĪe byü podzielone z kolei na podzadaprogramowa-nia - w ątki. Naj-waĪniejszą cechą systemu jest wprowadzenie braku wspó ádzielenia przez procesy jakiejkol-wiek informacji lub zasobów, a synchronizacja procesów odbywa si Ċ za pom ocą komunika-tów. Moduáy zadaniowe nazywane są agentami. Atrybutami agentów (poĞredników) są ĞciĞle zdefiniowane pomiĊdzy nimi kanaáy komunikacyjne jako kolejki FIFO oraz algorytm y funk-cjonalne kaĪdego z agentów.
Tak zdefiniowany system zdaniowo-operacyjny u áatwia zespo áowe tworzenie opro-gramowania obejmującego wiele zada Ĕ dla robota oraz dostarcza narz Ċdzia do realizacji do-wolnie zaprojektowanej komunikacji pomiĊdzy agentami.
3.2. Postaü systemu sterowania
Moduáowy system sterowania oparty jest o skáadniki sprzĊtowe oraz aplikacje wspóápracujące w ram ach zada Ĕ na poszczególnych elem entów sprz Ċtowych wym ieniających dane si Ċ po-przez dedykowane sieci kom unikacyjne (rozwi ązanie sprz Ċtowo-programowe) lub popo-przez system dynam icznych kolejek kom unikacyjnych (rozwi ązanie program owe). W pierwszym przypadku wykorzystywane s ą sieci: ethernet z dost Ċpnym kablowo-radiowym osprz Ċtem i niezbĊdnym oprogramowaniem konfiguracyjnym oraz sieci CAN w dwóch postaciach: m a-gistrala sterowa Ĕ STER-CAN oraz m aa-gistrala sensoryczna SENSO-CAN. W drugim przy-padku s ą to kolejki kom unikatów jako produkty program owania dynam icznego okre Ğlone wzorcem komunikatów pomiĊdzy procesami oraz ich wątkami w wieloprocesowym systemie sterowania. Podstawowym i elem entami system u sterowania s ą: sterownik m otoryki wraz z sterownikami nap Ċdów robota, kom puter pok áadowy, którego oprogram owanie system owe oparte jest o klasyczny system operacyjny oraz wielow ątkowe aplikacje realizuj ące zadania programowania, odtwarzania zadania, generowania ĞcieĪki lub trajektorii. G áównym elemen-tem syselemen-temu sterowania jest pok áadowy komputer nadrzĊdny, wypeániający role serwera na-stĊpujących zadaĔ:
x Sterowanie rĊczne w oparciu o joystick programowy w konsoli operatorskiej (rys. 2a), umoĪliwiając kontrolowane prowadzenie robota w przestrzeni prĊdkoĞci liniowej i ką-towej lub w przestrzeni prĊdkoĞci kóá robota
x Sterowanie rĊczne z zapam iĊtywaniem punktów przejechanej ĞcieĪki, z m oĪliwoĞcią korekty pozycji kaĪdego zapamiĊtanego punktu
x Sterowanie automatyczne w zamkniĊtej pĊtli sprzĊĪenia zwrotnego z zadanym i punk-tami ĞcieĪki z prostą korekcją sensoryczną
x Sterowanie automatyczne w zamkniĊtej pĊtli sprzĊĪenia zwrotnego z uwzgl Ċdnieniem zaawansowanych systemów sensoryki wizyjnej oraz pom iarów odleg áoĞci i lokalnej chwilowej mapy otoczenia.
4. STEROWANIE NISKOPOZIOMOWE 4.1. Architektura sterownika pokáadowego
Sterownik pokáadowy robota to urządzenie odpowiedzialne za wykonywanie polece Ĕ przeka-zywanych z kom putera pokáadowego i realizacji za pom ocą elementów wykonawczych, ste-rowników osi po áączonych z silnikami wykonawczymi, realizacji pomiarów i tworzenia sys-temu pom iarowego opartego o odom etryczne przetworniki pom iarowe. Zasadnicz ą funkcj ą tego sterownika jest realizacja elem entarnego zadania sterowania za pom ocą niskopoziomo-wego algorytmu sterowania z zaprogramowaną i parametryzowaną strategią.
4.2. Algorytm sterowania niskopoziomowego
Dla projektowanego robota moĪna wyróĪniü nastĊpujące zadania ruchu:
autonomiczny przejazd przez zbiór punktów referencyjnych/przejazdowych w Ğrodowisku z przeszkodami,
parkowanie w stacji áadowania akumulatorów, podjazd i podjĊcie pasywnej przyczepy,
ruch z przyczepą zespoloną z robotem.
Pierwsze z nich związane jest z praktycznym uproszczeniem zagadnienia odtwarzania w peáni zdefiniowanej ĞcieĪki/trajektorii. Polega ono na zaplanowaniu przejazdu przez zbiór punktów St= {q0; q1t; : : : ; qNt}, taki, Īe pierwszy punkt pokrywa siĊ ze stanem początkowym robota,
natomiast punkt qNt definiuje docelową pozycjĊ i orientacjĊ pojazdu. W pracy [MiKo2009_1]
zaproponowano prosty algorytm planowania i nast Ċpującego po nim etapu realizacji przejaz-du przez punkty ze zbioru St z zadaną i odcinkami staáą prĊdkoĞcią postĊpową oraz z etapem
páynnego zatrzymania robota w punkcie docelowym . Etap planowania polega na okre Ğleniu orientacji referencyjnych w poszczególnych punktach po Ğrednich qit dla i = N–1...,1
rozpo-czynając obliczenia od punktu docelowego, jak zaznaczono na schemacie z rys. 4.
Rys. 4. Strategia planowania ruchu przez zbiór punktów przejazdowych
Przedstawiona koncepcja planera ruchu oraz m etoda sterowania ze sprz ĊĪeniem zwrotnym wynika z zastosowania i rozszerzenia autorskiej metody VFO (metody orientowania pól
wek-torowych). ElastycznoĞü proponowanego podej Ğcia wynika z m oĪliwoĞci swobodnego okre-Ğlania strategii przejazdu robota (przodem /tyáem) przez bie Īący punkt ze zbioru St oraz
z moĪliwoĞci prostego i praktycznie u Īytecznego kszta átowania stanów przej Ğciowych po-przez wykorzystanie tzw. efektu naprowadzania pojazdu i zwi ązanego bezpo Ğrednio z wartoĞcią parametrów sterownika VFO. Przykáadowe wyniki dziaáania systemu planowania i realizacji ruchu z róĪnymi strategiami przejazdu przez punkty poĞrednie przedstawia rys. 5.
Rys. 5. Przykáadowy wynik dziaáania algorytmu planowania i realizacji ruchu przez zbiór punktów przejazdowych z wykorzystaniem algorytmu VFO
Zadanie parkowania robota zwi ązane jest z problem em sterowania do punktu, które dla sys-temów z na áoĪonymi wi Ċzami nieholonomicznymi jest problem em nietrywialnym. W pracy [MiKo2009_1] zaproponowano m etodĊ sterowania do punktu dla robota o kinem atyce (2,0) z napĊdem ró Īnicowym przy czym uwzgl Ċdniono ograniczenia na dopuszczalne pr ĊdkoĞci napĊdów pojazdu. PrĊdkoĞci dopuszczalne mogą tu wynikaü z fizycznych ograniczeĔ związa-nych z zastosowanymi napĊdami lub te Ī mogą zostaü naáoĪone sztucznie (program owo) tak, aby zachowaü warunki bezpiecznego ruchu robota w rzeczywistym Ğrodowisku z przeszko-dami i w obecnoĞci czáowieka.
PodjĊto takĪe problem sterowania z czasie sko Ĕczonym, które gwarantuje zbie ĪnoĞü uchybów w skoĔczonym interwale czasowym W [0, T], przy czym horyzont sterowania T < v moĪna oszacowaü a priori. Ponadto zastosowany algorytm sterowania zapewnia wi Ċk-szą odporno Ğü uk áadu zamkniĊtego (w porównaniu ze stabilizatoram i asymptotycznymi) na pewnego rodzaju d áugotrwaáe zakáócenia dziaáające na pojazd podczas ruchu. Rys. 6 przed-stawia wyniki sym ulacyjne zaproponowanego uk áadu sterowania FT-VFO dla zadania par-kowania równolegáego.
Rys. 6. Przebiegi uchybów oraz ograniczonych sygnaáów sterujących uzyskane w ukáadzie sterowania FT-VFO (Finite-Time-VFO) dla robota z napĊdem róĪnicowym
Rys. 7. Definicja zmiennych konfiguracyjnych robota przegubowego oraz dwa sposoby mocowania przyczepy do dwukoáowego ciągnika z napĊdem róĪnicowym – mocowanie osiowe (lewy) i pozaosiowe (prawy) Ruch robota m obilnego z do áączoną przyczepą naleĪy do zada Ĕ, których trudno Ğü realizacji wynika z obecno Ğci wiĊzów nieholonomicznych oraz ze strukturalnej niestabilno Ğci podsys-temu reprezentowanego przez kąt skrĊcenia przyczepy przy manewrach cofania (skáadanie siĊ przyczepy – efekt scyzoryka). MoĪliwe są dwa sposoby m ocowania przyczepy do ci ągnika: osiowe (z punktem mocowania poáoĪonym na Ğrodku osi kóá ciągnika) i pozaosiowe (z punk-tem mocowania poáoĪonym poza osią kóá ciągnika), jak przedstawiono na rys. 7. Zadania Ğle-dzenia trajektorii dopuszczalnych oraz sterowania do punktu dla pojazdów przegubowych obu typów z rys. 8 zosta áy przedstawione odpowiednio w pracach [MiKo2009_2] oraz [Mi-Ko2009_3]. Zastosowano tam koncepcjĊ VFO, podając sposób rozszerzenia jej stosowalnoĞci na roboty m obilne definiowane m odelem o wym iarze wektora konfiguracji wi Ċkszym od trzech. Oba rodzaje zada Ĕ potraktowano w podobny zunifi kowany sposób poprzez analogi Ċ do pierwotnego wykorzystania m etody VFO dla ci ągnika dwukoáowego. Zastosowanie kon-cepcji VFO byáo moĪliwe dziĊki wprowadzeniu odwracalnej transform acji sygnaáów sterują-cych i przeformuáowaniu równaĔ modeli robotów przegubowych w taki sposób, aby uzyska ü podsystem z modelem monocykla. Uzyskane wyniki pokazuj ą ponownie elastycznoĞü w wy-borze strategii ruchu pojazdów: ruch przodem/ruch tyáem z zachowaniem stabilnoĞci dla pod-systemu skrĊcenia przyczepy podczas m anewrów cofania. Przyk áadowe wyniki sym ulacyjne przedstawiono na rys. 8 oraz 9.
Rys. 8. Wyniki realizacji zadania Ğledzenia trajektorii (lewy: ruch tyáem) oraz sterowania do punktu (prawy: ruch przodem) w ukáadzie sterowania VFO dla robota z przyczepą mocowaną osiowo
Rys. 9. Wyniki realizacji zadania Ğledzenia trajektorii (lewy: ruch tyáem) oraz sterowania do punktu (prawy: ruch tyáem) w ukáadzie sterowania VFO dla robota z przyczepą mocowaną pozaosiowo
Algorytm VFO wykorzystywany do realizacji podstawowych zada Ĕ ruchu m aksymalizuje podstawowe kierunki ruchu wyznaczane przez pola wektorowe-generatory zwi ązane z kine-matyką robota. W efekcie zadanie sterowania orientacj ą robota staje si Ċ wtórne wobec uzy-skania za áoĪonej pozycji. Dlatego w stanach przej Ğciowych zm iany orientacji robota m ogą byü stosunkowo duĪe – w pewnych przypadkach taka realizacja ruchu nie jest akceptowalna. Biorąc pod uwagĊ podane ograniczenie w opisywanym robocie opcjonalnie m oĪna wykorzy-staü uniwersalne techniki czĊstotliwoĞciowe, które pozwalają realizowaü ruch wysokoenerge-tyczny w tzw. kierunkach trudnych (wskazywa nych przez nawiasy Liego). W tym celu pro-ponuje si Ċ im plementacje algorytm ów wykorzystuj ących funkcje transwersalne opracowa-nych przez Morina i Sam sona [MS2009]. IlustracjĊ dziaáania tych algorytmów przedstawiają wyniki symulacji numerycznych przedstawionych na rys. 10.
Rys. 10. ĝcieĪka pozycji dla robota dwukoáowego w zadaniu parkowania z wykorzystaniem funkcji transwer-salnych. Tunel báĊdu pozycji zaáoĪono równy 0,02 m, dopuszczalny báąd orientacji 0,2 rad (po lewej) lub 0,5 rad
(po prawej).
Ksztaátując odpowiednio wspóáczynniki projektowe moĪna uzyskaü zaáoĪony przebieg trajek-torii pozycji (leĪącej w tunelu o projektowanej szerokoĞci) jak równieĪ ograniczenie (do
zaáo-Īonego poziom u) zm ian orientacji. Dodatkowo, uwzgl Ċdniając autorskie m etody strojenia tych algorytmów [PiK2008] moĪliwe jest ograniczanie oscylacyjnoĞci ruchu, gdy nie jest ona wymagana do realizacji zadania. Opisywana strategia m oĪe by ü efektywnie wykorzystana w przypadku dokowania robota i realizacji ruchu w bezpo Ğrednim s ąsiedztwie punktu zadanego.
5. SYSTEM SENSORYCZNY
W skáad ukáadu sensoryczny robota wchodz ą dwa podsystemy. Pierwszy odpowiada za kon-trolĊ stanu wewn Ċtrznego robota i jest bezpo Ğrednio związanym z uk áadem napĊdowym, po-mocniczymi elementami wykonawczymi (obejmuje odczyt stanu liczników opisujących drogĊ kątową, stanu wyáączników kraĔcowych, wartoĞci prądu silników, itd.) oraz uk áadem bezpie-czeĔstwa (m. in. pozwala na odczyt stanu zderzaków opisuj ący miejsce fizycznego kontaktu robota z przeszkodą).
Drugi podsystem pomiarowy jest bardziej z áoĪony i odpowiada za analiz Ċ stanu Ğro-dowiska, którego celem jest lokalizacja robota w Ğrodowisku, wykrywanie przeszkód statycz-nych i dynam iczstatycz-nych oraz sytuacji potencjalnie niebezpieczstatycz-nych. Uk áad ten zaprojektowany jako system rozproszony wykorzystuje dalm ierze ultradĨwiĊkowe, dalmierze triangulacyjne w zakresie podczerwieni, zintegrowane Īyroskopy i akcelerom etry, jak równie Ī skaner lase-rowy i wielokamelase-rowy system wizyjny.
5.1. OkreĞlanie pozycji i orientacji robota
Z punktu widzenia moĪliwoĞci realizacji podstawowych zadaĔ ruchu kluczowe znaczenie ma system lokalizacji. Uk áad ten powinien um oĪliwiü okre Ğlanie konfiguracji robota w wybra-nym ukáadzie odniesienia z m oĪliwie duĪą precyzją przy zachowaniu krótkich opó ĨnieĔ. W roli podstawowego system u lokalizacji zastosowano lokalizacj Ċ przyrostow ą realizowan ą przez algorytm odometrii, co pozwala uzyska ü dobre w áaĞciwoĞci dynamiczne lokalnej p Ċtli sprzĊĪenia zwrotnego i du Īą czĊstotliwoĞü pracy uk áadu sterowania ruchem robota. Metoda ta, b Ċdąca w istocie technik ą predykcyjn ą bez sprz ĊĪenia zwrotnego, nie m oĪe by ü jednak stosowana sam odzielnie. Dlatego w celu bezwzgl Ċdnego okre Ğlenia konfiguracji robota w wybranym uk áadzie wspó árzĊdnych oraz w celu korekcji estym acji konfiguracji zwracanej przez odometriĊ wykorzystano system wizyjny. Projektuj ąc caáoĞciowy algorytm lokalizacji zaáoĪono, Īe algorytm odometrii jest dobrze nastrojony (tj. wspóáczynniki geometryczne takie jak rozstaw kó á oraz ich prom ienie zostaáy poprawnie zidentyfikowane np. za pom ocą testu UMB [Skrz2007]) i b áąd systematyczny jest pom ijalnie maáy. Z kolei dane zwracane przez system lokalizacji zewn Ċtrznej (w tym przypadku jest nim system wizyjny obserwuj ący sztuczne znaczniki o znanych wspó árzĊdnych wprowadzone do Ğrodowiska) zapewniają sta-bilną estymacjĊ, choü obarczoną stosunkowo duĪym rozrzutem. UwzglĊdniając niską czĊsto-tliwoĞü aktualizacji danych przez system wizyjny do realizacji fuzji danych zwracanych przez oba algorytmy lokalizacji klasyczne podejĞcie w oparciu o filtr Kalmana nie wydaje siĊ byü w rozwaĪanym przypadku wáaĞciwe. Z kolei za áoĪenie korekcji odometrii wyáącznie w oparciu o informacje podawane przez zewnĊtrzny algorytm lokalizacji skutkowaáoby skokową zmianą wartoĞci konfiguracji, która jest wykorzystywana przez algorytm sterowania. Zaprojektowany algorytm wykorzystuje stosunkowo proste podej Ğcie do fuzji danych w oparciu o Ğrednią wa-Īoną. Idea algorytm u polega na jednoczesnym obliczaniu odom etrii dla ró Īnych warunków początkowych, które wynikaj ą ze stanu poprzedniego zwracanego przez odom etriĊ oraz
no-wej wartoĞci bĊdącej liniową kombinacją poprzedniej estym acji konfiguracji i danych poda-wanych przez system zewnĊtrzny. Na rys. 11 przedstawiono ogólną zasadĊ algorytmu.
Rys. 11. Ilustracja algorytmu fuzji danych w ukáadzie lokalizacji robota
Szarym kóákiem oznaczono informacjĊ o konfiguracji zwracan ą przez zewnĊtrzny system lo-kalizacji, kó ákiem pustym warunek ko Ĕcowy estym acji konfiguracji za pom ocą algorytm u odometrii w chwili pojawienia si Ċ informacji zewnĊtrznej, natomiast kóákiem czarnym koĔ-cową wartoĞü estymaty kombinowanej. ĝcieĪka oznaczona ci ągáą czarną linią jest wynikiem kombinacji liniowej danych wyznaczanych przez dwa algorytmy odometrii. Warto zwróci ü uwagĊ na celowo Ğü wyboru funkcji wagowej do obliczania kom binacji liniowej, tak aby za-pewniü ci ągáoĞü ĞcieĪki wypadkowej pom imo wprowadzania dodatkowych danych przez zewnĊtrzny system lokalizacji.
5.2. Wykorzystanie skanera laserowego
WaĪnym elementem systemu lokalizacji jest sk aner laserowy, który dostarcza dane do pod-systemu m apowania (w postaci elem entarnej m apy wektorowej) i sterowania. W bie Īącej wersji ukáadu sterowania i sensoryki robota za áoĪono, Īe skaner w pierwszej kolejno Ğci ma dostarczaü lokalnych informacji o przeszkodach w Ğrodowisku poprzez okreĞlenie najbliĪszej odlegáoĞci od przeszkody i k ąta, pod którym jest ona widziana. W arto jednak wskaza ü na moĪliwoĞü dalszego rozszerzenia znaczenia skanera w celu wspom agania systemu lokalizacji i korekcji odometrii.
5.3. Elementy sieciowego systemu sensorów pomiaru odlegáoĞci
System skáada siĊ z ustalonej liczby moduáów sensorycznych. Ustalenia projektowe pozwala-ją na obs áugĊ przez jeden m oduá pomiarowy pojedynczego sensora podczerwieni oraz poje-dynczego sensora ultrad ĨwiĊkowego [MaPaKo10]. Czujnikami wspóápracującymi z opisywa-nym w pracy moduáem pomiarowym w jego podstawowej wersji są:
czujnik ultradĨwiĊkowy (US) SFR-04 firmy Devantech, wyposaĪony w przetworniki piezoelektryczne i o zasiĊgu od 0,03 - 3 m,
optyczny czujnik triangulacyjny (IR) GP2Y3A002K0F firmy Sharp, pracujący w zakre-sie mierzonej odlegáoĞci od 0,2–1,5 m i umoĪliwiający pomiar w piĊciu kierunkach o staáej róĪnicy kątowej pomiĊdzy nimi.
KaĪdy z m oduáów pracuje w cyklu dzia áania aktywnych sensorów do niego pod áączonych, wykorzystując przerwania czasowe. Ka Īdy sensor posiada specyficzny ze wzgl Ċdu na zasadĊ dziaáania cykl pom iarowy, co zwi ązane jest z asynchronizm em pracy sensorów. Procedura obsáugi sensorów zosta áa zamkniĊta w formĊ maszyny stanu, co um oĪliwia niezaleĪną trans-misjĊ wyników pomiarów natychmiast po ich uzyskaniu.
a) b) c) Rys. 12. Elementy sieciowego systemu sensorycznego, a) moduá
senso-ryczny, b) sensor US, c) sensor IR
Jako elementarną jednostkĊ pomiarową zaprojektowano od podstaw m oduá (rys. 12a) obsáugujący czujnik podczerwieni (rys. 12c) oraz czujnik ultrad ĨwiĊkowy (rys 12b). W zre-alizowanej wersji pracuje osiem takich zestawów pod áączonych do magistrali CAN. Sensory umieszczono w przewidzianych konstrukcją robota miejscach jego korpusu.
Interfejsy m oduáu pom iarowego zosta áy przygotowane do wspó ápracy z wybranym i czujnikami odleg áoĞci. Dwukierunkowa kom unikacja z czujnikiem ultrad ĨwiĊkowym reali-zowana jest m etodą cyfrow ą i polega na cyklicznej inicjalizacji wys áania wi ązki impulsów fali ultradĨwiĊkowej oraz odczycie wyniku na podstawie pom iaru czasu trwania od m omentu wysáania do chwili odbioru im pulsu wyj Ğciowego (odpowiadaj ącego czasowi przelotu fali akustycznej od sensora do przeszkody i z powrotem ). Podobnie sterowanie prac ą triangula-cyjnego, 5-kana áowego sensora podczerwieni realizowane jest na drodze cyfrowej (wybór kanaáu, tj. kąta emisji wiązki Ğwiatáa podczerwonego i rozpoczĊcie przetwarzania), natomiast odczyt wyniku dokonywany jest poprzez pomiar wartoĞci analogowej z wbudowanego w sen-sor czujnika PSD.
Z uwagi na prac Ċ w system ie wielu sensorów danego rodzaju, kluczowym zagadnie-niem jest rozwi ązanie problem u przes áuchów i w áaĞciwej separacji. Poniewa Ī zastosowane czujniki IR lub US s ą tego samego typu, istnieje moĪliwoĞü wzajemnego nakáadania siĊ wią-zek pom iarowych wykorzystuj ących te sam e zjawiska fizyczne, skutkiem czego powstaje moĪliwoĞü otrzym ania b áĊdnych wyników pom iarów. Dlatego w projektowanym uk áadzie pomiarowym zaproponowano m oĪliwoĞü okreĞlania sekwencji wyzwalania czujników, opar-tej na jednostkowym czasie pom iaru kaĪdego czujnika, tak aby elim inowaü ryzyko nak áada-nia siĊ wiązek pomiarowych dziaáających identycznie. W tym dokonano synchronizacji dzia-áania m oduáów. Pom iarami zarz ądzają dwie niezale Īne sekwencje, a num er pom iaru w se-kwencji ustalany jednoznacznie odpowiedni ą kom endą. Dzi Ċki tem u sekwencje m ogą by ü przeprogramowane, umoĪliwiając wykluczanie nak áadania siĊ sygnaáów pomiarowych w ra-cjonalnie rozmieszczonych sensorach oraz np. dopasowane sekwencji dzia áania sensorów do kierunku ruchu robota.
5.4. BezwáadnoĞciowy system pomiaru pozycji
Moduáem uzupeániającym jest moduá IMU zbudowany w oparciu o uk áad ADIS16355, który odpowiada za ocenĊ orientacji robota w przestrzeni 3D wyra Īonej wzglĊdem wektora grawi-tacji (inklinometr), co pozwala wykrywa ü stopieĔ nachylenia p áaszczyzny, po której porusza siĊ robot. Dodatkowo moĪliwe jest wykorzystanie tego systemu do wspomagania lokalizacji – zagadnienie to ma jednak charakter rozwojowy.
6. SYSTEM BEZPIECZEēSTWA
System bezpieczeĔstwa jest z áoĪony z funkcji poszczególnych sterowników robota. Podsta-wowymi elem entami bezpiecze Ĕstwa s ą program owe ograniczenia pr ĊdkoĞci ruchu robota, elementy kontaktowe i zderzaki na obwodzie r obota, skaner laserowy (na rys. 1) z wyró Īnio-nymi strefami bezpieczeĔstwa, których wielko Ğü jest sta áa, ale docelowo m oĪe byü modyfi-kowana w funkcji pr ĊdkoĞci postĊpowej, oraz pom iarowy system sieciowy, który pe áni po-dobną rolĊ co strefy bezpiecze Ĕstwa w skanerze laserowym , ale w przeciwie Ĕstwie do niego elementy tego system u są skierowane we wszystkich kierunkach przem ieszczenia siĊ robota wáączając ruch obrotowy.
7. PODSUMOWANIE
W pracy przedstawiono koncepcj Ċ budowy moduáowego robota mobilnego z przyczepą, jego konstrukcjĊ, system sterowania i program owania oraz zestaw sensorów, które um oĪliwiają autonomiczny ruch robota w Ğrodowisku czĊĞciowo uporządkowanym. Istotną rolĊ peáni ory-ginalny system sterowania zbudowany w oparciu o m etodĊ VFO oraz znan ą z literatury m e-todĊ z zastosowaniem funkcji transwersalnych. Bardzo istotny jest te Ī system kom unikacji wewnĊtrznej i zewnĊtrznej oraz nowy jĊzyk programowania robota. W obecnej realizacji pro-jektu prowadzone s ą testy system u sterowania i nawigacji, których wyniki b Ċdą dostĊpne w czasie trwania konferencji Automation.
BIBLIOGRAFIA
[MiKo2009_1] M. Michaáek, K. Kozáowski. Motion planning and feedback control for a unicy cle in a way point following task: The VFO approach. Int. J. Appl. Math. Comput. Sci., 19(4), s. 533–545, 2009.
[MiKo2009_2] M. Micha áek, K. Koz áowski. Tracking and set-point VFO control for an articulated mobile robot with on-axle hitched trailer. 2009 American Control Conference, s. 919–924, St. Louis, USA, 2009.
[MiKo2009_3] M. Michaáek, K. Kozáowski. VFO tracking and set-point control for an articulated ve-hicle with off-axle hitched trailer. Proceedings of the European Control Conference 2009,
s. 266–271, Budapeszt, WĊgry, 2009.
[PiK2008] D. Pazderski, K. Koz áowski. Trajectory Tracking Control of Skid-S teering Robot – Expe-rimental Validation, Proc. of the 17th IFAC World Congress, s. 5377–5382, 2008.
[MS2009] P. Morin, C. Samson. Control of nonholonomic mobile robots based on the transverse func-tion approach, IEEE Transacfunc-tions on Robotics, Vol. 25 (5), s. 1058–1073, 2009.
[Skrz2007] P. Skrzy pczyĔski, Metody analizy i redukcji niepewno Ğci percepcji w sy stemie nawigacji robota mobilnego, Rozprawy, nr 407, PoznaĔ, Wyd. Politechniki PoznaĔskiej 2007.
[Fel08] A. Fellenberg, Pok áadowy sy stem operacy jny m oduáowego robota m obilnego, Praca dyplo-mowa magisterska, Politechnika PoznaĔska, Katedra Sterowania i InĪynierii Systemów, 2008.
[MaPaKo10] J. Majchrzak, D. Pazderski, K. Koz áowski, Sieü sensorów do pom iarów odlegáoĞci dla robota mobilnego, Prace naukowe – Elektronika, z.175, problem y Roboty ki T. I, red K. Tcho Ĕ i C. ZieliĔski, Wydawnictwo PW, Warszawa 2010, s. 209–220.
