MODELLEZÉS - SZIMULÁCIÓ

Átírás

1 Mechatronika = Mechanikai elemek+ elektromechanikai átalakítók+ villamos rendszerek+ számítógép elemek integrációja Eszközök, rendszerek, gépek és szerkezetek felügyeletére, vezérlésére (manapság miniatürizált) MODELLEZÉS - SZIMULÁCIÓ 1

2 1 Merev testek kinematikája 1.1 Mozgásleírás adatokkal 6 szabadságfok pl. 3 pozíció, 3 szög 1. Forgatás térben 1..1 Euler szögek z--z konvenció: α az tengely és az N csomóvonal közötti szög. β a z tengely és a Z tengely közötti szög. γ az X tengely és az N csomóvonal közötti szög. cos γ sin γ R = sin γ cos γ 0 0 cos β sin β sin β cosβ 1.. Y-P-R z-y- konvenció: α a z tengely körül - Yaw fordul β az y tengely körül - Pitch - bukdácsol γ az tengely körül - Roll - dülöngél cos α sin α 0 sin α cos α R = cos γ sin γ 0 sin γ cos γ cos β 0 sin β sin β 0 cos β 1 cos α sin α 0 sin α cos α érbeli pozíciót ír le (R), minden pozíció leírható? Probléma a sorrend nem egyértelmű

3 1.. Rodrigues képlet Bármely r vektort elforgatja α szöggel az origón átmenő w tengely körül r -be. (abs(w)=1) r r r r r w φ w r α r w r r = r = w (w r) r = r r = r w (w r) w r w r abs w r = abs w abs r sin(φ) abs w r = abs w abs r abs r = abs r sin φ abs w r = abs w abs r sin(φ) α r r = r + r = r cosα + w r sinα + w w r r = r = r w (w r) cosα + w r sinα + w w r r = r cosα + w r sinα + w w r 1 cosα 3

4 w = w 1, w, w 3 r = r cosα + w r sinα + w w r 1 cosα W = 0 w 3 w w 3 0 w 1 w w 1 0 W r = w r W = i j k w 1 w w 3 r 1 r r 3 r = r cosα + W r sinα + w w r 1 cosα r = r + W r sinα + w w r r 1 cosα w w r r w w = w w r a b c = b a c c a b a b: = w c: = r w w r r = W r r = r + W r sinα + W r 1 cosα r = R r R = I + W sinα + W 1 cosα A forgatási mátri 4

5 1..3 Kvaternió - Komple számok általánosítása q = s,, y, z = s, v = s + i + y j + z k q 1 + q = s 1 + s, 1 +, y 1 + y, z 1 + z α q = α s, α, α y, α z q = s + + y + z s, v t, w = s t v w, s w + t v + v w q = s, v q q 1 = 1,0,0,0 q 1 = s, v q i j = k, j k = i, k i = j jobbsodrású j i = k, k j = i, i k = j 5

6 Forgatás w tengely körül (abs(w)=1) szöggel forgat a 0, v elforgatott = q 0, v q 1 q = cos α, w sin α z α v elforgatott y v A forgatás mátria v elforgatott = y z = R v = r 11 r 1 r 13 r 1 r r 3 r 31 r 3 r 33 y z q = cos α, w sin α 0, r 11, r 1, r 31 = q 0, 1,0,0 q 1 0, r 1, r, r 3 = q 0, 0,1,0 q 1 0, r 13, r 3, r 33 = q 0, 0,0,1 q 1 6

7 Példa 1 w = 0,0,1 α = π q = cos π, w sin π = 0, w = 0, 0,0,1 w z α y q = s + + y + z = 1 q 1 s, v = q = 0, w = 0, 0,0, 1 = 1,1,0 = 0, w 0, 1,1,0 0, w = = 0, 0,0,1 * 0, 1,1,0 * 0, 0,0, 1 i j k , 1,1,0 = 0, 1, 1,0 i j k , 1, 1,0 s, v t, w = s t v w, s w + t v + v w 7

8 Példa w z q = cos π, w sin π = 0, w = 0, 0,0,1 q 1 s, v = q = 0, w = 0, 0,0, 1 0, r 11, r 1, r 31 = q 0, 1,0,0 q 1 0, r 1, r, r 3 = q 0, 0,1,0 q 1 0, r 13, r 3, r 33 = q 0, 0,0,1 q 1 s, v t, w = s t v w, s w + t v + v w α v y 0, r 11, r 1, r 31 = 0, 0,0,1 0, 1,0,0 0, 0,0, 1 i j k , 0,1,0 i j k , 1,0,0 0, r 1, r, r 3 = 0, 0,0,1 0, 0,1,0 0, 0,0, 1 i j k , 1,0,0 i j k , 0, 1,0 0, r 13, r 3, r 33 = 0, 0,0,1 0, 0,0,1 0, 0,0, 1 i j k , 0,0,0 i j k , 0,0,0 R v = =

9 1.3 A mozgást leíró adatok Állapottér modell (mechatronikai) Pozíció r Sebesség v Gyorsulás a Szögsebesség ω Szöggyorsulás β z r c C ω ρ P r p = r c + ρ v p = v c + ωρ a p = a c + βρ y 9

10 1.4 Gép - merev testek összekapcsolása kényszerekkel y Kényszerek : csapágy, vezeték, 0 q fogaskerék Homogén koordináták D z (λ,λy,λ) 1 (,y,1) y, y, y, 1, y, z z, y z, 1 q 1 q 3 0 Az eltolás T y + a y + b 1 0 a 0 1 b y 1 = + a y + b 1 10

11 3D, y, z, y, z, 1, y, z, w w, y w, z w, 1 Perspektív transzformáció y η P y P Π y P k z d z z d, y z y d z d, d,1, y, z, z d C O Π P y s ξ P d z 0 d z d z y z 1 => ξ η d 1 11

12 1.4. Robotelemek csatolása - Denavit-Hartenberg transzformáció Az ízületek: Csúszó Forgó α i- a i-1 z i-1 y i-1 i-1 b i α i-1 a i ϴ i y i z i i α i z i-1 tengely az i. ízület forgás- vagy csúszó tengelye. i-1 tengely a z i-1 és a z i tengely közös normálisában Az i. koordináta-rendszer origója a z i-1 és a z i közös normálisa és a z i metszéspontja Párhuzamos forgástengelyek esetén a normális a megelőző ízülethez rendelt koordináta-rendszer origóján halad át. Egymást metsző tengelyeknél a koordináta-rendszer origója a tengelyek metszéspontja, az i tengely irányultsága pedig a (z i-1 z i ) vektoriális szorzattal párhuzamos. 1

13 α i- a i-1 z i-1 y i-1 i-1 b i α i-1 a i ϴ i y i z i i α i Elfordulás z i-1 körül ϴ i szöggel H θ i = Elmozdulás z i-1 mentén b i -vel H b i = cosθ i sinθ i 0 0 sinθ i cosθ i b i Elmozdulás i-1 mentén a i -vel H a i = Elfordulás i-1 körül α i -vel H α i = a i cosα i sinα i 0 0 sinα i cosα i Csukló Csúszka cosθ i sinθ i cosα i sinθ i sinα i a i cosθ i sinθ H α i H a i H b i H θ i = i cosθ i cosα i cosθ i sinα i a i sinθ i 0 sinα i cosα i b i cosθ i sinθ i cosα i sinθ i sinα i 0 sinθ H α i H b i H(θ i ) = i cosθ i cosα i cosθ i sinα i 0 0 sinα i cosα i b i

14 1.4.3 Általános koordináták Mozgásleírás egyéb általános koordinátákkal (pl. relatív szöghelyzet) q i (t) i=1..n Egyértelmű kapcsolat a fizikai koordinátákkal. r p = r p (q) p=1..n Általános sebesség Összetett függvény deriválása, Jacobi mátri. v p = dr p n dt = k=1 r p dq k q k dt p = 1.. n v = J q = r 1 q 1 r 1 q n r n q 1 r n q n q 1 q n 14

15 , cos cos l l , sin sin y l l , d, d d dt dt dt 1, d y, d y y d dt dt dt 1 d 1, 1, d1 dt dt dy y y dt dt 1 1 1, 1, d v Jφ 15

16 Dinamikai egyenletek.1 Newton-Euler egyenletek r c = v c.1.1 Newton.1. Euler Perdület v c = a c m v c = F = L = m r v L = Θ ω F i Tömegpont Merev test I = I yy = I zz = V V y + z + z + y ρ, y, z ddydz ρ, y, z ddydz ρ, y, z ddydz V Inercia tenzor I y = I y = I I y I z V Θ = I y I yy I yz I z = I z = V I z I zy I zz I yz = I zy = Szimmetrikus V valós sajátérték, sajátvektor (tehetetlenségi főtengelyek) z r c C ω e 1 ρ e e 1 y P y ρ, y, z ddydz z ρ, y, z ddydz yz ρ, y, z ddydz 16

17 pl. főtengely kr.-ban L c = I c ω = I 1c ω 1 e 1 + I c ω e + I 3c ω 3 e 3 Euler egyenlet L c = M c Newton-Euler egyenletek 6 szabadságfok 6 másodrendű lineáris differenciálegyenlet m v c = F = F i L c = M c Főtengely kr.-ban (a forgó koordinátarendszer miatt) M 1c = I 1c ω 1 + I 3c I c ω ω 3 M c = I c ω + I 1c I 3c ω 3 ω 1 M 3c = I 3c ω 3 + I c I 1c ω 1 ω 17

18 .1.3 Példa, kettős inga Θ = m 1 l 1 + m l Θ q = m 1 g l 1 sin q m g l sin q c sgn Θ q + c sgn L c = M c Elhanyagolás m R m 1 ; m R m I. Nem lineáris - Coulomb surlódás L = Θ ω q m 1 g l 1 m g l sin q = 0 q m 1 l1 q m R q+ M+ l mm g II. Lineáris - kis elmozdulás, viszkózus surlódás sin q ~ q c sgn q ~μ Θ Θ q + μ q III. Surlódásmentes q m 1 g l 1 m g l q = 0 q m 1 g l 1 m g l q = 0 18

19 Θ q t = e rt Θ r m 1 g l 1 m g l = 0 Θ q + c sgn q m 1 g l 1 m g l sin q = A lineáris, surl.mentes egyenlet zárt alakú megold. (III) q m 1 g l 1 m g l q = 0 r 1, = ± m 1 g l 1 m g l Θ m 1 l 1 < m l ω r = m 1 g l 1 m g l Θ q t e r 1 t = e +iω r t = cos ω r t + i sin ω r t e r t = e iω r t = cos ω r t i sin ω r t r 1 t = er 1 t r +e t = cos ω r t r t = er 1 t e r t i = c 1 cos ω r t + c sin ω r t = sin ω r t 19

20 .1.3. A lineáris, viszkózus surl. egy. zárt alakú megold. (II) Θ q + μ q m 1 g l 1 m g l q = 0 q t = e rt Θ q + c sgn q m 1 g l 1 m g l sin q = 0 Θ r + μ r m 1 g l 1 m g l = 0 r 1, = μ ± μ + 4 I m 1 g l 1 m g l Θ μ + 4 Θ m 1 g l 1 m g l < 0 ρ = μ Θ ω r = 4 I m g l m 1 g l 1 μ Θ 0

21 q t e r 1 t = e ρ t+iω r t = e ρ t cos ω r t e r t = e ρ t iω r t = e ρ t cos ω r t r 1 t = er 1 t r +e t = e ρ t cos ω r t r t = er 1 t r e t = e ρ t sin ω i r t + i sin ω r t i sin ω r t = c 1 e ρ t cos ω r t + c e ρ t sin ω r t 1

22 A nem lineáris surl.mentes egyenlet szétválasztható Θ q m 1 g l 1 m g l sin q = 0 Legyen ismeretlen függvény az ω(t)! ω = q = dq dt q = d q dt = dω dt = dω dq dq dt = dω dq ω dω dq ω = m 1 g l 1 m g l sin q Θ ω = dq dt Θ q + c sgn q m 1 g l 1 m g l sin q = 0

23 ω ω dω = m 1 g l 1 m g l Θ = m g l m 1 g l 1 Θ cos q + c 1 ω = ± m g l m 1 g l 1 cos q Θ sin q dq + c 1 dq dt = ± m g l m 1 g l 1 cos q Θ + c 1 ± Θ m g l m 1 g l 1 cos q + Θ c 1 dq = dt? ± Θ m 1 g l 1 m g l cos q + Θ c 1 dq = t + c 3

24 .1..4 A nem lineáris Coulomb surlódásos egyenlet Θ q + c sgn q m 1 g l 1 m g l sin q = 0 p = q q = p p = m 1 g l 1 m g l sin q c sgn p Θ? 4

25 . Numerikus megoldások..1 Numerikus deriválás integrálás..1.1 Az első deriváltak közelítése y n 1 y n f = y n y n 1 n n 1 ( n 1 ) + y n 1 n 1 n h = n n 1 df() d y n y n 1 h 5

26 ..1. Az első derivált másodrendű közelítése Lagrange féle súlyfüggvények f = y n 1 n n+1 n 1 n n 1 n+1 + y n 1 y n y n+1 y n n 1 n+1 n n 1 n n+1 + n 1 n n+1 y n+1 n 1 n n+1 n 1 n+1 n f = f i s i () df n d = y n 1 n ( n + n+1 ) n 1 n n 1 n+1 + i y n n ( n 1 + n+1 ) n n 1 n n+1 + y n+1 n ( n 1 + n ) n+1 n 1 n+1 n 6

27 df n d = y n 1 n ( n + n+1 ) +y n 1 n n 1 n n ( n 1 + n+1 ) +y n+1 n n 1 n n+1 n+1 n ( n 1 + n ) n+1 n 1 n+1 n df n d = y n 1 n n+1 n 1 n n 1 n+1 + y n y n+1 n n 1 + n n+1 n n 1 n n+1 + n n 1 n+1 n 1 n+1 n h = n n 1 = n+1 n y n 1 y n y n+1 n 1 n n+1 Az első derivált másodrendű közelítése df d = y n+1 y n 1 h Hiba a Taylor polinom maradéktagja Ο(h n ) g=numdiff(fun, [,d]) fun - SciLab függvény - a függvény független változója (vektor) d - a differencia vektor g - a közelítő gradiens (derivált) 7

28 ..1.3 Kvadratúrák (területszámítás szóból ered) h (f i + f i+1 ) trapéz Elsőfokú interpoláció. A hiba: h3 1 f (ξ) Simpson Newton 3/8 h (f i f i + f i+1 ) Másodfokú interpoláció Illetve súlyozott átlag. A hiba: a középpont trapéz m = h f i t = h I i = m+t 3 Harmadfokú interpoláció. A hiba: 3 f i 1 +f i+1 h f (ξ) 3 h (f i + 3 f i f i+ + f i+3 ) h f (ξ) 8 8

29 inttrap([,] y) Mérési adatok integrálása trapéz szabállyal - növekvő független változók vektora (def:1:size(y,'*') matri 1*m) y - a függő (mért változók) integrate("fvstr","valtstr",tol,ig[,ah[,rh]]) Definiált függvény (fvstr) integrálása kvadratúrával valtstr - a változó az fv-ben ah - abszolút hibahatár (def: 1.e-8 ) rh - relatív hibahatár (def:1.e-14) intg(tol,ig,fv) Külső függvény (fv) integrálása kvadratúrával intsplin([,] y) Mérési adatok integrálása spline interpolációval - növekvő független változók vektora (def: 1:size(y,'*') y - a függő (mért változók) deff("y=f()", "y=sin()") in=integrate("f()","",0,6.8) disp (in)

30 .. Differenciálegyenletek megoldása...1 Sorozatos közelítés (Szukcesszív approimáció) dy i d = f i, y 1, y y n i = 1,, n y i 0 = y i,0 kezdeti feltétel dy i = f i y i () y i,0 dy i = y i y i,0 = y i = y i,0 +, y 1, y y n d f i, y 1, y y n d 0 f i 0, y 1, y y n d f i, y 1, y y n d 0 30

31 y i 0 = y 0 y i,m = y i,0 + 0 y i = y i,0 + f i 0 f i, y 1,m 1, y,m 1 y n,m 1 d, y 1, y y n d Ha K 0,y i,0 környezetben f i εc 0 Ha f i, y 1, y y n < M és K > 0 folytonos f i, y 1 + y 1, y + y, y n + y n K y 1 + y + + y n Lipschitz y i,m = y i,0 + 0 fi ξ, y 1,m 1 ξ, y n1,m 1 ξ dξ y i,0 y i,0 31 Abszolút és egyenletesen konvergál az y i megoldáshoz Ο(h n )

32 Példa y i,m dy = y d y 0 = 1 kezdetiérték feladat y 0 () 1 y 1 = 1 + 1d = 1 + y = 1 + y 3 = 1 + y n = d = = y i,0 + 0 fi ξ, y 1,m 1 ξ, y n1,m 1 ξ dξ y i,0 y i,0 d = y n 1 d = n n! e Az e Taylor sora a 0 körül 3

33 ... Euler-Cauchy féle törtvonal módszer y(t) = f t, y y t 0 = y 0 t i = t 0 + i h y i = y t i y t i y i+1 y i h = f t i, y i y i+1 = y i + h f t i, y i y f(t) t 33

34 ...1. Első javítás f i = f t i, y i Prediktor-korrektor módszerek y i+1 = y i + h f i t i, y i y i+1 y i+1 első közelítése f i+1 = f t i+1, y i+1 itt f i+1 f korrigált értéke y i+1 = y i + h f i + f i+1 y f(t) t 34

35 ... Második javítás y i+1 (0) = y i + h f t i, y i f többször is korrigálható y (k) i+1 = y i + h f t k 1 i, y i + f t i+1, y i+1 k = 1, m ameddig y i+1 (m) y i+1 (m+1) > ε 35

36 ...3. Például Euler-Cauchy dy = y d y 0 = 1 y 0 = 1 h = 0.1 e y 1 = y 0 + h y 0 = = y = y 1 + h y 1 = = y 3 = y + h y = = y 4 = y 3 + h y 3 = = Euler-Cauchy 1.javítás y 0 = 1 y 1 = y 0 + h y 0 = = 1.1 y 1 = y 0 + h y 0 + y 1 = y i+1 = y i + h f y i+1 = y i + h f i f i+1 = f = y = y 1 + h y 1 = = y = y 1 + h y 1 + y = = y 3 = y + h y = = y 3 = y + h y + y = = t i, y i t i, y i t i+1, y i+1 y i+1 = y i + h f i + f i+1 36

37 ...3 Runge-Kutta féle módszer RK4 y = f t, y y t 0 = y 0 t i = t 0 + i h k 1 (i) = h f k (i) = h f k 3 (i) = h f k 4 (i) = h f t i, y i y i = y t i t i + h, y i + k 1 (i) t i + h, y i + k (i) (i) t i + h, y i + k 3 y i+1 = y i k 1 (i) + k i + k 3 i + k 4 (i) ha f C 5 hiba = θ h 5 37

38 Eplicit Runge-Kutta (i) y i+1 = y i + h b j k j s j=1 k (i) 1 = h f t i, y i k (i) = h f (i) t i + c h, y i + a,1 h k 1 k (i) 3 = h f k s (i) = h f Butcher tábla t i + c 3 h, y i + a 3,1 h k 1 i t i + c s h, y i + a s,1 h k 1 i + a 3, h k i + a s, h k i 0 c a,1 c 3 a 3,1 a 3, c s a s,1 a s, a s,s-1 b 1 b b s-1 b s RK4 0 1/ 1/ 1/ 0 ½ /6 1/3 1/3 1/6 + + a s,s 1 h k s 1 i i 1 j=1 s a i,j = c i b i = 1 i =,, s j=1 38

39 ...3. Például RK4 y = f t, y 0 1/ 1/ 1/ 0 ½ /6 1/3 1/3 1/6 y t 0 = y 0 t i = t 0 + i h k (i) 1 = h f t i, y i k (i) = h f k 3 (i) = h f k 4 (i) = h f t i + h, y i + k 1 (i) t i + h, y i + k (i) (i) t i + h, y i + k 3 dy y d = y y 0 = y i+1 = y i h = 0.1 e t 0 = 0 y 0 = 1 k (0) 1 = h y 0 = 0.1 k (0) = h (y 0 + k 1 0 ) = k (0) 3 = h (y 0 + k 0 ) = k 4 0 = h (y 0 +k 0 3 ) = t 1 = 0,1 y 1 = = k (1) 1 = h y 1 = k (1) = h (y 1 + k 1 1 ) = k (1) 3 = h (y 1 + k 1 ) = k 4 0 = h (y 1 +k 0 3 ) = 0.1 k 1 (i) + k i + k 3 i + k 4 (i) t 1 = 0, y = =