Dynamika manipulatora. Robert Muszyński Janusz Jakubiak Instytut Cybernetyki Technicznej Politechnika Wrocławska. Podstawy robotyki wykład VI

Transkrypt

1 Podstawy robotyki Wykład VI Robert Muszyński Janusz Jakubiak Instytut Cybernetyki Technicznej Politechnika Wrocławska

2 Dynamika opisuje sposób zachowania się manipulatora poddanego wymuszeniu w postaci momentów/sił napędowych Proste zadanie dynamiki, Odwrotne zadanie dynamiki, τ q q τ D(q) q + C(q, q) q + G(q) = τ, Formalizmy do wyprowadzania równań dynamiki: 1. Newtona-Eulera 2. Eulera-Lagrange a 3. Kane a

3 Porównanie formalizmów wyprowadzania równań dynamiki Eulera-Lagrange a: systematyczny, wyprowadzone równania ruchu są w zwartej, analitycznej formie, wygodnej przy projektowaniu układu sterowania umożliwia efektywne dodanie złożonych efektów mechanicznych, takich jak elastyczności przegubów/ramion Newtona-Eulera: ze swej natury rekurencyjny, efektywny obliczeniowo

4 Założenia metody Newtona-Eulera Prawa mechaniki newtonowskiej wykorzystywane w formaliźmie Newtona Eulera: 1. każda akcja wywołuje reakcję; 2. prędkość zmian pędu ciała jest równa sumie sił przyłożonych do niego; 3. prędkość zmian popędu ciała jest równa sumie momentów sił przyłożonych do niego.

5 Założenia metody Newtona-Eulera ilustracja τ i r i,ci r i+1,ci -R i i+1 τ i+1 f i mi g i -R i i+1 f i+1 m i masa i-tego ogniwa; r i,cj wektor z przegubu i-tego do śr. masy ogniwa j-tego; r i,i+1 wektor z przegubu i-tego do i + 1-ego; f i siła wywierana przez ogniwo i 1 na ogniwo i-te; τ i moment wywierany przez ogn. i 1 na ogn. i-te; g i przyspieszenie ziemskie wyrażone w i-tym układzie.

6 Założenia metody Newtona-Eulera siły prędkość zmian pędu ciała jest równa sumie sił przyłożonych do niego m masa ciała, v prędkość środka masy, f wypadkowa sił zewnętrznych. U nas dm dt = 0 stąd d(mv) dt ma = f, a = v przyspieszenie środka masy ciała. = f, (1)

7 Założenia metody Newtona-Eulera momenty sił prędkość zmian popędu ciała jest równa sumie momentów sił przyłożonych do niego d(i 0ω 0) = τ 0, (2) dt I 0 moment bezwładności ciała wyrażony w inercyjnym układzie odniesienia, ω 0 prędkość kątowa ciała, τ 0 wypadkowy moment przyłożony do ciała. Równanie (2) w lokalnym układzie współrzędnych ma postać 1 : I ω + ω (I ω) = τ. 1 przy I = const

8 Założenia metody Newtona-Eulera prawo równowagi Równanie równowagi sił: f i R i+1 i f i+1 + m i g i = m i a c,i, gdzie a c,i przyspieszenie środka masy i-tego ogniwa wyrażone w układzie inercyjnym. Równanie równowagi momentów: τ i R i+1 i τ i+1 + f i r i,ci + (R i+1 i f i+1 ) r i+1,ci = I i α i + ω i (I i ω i ), gdzie I i macierz inercji ogniwa i-tego wyrażona w ukł. współ. równoległym do ukł. i-tego o środku umieszczonym w środku masy ogniwa i-tego, α i przyspieszenie kątowe ogniwa i-tego wyrażone w układzie inercyjnym.

9 Idea metody Newtona Eulera Idea metody Newtona Eulera: dla danych współrzędnych uogólnionych i ich pochodnych q, q, q określić wektory sił f 1, f 2,..., f n i momentów τ 1, τ 2,..., τ n w następujący sposób: dla danych q, q, q określić wszystkie prędkości i przyspieszenia występujące w układzie; mając wszystkie prędkości i przyspieszenia wyznaczyć siły i momenty występujące w układzie.

10 Metoda Newtona Eulera W celu wykorzystania metody Newtona Eulera należy: zakładając ω 0 = 0, α 0 = 0, a c,0 = 0, a e,0 = 0, a e,i przyspieszenie końca i-tego ogniwa, wyliczyć równania: ω i = (Ri 1) i T ω i 1 + b i q i, b i = (R0) i T z i 1, α i = (Ri 1) i T α i 1 + b i q i + ω i b i q i, a e,i = (Ri 1) i T a e,i 1 + ω i r i,i+1 + ω i (ω i r i,i+1 ), a c,i = (Ri 1) i T a e,i 1 + ω i r i,ci + ω i (ω i r i,ci ), dla i = 1, 2,..., n.

11 Metoda Newtona Eulera zakładając wyliczyć równania f n+1 = 0, τ n+1 = 0 f i = R i+1 i f i+1 m i g i + m i a c,i, τ i = R i+1 i τ i+1 f i r i,ci + dla i = n, n 1,..., 1. + (R i+1 i f i+1 ) r i+1,ci + I i α i + ω i (I i ω i ),

12 Metoda Newtona Eulera Otrzymane równania są postaci: n n d ik (q) q k + cjk(q) i q j q k + g i (q) = τ i, k=1 j,k=1 dla i = 1, 2,..., n. Definiując n c ij (q, q) cjk i q k, k=1 otrzymujemy wektorowe równanie dynamiki: D(q) q + C(q, q) q + G(q) = τ,

13 Model dynamiki D(q) q + C(q, q) q + G(q) = τ, D(q) macierz inercji (symetryczna i dodatniookreślona); C(q, q) macierz sił Coriolisa i odśrodkowych; G(q) wektor sił związanych z siłami grawitacji; τ wektor sił i momentów niepotencjalnych działających na układ (tarcia, opory ruchu, więzy, sterowania). Własności modelu dynamiki D(q) macierz symetryczna i dodatniookreślona; d dt D(q(t)) = C(q(t), q(t)) + C T (q(t), q(t))

14 Formalizm Eulera-Lagrange a Zgodnie z zasadą najmniejszego działania dla układu o współrzędnych uogólnionych q i prędkościach uogólnionych q t2 t 1 L(q, q)dt min, gdzie L(q, q) = K(q, q) V (q) lagranżian układu. K(q, q) energia kinetyczna układu V (q) energia potencjalna.

15 Formalizm Eulera-Lagrange a Warunkiem koniecznym minimum działania jest spełnienie równań Lagrange a II rodzaju: d L(q(t), q(t)) L(q(t), q(t)) = u i (t), (3) dt q i q i dla i = 1,..., n, gdzie u i wszystkie momenty i siły zewnętrzne typu niepotencjalnego.

16 Macierz pseudoinercji Jk k macierz psudoinercji ktego przegubu wyrażona w układzie ktym x 2 xy xz x Jk k xy y = 2 yz y m k xz yz z 2 z dm x y z 1 Uwagi: Jk k jest symetryczna ( ) element Jk k jest równy masie ogniwa k 44 3 pierwsze elementy 4 kolumny opisują położenie środka masy ogniwa k (przemnożone przez masę ogniwa)

17 Obliczanie równania dynamiki gdzie ḡ = [ ] g a r 0 k = D ij = tr G i = [ rk 0 n k=1 n k=1 ] A k 0 q i J k k m k ( A k 0 q j ḡ, Ak 0 q i n C ij = Ckj i q k, Ckj i = 1 ( Dij 2 q k=1 k r k ) T, r k położenie środka masy ogniwa k, + D ik q j D ) jk q i

18 Manipulator jako obiekt sterowania Dla manipulatora o dynamice i kinematyce danej układem równań: { τ = D(q) q + C(q, q) q + G(q), y = k(q) połóżmy x(t) = q(t), ξ(t) = q(t), u(t) = τ(t). Wówczas otrzymamy układ rr pierwszego rzędu ẋ = ξ ξ = D 1 (x)(c(x, ξ)ξ + G(x)) + D 1 (x)u, y = k(x) czyli afiniczny układ sterowania o postaci: ẋ = ξ ξ = F (x, ξ) + G(x)u, y = k(x)