Manipulacja obiektami przez roboty
Transcript of Manipulacja obiektami przez roboty
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