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)

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 wykł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- mulacji, 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 δ ˙q_i

− δL

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

L = K − P (2)

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

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) (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 położeń kątowych w poszczególnych przegubach.

(5)

Model dynamiki mechanizmów wieloczłonowych

Przyjmując następujące macierze

Q =







0 −1 0 0

1 0 0 0

0 0 0 0







- dla przegubu obrotowego (4)

Q =







0 0 0 0

0 0 0 1

0 0 0 0







- dla przegubu przesuwnego (5)

Można wyznaczyć pochodne cząstkowe Ujk ≡ δ⁰T_i

δqj

=

₀

Tj −1Q^{j −1}Ti, dlaj ¬ i

0, dlaj > i (6)

U_ijk ≡δ⁰U_ij δqk

=







0T_{j −1}Q^{j −1}_j T_k−1Q^k−1_k T_i, dlaj ¬ k ¬ i

0Tk−1Q^k−1_k Tj −1Q^{j −1}_j Ti, dla k ¬ j ¬ i 0, dla j > i lub i < k

(7)

(6)

Model dynamiki mechanizmów wieloczłonowych

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

Ji=R rir_i^Tdm =







−IXXi + IYYi+ IZZi

2 IXYi IXZi mixi

IXYi

I_XXi − IYYi+ I_ZZi

2 IYZi miy_i

I_XZi I_YZi I_XXi + I_YYi− I_ZZi

2 m_iz_i

mixi miy_i mizi mi





 (8) IXXi,IYYi,IZZi - masowe momenty bezwładności członu i , IXYi,IXZi,IYZi - masowe momenty dewiacji członu i , Ri = [xi yi] zi]^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:

m_ik=

n

X

j =max(i ,k)

Tr (U_jkJjU^T_jk), i , k = 1, 2, . . . , n (9)

gdzie: Tr (A) =Pn

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

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

Vi= ˙θ^THiθ, H˙ i ∈ 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 =

n

X

j =i

(−mjGr U^j_ijrj), 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)

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

(9)

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

(10)

Opis mechanizmów wieloczłonowych w przestrzeni stanu

Definiując wektor stanu dla robota w postaci

X =

x₁ x2

=

θ θ˙

(13)

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

Z =˙

x2

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

+

0

M⁻¹(θ)

τ y = [I 0] x

(14)

(11)

Opis mechanizmów wieloczłonowych w przestrzeni stanu

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

˙

x = A(x ) + B(x )τ

y = Cx (15)

gdzie:

A(x ) =

x2

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

(16)

B(x ) =

0

M⁻¹(θ)

(17)

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

(12)

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

WM1 Macierz inercji M(q) jest symetryczna

M(Q) = M^T(Q) (19)

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

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

WM3 Elementy mi ,j(q) macierzy inercji M(Q) nie zależą od przemieszczeń uogólnionych w ’aktualnym’ przegubie i przegubach ’poprzednich’

m_{i ,j}(Q) = m_{i ,j}(q_k+1, q_k+2, . . . , q_n), k = min(i , j ), ∀i , j = 1, . . . , n (21)

(13)

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

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

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

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) = [ ¯mi ,j(Q)]n×njest symetryczna

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

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

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

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

(14)

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

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

M(Q) αI ∀Q ∈ Rⁿ (27) 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ⁿ (28) Stałą β można oszacowac następująco

β n

max

i ,j |mij(Q)|

(29)

(15)

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

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

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

kM  n²

max

i ,j ,k

∂Mij(Q)

∂q_k

(31)

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 (32) dla wszystkich wektorów x , y ∈ Rⁿ

(16)

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 (33) V (q, z + αx )y = V (q, z )y + αV (q, x )y (34)

(17)

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^TV1(q)y x^TV2(q)y

... x^TVn(q)y







(35)

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

vijk(q) = 1 2

 ∂Mkj(Q)

∂q_i +∂Mki(Q)

∂q_j −∂Mij(Q)

∂q_k

. (36)

(18)

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 ¬ kV₁kxkky k (37) dla wszystkich q, x , y ∈ Rⁿ.

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

kV (x, z)w − V (y , v )w k ¬ kC1kz − v kkw k

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

(19)

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ⁿ (39)

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 (40) 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ⁿ (41)

(20)

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))˙ (42)

dla wszystkich T ∈ R+.

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

dla wszystkich T ∈ R₊, kiedy k_U = min_q{U (q)}.

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

kG (q)k ¬ k⁰ (44)

dla wszystkich q ∈ Rⁿ.

(21)

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 (45) for all x , y ∈ Rⁿ. Stałą k_g można oszacować korzystając z pochodnej cząstkowej

k_g  n

max

i ,j ,q

∂G_i(q)

∂qj

(46)

Ponadto, kg spełnia zależność

kg  k∂G (q)

∂q || λMax

 ∂G (q)

∂q

(47)

(22)