FSB

ROBOTIKA
Kinematika robota
 FSB
Transformacija koordinata u ravnini - translacija
y y1
 x1v   px 
yv y1v Poznato v1 =  v  p =  
 y1   py 

 xv 
Traži se v =  v
y 
py
O1 x1v x1

O0 px xv x

 x1v   x v  1 0 px   x1v 
 x   x   px 
v v
 x  1 0
v
px   v   v   
 v =  +p   v  = 0 1  y  = 0 1 p y   y1v 
1
p y   1 
v
y
y  y   y y   1  1  0 0 1   1 
1
   
 FSB
Transformacija koordinata u ravnini - rotacija
y1
y y1v cos ϕ
 x1v 
yv Poznato v1 =  v  ϕ
 y1 
ϕ x1
x1v
 xv 
v
Traži se v =  v
y 1 x1v sin ϕ y 
ϕ
O0 O1 xv x
y sin ϕ
v

x cos ϕ
v 1
1

 x v  cos ϕ − sin ϕ 0   x1v 

 x v  cos ϕ − sin ϕ   x1v   v   
 v =     y  =  sin ϕ cos ϕ 0   y1v 
 y   sin ϕ cos ϕ   y1v   1   0
  0 1   1 
 FSB
Transformacija koordinata u ravnini - rotacija
y1
y

yv

x1v x1

y1v
ϕ
O0 O1 xv x

 x v  cos ϕ − sin ϕ 0   x1v 

 x v  cos ϕ − sin ϕ   x1v   v 
= ϕ ϕ   yv 
 v =   v
y
   sin cos 0  1 
y sin ϕ cos
  1442443  y1 ϕ   1   1 
 1  1440 0
424443
R
A
 x1v   cos ϕ sin ϕ   x v   x1v   cos ϕ sin ϕ 0   x v 
 v =   v  v    yv 
 − sin ϕ ϕ = − ϕ ϕ
2443  
y
 1  144 cos y y
 1  sin cos 0  
R−1  1   0 0 1   1 
  14442444 3
A−1
 FSB
Transformacija koordinata u ravnini – translatacija i rotacija

 x v  cos ϕ − sin ϕ px   x1v 

 v    yv 
y
   = sin ϕ cos ϕ p y 1 
 1   0 0 1   1 
  144424443
A
 FSB
Transformacija koordinata u ravnini – translatacija i rotacija

 x v  cos ϕ − sin ϕ px   x1v   x v  ix jx px   x1v 

 v    yv   v    yv 
y
   = sin ϕ cos ϕ p y 1 
y
  y = i j y p y 1 
 1   0 0 1   1   1   0 0 1   1 
  144424443   14 4244 3 
A A
A = matrica transformacija
 FSB

Homogene transformacije
♦ desnokretni pravokutni koordinatni sustav

k
O
j y
i
x
 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 o j = jo k = k o i = 0  
0 0 0 1
 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
 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 −poi 
j − p o j 
(m −1
)
An =n Am =  x
k x
jy
ky kz
jz
− p o k
 
0 0 0 1 
Domaća zadaća: provjeriti (dokazati) gornju relaciju
 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
 
0 0 0 1
 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
 
0 0 0 1
 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
 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
 FSB
Matrica transformacija
♦ upotrebljavat ćemo samo relativne transformacije
p 0 = 0 A1 p1
p 0 = 0 A11 A 2 p 2
p1 = 1A 2 p 2
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
 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
 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 
 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
 FSB
Matrica prihvata
z

a n= vektor normale
O o = vektor orijentacije
n o y a = vektor djelovanja
x p = vektor položaja

 nx ox ax px 
n p y 
n o a p   oy ay
0
Tn =   = y
 0 0 0 1   nz oz az pz 
 
0 0 0 1
 FSB
RRTR struktura robota
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 qi=0 nulti položaj
 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 
 FSB
0
T4 = A1 A 2 A3 A 4

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

s c c1 s1s24 s1s2 ( L3 + q3 ) + s1s24 L4 + c1L2 
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 
 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°

Rješenje direktnog kinematičkog problema

 FSB
Opis robota i okoline
z SBG = ET6
y

T6 G
E
B
S

T6 = E-1SBG
x
 FSB
Definiranje okoline kamerom
z
KDG = ET6
y

T6 G
E

D
K

T6 = E-1KDG
x
 FSB

z
Eulerovi kutevi
O
a n= vektor normale
n o y o = vektor orijentacije
x a = vektor djelovanja
p = vektor položaja
 FSB

z
Eulerovi kutevi
O
a n= vektor normale
n o y o = vektor orijentacije
x a = vektor djelovanja
p = vektor položaja

skretanje
posrtanje
valjanje
 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ψ
noo = ooa = aon = 0
o y = − sin ϑ cos ϕ sinψ + cosϑ cosψ
Rot ( z ,ϑ ) Rot ( y, ϕ )Rot ( z ,ψ ) oz = sin ϕ sinψ

υ - skretanje ax = cosϑ sin ϕ

ϕ - nagib a y = sin ϑ sin ϕ
ψ - valjanje az = cos ϕ
 FSB
Eulerovi kutevi za RRTR robota
z4
O4
x4 y4
z3
q4
y3
z2 x O3
3

z z1 ϑ = q1
q3
q2 ϕ = q2 + q4 = q24
y2 ψ =0
O2
x2 q O0 y1
1
O1 y

x
x1
29
 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ϑ )
 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
 FSB

Unutarnje i vanjske koordinate

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

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

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

Direktan kinematički problem q → r

Inverzan kinematički problem r → q

 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
 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
 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
 FSB
IKP za RRTR robota
(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
 FSB

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

px = r cosη r= px2 + p 2y
supstitucija
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   
q1 = arctan  − arctan  L2 
 
 px   + r 2
− L2
2 
 FSB

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

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

q24 = ϕ
 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 px + s1 p y − s24 L4 
q2 = arctan 
 p z − L1 − c24 L4 

q4 = q24 − q2 = ϕ − q2
 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
 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 RRTR robota

 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
 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
 FSB

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

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

r& = J (q) q& q& = J −1 (q) r&

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

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

 FSB
Jacobieva matrica RRTR robota
∂p x
J11 = = −c1L2 − s1s2 ( L3 + q3 ) − s1s24 L4 = − p y
∂q1

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

∂px
J13 = = c1s2
∂q3

∂px
J14 = = c1c24 L4
∂q4
 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
 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
 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.229o 0.229o 4.7mm - 0.229o ] T
 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*)
Korak 2.
 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

 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
 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
τ1 ≠ τ2 ≠ τi
 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
 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

τ1 = τ2 = τi = τ
 FSB
Vođenje po vektoru položaja
z

O y

x
između točaka: vođenje točka-točka slijednog tipa