Układanie wież Hanoi z wykorzystaniem robota ... · Robot, który traﬁł do Laboratorium Robotyki wymagał kilku korekt pozwala-jących na wykorzystanie go do badań laboratoryjnych[15]

Rok akademicki 2015/2016

POLITECHNIKA WARSZAWSKA

WYDZIAŁ ELEKTRONIKI I TECHNIK INFORMACYJNYCH

INSTYTUT AUTOMATYKI I INFORMATYKI STOSOWANEJ

PRACA DYPLOMOWA INŻYNIERSKA

Patryk Wąsowski

Układanie wież Hanoi z wykorzystaniem robotamanipulacyjnego IRp-6

Opiekun pracy:dr inż. Tomasz Winiarski

Ocena pracy: . . . . . . . . . . . . . . . . . . . . . . . . . . . .

. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

Data i podpis Przewodniczącego

Komisji Egzaminu Dyplomowego

Streszczenie

Tytuł: Układanie wież Hanoi z wykorzystaniem robota manipulacyj-nego IRp-6

Celem pracy było stworzenie oprogramowania dla robota IRp-6, które pozwolirozwiązać problem wież Hanoi. Manipulator musi sam wykryć początkowe położeniełamigłówki, a następnie na podstawie wyznaczonej listy kroków musi odpowiednioprzekładać krążki tak, aby przenieść stos ze słupka początkowego na końcowy. Modułwizyjny aplikacji, który pobiera obraz z kamery zamontowanej przy chwytaku i wyszu-kuje elementy układanki został napisany przy użyciu struktury ramowej DisCODe.Moduł decyzyjny sekwencyjnie wykonuje zadania podnoszenia krążka, przenoszeniago nad odpowiedni słupek i odkładania go na podstawie informacji pozyskanych z mo-dułu wizyjnego. Do manipulacji robotem użyty został interfejs operatorski IRPOS.Trzecią częścią pracy jest moduł wizualizacyjny, który na ekranie komputera w pro-gramie RViz przedstawia aktualny stan manipulatora i łamigłówki. Cała komunikacjapomiędzy modułami i sterowanie robotem zostały zrealizowane w oparciu o systemROS/OROCOS.

Słowa kluczowe: problem wież Hanoi, IRp-6, ROS, OROCOS, IRPOS, Di-sCODe, RViz

Abstract

Title: Tower of Hanoi solving using IRp-6 robot

The aim of the thesis was to create a software for IRp-6 robot that would allowto solve Tower of Hanoi problem. Manipulator has to detect starting position ofthe puzzle and then shift disks basing on generated step list. The vision moduleof the application which receives images from a camera and detects elements ofthe puzzle was implemented with DisCODe framework. A decision-making moduleperform tasks of lifting a disk, moving it over appropriate and put the disk basingon information collected from the vision module. Robot control was reached withIRPOS interface. Third part of the thesis was a visualisation module which usesRViz program to show current state of the robot and the puzzle. Communicationbetween modules and robot control were based on ROS/OROCOS system.

Keywords: Tower of Hanoi problem, IRp-6, ROS, OROCOS, IRPOS, DisCODe,RViz

4

Spis treści

1 Wstęp 71.1 Wprowadzenie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71.2 Motywacje . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71.3 Cel . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71.4 Układ pracy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

2 Wykorzystany sprzęt i technologie 92.1 Sprzęt . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

2.1.1 Manipulator IRp-6 . . . . . . . . . . . . . . . . . . . . . . . . 92.1.2 Kamera . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 102.1.3 Wieże Hanoi . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

2.2 Oprogramowanie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 112.2.1 ROS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 112.2.2 RViz . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 122.2.3 OROCOS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 132.2.4 IRPOS . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 142.2.5 DisCODe . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 142.2.6 OpenCV . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

3 Realizacja 173.1 Stanowisko pracy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 173.2 Założenia projektowe . . . . . . . . . . . . . . . . . . . . . . . . . . . 173.3 Struktura systemu . . . . . . . . . . . . . . . . . . . . . . . . . . . . 183.4 Moduł wizyjny . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18

3.4.1 Akwizycja obrazu . . . . . . . . . . . . . . . . . . . . . . . . . 193.4.2 Wykrycie czerwonego krążka . . . . . . . . . . . . . . . . . . . 213.4.3 Wykrycie słupków . . . . . . . . . . . . . . . . . . . . . . . . 22

3.5 Moduł decyzyjno-manipulacyjny . . . . . . . . . . . . . . . . . . . . . 233.5.1 Klasa Hanoi . . . . . . . . . . . . . . . . . . . . . . . . . . . . 233.5.2 Klasa Irp6Hanoi . . . . . . . . . . . . . . . . . . . . . . . . . 243.5.3 Przemieszczenie chwytaka nad słupek . . . . . . . . . . . . . . 263.5.4 Pobranie krążka . . . . . . . . . . . . . . . . . . . . . . . . . . 283.5.5 Przeniesienie krążka . . . . . . . . . . . . . . . . . . . . . . . 283.5.6 Odłożenie krążka . . . . . . . . . . . . . . . . . . . . . . . . . 29

3.6 Moduł wizualizacyjny . . . . . . . . . . . . . . . . . . . . . . . . . . . 313.6.1 Aktualizacja stanu wież Hanoi . . . . . . . . . . . . . . . . . . 313.6.2 Podejmowanie i upuszczanie krążka . . . . . . . . . . . . . . . 32

5

6 SPIS TREŚCI

4 Podsumowanie 334.1 Testy oprogramowania . . . . . . . . . . . . . . . . . . . . . . . . . . 33

4.1.1 Testy modułu wizyjnego . . . . . . . . . . . . . . . . . . . . . 334.1.2 Testy modułu decyzyjno-manipulacyjnego . . . . . . . . . . . 344.1.3 Testy modułu wizualizacyjnego . . . . . . . . . . . . . . . . . 34

4.2 Wnioski . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35

Rozdział 1

Wstęp

Pierwszy rozdział pracy stanowi wstęp, na który składają się wprowadzeniedo tematyki(sekcja 1.1), motywacje autora do napisania pracy(sekcja 1.2), jejcel(sekcja 1.3) oraz układ(sekcja 1.4).

1.1 Wprowadzenie

Robotyka jest jedną z najmłodszych dziedzin nauki. Obejmuje ona swoim za-sięgiem zagadnienia z zakresu automatyki, informatyki, elektroniki i cybernetyki.Wszystkie te dziedziny są przedmiotem nauczania na Wydziale Elektroniki i Tech-nik Informacyjnych. Rozwój robotyki oraz dotychczasowe zdobycze nauki pozwa-lają ograniczyć wykorzystanie zasobów ludzkich na rzecz manipulatorów i robotówmobilnych w środowisku pracy nieprzyjaznym dla człowieka lub przy czynnościachwymagających wysokiej dokładności. Ponadto są one coraz częściej wykorzystywanedla wygody ludzi, jak chociażby w przypadku robotów koszących trawniki czy odnaj-dujących żądane produkty w sklepach. Dlatego rozwijanie się w dziedzinie robotykiotwiera szerokie horyzonty dla inżynierów.

1.2 Motywacje

Możliwość pracy na manipulatorze IRp-6 w Zespole Programowania Robotówi Systemów Rozpoznających jest niecodzienną okazją do rozwinięcia wiedzy z za-kresu robotyki oraz przetwarzania obrazów. Praca w powszechnie stosowanym robo-tycznym środowisku programistycznym z robotem, którego można spotkać w wielumiejscach pracy jest kolejną zaletą. Stąd też silną motywacją przy wyborze tematupracy inżynierskiej była duża wartość edukacyjna projektu, który wydaje mi siębyć idealnym podsumowaniem studiów inżynierskich kierunku informatycznego. Te-mat ten, jak się później okaże jest przekrojem całej wiedzy, jaką student ma szansęposiąść.

1.3 Cel

Celem mojej pracy było zaprojektowanie i zaimplementowanie aplikacji umożli-wiającej rozwiązanie problemu wież Hanoi przez manipulatora robotycznego IRp-6.Całość składa się z trzech zasadniczych części:

7

8 ROZDZIAŁ 1. WSTĘP

• wizji, czyli odbioru i przetworzenia obrazu z kamery, z którego należy wyłuskaćniezbędne informacje o obiekcie zainteresowania

• manipulacji, czyli odpowiedniego wykonywania ruchów manipulatora, celemczego jest odpowiednie przełożenie krążków

• wizualizacji, czyli przedstawienia aktualnego stanu robota i układanki naekranie monitora

1.4 Układ pracy

Praca składa się z 4 rozdziałów, łącznie z niniejszym, który stanowi wstęp i celpracy. Następny rozdział poświęcony jest sprzętowi i technologiom niezbędnym dowykonania postawionego zadania. Kolejny opisuje realizację trzech modułów po-trzebnych do rozwiązania problemu wież Hanoi. Ostatni rozdział przedstawia wynikidziałania aplikacji i wyciągnięte wnioski.

Rozdział 2

Wykorzystany sprzęt i technologie

Rozwiązanie zadania postawionego w tej pracy inżynierskiej jest połączeniemsprzętu (sekcja 2.1) udostępnionego w laboratorium 012 ZPRiSR oraz oprogramo-wania (sekcja 2.2).

2.1 Sprzęt

2.1.1 Manipulator IRp-6

Centralnym elementem całego systemu jest manipulator robotyczny IRp-6 sto-jący w Laboratorium Robotyki Instytutu Automatyki i Informatyki Stosowanej (ry-sunek 2.1(a))[8]. Został on wyprodukowany na początku lat osiemdziesiątych ubie-głego wieku w Przemysłowym Instytucie Automatyki i Pomiarów (wówczas MERAPIAP) na podstawie licencji wykupionej od szwedzkiej firmy ASEA. Roboty wy-produkowane w Szwecji służyły przede wszystkim w fabrykach samochodów. Wyko-rzystywano je między innymi na etapie spawania i lakierowania karoserii, czyli tamgdzie procesy przemysłowe są szkodliwe dla zdrowia ludzkiego lub wymagana jestwysoka precyzja.

Robot, który trafił do Laboratorium Robotyki wymagał kilku korekt pozwala-jących na wykorzystanie go do badań laboratoryjnych[15]. Przede wszystkim do-łączono ostatni człon, który zwiększył liczbę stopni swobody do 6. W ostatnimogniwie łańcucha kinematycznego zamontowano kiść z chwytakiem dwupalczastym(rysunek 2.1(b)). Znajduje on zastosowanie przy podejmowaniu niewielkich elemen-tów, takich jak krążki w wieżach Hanoi czy pionków przy grze w warcaby. Ponadtoobok kiści zamontowana jest kamera, o której więcej informacji będzie w następnejpodsekcji (2.1.2). Ostatnią znaczącą modyfikacją było dołożenie czujników siły przykiści. Pozwaliły one ulepszyć sterowniki dla silników robota.

Do chwytaka dołączyłem również szczypce. Zwiększają one odległość miejscapodjęcia krążka od kiści, dzięki czemu nie zawadza ona o słupki. Poza tym zmniej-szają one graniczne wartości szerokości chwytaka, co umożliwia złapanie najmnie-szych krążków.

W Laboratorium stoją dwa egzemplarze tego manipulatora - Postument orazTrack. Ten drugi dodatkowo posiada tor jezdny. Do realizacji mojego zadania wy-korzystałem robota IRp-6 Postument.

9

10 ROZDZIAŁ 2. WYKORZYSTANY SPRZĘT I TECHNOLOGIE

(a) Ramię robota (b) Kiść z chwytakiem dwupalcza-stym

Rysunek 2.1: Manipulator IRp-6 Postument

2.1.2 Kamera

Na kiści manipulatora zamontowana jest kamera cyfrowa Blackfly firmy PointGrey(rysunek 2.2(a)), wariant BFLYPGE-14S2C-CS[3]. Posiada ona matrycę o roz-dzielczości 1,4MPix. Zamontowany został obiektyw LC-M13VM2812IRD firmy LCSecurity o przesłonie F1,4 i regulowanej długości ogniskowej z zakresu 2,8-12 mm[2].Korzystając z pierścieni, stanowiących również część obudowy, użytkownik możedobrać szerokość otwarcia przesłony, długość ogniskowej oraz ostrość. Dołączony dokamery sterownik pozwala na dobór takich parametrów akwizycji jak liczba klatekna sekundę, długość otwarcia migawki, balans bieli czy wzmocnienie.

2.1.3 Wieże Hanoi

Problemem, który ma rozwiązać moja aplikacja, są wieże Hanoi. Jest to mate-matyczna łamigłówka stworzona w 1883 roku przez francuskiego uczonego EdouardaLucasa[9]. Z zagadką związana jest pewna legenda. Mówi ona, że bramińscy mnisiz zakonu Kashi Vishvanath mają za zadanie rozwiązać problem wież Hanoi prze-kładając 64 złote krążki z jednego słupka na drugi. Kiedy zakonnicy ukończą swojezadanie nastąpi koniec świata. Jednakże, można być pewnym, że potrwa to jeszczedługo. Zakładając, że przełożenie jednego krążka zajmuje tylko 1 sekundę, to narozwiązanie zagadki potrzeba będzie 264 − 1 sekund, czyli około 584 miliardów lat.

Łamigłówka składa się z trzech słupków. Na początku wszystkie krążki ułożonesą na jednym ze słupków od najmniejszego na dole do największego na szczycie.Zadaniem rozwiązującego ten problem, jest przełożenie stosu krążków ze słupkapoczątkowego na docelowy, podczas gdy trzeci słupek pełni rolę pomocniczego. Całałamigłówka ma tylko dwie zasady. Po pierwsze można przenosić tylko jeden krążekjednorazowo. Po drugie mniejszy krążek zawsze musi zostać położony na większymkrążku (lub na wolnym słupku).

Wieże Hanoi użyte w mojej pracy przedstawione są na rysunku 2.2(b).

2.2. OPROGRAMOWANIE 11

(a) Kamera Blackfly (b) Wieże Hanoi

Rysunek 2.2: Kamera i wieże Hanoi

2.2 Oprogramowanie

Do wykonania zadania potrzebna jest integracja pomiędzy wspomnianymi wcze-śniej modułami: wizyjnym, manipulacyjnym i wizualizacyjnym. Każdy z nich jestproblemem rozwiązywanym przez różne systemy i biblioteki. Środowisko stworzonew Laboratorium Robotyki posiada zainstalowane odpowiednie narzędzia do imple-mentacji i integracji tychże modułów.

2.2.1 ROS

ROS[7] (Robot Operating System) jest programową strukturą ramową pozwala-jąca programować różnego rodzaju roboty. Jest to projekt zespołu Stanford ArtificialIntelligence Laboratory rozprowadzany na podstawie licencji BSD. Pierwsza wersjasystemu powstała w 2007 roku. ROS składa się z szeregu narzędzi, konwencji i biblio-tek ułatwiających pisanie implementację algorytmów dla robotów i ich testowanie.Kolejne wydania systemu są tworzone przez robotyków z ośrodków naukowych ca-łego świata, dzięki czemu zapewniona jest uniwersalność oraz fachowe opracowanie.ROS realizuje funkcjonalność systemu operacyjnego, choć w rzeczywistości nim niejest. Tworzy warstwę abstrakcji sprzętowej, zapewnia niskopoziomową kontrolę nadrobotami, implementuje wykonywanie typowych funkcji, zarządza uruchomionymiprocesami (węzłami) i komunikacją pomiędzy nimi. ROS wspiera programowaniew językach C++ i Python.

Podstawową jednostką budującą system jest pakiet. Może on zawierać w sobieprogramy, skrypty, biblioteki, zbiory danych oraz pliki konfiguracyjne. Obok tychplików znajduje się zawsze manifest, czyli dokument XML zawierający podstawowedane o pakiecie, m.in. nazwę, wersję, opis, zależność od innych pakietów i bibliotek.W systemie ROS jest dostępnych wiele predefiniowanych pakietów, ale użytkownikjest zachęcany do pisania własnych.

Podstawowe elementy systemu ROS wykorzystane w mojej pracy:

• Węzeł - każdy uruchomiony program czy skrypt staje się węzłem, czyli nie-zależnym procesem wykonującym obliczenia. Węzły działają autonomicznie,


Rysunek 2.3: Schemat tematu w systemie ROS

mają własne nazwy, dzięki którym można się do nich później odwołać. Komu-nikacja z innymi procesami następuje przez tematy lub usługi.

• Wiadomości - struktury danych określające przesyłaną między węzłami treść.Mogą się składać z podstawowych typów lub ich wektorów. Oprócz standar-dowych wiadomości, użytkownik może zdefiniować własną strukturę.

• Temat - kanał komunikacyjny służący do wymiany danych między wę-złami(rysunek 2.3). Każdy węzeł będący wydawcą (ang. publisher) tematu,może wysyłać wiadomości. Natomiast węzły określone jako subskrybent (ang.subscriber) tematu mogą je odbierać. Typ wiadomości, jaki będzie wysyłanyw danym temacie jest określony przy nadawaniu węzłowi statusu wydawcy lubsubskrybenta. Przy tak zdefiniowanej komunikacji odbiorcy i nadawcy wiado-mości nie wiedzą o sobie nawzajem.

2.2.2 RViz

Jednym z modułów dołączonych do systemu ROS jest RViz[5] (rysunek 2.4).Pozwala on wizualizować w przestrzeni trójwymiarowej dane z czujników robota.Komunikacja z modułem następuje przez wspomniane wcześniej tematy. Użytkow-nik może wybrać między innymi elementy do wyświetlenia, człon robota, do któregoodnosić się będzie cały widok albo kolor tła. Ponadto program pozwala na wyświe-tlanie znaczników (ang. markers), które odpowiadają elementom nie będącym czę-ścią systemu robotycznego. Reprezentowane przez prymitywy takie jak punkt, sfera,prostopadłościan czy stożek mogą wizualizować przedmioty z otoczenia robota.

Rysowanie obiektów z prymitywów możliwe jest dzięki strukturze Marker, któraznajduje się w pakiecie visualization msgs systemu ROS. Użytkownik może podaćnastępujące parametry definiujące marker, który ma zostać dodany, zmodyfikowanylub usunięty z widoku w programie RViz:


Rysunek 2.4: RViz - moduł do wizualizacji w systemie ROS

• id - unikalny numer każdego markera, który jednoznacznie go identyfikuje

• type - typ markera, może to być m.in. strzałka, sfera, sześcian, walec, stożek

• action - rodzaj wykonywanej akcji, może to być dodanie, modyfikacja lubusunięcie markera o wskazanym numerze id

• pose - struktura o trzech polach x, y i z, wyznacza pozycję markera

• scale - struktura o trzech polach x, y i z, określa wymiary markera w trzechpłaszczyznach

• color - struktura o czterech polach r, g, b i a, określa barwę markera w modeluRGBA

• frame locked - za każdym odświeżeniem obrazu w programie RViz dokony-wane jest przeliczenie pozycji markera w danym kadrze, stosowane np. kiedyma zostać zachowana stała pozycja krążka względem chwytaka podczas prze-noszenia

2.2.3 OROCOS

Główną wadą struktury ramowej ROS jest to, że nie implementuje ona cechsystemu czasu rzeczywistego. Z tego powodu ograniczenia czasowe narzucone przezelektronikę robota nie mogą zostać spełnione. Dlatego w Laboratorium Robotyki zo-stał zainstalowany system OROCOS[6], czyli Open Robot Control Software. Jest toprogramowa struktura ramowa umożliwiająca pisanie programów dla robotów dzia-łających w czasie rzeczywistym[10]. Ma on budowę modułową, składa się z konfigu-rowalnych komponentów, których kod wykonywany jest z częstotliwością taktowaniasystemu. OROCOS może być uruchamiany w węźle systemu ROS.


2.2.4 IRPOS

W Zespole Programowa Robotów i Systemów Rozpoznających został stworzonyIRPOS - interfejs operatorski dla sterownika ROS/OROCOS. Napisany w języku Py-thon, dostarcza metody służące do sterowania robotami IRp-6 Postument i Tracktakie jak ustawienie wszystkich stawów w zadanych położeniach, przemieszczeniekońcówki w układzie operacyjnej czy zmiana szerokości rozstawu palców chwytaka.IRPOS osadzony jest w węźle systemu ROS, a dzięki komponentom OROCOS moż-liwe jest sterowanie w czasie rzeczywistym. Metody klasy IRPOS wykorzystanew mojej pracy to między innymi:

1 IRPOS (nodeName , robotName , robotJointNumbers)

Konstruktor klasy IRPOS, tworzy węzeł systemu ROS i nawiązuje komunikacjęz komponentami OROCOS. Ponadto uruchamia tematy, do których publikowane sąinformacje o stanie robota. Parametry wywołania: nodeName - nazwa węzła ROS,robotName - nazwa robota, który ma być używany, przyjmuje wartość Postumentlub Track, robotJointNumber - liczba stawów robota, 6 lub 7 zależnie od wybranegomanipulatora.

1 move_rel_to_cartesian_pose(time_from_start , rel_pose)

Funkcja pozwala przesunąć końcówkę roboczą względem aktualnej pozycji w czasietime from start o wektor rel pose. Wartości są określone w układzie operacyjnym,tzn. podawane jest przesunięcie wyrażone w metrach dla każdej z osi OX, OY i OZrobota.

1 move_to_joint_position(joint_positions , time_from_start)

Ustawia wszystkie stawy robota do konfiguracji zadanej w wektorze joint positionsw czasie time from start. Pozycje stawów podaje się od podstawy do najbliższegochwytakowi, czyli obrót końcówki roboczej wokół własnej osi.

1 tfg_to_joint_position(joint_position , time_from_start)

Przesuwa palce chwytaka do szerokości zadanej w joint position wyrażonej w me-trach w czasie time from start.

2.2.5 DisCODe

Kolejną programową strukturą ramową stworzoną w Zespole Programowania Ro-botów i Systemów Rozpoznających jest DisCODe[12], czyli Distributed ComponentOriented Data Processing. Celem tej platformy jest obsługa danych sensorycznych,co w przypadku mojej pracy oznacza odbieranie i przetwarzanie obrazu rejestro-wanego przez kamerę cyfrową. Narzędzie zostało napisane w języku C++ przez drinż. Tomasza Kornutę i mgr inż. Macieja Stefańczyka. DisCODe ma budowę modu-łową, dzięki czemu można wielokrotnie wykorzystać ten sam kod i budować łatwomodyfikowalne systemy dowolnej wielkości.

Podstawowym elementem budującym potok przetwarzania danych jest kompo-nent (rysunek 2.5(a)), który realizuje pojedynczą funkcję jak na przykład rozmycieGaussa, wykrycie obiektów. Każdy komponent może mieć zdefiniowane:

• strumień danych wejściowych

• strumień danych wyjściowych


Port wyjściowyPort wejściowyStrumień danych

Funkcja p

Komponent Funkcja przejściaPriorytet funkcji

Strumień danych

Argument funkcjiKomponentk

Kolejność

Wartość funkcji

(a) Struktura komponentu DisCODe[13]

Port wyjściowyPort wejściowy

Strumień danych

Nazwa

KomponentEgzekutorPodzadanie

Strumień danych

Nazwaegzekutorak

Kolejność Nazwapodzadania

Typ komponentu

(b) Struktura zadania systemu DisCODe[13]

Rysunek 2.5: Struktura komponentu i zadania systemu DisCODe

• procedury obsługi danych (ang. handler)

• parametry, określające dowolne właściwości komponentu

Użytkownik systemu DisCODe zachęcany jest do korzystania z bibliotekiOpenCV przy tworzeniu swoich komponentów.

Oprócz tych tworzonych przez użytkownika, DisCODe posiada sporo wbudowa-nych komponentów realizujących podstawowe i najczęściej używane operacje jakodbiór obrazu z kamery, eliminację efektu rybiego oka, operacje morfologiczne.Nastepnie komponenty są agregowane w egzekutorze, który jest oddzielnym wąt-kiem systemu. Egzekutor zapewnia sekwencyjne wykonywanie operacji określonychw komponentach. Może być uruchomiony w trybie okresowym lub ciągłym jakojedno z podzadań. Cały tok przetwarzania danych wizyjnych jest określony w zada-niu (rysunek 2.5(b)). Zadanie składa się ono z egzekutorów, ma postać pliku XML.To w nim określa się połączenia strumieni danych wejściowych i wyjściowych po-między komponentami, priorytety egzekutorów i parametry komponentów.

2.2.6 OpenCV

OpenCV[4] (ang. Open Source Computer Vision) jest biblioteką funkcji do prze-twarzania obrazu. Stworzona i rozwijana przez firmę Intel kładzie nacisk na wydajneobrabianie grafik w czasie rzeczywistym. Napisana w języku C++, ma także wspar-cie dla języków C, Java oraz Python. Biblioteka posiada blisko 2500 algorytmów[11].Niektóre z nich wykorzystane w mojej pracy to[1]:

1 cvtColor(InputArray src , OutputArray dst , int code , int dstCn)

Konwertuje obraz src do modelu przestrzeni barw określonego w parametrze code.Można również określić liczbę kanałów dstCn. Przekonwertowany obraz zapisywanyjest w tablicy dst.

1 inRange(InputArray src , InputArray lowerb , InputArray upperb ,OutputArray dst)

Wykonuje zadanie progowania na tablicy src. Parametry lowerb i upperb określajągraniczne wartości operacji. Wynik zapisywany jest do tablicy dst.

1 findContours(InputOutputArray image , OutputArrayOfArrays contours ,OutputArray hierarchy , int mode , int method , Point offset)

Wyszukuje konturów w obrazie image i zapisuje je jako wektory punktów w tablicyarrays. Funkcja pozwala dobrać odpowiednie do postawionego problemu opcje.


1 approxPolyDP(InputArray curve , OutputArray approxCurve , doubleepsilon , bool closed)

Odnajduje wielokąt aproksymowany tablicą punktów curve. Parametr epsilon okre-śla dokładność przybliżenia, a closed mówi, czy wielokąt jest zamknięty lub otwarty.Znaleziony wielokąt zapisywany jest do tablicy approxCurve.

Rozdział 3

Realizacja

Niniejszy rozdział opisuje rozwiązanie problemu postawionego w mojej pracy. Napoczątek przedstawię stanowisko pracy (sekcja 3.1). Dalej wymienione są przyjętezałożenia projektowe (sekcja 3.2). Następnie zaprezentuję strukuturę całego systemu(sekcja 3.3) z realizacją jego modułów składowych: wizyjnego (sekcja 3.4), decyzyjno-manipulacyjnego(sekcja 3.5) i wizualizacyjnego(sekcja 3.4).

3.1 Stanowisko pracy

Na stanowisko robocze składają się dwa komputery - gerwazy i dowolny kompu-ter podłączony do sieci lokalnej Laboratorium Robotyki. Gerwazy pełni rolę serwerai nadzorcy dla robotów IRp-6. To na nim uruchamiany jest rdzeń systemu ROSi podległy mu OROCOS odpowiedzialny za wykonywanie zadań w czasie rzeczywi-stym. Do niego też są bezpośrednio podłączone sterowniki robota poprzez interfejsEtherCAT (Ethernet for Control Automation Technology). Współpraca między ger-wazym a komputerem roboczym zrezalizowana jest w architekturze klient-serwer,gdzie wykonywanie obliczeń i wydawanie komend dla manipulatora jest po stroniestacji roboczej.

Łączność z kamerą cyfrową zamontowaną przy chwytaku następuje przez interfejsEthernet bezpośrednio z komputera roboczego.

W pobliżu robotów IRp-6 znajduje się kilka przenośnych i dwa stałe awaryjnewyłączniki prądu, które można użyć w przypadku awarii lub niepożądanego zacho-wania manipulatorów.

3.2 Założenia projektowe

Założenia przyjęte w rozwiązaniu problemu inżynierskiego czynią go klarownymi wyznaczają cele niezbędne do osiągnięcia ostatecznego sukcesu oprogramowania.

• Manipulatorem rozwiązującym problem wież Hanoi jest IRp-6 Postument

• Na początku wieże Hanoi muszą stać w takim miejscu, aby w polu widzeniakamery znajdowały się wszystkie trzy słupki

• Wieże na początku są ustawione w pozycji horyzontalnej względem widokuz kamery, jednak możliwe są odchylenia do 40◦

17

18 ROZDZIAŁ 3. REALIZACJA

• Liczba krążków, które mają być przeniesione ze słupka początkowego na koń-cowy jest zawsze równa 4

• Rozwiązywany jest podstawowy wariant wież Hanoi, tzn. na początku wszyst-kie krążki w szyku malejącym (zaczynając od najniższego) są ułożone na jed-nym stosie

• Przed rozwiązaniem problemu przez manipulator, program posiada informacjeo wymiarach podstawki, słupków i krążków

• W układaniu biorą udział cztery krążki, zaczynając od największego - niebie-ski, zielony, żółty i czerwony

• Słupek początkowy nie jest z góry znany, określa go położenie stosu krążków

• Słupek końcowy jest wybierany przez moduł decyzyjny programu

• Możliwe są przesunięcia wieży i obroty, o ile modyfikacja położenia nie spowo-duje utraty wszystkich słupków z pola widzenia kamery

• Możliwe jest zmiana jasności oświetlenia, warunki pogodowe nie mają wpływuna wykrywanie obiektów na obrazie z kamery

• W celu ułatwienia wykrycia, czubki słupków zostały pomalowane różowymlakierem

• Wizualizowane są: manipulator IRp-6 Postument, podstawka ze słupkami i 4krążki

3.3 Struktura systemu

System zgodnie z założeniami składa się z trzech modułów: wizyjnego, decyzyjno-manipulacyjnego i wizualizacyjnego. Pierwszy z wymienionych odpowiedzialny jestza odbiór obrazu z kamery, obróbkę i wyłuskanie informacji niezbędnych dla popraw-nego sterowania robotem. Moduł decyzyjno-manipulacyjny generuje listę przełożeń,na podstawie której określane jest sterowanie robotem. Natomiast ostatni modułkomunikuje się z programem RViz, dzięki czemu możliwe jest wizualizowanie aktu-alnego stanu robota i łamigłówki na ekranie monitora. Struktura systemu zostałaprzedstawiona na rysunku 3.1.

Komunikacja pomiędzy modułem wizyjnym a decyzyjnym następuje poprzez te-maty systemu ROS. Wybrane typy wiadomości to Int32MultiArray, czyli wektor32-bitowych liczb całkowitych, oraz Int16, czyli 16-bitowa liczba całkowita.

Moduł wizualizacyjny jest podobiektem modułu decyzyjno-manipulacyjnego.Komunikacja następuje poprzez wybieranie odpowiednich metod klasy odpowie-dzialnej za przesyłanie danych do RViz.

3.4 Moduł wizyjny

Moduł wizyjny został zrealizowany z pomocą struktury ramowej DisCODe. Za-daniem tej części systemu jest odbiór danych z kamery cyfrowej, odpowiednie prze-tworzenie obrazu i wyciągnięcie informacji o położeniu istotnych elementów wież

3.4. MODUŁ WIZYJNY 19

Rysunek 3.1: Struktura systemu

Hanoi, czyli słupków oraz czerwonego krążka. Zadanie w systemie DisCODe zostałopodzielone na dwa podzadania: Processing oraz Visualisation. To drugie służy tylkodo zaprezentowania użytkownikowi obrazów uzyskanych na poszczególnych etapachobróbki i detekcji. Struktura modułu wizyjnego przedstawiona jest na rysunku 3.2

3.4.1 Akwizycja obrazu

Odbiór danych z kamery umożliwia sterownik dołączony przez producenta orazkomponent CameraPGR stworzony w Laboratorium Robotyki. Egzekutor zawiera-jący ten komponent znajduje się w podzadaniu Processing.

Parametrami komponentu CameraPGR są właściwości odbieranego obrazu,które można programowo zmieniać, m.in. czas otwarcia przesłony, wzmocnienie,liczba klatek na sekundę, jasność. Ponadto użytkownik musi podać wymiary ob-razu oraz numer seryjny wybranej kamery, gdyż tym samym komponentem możemyobsłużyć kamery zamontowane przy chwytakach manipulatorów Postument i Track.

Niestety otrzymany obraz nie nadaje się od razu do obróbki. Pomimo korekcjiwyżej wymienionych właściwości nie można pozbyć się efektu rybiego oka, który jestbeczkowatym zniekształceniem obrazu. Powstaje ono na skutek zastosowanych w ka-merze soczewek. Dystorsja korygowana jest przez wbudowany komponent nazwanyCvUndistort. Oparty o funkcję undistort biblioteki OpenCV komponent CvUndi-stort na porcie strumienia danych wejściowych oprócz samego obrazu otrzymuje rów-nież dane pochodzące z komponentu CameraInfoProvider. Zawiera on dane otrzy-mane w wyniku przeprowadzenia kalibracji, czyli 5-elementowy wektor współczyn-ników dystorsji oraz macierz kamery wyrażoną w postaci:

A =

fx 0 cx0 fy cy0 0 1

gdzie fx i fy to ogniskowe osi OX i OY w układzie kamery wyrażone w pikselach,a cx i cy to współrzędne środka obrazu.

Porównanie obraz przed i po dokonaniu korekcji dystorsji znajduje się na rysun-kach 3.3(a) i 3.3(b).


Rysunek 3.2: Struktura modułu wizyjnego

(a) Obraz z efektem rybiego oka (b) Obraz po zastosowaniu funkcji undistort

Rysunek 3.3: Obraz przed i po korekcji dystorsji

3.4. MODUŁ WIZYJNY 21

3.4.2 Wykrycie czerwonego krążka

W pracy przyjąłem założenie, że rozwiązywana będzie podstawowa wersja pro-blemu wież Hanoi, tzn. wszystkie krążki początkowo znajdują się na jednym stosie.W związku z tym, że widoczność większych krążków jest ograniczona, zdecydo-wałem się na wykrywanie tylko czerwonego krążka, który znajduje się najwyżeji jednoznacznie określa słupek startowy. Jego położenie względem środka obrazudetereminuje słupek początkowy.

Za wykrycie czerwonego krążka odpowiedzialny jest komponent o nazwie Red-DiskDetection. Na strumieniu danych wejściowych otrzymywany jest obraz bez dys-torsji, które nadejście wywołuje uruchomienie funkcji obsługującej ten komponent.

Pierwszym etapem detekcji krążka jest wydzielenie wszystkich elementów o in-teresującym kolorze, w tym wypadku czerwonym, od pozostałych.

Wykrywanie barw w modelu RGB byłoby słuszne, gdyby nie to, że możliwośćzmiany oświetlenia modyfikuje wszystkie składowe modelu barw. Zaciemnienie w La-boratorium powoduje spadek wartości współrzędnych Red, Green oraz Blue o mniejwięcej tą samą wartość. Wzrost natężenia daje efekt analogiczny. Z tej przyczynyzastosowałem konwersję obrazu do przestrzeni barw HSV, w której informacja o od-cieniu barwy (ang. hue) jest niezależna od jej nasycenia (ang. saturation) i jasności(ang. value). Współrzędna Hue wyrażana jest w tym modelu jako wartość z prze-działu 0◦ - 360◦ (W bibliotece OpenCV twórcy zmniejszyli zakres do 0◦ - 180◦).Pozostałe dwie przyjmują liczbę całkowitą od 0 do 100.

1 cv:: cvtColor(src , hsv , CV_BGR2HSV); // src - odebrany obraz , hsv -obraz uzyskany po konwersji

Z modelu HSV interesująca jest dla mnie składowa Hue i na jej podstawie obrazjest progowany. W wyniku funkcji inRange wszystkie składowe, których wartośćmieści się w podanym przedziale, na finalnym obrazie binarnym są równe 1, pozostałe- 0. Krańcowe wartości przedziału (min h i max h) zostały dobrane empirycznie.

1 split(hsv , hsv_split);2

3 cv:: inRange(hsv_split [0], min_h , max_h , thresh); // thresh - obrazbinarny

Wieże Hanoi ustawione są na szarym taśmociągu, którego barwa czasem wpa-sowuje się w zakres progowania. Pomarańczone obramowanie (widoczne na górzerysunku 3.3(b)) również miejscami spełnia te warunki. Stąd potrzebne jest wykorzy-stanie operacji morfologicznych, do usunięcia zbędnych śzumów”. Dylatacja (ang.dilate) powoduje wypełnianie ”dziur”w obrazie binarnym. Funkcja porównuje są-siednie piksele, i jeżeli jeden z nich wynosi 1, to piksel centralny, dla którego byłaprzeprowadzana dylatacja, również przyjmie tą wartość. Erozja (ang. erode) działaanalogicznie do poprzedniej funkcji. Jeśli nie wszystkie piksele z sąsiedztwa są równe1, to piksel centralny jest zerowany. Pozwala to usunąć wszystkie szumy i nierównościna granicach obiektów.

1 cv::blur(thresh , thresh , cv::Mat());2 cv:: erode(thresh , thresh , cv::Mat());

Uzyskany w ten sposób obraz pozwala wykryć na nim krążek, czyli okrągłyobiekt z wycięciem w środku (jest to kawałek słupka). Niestety funkcja OpenCVdedykowana do detekcji kół i okręgów HoughCircles nie spełnia dobrze swojej roli.


Zdecydowałem się zastosować w tym momencie funkcję findContours, która wyszu-kuje konturów na obrazie wejściowym. W związku z tym, że dylatacja i erozja niewyeliminowały szumów w stu procentach, wykrywane są kontury nie należące do po-szukiwanego krążka. Dlatego spośród odnalezionych kształtów wybierany jest ten,który ma największe pole.

1 cv:: findContours(thresh , contours , CV_RETR_EXTERNAL ,CV_CHAIN_APPROX_SIMPLE , cv::Point(0, 0)); // contours - wektorwektorow punktow

W ten sposób uzyskiwana jest informacja o położeniu czerwonego krążka. Na-stępnie pozycja krążka w osi OX (druga współrzędna nie jest istotna, gdyż założonejest położenie horyzontalne łamigłówki) przesyłana jest za pośrednictwem tematured disk do modułu decyzyjnego. Jest to wartość od 0 do 1032, dlatego zdecydo-wałem się na typ wiadomości Int16. Na podstawie tej informacji moduł decyzyjnyokreśla początkowe położenie stosu krążków.

3.4.3 Wykrycie słupków

Drugim obowiązkiem, który musi spełniać moduł wizyjny, jest wykrywanie poło-żenia słupków. W związku z tym, że słupki są tego samego koloru co podstawka wież,wykrycie ich byłoby bardzo ciężkie lub niemożliwe. Dlatego pomalowałem czubki ró-żowym lakierem, co zapewniło ich stu procentową wykrywalność.

Za odnajdywanie słupków, a dokładniej ich czubków, na obrazie otrzymanymz komponentu Undistort odpowiada komponent o nazwie RodsDetection. Działa onbardzo podobnie do wyżej opisanego RedDiskDetection.

Na początku przeprowadzana jest konwersja do modelu HSV. Dalej wykonywanajest funkcja inRange, przy czym podane są wartości min h i max h wyznaczającezakres progowania dla koloru różowego. Następnie po przeprowadzeniu operacji dy-latacji i erozji wyszukiwane są kontury przy użyciu findContours. Tu również jestwykrywanych wiele konturów nie należących do poszukiwanych słupków. Odnale-zione kontury, gdzie każdy z nich jest wektorem wektorów punktów, są zapisywanew strukturach contour. Stworzyłem te struktury na potrzeby sortowania konturówwedług wielkości.

1 struct contour2 {3 std::vector <cv::Point > cont;4 int area;5 cv::Rect rect;6 // przeciazony operator < do sortowania7 bool operator < (const contour &x) const8 {9 return area > x.area;10 }11 };

Tuż przed zapisaniem konturu do wymienionej struktury, sprawdzana jest po-wierzchnia kształtu, w którym ten kontur się zawiera. Wszystkie obiekty, którychwielkość jest dużo mniejsza lub dużo większa niż poszukiwanego słupka, są usuwane,pozostałe zaś trafiają w wektorze struktur contour. Dzięki tej wstępnej filtracji, przy-spieszone zostanie dalsze porządkowanie struktur. Skorzystałem tutaj z szablonufunkcji sort znajdującej się w bibliotece algorithm języka C++. Zależnie od imple-

3.5. MODUŁ DECYZYJNO-MANIPULACYJNY 23

mentacji, funkcja generyczna może korzystać z różnych algorytmów sortowania, aleużytkownik może być pewny, że złożoność nie będzie gorsza niż O(N log N).

Z posortowanego wektora wyciągane są jedna, dwie lub trzy pierwsze pozy-cje. Zależy to od tego, czy drugi i trzeci wykryty kształt mają nieznaczne różnicepowierzchni względem pierwszego. Dla każdego wybranego konturu tworzony jestprostokąt obramowujący go i obliczany jest punkt centralny znalezionego czubka.Po narysowaniu na obrazie wyjściowym wspomnianego prostokąta, do wiadomościtypu Int16MultiArray dopisywane są współrzędne środka wykrytego kształtu. Kiedywspółrzędne wszystkich czubków znajdą się w wektorze dopisywane są koordynatypunktu, który znajduje się dokładnie pod chwytakiem. Punkt ten został określonydoświadczalnie. Na koniec wiadomość przesyłana jest przez temat rods do modułudecyzyjnego.

3.5 Moduł decyzyjno-manipulacyjny

Moduł decyzyjno-manipulacyjny tworzy listę kroków potrzebnych do rozwiąza-nia problemu wież Hanoi, które dalej wykonuje biorąc poprawki w swoim modeluna dane odebrane z modułu wizyjnego. Został on napisany w języku Python, zaim-plementowany jako skrypty pakietu irp6 hanoi w systemie ROS. W zadaniu mani-pulacji ramieniem robota wykorzystany został interfejs operatorski IRPOS.

Na początku manipulator ustawia się w pozycji startowej. Przed nim powinna sięznajdować łamigłówka z widocznymi wszystkimi słupkami i czerwonym krążkiem.Wykrycie tego ostatniego pozwala utworzyć obiekt klasy Hanoi, która przygotowujelistę kroków do rozwiązania wież na podstawie wirtualnego modelu.

3.5.1 Klasa Hanoi

Jest to klasa pomocnicza. Do konstruktora obiektu przekazywane są dwa argu-menty: liczba krążków oraz słupek, który zawiera początkowy stos krążków. Bazującna tych danych, tworzona jest tablica trzech stosów, które reprezentują zawartościsłupków. Początkowo wszystkie n krążków jest odłożonych na stosie słupka wskaza-nego jako argument konstruktora. Ponadto klasa zawiera również wektor step list,który zapamiętuje kroki poczynione w trakcie rozwiązywania problemu. Lista prze-chowuje informacje o krążku, który jest przekładany, o słupkach początkowym i do-celowym oraz o liczbie krążków pozostałych na słupku początkowym.

1 def __init__(self , nr , rod):2 self.disks_number = nr3 self.starting_rod = rod # rod that contains disks

on start4 self.buffering_rod = (rod + 1) % 35 self.target_rod = (rod + 2) % 36

7 if self.starting_rod == LEFT:8 for i~in range (self.disks_number , 0, -1):9 self.left_rod.append(i)10 elif self.starting_rod == CENTER:11 for i~in range (self.disks_number , 0, -1):12 self.center_rod.append(i)13 else:14 for i~in range (self.disks_number , 0, -1):


15 self.right_rod.append(i)

Główną metodą klasy Hanoi jest solve hanoi. Funkcja ta rozwiązuje problemwież Hanoi implementując podręcznikowy algorytm rekurencyjny[16]. Jest to jedenz niewielu algorytmów, gdzie rekursja wydaje się naturalną, a przy tym efektywnądrogą do osiągnięcia zamierzonego celu. Funkcja działa następująco:

• Jeśli do przełożenia jest tylko jeden krążek, to zadanie jest trywialne i należyprzełożyć go ze słupka początkowego na końcowy. W metodzie solve hanoikażde przełożenie pozostawia odpowiedni wpis w liście kroków.

• Jeśli trzeba przełożyć n krążków (n 2) , to należy:

– przełożyć n - 1 górnych krążków na słupek pomocniczny

– przełożyć pozostały krążek na słupek docelowy

– przełożyć n - 1 krążków ze słupka pomocnicznego na docelowy

Patrząc na tak sformułowany algorytm rekurencyjny łatwo zauważyć, że liczba prze-łożeń potrzebnych do rozwiązania problemu wież Hanoi wyraża się wzorem:

L(n) = L(n− 1) + 1 + L(n− 1) (3.1)

Po prostym podstawieniu L′(n) = L(n) + 1 otrzymujemy:

L′(n) = 2L′(n− 1) = 2n (3.2)

Wracając z podstawieniem, ostateczna liczba przełożeń wynosi:

L(n) = 2n − 1 (3.3)

Zatem jest to problem o złożoności obliczeniowej klasy O(2n), czyli bardzo słabej.Niestety nie istnieje algorytm, na rozwiązanie problemu wież Hanoi, który byłby lep-szy. Między innymi z tego powodu w mojej pracy liczba krążków została ograniczonado 4.

W klasie Hanoi zaimplementowane zostały jeszcze dwie metody publiczne -show step list i get step list. Pierwsza w sposób graficzny w terminalu ilustruje ko-lejne etapy układania łamigłówki, a druga zwraca wektor z informacjami o przeło-żeniach poczynionych w trakcie wykonywania metody solve hanoi.

3.5.2 Klasa Irp6Hanoi

Główną klasą modułu decyzyjno-manipulacyjnego jest Irp6Hanoi. Implementa-cja klasy oraz funkcja testująca ją znajdują się w skrypcie irp6 hanoi.py pakietu ha-noi. Węzeł systemu ROS, który został zainicjowany przy uruchomieniu tego skryptujest subskrybentem tematów red disk i rods, do których publikują komponenty mo-dułu wizyjnego.

Klasa Irp6Hanoi dostarcza tylko dwie metody publiczne. Pierwsza z nich tomove to starting pose. Pozwala ona ustawić robota w pozycji wyjściowej do rozwią-zania problemu. Pozycja startowa zadawana jest w przestrzeni konfiguracyjnej i jestzapisana na stałe w pliku hanoi constants.py. We wspomnianym pliku znajdują sięwszystkie stałe wartości i parametry aplikacji, takie jak liczba krążków wież Hanoi,


Rysunek 3.4: Schemat blokowy rozwiązania problemu wież Hanoi

wymiary układanki, odległość chwytaka od czubków słupków czy długości ognisko-wych kamery. Kiedy już robot przesunie się do pozycji początkowej, możliwy stajesię odbiór danych z tematów modułu wizyjnego. Jeśli nie będzie wykryty czerwonyklocek i nie będą widoczne wszystkie słupki, wówczas program nie może rozwiązaćpostawionego problemu. W przeciwym przypadku może być użyta druga metoda -solve hanoi. Jest to jądro modułu decyzyjno-manipulacyjnego.

Działanie metody solve hanoi przedstawione jest w postaci schematu blokowegona rysunku 3.4.

Na początku ramię robota przesuwane jest w celu wykrycia dokładnych po-zycji słupków. Są one następnie zapamiętywane w odpowiednich polach obiektuIrp6Hanoi. Po uzyskaniu informacji o współrzędnych słupków i położenia począko-wego stosu możliwa jest wizualizacja aktualnego stanu i przejście do rozwiązywaniaproblemu.

Moduł decyzyjno-manipulacyjny wie już dokładnie, jakie jest ułożenie układankii powołuje do życia obiekt klasy Hanoi (podsekcja 3.5.1). Sformułowana lista kroków(w metodzie solve hanoi klasy Hanoi) zostaje zapamiętana i według tej listy będziepostępowała manipulacja robotem.

1 for i~in range(0, len(self.step_list)):2 self.shift_disk(self.step_list[i][0],

self.step_list[i][1], self.step_list[i][2],self.step_list[i][3])

Gdzie kolejne wartości metody shift disk to słupek początkowy, słupek końcowy,krążek i liczba krążków znajdujących się pod pobieranym. step list to lista krokówpozyskana z obiektu klasy Hanoi.

Metoda shift disk jest sekwencją czterech powtarzających się kroków. Na po-


czątku chwytak musi się przemieścić na pozycję dokładnie nad słupkiem. Następniepobierany jest dysk i przenoszony nad następny słupek. Sekwencja kończy się pre-cyzyjnym nałożeniem dysku na słupek końcowy. W międzyczasie aktualizowane sąinformacje dotyczące trzymania krążka i aktualnego położenia chwytaka. Potrzebneteż są przerwy pomiędzy ruchami, aby mieć pewność, że obraz otrzymywany z ka-mery jest stabilny i został przetworzony przez moduł wizualizacyjny, czego efektemjest wykrycie pozycji słupków.

1 def shift_disk(self , src , dst , disk , disks_below):2

3 rospy.sleep (2.) # to be sure that new rod is found4 self.move_to_rod(src)5 self.grab_disk(disk , disks_below , src)6

7 self.carrying_disk = 18 self.current_disk = disk9 self.rod_position[src] = self.irpos.get_joint_position ()10

11 self.move_to_rod(dst)12 self.leave_disk(disk , dst)13 self.carrying_disk = 0

3.5.3 Przemieszczenie chwytaka nad słupek

Pierwszym etapem zmiany położenia dysku jest odnalezienie i przesunięcie chwy-taka nad odpowiedni słupek. Informacja otrzymana od modułu wizyjnego w temacierods to wektor współrzędnych wykrytych czubków wyrażonych w pikselach w ukła-dzie kamery. Zatem dokładna odległość w przestrzeni operacyjnej robota zostajeokreślona w module decyzyjnym. Wszystkie te parametry znajdują się w pliku ha-noi constants.py.

Jeśli wektor współrzędnych jest pusty oznacza to, że nie widać żadnego ze słup-ków i rozwiązywanie wież Hanoi nie może być kontynuowane, co implikuje zakoń-czenie działania całego programu.

Pierwszy ruch robota, to przesunięcie chwytaka do pozycji, która została zapa-miętana dla danego słupka. Przyspiesza to znacznie etap przesuwania kiści. Jeżeliwieże Hanoi nie były przesunięte przez użytkownika lub w wyniku poprzedniegoprzeniesienia dysku, to eliminuje to potrzebę dalszych korekt położenia. Należy jed-nak sprawdzić, jaka jest relacja pozycji kiści i słupka.

Do wyliczenia dokładnego przesunięcia należy wstępnie posortować odebranewspółrzędne według rosnącej wartości odciętych. Słupki są wówczas ustawione w ko-lejności od lewego. Wybierane są koordynaty poszukiwanego słupka i wyliczane sądokładne odległości zgodnie ze wzorem:

x =dxfx∗ z =

xobr − cxfx

∗ z (3.4)

y =dyfy∗ z =

yobr − cyfy

∗ z (3.5)

Gdzie x i y to wyliczone przesunięcia w układzie operacyjnym w metrach, dx i dyto odległości punktu na obrazie od jego środka wyrażone w pikselach, xobr i yobr- współrzędne w układzie kamery wyrażone w pikselach, cx i cy - środek obrazu


Rysunek 3.5: Odległości pomiędzy kamerą a czubkami słupków

wyliczony w kalibracji, fx i fy - długości ogniskowych w osiach OX i OY kamery,z - odległość obiektywu od czubka słupka w osi OZ układu operacyjnego wyrażonaw metrach.

Wymienione wyżej wzory wynikają z zastosowania twierdzenia Talesa (rysu-nek 3.5). Na górze schematu symbolicznie przedstawione zostały obiektyw i matrycakamery.

Uzyskane w ten sposób przemieszczenie pozwala przesunąć się do punktu znaj-dującego się dokładnie na środku obrazu. Jednakże, ten punkt nie jest dokładniepod chwytakiem (nazwijmy go wektor A). Dlatego należałoby wyliczyć jeszcze je-den wektor przemieszczenia z punktu centralnego obrazu do punktu spod chwytaka(nazwijmy go wektor B), który jest przesyłany wraz z wektorem współrzędnych słup-ków. Istotą problemu, jest takie przemieszczenie chwytaka, aby dokładnie pod nimznajdował się słupek. Zatem poszukiwany wektor przesunięcia jest różnicą wekto-rów B i A. Powoduje to lekką modyfikacje wymienionych wyżej wzorów. Mianowiciewartości cx i cy zostają zastąpione współrzędnymi punkt znajdującego się dokładniepod chwytakiem.

Na tę chwilę, znany jest wektor przesunięcia chwytaka w układzie operacyjnymrobota. Nie pozostaje nic innego jak wywołać metodę move rel to cartesian poseklasy IRPOS. Po przemieszczeniu chwytaka ponownie sprawdzana jest odległośćkiści od wskazanego słupka. Jeśli dystans w osiach OX i OY nie przekracza jed-nego milimetra, to można uznać, że chwytaka znajduje się dokładnie nad czubkiem.Zapisywany jest również stan układu konfiguracyjnego manipulatora, co później bę-dzie wykorzystane przy przemieszczaniu chwytaka nad odwiedzony wcześniej słupek.W przeciwnym razie dokonywana jest korekta położenia kiści.


3.5.4 Pobranie krążka

Kiedy już chwytak znajdzie się w odpowiednim miejscu nad słupkiem, należypodjąć najwyżej położony na stosie krążek. Problem ten, wydawałoby się trywialny,jest komplikowany z powodu niedokładności wykonania wież Hanoi przez chińskiegoproducenta. Okazuje się, że krążek żółty, jako jedyny ma wysokość 1,18cm, podczasgdy pozostałe mają równo 1cm. Dlatego nie wystarczy informacja o tym, ile krążkówpozostanie na stosie, która jest dostarczana z listą kroków. Trzeba także sprawdzićwirtualnym modelu łamigłówki, który jest implementowany w module decyzyjnym,rozłożenie dysków.

Wysokość podjęcia krążka jest równa odległości chwytaka od podstawki orazpomniejszona jest o wysokość każdego krążka:

h = dp − db −n−1∑i=0

h(s(i)) (3.6)

Gdzie dp to odległość chwytaka od powierzchni taśmociągu, na którym stojąwieże, db - odległość od podstawki, h(s(i)) wysokość i -tego krążka w danym stosies.

Kod metody grab disk :1 def grabbing_height(self , disk , rod):2 hg = distance_to_conv - distance_to_base3

4 for i~in range (0, len(self.rod_content[rod]) - 1):5 if(self.rod_content[rod][i] == 2):6 hg = hg - 0.01187 else:8 hg = hg - 0.019

10 return hg

Następnie należy wywołać metodę move rel to cartesian pose with contactobiektu klasy IRPOS. Zastosowałem funkcję z wykryciem kontaktu końcówki, żebychwytak nie naciskał zbyt mocno na podstawkę, w przypadku osiągnięcia jej. Jeśliwykryty zostanie nacisk, robot przestaje poruszać się w dół. Po zakończeniu dzia-łania wspomniane metody chwytak jest podnoszony o 3mm w górę, aby podczasłapania dysku nie chwycił przez przypadek krążka, który jest jeden poziom niżej oddocelowego.

Chwytak jest już na odpowiedniej wysokości, więc można złapać dysk. W tymcelu użyłem metody tfg to joint position, która manipuluje rozstawem palców chwy-taka. W związku z typem zamontowanych przy chwytaku szczypiec, kontakt pomię-dzy nimi a krążkiem ma charakter punktowy. Dlatego też zdecydowałem się na chwytdomknięty siłowo (ang. force-enclosure grasp)[14]. Tak więc docelowa szerokość kiśćustawiana jest na średnicę danego krążka minus 3mm. Zapewnia to pewny chwytdysku (rysunek 3.6(a)) i gwarantuje, że dysk nie zostanie wypuszczony przedwcze-śnie ani zahaczenie krążkiem o czubek słupka nie obróci go.

Na koniec chwytak podnosi się do pozycji wyjściowej.

3.5.5 Przeniesienie krążka

Przemieszczenie chwytaka z krążkiem nad końcowy słupek byłoby identycznez przemieszczeniem bez krążka (podsekcja 3.5.3), gdyby nie fakt, że trzymany dysk


(a) Chwyt krążka domknięty siłowo (b) Widok z kamery podczas trzymaniakrążka przez chwytak

Rysunek 3.6: Chwytak trzyma krążek

zasłania część pola widzenia kamery (rysunek 3.6(b)). W takiej sytuacji, należyw jakiś inny sposób zidentyfikować wzajemne położenie kiści i słupka.

Pierwszy ruch robota, to przesunięcie do ostatniej zapamiętanej pozycji, w któ-rej chwytak był dokładnie nad docelowym słupkiem. Dalej kiść przemieszczana jesto 2,5cm w stronę użytkownika, tj. w osi OY układu operacyjnego robota, co zapewnipełną widoczność wież przez kamerę. Wówczas sprawdzane jest, czy w międzycza-sie nie występiło przesunięcie układanki. Jeśli wyliczona w tym momencie odległośćwynosi 2, 5 ± 0, 5cm w osi OX i 0 ± 0, 5cm w osi OY, to znaczy, że nie wystąpiłoprzesunięcie lub było ono niewielkie. Wtedy chwytak przemieszcza się od wektorwyliczony w funkcji compute move klasy irp6 hanoi. Natomiast jeśli przesunięciewykracza poza wymienione wyżej normy, wówczas po przemieszczeniu chwytaka dowyliczonej pozycji powtarzana jest sekwencja cofnięcia chwytaka o 2,5cm w osi OYukładu operacyjnego manipulatora i wyliczenia aktualnej odległości od szukanegosłupka. Sekwencja ta jest powtarzana aż do spełnienia podanego wyżej warunku. Ta-kie rozwiązanie dopuszcza sytuację, w której po pierwszym zatrzymaniu chwytaka,układanka wciąż jest przesuwana. Algorytm ten przedstawiony jest na schemacie 3.7.

3.5.6 Odłożenie krążka

Ostatnim etapem metody shift disk jest odłożenie krążka na stos. Do tego mo-mentu dysk został podjęty i przeniesiony dokładnie nad docelowy słupek.

Chwytak przenosi dysk w dół, tzn. wzdłuż osi OZ układu operacyjnego robota.Zastosowałem do tego metodę move rel to cartesian pose with contact Na wszelkiwypadek, gdyby się okazało, że dysk zacząłby naciskać na słupek z powodu nie tra-fienia na palik, zastosowałem funkcję z wykryciem kontaktu. Kiść przesuwa się w dóło 3cm, w wyniku czego krążek na pewno znajduje się poniżej czubka. W ogólnymprzypadku oznacza to poprawne nałożenie dysku na docelowy słupek.

Następnie należy wypuścić trzymany krążek. Palce chwytaka rozstawiane są naszerokość o 7mm większą niż średnica największego dysku. Dzięki temu w kolejnymkroku od razu można będzie złapać krążek bez kolejnej manipulacji palców chwytaka.Użyta metoda interfejsu IRPOS to tfg to joint position.

Na sam koniec chwytak powraca do poprzedniej pozycji, tzn. przesuwa się w góręwzdłuż osi OZ o 3cm. Dalej powtarzana od początku jest sekwencja ruchów opisa-


Rysunek 3.7: Schemat blokowy algorytmu przeniesienia krążka na docelowy słupek

3.6. MODUŁ WIZUALIZACYJNY 31

Rysunek 3.8: Program RViz z wizualizowanymi stanami robota i wież Hanoi

nych w podsekcjach 3.5.3 - 3.5.6 zgodnie z kolejnymi pozycjami listy kroków aż doukończenia rozwiązywania problemu.

3.6 Moduł wizualizacyjny

Ostatnim modułem, który zaimplementowałem w moim programie jest wizuali-zacja. Stworzona została w celu prezentacji aktualnego stanu robota i układanki.Wykorzystałem w tym zadaniu program RViz, który udostępniony jest wraz z sys-temem ROS.

Moduł jest zaimplementowany w postaci klasy VisualisationBuilder zapisanejw pliku vis builder.py. Dostarcza on 5 metod publicznych, które są wykorzystywanew module decyzyjno-manipulacyjnym. Komunikują się z programem RViz przez te-mat vis rviz. Model robota IRp-6 Postument został przygotowany wcześniej w Ze-spole Programowania Robotów i Systemów Rozpoznających, ja skonstruowałem mo-del wież Hanoi, który składa się z podstawki, krążków oraz słupków zwieńczonychróżowymi czubkami.

Orientacja i pozycja układanki wraz ze stosami krążków są aktualizowane pokażdorazowym podjęciu dysku i jego odłożeniu. Ponadto, kiedy chwytak trzymakrążek, jest to również uwzględniane w wizualizacji.

Obraz tworzony w programie RViz w celu wizualizacji działania mojej aplikacjiprzedstawiony jest na rysunku 3.8

3.6.1 Aktualizacja stanu wież Hanoi

Jak wspomniałem wcześniej, aktualizacja stanu układanki następuje zawszeprzed pobraniem i po odłożeniu krążka. Jest wtedy pewne, że widoczne są co naj-mniej dwa słupki i na tej podstawie możliwe jest zbudowanie całego modelu. Napoczątku należy zdobyć informacje o aktualnym położeniu chwytaka. Dzięki temuprogram może zorientować się, który słupek jest na środku kamery i pozostałe.W przypadku, gdy kiść znajduje się nad jednym ze skrajnych słupków, widziane są


tylko dwa - aktualny i środkowy. Następnie obliczany jest kąt pod jakim ułożonewzględem układu kamery położona jest układanka. Posłużyłem się tu funkcją arcustangens, podając jako argument iloraz różnic współrzędnych w osiach OY i OX.Znając dokładną pozycję aktualnego słupka oraz kąt obrotu układanki wokół niego,a także wymiary wież określane są odległości pomiędzy słupkami, czego efektem jestaktualizacja danych w obiekcie klasy VisualisationBuilder. Następnie kopiowana jestzawartość modelu stosów krążków z modułu decyzyjnego-manipulacyjnego do mo-dułu wizualizacyjnego. Na bazie wymienionych wyżej informacji tworzony jest ob-raz aktualnego stanu układanki w programi RViz. Służą do tego zaimplementowaneprzeze mnie funkcje rysujące słupki, krążki oraz podstawkę.

3.6.2 Podejmowanie i upuszczanie krążka

Kolejnym problemem, jaki napotkałem przy budowaniu modułu wizualizacyj-nego, jest podnoszenie i wypuszczanie krążka przez chwytak. Po zaciśnięciu siępalców chwytaka położenie dysku względem kiści jest stałe. Dlatego zastosowanyzostała tu blokada kadru, czyli wysłanie wiadomości typu Marker z parametremframe locked (opisany w podsekcji 2.2.2). Przez cały czas przenoszenia krążka, jegopozycja jest aktualizowana zgodnie z położeniem chwytaka, zachowując odpowiedniąodległość. Następnie, kiedy dysk jest upuszczany, marker przestaje być powiązanyz układem odniesienia kiści. Krążek pojawia się wówczas na szczycie stosu aktual-nego słupka pamiętając (w modelu układanki modułu wizualizacyjnego), jaka jestjego zawartość. Dzięki temu krążek rysowany jest na odpowiedniej wysokości.

Rozdział 4

Podsumowanie

Ostatni rozdział pracy przedstawia warunki oraz wyniki testowania aplika-cji(sekcja 4.1) oraz wyciągnięte wnioski(sekcja 4.2).

4.1 Testy oprogramowania

Wszystkie testy oprogramowania przeprowadzane były w Laboratorium Robo-tyki na rzeczywistym manipulatorze robotycznym IRp-6 Postument. Kolejność im-plementacji modułów jest taka sama, jak kolejność sekcji w rozdziale poświęconymrealizacji aplikacji. Całe oprogramowanie było tworzone w rozwiązaniu programi-stycznym top - down. Na początku ustalone zostałe podstawowe zadania, które mu-szą zostać spełnione, aby wykonać przełożenie krążków ze słupka początkowego nakońcowy. Dalej te zadania były dzielone na coraz mniejsze, aż powstawały pojedyn-cze metody spełniające dane wymaganie. Każda z funkcji była następnie testowanapod kątem poprawności działania.

4.1.1 Testy modułu wizyjnego

Testowanie modułu wizyjnego polegało na sprawdzeniu wykrycia słupków i czer-wonego krążka w różnych warunkach. Przykładowe warunki testowe:

• Zmiana oświetlenia sztucznego w Laboratorium

• Zmiana oświetlenia słonecznego - podczas pełnego zachmurzenia, przy czystymniebie itd.

• Zmiana położenia układanki względem kamery i pozycji wyjściowej do rozwią-zania problemu

• Wykrycie elementów układanki podczas ruchu chwytaka

• Położenie ręki obok wież

W implementacji moich komponentów systemu DisCODe zastosowałem modelprzestrzeni barw HSV celem wyeliminowania wpływu oświetlenia na wykrywalność

33

34 ROZDZIAŁ 4. PODSUMOWANIE

obiektów. O ile sztuczne oświetlenie nie stanowi problemu dla mojego oprogramo-wania, to zmiana oświetlenia słonecznego decyduje o znalezienie poszukiwanych ele-mentów. Przy silnym południowym słońcu, którego promienie wciąż nie padają bez-pośrednio na wieże Hanoi, wymagana była zmiana zakresu barw w operacji progo-wania.

Kolejnym problemem, było wykrycie wspomnianych wyżej obiektów podczasruchu końcówki. Otóż powodzenie detekcji była ciężkie do przewidzenia. Ponadtoakwizycja obrazu i jego obróbka prowadząca do wyznaczenia pozycji elementówukładanki nie spełniały twardych ograniczeń czasowych i okreso opóźnienia wahałsię, momentami do 1,5s. Z tego powodu zdecydowałem się po każdym ruchu chwy-taka zatrzymywać czynności na okres 2s, aby mieć pewność, że obraz, który poddanybędzie obróbce będzie już ustabilizowany i nie nastąpią problemy z wykryciem słup-ków.

Okazało się również, że czasem fragmenty skóry dłoni pokrywały się z różowymkolorem czubków słupków, dlatego nie należy kłaść ręki na dłuższy czas nieopodalukładanki, gdyż może to przeszkodzić aplikacji w poprawnym rozwiązaniu problemuwież Hanoi.

4.1.2 Testy modułu decyzyjno-manipulacyjnego

Moduł decyzyjno-manipulacyjny jest jądrem całego oprogramowania, dlatego na-leżało poświęcić wiele czasu na testowanie poszczególnych metod. Wszystkie scena-riusze testowe, podyktowane były przeznaczeniem funkcji i oczekiwanych rezulta-tów. Ciężko było wymyślić tu jakieś szczególne warunki. Sprawdzone zostałe takieelementy jak:

• zakres rozstawu palców chwytaka

• jakość i pułap chwytu krążków

• głębokość nakładania dysku na słupek

• dokładność obliczeń przemieszczeń

• komunikacja z innymi modułami przez tematy systemu ROS

Wszystkie testy przeszły pomyślnie, czego efektem jest poprawnie działającaaplikacja.

4.1.3 Testy modułu wizualizacyjnego

Zadaniem wizualizacji jest przedstawienie rzeczywistej sytuacji robota i wieżHanoi na ekranie monitora. Zatem poprawność działania modułu sprawdzana jestpoprzez ich porównanie. Należy tu zwrócić szczególną uwagę na:

• stosunek położenia chwytaka i elementów układanki

• kąt obrotu podstawki

• dokładność nakładania krążka na słupek końcowy

4.2. WNIOSKI 35

Jedynie ostatni punkt nie jest spełniony w stu procentach - zdarza się, że środekpodstawy krążka i środek podstawy słupka nie pokrywają się. Wynika to z nie-wielkich błędów w operacji usuwania dystorsji oraz błędu pralaksy, który występujepodczas określania położenia słupków względem chwytaka. Jednakże, omawiana nie-dokładność jest niewielka i w małym stopniu wpływa na jakość wizualizacji.

4.2 Wnioski

Na wstępie należy zauważyć, że osiągnięto zakładany cel pracy. Stworzone opro-gramowanie w pełni pozwala rozwiązać problem wież Hanoi zgodnie ze wszystkimizałożeniami. Działanie aplikacji było również wielokrotnie prezentowane podczas dniotwartych wydziału.

Jednakże, warto przemyśleć, jakie udoskonalenia mogłyby pozytywnie wpłynąćna oprogramowanie. Po pierwsze należy zwrócić uwagę na szybkość wykonywaniaruchów, która wydaje się być niezadowalająca. Można przyspieszyć ruchy robota,pamiętając o nałożonych ograniczeniach. Wówczas należałoby obliczać odległość,jaką musi pokonać końcówka chwytaka, i wyznaczać czas wykonania ruchu w oparciuo przyjętą szybkość maksymalną. Dla takiego rozwiązania wymagana jest bardzowysoka precyzja wyznaczania pozycji słupków oraz jak najmniejszy, najlepiej dążącydo zera, odsetek błędnie wykrytych obiektów. Pozwoli to zachować bezpieczeństwoużytkowania robota.

Kolejną niedogodnością przyjętego rozwiązania jest niepełna odporność systemuwizyjnego na warunki oświetlenia słonecznego. W tym wypadku można byłoby za-stosować automatyczną kalibrację balansu bieli, która odciąży użytkownika z obo-wiązku ręcznego, doświadczalnego dobierania wartości zakresu barwy w odebranymobrazie przekonwetowanym do modelu HSV.

Warto również zastanowić się, czy można jakoś zabezpieczyć aplikację przed za-bieraniem krążków przez użytkownika. Podobnie rzecz ma się, kiedy źle nałożonykrążek spadnie obok słupka - nie jest to wykrywane przez aplikację. W przypadkukiedy dysk zostanie zabrany z układanki (lub nie trafi na słupek) w trakcie wykony-wania programu, model nie uwzględnia zniknięcia i prowadzi to dalej do niepopraw-nej manipulacji. Należałoby dokonywać detekcji wszystkich krążków. Miałoby toznaczący wpływ na długość rozwiązywania problemu, ale możliwe byłoby wykryciebłędnych stanów układanki względem modelu programu.

36 ROZDZIAŁ 4. PODSUMOWANIE

Bibliografia

[1] Oficjalna strona API OpenCV. http://docs.opencv.org/2.4/modules/refman.html.

[2] Oficjalna strona firmy LC Security. http://www.lc-security.pl.

[3] Oficjalna strona firmy Point Grey. http://www.ptgrey.com.

[4] Oficjalna strona OpenCV. http://opencv.org.

[5] Oficjalna strona programu RViz. http://wiki.ros.org/rviz.

[6] Oficjalna strona systemu OROCOS. http://www.orocos.org.

[7] Oficjalna strona systemu ROS. http://www.ros.org.

[8] Oficjalna strona Zakładu Programowania Robotów i Systemów Rozpoznają-cych. http://www.robotyka.ia.pw.edu.pl.

[9] Wikipedia - Wieże Hanoi. http://pl.wikipedia.org/wiki/Wie%C5%BCe_Hanoi.

[10] H. Bruyninckx, P. Soetens, B. Koninckx. The real-time motion control coreof the orocos project. Robotics and Automation, 2003. Proceedings. ICRA ’03.IEEE International Conference on, wolumen 2, strony 2766–2771 vol.2, Sept2003.

[11] Andrzej Rusiecki Ewaryst Rafajłowicz, Wojciech Rafajłowicz. Algorytmy prze-twarzania obrazów i wstęp do pracy z biblioteką OpenCV. Oficyna WydawniczaPolitechniki Wrocławskiej, 2009.

[12] Tomasz Kornuta, Maciej Stefańczyk. DisCODe: a component oriented frame-work for procesing of sensory data (in Polish). Pomiary Automatyka Robotyka,16(7-8):76–85, 2012. (DisCODe: komponentowa struktura ramowa do przetwa-rzania danych sensorycznych).

[13] Tomasz Kornuta, Maciej Stefańczyk, Michał Laszkowski, Maksym Figat, JanFigat, Cezary Zieliński. Asynchronous data flow handling in component-basedrobot perception subsystems (in Polish). Pomiary – Automatyka – Robo-tyka PAR, 18(5):127–133, 2014. Obsługa asynchronicznego przepływu danychw komponentowych podsystemach percepcji robotów.

[14] Wojciech Szynkiewicz. Rozszerzony konspekt wykładu Wstęp do robotyki.

37

38 BIBLIOGRAFIA

[15] T. Winiarski, C. Zieliński. Podstawy sterowania siłowego w robotach. PomiaryAutomatyka Robotyka, 12(6):5–10, Czerwiec 2008.

[16] Piotr Wróblewski. Algorytmy. Struktury danych i techniki programowania. He-lion, 2009.

Documents

Układanie wież Hanoi z wykorzystaniem robota ... · Robot, który traﬁł do Laboratorium Robotyki wymagał kilku korekt pozwala-jących na wykorzystanie go do badań laboratoryjnych[15]