Politechnika Warszawska
Wydział Elektroniki i Technik
Informacyjnych
Instytut Automatyki i Informatyki
Stosowanej
Wrzesien 2007
Praca dyplomowa inzynierska
Paweł Najgebauer
Serwomechanizm wizyjnyw systemie MRROC++
Opiekun pracy:
mgr Tomasz Winiarski
Streszczenie
W ramach niniejszej pracy powstała aplikacja w systemie MRROC++,
która realizowała sledzenie obiektu w czasie rzeczywistym przy pomocy ma-
nipulatora. Tematyka pracy łaczy ze soba dwie dziedziny - dziedzine rozpoz-
nawania obrazu i robotyki. Z załozenia, manipulator miał sledzic niebieska
piłke, utrzymujac sie w odległosci 20 cm od niej. Wprowadzono dwa kryteria
poprawnosci systemu. Po pierwsze system miał działac w czasie rzeczy-
wistym - sledzenie piłki miało byc jak najszybsze. Po drugie system miał
działac bezpiecznie - manipulator nie mógł gwałtownie ruszac i zatrzymywac
sie, płynnie zmieniac kierunek ruchu i byc odpornym na gwałtowne zniknie-
cia i pojawienia sie piłki w polu widzenia. Równiez manipulator powinien
byc odporny na błedne odczyty kamery. Aplikacja, która powstała, w pełni
realizowała postawione przed nia zadania.
Słowa kluczowe: robotyka serwomechanizm rozpoznawanie obrazu manip-
ulacja sledzenie MRROC++.
Abstract
Title: Visual servo control in MRROC++ system.
This thesis describes the application written in MRROC++ system whose
purpose was to follow a specific object with the manipulator. Both image
recognition and robotics fields were joined within this thesis. On the assupm-
tion that the followed object was a blue ball, two correctness criteria were
taken into consideration. The first criterion was that the system ought to
work in real time, following the ball the fastest it could. The latter was that the
system had to work safely: the manipulator should move smoothly, without
rapid starts and stops. Furthermore, safety implies insensitivity to sudden
appearance and disappearance of the ball or erroneous camera responses.
All aforementioned requirements were met in present thesis.
Key words: robotics servomechanism image recognition manipulation follow-
ing MRROC++.
Specjalne podziekowania kieruje do Romana, Romka oraz Rysia za
nieocenione wskazówki i rady podczas pisania pracy dyplomowej.
Spis tresci
1. Wprowadzenie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61.1. Serwomechanizmy wizyjne . . . . . . . . . . . . . . . . . . . . . . . . . . 61.2. Cel pracy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7
2. Srodowisko . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82.1. Manipulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82.2. Wizja . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 92.3. System MRROC++ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
2.3.1. Struktura systemu MRROC++ . . . . . . . . . . . . . . . . . . . . 10
3. Architektura aplikacji . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133.1. Zadanie procesu ECP . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 133.2. Proces VSP . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
3.2.1. Zasada działania . . . . . . . . . . . . . . . . . . . . . . . . . . . 143.2.2. Załozenia . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 143.2.3. Przestrzen kolorów . . . . . . . . . . . . . . . . . . . . . . . . . . 153.2.4. Opis algorytmu lokalizacji . . . . . . . . . . . . . . . . . . . . . . 163.2.5. Opis innych testowanych algorytmów . . . . . . . . . . . . . . . 20
3.3. Generator ruchu . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 213.3.1. Wyznaczenie połozenia piłki w 3D . . . . . . . . . . . . . . . . . 223.3.2. Regulator . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
4. Badania i testy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 284.1. Zadanie przestawiania . . . . . . . . . . . . . . . . . . . . . . . . . . . . 284.2. Zadanie nadazania . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35
4.2.1. Badanie na tasmociagu z predkoscia 2cm/s . . . . . . . . . . . 354.2.2. Badanie na tasmociagu z predkoscia 4cm/s . . . . . . . . . . . 37
5. Podsumowanie i wnioski . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 415.1. Perspektywy rozwoju . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42
Bibliografia . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
1. Wprowadzenie
1.1. Serwomechanizmy wizyjne
W dziedzinie robotyki niezwykle waznym aspektem jest sprzezanie pracy
robota w wizja. Zajmuja sie tym tzw. serwomechanizmy wizyjne. Sa
to układy, które zbieraja informacje przekazywane przez jedna lub wiele
kamer, przetwarzaja te dane i wyznaczaja róznice miedzy aktualnym połoze-
niem robota a tym pozadanym. W zaleznosci od typu zadania jakiemu
poswiecony jest serwomechanizm, kamera moze obserwowac przykładowo
koncówke manipulatora, całego robota badz byc umieszczona w chwytaku i
obserwowac sledzony lub chwytany obiekt. Przyjmuje sie pewne oznaczenia
w ramach klasyfikacji serwomechanizmów wizyjnych ( [4] i [5]). Rozróznia
sie układy z kamera nieruchoma (SAC - ang. Stand Alone Camera) lub
ruchoma, umieszczona w koncówce manipulatora (EIH - ang. Eye-in-hand).
Ukad z kamera obserwujaca koncówke manipulatora oznacza sie sym-
bolem ECL (ang. Endpoint closed-loop) natomiast układ nie obserwujacy
koncówki - EOL (ang. Endpoint open-loop). Stosujac algorytmy rozpoz-
nawania obrazu mozna przetworzyc surowe dane z kamery w informacje
o koniecznej dla zadania semantyce. Moze to byc na przykład połoze-
nie i orientacja koncówki manipulatora wzgledem ustalonego miejsca lub
połozenie sledzonego obiektu. Regulator serwomechanizmu wizyjnego ma
za zadanie, na podstawie wyznaczonych połozen, obliczyc uchyb wizyjny po
czym obliczyc sterowanie. Uchyb wizyjny moze byc mierzony w przestrzeni
cech obrazu (IB - ang. Image Based) lub w przestrzeni zadaniowej robota
(PB - ang. Position Based). Sterowanie moze byc wyznaczone wzgledem ak-
tualnej pozycji lub bezwzglednie, przy czym zazwyczaj jest to wartosc zadana
dla regulatorów nizszego poziomu, wyznaczajacych uchyby w przestrzeni
konfiguracyjnej robota.
1.2. Cel pracy 7
1.2. Cel pracy
Celem pracy inzynierskiej było stworzenie systemu roboty-
cznego sledzacego ruch z góry zdefiniowanego obiektu. Prace
były prowadzone w oparciu o istniejacy system MRROC++ (ang.
Multi − RobotResearchOrientedController, 2.3) i manipulatory, którymi
dysponuje laboratorium robotyki. Przyjete zostały nastepujace załozenia
dotyczace pracy:
— Sledzonym obiektem jest niebieska piłka o znanych wymiarach (srednica
5 cm)
— Detekcja obiektu odbywa sie poprzez zamontowana w chwytaku manip-
ulatora kamere
— Sledzony obiekt moze poruszac sie w przestrzeni 3D
— Sledzenie obiektu odbywa sie wyłacznie jesli jest on widziany przez
kamere
— Manipulator ma za zadanie nadazac za obiektem w czasie rzeczywistym -
trajektoria obiektu nie jest buforowana i nie moze byc pózniej odtworzona
— Manipulator ma utrzymywac odległosc 20 cm miedzy obiektywem
kamery a obiektem
— Kryteriami poprawnosci działania systemu sa:
— Szybkosc nadazania za obiektem
— Bezpieczenstwo działania systemu, rozumiana jako odpornosc na
nieprawidłowe dane z kamery, łagodne rozpoczynanie i konczenie
ruchu oraz wygładzanie trajektorii.
2. Srodowisko
2.1. Manipulator
Manipulatorem wykorzystywanym w pracy był Irp-6 (rysunek 2.1) o 8
stopniach swobody. Roboty przemysłowe takie jak Irp-6 sa uniwersalnymi
srodkami automatyzacji procesów przemysłowych, zastepujacymi człowieka
w pracach uciazliwych lub trudnych, wymagajacych precyzji, szybkosci
i wielokrotnej powtarzalnosci. Roboty tego typu moga same wykonywac
pewne prace przy uzyciu narzedzi, jak na przykład spawanie łukowe, szli-
fowanie czy stepianie krawedzi.
Rysunek 2.1. Robot Irp6
Robot Irp-6 składa sie z czesci manipulacyjnej i oddzielonej konstruk-
cyjnie szafy sterowniczej. W szafie sterowniczej sa umieszczone moduły
układu sterowania łacznie ze sterownikami mocy silników, dzieki czemu
czesc manipulacyjna jest nieduza i wzglednie lekka. Szafa sterownicza
z elektronicznymi elementami układu sterowania moze byc umieszczona
oddzielnie z dala od czesci manipulacyjnej, co stosuje sie w przypadku pracy
robota w szczególnie ciezkich warunkach otoczenia.
2.2. Wizja 9
Roboty komunikuja sie z komputerami PC przez dedykowane karty
rozszerzen ISA oraz siec ethernet. System operacyjny czasu rzeczywistego
QNX pozwala na swobodna komunikacje procesów miedzy osobnymi maszy-
nami, dzieki czemu mozliwe jest rozpraszanie obliczen na rózne komputery.
Rysunek 2.2. Koncówka manipulatora Irp6
2.2. Wizja
Robot został wyposazony w miniaturowa kamere P1 umiejscowiona w
chwytaku manipulatora (rysunek 2.3). Kamera połaczona była z karta ak-
wizycji zainstalowana w komputerze klasy PC. Do komunikacji z kamera
uzyto wczesniej opracowanego sterownika bttv do kart z chipsetem bt848
lub bt878. Kamera pracowała z czestotliwoscia 60 Hz co w praktyce oz-
naczało dostarczanie maksymalnie 60 półobrazów na sekunde.
2.3. System MRROC++
MRROC++ (ang. Multi − RobotResearchOrientedController, [6], [7]) jest
programowa struktura ramowa wspomagajaca tworzenie sterowników dla
systemów wielorobotowych. Sterowniki tworzy sie w jezyku C++ - tym
samym, w którym powstał MRROC++. Aplikacje napisane w oparciu o
MRROC++ działaja pod systemem operacyjnym QNX i maja cechy systemu
czasu rzeczywistego. Aplikacja, bedaca integralna czescia niniejszej pracy
dyplomowej, została stworzona całkowicie w ramach systemu MRROC++.
2.3. System MRROC++ 10
Rysunek 2.3. Kamera wewnatrz chwytaka
2.3.1. Struktura systemu MRROC++
Sterownik zbudowany w srodowisku MRROC++ mam strukture hierar-
chiczna. Na szczycie owej hierarchii znajduja sie procesy nadrzedne, steru-
jace cała aplikacja. Procesy znajdujace sie w dole hierarchii odwołuja sie do
sterowników rzeczywistych urzadzen. Aktualna struktura systemu została
przedstawiona na rysunku 2.4 (za [7, str. 14]). Procesy sterownika MR-
ROC++ działaja w sieci lokalnej pod kontrola systemu QNX 6.3. W na-
jnizszej warstwie znajduja sie procesy EDP (ang. EffectorDriverProcess)
odpowiedzialne za komunikacje ze sterownikami efektorów i kinematyke
robota oraz procesy VSP (ang. V irtualSensorProcess). W ramach komu-
nikacji EDP ze sterownikami efektorów nastepuje obukierunkowa wymina
informacji, tj. EDP wysyła rozkazy dotyczace ruchów manipulatora a
odbiera dane z czujników wewnetrznych, mówiacych w szczególnosci o
orientacji, połozeniu lub obrocie przegubów. Procesy VSP natomiast
odpowiadaja za komunikacje ze sterownikami czujników systemu roboty-
cznego. W warstwie wyzszej znajduja sie procesy ECP i MP. ECP (ang.
EffectorControlProcess) jest procesem, który w uogólnieniu zajmuje sie zle-
caniem zadan dla elementów systemu robotycznego i odbiorem informacji
o jego stanie. Zlecanie zadan odbywa sie w komunikacji z EDP, natomi-
ast odbiór informacji o stanie systemu - w komunikacji zarówno z EDP
jak i VSP. W srodowisku wielorobotowym synchronizacja zadan zajmuje sie
2.3. System MRROC++ 11
Rysunek 2.4. Struktura systemu MRROC++
MP (ang. MasterProcess). W najwyzszej warstwie znajduje sie proces UI
(ang. UserInterface) zajmujacy sie komunikacja z uzytkownikiem i proce-
sami ECP-MP. Odbiera równiez komunikaty od EDP i VSP.
System MRROC++ umozliwia sterowanie jednym lub wieloma robotami w
sposób pozycyjny, pozycyjno-siłowy lub siłowy. Sterowanie pozycyjne moze
odbywac sie na przykład za pomoca:
— zmiennych wewnetrznych (połozenia wałów silników lub tłoków siłown-
ików)
— zmiennych zewnetrznych (pozycja w przestrzeni kartezjanskiej, orien-
tacja w przestrzeni katów Eulera)
W sterowaniu pozycyjnym okresla sie wartosci połozenia lub orientacji w
metrach badz radianach i sa to wartosci zadane dla regulatorów manipula-
tora. Układy pomiarowe w kazdym członie manipulatora dostarczaja infor-
macji o jego aktualnym połozeniu. Róznica wartosci zadanej i aktualnego
połozenia wyznacza uchyb regulacji. Zadaniem tych regulatorów jest wyz-
naczanie takiego sterowania aby uchyb sie zmniejszał. Poniewaz system
2.3. System MRROC++ 12
MROCC++ umozliwia automatyczne przeliczanie zmiennych zenetrznych
(przestrzen operacyjna) na zmienne wewnetrzne (przestrzen konfigura-
cyjna), zadania dla manipulatorów zazwyczaj formułowane sa w przestrzeni
operacyjnej, która jest intuicyjna dla twórcy sterownika, a sterowanie
odbywa sie w przestrzeni konfiguracyjnej.
3. Architektura aplikacji
Aplikacja realizujaca serwomechanizm wizyjny została podzielona
na kilka logicznych czesci:
— Zadanie - algorytm nadrzedny, który powołuje do zycia wirtualny sensor
i generator
— Wirtualny sensor - proces realizujacy czytanie obrazów z urzadzenia
reprezentujacego w systemie operacyjnym kamere (w postaci pliku),
wyszukiwanie na obrazie wzorca i zapisywanie wyniku do bufora komu-
nikacyjnego
— Generator - algorytm generujacy trajektorie ruchu koncówki manipula-
tora
Zgodnie z załozeniami pracy, obiekt sledzony jest przez kamere umiejs-
cowiona w chwytaku manipulatora. Uchyb wizyjny, o czym bedzie mowa w
3.3.2, mierzony jest po czesci w przestrzeni cech obrazu (połozenie w X i
Y ) a po czesci w układzie zwiazanym z obiektywem kamery (Z - odległosc
obiektu od obiektu). W zwiazku z tym stworzony serwomechanizm wizyjny
mozna zakwalifikowac jako IB/PB-EOL-EIH.
3.1. Zadanie procesu ECP
Aplikacja jednorobotowa w systemie MRROC++ moze byc zdefiniowana
jako zadanie procesu ECP. Tak tez jest w opisywanym systemie. W zada-
niu powołany jest obiekt reprezentujacy robota, nastepnie uruchamiany
jest proces VSP o którym mowa w punkcie 3.2. Nastepnie tworzony jest
obiekt generatora ruchu (3.3) i przekazywany jest wskaznik do komunikacji
z VSP. W głównej funkcji zadania ECP w petli nieskonczonej uruchamiane
jest polecenie Move, które jako parametr pobiera obiekt generatora. Polece-
nie Move jest odpowiedzialne za wszelka komunikacje miedzy generatorem
ruchu a pozostałymi modułami systemu, takimi jak sensory wirtualne (VSP)
3.2. Proces VSP 14
czy sterowniki efektorów (EDP), czyli w efekcie, za współprace całej aplikacji
i w ostatecznosci - za ruch robota.
3.2. Proces VSP
3.2.1. Zasada działania
Na ogół dane, które dostarczaja czujniki, sa nieadekwatne do potrzeb
zadania, jakie ma wykonac aplikacja MRROC++. VSP (ang. Virtual Sen-
sor Process, [2]) ma za zadanie zbierac informacje z rzeczywistych recep-
torów i przetwarzac je na potrzeby innych modułów aplikacji. W przy-
padku niniejszej pracy jedynym sensorem jest kamera dostarczajaca obrazy,
których w postaci surowej nie dałoby sie wykorzystac. Wirtualny proces
przetwarza kazdy taki obraz (w rozdzielczosci 768 na 576 pikseli) na trzy
liczby: współrzedne znalezionego obiektu i jego rozmiar.
Współpraca VSP z procesami warstwy wyzszej odbywa sie na jeden z
trzech sposobów:
— współpraca interaktywna z oczekiwaniem - procesy ECP/MP musza za-
czekac az VSP zakonczy odczyt
— współpraca interaktywna bez oczekiwania - jeden watek procesu VSP
komunikuje sie z procesami warstwy wyzszej, drugi wykonuje polecenia,
jednak nie zatrzymuje pracy procesów ECP/MP
— współpraca nieinteraktywna - jeden watek procesu VSP cyklicznie
dokonuje odczytów z czujnika, drugi watek komunikuje sie z warstwa
wyzsza
W zaleznosci od typu współpracy, programista dostarczajacy wirtu-
alny sensor ma za zadanie wybranie jednej z trzech powłok realizu-
jaca odpowiedni typ współpracy. W aplikacji realizujacej serwomecha-
nizm wizyjny zdecydowano sie na współprace interaktywna bez oczekiwa-
nia, poniewaz proces VSP nie moze blokowac działania ECP. Nie było tez
potrzeby cyklicznego odczytywania czujnika.
3.2.2. Załozenia
Poniewaz kamera pracuje z czestotliwoscia 60Hz, aby w pełni wykorzys-
tac jej szybkosc algorytm lokalizowania obiektu na obrazie nie powinien
3.2. Proces VSP 15
przekraczac 160
sekundy czyli ok 16.7ms. Kamera przesyła z taka czes-
totliwoscia obrazy z przeplotem co w praktyce oznacza otrzymywanie je-
dynie półobrazów. Aby uniknac nieprawidłowosci w odczycie, co druga linia
obrazu była ignorowana.
3.2.3. Przestrzen kolorów
Jednym z załozen pracy był fakt, iz sledzonym obiektem jest niebieska
piłka o srednicy 5cm. Aby efektywnie odnajdowac obiekt na obrazie,
nalezało najpierw odnalezc wszystkie piksele, które moga potencjalnie
nalezec do obrazu obiektu. Kazdy punkt obrazu jest opisany przez zestaw
liczb definiujacych jego kolor. Wybór tylko tych pikseli, które prezen-
tuja wszelkie odcienie szukanego koloru, wymagał ustalenia odpowiedniego
podzbioru przestrzeni kolorów. Wprawdzie kamera przekazuje informacje
o kolorach w przestrzeni RGB, jednak przetestowano czy dobranie progów
w innych przestrzeniach kolorów nie byłoby łatwiejsze. Po sprawdzeniu
przestrzeni RGB (ang. Red Green Blue - Czerwony Zielony Niebieski), YUV,
HSL (ang. Hue Saturation Lightness - Barwa Nasycenie Jasnosc) i HSV
(ang. Hue Saturation Value - Barwa Nasycenie Wartosc) zauwazono iz w
tej ostatniej przestrzeni szukany podzbiór wyrazony był minimalna iloscia
nierównosci (H powinno miescic sie miedzy wartosciami 160 a 240; S i V
powinny byc wieksze od 0.3). Jednak kazdorazowe przeliczanie RGB na
HSV okazało sie bardzo czasochłonne i algorytm wyszukiwania przekraczał
załozone 160
sekundy.
Rozwiazaniem tego problemu jest stworzenie tzw. lookup table, czyli
zestawu trzech tablic odwzorowujacych przestrzen RGB na HSV. Jej opra-
cowaniem zajmuje sie funkcja countLUT , która uruchamia sie jednorazowo
na poczatku działania aplikacji. Czas działania funkcji countLUT zamyka
sie w 9ms. Algorytm wyszukujacy obiekt na obrazie, odwołuje sie do
lookup table co znacznie przyspiesza działanie w porówaniu z ciagłym
przeliczaniem RGB na HSV.
1 int RGB2H[0 x f f f f ] ;float RGB2S[0 x f f f f ] ;float RGB2V[0 x f f f f ] ;
5 bool countLUT ( ) {
3.2. Proces VSP 16
RGB2H[0x0]=0;RGB2S[0x0]=0.0;RGB2V[0x0]=0.0;
10
for (unsigned short int i =0x1 ; i <=0x f f f e ; i ++) {
float rr , gg ,bb,mx,mn,oh, os , ov ;
15 int maxrgb=0,minrgb=0;
rr = ( ( float ) ( ( i&R_MASK) > >11))/31.0;gg = ( ( float ) ( ( i&G_MASK) > >5))/63.0;bb= ( ( float ) ( ( i&B_MASK) ) )/31 .0 ;
20
mx=max3( rr , gg ,bb ) ;mn=min3( rr , gg ,bb ) ;
i f (mx==rr ) {25 i f ( gg>=bb ) { oh=60∗(gg−bb)/ (mx−mn) ;
} else { oh=360+60∗(gg−bb)/ (mx−mn) ;}
}else i f (mx==gg ) oh=120+60∗(bb−rr ) / (mx−mn) ;
30 else i f (mx==bb) oh=240+60∗(rr−gg )/ (mx−mn) ;
os=(mx−mn)/mx;ov=mx;
35 RGB2H[ i ] = ( int ) ( oh ) ;RGB2S[ i ] = ( float ) ( os ) ;RGB2V[ i ] = ( float ) ( ov ) ;
}40
RGB2H[0 x f f f f ]=0;RGB2S[0 x f f f f ]=0.0;RGB2V[0 x f f f f ]=0.0;
45 return 0;}
Wydruk 3.1. Funkcja countLUT
3.2.4. Opis algorytmu lokalizacji
W przypadku tak prostego obiektu jakim jest piłka, stosowanie skomp-
likowanego algorytmu nie zwiekszyło (o czym mowa w 3.2.5) precyzji otrzy-
manych rezultatów a zwiekszyło tylko czas działania programu. W ra-
mach testowania róznych algorytmów sprawdzono m.in. wyszukiwanie
dominujacej elipsy jednak uzyskiwane czasy nie schodziły ponizej 30ms,
3.2. Proces VSP 17
Rysunek 3.1.
co oznaczało, ze algorytm nie nadazał z przetwarzaniem wszystkich dostar-
czanych przez kamere obrazów. Dlatego własnie zdecydowano o lokalizowa-
niu piłki w najprostszy mozliwy sposób, który dawał wystarczajaco pre-
cyzyjne rezultaty. W algorytmie uzyto dwóch tablic: col i row, które gro-
madziły informacje o sumie punktów, które odpowiadały wzorcowi koloru,
odpowiednio dla kolumn i wierszy obrazu. Zastosowany algorytm działa
nastepujaco:
1. Odczytanie ze sterownika aktualnej klatki z kamery.
2. Iteracyjne przejscie przez wszystkie punkty obrazu.
— Wyciagniecie z lookup table wartosci kolorów w przestrzeni HSV dla
kazdego punktu.
— Sprawdzenie czy wartosci kolorów mieszcza sie w ustalonym progu.
Próg został dobrany heurystycznie przez systematyczne zawezanie
dopuszczalnych przedziałów odpowiednio dla wartosci H, S i V w
3.2. Proces VSP 18
Rysunek 3.2.
przestrzeni HSV i empiryczne ocenienie czy przy róznych natezeni-
ach swiatła i przy róznym oddaleniu od obiektywu niebieskie piksele
obrazu obiektu sa poprawnie odnajdowane.
— Jesli tak, to do elementu tablicy col o indeksie bedacym kolumna
aktualnie sprawdzanego punktu dodawane jest 1. Podobnie,
do elementu tablicy row o indeksie bedacym wierszem aktualnie
sprawdzanego punktu równiez dodawane jest 1.
3. W tablicach row i col zerowane sa elementy, które maja mniejsza wartosc
od ustalonego progu szumu MINSIZE = 20.
4. Znalezienie najwiekszych obszarów spójnych w row i col, zawieraja-
cych niezerowe elementy. Najwieksze obszary spójne w tablicach col
i row rozumiane były jako najdłuzsze ciagi kolejnych niezerowych ele-
mentów tych tablic. Srodek najwiekszego obszaru w row jest poszuki-
wana współrzedna y, natomiast srodek najwiekszego obszaru w col
jest współrzedna x. Promien piłki obliczony jest jako połowa długosci
znalezionych obszarów.
3.2. Proces VSP 19
1 typedef struct {
int h;float s ;
5 float v ;} Thsv ;
int threshold ( Thsv a ) {// wartosci progowe dla nieb ieskie j p i ł k i
10 return a .h > 160 && a .h < 240 && a . s > 0.3 && a . v > 0.3;}
Wydruk 3.2. Progowanie kolorów w przestrzeni HSV
1 size_read = read ( fd , buffer , sizeof ( buffer ) ) ;lseek ( fd ,0 ,SEEK_SET) ;
double x , y , z ;5 int n, i , j , st ,ko ,mst ,mko;
bal l . x=0;bal l . y=0;bal l . z=0;
10 for ( i =2; i <YMAX−2; i +=2)for ( j =2; j <XMAX−2; j ++) {
// prze l iczenie współrzednych na adres w buforzen= i ∗XMAX+ j ;
15
// odwołanie si e do lookup tablehsv .h=RGB2H[ buffer [n ] ] ;hsv . s=RGB2S[ buffer [n ] ] ;hsv . v=RGB2V[ buffer [n ] ] ;
20
// sumowanie i l o s c i odpowiednich p ikse l i w wierszach i kolumnachcol [ j ]+= threshold ( hsv ) ;row [ i ]+= threshold ( hsv ) ;
}25
// usuniecie szumufor ( i =2; i <YMAX−2; i +=2) i f ( row [ i ] <MINSIZE ) { row [ i ]=0; }for ( j =2; j <XMAX−2; j ++) i f ( col [ j ] <MINSIZE ) { col [ j ]=0; }
30 st=ko=mst=mko=0;
// znalezienie najwiekszego obszaru spójnego w wierszachfor ( i =2; i <YMAX−2; i +=2){
i f ( row [ i ]>0 && st ==0){ st=ko= i ; }35 i f ( row [ i ]>0 && st >0) {
ko= i ;i f ( ko−st>mko−mst ) {
mst=st ;mko=ko ;
40 }}i f ( row [ i ]==0 && st>0 ) {
st =0;
3.2. Proces VSP 20
ko=0;45 }
}
// srodek najwiekszego obszaru spójnego w wierszachbal l . y=(mko+mst)/2;
50
st=ko=mst=mko=0;
// znalezienie najwiekszego obszaru spójnego w kolumnachfor ( i =2; i <XMAX−2; i ++) {
55 i f ( col [ i ]>0 && st ==0){ st=ko= i ; }i f ( col [ i ]>0 && st >0) {
ko= i ;i f ( ko−st>mko−mst ) {
mst=st ;60 mko=ko ;
}}i f ( col [ i ]==0 && st>0 ) {
st =0;65 ko=0;
}}
// srodek najwiekszego obszaru spójnego w kolumnach70 bal l . x=(mko+mst)/2;
// promien obiektubal l . z =(mko−mst)/2;
Wydruk 3.3. Algorytm wyszukiwania piłki na obrazie
Algorytm działajacy w ten sposób osiagał czasy rzedu 8ms, co oznaczało
prace z czestotliwoscia 125Hz. Na rysunkach 3.1 i 3.2 zaprezentowany jest
efekt działania algorytmu. Krzywe po lewej stronie i na górze obrazków
pokazuja wykresy wartosci odpowiednio col i row. Czerwony punkt w srodku
piłki jest znalezionym srodkiem obiektu.
3.2.5. Opis innych testowanych algorytmów
W trakcie tworzenia algorytmu lokalizacji obiektu na obrazie, testowano
wiele róznych rozwiazan. Jednym z nich była metoda ROI (ang. Region of
interest) czyli przeszukiwanie jedynie fragmentu obrazu, w którym praw-
dopodobnie znajduje sie obiekt. Obszar taki mozna wyznaczyc przykładowo
jako kwadrat opisany na kole, którego srodkiem jest poprzednie połozenie
obiektu a promieniem jest odległosc jaka moze ów obiekt przebyc w krótkim
czasie przy załozonej pewnej maksymalnej predkosci. Jednak metoda ROI
nie przyspieszyła działania całego algorytmu, poniewaz de facto obszar
3.3. Generator ruchu 21
poszukiwania był niewiele mniejszy od całego obrazu. W przypadku wiek-
szego organiczenia tego obszaru było znacznie wieksze prawdopodobienstwo
nieodnalezienia w nim obiektu a co za tym idzie, opóznienia algorytmu przez
ponowna próbe przeszukania całego obrazu.
Próbowano tez wyszukiwania elipsy dominujacej. Algorytm działa
nastepujaco:
— Stworzenie obrazu krawedziowego na podstawie obrazu z kamery. Polega
to na obliczeniu lokalnych pochodnych (gradientów) kolorów dla kazdego
piksela i wybranie tylko tych, dla których gradienty sa wieksze od
ustalonego progu.
— W krawedziach usuwane sa przeciecia
— Krawedzie rozłaczne sa etykietowane
— Dla kazdej pary segmentów dopasowywana jest elipsa, wynik zapamie-
tywany jest wyłacznie jesli elipsa spełnia odpowiednie kryterium - w
tym wypadku musi to byc elipsa o promieniach w stosunku około 1:2
(wprawdzie piłka jest niemal idealnie okragła, jednak likwidujac przeplot
przez ignorowanie co drugiej linii obrazu, ostatecznym kształtem jest
elipsa)
— W wielowymiarowej tabeli indeksowanej srodkami i promieniami elips,
zapamietuje sie ile razy w danym miejscu pojawiła sie elipsa w danym
rozmiarze. Ta elipsa, która została dopasowana najwieksza ilosc razy
zostaje wybrana jako elipsa dominujaca.
Oczywiscie jest to algorytm bardzo złozony i doskonały do zastosowa-
nia w obrazach nieruchomych, jednak w przypadku aplikacji działajacej w
czasie rzeczywistym jego czasochłonnosc jest nie do zaakceptowania.
3.3. Generator ruchu
Generator ruchu jest obiektem, który ma za zadanie, na podstawie infor-
macji uzyskanych od pozostałych modułów aplikacji MRROC++, decydowac
o ruchu manipulatora. W przypadku niniejszej pracy generator pobiera
dane jedynie od procesu VSP a wygenerowany wektor wartosci zadanych
sterowania dla manipulatora jest w układzie odniesienia zwiazanym z ak-
tualnym połozeniem manipulatora. Oznaczo to, ze nie jest konieczne
sprawdzanie stanu w jakim znajduje sie manipulator, a jedynie przesłanie
3.3. Generator ruchu 22
Rysunek 3.3. Zaleznosc promienia piłki w obrazie i jej odległosci od obiek-tywu
do odpowiedniego bufora komunikacyjnego wektora wartosci zadanych
sterowania.
3.3.1. Wyznaczenie połozenia piłki w 3D
Jednym z załozen pracy było utrzymywanie piłki w srodku obrazu
otrzymywanego z kamery w odległosci ok. 20 cm od obiektywu. W VSP
odbywało sie wykrycie połozenia piłki na obrazie i przekazanie do bufora
komunikacyjnego zestawu trzech liczb: współrzednych srodka piłki i jej
promienia w przestrzeni cech obrazu. O ile współrzedne obiektu mówia
nam bezposrednio o uchybie wizyjnym, który wyznaczany jest jako róznice
srodka obrazu i obiektu, o tyle promien piłki nalezy przekształcic. Z maksy-
malna precyzja na jaki pozwalały narzady pomiarowe i szumy kamery
zbadano zaleznosc miedzy oddaleniem od obiektywu a promieniem piłki.
Tabela 3.1 prezentuje wynik pomiarów.
Przy wyznaczaniu wzoru na zaleznosc posiłkowano sie programem Prism.
Metoda najmniejszych kwadratów otrzymano przyblizenie przedstawione na
rysunku 3.3.
3.3. Generator ruchu 23
Tablica 3.1. Zaleznosc promienia piłki w obrazie i jej odległosci od obiek-tywu
cm piksele
5 20810 13915 9718 7020 6222 5524 5126 4028 3430 2832 22
3.3. Generator ruchu 24
Przyblizeniem jest funkcja:
D =3761
z + 67.1− 8.537, (3.1)
gdzie: D - odległosc piłki od obiektywu, z - rozmiar piłkiwa obrazie wyrazony
w pikselach.
Kolejnym problemem była odległosc od piłki w przypadku gdy nie jest
ona w centrum obrazu. Wówczas aby manipulator niepotrzebnie sie nie
przyblizał, załozono iz odległosc piłki od manipulatora bedzie rozumiana
jako odległosc piłki od płaszczyzny prostopadłej do osi optycznej obiektywu
i stycznej do obiektywu. Do przeliczenia mozna było wykorzystac jedynie
informacje, które zostały dostarczone z kamery, oraz obliczona odległosc D.
D2 =
√D2 − (
√(Xcenter − x)2 + (Ycenter − y)2
k)2, (3.2)
gdzie: D2 - odległosc piłki od płaszczyzny prostopadłej do osi optycznej
obiektywu i stycznej do obiektywu (w przestrzeni operacyjnej manipula-
tora), Xcenter i Ycenter - współrzedne srodka obrazu w przestrzeni obrazu,
x i y - współrzedne obiektu w przestrzeni obrazu. Ze wzgledu na fakt,
ze korzystano z wielkosci w róznych przestrzeniach i nie było mozli-
wosci wyprowadzenia bezposredniej zaleznosci miedzy nimi, wprowadzono
współczynnik k słuzacy do normalizacji wartosci x i y. Na drodze ekspery-
mentów wartosc k ustalono na 25.
3.3.2. Regulator
Stworzony układ regulacji w systemie MROCC++ tworzy strukture
kaskadowa. Serwomechanizm wizyjny jest sterownikiem którego wyjscia
sa wartosciami w przestrzeni operacyjnej robota zadanymi regulatorom
nizszego poziomu, sterujacym manipulatorem w jego przestrzeni konfigu-
racyjnej. Zastosowana regulacja jest próba sprawdzenia czy mozliwa jest
implementacja wirtualnej inercji (stosowanej w sterowaniu siłowym) w ra-
mach serwomechanizmu wizyjnego. Rysunek 3.4 prezentuje strukture ser-
womechanizmu.
3.3. Generator ruchu 25
Rysunek 3.4. Struktura zaimplementowanego serwomechanizmu wiz-yjnego
Zadaniem regulatora w opisywanym systemie było wyznaczenie na pod-
stawie uchybów kolejnych pozycji (wzgledem pozycji aktualnej), do których
manipulator ma dazyc. Wartosci zadane sterowania w osiach X i Y były
niezalezne wzgledem siebie. Wartosci zadane sterowanie w osi Z było za-
lezne od pozostałych dwóch wymiarów ze wzgledu na obliczenia o których
mowa była we wzorze 3.2. Jako uchyby przyjeto ex = Xcenter−x, ey = Ycenter−y
i ez = D2−Zcenter. Pierwsze dwie wartosci wyrazone były w pikselach, ostatnia
ze wzgledu na wczesniejsze obliczenia - w centymetrach. Zcenter ustalono na
20cm, co było jednym z załozen pracy.
Po zapisaniu poprzednich wartosci zadanych sterowania (ux1, uy1 i uz1)
regulator wyznaczał nowe:
ux =ex + ux1 ∗ I
I + F(3.3)
uy =ey + uy1 ∗ I
I + F(3.4)
3.3. Generator ruchu 26
uz = Zamplifier ∗ez + uz1 ∗ I
I + F(3.5)
gdzie I - wirtualna inercja, F - wirtualne opory ruchu, Zamplifier - wzmocnie-
nie wartosci zadanej sterowania w osi Z. Ze wzgledu na to, ze uchyb w osi
Z wyrazony był w centymetrach, przyjmował mniejsze wartosci niz uchyby
w pozostałych osiach. Aby w kazdej z osi regulator reagował podobnie na
uchyby, konieczne było wprowadzenia wzmocnienia. Na drodze ekspery-
mentów ustalono nastepujaca wartosc wzmocnienia: Zamplifier = 5.
Wprowadzenie wirtualnej inercji i oporu ruchu było nieodzowne aby ruch
manipulatora stał sie płynny i bezpieczny. Przez ruch bezpieczny rozumi-
ano ruch, podczas którego nie dochodzi do osiagania duzego przyspieszenia.
Przez ruch płynny rozumiano taki ruch, w którym nie dochodzi do gwał-
townych zmian zwrotu wektora predkosci. To pierwsze zapewniał wirtualny
opór, to drugie - wirtualna inercja, która sprzezona była z poprzednimi
wartosciami zadanymi sterowaniami. Dobranie odpowiednich wielkosci
tych współczynników wiazało sie z przetestowaniem róznych konfiguracji.
Przyjeto nastepujaca strategie działania:
— wyjsciowo wartosc inercji przyjeto jako 0 a oporu jako 1000000 (to rzad
wielkosci wiecej niz iloraz maksymalnego uchybu a maksymalnej do-
puszczalnej wartosci zadanej sterowania)
— nastepnie zmniejszano opór do uzyskania oscylacji
— w kolejnym kroku zwiekszano inercje az do zlikwidowania oscylacji
— powtarzono ostatnie dwa kroki cyklicznie (kilkakrotnie) az do uzyskania
widocznie zadowalajacego rezultatu
Jest to metoda w pewnym sensie analogiczna do metody doboru nastaw
Zieglera-Nicholsa dla regulatorów PID. Ostatecznie uzyskano nastepujace
wartosci: I = 8000, F = 40000.
Na drodze testów uznano, iz godnym sprawdzenia rozwiazaniem jest
oddalanie manipulatora od piłki proporcjonalnie do jej predkosci w osiach
X i Y . Wynikało to ze stosunkowo niewielkiego pola widzenia kamery i co
za tym idzie - duzego prawdopodobienstwa ze przy pewnych predkosciach
piłka z niego ucieknie. dla bezpieczenstwa przyjeto wartosc MAXD = 25cm
jako górne ograniczenie odległosci, na jaka manipulator moze sie oddalic
3.3. Generator ruchu 27
od piłki. Zmodyfikowany wzór na wartosci zadane sterowania w osi Z był
nastepujacy:
uz = Zamplifier ∗min(ez + V ∗ Vamplifier, MAXD) + uz1 ∗ I
I + F(3.6)
gdzie V - predkosc piłki na obrazie w osiach X i Y , Vamplifier - współczyn-
nik wzmocnienia predkosci (dobrany w drodze eksperymentów). Predkosc
została obliczona w sposób nastepujacy: V =√
(ex − ex1)2 + (ey − ey1)2, gdzie
ex1 i ey1 - poprzednie uchyby.
W przypadku gdy regulator otrzyma od VSP nieprawidłowe współrzedne
(spoza dopuszczalnego zakresu), wchodzi w stan oczekiwania na koordy-
naty. Nie nastepuje jednak całkowite zatrzymanie manipulatora co objaw-
iłoby sie gwałtownym szarpnieciem robota. Zeby zabezpieczyc sie przed
taka sytuacja w stanie oczekiwania kolejne uchyby obliczone sa jako 90%
poprzedniego uchybu a predkosc V zostaje wyzerowana. W wyniku takiego
schematu działania, robot łagodnie zatrzymuje sie po utracie piłki z pola
widzenia.
Ostatnim zadaniem generatora jest przekazanie wartosci zadanych
sterowania do robota poprzez bufor komunikacyjny umiejscowiony w EDP
(Effector Driver Process). EDP jest ostatnia warstwa aplikacji oddzielajaca
logike od sterowników sprzetowych robota.
4. Badania i testy
4.1. Zadanie przestawiania
Zadanie przestawiania polegało na umieszczeniu obiektu w pewnym odd-
aleniu od osi optycznej kamery, jednak w widocznym dla niej miejscu i uru-
chomieniu systemu w celu sprawdzenia szybkosci wyzerowania uchybu.
Zmiany uchybów w czasie w osiach X i Y dla zadania
przestawiania
-300
-200
-100
0
100
200
300
400
0 1 2 3 4 5 6 7 8
czas (s)
uchyb (px)
X
Y
Rysunek 4.1.
Na rysunku 4.1 pokazana jest zmiana uchybów w osiach X i Y w czasie
od chwili t = 0. Widac, iz w osi Y manipulator doszedł do obiektu w ok 2
sekundy, natomiast w osi X potrzebował ok 2.5sekundy. Obraz z kamery ma
wymiary 768 na 576 pikseli, co oznacza iz maksymalny mozliwy uchyb w
osi X wynosi 334 a w osi Y - 288. W przypadku tego testu piłka na poczatku
umiejscowiona była w samym rogu obrazu z kamery, stad czas dojscia w osi
X był znaczaco wiekszy.
4.1. Zadanie przestawiania 29
Zmiany uchybu w czasie w osi Z dla zadania przestawiania
-6
-4
-2
0
2
4
6
8
10
0 1 2 3 4 5 6 7 8
czas (s)
uchyb (cm)
Rysunek 4.2.
Na rysunku 4.2 zaprezentowano zmiane uchybu w osi Z dla tego samego
testu. Stanowi to osobny wykres poniewaz uchyb w osi Z liczony jest w
centymetrach, podczas gdy w osiach X i Y - w pikselach. Szumy na wykre-
sie spowodowane sa zła jakoscia obrazu z kamery. Wahania rozmiaru piłki
na obrazie z kamery powodowało zmiany w wyliczonej odległosci siegajace
nawet 1cm. Mimo tego, widac jak zmieniał sie uchyb w czasie. Mozna za-
uwazyc, ze w osi Z pojawia sie duze przeregulowanie i manipulator objawia
drgania, które wygasaja ok 4. sekundy. Jest to oczywiscie duza wada tego
układu regulacji i przemawia za wyzszoscia układu regulacji bez zaimple-
mentowanego oddalania sie kamery (rysunek 4.8).
To co wydarzyło sie w osi Z wyjasnia rysunek 4.3, który przedstawia
całke wartosci zadanych sterownikowi robota po czasie we wszystkich os-
iach. Poniewaz wartosci zadane, jakie generator przekazuje do EDP mier-
zone sa w metrach w układzie współrzednych zewnetrznych zwiazanych z
aktualnym połozeniem ramienia, ponizszy wykres defacto pokazuje zmiane
połozenia manipulatora wzgledem pozycji startowej. O ile zachowanie ma-
nipulatora w osiach X i Y jest klarowne, o tyle zachowanie w osi Z wymaga
4.1. Zadanie przestawiania 30
Całka wartości zadanych sterowania po czasie w kaŜdej z
osi dla zadania przestawiania
-0,15
-0,1
-0,05
0
0,05
0,1
0,15
0,2
0 1 2 3 4 5 6 7 8
czas (s)
całka wartości zadanych
sterowania (m)
X
Y
Z
Rysunek 4.3.
wyjasnienia. Z przebiegu wykresu mozna wyczytac, ze na samym poczatku
manipulator zblizył koncówke do obiektu, po czym w okolicach 0.5 sekundy
zaczał sie oddalac i sekunde pózniej, w czasie gdy w osiach X i Y uchyb
był juz nieznaczny, ponownie zaczał sie przyblizac. Ostatecznie odległosc
manipulatora od obiektu ustabilizowała sie ok 4. sekundy. Wyjasnienia
nalezy szukac w zaimplementowanej funkcji oddalania sie manipulatora
od obiektu, proporcjonalnie do predkosci obiektu na obrazie. Wprawdzie
rzeczywisty obiekt nie poruszał sie, w momencie gdy generator rozpoczał
minimalizacje uchybów w osiach X i Y , w układzie zwiazanym z manipu-
latorem obiekt uzyskał wzgledna predkosc równa predkosci poruszania sie
manipulatora. Na poczatku predkosc ta była niewielka, stad manipula-
tor zaczał sie przyblizac. W miare przyspieszania manipulatora, wzgledna
predkosc obiektu rosła, przez co generator nakazał manipulatorowi oddalic
sie. Wirtualna inercja przeszkodziła gwałtownej zmianie wektora predkosci
w osi Z i spowodowała łagodne wyhamowanie i rozpoczecie oddalania sie.
Analogicznie w okolicach 1.5 sekundy, gdy manipulator zaczał zwalniac (ze
wzgledu na bliskosc obiektu), predkosc piłki na obrazie malała i co za tym
4.1. Zadanie przestawiania 31
idzie manipulator zwolnił tempo oddalania sie az w koncu zaczał sie przy-
blizac na ustalona odległosc 20 cm.
Na rysunkach 4.4, 4.5 i 4.6 zaprezentowano wykresy dla podobnego
badania.
Zmiany uchybów w czasie w osiach X i Y dla zadania
przestawiania
-50
0
50
100
150
200
250
300
350
400
0 0,5 1 1,5 2 2,5 3 3,5 4 4,5 5
czas (s)
uchyb (px)
X
Y
Rysunek 4.4.
Na rysunkach 4.7, 4.8 i 4.9 zaprezentowano wykresy dla podobnego
badania, jednak bez zaimplementowanej funkcji oddalania manipulatora
proporcjonalnie do predkosci obiektu. W takim przypadku widac, ze w osi Z
regulator działa tak samo jak w osiach X i Y , co dowodzi dobrze dobranego
współczynnika Zamplifier we wzorze 3.5.
4.1. Zadanie przestawiania 32
Zmiany uchybu w czasie w osi Z dla zadania przestawiania
-8
-6
-4
-2
0
2
4
0 0,5 1 1,5 2 2,5 3 3,5 4 4,5 5
czas (s)
uchyb (cm)
Rysunek 4.5.
Całka wartości zadanych sterowania po czasie w kaŜdej z
osi dla zadania przestawiania
-0,1
-0,05
0
0,05
0,1
0,15
0,2
0 0,5 1 1,5 2 2,5 3 3,5 4 4,5 5
czas (s)
całka wartości zadanych
sterowania (m)
X
Y
Z
Rysunek 4.6.
4.1. Zadanie przestawiania 33
Zmiany uchybów w czasie w osiach X i Y dla zadania
przestawiania
-250
-200
-150
-100
-50
0
50
100
0 1 2 3 4 5 6
czas (s)
uchyb (px)
X
Y
Rysunek 4.7.
Zmiany uchybu w czasie w osi Z dla zadania przestawiania
-2
0
2
4
6
8
10
0 1 2 3 4 5 6
czas (s)
uchyb (cm)
Rysunek 4.8.
4.1. Zadanie przestawiania 34
Całka wartości zadanych sterowania po czasie w kaŜdej z
osi dla zadania przestawiania
-0,2
-0,15
-0,1
-0,05
0
0,05
0,1
0,15
0,2
0 1 2 3 4 5 6
czas (s)
całka wartości zadanych
sterowania (m)
X
Y
Z
Rysunek 4.9.
4.2. Zadanie nadazania 35
4.2. Zadanie nadazania
W tym badaniu przesuwano obiekt ze stała predkoscia. Do realizacji
tego badania wykorzystany został dodatkowy robot bedacy tasmociagiem.
Przeprowadzono badania przy dwóch predkosciach tasmy: 2cm/s i 4cm/s.
Dodatkowo przy predkosci 4cm/s badanie przeprowadzono dla regulatora
bez zaimplementowanej wirtualnej inercji (regulator był zwykłym regula-
torem proporcjonalnym). Wszystkie badania przeprowadzono tak, aby
uchyb pojawił sie wyłacznie w osi X.
4.2.1. Badanie na tasmociagu z predkoscia 2cm/s
Całka wartości zadanych sterowania po czasie w osi X przy
stałej prędkości poruszania się obiektu (2cm/s)
-0,05
0
0,05
0,1
0,15
0,2
0,25
0,3
0,35
0,4
0,45
0 5 10 15 20 25 30
czas (s)
całka wartości zadanych
sterowania (m)
Rysunek 4.10.
Na rysunku 4.10 pokazana jest całka sterowan (analogicznie do poprzed-
niego podrozdziału). Mozna zaobserwowac, ze w pewnym momencie obiekt
wjezdza w pole widzenia kamery. Wówczas ciemna linia schodzi ponizej
zera - oznacza to, iz manipulator dochodzi do obiektu w kierunku przeci-
wnym do jego ruchu. Nastepnie, gdy obiekt porusza sie jednostajnie dalej,
manipulator zawraca i w pewnym momencie osiaga uchyb ustalony, który
4.2. Zadanie nadazania 36
stabilizuje sie ok 10. sekundy. Nachylenie lini odpowiadajacej uchybowi w
osi X pokazuje iz, manipulator przebywa ok 0.2 m w czasie 10 sekund, co
odpowiada predkosci obiektu.
Zmiana uchybów w czasie w osiach X i Y przy stałej
prędkości poruszania się obiektu (2cm/s)
-20
-10
0
10
20
30
40
50
60
70
0 5 10 15 20 25 30
czas (s)
uchyb (px)
X
Y
Rysunek 4.11.
Rysunek 4.11 pokazuje jaki uchyb ustalony osiaga regulator. Widac,
ze jest to wartosc ok 38px. W momencie poczatkowego dochodzenia do
obiektu, widac, iz regulator stabilizuje pozycje zarówno w osi X jak i Y .
Zanim regulator osiaga uchyb ustalony widac, ze w osi X uchyb rosnie
ponad 60px. Wynika to z faktu istnienia wirtualnej inercji. W momen-
cie gdy manipulator zbliza sie na poczatku do obiektu, ten porusza sie w
przeciwnym kierunku i manipulator potrzebuje troche czasu aby płynnie
zmienic kierunek ruchu.
Wyjasnienia wymaga równiez powstania uchybu ustalonego. Zas-
tosowany regulator nie całkuje uchybów. Skutkuje to niemoznoscia ‘do-
gonienia’ obiektu w przypadku jego stałej predkosci. Nie zdecydowano sie
jednak na całkowanie uchybów ze wzgledu na mniejsza stabilnosc takiego
regulatora co w rezultacie obnizyłoby bezpieczenstwo pracy robota.
4.2. Zadanie nadazania 37
4.2.2. Badanie na tasmociagu z predkoscia 4cm/s
Całka wartości zadanych sterowania po czasie w osi X przy
stałej prędkości obiektu (4cm/s)
-0,1
0
0,1
0,2
0,3
0,4
0,5
0,6
0,7
0,8
0,9
0 5 10 15 20 25 30 35
czas (s)
całka wartości zadanych
sterowania (m)
Rysunek 4.12.
W tym badaniu poza inna predkoscia obiektu, inny tez był poczatek
ruchu. Od samego poczatku obiekt był w polu widzenia kamery. Dopiero
gdy ustabilizowała sie pozycja manipulatora, obiekt ruszył na tasmociagu.
Wykres 4.12 pokazuje, ze manipulator bardzo szybko uzyskał predkosc
obiektu. Uchyb ustalił sie na poziomie 75 pikseli, co widac na 4.13. Na
poczatku ruchu obiektu uchyb doszedł prawie do poziomu 100 pikseli,
czemu winne sa wirtualny opór ruchu i wirtualna inercja, nie pozwalajace
na gwałtowne rozpoczecie ruchu.
4.2. Zadanie nadazania 38
Zmiana uchybów w czasie w osiach X i Y przy stałej
prędkości obiektu (4cm/s)
-20
0
20
40
60
80
100
120
0 5 10 15 20 25 30 35
czas (s)
uchyb (px)
X
Y
Rysunek 4.13.
4.2. Zadanie nadazania 39
Całka wartości zadanych sterowania po czasie dla osi X
przy stałej prędkości obiektu (4cm/s)
0
0,1
0,2
0,3
0,4
0,5
0,6
0,7
0,8
0,9
1
80 85 90 95 100 105 110 115
czas (s)
całka wartości zadanych
sterowania (m)
Rysunek 4.14.
Wykresy 4.14 i 4.15 pokazuja jak zachowuje sie regulator bez wirtual-
nej inercji i oporu ruchu. Manipulator duzo szybciej dochodzi do uchybu
ustalonego, który jest na podobnym poziomie jak w poprzednim badaniu.
4.2. Zadanie nadazania 40
Zmiana uchybów w czasie w osiach X i Y przy stałej
prędkości obiektu (4cm/s)
-10
0
10
20
30
40
50
60
70
80
90
80 85 90 95 100 105 110 115
czas (s)
uchyb (px)
X
Y
Rysunek 4.15.
5. Podsumowanie i wnioski
Dzieki tej pracy poznałem konstrukcje systemu MRROC++, jak równiez
posiadłem ogólna wiedze o sterowaniu pozycyjnym i metodach rozpoznawa-
nia obrazu.
W ramach pracy dyplomowej powstał sterownik w systemie MROCC++,
którego struktura struktura odzwierciedlała strukture serwomechanizmu
wizyjnego typu IB/PB-EOL-EIH zgodnego z regułami przedstawionymi w
[5]. Dekompozycja sterownika na zadanie (algorytm zarzadzajacy sterown-
ikiem), wirtualny sensor (rozpoznawanie obrazu) i generator ruchu (regula-
tor) pozwoliła na efektywne rozdzielenie zadan na kilka maszyn.
Przetestowanie kilku metod wyszukiwania obiektu w obrazie poz-
woliło na odrzucenie skomplikowanych i czasochłonnych metod, których
skutecznosc w przypadku tak prostego obiektu nie przewyzszała znaczaco
zastosowanego ostatecznie algorytmu.
Zastosowany regulator był próba implementacji regulatora z wirtualna
inercja, stosowanego w sterowaniu siłowym. Rozwiazanie to oznaczyło sie
szybkoscia działania przy jednoczesnym wygładzaniu trajektorii i uniemozli-
wieniu gwałtownych zmian kierunku ruchu manipulatora. Mechanizm au-
tomatycznego oddalania sie manipulatora proporcjonalnego do predkosci
obiektu miał swoje zalety w postaci zmniejszonego prawdopodobienstwa
ucieczki obiektu z pola widzenia, jednak powodował przeregulowanie w osi
Z co było widoczne przy zadaniu przestawiania (rysunek 4.2). W zwiazku
z tym ten mechanizm nie powinien byc zastosowany w sterowniku w przy-
padku gdy istnieje mozliwosc uderzenia manipulatorem w jakas przeszkode
w jego przestrzeni operacyjnej lub w sam obiekt. Ostateczny efekt działania
sterownika jest zadowalajacy i spełnia wszystkie załozenia pracy.
5.1. Perspektywy rozwoju 42
5.1. Perspektywy rozwoju
Podczas pracy nad aplikacja rozwazyłem perspektywy rozwoju tej pracy.
Na pewno mozna rozszerzyc algorytmy rozpoznawania obrazu o detekcje
obiektów o bardziej złozonym kształcie np kostki rubika badz drugiego
manipulatora, dla których dodatkowymi informacjami, oprócz połozenia,
byłaby orientacja i układ. Dla zwiekszenia precyzji lokalizacji obiektu
bedzie mozna zastosowac stereowizje i wykorzystac kamery bardziej za-
awansowane technologicznie. Mozna tez rozwinac mozliwosci generatora
o wykorzystywanie całej przestrzen przestrzeni roboczej, co wiazałoby sie z
obsługiwaniem zmiennej orientacji chwytaka i sprzezenia tego z wizja. Do-
datkowo sledzenie mozna rozszerzyc o próbe chwycenia obiektu. Wówczas
nalezy skorzystac ze sterowania pozycyjno-siłowego.
Bibliografia
[1] Eryk Chmielnicki. Lokalizacja i sledzenie ruchu obiektów za pomoca robotawyposazonego w kamere, Praca dyplomowa magisterska. Warszawa, 2005.
[2] Tomasz Kornuta. Szablony do przetwarzania danych sensorycznych wukładach sterowania robotów, Praca dyplomowa inzynierska. Warszawa, 2003.
[3] Maciej Staniak, Witold Czajewski. Real-time image segmentation for visualservoing. 2007.
[4] Maciej Staniak, Cezary Zielinski. Visual servoing – part 1. 2006.[5] Maciej Staniak, Cezary Zielinski. Visual servoing – part 2. 2006.[6] Cezary Zielinski, Wojciech Szynkiewicz, Tomasz Winiarski. Applications of
mrroc++ robot programming framework.[7] Cezary Zielinski, Wojciech Szynkiewicz, Tomasz Winiarski, Tomasz Kornuta.
MRROC++ Based System Description. Warszawa, 2007.