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
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 γ 0 1 0 0 R = sin γ cos γ 0 0 cos β sin β 0 0 1 0 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 α 0 0 0 1 R = 1 0 0 0 cos γ sin γ 0 sin γ cos γ cos β 0 sin β 0 0 1 0 sin β 0 cos β 1 cos α sin α 0 sin α cos α 0 0 0 1 érbeli pozíciót ír le (R), minden pozíció leírható? Probléma a sorrend nem egyértelmű
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
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
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
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
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 0 0 1 1 1 0 0, 1,1,0 = 0, 1, 1,0 i j k 1 1 0 0 0 1 0, 1, 1,0 s, v t, w = s t v w, s w + t v + v w 7
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 0 1 1 0 0 0, 0,1,0 i j k 0 1 0 0 0 1 0, 1,0,0 0, r 1, r, r 3 = 0, 0,0,1 0, 0,1,0 0, 0,0, 1 i j k 0 0 1 0 1 0 0, 1,0,0 i j k 1 0 0 0 0 1 0, 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 1 0 0 1 0, 0,0,0 i j k 0 0 0 0 0 1 0, 0,0,0 R v = 1 0 0 0 1 0 0 0 0 1 1 0 = 1 1 0 8
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
1.4 Gép - merev testek összekapcsolása kényszerekkel y Kényszerek : csapágy, vezeték, 0 q fogaskerék 1.4.1 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 0 0 1 y 1 = + a y + b 1 10
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 0 0 0 0 0 0 0 d z 0 0 0 0 1 y z 1 => ξ η d 1 11
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
α 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 0 0 0 0 1 0 0 0 0 1 1 0 0 0 0 1 0 0 0 0 1 b i 0 0 0 1 Elmozdulás i-1 mentén a i -vel H a i = Elfordulás i-1 körül α i -vel H α i = 1 0 0 a i 0 1 0 0 0 0 1 0 0 0 0 1 1 0 0 0 0 cosα i sinα i 0 0 sinα i cosα i 0 0 0 0 1 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 0 0 0 1 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 0 0 0 1 13
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 1.4.4 Á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
, cos cos l l 1 1 1 1, sin sin y l l 1 1 1 1, d, d 1 1 1 d dt dt dt 1, d y, d y y 1 1 1 d dt dt dt 1 d 1, 1, d1 dt dt dy y y dt dt 1 1 1, 1, d v Jφ 15
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
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
.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
Θ 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 = 0.1.3.1 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
.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
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
.1.3.3 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
ω ω 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
.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
. 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
..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
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
..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 1 + 4 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 5 880 f (ξ) 3 h (f i + 3 f i+1 + 3 f i+ + f i+3 ) h 5 6480 f (ξ) 8 8
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) 0.0000051 9
.. 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
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 )
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 = 1 + 0 0 0 0 1 + d = 1 + + 1 + + = y i,0 + 0 fi ξ, y 1,m 1 ξ, y n1,m 1 ξ dξ y i,0 y i,0 d = 1 + + + 3 3 y n 1 d = 1 + + + + n n! e Az e Taylor sora a 0 körül 3
... 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
...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
... 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
...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 = 1 + 0.1 1 = 1.1 1.105 y = y 1 + h y 1 = 1.1 + 0.1 1.1 = 1.1 1.14 y 3 = y + h y = 1.1 + 0.1 1.1 = 1.331 1.3499 y 4 = y 3 + h y 3 = 1.331 + 0.1 1.331 = 1.4641 1.4918...4. Euler-Cauchy 1.javítás y 0 = 1 y 1 = y 0 + h y 0 = 1 + 0.1 1 = 1.1 y 1 = y 0 + h y 0 + y 1 = 1 + 0.1 1 + 1.1 y i+1 = y i + h f y i+1 = y i + h f i f i+1 = f = 1.105 1.105 y = y 1 + h y 1 = 1.105 + 0.1 1.105 = 1.155 y = y 1 + h y 1 + y 1.105 + 1.155 = 1.105 + 0.1 = 1.1 1.14 y 3 = y + h y = 1.1 + 0.1 1.1 = 1.3431 y 3 = y + h y + y 3 1.1 + 1.3431 = 1.1 + 0.1 = 1.3431 1.3499 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
...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 + 1 6 k 1 (i) + k i + k 3 i + k 4 (i) ha f C 5 hiba = θ h 5 37
...3.1 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 ½ 1 0 0 1 1/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
...3. Például RK4 y = f t, y 0 1/ 1/ 1/ 0 ½ 1 0 0 1 1/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 + 1 6 0 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 ) = 0.105 k (0) 3 = h (y 0 + k 0 ) = 0.105 k 4 0 = h (y 0 +k 0 3 ) = 0.105 t 1 = 0,1 y 1 = 1 + 1 0.1 + 0.105 + 0.105 + 0.105 = 1.104 1.105 6 k (1) 1 = h y 1 = 0.1104 k (1) = h (y 1 + k 1 1 ) = 0.1159 k (1) 3 = h (y 1 + k 1 ) = 0.116 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 = 1.105 + 1 0.1104 + 0.1159 + 0.116 + 0.1 = 1.13 1.14 6 39