Manipulator przeznaczony do celów dydaktycznych

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 MICROMOTORS i AUSTRIAMICROSYSTEMS za bezpłatne przekazanie
silników i koderów.
Spis treści
1 Wstęp
1.1 Cel i zakres pracy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6
6
2 Kinematyka manipulatora
2.1 Reprezentacja Denavita-Hartenberga . . . . . . . . . . . . . .
2.2 Wyprowadzenie kinematyki prostej
manipulatora . . . . . . . . . . . . . . . . . . . . . . . . . . .
2.3 Kinematyka odwrotna manipulatora – podejście geometryczne
2.4 Konfiguracje wyróżnione manipulatora . . . . . . . . . . . . .
2.5 Transformacja napędowa osi robota . . . . . . . . . . . . . . .
2.5.1 Rozwiązanie napędów ogniwa 1,2 i 3 . . . . . . . . . .
2.5.2 Rozwiązanie napędu ogniwa 4 i 5 . . . . . . . . . . . .
7
7
3 Konstrukcja robota ZS5R
3.1 Konstrukcja mechaniczna . . . . . . . . . . . .
3.1.1 Silniki manipulatora . . . . . . . . . .
3.2 Konstrukcja elektroniczna – sterownik robota
3.2.1 Budowa modułu sterowania . . . . . .
3.2.2 Budowa modułu mocy . . . . . . . . .
3.2.3 Opis łączówek sterownika . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
. . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
8
10
13
14
14
16
.
.
.
.
.
.
18
18
20
23
24
25
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
10.1 Cel ćwiczenia. . . . . . . . . . . . . . . . . . . . . . . . . .
10.2 Podstawy teoretyczne. . . . . . . . . . . . . . . . . . . . .
10.2.1 Charakterystyka robota ZS5R. . . . . . . . . . . . .
10.2.2 Struktura kinematyczna dla współrzędnych
przegubowych qi . . . . . . . . . . . . . . . . . . . .
10.2.3 Transformacja Denavita-Hartenberga dla zmiennych
10.3 Przebieg ćwiczenia . . . . . . . . . . . . . . . . . . . . . .
10.3.1 Obsługa stanowiska . . . . . . . . . . . . . . . . . .
10.3.2 Wstępne przygotowanie do ćwiczenia. . . . . . . . .
10.3.3 Zadania do wykonania. . . . . . . . . . . . . . . . .
2
73
. . . . . . . . . 73
. . . . . . . . . 73
. . . . . . . . . 73
. .
qi .
. .
. .
. .
. .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
74
75
75
75
76
76
Spis rysunków
2.1
2.2
2.3
2.4
2.5
Struktura
Struktura
Struktura
Struktura
Struktura
kinematyczna manipulator ZS5R. . . . . . . . . . .
szkieletowa robota. . . . . . . . . . . . . . . . . . .
układu przeniesienia napędu ogniwa 1 i 2. . . . . . .
układu przeniesienia napędu ogniwa 3. . . . . . . .
układu różnicowego napędzającego osie nadgarstka.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
8
11
14
16
17
3.1
3.2
3.3
3.4
3.5
3.6
3.7
3.8
3.9
3.10
3.11
Zdjęcie manipulator. . . . . . . . . . . . . . . . . . . . . . . . . . . .
Rysunek złożeniowy manipulatora z oznaczonymi głównymi częściami.
Zdjęcie silnika HLE149.24.43. . . . . . . . . . . . . . . . . . . . . . .
Zdjęcie silnika RHE158.24.100. . . . . . . . . . . . . . . . . . . . . . .
Zdjęcie ilustrujące sposób zamontowania magnesu na osi silnika. . . .
Zdjęcie ilustrujące sposób zamontowania kodera AS5040. . . . . . . .
Schemat blokowy układu sterowania manipulatora. . . . . . . . . . .
Płytka sterownika . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Moduł EM332/B. . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
Schemat blokowy regulatora prądu. . . . . . . . . . . . . . . . . . . .
Zasilacz części cyfrowej i analogowej . . . . . . . . . . . . . . . . . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
18
19
21
21
22
22
23
24
25
26
26
4.1
Schemat blokowy układu sterowania. . . . . . . . . . . . . . . . . . . . . . 34
5.1
5.2
5.3
Wykres odpowiedzi regulatora na zadany prąd. . . . . . . . . . . . . . . . 36
Rozpędzanie osi q0 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
Regulacja prędkości osi q0 . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37
6.1
Widok skonstruowanego manipulatora. . . . . . . . . . . . . . . . . . . . . 38
7.1
7.2
Rysunek techniczny silnika RHE 158.24.100. . . . . . . . . . . . . . . . . . 42
Rysunek techniczny silnika HLE 149.24.43. . . . . . . . . . . . . . . . . . . 42
9.1
9.2
9.3
9.4
9.5
9.6
9.7
9.8
9.9
9.10
9.11
Schemat ideowy jednego z regulatorów prądu modułu mocy. . . . . . .
Schemat ideowy połączenia modułu EM332 z układami peryferyjnymi.
Schemat ideowy zasilacza. . . . . . . . . . . . . . . . . . . . . . . . . .
Schemat ideowy układu sterowania cewką przekaźnika. . . . . . . . . .
Schemat elektryczny kodera AS5040. . . . . . . . . . . . . . . . . . . .
Obwód drukowany wierzchniej warstwy płytki zasilacza. . . . . . . . . .
Obwód drukowany spodniej warstwy płytki zasilacza. . . . . . . . . . .
Rozmieszczenie elementów na wierzchniej stronie płytki zasilacza. . . .
Obwód drukowany wierzchniej warstwy płytki sterownika. . . . . . . .
Obwód drukowany spodniej warstwy płytki sterownika. . . . . . . . . .
Rozmieszczenie elementów na wierzchniej stronie płytki sterownika. . .
3
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
63
64
65
65
66
66
66
67
68
69
70
9.12
9.13
9.14
9.15
9.16
Rozmieszczenie elementów na spodniej stronie płytki sterownika. . . . . .
Obwód drukowany wierzchniej warstwy płytki kodera AS5040. . . . . . .
Obwód drukowany spodnie warstwy płytki kodera AS5040. . . . . . . . .
Rozmieszczenie elementów na wierzchniej warstwie płytki kodera AS5040.
Rozmieszczenie elementów na spodniej warstwie płytki kodera AS5040. .
.
.
.
.
.
71
72
72
72
72
10.1 Struktura kinematyczna manipulator ZS5R. . . . . . . . . . . . . . . . . . 74
4
Spis tabel
2.1
Parametry Denavita-Hartenberga manipulatora. . . . . . . . . . . . . . . .
3.1
3.2
3.3
3.4
3.5
3.6
3.7
3.8
3.9
3.10
3.11
3.12
Parametry silnika typu RHE158.24.100. . . .
Parametry silnika typu HLE149.24.43. . . .
Łączówka transmisji szeregowej Z10 . . . . . .
Sygnały łączówki transmisji szeregowej Z10 .
Łączówka Z8 . . . . . . . . . . . . . . . . . .
Opis sygnałów łączówki Z8 . . . . . . . . . .
Łączówka Z9 . . . . . . . . . . . . . . . . . .
Opis sygnałów łączówki Z9 . . . . . . . . . .
Opis złącza zasilania modułu mocy Z7 . . . .
Opis złącza zasilania Z12 . . . . . . . . . . . .
Opis złącza na płytce zasilacz Z13 . . . . . . .
Opis złącz Z1 ,...Z6 . . . . . . . . . . . . . . .
8.1
8.2
8.3
8.4
Zestawienie
Zestawienie
Zestawienie
Zestawienie
elementów
elementów
elementów
elementów
elektronicznych
elektronicznych
elektronicznych
elektronicznych
5
na
na
na
na
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
płytce
płytce
płytce
płytce
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
8
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
20
21
27
27
28
29
29
30
30
30
30
30
zasilacza. . . . .
kodera AS5040.
sterownika. . . .
sterownika. . . .
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
.
60
60
61
62
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.
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 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ś Zi jest osią (i + 1)-go złącza, a zwrot tej osi może być przyjęty dowolnie;
• oś Xi 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ś Yi 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 Zi oraz Xi zatem niektórzy pomijają ją aby zwiększyć czytelność rysunków.
Transformacja Aii−1 pomiędzy układami (i − 1) oraz i jest zdefiniowana jako iloczyn
czterech macierzy elementarnych obrotów i przesunięć.
Aii−1 (qi ) = Rot(Z, θi )Trans(Z, di )Trans(X, ai )Rot(X, αi )
(2.1)
gdzie:
θi - kąt obrotu wokół osi Zi−1 ,
di - translacja wzdłuż bieżącej osi Z,
ai - 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
Aii−1 (qi )
=
i=1
7
"
Rn0 (q) Tn0 (q)
0
1
#
.
(2.2)
2.2
Wyprowadzenie kinematyki prostej
manipulatora
Rozważany manipulator jest manipulatorem klasy 5R1 , posiadającym pięć stopni swobody. Manipulator ten składa się z pionowej, obrotowej kolumny o wysokości l1 , dwuczłonowego ramienia o długości l2 i l3 oraz nadgarstka z efektorem o długości l4 , ujmującej długość palców chwytaka. Wszystkie te ogniwa - ramiona są połączone przegubami, których
aktualny stan określa wektor stanu q = (q1 , q2 , q3 , q4 , q5 )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
1
2
3
4
5
θi
q1
q2
q3
q4 −
q5
π
2
di
l1
0
0
0
l4
ai
0
l2
l3
0
0
αi
π
2
0
0
− π2
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).
A10 (q1 ) = Rot(Z, q1 )Trans(Z, l1 )Rot(X, π2 ),
A21 (q2 ) = Rot(Z, q2 )Trans(X, l2 ),
A32 (q3 ) = Rot(Z, q3 )Trans(X, l3 ),
(2.3)
A43 (q4 ) = Rot(Z, q4 )Rot(Z, − π2 )Rot(X, − π2 ),
A45 (q5 ) = Rot(Z, q5 )Trans(Z, l4 ),
Po wprowadzeniu oznaczeń sinqi = si , cosqi = ci , kolejne macierze przyjmują postać:
• przejście od układu 0 do 1:




A10 (q1 ) = 
c1
s1
0
0
0 s1
0 −c1
1 0
0 0
0
0
l1
1



,

• przejście od układu 1 do 2:


A21 (q2 ) = 



c2 −s2
s2 c2
0
0
0
0
0 l 2 c2
0 l2 s2 

,
1 0 
0 1
c3 −s3
s3 c3
0
0
0
0
0 l 3 c3
0 l3 s3 

,
1 0 
0 1
• przejście od układu 2 do 3:




A32 (q3 ) = 

• przejście od układu 3 do 4:




A43 (q4 ) = 
s4
−c4
0
0
• przejście od układu 4 do 5:
9
0 c4
0 s4
1 0
0 0
0
0
0
1



,





A54 (q5 ) = 
c5 −s5
s5 c5
0
0
0
0
0
0
1
0
0
0
l4
1



.

Mając wyliczone transformacje Aii−1 (qi ) między sąsiednimi układami współrzędnych definiujemy kinematykę manipulatora zgodnie z (2.2) jako
K(q) = A10 A21 A32 A43 A54 (q): X0 Y0 Z0 7−→ X5 Y5 Z5
czyli




K(q) = 

r11 r12 r13 lx
r21 r22 r23 ly 

,
r31 r32 r33 lz 
0
0
0 1
(2.4)
gdzie elementy tej macierzy są następujące:
r11
r12
r13
r21
r22
r23
r31
r32
r33
lx
ly
lz
2.3
=
=
=
=
=
=
=
=
=
=
=
=


cosq1 cosq5 sin(q2 + q3 + q4 ) − sinq1 sinq5 ,





−cosq1 sinq5 sin(q2 + q3 + q4 ) − sinq1 cosq5 ,





cosq1 cos(q2 + q3 + q4 ),




sinq1 cosq5 sin(q2 + q3 + q4 ) + cosq1 sinq5 ,





−sinq1 sinq5 sin(q2 + q3 + q4 ) + cosq1 cosq5 ,




sinq1 cos(q2 + q3 + q4 ),

−cos(q2 + q3 + q4 )cosq5 ,





cos(q2 + q3 + q4 )sinq5 ,





sin(q2 + q3 + q4 ),



l2 cosq1 cos2 + l3 cosq1 cos(q2 + q3 ) + l4 cosq1 cos(q2 + q3 + q4 ), 




l2 sinq1 cos2 + l3 sinq1 cos(q2 + q3 ) + l4 sinq1 cos(q2 + q3 + q4 ), 




l1 + l2 sinq2 + l3 sin(q2 + q3 ).
(2.5)
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.
10
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 (Px , Py , Pz , η, %), gdzie Px , Py , Pz 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 q5 , który odpowiedzialny jest za rotację wokół osi
Z5 .
Rysunek 2.2: Struktura szkieletowa robota.
W celu wyznaczenia kąta q1 , rzutujemy punkt pracy chwytaka P na płaszczyznę wyzna11
czoną przez osie X0 , Y0 , stąd widzimy, że
tan q1 =
Py
,
Px
(2.6)
czyli
q1 = Atan2(Py , Px ),
(2.7)
gdzie Atan2(·, ·) oznacza dwuargumentową funkcję arcus tangens.
W celu ułatwienia obliczenia kolejnych zmiennych przegubowych wyznaczony został punkt
pomocniczy Pc o współrzędnych




Pcx
Px − k cos q1




Pc =  Pcy  =  Py − k sin q1  ,
Pcz
Pz − l4 sin η
gdzie k = l4 cos η.
(2.8)
Aby wyznaczyć kąt q3 wykorzystuje się twierdzenie cosinusów
d2 = l22 + l32 − 2l2 l3 cos (π − q3 ).
(2.9)
Ponieważ zachodzi zależność cos (π − q3 ) = − cos q3 , oraz dla dowolnej konfiguracji mani2
2
pulatora jest spełniony związek d2 = r 2 +(Pcz −l1 )2 , gdzie r 2 = Pcx
+Pcy
, zatem otrzymuje
się dalej
2
2
Pcx
+ Pcy
+ (Pcz − l1 )2 = l22 + l32 + 2l2 l3 cos q3 ,
(2.10)
i stąd
2
2
Pcx
+ Pcy
+ (Pcz − l1 )2 − l22 − l32 .
= D.
2l2 l3
Istnieją dwa rozwiązania powyższego równania, a mianowicie
cos q3 =
q3 = arccos
2
2
Pcx
+ Pcy
+ (Pcz − l1 )2 − l22 − l32
,
2l2 l3
stąd 0 ¬ q3 ¬ π,
(2.11)
(2.12)
oraz
q3 = − arccos
2
2
Pcx
+ Pcy
+ (Pcz − l1 )2 − l22 − l32
,
2l2 l3
stąd
− π ¬ q3 ¬ 0.
(2.13)
Jednakże lepszym sposobem znalezienia kąta q3 jest zauważenie, że jeśli cos q3 jest dany
wzorem (2.11), to sin q3 jest dany odpowiednio wzorem
√
sin q3 = ± 1 − D 2 ,
(2.14)
i stąd można wyznaczyć
√
q3 = 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 q2 wyznacza się wyrażenia na kąty pomocnicz δ oraz α zaznaczone
na rysunku 2.2. Dla kąta δ zachodzi
tan δ =
Pcz − l1
Pcz − l1
=q
,
2 − P2
r
Pcx
cy
12
(2.16)
czyli
δ = Atan2(Pcz − l1 ,
Podobnie wyznaczamy kat α
tan α =
q
2 − P 2 ).
Pcx
cy
(2.17)
l3 sinq3
,
l2 + l3 cosq3
(2.18)
czyli
α = Atan2(l3 sinq3 , l2 + l3 cosq3 ).
(2.19)
Z rysunku otrzymuje się prostą zależność między kątami q2 , δ, α, a mianowicie q2 = δ − α,
z której wynika,że
q2 = Atan2(Pcz − l1 ,
q
2 − P 2 ) − Atan2(l sinq , l + l cosq ).
Pcx
3
3 2
3
3
cy
(2.20)
Wartość kąta q2 zależy od kąta q3 . Ma to znaczenie fizyczne, ponieważ otrzymamy
różne wartości kąta q2 w zależności od tego, które rozwiązanie wybierzemy dla kąta q3 .
Z kolei kąt q4 wyznaczamy z następującego związku η = q2 + q3 + q4 czyli
q4 = η − q 2 − q 3 .
(2.21)
Reasumując dla P (Px , Py , Pz , η, %) otrzymujemy następujące zależności:



q1 = Atan2(Py , Px ),




q


2
2

q2 = Atan2(Pcz − l1 , Pcx − Pcy ) − Atan2(l3 sinq3 , l2 + l3 cosq3 ), 



√
q3 = Atan2(± 1 − D 2 , D),
q4 = η − q 2 − q 3 ,
q5 = %.
gdzie
Pcx = Px − k cos q1 , Pcy = Py − k sin q1 , Pcz = Pz − l4 sin η, D =
k = l4 cos η.
2.4













(2.22)
2 +P 2 +(P −l )2 −l2 −l2
Pcx
cz
1
cy
2
3
,
2l2 l3
Konfiguracje wyróżnione manipulatora
W celu mechanicznego sprawdzenia poprawności pozycjonowania manipulatora wyróżniono konfigurcję zerową manipulatora zwaną również konfiguracją geometryczną q 0 =
(q01 , q02 , q03 , q04 , q05 ), dla której zmienne przegubowe przyjmują następujace wartości:
q01 = 0 [◦ ],
q02 = 0 [◦ ],
q03 = 0 [◦ ],
q04 = 0 [◦ ],
q05 = 0 [◦ ].
13
(2.23)
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 Mi ze zmiennymi przegubowymi qi .
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
G1 = G2 = G3 = G6 = 94.37 i G4 = G5 = 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 G1 a następnie przez przekładnię jednostopniową o przełożeniu ZZ12 , gdzie Z1
i Z2 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.
14
q1 − q01 =
Z1 2π
(M1 − M01 ),
Z2 DG1
(2.24)
gdzie q0i jest wartością zmiennej przegubowej qi w konfiguracji zerowej, a M0i jest wartością zmiennej napędowej Mi w konfiguracji zerowej.
Transformacja prosta napędu
c1 = cos q1
(2.25)
s1 = sin q1 ,
ostatecznie ma postać
c1 = cos (q01 +
Z1 2π
(M1 − M01 )),
Z2 DG1
(2.26)
s1 = sin (q01 +
Z1 2π
(M1 − M01 )),
Z2 DG1
(2.27)
a transformacja odwrotna
q1 = Atan2(s1 , c1 ),
(2.28)
czyli
Z2 DG1
(Atan2(s1 , c1 ) − q01 ).
(2.29)
Z1 2π
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
Z3 2π
q2 − q02 =
(M2 − M02 ),
(2.30)
Z4 DG2
Transformacja prosta napędu ma postać
M1 = M01 +
c2 = cos (q02 +
Z3 2π
(M2 − M02 )),
Z4 DG2
(2.31)
s2 = sin (q02 +
Z3 2π
(M2 − M02 )),
Z4 DG2
(2.32)
a transformacja odwrotna
q2 = Atan2(s2 , c2 ).
(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 Z5 przekładni jednostopniowej nie jest zamocowane
na osi przekładni G3 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
q3 − q03 =
Z5 2π
(M3 − M03 ),
Z6 DG3
(2.34)
Transformacja prosta napędu ma postać
c3 = cos (q03 +
Z5 2π
(M3 − M03 )),
Z6 DG3
(2.35)
s3 = sin (q03 +
Z5 2π
(M3 − M03 )),
Z6 DG3
(2.36)
a transformacja odwrotna
q3 = Atan2(s3 , c3 ).
15
(2.37)
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 G4 = G5 . 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 Z7 są osadzone na osiach silników, kolejne
dwa koła o ilości zębów Z8 wykonują ruch obrotowy w osi kąta q4 , natomiast ostatnie
koło zębate o liczbie zębów Z9 obraca się w osi kąta q5 . Równania przeniesienia napędu
dla ogniwa 4 wyrażają się następująco
q4 − q04 =
Z7 2π
((M5 − M05 ) − (M4 − M04 )),
2Z8 DG4
(2.38)
Transformacja prosta napędu
c4 = cos (q04 +
Z7 2π
((M5 − M05 ) − (M4 − M04 ))),
2Z8 DG4
(2.39)
s4 = sin (q04 +
Z7 2π
((M5 − M05 ) − (M4 − M04 ))),
2Z8 DG4
(2.40)
a transformacja odwrotna
q2 = Atan2(s2 , c2 ).
(2.41)
Z kolei dla ogniwa 5 otrzymujemy równania przeniesienia napędu postaci
q5 − q05 = −
Z7 2π
((M4 − M04 ) + (M5 − M05 )),
2Z9 DG5
(2.42)
Z7 2π
((M5 − M05 ) − (M4 − M04 ))),
2Z9 DG4
(2.43)
Transformacja prosta napędu
c5 = cos (q05 −
16
Rysunek 2.5: Struktura układu różnicowego napędzającego osie nadgarstka.
s5 = sin (q05 −
Z7 2π
((M5 − M05 ) − (M4 − M04 ))),
2Z9 DG4
(2.44)
a transformacja odwrotna
q2 = Atan2(s2 , c2 ).
(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 (ci , si ).
17
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 M 3. 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.
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,
M1 – silnik napędzający oś kolumny ,
M2 – silnik napędzający oś głównego ramienia,
M3 – silnik napędzający oś przedramienia ,
M45 – dwa silniki napędzjące osie chwytaka ,
M6 – 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
przełożenie przekładni
maksymalny ciągły moment obrotowy
prędkość obrotowa bez obciązenia
prędkość obrotowa z max. momentem obciążenia
pobór prądu silnika w stanie jałowym
pobór prądu silnika przy max. obciążeniu
24[V ]
94.37 : 1
60[N cm]
66[obr/min]
45[obr/min]
< 70[mA]
340[mA]
Tabela 3.1: Parametry silnika typu RHE158.24.100.
20
Polecamy serwis z poradami dotyczącymi procesu pisania prac magisterskich - prace
magisrterskie - serwis z wytycznymi, wskazówkami, normami - całkowicie za darmo
Polecamy serwis darmowe _ prace mgr z bankowości - strona z darmowym i fragmentami
prac dyplomowych - prace magisterskie, licencjackie, semestralne, zaliczeniowe.
Polecamy serwis darmowe _ prace mgr z prawa - strona z darmowym i fragmentami prac
dyplomowych - prace magisterskie, licencjackie, semestralne, zaliczeniowe.
Polecamy solidny serwis darmowe prace mgr z prawa - znajdziecie tam darmowe fragmenty prac
dyplomowych - prace magisterskie, licencjackie, semestralne, zaliczeniowe.
Polecamy serwis z darmowymi referatami - darmowe prace - strona z darmowym referatami, esejami,
konspektami prac...
Polecamy serwis darmowe _ prace mgr z _ pedagogiki - strona z darmowym i fragmentami
prac dyplomowych - prace magisterskie, licencjackie, semestralne, zaliczeniowe.
Polecamy internetowy serwis darmowe _prace mgr z _zarządzania - strona z całkowicie
darmowym i fragmentami prac dyplomowych - prace licencjackie, magisterskie, semestralne i
zaliczeniowe.
Poradnik jak pisać prace dyplomowe znajdziesz na stronie abcpisania.
Forum o serwisach zajmujących się pisaniem prac licencjackich. Zamawialiście prace licencjackie,
chcecie o tym napisać? To jest właśnie odpowiednie miejsce - forum o pracach magisterskich
Mamy specjalistów z zakresu bankowości, rachunkowości, finansów, ekonomii, marketingu,
zarządzania, filologii polskiej, pedagogiki, socjologii, zarządzania zasobami ludzkimi, prawa,
administracji, hotelarstwa i turystyki.
Pisanie prac magisterskich i licencjackich
Nie kopiujemy z innych prac, nie przepisujemy, a każda z naszych prac tworzona jest bezpośrednio
pod potrzeby klienta i wykorzystywana tylko przez niego!
Prace magisterskie - dobre
Serwis "Prace magisterskie" pomoże napisać prace magisterskie i prace licencjackie zapracowanym
studentom.
Rysunek 3.3: Zdjęcie silnika HLE149.24.43.
Rysunek 3.4: Zdjęcie silnika RHE158.24.100.
znamionowe napięcie zasilania
przełożenie przekładni
maksymalny ciągły moment obrotowy
prędkość obrotowa bez obciązenia
prędkość obrotowa z max. momentem obciążenia
pobór prądu silnika w stanie jałowym
pobór prądu silnika przy max. obciążeniu
24[V ]
43.3 : 1
15[N cm]
78[obr/min]
55[obr/min]
< 50[mA]
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 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].
22
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,
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 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,
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ę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 Rp1 i Rp2 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 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
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 Z10 .
wolny
RxD
GND
wolny
wolny
1 2
3 4
5 6
7 8
9 10
TxD
wolny
wolny
wolny
wolny
Tabela 3.3: Łączówka transmisji szeregowej Z10 .
oznaczenie
TxD
RxD
GND
opis
wyjście danych nadawanych (RS232C)
wejście danych odbieranych (RS232C)
masa sygnałowa
styk
2
3
5
Tabela 3.4: Sygnały łączówki transmisji szeregowej Z10 .
Na złączu Z8 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 Z11 jest zamieszczony w raporcie [5]. Dwustykowe złącza
Z1 ,...,Z6 są wyjściami z poszczególnych stopni mocy i służą do przyłączenia silników.
27
GND
TPU10
TPU8
TPU6
TPU4
TPU2
TPU0
POM I1
POM I3
POM I5
STER I1
STER I2
STER I3
STER I4
STER I5
STER I6
wolny
1
3
5
7
9
11
13
15
17
19
21
23
25
27
29
31
33
2
Vcc
4 TPU11
6 TPU9
8 TPU7
10 TPU5
12 TPU3
14 TPU1
16 POM I2
18 POM I4
20 POM I6
22 STERI1
24 STERI2
26 STERI3
28 STERI4
30 STERI5
32 STERI6
34
wolny
Tabela 3.5: Łączówka Z8 .
28
oznaczenie
GND
Vcc
TPU10, TPU11
TPU8, TPU9
TPU6, TPU7
TPU4, TPU5
TPU2, TPU3
TPU0, TPU1
POM I1
POM I2
POM I3
POM I4
POM I5
POM I6
STER I1
STER I2
STER I3
STER I4
STER I5
STER I6
STERI1
STERI2
STERI3
STERI4
STERI5
STERI6
opis
masa sygnałowa
+5V
wyjścia kodera A/B na silniku M6
wyjścia kodera A/B na silniku M5
wyjścia kodera A/B na silniku M4
wyjścia kodera A/B na silniku M3
wyjścia kodera A/B na silniku M2
wyjścia kodera A/B na silniku M1
sygnał pomiaru prądu silnika M1 (-5V<POM I1<+5V)
sygnał pomiaru prądu silnika M2 (-5V<POM I2<+5V)
sygnał pomiaru prądu silnika M3 (-5V<POM I3<+5V)
sygnał pomiaru prądu silnika M4 (-5V<POM I4<+5V)
sygnał pomiaru prądu silnika M5 (-5V<POM I5<+5V)
sygnał pomiaru prądu silnika M6 (-5V<POM I6<+5V)
sygnał zadający prąd silnika M1 (0-2.5V)
sygnał zadający prąd silnika M2 (0-2.5V)
sygnał zadający prąd silnika M3 (0-2.5V)
sygnał zadający prąd silnika M4 (0-2.5V)
sygnał zadający prąd silnika M5 (0-2.5V)
sygnał zadający prąd silnika M6 (0-2.5V)
sygnał z C/A zadający prąd silnika M1 (0-2.5V)
sygnał z C/A zadający prąd silnika M2 (0-2.5V)
sygnał z C/A zadający prąd silnika M3 (0-2.5V)
sygnał z C/A zadający prąd silnika M4 (0-2.5V)
sygnał z C/A zadający prąd silnika M5 (0-2.5V)
sygnał z C/A zadający prąd silnika M6 (0-2.5V)
Tabela 3.6: Opis sygnałów łączówki Z8 .
INDEX2
INDEX1
INDEX4
INDEX3
INDEX6
INDEX5
TPU0
TPU2
TPU4
TPU6
TPU8
TPU10
GND
1
3
5
7
9
11
13
15
17
19
21
23
25
2
4
6
8
10
12
14
16
18
20
22
24
26
SYNCHR2
SYNCHR1
SYNCHR4
SYNCHR3
SYNCHR6
SYNCHR5
TPU1
TPU3
TPU5
TPU7
TPU9
TPU11
Vcc
Tabela 3.7: Łączówka Z9 .
29
styk
1
2
3, 4
5, 6
7, 8
9, 10
11, 12
13, 14
15
16
17
18
19
20
21
23
25
27
29
31
22
24
26
28
30
32
oznaczenie
SYNCHR1,...,SYNCHR6
INDEX1,...,INDEX6
TPU0, TPU1
TPU2, TPU3
TPU4, TPU5
TPU6, TPU7
TPU8, TPU9
TPU10, TPU11
GND
Vcc
opis
sygnały z czujników synchronizacji q1 ,..,q6
sygnały z kanałów indeks koderów D1 ,..,D6
wyjścia kodera A/B na silniku M1
wyjścia kodera A/B na silniku M2
wyjścia kodera A/B na silniku M3
wyjścia kodera A/B na silniku M4
wyjścia kodera A/B na silniku M5
wyjścia kodera A/B na silniku M6
masa sygnałowa
+5V
Tabela 3.8: Opis sygnałów łączówki Z9 .
opis styk
∼24V
1
∼24V
2
Tabela 3.9: Opis złącza zasilania modułu mocy Z7 .
opis styk
-5V
1
+5V
2
GND
3
+5V
4
Tabela 3.10: Opis złącza zasilania Z12 .
opis styk
+5V
1
+5V
2
GND
3
-5V
4
Tabela 3.11: Opis złącza na płytce zasilacz Z13 .
złącze
Z1
Z2
Z3
Z4
Z5
Z6
funkcja
wyjście dla silnika
wyjście dla silnika
wyjście dla silnika
wyjście dla silnika
wyjście dla silnika
wyjście dla silnika
M1
M2
M3
M6
M4
M5
Tabela 3.12: Opis złącz Z1 ,...Z6 .
30
styk
4,2,8,6,12,10
3,1,7,5,11,9
13, 14
15, 16
17, 18
19, 20
21, 22
23, 24
25
26
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();
qsm_init();
tpu_init();
Cur_init();
/*
/*
/*
/*
inicjalizacja modulu SIM */
inicjalizacja modulu QSM */
inicjalizacja modulu TPU */
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<<8)+PI_VECT;
PITR = PI_CONST;
/* poziom i wektor przerwania PIT*/
/* 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;
QILR = (QSPI_LEVEL<<3 )+SCI_LEVEL;
31
/* wektor przerwania */
/* poziom przerwania */
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
{
.\\
.\\
32
.\\
}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]
state.speed[1]
state.speed[2]
state.speed[3]
state.speed[4]
state.speed[5]
=
=
=
=
=
=
QDVEL2Speed((int)TPURAM(QDVEL1,2));
QDVEL2Speed((int)TPURAM(QDVEL2,2));
QDVEL2Speed((int)TPURAM(QDVEL3,2));
((TPSEC*(state.M[3]-state.oldposition[2])+2)/4);
((TPSEC*(state.M[4]-state.oldposition[0])+2)/4);
((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]
state.MCurrent[1]
state.MCurrent[2]
state.MCurrent[3]
state.MCurrent[4]
state.MCurrent[5]
=
=
=
=
=
=
ADBip2Int(QREC[1]>>4);
ADBip2Int(QREC[2]>>4);
ADBip2Int(QREC[3]>>4);
ADBip2Int(QREC[4]>>4);
ADBip2Int(QREC[5]>>4);
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 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))
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 robota Romik (w zakresie menu). Pozwala również na zbieranie wyników pomiarów parametró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 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 q1 (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
’pom2.9.txt’ using 1:2
’pom2.9.txt’ using 1:3
4000
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
36
przegubu q0 . 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
’pom1.txt’ using 1:2
’pom1.txt’ using 1:3
’pom1.txt’ using 1:4
6000
5000
4000
3000
2000
1000
0
-1000
0
10
20
30
40
50
60
Rysunek 5.2: Rozpędzanie osi q0 .
8000
’pom1.txt’ using 1:2
’pom1.txt’ using 1:3
’pom1.txt’ using 1:4
7000
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 q0 .
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.
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 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
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 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
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/
40
Rozdział 7
Dodatek A
7.1
Parametry mechaniczne manipulatora
Układ kinematyczny
Napęd
Pomiar położenia
Dane techniczne robota ZS5R
5 stopni swobody
6 silników prądu stałego
względny, kodery przyrostowo-impulsowe
o rozdzielczości D=1024
Przestrzeń robocza
Oś 1 (obrót kolumny)
Oś 2 (ramię)
Oś 3 (przedramię)
Oś 4 (zgięcie nadgarstka)
Oś 5 (obrót chwytaka)
Rodzaj chwytaka
l1
l2
l3
l4
G1 = G 2 = G 3 = G 6
G4 = G 5
Z1
Z2
Z3
Z4
Z5
Z6
Z7
Z8
Z9
±90◦
±40◦
±90◦
±60◦
±360◦
Chwytak dwupalcowy, o rozwarciu równoległym
w zakresie 0-60mm.
Stopień rozwarcia i siła uchwytu programowane.
Wymiary mechaniczne
201mm
217.5mm
134mm
90mm
Parametry przekładni
94.37:1
43.3:1
16 zębów, moduł 1
70 zębów, moduł 1
15 zębów, moduł 0.8
104 zęby moduł 0.8
20 zębów, moduł 1
68 zębów, moduł 1
16 zębów, moduł 1
24 zęby moduł 1
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ść
6
3
2
100n
1000u
2200u
1
Prostownik
1
1
1
7905
L4940V5
LM2940CT
1
TS8/220
3
MSTBA4
2
Radiator
obudowa
oznaczenie na schemacie
Kondensatory
EU025
C1, C2, C3, C4, C5, C6
EUTT5D10 C9, C10, C11
EUE5
C7, C8
Diody
RB1A
RB1A
Układy scalone
TO2220
IC3
TO2220
IC1
TO2220
IC2
Transformator
–
–
Złącza
–
Z12, Z13, Z14
inne
SK104
KK1, KK2
Tabela 8.1: Zestawienie elementów elektronicznych na płytce zasilacza.
liczba
1
1
1
wartość
obudowa oznaczenie na schemacie
Kondensatory
100n
EUC0805 C1
1u
EUC0805 C2
Układy scalone
AS5040 SSOP16
U1
Tabela 8.2: Zestawienie elementów elektronicznych na płytce kodera AS5040.
60
liczba
12
24
6
6
36
8
18
8
4
24
73
12
1
1
7
24
1
1
wartość
obudowa
oznaczenie na schemacie
Rezystory
1R
R-EU0411 R2, R105, R106, R107, R108, R109, R110,
R111, R112, R113, R114, R115
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
3k
M0805
R31, R48, R62, R76, R90, R104
5k
CA6V
R117, R118, R119, R120, R121, R122
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
18k
M0805
R21, R22, R35, R36, R49, R50, R91, R92
100k
M1206
R3, R4, R5, R6, R7, R8, R9, R10, R11,
R12, R13, R14, R15, R16, R17, R18, R19, R20
130k
M0805
R23, R24, R37, R41, R51, R52, R93, R94
200k
M0805
R23, R24, R37, R41, R51, R52, R93, R94
Kondensatory
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
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
1u
EUC0805 C56, C58, C64, C66, C78, C80, C86, C88, C96,
C98, C106, C108
4.7u
B45181A
C2
10000u/50V EUE10-30 C50
Diody
BAT42
BAT42
D25, D26, D27, D28, D29, D30, D31
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
Prostownik FBU4
B1
Tranzystory
BC640-16
TO92
T1
Tabela 8.3: Zestawienie elementów elektronicznych na płytce sterownika.
61
liczba
6
3
3
3
6
6
7
6
1
2
1
1
1
1
1
1
1
1
6
wartość
obudowa
oznaczenie na schemacie
Układy scalone
74HC123D
SO-16
IC12, IC13, IC14, IC15, IC16, IC18
74HCT00D SOIC-14
IC19, IC20, IC21
74HCT08D SO-16
IC22, IC24, IC25
74HCT132D SO-14
IC1, IC2, IC3
L298N
MULTIWATT15 IC4, IC5, IC6, IC8, IC9, IC10
LM311D
SO-8
IC26, IC27, IC28, IC29, IC30, IC31
LM385-Z
TO92
UZ1, UZ2, UZ3, UZ4, UZ5, UZ6, UZ7
MAX4164
SO-14
IC7, IC11, IC17, IC23, IC32, IC33
MAX232A
SO-16
U$2
MAX525
SSOP
US, US1
MAX1270
SSOP
U$1
MOD1
–
ELFIN-EM332/A-4X24
Przekaźnik
G2R2
G2R2
K1
Złącza
–
ML10
Z10
–
ML26
Z9
–
ML34
Z8
–
W237-02P
Z7
–
W237-04P
Z12
–
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 elektronicznej
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 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
(Mi ). 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 = qsi |i = 1, 2, 3}, co odpowiada
ustalonym pozycjom silników Ms1 , Ms2 , Ms3 . 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
q4 jest proporcjonalna do sumy zmian pozycji silników (M4 + M5 ), a zmiana kąta q5 - do
ich różnicy (M5 − M4 ).
73
10.2.2
Struktura kinematyczna dla współrzędnych
przegubowych qi.
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 Y0
układu bazowego.
Współrzędnymi wewnętrznymi są:
q1 – kąt obrotu kolumny [◦ ],
q2 – kąt obrotu głównego ramienia [◦ ],
q3 – kąt obrotu przedramienia [◦ ],
q4 – kąt odchylenia chwytaka względem przedramienia [◦ ],
q5 – kąt obrotu chwytaka wokół własnej osi [◦ ],
q6 – 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 X0 Y0 Z0 [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 qi.
Prosty model kinematyki dla robota ZS5R uzyskuje się przy użyciu notacji DenavitaHartenberga. Transformacja Denavita-Hartenberga pomiędzy lokalnymi układami współrzędnych stowarzyszonymi z poszczególnymi ogniwami robota jest następująca
A10 (q1 ) = Rot(Z, q1 )Trans(Z, l1 )Rot(X, π2 ),
A21 (q2 ) = Rot(Z, q2 )Trans(X, l2 ),
A32 (q3 ) = Rot(Z, q3 )Trans(X, l3 ),
(10.1)
A43 (q4 ) = Rot(Z, q4 )Rot(Z, − π2 )Rot(X, − π2 ),
A45 (q5 ) = Rot(Z, q5 )Trans(Z, l4 ),
gdzie symbolami li oznaczono następujące parametry geometryczne robota
l1 – wysokość kolumny [mm],
l2 – długość ramienia [mm],
l3 – długość przedramienia [mm],
l4 – 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 qi (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:
• {Mi } - przestrzeń napędowa (pozycje silników wyrażone w impulsach),
75
• {qi } - 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
(q1 , ..., q6 ) → (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 , l2 , l3 , l4 .
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 li 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 li .
Instrukcja ta została przygotowana w oparciu o instrukcję ”Identyfikacja parametrów geometrycznych robota dydaktycznego ROMIK” autorstwa I. Dulęba, A. Mazur, M. Wnuk.
76
Polecamy serwis z poradami dotyczącymi procesu pisania prac magisterskich - prace
magisrterskie - serwis z wytycznymi, wskazówkami, normami - całkowicie za darmo
Polecamy serwis darmowe _ prace mgr z bankowości - strona z darmowym i fragmentami
prac dyplomowych - prace magisterskie, licencjackie, semestralne, zaliczeniowe.
Polecamy serwis darmowe _ prace mgr z prawa - strona z darmowym i fragmentami prac
dyplomowych - prace magisterskie, licencjackie, semestralne, zaliczeniowe.
Polecamy solidny serwis darmowe prace mgr z prawa - znajdziecie tam darmowe fragmenty prac
dyplomowych - prace magisterskie, licencjackie, semestralne, zaliczeniowe.
Polecamy serwis z darmowymi referatami - darmowe prace - strona z darmowym referatami, esejami,
konspektami prac...
Polecamy serwis darmowe _ prace mgr z _ pedagogiki - strona z darmowym i fragmentami
prac dyplomowych - prace magisterskie, licencjackie, semestralne, zaliczeniowe.
Polecamy internetowy serwis darmowe _prace mgr z _zarządzania - strona z całkowicie
darmowym i fragmentami prac dyplomowych - prace licencjackie, magisterskie, semestralne i
zaliczeniowe.
Poradnik jak pisać prace dyplomowe znajdziesz na stronie abcpisania.
Forum o serwisach zajmujących się pisaniem prac licencjackich. Zamawialiście prace licencjackie,
chcecie o tym napisać? To jest właśnie odpowiednie miejsce - forum o pracach magisterskich
Mamy specjalistów z zakresu bankowości, rachunkowości, finansów, ekonomii, marketingu,
zarządzania, filologii polskiej, pedagogiki, socjologii, zarządzania zasobami ludzkimi, prawa,
administracji, hotelarstwa i turystyki.
Pisanie prac magisterskich i licencjackich
Nie kopiujemy z innych prac, nie przepisujemy, a każda z naszych prac tworzona jest bezpośrednio
pod potrzeby klienta i wykorzystywana tylko przez niego!
Prace magisterskie - dobre
Serwis "Prace magisterskie" pomoże napisać prace magisterskie i prace licencjackie zapracowanym
studentom.