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
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
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 (x 0 y 0 z 0 - układ bazowy manipulatora, x M y M z M - układ związany z markerem, x K y K z K - układ związany z kamerą (obrazem z kamery)). Położenie układu x K y K z K 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 z K, jednak przyjmuje się, że równoległość osi z K, z M, z 0 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
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 L 2 = 17.4 cm L 3 +L 4 = L 34 = 22 cm 4
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 s i = sin(θ i ), c i = cos(θ i ) oraz s ij = sin(θ i + Θ j ), c ij = 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 d x oraz d y 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
W związku z ograniczeniami nałożonymi na kąt Θ 3 można stwierdzić, że s 3 będzie zawsze ujemny. W związku z tym prawdziwe jest: Następnie, znając s 3 oraz c 3 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 c 3 > 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 x K y K z K związany z obrazem z kamery umieszczonej 6
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 0 T K z układu współrzędnych x K y K z K do układu bazowego manipulatora; aby ją otrzymać, wprowadzono pomocniczo układ związany z markerem x M y M z M. Prawdziwe jest: 0 T K = ( K T 0 ) -1 = ( K T M M T 0 ) -1. Przejście K T M 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 x M y M z M ) wyrażone w układzie x K y K z K, następnie rotacja o 180 wokół osi x, rotacja wokół osi z o kąt skręcenia osi x M względem x K, 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 T M = Trans (x K M, y K M, 0) Rot (x, 180 ) Rot (z, α K M) Scale (r K / r M, r K / r M, r K / r M ). Parametry x K M, y K M, α K M, r K obliczane są w programie na podstawie obrazu z kamery, za każdym razem, gdy pobierany jest nowy obraz, zaś r M jest wielkością stałą i według pomiaru wynosi 3.85 cm. Z kolei orientacja osi x M, y M, z M została tak dobrana, że transformacja M T 0 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 T 0 = Trans (o M 0) = Trans (22.4, -44.25, 0). Mając współrzędne danego punktu na obrazie [x, y, z] K p, można zatem otrzymać jego współrzędne w układzie bazowym manipulatora jako: [x, y, z] 0 p = 0 T K [x, y, z] K p. 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 0 T K. Następnie na obrazie wyszukiwane są okrągłe 7
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