Manipulator przeznaczony do celów dydaktycznych - Laboratorium ...

Politechnika Wrocławska
Wydział Elektroniki
Kierunek:
Specjalność:
Automatyka i Robotyka (AiR)
Robotyka (ARR)
PRACA DYPLOMOWA
MAGISTERSKA
Manipulator
przeznaczony do celów
dydaktycznych.
Autor:
Zbigniew Struzik
Prowadzący pracę:
dr inż. Marek Wnuk, I-6
Ocena pracy:
Wrocław 2005

Składam serdeczne podziękowania Panu
dr. Markowi Wnukowi za pomoc,
poświęcony czas oraz zaangażowanie.
Chciałem również podziękować Panu
Ryszardowi Krzywańskiemu za
cenne rady i pomoc w wykonaniu konstrukcji
mechanicznej. Na końcu chciałem
podziękować firmom MICRO-
MOTORS i AUSTRIAMICRO-
SYSTEMS za bezpłatne przekazanie
silników i koderów.

1
Spis treści
1 Wstęp 6
1.1 Cel i zakres pracy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
2 Kinematyka manipulatora 7
2.1 Reprezentacja Denavita-Hartenberga . . . . . . . . . . . . . . . . . . . . . 7
2.2 Wyprowadzenie kinematyki prostej
manipulatora . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
2.3 Kinematyka odwrotna manipulatora – podejście geometryczne . . . . . . . 10
2.4 Konfiguracje wyróżnione manipulatora . . . . . . . . . . . . . . . . . . . . 13
2.5 Transformacja napędowa osi robota . . . . . . . . . . . . . . . . . . . . . . 14
2.5.1 Rozwiązanie napędów ogniwa 1,2 i 3 . . . . . . . . . . . . . . . . . 14
2.5.2 Rozwiązanie napędu ogniwa 4 i 5 . . . . . . . . . . . . . . . . . . . 16
3 Konstrukcja robota ZS5R 18
3.1 Konstrukcja mechaniczna . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
3.1.1 Silniki manipulatora . . . . . . . . . . . . . . . . . . . . . . . . . . 20
3.2 Konstrukcja elektroniczna – sterownik robota . . . . . . . . . . . . . . . . 23
3.2.1 Budowa modułu sterowania . . . . . . . . . . . . . . . . . . . . . . 24
3.2.2 Budowa modułu mocy . . . . . . . . . . . . . . . . . . . . . . . . . 25
3.2.3 Opis łączówek sterownika . . . . . . . . . . . . . . . . . . . . . . . 27
4 Oprogramowanie sterownika 31
4.1 Funkcja główna main() . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31
4.2 Procedura obsługi przerwania cyklicznego . . . . . . . . . . . . . . . . . . 32
5 Badania i wyniki eksperymentów 36
6 Podsumowanie 38
7 Dodatek A 41
7.1 Parametry mechaniczne manipulatora . . . . . . . . . . . . . . . . . . . . . 41
7.2 Rysunki techniczne części mechanicznej . . . . . . . . . . . . . . . . . . . . 42
8 Dodatek B 60
8.1 Wykaz elementów części elektronicznej . . . . . . . . . . . . . . . . . . . . 60
9 Dodatek C 63
9.1 Schematy ideowe i rysunki montażowe części elektronicznej . . . . . . . . . 63

10 Przykładowa instrukcja laboratoryjna 73
10.1 Cel ćwiczenia. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
10.2 Podstawy teoretyczne. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73
10.2.1 Charakterystyka robota ZS5R. . . . . . . . . . . . . . . . . . . . . . 73
10.2.2 Struktura kinematyczna dla współrzędnych
przegubowych q i . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74
10.2.3 Transformacja Denavita-Hartenberga dla zmiennych q i . . . . . . . . 75
10.3 Przebieg ćwiczenia . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75
10.3.1 Obsługa stanowiska . . . . . . . . . . . . . . . . . . . . . . . . . . . 75
10.3.2 Wstępne przygotowanie do ćwiczenia. . . . . . . . . . . . . . . . . . 76
10.3.3 Zadania do wykonania. . . . . . . . . . . . . . . . . . . . . . . . . . 76
2

3
Spis rysunków
2.1 Struktura kinematyczna manipulator ZS5R. . . . . . . . . . . . . . . . . . 8
2.2 Struktura szkieletowa robota. . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.3 Struktura układu przeniesienia napędu ogniwa 1 i 2. . . . . . . . . . . . . . 14
2.4 Struktura układu przeniesienia napędu ogniwa 3. . . . . . . . . . . . . . . 16
2.5 Struktura układu różnicowego napędzającego osie nadgarstka. . . . . . . . 17
3.1 Zdjęcie manipulator. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18
3.2 Rysunek złożeniowy manipulatora z oznaczonymi głównymi częściami. . . . 19
3.3 Zdjęcie silnika HLE149.24.43. . . . . . . . . . . . . . . . . . . . . . . . . . 21
3.4 Zdjęcie silnika RHE158.24.100. . . . . . . . . . . . . . . . . . . . . . . . . . 21
3.5 Zdjęcie ilustrujące sposób zamontowania magnesu na osi silnika. . . . . . . 22
3.6 Zdjęcie ilustrujące sposób zamontowania kodera AS5040. . . . . . . . . . . 22
3.7 Schemat blokowy układu sterowania manipulatora. . . . . . . . . . . . . . 23
3.8 Płytka sterownika . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
3.9 Moduł EM332/B. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
3.10 Schemat blokowy regulatora prądu. . . . . . . . . . . . . . . . . . . . . . . 26
3.11 Zasilacz części cyfrowej i analogowej . . . . . . . . . . . . . . . . . . . . . . 26
4.1 Schemat blokowy układu sterowania. . . . . . . . . . . . . . . . . . . . . . 34
5.1 Wykres odpowiedzi regulatora na zadany prąd. . . . . . . . . . . . . . . . 36
5.2 Rozpędzanie osi q 0 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
5.3 Regulacja prędkości osi q 0 . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
6.1 Widok skonstruowanego manipulatora. . . . . . . . . . . . . . . . . . . . . 38
7.1 Rysunek techniczny silnika RHE 158.24.100. . . . . . . . . . . . . . . . . . 42
7.2 Rysunek techniczny silnika HLE 149.24.43. . . . . . . . . . . . . . . . . . . 42
9.1 Schemat ideowy jednego z regulatorów prądu modułu mocy. . . . . . . . . 63
9.2 Schemat ideowy połączenia modułu EM332 z układami peryferyjnymi. . . 64
9.3 Schemat ideowy zasilacza. . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
9.4 Schemat ideowy układu sterowania cewką przekaźnika. . . . . . . . . . . . 65
9.5 Schemat elektryczny kodera AS5040. . . . . . . . . . . . . . . . . . . . . . 66
9.6 Obwód drukowany wierzchniej warstwy płytki zasilacza. . . . . . . . . . . . 66
9.7 Obwód drukowany spodniej warstwy płytki zasilacza. . . . . . . . . . . . . 66
9.8 Rozmieszczenie elementów na wierzchniej stronie płytki zasilacza. . . . . . 67
9.9 Obwód drukowany wierzchniej warstwy płytki sterownika. . . . . . . . . . 68
9.10 Obwód drukowany spodniej warstwy płytki sterownika. . . . . . . . . . . . 69
9.11 Rozmieszczenie elementów na wierzchniej stronie płytki sterownika. . . . . 70

4
9.12 Rozmieszczenie elementów na spodniej stronie płytki sterownika. . . . . . . 71
9.13 Obwód drukowany wierzchniej warstwy płytki kodera AS5040. . . . . . . . 72
9.14 Obwód drukowany spodnie warstwy płytki kodera AS5040. . . . . . . . . . 72
9.15 Rozmieszczenie elementów na wierzchniej warstwie płytki kodera AS5040. . 72
9.16 Rozmieszczenie elementów na spodniej warstwie płytki kodera AS5040. . . 72
10.1 Struktura kinematyczna manipulator ZS5R. . . . . . . . . . . . . . . . . . 74

5
Spis tabel
2.1 Parametry Denavita-Hartenberga manipulatora. . . . . . . . . . . . . . . . 8
3.1 Parametry silnika typu RHE158.24.100. . . . . . . . . . . . . . . . . . . . . 20
3.2 Parametry silnika typu HLE149.24.43. . . . . . . . . . . . . . . . . . . . . 21
3.3 Łączówka transmisji szeregowej Z 10 . . . . . . . . . . . . . . . . . . . . . . . 27
3.4 Sygnały łączówki transmisji szeregowej Z 10 . . . . . . . . . . . . . . . . . . 27
3.5 Łączówka Z 8 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28
3.6 Opis sygnałów łączówki Z 8 . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
3.7 Łączówka Z 9 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29
3.8 Opis sygnałów łączówki Z 9 . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
3.9 Opis złącza zasilania modułu mocy Z 7 . . . . . . . . . . . . . . . . . . . . . 30
3.10 Opis złącza zasilania Z 12 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
3.11 Opis złącza na płytce zasilacz Z 13 . . . . . . . . . . . . . . . . . . . . . . . . 30
3.12 Opis złącz Z 1 ,...Z 6 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30
8.1 Zestawienie elementów elektronicznych na płytce zasilacza. . . . . . . . . . 60
8.2 Zestawienie elementów elektronicznych na płytce kodera AS5040. . . . . . 60
8.3 Zestawienie elementów elektronicznych na płytce sterownika. . . . . . . . . 61
8.4 Zestawienie elementów elektronicznych na płytce sterownika. . . . . . . . . 62

6
Rozdział 1
Wstęp
W laboratorium robotyki Zakładu Podstaw Cybernetyki i Robotyki, znajduje się manipulator
dydaktyczny Romik. Robot ten jest klasy 5R, posiadający pięć stopni swobody. Do
napędu jego poszczególnych osi zastosowano silniki skokowe. Konstrukcja mechaniczna i
układ sterowania robota, pozwala jedynie na wykonywanie na nim podstawowych ćwiczeń
związanych z identyfikacją parametrów geometrycznych manipulatora. Natomiast ze
względu na zastosowane do napędu osi silniki, nie pozwala on już na implementację algorytmów
sterowania uwzględniających dynamikę robota. Poza tym, konstrukcja robota
jest dość przestarzała i wyeksploatowana mechanicznie. Z tego względu powstała potrzeba
zbudowania nowego manipulatora dydaktycznego, w którym napęd przegubów byłby zrealizowany
za pomocą silników prądu stałego. Powinien on dawać możliwość wykonywania
dotychczasowych ćwiczeń jak i umożliwić w przyszłości implementację algorytmów sterowania
opartych na dynamice robota. Także elektronika nowego robota powinien dawać
możliwość podłączenia zewnętrznego układu sterowania, na przykład karty współpracującej
z pakietem MATLAB.
1.1 Cel i zakres pracy
Podstawowym celem pracy jest stworzenie stanowiska laboratoryjnego, którego głównym
elementem jest manipulator dydaktyczny. Stworzenie stanowiska realizowane jest przez:
• wykonanie od podstaw części mechanicznej manipulatora,
• zaprojektowanie i wykonanie elektroniki robota, pozwalającej współpracować z zewnętrznym
układem sterowania,
• napisanie oprogramowania,
• przeprowadzenie badań,
• opracowanie przykładowej instrukcji laboratoryjnej.
W pracy tej zawarto teoretyczne podstawy dotyczące kinematyki prostej i odwrotnej
manipulatora jak i przedstawiono transformację napędową. Zamieszczono opis konstrukcji
części mechanicznej i elektronicznej robota (dokładną dokumentację techniczną robota
można znaleźć w dodatkach A, B i C). Opisano też oprogramowanie służące do obsługi
układów wykonawczych. Udokumentowano wykresy i wyniki eksperymentów uzyskanych
za pomocą wewnętrznych czujników manipulatora.

7
Rozdział 2
Kinematyka manipulatora
2.1 Reprezentacja Denavita-Hartenberga
Wyznaczenie kinematyki według notacji Denavita-Hartenberga [4] polega na związaniu
z każdym ramieniem (ogniwem) lokalnego układu współrzędnych umieszczonego w odpowiednim
przegubie, a następnie wyznaczeniu ciągu transformacji pomiędzy kolejnymi
układami. W podstawie zaczepiamy nieruchomy układ oznaczony numerem 0. Dalej wybieramy
układy od 1 do n tak, że układ i jest na sztywno związany z ogniwem i w taki
sposób, że
• oś Z i jest osią (i + 1)-go złącza, a zwrot tej osi może być przyjęty dowolnie;
• oś X i jest wspólną normalną do osi złączy i-tego i (i + 1)-go i skierowana jest w
stronę ogniw o wyższych numerach;
• oś Y i jest uzupełnieniem dwóch poprzednich osi do prawoskrętnego, kartezjańskiego
układu współrzędnych. Ponieważ jest ona jednoznacznie określona przez położenia
osi Z i oraz X i zatem niektórzy pomijają ją aby zwiększyć czytelność rysunków.
Transformacja A i i−1 pomiędzy układami (i − 1) oraz i jest zdefiniowana jako iloczyn
czterech macierzy elementarnych obrotów i przesunięć.
A i i−1 (q i) = Rot(Z, θ i )Trans(Z, d i )Trans(X, a i )Rot(X, α i ) (2.1)
gdzie:
θ i - kąt obrotu wokół osi Z i−1 ,
d i - translacja wzdłuż bieżącej osi Z,
a i - translacja wzdłuż bieżącej osi X,
α i - kąt obrotu wokół bieżącej osi X,
są parametrami Denavita-Hartenberga. Kinematyka manipulatora wyznacza pozycję i
orientację układu efektora w bazowym układzie współrzędnych, i jest opisana złożeniem
transformacji (2.1) czyli
n∏
K(q) = A i i−1(q i ) =
i=1
[
R
n
0 (q) T n 0(q)
0 1
]
. (2.2)

8
2.2 Wyprowadzenie kinematyki prostej
manipulatora
Rozważany manipulator jest manipulatorem klasy 5R 1 , posiadającym pięć stopni swobody.
Manipulator ten składa się z pionowej, obrotowej kolumny o wysokości l 1 , dwuczłonowego
ramienia o długości l 2 i l 3 oraz nadgarstka z efektorem o długości l 4 , ujmującej długość
palców chwytaka. Wszystkie te ogniwa - ramiona są połączone przegubami, których
aktualny stan określa wektor stanu q = (q 1 , q 2 , q 3 , q 4 , q 5 ) T . Strukturę tego manipulatora
wraz z układami współrzędnych pokazano na rysunku 2.1. Układy współrzędnych zostały
Rysunek 2.1: Struktura kinematyczna manipulator ZS5R.
przyporządkowane zgodnie z algorytmem Denavita-Hartenberga. Następnym krokiem jest
wyznaczenie parametrów i zmiennych wiążących te układy. Zestawiono je w tabeli 2.1.
Nr ogniwa θ i d i a i α i
π
1 q 1 l 1 0
2
2 q 2 0 l 2 0
3 q 3 0 l 3 0
4 q 4 − π 0 0 − π 2 2
5 q 5 l 4 0 0
Tabela 2.1: Parametry Denavita-Hartenberga manipulatora.
1 Oznacza to, że manipulator posiada pięć przegubów typu rotacyjnego (R).

9
W celu wyliczenia kinematyki manipulatora wyznaczamy transformacje między kolejnymi
układami współrzędnych zgodnie z (2.1).
A 1 0 (q 1) = Rot(Z, q 1 )Trans(Z, l 1 )Rot(X, π),
2
A 2 1 (q 2) = Rot(Z, q 2 )Trans(X, l 2 ),
A 3 2 (q 3) = Rot(Z, q 3 )Trans(X, l 3 ),
A 4 3 (q 4) = Rot(Z, q 4 )Rot(Z, − π)Rot(X, − π),
2 2
A 4 5(q 5 ) = Rot(Z, q 5 )Trans(Z, l 4 ),
(2.3)
Po wprowadzeniu oznaczeń sinq i = s i , cosq i = c i , kolejne macierze przyjmują postać:
• przejście od układu 0 do 1:
• przejście od układu 1 do 2:
⎡
A 1 0(q 1 ) = ⎢
⎣
c 1 0 s 1 0
s 1 0 −c 1 0
0 1 0 l 1
0 0 0 1
⎤
⎥
⎦ ,
• przejście od układu 2 do 3:
⎡
A 2 1 (q 2) = ⎢
⎣
c 2 −s 2 0 l 2 c 2
s 2 c 2 0 l 2 s 2
0 0 1 0
0 0 0 1
⎤
⎥
⎦ ,
• przejście od układu 3 do 4:
⎡
A 3 2 (q 3) = ⎢
⎣
c 3 −s 3 0 l 3 c 3
s 3 c 3 0 l 3 s 3
0 0 1 0
0 0 0 1
⎤
⎥
⎦ ,
• przejście od układu 4 do 5:
⎡
A 4 3(q 4 ) = ⎢
⎣
s 4 0 c 4 0
−c 4 0 s 4 0
0 1 0 0
0 0 0 1
⎤
⎥
⎦ ,

10
⎡
A 5 4 (q 5) = ⎢
⎣
c 5 −s 5 0 0
s 5 c 5 0 0
0 0 1 l 4
0 0 0 1
Mając wyliczone transformacje A i i−1 (q i) między sąsiednimi układami współrzędnych definiujemy
kinematykę manipulatora zgodnie z (2.2) jako
⎤
⎥
⎦ .
czyli
K(q) = A 1 0 A2 1 A3 2 A4 3 A5 4 (q): X 0Y 0 Z 0 ↦−→ X 5 Y 5 Z 5
⎡
K(q) = ⎢
⎣
r 11 r 12 r 13 l x
r 21 r 22 r 23 l y
r 31 r 32 r 33 l z
0 0 0 1
⎤
⎥
⎦ , (2.4)
gdzie elementy tej macierzy są następujące:
⎫
r 11 = cosq 1 cosq 5 sin(q 2 + q 3 + q 4 ) − sinq 1 sinq 5 ,
r 12 = −cosq 1 sinq 5 sin(q 2 + q 3 + q 4 ) − sinq 1 cosq 5 ,
r 13 = cosq 1 cos(q 2 + q 3 + q 4 ),
r 21 = sinq 1 cosq 5 sin(q 2 + q 3 + q 4 ) + cosq 1 sinq 5 ,
r 22 = −sinq 1 sinq 5 sin(q 2 + q 3 + q 4 ) + cosq 1 cosq 5 ,
r 23 = sinq 1 cos(q 2 + q 3 + q 4 ),
⎪⎬
r 31 = −cos(q 2 + q 3 + q 4 )cosq 5 ,
r 32 = cos(q 2 + q 3 + q 4 )sinq 5 ,
r 33 = sin(q 2 + q 3 + q 4 ),
l x = l 2 cosq 1 cos 2 + l 3 cosq 1 cos(q 2 + q 3 ) + l 4 cosq 1 cos(q 2 + q 3 + q 4 ),
l y = l 2 sinq 1 cos 2 + l 3 sinq 1 cos(q 2 + q 3 ) + l 4 sinq 1 cos(q 2 + q 3 + q 4 ),
l z = l 1 + l 2 sinq 2 + l 3 sin(q 2 + q 3 ).
⎪⎭
(2.5)
2.3 Kinematyka odwrotna manipulatora – podejście
geometryczne
W poprzednim punkcie została wyznaczona kinematyka prosta manipulatora, która wyznacza
położenie i orientację efektora w zależności od konfiguracji jego przegubów. W
tym punkcie zostanie przedstawione zadanie odwrotne, a mianowicie wyliczenie wartości
kolejnych zmiennych przegubowych przy narzuconym położeniu i orientacji efektora. Zadanie
to jest nazywane odwrotnym zadaniem kinematyki. Rozwiązanie zadania kinematyki
odwrotnej jest niezwykle istotne ze względu na efektywność sterowania manipulatorem
robota. Zadania do wykonania przez robota są bowiem w sposób naturalny formułowane
w kategoriach pożądanych położeń i orientacji efektora, wyrażonych w układzie kartezjańskim
związanym z podstawą robota.

11
Przy podejściu geometrycznym [1] [2] do rozwiązania zagadnienia kinematycznego konieczne
jest zdekomponowanie przestrzennego zagadnienia geometrycznego na szereg zagadnień
planarnych. Przy tej metodzie zmienne przegubowe mogą być wyznaczone za
pomocą metod stosowanych w płaskiej geometrii. Na rysunku 2.2 przedstawiony jest
szkieletowy model robota wraz z odpowiednio oznaczonymi kątami. Danymi wejściowymi
są położenie i orientacja chwytaka P (P x , P y , P z , η, ϱ), gdzie P x , P y , P z określa położenie
punktu pracy chwytaka względem bazowego układu odniesienia, kąt η określa kąt nutacji
efektora, zaś kąt ϱ jest równy kątowi q 5 , który odpowiedzialny jest za rotację wokół osi
Z 5 .
Rysunek 2.2: Struktura szkieletowa robota.
W celu wyznaczenia kąta q 1 , rzutujemy punkt pracy chwytaka P na płaszczyznę wyzna-

12
czoną przez osie X 0 , Y 0 , stąd widzimy, że
czyli
tan q 1 = P y
P x
, (2.6)
q 1 = Atan2(P y , P x ), (2.7)
gdzie Atan2(·, ·) oznacza dwuargumentową funkcję arcus tangens.
W celu ułatwienia obliczenia kolejnych zmiennych przegubowych wyznaczony został punkt
pomocniczy P c o współrzędnych
⎡ ⎤ ⎡
⎤
P cx P x − k cos q 1
⎢ ⎥ ⎢
⎥
P c = ⎣ P cy ⎦ = ⎣ P y − k sin q 1 ⎦ , gdzie k = l 4 cos η. (2.8)
P cz P z − l 4 sin η
Aby wyznaczyć kąt q 3 wykorzystuje się twierdzenie cosinusów
d 2 = l 2 2 + l 2 3 − 2l 2 l 3 cos (π − q 3 ). (2.9)
Ponieważ zachodzi zależność cos (π − q 3 ) = − cos q 3 , oraz dla dowolnej konfiguracji manipulatora
jest spełniony związek d 2 = r 2 +(P cz −l 1 ) 2 , gdzie r 2 = Pcx 2 +P cy 2 , zatem otrzymuje
się dalej
Pcx 2 + P cy 2 + (P cz − l 1 ) 2 = l2 2 + l2 3 + 2l 2l 3 cos q 3 , (2.10)
i stąd
cos q 3 = P 2 cx + P 2 cy + (P cz − l 1 ) 2 − l 2 2 − l2 3
2l 2 l 3
. = D. (2.11)
Istnieją dwa rozwiązania powyższego równania, a mianowicie
oraz
q 3 = arccos P 2 cx + P 2 cy + (P cz − l 1 ) 2 − l 2 2 − l2 3
2l 2 l 3
, stąd 0 q 3 π, (2.12)
q 3 = − arccos P 2 cx + P 2 cy + (P cz − l 1 ) 2 − l 2 2 − l 2 3
2l 2 l 3
, stąd − π q 3 0. (2.13)
Jednakże lepszym sposobem znalezienia kąta q 3 jest zauważenie, że jeśli cos q 3 jest dany
wzorem (2.11), to sin q 3 jest dany odpowiednio wzorem
i stąd można wyznaczyć
sin q 3 = ± √ 1 − D 2 , (2.14)
q 3 = Atan2(± √ 1 − D 2 , D). (2.15)
Zaletą tego drugiego sposobu jest rozróżnienie obu konfiguracji ”łokieć u góry” i ”łokieć
u dołu” przez wybór odpowiedniego znaku w równaniu (2.15).
W celu wyliczenia kąta q 2 wyznacza się wyrażenia na kąty pomocnicz δ oraz α zaznaczone
na rysunku 2.2. Dla kąta δ zachodzi
tan δ = P cz − l 1
r
= P cz − l 1
√ , (2.16)
P
2
cx − Pcy
2

13
czyli
Podobnie wyznaczamy kat α
czyli
δ = Atan2(P cz − l 1 , √ Pcx 2 − P cy 2 ). (2.17)
tan α = l 3sinq 3
l 2 + l 3 cosq 3
, (2.18)
α = Atan2(l 3 sinq 3 , l 2 + l 3 cosq 3 ). (2.19)
Z rysunku otrzymuje się prostą zależność między kątami q 2 , δ, α, a mianowicie q 2 = δ − α,
z której wynika,że
q 2 = Atan2(P cz − l 1 , √ P 2 cx − P 2 cy ) − Atan2(l 3sinq 3 , l 2 + l 3 cosq 3 ). (2.20)
Wartość kąta q 2 zależy od kąta q 3 . Ma to znaczenie fizyczne, ponieważ otrzymamy
różne wartości kąta q 2 w zależności od tego, które rozwiązanie wybierzemy dla kąta q 3 .
Z kolei kąt q 4 wyznaczamy z następującego związku η = q 2 + q 3 + q 4 czyli
q 4 = η − q 2 − q 3 . (2.21)
Reasumując dla P (P x , P y , P z , η, ϱ) otrzymujemy następujące zależności:
⎫
q 1 = Atan2(P y , P x ),
q 2 = Atan2(P cz − l 1 , √ Pcx 2 − P cy 2 ) − Atan2(l 3sinq 3 , l 2 + l 3 cosq 3 ),
q 3 = Atan2(± √ ⎪⎬
1 − D 2 , D),
q 4 = η − q 2 − q 3 ,
q 5 = ϱ.
⎪⎭
(2.22)
gdzie
P cx = P x − k cos q 1 , P cy = P y − k sin q 1 , P cz = P z − l 4 sin η, D = P cx+P 2 cy+(P 2 cz−l 1 ) 2 −l2 2−l2 3
2l 2 l 3
,
k = l 4 cos η.
2.4 Konfiguracje wyróżnione manipulatora
W celu mechanicznego sprawdzenia poprawności pozycjonowania manipulatora wyróżniono
konfigurcję zerową manipulatora zwaną również konfiguracją geometryczną q 0 =
(q 01 , q 02 , q 03 , q 04 , q 05 ), dla której zmienne przegubowe przyjmują następujace wartości:
q 01 = 0 [ ◦ ],
q 02 = 0 [ ◦ ],
q 03 = 0 [ ◦ ],
q 04 = 0 [ ◦ ],
q 05 = 0 [ ◦ ].
(2.23)

14
Ogniwa 1,2,3,4,5 są ustawione w pozycji swojego środkowego zakresu pracy.
Drugą wyróżnioną konfiguracją jest konfiguracja synchronizacji. Jej osiągnięcie jest
sygnalizowane przez czujniki zamontowane w poszczególnych przegubach i kanały indeksujące
koderów, odczytywane przez układ sterowania robota. Dla ogniw 1,2,4,5 czujniki
zostały zamontowane tak, aby zachować zgodność z konfiguracją zerową. W przypadku
ogniwa 3 położenie czujnika jest różne od położenia zerowego. Dokładne wartości położeń
napędów ogniw w konfiguracji synchronizacji podano w dodatku A.
2.5 Transformacja napędowa osi robota
Sterownik robota zarówno mierzy, jak i zadaje pozycje i prędkości poszczególnych osi w
tzw. przestrzeni napędowej, której współrzędne opisują położenia wałów poszczególnych
silników. Istotne jest więc podanie sposobu przeniesienia napędu dla poszczególnych osi
jak i równań wiążących położenia wałów silników M i ze zmiennymi przegubowymi q i .
Do napędu poszczególnych osi robota jak i do napędu mechanizmu zamykającego chwytak
zastosowano silniki elektryczne prądu stałego z magnesem trwałym. Pomiar położenia
wałów silników jest realizowany przy pomocy koderów przyrostowych o rozdzielczości D
= 1024 impulsów na jeden obrót.
Wszystkie silniki napędowe, są wyposażone w przekładnie wielostopniowe o przełożeniach
G 1 = G 2 = G 3 = G 6 = 94.37 i G 4 = G 5 = 43.3.
2.5.1 Rozwiązanie napędów ogniwa 1,2 i 3
Napęd ogniwa 1 (obrót kolumny) jest przenoszony przez przekładnię wielostopniową o
przełożeniu G 1 a następnie przez przekładnię jednostopniową o przełożeniu Z 1
Z 2
, gdzie Z 1
i Z 2 oznacza ilość zębów dla poszczególnych kół przekładni jednostopniowej. Na rysunku
2.3 jest pokazany sposób przeniesienia napędu dla ogniwa 1 i 2. Równanie opisujące
przeniesienie napędu jest następujące
Rysunek 2.3: Struktura układu przeniesienia napędu ogniwa 1 i 2.

15
q 1 − q 01 =
Z 12π
Z 2 DG 1
(M 1 − M 01 ), (2.24)
gdzie q 0i jest wartością zmiennej przegubowej q i w konfiguracji zerowej, a M 0i jest wartością
zmiennej napędowej M i w konfiguracji zerowej.
Transformacja prosta napędu
c 1 = cos q 1
(2.25)
s 1 = sin q 1 ,
ostatecznie ma postać
a transformacja odwrotna
czyli
c 1 = cos (q 01 +
s 1 = sin (q 01 +
Z 12π
Z 2 DG 1
(M 1 − M 01 )), (2.26)
Z 12π
Z 2 DG 1
(M 1 − M 01 )), (2.27)
q 1 = Atan2(s 1 , c 1 ), (2.28)
M 1 = M 01 + Z 2DG 1
Z 1 2π (Atan2(s 1, c 1 ) − q 01 ). (2.29)
W przypadku ogniwa 2 (główne ramię) sposób przeniesienia napędu jest analogiczny jak
w przypadku ogniwa 1 rysunek 2.3. A mianowicie, dla ogniwa drugiego, równanie napędu
wyraża się wzorem
q 2 − q 02 =
Z 32π
(M 2 − M 02 ), (2.30)
Z 4 DG 2
Transformacja prosta napędu ma postać
a transformacja odwrotna
c 2 = cos (q 02 +
s 2 = sin (q 02 +
Z 32π
Z 4 DG 2
(M 2 − M 02 )), (2.31)
Z 32π
Z 4 DG 2
(M 2 − M 02 )), (2.32)
q 2 = Atan2(s 2 , c 2 ). (2.33)
Z kolei dla ogniwa trzeciego sposób przeniesienia napędu różni się od sposobu w ogniwie 1
i 2 jedynie tym, że czynne koło zębate Z 5 przekładni jednostopniowej nie jest zamocowane
na osi przekładni G 3 a jest napędzane poprzez przekładnie pasową zębatą o przełożeniu
równym jeden, co widać na rysunku 2.4. Równanie napędu ogniwa 3 wyraża się wzorem
q 3 − q 03 =
Transformacja prosta napędu ma postać
a transformacja odwrotna
c 3 = cos (q 03 +
s 3 = sin (q 03 +
Z 52π
Z 6 DG 3
(M 3 − M 03 ), (2.34)
Z 52π
Z 6 DG 3
(M 3 − M 03 )), (2.35)
Z 52π
Z 6 DG 3
(M 3 − M 03 )), (2.36)
q 3 = Atan2(s 3 , c 3 ). (2.37)

16
Rysunek 2.4: Struktura układu przeniesienia napędu ogniwa 3.
2.5.2 Rozwiązanie napędu ogniwa 4 i 5
Ruch w ogniwie 4 (pochylenie nadgarstka) i ogniwie 5 (obrót chwytaka) zapewniają dwa
silniki elektryczne o przełożeniu G 4 = G 5 . Silniki te pracują w układzie różnicowym co
widać na rysunku 2.5. Mechanizm różnicowy jest zbudowany z pięciu stożkowych kół
zębatych, z których dwa koła o ilości zębów Z 7 są osadzone na osiach silników, kolejne
dwa koła o ilości zębów Z 8 wykonują ruch obrotowy w osi kąta q 4 , natomiast ostatnie
koło zębate o liczbie zębów Z 9 obraca się w osi kąta q 5 . Równania przeniesienia napędu
dla ogniwa 4 wyrażają się następująco
q 4 − q 04 =
Transformacja prosta napędu
c 4 = cos (q 04 +
s 4 = sin (q 04 +
a transformacja odwrotna
Z 72π
2Z 8 DG 4
((M 5 − M 05 ) − (M 4 − M 04 )), (2.38)
Z 72π
2Z 8 DG 4
((M 5 − M 05 ) − (M 4 − M 04 ))), (2.39)
Z 72π
2Z 8 DG 4
((M 5 − M 05 ) − (M 4 − M 04 ))), (2.40)
q 2 = Atan2(s 2 , c 2 ). (2.41)
Z kolei dla ogniwa 5 otrzymujemy równania przeniesienia napędu postaci
Transformacja prosta napędu
q 5 − q 05 = −
Z 72π
((M 4 − M 04 ) + (M 5 − M 05 )), (2.42)
2Z 9 DG 5
c 5 = cos (q 05 −
Z 72π
2Z 9 DG 4
((M 5 − M 05 ) − (M 4 − M 04 ))), (2.43)

17
Rysunek 2.5: Struktura układu różnicowego napędzającego osie nadgarstka.
s 5 = sin (q 05 −
a transformacja odwrotna
Z 72π
2Z 9 DG 4
((M 5 − M 05 ) − (M 4 − M 04 ))), (2.44)
q 2 = Atan2(s 2 , c 2 ). (2.45)
Warto zwrócić uwagę, że efektywniejszą obliczeniowo implementację transformacji prostych
i odwrotnych uzyskuje się dzięki nadmiarowej reprezentacji kątów przegubowych q i
jako par (c i , s i ).

18
Rozdział 3
Konstrukcja robota ZS5R
3.1 Konstrukcja mechaniczna
Podstawowymi materiałami, z których została wykonana konstrukcja manipulatora robota
stanowią blacha aluminiowa, kształtowniki aluminiowe (kątowniki, ceowniki) oraz
pręty stalowe. Poszczególne elementy konstrukcyjne zostały wycięte z blachy aluminiowej
o grubości 1-3mm i po odpowiednim wygięciu, poskręcane śrubami M3. Tak zmontowane
elementy stanowią korpusy poszczególnych ramion robota. Konstrukcja chwytaka
wykonano ceowników aluminowych. Na rysunku 3.1 przedstawiono wygląd manipulatora,
z kolei na rysunku 3.2 przedstawiono rysunek złożeniowy, który zawiera główne zespoły
konstrukcji mechanicznej.
Oznaczenia na rysunku 3.2
Rysunek 3.1: Zdjęcie manipulator.

Rysunek 3.2: Rysunek złożeniowy manipulatora z oznaczonymi głównymi częściami.
19

20
1 – podstawa manipulatora,
2 – kolumna obrotowa,
3 – główne ramie manipulatora,
4 – przedramie manipulatora,
5 – chwytak,
6 – piasta w której obraca się oś kolumny manipulatora,
M 1 – silnik napędzający oś kolumny ,
M 2 – silnik napędzający oś głównego ramienia,
M 3 – silnik napędzający oś przedramienia ,
M 45 – dwa silniki napędzjące osie chwytaka ,
M 6 – silnik napędzjący mechanizm zamykania i otwierania kiści chwytaka.
Manipulator posiada pięć przegubów obrotowych, gdzie do 1-go (obrót kolumny) i 2-go
(ruch głównego ramienia) napęd jest przenoszony przez wielostopniową przekładnię zębatą
a następnie przez jednostopniową przekładnię. Zastosowanie dodatkowej jednostopniowej
przekładni związane jest z tym, iż zastosowane silniki nie posiadały dostatecznie dużego
momentu obrotowego do porusznia poszczególnych ramion manipulatora. Do układu napędowego
przegubu trzeciego (ruch przedramienia) dodatkowo zastosowano pasek zębaty,
rysunek 2.4. Zasosowanie paska zębatego spowodowane jest tym, że silnik napędowy osi 3
jest umieszczony na końcu głównego ramienia rysunek 3.2. Umieszczenie tam silnika ma
swoje pozytywne konsekwencje, a mianowicie stanowi on swego rodzaju przeciwwagę, co
ma znaczenie ze względu na moment obrotowy potrzebny do ruchu głównego ramienia. Z
kolei dla przegubu 4-go i 5-go napęd jest zrealizowany przez układ różnicowy. Powoduje
to pewne utrudnienia w sterowaniu tymi osiami, równania przeniesienia napędu (2.38),
(2.42) pokazują, że są zależne od siebie, (rysunek 2.5).
Wszystkie parametry i rysunki wykonawcze poszczególnych części manipulatora zamieszczone
są w dodatku A.
3.1.1 Silniki manipulatora
Manipulator posiada sześć silników prądu stałego firmy MICROMOTORS S.R.L [10], z
których cztery są typu RHE158.24.100 i dwa typu HLE149.24.43. Wszystkie te silniki są
konstrukcyjnie sprzężone z przekładniami wielostopniowymi, co widać na rysunkach 3.3 i
3.4. Do napędu kolumny robota, ramienia, przedramienia i wciągarki chwytaka wykorzystano
silniki typu RHE158.24.100, a silniki typu HLE149.24.43 zastosowano do napędu
układu różnicowego chwytaka. Dane związane z wymiarami fizycznymi tych silników są
przedstawione na rysunkach umieszczonych w dodatku A. W poniższych tabelach zestawiono
parametry techniczne silników napędowych.
znamionowe napięcie zasilania 24[V ]
przełożenie przekładni 94.37 : 1
maksymalny ciągły moment obrotowy 60[Ncm]
prędkość obrotowa bez obciązenia 66[obr/min]
prędkość obrotowa z max. momentem obciążenia 45[obr/min]
pobór prądu silnika w stanie jałowym < 70[mA]
pobór prądu silnika przy max. obciążeniu 340[mA]
Tabela 3.1: Parametry silnika typu RHE158.24.100.

21
Rysunek 3.3: Zdjęcie silnika HLE149.24.43.
Rysunek 3.4: Zdjęcie silnika RHE158.24.100.
znamionowe napięcie zasilania 24[V ]
przełożenie przekładni 43.3 : 1
maksymalny ciągły moment obrotowy 15[Ncm]
prędkość obrotowa bez obciązenia 78[obr/min]
prędkość obrotowa z max. momentem obciążenia 55[obr/min]
pobór prądu silnika w stanie jałowym < 50[mA]
pobór prądu silnika przy max. obciążeniu 120[mA]
Tabela 3.2: Parametry silnika typu HLE149.24.43.

22
Silniki te dodatkowo są fabrycznie wyposażone w kodery halotronowe. Jednak mała rozdzielczość
(trzy impulsy na obrót wału silnika) nie pozwoliła na zastosowanie ich w tej
konstrukcji. Dlatego silniki te zostały wyposażone w kodery przyrostowo-impulsowe typu
AS5040 firmy AUSTRIAMICROSYSTEMS [6]. Możliwość zamontowania tych koderów
wiązała się z koniecznością modyfikacji konstrukcji silników. Przeróbki polegały na skróceniu
wału silnika o 3mm a następnie zamontowaniu na nim magnesu neodymowego [7].
Wykonano również płytki drukowane do AS5040(dodatek C) przystosowane do zamocowania
w miejsce fabrycznego kodera. Aby zamknąć nowy koder w obudowie, należało w
pokrywie wykonać podtoczenie o głębokości 1mm. Na rysunkach 3.5 i 3.6 przedstawione
Rysunek 3.5: Zdjęcie ilustrujące sposób zamontowania magnesu na osi silnika.
Rysunek 3.6: Zdjęcie ilustrujące sposób zamontowania kodera AS5040.
są zdjęcia ilustrujące sposób zamontowania koderów AS5040. Parametry tych koderów są
dostępne na stronie internetowej producenta [6].

23
3.2 Konstrukcja elektroniczna – sterownik robota
Robot ZS5R posiada manipulator, którego poszczególne ramiona mogą wykonywać sekwencje
ruchów, za które odpowiedzialny jest układ sterowania. Między innymi zapewnia
on pomiar niezbędnych parametrów ruchu, takich jak położenia i prędkości. Ponadto
układ sterowania zapewnia komunikację z komputerem nadrzędnym w celu pobrania parametrów
do realizacji zadania, oraz wysyłania do komputera nadrzędnego aktualnych
parametrów stanu robota. Na podstawie zadanych i zmierzonych parametrów sterownik
wymusza na silnikach umieszczonych w ramionach takie sterowania, aby przeprowadzić
efektor manipulatora z punktu początkowego do punktu docelowego. Moc obliczeniowa
jednostki centralnej powinna zapewnić wykonanie wszystkich niezbędnych obliczeń w jak
najkrótszym czasie. Na rysunku 3.7 przedstawiono schemat blokowy sterownika robota.
Szczegółowe schematy elektroniczne, rysunki montażowe płytki (rozmieszczenie elemen-
Rysunek 3.7: Schemat blokowy układu sterowania manipulatora.
tów i ścieżek) oraz wykaz elementów znajdują się w dodatku B i C.
Sterownik został zmontowany na płytce dwustronnej o wymiarach 94x260mm, co widać
na rysunku 3.8. Na płytce tej znajdują się:
• moduł EM332/B z mikrokontrolerem MC68332, zamocowany na dwóch złączach
48-stykowych każdy,
• przetwornik A/C typu MAX1270 pracujący na magistrali QSPI,

24
Rysunek 3.8: Płytka sterownika .
• dwa przetworniki C/A typu MAX525 pracujących na magistrali QSPI,
• układ transmisji szeregowej RS232C wykorzystujący SCI i translator poziomów napięciowych
- układ MAX232A,
• klucz sterujący cewką przekaźnika,
• układ bramek NAND typu 74HCT132 będący układem wejściowym dla czujników
synchronizacji,
• sześć regulatorów prądu wykorzystujących układ wykonawczy L298N.
Sterownik manipulatora można podzielić na dwa moduły:
• moduł sterowania oparty na module EM332/B firmy ELFIN,
• moduł mocy zawierający układy wykonawcze wraz z regulatorami prądu,
3.2.1 Budowa modułu sterowania
Głównym elementem modułu sterowania jest moduł EM332/B, na którym znajduje się
jednostka centralna MC68332, rysunek 3.9. Wszystkie schematy elektryczne, rysunek płytki,
rozmieszczenie elementów, zasoby mikrokontrolera MC68332 oraz zasada działania
modułu EM332/B są opisane w raporcie [5].
Schemat ideowy modułu sterowania znajduje się na rysunku 9.2 w dodatku C.
Za pomocą magistrali QSPI do modułu EM332/B podłączone są przetworniki:
• dwa układy MAX525 [9](czterokanałowy, 12-bitowy przewornik cyfrowo-analogowy
unipolarny o zakresie napięcia wyjściowego 0-2.5V, wymaga zewnętrznego napięcia
odniesienia) służą do zadawania prądu w układach wykonawczych,
• układ MAX1270 [9](ośmiokanałowy, 12-bitowy przetwornik analogowo-cyfrowy
uni/bipolarny o 10V zakresie pomiaru z wbudowanym źródłem napięcia odniesienia)
służy do mierzenia prądu pobieranego przez silniki wykonawcze,

25
Rysunek 3.9: Moduł EM332/B.
Do bloku TPU mikrokontrolera podłączone są kodery przyrostowe AS5040 [6]. Zapewniają
one pomiar położenia i prędkości. Zasotosowane kodery posiadają dwa wyjścia A/B, które
generują dwa przebiegi kwadraturowe przesunięte w fazie o 90 stopni. Rozdzielczość tych
koderów wynosi 256 impulsów/obrót osi silnika. Sygnały kodera są dekodowane w bloku
TPU standardową funkcją QDEC, dzięki której otrzymujemy informacje o położeniu wału
silnika. Ponieważ dekodowanie uwzględnia wszystkie zbocza sygnałów wychodzących
z kodera, pomiar położenia osi silnika odbywa się z rozdzielczością 1024 impulsów/obrót
wału silnika. Natomiast prędkości silników mierzone są pośrednio przez pomiar okresu
sygnałów kwadraturowych generowanych przez kodery. Pomiar ten także jest realizowany
w bloku TPU. Funkcja QDVEL dokonująca pomiaru zlicza ilość impulsów zegara taktującego
blok TPU w trakcie trwania jednego okresu sygnału kwadraturowego. Na podstawie
zliczonej ilości impulsów oprogramowanie sterownika oblicza aktuanle prędkści silników.
3.2.2 Budowa modułu mocy
Zastosowanie silników prądu stałego do napędu osi robota podyktowane było tym, że
silniki prądu stałego z magnesami trwałymi posiadą liniową zależność momentu napędowego
generowanego przez silnik od prądu pobieranego. W rezultacie sterowanie prądowe
silników zapewnia kontrolę nad momentem uzyskiwanym na wale silnika. Z tego względu
moduł mocy został zbudowany z sześciu regulatorów prądu, które zawierają układy wykonawcze
L298N [12]. Schemat ideowy jednego z tych regulatorów jest zamieszczony na
rysunku 9.1 w dodatku C. Na rysunku blokowym 3.10 przedstawiona jest idea działania
układu regulacji prądu. Regulacja ta polega na porównaniu wartości zadanej prądu silnika
z wartością rzeczywistą. Wartość rzeczywista prądu jest mierzona za pośrednictwem rezystorów
pomiarowych R p1 i R p2 jako spadek napięcia na nich. Napięcie z dwóch rezystorów
pomiarowych jest podane na wejście wzmacniacza różnicowego. W układzie wykonawczym
prąd może płynąć tylko przez jeden rezystor w zależności od trybu pracy układu (obroty
w lewo, prawo). Po znaku różnicy potencjału na rezystorach układ rozpoznaje kierunek
płynącego prądu w silniku. Wartość napięcia ze wzmacniacza różnicowego podana jest na
filtr RC, a następnie na układ komparatora, porównujący prąd rzeczywisty z wartością
zadaną prądu. Wartość zadaną prądu ustawia się za pośrednictwem przetwornika C/A.
Filtr RC ma za zadanie uśrednić wartość prądu silnika i podać ją do przetwornika A/C,
dzięki któremu możemy diagnostycznie sprawdzać wartość rzeczywistą prądu płynącego

26
Rysunek 3.10: Schemat blokowy regulatora prądu.
przez silnik. Sygnał z układu komparatora jest podawany na układy logiczne, ktorych zadaniem
jest wprowadzenie czasów opóźnień pomiędzy wyłączeniem jednej gałęzi mostka
a włączeniem drugiej gałęzi mostka. Blok układów logicznych zawiera układy 74HC123,
które są odpowiedzialne za czasy opóźnień.
Układ wykonawczy (mostek mocy) jest zasilany z napięcia +36V, natomiast pozostała
część elektroniki jest zasilana napięciem +5V (układy cyfrowe, przetworniki A/C i C/A),
±5V(wzmacniacze operacyjne, komparatory). Schemat ideowy zasilacza jest zamieszczony
w dodatku C. Na rysunku 3.11 jest przedstawione zdjęcie zasilacza.
Rysunek 3.11: Zasilacz części cyfrowej i analogowej

27
3.2.3 Opis łączówek sterownika
Przyłączenie zewnętrznego terminala (lub komputera wyposażonego w złącze szeregowe)
jest możliwe dzięki złączu Z 10 .
wolny 1 2 TxD
RxD 3 4 wolny
GND 5 6 wolny
wolny 7 8 wolny
wolny 9 10 wolny
Tabela 3.3: Łączówka transmisji szeregowej Z 10 .
oznaczenie opis styk
TxD wyjście danych nadawanych (RS232C) 2
RxD wejście danych odbieranych (RS232C) 3
GND masa sygnałowa 5
Tabela 3.4: Sygnały łączówki transmisji szeregowej Z 10 .
Na złączu Z 8 wyprowadzone są sygnały, które umożliwiają podłączenie zewnętrznego sterownika.
W przypadku gdy wykorzystywany jest wewnętrzny układ sterowania sygnały
od STER I1 do STER I6 są odpowiednio połączone z STERI1 do STERI6 za pomocą
zworek. W przypadku gdy chcemy podłączyć zewnętrzny sterownik (na karta współpracująca
z pakietem MATLAB ) rozwieramy powyższe sygnały, a do styków sygnałowych
oznaczonych STER I1,..., STER I6 podłączamy sygnały zewnętrzne o zakresie napięciowym
0-2.5V.
Wyprowadzenia i opis łączówki Z 11 jest zamieszczony w raporcie [5]. Dwustykowe złącza
Z 1 ,...,Z 6 są wyjściami z poszczególnych stopni mocy i służą do przyłączenia silników.

28
GND 1 2 Vcc
TPU10 3 4 TPU11
TPU8 5 6 TPU9
TPU6 7 8 TPU7
TPU4 9 10 TPU5
TPU2 11 12 TPU3
TPU0 13 14 TPU1
POM I1 15 16 POM I2
POM I3 17 18 POM I4
POM I5 19 20 POM I6
STER I1 21 22 STERI1
STER I2 23 24 STERI2
STER I3 25 26 STERI3
STER I4 27 28 STERI4
STER I5 29 30 STERI5
STER I6 31 32 STERI6
wolny 33 34 wolny
Tabela 3.5: Łączówka Z 8 .

29
oznaczenie opis styk
GND masa sygnałowa 1
Vcc +5V 2
TPU10, TPU11 wyjścia kodera A/B na silniku M 6 3, 4
TPU8, TPU9 wyjścia kodera A/B na silniku M 5 5, 6
TPU6, TPU7 wyjścia kodera A/B na silniku M 4 7, 8
TPU4, TPU5 wyjścia kodera A/B na silniku M 3 9, 10
TPU2, TPU3 wyjścia kodera A/B na silniku M 2 11, 12
TPU0, TPU1 wyjścia kodera A/B na silniku M 1 13, 14
POM I1 sygnał pomiaru prądu silnika M 1 (-5V

oznaczenie opis styk
SYNCHR1,...,SYNCHR6 sygnały z czujników synchronizacji q 1 ,..,q 6 4,2,8,6,12,10
INDEX1,...,INDEX6 sygnały z kanałów indeks koderów D 1 ,..,D 6 3,1,7,5,11,9
TPU0, TPU1 wyjścia kodera A/B na silniku M 1 13, 14
TPU2, TPU3 wyjścia kodera A/B na silniku M 2 15, 16
TPU4, TPU5 wyjścia kodera A/B na silniku M 3 17, 18
TPU6, TPU7 wyjścia kodera A/B na silniku M 4 19, 20
TPU8, TPU9 wyjścia kodera A/B na silniku M 5 21, 22
TPU10, TPU11 wyjścia kodera A/B na silniku M 6 23, 24
GND masa sygnałowa 25
Vcc +5V 26
Tabela 3.8: Opis sygnałów łączówki Z 9 .
opis styk
∼24V 1
∼24V 2
Tabela 3.9: Opis złącza zasilania modułu mocy Z 7 .
opis styk
-5V 1
+5V 2
GND 3
+5V 4
Tabela 3.10: Opis złącza zasilania Z 12 .
opis styk
+5V 1
+5V 2
GND 3
-5V 4
Tabela 3.11: Opis złącza na płytce zasilacz Z 13 .
złącze
funkcja
Z 1 wyjście dla silnika M 1
Z 2 wyjście dla silnika M 2
Z 3 wyjście dla silnika M 3
Z 4 wyjście dla silnika M 6
Z 5 wyjście dla silnika M 4
Z 6 wyjście dla silnika M 5
Tabela 3.12: Opis złącz Z 1 ,...Z 6 .
30

31
Rozdział 4
Oprogramowanie sterownika
Sterownik robota jest zbudowany w oparciu o mikrokontroler MC68332. Oprogramowanie
dla tego mikrokontrolera można napisać w języku asembler C oraz C++. W tym przypadku
oprogramowanie zostało napisane w języku C z użyciem makr w asemblerze. Oprogramowanie
sterownika robota ZS5R zostało przygotowane w środowisku HI-CROSS
firmy HIWARE. Zasadniczymi częściami programu sterownika są funkcja główna main()
oraz procedura obsługi przerwania cyklicznego interrupt void int_PIT().
4.1 Funkcja główna main()
Na początku funkcji main() wywoływane są procedury inicjujące podstawowe bloki mikrokontrolera
SIM, TPU, QSM oraz inicjowana jest początkowa wartość prądu regulatorów.
sim_init(); /* inicjalizacja modulu SIM */
qsm_init(); /* inicjalizacja modulu QSM */
tpu_init(); /* inicjalizacja modulu TPU */
Cur_init();
/* ustawienie prądów zerowych regulatorów*/
Funkcja sim_int() między innymi konfiguruje port E i ustawia wektor i poziom przerwania
cyklicznego PIT
/*Konfiguracja przerwania cyklicznego*/
*((void (**) ())(4*PI_VECT)) = int_PIT; /* ustawienie wektora */
PICR = (PI_LEVEL

32
Z kolei funkcja tpu_int() inicjalizuje odpowiednie kanały bloku TPU dla funkcji QDEC
i QDVEL, które to funkcje służą do pomiaru położenia i prędkości.
W pętli głównej umieszczono obsługę menu pozwalającego interakcyjnie wybierać tryb
pracy robota.
out_text("\n\n\rPRACA DYPLOMOWA\n\n\r");
out_text("Robot dydaktyczny ZS5R\n\r"); /* Intro */
out_text("Autor: Zbigniew Struzik, Wroclaw 2005\n\r");
out_text("h - Pomoc\n\r");
out_text("\n\n\rMenu Glowne:\n\n\r");
out_text("P - Pozycjonowa\n\r");
out_text("M - Tryb sterowania recznego\n\r");
out_text("X - Wylaczenie silnikow\n\r");
out_text("S - przeslanie\n\r");
out_text("H - Pomoc\n\n\r");
Wybierając np. pozycję oznaczoną literką M lub m przechodzimy do menu sterowania
ręcznego.
out_text("\n\n\rMenu sterowania recznego:\n\n\r");
out_text("1,Q - Wieza\n\r");
out_text("2,W - Ramie\n\r");
out_text("3,E - Przedramie\n\r");
out_text("4,R - Nadgarstek (gora/dol)\n\r");
out_text("5,T - Nadgarstek (obrot)\n\r");
out_text("6,Y - Chwytak\n\r");
out_text("SPACE - Silniki STOP\n\r");
out_text("V - Definicja predkosci\n\r");
out_text("ESC - Powrot do Menu Glownego\n\r");
out_text(">>");
4.2 Procedura obsługi przerwania cyklicznego
Wszystkie pomiary i wyliczenie wielkości sterujące są przechowywane w strukturze state.
typedef struct{
. \\
. \\
. \\
}STAN;
STAN
state;
Nastawy i inne parametry regulatorów prędkości i położenia są przechowywane w strukturze
nast.
typedef struct
{
.\\
.\\

33
.\\
}NAS;
NAS
nast;
Wszystkie procesy związane ze sterowaniem oraz akwizycją danych pomiarowych, obsługiwane
są w funkcji przerwania cyklicznego interrupt void int_PIT(). Funkcja ta jest
wywoływana co 2ms. Jest w niej realizowany pomiar położenia i prędkości, przy pomocy
funkcji QDEC i QDVEL:
state.temposition[0] = TPURAM(QDEC1A,1);/* odczyt licznika pozycji q1*/
state.M[0] &= 0xffff0000;
state.M[0] += state.temposition[0];
if((state.lastposition[0]-state.temposition[0]) > 32767)
{
state.M[0] += 0x10000;
}
if((state.temposition[0]-state.lastposition[0]) > 32767)
{
state.M[0] -= 0x10000;
}
state.lastposition[0] = state.temposition[0];
state.speed[0] = QDVEL2Speed((int)TPURAM(QDVEL1,2));
state.speed[1] = QDVEL2Speed((int)TPURAM(QDVEL2,2));
state.speed[2] = QDVEL2Speed((int)TPURAM(QDVEL3,2));
state.speed[3] = ((TPSEC*(state.M[3]-state.oldposition[2])+2)/4);
state.speed[4] = ((TPSEC*(state.M[4]-state.oldposition[0])+2)/4);
state.speed[5] = ((TPSEC*(state.M[5]-state.oldposition[1])+2)/4);
Pomiar prądu jest realizowany za pomocą przetwornika A/C pracującego na kolejkowej
magistrai QSPI. Aktualizacja zmierzonego prądu również jest realizowana w funkcji przerwania
cyklicznego PIT.
state.MCurrent[0] = ADBip2Int(QREC[1]>>4);
state.MCurrent[1] = ADBip2Int(QREC[2]>>4);
state.MCurrent[2] = ADBip2Int(QREC[3]>>4);
state.MCurrent[3] = ADBip2Int(QREC[4]>>4);
state.MCurrent[4] = ADBip2Int(QREC[5]>>4);
state.MCurrent[5] = ADBip2Int(QREC[0]>>4);
Proces wyliczenia sterowań dla poszczególnych osi jest realizowany za pomocą regulatorów
PID położenia i prędkości zaimplementowanych w funkcjach:
void ControlPID_pos(NAS *ptr_n, STAN *ptr_s, int i)
void ControlPID_speed(NAS *ptr_n, STAN *ptr_s, int i)
wywoływanych w obsłudze przerwania PIT. Na rysunku blokowym 4.1 jest przedstawiony
sposób realizacji tych regulatorów. Po wyliczeniu sterowań przez funkcje regulatorów,
wartości zadanych prądów są wpisywane do TRANSMIT RAM w bloku QSPI:

34
Rysunek 4.1: Schemat blokowy układu sterowania.
QTRAN[6] = (state.SetCurrent[0] & 0x0FFF)|0x3000;
QTRAN[7] = (state.SetCurrent[1] & 0x0FFF)|0x7000;
QTRAN[8] = (state.SetCurrent[2] & 0x0FFF)|0xB000;
QTRAN[9] = (state.SetCurrent[3] & 0x0FFF)|0xF000;
QTRAN[10] = (state.SetCurrent[4] & 0x0FFF)|0x3000;
QTRAN[11] = (state.SetCurrent[5] & 0x0FFF)|0x7000;
Do przeprowadzenia eksperymentów i akwizycji danych pomiarowych w funkcji
interrupt void int_PIT() jest wykonywany zapis wybranych parametrów do tablicy.
if (flaga_pomiarow) {
tabpom[l][0]=state.SetCurrent[1];
tabpom[l][1]=state.MCurrent[1];
tabpom[l][2]=state.speed[1];
tabpom[l][3]=nast.W_set[1];
if (++l>=1000)
{
l=0;
flaga_pomiarow=0;
}
}
Następnie tablica z pomiarami jest wysyłana do komputera nadrzędnego przez łącze szeregowe
za pomocą funkcji wywoływanej w menu głównym (S), podanej poniżej.
void out_text(char *_text) {
byte length=0;
while(n_bufor);
while(*(length+_text))

35
length++;
while(length)
{
length--;
bufor[n_bufor]=*(length+_text);
n_bufor++;
}
SCCR1=SCCR1|0x0080;
return;
}
Opisane oprogramowanie zachowuje w dużym stopniu zgodność z oprogramowaniem robota
Romik (w zakresie menu). Pozwala również na zbieranie wyników pomiarów parametrów
ruchu osi robota przy prowadzeniu eksperymentów.

36
Rozdział 5
Badania i wyniki eksperymentów
W celu zbadanie konstrukcji sterownika i zaobserwowanie parametrów jakościowych przeprowadzono
odpowiednie eksperymenty. W celu zebrania pomiarów zaimplementowano
program testowy. W programie tym, cyklicznie co 2ms wywoływana jest procedura pozwalająca
zadawać momenty napędowe silników oraz zebrać próbki pomiarowe. Zebrane
próbki pomiarowe ze sterownika zostały wysłane do komputera nadrzędnego, na którym
zostały zwizualizowane przy pomocy programu gnuplot. Wysłane próbki nie zostały
wyskalowane dla nich odpowiednich jednostkach. Oś odciętych (czasu) na wszystkich wykresach
jest skalowana w cyklach przerwania PIT (2ms). Osie rzędnych wyskalowane są
bezpośrednio w jednostkach pomiarowych z przetworników.
Na rysunku 5.1 przedstawiony jest wykres ilustrujący odpowiedź sprzętowego regulatora
prądu na zadany prąd. Prąd zadany został wygenerowany przez regulator prędkości dla
przegubu q 1 (nastawy regulatora PID kp w=20, kd w=10, ki w=5, lim int=500 lim curr
= 2047 w set=10000).
Na wykresie można zaobserwować opóźnienie odpowiedzi regulatora na zadany prąd.
4500
4000
’pom2.9.txt’ using 1:2
’pom2.9.txt’ using 1:3
3500
3000
2500
2000
1500
1000
500
0
-500
-1000
0 5 10 15 20 25 30
Rysunek 5.1: Wykres odpowiedzi regulatora na zadany prąd.
Wynika to z tego, iż zadany prąd regulatora jest ustawiany przez przetwornik C/A, który
pracuje na magistrali QSPI. Warto zwrócić uwagę na odpowiedź skokową regulatora
prądu typową dla układu inercyjnego.
Na rysunku 5.2 przedstawiono wynik pracy regulatora prędkości podczas rozpędzania

37
przegubu q 0 . Widać, że w początkowym okresie prędkość narasta liniowo pod wpływem
ograniczonego do zadanej wartości prądu. Po osiągnięciu zadanej prędkości regulator prędkości
steruje prądem silnika utrzymując prędkość ruchu przegubu (rysunek 5.3).
7000
6000
’pom1.txt’ using 1:2
’pom1.txt’ using 1:3
’pom1.txt’ using 1:4
5000
4000
3000
2000
1000
0
-1000
0 10 20 30 40 50 60
Rysunek 5.2: Rozpędzanie osi q 0 .
8000
7000
’pom1.txt’ using 1:2
’pom1.txt’ using 1:3
’pom1.txt’ using 1:4
6000
5000
4000
3000
2000
1000
0
-1000
0 100 200 300 400 500 600 700 800 900 1000
Rysunek 5.3: Regulacja prędkości osi q 0 .
Jak widać, opracowany sterownik pozwala gromadzić pomiary parametrów ruchu poszczególnych
osi, co jest szczególnie przydatne przy badaniu własności regulatorów, dobieraniu
ich nastaw, szukaniu charakterystyk napędów i własności dynamicznych robota.

38
Rozdział 6
Podsumowanie
W wyniku niniejszej pracy powstało stanowisko laboratoryjne, którego główną częścią jest
manipulator dydaktyczny o nazwie ZS5R. Cykl pracy rozpoczęto od doboru silników na-
Rysunek 6.1: Widok skonstruowanego manipulatora.
pędowych, a następnie wykonano konstrukcję mechaniczną manipulatora. Skonstruowany
manipulator jest klasy 5R, (posiadający pięć stopni swobody) oraz jest wyposażony w
chwytak dwupalczasty. Następnie zaprojektowano i wykonano w pełni działający układ
sterowania, który pozwala na sterowanie prądem silników prądu stałego (a więc i momentem
napędowym), przez co istnieje w przyszłości możliwość implementacji algorytmów
sterowania uwzględniających dynamikę robota. W budowie układu elektronicznego

przewidziano złącze pozwalające na podłączenie zewnętrznego układu sterowania, jak na
przykład karty współpracującej ze środowiskiem MATLAB/SIMULINK. Wykonano testowe
oprogramowanie pozwalające na zbieranie danych pomiarowych z wewnętrznych
czujników, a następnie wysyłanie ich za pomocą interfejsu szeregowego na terminal w
postaci pliku txt. Zebrane dane pozwoliły na przeprowadzenie eksperymentów.
W pracy zamieszczono również, przykładową instrukcję laboratoryjną stanowiska oraz
dołączono dodatki, w których znajduje się pełna dokumentacja techniczna konstrukcji
mechanicznej i elektronicznej.
Wykonane oprogramowanie dołączono na nośniku elektronicznym (CD-ROM).
Opisany robot może być wykorzystany do celów dydaktycznych w znacznie szerszym zakresie
niż wcześniej wspomniany Romik, ze względu na możliwość sterowania momentem
napędowym i otwartą konstrukcję układu sterowania.
39

40
Bibliografia
[1] E. JEZIERSKI. Robotyka kurs podstawowy. Wydawnictwo Politechniki Łódzkiej,
Łódź, 2002.
[2] M.W. SPONG and M. VIDYASAGAR. Dynamika i sterowanie robotów. Wydawnictwo
Naukowo-Techniczne, Warszawa, 1997.
[3] K. TCHOŃ and W. JACAK. Podstawy robotyki. Wydawnictwo Politechniki Wrocławskiej,
Wrocław, 1992.
[4] K. TCHOŃ, A. MAZUR, I. DULĘBA, R. HOSSA, i R. MUSZYŃSKI. Manipulatory
i roboty mobilne: modele, planowanie ruchu, sterowanie. Akademicka Oficyna
Wydawnicza PLJ, Warszawa, 2000.
[5] M. WNUK. Moduł z mikrokontrolerem mc68332. Raport serii SPR 7/2004, Instytut
Cybernetyki Technicznej Politechniki Wrocławskiej, Wrocław, 2004.
[6] http://www.austriamicrosystems.com/
[7] http://www.bomatec.ch/
[8] http://www.freescale.com/
[9] http://www.maxim ic.com/
[10] http://www.micromotorssrl.com/
[11] http://www.motorola.com/
[12] http://www.st.com/stonline/

41
Rozdział 7
Dodatek A
7.1 Parametry mechaniczne manipulatora
Dane techniczne robota ZS5R
Układ kinematyczny 5 stopni swobody
Napęd
6 silników prądu stałego
Pomiar położenia względny, kodery przyrostowo-impulsowe
o rozdzielczości D=1024
Przestrzeń robocza
Oś 1 (obrót kolumny) ±90 ◦
Oś 2 (ramię)
±40 ◦
Oś 3 (przedramię) ±90 ◦
Oś 4 (zgięcie nadgarstka) ±60 ◦
Oś 5 (obrót chwytaka) ±360 ◦
Rodzaj chwytaka Chwytak dwupalcowy, o rozwarciu równoległym
w zakresie 0-60mm.
Stopień rozwarcia i siła uchwytu programowane.
Wymiary mechaniczne
l 1
201mm
l 2
217.5mm
l 3
134mm
l 4
90mm
Parametry przekładni
G 1 = G 2 = G 3 = G 6 94.37:1
G 4 = G 5 43.3:1
Z 1 16 zębów, moduł 1
Z 2 70 zębów, moduł 1
Z 3 15 zębów, moduł 0.8
Z 4 104 zęby moduł 0.8
Z 5 20 zębów, moduł 1
Z 6 68 zębów, moduł 1
Z 7 16 zębów, moduł 1
Z 8 24 zęby moduł 1
Z 9 42 zęby moduł 1

42
7.2 Rysunki techniczne części mechanicznej
Rysunek 7.1: Rysunek techniczny silnika RHE 158.24.100.
Rysunek 7.2: Rysunek techniczny silnika HLE 149.24.43.
Rysunki techniczne silników zostały wzięte ze strony producenta silników [10].

60
Rozdział 8
Dodatek B
8.1 Wykaz elementów części elektronicznej
liczba wartość obudowa oznaczenie na schemacie
Kondensatory
6 100n EU025 C1, C2, C3, C4, C5, C6
3 1000u EUTT5D10 C9, C10, C11
2 2200u EUE5 C7, C8
Diody
1 Prostownik RB1A RB1A
Układy scalone
1 7905 TO2220 IC3
1 L4940V5 TO2220 IC1
1 LM2940CT TO2220 IC2
Transformator
1 TS8/220 – –
Złącza
3 MSTBA4 – Z12, Z13, Z14
inne
2 Radiator SK104 KK1, KK2
Tabela 8.1: Zestawienie elementów elektronicznych na płytce zasilacza.
liczba wartość obudowa oznaczenie na schemacie
Kondensatory
1 100n EUC0805 C1
1 1u EUC0805 C2
Układy scalone
1 AS5040 SSOP16 U1
Tabela 8.2: Zestawienie elementów elektronicznych na płytce kodera AS5040.

61
liczba wartość obudowa oznaczenie na schemacie
Rezystory
12 1R R-EU0411 R2, R105, R106, R107, R108, R109, R110,
R111, R112, R113, R114, R115
24 1k M0805 R27, R28, R32, R34, R40, R42, R43, R47, R55,
R56, R57, R60, R67, R70, R71, R75, R83, R84,
R85, R88, R95, R96, R101, R103
6 3k M0805 R31, R48, R62, R76, R90, R104
6 5k CA6V R117, R118, R119, R120, R121, R122
36 10k M0805 R1, R25, R26, R29, R30, R33, R38, R39, R44,
R45, R46, R53, R54, R58, R59, R61, R63, R64,
R68, R69, R72, R73, R74, R77, R78, R81, R82,
R86, R87, R89, R97, R98, R99, R100, R102,
R123
8 18k M0805 R21, R22, R35, R36, R49, R50, R91, R92
18 100k M1206 R3, R4, R5, R6, R7, R8, R9, R10, R11,
R12, R13, R14, R15, R16, R17, R18, R19, R20
8 130k M0805 R23, R24, R37, R41, R51, R52, R93, R94
4 200k M0805 R23, R24, R37, R41, R51, R52, R93, R94
Kondensatory
24 270p EUC0805 C53, C54, C59, C60, C67, C68, C69, C70, C73,
C74, C75, C76, C83, C84, C89, C90, C93, C94,
C99, C100, C103, C104, C109, C110
73 100n EUC0805 C1, C3, C4, C5, C6, C7, C8, C9, C10,
C11, C12, C13, C14, C15, C16, C17, C18, C19,
C20, C21, C22, C23, C24, C25, C26, C27, C28,
C29, C30, C31, C32, C33, C34, C35, C36, C37,
C38, C39, C40, C41, C42, C43, C44, C45, C46,
C47, C48, C49, C51, C52, C55, C57, C61, C62,
C63, C65, C71, C72, C77, C79, C81, C82, C85,
C87, C91, C92, C95, C97, C101, C102, C105,
C107, C111
12 1u EUC0805 C56, C58, C64, C66, C78, C80, C86, C88, C96,
C98, C106, C108
1 4.7u B45181A C2
1 10000u/50V EUE10-30 C50
Diody
7 BAT42 BAT42 D25, D26, D27, D28, D29, D30, D31
24 BAT49 BAT49 D1, D2, D3, D4, D5, D6, D7, D8, D9,
D10, D11, D12, D13, D14, D15, D16, D17,
D18, D19, D20, D21, D22, D23, D24
1 Prostownik FBU4 B1
Tranzystory
1 BC640-16 TO92 T1
Tabela 8.3: Zestawienie elementów elektronicznych na płytce sterownika.

62
liczba wartość obudowa oznaczenie na schemacie
Układy scalone
6 74HC123D SO-16 IC12, IC13, IC14, IC15, IC16, IC18
3 74HCT00D SOIC-14 IC19, IC20, IC21
3 74HCT08D SO-16 IC22, IC24, IC25
3 74HCT132D SO-14 IC1, IC2, IC3
6 L298N MULTIWATT15 IC4, IC5, IC6, IC8, IC9, IC10
6 LM311D SO-8 IC26, IC27, IC28, IC29, IC30, IC31
7 LM385-Z TO92 UZ1, UZ2, UZ3, UZ4, UZ5, UZ6, UZ7
6 MAX4164 SO-14 IC7, IC11, IC17, IC23, IC32, IC33
1 MAX232A SO-16 U$2
2 MAX525 SSOP US, US1
1 MAX1270 SSOP U$1
1 MOD1 – ELFIN-EM332/A-4X24
Przekaźnik
1 G2R2 G2R2 K1
Złącza
1 – ML10 Z10
1 – ML26 Z9
1 – ML34 Z8
1 – W237-02P Z7
1 – W237-04P Z12
6 – MPT2 Z1, Z2, Z3, Z4, Z5, Z6
Tabela 8.4: Zestawienie elementów elektronicznych na płytce sterownika.

63
Rozdział 9
Dodatek C
9.1 Schematy ideowe i rysunki montażowe części elektronicznej
Rysunek 9.1: Schemat ideowy jednego z regulatorów prądu modułu mocy.

Rysunek 9.2: Schemat ideowy połączenia modułu EM332 z układami peryferyjnymi.
64

65
Rysunek 9.3: Schemat ideowy zasilacza.
Rysunek 9.4: Schemat ideowy układu sterowania cewką przekaźnika.

66
Rysunek 9.5: Schemat elektryczny kodera AS5040.
Rysunek 9.6: Obwód drukowany wierzchniej warstwy płytki zasilacza.
Rysunek 9.7: Obwód drukowany spodniej warstwy płytki zasilacza.

Rysunek 9.8: Rozmieszczenie elementów na wierzchniej stronie płytki zasilacza.
67

Rysunek 9.9: Obwód drukowany wierzchniej warstwy płytki sterownika.
68

Rysunek 9.10: Obwód drukowany spodniej warstwy płytki sterownika.
69

Rysunek 9.11: Rozmieszczenie elementów na wierzchniej stronie płytki sterownika.
70

Rysunek 9.12: Rozmieszczenie elementów na spodniej stronie płytki sterownika.
71

72
Rysunek 9.13: Obwód drukowany wierzchniej warstwy płytki kodera AS5040.
Rysunek 9.14: Obwód drukowany spodnie warstwy płytki kodera AS5040.
Rysunek 9.15: Rozmieszczenie elementów na wierzchniej warstwie płytki kodera AS5040.
Rysunek 9.16: Rozmieszczenie elementów na spodniej warstwie płytki kodera AS5040.

73
Rozdział 10
Przykładowa instrukcja
laboratoryjna
10.1 Cel ćwiczenia.
Celem ćwiczenia jest zapoznanie się ze strukturą kinematyczną robota dydaktycznego
ZS5R oraz identyfikacja jego parametrów geometrycznych.
10.2 Podstawy teoretyczne.
10.2.1 Charakterystyka robota ZS5R.
Robot dydaktyczny ZS5R jest manipulatorem, którego napęd poszczególnych osi jest zrealizowany
za pomocą silników prądu stałego. Sterownik robota składa się z modułu mocy
i modułu sterowania. Moduł mocy jest zbudowany z sześciu regulatorów prądu, natomiast
moduł sterowania oparty jest na module EM332/B firmy ELFIN, który wykorzystuje
mikrokontroler MC68332. Oprogramowanie sterownika komunikuje się z komputerem
nadrzędnym przy pomocy asynchronicznej transmisji szeregowej (19200, 8N1). Pozwala
to na sterowanie ręczne oraz na zadawanie i odczytywanie chwilowych położeń silników
(M i ). Pomiar położenia wału silników jest realizowany za pomocą koderów impulsowoprzyrostowych
(AS5040) o rozdzielczości 1024 imp/obr. Położenia napędów wyrażone są
w postaci liczb całkowitych oznaczających ilość impulsów zliczanych od pewnego stanu
początkowego. Ustalenie stanu początkowego po włączeniu sterownika odbywa się automatycznie
przez doprowadzenie poszczególnych osi do położeń, w których sygnały czujników
synchronizacji umieszczone w przegubach i sygnaly z kanalów indeks koderów dadzą
wartość 1 (logicznie) (procedura synchronizacji). W tej konfiguracji zwanej konfiguracją
synchronizacji, wszystkie liczniki położenia silników są ustawiane na odpowiednie wartość.
Każde wyłączenie zasilania robota powoduje utratę informacji o pozycji i konieczność
powtórzenia procedury synchronizacji.
Ze względu na bezpośrednie przeniesienie napędu na przeguby 1, 2 i 3 ich czujniki synchronizacji
są osiągane przy ustalonych wartościach kątów {θ si = q si |i = 1, 2, 3}, co odpowiada
ustalonym pozycjom silników M s1 , M s2 , M s3 . Przeniesienie napędu przegubów 4 i 5 jest
także bezpośrednie i odbywa się za pośrednictwem mechanizmu różnicowego. Zmiana kąta
q 4 jest proporcjonalna do sumy zmian pozycji silników (M 4 + M 5 ), a zmiana kąta q 5 - do
ich różnicy (M 5 − M 4 ).

74
10.2.2 Struktura kinematyczna dla współrzędnych
przegubowych q i .
Struktura kinematyczna robota dydaktycznego ZS5R jest przedstawiona na rys. 10.1. Na
Rysunek 10.1: Struktura kinematyczna manipulator ZS5R.
końcu każdego ogniwa robota umieszczony jest lokalny układ współrzędnych związany
z tym ogniwem. Płaszczyzna planarności robota jest wyznaczona przez osie X 0 i Y 0
układu bazowego.
Współrzędnymi wewnętrznymi są:
q 1 – kąt obrotu kolumny [ ◦ ],
q 2 – kąt obrotu głównego ramienia [ ◦ ],
q 3 – kąt obrotu przedramienia [ ◦ ],
q 4 – kąt odchylenia chwytaka względem przedramienia [ ◦ ],
q 5 – kąt obrotu chwytaka wokół własnej osi [ ◦ ],
q 6 – rozstaw szczęk chwytaka [mm].
Jako współrzędne zewnętrzne wybrano następujące zmienne
x, y, z – współrzędne kartezjańskie końca efektora wyrażone
w układzie bazowym X 0 Y 0 Z 0 [mm],

75
η – kąt podejścia chwytaka (kąt pomiędzy nieskręconą
płaszczyzną chwytaka a płszczyzną poziomą) [ ◦ ] ,
ρ – kąt obrotu chwytaka wokół własnej osi [ ◦ ] ,
s – rozstaw szczęk [mm].
10.2.3 Transformacja Denavita-Hartenberga dla zmiennych q i .
Prosty model kinematyki dla robota ZS5R uzyskuje się przy użyciu notacji Denavita-
Hartenberga. Transformacja Denavita-Hartenberga pomiędzy lokalnymi układami współrzędnych
stowarzyszonymi z poszczególnymi ogniwami robota jest następująca
A 1 0 (q 1) = Rot(Z, q 1 )Trans(Z, l 1 )Rot(X, π),
2
A 2 1(q 2 ) = Rot(Z, q 2 )Trans(X, l 2 ),
A 3 2 (q 3) = Rot(Z, q 3 )Trans(X, l 3 ),
A 4 3(q 4 ) = Rot(Z, q 4 )Rot(Z, − π)Rot(X, − π),
2 2
A 4 5 (q 5) = Rot(Z, q 5 )Trans(Z, l 4 ),
(10.1)
gdzie symbolami l i oznaczono następujące parametry geometryczne robota
l 1 – wysokość kolumny [mm],
l 2 – długość ramienia [mm],
l 3 – długość przedramienia [mm],
l 4 – długość chwytaka ujmująca długość palców chwytaka [mm],
10.3 Przebieg ćwiczenia
10.3.1 Obsługa stanowiska
Stanowisko laboratoryjne składa się z robota ZS5R oraz komputera PC połączonego ze
sterownikiem robota za pośrednictwem portu szeregowego. Oprogramowanie stanowi program
ZSident uruchamiany w środowisku systemu Linux.
Robot powinien być ustawiony przy krwędzi stołu, równolegle do przedniej ściany podstawy
robota.
Przed uruchomieniem programu należy włączyć zasilanie robota i poczekać na zakończenie
synchronizacji (zatrzymanie wszystkich silników).
Po uruchomieniu programu ZSident powinna nastąpić powtórna synchronizacja, a na
ekranie pojawi się menu.
Komenda I pozwala zadawać konfigurację we współrzędnych q i (wprowadzenie pustej linii
powoduje pozostawienie dotychczasowej wartości). Po zakończeniu wprowadzania następuje
wykonanie ruchu.
Komenda X podaje odczyt położenia robota w trzech układach współrzędnych:
• {M i } - przestrzeń napędowa (pozycje silników wyrażone w impulsach),

76
• {q i } - przestrzeń konfiguracyjna,
• {x, y, z, η, ρ, s} - przestrzeń zadaniowa.
Komenda Q powoduje zakończenie pracy programu ident i wyłączenie zasilania napędów.
10.3.2 Wstępne przygotowanie do ćwiczenia.
1. używając transformacji Denavita-Hartenberga przedstawionej w rozdziale 10.2.3 obliczyć
prosty model kinematyki
(q 1 , ..., q 6 ) → (x, y, z, η, ρ, s)
2. zastanowić się jak należy wybrać konfigurację robota dydaktycznego, aby z równań
kinematyki uzyskać jednoznacznie wartości parametrów geometrycznych l 1 , l 2 , l 3 , l 4 .
10.3.3 Zadania do wykonania.
Poruszając poszczególnymi przegubami robota ustalić położenie układu bazowego oraz
sprawdzić, jakie zakresy zmienności mają poszczególne współrzędne wenętrzne q i . Aby
zidentyfikować parametry geometryczne l i należy rozpocząć od ustawienia robota w pewnej
konfiguracji poprzez zadanie określonych wartości współrzędnych wewnętrznych q i .
Następnie dla tak zadanej konfiguracji należy odczytać odpowiadające jej współrzędne
zewnętrzne z menu programu. W sprawozdaniu należy umieścić wyliczony prosty model
kinematyki i w oparciu o ten model wyliczyć z układu równań stałe l i .
Instrukcja ta została przygotowana w oparciu o instrukcję ”Identyfikacja parametrów geometrycznych
robota dydaktycznego ROMIK” autorstwa I. Dulęba, A. Mazur, M. Wnuk.