Sterowanie mechanizmów wieloczłonowych Wykład 2 - standardowe modele układów wieloczłonowych dr inż. Jakub Możaryn, mgr inż. Jan Klimaszewski

Sterowanie mechanizmów wieloczłonowych

Wykład 2 - standardowe modele układów wieloczłonowych

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

Instytut Automatyki i Robotyki

Warszawa, 2018

Wahadło odwrócone - Opis 1/2

Wahadło odwrócone składa się z pręta swobodnie przymocowanego do wózka jeżdżącego po prowadnicy. Ruch wózkiem pozwala na realizację przemieszczenia liniowego wahadła, natomiast ruch kątowy pręta jest wy- nikiem działania sił bezwładności. Wahadło posiada punkt stabilności w dolnym położeniu pręta oraz punkt chwiejnej równowagi w pozycji górnej.

W momencie gdy układ jest utrzymywany w górnej pozycji, mówi się o wahadle odwróconym.

Z przedstawionego opisu wynika, że jest to obiekt regulacji typu SIMO (ang. single-input, multi-output) i stanowi przykad układu niedostero- wanego. Jako sygnał sterujący przyjmuje się moment silnika powodujący przemieszczenie wózka, natomiast sygnałami wyjściowymi jest położenie liniowe i kątowe wahadła.

Wahadło odwrócone - Opis 2/2

Model wahadła został wyznaczony w oparciu o schemat przedstawiony na rys.1. Początek układu współrzędnych przyjęto w początkowym położeniu wózka.

Rysunek:Schemat wahadła odwróconego

Wózek wahadła ma możliwość przemieszczania się w osi x . Pręt waha- dła zamocowany jest jednym końcem na wózku i obraca się wokół osi przechodzącej prostopadle przez wózek o kąt θ. W dalszych rozważaniach parametry związane z wózkiem zostały oznaczone indeksemw, natomiast parametry związane z prętem indeksemp.

Model kinematyki

Parametry DH wahadła odwróconego

i ai αi di θi

1 a1= 0 α1= 0 d1 θ1= 0 2 a2= 0 α2= π d2= 0 θ2

Tablica:Parametry DH.

Macierz przekształceń - postać ogólna

T^{i −1}_i =





cos θi − cos α_isin θi sin αi· sin θ_i ai· cos θ_i sin θi cos αi· cos θi − sin αi· cos θi ai· sin θi

0 sin α_i cos α_i d_i

0 0 0 1



 (1)

Model kinematyki

Parametry DH wahadła odwróconego

i ai αi di θi

1 a1= 0 α1= 0 d1 θ1= 0 2 a2= 0 α2= π d2= 0 θ2

Tablica:Parametry DH.

Macierze przekształceń, dla danych szczegółowych

T⁰₁=





cos θ1 − cos α₁sin θ1 sin α1· sin θ₁ a1· cos θ₁ sin θ1 cos α1· cos θ₁ − sin α₁· cos θ₁ a1· sin θ₁

0 0 1 d1

0 0 0 1



 ⁽²⁾

T¹₂=





cos θ2 − cos α2sin θ2 sin α2· sin θ2 a2· cos θ2

sin θ2 cos α2· cos θ2 − sin α2· cos θ2 a2· sin θ2

0 0 −1 0

0 0 0 1



 (3)

Zadanie proste kinematyki

Dane: Q = [Q1 Q2] = [d1 θ2];

Szukane: T₂⁰

T⁰₂(Q) = T⁰₁(d1) · T¹₂(θ2) (4)

Zadanie odwrotne kinematyki

Mając dane położenie i orientację pręta wahadla w postaci transformacji H =R D

0 1



= T⁰₂(Q) = T⁰₁(d₁) · T¹₂(θ₂), H ∈ R^4×4, (5) rozwiąż równanie (znajdź współrzędne uogólnione d₁, θ₂)

T₂⁰(d1, θ2) = H (6)

Zadanie odwrotne kinematyki

Otrzymujemy układ równań skalarnych:



























T11(d1, θ2) = h11, T12(d1, θ2) = h12, T13(d1, θ2) = h13, T14(d1, θ2) = h14, T21(d1, θ2) = h21, T22(d1, θ2) = h22, T23(d1, θ2) = h23, T24(d1, θ2) = h24,



























T31(d1, θ2) = h31, T32(d1, θ2) = h32, T33(d1, θ2) = h33, T34(d1, θ2) = h34, T41(d1, θ2) = h41, T42(d1, θ2) = h42, T43(d1, θ2) = h43, T44(d1, θ2) = h44,

(7)

Układ może mieć wiele rozwiązań.

Zadanie proste kinematyki prędkości

Wyznaczenie prędkości V_n⁰i ω_n⁰pręta wahadła na podstawie znanych prędkości współrzędnych uogólnionych ˙Q = [ ˙d1, ˙θ2]^T.

V_n⁰ ω⁰_n



= J(Q) · ˙Q =JD(Q) JR(Q)



· ˙Q (8)

gdzie: J - jakobian.

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

JD(Q) =h

∂D⁰₂

∂d1

∂D⁰₂

∂θ2

i

(9) JR(Q) =ρ1Z₀⁰ ρ2Z₁⁰

(10) gdzie Z₀⁰=0 0 1^T, Z₁⁰= R⁰₁0 0 1^T, ρ1= 1 (przegub

przesuwny), ρ2= 0 (przegub obrotowy).

Zadanie odwrotne kinematyki prędkości

Wyznaczenie prędkości współrzędnych uogólnionych ˙Q = [ ˙d1, ˙θ2]^T na pod- stawie znanych prędkości V i ω środka cięzkości pręta wahadła.

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



(11)

Model dynamiki

Parametry masowe i bezwłasnościowe wahadła odwróconego i m_i x_i y_i z_i

1 m₁ x₁ y₁ z₁ 2 m2 x2 y2 z2

Tablica:Masy i położenia środków ciężkości.

i IXXi IYYi IZZi IXYi IXZi IYZi

1 IXX 1 IYY 1 IZZ 1 IXY 1 IXZ 1 IYZ 1

2 IXX 2 IYY 2 IZZ 2 IXY 2 IXZ 2 IYZ 2

Tablica:Momenty bezwładności.

Model dynamiki - jednorodne macierze inercji

Człon 1

J1=













−I_{XX 1}+ IYY 1+ IZZ 1

2 IXY 1 IXZ 1 m1x1

IXY 1

I_{XX 1}− I_{YY 1}+ I_{ZZ 1}

2 IYZ 1 m1y₁

I_{XZ 1} I_{YZ 1} IXX 1+ IYY 1− I_{ZZ 1}

2 m1z1

m1x1 m1y₁ m1z1 m1













(12) Człon 2

J2=













−I_{XX 2}+ IYY 2+ IZZ 2

2 IXY 2 IXZ 2 m2x2

IXY 2

I_{XX 2}− I_{YY 2}+ I_{ZZ 2}

2 IYZ 2 m2y₂

I_{XZ 2} I_{YZ 2} IXX 2+ IYY 2− I_{ZZ 2}

2 m2z2

m2x2 m2y₂ m2z2 m2













(13)

Model dynamiki - macierze pomocnicze do liczenia pochodnych cząstkowych

Przegub 1 - przegub przesuwny

Si=









0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0









(14)

Przegub 2 - przegub obrotowy

S2=









0 −1 0 0

1 0 0 0

0 0 0 0

0 0 0 0









(15)

Model dynamiki - macierze pomocnicze do liczenia pochodnych cząstkowych

Do wyznaczania pochodnych cząstkowych w modelu dynamiki, wykorzystujemy następujące definicje

Uij≡ δT⁰_i δqj

=

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

0, dlaj > i (16)

U_ijk ≡ δU_ij δqk

=







T⁰_{j −1}S_jT^{j −1}_k−1S_kT^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

(17)

Model dynamiki - elmenty macierzy bezwładności

Elementy macierzy bezwładności wahadła odwróconego M(Q) =

 m11(Q) m12(Q) m21(Q) m22



(18)

mik(Q) =

n

X

j =max(i ,k)

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

gdzie: Tr (A) =Pn

i =1a_ii - ślad macierzy kwadratowej A ∈ R^n×n. wyznacza się korzystając z zależności:

m_ik(Q) =

n

X

j =max(i ,k)

Tr (U_jkJ_jU^T_jk), i , k = 1, 2, . . . , n (20)

gdzie: Tr (A) =Pn

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

Model dynamiki - elmenty macierzy bezwładności

Elementy macierzy bewładności wahadła odwróconego

m11(Q) =

2

X

j =1

Tr (Uj 1JjU^T_{j 1}) = Tr (U11J1U^T₁₁) + Tr (U21J2U^T₂₁) (21)

m₁₂(Q) = m₂₁(Q) = Tr (U₂₁J₂U^T₂₁) (22)

m22=

2

X

j =2

Tr (U22J2U^T₂₂) (23)

Uwaga: zgodnie w własnościami strukturalnymi modelu mechanizmu wieloczłonowego, element m22 ma wartość stałą, natomiast elementy m12(Q) = m21(Q).

Model dynamiki - elmenty wektora V (Q, ˙ Q)

Elementy wektora wyrazów zawierających składowe momentu zależne od sił odśrodkowych i Coriolisa

V(Q, ˙Q) =

 v₁(Q, ˙Q) v2(Q, ˙Q)



(24)

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

hikl(Q) =

n

X

j =max(i ,k,l )

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

Stąd

 v1(Q, ˙Q) = ˙q^TH1(Q) ˙q

v2(Q, ˙Q) = ˙q^TH2(Q) ˙q (27)

Model dynamiki - elmenty wektora V (Q, ˙ Q)

Elementy macierzy H₁(Q)

h₁₁₁(Q) =

2

X

j =1

Tr (U_{j 11}J_jU^T_{j 1}) = Tr (U₁₁₁J₁U^T₁₁) + Tr (U₂₁₁J₂U^T₂₁) (28)

h₁₁₂(Q) = Tr (U₂₁₂J₂U^T₂₁) (29)

h121(Q) = Tr (U221J2U^T₂₁) (30)

h122= Tr (U222J2U^T₂₁) (31)

Model dynamiki - elmenty wektora V (Q, ˙ Q)

Elementy macierzy H2(Q)

h211(Q) = Tr (U211J2U^T₂₂ (32)

h212(Q) = Tr (U212J2U^T₂₂) (33)

h221(Q) = Tr (U221J2U^T₂₂) (34)

h₂₂₂= Tr (U₂₂₂J₂U^T₂₂) (35)

Model dynamiki - elmenty wektora G (Q, ˙ Q)

Elementy wektora wyrazów zawierających składowe momentu zależne od sił grawitacji

G(Q) =

 g1(Q) g2(Q)



(36) można wyznaczyć korzystając z następujących zależności:

gi(Q) =

n

X

j =i

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

Stąd

g1(Q) =

2

X

j =1

(−mjGr UijRj) = −m1Gr U11R1− m2Gr U12R2 (38)

g₂(Q) = −m₂Gr U₂₂R₂ (39)

Model dynamiki - podsumowanie

Model dynamiki wahadła odwróconego ma postać τ =

 τ1

τ2



=

 f 0



=

 m11(Q) m12(Q) m21(Q) m22

 Q+¨

 v1(Q, ˙Q) v₂(Q, ˙Q)

 +

 g1(Q) g2(Q)

 (40) Wahadło odwrócone jest układem niedosterowanym. Tak więc

w przegubie 2 nie ma momentu napędowego: τ₂= 0, na wozek działa siła napędowa: τ₁= f .

Dodatkowo

Q =

 q1

q2



=

 d1

θ2



(41)

Model dynamiki - podsumowanie

Równania dynamiki (wyprowadzenie znane z literatury) (

τ1= −(m1+ m2) ¨d1+ ml sin θ2 ˙θ2

2

− ml cos θ2θ¨2

τ2= 0 = −(I1+ I2) ¨θ2+ mgl sin θ2− ml cos θ2d¨1

(42)

Model dynamiki wahadła odwróconego ma postać

τ =

h _f

0

i

=

h _−(m

1+ m2) −ml cos θ₂

−ml cos θ₂ −(I₁+ I2)

i _¨

d1

θ¨2



+

 ml sin θ2 θ˙2
²

0



+

h ₀

mgl sin θ2

i

(43)

Linearyzacja - równania dynamiki układów liniowych

Zasada superpozycji:

f (x1+ x2) = f (x1) + f (x2), and f (0) = 0 (44) przestrzeń rozwiązań równania spełniającego zasadę superpozycji (1) jest przestrzenią liniową.

Jednorodność (implikuje niezmienność skalowania):

Funkcja f (x , y ) jest jednorodna w stopniu k jeżeli

f (βx , βy ) = β^kf (x , y ), i f (0) = 0. (45) gdzie: β - stały współczynnik.

Układ liniowy

Układ opisany funkcją jednorodną, w którym zachowana jest zasada superpozycji.

Układ nieliniowy

Układ, w którym nie jest zachowana jest zasada superpozycji i/lub nie jest opisany funkcją jednorodną.

Linearyzacja - równania dynamiki układów liniowych

Ogólna postać równania różniczkowego układu liniowego:

an

dⁿy dtⁿ+an−1

dⁿ⁻¹y

dtⁿ⁻¹+· · ·+a0y = bm

d^mx dt^m+bm−1

d^m−1x

dt^m−1+· · ·+b0x (46) gdzie: y - sygnał wyjściowy, x - sygnał wejściowy, ai, bi - stałe współczyn- niki.

Linearyzacja - definicje

Tworzenie opisu liniowego na podstawie opisu nieliniowego nazywa się li- nearyzacją.

Linearyzacja opisu nieliniowego w postaci równań algebraicznych nazywa się linearyzacją statyczną. (brak pochodnych)

Metody linearyzacji statycznej

linearyzacja metodą siecznej: uzyskanie najlepszej zgodności opisu liniowego z nieliniowym w określonym przedziale zmian zmiennej niezależnej.

linearyzacja metodą stycznej: uzyskanie najlepszej zgodności opisu liniowego z nieliniowym dla określonej wartości zmiennej

niezależnej, a więc i określonej wartości zmiennej zależnej.

Linearyzacja opisu nieliniowego w postaci równań różniczkowych nazywa się linearyzacją dynamiczną.

Linearyzacja statyczna metodą stycznej

Przeprowadzony proces linearyzacji metodą stycznej polega na : zastąpieniu krzywej, reprezentującej nieliniową zależność y = f (x ) styczną do niej w punkcie pracy,

przeniesieniu początku układu współrzędnych do punktu pracy, zastąpieniu w modelu matematycznym zmiennych absolutnych x i y odchyleniami tych zmiennych od punktu pracy - zmiennymi

przyrostowymi ∆x i ∆y .

Charakterystyka statyczna wyznaczona na podstawie równania zli- nearyzowanego względem określonego punktu pracy jest funkcją li- niową. Można ją także wyznaczyć linearyzując charakterystykę rzeczywistą względem tego samego punktu pracy

Linearyzacja dynamiczna

Przykład równania różniczkowego, będącego nieliniową zależnością pomię- dzy funkcjami x (t) i y (t) i ich pochodnymi.

F [y (t), ˙y (t), ¨y (t), . . . , y⁽ⁿ⁾(t), x (t), ˙x (t), ¨x (t), . . . , x^(m)(t)] = 0 (47) Podczas linearyzacji dynamicznej funkcje x (t) i y (t), oraz ich pochodne traktuje się analogicznie jak zmienne funkcji uwikłanej.

n

X

i =0

( ∂F

∂y^{(i )}



y₀^{(i )}

∆y^{(i )} )

+

m

X

j =0

( ∂F

∂x^{(j )}



x^{(j )}₀

∆x^{(j )} )

= 0 (48)

gdzie:

∆y = y (t) − y₀, ∆y⁽¹⁾= d ∆y

dt , . . . , ∆y⁽ⁿ⁾= dⁿ∆y dtⁿ

∆x = x (t) − x₀, ∆x⁽¹⁾= d ∆x

dt , . . . , ∆x^(m)= d^m∆x dt^m

Linearyzacja dynamiczna - przykład 1

Funkcja niejednorodna (nie jest liniowa)

y = mx + b (49)

Przyjmując punkt pracy - {x0, y0}, y0= f (x0)

Rozwinięcie w szereg Taylora w punkcie pracy ma postać y = f (x ) = f (x₀) +df

dx|x =x0

(x − x₀) 1! +d²f

dx²|x =x0

(x − x₀)²

2! + ... (50) Prosta styczna (pierwsza pochodna rozwinięcia) w punkcie pracy {x₀, y₀} jest dobrą aproksymacją w małym zakresie zmian argumentu funkcji (wiel- kości wejściowej).

Tak więc

y = f (x0) +df

dx|x =x₀(x − x0) = y0+ m(x − x0) (51) i ostatecznie

Linearyzacja dynamiczna - przykład 2

Przeprowadzić linearyzację układpu opisanego równaniem

y (t) = 2x (t)²+ x (t) ˙x (t) + 2¨x (t)² (53) W punkcie pracy - {x0, y0}, x0= 1, ˙x0= 0, ¨x0= 0.

Stosując rozwinięcie w szereg Taylora wokół punktu pracy

∆y (t)+[−4x (t)− ˙x (t)]0∆x (t)−[x (t)]0∆¨x (t)−[4¨x (t)]0∆¨x (t) = 0. (54) W punkcie pracy, zliearyzowany model ma postać

∆y (t) − 4∆x (t) − ∆ ˙x (t) = 0. (55) Charakterystyka statyczna układu nieliniowego (19)

y = 2x². (56)

Charakterystyka statyczna uladu liniowego (styczna do charakterystyki układu nieliniowego w punkcie pracy, pochodne równe zero)

∆y = 4∆x . (57)

Model dynamiki wahadła odwróconego - linearyzacja

Przyjmując punkt pracy

P₀= {d_1p = 0 θ_2p= 0} (58) oraz następujące uproszenia w punkcie pracy (małe przemieszczenia)











sin(θ) ≈ θ cos(θ) ≈ 1

 dθ dt

2

≈ 0

(59)

Równania dynamiki (wyprowadzenie znane z literatury)

 τ1= (m1+ m2) ¨d1− ml ¨θ2

τ2= 0 = (I1+ I2) ¨θ2+ mgl θ2− ml ¨d1

(60) Model dynamiki wahadła odwróconego ma postać

h _f i h _{(m + m) −ml cos θ} i _¨

d  

ml sin θ θ˙
²  h ₀ i