Manipulator przeznaczony do celów dydaktycznych - Laboratorium ...
Manipulator przeznaczony do celów dydaktycznych - Laboratorium ...
Manipulator przeznaczony do celów dydaktycznych - Laboratorium ...
Create successful ePaper yourself
Turn your PDF publications into a flip-book with our unique Google optimized e-Paper software.
Politechnika Wrocławska<br />
Wydział Elektroniki<br />
Kierunek:<br />
Specjalność:<br />
Automatyka i Robotyka (AiR)<br />
Robotyka (ARR)<br />
PRACA DYPLOMOWA<br />
MAGISTERSKA<br />
<strong>Manipulator</strong><br />
<strong>przeznaczony</strong> <strong>do</strong> celów<br />
<strong>dydaktycznych</strong>.<br />
Autor:<br />
Zbigniew Struzik<br />
Prowadzący pracę:<br />
dr inż. Marek Wnuk, I-6<br />
Ocena pracy:<br />
Wrocław 2005
Składam serdeczne podziękowania Panu<br />
dr. Markowi Wnukowi za pomoc,<br />
poświęcony czas oraz zaangażowanie.<br />
Chciałem również podziękować Panu<br />
Ryszar<strong>do</strong>wi Krzywańskiemu za<br />
cenne rady i pomoc w wykonaniu konstrukcji<br />
mechanicznej. Na końcu chciałem<br />
podziękować firmom MICRO-<br />
MOTORS i AUSTRIAMICRO-<br />
SYSTEMS za bezpłatne przekazanie<br />
silników i koderów.
1<br />
Spis treści<br />
1 Wstęp 6<br />
1.1 Cel i zakres pracy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6<br />
2 Kinematyka manipulatora 7<br />
2.1 Reprezentacja Denavita-Hartenberga . . . . . . . . . . . . . . . . . . . . . 7<br />
2.2 Wyprowadzenie kinematyki prostej<br />
manipulatora . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8<br />
2.3 Kinematyka odwrotna manipulatora – podejście geometryczne . . . . . . . 10<br />
2.4 Konfiguracje wyróżnione manipulatora . . . . . . . . . . . . . . . . . . . . 13<br />
2.5 Transformacja napę<strong>do</strong>wa osi robota . . . . . . . . . . . . . . . . . . . . . . 14<br />
2.5.1 Rozwiązanie napędów ogniwa 1,2 i 3 . . . . . . . . . . . . . . . . . 14<br />
2.5.2 Rozwiązanie napędu ogniwa 4 i 5 . . . . . . . . . . . . . . . . . . . 16<br />
3 Konstrukcja robota ZS5R 18<br />
3.1 Konstrukcja mechaniczna . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18<br />
3.1.1 Silniki manipulatora . . . . . . . . . . . . . . . . . . . . . . . . . . 20<br />
3.2 Konstrukcja elektroniczna – sterownik robota . . . . . . . . . . . . . . . . 23<br />
3.2.1 Bu<strong>do</strong>wa modułu sterowania . . . . . . . . . . . . . . . . . . . . . . 24<br />
3.2.2 Bu<strong>do</strong>wa modułu mocy . . . . . . . . . . . . . . . . . . . . . . . . . 25<br />
3.2.3 Opis łączówek sterownika . . . . . . . . . . . . . . . . . . . . . . . 27<br />
4 Oprogramowanie sterownika 31<br />
4.1 Funkcja główna main() . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31<br />
4.2 Procedura obsługi przerwania cyklicznego . . . . . . . . . . . . . . . . . . 32<br />
5 Badania i wyniki eksperymentów 36<br />
6 Podsumowanie 38<br />
7 Dodatek A 41<br />
7.1 Parametry mechaniczne manipulatora . . . . . . . . . . . . . . . . . . . . . 41<br />
7.2 Rysunki techniczne części mechanicznej . . . . . . . . . . . . . . . . . . . . 42<br />
8 Dodatek B 60<br />
8.1 Wykaz elementów części elektronicznej . . . . . . . . . . . . . . . . . . . . 60<br />
9 Dodatek C 63<br />
9.1 Schematy ideowe i rysunki montażowe części elektronicznej . . . . . . . . . 63
10 Przykła<strong>do</strong>wa instrukcja laboratoryjna 73<br />
10.1 Cel ćwiczenia. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73<br />
10.2 Podstawy teoretyczne. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 73<br />
10.2.1 Charakterystyka robota ZS5R. . . . . . . . . . . . . . . . . . . . . . 73<br />
10.2.2 Struktura kinematyczna dla współrzędnych<br />
przegubowych q i . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74<br />
10.2.3 Transformacja Denavita-Hartenberga dla zmiennych q i . . . . . . . . 75<br />
10.3 Przebieg ćwiczenia . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75<br />
10.3.1 Obsługa stanowiska . . . . . . . . . . . . . . . . . . . . . . . . . . . 75<br />
10.3.2 Wstępne przygotowanie <strong>do</strong> ćwiczenia. . . . . . . . . . . . . . . . . . 76<br />
10.3.3 Zadania <strong>do</strong> wykonania. . . . . . . . . . . . . . . . . . . . . . . . . . 76<br />
2
3<br />
Spis rysunków<br />
2.1 Struktura kinematyczna manipulator ZS5R. . . . . . . . . . . . . . . . . . 8<br />
2.2 Struktura szkieletowa robota. . . . . . . . . . . . . . . . . . . . . . . . . . 11<br />
2.3 Struktura układu przeniesienia napędu ogniwa 1 i 2. . . . . . . . . . . . . . 14<br />
2.4 Struktura układu przeniesienia napędu ogniwa 3. . . . . . . . . . . . . . . 16<br />
2.5 Struktura układu różnicowego napędzającego osie nadgarstka. . . . . . . . 17<br />
3.1 Zdjęcie manipulator. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 18<br />
3.2 Rysunek złożeniowy manipulatora z oznaczonymi głównymi częściami. . . . 19<br />
3.3 Zdjęcie silnika HLE149.24.43. . . . . . . . . . . . . . . . . . . . . . . . . . 21<br />
3.4 Zdjęcie silnika RHE158.24.100. . . . . . . . . . . . . . . . . . . . . . . . . . 21<br />
3.5 Zdjęcie ilustrujące sposób zamontowania magnesu na osi silnika. . . . . . . 22<br />
3.6 Zdjęcie ilustrujące sposób zamontowania kodera AS5040. . . . . . . . . . . 22<br />
3.7 Schemat blokowy układu sterowania manipulatora. . . . . . . . . . . . . . 23<br />
3.8 Płytka sterownika . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24<br />
3.9 Moduł EM332/B. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25<br />
3.10 Schemat blokowy regulatora prądu. . . . . . . . . . . . . . . . . . . . . . . 26<br />
3.11 Zasilacz części cyfrowej i analogowej . . . . . . . . . . . . . . . . . . . . . . 26<br />
4.1 Schemat blokowy układu sterowania. . . . . . . . . . . . . . . . . . . . . . 34<br />
5.1 Wykres odpowiedzi regulatora na zadany prąd. . . . . . . . . . . . . . . . 36<br />
5.2 Rozpędzanie osi q 0 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37<br />
5.3 Regulacja prędkości osi q 0 . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37<br />
6.1 Wi<strong>do</strong>k skonstruowanego manipulatora. . . . . . . . . . . . . . . . . . . . . 38<br />
7.1 Rysunek techniczny silnika RHE 158.24.100. . . . . . . . . . . . . . . . . . 42<br />
7.2 Rysunek techniczny silnika HLE 149.24.43. . . . . . . . . . . . . . . . . . . 42<br />
9.1 Schemat ideowy jednego z regulatorów prądu modułu mocy. . . . . . . . . 63<br />
9.2 Schemat ideowy połączenia modułu EM332 z układami peryferyjnymi. . . 64<br />
9.3 Schemat ideowy zasilacza. . . . . . . . . . . . . . . . . . . . . . . . . . . . 65<br />
9.4 Schemat ideowy układu sterowania cewką przekaźnika. . . . . . . . . . . . 65<br />
9.5 Schemat elektryczny kodera AS5040. . . . . . . . . . . . . . . . . . . . . . 66<br />
9.6 Obwód drukowany wierzchniej warstwy płytki zasilacza. . . . . . . . . . . . 66<br />
9.7 Obwód drukowany spodniej warstwy płytki zasilacza. . . . . . . . . . . . . 66<br />
9.8 Rozmieszczenie elementów na wierzchniej stronie płytki zasilacza. . . . . . 67<br />
9.9 Obwód drukowany wierzchniej warstwy płytki sterownika. . . . . . . . . . 68<br />
9.10 Obwód drukowany spodniej warstwy płytki sterownika. . . . . . . . . . . . 69<br />
9.11 Rozmieszczenie elementów na wierzchniej stronie płytki sterownika. . . . . 70
4<br />
9.12 Rozmieszczenie elementów na spodniej stronie płytki sterownika. . . . . . . 71<br />
9.13 Obwód drukowany wierzchniej warstwy płytki kodera AS5040. . . . . . . . 72<br />
9.14 Obwód drukowany spodnie warstwy płytki kodera AS5040. . . . . . . . . . 72<br />
9.15 Rozmieszczenie elementów na wierzchniej warstwie płytki kodera AS5040. . 72<br />
9.16 Rozmieszczenie elementów na spodniej warstwie płytki kodera AS5040. . . 72<br />
10.1 Struktura kinematyczna manipulator ZS5R. . . . . . . . . . . . . . . . . . 74
5<br />
Spis tabel<br />
2.1 Parametry Denavita-Hartenberga manipulatora. . . . . . . . . . . . . . . . 8<br />
3.1 Parametry silnika typu RHE158.24.100. . . . . . . . . . . . . . . . . . . . . 20<br />
3.2 Parametry silnika typu HLE149.24.43. . . . . . . . . . . . . . . . . . . . . 21<br />
3.3 Łączówka transmisji szeregowej Z 10 . . . . . . . . . . . . . . . . . . . . . . . 27<br />
3.4 Sygnały łączówki transmisji szeregowej Z 10 . . . . . . . . . . . . . . . . . . 27<br />
3.5 Łączówka Z 8 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28<br />
3.6 Opis sygnałów łączówki Z 8 . . . . . . . . . . . . . . . . . . . . . . . . . . . 29<br />
3.7 Łączówka Z 9 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29<br />
3.8 Opis sygnałów łączówki Z 9 . . . . . . . . . . . . . . . . . . . . . . . . . . . 30<br />
3.9 Opis złącza zasilania modułu mocy Z 7 . . . . . . . . . . . . . . . . . . . . . 30<br />
3.10 Opis złącza zasilania Z 12 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30<br />
3.11 Opis złącza na płytce zasilacz Z 13 . . . . . . . . . . . . . . . . . . . . . . . . 30<br />
3.12 Opis złącz Z 1 ,...Z 6 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30<br />
8.1 Zestawienie elementów elektronicznych na płytce zasilacza. . . . . . . . . . 60<br />
8.2 Zestawienie elementów elektronicznych na płytce kodera AS5040. . . . . . 60<br />
8.3 Zestawienie elementów elektronicznych na płytce sterownika. . . . . . . . . 61<br />
8.4 Zestawienie elementów elektronicznych na płytce sterownika. . . . . . . . . 62
6<br />
Rozdział 1<br />
Wstęp<br />
W laboratorium robotyki Zakładu Podstaw Cybernetyki i Robotyki, znajduje się manipulator<br />
dydaktyczny Romik. Robot ten jest klasy 5R, posiadający pięć stopni swobody. Do<br />
napędu jego poszczególnych osi zastosowano silniki skokowe. Konstrukcja mechaniczna i<br />
układ sterowania robota, pozwala jedynie na wykonywanie na nim podstawowych ćwiczeń<br />
związanych z identyfikacją parametrów geometrycznych manipulatora. Natomiast ze<br />
względu na zastosowane <strong>do</strong> napędu osi silniki, nie pozwala on już na implementację algorytmów<br />
sterowania uwzględniających dynamikę robota. Poza tym, konstrukcja robota<br />
jest <strong>do</strong>ść przestarzała i wyeksploatowana mechanicznie. Z tego względu powstała potrzeba<br />
zbu<strong>do</strong>wania nowego manipulatora dydaktycznego, w którym napęd przegubów byłby zrealizowany<br />
za pomocą silników prądu stałego. Powinien on dawać możliwość wykonywania<br />
<strong>do</strong>tychczasowych ćwiczeń jak i umożliwić w przyszłości implementację algorytmów sterowania<br />
opartych na dynamice robota. Także elektronika nowego robota powinien dawać<br />
możliwość podłączenia zewnętrznego układu sterowania, na przykład karty współpracującej<br />
z pakietem MATLAB.<br />
1.1 Cel i zakres pracy<br />
Podstawowym celem pracy jest stworzenie stanowiska laboratoryjnego, którego głównym<br />
elementem jest manipulator dydaktyczny. Stworzenie stanowiska realizowane jest przez:<br />
• wykonanie od podstaw części mechanicznej manipulatora,<br />
• zaprojektowanie i wykonanie elektroniki robota, pozwalającej współpracować z zewnętrznym<br />
układem sterowania,<br />
• napisanie oprogramowania,<br />
• przeprowadzenie badań,<br />
• opracowanie przykła<strong>do</strong>wej instrukcji laboratoryjnej.<br />
W pracy tej zawarto teoretyczne podstawy <strong>do</strong>tyczące kinematyki prostej i odwrotnej<br />
manipulatora jak i przedstawiono transformację napę<strong>do</strong>wą. Zamieszczono opis konstrukcji<br />
części mechanicznej i elektronicznej robota (<strong>do</strong>kładną <strong>do</strong>kumentację techniczną robota<br />
można znaleźć w <strong>do</strong>datkach A, B i C). Opisano też oprogramowanie służące <strong>do</strong> obsługi<br />
układów wykonawczych. U<strong>do</strong>kumentowano wykresy i wyniki eksperymentów uzyskanych<br />
za pomocą wewnętrznych czujników manipulatora.
7<br />
Rozdział 2<br />
Kinematyka manipulatora<br />
2.1 Reprezentacja Denavita-Hartenberga<br />
Wyznaczenie kinematyki według notacji Denavita-Hartenberga [4] polega na związaniu<br />
z każdym ramieniem (ogniwem) lokalnego układu współrzędnych umieszczonego w odpowiednim<br />
przegubie, a następnie wyznaczeniu ciągu transformacji pomiędzy kolejnymi<br />
układami. W podstawie zaczepiamy nieruchomy układ oznaczony numerem 0. Dalej wybieramy<br />
układy od 1 <strong>do</strong> n tak, że układ i jest na sztywno związany z ogniwem i w taki<br />
sposób, że<br />
• oś Z i jest osią (i + 1)-go złącza, a zwrot tej osi może być przyjęty <strong>do</strong>wolnie;<br />
• oś X i jest wspólną normalną <strong>do</strong> osi złączy i-tego i (i + 1)-go i skierowana jest w<br />
stronę ogniw o wyższych numerach;<br />
• oś Y i jest uzupełnieniem dwóch poprzednich osi <strong>do</strong> prawoskrętnego, kartezjańskiego<br />
układu współrzędnych. Ponieważ jest ona jednoznacznie określona przez położenia<br />
osi Z i oraz X i zatem niektórzy pomijają ją aby zwiększyć czytelność rysunków.<br />
Transformacja A i i−1 pomiędzy układami (i − 1) oraz i jest zdefiniowana jako iloczyn<br />
czterech macierzy elementarnych obrotów i przesunięć.<br />
A i i−1 (q i) = Rot(Z, θ i )Trans(Z, d i )Trans(X, a i )Rot(X, α i ) (2.1)<br />
gdzie:<br />
θ i - kąt obrotu wokół osi Z i−1 ,<br />
d i - translacja wzdłuż bieżącej osi Z,<br />
a i - translacja wzdłuż bieżącej osi X,<br />
α i - kąt obrotu wokół bieżącej osi X,<br />
są parametrami Denavita-Hartenberga. Kinematyka manipulatora wyznacza pozycję i<br />
orientację układu efektora w bazowym układzie współrzędnych, i jest opisana złożeniem<br />
transformacji (2.1) czyli<br />
n∏<br />
K(q) = A i i−1(q i ) =<br />
i=1<br />
[<br />
R<br />
n<br />
0 (q) T n 0(q)<br />
0 1<br />
]<br />
. (2.2)
8<br />
2.2 Wyprowadzenie kinematyki prostej<br />
manipulatora<br />
Rozważany manipulator jest manipulatorem klasy 5R 1 , posiadającym pięć stopni swobody.<br />
<strong>Manipulator</strong> ten składa się z pionowej, obrotowej kolumny o wysokości l 1 , dwuczłonowego<br />
ramienia o długości l 2 i l 3 oraz nadgarstka z efektorem o długości l 4 , ujmującej długość<br />
palców chwytaka. Wszystkie te ogniwa - ramiona są połączone przegubami, których<br />
aktualny stan określa wektor stanu q = (q 1 , q 2 , q 3 , q 4 , q 5 ) T . Strukturę tego manipulatora<br />
wraz z układami współrzędnych pokazano na rysunku 2.1. Układy współrzędnych zostały<br />
Rysunek 2.1: Struktura kinematyczna manipulator ZS5R.<br />
przyporządkowane zgodnie z algorytmem Denavita-Hartenberga. Następnym krokiem jest<br />
wyznaczenie parametrów i zmiennych wiążących te układy. Zestawiono je w tabeli 2.1.<br />
Nr ogniwa θ i d i a i α i<br />
π<br />
1 q 1 l 1 0<br />
2<br />
2 q 2 0 l 2 0<br />
3 q 3 0 l 3 0<br />
4 q 4 − π 0 0 − π 2 2<br />
5 q 5 l 4 0 0<br />
Tabela 2.1: Parametry Denavita-Hartenberga manipulatora.<br />
1 Oznacza to, że manipulator posiada pięć przegubów typu rotacyjnego (R).
9<br />
W celu wyliczenia kinematyki manipulatora wyznaczamy transformacje między kolejnymi<br />
układami współrzędnych zgodnie z (2.1).<br />
A 1 0 (q 1) = Rot(Z, q 1 )Trans(Z, l 1 )Rot(X, π),<br />
2<br />
A 2 1 (q 2) = Rot(Z, q 2 )Trans(X, l 2 ),<br />
A 3 2 (q 3) = Rot(Z, q 3 )Trans(X, l 3 ),<br />
A 4 3 (q 4) = Rot(Z, q 4 )Rot(Z, − π)Rot(X, − π),<br />
2 2<br />
A 4 5(q 5 ) = Rot(Z, q 5 )Trans(Z, l 4 ),<br />
(2.3)<br />
Po wprowadzeniu oznaczeń sinq i = s i , cosq i = c i , kolejne macierze przyjmują postać:<br />
• przejście od układu 0 <strong>do</strong> 1:<br />
• przejście od układu 1 <strong>do</strong> 2:<br />
⎡<br />
A 1 0(q 1 ) = ⎢<br />
⎣<br />
c 1 0 s 1 0<br />
s 1 0 −c 1 0<br />
0 1 0 l 1<br />
0 0 0 1<br />
⎤<br />
⎥<br />
⎦ ,<br />
• przejście od układu 2 <strong>do</strong> 3:<br />
⎡<br />
A 2 1 (q 2) = ⎢<br />
⎣<br />
c 2 −s 2 0 l 2 c 2<br />
s 2 c 2 0 l 2 s 2<br />
0 0 1 0<br />
0 0 0 1<br />
⎤<br />
⎥<br />
⎦ ,<br />
• przejście od układu 3 <strong>do</strong> 4:<br />
⎡<br />
A 3 2 (q 3) = ⎢<br />
⎣<br />
c 3 −s 3 0 l 3 c 3<br />
s 3 c 3 0 l 3 s 3<br />
0 0 1 0<br />
0 0 0 1<br />
⎤<br />
⎥<br />
⎦ ,<br />
• przejście od układu 4 <strong>do</strong> 5:<br />
⎡<br />
A 4 3(q 4 ) = ⎢<br />
⎣<br />
s 4 0 c 4 0<br />
−c 4 0 s 4 0<br />
0 1 0 0<br />
0 0 0 1<br />
⎤<br />
⎥<br />
⎦ ,
10<br />
⎡<br />
A 5 4 (q 5) = ⎢<br />
⎣<br />
c 5 −s 5 0 0<br />
s 5 c 5 0 0<br />
0 0 1 l 4<br />
0 0 0 1<br />
Mając wyliczone transformacje A i i−1 (q i) między sąsiednimi układami współrzędnych definiujemy<br />
kinematykę manipulatora zgodnie z (2.2) jako<br />
⎤<br />
⎥<br />
⎦ .<br />
czyli<br />
K(q) = A 1 0 A2 1 A3 2 A4 3 A5 4 (q): X 0Y 0 Z 0 ↦−→ X 5 Y 5 Z 5<br />
⎡<br />
K(q) = ⎢<br />
⎣<br />
r 11 r 12 r 13 l x<br />
r 21 r 22 r 23 l y<br />
r 31 r 32 r 33 l z<br />
0 0 0 1<br />
⎤<br />
⎥<br />
⎦ , (2.4)<br />
gdzie elementy tej macierzy są następujące:<br />
⎫<br />
r 11 = cosq 1 cosq 5 sin(q 2 + q 3 + q 4 ) − sinq 1 sinq 5 ,<br />
r 12 = −cosq 1 sinq 5 sin(q 2 + q 3 + q 4 ) − sinq 1 cosq 5 ,<br />
r 13 = cosq 1 cos(q 2 + q 3 + q 4 ),<br />
r 21 = sinq 1 cosq 5 sin(q 2 + q 3 + q 4 ) + cosq 1 sinq 5 ,<br />
r 22 = −sinq 1 sinq 5 sin(q 2 + q 3 + q 4 ) + cosq 1 cosq 5 ,<br />
r 23 = sinq 1 cos(q 2 + q 3 + q 4 ),<br />
⎪⎬<br />
r 31 = −cos(q 2 + q 3 + q 4 )cosq 5 ,<br />
r 32 = cos(q 2 + q 3 + q 4 )sinq 5 ,<br />
r 33 = sin(q 2 + q 3 + q 4 ),<br />
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 ),<br />
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 ),<br />
l z = l 1 + l 2 sinq 2 + l 3 sin(q 2 + q 3 ).<br />
⎪⎭<br />
(2.5)<br />
2.3 Kinematyka odwrotna manipulatora – podejście<br />
geometryczne<br />
W poprzednim punkcie została wyznaczona kinematyka prosta manipulatora, która wyznacza<br />
położenie i orientację efektora w zależności od konfiguracji jego przegubów. W<br />
tym punkcie zostanie przedstawione zadanie odwrotne, a mianowicie wyliczenie wartości<br />
kolejnych zmiennych przegubowych przy narzuconym położeniu i orientacji efektora. Zadanie<br />
to jest nazywane odwrotnym zadaniem kinematyki. Rozwiązanie zadania kinematyki<br />
odwrotnej jest niezwykle istotne ze względu na efektywność sterowania manipulatorem<br />
robota. Zadania <strong>do</strong> wykonania przez robota są bowiem w sposób naturalny formułowane<br />
w kategoriach pożądanych położeń i orientacji efektora, wyrażonych w układzie kartezjańskim<br />
związanym z podstawą robota.
11<br />
Przy podejściu geometrycznym [1] [2] <strong>do</strong> rozwiązania zagadnienia kinematycznego konieczne<br />
jest zdekomponowanie przestrzennego zagadnienia geometrycznego na szereg zagadnień<br />
planarnych. Przy tej metodzie zmienne przegubowe mogą być wyznaczone za<br />
pomocą metod stosowanych w płaskiej geometrii. Na rysunku 2.2 przedstawiony jest<br />
szkieletowy model robota wraz z odpowiednio oznaczonymi kątami. Danymi wejściowymi<br />
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<br />
punktu pracy chwytaka względem bazowego układu odniesienia, kąt η określa kąt nutacji<br />
efektora, zaś kąt ϱ jest równy kątowi q 5 , który odpowiedzialny jest za rotację wokół osi<br />
Z 5 .<br />
Rysunek 2.2: Struktura szkieletowa robota.<br />
W celu wyznaczenia kąta q 1 , rzutujemy punkt pracy chwytaka P na płaszczyznę wyzna-
12<br />
czoną przez osie X 0 , Y 0 , stąd widzimy, że<br />
czyli<br />
tan q 1 = P y<br />
P x<br />
, (2.6)<br />
q 1 = Atan2(P y , P x ), (2.7)<br />
gdzie Atan2(·, ·) oznacza dwuargumentową funkcję arcus tangens.<br />
W celu ułatwienia obliczenia kolejnych zmiennych przegubowych wyznaczony został punkt<br />
pomocniczy P c o współrzędnych<br />
⎡ ⎤ ⎡<br />
⎤<br />
P cx P x − k cos q 1<br />
⎢ ⎥ ⎢<br />
⎥<br />
P c = ⎣ P cy ⎦ = ⎣ P y − k sin q 1 ⎦ , gdzie k = l 4 cos η. (2.8)<br />
P cz P z − l 4 sin η<br />
Aby wyznaczyć kąt q 3 wykorzystuje się twierdzenie cosinusów<br />
d 2 = l 2 2 + l 2 3 − 2l 2 l 3 cos (π − q 3 ). (2.9)<br />
Ponieważ zachodzi zależność cos (π − q 3 ) = − cos q 3 , oraz dla <strong>do</strong>wolnej konfiguracji manipulatora<br />
jest spełniony związek d 2 = r 2 +(P cz −l 1 ) 2 , gdzie r 2 = Pcx 2 +P cy 2 , zatem otrzymuje<br />
się dalej<br />
Pcx 2 + P cy 2 + (P cz − l 1 ) 2 = l2 2 + l2 3 + 2l 2l 3 cos q 3 , (2.10)<br />
i stąd<br />
cos q 3 = P 2 cx + P 2 cy + (P cz − l 1 ) 2 − l 2 2 − l2 3<br />
2l 2 l 3<br />
. = D. (2.11)<br />
Istnieją dwa rozwiązania powyższego równania, a mianowicie<br />
oraz<br />
q 3 = arccos P 2 cx + P 2 cy + (P cz − l 1 ) 2 − l 2 2 − l2 3<br />
2l 2 l 3<br />
, stąd 0 q 3 π, (2.12)<br />
q 3 = − arccos P 2 cx + P 2 cy + (P cz − l 1 ) 2 − l 2 2 − l 2 3<br />
2l 2 l 3<br />
, stąd − π q 3 0. (2.13)<br />
Jednakże lepszym sposobem znalezienia kąta q 3 jest zauważenie, że jeśli cos q 3 jest dany<br />
wzorem (2.11), to sin q 3 jest dany odpowiednio wzorem<br />
i stąd można wyznaczyć<br />
sin q 3 = ± √ 1 − D 2 , (2.14)<br />
q 3 = Atan2(± √ 1 − D 2 , D). (2.15)<br />
Zaletą tego drugiego sposobu jest rozróżnienie obu konfiguracji ”łokieć u góry” i ”łokieć<br />
u <strong>do</strong>łu” przez wybór odpowiedniego znaku w równaniu (2.15).<br />
W celu wyliczenia kąta q 2 wyznacza się wyrażenia na kąty pomocnicz δ oraz α zaznaczone<br />
na rysunku 2.2. Dla kąta δ zachodzi<br />
tan δ = P cz − l 1<br />
r<br />
= P cz − l 1<br />
√ , (2.16)<br />
P<br />
2<br />
cx − Pcy<br />
2
13<br />
czyli<br />
Po<strong>do</strong>bnie wyznaczamy kat α<br />
czyli<br />
δ = Atan2(P cz − l 1 , √ Pcx 2 − P cy 2 ). (2.17)<br />
tan α = l 3sinq 3<br />
l 2 + l 3 cosq 3<br />
, (2.18)<br />
α = Atan2(l 3 sinq 3 , l 2 + l 3 cosq 3 ). (2.19)<br />
Z rysunku otrzymuje się prostą zależność między kątami q 2 , δ, α, a mianowicie q 2 = δ − α,<br />
z której wynika,że<br />
q 2 = Atan2(P cz − l 1 , √ P 2 cx − P 2 cy ) − Atan2(l 3sinq 3 , l 2 + l 3 cosq 3 ). (2.20)<br />
Wartość kąta q 2 zależy od kąta q 3 . Ma to znaczenie fizyczne, ponieważ otrzymamy<br />
różne wartości kąta q 2 w zależności od tego, które rozwiązanie wybierzemy dla kąta q 3 .<br />
Z kolei kąt q 4 wyznaczamy z następującego związku η = q 2 + q 3 + q 4 czyli<br />
q 4 = η − q 2 − q 3 . (2.21)<br />
Reasumując dla P (P x , P y , P z , η, ϱ) otrzymujemy następujące zależności:<br />
⎫<br />
q 1 = Atan2(P y , P x ),<br />
q 2 = Atan2(P cz − l 1 , √ Pcx 2 − P cy 2 ) − Atan2(l 3sinq 3 , l 2 + l 3 cosq 3 ),<br />
q 3 = Atan2(± √ ⎪⎬<br />
1 − D 2 , D),<br />
q 4 = η − q 2 − q 3 ,<br />
q 5 = ϱ.<br />
⎪⎭<br />
(2.22)<br />
gdzie<br />
P cx = P x − k cos q 1 , P cy = P y − k sin q 1 , P cz = P z − l 4 sin η, D = P cx+P 2 cy+(P 2 cz−l 1 ) 2 −l2 2−l2 3<br />
2l 2 l 3<br />
,<br />
k = l 4 cos η.<br />
2.4 Konfiguracje wyróżnione manipulatora<br />
W celu mechanicznego sprawdzenia poprawności pozycjonowania manipulatora wyróżniono<br />
konfigurcję zerową manipulatora zwaną również konfiguracją geometryczną q 0 =<br />
(q 01 , q 02 , q 03 , q 04 , q 05 ), dla której zmienne przegubowe przyjmują następujace wartości:<br />
q 01 = 0 [ ◦ ],<br />
q 02 = 0 [ ◦ ],<br />
q 03 = 0 [ ◦ ],<br />
q 04 = 0 [ ◦ ],<br />
q 05 = 0 [ ◦ ].<br />
(2.23)
14<br />
Ogniwa 1,2,3,4,5 są ustawione w pozycji swojego środkowego zakresu pracy.<br />
Drugą wyróżnioną konfiguracją jest konfiguracja synchronizacji. Jej osiągnięcie jest<br />
sygnalizowane przez czujniki zamontowane w poszczególnych przegubach i kanały indeksujące<br />
koderów, odczytywane przez układ sterowania robota. Dla ogniw 1,2,4,5 czujniki<br />
zostały zamontowane tak, aby zachować zgodność z konfiguracją zerową. W przypadku<br />
ogniwa 3 położenie czujnika jest różne od położenia zerowego. Dokładne wartości położeń<br />
napędów ogniw w konfiguracji synchronizacji podano w <strong>do</strong>datku A.<br />
2.5 Transformacja napę<strong>do</strong>wa osi robota<br />
Sterownik robota zarówno mierzy, jak i zadaje pozycje i prędkości poszczególnych osi w<br />
tzw. przestrzeni napę<strong>do</strong>wej, której współrzędne opisują położenia wałów poszczególnych<br />
silników. Istotne jest więc podanie sposobu przeniesienia napędu dla poszczególnych osi<br />
jak i równań wiążących położenia wałów silników M i ze zmiennymi przegubowymi q i .<br />
Do napędu poszczególnych osi robota jak i <strong>do</strong> napędu mechanizmu zamykającego chwytak<br />
zastosowano silniki elektryczne prądu stałego z magnesem trwałym. Pomiar położenia<br />
wałów silników jest realizowany przy pomocy koderów przyrostowych o rozdzielczości D<br />
= 1024 impulsów na jeden obrót.<br />
Wszystkie silniki napę<strong>do</strong>we, są wyposażone w przekładnie wielostopniowe o przełożeniach<br />
G 1 = G 2 = G 3 = G 6 = 94.37 i G 4 = G 5 = 43.3.<br />
2.5.1 Rozwiązanie napędów ogniwa 1,2 i 3<br />
Napęd ogniwa 1 (obrót kolumny) jest przenoszony przez przekładnię wielostopniową o<br />
przełożeniu G 1 a następnie przez przekładnię jednostopniową o przełożeniu Z 1<br />
Z 2<br />
, gdzie Z 1<br />
i Z 2 oznacza ilość zębów dla poszczególnych kół przekładni jednostopniowej. Na rysunku<br />
2.3 jest pokazany sposób przeniesienia napędu dla ogniwa 1 i 2. Równanie opisujące<br />
przeniesienie napędu jest następujące<br />
Rysunek 2.3: Struktura układu przeniesienia napędu ogniwa 1 i 2.
15<br />
q 1 − q 01 =<br />
Z 12π<br />
Z 2 DG 1<br />
(M 1 − M 01 ), (2.24)<br />
gdzie q 0i jest wartością zmiennej przegubowej q i w konfiguracji zerowej, a M 0i jest wartością<br />
zmiennej napę<strong>do</strong>wej M i w konfiguracji zerowej.<br />
Transformacja prosta napędu<br />
c 1 = cos q 1<br />
(2.25)<br />
s 1 = sin q 1 ,<br />
ostatecznie ma postać<br />
a transformacja odwrotna<br />
czyli<br />
c 1 = cos (q 01 +<br />
s 1 = sin (q 01 +<br />
Z 12π<br />
Z 2 DG 1<br />
(M 1 − M 01 )), (2.26)<br />
Z 12π<br />
Z 2 DG 1<br />
(M 1 − M 01 )), (2.27)<br />
q 1 = Atan2(s 1 , c 1 ), (2.28)<br />
M 1 = M 01 + Z 2DG 1<br />
Z 1 2π (Atan2(s 1, c 1 ) − q 01 ). (2.29)<br />
W przypadku ogniwa 2 (główne ramię) sposób przeniesienia napędu jest analogiczny jak<br />
w przypadku ogniwa 1 rysunek 2.3. A mianowicie, dla ogniwa drugiego, równanie napędu<br />
wyraża się wzorem<br />
q 2 − q 02 =<br />
Z 32π<br />
(M 2 − M 02 ), (2.30)<br />
Z 4 DG 2<br />
Transformacja prosta napędu ma postać<br />
a transformacja odwrotna<br />
c 2 = cos (q 02 +<br />
s 2 = sin (q 02 +<br />
Z 32π<br />
Z 4 DG 2<br />
(M 2 − M 02 )), (2.31)<br />
Z 32π<br />
Z 4 DG 2<br />
(M 2 − M 02 )), (2.32)<br />
q 2 = Atan2(s 2 , c 2 ). (2.33)<br />
Z kolei dla ogniwa trzeciego sposób przeniesienia napędu różni się od sposobu w ogniwie 1<br />
i 2 jedynie tym, że czynne koło zębate Z 5 przekładni jednostopniowej nie jest zamocowane<br />
na osi przekładni G 3 a jest napędzane poprzez przekładnie pasową zębatą o przełożeniu<br />
równym jeden, co widać na rysunku 2.4. Równanie napędu ogniwa 3 wyraża się wzorem<br />
q 3 − q 03 =<br />
Transformacja prosta napędu ma postać<br />
a transformacja odwrotna<br />
c 3 = cos (q 03 +<br />
s 3 = sin (q 03 +<br />
Z 52π<br />
Z 6 DG 3<br />
(M 3 − M 03 ), (2.34)<br />
Z 52π<br />
Z 6 DG 3<br />
(M 3 − M 03 )), (2.35)<br />
Z 52π<br />
Z 6 DG 3<br />
(M 3 − M 03 )), (2.36)<br />
q 3 = Atan2(s 3 , c 3 ). (2.37)
16<br />
Rysunek 2.4: Struktura układu przeniesienia napędu ogniwa 3.<br />
2.5.2 Rozwiązanie napędu ogniwa 4 i 5<br />
Ruch w ogniwie 4 (pochylenie nadgarstka) i ogniwie 5 (obrót chwytaka) zapewniają dwa<br />
silniki elektryczne o przełożeniu G 4 = G 5 . Silniki te pracują w układzie różnicowym co<br />
widać na rysunku 2.5. Mechanizm różnicowy jest zbu<strong>do</strong>wany z pięciu stożkowych kół<br />
zębatych, z których dwa koła o ilości zębów Z 7 są osadzone na osiach silników, kolejne<br />
dwa koła o ilości zębów Z 8 wykonują ruch obrotowy w osi kąta q 4 , natomiast ostatnie<br />
koło zębate o liczbie zębów Z 9 obraca się w osi kąta q 5 . Równania przeniesienia napędu<br />
dla ogniwa 4 wyrażają się następująco<br />
q 4 − q 04 =<br />
Transformacja prosta napędu<br />
c 4 = cos (q 04 +<br />
s 4 = sin (q 04 +<br />
a transformacja odwrotna<br />
Z 72π<br />
2Z 8 DG 4<br />
((M 5 − M 05 ) − (M 4 − M 04 )), (2.38)<br />
Z 72π<br />
2Z 8 DG 4<br />
((M 5 − M 05 ) − (M 4 − M 04 ))), (2.39)<br />
Z 72π<br />
2Z 8 DG 4<br />
((M 5 − M 05 ) − (M 4 − M 04 ))), (2.40)<br />
q 2 = Atan2(s 2 , c 2 ). (2.41)<br />
Z kolei dla ogniwa 5 otrzymujemy równania przeniesienia napędu postaci<br />
Transformacja prosta napędu<br />
q 5 − q 05 = −<br />
Z 72π<br />
((M 4 − M 04 ) + (M 5 − M 05 )), (2.42)<br />
2Z 9 DG 5<br />
c 5 = cos (q 05 −<br />
Z 72π<br />
2Z 9 DG 4<br />
((M 5 − M 05 ) − (M 4 − M 04 ))), (2.43)
17<br />
Rysunek 2.5: Struktura układu różnicowego napędzającego osie nadgarstka.<br />
s 5 = sin (q 05 −<br />
a transformacja odwrotna<br />
Z 72π<br />
2Z 9 DG 4<br />
((M 5 − M 05 ) − (M 4 − M 04 ))), (2.44)<br />
q 2 = Atan2(s 2 , c 2 ). (2.45)<br />
Warto zwrócić uwagę, że efektywniejszą obliczeniowo implementację transformacji prostych<br />
i odwrotnych uzyskuje się dzięki nadmiarowej reprezentacji kątów przegubowych q i<br />
jako par (c i , s i ).
18<br />
Rozdział 3<br />
Konstrukcja robota ZS5R<br />
3.1 Konstrukcja mechaniczna<br />
Podstawowymi materiałami, z których została wykonana konstrukcja manipulatora robota<br />
stanowią blacha aluminiowa, kształtowniki aluminiowe (kątowniki, ceowniki) oraz<br />
pręty stalowe. Poszczególne elementy konstrukcyjne zostały wycięte z blachy aluminiowej<br />
o grubości 1-3mm i po odpowiednim wygięciu, poskręcane śrubami M3. Tak zmontowane<br />
elementy stanowią korpusy poszczególnych ramion robota. Konstrukcja chwytaka<br />
wykonano ceowników aluminowych. Na rysunku 3.1 przedstawiono wygląd manipulatora,<br />
z kolei na rysunku 3.2 przedstawiono rysunek złożeniowy, który zawiera główne zespoły<br />
konstrukcji mechanicznej.<br />
Oznaczenia na rysunku 3.2<br />
Rysunek 3.1: Zdjęcie manipulator.
Rysunek 3.2: Rysunek złożeniowy manipulatora z oznaczonymi głównymi częściami.<br />
19
20<br />
1 – podstawa manipulatora,<br />
2 – kolumna obrotowa,<br />
3 – główne ramie manipulatora,<br />
4 – przedramie manipulatora,<br />
5 – chwytak,<br />
6 – piasta w której obraca się oś kolumny manipulatora,<br />
M 1 – silnik napędzający oś kolumny ,<br />
M 2 – silnik napędzający oś głównego ramienia,<br />
M 3 – silnik napędzający oś przedramienia ,<br />
M 45 – dwa silniki napędzjące osie chwytaka ,<br />
M 6 – silnik napędzjący mechanizm zamykania i otwierania kiści chwytaka.<br />
<strong>Manipulator</strong> posiada pięć przegubów obrotowych, gdzie <strong>do</strong> 1-go (obrót kolumny) i 2-go<br />
(ruch głównego ramienia) napęd jest przenoszony przez wielostopniową przekładnię zębatą<br />
a następnie przez jednostopniową przekładnię. Zastosowanie <strong>do</strong>datkowej jednostopniowej<br />
przekładni związane jest z tym, iż zastosowane silniki nie posiadały <strong>do</strong>statecznie dużego<br />
momentu obrotowego <strong>do</strong> porusznia poszczególnych ramion manipulatora. Do układu napę<strong>do</strong>wego<br />
przegubu trzeciego (ruch przedramienia) <strong>do</strong>datkowo zastosowano pasek zębaty,<br />
rysunek 2.4. Zasosowanie paska zębatego spowo<strong>do</strong>wane jest tym, że silnik napę<strong>do</strong>wy osi 3<br />
jest umieszczony na końcu głównego ramienia rysunek 3.2. Umieszczenie tam silnika ma<br />
swoje pozytywne konsekwencje, a mianowicie stanowi on swego rodzaju przeciwwagę, co<br />
ma znaczenie ze względu na moment obrotowy potrzebny <strong>do</strong> ruchu głównego ramienia. Z<br />
kolei dla przegubu 4-go i 5-go napęd jest zrealizowany przez układ różnicowy. Powoduje<br />
to pewne utrudnienia w sterowaniu tymi osiami, równania przeniesienia napędu (2.38),<br />
(2.42) pokazują, że są zależne od siebie, (rysunek 2.5).<br />
Wszystkie parametry i rysunki wykonawcze poszczególnych części manipulatora zamieszczone<br />
są w <strong>do</strong>datku A.<br />
3.1.1 Silniki manipulatora<br />
<strong>Manipulator</strong> posiada sześć silników prądu stałego firmy MICROMOTORS S.R.L [10], z<br />
których cztery są typu RHE158.24.100 i dwa typu HLE149.24.43. Wszystkie te silniki są<br />
konstrukcyjnie sprzężone z przekładniami wielostopniowymi, co widać na rysunkach 3.3 i<br />
3.4. Do napędu kolumny robota, ramienia, przedramienia i wciągarki chwytaka wykorzystano<br />
silniki typu RHE158.24.100, a silniki typu HLE149.24.43 zastosowano <strong>do</strong> napędu<br />
układu różnicowego chwytaka. Dane związane z wymiarami fizycznymi tych silników są<br />
przedstawione na rysunkach umieszczonych w <strong>do</strong>datku A. W poniższych tabelach zestawiono<br />
parametry techniczne silników napę<strong>do</strong>wych.<br />
znamionowe napięcie zasilania 24[V ]<br />
przełożenie przekładni 94.37 : 1<br />
maksymalny ciągły moment obrotowy 60[Ncm]<br />
prędkość obrotowa bez obciązenia 66[obr/min]<br />
prędkość obrotowa z max. momentem obciążenia 45[obr/min]<br />
pobór prądu silnika w stanie jałowym < 70[mA]<br />
pobór prądu silnika przy max. obciążeniu 340[mA]<br />
Tabela 3.1: Parametry silnika typu RHE158.24.100.
21<br />
Rysunek 3.3: Zdjęcie silnika HLE149.24.43.<br />
Rysunek 3.4: Zdjęcie silnika RHE158.24.100.<br />
znamionowe napięcie zasilania 24[V ]<br />
przełożenie przekładni 43.3 : 1<br />
maksymalny ciągły moment obrotowy 15[Ncm]<br />
prędkość obrotowa bez obciązenia 78[obr/min]<br />
prędkość obrotowa z max. momentem obciążenia 55[obr/min]<br />
pobór prądu silnika w stanie jałowym < 50[mA]<br />
pobór prądu silnika przy max. obciążeniu 120[mA]<br />
Tabela 3.2: Parametry silnika typu HLE149.24.43.
22<br />
Silniki te <strong>do</strong>datkowo są fabrycznie wyposażone w kodery halotronowe. Jednak mała rozdzielczość<br />
(trzy impulsy na obrót wału silnika) nie pozwoliła na zastosowanie ich w tej<br />
konstrukcji. Dlatego silniki te zostały wyposażone w kodery przyrostowo-impulsowe typu<br />
AS5040 firmy AUSTRIAMICROSYSTEMS [6]. Możliwość zamontowania tych koderów<br />
wiązała się z koniecznością modyfikacji konstrukcji silników. Przeróbki polegały na skróceniu<br />
wału silnika o 3mm a następnie zamontowaniu na nim magnesu neodymowego [7].<br />
Wykonano również płytki drukowane <strong>do</strong> AS5040(<strong>do</strong>datek C) przystosowane <strong>do</strong> zamocowania<br />
w miejsce fabrycznego kodera. Aby zamknąć nowy koder w obu<strong>do</strong>wie, należało w<br />
pokrywie wykonać podtoczenie o głębokości 1mm. Na rysunkach 3.5 i 3.6 przedstawione<br />
Rysunek 3.5: Zdjęcie ilustrujące sposób zamontowania magnesu na osi silnika.<br />
Rysunek 3.6: Zdjęcie ilustrujące sposób zamontowania kodera AS5040.<br />
są zdjęcia ilustrujące sposób zamontowania koderów AS5040. Parametry tych koderów są<br />
<strong>do</strong>stępne na stronie internetowej producenta [6].
23<br />
3.2 Konstrukcja elektroniczna – sterownik robota<br />
Robot ZS5R posiada manipulator, którego poszczególne ramiona mogą wykonywać sekwencje<br />
ruchów, za które odpowiedzialny jest układ sterowania. Między innymi zapewnia<br />
on pomiar niezbędnych parametrów ruchu, takich jak położenia i prędkości. Ponadto<br />
układ sterowania zapewnia komunikację z komputerem nadrzędnym w celu pobrania parametrów<br />
<strong>do</strong> realizacji zadania, oraz wysyłania <strong>do</strong> komputera nadrzędnego aktualnych<br />
parametrów stanu robota. Na podstawie zadanych i zmierzonych parametrów sterownik<br />
wymusza na silnikach umieszczonych w ramionach takie sterowania, aby przeprowadzić<br />
efektor manipulatora z punktu początkowego <strong>do</strong> punktu <strong>do</strong>celowego. Moc obliczeniowa<br />
jednostki centralnej powinna zapewnić wykonanie wszystkich niezbędnych obliczeń w jak<br />
najkrótszym czasie. Na rysunku 3.7 przedstawiono schemat blokowy sterownika robota.<br />
Szczegółowe schematy elektroniczne, rysunki montażowe płytki (rozmieszczenie elemen-<br />
Rysunek 3.7: Schemat blokowy układu sterowania manipulatora.<br />
tów i ścieżek) oraz wykaz elementów znajdują się w <strong>do</strong>datku B i C.<br />
Sterownik został zmontowany na płytce dwustronnej o wymiarach 94x260mm, co widać<br />
na rysunku 3.8. Na płytce tej znajdują się:<br />
• moduł EM332/B z mikrokontrolerem MC68332, zamocowany na dwóch złączach<br />
48-stykowych każdy,<br />
• przetwornik A/C typu MAX1270 pracujący na magistrali QSPI,
24<br />
Rysunek 3.8: Płytka sterownika .<br />
• dwa przetworniki C/A typu MAX525 pracujących na magistrali QSPI,<br />
• układ transmisji szeregowej RS232C wykorzystujący SCI i translator poziomów napięciowych<br />
- układ MAX232A,<br />
• klucz sterujący cewką przekaźnika,<br />
• układ bramek NAND typu 74HCT132 będący układem wejściowym dla czujników<br />
synchronizacji,<br />
• sześć regulatorów prądu wykorzystujących układ wykonawczy L298N.<br />
Sterownik manipulatora można podzielić na dwa moduły:<br />
• moduł sterowania oparty na module EM332/B firmy ELFIN,<br />
• moduł mocy zawierający układy wykonawcze wraz z regulatorami prądu,<br />
3.2.1 Bu<strong>do</strong>wa modułu sterowania<br />
Głównym elementem modułu sterowania jest moduł EM332/B, na którym znajduje się<br />
jednostka centralna MC68332, rysunek 3.9. Wszystkie schematy elektryczne, rysunek płytki,<br />
rozmieszczenie elementów, zasoby mikrokontrolera MC68332 oraz zasada działania<br />
modułu EM332/B są opisane w raporcie [5].<br />
Schemat ideowy modułu sterowania znajduje się na rysunku 9.2 w <strong>do</strong>datku C.<br />
Za pomocą magistrali QSPI <strong>do</strong> modułu EM332/B podłączone są przetworniki:<br />
• dwa układy MAX525 [9](czterokanałowy, 12-bitowy przewornik cyfrowo-analogowy<br />
unipolarny o zakresie napięcia wyjściowego 0-2.5V, wymaga zewnętrznego napięcia<br />
odniesienia) służą <strong>do</strong> zadawania prądu w układach wykonawczych,<br />
• układ MAX1270 [9](ośmiokanałowy, 12-bitowy przetwornik analogowo-cyfrowy<br />
uni/bipolarny o 10V zakresie pomiaru z wbu<strong>do</strong>wanym źródłem napięcia odniesienia)<br />
służy <strong>do</strong> mierzenia prądu pobieranego przez silniki wykonawcze,
25<br />
Rysunek 3.9: Moduł EM332/B.<br />
Do bloku TPU mikrokontrolera podłączone są kodery przyrostowe AS5040 [6]. Zapewniają<br />
one pomiar położenia i prędkości. Zasotosowane kodery posiadają dwa wyjścia A/B, które<br />
generują dwa przebiegi kwadraturowe przesunięte w fazie o 90 stopni. Rozdzielczość tych<br />
koderów wynosi 256 impulsów/obrót osi silnika. Sygnały kodera są deko<strong>do</strong>wane w bloku<br />
TPU standar<strong>do</strong>wą funkcją QDEC, dzięki której otrzymujemy informacje o położeniu wału<br />
silnika. Ponieważ deko<strong>do</strong>wanie uwzględnia wszystkie zbocza sygnałów wychodzących<br />
z kodera, pomiar położenia osi silnika odbywa się z rozdzielczością 1024 impulsów/obrót<br />
wału silnika. Natomiast prędkości silników mierzone są pośrednio przez pomiar okresu<br />
sygnałów kwadraturowych generowanych przez kodery. Pomiar ten także jest realizowany<br />
w bloku TPU. Funkcja QDVEL <strong>do</strong>konująca pomiaru zlicza ilość impulsów zegara taktującego<br />
blok TPU w trakcie trwania jednego okresu sygnału kwadraturowego. Na podstawie<br />
zliczonej ilości impulsów oprogramowanie sterownika oblicza aktuanle prędkści silników.<br />
3.2.2 Bu<strong>do</strong>wa modułu mocy<br />
Zastosowanie silników prądu stałego <strong>do</strong> napędu osi robota podyktowane było tym, że<br />
silniki prądu stałego z magnesami trwałymi posiadą liniową zależność momentu napę<strong>do</strong>wego<br />
generowanego przez silnik od prądu pobieranego. W rezultacie sterowanie prą<strong>do</strong>we<br />
silników zapewnia kontrolę nad momentem uzyskiwanym na wale silnika. Z tego względu<br />
moduł mocy został zbu<strong>do</strong>wany z sześciu regulatorów prądu, które zawierają układy wykonawcze<br />
L298N [12]. Schemat ideowy jednego z tych regulatorów jest zamieszczony na<br />
rysunku 9.1 w <strong>do</strong>datku C. Na rysunku blokowym 3.10 przedstawiona jest idea działania<br />
układu regulacji prądu. Regulacja ta polega na porównaniu wartości zadanej prądu silnika<br />
z wartością rzeczywistą. Wartość rzeczywista prądu jest mierzona za pośrednictwem rezystorów<br />
pomiarowych R p1 i R p2 jako spadek napięcia na nich. Napięcie z dwóch rezystorów<br />
pomiarowych jest podane na wejście wzmacniacza różnicowego. W układzie wykonawczym<br />
prąd może płynąć tylko przez jeden rezystor w zależności od trybu pracy układu (obroty<br />
w lewo, prawo). Po znaku różnicy potencjału na rezystorach układ rozpoznaje kierunek<br />
płynącego prądu w silniku. Wartość napięcia ze wzmacniacza różnicowego podana jest na<br />
filtr RC, a następnie na układ komparatora, porównujący prąd rzeczywisty z wartością<br />
zadaną prądu. Wartość zadaną prądu ustawia się za pośrednictwem przetwornika C/A.<br />
Filtr RC ma za zadanie uśrednić wartość prądu silnika i podać ją <strong>do</strong> przetwornika A/C,<br />
dzięki któremu możemy diagnostycznie sprawdzać wartość rzeczywistą prądu płynącego
26<br />
Rysunek 3.10: Schemat blokowy regulatora prądu.<br />
przez silnik. Sygnał z układu komparatora jest podawany na układy logiczne, ktorych zadaniem<br />
jest wprowadzenie czasów opóźnień pomiędzy wyłączeniem jednej gałęzi mostka<br />
a włączeniem drugiej gałęzi mostka. Blok układów logicznych zawiera układy 74HC123,<br />
które są odpowiedzialne za czasy opóźnień.<br />
Układ wykonawczy (mostek mocy) jest zasilany z napięcia +36V, natomiast pozostała<br />
część elektroniki jest zasilana napięciem +5V (układy cyfrowe, przetworniki A/C i C/A),<br />
±5V(wzmacniacze operacyjne, komparatory). Schemat ideowy zasilacza jest zamieszczony<br />
w <strong>do</strong>datku C. Na rysunku 3.11 jest przedstawione zdjęcie zasilacza.<br />
Rysunek 3.11: Zasilacz części cyfrowej i analogowej
27<br />
3.2.3 Opis łączówek sterownika<br />
Przyłączenie zewnętrznego terminala (lub komputera wyposażonego w złącze szeregowe)<br />
jest możliwe dzięki złączu Z 10 .<br />
wolny 1 2 TxD<br />
RxD 3 4 wolny<br />
GND 5 6 wolny<br />
wolny 7 8 wolny<br />
wolny 9 10 wolny<br />
Tabela 3.3: Łączówka transmisji szeregowej Z 10 .<br />
oznaczenie opis styk<br />
TxD wyjście danych nadawanych (RS232C) 2<br />
RxD wejście danych odbieranych (RS232C) 3<br />
GND masa sygnałowa 5<br />
Tabela 3.4: Sygnały łączówki transmisji szeregowej Z 10 .<br />
Na złączu Z 8 wyprowadzone są sygnały, które umożliwiają podłączenie zewnętrznego sterownika.<br />
W przypadku gdy wykorzystywany jest wewnętrzny układ sterowania sygnały<br />
od STER I1 <strong>do</strong> STER I6 są odpowiednio połączone z STERI1 <strong>do</strong> STERI6 za pomocą<br />
zworek. W przypadku gdy chcemy podłączyć zewnętrzny sterownik (na karta współpracująca<br />
z pakietem MATLAB ) rozwieramy powyższe sygnały, a <strong>do</strong> styków sygnałowych<br />
oznaczonych STER I1,..., STER I6 podłączamy sygnały zewnętrzne o zakresie napięciowym<br />
0-2.5V.<br />
Wyprowadzenia i opis łączówki Z 11 jest zamieszczony w raporcie [5]. Dwustykowe złącza<br />
Z 1 ,...,Z 6 są wyjściami z poszczególnych stopni mocy i służą <strong>do</strong> przyłączenia silników.
28<br />
GND 1 2 Vcc<br />
TPU10 3 4 TPU11<br />
TPU8 5 6 TPU9<br />
TPU6 7 8 TPU7<br />
TPU4 9 10 TPU5<br />
TPU2 11 12 TPU3<br />
TPU0 13 14 TPU1<br />
POM I1 15 16 POM I2<br />
POM I3 17 18 POM I4<br />
POM I5 19 20 POM I6<br />
STER I1 21 22 STERI1<br />
STER I2 23 24 STERI2<br />
STER I3 25 26 STERI3<br />
STER I4 27 28 STERI4<br />
STER I5 29 30 STERI5<br />
STER I6 31 32 STERI6<br />
wolny 33 34 wolny<br />
Tabela 3.5: Łączówka Z 8 .
29<br />
oznaczenie opis styk<br />
GND masa sygnałowa 1<br />
Vcc +5V 2<br />
TPU10, TPU11 wyjścia kodera A/B na silniku M 6 3, 4<br />
TPU8, TPU9 wyjścia kodera A/B na silniku M 5 5, 6<br />
TPU6, TPU7 wyjścia kodera A/B na silniku M 4 7, 8<br />
TPU4, TPU5 wyjścia kodera A/B na silniku M 3 9, 10<br />
TPU2, TPU3 wyjścia kodera A/B na silniku M 2 11, 12<br />
TPU0, TPU1 wyjścia kodera A/B na silniku M 1 13, 14<br />
POM I1 sygnał pomiaru prądu silnika M 1 (-5V
oznaczenie opis styk<br />
SYNCHR1,...,SYNCHR6 sygnały z czujników synchronizacji q 1 ,..,q 6 4,2,8,6,12,10<br />
INDEX1,...,INDEX6 sygnały z kanałów indeks koderów D 1 ,..,D 6 3,1,7,5,11,9<br />
TPU0, TPU1 wyjścia kodera A/B na silniku M 1 13, 14<br />
TPU2, TPU3 wyjścia kodera A/B na silniku M 2 15, 16<br />
TPU4, TPU5 wyjścia kodera A/B na silniku M 3 17, 18<br />
TPU6, TPU7 wyjścia kodera A/B na silniku M 4 19, 20<br />
TPU8, TPU9 wyjścia kodera A/B na silniku M 5 21, 22<br />
TPU10, TPU11 wyjścia kodera A/B na silniku M 6 23, 24<br />
GND masa sygnałowa 25<br />
Vcc +5V 26<br />
Tabela 3.8: Opis sygnałów łączówki Z 9 .<br />
opis styk<br />
∼24V 1<br />
∼24V 2<br />
Tabela 3.9: Opis złącza zasilania modułu mocy Z 7 .<br />
opis styk<br />
-5V 1<br />
+5V 2<br />
GND 3<br />
+5V 4<br />
Tabela 3.10: Opis złącza zasilania Z 12 .<br />
opis styk<br />
+5V 1<br />
+5V 2<br />
GND 3<br />
-5V 4<br />
Tabela 3.11: Opis złącza na płytce zasilacz Z 13 .<br />
złącze<br />
funkcja<br />
Z 1 wyjście dla silnika M 1<br />
Z 2 wyjście dla silnika M 2<br />
Z 3 wyjście dla silnika M 3<br />
Z 4 wyjście dla silnika M 6<br />
Z 5 wyjście dla silnika M 4<br />
Z 6 wyjście dla silnika M 5<br />
Tabela 3.12: Opis złącz Z 1 ,...Z 6 .<br />
30
31<br />
Rozdział 4<br />
Oprogramowanie sterownika<br />
Sterownik robota jest zbu<strong>do</strong>wany w oparciu o mikrokontroler MC68332. Oprogramowanie<br />
dla tego mikrokontrolera można napisać w języku asembler C oraz C++. W tym przypadku<br />
oprogramowanie zostało napisane w języku C z użyciem makr w asemblerze. Oprogramowanie<br />
sterownika robota ZS5R zostało przygotowane w śro<strong>do</strong>wisku HI-CROSS<br />
firmy HIWARE. Zasadniczymi częściami programu sterownika są funkcja główna main()<br />
oraz procedura obsługi przerwania cyklicznego interrupt void int_PIT().<br />
4.1 Funkcja główna main()<br />
Na początku funkcji main() wywoływane są procedury inicjujące podstawowe bloki mikrokontrolera<br />
SIM, TPU, QSM oraz inicjowana jest początkowa wartość prądu regulatorów.<br />
sim_init(); /* inicjalizacja modulu SIM */<br />
qsm_init(); /* inicjalizacja modulu QSM */<br />
tpu_init(); /* inicjalizacja modulu TPU */<br />
Cur_init();<br />
/* ustawienie prądów zerowych regulatorów*/<br />
Funkcja sim_int() między innymi konfiguruje port E i ustawia wektor i poziom przerwania<br />
cyklicznego PIT<br />
/*Konfiguracja przerwania cyklicznego*/<br />
*((void (**) ())(4*PI_VECT)) = int_PIT; /* ustawienie wektora */<br />
PICR = (PI_LEVEL
32<br />
Z kolei funkcja tpu_int() inicjalizuje odpowiednie kanały bloku TPU dla funkcji QDEC<br />
i QDVEL, które to funkcje służą <strong>do</strong> pomiaru położenia i prędkości.<br />
W pętli głównej umieszczono obsługę menu pozwalającego interakcyjnie wybierać tryb<br />
pracy robota.<br />
out_text("\n\n\rPRACA DYPLOMOWA\n\n\r");<br />
out_text("Robot dydaktyczny ZS5R\n\r"); /* Intro */<br />
out_text("Autor: Zbigniew Struzik, Wroclaw 2005\n\r");<br />
out_text("h - Pomoc\n\r");<br />
out_text("\n\n\rMenu Glowne:\n\n\r");<br />
out_text("P - Pozycjonowa\n\r");<br />
out_text("M - Tryb sterowania recznego\n\r");<br />
out_text("X - Wylaczenie silnikow\n\r");<br />
out_text("S - przeslanie\n\r");<br />
out_text("H - Pomoc\n\n\r");<br />
Wybierając np. pozycję oznaczoną literką M lub m przechodzimy <strong>do</strong> menu sterowania<br />
ręcznego.<br />
out_text("\n\n\rMenu sterowania recznego:\n\n\r");<br />
out_text("1,Q - Wieza\n\r");<br />
out_text("2,W - Ramie\n\r");<br />
out_text("3,E - Przedramie\n\r");<br />
out_text("4,R - Nadgarstek (gora/<strong>do</strong>l)\n\r");<br />
out_text("5,T - Nadgarstek (obrot)\n\r");<br />
out_text("6,Y - Chwytak\n\r");<br />
out_text("SPACE - Silniki STOP\n\r");<br />
out_text("V - Definicja predkosci\n\r");<br />
out_text("ESC - Powrot <strong>do</strong> Menu Glownego\n\r");<br />
out_text(">>");<br />
4.2 Procedura obsługi przerwania cyklicznego<br />
Wszystkie pomiary i wyliczenie wielkości sterujące są przechowywane w strukturze state.<br />
typedef struct{<br />
. \\<br />
. \\<br />
. \\<br />
}STAN;<br />
STAN<br />
state;<br />
Nastawy i inne parametry regulatorów prędkości i położenia są przechowywane w strukturze<br />
nast.<br />
typedef struct<br />
{<br />
.\\<br />
.\\
33<br />
.\\<br />
}NAS;<br />
NAS<br />
nast;<br />
Wszystkie procesy związane ze sterowaniem oraz akwizycją danych pomiarowych, obsługiwane<br />
są w funkcji przerwania cyklicznego interrupt void int_PIT(). Funkcja ta jest<br />
wywoływana co 2ms. Jest w niej realizowany pomiar położenia i prędkości, przy pomocy<br />
funkcji QDEC i QDVEL:<br />
state.temposition[0] = TPURAM(QDEC1A,1);/* odczyt licznika pozycji q1*/<br />
state.M[0] &= 0xffff0000;<br />
state.M[0] += state.temposition[0];<br />
if((state.lastposition[0]-state.temposition[0]) > 32767)<br />
{<br />
state.M[0] += 0x10000;<br />
}<br />
if((state.temposition[0]-state.lastposition[0]) > 32767)<br />
{<br />
state.M[0] -= 0x10000;<br />
}<br />
state.lastposition[0] = state.temposition[0];<br />
state.speed[0] = QDVEL2Speed((int)TPURAM(QDVEL1,2));<br />
state.speed[1] = QDVEL2Speed((int)TPURAM(QDVEL2,2));<br />
state.speed[2] = QDVEL2Speed((int)TPURAM(QDVEL3,2));<br />
state.speed[3] = ((TPSEC*(state.M[3]-state.oldposition[2])+2)/4);<br />
state.speed[4] = ((TPSEC*(state.M[4]-state.oldposition[0])+2)/4);<br />
state.speed[5] = ((TPSEC*(state.M[5]-state.oldposition[1])+2)/4);<br />
Pomiar prądu jest realizowany za pomocą przetwornika A/C pracującego na kolejkowej<br />
magistrai QSPI. Aktualizacja zmierzonego prądu również jest realizowana w funkcji przerwania<br />
cyklicznego PIT.<br />
state.MCurrent[0] = ADBip2Int(QREC[1]>>4);<br />
state.MCurrent[1] = ADBip2Int(QREC[2]>>4);<br />
state.MCurrent[2] = ADBip2Int(QREC[3]>>4);<br />
state.MCurrent[3] = ADBip2Int(QREC[4]>>4);<br />
state.MCurrent[4] = ADBip2Int(QREC[5]>>4);<br />
state.MCurrent[5] = ADBip2Int(QREC[0]>>4);<br />
Proces wyliczenia sterowań dla poszczególnych osi jest realizowany za pomocą regulatorów<br />
PID położenia i prędkości zaimplementowanych w funkcjach:<br />
void ControlPID_pos(NAS *ptr_n, STAN *ptr_s, int i)<br />
void ControlPID_speed(NAS *ptr_n, STAN *ptr_s, int i)<br />
wywoływanych w obsłudze przerwania PIT. Na rysunku blokowym 4.1 jest przedstawiony<br />
sposób realizacji tych regulatorów. Po wyliczeniu sterowań przez funkcje regulatorów,<br />
wartości zadanych prądów są wpisywane <strong>do</strong> TRANSMIT RAM w bloku QSPI:
34<br />
Rysunek 4.1: Schemat blokowy układu sterowania.<br />
QTRAN[6] = (state.SetCurrent[0] & 0x0FFF)|0x3000;<br />
QTRAN[7] = (state.SetCurrent[1] & 0x0FFF)|0x7000;<br />
QTRAN[8] = (state.SetCurrent[2] & 0x0FFF)|0xB000;<br />
QTRAN[9] = (state.SetCurrent[3] & 0x0FFF)|0xF000;<br />
QTRAN[10] = (state.SetCurrent[4] & 0x0FFF)|0x3000;<br />
QTRAN[11] = (state.SetCurrent[5] & 0x0FFF)|0x7000;<br />
Do przeprowadzenia eksperymentów i akwizycji danych pomiarowych w funkcji<br />
interrupt void int_PIT() jest wykonywany zapis wybranych parametrów <strong>do</strong> tablicy.<br />
if (flaga_pomiarow) {<br />
tabpom[l][0]=state.SetCurrent[1];<br />
tabpom[l][1]=state.MCurrent[1];<br />
tabpom[l][2]=state.speed[1];<br />
tabpom[l][3]=nast.W_set[1];<br />
if (++l>=1000)<br />
{<br />
l=0;<br />
flaga_pomiarow=0;<br />
}<br />
}<br />
Następnie tablica z pomiarami jest wysyłana <strong>do</strong> komputera nadrzędnego przez łącze szeregowe<br />
za pomocą funkcji wywoływanej w menu głównym (S), podanej poniżej.<br />
void out_text(char *_text) {<br />
byte length=0;<br />
while(n_bufor);<br />
while(*(length+_text))
35<br />
length++;<br />
while(length)<br />
{<br />
length--;<br />
bufor[n_bufor]=*(length+_text);<br />
n_bufor++;<br />
}<br />
SCCR1=SCCR1|0x0080;<br />
return;<br />
}<br />
Opisane oprogramowanie zachowuje w dużym stopniu zgodność z oprogramowaniem robota<br />
Romik (w zakresie menu). Pozwala również na zbieranie wyników pomiarów parametrów<br />
ruchu osi robota przy prowadzeniu eksperymentów.
36<br />
Rozdział 5<br />
Badania i wyniki eksperymentów<br />
W celu zbadanie konstrukcji sterownika i zaobserwowanie parametrów jakościowych przeprowadzono<br />
odpowiednie eksperymenty. W celu zebrania pomiarów zaimplementowano<br />
program testowy. W programie tym, cyklicznie co 2ms wywoływana jest procedura pozwalająca<br />
zadawać momenty napę<strong>do</strong>we silników oraz zebrać próbki pomiarowe. Zebrane<br />
próbki pomiarowe ze sterownika zostały wysłane <strong>do</strong> komputera nadrzędnego, na którym<br />
zostały zwizualizowane przy pomocy programu gnuplot. Wysłane próbki nie zostały<br />
wyskalowane dla nich odpowiednich jednostkach. Oś odciętych (czasu) na wszystkich wykresach<br />
jest skalowana w cyklach przerwania PIT (2ms). Osie rzędnych wyskalowane są<br />
bezpośrednio w jednostkach pomiarowych z przetworników.<br />
Na rysunku 5.1 przedstawiony jest wykres ilustrujący odpowiedź sprzętowego regulatora<br />
prądu na zadany prąd. Prąd zadany został wygenerowany przez regulator prędkości dla<br />
przegubu q 1 (nastawy regulatora PID kp w=20, kd w=10, ki w=5, lim int=500 lim curr<br />
= 2047 w set=10000).<br />
Na wykresie można zaobserwować opóźnienie odpowiedzi regulatora na zadany prąd.<br />
4500<br />
4000<br />
’pom2.9.txt’ using 1:2<br />
’pom2.9.txt’ using 1:3<br />
3500<br />
3000<br />
2500<br />
2000<br />
1500<br />
1000<br />
500<br />
0<br />
-500<br />
-1000<br />
0 5 10 15 20 25 30<br />
Rysunek 5.1: Wykres odpowiedzi regulatora na zadany prąd.<br />
Wynika to z tego, iż zadany prąd regulatora jest ustawiany przez przetwornik C/A, który<br />
pracuje na magistrali QSPI. Warto zwrócić uwagę na odpowiedź skokową regulatora<br />
prądu typową dla układu inercyjnego.<br />
Na rysunku 5.2 przedstawiono wynik pracy regulatora prędkości podczas rozpędzania
37<br />
przegubu q 0 . Widać, że w początkowym okresie prędkość narasta liniowo pod wpływem<br />
ograniczonego <strong>do</strong> zadanej wartości prądu. Po osiągnięciu zadanej prędkości regulator prędkości<br />
steruje prądem silnika utrzymując prędkość ruchu przegubu (rysunek 5.3).<br />
7000<br />
6000<br />
’pom1.txt’ using 1:2<br />
’pom1.txt’ using 1:3<br />
’pom1.txt’ using 1:4<br />
5000<br />
4000<br />
3000<br />
2000<br />
1000<br />
0<br />
-1000<br />
0 10 20 30 40 50 60<br />
Rysunek 5.2: Rozpędzanie osi q 0 .<br />
8000<br />
7000<br />
’pom1.txt’ using 1:2<br />
’pom1.txt’ using 1:3<br />
’pom1.txt’ using 1:4<br />
6000<br />
5000<br />
4000<br />
3000<br />
2000<br />
1000<br />
0<br />
-1000<br />
0 100 200 300 400 500 600 700 800 900 1000<br />
Rysunek 5.3: Regulacja prędkości osi q 0 .<br />
Jak widać, opracowany sterownik pozwala gromadzić pomiary parametrów ruchu poszczególnych<br />
osi, co jest szczególnie przydatne przy badaniu własności regulatorów, <strong>do</strong>bieraniu<br />
ich nastaw, szukaniu charakterystyk napędów i własności dynamicznych robota.
38<br />
Rozdział 6<br />
Podsumowanie<br />
W wyniku niniejszej pracy powstało stanowisko laboratoryjne, którego główną częścią jest<br />
manipulator dydaktyczny o nazwie ZS5R. Cykl pracy rozpoczęto od <strong>do</strong>boru silników na-<br />
Rysunek 6.1: Wi<strong>do</strong>k skonstruowanego manipulatora.<br />
pę<strong>do</strong>wych, a następnie wykonano konstrukcję mechaniczną manipulatora. Skonstruowany<br />
manipulator jest klasy 5R, (posiadający pięć stopni swobody) oraz jest wyposażony w<br />
chwytak dwupalczasty. Następnie zaprojektowano i wykonano w pełni działający układ<br />
sterowania, który pozwala na sterowanie prądem silników prądu stałego (a więc i momentem<br />
napę<strong>do</strong>wym), przez co istnieje w przyszłości możliwość implementacji algorytmów<br />
sterowania uwzględniających dynamikę robota. W bu<strong>do</strong>wie układu elektronicznego
przewidziano złącze pozwalające na podłączenie zewnętrznego układu sterowania, jak na<br />
przykład karty współpracującej ze śro<strong>do</strong>wiskiem MATLAB/SIMULINK. Wykonano testowe<br />
oprogramowanie pozwalające na zbieranie danych pomiarowych z wewnętrznych<br />
czujników, a następnie wysyłanie ich za pomocą interfejsu szeregowego na terminal w<br />
postaci pliku txt. Zebrane dane pozwoliły na przeprowadzenie eksperymentów.<br />
W pracy zamieszczono również, przykła<strong>do</strong>wą instrukcję laboratoryjną stanowiska oraz<br />
<strong>do</strong>łączono <strong>do</strong>datki, w których znajduje się pełna <strong>do</strong>kumentacja techniczna konstrukcji<br />
mechanicznej i elektronicznej.<br />
Wykonane oprogramowanie <strong>do</strong>łączono na nośniku elektronicznym (CD-ROM).<br />
Opisany robot może być wykorzystany <strong>do</strong> celów <strong>dydaktycznych</strong> w znacznie szerszym zakresie<br />
niż wcześniej wspomniany Romik, ze względu na możliwość sterowania momentem<br />
napę<strong>do</strong>wym i otwartą konstrukcję układu sterowania.<br />
39
40<br />
Bibliografia<br />
[1] E. JEZIERSKI. Robotyka kurs podstawowy. Wydawnictwo Politechniki Łódzkiej,<br />
Łódź, 2002.<br />
[2] M.W. SPONG and M. VIDYASAGAR. Dynamika i sterowanie robotów. Wydawnictwo<br />
Naukowo-Techniczne, Warszawa, 1997.<br />
[3] K. TCHOŃ and W. JACAK. Podstawy robotyki. Wydawnictwo Politechniki Wrocławskiej,<br />
Wrocław, 1992.<br />
[4] K. TCHOŃ, A. MAZUR, I. DULĘBA, R. HOSSA, i R. MUSZYŃSKI. <strong>Manipulator</strong>y<br />
i roboty mobilne: modele, planowanie ruchu, sterowanie. Akademicka Oficyna<br />
Wydawnicza PLJ, Warszawa, 2000.<br />
[5] M. WNUK. Moduł z mikrokontrolerem mc68332. Raport serii SPR 7/2004, Instytut<br />
Cybernetyki Technicznej Politechniki Wrocławskiej, Wrocław, 2004.<br />
[6] http://www.austriamicrosystems.com/<br />
[7] http://www.bomatec.ch/<br />
[8] http://www.freescale.com/<br />
[9] http://www.maxim ic.com/<br />
[10] http://www.micromotorssrl.com/<br />
[11] http://www.motorola.com/<br />
[12] http://www.st.com/stonline/
41<br />
Rozdział 7<br />
Dodatek A<br />
7.1 Parametry mechaniczne manipulatora<br />
Dane techniczne robota ZS5R<br />
Układ kinematyczny 5 stopni swobody<br />
Napęd<br />
6 silników prądu stałego<br />
Pomiar położenia względny, kodery przyrostowo-impulsowe<br />
o rozdzielczości D=1024<br />
Przestrzeń robocza<br />
Oś 1 (obrót kolumny) ±90 ◦<br />
Oś 2 (ramię)<br />
±40 ◦<br />
Oś 3 (przedramię) ±90 ◦<br />
Oś 4 (zgięcie nadgarstka) ±60 ◦<br />
Oś 5 (obrót chwytaka) ±360 ◦<br />
Rodzaj chwytaka Chwytak dwupalcowy, o rozwarciu równoległym<br />
w zakresie 0-60mm.<br />
Stopień rozwarcia i siła uchwytu programowane.<br />
Wymiary mechaniczne<br />
l 1<br />
201mm<br />
l 2<br />
217.5mm<br />
l 3<br />
134mm<br />
l 4<br />
90mm<br />
Parametry przekładni<br />
G 1 = G 2 = G 3 = G 6 94.37:1<br />
G 4 = G 5 43.3:1<br />
Z 1 16 zębów, moduł 1<br />
Z 2 70 zębów, moduł 1<br />
Z 3 15 zębów, moduł 0.8<br />
Z 4 104 zęby moduł 0.8<br />
Z 5 20 zębów, moduł 1<br />
Z 6 68 zębów, moduł 1<br />
Z 7 16 zębów, moduł 1<br />
Z 8 24 zęby moduł 1<br />
Z 9 42 zęby moduł 1
42<br />
7.2 Rysunki techniczne części mechanicznej<br />
Rysunek 7.1: Rysunek techniczny silnika RHE 158.24.100.<br />
Rysunek 7.2: Rysunek techniczny silnika HLE 149.24.43.<br />
Rysunki techniczne silników zostały wzięte ze strony producenta silników [10].
60<br />
Rozdział 8<br />
Dodatek B<br />
8.1 Wykaz elementów części elektronicznej<br />
liczba wartość obu<strong>do</strong>wa oznaczenie na schemacie<br />
Kondensatory<br />
6 100n EU025 C1, C2, C3, C4, C5, C6<br />
3 1000u EUTT5D10 C9, C10, C11<br />
2 2200u EUE5 C7, C8<br />
Diody<br />
1 Prostownik RB1A RB1A<br />
Układy scalone<br />
1 7905 TO2220 IC3<br />
1 L4940V5 TO2220 IC1<br />
1 LM2940CT TO2220 IC2<br />
Transformator<br />
1 TS8/220 – –<br />
Złącza<br />
3 MSTBA4 – Z12, Z13, Z14<br />
inne<br />
2 Radiator SK104 KK1, KK2<br />
Tabela 8.1: Zestawienie elementów elektronicznych na płytce zasilacza.<br />
liczba wartość obu<strong>do</strong>wa oznaczenie na schemacie<br />
Kondensatory<br />
1 100n EUC0805 C1<br />
1 1u EUC0805 C2<br />
Układy scalone<br />
1 AS5040 SSOP16 U1<br />
Tabela 8.2: Zestawienie elementów elektronicznych na płytce kodera AS5040.
61<br />
liczba wartość obu<strong>do</strong>wa oznaczenie na schemacie<br />
Rezystory<br />
12 1R R-EU0411 R2, R105, R106, R107, R108, R109, R110,<br />
R111, R112, R113, R114, R115<br />
24 1k M0805 R27, R28, R32, R34, R40, R42, R43, R47, R55,<br />
R56, R57, R60, R67, R70, R71, R75, R83, R84,<br />
R85, R88, R95, R96, R101, R103<br />
6 3k M0805 R31, R48, R62, R76, R90, R104<br />
6 5k CA6V R117, R118, R119, R120, R121, R122<br />
36 10k M0805 R1, R25, R26, R29, R30, R33, R38, R39, R44,<br />
R45, R46, R53, R54, R58, R59, R61, R63, R64,<br />
R68, R69, R72, R73, R74, R77, R78, R81, R82,<br />
R86, R87, R89, R97, R98, R99, R100, R102,<br />
R123<br />
8 18k M0805 R21, R22, R35, R36, R49, R50, R91, R92<br />
18 100k M1206 R3, R4, R5, R6, R7, R8, R9, R10, R11,<br />
R12, R13, R14, R15, R16, R17, R18, R19, R20<br />
8 130k M0805 R23, R24, R37, R41, R51, R52, R93, R94<br />
4 200k M0805 R23, R24, R37, R41, R51, R52, R93, R94<br />
Kondensatory<br />
24 270p EUC0805 C53, C54, C59, C60, C67, C68, C69, C70, C73,<br />
C74, C75, C76, C83, C84, C89, C90, C93, C94,<br />
C99, C100, C103, C104, C109, C110<br />
73 100n EUC0805 C1, C3, C4, C5, C6, C7, C8, C9, C10,<br />
C11, C12, C13, C14, C15, C16, C17, C18, C19,<br />
C20, C21, C22, C23, C24, C25, C26, C27, C28,<br />
C29, C30, C31, C32, C33, C34, C35, C36, C37,<br />
C38, C39, C40, C41, C42, C43, C44, C45, C46,<br />
C47, C48, C49, C51, C52, C55, C57, C61, C62,<br />
C63, C65, C71, C72, C77, C79, C81, C82, C85,<br />
C87, C91, C92, C95, C97, C101, C102, C105,<br />
C107, C111<br />
12 1u EUC0805 C56, C58, C64, C66, C78, C80, C86, C88, C96,<br />
C98, C106, C108<br />
1 4.7u B45181A C2<br />
1 10000u/50V EUE10-30 C50<br />
Diody<br />
7 BAT42 BAT42 D25, D26, D27, D28, D29, D30, D31<br />
24 BAT49 BAT49 D1, D2, D3, D4, D5, D6, D7, D8, D9,<br />
D10, D11, D12, D13, D14, D15, D16, D17,<br />
D18, D19, D20, D21, D22, D23, D24<br />
1 Prostownik FBU4 B1<br />
Tranzystory<br />
1 BC640-16 TO92 T1<br />
Tabela 8.3: Zestawienie elementów elektronicznych na płytce sterownika.
62<br />
liczba wartość obu<strong>do</strong>wa oznaczenie na schemacie<br />
Układy scalone<br />
6 74HC123D SO-16 IC12, IC13, IC14, IC15, IC16, IC18<br />
3 74HCT00D SOIC-14 IC19, IC20, IC21<br />
3 74HCT08D SO-16 IC22, IC24, IC25<br />
3 74HCT132D SO-14 IC1, IC2, IC3<br />
6 L298N MULTIWATT15 IC4, IC5, IC6, IC8, IC9, IC10<br />
6 LM311D SO-8 IC26, IC27, IC28, IC29, IC30, IC31<br />
7 LM385-Z TO92 UZ1, UZ2, UZ3, UZ4, UZ5, UZ6, UZ7<br />
6 MAX4164 SO-14 IC7, IC11, IC17, IC23, IC32, IC33<br />
1 MAX232A SO-16 U$2<br />
2 MAX525 SSOP US, US1<br />
1 MAX1270 SSOP U$1<br />
1 MOD1 – ELFIN-EM332/A-4X24<br />
Przekaźnik<br />
1 G2R2 G2R2 K1<br />
Złącza<br />
1 – ML10 Z10<br />
1 – ML26 Z9<br />
1 – ML34 Z8<br />
1 – W237-02P Z7<br />
1 – W237-04P Z12<br />
6 – MPT2 Z1, Z2, Z3, Z4, Z5, Z6<br />
Tabela 8.4: Zestawienie elementów elektronicznych na płytce sterownika.
63<br />
Rozdział 9<br />
Dodatek C<br />
9.1 Schematy ideowe i rysunki montażowe części elektronicznej<br />
Rysunek 9.1: Schemat ideowy jednego z regulatorów prądu modułu mocy.
Rysunek 9.2: Schemat ideowy połączenia modułu EM332 z układami peryferyjnymi.<br />
64
65<br />
Rysunek 9.3: Schemat ideowy zasilacza.<br />
Rysunek 9.4: Schemat ideowy układu sterowania cewką przekaźnika.
66<br />
Rysunek 9.5: Schemat elektryczny kodera AS5040.<br />
Rysunek 9.6: Obwód drukowany wierzchniej warstwy płytki zasilacza.<br />
Rysunek 9.7: Obwód drukowany spodniej warstwy płytki zasilacza.
Rysunek 9.8: Rozmieszczenie elementów na wierzchniej stronie płytki zasilacza.<br />
67
Rysunek 9.9: Obwód drukowany wierzchniej warstwy płytki sterownika.<br />
68
Rysunek 9.10: Obwód drukowany spodniej warstwy płytki sterownika.<br />
69
Rysunek 9.11: Rozmieszczenie elementów na wierzchniej stronie płytki sterownika.<br />
70
Rysunek 9.12: Rozmieszczenie elementów na spodniej stronie płytki sterownika.<br />
71
72<br />
Rysunek 9.13: Obwód drukowany wierzchniej warstwy płytki kodera AS5040.<br />
Rysunek 9.14: Obwód drukowany spodnie warstwy płytki kodera AS5040.<br />
Rysunek 9.15: Rozmieszczenie elementów na wierzchniej warstwie płytki kodera AS5040.<br />
Rysunek 9.16: Rozmieszczenie elementów na spodniej warstwie płytki kodera AS5040.
73<br />
Rozdział 10<br />
Przykła<strong>do</strong>wa instrukcja<br />
laboratoryjna<br />
10.1 Cel ćwiczenia.<br />
Celem ćwiczenia jest zapoznanie się ze strukturą kinematyczną robota dydaktycznego<br />
ZS5R oraz identyfikacja jego parametrów geometrycznych.<br />
10.2 Podstawy teoretyczne.<br />
10.2.1 Charakterystyka robota ZS5R.<br />
Robot dydaktyczny ZS5R jest manipulatorem, którego napęd poszczególnych osi jest zrealizowany<br />
za pomocą silników prądu stałego. Sterownik robota składa się z modułu mocy<br />
i modułu sterowania. Moduł mocy jest zbu<strong>do</strong>wany z sześciu regulatorów prądu, natomiast<br />
moduł sterowania oparty jest na module EM332/B firmy ELFIN, który wykorzystuje<br />
mikrokontroler MC68332. Oprogramowanie sterownika komunikuje się z komputerem<br />
nadrzędnym przy pomocy asynchronicznej transmisji szeregowej (19200, 8N1). Pozwala<br />
to na sterowanie ręczne oraz na zadawanie i odczytywanie chwilowych położeń silników<br />
(M i ). Pomiar położenia wału silników jest realizowany za pomocą koderów impulsowoprzyrostowych<br />
(AS5040) o rozdzielczości 1024 imp/obr. Położenia napędów wyrażone są<br />
w postaci liczb całkowitych oznaczających ilość impulsów zliczanych od pewnego stanu<br />
początkowego. Ustalenie stanu początkowego po włączeniu sterownika odbywa się automatycznie<br />
przez <strong>do</strong>prowadzenie poszczególnych osi <strong>do</strong> położeń, w których sygnały czujników<br />
synchronizacji umieszczone w przegubach i sygnaly z kanalów indeks koderów dadzą<br />
wartość 1 (logicznie) (procedura synchronizacji). W tej konfiguracji zwanej konfiguracją<br />
synchronizacji, wszystkie liczniki położenia silników są ustawiane na odpowiednie wartość.<br />
Każde wyłączenie zasilania robota powoduje utratę informacji o pozycji i konieczność<br />
powtórzenia procedury synchronizacji.<br />
Ze względu na bezpośrednie przeniesienie napędu na przeguby 1, 2 i 3 ich czujniki synchronizacji<br />
są osiągane przy ustalonych wartościach kątów {θ si = q si |i = 1, 2, 3}, co odpowiada<br />
ustalonym pozycjom silników M s1 , M s2 , M s3 . Przeniesienie napędu przegubów 4 i 5 jest<br />
także bezpośrednie i odbywa się za pośrednictwem mechanizmu różnicowego. Zmiana kąta<br />
q 4 jest proporcjonalna <strong>do</strong> sumy zmian pozycji silników (M 4 + M 5 ), a zmiana kąta q 5 - <strong>do</strong><br />
ich różnicy (M 5 − M 4 ).
74<br />
10.2.2 Struktura kinematyczna dla współrzędnych<br />
przegubowych q i .<br />
Struktura kinematyczna robota dydaktycznego ZS5R jest przedstawiona na rys. 10.1. Na<br />
Rysunek 10.1: Struktura kinematyczna manipulator ZS5R.<br />
końcu każdego ogniwa robota umieszczony jest lokalny układ współrzędnych związany<br />
z tym ogniwem. Płaszczyzna planarności robota jest wyznaczona przez osie X 0 i Y 0<br />
układu bazowego.<br />
Współrzędnymi wewnętrznymi są:<br />
q 1 – kąt obrotu kolumny [ ◦ ],<br />
q 2 – kąt obrotu głównego ramienia [ ◦ ],<br />
q 3 – kąt obrotu przedramienia [ ◦ ],<br />
q 4 – kąt odchylenia chwytaka względem przedramienia [ ◦ ],<br />
q 5 – kąt obrotu chwytaka wokół własnej osi [ ◦ ],<br />
q 6 – rozstaw szczęk chwytaka [mm].<br />
Jako współrzędne zewnętrzne wybrano następujące zmienne<br />
x, y, z – współrzędne kartezjańskie końca efektora wyrażone<br />
w układzie bazowym X 0 Y 0 Z 0 [mm],
75<br />
η – kąt podejścia chwytaka (kąt pomiędzy nieskręconą<br />
płaszczyzną chwytaka a płszczyzną poziomą) [ ◦ ] ,<br />
ρ – kąt obrotu chwytaka wokół własnej osi [ ◦ ] ,<br />
s – rozstaw szczęk [mm].<br />
10.2.3 Transformacja Denavita-Hartenberga dla zmiennych q i .<br />
Prosty model kinematyki dla robota ZS5R uzyskuje się przy użyciu notacji Denavita-<br />
Hartenberga. Transformacja Denavita-Hartenberga pomiędzy lokalnymi układami współrzędnych<br />
stowarzyszonymi z poszczególnymi ogniwami robota jest następująca<br />
A 1 0 (q 1) = Rot(Z, q 1 )Trans(Z, l 1 )Rot(X, π),<br />
2<br />
A 2 1(q 2 ) = Rot(Z, q 2 )Trans(X, l 2 ),<br />
A 3 2 (q 3) = Rot(Z, q 3 )Trans(X, l 3 ),<br />
A 4 3(q 4 ) = Rot(Z, q 4 )Rot(Z, − π)Rot(X, − π),<br />
2 2<br />
A 4 5 (q 5) = Rot(Z, q 5 )Trans(Z, l 4 ),<br />
(10.1)<br />
gdzie symbolami l i oznaczono następujące parametry geometryczne robota<br />
l 1 – wysokość kolumny [mm],<br />
l 2 – długość ramienia [mm],<br />
l 3 – długość przedramienia [mm],<br />
l 4 – długość chwytaka ujmująca długość palców chwytaka [mm],<br />
10.3 Przebieg ćwiczenia<br />
10.3.1 Obsługa stanowiska<br />
Stanowisko laboratoryjne składa się z robota ZS5R oraz komputera PC połączonego ze<br />
sterownikiem robota za pośrednictwem portu szeregowego. Oprogramowanie stanowi program<br />
ZSident uruchamiany w śro<strong>do</strong>wisku systemu Linux.<br />
Robot powinien być ustawiony przy krwędzi stołu, równolegle <strong>do</strong> przedniej ściany podstawy<br />
robota.<br />
Przed uruchomieniem programu należy włączyć zasilanie robota i poczekać na zakończenie<br />
synchronizacji (zatrzymanie wszystkich silników).<br />
Po uruchomieniu programu ZSident powinna nastąpić powtórna synchronizacja, a na<br />
ekranie pojawi się menu.<br />
Komenda I pozwala zadawać konfigurację we współrzędnych q i (wprowadzenie pustej linii<br />
powoduje pozostawienie <strong>do</strong>tychczasowej wartości). Po zakończeniu wprowadzania następuje<br />
wykonanie ruchu.<br />
Komenda X podaje odczyt położenia robota w trzech układach współrzędnych:<br />
• {M i } - przestrzeń napę<strong>do</strong>wa (pozycje silników wyrażone w impulsach),
76<br />
• {q i } - przestrzeń konfiguracyjna,<br />
• {x, y, z, η, ρ, s} - przestrzeń zadaniowa.<br />
Komenda Q powoduje zakończenie pracy programu ident i wyłączenie zasilania napędów.<br />
10.3.2 Wstępne przygotowanie <strong>do</strong> ćwiczenia.<br />
1. używając transformacji Denavita-Hartenberga przedstawionej w rozdziale 10.2.3 obliczyć<br />
prosty model kinematyki<br />
(q 1 , ..., q 6 ) → (x, y, z, η, ρ, s)<br />
2. zastanowić się jak należy wybrać konfigurację robota dydaktycznego, aby z równań<br />
kinematyki uzyskać jednoznacznie wartości parametrów geometrycznych l 1 , l 2 , l 3 , l 4 .<br />
10.3.3 Zadania <strong>do</strong> wykonania.<br />
Poruszając poszczególnymi przegubami robota ustalić położenie układu bazowego oraz<br />
sprawdzić, jakie zakresy zmienności mają poszczególne współrzędne wenętrzne q i . Aby<br />
zidentyfikować parametry geometryczne l i należy rozpocząć od ustawienia robota w pewnej<br />
konfiguracji poprzez zadanie określonych wartości współrzędnych wewnętrznych q i .<br />
Następnie dla tak zadanej konfiguracji należy odczytać odpowiadające jej współrzędne<br />
zewnętrzne z menu programu. W sprawozdaniu należy umieścić wyliczony prosty model<br />
kinematyki i w oparciu o ten model wyliczyć z układu równań stałe l i .<br />
Instrukcja ta została przygotowana w oparciu o instrukcję ”Identyfikacja parametrów geometrycznych<br />
robota dydaktycznego ROMIK” autorstwa I. Dulęba, A. Mazur, M. Wnuk.