Warszawa,2018 Wykład1-Wprowadzenie,pojęciapodstawowedrinż.JakubMożaryn,mgrinż.JanKlimaszewski SterowanieMechanizmówWieloczłonowych

(1)

Sterowanie Mechanizmów Wieloczłonowych

Wykład 1 - Wprowadzenie, pojęcia podstawowe

dr inż. Jakub Możaryn, mgr inż. Jan Klimaszewski

Instytut Automatyki i Robotyki

Warszawa, 2018

(2)

Dane dotyczące przedmiotu

Dane dotyczące przedmiotu Prowadzący

dr inż. Jakub Możaryn, Gmach Mechatroniki, p. 346, e-mail:

j.mozaryn@mchtr.pw.edu.pl,

mgr inż. Jan Klimaszewski, Gmach Mechatroniki, p. 307, e-mail:

j.klimaszewski@mchtr.pw.edu.pl

Strona www przedmiotu: http://jakubmozaryn.esy.es Informacje o przedmiocie Wykład - 15 godzin

Projektowanie - 15 godzin

dr inż. Jakub Możaryn, mgr inż. Jan Klimaszewski Sterowanie Mechanizmów Wieloczłonowych

(3)

Warunki zaliczenia

Warunki zaliczenia

Realizacja projektu zespołowego - 40% oceny końcowej Raport na zadany temat (indywidualny) - 30% oceny końcowej Zespołowa prezentacja projektu - 30% oceny końcowej

Liczba punktów ECTS - 3

(4)

Program

Cel przedmiotu

Umiejętność projektowania ciągłych i dyskretnych układów regulacji mechanizmami wieloczłonowymi (robotami).

(5)

Program - wykłady

Tematyka wykładów cz. 1

1: Model geometrii, kinematyki i dynamiki mechanizmów wieloczłonowych. Identyfikacja parametrów kinematycznych i dynamicznych robotów. Opis mechanizmów wieloczłonowych w przestrzeni stanu. Zastosowanie kwaternionów. Otwarte i zamknięte łańcuchy kinematyczne. Układy niedosterowane. Układy

nieholonomiczne. Kolizje.

2: Modele układów wieloczłonowych: manipulator, acrobot, wahadło odwrócone, maszyny kroczące.

3: Planowanie trajektorii ruchu robota w przestrzeni wewnętrznej, zewnętrznej i kartezjańskiej. Koordynacja ruchu robotów w przestrzeni zadań. Planowanie ruchu jako przeszukiwanie.

4: Serwomechanizmy przegubów. Sterowanie w przestrzeni przegubów. Sterowanie adaptacyjne.

(6)

Program - wykłady

Tematyka wykładów, cz. 2

5: Regulator dla robota o wielu stopniach swobody. Linearyzacja sprzężenia zwrotnego. Dobór funkcji Lapunova dla potrzeb sterowania manipulatorów. Sterownie pozycyjne i sterowanie nadążne. Projektowanie sterowania jako zadanie optymalizacji.

6: Sterowanie impedancyjne. Sterowanie siłowe. Sterowanie pozycyjno-siłowe. Modele tarcia.

7: Sterowanie o zmiennej strukturze mechanizmów wieloczłonowych.

Sterowanie predykcyjne mechanizmów wieloczłonowych. Sterowanie LQR/LQG mechanizmów wieloczłonowych.

8: Zastosowanie sieci neuronowych w układach sterowania mechanizmów wieloczłonowych.

(7)

Program - projektowanie

Tematyka projektów

1: Modelowanie i sterowanie układu wieloczłonowego – robot o wielu stopniach swobody.

2: Modelowanie układu wieloczłonowego, wyprowadzenie modelu zmiennych stanu, symulacja kinematyki i dynamiki – acrobot, robot kroczący, wahadło odwrócone.

3: Sterowanie o zmiennej strukturze mechanizmów wieloczłonowych.

Sterowanie predykcyjne mechanizmów wieloczłonowych. Sterowanie LQR/LQG mechanizmów wieloczłonowych.

4: Eksperymentalna identyfikacja parametrów kinematycznych i dynamicznych wahadła odwróconego.

5: Symulacyjne planowanie trajektorii ruchu współpracujących manipulatorów przemysłowych, pod kątem realizacji zadań montażowych.

(8)

Literatura

Fu K.S., Gonzalez R.C, Lee. C.S.G.: Robotics: Control, Sensing, Vision and Intelligence. McGraw-Hill Book Company, New York, 1987.

Craig J. J.:Wprowadzenie do robotyki. WNT, Warszawa, 1993.

Spong M. W., Vidyasagar M.: Dynamika i sterowanie robotów. WNT, Warszawa, 1997.

Kozlowski, K.: Modelling and Identification in Robotics. Advances in Industrial Control. Springer, London,1998.

Giergiel, M.J., Hendzel, Z., Żylski, W.: Modelowanie i Sterowanie Mobilnych Robotów Kołowych, PWN, Warszawa, 2002.

Kozłowski K., Dutkiewicz P., Wróblewski W.: Modelowanie i sterowanie robotów. PWN, Warszawa, 2003.

Spong M. W., Hutchinson S., Vidyasagar M.: Robot Modeling and Control, John Wiley & Sons, 2006.

Dombre, E., Khail, W.: Robot Manipulators: Modelling, Performance Analysis and Cotrol, John Wiley & Sons, 2007.

Siciliano B., Khatib O. : Siciliano B., Khatib O. (eds) Springer Handbook of Robotics. Springer, Berlin, Heidelberg, 2008.

Corke P.: Robotics, Vision and Control. Springer Tracts in Advanced Robotics, vol 118. Springer, Cham, 2017.

(9)

Co to jest mechanizm wieloczłonowy?

Mechanizm

Mechanizm - zespół współpracujących ze sobą elementów maszyny wykonującej określone zadanie (np. poruszanie się po zadanej trajektorii).

Mechanizm wieloczłonowy (MW)

Mechanizm wieloczłonowy (MW) - zespół połączonych brył

sztywnych (członów). Połączenia (lub stawy, przeguby) określają w jaki sposób człony mogą się poruszać względem siebie.

(10)

Geometria, kinematyka, dynamika

Geometria

Geometria - dziedzina matematyki badająca własności niezmienników typu odległość, pole powierzchni, czy np. miara kąta. Do opisu MW najbardziej przydatna jest geometria euklidesowa.

Kinematyka

Kinematyka - dziedzina fizyki (mechaniki, matematyki) zajmująca się badaniem geometrycznych właściwości ruchu ciał bez uwzględniania ich cech fizycznych (np. masy) i działających na nie sił.

Dynamika

Dynamika - dziedzina fizyki (mechaniki) zajmująca się ruchem ciał materialnych (z uwzględnieniem ich cech fizycznych - np. masy, momentu bezwładności) pod działaniem sił. Czasem utożsamiana z kinetyką - badaniem zachowania ciał fizycznych w ruchu.

(11)

Notacja - skalar i wektor

W uproszczeniu skalar - pojedyncza wartość liczbowa.

y = a · x + b (1)

W uproszczeniu wektor - seria wartości skalarnych.

V =





 v1

v2

v3

v4







(2)

W uproszczeniu macierz - zestaw wartości skalarnych zorganizowany w postaci układu prostokątnego (wiersze i kolumny).

A =







a11 a12 a13 a14

a21 a22 a23 a24

a31 a32 a33 a34

a41 a42 a43 a44







(3)

(12)

Macierzowe przekształcenia liniowe

Układ n równań liniowych o m zmiennych postaci











a11x1+ a12x2+ · · · + a1mxm= b1

a21x1+ a22x2+ · · · + a2mxm= b2

...

an1x1+ an2x2+ · · · + anmxm= bn

(4)

można zapisać w postaci równania macierzowego

AX = B, (5)

gdzie: A = [aij], i = 1, . . . , n, j = 1, . . . , m to macierz główna układu, X = [xj], oraz B = [bi].

(13)

Macierzowe przekształcenia w przestrzeni 3D

Każda macierz A o wymiarach m na n opisuje pewne przekształcenie li- niowe odwzorowujące wektor X o wymiarach m na 1 w wektor B o wymiarach n na 1.

Wygodnie jest stosować współrzędne jednorodne ponieważ można wtedy łatwo zapisać obroty i przemieszczenia w postaci jednej macierzy. W takim przypadku:





 x⁰ y⁰ z⁰ 1







= T





 x y z 1







(6)

gdzie: x , y , z - współrzędne punktu oryginalnego, x⁰, y⁰, z⁰ - współrzędne punktu przekształconego, T - macierz przekształcenia.

(14)

Macierz przesunięcia w przestrzeni 3D

Macierz przesunięcia (translacji):

Tabc = Tran(a, b, c) =







1 0 0 a

0 1 0 b

0 0 1 c

0 0 0 1







(7)

gdzie:

T - macierz przekształcenia - translacji, a, b, c - przesunięcie wzdłuż osi X , Y oraz Z przestrzeni trójwymiarowej.

(15)

Macierz przesunięcia - przykład

Rysunek:Przykład przesunięcia.

T−6,2,0=







1 0 0 −6

0 1 0 2

0 0 1 0

0 0 0 1







(16)

Macierz obrotu w przestrzeni 3D

Macierze obrotu (rotacji) wokół osi odpowiednio X, Y, Z:

Tx ,α = RotX (α) =







1 0 0 0

0 cos α − sin α 0 0 sin α cos α 0

0 0 0 1







(8)

Ty ,β= RotY (β) =







cos β 0 sin β 0

0 1 0 0

− sin β 0 cos β 0

0 0 0 1







(9)

T_z,γ= RotZ (γ) =







cos γ − sin γ 0 0 sin γ cos γ 0 0

0 0 1 0

0 0 0 1







(10)

gdzie: α, β, γ - kąt obrotu woków osi odpowiednio X , Y oraz Z .

(17)

Macierz obrotu - przykład

Rysunek:Przykład obrotu.

T_{z,pi /2}=







cos(pi /2) −sin(pi /2) 0 0 sin(pi /2) cos(pi /2) 0 0

0 0 1 0

0 0 0 1







(18)

Składanie macierzy przekształceń

Składanie transformacji - macierz wynikowa złożenia przekształceń to iloczyn składowych macierzy transformacji. Kolejność mnożenia ma zna- czenie.

Przykładowo złożenie obrotu Tγ, a następnie przesunięcia Tabc można zapisać następująco:





 x⁰ y⁰ z⁰ 1







=







1 0 0 a

0 1 0 b

0 0 1 c

0 0 0 1













cos γ − sin γ 0 0 sin γ cos γ 0 0

0 0 1 0

0 0 0 1











 x y z 1







(11)

Macierz wynikowa T_γabc jest złożeniem dwóch powyżdzych przekształceń.





 x⁰ y⁰ z⁰ 1







= T_γabc





 x y z 1







, T_γabc =







cos γ − sin γ 0 a sin γ cos γ 0 b

0 0 1 c

0 0 0 1







(12)

,

(19)

Ruch ciała sztywnego

Macierz opisująca przekształcenie możliwe do wykonania na ciałach sztywnych (ruch ciała sztywnego) obejmuje dowolne złożenie obrotów i przesu- nięć. Taka macierz reprezentuje obrót a następnie przesunięcie ciała sztywnego i można ją zapisać jako

T =R D

0 1

(13) gdzie:

R - macierz obrotu, D - wektor opisujący przesunięcie.

(20)

Model kinematyki i stopnie swobody

Model kinematyki

Model kinematyki MW - matematyczny opis uwzględniający kształty, rozmiary i położenia elementów składowych MW. Bez prędkosci, sił oraz momentów bezwladnosci.

Stopnie swobody

Stopnie swobody określają niezależne możliwości ruchu MW. Liczba stopni swobody określa minimalną liczbę współrzędnych pozwalających jednoznacznie określić położenie MW względem określonego układu odniesienia.

Współrzędne uogólnione

Zestaw wszystkich niezależnych zmiennych pozwalający jednoznacznie określić położenie MW nazywamy współrzędnymi uogólnionymi.

Np. bryła sztywna w przestrzeni 3D ma 6 st. swobody - trzy przemieszczenia i trzy rotacje.

(21)

Współrzędne uogólnione - definicja

Dla układu n cząstek w przestrzeni ich położenie w chwili t można opisać za pomocą 3n współrzędnych kartezjańskich X = [x1, x2, ..., x3n]. Jeżeli jednak wprowadziny r więzów opisanych za pomocą r równań:

ϕ_i(X , t) = 0, i = 1, ..., r (14) Wtedy zamiast współrzędnych X można wprowadzić f = 3n − r nowych współrzędnych Q = [q₁, q₂, . . . , q_f] zadanych za pomocą f niezależnych funkcji współrzędnych X oraz czasu t:

qi= qi(X , t), i = 1, ..., f (15) Współrzędne Q nazywa się współrzędnymi uogólnionymi. Wielkość Q oznacza położenie układu w przestrzeni konfiguracyjnej, w której wprowadzono współrzędne uogólnione.

(22)

Człony i połączenia

Człon

Człon - bryła sztywna, będąca częcią MW.

Para kinematyczna

Para kinematyczna - połączenie dwóch członów ograniczające ich możliwości ruchu względnego. Klasa pary kinematycznej określa liczbę st.

swobody, którą ogranicza para.

Przegub / staw / połączenie

Przegub / staw / połączenie - para kinematyczna określonej klasy, zazwyczaj w MW występują:

przeguby przesuwne klasy 5 - możliwy jest ruch względny wzdłuż jednej osi układu współrzędnych,

przeguby obrotowe klasy 5 - możliwe są tylko względne obroty wokół jednej osi układu współrzędnych.

(23)

Notacja DH

Istnieje wiele sposobów na przypisanie układów współrzędnych do czło- nów mechanizmu. Powszechnie stosuje się system określany jako notacja Denavita-Hartenberga (DH).

Każda transformacja pomiędzy sąsiednimi układami współrzędnych opisana jest jako złożenie czterech podstawowych transformacji:

T^{i −1}_i =R^{i −1}_i D_i^{i −1}

0 1

= Rot_z,θ_iTrans_z,d_iTrans_{x ,a}_iRot_{x ,α}_i (16) gdzie:

α_i - kąt skręcenia członu, a_i - długość członu, d_i - odsunięcie przegubu, θ_i - kąt przegubu.

Parametry θ_i, d_i, a_i, α_i związane z członem oraz połączeniem i opisują przekształcenie między układem i − 1, a układem i .

Macierz T^{i −1}_i jest zwykle funkcją zmiennych θi lub di - w zależności od rodzaju przegubu (obrotowe lub przesuwne).

(24)

Cztery parametry notacji DH

Przykład - uproszczony model nogi 2D.

Rysunek:Notacja DH.

αi - kąt skręcenia członu ai - długość członu di - odsunięcie przegubu θi - kąt przegubu

Zestaw wszystkich zmiennych parametrów MW określa współrzędne uogólnione (nazywane czasem wewnętrzne, złączowe lub konfiguracyjne) układu.

(25)

Przypisanie układów w notacji DH (1)

1 Ustalamy osie zi jako osie ruchu przegubów i + 1. W przypadku przegubu obrotowego - oś obrotu, a przegubu przesuwnego - oś translacji.

2 Ustalamy środek układu bazowego jako dowolny punkt na z0. Następnie ustalamy w dowolny sposób osie x0, y0- układ musi być prawoskrętny.

3 Dla każdego kolejnego układu zaczynając od 1 ustalamy jego położenie i orientację na podstawie układu poprzedniego.

4 Jeśli osie zi −1, zi nie są współpłaszczyznowe:

Wybieramy oś xi tak, aby zwierała najkrótszy odcinek łączący obie osie z i prostopadły do nich.

Przecięcie zi oraz xi to środek układu oi.

Wybieramy oś yi tak, aby układ i był prawoskrętny.

(26)

Przypisanie układów w notacji DH (2)

5 Jeśli osie zi −1, zi są równoległe:

Wybieramy środek układu oi jako dowolny punkt na prostej zi. Oś xi zawiera punkt oi oraz przecina zi −1.

6 Jeśli osie z_{i −1}, z_i przecinają się:

Wybieramy środek układu oi jako dowolny punkt na prostej zi. Wybieramy oś xi jako normalną do płaszczyzny zwierającej obie osie z.

(27)

Przykład notacji DH (1/2)

Przykład - model robota kroczącego.

Rysunek:Wizualizacja 3D robota czteronożnego

(28)

Przykład notacji DH (2/2)

Przykład - uproszczony model nogi 2D.

Rysunek:Schemat nogi z DH.

i ai αi di θi

1 l1 0 0 θ1

2 l2 0 0 θ2

Tablica:Parametry DH.

T⁰₁=







c₁ −s1 0 l₁c₁ s₁ c₁ 0 l₁s₁

0 0 1 0

0 0 0 1







T¹₂=







c2 −s2 0 l2c2

s2 c2 0 l2s2

0 0 1 0

0 0 0 1







(29)

Zadanie proste kinematyki - ujęcie ogólne

Mając dane współrzędne uogólnione q₁, · · · , q_nnależy wyznaczyć położe- nie i orientację ostatniego członu MW

H =R D 0 1

(17) gdzie

H = T⁰_n(q₁, · · · , q_n) = T⁰₁(q₁)T¹₂(q₂) · · · Tⁿ⁻¹_n (q_n) (18) Dla zadania prostego kinematyki zawsze istnieje rozwiązanie.

(30)

Zadanie proste kinematyki

Zadanie proste kinematyki MW - obliczenie współrzędnych kartezjańskich (zewnętrznych) punktu na MW znając jego współrzędne uogólnione. Za- zwyczaj chodzi o jeden z punktów ostatniego członu MW w układzie bazowym.

PAm = T^m_kPAk, k > m (19) gdzie:

T^m_k = T^m_m+1T^m+1_m+2. . . T^k−1_k (20)

PAk =





 xAk

yAk

zAk

1







- współrzędne punktu A w k-tym układzie

PAm=





 xAm

yAm

zAm

1







- współrzędne punktu A w m-tym układzie

(31)

Zadanie proste kinematyki - przykład

Dla omawianego modelu z rys. 5 macierz transformacji z układu 0 do układu 2 jest równa:

T⁰₂= T⁰₁· T¹₂ (21) przyjmując oznaczenie s_nm= sin(θ_n+ θ_m) otrzymujemy:

T⁰₂=







c₁₂ −s₁₂ 0 l₁c₁+ l₂c₁₂ s12 c12 0 l1s1+ l2s12

0 0 1 0

0 0 0 1







(22)

(32)

Zadanie odwrotne kinematyki - ujęcie ogólne

Mając dane położenie i orientację ostatniego członu MW w postaci transformacji

H =R D 0 1

(23) rozwiąż równanie (znajdź współrzędne uogólnione q1, · · · , qn)

T⁰_n(q1, · · · , qn) = T⁰₁(q1)T¹₂(q2) · · · Tⁿ⁻¹_n (qn) = H (24) Otrzymujemy układ równań skalarnych:

T_ij(q₁, · · · , q_n) = h_ij (25) Układ może mieć wiele rozwiązań lub nie mieć żandego.

(33)

Zadanie odwrotne kinematyki - metody

Rozwiązanie analityczne w formie zamkniętej

(qk = fk(h11, · · · , h34), k = 1, · · · , n). Jest lepsze niż numeryczne, ale trudne do otrzymania.

Rozwiązanie algebraiczne.

Rozwiązanie geometryczne.

Rozwiązanie numeryczne, np. całkując równanie różniczkowe

v ω

= J(Q) · ˙Q, gdzie ˙Q = [ ˙q1, · · · , ˙qn]^T, J - Jakobian.

Istnieją metody upraszczające rozwiązywanie zadania odwrotnego. Np.

odsprzęganie kinematyczne - dla manipulatorów o 6-ciu stawach, jeśli osie 3 ostatnich przecinają się w jednym punkcie, możliwe jest rozdzielenie problemu na dwa prostsze: zadanie odwrotnej kinematyki pozycji i zadanie odwrotnej kinematyki orientacji.

(34)

Prędkość ciała sztywnego

Każdy człon MW posiada prędkość kątową ω i prędkość liniową V . Prędkość kątową członu i w układzie bazowym 0 można wyrazić jako

ω⁰_i =

i

X

j =1

ω⁰_j =

i

X

j =1

ρjq˙jZ_{j −1}⁰ (26)

gdzie

ρ_j =

(1 jeżeli przegub j jest obrotowy

0 jeżeli przegub j jest przesuwny (27) oraz

Z_{j −1}⁰ = R⁰_{j −1}0 0 1^T (28) Prędkość liniowa środka układu współrzędnych członu i w ukłdzie bazowym 0 wynosi:

V_i⁰= ˙D_i⁰=

i

X

j =1

∂D_i⁰

∂qj

˙

qj (29)

(35)

Zadanie proste kinematyki prędkości - jakobian

Wyznaczenie prędkości V_n⁰i ω⁰_nostatniego członu MW na podstawie znanych prędkości współrzędnych uogólnionych ˙Q = [ ˙q1, · · · , ˙qn]^T.

V_n⁰ ω_n⁰

= J(Q) · ˙Q (30)

gdzie: J - jakobian

Równanie (30) można przedstawić jako

V_n⁰ ω⁰_n

=J_D(Q) J_R(Q)

· ˙Q (31)

gdzie: JD - jakobian przemieszczenia, JR - jakobian obrotu.

(36)

Jakobian przemieszczenia i obrotu

Jakobiany JD(Q) oraz JR(Q) możemy wyznaczyć na podstawie znanych prędkości współrzędnych uogólnionych ˙Q = [ ˙q1, · · · , ˙qn]^T.

J_D(Q) =h

∂D⁰_n

∂q1

∂D⁰_n

∂q2 · · · ^∂D_∂q⁰ⁿ

n

i

(32) JR(Q) =ρ1Z₀⁰ ρ2Z₁⁰ · · · ρnZ_n−1⁰

(33) gdzie Z₀⁰=0 0 1^T.

(37)

Zadanie proste kinematyki prędkości - przykład

Dla omawianego modelu z rys. 5 położenie ostatniego układu współrzęd- nych:

(d_x = l₁c₁+ l₂c₁₂ dy = l1c1+ l2c12

(34) różniczkując po czasie otrzymujemy

(v_x = ˙d_x = −l₁s₁θ˙₁− l2s₁₂( ˙θ₁+ ˙θ₂)

vy = ˙dy = l1c1θ˙1+ l2c12( ˙θ1+ ˙θ2) (35) ostatecznie:

V =vx

vy

=

d˙x

d˙_y

 −l1s1− l2s12 −l2s12

l1c1+ l2c12 l2c12

θ˙1

θ˙₂

= J_D(θ)

θ˙1

θ˙₂

(36)

(38)

Zadanie odwrotne kinematyki prędkości

Wyznaczenie prędkości współrzędnych uogólnionych ˙Q = [ ˙q1, · · · , ˙qn]^T na podstawie znanych prędkości V i ω końcówki MW.

Q = [J(Q)]˙ ⁻¹V ω

(37) Rozwiązanie takiego problemu jest możliwe tylko, gdy J jest macie- rzą nieosobliwą, tzn. det(J(Q)) 6= 0

(39)

Model dynamiki mechanizmów wieloczłonowych (K)

Istnieje szereg metod wyznaczania równań dynamiki robota:

metoda Lagrange’a-Eulera, metoda Newtona-Eulera, uogólniona metoda d’Alamberta, metoda Christoffela-Lagrange’a, metoda Walkera-Orina,

metoda Featherstone’a

Ich powstanie wynikło głównie z potrzeby stworzenia szybkich algorytmów do obliczeń numerycznych.

Podczas wyładu będziemy stosować algorytm Lagrange’a-Eulera. Wyzna- czona tym sposobem postać równań dynamiki pozwala na łatwą analizę właściwości robota, zaprojektowania uniwersalnego algorytmu do jego sy-

(40)

Model dynamiki mechanizmów wieloczłonowych

Zależności opisujące dynamikę robota o n-stopniach swobody można wy- znaczyć wykorzystując uogólnione równanie Lagrange’a - Eulera o postaci

τ_i= d dt

 δL δ ˙qi

−δL δqi

, i = 1, 2, . . . , n (38) gdzie: L - funkcja Lagrange’a (lagrangian), funkcja potencjału kinetycz- nego.

L = K − U (39)

K - całkowita energia kinetyczna układu, U - całkowita energia potencjalna układu, qi - uogólnione współrzędne układu w przegubie i (współrzędne maszynowe), τi - uogólnione wymuszenia (siły lub momenty) działające w przegubie i

(41)

Model dynamiki mechanizmów wieloczłonowych

Korzystając z równań Lagrange’a-Eulera, zależności opisujące dynamikę MW można przedstawić w następujący sposób.

τ = M(Q) ¨Q + V (Q, ˙Q) + G (Q) + F (Q, ˙Q) (40) gdzie: τ ∈ Rⁿ - wektor momentów napędowych w przegubach, n - liczba stopni swobody MW, M(Q) ∈ R^n×n- macierz bezwładności robota, V (Q, ˙Q) - wektor wyrazów zawierających składowe momentu zależne od sił odśrodkowych i Coriolisa, G (Q) ∈ Rⁿ - wektor wyrazów zawierających składowe momentu zależne od sił grawitacji, F (Q, ˙Q) ∈ Rⁿ- wektor wyra- zów zawierających składowe momentu zależne od sił tarcia, Q = [q_i] ∈ Rⁿ - wektor położeń kątowych w poszczególnych przegubach.

(42)

Model dynamiki mechanizmów wieloczłonowych

Przyjmując następujące macierze

Si =





0 −1 0 0

1 0 0 0

0 0 0 0



- dla przegubu i obrotowego (41)

Si =





0 0 0 0

0 0 0 1

0 0 0 0



- dla przegubu i przesuwnego (42) Można wyznaczyć pochodne cząstkowe

U_ij≡δT⁰_i δq_j =

T⁰_{j −1}SjT^{j −1}_i , dla j ¬ i

0, dlaj > i (43)

Uijk ≡δU_ij δqk

=







T⁰_{j −1}SjT^{j −1}_k−1SkT^k−1_i , dla j ¬ k ¬ i T⁰_k−1SkT^k−1_{j −1}SjT^{j −1}_i , dla k ¬ j ¬ i 0, dla j > i lub i < k

(44)

(43)

Model dynamiki mechanizmów wieloczłonowych

Dodatkoww przyjmuje się J_i - jednorodna macierz inercji członu i wyrażona zależnością:

J_i=R RiR_i^Tdm =







−IXXi + IYYi+ IZZi

2 I_XYi I_XZi m_ix_i

IXYi

IXXi − IYYi+ IZZi

2 IYZi miy_i

IXZi IYZi

IXXi + IYYi− IZZi

2 mizi

mixi miy_i mizi mi





 (45) IXXi,IYYi,IZZi - masowe momenty bezwładności członu i , IXYi,IXZi,IYZi - masowe momenty dewiacji członu i , R_i = [x_i y_i z_i]^T - jednorodny wektor współrzędnych środka masy członu i wyrażony w i -tym układzie współrzędnych,

(44)

Model dynamiki mechanizmów wieloczłonowych

Elementy macierzy bezwładności wyznacza się korzystając z zależności:

mik(Q) =

n

X

j =max(i ,k)

Tr (UjkJjU^T_jk), i , k = 1, 2, . . . , n (46)

gdzie: Tr (A) =Pn

i =1aii- ślad macierzy kwadratowej A ∈ R^n×n.

Elementy wektora V (Q, ˙Q) można wyznaczyć korzystając z następujących zależności:

vi(Q, ˙Q) = ˙θ^THiθ,˙ Hi∈ R^n×n, i = 1, 2, . . . , n (47)

hikl(Q) =

n

X

j =max(i ,k,l )

Tr (UjklJjU^T_ji), i , k, l = 1, 2, . . . , n (48)

Elementy wektora G (q) można wyznaczyć korzystając z zależności:

gi(Q) =

n

X

j =i

(−mjGr UijRj), i = 1, 2, . . . , n (49)

gdzie: Gr = [gx gy gz 0]^T -jednorodny wektor grawitacji wyrażony w bazowym układzie współrzędnych,

p

Gr Gr^T = 9.8062[m/s2] - na powierzchni ziemi, mi masa członu i .

(45)

Opis dynamiki z wykorzystaniem równań stanu

Współrzędne stanu

Współrzędne stanu to wielkości charakteryzujące zachowanie się układu dynamicznego, opisujące jego stan (np. położenie, prędkość, przyspiesze- nie).

Wektor stanu

Wektor stanu układu dynamicznego to minimalny zbiór współrzędnych stanu wystarczający łącznie ze znajomością wielkości wejściowych do okre- ślenia zachowania się układu w przyszłości.

Opis układów we współrzędnych stanu jest trudniejszy do interpretacji fizycznej niż opis w postaci transmitancji i niemożliwy do bezpośredniego określenia na drodze pomiarowej. Jest jednak wygodniejszy do celów modelowania oraz projektowania wielowymiarowych układów stero- wania i regulacji.

(46)

Przestrzeń stanów

Rysunek:Trajektoria fazowa - przykład

Przestrzeń stanów, przestrzeń fazowa

Zbiór wszystkich możliwych wartości wektora stanu X (t) w chwilach t tworzy przestrzeń stanów układu (przestrzeń fazową).

trajektoria stanu

Zbiór wartości wektora stanu układu w kolejnych chwilach czasu tworzy w tej przestrzeni krzywą, zwaną trajektorią stanu układu (trajektorią fazową).

(47)

Opis mechanizmów wieloczłonowych w przestrzeni stanu

Definiując wektor stanu dla robota w postaci X =

X1

X2

=

Q Q˙

(50) oraz przyjmując, że wejściami do układu są sygnały sterujące τi, natomiast wyjściami współrzędne uogólnione qi, równanie dynamiki można wyrazić w przestrzeni stanu jako

X =˙

X2

−M⁻¹(Q)[V (Q, ˙Q) + G (Q) + F (Q)]

+

0

M⁻¹(Q)

τ Y = [I 0] X

(51)

(48)

Opis mechanizmów wieloczłonowych w przestrzeni stanu

Stosując standardowe oznaczenia modeli stanu, układ rówań (51) zapisuje się następująco

X = A(X ) + B(X )τ˙

Y = CX (52)

gdzie:

A(X ) =

X₂

−M⁻¹(Q)[V (Q, ˙Q) + G (Q) + F (Q)]

(53)

B(X ) =

0

M⁻¹(Q)

(54)

C (X ) = [I 0] (55)

(49)

Właściwości strukturalne modelu dynamiki - macierz M(Q)

WM1 Macierz inercji M(q) jest symetryczna

M(Q) = M^T(Q) (56)

WM2 Macierz inercji M(q) jest dodatnio określona

X^TM(Q)X > 0 ∀X = [x_i]_n×1, x 6= 0 (57) WM3 Elementy mi ,j(q) macierzy inercji M(Q) nie zależą od

przemieszczeń uogólnionych w ’aktualnym’ przegubie i przegubach ’poprzednich’

mi ,j(Q) = mi ,j(qk+1, qk+2, . . . , qn), k = min(i , j ), ∀i , j = 1, . . . , n (58)

(50)

Właściwości strukturalne modelu dynamiki - macierz M(Q)

Z własności WM1 - WM3 wynikają następujące wnioski

1 Macierz M(Q) jest nieosobliwa

det[M(Q)] 6= 0 (59)

2 Można dokonać dekompozycji macierzy M(q)

M(Q) = P^TP det P 6= 0 (60)

3 Żaden element macierzy inercji nie zależy od przemieszczenia uogólnionego w pierwszym przegubie, natomiast element mn,nnie zależy od przemieszczeń uogólnionych w żadnym z przegubów i ma wartość stałą.

4 Odwrócona macierz inercji ¯M(Q) = [ ¯m_{i ,j}(Q)]n×njest symetryczna

M(Q) = M(Q)¯ ⁻¹= ¯M^T(Q) (61)

5 Odwrócona macierz inercji ¯M(q) jest dodatnio określona

x^TM(Q)x > 0¯ ∀x = [x_i]n×1, x 6= 0 (62)

6 Każdy element odwróconej macierzy inercji ¯M(Q) jest funkcją przemieszczeń uogólnionych we wszystkich przegubach oprócz pierwszego

¯

mi ,j(Q) = ¯mi ,j(q2, . . . , qn) ∀i , j = 1, . . . , n (63)

(51)

Właściwości strukturalne modelu dynamiki - macierz M(Q)

WM4 Istnieje stała α ∈ R+, taka że

M(Q) αI ∀Q ∈ Rⁿ (64)

gdzie I oznacza macierz jednostkową o wymiarach n × n.

WM5 Dla robotów mających tylko połączenia obrotowe istnieje stała β > 0, taka że

λ_Max{M(Q)} ¬ β ∀Q ∈ Rⁿ (65) Stałą β można oszacowac następująco

β n

max

i ,j |m_ij(Q)|

(66)

(52)

Właściwości strukturalne modelu dynamiki - macierz M(Q)

WM6 Dla robotów mających tylko połączenia obrotowe istnieje stała kM > 0, taka że

kM(x)z − M(y )zk ¬ kMkx − y kkzk (67) dla wszystkich wektorów x , y , z ∈ Rⁿ. Stałą kM można oszacowac następująco

k_M  n²

max

i ,j ,k

∂M_ij(Q)

∂qk

(68)

WM7 Dla robotów mających tylko połączenia obrotowe istnieje stała k_M⁰ > 0 such that

kM(x)y k ¬ k_M⁰ ky k (69) dla wszystkich wektorów x , y ∈ Rⁿ

(53)

Właściwości strukturalne modelu dynamiki - wektor V (Q, ˙ Q)

WV1 Dla danego manipulatora macierz V (Q, ˙Q) może nie być jednoznaczna, ale wektor V (Q, ˙Q) ˙Q jest jednoznaczny WV2 Dla wszystkich wektorów q, x , y , z ∈ Rⁿi stałej α

spełniona jest zależność

V (q, x )y = V (q, y )x (70) V (q, z + αx )y = V (q, z )y + αV (q, x )y (71)

(54)

Właściwości strukturalne modelu dynamiki - wektor V (Q, ˙ Q)

WV3 Wektor V (q, x )y może być zapisany w postaci

V (q, x )y =







x^TV₁(q)y x^TV2(q)y

... x^TVn(q)y







(72)

gdzie Vk(q) są symetryczymi macierzami o wymiarach n × n dla wszystkich k = 1, 2, . . . , n. Element Vkij(q) macierzy Vk(q) odpowieda tzw. symbolowi Christoffela pierwszego rodzaju vjik(q) postaci

v_ijk(q) = 1 2

 ∂M_kj(Q)

∂qi

+∂M_ki(Q)

∂qj

−∂M_ij(Q)

∂qk

. (73)

(55)

Właściwości strukturalne modelu dynamiki - wektor V (Q, ˙ Q)

WV4 Dla robotów mających tylko połączenia obrotowe istnieje stała kC1 > 0, taka że

kV (q, x)y k ¬ kV1kxkky k (74) dla wszystkich q, x , y ∈ Rⁿ.

WV5 Dla robotów mających tylko połączenia obrotowe istnieją stałe k_C₁> 0 and k_C₂> 0, takie że

kV (x, z)w − V (y , v )w k ¬ kC₁kz − v kkw k

+ k_C₂kx − y kkw kkzk (75) dla wszystkich v , x , y , z, w ∈ Rⁿ.

(56)

Właściwości strukturalne modelu dynamiki - wektor V (Q, ˙ Q)

WV6 Macierz V (Q, ˙Q), zależy od macierzy M(Q) zgodnie z za- leżnością

x^T 1 2

M(q) − V (q, q)˙

x = 0 ∀q, ˙q, x ∈ Rⁿ (76) i w rzeczywistości¹₂M(q)−V (q, q) jest skośnie-symetryczna.˙ Równoważnie, macierz ˙M(q)−2V (q, q) jest skośnie-symetryczna, spełnia następującą zależność

M(q) = V (q, ˙˙ q) + C (q, ˙q)^T (77) Niezależnie od sposobu, w jaki V (q, ˙q) jest wyprowadzana, zawsze spełnia

q˙^T 1 2

M(q) − V (q, ˙˙ q)

q = 0˙ ∀q, ˙q ∈ Rⁿ (78)

(57)

Właściwości strukturalne modelu dynamiki - wektor G (Q)

WG1 Wektor G (Q) i wektor ˙Q są ze sobą skorelowane następująco

Z T 0

G (q(t))^Tq(t)dt = U (q(T )) − U (q(0))˙ (79) dla wszystkich T ∈ R+.

WG2 Dla robotów mających tylko połączenia obrotowe istnieje stała kU, taka że

U (q(0)) + Z T

0

G (q(t))^Tq(t)dt k˙ _U (80) dla wszystkich T ∈ R+, kiedy kU = minq{U (q)}.

WG3 Dla robotów mających tylko połączenia obrotowe istnieje stała k⁰ taka że

kG (q)k ¬ k⁰ (81)

dla wszystkich q ∈ Rⁿ.

(58)

Właściwości strukturalne modelu dynamiki - wektor G (Q)

WG4 Dla robotów mających tylko połączenia obrotowe, wektor G (q) jest lipszycowski (ang. Lipschitz vector ), co oznacza, że istnieje stała kg> 0, taka że

kG (x) − G (y )k ¬ kgkx − y k (82) for all x , y ∈ Rⁿ. Stałą kg można oszacować korzystając z pochodnej cząstkowej

kg  n

maxi ,j ,q

∂Gi(q)

∂qj

(83) Ponadto, kg spełnia zależność

k_g  k∂G (q)

∂q || λMax

 ∂G (q)

∂q

(84)

(59)

Sterowanie Mechanizmów Wieloczłonowych

Wykład 1 - Wprowadzenie, pojęcia podstawowe

dr inż. Jakub Możaryn, mgr inż. Jan Klimaszewski

Instytut Automatyki i Robotyki

Warszawa, 2018