Rok akademicki 2007/2008

Przemysław Piłaciński

nr indeksu 185576

Praca dyplomowa inżynierska

Manipulacja obiektami przez roboty

opiekun naukowy: dr inż. W. Szynkiewicz

Warszawa, styczeń 2008

1

Streszczenie

W ramach niniejszej pracy zrealizowano algorytm manipulacji dwurobotowej

polegającej na przelaniu wody z butelki do kubka. Pozycje i wymiary obiektów były

znane. Powstała aplikacja w systemie MRROC++ realizująca ten algorytm. Na potrzeby

zadania manipulacji powstał generator trajektorii płynnego ruchu manipulatora, który

znalazł zastosowanie również w innych zadaniach.

Słowa kluczowe: robotyka, manipulacja, generacja trajektorii, MRROC++

Abstract

Title: „Object manipulation with robots”

This thesis concerns the problem of two-handed robotic manipulation. The thesis

describes the algorithm for the task of pouring water from a bottle to a cup using two

IRp-6 robots. The poses and dimensions of the objects were known. An application

performing this task was written in MRROC++. For the use of this task, a trajectory

generator for smooth robot moves was created. The generator was also used in other

tasks.

Key words: robotics, manipulation, trajectory generation, MRROC++

2

Spis treści:

1 Wstęp............................................................................................................................5

1.1 Cel i zakres pracy...................................................................................................5

1.2 Przyjęte założenia..................................................................................................5

1.3 Zawartość pracy.....................................................................................................5

2 Manipulacja – przegląd wybranych zagadnień.......................................................7

2.1 Opis problemu........................................................................................................7

2.2 Wybrane podejścia do manipulacji........................................................................7

2.2.1 Manipulacja z użyciem wizji.........................................................................7

2.2.2 Manipulacja w środowiskach ludzkich.........................................................8

3 Opis zadania manipulacji dwuręcznej....................................................................11

3.1 Sformułowanie zadania przelewania...................................................................11

3.1.1 Przyjęte założenia........................................................................................12

3.1.2 Algorytm manipulacji dwuręcznej..............................................................13

3.2 Generator trajektorii dla płynnego ruchu manipulatora.......................................20

3.2.1 Przyjęte założenia........................................................................................20

3.2.2 Opis algorytmu generatora płynnego ruchu................................................20

3.2.2.1 Określenie drogi w pojedynczej współrzędnej...................................21

3.2.2.2 Określenie modelu ruchu....................................................................21

3.2.2.3 Określenie poszczególnych czasów ruchu i czasu całego ruchu........25

3.2.2.4 Określenie prędkości i przyspieszeń dla kolejnych współrzędnych...27

3.2.2.5 Obliczanie położeń w kolejnych makrokrokach.................................29

4 Realizacja zadania manipulacji...............................................................................31

4.1 System MRROC++..............................................................................................31

4.1.1 Opis pozycji końcówki manipulatora w MRROC++..................................31

4.1.2 Opis systemu robotycznego.........................................................................31

4.1.3 Struktura sterownika....................................................................................32

4.1.4 Generatory trajektorii..................................................................................33

4.1.5 Funkcja Move.............................................................................................34

4.2 Implementacja zadania manipulacji.....................................................................34

3

4.3 Implementacja generatora płynnego ruchu..........................................................37

4.3.1 Klasa ecp_smooth_generator...........................................................37

4.3.1.1 Hierarchia klas................................................................................37

4.3.1.2 Zmienne klasy ecp_smooth_generator...............................39

4.3.1.3 Metody klasy ecp_smooth_generator.................................40

4.3.1.3.1 Metody związane z obsługą listy pozycji..............................40

4.3.1.3.2 Metody wczytujące dane z plików.........................................41

4.3.1.3.3 Konstruktory..........................................................................42

4.3.1.3.4 Pomocnicze funkcje obliczeniowe.........................................43

4.3.1.3.5 Metoda first_step..........................................................43

4.3.1.3.6 Metoda next_step.............................................................43

4.3.1.3.7 Wyświetlanie informacji o przebiegu ruchu..........................47

4.3.2 Klasa ecp_smooth_taught_in_pose..........................................49

5 Eksperymenty i wyniki testów.................................................................................51

5.1 Testy generatora płynnego ruchu.........................................................................51

5.1.1 Ruch do punktu docelowego bez punktów pośrednich...............................51

5.1.2 Ruch do punktu docelowego z jednym punktem pośrednim,

niezerowymi prędkościami w punkcie pośrednim i jednakową

zadaną prędkością ruchu do obu punktów.......................................................53

5.1.3 Ruch do punktu docelowego z jednym punktem pośrednim,

niezerowymi prędkościami w punkcie pośrednim i różnymi

zadanymi prędkościami ruchu.........................................................................58

5.1.4 Wnioski.......................................................................................................61

5.2 Zastosowanie generatora płynnego ruchu w zadaniu układania kostki Rubika. .61

5.2.1 Modyfikacja plików z trajektoriami............................................................62

5.2.2 Zalety zastosowania generatora płynnego ruchu.........................................62

5.2.3 Wnioski.......................................................................................................62

5.3 Testy algorytmu zadania przelewania..................................................................62

5.3.1 Wnioski.......................................................................................................68

6 Podsumowanie............................................................................................................73

Literatura..................................................................................................................75

4

Rozdział 1

Wstęp

1.1. Cel i zakres pracy

Celem pracy jest opracowanie, implementacja i eksperymentalna weryfikacja

algorytmów manipulacji dwuręcznej w zadaniu polegającym na przelewaniu płynu z

butelki do kubka. Do realizacji zadania zastosowano system dwurobotowy składający się

z dwóch manipulatorów IRp-6. Sterownik systemu napisano z wykorzystaniem

programowej struktury ramowej MRROC++.

1.2. Przyjęte założenia

W zadaniu wykorzystywane są odpowiednio dobrane obiekty. Ich wymiary i

początkowe położenia są znane. Ruchy manipulatorów w poszczególnych fazach

manipulacji wykonywane są z użyciem opracowanego w niniejszej pracy generatora

ruchu. Zadawane pozycje nie podlegają zmianie w trakcie wykonywania zadania.

Zadanie wykonywane jest z użyciem sieci komputerów PC, pracujących pod kontrolą

systemu czasu rzeczywistego QNX 6.3.

1.3. Zawartość pracy

Niniejsza praca składa się z sześciu rozdziałów.

W rozdziale drugim dokonano przeglądu wybranych zagadnień na temat manipulacji.

Omówiono w nim pojęcie manipulacji, przedstawiono przykłady zadań manipulacji z

literatury i problemy napotykane w trakcie realizacji takich zadań.

Rozdział trzeci poświęcono opisowi zadania manipulacji dwuręcznej, polegającego na

5

przelaniu płynu z butelki do kubka. Przedstawiono w nim przyjęte założenia,

sformułowanie zadania i algorytm jego rozwiązania. Następnie opisano algorytm

działania generatora ruchu opracowanego w niniejszej pracy.

W rozdziale czwartym opisano realizację zadania manipulacji. W pierwszym

podrozdziale przedstawiono system MRROC++ – strukturę sterownika i system

robotyczny. Omówiono pojęcie generatora trajektorii i zasady działania generatorów w

systemie MRROC++. W kolejnych podrozdziałach opisano implementację generatora

ruchu i samego zadania manipulacji.

Rozdział czwarty zawiera opis eksperymentów i wyniki badań. Opisano w nim testy

generatora ruchu dla różnych zadanych trajektorii, efekty użycia tego generatora w

zadaniu układania kostki Rubika oraz wyniki testów algorytmu zadania przelewania.

W rozdziale szóstym zamieszczono podsumowanie pracy.

6

Rozdział 2

Manipulacja – przegląd wybranych zagadnień

2.1. Opis problemu

Problem manipulacji obiektami przez roboty można zdekomponować do

następujących podzadań:

1. Planowanie trajektorii ruchu

2. Planowanie chwytów

3. Przetwarzanie danych z czujników

W niniejszej pracy skupiono się na problemie planowania trajektorii ruchu.

2.2. Wybrane podejścia do manipulacji za pomocą robotów

Poniżej przedstawiono krótki przegląd kilku spośród podejść do manipulacji,

zawartych w dostępnych pracach, z naciskiem na napotykane problemy i opis

przykładowych zadań manipulacji.

2.2.1. Manipulacja z użyciem wizji

W pracy [GT06], skupiającej się na manipulacji z użyciem wizji, zawarto opis trzech

zadań manipulacji. Pierwszym jest chwytanie nieznanego obiektu, drugim

przesypywanie ryżu z jednego pojemnika do drugiego, a trzecim chwycenie kubka z

etanolem, odróżnianego od podobnego kubka zawierającego wodę na podstawie zapachu.

Wykorzystywane są algorytmy identyfikacji obiektów na podstawie obrazu z kamery

7

oparte na rozpoznawaniu kształtów i kolorów. W planowaniu trajektorii ruchu końcówki

do punktu chwytania obiektu uwzględnia się punkt pośredni, dobierany tak, aby nie

nastąpiła kolizja z obiektem. Przykład trajektorii z punktem pośrednim przedstawiono na

rysunku poniżej. Od momentu gdy końcówka znajdzie się w polu widzenia kamery

dokonywana jest korekcja trajektorii w czasie rzeczywistym na podstawie obrazu

z kamery.

W zadaniu przesypywania, ruch przesypywania jest zadawany jako zaplanowana

wcześniej trajektoria, przesunięta z uwzględnieniem pozycji miski. Kubek jest

umiejscawiany dokładnie na środku miski, nieco ponad nią w pozycji pionowej, a

następnie obracany o 120º i systematycznie obniżany.

2.2.2. Manipulacja w środowiskach ludzkich

Problematyki manipulacji za pomocą robotów w typowych dla ludzi środowiskach

dotyczy praca [AE07]. Skupia się ona na opisaniu cech konstrukcyjnych humanoidalnego

robota, który ma działać w ludzkim środowisku i metod umożliwiającym mu

wykonywanie typowo ludzkich zadań na przykładzie wykorzystywanego w badaniach

robota Domo.

W pracy omówiono cztery zadania manipulacji: przekładanie cylindrycznego obiektu

8

Rysunek 1: Trajektoria z punktem pośrednim

z jednej ręki robota do drugiej, dwuręczne trzymanie obiektu (pudełka), mieszaniu łyżką

w kubku i odkładanie przedmiotów na półkę.

Autorzy podkreślają zasadność korzystania z obrazu z kamery nawet w zadaniach

z pozoru tego nie wymagających. Zadanie przekładania zostało wykonane zarówno

z użyciem kamery, jak i bez żadnych czujników. Wykazano, że drobne różnice w

sposobie uchwycenia obiektu i błędy kalibracji mogą prowadzić do kolizji chwytaka

z chwytanym obiektem. Ponadto jeśli szerokość szczęk chwytaka jest bliska wymiarowi

chwytanego obiektu, prawdopodobieństwo kolizji jest duże.

Zadanie mieszania łyżką w kubku wykonano jako zadanie manipulacji dwuręcznej.

Trzymanie drugiego z obiektów (kubka) przez robota, w odróżnieniu od przypadku, gdy

zadanie wykonywane jest jednoręcznie, a kubek stoi na powierzchni, daje większą

możliwość kontrolowania przebiegu zadania i większą elastyczność w jego

opracowywaniu, ze względu na brak problemów z potrąceniem kubka. Wykonanie

zadanie za pomocą dwóch ramion zwiększa też jednakże niepewność, związaną z

położeniem obiektów względem siebie.

9

10

Rozdział 3

Opis zadania manipulacji dwuręcznej

3.1. Sformułowanie zadania przelewania

Realizowane w ramach niniejszej pracy zadanie manipulacji polega na przelaniu płynu

z jednego naczynia (butelki) do drugiego (kubka) za pomocą dwóch manipulatorów

IRp-6, wyposażonych w chwytaki o równoległych szczękach.

W zadaniu wykorzystywane są dwa manipulatory IRp-6. Pierwszy z nich (nazywany

dalej: „OnTrack”) ma 7 stopni swobody – w tym tor jezdny. Drugi manipulator

(nazywany dalej: „Postument”) posiada 6 stopni swobody.

Rysunek 2: Robot IRp-6 "OnTrack" Rysunek 3: Robot IRp-6 "Postument"

11

Obiekty wykorzystywane w zadaniu to metalowa butelka z podstawą o średnicy 6cm

i o wysokości 26cm, oraz metalowy kubek z podstawą o średnicy 7cm i o wysokości

8,5cm. Obiekty są dodatkowo oklejone gąbką. Przelewanym płynem jest woda.

Rysunek 4: Butelka i związany z nią układ współrzędnych

Rysunek 5: Kubek i związany z nim układ współrzędnych

Ruchy manipulatorów odbywają się w dwóch rodzajach układów współrzędnych:

współrzędnych wewnętrznych i współrzędnych zewnętrznych z opisem orientacji

końcówki za pomocą kątów Eulera Z-Y-Z [CC93].

Planowanie chwytów w zadaniu sprowadza się do planowania trajektorii, gdyż

położenia i rozmiary obiektów są znane, a chwytak jest bardzo prosty. Chwyty

ograniczają się do ustawienia końcówki chwytającej w odpowiedniej pozycji

i zaciśnięcia szczęk chwytaka. Również z powodu znanego a priori położenia

i rozmiarów obiektów nie wykorzystuje się do ich określenia danych z czujników.

3.1.1. Przyjęte założenia

W chwili początkowej oba naczynia znajdują się w znanych pozycjach początkowych,

12

a manipulatory – w pozycjach synchronizacji. Zawartość płynu w naczyniach jest

uprzednio znana. Wymiary obiektów są znane, podobnie środowisko pracy robotów, w

którym nie następują żadne dynamiczne zmiany.

Ze względu na używanie chwytaków o specyficznym kształcie szczęk, dedykowanych

do innego zadania (układania kostki Rubika) została dokonana modyfikacja obiektów

manipulacji przez obłożenie ich gąbką. W przeciwnym przypadku butelka i kubek

wyślizgiwałyby się z chwytaków.

Manipulacja wykonywana jest przez realizację trajektorii do zapisanych w plikach

pozycji, które zostały uprzednio wyznaczone podczas ręcznego sterowania robotami.

3.1.2. Algorytm manipulacji dwuręcznej

Zadanie zostało podzielone na dziewięć etapów. Kolejne ruchy manipulatorów

przedstawiono na rysunkach na kolejnych stronach. Docelowe położenia od momentu

chwycenia obiektów oznaczono w układach współrzędnych związanych z obiektami.

Wszystkie trajektorie obliczane są na podstawie równań z podrozdziału 3.2. Poniżej

przedstawiono sieć działań algorytmu.

13

Rysunek 6: Sieć działań algorytmu manipulacji

14

1) W pierwszym etapie oba manipulatory przemieszczają z pozycji synchronizacji Q1,0

i Q2,0 w pobliże obiektów i ich chwytaki przyjmują odpowiednią orientację do chwytów,

realizując ruchy do zapisanych w plikach pozycji Q1,1 i Q2,1. Indeksy w oznaczeniach

punktów odpowiadają kolejno za numer robota (1 – OnTrack, 2 – Postument) i numer

punktu. Obliczane są następujące trajektorie:

π 1=q1,1 t ,q1,2 t ,q1,3 t ,q1,4 t ,q1,5 t , q1,6 t , q1,7 t dla robota OnTrack π 14=q2,1 t ,q2,2 t , q2,3 t ,q2,4 t , q2,5t ,q2,6 t dla robota Postument, gdzie q1,n jest n-tą współrzędną wewnętrzną robota OnTrackq2, n jest n-tą współrzędną wewnętrzną robota Postument

Ruch odbywa się we współrzędnych wewnętrznych, ze stosunkowo dużą prędkościąv w , d .

2) Drugim etapem jest pozycjonowanie chwytaków względem obiektów i chwycenie

obiektów. Ruch do punktów P1,2 i P2,2 odbywa się we współrzędnych zewnętrznych, co

umożliwiło łatwy dobór punktów docelowych przez przesunięcie względem punktów, w

których zakończono etap pierwszy. Ponadto ważne jest w tym etapie zachowanie stałej

orientacji końcówek manipulatorów i ruch z małą prędkością v z , m , aby nie przewrócić

obiektów w wypadku ich potrącenia. Obliczane są trajektorie:

p2= x1 t , y1 t , z1 t ,α1 t , β1 t , γ1 t dla robota OnTrack p15= x1 t , y1 t , z1 t , α1 t , β1 t , γ1 t dla robota Postument, gdziex1, y1, z1 to współrzędne kartezjańskie położenia końcówki robota OnTrackx2, y2, z2 to współrzędne kartezjańskie położenia końcówki robota Postumentα1, β1, γ1 to orientacja końcówki robota OnTrack w postaci kątów Eulera Z-Y-Zα2, β2, γ2 to orientacja końcówki robota Postument w postaci kątów Eulera Z-Y-Z

Po zakończeniu ruchu naczynia są chwytane poprzez zaciśnięcie szczęk chwytaków.

Realizacja dwóch trajektorii z etapu pierwszego i drugiego odpowiada modelowi ruchu z

punktem pośrednim, omawianemu w podrozdziale 2.2.1. Pierwszy i drugi etap

przedstawiono na rysunku 3.

15

Rysunek 7: Pierwszy i drugi etap manipulacji - chwycenie obiektów

3) W trzecim etapie naczynia są podnoszone. Obliczane są trajektorie dla współrzędnych

zewnętrznych:

p3= x1 t , y1 t , z1 t ,α1 t , β1 t , γ1 t

p16= x1 t , y1 t , z1 t ,α1 t , β1 t , γ1 t

Trajektorie te realizują ruch do punktów P1,3 i P2,3.

Rysunek 8: Trzeci etap manipulacji - podniesienie obiektów

4) W czwartym etapie manipulatory ustawiają się względem siebie tak, aby umożliwić

przelanie płynu z butelki do kubka (punkty P1,8 i P2,4). Jest to realizacja trajektorii:

16

p16= x1 t , y1 t , z1 t ,α1 t , β1 t , γ1 t

Ruch robota OnTrack odbywa się częściowo we współrzędnych wewnętrznych

(trajektoria π 5=q1,1 t ,q1,2 t ,q1,3 t ,q1,4 t , q1,5 t , q1,6 t , q1,7 t do punktu Q1,7,

będącego odwzorowaniem punktu P1,7 we współrzędnych wewnętrznych), ponieważ we

współrzędnych zewnętrznych nie jest możliwe przemieszczenie robota na torze jezdnym,

a częściowo we współrzędnych zewnętrznych (trajektoria z punktami pośrednimi

P1,4 i P1,5 p4= x1 t , y1 t , z1 t ,α1 t , β1 t , γ1 t do punktu P1,6 oraz trajektoria p6= x1 t , y1 t , z1 t ,α1 t , β 1 t , γ1 t do punktu P1,8). Ze względu na wykonywanie

ruchów we współrzędnych zewnętrznych (aby nie zmieniać orientacji końcówki

manipulatora trzymającego pełną butelkę, co mogłoby spowodować rozlanie płynu)

i ograniczenia przestrzeni roboczej manipulatora trajektoria z punktu P1,3 do punktu P1,6

musiała zostać określona z punktami pośrednimi P1,4 i P1,5. Ze względu na czytelność

rysunku pominięto na nim punkty P1,4 i P1,5 oraz punkt P1,6.

Rysunek 9: Czwarty etap manipulacji - zetknięcie obiektów

5) W piątym etapie następuje przelanie płynu. Wykonywany jest ruch przechylający

butelkę przy zachowaniu stałego położenia jej końca, co jest zapewniane przez zmianę

definicji końcówki, za którą przyjęty zostaje wylot butelki (punkt P1,9). Umożliwia to

zadanie bardziej złożonego z pozoru ruchu jako prosty obrót wokół jednej osi (osi y w

przyjętym na rysunku układzie współrzędnych). Ruch ten opisuje trajektoriap7= x1 t , y1 t , z1 t ,α1 t , β 1 t , γ1 t . Następnie wykonywany jest ruch

przywracający butelkę do pionu (przez punkt P1,9 do punktu P1,7).

Ruch ten opisuje trajektoria p8= x1 t , y1 t , z1 t , α1 t , β1 t , γ1 t . Manipulator

17

trzymający kubek pozostaje w tym etapie nieruchomy w punkcie P2,4.

Rysunek 10: Piąty etap manipulacji – przelewanie (kolejne pozycje)

6) W szóstym etapie roboty powracają do pozycji, z etapu trzeciego (Punkty P1,3 i P2,3).

Robot OnTrack wykonuje ruch kolejno do punktów P1,6, P1,5, P1,4 i P1,3. Robot Postument

wykonuje ruch do punktu P2,3. Realizowane są trajektorie:

p10= x1 t , y1 t , z1 t ,α1 t , β 1 t , γ1 t dla robota OnTrackp17= x1 t , y1 t , z1 t , α1 t , β1 t , γ1 t dla robota Postument

Rysunek 11: Szósty etap manipulacji - powrót do pozycji nad pozycją chwytania

7) W siódmym etapie naczynia są odstawiane na początkowe pozycje. Odbywa się to

poprzez ruch we współrzędnych zewnętrznych do punktów P1,2 i P2,2. Następnie obiekty

są puszczane przez manipulatory. Realizowane są trajektorie:

18

p11= x1 t , y1 t , z1 t , α1 t , β1 t , γ1 t dla robota OnTrackp18= x1 t , y1 t , z1 t , α1 t , β1 t , γ1 t dla robota Postument

8) W ósmym etapie manipulatory wycofują się do punktów P1,1 i P2,1. Ruchy opisane są

trajektoriami:

p12= x1 t , y1 t , z1 t , α1 t , β1 t , γ1 t dla robota OnTrackp19= x1 t , y1 t , z1 t , α1 t , β1 t , γ1 t dla robota Postument

9) W dziewiątym etapie roboty przyjmują pozycje synchronizacji. Ruch realizowany jest

we współrzędnych wewnętrznych i jest realizacją trajektorii:

π 13=q1,1 t ,q1,2 t ,q1,3 t ,q1,4 t , q1,5 t , q1,6 t , q1,7 t dla robota OnTrack π 20=q2,1 t , q2,2 t ,q2,3 t , q2,4 t ,q2,5 t ,q2,6 t dla robota Postument

19

3.2. Generator trajektorii dla płynnego ruchu manipulatora

3.2.1. Przyjęte założenia

Celem jest opracowanie generatora ruchu, generującego możliwie gładkie trajektorie

zapewniające płynność ruchu.

Zadaniem generatora jest obliczenie trajektorii do kolejnych zadanych punktów

w określonym układzie współrzędnych. Ponieważ w pewnych przypadkach drogi

w poszczególnych współrzędnych mogą znacząco się różnić, gdyby ruch odbywał się

w każdej współrzędnej z maksymalną prędkością, powodowałoby to szkodliwe dla

manipulatora szarpnięcia, a także mogłoby utrudniać manipulację. W wykonywanym

zadaniu nalewania płynu z jednego naczynia do drugiego wszelkie szarpnięcia mogą

powodować rozlanie płynu. Istotnym elementem działania generatora jest więc takie

dobranie trajektorii aby ruch przebiegał płynnie mimo różnic w drogach, dopuszczalnych

prędkościach i przyspieszeniach dla różnych współrzędnych.

Dotychczas generatory trajektorii w MRROC++ wykorzystywały zadaną z góry

informację o planowanym czasie ruchu. W generatorze płynnego ruchu czas nie jest

narzucony przez programistę, lecz jest obliczany na podstawie zadanej drogi

i maksymalnych prędkości i przyspieszeń.

Przedstawiona w niniejszej pracy koncepcja realizacji generatora płynnego ruchu

polega na rozpatrywaniu ruchu w każdej współrzędnej osobno, wyznaczeniu czasu

całego ruchu, jako czasu najdłuższego z ruchów i takie ograniczenie prędkości

w pozostałych współrzędnych aby czas przebycia w nich zadanej drogi był równy

wyznaczonemu czasowi maksymalnemu.

Generator umożliwia obliczenie trajektorii ruchu we współrzędnych wewnętrznych

(przegubowych) oraz współrzędnych zewnętrznych (położenie w układzie kartezjańskim

i orientacja w reprezentacji oś-kąt lub za pomocą kątów Eulera Z-Y-Z).

3.2.2. Opis algorytmu generatora płynnego ruchu

Poniżej przedstawiono równania dla generatora płynnego ruchu. Obliczenia są takie

20

same, niezależnie od przyjętego układu współrzędnych.

Programista zadaje punkt docelowy, przyspieszenia, prędkości początkowe i końcowe

oraz prędkości dla etapu ruchu jednostajnego. Algorytm generatora w pierwszym etapie

oblicza drogę do przebycia, następnie wyznacza czas ruchu. W kolejnym etapie

prędkości ruchu jednostajnego podlegają optymalizacji. Na końcu obliczane są pozycje

dla kolejnych makrokroków.

3.2.2.1. Określenie drogi w pojedynczej współrzędnej

Przyrost położenia wyraża się wzorem:

Δ x=x k−x p , (3.1)

gdzie

xk – położenie końcowex p – położenie początkowe

Droga s jest wartością bezwzględną przyrostu położenia:

s=∣Δx∣ (3.2)

3.2.2.2. Określenie modelu ruchu

Profil ruchu podzielono na trzy etapy:

1. rozpędzanie / hamowanie

2. ruch jednostajny

3. hamowanie / rozpędzanie

Ruch w etapach 1. i 3. modelowany jest jako jednostajnie przyspieszony / opóźniony.

Istnieją cztery warianty profilu ruchu, określane na podstawie stosunku prędkości ruchu

21

jednostajnego do prędkości początkowej i końcowej:

Rysunek 12: Pierwszy model ruchu - rozpędzanie w etapie 1, hamowanie w etapie 3

Rysunek 13: Drugi model ruchu - rozpędzanie w etapach 1 i 3

Rysunek 14: Trzeci model ruchu - hamowanie w 1. i 3. etapie

Rysunek 15: Czwarty model ruchu - hamowanie w etapie 1, rozpędzanie w etapie 3

gdzie

v – zadana prędkość ruchu jednostajnegov p – zadana prędkość początkowav k – zadana prędkość końcowa

Wartości prędkości v , v p i v k zadawane są przez programistę. Przyspieszenie jest

stałe i jednakowe dla etapów 1 i 3; również jest zadawane przez programistę. Długość

poszczególnych odcinków może być różna, nie jest zakładana symetryczność etapów 1 i

3.

22

Nie przy każdej zadanej prędkości v ruch jest możliwy do zrealizowania.

Ograniczenia na prędkość wynikają ze wzorów na drogę przebytą w etapach rozpędzania

i hamowania. Droga przebyta w tych etapach musi być mniejsza od zadanej drogi całego

ruchu:

2 v2−v p2 −v k

2

2 as ∧ v≥v p∧v≥v k (pierwszy model ruchu) (3.3)

∨

−v p2 v k

2

2as ∧ v≥v p∧vvk (drugi model ruchu) (3.4)

∨

v p2 −vk

2

2 as ∧ vv p∧v≥vk (trzeci model ruchu) (3.5)

∨

−2 v2v p2 vk

2

2as ∧ vv p∧vvk (czwarty model ruchu), (3.6)

gdzie

a – zadana wartość przyspieszenia

Wzory te wynikają ze wzorów na drogę w ruchu jednostajnie

przyspieszonym/opóźnionym:

y=v 0 t at2

2i t=

v−v0

a , zatem y=2 v0 v−v0v−v0

2

2 adla ruchu jednostajnie

przyspieszonego

y=v 0 t− at2

2i t=

v0 −va , zatem y=

2 v0 v0 −v −v0 −v 2

2 adla ruchu jednostajnie

opóźnionego

23

Jeśli nie zachodzi odpowiedni dla danego modelu ruchu wzór 3.3 – 3.6, ruch odbywa

się w dwóch etapach, z pominięciem etapu ruchu jednostajnego, a maksymalna prędkość

zostaje dopasowana do zadanej drogi według wzoru na drogę w ruchu jednostajnie

przyspieszonym/opóźnionym.

Rysunek 16: Pierwszy model ruchu - rozpędzanie w etapie 1, hamowanie w etapie 2

Rysunek 17: Drugi model ruchu - rozpędzanie w obu etapach

Rysunek 18: Trzeci model ruchu - hamowanie w obu etapach

Rysunek 19: Czwarty model ruchu - hamowanie w etapie 1, rozpędzanie w etapie 3

Jak wynika z wyżej wymienionych wzorów na drogę w ruchu jednostajnie

przyspieszonym/opóźnionym, droga przebyta w etapach 1. i 3. wyraża się ogólnym

wzorem:

s=2v p∣v−v p∣sgn v−v p v−v p

2 2 v∣v−vk∣sgn vk−v v−v k2

2a, (3.7)

Zatem dla pierwszego modelu ruchu:

24

v= v p2 v k

2 2a s2

, dla vv p∧vvk (3.8)

Dla drugiego i trzeciego modelu ruchu przy ruchu bez etapu ruchu jednostajnego

podział na 2 etapy jest sztuczny, zostaje jednak zachowany, żeby uprościć strukturę

algorytmu generatora. Prędkości v nie da się obliczyć - może mieć dowolną wartość z

przedziału ⟨v p ; vk ⟩ dla drugiego modelu ruchu, a z przedziału ⟨vk ; v p ⟩ dla trzeciego

modelu. Arbitralnie przyjęto, że dla obu tych modeli prędkość v będzie obliczana na

podstawie wzoru:

v=v pvk

2 , dla vv p∧vvk ∨vv p∧vv k (3.9)

Dla czwartego modelu ruchu:

v= v p2 v k

2 −2a s2

, dla vv p∧vvk (3.10)

Należy zauważyć, że w rzeczywistości ruch rzadko kiedy będzie przebiegał w dwóch

etapach. Obliczane tu prędkości są tylko górnymi ograniczeniami, a właściwe prędkości

ruchu zostaną obliczone w kolejnym etapie, po obliczeniu i kwantyzacji czasu całego

ruchu.

3.2.3. Określenie poszczególnych czasów ruchu i czasu całego ruchu

W przypadku ruchu w trzech etapach, czas t potrzebny do wykonania ruchu wynika

ze wzorów na ruch jednostajny i jednostajnie przyspieszony/opóźniony:

t=∣v−v p∣a

2 a s−2v p∣v−v p∣−sgnv−v pv−v p

2 −2 v∣v−v k∣−sgnvk−v v−v k 2

2 a v∣v – v k∣

a(3.11)

25

Przy czym trzy człony odpowiadają kolejno za:

∣v−v p∣a - czas rozpędzania / hamowania w pierwszym etapie, (3.12)

2 a s−2 v p∣v−v p∣−sgn v−v pv−v p2 −2v∣v−vk∣−sgnvk−v v−v k

2

2 a v (3.13)

- czas poruszania się ze stałą prędkością w drugim etapie. Został on obliczony na

podstawie wzoru 3.7 i wzoru na drogę w ruchu jednostajnym:

y=vt

i wynosi odpowiednio:

−2 v2 v p2 vk

2 2 as2 a v

, dla v≥v p∧v≥vk (pierwszy model ruchu) (3.14)

v p2 −vk

2 2 as2 a v

, dla v≥v p∧vvk (drugi model ruchu) (3.15)

−v p2 v k

2 2as2a v

, dla vv p∧v≥vk (trzeci model ruchu) (3.16)

2 v2 −v p2 −vk

2 2 as2 a v

, dla vv p∧vvk (czwarty model ruchu) (3.17)

∣v – vk∣a - czas hamowania/rozpędzania w trzecim etapie (3.18)

Przy ruchu w dwóch etapach czas t obliczany jest według następującego wzoru:

t=∣v−v p∣∣v−vk∣a (3.19)

26

Czas całego ruchu to najdłuższy z czasów:

tmax= maxi=1, .. , n

t i , (3.20)

gdzie

i – numer współrzędnej

n – liczba współrzędnych w danym układzie (zazwyczaj n=6 )

Ze względu na sposób zadawania parametrów ruchu w systemie MRROC++, czas

musi zostać zaokrąglony do wielokrotności długości pojedynczego kroku. Aby ruch był

możliwy do zrealizowania, czas jest zaokrąglany w górę. Ostateczny czas ruchu t r

wynosi:

t r=Δt s[t max

Δt s]1 , (3.21)

gdzie Δt s to czas pojedynczego kroku (2ms), a [tmax

Δt s] oznacza część całkowitą

tmax

Δt s.

3.2.2.4. Określenie prędkości i przyspieszeń dla kolejnych współrzędnych

Przyjmowane są stałe przyspieszenia o wartościach zadanych przez programistę, co

umożliwi uniknięcie niezamierzonej sytuacji, w której manipulator porusza się z bardzo

małym przyspieszeniem.

Aby ruch był płynny przyjęto takie prędkości w kolejnych współrzędnych, aby

spełniony był warunek:

∀i∈{1,.. ,n}t i=t r(3.22)

27

Przy danym czasie całego ruchu (jest to t r wyliczony w poprzednim kroku)

wyznaczenie dla każdej współrzędnej prędkości v sprowadza się do rozwiązania

równania 3.11.

Dla pierwszego modelu ruchu jest to równanie kwadratowe:

2 v2 – 2 v v kv pa t r v p2vk

22 a s=0 , dla vv p∧vvk (3.23)

Rozwiązaniami są:

v=2v kv pa t r∓Δ

4, (3.24)

gdzie

Δ=−4 v p2 – 4 v k

28v p vk8a t rv pvk4 a2 t r2 – 16 a s (3.25)

Zawsze istnieje tylko jedno dopuszczalne rozwiązanie. Wykluczyć należy wartości

prędkości większe od zadanych wcześniej prędkości maksymalnych i mniejsze od zera.

Dla drugiego modelu ruchu równanie jest liniowe:

2 v v k−v p−at r 2asv p2 −v k

2 =0 , dla vv p∧vvk (3.26)

i istnieje jedno rozwiązanie:

v=vk

2 −v p2 −2as

2v k−v p−at r(3.27)

Dla trzeciego modelu ruchu równanie jest liniowe:

2 v v p−vk−at r 2as−v p2 v k

2 =0 , dla vv p∧vvk (3.28)

28

i istnieje jedno rozwiązanie:

v=v p

2 −v k2 −2as

2v p−v k−at r(3.29)

Dla czwartego modelu ruchu równanie jest kwadratowe:

−2 v22 v pv k−at rv−v p2−vk

22 a s=0 , dla vv p∧vvk (3.30)

Rozwiązaniami są:

v=2v kv p−at r∓Δ

4, (3.31)

gdzie

Δ=−4 v p2 – 4 v k

28v p vk−8a t rv pvk4 a2 t r216 a s (3.32)

3.2.2.5. Obliczanie położeń w kolejnych makrokrokach

Ruch wykonywany jest w n makrokrokach:

n=t r

tk, (3.33)

gdzie tk – czas jednego makrokroku, zadawany przez programistę jako tk=mΔt s ,

gdzie m jest zadawaną liczbą kroków w makrokroku.

Położenie w i -tym makrokroku dla pojedynczej współrzędnej obliczane jest na

podstawie jednego z poniższych wzorów, w zależności od etapu ruchu:

29

xi=x piv p tkai2 t k

2

2, (3.34)

dla i∣v−v p∣at k

(pierwszy etap ruchu – rozpędzanie / hamowanie)

xi=x ps1it k−∣v−v p∣

a⋅v (3.35)

dla i∈⟨∣v−v p∣

atk;

t r

tk−∣v−vk∣

atk⟩ (drugi etap ruchu – ruch jednostajny),

gdzie s1 to droga przebyta w pierwszym etapie ruchu

xi=x ps1s2it k∣v−vk∣

a−t r v−it k

∣v−vk∣a

−t r2

a2

, (3.36)

dla i∈⟨t r

tk−∣v−vk∣

atk; n⟩ (trzeci etap ruchu – rozpędzanie / hamowanie),

gdzie s2 to droga przebyta w drugim etapie ruchu

Droga przebyta podczas pierwszego etapu ruchu s1 wyraża się wzorem:

s1=2v p∣v−v p∣v−v p

2

2 a(3.37)

Droga przebyta podczas drugiego etapu s2 natomiast wyraża się wzorem:

s2= t r−∣v−v p∣∣v−v k∣

av (3.38)

30

Rozdział 4

Realizacja zadania manipulacji

4.1. System MRROC++

MRROC++ (Multi-Robot Research Oriented Controler) jest programową strukturą

ramową wspomagającą tworzenie sterowników dla systemów wielorobotowych. Jej

językiem źródłowym jest C++. Tworzone sterowniki dedykowane są konkretnemu

zadaniu. Powstały sterownik jest systemem czasu rzeczywistego pracującym pod

nadzorem systemu operacyjnego czasu rzeczywistego QNX w wersji 6.3. [CZ06]

4.1.1. Opis pozycji końcówki manipulatora w MRROC++

Obecnie w systemie MRROC++ zdefiniowane jest 5 układów współrzędnych, w

których opisywana jest pozycja końcówki manipulatora: położenie kątowe wałów

silników, współrzędne wewnętrzne, trójścian związany z końcówką, kąty Eulera Z-Y-Z

oraz reprezentacja oś-kąt.

4.1.2. Opis systemu robotycznego

Każdy system robotyczny [CZ99] można podzielić na trzy główne części: efektory,

receptory i podsystem sterowania. Efektory są to elementy oddziałujące na otoczenie,

receptory (czujniki rzeczywiste) odpowiedzialne są za zbieranie informacji z otoczenia.

Podsystem sterowania składa się ze sprzętu obliczeniowego i oprogramowania. System

robotyczny można opisać jako:

s=⟨e ; r ; c ⟩ s∈S ;e∈E ;r∈R ;c∈C ,

31

gdzie:

s – stan systemu S – przestrzeń stanów systemu

e – stan efektorów E – przestrzeń stanów efektorów

r – stan receptorów R – przestrzeń stanów receptorów

c – stan podsystemu sterowania C – przestrzeń stanów podsystemu sterowania

Dane dostarczane przez czujniki rzeczywiste zwykle nie są w postaci użytecznej dla

sterowania, dlatego poddawane są ekstrakcji informacji użytecznej (agregacji).

Zagregowana informacja nazywana jest odczytem czujnika wirtualnego.

v= f v ⟨ r ,c ,e ⟩ , v∈V ,

gdzie v jest stanem czujników wirtualnych, a V - przestrzenią odczytów czujników

wirtualnych. Funkcja fv nosi nazwę funkcji agregującej. Stan całego systemu

robotycznego, składającego się z wielu robotów, wyraża się wzorem:

s=⟨e1, .. , en ; v1, .. ,v m ;c ⟩ ,

gdzie n jest liczbą efektorów, a m – liczbą czujników wirtualnych. Podsystem

sterujący c dzieli się na (n+1) części: podsystem koordynujący pracę wszystkich robotów

i n podsystemów przyporządkowanych do poszczególnych robotów. W MRROC++

odpowiadają im kolejno proces MP (Master Process) i procesy ECP (Effector Control

Process).

4.1.3. Struktura sterownika

Sterownik MRROC++ [CZ99] jest podzielony na moduły uporządkowane według

hierarchicznej struktury funkcjonalnej. Moduły są odrębnymi procesami, nierzadko

dodatkowo tworzącymi wątki.

Na najniższą z warstw składają się procesy odwołujące się do rzeczywistych urządzeń:

32

procesy EDP odpowiedzialne są za rozwiązywanie prostego i odwrotnego zadania

kinematyki, wątki SG realizują serwomechanizmy poszczególnych osi, a procesy VSP

odczytują i przetwarzają informację z rzeczywistych czujników.

W wyższej warstwie, procesy ECP sterują poszczególnymi efektorami. W warstwie

nadrzędnej, proces MP koordynuje wszystkie procesy ECP.

W warstwie komunikacji z użytkownikiem wątek UI obsługuje zlecenia operatora, a

proces SRP odbiera komunikaty od innych procesów.

Strukturę sterownika w MRROC++ przedstawia poniższy rysunek:

4.1.4. Generatory trajektorii

Generator trajektorii [JN04] określa warunek końcowy i sposób sterowania ruchem

efektorów. Przemieszczenie z jednego położenia zadanego do drugiego podzielone

33

Rysunek 20: Struktura funkcjonalna i warstwy sterownika MRROC++

zostało w systemie MRROC++ na makrokroki. Makrokrok jest pojedynczym zleceniem

ruchu dla sterownika robota. Zawiera się w nim pojedynczy przedział interpolacji.

W jednym makrokroku następuje przejście z jednego węzła interpolacji do następnego.

Podzielony jest na kroki ruchu — przyrosty położenia ramienia robota, realizowane

w pojedynczym okresie próbkowania (2 ms – okres pracy algorytmu regulacji).

W pojedynczym kroku ruchu odbywa się prosta liniowa interpolacja dla każdego

połączenia ruchowego. W kolejnych makrokrokach interpolacją pomiędzy węzłami

zajmuje się konkretny generator. Generator jest obiektem w sensie C++. Wymaga

zdefiniowania dwóch funkcji: first_step i next_step. Trajektoria generowana

jest w wewnętrznych współrzędnych manipulatora.

4.1.5. Funkcja Move

Funkcja Move [JN04] służy do realizacji ruchu pojedynczego robota (gdy jest

wywoływana z poziomu ECP lub MP) lub też kilku robotów (z poziomu MP).

Wykorzystuje do tego informacje pochodzące z czujników wirtualnych i generator

trajektorii, określający specyfikację ruchu. Pierwszy makrokrok ruchu określony jest

przez metodę first_step generatora. Najczęściej jest to odczyt aktualnego położenia

robota. Następnie żądane są dane od wszystkich używanych czujników, po czym, zlecane

jest wykonanie makrokroku ruchu. Dalej, odczytywane są dane z czujników, a na koniec,

metoda next_step generatora ruchu sprawdza warunek końcowy. Jeżeli nie jest on

spełniony, oblicza kolejny krok ruchu, po czym powyższe operacje są ponawiane. Jeżeli

warunek końcowy jest spełniony, funkcja Move kończy działanie.

4.2. Implementacja zadania manipulacji

Zadanie przelewania zostało zaimplementowane jako zadanie dwurobotowe ze

sporadyczną koordynacją. Główny algorytm wykonywany jest w procesie MP, w którym

zlecane są rozkazy ruchu do procesów ECP obydwu manipulatorów. Procesy ECP

jedynie wykonują zlecone im przez MP ruchy.

34

Sieć działań procesu MP wygląda następująco:

Rysunek 21: Sieć działań procesu MP w zadaniu przelewania

35

Zadanie zostało w implementacji zdekomponowane w sposób odzwierciedlający

podział na etapy manipulacji zaproponowany w podrozdziale 3.1.3.

Pierwszemu etapowi odpowiada funkcja approach, realizująca przemieszczenie

końcówek manipulatorów w pobliże obiektów manipulacji i przyjęcie orientacji

umożliwiających schwycenie pionowo stojących obiektów. Funkcja zleca procesom ECP

wykonanie trajektorii z zadanym pojedynczym punktem docelowym.

Drugiemu etapowi odpowiada funkcja grab. Realizowane są trajektorie z jednym

zadanym punktem, we współrzędnych zewnętrznych z opisem orientacji za pomocą

kątów Eulera Z-Y-Z. Używanym generatorem jest ECP_smooth_generator. Samo

zaciśnięcie szczęk chwytaków realizowane jest z użyciem generatora

ECP_linear_generator.

Trzeci etap to funkcja lift, realizująca trajektorie z jednym zadanym punktem, we

współrzędnych zewnętrznych z opisem orientacji za pomocą kątów Eulera Z-Y-Z.

Podniesienie obiektów jest ruchem w tylko jednej współrzędnej (Z).

Ustawienie naczyń względem siebie (etap czwarty) realizuje funkcja meet. Dla robota

Postument realizowana jest trajektoria we współrzędnych zewnętrznych, bez punktów

pośrednich, umożliwiająca ruch do docelowego punktu bez zmiany orientacji. Dla robota

OnTrack realizowane są trzy trajektorie – pierwsza we współrzędnych zewnętrznych z

dwoma punktami pośrednimi ma na celu właściwe ustawienie butelki względem kubka.

Druga, we współrzędnych wewnętrznych ma na celu jedynie przesunięcie robota na torze

jezdnym, a trzecia, we współrzędnych zewnętrznych, przechylenie butelki.

Etap piąty, czyli przelewanie realizuje funkcja pour, zlecająca wykonanie robotowi

OnTrack wykonanie obrotu wokół nowo zdefiniowanej końcówki umiejscowionej na

ujściu butelki. Zmiana końcówki wykonywana jest za pomocą generatora

ECP_tool_change_generator. Następnie realizowany jest ruch przywracający

butelkę do pionowej orientacji.

Etap szósty, siódmy, ósmy i dziewiąty realizują odpowiednio funkcje go_back,

put_back i depart.

36

4.3. Implementacja generatora płynnego ruchu

4.3.1. Klasa ecp_smooth_generator

4.3.1.1. Hierarchia klas

Klasa ecp_smooth_generator dziedziczy z klasy ecp_delta_generator,

klasy bazowej dla generatorów o zadany przyrost położenia/orientacji, zawierającej już

potrzebne do określania maksymalnych prędkości i przyspieszeń atrybuty. Hierarchia

klas wygląda następująco:

ecp_generator ↓

ecp_delta_generator ↓

ecp_smooth_generator

Diagram klas generatora jest następujący:

37

38

Rysunek 22: Diagram klas ECP_smooth_generator

4.3.1.2. Zmienne klasy ecp_smooth_generator

Specyficzne dla ecp_smooth_generator pola to:

– final_position i start_position, w których przechowywane jest

początkowe i zadane położenie dla aktualnie wykonywanego ruchu; tablice o liczbie

elementów równej stałej MAX_SERVOS_NR– next_position, w którym przechowywane jest położenie zadane w aktualnym

makrokroku; tablica o liczbie elementów równej stałej MAX_SERVOS_NR– v_p i v_k – prędkości początkowe i końcowe w aktualnie wykonywanym ruchu;

tablice o liczbie elementów równej stałej MAX_SERVOS_NR– v – zadany dla każdej osi z osobna procent dopuszczanej prędkości maksymalnej,

który ma być osiągnięty w etapie ruchu jednostajnego aktualnie wykonywanego

ruchu; tablica o liczbie elementów równej stałej MAX_SERVOS_NR– a – zadany dla każdej osi z osobna procent dopuszczanego maksymalnego

przyspieszenia w aktualnie wykonywanym ruchu, z którym odbywa się

rozpędzanie/hamowanie w etapach 1 i 3; tablica o liczbie elementów równej stałej MAX_SERVOS_NR

– a_r, v_r – rzeczywiste maksymalne prędkości w aktualnie wykonywanym

ruchu, obliczane na początku ruchu za pomocą funkcji calculate; tablice o liczbie

elementów równej stałej MAX_SERVOS_NR– k – zwrot ruchu (+/- 1); tablica o liczbie elementów równej stałej

MAX_SERVOS_NR– zmienne przysp i jedn, w których zapamiętywane są makrokroki, w których

kończą się odpowiednio etapy pierwszy i drugi; tablice o liczbie elementów równej

stałej MAX_SERVOS_NR– zmienne s_przysp i s_jedn, w których zapamiętywane są drogi przebywane

odpowiednio w pierwszym i drugim etapie ruchu; tablice o liczbie elementów równej

stałej MAX_SERVOS_NR– t_max – czas całego aktualnie wykonywanego ruchu, obliczany w funkcji

39

calculate– zmienna boolowska is_synchronised, informująca o tym, czy robot jest

zsynchronizowany

– zmienna boolowska debug, informująca o tym, czy mają być wyświetlane

informacje o obliczeniach

– pose_list – lista kolejnych punktów ruchu, zawierająca elementy typu

smooth_taught_in_pose– pose_list_iterator – wskaźnik na element listy pose_list– maksymalne dopuszczalne prędkości i przyspieszenia dla poszczególnych opisów

położenia i orientacji v_max_motor, v_max_joint, v_max_zyz,

v_max_aa, a_max_motor, a_max_joint, a_max_zyz, a_max_aa,

wczytywane z zewnętrznego pliku przy tworzeniu generatora; tablice o liczbie

elementów równej stałej MAX_SERVOS_NR– v_grip – prędkość chwytaka w aktualnie wykonywanym ruchu

– v_grip_min – minimalna dopuszczalna prędkość chwytaka, wczytywana z

zewnętrznego pliku przy tworzeniu generatora

– node_counter – numer aktualnego makrokroku

– first interval – zmienna boolowska informująca o tym, czy wykonywany

jest pierwszy krok ruchu

4.3.1.3. Metody klasy ecp_smooth_generator

Oprócz niezbędnych metod first_step i last_step, klasa

ecp_smooth_generator zawiera szereg innych, pomocniczych metod.

4.3.1.3.1. Metody związane z obsługą listy pozycji

– flush_pose_list czyści listę

– initiate_pose_list ustawia wskaźnik na początek listy

– next_pose_list_ptr przesuwa wskaźnik na kolejny element listy

40

– get_pose przepisuje dane pozycji do zmiennych generatora

– set_pose zapisuje dane pozycji w elemencie listy; wywoływana jest z

parametrami kolejno: ps, v_p, v_k, v, a, coordinates, gdzie ps jest

specyfikacją opisu orientacji, a coordinates - współrzędnymi punktu docelowego

– is_pose_list_element informuje czy wskaźnik wskazuje na element listy

– is_last_list_element informuje czy wskaźnik wskazuje na ostatni element

listy

– create_pose_list_head tworzy pierwszy element listy; wywoływana z

takimi samymi parametrami jak funkcja set_pose– insert_pose_list_element wstawia nową pozycję na koniec listy;

wywoływana z takimi samymi parametrami jak funkcja set_pose– pose_list_length zwraca liczbę elementów listy

4.3.1.3.2. Metody wczytujące dane z plików

– metoda load_a_v_max ładuje z pliku wartości maksymalnych prędkości i

przyspieszeń. Jako parametr zadawana jest nazwa pliku, z którego mają być pobrane

dane. Format pliku jest następujący:

v_maxMOTOR, 1 ... v_maxMOTOR, MAX_SERVOS_NR

a_maxMOTOR, 1 ... a_maxMOTOR, MAX_SERVOS_NR

v_maxJOINT, 1 ... v_maxJOINT, MAX_SERVOS_NR

a_maxJOINT, 1 ... a_maxJOINT, MAX_SERVOS_NR

v_maxEULER ZYZ, 1 ... v_maxEULER ZYZ, MAX_SERVOS_NR

a_maxEULER ZYZ, 1 ... a_maxEULER ZYZ, MAX_SERVOS_NR

v_maxANGLE AXIS, 1 ... v_maxANGLE AXIS, MAX_SERVOS_NR

a_maxANGLE AXIS, 1 ... a_maxANGLE AXIS, MAX_SERVOS_NR

Wartości, wyznaczone na podstawie wcześniejszych doświadczeń są następujące:

41

120.0 120.0 120.0 120.0 120.0 120.0 120.0 120.0100.0 100.0 100.0 100.0 100.0 100.0 100.0 100.01.5 1.5 1.5 1.5 1.5 1.5 1.5 1.57.0 7.0 7.0 7.0 7.0 7.0 7.0 7.05.0 5.0 5.0 5.0 5.0 5.0 5.0 5.05.0 5.0 5.0 5.0 5.0 5.0 5.0 5.00.0 0.0 0.0 0.0 0.0 0.0 0.0 0.00.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0

I znajdują się w pliku /data/a_v_max.txt

– metoda load_a_v_min ładuje z pliku wartości minimalnych prędkości i

przyspieszeń. Aktualnie jest to tylko minimalna prędkość chwytaka, zadawana za

pomocą jednej liczby. Jako parametr zadawana jest nazwa pliku, z którego mają być

pobrane dane.

Minimalne prędkości i przyspieszenia znajdują się w pliku /data/a_v_min.txt

– metoda load_file_with_path wczytuje trajektorię z zadanego pliku.

Tworzona jest nowa lista pozycji, do której dodawane są kolejne punkty. Jako

parametr zadawana jest nazwa pliku, z którego mają być pobrane dane. Format pliku

z trajektorią omówiono w punkcie dotyczącym klasy

ecp_smooth_taught_in_pose.

4.3.1.3.3. Konstruktory

Istnieją dwa konstruktory. Pierwszy konstruktor przyjmuje jako parametry zadanie

ecp_task i wartość zmiennej is_synchronised. Minimalne i maksymalne

prędkości są wczytywane z domyślnych plików /data/a_v_max.txt i /data/a_v_min.txt. Zmienna debug przyjmuje wartość false. Drugi

konstruktor jako dodatkowy parametr przyjmuje wartość zmiennej debug.

42

4.3.1.3.4. Pomocnicze funkcje obliczeniowe

Klasa ecp_smooth_generator posiada dwie pomocnicze funkcje obliczeniowe:

– funkcja calculate odpowiedzialna jest za obliczenia związane z czasem ruchu

i prędkościami dla poszczególnych osi (wszelkie obliczenia opisane w podrozdziałach

3.2.1 do 3.2.4). Wywoływana jest po wczytaniu nowego punktu docelowego. W

przypadku gdy ruchu nie da się wykonać przy zadanych parametrach, rzucany jest

wyjątek. Jeśli dokonywana jest redukcja prędkości wysyłany jest o tym komunikat do

procesu UI, o strukturze: „Redukcja prędkości w osi X”.

– funkcja generate_next_coords odpowiedzialna jest za obliczanie

docelowych współrzędnych dla kolejnych makrokroków (obliczenia opisane

w podrozdziale 3.2.5). Wywoływana jest w każdym makrokroku.

4.3.1.3.5. Metoda first_step

Metoda first_step służy w generatorze ecp_smooth_generator tylko

nawiązaniu połączenia z procesem EDP. Wszelkie obliczenia wykonywane są w

metodzie next_step.

4.3.1.3.6. Metoda next_step

Działanie metody next_step opisuje poniższa sieć działań. Ze względu na jej

złożoność, jej fragmenty, nazwane tu „pierwszy krok” i „następny punkt” zostały opisane

osobnymi sieciami działań.

43

Rysunek 23: Sieć działań metody next_step

44

Sieć działań fragmentu oznaczonego jako „następny punkt”:

45

Rysunek 24: Następny punkt - sieć działań

Sieć działań fragmentu oznaczonego jako „pierwszy krok”:

W części metody next_step następującej po warunku

if(first_interval==true) najpierw wczytywane są dane dotyczące zadanego

ruchu (za pomocą metody get_pose). Początkowe położenie odczytywane jest z pola

EDP_data klasy robot. Następnie obliczane są maksymalne dopuszczalne w

aktualnym ruchu prędkości i przyspieszenia na podstawie danych o zadanym sposobie

opisu orientacji, maksymalnych prędkości i przyspieszeń wczytanych z pliku przy

46

Rysunek 25: Pierwszy krok - sieć działań

tworzeniu generatora i zadanych przez programistę wartości v, przez które prędkości te

są mnożone. Analogicznie obliczane są prędkości początkowe i końcowe. Obliczone

prędkości i przyspieszenia zapisywane są w zmiennych odpowiednio v_r i a_r.

Następnie obliczany jest za pomocą funkcji calculate czas ruchu i maksymalne

rzeczywiste prędkości ruchu dla poszczególnych osi, które są nadpisywane na zmiennych

v_r. Zmienna node_counter przyjmuje wartość 0.

Następnie wysyłana jest odpowiedź do procesu MP. Jeśli poprzedni punkt docelowy

był ostatnim na liście, wysyłana jest odpowiedź TASK_TERMINATED i metoda kończy

swoje działanie. Jeśli są kolejne punkty trajektorii, obliczenia dokonywane są

analogicznie jak dla pierwszego punktu, za wyjątkiem odczytu początkowego położenia,

które zadawane jest z docelowych współrzędnych punktu poprzedniego.

W zmiennej td typu trajectory_description przechowywana jest liczba

makrokroków (interpolation_node_no), liczba kroków w makrokroku

(internode_step_no) i numer kroku w którym zwracana ma być wartość położenia

(value_in_step_no). Przyjęto stałą długość makrokroku, gdyż upraszcza to

realizację i czyni ją bardziej przejrzystą. Za długość makrokroku arbitralnie przyjęto

20ms, zatem każdy makrokrok składa się z 10 kroków. Wartość zwracana jest w kroku

ósmym - przed końcem makrokroku aby nie zatrzymywać ruchu robota. Dane te

przepisywane są do bufora EDP_data.

Kolejnym zadaniem metody next_step jest generowanie punktów docelowych w

każdym makrokroku. Punkty te są obliczane za pomocą metody

generate_next_coords.

Ze względu na inną niż pozostałych osi reakcję chwytaka, a także stosunkowo krótkie

drogi, które ma do przebycia, przyjęto, że zadawany mu będzie ruch jednostajny, z

obliczoną uprzednio prędkością v_grip. Chwytak może więc zakończyć ruch

wcześniej niż pozostałe osie.

4.3.1.3.7. Wyświetlanie informacji o przebiegu ruchu

Ze względu na wymagającą wobec programisty metodę zadawania parametrów ruchu,

47

generator został wyposażony w możliwość wyświetlania na konsoli informacji o

wykonywanym ruchu, uruchamianą poprzez ustawienie wartości zmiennej debug na

true. W każdym punkcie trajektorii na konsolę wysyłane są następujące informacje: w

ilu etapach wykonywany jest ruch dla każdej współrzędnej, liczba makrokroków, liczba

kroków w makrokroku, czas jednego makrokroku, wartość stałej STEP, czas ruchu i

prędkości ruchu jednostajnego dla wszystkich współrzędnych, w formacie:

0 - n0 etapy..m - nm etapymakrokroki: i, kroki: j, czas kroku: tk, step: st: tv: v0 .. vm

gdzie wyróżnione w powyższym zapisie kursywą zmienne to:

m – liczba współrzędnych (odpowiadająca stałej MAX_SERVOS_NR)

n0 .. nm – liczba etapów ruchu we współrzędnej 0..m

i – liczba makrokroków

j – liczba kroków w obrębie jednego makrokroku

tk – czas kroku

s – wartość stałej STEP

v0 .. vm – wartość prędkości w etapie ruchu jednostajnego dla współrzędnej 0..m

W każdym makrokroku natomiast wysyłane są współrzędne docelowego punktu

interpolacji w formacie:

x0 .. xm

Przy używaniu generatora z wyłączonym trybem testowym debugowanie powinno być

wyłączone, gdyż nadmiernie obciąża komputer, co prowadzi do mało płynnych ruchów

manipulatora.

48

4.3.2. Klasa ecp_smooth_taught_in_pose

Klasa ecp_smooth_taught_in_pose służy do zapisu parametrów ruchu dla

generatora ecp_smooth_generator. Lista pozycji generatora jest listą obiektów tej

klasy. Z każdym punktem trajektorii dla generatora ecp_smooth_generator wiążą

się następujące parametry:

– rodzaj współrzędnych, w jakich odbywa sie ruch arm_type– prędkości początkowe v_p i końcowe v_k dla każdej osi w ruchu z aktualnej do

zadanej pozycji.

– prędkości v i przyspieszenia a, z jakimi ma się odbywać ruch do zadanej pozycji,

podawane jako procent prędkości i przyspieszeń maksymalnych manipulatora

– współrzędne punktu docelowego coordinates

Dobór właściwych prędkości początkowych i końcowych spoczywa na programiście

korzystającym z generatora. Ruch przez punkty 1m w n osiach powinien się

odbywać według następujących zależności:

∀i∈{0..n}v p ,i 1=0 (4.1)

∀i∈{0..n}vk ,im=0 (4.2)

∀i∈{0..n}∀ j∈{1..m−1 }vk , i j=v p ,i j1 (4.3)

Diagram klas dla ECP_smooth_taught_in_pose jest następujący:

49

Rysunek 26: Diagram klas ECP_smooth_taught_in_pose

Trajektorię można wczytywać z pliku. Służy do tego funkcja

ecp_smooth_load_file_with_path. Format pliku z trajektorią jest następujący:

arm_typeliczba punktów w liścievp,1 .. vp,MAX_SERVOS_NR

vk,1 .. vk,MAX_SERVOS_NR

v1 .. vMAX_SERVOS_NR

a1 .. aMAX_SERVOS_NR

coordinates1 .. coordinatesMAX_SERVOS_NR

Przykładowy plik z trajektorią (ruch przez 2 punkty z zatrzymaniem się w obu, we

współrzędnych MOTOR) wygląda następująco:

MOTOR20.0 0.0 0.0 0.0 0.0 0.0 0.0 0.00.0 0.0 0.0 0.0 0.0 0.0 0.0 0.01.0 1.0 1.0 1.0 1.0 1.0 1.0 1.01.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0100.0 100.0 100.0 100.0 100.0 50.0 0.0 0.0

0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.00.0 0.0 0.0 0.0 0.0 0.0 0.0 0.01.0 1.0 1.0 1.0 1.0 1.0 1.0 1.01.0 1.0 1.0 1.0 1.0 1.0 1.0 1.0100.0 100.0 70.0 100.0 25.0 50.0 0.0 0.0

50

Rozdział 5

Eksperymenty i wyniki testów

5.1. Testy generatora płynnego ruchu

Poniżej przedstawiono wyniki testów generatora płynnego ruchu, wykonanych dla

trzech różnych założeń: ruchu do punktu docelowego bez punktów pośrednich

(z prędkościami początkowymi i końcowymi równymi 0), ruchu do punktu docelowego

z jednym punktem pośrednim i jednakową prędkością obu ruchów, oraz ruchu do punktu

docelowego z jednym punktem pośrednim i z różnymi prędkościami dla poszczególnych

ruchów.

5.1.1. Ruch do punktu docelowego bez punktów pośrednich

Realizacja ruchu do punktu docelowego bez punktów pośrednich jest najprostszym

zastosowaniem generatora płynnego ruchu. Prędkości początkowe i końcowe powinny

zostać ustawione na 0, a ruch zawsze zostanie wykonany. Poniżej przedstawiono wykres

położenia i prędkości dla osi 5, dobrze ilustrujący działanie generatora. Zadane

parametry ruchu były następujące:

vp,0..7 = 0

vk,0..7 = 0

v = [0,75; 0,75; 0,75; 0,75; 0,75; 0,75; 0,75; 1.5]

a = [2,1; 2,1; 2,1; 2,1; 2,1; 2,1; 2,1; 7]

xp = [0; -0.087462; -1.541873; 0.024063; 1.218727; 2.591185; -2.664387; 0.074]

xk = [0; -0.000056; -1.570689; 0.000365; 1.559536; 1.570215; -2.320805; 0.088]

51

Obliczone prędkości dla etapu ruchu jednostajnego są następujące:

vr = [0; 0.051553; 0.016832; 0.013831; 0.210401; 0.748834; 0.212226; 0.008145]

Wykresy dla osi 5 wyglądają następująco:

52

5.1.2. Ruch do punktu docelowego z jednym punktem pośrednim, niezerowymi

prędkościami w punkcie pośrednim i jednakową zadaną prędkością ruchu do obu

punktów

Pewnych problemów w zadawaniu parametrów ruchu dostarczają ruchy przez kilka

punktów. Niektóre ruchy mogą okazać się nie do zrealizowania ze względu na drogę

przebywaną podczas rozpędzania i hamowania. Inne mogą powodować mało płynne

ruchy manipulatora – dzieje się tak na przykład przy ruchu przez dwa punkty przy

zadaniu dużej prędkości końcowej pierwszego ruchu (i zarazem początkowej

drugiego) i małej prędkości samych ruchów. O ile świadomego zadawania takich

parametrów można łatwo uniknąć, o tyle przy zadaniu pozornie dużej prędkości ruchu

może dojść do analogicznej sytuacji jeśli prędkość maksymalna zostanie zredukowana

(przypadki określane wzorami 3.3.1. - 3.3.4.). Manipulator będzie wtedy poruszać się

powoli aż do etapu końcowego przyspieszania w pierwszym ruchu, po czym wykona

szybki, gwałtowny ruch w pobliżu punktu docelowego pierwszego ruchu.

Problem ruchu z punktem pośrednim i jednakowymi prędkościami dotyczy przede

wszystkim trajektorii, które muszą uwzględniać znajdujące się na drodze manipulatora

przeszkody.

Poniżej przedstawiono wykresy położenia dla poszczególnych osi przy realizacji

dwóch przykładowych trajektorii we współrzędnych JOINT. Punktem początkowym

dla obu trajektorii jest pozycja synchronizacji:

xp = [0; -0.087462; -1.541873; 0.024063; 1.218727; 2.591185; -2.664387; 0.074]

Parametry pierwszego ruchu:

punkt 1:

vp,0..7 = 0

vk = [0; 0,266; 0,133; 0,133; 0,266; 0,266; 0,266; 0]

v = [0,5; 0,5; 0,5; 0,5; 0,5; 0,5; 0,5; 0,1]

a = [0,1; 0,3; 0,3; 0,3; 0,3; 0,3; 0,3; 0,1]

53

xk = [0; -0.041922; -1.556883; 0.011719; 1.396483; 2.056766; -2.485182; 0.081292]

punkt 2:

vp = [0; 0,266; 0,133; 0,133; 0,266; 0,266; 0,266; 0]

vk,0..7 = 0

v = [0,5; 0,5; 0,5; 0,5; 0,5; 0,5; 0,5; 0,1]

a = [0,1; 0,3; 0,3; 0,3; 0,3; 0,3; 0,3; 0,1]

xk = [0; -0.000056; -1.570689; 0.000365; 1.559536; 1.570215; -2.320805; 0.088]

Parametry drugiego ruchu:

punkt 1:

vp,0..7 = 0

vk = [0; 0,033; 0,013; 0,013; 0,033; 0,033; 0,033; 0]

v = [0,3; 0,15; 0,075; 0,075; 0,3; 0,3; 0,3; 0,15]

a = [0,7; 0,7; 0,7; 0,7; 0,7; 0,7; 0,7; 0,7]

xk = [0; -0.041922; -1.556883; 0.011719; 1.396483; 2.056766; -2.485182; 0.081292]

punkt 2:

vp = [0; 0,033; 0,013; 0,013; 0,033; 0,033; 0,033; 0]

vk,0..7 = 0

v = [0,3; 0,15; 0,075; 0,075; 0,3; 0,3; 0,3; 0,15]

a = [0,7; 0,7; 0,7; 0,7; 0,7; 0,7; 0,7; 0,7]

xk = [0; -0.000056; -1.570689; 0.000365; 1.559536; 1.570215; -2.320805; 0.088]

W pierwszym przypadku parametry zostały dobrane tak, aby nastąpiła redukcja

prędkości maksymalnych. Mimo pozornie dużej prędkości zadanej, ostateczna

prędkość ruchów jest mała, a prędkość w punkcie pośrednim - duża. Dodatkowo

dobrano duże przyspieszenia. Drugi ruch odbywa się z mniejszymi prędkościami

i przyspieszeniami i bez redukcji prędkości.

54

Wykresy położenia dla pierwszej osi, dobrze ilustrujące problem, wyglądają dla pierwszej trajektorii następująco:

55

Na wykresach widać gwałtowne wzrost prędkości w okolicy punktu przejściowego,

który przypada na 48 makrokrok. Wynika ono z dużej różnicy między prędkością

ruchu i prędkością końcową oraz dużego przyspieszenia, z jakim wykonywany jest

ruch.

Mimo zadanych prędkości dla osi 1-6 (po przemnożeniu przez prędkość

maksymalną dla współrzędnych JOINT: [0,75; 0.75; 0.75; 0.75; 0.75; 0.75], ruch

wykonywany jest w nich z prędkościami:

[0.009933, 0.006494, 0.003339, 0.186333, 0.732369, 0.188265], zatem w każdej osi

za wyjątkiem 5 dokonywana jest bardzo duża redukcja prędkości, duże są więc też

różnice między prędkościami ruchu, a prędkościami końcowymi.

56

Dla drugiej trajektorii wykresy wyglądają następująco:

57

Ostateczne wartości prędkości dla poszczególnych osi w tym ruchu to:

[0; 0.020949; 0.006908; 0.005658; 0.085097; 0.296626; 0.085825]

Przy prędkościach w punkcie pośrednim:

[0; 0,033; 0,013; 0,013; 0,033; 0,033; 0,033; 0]

W okolicy punktu pośredniego następuje przyspieszenie wynikające z tego, że

zadana prędkość końcowa dla pierwszego punktu jest większa od prędkości ruchu.

5.1.3. Ruch do punktu docelowego z jednym punktem pośrednim, niezerowymi

prędkościami w punkcie pośrednim i różnymi zadanymi prędkościami ruchu

Często spotykanym w manipulacji przypadkiem jest ruch w dwóch etapach przez

punkt pośredni tak, że pierwszy etap wykonywany jest z dużą prędkością, a drugi – z

małą. Sytuacja taka ma na przykład miejsce w przypadku chwytania obiektów.

pierwszy ruch wykonywany jest do punktu znajdującego się w pobliżu obiektu z dużą

prędkością, a drugi (umiejscawianie chwytaka względem obiektu) - z małą prędkością,

tak aby nie potrącić obiektu.

Przykładowa trajektoria spełniająca powyższe warunki, na której dokonano testów

generatora płynnego ruchu (przy pozycji synchronizacji robota jako punkcie

początkowym) została przedstawiona poniżej. Przyjęto następującą metodę jej

wyznaczania:

1. dobranie prędkości tak aby nie była dokonywana redukcja prędkości

maksymalnych w ruchu z zatrzymaniem w punkcie pośrednim

2. przyjęcie jako prędkości w punkcie pośrednim prędkości maksymalnych w

drugim (wolniejszym ruchu)

3. iteracyjna korekcja prędkości w punkcie pośrednim do wartości obliczonych

przez generator jako prędkości drugiego ruchu

58

Początkowe zadane parametry ruchu:

Punkt 1:

vp,0..7 = 0

vk = [0,00015; 0,015; 0,015; 0,015; 0,0075; 0,015; 0,015; 0]

v = [0,075; 0,15; 0,03; 0,075; 0,15; 0,15; 0,15; 0,15]

a = [0,14; 0,14; 0,14; 0,14; 0,14; 0,14; 0,14; 3,5]

xk = [0,160135; 1,73733; -1,50729; 0,126712; 0,603623; 2,39661; 2,14868; 0,057]

Punkt 2:

vp = [0,00015; 0,015; 0,015; 0,015; 0,0075; 0,015; 0,015; 0]

vk,0..7 = 0

v = [0,00015; 0,015; 0,015; 0,015; 0,0075; 0,015; 0,015; 0]

a = [0,014; 0,014; 0,014; 0,014; 0,014; 0,014; 0,014; 3,5]

xk = [0,160123; 1,716892; -1,487739; 0,109636; 0,615859; 2,364; 2,167032; 0.057]

Korekta prędkości w punkcie pośrednim wykonana została przy dokładności 10-6.

Prędkości w kolejnych iteracjach:

1. v = [0.00015; 0.015; 0.015; 0.015; 0.015; 0.015; 0.015; 0]

2. v = [0.000004; 0.007523; 0.006985; 0.005484; 0.004682; 0.014906; 0.006258; 0]

3. v = [0.000004; 0.008467; 0.008055; 0.006938; 0.004801; 0.014907; 0.007508; 0]

4. v = [0.000004; 0.008452; 0.008033; 0.006904; 0.004801; 0.014907; 0.007482; 0]

5. v = [0.000004; 0.008452; 0.008036; 0.006904; 0.004801; 0.014907; 0.007482; 0]

W piątej iteracji prędkości drugiego ruchu są przy założonej dokładności równe

prędkościom w punkcie pośrednim, co powinno zaowocować płynnym ruchem

manipulatora.

59

Wykresy położenia i prędkości dla osi 6 są następujące:

60

Wykres położenia ilustruje płynny ruch, o dobrze zmieniającej się prędkości, jednak

dobrane tą metodą parametry nie gwarantują optymalnego doboru prędkości. W części

przypadków w okolicy punktu przejściowego nastąpi niepotrzebne dodatkowe

zwolnienie/przyspieszenie.

5.1.4. Wnioski

Generator wymaga dodatkowego narzędzia wspomagającego dobór odpowiednich

prędkości w punktach pośrednich, zagadnienie to jednak wykracza poza tematykę i ramy

niniejszej pracy. Prędkości te mogą być tymczasowo zadawane przez programistę

w następujący sposób:

– jeśli w punkcie pośrednim zmienia się kierunek ruchu prędkość w nim powinna

być równa 0

– jeśli ruch w danej współrzędnej jest duży w stosunku do pozostałych, należy

przyjąć za prędkość w punkcie pośrednim wartość prędkości ruchu jednostajnego w

drugim z ruchów

– jeśli ruch w danej współrzędnej jest mały, należy przyjąć prędkość w punkcie

pośrednim równą 0

Metoda ta została sprawdzona w praktyce na trajektoriach z zadania układania kostki

Rubika i zaowocowała satysfakcjonującymi rezultatami.

5.2. Zastosowanie generatora płynnego ruchu w zadaniu układania kostki Rubika

Praktyczne zastosowanie generatora płynnego ruchu zostało sprawdzone poprzez

udostępnienie go do użycia w prowadzonym w IAiIS zadaniu układania kostki Rubika.

Zadanie to zostało szczegółowo opisane w pracy [JS07], tu skupiono się jedynie na

opisaniu wpływu generatora płynnego ruchu i problemów związanych z jego użyciem.

61

5.2.1. Modyfikacja plików z trajektoriami

Istniejące pliki z trajektoriami (realizowanymi wcześniej przez generator

ECP_teach_in_generator) zostały zmodyfikowane przez dodanie parametrów

niezbędnych dla generatora płynnego ruchu: prędkości początkowych i końcowych,

prędkości ruchu i przyspieszeń. Parametry dobrano intuicyjnie. Ruchy z prędkościami

początkowymi i końcowymi równymi 0 nie przysporzyły trudności, natomiast w

przypadku ruchów bez zatrzymania w punkcie pośrednim metoda zadawania parametrów

ruchu okazała się nieintuicyjna. Po wypracowaniu metody opisanej w podrozdziale 8.1.4

uzyskano satysfakcjonującą płynność ruchów dla wszystkich trajektorii.

5.2.2. Zalety zastosowania generatora płynnego ruchu

Ruchy wykonywane w zadaniu układania kostki Rubika są płynniejsze przy użyciu

ECP_smooth_generator, a przede wszystkim dużo szybsze. Uwolnienie

programisty od zadawania czasów ruchów zaowocowało też optymalizacją ruchów

wykonywanych z nieznanego położenia, które dotychczas wykonywane były z zadanym

długim czasem, który miał umożliwić realizację trajektorii z każdego punktu przestrzeni

roboczej manipulatora.

5.2.3. Wnioski

Generator płynnego ruchu dobrze sprawdza się w rzeczywistych zadaniach

manipulacji.

5.3. Testy algorytmu przelewania

Wyznaczono następujące współrzędne kolejnych punktów (z pominięciem pozycji

chwytaka):

62

Q1,1=[0 ;0.1 ;−1.162 ;0.172 ;1.367 ;2.936 ;1.371 ]

P 1,2=[0.948 ;−0.309 ;0.145 ;−1.269 ;1.529 ;0]

P 1,3=[0.948 ;−0.309 ;0.185 ;−1.269 ;1.529 ;0 ]

P 1,4=[0.777 ;−0.031 ;0.186 ;−0.031 ;1.529 ; 0 ]

P 1,5=[0.607 ;0.262 ;0.185 ;−0.031 ;1.529 ;0.0 ;]

P 1,6=[0.507 ;0.762 ;0.186 ;−0.032 ;1.528 ;0]

Q1,7=[0.16 ;1.247 ;−1.305 ;0.111 ;1.424 ;2.849 ;1.431]

P 1,8=[0.507 ;0.922 ;0.186 ;−0.033 ;1.528 ;−1.18]

P 1,10=[0.510 ;1.088 ;0.255 ;−0.034 ;1.528 ;−2 ]

Q2,1=[−0.072 ;−1.503 ;0.162 ;1.492 ;2.332 ;−1.622 ]

P 2,2=[0.84 ;1.774 ;0.083 ;−0.868 ;1.504 ;−3.128 ]

P 2,3=[0.84 ;1.774 ;0.123 ;−0.868 ;1.504 ;−3.128 ]

P 2,4=[0.429 ;1.105 ;0.152 ;−0.868 ;1.504 ;−3.128]

Przyjęto następujące wartości zadawanych prędkości i przyspieszeń dla ruchów we

współrzędnych wewnętrznych:

– prędkość maksymalna v w , max=0.75

– prędkość „duża” v w , d=0.15

– prędkość „mała” v w , m=0.075

– przyspieszenie a w=0.7

Dla ruchów we współrzędnych zewnętrznych przyjęto następujące wartości prędkości

i przyspieszeń:

– prędkość „duża” v z , d=0.5

– prędkość „średnia” v z , d=0.25

– prędkość „mała” v z , m=0.05

– przyspieszenie a z=0.5

Poniżej przedstawiono zestawienie wartości prędkości zadawanych i obliczonych

przez generator. Dla robota OnTrack:

63

Tr. Wartości zadane (a, v, vpo) Wartości obliczone (vr, t)π1 v1 =[0.75 ;0.75 ;0.75 ;0.75 ;0.75 ;0.75 ;0.75]

a1 =[0.7 ;0.7 ;0.7 ;0.7 ;0.7 ;0.7 ; 0.7]

v r ,1=[0 ;0.029 ;0.059 ;0.023;0.023 ;0.054 ;0.748]

t1 =6.46

p2 v 2 =[0.05 ;0.05 ;0.05 ;0.05 ;0.05 ; 0.05]

a 2 =[0.5 ; 0.5 ;0.5 ;0.5 ; 0.5 ;0.5]

v r ,2=[0 ;0.049 ;0 ;0 ;0 ;0]

t 2 =3.28

p3 v3 =[0.5 ;0.5 ;0.5 ;0.5 ;0.5 ;0.5]

a3 =[0.5 ;0.5 ;0.5 ;0.5 ;0.5 ;0.5]

v r ,3=[0 ;0 ;0.112 ;0 ;0 ;0]

t3 =0.58

p4 v 4,1 =[0.5 ;0.5 ;0.5 ; 0.5 ;0.5 ;0.5]

v 4,2 =[0.5 ;0.5 ;0.5 ; 0.5 ;0.5 ;0.5]

v 4,3 =[0.25 ;0.25 ;0.25 ;0.25 ;0.25 ;0.25 ;0.25]

a 4 =[0.5 ; 0.5 ;0.5 ;0.5 ;0.5 ;0.5]

v r ,4,1=[0.046 ;0.072;0 ;0.498 ;0 ;0]

v r ,4,2=[0.133;0.343 ;0 ;0 ;0 ;0]

v r ,4,3=[0.041;0.25 ;0 ;0 ;0; 0]

t 4,1=3.48

t 4,2=1.54

t 4,2=2.5

π5 v5 =[0.075; 0.075;0.075 ;0.075;0.075 ;0.075;0.075 ]

a5 =[0.35 ;0.35 ;0.35 ;0.35 ;0.35 ;0.35 ;0.35]

v r ,5=[0.074 ;0 ;0 ;0 ;0 ;0 ;0]

t5 =2.36

p6 v6 =[0.05 ;0.05 ;0.05 ;0.05 ;0.05 ;0.05 ;0.05]

a6 =[0.5 ;0.5 ;0.5 ;0.5 ;0.5 ;0.5 ;0.5]

v r ,6=[0 ;0 ;0 ;0 ;0 ;0.05]

t6 =23.72

p7 v7 =[0.05 ; 0.05 ;0.05 ;0.05 ;0.05 ;0.05 ]

a7 =[0.5 ;0.5 ;0.5 ;0.5 ;0.5 ;0.5]

v r ,7=[0 ;0 ;0 ;0 ;0 ;0.05]

t7 =16.52

p8 v8,1 =[0.25 ;0.25 ;0.25 ; 0.25 ;0.25 ; 0.25]

a8,1 =[0.5 ;0.5 ;0.5 ;0.5 ; 0.5 ;0.5]

v8,2 =[0.5 ; 0.5 ;0.5 ;0.5 ; 0.5 ;0.5]

a8,2 =[0.5 ;0.5 ;0.5 ;0.5 ; 0.5 ;0.5]

v r ,8,1=[0; 0;0 ;0 ;0 ;0.248]

t8,1 =3.8

v r ,8,2=[0; 0.05 ;0.02 ;0 ;0; 0.5]

t8,2 =3.36

π9 v9 =[0.075; 0.075;0.075 ;0.075;0.075 ;0.075; 0.075]

a9 =[0.35 ;0.35 ;0.35 ;0.35 ;0.35 ;0.35 ;0.35]

v r ,9=[0.074 ;0 ;0 ;0 ;0 ;0 ;0]

t9 =2.36

p10 v10,1 =[0.5 ;0.5 ;0.5 ;0.5 ;0.5 ;0.5]

v10,2 =[0.075;0.075 ;0.075 ;0.075; 0.075;0.075 ;0.075]

v10,3 =[0.5 ;0.5 ;0.5 ;0.5 ;0.5 ;0.5]

a10 =[0.5 ;0.5 ;0.5 ;0.5 ;0.5 ;0.5]

v r ,10 ,1=[0.051 ;0.436 ;0; 0;0 ;0 ]

v r ,10 ,2=[0.117 ;0.247 ;0 ;0 ;0; 0]

v r ,10 ,3=[0.05 ;0.083;0 ;0.498 ;0 ;0]

t10,1=2.02

t10,2=1.68

t10,3=3.48

p11 v11 =[0.05 ;0.05 ;0.05 ;0.05 ;0.05 ;0.05 ;0.05]

a11 =[0.5 ;0.5 ;0.5 ;0.5 ;0.5 ;0.5 ;0.5]

v r ,11=[0 ;0; 0.048;0 ;0 ]

t11 =0.92

64

Tr. Wartości zadane (a, v, vpo) Wartości obliczone (vr, t)p12 v12 =[0.05 ;0.05 ;0.05 ;0.05 ;0.05 ;0.05 ;0.05]

a12 =[0.5 ;0.5 ;0.5 ;0.5 ;0.5 ;0.5 ;0.5]

v r ,12=[0 ;0.05 ;0 ;0 ;0 ]

t12 =3.06

π13 v13 =[0.75 ;0.75 ;0.75 ;0.75 ;0.75 ;0.75 ;0.75]

a13 =[0.7 ;0.7 ;0.7 ;0.7 ;0.7 ; 0.7 ;0.7]

v r ,13=[0 ;0.027; 0.058;0.019 ;0.024 ;0.051 ;0.747]

t12 =6.48

Dla robota Postument:

Tr. Wartości zadane Wartości obliczoneπ14 v14 =[0.75 ;0.75; 0.75;0.75 ;0.75;0.75 ]

a14 =[0.7 ;0.7 ;0.7 ;0.7 ;0.7 ;0.7]

v r ,14=[0.011; 0.015;0.045 ;0.123;0.095 ;0.732]

t14 =2.56

p15 v15 =[0.05 ;0.05; 0.05;0.05 ;0.05;0.05 ]

a15 =[0.5 ;0.5 ;0.5 ; 0.5 ;0.5 ;0.5]

v r ,15=[0.035;0.048 ;0.05 ;0 ;0 ;0 ]

t15 =1.5

p16 v16 =[0.5 ;0.5 ;0.5 ;0.5 ;0.5 ;0.5 ]

a16 =[0.5 ;0.5 ;0.5 ; 0.5 ;0.5 ;0.5]

v r ,16=[0 ;0 ;0.05 ;0 ;0 ;0 ]

t16 =0.9

p17 v17 =[0.25 ;0.25 ;0.25 ;0.25;0.25 ;0.25]

a17 =[0.5 ;0.5 ;0.5 ;0.5 ;0.5 ;0.5]

v r ,17=[0.141;0.249 ;0.009 ;0 ;0 ;0]

t17 =3.18

p18 v18 =[0.5 ;0.5 ;0.5 ;0.5 ;0.5 ;0.5]

a18 =[0.5 ;0.5 ;0.5 ;0.5 ;0.5 ;0.5]

v r ,18=[0.215;0.497 ;0.012 ;0 ;0 ;0]

t18 =2.34

p19 v19 =[0.05 ;0.05 ;0.05 ;0.05;0.05 ;0.05]

a19 =[0.5 ;0.5 ;0.5 ;0.5 ;0.5 ;0.5]

v r ,19=[0 ;0 ;0.048;0 ;0 ;0 ]

t19 =0.92

π20 v 20 =[0.75 ;0.3 ; 0.45 ;0.3 ;0.3 ;0.45 ;0.6]

a 20 =[0.7 ;0.7 ; 0.7 ;0.7 ;0.7 ;0.7 ;0.7]

v r ,20=[0.011; 0.015;0.045 ;0.123;0.095 ;0.732]

t 20 =2.56

Na kolejnych stronach przedstawiono wykresy przykładowych trajektorii – dla ruchów

od punktu P1,2 do punktu P1,6 dla robota OnTrack (trajektorie p3 i p4)i od punktu P2,2 do

punktu P2,4 dla robota Postument (trajektoria p16 i p17). Ponieważ orientacja końcówki w

tych ruchach się nie zmienia, wykresy ograniczają się do współrzędnych kartezjańskich.

65

66

Rysunek 27: Trajektorie p3 i p4

Rysunek 28: Trajektorie p16 i p17

Wykresy prędkości dla poszczególnych współrzędnych są dla tych trajektorii

następujące:

67

Rysunek 29:Prędkości zadane dla trajektorii p3 i p4

Rysunek 30: Prędkości zadane dla trajektorii p16 i p17

W ruchu z punktu P1,3 do P1,6 przyjęto ruch z zatrzymaniem w obu punktach

pośrednich, ponieważ eksperymenty z ruchami z niezerowymi prędkościami w punktach

pośrednich nie dawały satysfakcjonujących rezultatów.

Na kolejnych stronach przedstawiono zdjęcia obrazujące kolejne etapy ruchu: chwyt z

przejściem przez punkt pośredni (etapy 1 i 2), zetknięcie obiektów ze sobą (etap 4) i

przelewanie (etap 5).

5.3.1 Wnioski

Opracowany algorytm dobrze realizuje wyznaczone zadanie. Wyznaczone prędkości

są zgodne z oczekiwaniami. Prędkości i czasy ruchów dla symetrycznych trajektorii są

takie same. W ruchach odbywających się tylko w jednej współrzędnej prędkość dla tej

współrzędnej jest bliska zadanej, a prędkości w pozostałych zostały zredukowane do

zera.

68

Rysunek 31: OnTrack w pozycji synchronizacji Rysunek 32: OnTrack w punkcie pośrednim między pozycją synchronizacji i punktem chwytania butelki

Rysunek 33: OnTrack w pozycji chwytania butelki Rysunek 34: Postument w pozycji chwytania kubka

69

Rysunek 35: Etap zbliżania obiektów do siebie, OnTrack w punkcie pośrednim P1,4

Rysunek 36: Etap zbliżania obiektów do siebie, OnTrack w punkcie P1,6 , Postument w punkcie P2,4

Rysunek 37: Etap zbliżania obiektów do siebie, ruch OnTracka do punktu P1,7 - obrót wokół osi z

Rysunek 38: Zetkniecie obiektów, OnTrack w punkcie P1,7 , Postument w punkcie P2,4

70

Rysunek 39: Kolejne pozycje przelewania - pozycja pierwsza

Rysunek 40: Kolejne pozycje przelewania - pozycja druga

Rysunek 41: Kolejne pozycje przelewania - pozycja trzecia

Rysunek 42: Kolejne pozycje przelewania - pozycja czwarta

71

72

Rozdział 6

Podsumowanie

Celem pracy było opracowanie, implementacja i eksperymentalna weryfikacja

algorytmów manipulacji dwuręcznej w zadaniu polegającym na przelewaniu płynu

z butelki do kubka. Do realizacji zadania zastosowano system dwurobotowy składający

się z dwóch manipulatorów IRp-6. Sterownik systemu napisano z wykorzystaniem

programowej struktury ramowej MRROC++. Zadanie wykonano z użyciem sieci

komputerów PC, pracujących pod kontrolą systemu czasu rzeczywistego QNX 6.3.

W zadaniu wykorzystano odpowiednio dobrane obiekty, a ich wymiary i początkowe

położenia były znane. Manipulację podzielono na 9 etapów, a ruchy manipulatorów

w poszczególnych etapach wykonywane były z użyciem opracowanego w niniejszej

pracy generatora płynnego ruchu. Kolejne położenia manipulatorów wyznaczono

podczas ręcznego sterowania robotami.

Opracowany algorytm działa poprawnie i realizuje przyjęte założenia.

Na potrzeby zadania manipulacji powstał generator trajektorii płynnego ruchu

manipulatora. Generator został przetestowany dla trzech rodzajów ruchu: do punktu

docelowego bez punktów pośrednich, z jednym punktem pośrednim i równymi

prędkościami obu etapów ruchu oraz z jednym punktem pośrednim i różnymi

prędkościami obu etapów ruchu. Generator został też wykorzystany w zadaniu układania

kostki Rubika. Testy generatora wypadły pomyślnie, usprawnienia wymaga tylko sposób

określania parametrów ruchu przez użytkownika dla ruchów z punktem pośrednim, gdyż

w obecnej postaci zadanie optymalnych parametrów jest bardzo trudne.

Zadanie pozostawia wiele możliwości jego rozszerzenia, związanych zwłaszcza

z użyciem czujników. Możliwa jest rozbudowa algorytmu o detekcję położenia i

kształtów obiektów na podstawie obrazu z kamery, pomiar siły zacisku chwytaków oraz

określanie masy obiektów (i zarazem stopnia ich napełnienia) za pomocą czujników siły.

73

Wskazana jest także przebudowa chwytaków manipulatorów tak, aby mogły one chwytać

obiekty nie przygotowywane do tego specjalnie przez oklejenie gąbką.

74

Literatura (alfabetycznie)

[JC93] J. J. Craig: “Wprowadzenie do robotyki. Mechanika i sterowanie.”,

Wydawnictwa Naukowo-Techniczne, Warszawa 1993

[AE07] A. L. Edsinger „Robot Manipulation in Human Environments”, Massachusetts

Institute of Technology, rozprawa doktorska, Massachusetts 2007

[BF04] B. Finkemeyer, T. Kröger, F. M. Wahl „Executing assembly tasks specified by

manipulation primitive nets”, Institute for Robotics and Process Control, Technical

University of Braunschweig, Braunschweig 2004

[KK03] K. Kozłowski, P. Dutkiewicz, W. Wróblewski: „Modelowanie i sterowanie

robotów”, Wydawnictwo Naukowe PWN, Warszawa 2003

[JN04] J. K. Nowacki: “Generatory trajektorii w systemie MRROC++”, Instytut

Automatyki i Informatyki Stosowanej Politechniki Warszawskiej, praca dyplomowa

inżynierska, Warszawa 2004

[JS07] J. Sałacka „Szybkie algorytmy przeszukiwania drzew rozwiązań w celu

znalezienia sekwencji ruchów ścianami kostki Rubika”, Instytut Automatyki i

Informatyki Stosowanej Politechniki Warszawskiej, praca dyplomowa magisterska,

Warszawa 2007

[GT06] G. Taylor, L. Kleeman „Visual Perception and Robotic Manipulation. 3D

Object Recognition, Tracking and Hand-Eye Coordination”, Springer-Verlag Berlin

Heidelberg 2006

[TW04] T. Winiarski: “Wstępna implementacja sterowania pozycyjno-siłowego w

systemie MRROC++”, Instytut Automatyki i Informatyki Stosowanej Politechniki

Warszawskiej, raport IAIS nr 04-14, Warszawa 2004

75

[CZ99] C. Zieliński, W. Szynkiewicz „System MRROC++ dla robota IRp-6”, Instytut

Automatyki i Informatyki Stosowanej Politechniki Warszawskiej, raport IAIS nr 99-20,

Warszawa 1999

[CZ06] C. Zieliński, W. Szynkiewicz, T. Winiarski, T. Kornuta: „MRROC++ Based

System Description”, Instytut Automatyki i Informatyki Stosowanej Politechniki

Warszawskiej, raport IAIS nr 06-09, Warszawa 2006

76