4.

Direktna i inverzna kinematika

• zadavanje pokreta objekta
• KINEMATIKA
– pokreti objekata neovisni o silama i brzinama promatranih objekata
– položaj objekta je funkcija kutova između segmenata objekta
– interpolacija između zadanih ključnih položaja
– hijerarhijski modeli (model čovjeka, životinja, sustav planeta)
• DINAMIKA
– djelovanje sila u ostvarivanju pokreta (gravitacija, inercija …)
– fizikalno temeljeni algoritmi

• objekti kao što su ljudi ili životinje imaju krutu unutrašnju strukturu i
mišićnu strukturu izvana, pa se na taj način i grade
• Mr. Bones.mht kolizije: http://www.planetdan.net/pics/misc/georgie.htm

Ž. M, ZEMRIS, FER 4-1

 4.1 Hijerarhijski kinematički modeli

Struktura
• organizacija elemenata u stablastu strukturu
Artikuliranje
Manipulator
• promjene u orijentaciji elemenata strukture
Robotika
• većina termina preuzeta iz robotike Segment - čvor
Manipulator (link – node)
• zglob – ne uzimamo dimenziju u obzir
Zglob - grana
• segment – kruti element (joint - arc)
• niz segmenata zglobovima povezanih u lanac
• ponekad se zglob i segment promatraju kao
cjelina, odnosno jedan zglobni element vrh manipulatora

Ž. M, ZEMRIS, FER 4-2

 Vrste zglobova

Stupanj slobode u zglobu (DOF)

• jedan rotacijski stupanj slobode u zglobu (engl. Revolute Joint)
• jedan translacijski stupanj slobode u zglobu (engl. Prismatic Joint)
Složeni zglobovi
• povezivanje n 1DOF zglobova sa segmentima veličine nula

Ž. M, ZEMRIS, FER 4-3

Orijentacijski stupanj slobode u zglobu (uz ograničenje kutova)

1DOF 2DOF 3DOF

Ž. M, ZEMRIS, FER 4-4

 Izgradnja hijerarhijskih modela
• objekt
• apstraktan reprezentacija objekta
• stablo (čvor – segment, luk-zglob)
početna tranformacija

segment 0 čvor

luk tranformacija

segment 1

objekt apstraktna reprezentacija stablo

Ž. M, ZEMRIS, FER 4-5

lokalni koordinatni sustavi:

y y
V’0=T0V0 T1.1
V0
y
T0
T1
segment 0 x x
T0
y y
V’1=T0T1V1
Nizanje transformacija
V1
T1
T0
segment 1 x x segment 0
T1
y y
V’1.1=T0T1T1.1V1.1 segment 1

V1.1 T1.1
T1.1
segment 1.1
x x
segment 1.1
Stablo
Ž. M, ZEMRIS, FER 4-6
• hijerarhijski model
– ishodišta lokalnih koordinatnih sustava određuju se na osnovi osi rotacije u pojedinom
zglobu (os je obično poravnata s x, y ili z osi)
– zglob s 2 i više stupnja slobode ostvaruje se kao niz segmenata duljine 0 s 1 stupnjem
slobode
– mogu se koristiti višestruko referenciranje za pridjeljivanje svojstava materija pojedinih
segmenata (boja, materijal, tekstura, vidljivost, oblik) ili se atributu nasljeđuju
– struktura čvorova i lukova može biti načinjena odvojeno, tako da se pokreti ostvaruju
nezavisno o izgrađenom modelu (isti pokreti se izvode na različitim modelima,
kloniranje pokreta)

• kada izgradimo hijerarhijski model, načinili smo pasivni strukturni model

izgrađeni model je kao lutka bez lutkara
– potrebno je definirati gibanje koje će “prirodno” izgledati, vrlo težak problem
– tehnikama niže i više razine definiramo gibanje
• kontrola pojedinačnog pokreta zadavanjem kutova u zglobovima
• kontrola pokreta zadavanje putanja koju prati kraj manipulatora
• zadavanje naredbi oblika: sjedni, šeći, čučni …
robot: http://prt.fernuni-hagen.de/lehre/KURSE/PRT001/course_main/node19.html
http://accad.osu.edu/~pgerstma/class/vnv/examples/ik/robot.wrl

Ž. M, ZEMRIS, FER 4-7

• nakon definiranja statičkog položaja objekta definiramo pomake u zglobu:
– rotacija se obavlja prije konstantne (lokalne) transformacije u luku
– grananje stabla – više jednostrukih lanaca
– nasljeđivanje transformacija s roditelja na djecu
– jednostavni pokreti: http://mrl.nyu.edu/~harper/CompGraph/6/Animation3.html

T0
Θ1.1 segment 0

T1
R1(Θ1)

Θ1 segment 1 V’1=T0T1 R1(Θ1) V1

y T1.1
R1.1(Θ1.1)

T0 segment 1.1 V’1.1=T0T1 R1(Θ1) T1.1 R1.1(Θ1.1) V1.1

x
Stablo uz pomake
Ž. M, ZEMRIS, FER 4-8
 4.2 Kinematika
• DIREKTNA
– na osnovi stabla i zadanih parametara u zglobovima određuje se položaj
– transformacijom Ti odreñen je položaj objekta Vi+1 u sustavu Oi roditelja
– (stog s trenutnom transformacijskom matricom, rekurzivni pozivi)
• INVERZNA
– vrh manipulatora treba pratiti zadanu putanju, kutovi u zglobovima se računaju
– inverznom transformacijom Ti-1 odreñen je položaj Vi u sustavu Oi+1

y’

Vi+1(x’, y’)=(2 1)
y
Ti=(5 4) Oi+1 x’

Vi(x,y)=(2 1) Vi +1 u sustavu Oi Vi +1 ( x, y ) = Vi ( x, y ) Ti = (7 5)
Oi x Vi u sustavu Oi +1 Vi (x′, y′) = Vi +1 (x′, y′) Ti−1 = (− 3 − 3)

Ž. M, ZEMRIS, FER 4-9

 – točka Vi u sustavu Oi ima koordinate npr. Vi =(1 3)
 cos(ϕ ) sin (ϕ ) 0
Vi +1 u sustavu Oi Vi +1 ( x, y ) = Vi ( x, y )R i = (1 3 1) − sin (ϕ ) cos(ϕ ) 0
 0 0 1
Vi u sustavu Oi +1 Vi′( x′, y′) = Vi +1 ( x′, y′) R i−1

- rotacija točke matricom R, rotira vektor R-1

Vi+1(x’,y’)=(1 3) Vr = Vi R
y Vi(x,y)=(1 3) r r
x’ x ' = R −1 x
y’
- rotiramo jedinične vektore koji razapinju koordinatni
sustav
Oi x r r
x ' = R −1 x  1 0 
r r [xr ' y '] = R −1 [x
r r
y ] = R −1      
r
y ' = R −1 y  0 1 
– ako znamo novi koordinatni sustav – znamo matricu rotacije
• koordinatne osi x’ i y’ odreñuju matricu rotacije (osi x, y daju jediničnu matricu)
• lako odredimo za transformirani sustav gdje je točka u prethodnom sustavu
– npr. Vi+1 =(1, 3) onda je Vi+1 R-1 u sustavu Oi+1

Ž. M, ZEMRIS, FER 4-10

 y
Npr. U koordinatnom sustavu Oxy točka T ima
koordinate T(x,y)=(3 4). Koordinatni sustav v y T
zarotiramo za ϕ (točku nismo rotirali). U
rotiranom koordinatnom sustavu točka ima u
v u
koordinate T(u, v) = (4 3). Odrediti kut ϕ
rotacije ϕ .
O x x

Ž. M, ZEMRIS, FER 4-11

 Denavit-Hartenberg-ova notacija
• robotika – 4 parametra
• relacija između koordinatnih sustava roditelja i djeteta
z - os oko koje se obavlja rotacija
x - os određuje segment (engl. link)
y – os je z × x
duljina segmenta ai – udaljenost okomice izmeñu zi do zi+1
kut zgloba θ i+1 – kut izmeñu xi i xi+1 oko osi zi+1

Ž. M, ZEMRIS, FER 4-12

 Denavit-Hartenberg-ova notacija

duljina segmenta ai-1 – duljina okomice izmeñu zi-1 do zi

pomak segmenata di – duljina duž zi osi izmeñu sjecišta
- zi sa ai-1 i zi sa ai
kut uvijanja αi-1 – kut izmeñu zi-1 i zi oko osi xi-1
kut zgloba θ i – kut izmeñu xi-1 i xi oko osi zi
indeksi!

Θi

Ž. M, ZEMRIS, FER 4-13

 • transformacija točke iz sustava Oi u sustav Oi-1:
Vi −1 = T Vi

T = Tx (ai −1 ) Rx (α i −1 )Tz (d i ) Rz (θ i )

1 0 0 ai −1  1 0 0 0 1 0 0 0  cos(θ i ) − sin (θ i ) 0 0
0 1 0 0  0 cos(α i −1 ) − sin (α i −1 ) 0 0 1 0 0   sin (θ i ) cos(θ i ) 0 0
T =
0 0 1 0  0 sin (α i −1 ) cos(α i −1 ) 0  0 0 1 di   0 0 1 0
    
0 0 0 1  0 0 0 1  0 0 0 1 0 0 0 1

 cos(θ i ) − sin (θ i ) 0 ai −1 
cos(α )sin (θ ) cos(α ) cos(θ ) − sin (α ) − d sin (α )
T= i −1 i i −1 i i −1 i i −1 

 sin (α i −1 )sin (θ i ) sin (α i −1 ) cos(θ i ) cos(α i −1 ) d i cos(α i −1 ) 

 
 0 0 0 1 

Ž. M, ZEMRIS, FER 4-14

 Inverzna kinematika

– željena pozicija i orijentacija vrha manipulatora (npr. glodalica) je

zadana, a treba odrediti kutove u zglobovima da bi se to ostvarilo
http://www.zemris.fer.hr/predmeti/rg/diplomski/02Perkovic/IK.html
http://www.zemris.fer.hr/predmeti/rg/diplomski/02Perkovic/IK3d.html

– rješenja - niti jedno

- jedno
- puno rješenja
- postavljaju se ograničenja u zglobovima (predimenzioniran sustav,
poddimenzioniran sustav)
- zadaju se dva položaja vrha manipulatora koje je potrebno
ostvariti, a zatim između ta dva položaja načiniti interpolaciju
kutova u zglobovima

Ž. M, ZEMRIS, FER 4-15

 Radni prostor

• dohvatljiv radni prostor (engl. reachable workspace)

- spretno dohvatljiv radni prostor (engl. dexterous workspace)
- prostor dohvatljiv uz razne orijentacije

L2
L1

(L1 − L2 ) ≤ radni prostor ≤ (L1 + L2 )

Ž. M, ZEMRIS, FER 4-16

NPR: Određivanje kutova Θ1 i Θ2 iz zadanog položaja vrha manipulatora V(xe, ye)

• poučak o kosinusima Θ B
A
C = A + B − 2 AB cos (θ )
2 2 2

C
• trebamo odrediti Θ1 i Θ2 , dva rješenja
cos(θ ) =
xe
2 2
xe + ye
2 2
L12 + xe + ye − L22 Θ2
cos(θ1 − θ ) =
2
2 L1 xe + ye
2 L2
L1 180-Θ2
cos(180 − θ 2 ) =
L12 + L22 − xe + ye( 2 2
) Θ1
V(xe, ye)
2 L1 L2 x +y
2
e
2
e

• direktna kinematika Θ
O(0, 0)
 xe   L1 cos θ1 + L2 cos(θ1 + θ 2 )
V = f (θ1 , θ 2 )  y  =  L sin θ + L sin (θ + θ ) 
 e  1 1 2 1 2 

• inverzna kinematika
θ1   g x (xe , ye )
Φ =   = g( xe , ye ) =  
θ 2   g y ( xe , ye )
Ž. M, ZEMRIS, FER 4-17
• u općem slučaju složenog sustava imat ćemo puno rješenja
• u zglobovima je potrebno još postaviti ograničenja, odnosno minimalni i
maksimalni kut čime je zadan raspon pojedinog zgloba
• u animaciji treba kontinuirano pratiti cilj – iterativno numeričko rješenje
• matematički alat
– upotreba Jakobijeve matrice, rotacija je sadrži funkcije sin i cos, a to znači
da je problem nelinearan. Jakobijevom maticom lineariziramo problem
promatranjem prve derivacije (prvi član Taylor-ovog reda).
– transponirana Jakobijeva matrica
– CCD algoritam

Ž. M, ZEMRIS, FER 4-18

Jednostavni primjer kinematičkog lanca:
• Θi … ulazne varijable, kutovi u zglobovima (translacija u zglobovima)
• xe, ye … pozicija vrha manipulaora (orijentacija u vrhu manipulatora)

 xe  θ1   f1 (θ1 , θ 2 ) xe = f1 (θ1 ,θ 2 )

Y =  , X =  , F = 
 ye  θ 2 

 f 2 (θ1 ,θ 2 ) ye = f 2 (θ1 ,θ 2 )

Direktna kinematika:
Y = F(X )

Inverzna kinematika Θ2
L2
X= F −1
(Y ) L1
V(xe, ye)
• funkcije fi su nelinearne funkcije (sin, cos) Θ1
– analitički definiran F-1
– numeričko rješavanje – Jakobijeva matrica O(0, 0)

Ž. M, ZEMRIS, FER 4-19

 Jakobijeva matrica
y1 = f1 ( x1 , x2 , x3 , x3 , x5 , x6 )
∂F ∂( f1 , f 2 , ... f n )
y2 = f 2 ( x1 , x2 , x3 , x4 , x5 , x6 ) Y = F(X ), =
∂X ∂ ( x1 , x2 , ... x6 )
...
yn = f n ( x1 , x2 , x3 , x4 , x5 , x6 )
• opis promjene izlaznih varijabli obzirom na promjenu ulaznih varijabli
• malo promijenimo ulaznu varijablu i promatramo što će se dogoditi s izlaznom
varijablom
∆Y ∂F
∂f ∂f ∂f ∂f ∂f ∂f ≈
∂y1 = 1 δx1 + 1 δx2 + 1 δx3 + 1 δx4 + 1 δx5 + 1 δx6 ∆X ∂X
∂x1 ∂x2 ∂x3 ∂x4 ∂x5 ∂x6 ∂F
J=
... ∂X
∂f i ∂f ∂f ∂f ∂f ∂f ∆Y ≈ J ∆X
∂yi = δx1 + i δx2 + i δx3 + i δx4 + i δx5 + i δx6
∂x1 ∂x2 ∂x3 ∂x4 ∂x5 ∂x6
... Ž. M, ZEMRIS, FER 4-20
• Θi … ulazne varijable, kutovi u zglobovima
• ω = & i … kutne brzine u zglobovima
• V … vektor linearnih i rotacionih brzina na vrhu manipulatora
• J(θ) … Jakobijeva matrica - Jakobijan

[
V = vx vy vz ω x ω y ω z ]τ

 ∂f1 ∂f1 ∂f1 

 ∂θ ...
∂θ 2 ∂θ n 
 1 
 ∂f 2 ∂f 2
...
∂f 2 
V = J (θ ) &
J (θ ) =  ∂θ1 ∂θ 2 ∂θ n 
 ... ... ... 
 
 ∂f 6 ∂f 6 ∂f 6 
 ∂θ1 ∂θ 2 ∂θ n 

Ž. M, ZEMRIS, FER 4-21

• pojedini članovi Jakobijana odreñuju doprinos pojedinih zglobova promatranoj
promjeni na vrhu manipulatora npr. vy.
• važno je da su sve koordinatne vrijednosti u istom koordinatnom sustavu
• vektorski produkt osi zi oko koje se obavlja rotacija u zglobu
i vektora od vrha segmenta do vrha manipulatora pi odreñuje
doprinos komponenti brzine gi
∂vx
• u primjeru zi - osi su okomite
na ravninu prikaza G ∂θ1
g1
g2
• aproksimacija g3
E
∆Y ≈ J ∆X z2 ⊗ p2
vrijedi samo lokano pa se u malim L3
L1
koracima treba približavati rješenju L2 ⊗
p1 z3
(iterativno)
Θ1
df ( x ) ⊗
f ( x + ∆x ) ≈ f ( x ) + ∆x z1
dx

Ž. M, ZEMRIS, FER 4-22

• pojedini članovi Jakobijana odreñuju doprinos pojedinih zglobova
promatranoj promjeni na vrhu manipulatora npr. vy.

z1 = (0 0 1) z 2 = (0 0 1) z3 = (0 0 1)

(G − E )x  ( z1 × p1 )x (z2 × p2 )x (z3 × p3 )x  θ&1 

(G − E )  = ( z × p ) (z2 × p2 )y (z3 × p3 )y  θ&2  V = J (θ ) &
 y  1 1 y
 (G − E )z  ( z1 × p1 )z (z2 × p2 )z (z3 × p3 )z  θ&3 
G ciljna točka, E točka u kojoj se nalazimo
• Jakobijevu matrica potrebno je invertirati kako bi došli do rješenja, no ona
nije nužno kvadratna. Tada se određuje pseudo-inverzna matrica tako da se
jednadžba pred-multiplicira s transponiranom Jakobijevom matricom
V=J&
• robot:
JT V = JT J &
• http://www.ee.uwa.edu.au/~braunl/robosim/java/
(J J )
T −1
( ) −1
JT V = JT J JT J &

Ž. M, ZEMRIS, FER
(J J )
T −1
JT V = &
4-23
Pseudo inverzna Jakobijeva matrica J+

• U općem slučaju Jakobijeva matrica je veličine [6×n], pseudo-inverzna

matrica je
(JT J )−1 JT V = &
( ) −1
J + = JT J JT
& = J+V

– jedinstvena,
– nestabilna u blizini singulariteta,
– vremenski zahtjevna za izračunavanje

primjer singulariteta
CILJ

Ž. M, ZEMRIS, FER 4-24

Transponirana Jakobijeva matrica JT

• umjesto pseudo-inverzne matrice može se koristiti transponirana matrica

– korištenje transponirane matrice nije matematički egzaktno, teži ka rješenju,
no sporije konvergira
– računanje nije vremenski zahtjevno
– nema problema sa singularitetima
– nedostatak prema J+ je što promjene u zglobovima nisu ujednačene, obrtni
moment je jači dalje od kraja, pa su promjene disproporcionalne
– javljaju se problemi pri skaliranju
& = JT V

Ž. M, ZEMRIS, FER 4-25

CCD (engl. Cyclic Coordinate Descent)

– jednostavnija ideja, rješavaju se 1DOF problemi duž lanca

– iterativan postupak
– za svaki zglob se traži promjena koja će minimizirati udaljenost do cilja
• 1DOF problemi su jednostavni i imaju rješenje
– nedostatak je neujednačenost promjena u zglobovima
– gibanje često nije glatko, može se zaglaviti, (slično lancu čiji vrh vučemo
po podu)

CILJ

∆x
θ primjer pomaka koji minimizira
udaljenost do cilja

Ž. M, ZEMRIS, FER 4-26

Primjeri primjene
– animiranje likova (skeletal animation)
• model kostura služi kao osnova za dodavanje
kože, vrhovi koji definiraju kožu vežu se uz
pojedine elemente kosti s pripadnim težinama i
sve transformacije za kost primjenjuju se i na
kožu (problem nastaje u području preklapanja)

• model koji sadrži i mišićnu strukturu

http://www.cgcharacter.com/downloads/cgSkinLeg.avi

• jednostavni primjeri gitara

Ž. M, ZEMRIS, FER 4-27

Primjeri primjene
– proračuni i prikaz kod mehaničkih konstrukcija
• određivanje radnog prostora
• simulacija rada
– heksapod (Stewart-ova platforma)
• paralelni manipulatori
• kokpit kod simulatora leta
• CNC uređaji za obradu ploha
• roboti

Ž. M, ZEMRIS, FER 4-28

Primjeri primjene

– direktna kinematika proteina

• aminokiseline → proteini (bjelančevine)
• dihedralni kut – kut izmeñu dvije ravnine
koje određuju 4 atoma
• različiti 3D oblici (konformacije) istog
proteina uz varijaciju dihedralnih kutova
određeni rotacijski DOF na peptidnim vezama,
(proračun minimalne energije),

.pdb zapis dio iz Hemoglobina

ATOM 4384 CD2 HIS D 146
ATOM 4385 CE1 HIS D 146
ATOM 4386 NE2 HIS D 146
ATOM 4387 OXT HIS D 146
TER 4388 HIS D 146
HETATM 4389 FE HEM A 1
HETATM 4390 CHA HEM A 1
HETATM 4391 CHB HEM A 1
HETATM 4392 CHC HEM A 1
HETATM 4393 CHD HEM A 1
HETATM 4394 NA HEM A 1
-4.470 20.439 23.452 1.00 22.44 C
-3.622 22.457 23.406 1.00 28.57 C
-3.890 21.436 24.195 1.00 34.26 N
-2.678 20.431 18.984 1.00 24.06 O
15.703 15.811 15.254 1.00 18.90 FE
17.214 18.452 16.603 1.00 13.89 C
17.793 15.899 12.545 1.00 17.48 C
13.730 13.474 13.636 1.00 15.77 C
12.980 16.329 17.470 1.00 14.95 C
17.206 16.929 14.684 1.00 18.55 N

Ž. M, ZEMRIS, FER 4-29