FSB

ROBOTIKA
Kinematika robota

Općenito 1
 FSB
Transformacija koordinata u ravnini - translacija

zadano: x,y,a,b

traži se: x1,y1

y1

x = a + x1
y = b + y1
b

x1 = x − a
O1 a x1
y1 = y − b
Translacija 2
 FSB
Transformacija koordinata u ravnini - rotacija

zadano: x,y,ϕ
y1
traži se: x1,y1

O1 x1

x1 = x cos ϕ + y sin ϕ
y1 = y cos ϕ − x sin ϕ
Rotacija 3
 FSB
x1 = x cos ϕ + y sin ϕ
y1 = y cos ϕ − x sin ϕ

 x1   cos ϕ sin ϕ   x 
 y  = − sin ϕ   
cos ϕ   y 
 1 

p1 = A p A=
matrica
transformacija
−1
p = A p1
 x  cos ϕ − sin ϕ   x1 
 y  =  sin ϕ  
cos ϕ   y1  
  
Matrica A 4
 FSB
Homogene transformacije

♦ desnokretni pravokutni koordinatni sustav

k
O
j y
i
x

Homogene t. 5
 FSB
Matrica homogenih transformacija

♦ opis položaja i orijentacije sustava n prema sustavu m

ix jx kx px 
i p y   m R n
jy ky m
pn 
m
An =  y = 
 iz jz kz pz   0 1 
 
0 0 0 1

♦ svojstva referentni sustav

i = j = k =1 1 0 0 0
0 1 0 0
i× j = k n
An = 
0 0 1 0
i
j = j
k = k
i = 0  
0 0 0 1
Matrica HT 6
 FSB
 0 −1 0 1
0 0 1 2 y1
0
A1 =  z
− 1 0 0 2
  2
0 0 0 1 j1
O1
k1 z1
1
p i1

k
O0
j 1 2
y
1
i

2 x1
x
Primjer 7
 FSB

♦ prijelaz izmeñu koordinatnih sustava

p0 = A1 p1
0

( )
p1 = A1
0 −1
p0 = A0 p0
1

♦ inverzna matrica homogenih transformacija

 ix iy iz −p
i 
j − p
j 
(m −
)
1
An =n Am =  x
k x
jy
ky kz
jz
− p
k
 
0 0 0 1 

Prijelaz 8
 FSB
Transformacije translacije
z1

2
c

k1
O10 b
j1 1 2
y1
1
i1
a 1 0 0 a
2 0 1 0 b 
x1 A = Tran (a, b, c) = 
0 0 1 c
 
Translacija
0 0 0 1
9
 FSB
Transformacije rotacije
z
z1
2

1
α
k1
O0 O1
i1 j1 1 2 y1y
1
i
1 0 0 0
2 0 cos α − sin α 0
x x1 A = Rot ( x,α ) = 
0 sin α cos α 0
 
Rotacija
0 0 0 1
10
 FSB

 cos α 0 sin α 0
 0 1 0 0
A = Rot ( y,α ) = 
− sin α 0 cos α 0
 
 0 0 0 1

cos α − sin α 0 0
 sin α cos α 0 0
A = Rot ( z ,α ) = 
 0 0 1 0
 
 0 0 0 1

Rot y i Rot z 11
 FSB
Relativne i apsolutne transformacije

Rot(x,90°) = 0A1
 Tran(0,b,c) = 1A2 y2

y2
z y z y
1 1

z2 O2
O2
z2 x2
O1 O1
z1 O0 x2 y z1 O0 y
x x
x1 x1
T= 1A2 0A1 T= 0A1 1A2
♦ Apsolutne transformacije ♦ Relativne transformacije
Relativne i apsolutne 12
 FSB
Matrica transformacija

♦ upotrebljavat ćemo samo relativne transformacije

p 0 = 0 A1 p1
p0 = 0 1
A1 A 2 p 2
p1 = A 2 p 2
1

0
T2 = 0 A11 A 2 matrica transformacija

p 0 = 0 T2 p 2

p 2 = T2( )0 −1
p 0 = 2 T0 p0
n
♦ prijelaz iz n-tog u m-ti sustav m
Tn = ∏ i −1
Ai
i = m +1
Matrica transformacija 13
 FSB
cos α − sin α 0 0
 sin α cos α 0 0

Rot(z,α) A1 = Rot ( z,α ) = 
 0 0 1 0
 
 0 0 0 1

1 0 0 a
0 1 0 b 
 Tran(a,b,0) A 2 = Tran (a, b,0) = 
0 0 1 0
 
0 0 0 1

1 0 0 0
0 cos β − sin β 0
 Rot(x,β) A3 = Rot ( x, β ) = 
0 sin β cos β 0
 
0 0 0 1
Primjer 14
 FSB
0
T3 = A1A 2 A 3

cos α − sin α cos β sin α sin β a cos α − b sin β 

 sin α cos α cos β − cos α sin β a sin α + b cos β 
0
T3 = 
 0 sin β cos β 0 
 
 0 0 0 1 

α , β , γ ,... ⇒ q1, q2 , q3 ,...

si = sin qi c1 − s1c2 s1s2 ac1 − bs2 

s −c1s2 as1 + bc2 
ci = cos qi c1c2
0
T3 =  1
sij = sin(qi + q j ) 0 s2 c2 0 
 
cij = cos(qi + q j ) 0 0 0 1 

Primjer 15
 FSB
Denavit-Hartembergov zapis strukture robota

Os zi-1 koordinatnog sustava (i-1), leži u osi gibanja i-tog

stupnja slobode gibanja.

 Os xi-1 okomita je na os zi-1 i paralelna je s osi koja ide

uzduž segmenta.

 Os yi-1 postavlja se tako da čini desnokretni koordinatni

sustav.
zi-1 qi
Oi-1

yi-1
xi-1

DH zapis strukture 16
 FSB
Matrica prihvata
z
n = vektor normale
a
o = vektor orijentacije
O a = vektor djelovanja
o y
n p = vektor položaja
x

 nx ox ax px 
n p y 
n o a p   y oy ay
0
Tn =  =
 n
 0 0 0 1  z oz az pz 
 
0 0 0 1
Matrica prihvata 17
 FSB
Stanford manipulator

z4
O4
x4 y4
L4 z3
q4
y3
z2 x O3
3

z z1 q3
q2
L3 y2
O2
L1 x2 q O0 y1
1
O1 y

x
L2 x1
Stanford 18
 FSB
z4 c1 − s1 0 0
O4 s c1 0 0
y4 A1 = Rot ( z, q1) = 1
x4 z3 0 0 1 0
q4  
0 0 0 1
y3
z2 x O3  c2 0 s2 0
3  0 1 0 0 
A 2 = Tran (0,0, L1 )Rot ( y, q2 ) =
− s2 0 c2 L1 
z z1  
q3  0 0 0 1
q2
1 0 0 0 
y2 0 1 0 L2 
A3 = Tran (0, L2 , L3 + q3 ) =
O2 0 0 1 L3 + q3 
 
x2 q O0 y1 0 0 0 1 
1
O1 y  c4 0 s4 s4 L4 
 0 1 0 0 
x A 4 = Rot ( y, q4 )Tran (0,0, L4 ) = 
x1 − s4 0 c4 c4 L4 
 
 0 0 0 1 

Stanford-transformacije 19
 FSB
0
T4 = A1 A 2 A3 A 4

c1c24 − s1 c1s24 c1s2 ( L3 + q3 ) + c1s24 L4 − s1 L2 

s c c1 s1s24 s1s2 ( L3 + q3 ) + s1s24 L4 + c1 L2 
0
T4 =  1 24
 − s24 0 c24 L1 + c2 ( L3 + q3 ) + c24 L4 
 
 0 0 0 1 

qi=0 nulti položaj

1 0 0 0 
0 1 0 L2 
0
T4 =  
0 0 1 L1 + L3 + L4 
 
0 0 0 1 

Stanford-kinematika 20
 FSB

L1=0.5 m
L2=0.15 m
L3=0.3 m
L4=0.15 m − 0.469 − 0.342 0.814 0.34257
 − 0.171 0.939 0 .296 0.28431
0
T4 =  
− 0.866 0.000 − 0.500 0.76972
 
q1=20°  0 0 0 1 
q2=40°
q3=0.15 m
q4=80°

Stanford-primjer 21
 FSB
Opis robota i okoline
z
SBG = ET6

T6 G
E
B
S

T6 = E-1SBG
x 22
Opis robota
 FSB
Definiranje okoline kamerom
z
KDG = ET6

T6 G
E

D
K

T6 = E-1KDG
x 23
Opis kamerom
 FSB
Eulerovi kutevi
 nx ox ax px 
n oy ay p y  nx = cosϑ cos ϕ cosψ − sin ϑ sinψ
0
Tn =  y
 nz oz az pz  n y = sin ϑ cos ϕ cosψ + cosϑ sinψ
 
0 0 0 1
nz = sin ϕ cosψ
n = o = a =1
n×o = a ox = − cosϑ cos ϕ sinψ − sin ϑ cosψ
n
o = o
a = a
n = 0
o y = − sin ϑ cos ϕ sinψ + cosϑ cosψ
Rot ( z ,ϑ )Rot ( y, ϕ )Rot ( z ,ψ ) oz = sin ϕ sinψ

υ - skretanje
ϕ - nagib ax = cosϑ sin ϕ
ψ - valjanje a y = sin ϑ sin ϕ
az = cos ϕ
Eulerovi kutevi 24
 FSB
Eulerovi kutevi za Stanford manipulator

ϑ = q1
ϕ = q2 + q4 = q24 ϕ = q24
ψ =0

Eulerovi kutevi za S. man. 25

 FSB
Eulerovi kutevi – inverzna rješenja

 ay  ATAN2
ϑ = arctan  y
 ax 
ϕ = arccos(az )
- +
 − nz 
ψ = arccos  x
 sin ϕ  + -

ϑ = ATAN2(a y , ax )
ϕ = ATAN2(ax cosϑ + a y sin ϑ , az )
ψ = ATAN2(-nx sin ϑ + n y cosϑ , - ox sin ϑ + o y cosϑ )

Euler-inverzni 26
 FSB
Kvaternion
1
k1 = nx + o y + a z + 1
2
1
k2 = nx − o y − a z + 1 ⇒ sign k2 = sign (oz -a y )
2
1
k3 = o y − nx − a z + 1 ⇒ sign k3 = sign (a x-nz )
2
1
k4 = a z − nx − o y + 1 ⇒ sign k4 = sign (n y -ox )
2

k12 + k22 + k32 + k42 = 1

referentni sustav
1 0 0 0 k1 = 1
0 
1 0 0 k2 = 0
n
An =  k3 = 0
0 0 1 0
  k4 = 0
0 0 0 1
Kvaternion 27
 FSB
Unutarnje i vanjske koordinate

♦ vektor unutarnjih koordinata q = [q1 q2 q3 q4 q5 q6 ]T

♦ vektor vanjskih koordinata r = [ px py pz ϑ ϕ ψ ]T

♦ Stanford manipulator
q = [q1 q2 q3 q4 ]T ϑ = q1
ϕ = q2 + q4
r = [ px py pz ϕ ]T ψ =0

Unutarnje i vanjske ko. 28

 FSB
Inverzni kinematički problem

♦ poznato T=[ n o a p ]T ili r=[ px py pz υ ϕ ψ ]T

♦ traži se q=[ q1 q2 q3 q4 q5 q6 ]T

(x,y) x = L1 cos q1 + L2 cos(q1 + q2 )

y y = L1 sin q1 + L2 sin(q1 + q2 )
L2
q2 q1 = ?
q2 = ?
L1
q1
x
Inverzni KP 29
 FSB

Upute za rješavanje

♦ rješenje mora biti jednoznačno

♦ ne postoji univerzalni algoritam ali postoji postupak
♦ postupak ovisi od strukture robota
♦ izbjegavati dijeljenje sa sinq i cosq

♦ obratiti pažnju na karakteristike programskog

jezika u kojem se rješava problem

Upute 30
 FSB
Postupak

(0 A1 )−1 0 T6 = 1T6

(1A 2 ) −1 (0 A1 )−1 0 T6 = 2 T6

( 2 A3 ) −1 (1A 2 ) −1 (0 A1 ) −1 0 T6 = 3T6

(3 A 4 ) −1 (2 A3 ) −1 (1A 2 ) −1 (0 A1 )−1 0 T6 = 4 T6

( 4 A5 )−1 (3 A 4 ) −1 (2 A3 )−1 (1A 2 )−1 (0 A1 ) −1 0 T6 = 5 T6

0
T6 = 0 T6

Postupak 31
 FSB
IKP za Stanford manipulator
(0 A1 )−1 0 T4 = 1T4
 c1 s1 0 0  nx ox ax px   c24 0 s24 s2 ( L3 + q3 ) + s24 L4 
− s c 0 0 n y oy ay p y   0 1 0 L2 
 1 1 = 
 0 0 1 0  nz oz az pz  − s24 0 c24 L1 + c2 ( L3 + q3 ) + c24 L4 
    
 0 0 0 1  0 0 0 1  0 0 0 1 

 f11(n) f11 (o) f11(a) f11 (p)   c24 0 s24 s2 ( L3 + q3 ) + s24 L4 

 f (n) f12 (o) f12 (a) f12 (p)  0 1 0 L2 
 12 = 
 f13 (n) f13 (o) f13 (a) f13 (p)  − s24 0 c24 L1 + c2 ( L3 + q3 ) + c24 L4 
   
 0 0 0 1   0 0 0 1 
f11 = c1x + s1 y
f12 = − s1x + c1 y
f13 = z
IKP - Stanford 32
 FSB

(2,4) = (2,4) − s1 px + c1 p y = L2

supstitucija px = r cosη r= px2 + p 2y

p y = r sin η  py 
η = arctan 
 px 

− sin q1 cosη + cos q1 sin η = L2 / r

sin(η − q1) = L2 / r L2
tan(η − q1) =
cos(η − q1) = 1 − ( L2 / r )2 ± r 2 − L22

 py   
 − arctan 
L2
q1 = arctan
 
 px   + r 2
− L2
2 
IKP – Stanford 1 33
 FSB

 ay 
(2,3) = (2,3) q1 = arctan 
 ax 

 ny 
(2,1) = (2,1) q1 = arctan 
 nx 

q24 = ϕ

IKP – Stanford 2 34
 FSB
(1,4) = (1,4) c1 px + s1 p y = s2 ( L3 + q3 ) + s24 L4 (*)

(3,4) = (3,4) p z = L1 + c2 ( L3 + q3 ) + c24 L4 (**)

c1 px + s1 p y − s24 L4
iz (*) s2 =
L3 + q3
pz − L1 − c24 L4
iz (**) c2 =
L3 + q3

 c1 p x + s1 p y − s24 L4 
q2 = arctan 
 pz − L1 − c24 L4 

q4 = q24 − q2 = ϕ − q2
IKP – Stanford 2 35
 FSB

( 1 A 2 ) −1 ( 0 A1 ) −1 0
T4 = 2 T4

c2 0 − s2 s2 L1   f11 (n) f11 (o) f11 (a) f11(p)   c4 0 s4 s4 L4 

0 1 0 0   f12 (n) f12 (o) f12 (a) f12 (p)  0 1 0 L2 
 = 
 s2 0 c2 − c2 L1   f13 (n) f13 (o) f13 (a) f13 (p)  − s4 0 c4 L3 + q3 + c4 L4 
    
0 0 0 1  0 0 0 1   0 0 0 1 

 f 21(n) f 21(o) f 21(a) f 21(p)   c4 0 s4 s4 L4 

 f (n) f 22 (o) f 22 (a) f 22 (p)  0 1 0 L2 
 22 = 
 f 23 (n) f 23 (o) f 23 (a) f 23 (p) − s4
 0 c4 L3 + q3 + c4 L4 
   
 0 0 0 1   0 0 0 1 

f 21 = c2 (c1x + s1 y ) − s2 z {+ s2 L1}p
f 22 = − s1x + c1 y
f 23 = s2 (c1x + s1 y ) + c2 z {− c2 L1}p
IKP – Stanford 3 36
 FSB

(3,4) = (3,4) s2 (c1 px + s1 p y ) + c2 pz − c2 L1 = L3 + q3 + c4 L4

q3 = s2 (c1 px + s1 p y ) + c2 ( pz − L1) − c4 L4 − L3

♦ rješenje IKP-a za Stanford manipulator

 py   
q1 = arctan  − arctan  L2 
 
 px   + r 2
− L2
2 
q24 = ϕ
 c1 p x + s1 p y − s24 L4 
q2 = arctan 
 pz − L1 − c24 L4 
q4 = q24 − q2

q3 = s2 (c1 px + s1 p y ) + c2 ( pz − L1) − c4 L4 − L3
IKP – Stanford 4 37
 FSB
Jacobieva matrica

r = f (q) ∂px ∂px ∂px ∂px ∂px ∂px

∂q1 ∂q2 ∂q3 ∂q4 ∂q5 ∂q6
∂r ∂p y ∂p y ∂p y ∂p y ∂p y ∂p y
J=
∂q
∂q1 ∂q2 ∂q3 ∂q4 ∂q5 ∂q6
∂pz ∂pz ∂pz ∂pz ∂pz ∂pz
J= ∂q1 ∂q2 ∂q3 ∂q4 ∂q5 ∂q6
∂ϑ ∂ϑ ∂ϑ ∂ϑ ∂ϑ ∂ϑ
∂q1 ∂q2 ∂q3 ∂q4 ∂q5 ∂q6
∂ϕ ∂ϕ ∂ϕ ∂ϕ ∂ϕ ∂ϕ
∂q1 ∂q2 ∂q3 ∂q4 ∂q5 ∂q6
∂ψ ∂ψ ∂ψ ∂ψ ∂ψ ∂ψ
∂q1 ∂q2 ∂q3 ∂q4 ∂q5 ∂q6
Jacobi 38
 FSB

∆r = J (q) ∆q / ∆t

∆r ∆q / lim
= J (q) ∆t →0
∆t ∆t

rɺ = J (q) qɺ

dJ (q)
ɺrɺ = qɺ + J (q) q
ɺɺ
dt

det(J(q)) = 0  singularni položaj robota

Jacobi 1 39
 FSB
Jacobieva matrica Stanford manipulatora
∂px
J11 = = −c1L2 − s1s2 ( L3 + q3 ) − s1s24 L4 = − p y
∂q1

∂px
J12 = = c1c2 ( L3 + q3 ) + c1c24 L4
∂q2

∂px
J13 = = c1s2
∂q3

∂px
J14 = = c1c24 L4
∂q4

Stanford Jacobi 1 40
 FSB

∂p y
J 21 = = − s1L2 + c1s2 ( L3 + q3 ) + c1s24 L4 = px
∂q1

∂p y
J 22 = = s1c2 ( L3 + q3 ) + s1c24 L4
∂q2

∂p y
J 23 = = s1s2
∂q3

∂p y
J 24 = = s1c24 L4
∂q4

Stanford Jacobi 2 41
 FSB

∂pz ∂ϕ
J 31 = =0 J 41 = =0
∂q1 ∂q1
∂pz
J 32 = = s2 ( L3 + q3 ) − s24 L4 ∂ϕ
∂q2 J 42 = =1
∂q2
∂pz
J 33 = = c2 ∂ϕ
∂q3 J 43 = =0
∂q3
∂pz
J 34 = = − s24 L4 ∂ϕ
∂q4 J 44 = =1
∂q4

Stanford Jacobi 3 42
 FSB
L1=0.5 m
L2=0.15 m
L3=0.3 m r = [0.3425 0.2843 0.7697 2/3π ]T
L4=0.15 m

− 0.282 0.253 0.604 − 0.070

 0.342 0.119 0.220 − 0 .026 
q1=20° J= 
 0 − 0.284 0.766 − 0.130
q2=40°  
q3=0.15 m  0 1 0 1 
q4=80°

∆r = [0.003 0.003 0.003 0]T

∆q = [0.0040 0.0040 0.0047 − 0.0040]T

[
∆q = 0.229



0.229 4.7mm - 0.229
T
]
Primjer 43
 FSB
Rješavanje IKP iteracijom

Saznati približno rješenje q* (kojemu odgovara r*)

 Izračunati pogrešku ∆r = r - r*.
Ako je ∆r < ε rješenje je q= q* . Stop.
 Prebaciti pogrešku ∆r u unutarnje koordinate
∆q = J-1 ∆r

 Popraviti približno rješenje

q* = q* + ∆q

 Izračunati novo približno rješenje iz DKP-a

r*= f (q*)
Iteracija
Korak 2. 44
 FSB
Kriteriji voñenja robota

♦ Voñenje točka-točka (slobodni prijelaz i slijedno voñenje)

♦ Voñenje po vektoru položaja (i orijentacije)

♦ Voñenje po vektoru brzine

♦ Voñenje po vektoru ubrzanja

♦ Voñenje po vektoru sile/momenta

♦ Voñenje po kriteriju minimalnog utroška energije

Kriteriji voñenja 45
 FSB

Voñenje točka-točka “slobodni prijelaz”

qA=[ q1 q2 q3 q4 q5 q6 ]T  q B=[ q1 q2 q3 q4 q5 q6 ]T

Slobodni prijelaz 46
 FSB
q1 q1B q2
q2B

q2A
q1A

τ1 t
τ2 t
qɺ1
∆qi qɺ2
τi = 2
qɺɺi.max

τ1 t
τ2 t
qɺɺ1.max
qɺɺ2.max

τ1 t
τ2 t

Slobodni prijelaz 1
τ1 ≠ τ2 ≠ τi 47
 FSB

Voñenje točka-točka “slijedno voñenje”

qA=[ q1 q2 q3 q4 q5 q6 ]T  qB=[ q1 q2 q3 q4 q5 q6 ]T

Slijedno voñenje 48
 FSB
q1 q1B q2
q2B

q2A
q1A

τ1 t τ2 t
qɺ1 qɺ2

τ1 t τ2 t
qɺɺ1 4∆qi qɺɺ
qɺɺi = 2 2
τ
τ1 t τ2 t

Slijedno voñenje 1
τ1 = τ2 = τi = τ 49
 FSB
Voñenje po vektoru položaja
z

O y

x
izmeñu točaka: voñenje točka-točka slijednog tipa
Voñenje po položaju 50