%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Niniejszy plik jest przykładem wykorzystania stylu kkr9.sty dla osób
% korzystajšcych z systemu składu tekstu LaTeX, które pragnš przygotować
% propozycję artykułu na IX Krajowš Konferencję Robotyki.
% Jednoczenie plik ten zawiera podstawowe informacje dotyczšce organizacji
% tekstu artykułu.
%
% Autor: Bogdan Kreczmer
% Data:  20.02.2006
% E-mail: bogdan.kreczmer@pwr.wroc.pl
%
% Ostatnia modyfikacja: 24.02.2006
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
\documentclass[11pt,a4paper,twoside]{article}
\bibliographystyle{kkr9}
\usepackage[latin2]{inputenc}
\usepackage{kkr9}
\usepackage[pdftex]{graphicx}
\usepackage{amsmath}
\usepackage{amssymb}

%
% Pakiety dołšczane w pliku kkr9.sty:
%       polski
%       pslatex
%       ifthen
%       fancyhdr
%       calc
%
% Pakiety te sš dostępne w standardowych dystrybucjach.
% Sš również dostępne w archiwum:
%  ftp://ctan.tug.org
%  ftp://ftp.gust.org.pl/pub/CTAN/index.html
%

\tytulArtykulu{Sterowanie zespołu heterogenicznych\mbox{\hspace{2cm}} robotów 
na przykładzie pchania pudła}
%
% Jeli artykuł prezentuje wyniki prac nad projektem realizowanego
% np. w ramach grantu, to informacje o samym projekcie lub grancie
% można zamiecić w stopce artykułu wykorzystujšc makro
% \titleFootNote. Jego użycie spowoduje umieszczenie na końcu
% tytułu odnonika w postaci gwiazdki. Sama za treć informacji
% zostanie umieszczona w stopce artykułu na pierwszej stronie.
%
\stopkaPrzypisTytulu{Praca jest finansowana przez grant MEiN: 3 T11C038 29}


%
% Poniższe makro można wykorzystać w przypadku, gdy zachodzi potrzeba
% skrócenia tytułu, który ma pojawić się w nagłówkach artykułu.
% Poniżej przedstawiony jest sposób użycia tego makra. Należy pamiętać,
% aby na końcu użyć \ldots.
%
\naglowekTytulSkrocony{Sterowanie zespołu robotów
heterogenicznych}

%
% Makro \autorArtykulu umożliwia umieszczenie imienia i nazwiska
% autora zarówno pod tytułem artykułu jak też w nagłówkach kolejnych
% stron.
%
% Parametry:
%   1 - inicjałem imienia (imion) autora,
%   2 - imię (imona) autora,
%   3 - nazwisko autora.

\autorArtykulu{C.}{Cezary}{Zieliński}
%
% Dane dotyczšce instytucji, którš reprezentuje autor sš umieszczane
% poprzez makro \instytucja.
% Musi ono wystšpić bezporednio po makrze \autorArtykulu.
%
% Jeli dwóch autorów reprezentuje tę samš instytucję, to wystarczy
% makro to umiecić tylko po pierwszym z autorów.
%
\instytucja[IAIS]{Instytut Automatyki i~Informatyki Stosowanej,
Wydział Elektroniki i~Technik Informacyjnych, Politechnika
Warszawska, ul.~Nowowiejska 15/19, 00--665~Warszawa,
[C.Zielinski, W.Szynkiewicz, P.Trojanek, M.Majchrowski]@ia.pw.edu.pl}
\autorArtykulu{W.}{Wojciech}{Szynkiewicz}
\autorArtykulu{P.}{Piotr}{Trojanek}
\zlamLinieAutorow
\autorArtykulu{M.}{Marek}{Majchrowski}

%
% Poniższe makro może być wykorzystane do wymuszenia własnej postaci
% nagłówka z nazwiskami autorów, jeżeli standardowo generowany napis
% nie jest zadawalajšcy.
%
%\naglowekAutorzyArtykulu{J. Kowalczyk, A. Kowalczyk, J. Nowak}

% Inne pomocnicze makra ==============================================
% Do odwołań można wykorzystać dodatkowe makra:
%
%  \refwzor{etykieta}  -  odwołanie do wzoru.
%                  Przykład rezultatu odwołania:  (3)
%  \reftab{etykieta}  -  odwołanie do tablicy.
%                  Przykład rezultatu odwołania:  tab. 2
%  \refTab{etykieta}  -  odwołanie do tablicy.
%                  Przykład rezultatu odwołania:  Tab. 2
%  \refrys{etykieta}  -  odwołanie do rysunku.
%                  Przykład rezultatu odwołania:  rys. 2
%  \refRys{etykieta}  -  odwołanie do tablicy.
%                  Przykład rezultatu odwołania:  Rys. 2
%

\def\FuncFont{\tt}         % font nazw funkcji
\def\transmit{\rightarrowtail}

\begin{document}


\streszczenie{ %
W~artykule przedstawiono realizację zadania wspólnego pchania pudła przez zespół heterogenicznych robotów  mobilnych.
Zaproponowano formalny opis zadania z podziałem ról dla poszczególnych robotów z wykorzystaniem funkcji przejcia.
Proponowane podejcie zostało zweryfikowane zarówno w~badaniach symulacyjnych, jak i~na rzeczywistych robotach.
W~artykule zamieszczono wyniki realizacji zadania pchania dużego obiektu z wykorzystaniem trzech robotów nieholonomicznych Elektron.
}

\maketitle

\section{WSTĘP}

Dowiadczenia 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
\cite{Arkin:98,Khatib:99,Kube:99}. Jeli wiele robotów porusza się
w~tej samej przestrzeni roboczej ich ruchy muszš być koordynowane
(harmonizowane, jeli nie ma nadrzędnego koordynatora) w~celu uniknięcia
wzajemnych kolizji i~zakleszczeń. Złożonoć problemu planowania działań
zależy od bardzo wielu czynników m.in.\ liczby i~typu robotów, stopnia
ich współpracy, rodzaju rodowiska (statyczne/dynamiczne, znane/nieznane)
itp., \cite{Arkin:98,Dudek:02,Zie:05}.
Jednym z~zadań testowych dla współpracujšcych robotów mobilnych
jest wspólne pchanie pudła ({\itshape box pushing}) przez kilka
robotów \cite{Kube:99,Yamada:01,Wang:03}. Jest ono szczególnym
przypadkiem zadania transportu obiektu. Pchanie pudła odbywa się
na~płaszczynie, podczas gdy transport obiektów zazwyczaj wymaga
podnoszenia i~przenoszenia przedmiotów zespołowo przez kilka robotów
co istotnie zwiększa trudnoć zadania \cite{Miyata:02}. Wspólne
przemieszczanie obiektów jest przykładem zadania testowego dla takich
problemów jak koordynacja ruchów oraz utrzymanie szyku (formacji)
robotów \cite{Kube:99,Miyata:02,Yamada:01,Wang:03}. Wród
proponowanych podejć do zadania pchania pudła można wyróżnić podejcie
behawioralne polegajšce na~doborze zachowań i~takim ich rozdziale między
roboty aby osišgnšć pożšdany cel \cite{Kube:99,Parker98,Wang:03}. Inny
sposób polega na~zamknięciu obiektu, czyli otoczeniu obiektu przez trzy
lub więcej robotów, a~następnie zachowanie tych więzów w~trakcie całego
ruchu~\cite{Pereira:04}.

Opisane w tym artykule wspólne pchanie pudła jest reprezentantem szerszej klasy zadań wymagajšcych kooperatywnego przemieszczania przedmiotów bez ich unieruchomienia względem agentów przemieszczajšcych. W życiu codziennym często spotykamy się z koniecznociš zmiany umeblowania pomieszczeń, a co za tym idzie poprzesuwania ciężkich mebli, a to zazwyczaj wymaga współpracy osób pchajšcych.
%W~niniejszej pracy przedstawiono sformułowanie zadania, jego implementację oraz eksperymentalnš weryfikację z wykorzystaniem grupy składajšcej się z  trzech nieholnomicznych robotów Elektron. Dwa roboty wykonujš właciwe zadanie pchajšc pudło i~podšżajšc za trzecim robotem, który zadaje im prędkoć liniowš ruchu. W~strukturze tej zakłada się jawnš komunikację między robotem prowadzšcym a~robotami nadšżajšcymi.
W~niniejszej pracy przedstawiono sformułowanie zadania, implementację systemu sterujšcego realizujšcego to zadanie oraz eksperymentalnš weryfikację poprawnego jego wykonania przez grupę składajšcš się z trzech nieholnomicznych robotów Elektron.

\section{STRUKTURA STEROWANIA ROBOTA}

%Do opisu działania robotów tworzšcych zespół przyjęto formalizm opisany w \cite{Zie:05,Zie:05BulPAN}.
Do prezentacji struktury systemu sterujšcego zespołem robotów użyto
formalizmu zdefiniowanego w~\cite{Zie:05,Zie:05BulPAN}. Formalizm ten,
z jednej strony, okrela sposób zachowania się pojedynczego agenta
(przez zdefiniowanie jego funkcji przejcia), a z drugiej strony,
opisuje przepływy informacji niezbędnych do sterowania zarówno w agencie
jak i pomiędzy agentami. Z punktu widzenia implementacji oprogramowania
systemu sterujšcego formalny zapis funkcji przejcia oraz przepływu
danych wewnštrz układu stanowi specyfikację, według której projektant
tworzy system. Efektem ubocznym wprowadzenia formalizmu jest stworzenie
narzędzia do przeprowadzenia racjonalnych rozważań na temat funkcji
poszczególnych jego elementów składowych. Wtedy dyskusja sprowadza się
do okrelenia postaci poszczególnych funkcji przejcia -- matematyczna
postać tych funkcji okrela zachowanie się agentów, natomiast argumenty
tych funkcji okrelajš przepływy danych pomiędzy elementami składowymi
agentów jak i pomiędzy nimi.
Dla zwięzłoci oraz ze względu na~oczywistoć kontekstu oznaczenia
składowych układu sterowania robota oraz ich stanów nie będš tu
rozróżniane. Stan robota $a_{j}$, $j=0,\ldots,n_{a}$, traktowanego
tutaj jako agenta upostaciowionego, jest wyrażany jako:
\begin{equation}
s_{j} = \langle c_{j}, e_{j}, V_{j}, T_{j} \rangle
\label{EmbodiedAgentj}
\end{equation}
\begin{tabular}{lcl}
$c_{j}$ & -- & stan {\bf podsystemu sterujšcego} robotem (pamięć: zmienne, program etc.),\\
$e_{j}$ & -- & stan jego {\bf efektorów},\\
$V_{j}$ & -- & zestaw odczytów {\bf czujników wirtualnych}
wykorzystywanych przez robota,\\
$T_{j}$ & -- & informacja wymieniana z innymi robotami.\\
\end{tabular}\\
Zestaw odczytów czujników wirtualnych robota $a_{j}$ zawiera
$n_{v_{j}}$ składowych:
\begin{equation}
V_{j} = \langle v_{j_{1}}, \ldots,  v_{j_{n_{v_{j}}}}\rangle
\label{VSBundle}
\end{equation}
Odczyt każdego czujnika wirtualnego $v_{j_{k}}$, $k = 1, \ldots,
n_{v_{j}}$, tworzony jest przez agregację odczytów z jednego lub
więcej eksteroreceptorów. Tak więc $k$-ty czujnik wirtualny agenta
$a_{j}$ dostarcza odczyt:
\begin{equation}
v_{j_{k}} = f_{v_{_{j_{k}}}} \left (c_{j}, R_{j_{k}} \right ),
\label{VirtualSensor}
\end{equation}
gdzie $R_{j_{k}}$ jest zestawem odczytów z eksteroreceptorów skojarzonych
z $k$-tym czujnikiem wirtualnym:
\begin{equation}
R_{j_{k}} = \langle r_{j_{_{k_{1}}}}, \ldots, r_{j_{k_{n_{r}}}}
\rangle, \label{RBundle}
\end{equation}
gdzie $n_{r}$ jest liczbš tych eksteroreceptorów. Intereoreceptory
dostarczajš informacji o stanie wewnętrznym podsystemów
robota, natomiast proprioreceptory okrelajš stan jego efektorów,
przez co te grupy czujników zwišzane sš z innymi podsystemami niż
wyróżnione tutaj czujniki wirtualne.

Zadaniem układu sterowania $c_{j}$ robota $a_{j}$ jest:
\begin{itemize}
\item zebrać informacje o~stanie rodowiska agenta poprzez zestaw
czujników wirtualnych $V_{j}$,
\item uzyskać informacje od innych
robotów $a_{j^{\prime}}$ ($j^{\prime} \ne j$),
\item ledzić stan swych własnych efektorów $e_{j}$
\end{itemize}
i~na~tej podstawie oraz korzystajšc ze~zgromadzonej informacji
w~wewnętrznych strukturach danych układu sterujšcego robota ($c_{j}$)
wytworzyć:
\begin{itemize}
\item nowy (pożšdany) stan efektorów $e_{j}$,
\item wpłynšć na~przyszłe
działanie czujników wirtualnych $V_{j}$,
\item wysłać informacje do
innych robotów $a_{j^{\prime}}$,
\item ustalić nowe wartoci $c_{j}$
wewnętrznych struktur danych układu sterujšcego robotem.
\end{itemize}
\begin{figure}[htb]
    \begin{center}
       %\includegraphics[width=.76\textwidth]{strram12.pdf}
       \includegraphics[width=.76\textwidth]{strram12.pdf}
    \end{center}
\caption{Ogólna struktura sterowania pojedynczym robotem}
    \label{StrukturaAgenta}
\end{figure}
Dla efektywnego wykonania tych operacji zdekomponowano podsystem
sterujšcy $c_{j}$
w~następujšcy sposób (rys.~\ref{StrukturaAgenta}):\\[2mm]
\begin{tabular}{lcp{11.4cm}}
$c_{e_{j}}$ & -- & obraz efektora, czyli reprezentacja efektora w
podsystemie sterowania (np.\ położenia wałów silników, prędkoć liniowa i~obrotowa korpusu robota mobilnego),\\
$c_{V_{j}}$ & -- & obrazy czujników wirtualnych, czyli odczyty
czujników
wirtualnych i~ich aktualna konfiguracja,\\
$c_{T_{j}}$ & -- & bufory transmisyjne, czyli informacja
wzajemnie przekazywana między agentami,\\
$c_{c_{j}}$ & -- & pozostałe zmienne istotne dla sterowania agentem.\\
\end{tabular}\\[2mm]
Tak więc wyróżniono trzy typy komponentów podsystemu sterowania:
\begin{itemize}
\item komponenty dostarczajšce dane wejciowe: o~stanie efektorów,
odczytach czujników wirtualnych oraz informacji uzyskanej od innych
robotów (te komponenty wyróżniono dolnym lewym indeksem $x$),
\item komponenty przekazujšce dane wyjciowe z układu sterujšcego do efektorów,
czujników wirtualnych oraz do innych robotów (te komponenty
wyróżniono dolnym lewym indeksem $y$),
\item inne komponenty potrzebne do przetwarzania danych w~podsystemie
sterowania (bez wyróżniajšcego indeksu).
\end{itemize}
W~konsekwencji wyróżniono następujšce częci składowe
(rys.~\ref{StrukturaAgenta}):\\[2mm]
\begin{tabular}{lcl}
$_{x}c_{e_{j}}$ & -- & informacja o~aktualnym stanie efektora,\\
$_{x}c_{V_{j}}$ & -- & odczyty z czujników wirtualnych,\\
$_{x}c_{T_{j}}$ & -- & informacja uzyskana od innych robotów,\\
$_{y}c_{e_{j}}$ & -- & sterowanie efektorem,\\
$_{y}c_{V_{j}}$ & -- & polecenia dla czujników wirtualnych,\\
$_{y}c_{T_{j}}$ & -- & informacja dla innych robotów,\\
$c_{c_{j}}$ & -- & wewnętrzne struktury danych układu sterowania.\\
\end{tabular}\\[2mm]
Stan systemu zmienia się z pewnym kwantem czasu. % -- najczęciej zwišzanym z czasem próbkowania serwomechanizmów.
Niech $i$ będzie numerem aktualnego kwantu czasu. Wtedy kolejny kwant oznaczany jest jako $i+1$. Ponadto
przyjęto, że upływ czasu $i$ dla każdego robota jest niezależny,
ale nie wprowadzono oddzielnego symbolu dla zaznaczenia tego faktu, gdyż
symbol, którego indeksem jest $i$, zawiera również indeks $j$, a~więc w~pełni determinuje czas,
o który chodzi. W~prowadzonych tu
rozważaniach nie wymaga się synchronizmu pracy zegarów niezależnych układów sterujšcych pojazdami.

Podsystem sterowania robota $a_{j}$ na~podstawie:
\begin{equation}
_{x}c_{j}^{i} = \langle c_{c_{j}}^{i},\, _{x}c_{e_{j}}^{i},\,
_{x}c_{V_{j}}^{i},\, _{x}c_{T_{j}}^{i} \rangle
\label{inputquantities}
\end{equation}
wytwarza:
\begin{equation}
_{y}c_{j}^{i+1} = \langle c_{c_{j}}^{i+1},\, _{y}c_{e_{j}}^{i+1},\,
_{y}c_{V_{j}}^{i+1},\, _{y}c_{T_{j}}^{i+1} \rangle
\label{outputquantities}
\end{equation}
Do tego celu wykorzystuje {\bf funkcje przejcia}:
\def\arraystretch{1.3}
\begin{equation}
\left\{
\begin{array}{lcl}
c_{c_{j}}^{i+1} & = & f_{c_{c_{j}}} (c_{c_{j}}^{i},\, _{x}c_{e_{j}}^{i},\, _{x}c_{V_{j}}^{i},\, _{x}c_{T_{j}}^{i}) \\
_{y}c_{e_{j}}^{i+1} & = & f_{c_{e_{j}}} (c_{c_{j}}^{i},\, _{x}c_{e_{j}}^{i},\, _{x}c_{V_{j}}^{i},\, _{x}c_{T_{j}}^{i}) \\
_{y}c_{V_{j}}^{i+1} & = & f_{c_{V_{j}}} (c_{c_{j}}^{i},\, _{x}c_{e_{j}}^{i},\, _{x}c_{V_{j}}^{i},\, _{x}c_{T_{j}}^{i}) \\
_{y}c_{T_{j}}^{i+1} & = & f_{c_{T_{j}}} (c_{c_{j}}^{i},\, _{x}c_{e_{j}}^{i},\, _{x}c_{V_{j}}^{i},\, _{x}c_{T_{j}}^{i}) \\
\end{array}
\right.
\label{TransitionFunctions}
\end{equation}
\def\arraystretch{1}
%co można zapisać bardziej zwięle jako:
%\begin{equation}
%_{y}c_{j}^{i+1} = f_{c_{j}} (\, _{x}c_{j}^{i})
%\label{CompactTransitionFunction}
%\end{equation}

\section{PCHANIE PUDŁA}

Trzy roboty $a_{j}$, $j \in \{0, L, P\}$, majš za zadanie wspólnie pchać pudło $B$.
Robot $a_{0}$ jest robotem koordynujšcym. Jest on wyposażony w~skaner laserowy (eksteroreceptor),
dzięki czemu jest w~stanie okrelić
orientację pudła względem wybranego kierunku. Pozostałe dwa roboty faktycznie pchajš pudło. W~tym celu
ustawiajš się z jednej jego strony, tak aby czoło każdego z nich częciowo wystawało za róg pudła,
robot $a_{L}$ wystaje poza lewy róg, natomiast $a_{P}$ -- poza prawy.
Każdy z~robotów $a_{L}$, $a_{P}$ w~zderzaku przednim ma zainstalowane czujniki odległoci (eksteroreceptory),
na podstawie których
jest w~stanie ocenić jaka jest relacja pomiędzy rogiem pudła a~rodkiem pojazdu (\refrys{ustawienie}). Stosujšc opisany powyżej
formalizm, zadania robotów mogš być okrelone w~poniższy sposób. W~tym opisie pominięto składowe
stanu nieistotne z punktu widzenia realizacji zadania.

\begin{figure}[h]
 \begin{center}
 \includegraphics[width=.7\textwidth]{rys2b_draft.png}
 \end{center}
 \caption{Schemat ustawienia robotów w~trakcie wykonywania zadania}
 \label{ustawienie}
\end{figure}

Stan układu sterujšcego robota nadzorujšcego $a_{0}$ w~chwili $i$ jest następujšcy:
\def\arraystretch{1.2}
\begin{equation}\label{StanRobota0}
\left\{
\begin{array}{ccl}
    c_{c_{0}}^{i} & = & \langle \alpha^{i}_{D},  w^{i}_{D_{0}}, \omega^{i}_{D_{0}}, \alpha^{i-1}_{D}, \alpha^{i-1}_{B}, u^{i-1} \rangle\\
    _{x}c_{e_{0}}^{i} & = & \langle w^{i}_{E_{0}}, \omega^{i}_{E_{0}} \rangle\\
    _{x}c_{V_{0}}^{i} & = & \langle \alpha^{i}_{B} \rangle\\
\end{array}
\right.
\end{equation}
\def\arraystretch{1}
gdzie:\\
\begin{tabular}{lcp{11.2cm}}
$\alpha^{i}_{B}$ & -- & azymut pudła względem globalnego układu odniesienia odczytany za~pomocš skanera laserowego w~chwili $i$\\
$\alpha^{i}_{D}$ & -- & pożšdany azymut pudła w~chwili $i+1$ -- wartoć zadana pobrana z pamięci w~chwili $i$, ale do wykorzystania  w~chwili $i+1$\\
$\alpha^{i-1}_{B}$ & -- & azymut pudła względem globalnego układu odniesienia odczytany za~pomocš skanera laserowego w~chwili $i-1$, wartoć pobrana z~pamięci w~chwili $i$\\
$\alpha^{i-1}_{D}$ & -- & pożšdany azymut pudła w~chwili $i$ -- wartoć zadana pobrana z pamięci w~chwili $i$\\
\end{tabular}\\[2mm]
\begin{tabular}{lcp{11.2cm}}
$w^{i}_{D_{0}}$ & -- & pożšdana prędkoć liniowa korpusu robota koordynujšcego dla chwili \mbox{$i+1$} -- wartoć zadana pobrana z pamięci w~chwili $i$, ale do wykorzystania w~chwili $i+1$\\
$\omega^{i}_{D_{0}}$ & -- & pożšdana prędkoć obrotowa korpusu robota koordynujšcego dla chwili $i+1$ -- wartoć zadana pobrana z pamięci w chwili $i$, ale do wykorzystania w~chwili $i+1$\\
$w^{i}_{E_{0}}$ & -- & odczytana prędkoć liniowa korpusu robota koordynujšcego dla chwili~$i$\\
$\omega^{i}_{E_{0}}$ & -- & odczytana prędkoć obrotowa korpusu robota koordynujšcego dla chwili~$i$\\
$u^{i-1}$ & -- & wyjcie układu regulacji prędkoci liniowej robotów w~chwili $i-1$\\
\end{tabular}\\[2mm]
Na tej podstawie robot koordynujšcy oblicza sterowanie dla chwili $i+1$.
\begin{equation}\label{SterowanieRobota0}
\left\{
\begin{array}{ccl}
   _{y}c_{e_{0}}^{i+1} & = & f_{c_{e_{j}}} (\langle w^{i}_{D_{0}}, \omega^{i}_{D_{0}} \rangle, \langle w^{i}_{E_{0}}, \omega^{i}_{E_{0}} \rangle)\\
   _{y}c_{T_{0}}^{i+1} & = & f_{c_{T_{j}}} (\langle \alpha^{i}_{D}, \alpha^{i-1}_{D}, \alpha^{i-1}_{B}, u^{i-1} \rangle, \langle \alpha^{i}_{B} \rangle),\\
\end{array}
\right.
\end{equation}
gdzie $_{y}c_{e_{0}}^{i+1}$ stanowi nowe sterowanie ruchem robota koordynujšcego, natomiast $_{y}c_{T_{0}}^{i+1}$ jest informacjš
przekazywanš robotom pchajšcym, jak majš się zachować w~następnym kwancie czasu.
\begin{equation}\label{SterowanieRobotaLP}
    _{y}c_{T_{0}}^{i+1}  =  \langle  _{y}c_{T_{0L}}^{i+1}, \, _{y}c_{T_{0P}}^{i+1} \rangle,
\end{equation}
gdzie:
\def\arraystretch{1.4}
\begin{equation}\label{SterowanieRobotaJ}
\left\{
\begin{array}{ll}
u^i = u^{i-1} + K \left ( 1 + \frac{T_p}{2T_i} \right ) \frac{|\alpha^{i}_{D}-\alpha^{i}_{B}|}{\pi/2} + K \left( \frac{T_p}{2T_i} - 1 \right ) \frac{|\alpha^{i-1}_{D}-\alpha^{i-1}_{B}|}{\pi/2} \\
\begin{array}{ll}
\left\{
\begin{array}{ccl}
    %_{y}c_{T_{0L}}^{i+1} & = & w_{max} \left( 1 - \frac{|\alpha^{i}_{D}-\alpha^{i}_{B}|}{\pi/2} \right)\\
    _{y}c_{T_{0L}}^{i+1} & = & w_{max} \left( 1 - u^i \right ) \\
    _{y}c_{T_{0P}}^{i+1} & = & w_{max} \\
\end{array}
\right. & \mbox{dla } \alpha^{i}_{D}-\alpha^{i}_{B} > 0 \\
%
\left\{
\begin{array}{ccl}
    _{y}c_{T_{0L}}^{i+1} & = & w_{max} \\
    _{y}c_{T_{0P}}^{i+1} & = & w_{max} \\
\end{array}
\right. & \mbox{dla } \alpha^{i}_{D}-\alpha^{i}_{B} = 0 \\
%
\left\{
\begin{array}{ccl}
    _{y}c_{T_{0L}}^{i+1} & = & w_{max}  \\
    %_{y}c_{T_{0P}}^{i+1} & = & w_{max} \left( 1 - \frac{|\alpha^{i}_{D}-\alpha^{i}_{B}|}{\pi/2} \right) \\
    _{y}c_{T_{0P}}^{i+1} & = & w_{max} \left( 1 - u^i \right) \\
\end{array}
\right. & \mbox{dla } \alpha^{i}_{D}-\alpha^{i}_{B} < 0, \\
%
\end{array}
\end{array}
\right.
\end{equation}
\def\arraystretch{1}
gdzie $w_{max}$ jest maksymalnš prędkociš, z jakš może poruszać się pojazd, natomiast
$\alpha^{i}_{D}-\alpha^{i}_{B} > 0$ oznacza skręt pudła w~lewo, stšd robot $a_{L}$ musi
poruszać się wolniej niż robot $a_{P}$. Dla $\alpha^{i}_{D}-\alpha^{i}_{B} < 0$ pudło skręca w~prawo,
natomiast dla $\alpha^{i}_{D}-\alpha^{i}_{B} = 0$ pchane jest na~wprost (oczywicie w~rzeczywistej
implementacji wprowadzono ustalone eksperymentalnie marginesy). Element $u^i$ jest
sterowaniem wyznaczonym przez regulator PI, gdzie człon całkujšcy jest wymagany
ze względu na~różnice w~polizgu kół obydwu robotów (stałe $K$ i~$T_i$
zostały dobrane eksperymentalnie, $T_p=100\textrm{ms}$).

Stan każdego z robotów pchajšcych $a_{j}$, $j \in \{ L, P\}$ w~chwili $i$ jest okrelony w~następujšcy sposób:
\def\arraystretch{1.2}
\begin{equation}\label{StanRobotaJ}
\left\{
\begin{array}{ccl}
    c_{c_{j}}^{i} & = & \langle d^{i}_{D_{j}} \rangle\\
    _{x}c_{e_{j}}^{i} & = & \langle w^{i}_{E_{j}}, \omega^{i}_{E_{j}} \rangle\\
    _{x}c_{V_{j}}^{i} & = & \langle d^{i}_{B_{j}} \rangle\\
    _{x}c_{T_{j}}^{i} & = & \langle _{y}c_{T_{0j}}^{i} \rangle\\
\end{array}
\right.
\end{equation}
\def\arraystretch{1}
gdzie:\\
\begin{tabular}{lcp{11.1cm}}
$_{y}c_{T_{0j}}^{i}$ & = & $w^{i}_{D_{j}}$ -- zadana prędkoć liniowa pojazdu obliczona przez robota $a_{0}$ zgodnie z (\ref{SterowanieRobotaJ})\\
$d^{i}_{B_{j}}$ & -- & położenie rodka zderzaka pojazdu $a_{j}$ względem rogu pudła w~chwili~$i$\\
$d^{i}_{D_{j}}$ & -- & pożšdane  położenie rodka zderzaka pojazdu
$a_{j}$ względem rogu pudła w~chwili \mbox{$i+1$} -- wartoć zadana
obliczona w~kwancie czasu $i$ na~podstawie zależnoci
(\ref{SterowanieRobotaJd}) -- wartoć ta
będzie wykorzystania w~chwili $i+1$\\
\end{tabular}\\[2mm]
Wartoć $d^{i}_{B_{j}}$ okrelana jest za pomocš odpowiedniej funkcji agregujšcej (\ref{VirtualSensor})
na podstawie odczytów czujników odległoci zamontowanych na~przednim zderzaku pojazdu.

Zachowanie robota pchajšcego zależy od polecenia uzyskanego od robota koordynujšcego:
$_{y}c_{T_{0j}}^{i} = w^{i}_{D_{j}}$ oraz położenia rodka jego zderzaka względem
rogu $j$ pudła.
\begin{equation}
\label{SterowanieRobotaJE}
_{y}c_{e_{j}}^{i+1}  =  f^{\prime}_{c_{e_{j}}} \left( \langle w^{i}_{D_{j}}, \omega^{i}_{D_{j}} \rangle \right)
\end{equation}
gdzie:
\begin{equation}\label{SterowanieRobotaJomega}
\omega^{i}_{D_{j}}  =  f^{\prime\prime}_{c_{e_{j}}} (d^{i}_{D_{j}},
d^{i}_{B_{j}}, w^{i}_{D_{j}})
\end{equation}
natomiast
\def\arraystretch{1.2}
\begin{equation}
\label{SterowanieRobotaJd}
d^{i}_{D_{j}} = \left\{
\begin{array}{lll}
 0   & & \mbox{dla } w^{i}_{D_{j}} = w_{max} \\
 d_{Dm}  & & \mbox{dla } w^{i}_{D_{j}} < w_{max}\\
\end{array}
\right.
\end{equation}
\def\arraystretch{1}
Wartoć $d_{Dm}$ dobrano eksperymentalnie. Dla robota $a_{L}$ jest to odległoć pomiędzy rodkiem zderzaka, a
punktem położonym porodku między ostatnim i~przedostatnim czujnikiem odległoci po lewej stronie pojazdu.
Dla robota $a_{P}$ odległoć ta dotyczy pary czujników położonych z prawej strony pojazdu.

Funkcja $f^{\prime\prime}_{c_{e_{j}}} (\bullet)$ użyta w
(\ref{SterowanieRobotaJE}) ma dla pojazdu $a_{j}$ następujšcš
postać:
\begin{equation}%\label{SterowanieRobotaJE}
f^{\prime\prime}_{c_{e_{j}}} (d^{i}_{D_{j}}, d^{i}_{B_{j}},
w^{i}_{D_{j}}) = k \left(  d^{i}_{D_{j}} - d^{i}_{B_{j}} \right)
\end{equation}
Wartoć współczynnika wzmocnienia $k$ dobrano eksperymentalnie. Argument
$w^{i}_{D_{j}}$ funkcji $f^{\prime\prime}_{c_{e_{j}}} (\bullet)$
jest potrzebny do wyznaczenia aktualnej wartoci zadanej położenia
$d^{i}_{D_{j}}$ -- jest to robione na~podstawie zależnoci
(\ref{SterowanieRobotaJd}).

Wszystkie trzy roboty działajš w~oparciu o~poniższy prosty algorytm:
\def\arraystretch{1.2}
\begin{equation}
\begin{array}{llll}

\multicolumn{4}{l}{\mbox{\FuncFont loop }}\\

\multicolumn{4}{l}{\mbox{\hspace*{3mm}\ // Okrelenie aktualnego stanu robota}}\\

\multicolumn{4}{l}{\mbox{\FuncFont \hspace*{3mm}\ } e_{j} \transmit\, _{x}c_{e_{j}}^{i}; \;\;\; V_{j} \transmit\, _{x}c_{V_{j}}^{i}; \;\;\; c_{T_{j^{\prime}}} \transmit\, _{x}c_{T_{j}}^{i};}\\

\multicolumn{4}{l}{\mbox{\hspace*{3mm}\ // Okrelenie następnego stanu robota}}\\

\hspace*{3mm} & _{y}c_{j}^{i+1} & :=  & f_{c_{j}} (_{x}c_{j}^{i})  \\

\multicolumn{4}{l}{\mbox{\hspace*{3mm}\ // Wysłanie rezultatów obliczeń}}\\

\multicolumn{4}{l}{\mbox{\FuncFont \hspace*{3mm}\ } _{y}c_{e_{j}}^{i+1} \transmit e_{j}; \;\;\; _{y}c_{V_{j}}^{i+1} \transmit V_{j}; \;\;\; _{y}c_{T_{j}}^{i+1} \transmit c_{T_{j^{\prime}}};}\\

\multicolumn{4}{l}{\mbox{\FuncFont \hspace*{3mm}\ } i := i+1;}\\

\multicolumn{4}{l}{\mbox{\FuncFont endloop\ }}
\end{array}
\label{EndlessLoopSingleStepBasic}
\end{equation}
\def\arraystretch{1}
gdzie symbol ``$\transmit$'' okrela przekazanie informacji pomiędzy podsystemami,
natomiast podwójny ukonik poprzedza komentarz. Ponieważ algorytm
(\ref{EndlessLoopSingleStepBasic}) dotyczy wszystkich robotów,
niektóre przesłania mogš nie być wykorzystywane w~konkretnym przypadku.

\section{EKSPERYMENTY}

Zaproponowana metoda pchania została wstępnie zweryfikowana
z~wykorzystaniem symulatora trójwymiarowego \emph{Gazebo} (\refrys{symulator}) --
częci projektu \emph{Player/Stage}, gdzie do modelowania dynamiki
obiektów wykorzystywana jest biblioteka \emph{Open Dynamics Engine}
\cite{ode,playerstage}. Zaletš takiego podejcia jest to, że
symulator udostępnia ten sam interfejs programistyczny co rzeczywiste
roboty (w~ten sposób nie ma potrzeby duplikowania kodu dla obydwu tych
przypadków).

\begin{figure}[hbtp]
 \begin{center}
 \includegraphics[width=.499\textwidth]{DSC_1299_draft.jpg}
 \includegraphics[width=.49\textwidth]{scr.png}
 \end{center}
 \caption{Zadanie transportu pudła -- widok rzeczywistego eksperymentu oraz
          obraz z~symulatora}
 \label{symulator}
\end{figure}

Pchanie pudła zostało zrealizowane przy wykorzystaniu nieholonomicznych robotów
mobilnych typu ,,czołg'' -- gdzie napęd jest przenoszony za pomocš pasków
z~dwóch silników na~koła po lewej i~prawej stronie. Taki układ może być z
dobrym przybliżeniem traktowany jako układ różnicowy. Roboty sš wyposażone
m.in. w~6~czujników odległoci o~zakresie działania do
50 cm. Robot kontrolujšcy położenie obiektu jest wyposażony w~dalmierz
laserowy SICK model LMS200. Komunikacja między robotami zapewniona jest
przez łšcze WLAN.

%\begin{figure}[hbtp]
% \begin{center}
% \includegraphics[width=.6\textwidth]{scr.png}
% \end{center}
% \caption{Symulacja zadania -- wizualizacja 3D oraz cieżki obiektów}
% \label{symulator}
%\end{figure}

%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 $\phi$ pomiędzy prostopadłš do pudła poprowadzonš z~jego rodka --
%a~półprostš poprowadzonš z~tego punktu w~kierunku rodka robota
%wodzšcego (\refrys{ustawienie}). Poniżej zamieszczono uzyskanš
%trajektorię transportowanego obiektu oraz wykres kšta $\phi$
%traktowanego w~tym przypadku jako uchyb regulacji (\refrys{przebiegi}).
%%W~trakcie realizacji zadania przez rzeczywiste roboty prędkoci
%%$w^{i}_{E_{0}}$ oraz $\omega^{i}_{E_{0}}$ uzyskiwane na podstawie
%%odometrii z~enkoderów umieszczonych na osiach kół
%%obarczone były dużym błędem -- co wynikało z~faktu polizgu
%%robota w~trakcie pchania pudła o~wadze kilkunastu kilogramów.
%%W~celu likwidacji stałej różnicy kšta $\varphi$ konieczne okazało
%%się wprowadzenie do równań \refwzor{SterowanieRobotaJ} stanowišcych
%%w~istocie regulator proporcjonalny członu całkujšcego --
%%czego konsekwencjš jest także widoczne przeregulowanie
%%przebiegu na wykresie (\refrys{przebiegi}).
%Konsekwencjš zastosowania regulatora PI do likwidacji stałej różnicy kšta
%$\phi$ czego konsekwencjš jest widoczne przeregulowanie przebiegu
%na~wykresie.
W trakcie eksperymentów robot nadzorujšcy był nieruchomy i jedynie
przekazywał robotom pchajšcym informację o aktualnej wartoci kšta
$\phi$ pomiędzy prostš prostopadłš do pudła, poprowadzonš przez jego
rodek, a półprostš poprowadzonš z punktu rodkowego ciany pudła
zwróconej w kierunku robota nadzorujšcego, a~rodkiem tego robota
(\refrys{ustawienie}). Zadaniem robotów pchajšcych była realizacja
zleconej przez robota nadzorujšcego prędkoci liniowej, a w konsekwencji
minimalizacja kšta~$\phi$, oraz utrzymanie się na krawędzi pudła przez
odpowiednie zmiany swej prędkoci kštowej. Poniżej zamieszczono uzyskanš
trajektorię transportowanego obiektu oraz wykres kšta~$\phi$ traktowanego
w tym przypadku jako uchyb regulacji (\refrys{przebiegi}). Konsekwencjš
zastosowania regulatora PI do likwidacji stałej różnicy kšta~$\phi$
jest widoczne na wykresie przeregulowanie przebiegu.

Zadanie było kończone, gdy pudło znalazło się w pobliżu robota
nadzorujšcego -- wtedy dodatkowo zmieniano jego orientację tak, aby
linie rodkowe pudła i~robota wiodšcego były się równoległe.

\begin{figure}[hbtp]
 \begin{center}
 \includegraphics[width=.499\textwidth]{boxtrace2.pdf}
 \includegraphics[width=.49\textwidth]{phi2.pdf}
 \end{center}
 \caption{Położenie pudła w~trakcie pchania oraz wykres uchybu regulacji}
 \label{przebiegi}
\end{figure}

\section{WNIOSKI}

W~wyniku przeprowadzonych prac została zaproponowana metoda transportu
obiektu przez roboty nieholonomiczne wykorzystujšca warstwowy układ
regulacji -- zadaniem każdego z~robotów jest utrzymywanie odpowiedniej
orientacji względem obiektu, natomiast pożšdany ruch obiektu uzyskiwany
jest dopiero przez odpowiedniš koordynację prędkoci robotów.
Zaprezentowany algorytm został zweryfikowany zarówno w~rodowisku
symulacyjnym jak i~na rzeczywistych robotach. Bez utraty ogólnoci
możliwe jest zastosowanie takiego podejcia do manipulacji bardziej
złożonymi obiektami przez większe grupy robotów.

%Zastosowana metoda symulacji pozwala w~łatwy i~szybki sposób testować
%algorytmy manipulacji trójwymiarowymi obiektami uwzględniajšc rzeczywiste
%oddziaływania -- takie jak tarcie czy polizg dostarczajšc jednoczenie
%informacji z~czujników, w~które wyposażony jest modelowany robot.

\bibliography{pudlo}

%
% Tytuł artykułu w języku angielskim
%
% Parametry:
%  1 - tytuł referatu w języku angielskim,
%  2 - treć streszczenia w języku angielskim.
%
\streszczenieAngielskie{CONTROL OF A TEAM OF HETEROGENEOUS ROBOTS:
EXAMPLE OF COOPERATIVE BOX PUSHING}{The paper presents the implementation
of the task in which two robots push a~box under the supervision of
a~third robot. The pushing robots and the supervisory robot use different
sensors, thus the system is heterogeneous. The task is described
formally. Its execution has been verified both in simulation and on
Electron  non-holonomic robots.}

\end{document}
