Podstawy Robotyki Określenie kinematyki oraz dynamiki manipulatora

Transkrypt

1 Podstawy Robotyki Określenie kinematyki oraz dynamiki manipulatora AiR V sem. Gr. A4/ Wicher Bartłomiej Pilewski Wiktor 9 stycznia 011 1

2 1 Wstęp Rysunek 1: Schematyczne przedstawienie manipulatora W poniższym sprawozdaniu przedstawiono zależności pozwalające opisać ruch manipulatora o trzech stopniach swobody. Wybrano manupilator RRP w konfiguracji. Konfigurację przestrzenną przedstawiono na rys. 1 Zmiennymi konfiguracyjnymi są: q = [Θ 1 Θ d 3 ] T. Pozostałe parametry (L 1 oraz L ) przyjmują wartości stałe.

3 Rysunek : Manipulator zrealizowany w oprogramowaniu Autodesk Inventor 3

4 Rysunek 3: Rysunek techniczny zrealizowanego manipulatora 4

5 Kinematyka prosta Celem tej części sprawozdania jest określenie położenia końcówki manipulatora w układzie xyz0 na podstawie geometrii samego manipulatora oraz wartości zmiennych konfiguracyjnych. W celu zapisania macierzy przejść między poszczególnymi układami odniesienia, wykorzystano notację Denavita - Hartenberga, wyniki zestawiono w tabeli 1. L.p. Θ d α a π 1 Θ 1 l 1 0 Θ 0 π d 3 + l 0 0 Na podstawie tabeli 1, zapisano macierze przekształceń układów. Macierz przejścia z układu k do układu m oznaczono następująco: A m k. Macierze przejścia przedstawiono poniżej (wzory () - (4)). W celu uproszczenia zapisu zastosowano następujące skróty: cos(θ n ) = c n, cos(θ n + Θ m ) = c nm. Analogicznie postąpiono dla funkcji sin(x). A 1 0 = A 0 = A 3 0 = c 1 0 s 1 0 s 1 0 c l c 1 s s 1 c 1 s 0 s 1 c c 1 s 1 s 0 s 0 c l c 1 s s 1 c 1 s c 1 s (d 3 + l ) s 1 c c 1 s 1 s s 1 s (d 3 + l ) s 0 c c (d 3 + l ) + l Ostatecznie, położenie końcówki manipulatora w układzie xyz0 dane jest następującym wektorem: p x p z c 1 s (d 3 + l ) s 1 s (d 3 + l ) c (d 3 + l ) + l 1 (1) () (3) (4) (5) 5

6 Rysunek 4: Prezentacja dwóch rozwiązań dla tego samego punktu 3 Kinematyka odwrotna Celem tej części sprawozdania jest wyznaczenie zależności współrzędych konfiguracyjnych manipulatora Θ 1, Θ oraz d 3 gdy dane jest położenie końcówki manipulatora. Sprowadza się to do wykonania odpowiednich przekształceń algebraiczynych wzoru (5). Dla każdego punktu można podać rozwiązania, jedno dla Θ > 0, i drugie dla Θ < 0. Zademonstrowano to na rysunku 4. Dla Θ > 0 uzyskano następujący rezultat: Θ 1 Θ d 3 atan ( ) p x atan ( p x (p z l 1 )cos(θ 1 ) p z l 1 cos(θ ) + l ) (6) Dla każdego punktu z poza tych pokrywających się z osią Z, możliwe jest drugie rozwiązanie: 6

7 Θ 1od Θ od d 3od Θ Θ d 3 (7) Wzór (6) nie wyczerpuje jednak wszystkich możliwości określenia kinematyki odwrotnej. Konieczne okazuje się rozpatrzenie dwóch przypadków: 1. Gdy θ 1 = π ; wówczas kinematyka prosta opisana jest zależnościami: p x p z 0 s (d 3 + l ) c (d 3 + l ) + l 1 Co po przekształceniach daje zmienne konfiguracyjne (dla θ 0): Θ 1 Θ d 3 Jeśli zaś θ = 0 i jednocześnie θ 1 = π p x p z π atan ( (p z l 1 ) sinθ l ) (8) (9) to kinematyka prosta dana wektorem: 0 0 d 3 + l + l 1 (10) Skąd kinematyka odwrotna dla tego przypadku opisana jest zależnością: Θ 1 Θ d 3 π 0 p z (l 1 + l ). Gdy θ = π ; wówczas kinematyka prosta opisana jest zależnościami: p x p z c 1 (d 3 + l ) s 1 (d 3 + l ) l 1 (11) (1) Skąd przy założeniu, że θ 1 0 kinematyka odwrotna dana jest wektorem: Θ 1 Θ d 3 atan ( p x ) π sinθ 1 l (13) Jeśli θ 1 = 0 i jednocześnie θ = π, to kinematykę prostą można opisać zależnościami: p x p z (d 3 + l ) 0) l Kinematyka odwrotna dla tego przypadku dana jest wzorami: 7 (14)

8 Θ 1 Θ d 3 0 π l p x (15) 8

9 4 Jakobian W celu wyznaczenia prędkości końcówki manipulatora w układzie xyz0, gdy znana jest prędkość zmian współrzędych konfiguracyjnych q = [ Θ 1 Θ d 3 ] T konieczne jest znalezienie macierzy przejścia z układu związanego z końcówką manipulatora do układu xyz0. Jakobian przekształcenia jest macierzą o 6 wierszach oraz 3 kolumnach. Daną wzorem: J0 3 = 5 Obliczanie dynamiki s 1 s (d 3 + l ) c 1 c (d 3 + l ) c 1 s c 1 s (d 3 + l ) s 1 c (d 3 + l ) s 1 s 0 s (s 1 c 1)(d 3 + l ) c 0 s c Kolejnym etapem jest znalezienie równań dynamiki przedstawionego manipulatora, do tego celu wykorzystano równania lagrange a: (16) d δl L = τ i (17) dt δq i q i Gdzie L = K V, czyli energia kinetyczna pomniejszona o energię potencjalną, a τ i jest uogulnioną siłą wymuszającą. Wzór na energię kinetyczną danego elemenu łańcucha kinematycznego ma postać: K 1 = 1 qt [m i J T vcij vci + J T wcir i 0I 1 R i 0T Jwci ] q (18) Należy więć wyznaczyć jakobiany środków masy poszczególnych elementów ramienia robota. 5.1 Jakobiany środków masy Jakobian środka masy pierwszego elementu łańcucha kinematycznego dany jest on wzorem: [ ] ẑ0 (o J c1 = c1 o 0 ) 0 0 (19) ẑ Gdzie o c1, a o 0 jest pozycją zerowego układu współrzędnych. Po podstawieniu odpowiednich wartości otrzymujemy, że J vc1 (górna częsć jakobianu- odpowiadająca za prędkości) jest macierzą zer, natomiast jakobian prędkości kątowych jest równy: J wc1 = (0) Postępując analogicznie dla drugiego i trzeciego przegubu otrzymujemy: 9

10 J c = J c3 = Przy czym r = l + d 3 1 d max. 1s 1s l 1c 1c l 0 1c 1c l 1s 1c l 0 0 1l (s 1s c 1s ) 0 0 s c s 1 s r c 1 c r c 1 s c 1 s r s 1 c r s 1 s 0 s r(s 1 c 1) c 0 s c (1) () 5. Obliczanie energji kinetycznej Korzystając ze wzoru (9) dla wszystkich przegubów, po zsumowaniu energii K 1 +K +K 3 otrzymujemy wzór w postaci: K = 1 qt d 11 d 1 d 13 d 1 d d 3 d 31 d 3 d 33 q (3) Ze względu na objętość macierzy potrzebnej od obliczenia energii kinetycznej, zdecydowano się na skrótowy jej zapis. Poszczególne elementy przedstawiono poniżej. d 11 = I 1x +I z +I 3z +I x s I z s +I 3x s I 3z s +(l m s ) )/4+m 3 r s ) (4) d 1 = (c 1 s 1 s ( m c l 4m 3 c r +I xx c +4I zz c +I 3xx c +4I 3zz c I xx s 1 I 3xx s 1 ))/ (5) d 13 = c 1 c s (I zz c I xx s 1 ) (6) d 1 = (c 1 s 1 s ( m c l 4m 3 c r +I xx c +4I zz c +I 3xx c +4I 3zz c I xx s 1 I 3xx s 1 ))/ (7) d = I yy + I 3yy + (l m )/4 + m 3 r + I xx s 1 I xx s I xx s 5 1 I xx s I 3xx s 1 I 3xx s I 3xx s 5 1 I 3xx s 6 1 I xx s 1s + I xx s 4 1s + 4I zz s 1s 4I zz s 4 1s I 3xx s 1s + I 3xx s 4 1s + 4I 3zz s 1s 4I 3zz s 4 1s + 4I xx sin(θ /) s 3 1 4I xx sin(θ /) s 5 1 (8) + 4I 3xx sin(θ /) s 3 1 4I 3xx sin(θ /) s 5 1 l m s 1s + l m s 4 1s 4m 3 r s 1s + 4m 3 r s 4 1s 10

11 d 3 = c s 1 (I yy (c 1 1) I yy c 1 + I xx c 1(c 1 1) + I zz c 1(c 1) + I xx c 1c s 1 ) (9) d 31 = c 1 c s (I zz c I xx s 1 ) (30) d 3 = c s 1 (I yy (c 1 1) I yy c 1 + I xx c 1(c 1 1) + I zz c 1(c 1) + I xx c 1c s 1 ) (31) d 33 = m 3 + I yy c + I xx c 1c I xx c 4 1c I yy c 1c + I zz c 1c I zz c 1c 4 (3) 5.3 Obliczanie energji potencjalnej W układzie jedyną siłą zachowawczą jest oddzialywanie grawitacyjne. Wektor przyspieszenia grawitacyjnego ma kierunek równoległy a zwrot przeciwny do osi z. Z tego powodu energia potencjalna elementu równa się iloczynowi jego masy i z-towej składowej pozycji środka masy. Energia potencjalna opisana jest wzorem: V = 1 m 1l 1 + m ( 1 c l + l 1 ) + m 3 (c (l + d 3 1 d max) + l 1 ) (33) Należy następnie obliczyć pochodne cząskowe po zmiennych stanu: δv δθ 1 = 0 (34) δv δθ = m s l m 3 s (l + d 3 1 d max) (35) δv δd 3 = m 3 c (36) Co potrzebne jest do zapisania równania dynamiki (następny podrozdział). 5.4 Równanie końcowe dynamiki Równanie końcowe dynamiki zapisano w skróconej formie z wykorzystaniem symboli Christofella. τ k = δv δq k + j d kj q j + ij c ijk q i q j (37) Gdzie d kj są elementami macierzy K podanej w rozdziale XXX. Natomiast cijk dane jest wzorem: c ijk = 1 (δd kj δq i Poszczególne elementy dane są wzorami: + δd ki δq j δd ij δq k ) (38) c 111 = 0 (39) 11

12 c 11 = (sin(θ )(m l + 4m 3 r + 4I xx 4I zz + 4I 3xx 4I 3zz ))/8 (40) c 131 = 0 (41) c 11 = (sin(θ )(m l + 4m 3 r + 4I xx 4I zz + 4I 3xx 4I 3zz ))/8 (4) c 1 = (c 1 s 1 (I xx + I 3xx 6I xx s 1 6I 3xx s 1 + I xx c + 4I zz c + I 3xx c + 4I 3zz c + 10I xx s 3 1 4I xx s 6I xx s I zz s + 10I 3xx s 3 1 4I 3xx s 6I 3xx s I 3zz s + 1I xx sin(θ /) s 1 + 1I 3xx sin(θ /) s 1 + 4I xx s 1s 16I zz s 1s + 4I 3xx s 1s (43) 16I 3zz s 1s I xx c s 1 I 3xx c s 1 0I xx sin(θ /) s 3 1 0I 3xx sin(θ /) s 3 1 l m c 4m 3 r c l m s 4m 3 r s + 4l m s 1s + 16m 3 r s 1s ))/ c 31 = (c 1 (I xx c I yy c + 6I zz c I xx s 1 7I zz c 3 + 6I zz c 1c 3 7I xx c 1c + 5I xx c 4 1c 6I zz c 1c + 4I xx c 1c s 1 ))/ (44) c 311 = 0 (45) c 31 = (c 1 (I xx c I yy c + 6I zz c I xx s 1 7I zz c 3 + 6I zz c 1c 3 7I xx c 1c + 5I xx c 4 1c 6I zz c 1c + 4I xx c 1c s 1 ))/ (46) c 331 = c 1 c s 1 (I xx c 1 + I zz c I xx + I yy I zz ) (47) c 11 = (s (4I xx s 1 1I 3zz c 1I zz c + 4I 3xx s 1 + 3l m c + 1m 3 r c + 8I xx c 1c + 16I zz c 1c + 8I 3xx c 1c + 16I 3zz c 1c 1I xx c 1s 1 1I 3xx c 1s 1 4l m c 1c 16m 3 r c 1c ))/4 (48) c 1 = c 1 s 1 (I xx + I 3xx 3I xx s 1 3I 3xx s 1 + 5I xx s 3 1 I xx s 3I xx s I zz s + 5I 3xx s 3 1 I 3xx s 3I 3xx s I 3zz s + 6I xx sin(θ /) s 1 + 6I 3xx sin(θ /) s 1 + I xx s 1s 8I zz s 1s + I 3xx s 1s (49) 8I 3zz s 1s 10I xx sin(θ /) s I 3xx sin(θ /) s 3 1 l m s 4m 3 r s + l m s 1s + 8m 3 r s 1s ) c 13 = (c 1 (I xx c I yy c + 6I zz c I xx s 1 7I zz c 3 + 6I zz c 1c 3 7I xx c 1c + 5I xx c 4 1c 6I zz c 1c + 4I xx c 1c s 1 ))/ (50) 1

13 c 1 = c 1 s 1 (I xx + I 3xx 3I xx s 1 3I 3xx s 1 + 5I xx s 3 1 I xx s 3I xx s I zz s + 5I 3xx s 3 1 I 3xx s 3I 3xx s I 3zz s + 6I xx sin(θ /) s 1 + 6I 3xx sin(θ /) s 1 + I xx s 1s 8I zz s 1s + I 3xx s 1s 8I 3zz s 1s (51) 10I xx sin(θ /) s I 3xx sin(θ /) s 3 1 l m s 4m 3 r s + l m s 1s + 8m 3 r s 1s ) c = s 1(s 1 1)(m c s l + 4m 3 c s r + I xx c s 4I zz c s + I 3xx c s 4I 3zz c s I xx cos(θ /)sin(θ /)s 1 I 3xx cos(θ /)sin(θ /)s 1 ) (5) c 3 = 0 (53) c 31 = (c 1 (I xx c I yy c + 6I zz c I xx s 1 7I zz c 3 + 6I zz c 1c 3 7I xx c 1c + 5I xx c 4 1c 6I zz c 1c + 4I xx c 1c s 1 ))/ (54) c 3 = 0 (55) c 33 = c s (I yy + I xx c 1 I xx c 4 1 I yy c 1 + I zz c 1 I zz c 1c ) (56) c 113 = s (sin(θ /) 1)(I xx (s 1 1) + I xx s 1 + I zz s 1 (sin(θ /) 1)) (57) c 13 = (c 1 (I xx c I yy c + I zz c + I xx s 1 I zz c 3 + 6I zz c 1c 3 7I xx c 1c + 5I xx c 4 1c 6I zz c 1c 4I xx c s 1 + 4I xx c 1c s 1 ))/ (58) c 133 = c 1 c s 1 (I xx c 1 + I zz c I xx + I yy I zz ) (59) c 13 = (c 1 (I xx c I yy c + I zz c + I xx s 1 I zz c 3 + 6I zz c 1c 3 7I xx c 1c + 5I xx c 4 1c 6I zz c 1c 4I xx c s 1 + 4I xx c 1c s 1 ))/ (60) c 3 = s 1 s (I yy + I xx c 1 I xx c I zz c 1 6I zz c 1c I xx c 1c s 1 ) (61) c 33 = c s (I yy + I xx c 1 I xx c 4 1 I yy c 1 + I zz c 1 I zz c 1c ) (6) c 313 = c 1 c s 1 (I xx c 1 + I zz c I xx + I yy I zz ) (63) c 33 = c s (I yy + I xx c 1 I xx c 4 1 I yy c 1 + I zz c 1 I zz c 1c ) (64) c 333 = 0 (65) 13

14 6 Materiały pomocnicze Niniejsze sprawozdanie zostało wykonane w oparciu o następujące pomoce: 1. Własnoręczne notatki z przedmiotu Podstawy robotyki (wykłady + ćwiczenia);. demonstracyjna wersja oprogramowania Inventor. 14