raport z wykonania projektu - Encoder

Studenckie Koło Naukowe Robotyki „Encoder”
Wydział Automatyki, Elektroniki i Informatyki
Politechnika Śląska
Manipulator OOO
z systemem wizyjnym
Raport z realizacji projektu
Daniel Dreszer
Kamil Gnacik
Paweł Kaleta
Oliwia Szymańska
Gliwice, czerwiec 2013
[SKN Encoder]
Manipulator OOO z systemem wizyjnym
Spis treści
1
Krótki opis projektu .................................................................................................. 2
2
Model stanowiska .................................................................................................... 3
3
Zadanie proste kinematyki ....................................................................................... 4
4
Zadanie odwrotne kinematyki ................................................................................. 5
5 Transformacja z układu współrzędnych związanego z obrazem do układu
bazowego manipulatora............................................................................................................. 6
6
Opis funkcjonalności programu ............................................................................... 7
Bibliografia ...................................................................................................................... 8
1 Krótki opis projektu
Rysunek 1. Manipulator OOO sterowany z użyciem informacji z umieszczonej nad nim kamery
W ramach prac SKN Encoder posiadany manipulator OOO wyposażono w system
sterowania oparty na informacji wizyjnej pochodzącej z kamery RGB umieszczonej nad
stanowiskiem. Na podstawie danych z obrazu, za pomocą odpowiednich przekształceń
uzyskiwane są informacje o obecnym i docelowym położeniu elementów do przeniesienia;
następnie na podstawie rozwiązania zadania odwrotnego kinematyki manipulatora
wypracowywane jest sterowanie serwomechanizmów poruszających członami manipulatora
2
[SKN Encoder]
Manipulator OOO z systemem wizyjnym
oraz jego chwytakiem. System, oprogramowany w środowisku LabVIEW, zapewnia interakcję
z użytkownikiem. Manipulator podczas pracy przedstawia rysunek 1.
2 Model stanowiska
Na rysunku 2. przedstawiono schemat rozmieszczenia elementów systemu wraz
z zastosowanymi układami współrzędnych (x0y0z0 - układ bazowy manipulatora, xMyMzM układ związany z markerem, xKyKzK - układ związany z kamerą (obrazem z kamery)).
Położenie układu xKyKzK na rysunku jest jedynie orientacyjne, gdyż mocowanie kamery nie
jest zupełnie usztywnione; może więc zachodzić rotacja układu o niewielki kąt wokół osi zK,
jednak przyjmuje się, że równoległość osi zK, zM, z0 jest zachowana.
Rysunek 2. Rozmieszczenie elementów systemu (widok z góry) oraz zdefiniowane układy współrzędnych
Schemat połączeń komunikacyjnych między elementami systemu przedstawiono na
rysunku 3.
3
[SKN Encoder]
Manipulator OOO z systemem wizyjnym
Rysunek 3. Schemat komunikacji w systemie
3 Zadanie proste kinematyki
Schemat kinematyczny manipulatora jest następujący (rysunek 4):
Rysunek 4. Schemat kinematyczny posiadanego manipulatora OOO
Długości przedstawione na rysunku mają wartości:
λ1 = 16.5 cm
L2 = 17.4 cm
L3+L4 = L34 = 22 cm
4
[SKN Encoder]
Manipulator OOO z systemem wizyjnym
Zgodnie ze schematem, korzystając z przekształceń opisanych w podręczniku [1],
transformacja z układu współrzędnych skojarzonego z końcówką chwytaka do układu
bazowego manipulatora jest opisana jako:
gdzie si = sin(Θi), ci = cos(Θi) oraz sij = sin(Θi + Θj), cij = cos(Θi + Θj), i, j = 1, 2, 3.
4 Zadanie odwrotne kinematyki
By możliwe było wyznaczenie kątów Θi, i = 1, 2, 3, na podstawie zadanego położenia
elementu wykonawczego, należało rozwiązać zadanie odwrotne kinematyki manipulatora. W
tym celu posłużono się algorytmem zawartym w [1] na stronie 116. Algorytm został
uproszczony na mocy ograniczeń nałożonych na kąty Θi, które są następujące:
W praktyce oznacza to ograniczenie ilości sposobów osiągnięcia danego położenia
elementu wykonawczego do jednego. Uwzględnione zostały tutaj również fizyczne
ograniczenia manipulatora.
Zgodnie ze wspomnianym algorytmem, w pierwszej kolejności należy sprawdzić
przynależność zadanego położenia do przestrzeni roboczej manipulatora. Według
zastosowanej w niniejszym projekcie metody, dla uproszczenia dokonywane jest to jednak
po próbie wyliczenia kątów Θi, co zostanie omówione za chwilę.
Kolejnym krokiem jest ustalenie, czy orientacja zadana jest zdefiniowana
jednoznacznie. W przypadku tego projektu orientacja zadana jest dowolna.
W kolejnym kroku wyliczony zostaje kąt Θ1. Obliczony jest on na podstawie wzoru
Biorąc pod uwagę ograniczenia nałożone na kąt Θ1 oraz analizując możliwe
współrzędne dx oraz dy dochodzi się do wniosku, że otrzymany kąt Θ1 jest wynikiem
ostatecznym.
Kolejnym obliczanym kątem jest kąt Θ3. Obliczany jest on dzięki wyznaczeniu wartości
funkcji sinus i cosinus tego kąta.
5
[SKN Encoder]
Manipulator OOO z systemem wizyjnym
W związku z ograniczeniami nałożonymi na kąt Θ3 można stwierdzić, że s3 będzie
zawsze ujemny. W związku z tym prawdziwe jest:
.
Następnie, znając s3 oraz c3 możemy obliczyć kąt Θ3 z następującego wzoru:
.
Ostatnim krokiem jest obliczenie kąta Θ2. Obliczony jest on na podstawie wzorów:
λ
Dzięki przyjęciu odpowiednich ograniczeń, otrzymujemy jedno rozwiązanie zadania
kinematyki odwrotnej manipulatora. Ostatnim etapem algorytmu jest wcześniej
wspomniane sprawdzenie przynależności zadanej pozycji do przestrzeni roboczej
manipulatora. Zrealizowane jest to na podstawie analizy otrzymanych wyników zadania
odwrotnego. Jeśli kąty Θ2 oraz Θ3 mają przypisaną wartość NaN (Not a Number), to zadany
punkt jest zbyt odległy od manipulatora. Objawia się to wyliczeniem c3 > 1, co jest oczywiście
niemożliwe i kąty Θ2 oraz Θ3 nie mogę zostać obliczone. Z kolei jeśli wszystkie wartości kątów
zostały obliczone, to należy jedynie sprawdzić zgodność z przyjętymi ograniczeniami, co
zapewni fizyczną realizowalność dojścia do zadanego położenia.
5 Transformacja z układu współrzędnych związanego
z obrazem do układu bazowego manipulatora
Rozwiązanie zadania odwrotnego wystarcza do takiego wysterowania
serwomechanizmów manipulatora, aby końcówka chwytaka znalazła się w zadanym
położeniu; jednakże, aby ułatwić użytkownikowi definiowanie tych położeń, wprowadzono
dodatkowy układ współrzędnych: układ xKyKzK związany z obrazem z kamery umieszczonej
6
[SKN Encoder]
Manipulator OOO z systemem wizyjnym
nad stanowiskiem. Wskazanie punktu myszką na obrazie jest bowiem dla użytkownika
znacznie wygodniejsze, niż podawanie współrzędnych w [cm] w układzie bazowym. Ponadto,
dodanie podsystemu wizyjnego wprowadza dodatkowe możliwości automatyzacji zadania
przenoszenia elementów - umożliwia bowiem programowe wyszukiwanie położenia
elementów do przeniesienia.
Aby jednak możliwe było użyteczne wykorzystanie informacji uzyskanych z obrazu,
potrzebna jest transformacja 0TK z układu współrzędnych xKyKzK do układu bazowego
manipulatora; aby ją otrzymać, wprowadzono pomocniczo układ związany z markerem
xMyMzM. Prawdziwe jest:
0
TK = (KT0)-1 = (KTM · MT0)-1 .
Przejście KTM z układu markera do układu związanego z obrazem zrealizowane jest
jako translacja o współrzędne środka większego koła markera (czyli współrzędne początku
układu xMyMzM) wyrażone w układzie xKyKzK, następnie rotacja o 180° wokół osi x, rotacja
wokół osi z o kąt skręcenia osi xM względem xK, a na koniec przeskalowanie macierzy ze
współczynnikiem skali równym ilorazowi promienia większego koła markera wyrażonego
w pikselach do promienia wyrażonego w centymetrach:
K
TM = Trans (xKM, yKM, 0) · Rot (x, 180°) · Rot (z, αKM) · Scale (rK/ rM, rK/ rM, rK/ rM) .
Parametry xKM, yKM, αKM, rK obliczane są w programie na podstawie obrazu z kamery,
za każdym razem, gdy pobierany jest nowy obraz, zaś rM jest wielkością stałą i według
pomiaru wynosi 3.85 cm.
Z kolei orientacja osi xM, yM, zM została tak dobrana, że transformacja MT0 jest
macierzą stałą i jest równa translacji o współrzędne punktu (0, 0, 0) układu bazowego
wyrażone w układzie markera:
M
T0 = Trans (oM0) = Trans (22.4, -44.25, 0) .
Mając współrzędne danego punktu na obrazie [x, y, z]Kp, można zatem otrzymać jego
współrzędne w układzie bazowym manipulatora jako:
[x, y, z]0p = 0TK · [x, y, z]Kp .
6 Opis funkcjonalności programu
Aplikacja oferuje dwa tryby: przenoszenie wybranego elementu na pozycję wskazaną
przez użytkownika oraz na wykryte automatycznie prostokątne pole - podest. Wówczas
należy dodatkowo wprowadzić informację o wysokości podestu, aby umożliwić poprawne
pozycjonowanie chwytaka podczas przenoszenia obiektu.
Po uruchomieniu programu następuje automatyczna kalibracja układu wizyjnego: na
obrazie pobranym z kamery określana jest pozycja i orientacja markera, by na ich podstawie
można było obliczyć macierz transformacji 0TK. Następnie na obrazie wyszukiwane są okrągłe
7
[SKN Encoder]
Manipulator OOO z systemem wizyjnym
kształty, czyli obiekty do przeniesienia. Znalezione elementy zostają zaznaczone na obrazie
różnymi kolorami, a użytkownikowi zostaje przedstawiona ich lista, aby mógł wybrać, który
obiekt należy przenieść. Środek obiektu zostaje ustalony jako punkt docelowy dla końcówki
chwytaka. Następnie określany jest punkt, do którego należy przenieść obiekt. Jeśli
uruchomiono program w trybie przenoszenia na podest, jego kształt (prostokąt)
wyszukiwany jest na obrazie, a punkt nad środkiem podestu staje się kolejnym punktem
docelowym dla chwytaka. Jeśli zaś program działa w trybie przenoszenia na dowolną
pozycję, należy kliknąć pożądany punkt końcowy na obrazie. Następnie, jeśli punkty
początkowy i końcowy leżą w przestrzeni roboczej manipulatora, do serwomechanizmów
wysyłane są instrukcje sterujące i manipulator wykonuje żądaną operację przenoszenia. Jeśli
zaś któryś z punktów leży poza przestrzenią roboczą lub też podest czy żadne obiekty nie
zostały wykryte, informacja o tym przekazywana jest użytkownikowi, a manipulator nie jest
uruchamiany.
Zrzut ekranu stworzonej aplikacji przedstawiono na rysunku 5.
Rysunek 5. Panel użytkownika aplikacji sterującej manipulatorem
Bibliografia
1. Szkodny Tadeusz: Zbiór zadań z podstaw robotyki. Wydawnictwo Politechniki Śląskiej,
Gliwice 2010
8