STEROWANIE FORMACJĄ ROBOTÓW METODĄ ŚLEDZENIA LIDERA

(1)

220

STEROWANIE FORMACJĄ ROBOTÓW METODĄ ŚLEDZENIA LIDERA

Andrzej Burghardt

^1a

, Zenon Hendzel

^1b

, Józef Giergiel

^1c

, Marcin Nawrocki

^2d

1Katedra Mechaniki Stosowanej i Robotyki, Politechnika Rzeszowska,

2AGH w Krakowie

e-mail: ^aandrzejb@prz.edu.pl, ^bzenhen@prz.edu.pl, ^cj.giergiel@chello.pl, ^dnawro@agh.edu.pl

Streszczenie

W pracy zaproponowano model mobilnego robota kołowego wchodzącego w skład formacji. Wykorzystując struk- turę hierarchiczną układu sterowania podano algorytm sterowania od punktu do punktu, dla którego udowodniono stabilność w sensie Lapunowa. Zaproponowany algorytm został zaimplementowany w sterowaniu formacją robotów z wykorzystaniem algorytmu śledzenia lidera. Otrzymane rozwiązanie zasymulowano oraz zweryfikowano w autorskim środowisku kontrolno-pomiarowym zarządzającym oraz monitorującym pracę do 256 robotów mobilnych.

HIERARCHICAL CONTROL OF ROBOTS FORMATION USING LEADER FOLLOWING METHOD

Summary

The paper presents a model of a wheeled mobile robot as a part of a robot formation. Using a hierarchical control system structure, a point-to-point control algorithm was given, for which stability was proven in the sense of Lyapunov. The proposed algorithm can be implemented in the robot formation control in follow-the-leader algorithms.

The proposed solution was simulated in the built-up emulator of the n-robots’ working environment. The simula- tion results were verified under working conditions of a real formation, consisting of three Amigobot robots. During the verification research procedures, the original control-measurement environment, developed as a Matlab/Simulink toolbox, was used; the toolbox allows for controlling and monitoring up to 256 mobile robots.

1. WSTĘP

Głównym motorem prowadzonych prac dotyczących sterowania formacją są zastosowania militarne takie jak poruszanie się robotów w zdefiniowanym szyku, śledze- nie, patrolowanie granic itp. Inne cywilne silnie rozwija- ne zastosowania to: monitorowanie magazynów, budowa map, transport wielkogabarytowy oraz eksploracja kosmosu [1].

Sterowanie formacją robotów jest ściśle powiązane z rodzajem realizowanego zadania. W zadaniach patrolo- wych, bojowych inspekcyjnych dopuszcza się zmianę położenia robotów w szyku przy jego jednoczesnym

zachowaniu. Uzyskuje się to wykorzystując algorytmy sterowania polegające na śledzeniu lidera [7,11,13].

Odmienny typ zadań formacji to np. transport wielkogabarytowy, gdzie formacja musi zachować zdefiniowa- ny szyk geometryczny bez możliwości zmiany kolejności robotów w szyku. Tego typu zadania są realizowane z wykorzystaniem algorytmów wirtualnej struktury [9,10,12].

Metody sterowania formacją robotów, których inspiracją są wzorce pochodzące z obserwacji organizmów żywych, noszą nazwę metod behawioralnych [1].

(2)

Andrzej Burghardt, Zenon Hendzel, Józef Giergiel, Marcin Nawrocki

Osobną grupę stanowią metody oparte na sztucznych funkcjach potencjałowych [5,8] w przeciwieństwie do pozostałych, mają one charakter analityczny i zwykle możliwa jest analiza ich stabilności.

W pracy zaprezentowano model mobilnego robota kołowego wchodzącego w skład formacji. Wykorzystując strukturę hierarchiczną układu sterowania, podano algorytm sterowania od punktu do punktu, dla którego udowodniono stabilność w sensie Lapunowa. Zapropo- nowany algorytm został zaimplementowany w sterowaniu formacją robotów z wykorzystaniem algorytmu śledzenia lidera.

2. MODELOWANIE

Celem procesu modelowania jest uzyskanie opisu mate- matycznego obiektu w formie pozwalającej na jego wykorzystanie w algorytmach sterowania do kompensa- cji nieliniowości obiektów. Tego typu zastosowanie opisu obiektu determinuje jego formę oraz złożoność. Przyjęte w procesie modelowania uproszczenia powinny pozwolić na uzyskanie jak najprostszej formy równań dynamiki pozwalającej na przeprowadzenie dowodów stabilności algorytmów sterowania. Z drugiej strony muszą odwzo- rowywać jak najwięcej własności fizycznych obiektu ponieważ niemodelowane zjawiska nie będą kompenso- wane i w dalszej procedurze syntezy algorytmów sterowania będą traktowane jako zakłócenie. Przyjęcie założeń upraszczających jest więc kompromisem często poddawanym ocenie w procesie badania wrażliwości parametrów. Syntezę modeli matematycznych mobilnych robotów kołowych z wykorzystaniem takich forma- lizmów jak równania Lagrange’a, Maggiego, Appella zaprezentowano w pracach [3,4]. Proces modelowania kończy identyfikacja przyjętych parametrów modelu, została ona zaprezentowana w pracy [6].

Jako elementy składowe formacji przyjęto mobilne roboty kołowe AmigoBot. W robocie tym (rys.1) wystę- pują: rama 3, dwa koła jezdne napędzające 1 i 2, oraz koło swobodne samonastawne 4. Kąt obrotu ramy zdefiniowano jako β natomiast kąty obrotu kół o promieniach r1=r2=r, zdefiniowano odpowiednio jako α1 i α2. Dodatkowo wprowadzono koło zastępcze o promieniu r , drodze kątowej α, równej połowie sumy kątów obrotu α1 i α2. Opis ruchu mobilnego robota kołowego zrealizowano przy założeniach braku poślizgów kół napędzających, ponadto założono, że ruch robota odbywa się po poziomej jezdni.

Rys. 1. a) Mobilny robot AmigoBot, b) model mobilnego robota

Przyjęto wektor współrzędnych uogólnionych w postaci q=[xA,yA,β,α]^T i zapisano równanie więzów nieholono- micznych [6],

= 0

J(q)& q

, (1)

w którym jakobian określono jako:

 

 



= 

) rsin(

- 0 1 0

) rcos(

- 0 0 1

β

J(q) β

. (2)

Kinematykę mobilnego robota kołowego opisuje równa- nie:



 





















=

















β

β β

β ^u

y u x

v A

A

m Am

Am

ω 0

0 sin V

0 cos V

&

. (3)

Występujące w zależności wielkości VAm i ωm są odpowiednio maksymalnymi prędkościami punktu A i ramy mobilnego robota, natomiast uB=[uV,uβ]^T jest wektorem sterowań.

Zaprezentowany robot został wykorzystany do budowy formacji, przedstawionej w dalszej części pracy. Kształt formacji determinuje realizowane zadanie. Typowe rozważane w opracowaniach literaturowych formacje to kolumna, trójkąt, diament, linia prosta.

3. HIERARCHICZNY ALGORYTM STEROWANIA FORMACJĄ

ROBOTÓW

Architektura systemów sterujących powiązana jest z przyjętym rozwiązaniem planowania ruchu czy zadań.

Architektura wielopoziomowa jest charakterystyczna dla systemów, w których planowanie ruchów wynika z narzuconej przez operatora trajektorii lub wygenerowanej na podstawie przyjętych założeń czy bazy wiedzy.

(3)

W algorytmach sterowania zespołem robotów bazują- cych na wzorcach biologicznych (sterowanie behawioral- ne, reaktywne) najczęściej wykorzystuje się architekturę równoległą.

Rys.2. Hierarchiczny układ sterowania formacją metoda śledzenia lidera

Prezentowane hierarchiczne rozwiązanie wykorzystuje trajektorię ruchu lidera narzuconą przez operatora lub pochodzącą z układu sterowania nadrzędnego. W cen- tralnej części struktury hierarchicznej realizowany jest algorytm śledzenia lidera (rys. 2). Najniższy poziom układu sterowania zapewnia poprawną realizację trajektorii wygenerowanej w układzie centralnym. W warstwie tej znalazły zastosowanie algorytmy sterowania nadąż- nego m.in. sterowanie adaptacyjne, odporne, rozmyte czy neuronowe [3, 6].

4. STEROWANIE METODĄ ŚLEDZENIA LIDERA

W metodzie śledzenia lidera zakłada się kształt formacji, definiuje, który z robotów formacji jest liderem, ponadto przyjmuje się, że początkowe wartości położe- nia i orientacji robotów są znane. Trajektoria zadana dla ruchu robota lidera jest generowana przez nadrzęd- ny układ sterowania. Przyjęto współrzędne charaktery- stycznego punktu robota lidera jako xL, yL oraz orienta- cję jako βL. Zdefiniowano kształt formacji w odniesieniu do lidera jako odległości Li oraz kąty

φ

_i odchylenia kolejnych robotów od lidera.

Rys.3. Idea sterowania metodą śledzenia lidera Zdefiniowano punkty xGi, yGi jako:

L i i

Gi L

L i i

Gi L

β y

y x β x

cos sin sinβ L

cos L

cos cos sinβ L

sin L

i L i

φ φ

+ +

=

+

−

=

(4)

Ideą metody jest w kolejnych krokach iteracyjnych minimalizowanie odległości oraz kąta odchylenia od punktu związanego z robotem liderem. W efekcie każdy z robotów formacji oprócz lidera podąża za punktem xGi, yGi, których położenie jest wyznaczane na podstawie położenia i orientacji lidera. Prezentowane podej- ście pozwala na zachowanie kształtu formacji podczas jej przemieszczania (przemieszczanie lidera) przy dowolnej orientacji robotów.

5. STEROWANIE OD PUNKTU DO PUNKTU

Jeżeli założy się iteracyjne wykonywanie algorytmu, charakterystyczne dla przyjmowanych rozwiązań cyfro- wych układów sterowania, to sterowanie ruchem formacji sprowadza się ostatecznie do wyznaczenia sterowa- nia, w którym zakłada się, że wybrany punkt i-tego mobilnego robota (xAi, yAi) ma osiągnąć w kolejnych krokach iteracyjnych punkt o współrzędnych (xGi ,yGi), przy dowolnej konfiguracji ramy robota. W układzie nieruchomym aktualne położenie i-tego mobilnego robota opisują współrzędne zdefiniowane jako qi=[xAi,yAi,βi]^T, gdzie (xAi,yAi) to współrzędne charaktery- stycznego punktu robota natomiast βi to kąt obrotu ramy i-tego robota w globalnym układzie odniesienia.

Założono zadaną konfigurację charakterystycznych punktów na robotach w postaci:

[ x

Gi

, y

Gi

,0 ]

^T

Gi

=

q

. (5)

(4)

Zdefiniowano błąd konfiguracji zadanej i końcowej jako

[

i i i

]

^T

i

^~ x ^, ^~ y ^, β ^~

~ = q

_Gi

− q

_i

=

q

, (6)

a różniczkując (6), otrzymano

( )

_i _Bi

i

J q u

q ~ & = − ~

_. ₍₇₎

Zdefiniowano błąd qbi we współrzędnych biegunowych (rys.3)

[ δ

Gi

, ψ

Gi

]

^T

bi

=

q

, (8)

gdzie: δGi =

(

xGi−xi

)

²+

(

yGi−yi

)

²^,

( ) ( )

[

Gi i Gi i

]

Gi = atan y −y x −x

ψ

^,

ψ

_Gi

∈ ( − π , π )

^ψ^Gi^є[-π,

π].

Różniczkując wektor błędu (8), otrzymano

i vi Gi

1 Gi Gi

vi Gi Gi

u Ψ u

u

ψ

β

δ ψ δ

−

=

−

=

−

sin

) (

cos

&

. (9)

Aby wyznaczyć prawo sterowania generujące parametry ruchu i-tego mobilnego robota, zastosowano teorię stabilności Lapunowa. Rozważono funkcję dodatnio określoną

2 Gi 2

Gi

i 05 05

V = . (

δ

) + . (

ψ

) (10)

Różniczkując zależność (10), otrzymano

(

Vi Gi i

)

1 Gi Gi Gi Vi Gi

i u u u

V& =−

δ

cos

ψ

+

ψ

(

δ

)⁻ sin

ψ

− _β

. (11)

Przyjmując elementy wektora sterowania w postaci (12),

Gi vi Gi

u = ^k

₁_i

δ ^cos ψ

,

Gi Gi

Gi

u_βi =k₁_isin

ψ

cos

ψ

+k₂_i

ψ

, (12)

gdzie k1i, k2i>0, pochodną (11) zapisano jako 2 Gi Gi

2 2 i Gi

V & = − k

₁_i

( δ ) cos ψ − k

₂_i

( ψ )

_. ₍₁₃₎

Ostatecznie otrzymano V&_i<0 oznacza to, że funkcja (10) jest funkcją Lapunowa, a stan równowagi w zerze δGi=0, ψGi=0, jest globalnie ekspotencjalnie stabilny. Z przeprowadzonej syntezy sterowania wynika, że odle- głość δGi oraz kąt ψGi maleją ekspotencjalnie do zera, a spełnienie warunku δGi=0, ψGi=0 implikuje, że punkt (xAi, yAi) robota znajduje się w punkcie (xGi, yGi), co zapewnia zachowanie kształtu formacji.

6. SYMULACJA I WERYFIKACJA

Badania symulacyjne zaproponowanego rozwiązania podążania formacji po zdefiniowanym torze z wykorzystaniem algorytmu śledzenia lidera przeprowadzono w środowisku Matlab/Simulink z wykorzysta- niem zbudowanego emulatora pracy i-robotów mobil- nych. Dla zdefiniowanego kształtu formacji (rys. 3) przyjęto tor ruchu punktu A robota lidera (rys.4a) oraz założono jego prędkość liniową (rys.4b) oraz kątową (rys.4c).

W celu jakościowego porównania zachowania kształtu formacji przy realizacji różnego rodzaju trajektorii, testowanych z różnymi wartościami eksperymentalnie dobranych parametrów k1i, k2i wprowadzono wskaźnik jakości w postaci:

∑

=

²

1 i

Gi

E

L

δ

⁽¹⁴⁾

gdzie i to nr robota. Jako miarę jakości przyjęto zmianę kształtu formacji definiowaną jako zmiana odległości pkt. A i-tego robota od punk definiowanego jako

Gi Gi

y

x ,

zgodnie z zal. (4).

Rys.4a) Tor ruchu punktu A robota lidera, b,c) parametry ruchu robota lidera

(5)

Parametry ruchu robota nr 2 zarejestrowane podczas badań symulacyjnych przedstawiono na rys.5. Uzyskane

Rys.5. Parametry ruchu robota nr 2 a) sygnały sterowań, b) prędkości kątowe kół napędzających, c) błąd pozycji oraz orientacj robota, d) błąd realizacji formacji

Weryfikację zaproponowanego algorytmu sterowania formacją robotów przeprowadzono w zaprojektowanym przez autorów środowisku kontrolno

W celu zarejestrowania sygnałów z rzeczywistych robotów, wykorzystując biblioteki dostarczone przez producenta, zbudowano oprogramowanie w postaci toolboxes Matlab/Simulink [2]. Rozwiązanie to pozwala

Rys. 6. a) Okno konfiguracyjne toolboxa Amigo, b) wygląd emulatora pracy n Weryfikację algorytmu sterowania (rys.7) przeprow

dzono dla zadanego toru ruchu i parametrów ruchu

Rys.7. Parametry ruchu robota nr 2 a) sygnały sterowań, b) prędkości kątowe kół napędzających, c) błąd pozycji oraz orientacji robota, d) błąd realizacji formacji

Parametry ruchu robota nr 2 zarejestrowane podczas badań symulacyjnych przedstawiono na rys.5. Uzyskane

wyniki dotyczą realizacji zadania podążania formacji po trajektorii przedstawionej na rys. 4a.

Rys.5. Parametry ruchu robota nr 2 a) sygnały sterowań, b) prędkości kątowe kół napędzających, c) błąd pozycji oraz orientacj

Weryfikację zaproponowanego algorytmu sterowania zaprojektowanym przez autorów środowisku kontrolno-pomiarowym.

W celu zarejestrowania sygnałów z rzeczywistych robotów, wykorzystując biblioteki dostarczone przez producenta, zbudowano oprogramowanie w postaci ozwiązanie to pozwala

na monitorowanie pracy grupy mobilnych robotów AmigoBot, polegającej na rejestracji parametrów ruchu, wskazań z czujników odległości, pomiar napięcia bat rii, pomiar prądów silników napędzających koła oraz sterowanie napędami do 256 r

gląd okna konfiguracyjnego podsystemu wykonanego w programie Matlab/Simulink przedstawia rys. 6.

Rys. 6. a) Okno konfiguracyjne toolboxa Amigo, b) wygląd emulatora pracy n-robotów Weryfikację algorytmu sterowania (rys.7) przeprowa-

parametrów ruchu

wybranego robota analogicznych jak w symulacji (rys.4, 5).

sygnały sterowań, b) prędkości kątowe kół napędzających, c) błąd pozycji oraz orientacji wyniki dotyczą realizacji zadania podążania formacji po trajektorii przedstawionej na rys. 4a.

Rys.5. Parametry ruchu robota nr 2 a) sygnały sterowań, b) prędkości kątowe kół napędzających, c) błąd pozycji oraz orientacji

na monitorowanie pracy grupy mobilnych robotów AmigoBot, polegającej na rejestracji parametrów ruchu, wskazań z czujników odległości, pomiar napięcia bate- rii, pomiar prądów silników napędzających koła oraz sterowanie napędami do 256 robotów mobilnych. Wy- gląd okna konfiguracyjnego podsystemu wykonanego w programie Matlab/Simulink przedstawia rys. 6.

robotów

wybranego robota analogicznych jak w symulacji (rys.4,

sygnały sterowań, b) prędkości kątowe kół napędzających, c) błąd pozycji oraz orientacji

(6)

W badaniach weryfikacyjnych zrealizowanych z wykorzystaniem mobilnych robotów kołowych AmigoBot uzyskano wyniki analogiczne jak w symulacjach. Kilku- nastoprocentowo większe wartości błędów zachowania kształtu formacji podczas ruchu rzeczywistego układu robotów mobilnych wynikają z błędów modelowania i występowania niemodelowanych w symulacjach zakłóceń.

7. PODSUMOWANIE

Wyniki symulacji i weryfikacji potwierdzają popraw- ność przyjętych założeń, a zaproponowany algorytm

sterowania od punktu do punktu zrealizowany w ukła- dzie hierarchicznym pozwala na sterowanie ruchem zdefiniowanej formacji w układzie śledzenia lidera.

Zaproponowany algorytm może być wykorzystywany dla większej liczby robotów w formacji, dla dowolnie zdefiniowanego kształtu formacji i jest on stabilny w sensie Lapunowa.

Literatura

1 Arkin R.C.: Behavior-Based Robotics. The MIT Press, 1998.

2 Burghardt A.: Proposal for a rapid prototyping environment for algorithms intended for autonomous mobile robot control. “Mechanics and Mechanical Engineering” 2008, 1, p.5-16.

3 Burghardt A.: Hierarchical robot formation control system. “International Journal of Applied Mechanics and Engineering” 2012, Vol. 17, No. 3, p.733-741.

4 Burghardt A.: Modelowanie dynamiki mobilnego robota kołowego równaniami Appella. „Acta Mechanica et Automatica” 2010, Vol.4, No.1, p.9-12.

5 Ge S., Cui J.: New potential functions for mobile robot path planning. IEEE “Transactions on Robotics and Automa- tion” 2000, Vol. 16, No 5, p. 615-620.

6 Hendzel Z., Żylski W., Burghardt A.: Autonomiczne mobilne roboty kołowe: mechatroniczne projektowanie i sterowanie. Rzeszów: Ofic. Wyd. Pol. Rzesz., 2008.

7 Jongusuk J., Mita T.: Tracking control of multiple mobile robots: a case study of inter-robot collision-free problem.

“Asian Journal of Control” 2002, Vol. 4, No 3, p. 265-273.

8 Kanchanavally S., Ordonez R., Schumacher C. J.: Path planning in three dimensional environment using feedback linearization. American Control Conf. 2006, p.4951-4956.

9 Kar-Han Tan, Lewis M. A.: Virtual structures for high-precision cooperative mobile robotic control. In: Proceedings of the 1996 IEEE/RSJ International Conference on Intelligent Robots and Systems 1996, Vol. 1, p. 132-139.

10 Ogren P., Egerstedt M.: A control Lyapunov function approach to multiagent coordination. IEEE Trans. on Rob.

and Aut.2002, Vol. 18, No. 5, p. 847-851.

11 Ogren P., Leonard N.: Obstacle avoidance in formation. In: Proceedings of IEEE International Conference on Robot- ics and Automation 2003, Vol. 2, p. 2492-2497, Taipei, Taiwan.

12 Ren W., Beard R. W., Curtis X W.: Satisficing control for multi-agent formation manoeuvres. In: Proceedings of the 41st IEEE Conference on Decision and Control. Las Vegas, Nevada 2002, Vol. 3, p. 2433-2438.

13 Tanner H., Pappas G., Kumar V.: Leader-to-formation stability. IEEE “Transactions on Robotics and Automation”

2004, Vol. 20, No 3, p. 443-455.

Pracę wykonano w ramach realizacji projektu badawczego nr N N501 068838.

STEROWANIE FORMACJĄ ROBOTÓW METODĄ ŚLEDZENIA LIDERA