POLITECHNIKA ÅšL
SKA
WYDZIAA
ELEKTRYCZNY
KATEDRA MECHATRONIKI
Instrukcja do ćwiczenia laboratoryjnego
Przedmiot:
Sterowanie i programowanie robotów
Symbol ćwiczenia:
MP
Wyznaczanie przestrzeni roboczej
Tytuł ćwiczenia:
manipulatora płaskiego
SPIS TREÅšCI
Spis rysunków 2
1. Cele ćwiczenia 3
2. Podstawowe wiadomości 3
3. Laboratoryjne stanowisko badawcze 5
3.1. Obiekt badany 5
3.2. UrzÄ…dzenia dodatkowe 5
3.3. Oprogramowanie 5
4. Program ćwiczenia  wykaz zadań do realizacji 5
5. Przykład realizacji zadania  Człon 1 8
6. Raport 10
7. Pytania 10
Informacje dodatkowe 11
Literatura 11
STEROWANIE I PROGRAMOWANIE ROBOTÓW  instrukcja do ćwiczenia
laboratoryjnego
SPIS RYSUNKÓW
1. Obiekty badane 5
2. Widok głównego kodu programu 6
3. Widok panelu czołowego programu (z przykładowym rozwiązaniem) 6
4. Widok szablonu podprogramu  Matrix_gen_T_szablon.vi 7
5. Algorytm pracy programu 7
6. Zapisywanie danych ze wskaz
nika XY Graph do pliku 8
7. Schemat kinematyczny pierwszego członu robota 8
8. Prawidłowo wypełniony podprogram  Matrix_gen_T.vi 9
9. Widok diagramu programu realizującego zadanie opisywane w przykładzie 10
10. Prezentacja wyników obliczeń w postaci pliku graficznego (BMP) 10
11. Działania na obiektach typu Bundle 11
12. Opcje węzła Build Array 11
13. Struktura danych w klastrze 11
2
Wyznaczanie przestrzeni roboczej manipulatora płaskiego
STEROWANIE I PROGRAMOWANIE ROBOTÓW  instrukcja do ćwiczenia
laboratoryjnego
1. CELE ĆWICZENIA
Celem ćwiczenia jest:
 wyznaczenie przestrzeni roboczej manipulatora płaskiego o trzech stopniach swobody,
 wyznaczenie przestrzeni roboczej manipulatora płaskiego o parametrach geometrycznych
robota Lynx 6.
2. PODSTAWOWE WIADOMOÅšCI
W celu wyznaczenia przestrzeni roboczej manipulatora należy wykonać następujące kroki:
1) Wyliczyć macierz T0n - kinematyka prosta manipulatora,
2) Określić zakresy ruchu wszystkich przegubów,
3) Ustawić wszystkie przeguby w jednym położeniu ekstremalnym (np. minimum)
4) W kolejności od pierwszego do ostatniego przegubu przemieścić do drugiego
położenia ekstremalnego (np. maksimum) rejestrując ruch końcówki roboczej,
5) Powtórzyć krok 4 w odwrotnym kierunku i odwrotnej kolejności ruchu przegubów.
6) Sprawdzić, czy gdzieś nie pominięto pewnych przestrzeni roboczych wynikających
z zakresów ruchu przegubów.
7) Powtórzyć kroki 4 i 5 modyfikując algorytm ruchu przegubów z uwzględnieniem
przestrzeni pominiętych.
2.1. Notacja Denavita-Hatenberga
Kinematyka jest nauką zajmującą się badaniem ruchu, bez uwzględnienia przyczyn ruch ten
wywołujących. Kinematyka opisuje położenie i ruch członów robota oraz pochodne
wyższych rzędów. Kinematyka opisuje więc geometryczne i czasowe aspekty ruchu.
Kinematykę w robotyce można podzielić na kilka działów:
- kinematyka prosta położenia,
- kinematyka prosta prędkości,
- kinematyka odwrotna położenia,
- kinematyka odwrotna prędkości.
Zadaniem kinematyki prostej położenia jest obliczenie położenia i orientacji konkretnego
punktu robota (np. końcówka robocza) na podstawie informacji o aktualnych pozycjach
przegubów. Zadanie kinematyki prostej położenia jest zadaniem jednoznacznym tzn. jednej
wartości nastaw przegubów odpowiada dokładnie jedno położenie badanego punktu.
Zadaniem kinematyki odwrotnej położenia jest obliczenie pozycji przegubów wymagane do
osiągnięcia, przez określony punkt (np. przez końcówkę roboczą) określonej pozycji. Zadanie
to jest niejednoznaczne. Nawet w prostych konfiguracjach robotów (robot z łokciem)
możliwych jest kilka rozwiązań.
Matematyczny opis konfiguracji członów robota za pomocą czterech parametrów (długość,
skręcenie, przesunięcie, kąt konfiguracji) nosi nazwę Notacji Denavita-Hatenberga.
Wprowadzając zmienne przegubowe dla obrotu członów lub ich translacji (przesunięcia)
można przeprowadzać obliczenia kinematyki dla dowolnych ustawień przegubów.
Położenie każdego członu względem poprzedniego jest określone macierzą przekształcenia
jednorodnego (A) składającą się z czterech podstawowych ruchów (wzór 1) opisanych
wzorami (2):
Ai =ð Rotz,qð Transz,d Transx,a Rotx,að
(1)
3
Wyznaczanie przestrzeni roboczej manipulatora płaskiego
STEROWANIE I PROGRAMOWANIE ROBOTÓW  instrukcja do ćwiczenia
laboratoryjnego
cqð -ð sqð 0 0 1 0 0 0
éð Å‚ð éð Å‚ð
Ä™ðs cqð 0 0Å›ð Ä™ð0 cað -ð sað 0Å›ð
qð
Ä™ð Å›ð Ä™ð Å›ð
Rotz,qð =ð Rotx,að =ð
Ä™ð Å›ð Ä™ð Å›ð
0 0 1 0 0 sað cað 0
Ä™ð Ä™ð0 0 0 1Å›ð
0 0 0 1Å›ð
ëð ûð ëð ûð
(2)
1 0 0 0 1 0 0 a
éð Å‚ð éð Å‚ð
Ä™ð0 1 0 0Å›ð Ä™ð0 1 0 0Å›ð
Ä™ð Å›ð Ä™ð Å›ð
Transz,d =ð Transx,a =ð
Ä™ð Å›ð Ä™ð Å›ð
0 0 1 d 0 0 1 0
Ä™ð0 0 0 1Å›ð Ä™ð0 0 0 1Å›ð
ëð ûð ëð ûð
Budowa macierzy A została przedstawiona we wzorze 3:
cqð -ð sqð cað sqð sað acqð
éð Å‚ð
Ä™ð
sqð cqð cað -ð cqð sað asqð Å›ð [ðR3x3]ð [ðT3x1]ð
éð Å‚ð
i-ð1
Ä™ð Å›ð
Ai =ð (3)
Ai =ð [ð T]ð=ð
i Ä™ð
Ä™ð Å›ð
0 sað cað d
[ðf1x3]ð [ðs1x1]ðÅ›ð
ëð ûð
Ä™ð Å›ð
0 0 0 1
ëð ûð
Gdzie R  macierz rotacji, T  macierz translacji (przesunięcia), s  skala, f  perspektywa.
W robotyce wektor f oraz skalar s mają stałe wartości.
W celu wyliczenia położenia punktu w bazowym układzie współrzędnych należy wyliczyć
macierz przekształcenia (T):
(4)
T0n =ð A1...An
Gdzie 0 to bazowy układ współrzędnych, natomiast n to transformowany układ
współrzędnych, macierze A1 do An to wszystkie macierze przekształceń jednorodnych, przez
które następuje transformacja z układu n do układu 0.
Algorytm Denavita-Hatenberga:
1) Narysować schemat łańcucha kinematycznego manipulatora.
2) Umieścić i oznaczyć osie przegubów z0, & ,zn-1.
3) Przyjąć bazowy układ współrzędnych. Jego początek umieścić dowolnie na osi z0. Osie x0
i y0 wybrać tak, aby układ był prawoskrętny. Dla i = 1 , & , n-1 wykonać kroki 4, 5, 6.
4) Umieścić środek Oi w miejscu, gdzie wspólna normalna do osi zi i zi-1 przecina oś zi. Jeśli
oś zi przecina oś zi-1, to umieścić początek Oi w tym przecięciu. Jeśli te osie są równoległe,
to umieścić Oi na przegubie i.
5) Przyjąć oś xi wzdłuż wspólnej normalnej osi zi i zi-1 przechodzącej przez początek Oi lub
w kierunku normalnej do płaszczyzny obu tych osi, jeśli zi i zi-1 przecinają się.
6) Wybrać oś yi tak, by układ był prawoskrętny.
7) Ustalić układ współrzędnych końcówki roboczej Onxnynzn. Zakładając, że oś n jest
obrotowa, przyjąć kn = a wzdłuż kierunku zn-1. Wybrać On na osi zn; preferowany jest
środek chwytaka lub czubek narzędzia, z którym pracuje robot. Przyjąć jn = s w kierunku
zamykania chwytaka oraz in = n jako s ´ð a. JeÅ›li narzÄ™dzie nie jest zwykÅ‚ym chwytakiem,
przyjąć xn i yn według uznania, zachowując tylko zasady układu prawoskrętnego.
8) Utworzyć tabelÄ™ parametrów kinematycznych ai, di, aði, qði, gdzie:
- ai  (długość członu) odległość wzdłuż osi xi od początku Oi do przecięcia osi xi i zi-1.
- di  (odsunięcie) odległość wzdłuż osi zi-1 od początku Oi-1 do przecięcia osi xi i zi-1;
jeśli przegub jest pryzmatyczny to di jest zmienną,
- aði  (skrÄ™cenie) kÄ…t miÄ™dzy osiami zi-1 i zi mierzony wokół osi xi,
4
Wyznaczanie przestrzeni roboczej manipulatora płaskiego
STEROWANIE I PROGRAMOWANIE ROBOTÓW  instrukcja do ćwiczenia
laboratoryjnego
- qði  (kÄ…t konfiguracji) kÄ…t miÄ™dzy osiami xi-1 i xi mierzony wokół osi zi-1;
jeÅ›li przegub jest obrotowy, to qði jest zmiennÄ….
9) Zbudować macierze przekształceń jednorodnych Ai.
n
Utworzyć macierz T0 =ð A1...An ;
3. LABORATORYJNE STANOWISKO BADAWCZE
3.1. Obiekt badany
Obiektem badanym jest manipulator płaski przedstawiony na rys. X. a). W drugiej
części ćwiczenia obiektem badanym jest manipulator Lynx 6 przestawiony na rys. X. b).
a)
b)
Rys. 1. Obiekty badane.
a) Manipulator płaski (L1 = 400 mm, L2 = 350 mm, L3 = 150 mm),
b) Manipulator Lynx 6 [2].
3.2. UrzÄ…dzenia dodatkowe
Komputer PC,
3.3. Oprogramowanie
 LabVIEW,
 szablon programu napisanego w LabVIEW  SiPR_DH_full_szablon.vi oraz dodatkowe
pliki  Matrix_gen_T_szablon.vi i  DH_input_conv.vi .
4. PROGRAM ĆWICZENIA  WYKAZ ZADAC
DO REALIZACJI
 Wyliczyć macierz przekształcenia T0n dla manipulatora płaskiego z rys. X. a), posługując się
algorytmem DH podanym w rozdziale 2.
 wypełnić tabelę 1 (umieścić tabelę w protokole).
5
Wyznaczanie przestrzeni roboczej manipulatora płaskiego
STEROWANIE I PROGRAMOWANIE ROBOTÓW  instrukcja do ćwiczenia
laboratoryjnego
Tabela 1. Tabela parametrów kinematycznych
Długość Odsunięcie Skręcenie Konfiguracja
Przegub
ai-1 di
aði-1 qði
1
2
&
 wypełnić tabelę 2, dane do tabeli uzgodnić z prowadzącym ćwiczenie (umieścić tabelę
w protokole).
Tabela 2. Zakres ruchu przegubów
KÄ…t minimalny qðMIN KÄ…t maksymalny qðMAX
Przegub
[O] [O]
1
2
&
 Uruchomić program LabVIEW, otworzyć główny plik  SiPR_DH_full_szablon.vi (kod
programu  Rys. 2, panel  Rys. 3). Zapisać program pod nową nazwą z numerem sekcji.
Rys. 2. Widok głównego kodu programu.
Rys. 3. Widok panelu czołowego programu (z przykładowym rozwiązaniem).
6
Wyznaczanie przestrzeni roboczej manipulatora płaskiego
STEROWANIE I PROGRAMOWANIE ROBOTÓW  instrukcja do ćwiczenia
laboratoryjnego
Program składa się z trzech części:
üð Wprowadzania danych wejÅ›ciowych  zakresy kÄ…towe podawane jako wielokrotnoÅ›ci
lub podwielokrotnoÅ›ci liczby pð. Np. t1max = 0,5 wynosi po konwersji 0,5 x pð = 1,57.
üð ObliczeÅ„  część do uzupeÅ‚nienia na zajÄ™ciach laboratoryjnych,
üð Prezentacji wyników.
 W programie LabVIEW obliczyć macierz T0n , w celu obliczenia macierzy T przygotowano
plik subvi  Matrix_gen_T_szablon.vi w odpowiednich miejscach należy wpisać macierze
A1 A4 zgodnie z notacjÄ… stosowanÄ… w programie MatLAB (Rys. 4). W szablonie znajdujÄ…
siÄ™ macierze jednostkowe diagonalne.
Rys. 4. Widok szablonu podprogramu  Matrix_gen_T_szablon.vi .
 Zapisać plik  Matrix_gen_T_szablon.vi pod nową nazwą,
 Skopiować plik wymaganą liczbę razy, aby program zrealizował zadanie opisane
algorytmem przedstawionym na rysunku 5.
Rys. 5. Algorytm pracy programu.
7
Wyznaczanie przestrzeni roboczej manipulatora płaskiego
STEROWANIE I PROGRAMOWANIE ROBOTÓW  instrukcja do ćwiczenia
laboratoryjnego
 Upewnić się, czy nie pominięto jakichś przestrzeni  jeśli tak to powtórzyć symulację
zmieniając algorytm ruchu przegubów.
 Zarchiwizować dane, dane można zapisać w postaci pliku graficznego (bmp) lub
tekstowego pliku danych (txt). W tym celu należy kliknąć na XY Graph PPM -> Export (Rys.
6).
b)
a) c)
Rys. 6. Zapisywanie danych ze wskaz
nika XY Graph do pliku:
a) Wybór opcji PPM wskaz
nika XY Graph,
b) Fragment pliku tekstowego wyeksportowanego przez schowek,
c) WyglÄ…d pliku graficznego (bmp).
 Zmierzyć długości członów i zakresy ruchów wskazanych przegubów robota Lynx 6.
 Uprościć robota Lynx 6 do realizacji zadania manipulatora płaskiego i wyznaczyć
przestrzeń roboczą uproszczonego manipulatora Lynx 6.
5. PRZYKA
AD REALIZACJI ZADANIA  CZA
ON 1
Jako przykład podany jest ruch końcówki pierwszego członu ramienia robota o długości L1.
1. Schemat kinematyczny (Rys. 7)
Rys. 7. Schemat kinematyczny pierwszego członu robota.
8
Wyznaczanie przestrzeni roboczej manipulatora płaskiego
STEROWANIE I PROGRAMOWANIE ROBOTÓW  instrukcja do ćwiczenia
laboratoryjnego
Układ zawiera trzy układy współrzędnych:
- O0  bazowy (nieruchomy) układ współrzędnych,
- O1  układ współrzędnych połączony z członem 1 usytułowany w początku tego
członu. Oś OX jest  związana z członem 1. Układy O0 i O1 mają ten sam punkt
(0,0,0) więc występuje między nimi tylko rotacja.
- O2  układ współrzędnych połączony z członem 1 usytułowany w końcu tego członu.
Między układami O1 i O2 istnieje przesunięcie (translacja) równa długości członu 1.
Aby opisać położenie układu O2 w bazowym układzie O0 należy wyznaczyć dwie
macierze przekształcenia jednorodnego A1 oraz A2 a następnie wymnożyć je
odpowiednio.
(kroki od 1 do 7 algorytmu DH)
2. Na podstawie rysunku wypełniono tabelę 1 oraz zadano zakres ruchu (tabela 2)
Tabela 3. Tabela parametrów kinematycznych Tabela 4. Zakres ruchu przegubów
Długość Odsunięcie Skręcenie Konfiguracja Kąt Kąt
Przegub
ai-1 di minimalny maksymalny
aði-1 qði
Przegub
qðMIN qðMAX
1 0 0 0
qð1*
[O] [O]
2 L1 0 0 0
1 0
90 (pð/2)
(krok 8 algorytmu DH)
3. Na podstawie tabeli 3 opracowano macierze przekształceń jednorodnych A1 oraz A2:
;
(krok 9 algorytmu DH)
4. Uzupełniono podprogram  Matrix_gen_T.vi wpisując macierze A1 oraz A2 (rys. 8).
UWAGA! Macierze wpisuje siÄ™ zgodnie z notacjÄ… MATlaba.
Rys. 8. Prawidłowo wypełniony podprogram  Matrix_gen_T.vi .
9
Wyznaczanie przestrzeni roboczej manipulatora płaskiego
STEROWANIE I PROGRAMOWANIE ROBOTÓW  instrukcja do ćwiczenia
laboratoryjnego
5. Dostosowano program główny do obliczeń jednego członu w dwóch kierunkach ruchu
(Rys. 9).
Rys. 9. Widok diagramu programu realizującego zadanie opisywane w przykładzie.
6. Przeprowadzono eksport danych do pliku graficznego (BMP) oraz pliku danych (Rys.
10). Ponieważ w przykładzie ruch końcówki odbywa się po tej samej trajektorii w
obydwie strony widać tylko jedną krzywą. W przypadku bardziej złożonej struktury
kinematycznej widać krzywą zamkniętą (Rys. 3, Rys. 6c)
Rys. 10. Prezentacja wyników obliczeń w postaci pliku graficznego (BMP).
7. Zapis danych do pliku tekstowego umożliwia porównanie przestrzeni roboczych przy
różnych konfiguracjach robota (różnych zakresach ruchu lub długości członów).
6. RAPORT
Raport z ćwiczenia powinien zawierać:
 opis zadania do realizacji,
 wyniki realizacji każdego kroku,
 uwagi i wnioski.
7. PYTANIA
1) Założenia dla członów robota.
2) Czy układ współrzędnych musi znajdować się w środku przegubu?
10
Wyznaczanie przestrzeni roboczej manipulatora płaskiego
STEROWANIE I PROGRAMOWANIE ROBOTÓW  instrukcja do ćwiczenia
laboratoryjnego
INFORMACJE DODATKOWE
Podstawowe operacje w programie LabVIEW wymagane do wykonania ćwiczenia.
Powielanie węzłów lub fragmentów programu  zaznaczyć wymagany obiekt, przeciągnąć
fragment LPM+Ctrl.
Zmiana rozmiaru węzłów: Bungle By Name, Unbungle By Name, Build Array  zaznaczyć
obiekt, rozszerzyć obiekt ruchem kursora w dół lub w górę trzymając LPM (Rys. 11.a).
Wybór elementu z klastra danych w węzłach Bungle By Name, Unbungle By Name LPM
(Rys. 11.b).
a) b)
Rys. 11. Działania na obiektach typu Bundle:
a) Rozszerzanie  zwiększanie liczby elementów
b) Wybór elementu klastra danych
Składanie macierzy z wykorzystaniem węzła Build Array  PPM, zaznaczyć opcję
Concatenate Inputs (rys. 11).
Rys. 12. Opcje węzła Build Array Rys. 13. Struktura danych w klastrze
LITERATURA
[1] Spong M. W., Vidyasagar M.: Dynamika i sterowanie robotów. WNT Warszawa 1997,
[2] http://users.ece.gatech.edu/~fumin/Teaching.html
Opracowanie: Marek Kciuk
11
Wyznaczanie przestrzeni roboczej manipulatora płaskiego