MODEOWANIE INŻYNIERSKIE nr 45, t. 14, rok 01 ISSN 1896-771X STEROWANIE FORMACJĄ ROBOTÓW METODĄ ŚEDZENIA IDERA Andrzej Burghardt 1a, Zenon Hendzel 1b, Józef ergel 1c, Marcn Nawrock d 1 Katedra Mechank Stosowanej Robotyk, Poltechnka Rzeszowska, AGH w Krakowe e-mal: a andrzejb@prz.edu.pl, b zenhen@prz.edu.pl, c j.gergel@chello.pl, d nawro@agh.edu.pl Streszczene W pracy zaproponowano model moblnego robota kołowego wchodzącego w skład formacj. Wykorzystując strukturę herarchczną układu sterowana podano algorytm sterowana od punktu do punktu, dla którego udowodnono stablność w sense apunowa. Zaproponowany algorytm został zamplementowany w sterowanu formacją robotów z wykorzystanem algorytmu śledzena ldera. Otrzymane rozwązane zasymulowano oraz zweryfkowano w autorskm środowsku kontrolno-pomarowym zarządzającym oraz montorującym pracę do 56 robotów moblnych. HIERARCHICA CONTRO OF ROBOTS FORMATION USING EADER FOOWING METHOD Summary The paper presents a model of a wheeled moble robot as a part of a robot formaton. Usng a herarchcal control system structure, a pont-to-pont control algorthm was gven, for whch stablty was proven n the sense of yapunov. The proposed algorthm can be mplemented n the robot formaton control n follow-the-leader algorthms. The proposed soluton was smulated n the bult-up emulator of the n-robots workng envronment. The smulaton results were verfed under workng condtons of a real formaton, consstng of three Amgobot robots. Durng the verfcaton research procedures, the orgnal control-measurement envronment, developed as a Matlab/Smulnk toolbox, was used; the toolbox allows for controllng and montorng up to 56 moble robots. 1. WSTĘP Głównym motorem prowadzonych prac dotyczących sterowana formacją są zastosowana mltarne take jak poruszane sę robotów w zdefnowanym szyku, śledzene, patrolowane granc tp. Inne cywlne slne rozwjane zastosowana to: montorowane magazynów, budowa map, transport welkogabarytowy oraz eksploracja kosmosu [1]. Sterowane formacją robotów jest ścśle powązane z rodzajem realzowanego zadana. W zadanach patrolowych, bojowych nspekcyjnych dopuszcza sę zmanę położena robotów w szyku przy jego jednoczesnym zachowanu. Uzyskuje sę to wykorzystując algorytmy sterowana polegające na śledzenu ldera [7,11,13]. Odmenny typ zadań formacj to np. transport welkogabarytowy, gdze formacja mus zachować zdefnowany szyk geometryczny bez możlwośc zmany kolejnośc robotów w szyku. Tego typu zadana są realzowane z wykorzystanem algorytmów wrtualnej struktury [9,10,1]. Metody sterowana formacją robotów, których nspracją są wzorce pochodzące z obserwacj organzmów żywych, noszą nazwę metod behaworalnych [1]. 0
Andrzej Burghardt, Zenon Hendzel, Józef ergel, Marcn Nawrock Osobną grupę stanową metody oparte na sztucznych funkcjach potencjałowych [5,8] w przecweństwe do pozostałych, mają one charakter analtyczny zwykle możlwa jest analza ch stablnośc. W pracy zaprezentowano model moblnego robota kołowego wchodzącego w skład formacj. Wykorzystując strukturę herarchczną układu sterowana, podano algorytm sterowana od punktu do punktu, dla którego udowodnono stablność w sense apunowa. Zaproponowany algorytm został zamplementowany w sterowanu formacją robotów z wykorzystanem algorytmu śledzena ldera.. MODEOWANIE Celem procesu modelowana jest uzyskane opsu matematycznego obektu w forme pozwalającej na jego wykorzystane w algorytmach sterowana do kompensacj nelnowośc obektów. Tego typu zastosowane opsu obektu determnuje jego formę oraz złożoność. Przyjęte w procese modelowana uproszczena pownny pozwolć na uzyskane jak najprostszej formy równań dynamk pozwalającej na przeprowadzene dowodów stablnośc algorytmów sterowana. Z drugej strony muszą odwzorowywać jak najwęcej własnośc fzycznych obektu poneważ nemodelowane zjawska ne będą kompensowane w dalszej procedurze syntezy algorytmów sterowana będą traktowane jako zakłócene. Przyjęce założeń upraszczających jest węc kompromsem często poddawanym ocene w procese badana wrażlwośc parametrów. Syntezę model matematycznych moblnych robotów kołowych z wykorzystanem takch formalzmów jak równana agrange a, Maggego, Appella zaprezentowano w pracach [3,4]. Proces modelowana kończy dentyfkacja przyjętych parametrów modelu, została ona zaprezentowana w pracy [6]. Jako elementy składowe formacj przyjęto moblne roboty kołowe AmgoBot. W roboce tym (rys.1) występują: rama 3, dwa koła jezdne napędzające 1, oraz koło swobodne samonastawne 4. Kąt obrotu ramy zdefnowano jako β natomast kąty obrotu kół o promenach r1=r=r, zdefnowano odpowedno jako α1 α. Dodatkowo wprowadzono koło zastępcze o promenu r, drodze kątowej α, równej połowe sumy kątów obrotu α1 α. Ops ruchu moblnego robota kołowego zrealzowano przy założenach braku poślzgów kół napędzających, ponadto założono, że ruch robota odbywa sę po pozomej jezdn. Rys. 1. a) Moblny robot AmgoBot, b) model moblnego robota Przyjęto wektor współrzędnych uogólnonych w postac =[xa,ya,β,α] T zapsano równane węzów neholonomcznych [6], J()& = 0, (1) w którym jakoban określono jako: 1 0 0 - rcos( β ) J() =. () 0 1 0 - rsn( β ) Knematykę moblnego robota kołowego opsuje równane: x& A V = y& A V & β Am Am cos β 0 uv sn β 0 u 0 ωm β. (3) Występujące w zależnośc welkośc VAm ωm są odpowedno maksymalnym prędkoścam punktu A ramy moblnego robota, natomast ub=[uv,u β ] T jest wektorem sterowań. Zaprezentowany robot został wykorzystany do budowy formacj, przedstawonej w dalszej częśc pracy. Kształt formacj determnuje realzowane zadane. Typowe rozważane w opracowanach lteraturowych formacje to kolumna, trójkąt, dament, lna prosta. 3. HIERARCHICZNY AGORYTM STEROWANIA FORMACJĄ ROBOTÓW Archtektura systemów sterujących powązana jest z przyjętym rozwązanem planowana ruchu czy zadań. Archtektura welopozomowa jest charakterystyczna dla systemów, w których planowane ruchów wynka z narzuconej przez operatora trajektor lub wygenerowanej na podstawe przyjętych założeń czy bazy wedzy. 1
STEROWANIE FORMACJĄ ROBOTÓW METODĄ ŚEDZENIA IDERA W algorytmach sterowana zespołem robotów bazujących na wzorcach bologcznych (sterowane behaworalne, reaktywne) najczęścej wykorzystuje sę archtekturę równoległą. Rys.3. Idea sterowana metodą śledzena ldera Zdefnowano punkty x, y jako: x y = x = y snφ snβ + cosφ snβ + cosφ cosβ + snφ cosβ (4) Rys.. Herarchczny układ sterowana formacją metoda śledzena ldera Prezentowane herarchczne rozwązane wykorzystuje trajektorę ruchu ldera narzuconą przez operatora lub pochodzącą z układu sterowana nadrzędnego. W centralnej częśc struktury herarchcznej realzowany jest algorytm śledzena ldera (rys. ). Najnższy pozom układu sterowana zapewna poprawną realzację trajektor wygenerowanej w układze centralnym. W warstwe tej znalazły zastosowane algorytmy sterowana nadążnego m.n. sterowane adaptacyjne, odporne, rozmyte czy neuronowe [3, 6]. 4. STEROWANIE METODĄ ŚEDZENIA IDERA W metodze śledzena ldera zakłada sę kształt formacj, defnuje, który z robotów formacj jest lderem, ponadto przyjmuje sę, że początkowe wartośc położena orentacj robotów są znane. Trajektora zadana dla ruchu robota ldera jest generowana przez nadrzędny układ sterowana. Przyjęto współrzędne charakterystycznego punktu robota ldera jako x, y oraz orentację jako β. Zdefnowano kształt formacj w odnesenu do ldera jako odległośc oraz kąty kolejnych robotów od ldera. φ odchylena Ideą metody jest w kolejnych krokach teracyjnych mnmalzowane odległośc oraz kąta odchylena od punktu zwązanego z robotem lderem. W efekce każdy z robotów formacj oprócz ldera podąża za punktem x, y, których położene jest wyznaczane na podstawe położena orentacj ldera. Prezentowane podejśce pozwala na zachowane kształtu formacj podczas jej przemeszczana (przemeszczane ldera) przy dowolnej orentacj robotów. 5. STEROWANIE OD PUNKTU DO PUNKTU Jeżel założy sę teracyjne wykonywane algorytmu, charakterystyczne dla przyjmowanych rozwązań cyfrowych układów sterowana, to sterowane ruchem formacj sprowadza sę ostateczne do wyznaczena sterowana, w którym zakłada sę, że wybrany punkt -tego moblnego robota (xa, ya) ma osągnąć w kolejnych krokach teracyjnych punkt o współrzędnych (x,y), przy dowolnej konfguracj ramy robota. W układze neruchomym aktualne położene -tego moblnego robota opsują współrzędne zdefnowane jako =[xa,ya,β] T, gdze (xa,ya) to współrzędne charakterystycznego punktu robota natomast β to kąt obrotu ramy -tego robota w globalnym układze odnesena. Założono zadaną konfgurację charakterystycznych punktów na robotach w postac: [ x,,0] T = y. (5)
Andrzej Burghardt, Zenon Hendzel, Józef ergel, Marcn Nawrock Zdefnowano błąd konfguracj zadanej końcowej jako ~ [ ~ x y ~, ~, β ] T = =, (6) a różnczkując (6), otrzymano ~ & = J ~ u. (7) ( ) B Zdefnowano błąd b we współrzędnych begunowych (rys.3) [ ] T b = δ, ψ, (8) gdze: ( ) ( ) δ = x x + y y, ψ = atan[ ( y y ) ( x x )], ψ ( π, π ) π]. ψє[-π, Różnczkując wektor błędu (8), otrzymano & δ = uv. (9) 1 Ψ& = ( δ ) uv snψ uβ Aby wyznaczyć prawo sterowana generujące parametry ruchu -tego moblnego robota, zastosowano teorę stablnośc apunowa. Rozważono funkcję dodatno określoną = 0. 5( δ ) + 0. 5( ψ (10) V ) Różnczkując zależność (10), otrzymano V& = δ u V + ψ 1 ( δ ) u snψ u ) V β. (11) Przyjmując elementy wektora sterowana w postac (1), uv u = k 1 δ, β = k 1 snψ + k ψ, (1) gdze k1, k>0, pochodną (11) zapsano jako V & = k1 ( δ ) cos ψ k ( ψ ). (13) Ostateczne otrzymano V& < 0 oznacza to, że funkcja (10) jest funkcją apunowa, a stan równowag w zerze δ=0, ψ=0, jest globalne ekspotencjalne stablny. Z przeprowadzonej syntezy sterowana wynka, że odległość δ oraz kąt ψ maleją ekspotencjalne do zera, a spełnene warunku δ=0, ψ=0 mplkuje, że punkt (xa, ya) robota znajduje sę w punkce (x, y), co zapewna zachowane kształtu formacj. 6. SYMUACJA I WERYFIKACJA Badana symulacyjne zaproponowanego rozwązana podążana formacj po zdefnowanym torze z wykorzystanem algorytmu śledzena ldera przeprowadzono w środowsku Matlab/Smulnk z wykorzystanem zbudowanego emulatora pracy -robotów moblnych. Dla zdefnowanego kształtu formacj (rys. 3) przyjęto tor ruchu punktu A robota ldera (rys.4a) oraz założono jego prędkość lnową (rys.4b) oraz kątową (rys.4c). W celu jakoścowego porównana zachowana kształtu formacj przy realzacj różnego rodzaju trajektor, testowanych z różnym wartoścam eksperymentalne dobranych parametrów k1, k wprowadzono wskaźnk jakośc w postac: E δ (14) = = 1 gdze to nr robota. Jako marę jakośc przyjęto zmanę kształtu formacj defnowaną jako zmana odległośc pkt. A -tego robota od punk defnowanego jako x, zgodne z zal. (4). y Rys.4a) Tor ruchu punktu A robota ldera, b,c) parametry ruchu robota ldera 3
STEROWANIE FORMACJĄ ROBOTÓW METODĄ ŚEDZENIA IDERA Parametry ruchu robota nr zarejestrowane podczas wynk dotyczą realzacj zadana podążana formacj po badań symulacyjnych przedstawono na rys.5. Uzyskane trajektor przedstawonej na rys. 4a. Rys.5. Parametry ruchu robota nr a) sygnały sterowań, b) prędkośc kątowe kół napędzających, c) błąd pozycj oraz orentacj orentacj robota, d) błąd realzacj formacj Weryfkację zaproponowanego algorytmu sterowana na montorowane pracy grupy moblnych robotów formacją robotów przeprowadzono w zaprojektowanym AmgoBot, polegającej na rejestracj parametrów ruchu, przez kontrolno kontrolno-pomarowym. wskazań z czujnków odległośc, pomar napęca batebat W celu zarejestrowana sygnałów z rzeczywstych autorów środowsku r, pomar prądów slnków napędzających koła oraz robotów, wykorzystując bblotek dostarczone przez sterowane napędam do 56 robotów r moblnych. Wy- producenta, zbudowano oprogramowane w postac gląd okna konfguracyjnego podsystemu wykonanego toolboxes Matlab/Smulnk []. Rozwązane ozwązane to pozwala w programe Matlab/Smulnk przedstawa rys. 6. Rys. 6. a) Okno konfguracyjne toolboxa Amgo, b) wygląd emulatora pracy n-robotów n robotów Weryfkację algorytmu sterowana (rys.7) przeprowaprzeprow wybranego robota analogcznych jak w symulacj (rys.4, dzono dla zadanego toru ruchu parametrów ruchu 5). Rys.7. Parametry ruchu robota nr a) sygnały sterowań, b) prędkośc kątowe kół napędzających, c) błąd pozycj oraz orentacj robota, d) błąd realzacj formacj 4
Andrzej Burghardt, Zenon Hendzel, Józef ergel, Marcn Nawrock W badanach weryfkacyjnych zrealzowanych z wykorzystanem moblnych robotów kołowych AmgoBot uzyskano wynk analogczne jak w symulacjach. Klkunastoprocentowo wększe wartośc błędów zachowana kształtu formacj podczas ruchu rzeczywstego układu robotów moblnych wynkają z błędów modelowana występowana nemodelowanych w symulacjach zakłóceń. sterowana od punktu do punktu zrealzowany w układze herarchcznym pozwala na sterowane ruchem zdefnowanej formacj w układze śledzena ldera. Zaproponowany algorytm może być wykorzystywany dla wększej lczby robotów w formacj, dla dowolne zdefnowanego kształtu formacj jest on stablny w sense apunowa. 7. PODSUMOWANIE Wynk symulacj weryfkacj potwerdzają poprawność przyjętych założeń, a zaproponowany algorytm teratura 1 Arkn R.C.: Behavor-Based Robotcs. The MIT Press, 1998. Burghardt A.: Proposal for a rapd prototypng envronment for algorthms ntended for autonomous moble robot control. Mechancs and Mechancal Engneerng 008, 1, p.5-16. 3 Burghardt A.: Herarchcal robot formaton control system. Internatonal Journal of Appled Mechancs and Engneerng 01, Vol. 17, No. 3, p.733-741. 4 Burghardt A.: Modelowane dynamk moblnego robota kołowego równanam Appella. Acta Mechanca et Automatca 010, Vol.4, No.1, p.9-1. 5 Ge S., Cu J.: New potental functons for moble robot path plannng. IEEE Transactons on Robotcs and Automaton 000, Vol. 16, No 5, p. 615-60. 6 Hendzel Z., Żylsk W., Burghardt A.: Autonomczne moblne roboty kołowe: mechatronczne projektowane sterowane. Rzeszów: Ofc. Wyd. Pol. Rzesz., 008. 7 Jongusuk J., Mta T.: Trackng control of multple moble robots: a case study of nter-robot collson-free problem. Asan Journal of Control 00, Vol. 4, No 3, p. 65-73. 8 Kanchanavally S., Ordonez R., Schumacher C. J.: Path plannng n three dmensonal envronment usng feedback lnearzaton. Amercan Control Conf. 006, p.4951-4956. 9 Kar-Han Tan, ews M. A.: Vrtual structures for hgh-precson cooperatve moble robotc control. In: Proceedngs of the 1996 IEEE/RSJ Internatonal Conference on Intellgent Robots and Systems 1996, Vol. 1, p. 13-139. 10 Ogren P., Egerstedt M.: A control yapunov functon approach to multagent coordnaton. IEEE Trans. on Rob. and Aut.00, Vol. 18, No. 5, p. 847-851. 11 Ogren P., eonard N.: Obstacle avodance n formaton. In: Proceedngs of IEEE Internatonal Conference on Robotcs and Automaton 003, Vol., p. 49-497, Tape, Tawan. 1 Ren W., Beard R. W., Curts X W.: Satsfcng control for mult-agent formaton manoeuvres. In: Proceedngs of the 41st IEEE Conference on Decson and Control. as Vegas, Nevada 00, Vol. 3, p. 433-438. 13 Tanner H., Pappas G., Kumar V.: eader-to-formaton stablty. IEEE Transactons on Robotcs and Automaton 004, Vol. 0, No 3, p. 443-455. Pracę wykonano w ramach realzacj projektu badawczego nr N N501 068838. 5