LABORATORIUM
ZAKŁAD TEORII
KONSTRUKCJI
TEORII
MECHANIZMÓW
MASZYN
I MANIPULATORÓW
MECHANIZMÓW I MASZYN
nr ćw.:
TEMAT: Realizacja zaplanowanej trajektorii mechanizmu
płaskiego o dwóch stopniach ruchliwości
Planowanie trajektorii
W eksploatacji automatów i robotów podstawowym zadaniem jest zaplanowanie trajektorii punktu (na przykład środka chwytaka). Przez trajektorię rozumie się tor ruchu oraz przebieg prędkości wzdłuż tego toru. Użyteczną matematyczną postacią toru, w tym laboratorium, jest przedstawienie go w formie parametrycznej:
y = f (λ) , x = f (λ)
(1)
Q
y
Q
x
Tor ograniczają punkt początkowy Q0 ( x , y ) i końcowy Qk ( x , y ). Parametr λ
0
Q
0
Q
k
Q
k
Q
zmienia się w zakresie: λ ∈ (λ ;λ ) . Współrzędne punktów Q
y
= f (λ ) ,
0
k
0 i Qk wynoszą:
0
Q
y
0
x
= f (λ ) , y = f (λ ) , x = f (λ ) . Przebieg prędkości wzdłuż toru powinien zaczynać 0
Q
x
0
k
Q
y
k
k
Q
x
k
się i kończyć od zerowej wartości, zatem można go przedstawić za pomocą wyrażenia: v = s& = v
h( t)
(2)
max
gdzie:
0 ≤ t ≤ T , h(0) = 0 , h( T ) = 0 , 0 ≤ h( t) ≤ 1, T – całkowity czas ruchu.
y
y
Qk
k
Q
prędkość punktu Q
ds
α
dy
Q
s
tor ruchu punktu Q
y
0
Q
Q0
x
x
x
dx
0
Q
k
Q
Q
Rys.1. Trajektoria punktu Q.
Całkowitą długość toru można wyznaczyć z równania parametrycznego. Długość elementarna dx (λ)
toru
2
2
ds = ( dx
+ dy ) zawiera dx
Q
dx =
dλ = x′ dλ ,
Q
Q
Q i dyQ , które wyraża się jako:
Q
Q
dλ
dy (λ)
λ k
Q
2
2
dy =
dλ = y d
′ λ . Ostatecznie długość toru wynosi: L = ∫ ( x′ + y′ dλ . Na Q )
( Q)
Q
dλ
λ0
T
podstawie przebiegu prędkości wzdłuż toru wyznacza się jego długość: L = v
∫ h( t) dt .
max
0
Porównując do siebie wyrażenia na L otrzymuje się równanie, z którego zadając T wyznacza się v
:
max
T
λ k
2
2
v
∫ h( t) dt = ∫ x′ + y′ dλ → v (3)
max
( Q) ( Q)
max
0
λ0
W praktycznej realizacji trajektorii całkowity czas ruchu T dzieli się na małe równe przedziały: ∆ t . W każdym i-tym przedziale czasowym wyznacza się długość fragmentu toru
+∆
i
t
t
w oparciu o równanie prędkości oraz równania parametryczne toru: ∆ s = v
∫ h( t) dt ,
i
max
i
t
λ i
∆ s = ∫ x′ + y′ dλ . Porównując długości tych fragmentów toru wyznacza się i
( Q)2 ( Q)2
λ i 1−
parametr λ odpowiadającemu punktowi końcowemu rozpatrywanego fragmentu: i
+∆
λ
i
t
t
i
2
2
v
∫ h( t) dt = ∫ x′ + y′ dλ → λ
(4)
max
( Q) ( Q)
i
λ
i
t
i 1
−
Określenie λ umożliwia obliczenie z równań parametrycznych (1) współrzędnych punktu Q
i
i,
r
jakie powinien osiągać na końcu każdego przedziału czasowego. Wektor prędkości v jest i
nachylony pod kątem α :
i
dx
dx / dλ
x′
y′
cos
Q
Q
Q
α =
=
=
, sin
Q
α =
(5)
i
2
2
2
2
2
2
dx + dy
( dx / dλ) + ( dy / dλ)
x′ + y′
2
2
x′ + y′
Q
Q
Q
Q
Q
Q
λ
Q
Q
λ
i
i
Wartość prędkości Q na końcu i-tego przedziału na podstawie (2) wynosi: v = v h( t ) a jej
i
max
i
współrzędne:
x& = v cosα ,
y& = v sinα
(6)
i
Q
i
i
i
Q
i
i
Wyznaczenie dyskretnej postaci trajektorii punktu Q
Przyjmuje się T oraz wartość przyrostu czasowego t
∆ ∈( t
∆ ; t
∆
) tak, aby liczba
min
max
T
przedziałów, na jaką jest podzielony czas i tor ruchu: k =
była liczbą naturalną. Dyskretna
t
∆
postać trajektorii przedstawiona jest w poniższej tabeli:
Tab.1. Dyskretna postać trajektorii punktu Q.
wzór (4)
wzór (1)
wzór (6)
0
0
λ
x
y
0
0
0
0
Q
0
Q
Numer
Czas ruchu w końcu
λ
x
y
x&
y&
Q
Q
Q
Q
przedziału i-tego przedziału
1
t = t
∆
λ
x
y
x&
y&
1
1
1
Q
1
Q
1
Q
1
Q
2
t = 2 t
∆
λ
x
y
x&
y&
2
2
2
Q
2
Q
2
Q
2
Q
…
…
…
…
…
…
i
t = t + t
∆ = i ⋅ t
∆
λ
x
y
x&
y&
i
i 1
−
i
i
Q
i
Q
i
Q
2
Q
…
…
…
…
…
…
k
T = k ⋅ t
∆
λ
x
y
0
0
k
k
Q
k
Q
Mechanizm realizujący planowaną trajektorię
Mechanizm przedstawiony na rys.2. realizuje planowaną trajektorię. Składa się z dwóch napędów liniowych, których położenie określają s1 i s2. Punkt Q znajduje się na ogniwie 6, które jest podtrzymywane jest ruchomymi podporami 3, 4, i 5 o jednakowej długości l. Ogniwa 1, 3, 4 i 6 tworzą równoległobok co powoduje, że ogniwo 6 jest zawsze w pozycji poziomej.
W celu realizacji przez napędy założonej trajektorii punktu Q należy określić położenia zabieraków s1 i s2 w funkcji współrzędnych punktu Q oraz stałych wymiarów ogniw mechanizmu (rys.2):
y
Q
Dane:
l = 0,685 [m]
y
a = 0,120 [m]
Q
f
a
e
b = 0,080 [m]
c = 0,065 [m]
d = 0,052 [m]
3
6
5
e = -0,04 [m]
f = 0,015 [m]
l
l
4
l
1
2
a
x
Q
b
x
s
c
1
d
s
2
Rys.2. Mechanizm realizujący ruch punktu Q.
s = x − e − l − ( y − f )2
2
− c ,
s = x − e + l − y − f
+ b + d
(7)
2
Q
( Q )2
2
1
Q
Q
( y − f y&
( y − f y&
Q
)
Q
) Q
Q
s& = x& +
,
s& = x& −
(8)
1
Q
2
Q
2
l − ( y − f
2
l − ( y − f
Q
)
Q
)2
2
Do sterowania napędami potrzebna jest tabela sterowań współrzędnych s i s na 1
2
końcu i-tego przedziału, które wyznacza się na podstawie wzoru (7). Rzeczywisty sposób realizacji prędkości wzdłuż toru jest przedstawiony na rys.3. Przyjęcie dużej liczby przedziałów powoduje dokładniejszą realizację założonego ruchu punktu Q. Wyznaczenia tabeli sterowań jest realizowane w programie LabVIEW i nie jest elementem wykonania tego laboratorium.
W celu sprawdzenia czy wyznaczona trajektoria jest poprawna wyznacza się przebiegi prędkości napędów s& i s& za pomocą zależności (8). Warunkiem koniecznym jest osiąganie 1
2
na początku i końcu ruchu wartości prędkości równej zero oraz nie przekroczenie maksymalnych prędkości możliwych do zrealizowania w napędach.
zaplanowana
s
∆ i
&
prędkość napędu
s ≅
i
t
∆
s&
i
rzeczywista
prędkość napędu
∆ t
t
0
t
t
t
t
2
T
1
i 1
−
i
Rys.3. Rzeczywista realizacja przebiegu prędkości napędu.
Przykład z torem Q w postaci odcinka
Dane:
Współrzędne punktu początkowego: x
= 0 [ m], y = 0 [ m].
0
Q
0
Q
Współrzędne punktu końcowego:
x
= 0.2 [ m] , y = 0.4 [ m].
k
Q
0
Q
Całkowity czas ruchu: T = 0.5 [ s] .
2π
1− cos
t
T
Przyjęty przebieg prędkości wzdłuż toru: v ( t) = v
.
Q
max
2
Rozwiązanie:
y − y
x y − x y
Równanie prostej Q Q :
k
0
k
0
0
k
y =
x +
→
y = 2 x
0
k
x − x
x − x
k
0
k
0
Parametryczne równanie prostej Q Q : y = 2λ , x = λ
→
y′ = 2 ,
x′ = 1,
0
k
Q
Q
Q
Q
λ = 0,
λ = x = 0,2
0
k
k
Q
Długość toru na podstawie równań parametrycznych:
λ k
0.2
2
2
2
2
0.2
5
L = ∫ ( x′ + y′ dλ = ∫ ( )
1 + (2) dλ = 5λ
=
[m]
Q )
( Q)
λ
0
5
0
0
Długość toru na podstawie przebiegu prędkości:
2π
1− cos
t
T
0.5
0.5
T
v
T
2π
L = v
∫ h( t) dt = v ∫
dt = max t −
sin
t
=
max
max
2
2
2π
T
0
0
0
v
0.5
2π
1
max
=
0.5 −
sin
0.5 = v [ m]
2
2π
0.5
max
4
4 5
Porównanie wyliczonych długości umożliwia obliczenie: v
=
[ m / s] .
max
5
t
∆ =10 [ ms],
k = T / ∆ t = 0.5 / 0.01 = 50 .
Obliczenia w pierwszym przedziale:
0,01
λ
0,01
1
4 5
1
2π
2
0,5
2π
2
2
∫ (1−cos t) dt = ∫ 1 + 2 dλ ,
t −
sin
t
= λ ,
−5
λ =1,05⋅10 ,
5
2
T
1
5
2π
0,5
1
0
0
0
2
2
1
1
sinα =
=
= 0,8944 ,
cosα =
=
= 0,4472 ,
1
1
2
2
1 + 2
5
2
2
1 + 2
5
4 5 1
2π
v =
1− cos(
0, 01) = 0,00705 [ m / s] ,
1
5 2
0,5
x& = 0,00705 ⋅ 0.4472 = 0, 00315 [ m / s] , y& = 0, 00705⋅ 0.8944 = 0,00631[ m / s] .
1
Q
1
Q
wzór (4)
wzór (1)
wzór (6)
0
0
0
0
0
0
0
Numer
Czas ruchu w końcu
λ
x [m]
y [m]
x& [m/s]
y& [m/s]
Q
Q
Q
Q
przedziału i-tego przedziału [s]
1
0.01
5
1, 05 10−
⋅
5
1, 05 10−
⋅
5
2,10 10−
⋅
0, 00315 0, 00631
…
…
…
…
…
…
50
0.50
0.2
0.2
0.4
0
0
Budowa napędów
Rys.4. Schemat blokowy stanowiska
Stanowisko składa się z dwóch napędów liniowych zamieniających ruchy obrotowe wałków silników na ruchy liniowe zabieraków, do których przymocowane są ogniwa mechanizmu. Silnik indukcyjny pojedynczego napędu zasilany jest prądem zmiennym trójfazowym. W silniku jest zabudowany enkoder o wysokiej rozdzielczości (131072
imp/obr) wykorzystywany w pętli sprzężenia zwrotnego (niebieska linia na Rys.4). Prąd zmienny zasilający silnik wytwarzany jest w regulatorze, który modulując częstotliwość prądu decyduje o prędkości wałka silnika. Regulator zawiera wydajny procesor o częstotliwości odpowiedzi wynoszącej 550 Hz, dzięki czemu serwonapęd wykonuje bardzo szybko zadane pozycjonowanie. Silnik można sterować przez zadawanie: położenia, prędkości, momentu lub kombinacji dwóch z tych parametrów (na laboratorium
wykorzystywane jest sterowanie prędkością). Regulator komunikuje się z PC-tem za pomocą karty analogowo-cyfrowej PCI 7344 i elementów pośrednich (Listwa we/wy MR-TB50, Konektor UMI 7774). Procedura obliczeniowa w programie Labview generuje na podstawie danych wejściowych tabele sterowania dla obu serwosilników i przesyła ją za pośrednictwem karty pomiarowo-strerującej do regulatora. Regulator zamienia te dane na prąd o odpowiedniej częstotliwości zapewniający realizacje założonego ruchu i w razie potrzeby korektę położenia zabieraków.
W napędach stanowiska zainstalowane enkodery mierzą położenia wózków w zależności od czasu: s = s ( t) , s = s ( t) . Stabelaryzowane przebiegi s ( t) , s ( t) po zróżniczkowaniu w 1
1
2
2
1
2
programie Exel (funkacja „Reglinp”) są źródłem przebiegów s& ( t) , s& ( t) . Po przekształceniu 1
2
równań (7) i (8) można obliczyć: x& = g ( s& , s& , s , s ) , y& = g ( s& , s& , s , s ) . Są to składowe Q
x
1
2
1
2
Q
y
1
2
1
2
rzeczywistej prędkości punktu Q , które porównuje się do zaplanowanych (6).
2. Cel i zakres ćwiczenia
Praktyczna realizacja zaplanowanej trajektorii ruchu platformy 3. Przebieg ćwiczenia
1. Zaplanowanie trajektorii platformy.
2. Obliczenia toru i przebiegu prędkości platformy i przygotowanie tabel sterowań (Mathcad).
3. Weryfikacja obliczeń (LabView).
4. Wykonanie na stanowisku zaplanowanej trajektorii.
4. Wymagana zawartość sprawozdania
1. Schemat stanowiska.
2. Dane toru i przebiegu prędkości - wykresy.
3. Obliczenia sprawdzające rzeczywisty przebieg prędkości punktu Q.
5. Zagadnienia kontrolne
1. Schemat budowy stanowiska.
2. Przedstawienie równania w zmiennych kartezjańskich w postaci parametrycznej.
3. Sposób wyznaczania parametru λ .
4. Wyznaczenie położeń i prędkości napędów w zależności od współrzędnych toru i prędkości wzdłuż toru.
5. Wyznaczenie położeń i prędkości punktu Q w zależności od s& , s& , s , s .
1
2
1
2
6. Literatura
[1] Morecki A., Knapczyk J., Kędzior K.: „Teoria mechanizmów i manipulatorów”, WNT
Warszawa 2002.
[2] Gronowicz A. „Podstawy analizy układów kinematycznych”, OWPW Wrocław 2003.
Opracowanie: mgr inż. Artur Gawlik, dr inż. Grzegorz Tora