Politechnika Wrocławska

Wydział Elektroniki

Kierunek:

Automatyka i Robotyka (AiR)

Specjalność:

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 Pa-
nu dr. Markowi Wnukowi za po-
moc, poświęcony czas oraz zaangażowa-
nie. Chciałem również podziękować Pa-
nu Ryszardowi Krzywańskiemu za
cenne rady i pomoc w wykonaniu kon-
strukcji mechanicznej. Na końcu chcia-
łem podziękować firmom MICRO-
MOTORS i AUSTRIAMICRO-
SYSTEMS za bezpłatne przekazanie
silników i koderów.

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

1

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

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

3

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

4

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

5

Rozdział 1

Wstęp

W laboratorium robotyki Zakładu Podstaw Cybernetyki i Robotyki, znajduje się manipu-
lator 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 ćwi-
czeń 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ę al-
gorytmó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 zre-
alizowany 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 ste-
rowania 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ółpracu-
ją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 ze-

wnę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.

6

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 od-
powiednim przegubie, a następnie wyznaczeniu ciągu transformacji pomiędzy kolejnymi
układami. W podstawie zaczepiamy nieruchomy układ oznaczony numerem 0. Dalej wy-
bieramy 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

K(q) =

n

Y

i=1

A

i

i

−1

(q

i

) =

"

R

n

0

(q) T

n

0

(q)

0

1

#

.

(2.2)

7

2.2 Wyprowadzenie kinematyki prostej

manipulatora

Rozważany manipulator jest manipulatorem klasy 5R

1

, posiadającym pięć stopni swobo-

dy. Manipulator ten składa się z pionowej, obrotowej kolumny o wysokości l

1

, dwuczłono-

wego ramienia o długości l

2

i l

3

oraz nadgarstka z efektorem o długości l

4

, ujmującej dłu-

gość 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

−

π

2

0

0 −

π

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).

8

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, −

π

2

)Rot(X, −

π

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:

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 1 do 2:

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 2 do 3:

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 3 do 4:

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











,

• przejście od układu 4 do 5:

9

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 de-

finiujemy kinematykę manipulatora zgodnie z (2.2) jako

K(q) = A

1

0

A

2

1

A

3

2

A

4

3

A

5

4

(q): X

0

Y

0

Z

0

7−→ X

5

Y

5

Z

5

czyli

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 wy-
znacza 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. Za-
danie 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.

10

Przy podejściu geometrycznym [1] [2] do rozwiązania zagadnienia kinematycznego ko-

nieczne jest zdekomponowanie przestrzennego zagadnienia geometrycznego na szereg za-
gadnień 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-

11

czoną przez osie X

0

, Y

0

, stąd widzimy, że

tan q

1

=

P

y

P

x

,

(2.6)

czyli

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

c

=







P

cx

P

cy

P

cz







=







P

x

− k cos q

1

P

y

− k sin q

1

P

z

− l

4

sin η







,

gdzie k = l

4

cos η.

(2.8)

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 mani-

pulatora jest spełniony związek d

2

= r

2

+(P

cz

−l

1

)

2

, gdzie r

2

= P

2

cx

+P

2

cy

, zatem otrzymuje

się dalej

P

2

cx

+ P

2

cy

+ (P

cz

− l

1

)

2

= l

2

2

+ l

2

3

+ 2l

2

l

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

− l

2

3

2l

2

l

3

.

= D.

(2.11)

Istnieją dwa rozwiązania powyższego równania, a mianowicie

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 0 ¬ q

3

¬ π,

(2.12)

oraz

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

sin q

3

= ±

√

1 − D

2

,

(2.14)

i stąd można wyznaczyć

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

q

P

2

cx

− P

2

cy

,

(2.16)

12

czyli

δ

= Atan2(P

cz

− l

1

,

q

P

2

cx

− P

2

cy

).

(2.17)

Podobnie wyznaczamy kat α

tan α =

l

3

sinq

3

l

2

+ l

3

cosq

3

,

(2.18)

czyli

α

= 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

,

q

P

2

cx

− P

2

cy

) − Atan2(l

3

sinq

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

,

q

P

2

cx

− P

2

cy

) − Atan2(l

3

sinq

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

2

cx

+P

2

cy

+(P

cz

−l

1

)

2

−l

2

2

−l

2

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)

13

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 indek-
sują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 rysun-

ku 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.

14

q

1

− q

01

=

Z

1

2π

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

s

1

= sin q

1

,

(2.25)

ostatecznie ma postać

c

1

= cos (q

01

+

Z

1

2π

Z

2

DG

1

(M

1

− M

01

)),

(2.26)

s

1

= sin (q

01

+

Z

1

2π

Z

2

DG

1

(M

1

− M

01

)),

(2.27)

a transformacja odwrotna

q

1

= Atan2(s

1

, c

1

),

(2.28)

czyli

M

1

= M

01

+

Z

2

DG

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

3

2π

Z

4

DG

2

(M

2

− M

02

),

(2.30)

Transformacja prosta napędu ma postać

c

2

= cos (q

02

+

Z

3

2π

Z

4

DG

2

(M

2

− M

02

)),

(2.31)

s

2

= sin (q

02

+

Z

3

2π

Z

4

DG

2

(M

2

− M

02

)),

(2.32)

a transformacja odwrotna

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

=

Z

5

2π

Z

6

DG

3

(M

3

− M

03

),

(2.34)

Transformacja prosta napędu ma postać

c

3

= cos (q

03

+

Z

5

2π

Z

6

DG

3

(M

3

− M

03

)),

(2.35)

s

3

= sin (q

03

+

Z

5

2π

Z

6

DG

3

(M

3

− M

03

)),

(2.36)

a transformacja odwrotna

q

3

= Atan2(s

3

, c

3

).

(2.37)

15

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

=

Z

7

2π

2Z

8

DG

4

((M

5

− M

05

) − (M

4

− M

04

)),

(2.38)

Transformacja prosta napędu

c

4

= cos (q

04

+

Z

7

2π

2Z

8

DG

4

((M

5

− M

05

) − (M

4

− M

04

))),

(2.39)

s

4

= sin (q

04

+

Z

7

2π

2Z

8

DG

4

((M

5

− M

05

) − (M

4

− M

04

))),

(2.40)

a transformacja odwrotna

q

2

= Atan2(s

2

, c

2

).

(2.41)

Z kolei dla ogniwa 5 otrzymujemy równania przeniesienia napędu postaci

q

5

− q

05

= −

Z

7

2π

2Z

9

DG

5

((M

4

− M

04

) + (M

5

− M

05

)),

(2.42)

Transformacja prosta napędu

c

5

= cos (q

05

−

Z

7

2π

2Z

9

DG

4

((M

5

− M

05

) − (M

4

− M

04

))),

(2.43)

16

Rysunek 2.5: Struktura układu różnicowego napędzającego osie nadgarstka.

s

5

= sin (q

05

−

Z

7

2π

2Z

9

DG

4

((M

5

− M

05

) − (M

4

− M

04

))),

(2.44)

a transformacja odwrotna

q

2

= Atan2(s

2

, c

2

).

(2.45)

Warto zwrócić uwagę, że efektywniejszą obliczeniowo implementację transformacji pro-
stych i odwrotnych uzyskuje się dzięki nadmiarowej reprezentacji kątów przegubowych q

i

jako par (c

i

, s

i

).

17

Rozdział 3

Konstrukcja robota ZS5R

3.1 Konstrukcja mechaniczna

Podstawowymi materiałami, z których została wykonana konstrukcja manipulatora ro-
bota 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 zmonto-
wane 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.

Rysunek 3.1: Zdjęcie manipulator.

Oznaczenia na rysunku 3.2

18

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

19

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 na-
pę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 zamiesz-
czone 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 wykorzy-
stano 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 zesta-
wiono 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.

20

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.

21

Silniki te dodatkowo są fabrycznie wyposażone w kodery halotronowe. Jednak mała roz-
dzielczość (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 zamoco-
wania 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].

22

3.2 Konstrukcja elektroniczna – sterownik robota

Robot ZS5R posiada manipulator, którego poszczególne ramiona mogą wykonywać se-
kwencje 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 pa-
rametró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,

23

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 na-

pię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łyt-
ki, 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,

24

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ędo-
wego 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 wy-
konawcze 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 rezy-
storó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

25

Rysunek 3.10: Schemat blokowy regulatora prądu.

przez silnik. Sygnał z układu komparatora jest podawany na układy logiczne, ktorych za-
daniem 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 zamieszczo-

ny w dodatku C. Na rysunku 3.11 jest przedstawione zdjęcie zasilacza.

Rysunek 3.11: Zasilacz części cyfrowej i analogowej

26

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 ste-

rownika. 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ółpra-
cują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ęcio-
wym 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.

27

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

.

28

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<POM I1<+5V)

15

POM I2

sygnał pomiaru prądu silnika M

2

(-5V<POM I2<+5V)

16

POM I3

sygnał pomiaru prądu silnika M

3

(-5V<POM I3<+5V)

17

POM I4

sygnał pomiaru prądu silnika M

4

(-5V<POM I4<+5V)

18

POM I5

sygnał pomiaru prądu silnika M

5

(-5V<POM I5<+5V)

19

POM I6

sygnał pomiaru prądu silnika M

6

(-5V<POM I6<+5V)

20

STER I1

sygnał zadający prąd silnika M

1

(0-2.5V)

21

STER I2

sygnał zadający prąd silnika M

2

(0-2.5V)

23

STER I3

sygnał zadający prąd silnika M

3

(0-2.5V)

25

STER I4

sygnał zadający prąd silnika M

4

(0-2.5V)

27

STER I5

sygnał zadający prąd silnika M

5

(0-2.5V)

29

STER I6

sygnał zadający prąd silnika M

6

(0-2.5V)

31

STERI1

sygnał z C/A zadający prąd silnika M

1

(0-2.5V)

22

STERI2

sygnał z C/A zadający prąd silnika M

2

(0-2.5V)

24

STERI3

sygnał z C/A zadający prąd silnika M

3

(0-2.5V)

26

STERI4

sygnał z C/A zadający prąd silnika M

4

(0-2.5V)

28

STERI5

sygnał z C/A zadający prąd silnika M

5

(0-2.5V)

30

STERI6

sygnał z C/A zadający prąd silnika M

6

(0-2.5V)

32

Tabela 3.6: Opis sygnałów łączówki Z

8

.

INDEX2 1

2 SYNCHR2

INDEX1 3

4 SYNCHR1

INDEX4 5

6 SYNCHR4

INDEX3 7

8 SYNCHR3

INDEX6 9

10 SYNCHR6

INDEX5 11 12 SYNCHR5

TPU0

13 14

TPU1

TPU2

15 16

TPU3

TPU4

17 18

TPU5

TPU6

19 20

TPU7

TPU8

21 22

TPU9

TPU10

23 24

TPU11

GND

25 26

Vcc

Tabela 3.7: Łączówka Z

9

.

29

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

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 przy-
padku oprogramowanie zostało napisane w języku C z użyciem makr w asemblerze. Opro-
gramowanie 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 mikro-
kontrolera 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 przerwa-
nia cyklicznego PIT

/*Konfiguracja przerwania cyklicznego*/

*((void (**) ())(4*PI_VECT)) = int_PIT; /* ustawienie wektora */

PICR = (PI_LEVEL<<8)+PI_VECT;

/* poziom i wektor przerwania PIT*/

PITR = PI_CONST;

/* stala dzielnika PIT */

Funkcja qsm_int() inicjalizuje magistrale QSPI oraz blok transmisji szeregowej SCI

/*Konfiguracja przerwan SCI */

*((void (**) ())(4*SCI_VECT)) = my_SCI; /* ustawienie wektora */

QIVR = SCI_VECT;

/* wektor przerwania */

QILR = (QSPI_LEVEL<<3 )+SCI_LEVEL;

/* poziom przerwania */

31

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 struk-
turze nast.

typedef struct
{

.\\
.\\

32

.\\

}NAS;
NAS

nast;

Wszystkie procesy związane ze sterowaniem oraz akwizycją danych pomiarowych, obsłu-
giwane 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 prze-
rwania 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:

33

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 sze-
regowe 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))

34

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 ro-
bota Romik (w zakresie menu). Pozwala również na zbieranie wyników pomiarów para-
metrów ruchu osi robota przy prowadzeniu eksperymentów.

35

Rozdział 5

Badania i wyniki eksperymentów

W celu zbadanie konstrukcji sterownika i zaobserwowanie parametrów jakościowych prze-
prowadzono odpowiednie eksperymenty. W celu zebrania pomiarów zaimplementowano
program testowy. W programie tym, cyklicznie co 2ms wywoływana jest procedura po-
zwalają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 wy-
kresach 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.

-1000

-500

0

500

1000

1500

2000

2500

3000

3500

4000

4500

0

5

10

15

20

25

30

’pom2.9.txt’ using 1:2

’pom2.9.txt’ using 1:3

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

36

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ęd-
kości steruje prądem silnika utrzymując prędkość ruchu przegubu (rysunek 5.3).

-1000

0

1000

2000

3000

4000

5000

6000

7000

0

10

20

30

40

50

60

’pom1.txt’ using 1:2

’pom1.txt’ using 1:3

’pom1.txt’ using 1:4

Rysunek 5.2: Rozpędzanie osi q

0

.

-1000

0

1000

2000

3000

4000

5000

6000

7000

8000

0

100

200

300

400

500

600

700

800

900

1000

’pom1.txt’ using 1:2

’pom1.txt’ using 1:3

’pom1.txt’ using 1:4

Rysunek 5.3: Regulacja prędkości osi q

0

.

Jak widać, opracowany sterownik pozwala gromadzić pomiary parametrów ruchu po-

szczególnych osi, co jest szczególnie przydatne przy badaniu własności regulatorów, do-
bieraniu ich nastaw, szukaniu charakterystyk napędów i własności dynamicznych robota.

37

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 mo-
mentem napędowym), przez co istnieje w przyszłości możliwość implementacji algoryt-
mów sterowania uwzględniających dynamikę robota. W budowie układu elektronicznego

38

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 te-
stowe 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 za-
kresie niż wcześniej wspomniany Romik, ze względu na możliwość sterowania momentem
napędowym i otwartą konstrukcję układu sterowania.

39

Bibliografia

[1] E. JEZIERSKI. Robotyka kurs podstawowy. Wydawnictwo Politechniki Łódzkiej,

Łódź, 2002.

[2] M.W. SPONG and M. VIDYASAGAR. Dynamika i sterowanie robotów. Wydawnic-

two Naukowo-Techniczne, Warszawa, 1997.

[3] K. TCHOŃ and W. JACAK. Podstawy robotyki. Wydawnictwo Politechniki Wro-

cławskiej, Wrocław, 1992.

[4] K. TCHOŃ, A. MAZUR, I. DULĘBA, R. HOSSA, i R. MUSZYŃSKI. Manipula-

tory 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/

40

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

41

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].

42

43

44

45

46

47

48

49

50

51

52

53

54

55

56

57

58

59

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.

60

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.

61

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.

62

Rozdział 9

Dodatek C

9.1 Schematy ideowe i rysunki montażowe części elek-

tronicznej

Rysunek 9.1: Schemat ideowy jednego z regulatorów prądu modułu mocy.

63

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

64

Rysunek 9.3: Schemat ideowy zasilacza.

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

65

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.

66

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

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.

72

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 zre-
alizowany 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, nato-
miast moduł sterowania oparty jest na module EM332/B firmy ELFIN, który wykorzystu-
je 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 impulsowo-

przyrostowych (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ę auto-
matycznie przez doprowadzenie poszczególnych osi do położeń, w których sygnały czujni-
kó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 war-
tość. 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 synchro-
nizacji 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

).

73

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],

74

η

– 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, −

π

2

)Rot(X, −

π

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 pro-
gram ZSident uruchamiany w środowisku systemu Linux.
Robot powinien być ustawiony przy krwędzi stołu, równolegle do przedniej ściany pod-
stawy robota.
Przed uruchomieniem programu należy włączyć zasilanie robota i poczekać na zakończe-
nie 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),

75

• {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 ob-

liczyć 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 pew-

nej 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 geo-
metrycznych robota dydaktycznego ROMIK” autorstwa I. Dulęba, A. Mazur, M. Wnuk.

76