Sterowanie Mechanizmów Wieloczłonowych Wykład 3 - Dynamika Mechanizmów Wieloczłonowych dr inż. Jakub Możaryn, mgr inż. Jan Klimaszewski

(1)

Sterowanie Mechanizmów Wieloczłonowych

Wykład 3 - Dynamika Mechanizmów Wieloczłonowych

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

Instytut Automatyki i Robotyki

Warszawa, 2019

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

(2)

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 wykładu będzie opisany algorytm Lagrange’a-Eulera. Wyznaczona tym sposobem postać równań dynamiki pozwala na łatwą analizę właści- wości robota, zaprojektowania uniwersalnego algorytmu do jego symulacji, oraz syntezę układu sterowania.

(3)

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 (1)

gdzie: L - funkcja Lagrange’a (lagrangian), inaczej funkcja potencjału kinetycznego.

L = K − P (2)

K - całkowita energia kinetyczna układu, P - 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

(4)

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) (3) 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 = [qi] ∈ Rⁿ - wektor współrzędnych uogólnionych (przesunięcie, obrót) w poszczegól- nych przegubach.

(5)

Model dynamiki mechanizmów wieloczłonowych

Przyjmując następujące macierze (i -nr przegubu)

Qi =







0 −1 0 0

1 0 0 0

0 0 0 0







- dla przegubu obrotowego (4)

Qi=







0 0 0 0

0 0 0 1

0 0 0 0







- dla przegubu przesuwnego (5)

Można wyznaczyć pochodne cząstkowe macierzy przekształceń

Ujk ≡δT⁰_i δqj

=

T⁰_{j −1}Q_jT^{j −1}_i , dlaj ¬ i

0, dlaj > i (6)

Uijk ≡ δUij

δq_k =







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

(7)

(6)

Do obliczeń przyjmuje się jednorodną macierz inercji członu i wyrażoną zależnością:

Ji =R

riri^Tdm =







−IXXi+ IYYi+ IZZi

2 IXYi IXZi mixi

IXYi

IXXi− IYYi+ IZZi

2 IYZi miy_i

IXZi IYZi

IXXi+ IYYi− IZZi

2 mizi

mixi miy_i mizi mi





 (8)

gdzie IXXi,IYYi,IZZi - masowe momenty bezwładności członu i , IXYi,IXZi,IYZi

- masowe momenty dewiacji członu i , Rⁱ_i = [xi y_i zi 1]^T- jednorodny wektor współrzędnych środka masy członu i wyrażony w i -tym układzie współrzędnych,

(7)

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 (9)

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) = ˙Q^THiQ,˙ Hi ∈ R^n×n, i = 1, 2, . . . , n (10)

hikl=

n

X

j =max(i ,k,l )

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

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

gi(Q) =

n

X

j =i

(−mjGrUijR^j_j), i = 1, 2, . . . , n (12)

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

p

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

(8)

Siły tarcia, wystepujące w ukłaadach mechanicznych, zalicza się do sił dyssypatywnych (rozpraszających). Występujące w układach mechanicznych tarcie jest nieliniowym i nie- stacjonarnym zjawiskiem o parametrach rozłożonych. W systemach mechatronicznych zmniejsza ono dokładność oraz pogarsza jakość przebiegów dynamicznych.

Modele tarcia najogólniej można podzielić na trzy grupy:

white-box - wykorzystują podstawy fizyczne badanego zjawiska i dzieli się je na statyczne oraz dynamiczne.

black-box - modele parametryczne, bazujące na danych eksperymentalnych.

grey-box - łączą cechy modeli white-box i black-box.

W modelowaniu fizycznym przy modelowaniu mechanizmów wieloczłoowych stosuje się zwykle statyczne modele typu white-box, które przy pomocy równań algebraicznych, opi- sują podstawowe własności tarcia. Wyróżnia sie wśród nich modele klasyczne uwzględ- niające różne kombinacje tarcia Coulomba (parametr Fc), wiskotycznego (parametr Fv), statycznego (parametr Fs) oraz efektu Stribecka.

(9)

Model dynamiki mechanizmów wieloczłonowych

Rysunek:Modele tarcia

(10)

Modele tarcia (F (Q, ˙Q) = [fi(Q, ˙Q)]), przedstawione na rys. 9 opisywane są następująco

Model (a)

fi(Q, ˙Q) = Fvq˙i+ FCsign( ˙qi) (13)

Model (b)

fi(Q, ˙Q) =

n _±F

s dla q˙i= 0

Fvq˙i+ FCsign( ˙qi) dla q˙i6= 0 (14) Model (c)

fi(Q, ˙Q) =







±Fs dla q˙i = 0

Fvq˙i+

F_C+ (Fs− F_C) exp

−q˙i

˙ qst

2

sign( ˙qi) dla q 6= 0˙ (15) gdzie: ˙qst - prędkość Stribecka.

Model (d) - definiuje się otoczenie, wewnątrz którego prędkość jest zerowa. W tym przypadku tarcie dla jest zależne od zewnętrznych sił (Fe) utrzymujących układ w spoczynku. Poza przyjętym otoczeniem jest opisywane w funkcji prędkości (Modele (a)–(c))

τf( ˙q, Fe) =

n _τ

f( ˙q) dla | ˙q| β

τf(Fe) dla | ˙q| β (16)

(11)

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.

Liczba współrzędnych stanu jest równa rzędowi równania różniczkowego opisującego obiekt.

Opis układów we współrzędnych stanu jest trudniejszy do interpreta- cji fizycznej niż opis w postaci transmitancji i często trudny do bezpo- średniego określenia na drodze pomiarowej. Jest jednak wygodniejszy do celów modelowania oraz projektowania wielowymiarowych układów sterowania i regulacji.

(12)

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ą).

(13)

Opis mechanizmów wieloczłonowych w przestrzeni stanu

Definiując wektor stanu dla robota w postaci

X =

X1

X2

=

Q Q˙

(17)

oraz przyjmując, że wejściami do układu są sygnały sterujące τi, natomiast wyjściami położenia qi, równanie dynamiki można wyrazić w przestrzeni zmiennych stanu jako

X =˙

C2

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

+

0

M⁻¹(Q)

τ Y = [I 0] X

(18)

(14)

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

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

y = Cx (19)

gdzie:

A(x ) =

x2

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

(20)

B(x ) =

0

M⁻¹(Q)

(21)

C (x ) = [I 0] (22)

(15)

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

WM1 Macierz M(Q) jest symetryczna

M(Q) = M^T(Q) (23)

WM2 Macierz M(Q) jest dodatnio określona

X^TM(Q)X > 0 ∀X = [xi]n×1, x 6= 0 (24)

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 (25)

(16)

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

1 Macierz M(Q) jest nieosobliwa

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

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

M(Q) = P^TP, det P 6= 0 (27)

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) (28)

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

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

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

¯

m_{i ,j}(Q) = ¯m_{i ,j}(q2, . . . , qn), ∀i , j = 1, . . . , n (30)

(17)

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

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

M(Q) αI, ∀Q ∈ Rⁿ (31) 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ⁿ (32) Stałą β można oszacowaćc następująco

β n

max

i ,j |mij(Q)|

(33)

gdzie: λmax(A) - największa wartość własna macierzy A.

(18)

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

kM(X )Z − M(Y )Z k ¬ kMkX − Y kkZ k (34) dla wszystkich wektorów X , Y , Z ∈ Rⁿ. Stałą kM szacuje się jako

kM n²

max

i ,j ,k

∂Mij(Q)

∂q_k

(35)

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 (36) dla wszystkich wektorów X , Y ∈ Rⁿ

(19)

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

WV1 Dla danego mechanizmu wieloczłonowego 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 (37) V (Q, Z + αX )Y = V (Q, Z )Y + αV (Q, X )Y (38)

(20)

V (Q, ˙ Q)

WV3 Wektor V (Q, X )Y może być zapisany w postaci

V (Q, X )Y =







X^TV1(Q)Y X^TV2(Q)Y

... X^TV_n(Q)Y







(39)

gdzie V_k(Q) ∈ R^n×n, k = 1, 2, . . . , n są symetryczymi macierzami. Element vkij(Q) macierzy Vk(Q) odpowieda tzw. symbolowi Christoffela pierwszego rodzaju vjik(Q) postaci

vijk(Q) = 1 2

 ∂Mkj(Q)

∂q_i +∂Mki(Q)

∂q_j −∂Mij(Q)

∂q_k

. (40)

(21)

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

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

kV (Q, X )Y k ¬ kC1kX kkY k (41) dla wszystkich q, x , y ∈ Rⁿ.

gdzie

k_C₁ = n²

max

k,i ,j ,q

V_k_ij(Q)

(42)

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

kV (X1, X2)X3− X4(X5, X4)X3k ¬ kC₁kX2− X4kkX3k

+ k_C₂kX1− X5kkX3kkX2k (43)

dla wszystkich X₁, X₂, X₃, X₄, X₅∈ Rⁿ.

(22)

V (Q, ˙ Q)

WV6 Wektor 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ⁿ (44)

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) + V (Q, ˙Q)^T (45) 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ⁿ (46)

(23)

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

WG1 Wektory G (Q) i ˙Q są ze sobą skorelowane następująco Z T

0

G (Q(t))^TQ(t)dt = U (Q(T )) − U (Q(0))˙ (47)

dla wszystkich T ∈ R+, gdzie U [Q(T )] - funkcja energii potencjalnej

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

U (Q(0)) + Z T

0

G (Q(t))^TQ(t)dt k˙ _U (48)

dla wszystkich T ∈ R+, kiedy k_U = minQ{U (Q)}.

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

kG (Q)k ¬ k⁰ (49)

dla wszystkich Q ∈ Rⁿ.

(24)

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 (50) dla wektorów X , Y ∈ Rⁿ. Stałą kg można obliczyć

korzystając z pochodnej cząstkowej

kg  n

max

i ,j ,q

∂gi(Q)

∂qj

(51)

Ponadto, stała kg spełnia zależność

kg k∂G (Q)

∂Q || λmax

 ∂G (Q)

∂Q

(52)

(25)