FSB

ROBOTIKA
Dinamika robota
 FSB
Definicije i metode pristupa
♦ direktni problem dinamike: iz poznatih gibanja odrediti sile
♦ inverzni problem dinamike: iz poznatih sila odrediti gibanja

Euler-Lagrange metoda

Newton-Euler metoda

d’Alambertova metoda
 FSB
Kinetička energija
z z
m
v

dm
v
x x
ρ=const
1 1
dK = dm( v o v ) = dmv 2
2 2 dm ds m
= ⇒ dm = ds
m L L
1
K = ∫ v 2dm L
2 (m) m 2
K= ∫
2L 0
v (s) ds
 FSB
K = K1 + K 2
v
z 1 2 1 2 2
K1 = ω J1 = ω m1L1
2 6
L2
m2
K2 = ∫
m2,L2 2
v (s) ds
r 2 L2 0

ω
v 2 (s) = ω 2 ( L12 + s 2 )
1 2
x K 2 = ω m2 (3L12 + L22 )
6

1 2
6
[ 2 2 2
] 1 2
K = ω m1L1 + m2 (3L1 + L2 ) = ω J
2
 FSB
Potencijalna energija
z

g m T

pz

P = −m g p z
T

ako je z║g onda je P=-mgpz

 FSB

z
g P = P1 + P2
T2

ϕ m2,L2
L1
T1 P1 = − m1 g sin ϕ
2
 L2 
ϕ P2 = − m2 g  L1 sin ϕ − cos ϕ 
 2 
x

1  L2 
P = − g  m1 L1 sin ϕ + m2  L1 sin ϕ − cos ϕ  
2  2 
g = -9.81 m/s2
 FSB
Euler-Lagrange jednadžba
d  ∂K j  ∂K j ∂Pj
Tij =  − +
dt  ∂q&i  ∂qi ∂qi
Tij = sila u i -toj upravljanoj koordinati za pokretanje j -te mase
Kj = kinetička energija j -te mase
Pj = potencijalna energija j -te mase
qi = i -ta upravljana koordinata
.q = brzina i -te upravljane koordinate
i

♦ ukupni moment/sila u i -toj upravljanoj koordinati

N
Ti = ∑ Tij
j =1
 FSB
Opći oblik rješenja Euler-Lagrange jednadžbe
N N N
Ti = Gi (q) + ∑ H ij (q)q&& j + ∑ ∑ Dijk (q)q& j q&k
j =1 j =1 k = j
T = G (q) + H (q)q
&& + C(q, q& )
T = vektor momenata/sila
G = utjecaj gravitacije (∂P/∂q)
q = vektor upravljanih koordinata
.q = vektor brzina upravljanih koordinata (dq/dt)
..q = vektor ubrzanja upravljanih koordinata (d2q/dt2)
H = matrica inercija (N×N), Hii > 0 , Hij = Hji , nesingularna
C = vektor centrifugalnih i Coriolisovih sila
Dijk = konstante
 FSB

Inverzni dinamički problem

&& = H −1 (q)[T − G (q) − C(q, q& )]

q

q k +1 = q k + q& k ∆t + q
&& k 0.5(∆t ) 2
q& k +1 = q& k + q
&& k ∆t
 FSB
Dinamika pisača
♦ štap
z 1 2
K1 = ϕ& J
g 2
1
r P1 = −m1 g L sin ϕ
2
F
♦ klizač
ϕ
1
T x K 2 = m2 (ϕ& 2 r 2 + r& 2 )
2
P2 = −m2 gr sin ϕ
 FSB
d  ∂K1  ∂K1 ∂P1
T11 =   − +
dt  ∂ϕ&  ∂ϕ ∂ϕ

∂K1 d  ∂K1 
= Jϕ& ⇒   = Jϕ&&
∂ϕ& dt  ∂ϕ& 
∂K1
=0
∂ϕ
∂P1 1
= −m1 g L cos ϕ
∂ϕ 2

1
T11 = Jϕ − m1g L cos ϕ
&&
2
 FSB
d  ∂K 2  ∂K 2 ∂P2
T12 =   − +
dt  ∂ϕ&  ∂ϕ ∂ϕ

∂K 2 d  ∂K 2 
= m2r 2ϕ& ⇒   = m2 (r 2ϕ&& + 2rr&ϕ& )
∂ϕ& dt  ∂ϕ& 
∂K 2
=0
∂ϕ
∂P2
= −m2 gr cos ϕ
∂ϕ

T12 = m2r 2ϕ&& + 2m2rr&ϕ& − m2 gr cos ϕ

1
T1 = T11 + T12 = ( J + m2r )ϕ&& + 2m2rr&ϕ& − g cos ϕ ( m1L + m2r )
2
2
 FSB
T21 = 0
d  ∂K 2  ∂K 2 ∂P2
T22 =   − +
dt  ∂r&  ∂r ∂r
∂K 2 d  ∂K 2 
= m2r& ⇒   = m2&r&
∂r& dt  ∂r& 
∂K 2
= m2ϕ& 2r
∂r
∂P2
= −m2 g sin ϕ
∂r

T22 = m2&r& − m2ϕ& 2r − m2 g sin ϕ = F

 FSB

T  − g cos ϕ ( 1 m1L + m2r ) ( J + m2r 2 ) 0  ϕ&&  2m2rr&ϕ& 

F  =  2 +   + 
− ϕ 2 
   − g sin ϕ m2  

0 m2     2 r 
&
r& m &

1  1 
ϕ&& = 2
T + g cos ϕ ( m1L + m2r ) − 2m2rr&ϕ 
&
J + m2r  2 
F
&r& = + ϕ& 2r + g sin ϕ
m2
 FSB
Dinamika robota RRR strukture
♦ zadano: m1 , m2 , m3 , M , L1 , L2 , L3 , g
q (t ), dq (t )/dt, d2q (t )/dt 2

♦ traži se: T1 , T2 , T3

M
m1, L1
J1, R1
 FSB
c1 − s1 0 0
z s c1 0 0
A1 = Rot ( z, q1) = 1
z4 0 0 1 0
O4  
0 0 0 1
x4 y4
 c2 0 s2 0
 0 1 0 0 
z3 A 2 = Tran (0,0, L1 )Rot ( y, q2 ) =
q3 − s2 0 c2 L1 
 
y3  0 0 0 1
O3
x3
 c3 0 s3 0
z2  0 1 0 0 
q2 A3 = Tran (0,0, L2 )Rot ( y, q3 ) =
− s3 0 c3 L2 
O2 y2  
 0 0 0 1
z1
x2 O0 y1
q1 1 0 0 0
O1 y 0 1 0 0 
A 4 = Tran (0,0, L3 ) =
0 0 1 L3 
x x  
1 0 0 0 1
 FSB
c1 − s1 0 0
s c1 0 0
0
T1 = A1 = 1
0 0 1 0
 
0 0 0 1

c1c2 − s1 c1s2 0
s c c1 s1s2 0 
0
T2 = A1A 2 = 1 2
− s2 0 c2 L1 
 
 0 0 0 1

c1c23 − s1 c1s23 c1s2 L2 

s c c1 s1s23 s1s2 L2 
0
T3 = A1A 2 A3 =  1 23
− s23 0 c23 L1 + c2 L2 
 
 0 0 0 1 

c1c23 − s1 c1s23 c1( s2 L2 + s23 L3 ) 

s c c1 s1s23 s1( s2 L2 + s23 L3 ) 
0
T4 = A1A 2 A3A 4 =  1 23
− s23 0 c23 L1 + c2 L2 + c23L3 
 
 0 0 0 1 
 FSB
♦ Segment 1
m1, L1 1
1 K1 = J1q&12
J1, R1 J1 = m1R 2 2
2
P1 = const

d  ∂K1  ∂K1 ∂P1

T11 =  − +
dt  ∂q&1  ∂q1 ∂q1

T11 = J1q&&1
 FSB
♦ Segment 2
z2 dm2 du2
=
z m2 L2

p 2 =0 T2 [0 0 u2 1]T
dm2
O2 y2
p 2 = (c1s2u2 )i + ( s1s2u2 ) j + (c2u2 + L1)k
x2
p2
dp 2
v2 = = u2 (− s1q&1s2 + c1c2q&2 )i +
dt
O + u2 (c1q&1s2 + s1c2q&2 ) j −
y
− u2 s2q&2k
x

v 22 = v 2 o v 2 = u22 ( s22q&12 + q&22 )

 FSB
L2
m2 1
K2 = ∫ = +
2 2 2 2 2
v2 2(u ) du 2 m L ( s
2 2 2 1&
q &
q 2)
2 L2 0 6

 1 
P2 = − gm2  L1 + L2 c2 
 2 
d  ∂K 2  ∂K 2 ∂P2 1
T12 =  − + = m2 L22 (2s2c2q&1q&2 + s22q&&1 )
dt  ∂q&1  ∂q1 ∂q1 3

d  ∂K 2  ∂K 2 ∂P2
T22 =  − +
dt  ∂q&2  ∂q2 ∂q2
1 1
= m2 L2 (q&&2 − s2c2q&1 ) + gm2 L2 s2
2 2
3 2
 FSB
♦ Segment 3 dm3 du3
z y3 =
O3 m3 L3

x3
dm3 z3

p3
p3 =0 T3 [0 0 u3 1]T
O
y
x

p3 = c1 ( L2 s2 + u3s23 )i + s1( L2 s2 + u3s23 ) j + ( L1 + L2c2 + u3c23 )k

 FSB
= [− s1q&1( L2 s2 + u3s23 ) + c1( L2c2q&2 + u3c23 (q&2 + q&3 ))]i +
dp 3
v3 =
dt
+ [− c1q&1 ( L2 s2 + u3s23 ) + s1( L2c2q&2 + u3c23 (q&2 + q&3 ))] j +
+ [L2 s2q&2 − u3s23 (q&2 + q&3 )]k

v32 = v3 o v3 =
(
= L22 s22 + 2 L2 s2 s23u3 + s23 )
u3 q&1 + L22q&22 + 2 L2c3q&2 (q&2 + q&3 )u3 + (q&2 + q&3 ) 2 u32
2 2 2

L
m3 3 2
K3 = ∫
2 L3 0
v3 (u3 ) du3 =

1  2 2 1 2  2 1 2 2
= m3  L2 s2 + L2 L3s2 s23 + L23s23  1
&
q + L2 2
&
q
2 2 + L L c &
q (
2 3 3 2 2&
q + &
q3 ) + L (
3 2&
q + &
q )
3 
2  3  3 

 1 
P3 = − gm3  L1 + L2 c2 + L3c23 
 2 
 FSB

d  ∂K 3  ∂K 3 ∂P3
T13 =  − +
dt  ∂q&1  ∂q1 ∂q1

1 2 2
T13 = m3q1 ( L2 s2 + L2 L3s2 s23 + L3s23 ) +
&& 2 2
3
 2 2 2 
+ m3q&1q&2  2 L2 s2c2 + L2 L3 (c2 s23 + s2c23 ) + L3s23c23  +
 3 
2 2
+ m3q&1q&3 ( L2 L3s2c23 + L3s23c23 )
3
 FSB

d  ∂K 3  ∂K 3 ∂P3
T23 =  − +
dt  ∂q&2  ∂q2 ∂q2

1 2 1 1 2
T23 = m3q2 ( L2 + L2 L3c3 + L3 ) + m3q3 ( L2 L3c3 + L3 ) −
&& 2
&&
3 2 3
2 2 1 1 2 
− m3q&1  L2 s2c2 + L2 L3 (c2 s23 + c2 s23 ) + L3s23c23  −
 2 3 
1 1
− m3q&2q&3 L2 L3s3 − m3q&3 L2 L3s3 + m3 g ( L2 s2 + L3s23 )
2
2 2
 FSB

d  ∂K 3  ∂K 3 ∂P3
T33 =  − +
dt  ∂q&3  ∂q3 ∂q3

1 1 2 1
T33 = m3q&&2 ( L2 L3c3 + L3 ) + m3 L23q&&3 −
2 3 3
2 1 1 2 
− m3q&1  L2 L3s2c23 + L3s23c23  +
2 3 
1 1
+ m3q&2 L2 L3s3 + m3 gL3s23
2
3 2
 FSB
♦ Masa M
z y3
O3

z3 M y4
x3
O4

p3 z4
x4

O
y
x u3=L3
v32 = v3 o v3 =
(
= L22 s22 + 2 L2 s2 s23u3 + s23 )
u3 q&1 + L22q&22 + 2 L2c3q&2 (q&2 + q&3 )u3 + (q&2 + q&3 ) 2 u32
2 2 2
 FSB
2
vM = ( L2 s2 + L3s23 ) 2 q&12 + L22q&22 + 2 L2 L3c3q&2 (q&2 + q&3 ) + L23 ( q&2 + q&3 ) 2

KM =
1
2
2
MvM
1
[
= M ( L2 s2 + L3s23 ) 2 q&12 + L22q&22 + 2 L2 L3c3q&2 ( q&2 + q&3 ) + L23 (q&2 + q&3 ) 2
2
]
PM = − gM ( L1 + L2 c2 + L3c23 )

d  ∂K M  ∂K M ∂PM
T1M =  − +
dt  ∂q&1  ∂q1 ∂q1

T1M = Mq&&1 ( L2 s2 + L3s23 )2 +

+ 2Mq&1q&2 ( L2 s2 + L3s23 )( L2c2 + L3c23 ) +
+ 2Mq&1q&3 L3c23 ( L2 s2 + L3s23 )
 FSB

d  ∂K M  ∂K M ∂PM
T2 M =  − +
dt  ∂q&2  ∂q2 ∂q2

T2 M = Mq&&2 ( L22 + 2 L2 L3c3 + L23 ) + Mq&&3 L3 ( L2c3 + L3 ) −

− Mq&12 ( L2 s2 + L3s23 )( L2c2 + L3c23 ) −
− 2Mq&2q&3 L2 L3s3 − Mq&32 L2 L3s3 + gM ( L2 s2 + L3s23 )

d  ∂K M  ∂K M ∂PM
T3 M =  − +
dt  ∂q&3  ∂q3 ∂q3

T3M = Mq&&2 L3 ( L2c3 + L3 ) + Mq&&3 L23 − Mq&12 L3c23 ( L2 s2 + L3s23 ) +

+ Mq&22 L2 L3s3 + gML3s23
 FSB
T1 = T11 + T12 + T13 + T1M
T2 = T22 + T23 + T2 M
T3 = T33 + T3M

T1   G1   H11 H12 H13   q&&1   C1 

T  = G  +  H H 23  q&&2  + C2 
 2   2   21 H 22
T3  G3   H 31 H 32 H 33   q&&3  C3 

G1 = 0
1 1 
G2 = g  m2 L2 s2 + m3 ( L2 s2 + L3s23 ) + M ( L2 s2 + L3s23 )
2 2 
1 
G3 = g  m3 L3s23 + ML3s23 
2 
 FSB
1 1 2 2
H11 = J1 + m2 L2 s2 + m3 ( L2 s2 + L2 L3s2 s23 + L3s23 ) + M ( L2 s2 + L3s23 ) 2
2 2 2 2
3 3
H12 = 0
H13 = 0

H 21 = 0
1 1 2
H 22 = m2 L2 + m3 ( L2 + L2 L3c2 + L3 ) + M ( L22 + 2 L2 L3c3 + L23 )
2 2
3 3
1 2
H 23 = m3 ( L2 L3c3 + L23 ) + ML3 ( L2c3 + L3 )
2 3

H 31 = 0
H 32 = H 23
1
H 33 = ( m3 + M ) L23
3
 FSB
C1 = D112q&1q&2 + D113q&1q&3
C2 = D211q&12 + D223q&2q&3 + D233q&32
C3 = D311q&12 + D322q&22
2  2 
D112 = m2 L22 s2c2 + m3 2 L22 s2c2 + L2 L3 (c2 s23 + s2c23 ) + L23s23c23  +
3  3 
+ 2M ( L2 s2 + L3s23 )( L2c2 + L3c23 )
2
D113 = m3 ( L2 L3s2c23 + L23s23c23 ) + 2ML3c23 ( L2 s2 + L3s23 )
3
1
D211 = − D112
2
D223 = − L2 L3s3 (m3 + 2M )
1
D233 = − L2 L3s3 ( m3 + M )
2
1
D311 = D113
2
1
D322 = − D223
2
 FSB
Newton-Euler metoda
F = ma
T = Jω& + ω × Jω

 nx ox ax px 
n p y 
 R p   y oy ay
i
Ai −1 =   =
 0 0   nz oz az pz 
 
0 0 0 1
xi-1

yi-1

qi zi-1
O
 FSB
 i R i −1 (ωi −1 + z 0 q&i ) rotacijski SSG
ωi =  i
 R i −1ωi −1 translacijski SSG

 i R i −1 (ω&i −1 + z 0q&&i + ωi −1 × z 0 q&i ) rotacijski SSG

ω& i =  i
 R i −1ω&i −1 translacijski SSG

v& i = ...
ai = ...
 FSB

♦ prednosti:
jednostavne jednadžbe
univerzalni algoritam

♦ nedostaci:
rekurzivne jednadžbe
otežano ocjenjivanje utjecaja pojedinih parametara na
ukupno dinamičko ponašanje robota
 FSB
Usporedba složenosti metoda dinamike
Metoda Euler-Lagrange Newton-Euler D’Alambert

Množenja n4 n n3

Zbrajanja n4 n n3

Pristup 4x4 homogene matrice rotacija i matrice rotacija i

kinematici transformacije vektori položaja vektori položaja
Jednadžbe
dinamike diferencijalne rekurzivne diferencijalne
 FSB
Ukupni dinamički model robota
T = G (q) + H (q)q
&& + C(q, q& ) q = qm / N
 K K  K q& = q&m / N
J m q&&m +  Dm + m e q&m + Tm = m u
 R  R q&& = q&&m / N
T = Tm N
 Km Ke  2 Km
J m N q&& +  Dm +
2
 N q& + T = Nu
 R  R
&& + K vq& + T = K uu
Jq
J = diag ( J m N 2 )
 Km Ke 2 
K v = diag ( Dm + )N 
 R 
Km
K u = diag ( N)
R
 FSB

&& + K vq& + G (q) + H (q)q

Jq && + C(q, q& ) = K u u

G (q) + H* (q)q
&& + C* (q, q& ) = K u u
H* = H + J
C* = C + K vq&

♦ inverzno rješenje
q [
&& = H (q)
*
] [K u − G(q) − C (q, q& )]
−1
u
*

q k +1 = q k + q k ∆t + q k 0.5(∆t )
& && 2

q& k +1 = q& k + q
&& k ∆t