Realizacja zaplanowanej trajektorii mechanizmu płaskiego o dwóch stopniach ruchliwości 

INSTYTUT KONSTRUKCJI MASZYN

LABORATORIUM

TEORII

MECHANIZMÓW I MASZYN

ZAKŁAD TEORII MECHANIZMÓW I MANIPULATORÓW

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:

( )

Q y

y = f λ , x_Q = f_x( )λ (1) Tor ograniczają punkt początkowy Q0 (x ,Q₀ y ) i końcowy QQ0 k (x ,Qk y ). Parametr Qk λ zmienia się w zakresie:

λ λ λ

∈( ;_{0 k}). Współrzędne punktów Q0 i Qk wynoszą: yQ₀ = fy(

λ

0),

0 ( 0) Q x x = f

λ

, ( ) k Q y k y = f

λ

, ( ) k Q x k

x = f

λ

. Przebieg prędkości wzdłuż toru powinien zaczynać się i kończyć od zerowej wartości, zatem można go przedstawić za pomocą wyrażenia:

max ( )

v= =s& v h t ₍₂₎

gdzie:

0≤ ≤t T, h(0)=0, ( )h T =0, 0≤h t( ) 1≤ , T – całkowity czas ruchu.

Całkowitą długość toru można wyznaczyć z równania parametrycznego. Długość elementarna toru ds= (dx_Q2+dy_Q2)zawiera dxQ i dyQ , które wyraża się jako:

( ) Q Q Q dx dx d x d d

λ

_λ

_λ

λ

′ = = , ( ) Q Q dy dy d y d d

λ

_λ

_λ

λ

′

= = . Ostatecznie długość toru wynosi:

( ) ( )

0 2 2 k Q Q L x y d λ λ

λ

′ ′ =

_∫

+ . Na

podstawie przebiegu prędkości wzdłuż toru wyznacza się jego długość: _max 0 ( ) T L=v

∫

h t dt. y x tor ruchu punktu Q

Q0 Qk 0 Q x k Q x 0 Q y k Q y

Rys.1. Trajektoria punktu Q. s prędkość punktu Q Q dy Q dx ds

α

Porównując do siebie wyrażenia na L otrzymuje się równanie, z którego zadając T wyznacza się vmax:

( ) ( )

0 2 2 max max 0 ( ) k T Q Q v h t dt x y d v λ λ

λ

′ ′ = + →

∫

∫

(3)

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 w oparciu o równanie prędkości oraz równania parametryczne toru: _max ( )

i i t t i t s v h t dt +∆ ∆ =

_∫

,

( ) ( )

1 2 2 i i i Q Q s x y d λ λ

λ

− ′ ′

∆ =

_∫

+ . Porównując długości tych fragmentów toru wyznacza się parametr

λ

_i odpowiadającemu punktowi końcowemu rozpatrywanego fragmentu:

( ) ( )

1 2 2 max ( ) i i i i t t Q Q i t v h t dt x y d λ λ

λ

λ

− +∆ ′ ′ = + →

∫

∫

(4)

Określenie

λ

_i umożliwia obliczenie z równań parametrycznych (1) współrzędnych punktu Qi, jakie powinien osiągać na końcu każdego przedziału czasowego. Wektor prędkości vr_i jest nachylony pod kątem

α

_i:

2 2 2 2 2 2 / cos ( / ) ( / ) i Q Q Q i Q Q Q Q Q Q dx dx d x dx dy dx d dy d x y λ λ α λ λ ′ = = = ′ ′ + + + , sin 2 2 i Q Q Q y x y λ α = ′ ′ + ′ (5) Wartość prędkości Q na końcu i-tego przedziału na podstawie (2) wynosi: vi =vmaxh t( )i a jej współrzędne: cos i Q i i x& =v α _{, sin} i Q i i y& =v α ₍₆₎

Wyznaczenie dyskretnej postaci trajektorii punktu Q

Przyjmuje się T oraz wartość przyrostu czasowego ∆ ∈ ∆t ( t_min;∆t_max) tak, aby liczba przedziałów, na jaką jest podzielony czas i tor ruchu: k T

t

=

∆ była liczbą naturalną. Dyskretna 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 λ₀ 0 Q x 0 Q y 0 0 Numer przedziału

Czas ruchu w końcu i-tego przedziału λ x _Q y _Q x& _Q y& _Q 1 t₁= ∆t λ₁ 1 Q x 1 Q y 1 Q x& 1 Q y& 2 t₂ = ∆2 t λ₂ 2 Q x 2 Q y 2 Q x& 2 Q y& … … … … i t_i=t_i−1+ ∆ = ⋅ ∆t i t λ_i i Q x i Q y i Q x& 2 Q y& … … … … k T = ⋅∆k t λ_k k Q x k Q y 0 0

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

(

)

2 2 1 Q Q s =x − −e l − y − f −c, s₂ =x_Q− +e l2−

(

y_Q− f

)

2 + +b d (7)

(

)

(

)

1 ₂ 2 Q Q Q Q y f y s x l y f − = + − − & & & _,

(

)

(

)

2 ₂ 2 Q Q Q Q y f y s x l y f − = − − − & & & ₍₈₎

Do sterowania napędami potrzebna jest tabela sterowań współrzędnych s₁ i s₂ na 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 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.

l

l

l

a

a

_b

c

1

s

2

s

d

e

f

x

y

Q

x

Q

Rys.2. Mechanizm realizujący ruch punktu Q. 1 6 3 2 4 5 Q

y

Dane: l = 0,685 [m] a = 0,120 [m] b = 0,080 [m] c = 0,065 [m] d = 0,052 [m] e = -0,04 [m] f = 0,015 [m]

Przykład z torem Q w postaci odcinka Dane:

Współrzędne punktu początkowego:

0 0 [ ] Q x = m , 0 0 [ ] Q y = m . Współrzędne punktu końcowego: 0.2 [ ]

k Q x = m , 0 0.4 [ ] Q y = m . Całkowity czas ruchu: T =0.5 [ ]s .

Przyjęty przebieg prędkości wzdłuż toru: max

2 1 cos ( ) 2 Q t T v t v

π

  −     = . Rozwiązanie: Równanie prostej Q Q : _{0 k} 0 0 0 0 0 k k k k k y y x y x y y x x x x x − − = + → − − y=2x

Parametryczne równanie prostej Q Q : _{0 k} y_Q=2λ, x_Q=λ → y_Q′ =2, x_Q′ =1, 0 0

λ = , 0, 2 k

k xQ

λ = = Długość toru na podstawie równań parametrycznych:

( ) ( )

0 2 2 k Q Q L x y d λ λ λ ′ ′ =

_∫

+ =

( ) ( )

0.2 2 2 0 1 + 2 d

λ

∫

= 0.2 0 5λ = 5 5 [m] Długość toru na podstawie przebiegu prędkości:

max 0 ( ) T L=v

∫

h t dt= 0.5 max 0 2 1 cos 2 t T v dt

π

  −    

∫

= 0.5 max 0 2 sin 2 2 v T t t T

π

π

 ₋           = max 0.5 2 0.5 sin 0.5 2 2 0.5 v

π

π

   =  −      = max 1 [ ] 4v m Porównanie wyliczonych długości umożliwia obliczenie: _max 4 5[ / ]

5 v = m s . 10 [ ] t ms ∆ = , k =T/∆ =t 0.5 / 0.01 50= . s& t T 1 t t₂ 0 t ∆ 1 i t₋ t_i i s&

Rys.3. Rzeczywista realizacja przebiegu prędkości napędu. zaplanowana prędkość napędu rzeczywista prędkość napędu i i s s t ∆ ≅ ∆ &

Obliczenia w pierwszym przedziale: 1 0,01 2 2 0 0 4 5 1 2 (1 cos ) 1 2 5 2 T t dt d λ

π

_λ

− = +

∫

∫

, 0,01 1 0 2 0,5 2 sin 5 t 2 0,5t

π

_λ

π

− = , λ₁=1, 05 10⋅ −5, 1 _{2 2} 2 2 sin 0,8944 5 1 2 α = = = + , 1 _{2 2} 1 1 cos 0, 4472 5 1 2 α = = = + , 1 4 5 1 2 1 cos( 0, 01) 0, 00705 [ / ] 5 2 0,5 v = _ −

π

_= m s   , 1 0, 00705 0.4472 0, 00315 [ / ] Q x& = ⋅ = m s _, 1 0, 00705 0.8944 0, 00631[ / ] Q y& = ⋅ = m s _. wzór (4) wzór (1) wzór (6) 0 0 0 0 0 0 0 Numer przedziału

Czas ruchu w końcu i-tego przedziału [s] λ xQ[m] yQ[m] x&Q[m/s] y&Q[m/s] 1 0.01 5 1, 05 10⋅ − 1, 05 10⋅ −5 2,10 10⋅ −5 0, 00315 0, 00631 … … … … 50 0.50 0.2 0.2 0.4 0 0 Budowa napędów

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 ₂( ) programie Exel (funkacja „Reglinp”) są źródłem przebiegów s t&₁( )_,

2( )

s t& _{. Po przekształceniu}

równań (7) i (8) można obliczyć: x&_Q =g s s s s_x( , , ,& &_{1 2 1 2})_,

1 2 1 2 ( , , , )

Q y

y& =g s s s s& & _{. Są to składowe}

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& &₁, , ,_{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