Podstawy robotyki wykład V. Jakobian manipulatora. Osobliwości

Transkrypt

1 Podstawy robotyki Wykład V Jakobian manipulatora i osobliwości Robert Muszyński Janusz Jakubiak Instytut Informatyki, Automatyki i Robotyki Politechnika Wrocławska

2 Metoda bezpośrednia uzyskania macierzy Jacobiego Jeśli kinematyka manipulatora dana jest równaniem: y = k(q), y Y, q Q, wtedy po zróżniczkowaniu względem czasu otrzymujemy ẏ = k (q) q q wyznaczyć kinematykę manipulatora k(q), wybrać współrzędne, wyrazić kinematykę w wybranych współrzędnych, formalnie zróżniczkować kinematykę k k q.

3 Metoda pośrednia uzyskania macierzy Jacobiego Dla kinematyki danej w postaci [ ] T0 n R n = 0 d0 n 0 1 szukamy zależności prędkośći liniowej i kątowej efektora od prędkości w poszczególnych przegubach w postaci ẏ = J q, gdzie J jakobian manipulatora. Szukamy jakobianu J = [ ] Jv J ω v0 n = J v q ω0 n = J ω q

4 Metoda pośrednia uzyskania macierzy Jacobiego Prędkość liniowa Prędkość kątowa v n 0 = ḋ n 0 R n 0 (R n 0 ) T = I dr0 n dq (Rn 0 ) T + R0 n d(r0 n)t dq = 0 Zdefiniujmy Wówczas S drn 0 dq (Rn 0 ) T. S T = ( dr n ) T 0 dq (Rn 0 ) T = R0 n d(r0 n)t. dq

5 Metoda pośrednia... prędkość kątowa Mamy więc S drn 0 dq (Rn 0 ) T S T = R n 0 d(r n 0 )T dq S + S T = 0 0 ω 3 ω 2 S(ω0) n = ω 3 0 ω 1, ω n 0 = ω 2 ω 1 0 S(ω n 0)p = ω n 0 p ω 1 ω 2. ω 3

6 Metoda pośrednia... prędkość kątowa ω przegub rotacyjny ω i i 1 = q i k, k = [0, 0, 1] T przegub translacyjny ω i i 1 = 0

7 Metoda pośrednia... prędkość kątowa Można pokazać, że W naszym przypadku ω n 0 = ω R 1 0 ω R n 1 0 ω n n 1. ω n 0 = ρ 1 q 1 k + ρ 2 q 2 R 1 0 k ρ n q n R n 1 0 k = n ρ i q i z i 1, i=1 gdzie z i 1 = R0 i 1 k trzecia kolumna macierzy R i 1 { 0 przegub translacyjny ρ i =. 1 przegub rotacyjny 0,

8 Metoda pośrednia... prędkość liniowa Wyliczając różniczkę zupełną ḋ n 0 otrzymujemy ḋ n 0 = n i=1 d n 0 q i q i. Zatem w j-tej kolumnie macierzy J v znajduje się wektor dn 0 q i. Przypomnijmy [ ] R i i 1 di 1 i = Rot(z 0 1 i 1, θ i )Trans(z i 1, d i ) c θi s θi c αi s θi s αi a i c θi Trans(x i 1, a i )Rot(x i 1, α i ) = s θi c θi c αi c θi s αi a i s θi 0 s αi c αi d i

9 Metoda pośrednia... prędkość liniowa Stąd d i i 1 = d i k + a i R i i 1i = q i k + a i R i i 1i. Przy poruszanym tylko i-tym przegubie ḋ0 n = R0 i 1 ḋi 1 i = q i R0 i 1 k = q i z i 1. A zatem d n 0 q i = z i 1

10 Metoda pośrednia... prędkość liniowa przegub rotacyjny z 0 z i-1... y 0 x 0... d n 0 = d i 1 o + R i 1 0 d n i 1 Ponieważ d0 i 1, R0 i 1 niezależna od q i ḋ n 0 = R i 1 0 ḋ n i 1. Prędkość liniowa powodowana ruchem q iḋ i 1 n = q i k di 1. n

11 Metoda pośrednia... prędkość liniowa Stąd ḋ n 0 = R i 1 0 ( q i k d n i 1) = Mamy zatem q i R i 1 0 k R i 1 0 d n i 1 = q i z i 1 (d n 0 d i 1 0 ). d n 0 q i = z i 1 (d n 0 d i 1 0 )

12 Metoda pośrednia... podsumowanie Podsumowując jakobian dany jest w postaci ] J = [J 1 J 2... J n gdzie i-ta kolumna J i = [ zi 1 (d n 0 d i 1 z i 1 ] 0 ) dla przegubu rotacyjnego lub J i = dla przegubu translacyjnego. [ ] zi 1 0

13 Związek pomiędzy jakobianem a macierzą Jacobiego Po wybraniu współrzędnych w przestrzeni zewnętrznej jako współrzędne kartezjańskie + kąty orientacji dostajemy [ ] I 0 k J(q) = 0 M(y) q (q) [ ] gdzie M(y) = m 1 (y) m 2 (y) m 3 (y), m i (y) wektory jednostkowe osi obrotu wokół których mierzone są kąty orientacji. Np. dla kątów Eulera M(y) = określone dla 0 < θ < π. cos φ ctg θ sin φ ctg θ 1 sin φ cos φ 0 cos φ sin θ sin φ sin θ 0

14 Metoda jakobianowa rozwiązania OZK Mamy dane ẏ = k q q, y(t), t [t 0, t k ]. (1) Szukamy q(t) dla t [t 0, t k ]. W tym celu wyliczamy y(t 0 ) i przy użyciu algorytmu Newtona znajdujemy q(t 0 ). Następnie rozwiązujemy równanie różniczkowe q = z warunkiem początkowym q(t 0 ). ( ) k 1 ẏ (2) q

15 Metoda jakobianowa Inwers uogólniony Równania (1) są postaci y (m 1) = A (m n) q (n 1). Przy danym y szukamy q, gdy m n, ranka = m. Będziemy szukali rozwiązania szczegółowego minimalizującego energię H(q) = 1 q, q 2 przy ograniczeniach y = Aq. Tworzymy funkcję Lagrange a L(q) = 1 2 q, q +λt (y Aq) = 1 q, q + λ, y Aq. 2 Z warunków na minimum mamy L q = qt λ T A = 0 q = A T λ.

16 Metoda jakobianowa inwers uogólniony Stąd co prowadzi do λ = y = AA T λ ( AA T ) 1 y. (3) Wstawiając (3) do wyniku minimalizacji dostajemy q = A T ( AA T ) 1 y = A + y. Macierz A + = A T ( AA T ) 1 nazywa się pseudoinwersem Moore a Penrose a.

17 Metoda jakobianowa inwers uogólniony Równania y = Aq będą spełnione, jeżeli do rozwiązania szczegółowego q = A + y dodamy element z taki, że Az = 0, co oznacza, że z KerA. Stąd rozwiązanie ogólne lub przy ostatecznie q = A + y + z, z = (I A + A)ω, z KerA, ω R n q = A + y + (I A + A)ω, ω R n.

18 Metoda jakobianowa inwers uogólniony Sprawdźmy y = Aq = AA + y + (A AA }{{ + A} )ω = Iy + 0 = y. A Skoro ω dowolne, dobierzmy je tak by optymalizować np. manipulowalność ( ) ω = grad q det(aa T ).

19 Metoda jakobianowa rozwiązania OZK. Podsumowanie. Przy równaniu y = k(q), y Y, q Q, (4) dim Y = m, dim Q = n, i danym y d (t) dla t [t 0, t k ] (lub y d ) znaleźć q(t) (lub q) takie, że (4) spełnione dla y d (t) (lub y d ).

20 Metoda jakobianowa rozwiązania OZK. Podsumowanie. gdy n = m i macierz Jacobiego nieosobliwa: q(t) = ( k q (q(t)) ) 1 ẏ(t), k(q(t 0 )) = y(t 0 ), gdy n m i rank k q (q(t)) = m: q(t) = lub ogólniej ( k q (q(t)) ) + ẏ(t), k(q(t 0 )) = y(t 0 ), ( ) k + q(t) = q (q(t)) ẏ(t)+ ( ( ) ) k + I q (q(t)) k q (q(t)) ω(t).

21 Metoda jakobianowa rozwiązania OZK. Podsumowanie. Metoda Newtona gdy n = m i macierz Jacobiego nieosobliwa: ( ) k 1 q k+1 = q k + q (q k) (y d k(q k )), gdy n m i rank k q (q(t)) = m: ( ) k + q k+1 = q k + q (q k) (y d k(q k )), ( ) k + q (q k) = ( ) ( k T k q q ( ) ) k T 1 (q k ) q

22 Osobliwości Macierz Jacobiego kinematyki manipulatora k q (q) określa zależność ẏ = k (q) q, y Y, q Q, dim Y = m, dim Q = n, q co można zapisać jako dy = k q (q)dq, gdzie różniczki dy i dq określają kierunki w Y i Q. Jeśli więc rank k (q) < min(n, m) q istnieją w Y kierunki, które nie mogą zostać wygenerowane przez jakiekolwiek kierunki w Q.

23 Osobliwości Definicje Punktem regularnym kinematyki k nazywamy taką konfigurację q Q, że rank k (q) = min(n, m). q Punktem osobliwym nazywamy takie q Q, że rank k (q) < min(n, m). q Niech q Q będzie konfiguracją osobliwą. Wówczas dy = k q (q)dq, oraz Ker k q (q) 0. Oznacza to, że dq 0 która daje zerową zmianę dy.

24 Osobliwości W otoczeniu osobliwości: ruch w pewnych kierunkach w Y jest nieosiągalny; pewne ruchy w Q nie powodują ruchu w Y; ograniczone prędkości efektora mogą wymagać nieograniczonych prędkości w przegubach; może nie istnieć jednoznaczne rozwiązanie OZK; duże siły na efektorze mogą powodować niewielkie siły w przegubach.

25 Statyka Przy zależności wirtualna praca układu dy = k q (q)dq, dw = F T dy τ T dq, gdzie F wektor sił i momentów na chwytaku, τ wektor momentów/sił w przegubach, wyraża się jako dw = ( F T k ) q (q) τ T dq, co równe jest zeru jeśli manipulator pozostaje w równowadze. Stąd τ = ( k q (q) ) T F,