Download - Inverzna Kinematika Robota
Univerzitet u Kragujevcu
Fakultet inženjerskih nauka u Kragujevcu
Robotika i mehatronika
Inverzna kinematika robota
Mentor: Student:
dr Ilija Nikolić, red.prof. Aleksandar Stanisavljević 368/2011 Nikola Beloica 369/2011 Nikola Gačević 406/2011
Kragujevac, 2012.
2
Sadržaj
1. Uvod ........................................................................................................................................... 3
2. Kinematika robota ...................................................................................................................... 4
2.1 Matrica vektorskog proizvoda i Rodrigues-ova formula konačnih rešenja ................. 4
2.2 Homogene transformacije ............................................................................................ 7
2.3 Veza između unutrašnjih i spoljašnjih koordinata ....................................................... 8
2.4 Elementarne transformacije ......................................................................................... 9
2.4.1 Translacija ..................................................................................................... 9
2.4.2 Transformacija rotacije ............................................................................... 10
2.5 Denavit-Hartenberg-ova notacija ............................................................................... 11
3. Inverzna kinematika robota ...................................................................................................... 17
3.1 Rešivost ...................................................................................................................... 17
3.2 Egzistencija rešenja .................................................................................................... 18
3.3 Višestruka rešenja ...................................................................................................... 19
3.4 Metod rešavanja ......................................................................................................... 21
3.5 Generalisane brzine i ubrzanja ................................................................................... 30
3.7 Jakobijan transformacije ............................................................................................ 34
4. Primeri ...................................................................................................................................... 41
4.1 Primer inverzne kinematike u Robotics Toolbox-u ................................................... 41
4.1.1 Uvod ............................................................................................................ 41
4.1.2 Matrice transformacije ................................................................................ 42
4.2.3 Instalacija Robotics Toolbox-a ................................................................... 44
4.1.4 Primer inverzne kinematike ........................................................................ 44
4.1.5 Trajektorije .................................................................................................. 48
4.2 Primer inverzne kinematike u Simulink-u ................................................................. 51
Literatura ...................................................................................................................................... 57
3
1. Uvod
Kinematika je grana mehanike koja se bavi proučavanjem kretanja tela ne uzimajući u
obzir sile koje izazivaju to kretanje. Prostije rečeno, kinematika je formalni opis kretanja.
Kinematika je jedan od pristupa animacije modela. Pretpostavlja se da je model
sastavljen od određenog broja zglobova koji su povezani čvrstim vezama. Svaki zglob može
imati od jedan do tri stepeni slobode (eng. degree of freedom, DOF), zavisno od broja osa oko
kojih se može rotirati. Primene u robotici daju do 6 stepeni slobode kretanja, jer robotski zgobovi
mogu da se kreću translatorno i da rotiraju oko svake od tri koordinatnih osa.
Postoje dve vrste kinematike robota: direktna i inverzna.
Direktna kinematika za manipulator (manipulator je termin preuzet iz robotike koji se
odnosi na posmatrani model koji ima jedan nepokretni zglob – bazu, i skup povezanih pokretnih
zglobova) zadaje položaje i uglove svih zglobova, te na osnovu njih izračunava položaj krajnje
tačke manipulatora. Ovo je trivijalan zadatak koji ima samo jedno rešenje.
Inverzna kinematika je obrnut postupak direktnoj kinematici – za zadatu krajnju tačku
manipulatora inverzna kinematika pronalazi položaje i uglove svih zglobova tako da manipulator
bude u željenoj krajnjoj tački, ako je željena tačka fizički dostupna, ili što je moguće bliže ako
nije fizički dostupna. Ovo je mnogo složeniji problem koji često ima beskonačno mnogo rešenja.
Osnovni problem, tj. pitanje koje se postavlja u inverznoj kinematici glasi: Pod kojim uglom
moraju biti zglobovi robota, da bi krajnji položaj ruke robota bio u željenoj tački? U ovom radu
biće razmatran problem inverzne kinematike robota.
4
2. Kinematika robota
Kinematički model robota je najčešće prvi model koji je potreban za analizu rada robota.
Svaki zadatak koji robot obavlja može da se svede na ispravno pozicioniranje i orijentaciju
njegove hvataljke.
2.1 Matrica vektorskog proizvoda i Rodrigues-ova formula konačnih rotacija
Za dobijanje Rodrigues-ove formule konačnih rotacija potrebno je da se prethodno
definiše matrica vektorskog prizvoda dva vektora [6].
Neka su data dva vektora:
� = ��� + ��� + ��� 2.1.1
� = ��� + ��� + ��� 2.1.2
Gde su �, �, � jedinični vektori koordinatnih osa x, y, z. Tada je njihov vektorski proizvod:
� = {� × �} = �
� � ��� �� ��
�� �� ��
� 2.1.3
��� + ��� + ��� = ����� − ������ + (���� − ����)� + ����� − ������ 2.1.4
Ako se uvede sledeća kososimetrična matrica:
[��] = �
0 −� � ��
�� 0 −� �
−�� �� 0� 2.1.5
može da se napiše:
� = ��� + ��� + ��� = �
0 −� � ��
�� 0 −� �
−�� �� 0� �
��
��
��
� = �
���� − ����
���� − ����
���� − ����
� 2.1.6
Odnosno:
� = {� × �} = [��] ∙ � 2.1.7
Matrica [��] se naziva matrica vektorskog proizvoda ili dualni objekat vektora a.
5
Primer: Neka su data dva vektora:
� = 2� + 3� − �
� = −3� + 2� − 2�
Tada je njihov vektorski proizvod:
� = {� × �} = �� � �2 3 −1
−3 2 −2
�
��� + ��� + ��� = (3 ∙ (−2) − (−1) ∙ 2)� + (−1 ∙ (−3) − 2 ∙ (−2))� + (2 ∙ 2 − 3 ∙ (−3))� =
= −4� + 7� + 13�
Ako se uvede kososimetrična matrica:
[��] = �0 1 3
−1 0 −2−3 2 0
�
može da se napiše:
� = ��� + ��� + ��� = �0 1 3
−1 0 −2−3 2 0
� �−32
−2� = �
3 ∙ (−2) − (−1) ∙ 2
−1 ∙ (−3) − 2 ∙ (−2)2 ∙ 2 − 3 ∙ (−3)
� = �−47
13�
Svojstva matrice vektorskog proizvoda:
1. [��]� = −[��]
2. [��]� = � ∙ �� − ���
gde su:
� - jedinična matrica
� - intenzitet vektora �
Ovo je postupak za opisivanje kinematskih transformacija. Posmatra se telo koje može da se
obrće oko ose ��′, Sl.2.1.1. Definiše se tri uzajamno normalnih baznih vektora �, ��, �′′.
6
cos sinO M h he e
Sl.2.1.1. – Obrtanje tela oko ose ��′
Sa Sl.2.1.1. se može videti da je poluprečnik kruga čiji je centar O' određen izrazom:
ℎ = |� × �|= |�|sin(�, �) = ��′�� = ��′��′ 2.1.8
Jedinični vektor �′ se definiše kao:
�� =�
�(� × �) 2.1.9
Treći jedinični vektor �′′ je normalan na prva dva:
��� = �� × � =�
�(� × �) × � 2.1.10
Vektor � posle rotacije za ugao � postaje:
�� = ����⃗ � + ��′���⃗ ′ 2.1.11
����⃗ � = (� ∙ �) ∙ � 2.1.12
�� = (� ∙ �) ∙ � + (� × �) × ����� + (� × �)���� 2.1.13
Korišćenjem osobine antisimetrije vektorskog proizvoda:
�� = (� ∙ �) ∙ � − � × (� × �)����(� × �)���� 2.1.14
O'
M'
M
e'
e''
O e'
ee''
r'
r
r
7
Dvostruki vektorski proizvod može da se napiše kao:
� × (� × �) = � ∙ (� ∙ �) − � ∙ (� ∙ �) 2.1.15
Odnosno:
� ∙ (� ∙ �) = � × (� × �) + � 2.1.16
Smenom u jednačini za �′ dobija se Rodrigova formula konačnih rotacija:
�� = � + � × (� × �)(1 − ����) + (� × �)���� 2.1.17
Ova relacija može da se napiše i u matričnom obliku:
�� = � ∙ � 2.1.18
gde je � matrica transformacije reda 3 × 3, data sa:
� = � + [��]�(1 − ����) + [��]���� 2.1.19
��� = � + ��� ����
����1 − ������ + ���
�������� 2.1.20
gde je �� = 1, (��� = 0) ; ako je i - ti zglob translatoran.
Prema tome, u slučaju da je ugao rotacije nula (slučaj translatornog zgloba) matrica
transformacije je jedinična matrica �.
2.2 Homogene transformacije
Homogene transformacije definišu položaj i orijenatciju između dva koordinatna sistema.
Pod pojmom koordinatni sistem podrazumeva se pravougli desni karetezijanski sistem. Matrice
homogenih transformacija su kvadratne matrice reda 4x4.
Matrica homogenih transformacija ima izgled:
� = �
�� �� �� ��
��
��
0
��
��
0
��
��
0
��
��
1
� 2.2.1
Prema tome, matrica desnog nepokretnog sistema je jedinična matrica:
� = �
1 0 0 000
10
01
00
0 0 0 1
� 2.2.2
8
1 1 1
1 1 1
1 1 1
2
3
1
0 0 0 1
x x x
y y y
z z z
i j k
i j k
i j k
A
0 0 1 2
1 0 0 3
0 1 0 1
0 0 0 1
A
x y zp p p r
Primer: Na Sl.2.2.1. prikazana su dva koordinatna sistema, ���� i ��������. Naći
matricu transformacije sistema �������� na sistem ����.
Rešenje: Matrica transformacije sistema �������� na system ���� data je izrazom:
2.2.3
Odavde se dobija:
2.2.4
Sl.2.2.1. – Koordinatni sistemi ���� i ��������
2.3 Veza između unutrašnjih i spoljašnjih koordinata
Pod pojmom spoljašnjih koodinata podrazumeva se položaj robota (��, ��, ��) i njegova
orijentacija u prostoru određena uglovima (�, �, �). Ovim veličinama je definisan vektor:
2.3.1
Pod pojmom unutrašnjih koordinata podrazumevaju se rotacije i translacije u pojedinim
stepenima slobode. Stoga je njihov broj jednak broju stepeni slobode robota kojih obično ima
šest, pa vektor unutrašnjih koordinata glasi:
9
1 2 3 4 5 6 θ 2.3.2
Razlikuju se dva zadatka: direktni i inverzni kinematički problem. Direktni kinematički
problem se sastoji u tome da se na osnovu poznatih pomeranja i rotacija u zglobovima robota
(vektor �) izrazi vektor položaja hvataljke (vektor �). Ovaj zadatak je relativno jednostavan.
Inverzni kinematički problem se sastoji u tome da se na osnovu poznatog vektora
spoljašnjih koordinata � odrede unutrašnje koordinate u vektoru �.
Ako su dimenzije vektora � i � jednake, radi se o neredutantnom robotu i postoji
jednoznačno rešenje inverznog kimematičkog problema.
Ako je dimenzija vektora � veća od dimenzije vektora �, radi se o redudantnom robotu i
rešenje inverznog kinematičkog problema nije jednoznačno. Postoji više rešenja i obično se
prema nekom kriterijumu bira jedno. Zbog teškoća pri odabiranju većina robota koji se sreću u
tehničkoj praksi je neredutantna.
2.4 Elementarne transformacije
Elementarne koordinatne transformacije su translacija i rotacija.
2.4.1 Translacija
Translacija se vrši po odgovarajućem stepenu slobode duž jedne od osa pridruženog
koordinatnog sistema. Pri translaciji, ose pokretnog koordinatnog sistema ostaju paralelne sa
osama nepokretnog sistema, a koordinatni početak pokretnog sistema je pomeren za veličinu
translacije u odnosu na nepokretni sistem, kao što je pokazano na Sl.2.4.1.1. Prema tome,
translacija je linearna transformacija. Transformacija translacije se obeležava sa �(�, �, �) i
izgleda:
Sl.2.4.1.1. – Translacija pokretnog koordinatnog sistema u odnosu na nepokretni
10
1 0 0
0 1 0, ,
0 0 1
0 0 0 1
a
ba b c
c
A T
1 0 0 0
0 cos sin 0,
0 sin cos 0
0 0 0 1
x
R
cos 0 sin 0
0 1 0 0,
sin 0 cos 0
0 0 0 1
y
R
2.4.1.1
2.4.2 Transformacija rotacije
Rotacija se po odgovarajućem stepenu slobode odvija oko jedne od osa pridruženog
koordinatnog sistema. Posle rotacije, koordinatni početak novog koordinatnog sistema i osa
rotacije se poklapaju sa nepokretnim koordinatnim sistemom, a druge dve ose su zaokrenute za
ugao rotacije �, kao što je prikazano na Sl.2.4.2.1. Transformacija rotacije u slučaju rotacije oko
� ose za ugao � glasi:
2.4.2.1
a) oko � ose b) oko � ose c) oko � ose
Sl.2.4.2.1. - Rotacije oko koordinatnih osa
Za rotaciju oko ose � imamo:
2.4.2.2
Matrica transformacije za rotaciju oko ose � glasi:
11
cos sin 0 0
sin cos 0 0,
0 0 1 0
0 0 0 1
z
R
2.4.2.3
Zbog funkcija sinusa i kosinusa rotacija je nelinearna transformacija.
2.5 Denavit-Hartenberg-ova notacija
Ovaj način notacije je široko zastupljen u literaturi u oblasti robotike. Denavit i
Hartenberg su ovaj postupak notacije u originalnom radu iz 1955. godine (A Kinematic Notation
for Lower-Pair Mechanisms Based on Matricies, Trans. Of ASME J. App. Mech. vol. 77, pp.
215-221, June 1955.) namenili za kinematičku analizu prostornih polužnih mehanizama. Kako su
se prvi prototipovi IR pojavili 1962. godine i razvoj robotike kao samostalne naučne discipline je
usledio u narednim godinama, bilo je prirodno da se ovaj postupak notacije primeni na robote.
Stoga ovaj postupak notacije, iako široko primenjen u literaturi, nije najbolje prilagođen za
mehanizme IR koji su u primeni znatno jednostavniji od prostornih mehanizama po obliku
segmenata, i načinu njihovog povezivanja. Naime kod robota se ne koriste krivolinijski segmenti
i uglavnom se koriste zglobovi (kinematski parovi) V-te klase (sa po jednim stepenom slobode).
Sl.3.5.1. - Denavit-Hartenberg-ova notacija
Posmatraju se dva uzastopna segmenta sa odgovarajućim zglobovima (Sl.3.5.1.). Kao i
kod prethodne notacije, numeracija zglobova i segmenata je redosledna počev od oslonca tj.
12
zglob i-1 prethodi segmentu i-1, a i-ti zglob segmentu i. Ovde se međutim, pokretni koordinatni
sistemi postavljaju tako da im se z ose uvek poklapaju sa osom pripadajućeg zgloba.
Prema tome ose ���� i �� su ose � − 1-og i �-tog zgloba, respektivno. U opštem slučaju
(kod zglobova prostornih mehanizama), ove dve ose su mimoilazne. Da bi se ove ose dovele do
poklapanja potrebno je najpre zaokrenuti osu ���� oko ose ���� za ugao ����, pri čemu sistem
������������ prelazi u sistem �� �� �� , a potom translirati po pravcu zajedničke normale (duž ose
�� , odnosno, ����) za rastojanje ���� tako da sistem �� �� �� prelazi u sistem �� �� �� . Konačno
je potrebno zaokrenuti sistem �� �� �� oko ose �� (odnosno ����) za ugao obrtanja i-tog zgloba i,
tako da se prelazi u koordinatni sistem �� �� �� (�� �� ��). Da bi se ovaj sistem doveo do
poklapanja sa sistemom ������ potrebno je izvršiti translaciju za rastojanje ��. Prema tome,
celokupno kretanje između � − 1-og i �-tog koordinatnog sistema je opisano sa četiri parametra:
����, ����, ��, �� 3.5.1
Ukoliko je i-ti zglob rotacioni, parametri ����, ���� i �� su konstantni, a ugao ��
predstavlja koordinatu i-tog zgloba. U slučaju translatornog zgloba, konstantni su parametri
����, ���� i ��, a �� predstavlja translatorno pomeranje u i-tom zglobu.
Korišćenjem matrica homogenih koordinatnih transformacija, matrica prelaza iz � − 1-og
u �-ti koordinatni sistem može da se napiše kao:
����,�= ����,���,� ��,� ��,� 3.5.2
odnosno
����,�= ��(����) ∙ ��(����) ∙ ��(��) ∙ ��(��) 3.5.3
gde su:
1
1 11 1
1 1
1 0 0 0 1 0 0
0 cos sin 0 0 1 0 0
0 sin cos 0 0 0 1 0
0 0 0 1 0 0 0 1
i
i ix i x i
i i
a
R T a
cos sin 0 0 1 0 0 0
sin cos 0 0 0 1 0 0
0 0 1 0 0 0 1
0 0 0 1 0 0 0 1
i i
i iz i z i
i
R T dd
Konačno se dobija matrica transformacije u obliku:
13
1
1 1 1 1
1,
1 1 1 1
0
0 0 0 1
i i i
i i i i i i i
i i
i i i i i i i
c s a
s c c c s d s
s s c s c d c
A 3.5.4
gde je:
��� = �����; ��� = ����� …
Primenjujući ovaj postupak na sve segmente i zglobove IR, � = 1, 2, . . . , �, ukupna
matrica transformacije sa hvataljke na nepokretni koordinatni sistem je:
��� = ��� ∙ ��� ∙∙∙ ����,� 3.5.5
Ovaj postupak biće ilustrovan na primeru direktnog kinematičkog zadatka za Unimate
PUMA 560 robot, prikazan na Sl.3.5.2. Denavit-Hartenberg-ovi parametri segmenata robota
PUMA 560 su dati u Tab.3.5.1.
Segment i ���� ���� �� �� 1 0 0 0 �� 2 −90 � 0 0 �� 3 0 �� �� �� 4 −90 � �� �� �� 5 90 � 0 0 �� 6 −90 � 0 0 ��
Tab.3.5.1. Parametri segmenata robota PUMA 560
Sl.3.5.2. - Kinematski parametri i postavljanje koordinatnih sistema za robot PUMA 560
14
1 1
1
01
1
cos 0 sin 0
0 0 cos 0
sin 1 0 0
0 0 0 1
A
2 2
12
2 2
cos sin 0 0
0 0 1 0
sin cos 0 0
0 0 0 1
A
3 3 2
3 3
23
3
cos sin 0
sin cos 0 0
0 0 1
0 0 0 1
a
d
A
4 4 3
4
34
4 4
cos sin 0
0 0 1
sin cos 0 0
0 0 0 1
a
d
A
Direktni kinematički problem za robot Unimate PUMA 560 se rešava idući od oslonca do
hvataljke. Matrica transformacije od nepokretnog do prvog pokretnog segmenta iznosi:
3.5.6
Matrica koordinatne transformacije sa sistema 1 na sistem 2 glasi:
3.5.7
Matrica transformacije sa sistema 2 na sistem 3 je:
3.5.8
Sl.3.5.3. - Kinematski parametri i koordinatni sistemi za podlakticu robota PUMA 560
Matrica transformacije sa sistema 3 na sistem 4 je:
3.5.9
15
5 5
45
5 5
cos sin 0 0
0 0 1 0
sin cos 0 0
0 0 0 1
A
6 6
56
6 6
cos sin 0 0
0 0 1 0
sin cos 1 0
0 0 0 1
A
06 01 12 23 34 45 56A A A A A A A
5 6 5 6 5
6 6
46 45 56
5 6 5 6 5
cos cos sin sin sin 0
sin cos 0 0
sin cos sin sin cos 0
0 0 0 1
A A A
4 5 6 4 6 4 5 6 4 6 4 5 3
5 6 5 6 5 4
36 34 46
4 5 6 4 6 4 5 6 4 6 4 5 0
0 0 0 1
c c c s s c c s s c c s a
s c s s c d
s c c c s s c s c c s s
A A A
23 23 2 2
3
13
23 23 2 2
0
0 0 1
0
0 0 0 1
c s a c
d
s c a s
A
23 2 3 2 3 2 3
23 2 3 2 3 2 3
cos cos cos sin sin
sin cos sin sin cos
c
s
Matrica prenosa sa sistema 4 na sistem 5 iznosi:
3.5.10
Poslednja matrica prenosa je:
3.5.11
Sada je matrica transformacije između završnog segmenta i oslonca:
3.5.12
Množenjem matrica ��� i ��� dobija se matrica:
3.5.13
Množenjem sa leve strane matricom ��� dobija se:
3.5.14
Pošto su zglobovi 2 i 3 paralelni, matrice ��� i ��� mogu da se zamene jednom matricom:
3.5.15
gde su korišćene trigonometrijske formule:
3.5.16
16
11 12 13 1
21 22 23 1
16 13 36
31 32 33 1
0 0 0 1
x
y
z
t t t r
t t t r
t t t r
A A A
11 23 4 5 6 4 6 23 5 6
21 4 5 6 4 6
31 23 4 5 6 4 6 23 5 6
t c c c c s s s s s
t s c c c s
t s c c c s s c s c
12 23 4 5 6 4 6 23 5 6
22 4 5 6 4 6
32 23 4 5 6 4 6 23 5 6
t c c c c s c s s s
t s c s c c
t s c c c s c c s s
13 23 4 5 23 5
23 4 5
33 23 4 5 23 5
t c c s s c
t s s
t s c s c c
1 2 2 3 23 4 23
1 3
1 2 2 3 23 4 23
x
y
z
r a c a c d s
r d
r a s a s d c
11 12 13
21 22 23
06 01 16
31 32 33
0 0 0 1
x
y
z
p p p r
p p p r
p p p r
A A A
11 1 11 1 21
21 1 11 1 21
31 31
p c t s t
p s t c t
p t
Dalje se dobija:
3.5.17
gde je:
3.5.18
Konačno, množenjem ove matrice sa matricom ��� dobija se:
3.5.19
gde je:
17
12 1 12 1 22
22 1 12 1 22
32 32
p c t s t
p s t c t
p t
13 1 13 1 23
23 1 13 1 23
33 33
p c t s t
p s t c t
p t
1 1 1 1
1 1 1 1
1
x x y
y x y
z z
r c r s r
r s r c r
r r
3.5.20
Ove jednačine definišu kinematiku robota PUMA 560. One izračunavaju položaj i
orijentaciju koordinatnog sistema 6 u odnosu na nepokretni koordinatni sistem 0.
3. Inverzna kinematika robota
Razmatra se inverzni zadatak kinematike, tj. kako će da se na osnovu željenog položaja i
orijentacije izračuna skup uglova u zglobovima robota koji njima odgovaraju. Rešavanje
problema određivanja uglova u zglobovima za zadati položaj koordinatnog sistema alata {�} u
odnosu na nepokretni oslonac {�} je podeljen na dva dela. Prvo se transformacije vrše tako da se
nađe koordinatni sistem korena hvataljke {�} u odnosu na sistem oslonca {�}, pa se onda
inverznom kinematikom određuju uglovi u zglobovima. Pošto matrica transformacije ��� zavisi
od konfigurcije robota, odnosno od rasporeda i orijentacije zglobova i kretanja u njima, onda ne
postoji jedan postupak za rešavanje u opštem obliku za proizvoljnu konfiguraciju robota. Kod
rešavanja inverzne kinematike se mora ići od robota do robota i postupci rešavanja se razlikuju.
3.1 Rešivost
Problem rešavanja kinematičkih jednačina je nelinearan i svodi se na sledeće: za poznatu
numeričku vrednost matrice ��� treba naći vrednosti ��, ��, … , � �. Posmatrajmo jednačine
PUMA robota ����, ����, ��, ��, gde ���� predstavlja rotaciju zgloba oko odgovarajuće ose, ����
predstavlja translaciju po pravcu zajedničke normale, �� predstavlja koordinatu i-tog zgloba, a ��
predstavlja translatorno pomeranje u i-tom zglobu. Treba napomenuti da zglob � − 1 prethodi
segmentu � − 1, a i-ti zglob segmentu i. Ukoliko je i-ti zglob rotacioni, parametri ����, ���� i ��
su konstantni, a ugao �� je promenljiv. U slučaju translatornog zgloba, konstantni su parametri
����, ���� i ��, a parametar �� je promenljiv. Problem leži u tome da za datu matricu ��� postoji
16 numeričkih vrednosti (od kojih su četiri trivijalne), koje su rešenja jednačine ����, ����, ��, ��
za uglove u zglobovima.
18
Znači, u slučaju robota sa 6 stepeni slobode (kao što je PUMA robot) sa jednačinama
����, ����, ��, ��, postoji dvanaest jednačina sa 6 nepoznatih (��, ��, … , � �). Međutim, od devet
jednačina koje proističu iz matrice ���, samo tri jednačine su nezavisne. Kada se ovome dodaju
tri jednačine položaja (prva tri elementa četvrte kolone matrice ���), dobija se sistem od šest
jednačina sa šest nepoznatih. Ovo su nelinearne transcendentne jednačine koje su veoma teške za
rešavanje. Jednačine ����, ����, ��, �� su relativno jednostavne jer posmatrani PUMA robot ima
relativno jednostavnu geometriju: jedan broj parametara (uglova) �� jednak je 0 ili ±90 �.
Takođe, neka od rastojanja ���� i �� su jednaki nuli. Jasno je da, u slučaju složenog mehanizma
gde su svi parametri položaja zglobova različiti od nule, kinematičke jednačine biće znatno
složenije. Kod rešavanja nelinearnih jednačina ne postoji jedan opšti metod rešavanja i treba se
podsetiti egzistencije rešenja, višestrukosti rešenja i načina rešavanja [6].
3.2 Egzistencija rešenja
Pitanje kada rešenja postoje ili ne postoje blisko je povezano sa radnim prostorom
robota. Ukratko, radni prostor je ona zapremina u prostoru koju hvataljka robota može da
dosegne. Da bi postojalo rešenje za određenu tačku, ta tačka mora da leži unutar radnog prostora
robota. Ponekad su korisne još dve definicije radnog prostora: desni radni prostor i dostižni radni
prostor. Desni radni prostor je ona zapremina koju hvataljka robota može da dosegne sa svim
orijentacijama, odnosno u svakoj tački desnog radnog prostora hvataljka može da se orijentiše
slobodno. Dostižni radni prostor je ona zapremina u radnom prostoru koju robot može da
dosegne sa makar jednom orijentacijom. Znači desni radni prostor je podskup dostižnog radnog
prostora.
Kao primer ovoga, posmatrajmo ravanski dvosegmentni manipulator prikazan na
Sl.3.3.1. Ako je �� = �� dostižni radni prostor je disk radijusa 2��. Desni radni prostor je ovde
samo jedna tačka (koordinatni početak). Ako je �� ≠ ��, onda ne postoji desni radni prostor, a
dostižni radni prostor je prsten omeđen radijusima �� + �� i |�� − ��|. Unutar dostižnog radnog
prostora postoje dve orijentacije hvataljke, a na granicama radnog prostora postoji samo jedna
orijentacija. Ovo razmatranje radnog prostora dvosegmentnog robota je pretpostavljalo da
segmenti u zglobovima mogu da rotiraju za 360 �. Kod realnih robota ovo je vrlo retko moguće.
Kada su rotacije segmenta samo deo od 360 �, onda je i radni prostor redukovan i to, i po veličini
i po broju mogućih orijentacija. Na primer, za robot na Sl.3.3.2. ako prvi segment može da rotira
za 360 � a drugi samo u opsegu 0 ≤ � � ≤ 180 �, onda dostižni radni prostor ostaje isti, ali je
samo jedna orijentacija moguća u svakoj tački. Kada robot ima manje od 6 stepeni slobode
kretanja, on ne može da obezbedi proizvoljan položaj i orijentaciju u prostoru. Na primer, robot
na Sl.3.3.2. ne može da se kreće duž ose z. Radni prostor takođe zavisi i od koordinatnih
transformacija sistema vezanog za hvataljku robota.
19
3.3 Višestruka rešenja
Još jedan problem koji se javlja pri rešavanju kinematičkih jednačina su višestruka
rešenja. Robot sa tri rotaciona zgloba koji se kreće u ravni ima veliki desni prostor zahvaljujući
dužini segmenata i velikim opsegom rotacija zglobova, jer se bilo koja tačka unutar njega može
dostići sa bilo kojom orijenatacijom. Sl.3.3.1. pokazuje četiri rešenja za zadatu tačku i
orijentaciju hvataljke.
Činjenica da robot ima višestruka rešenja stvara probleme, jer sistem upravljanja
kretanjem robota treba da izabere samo jedno. Kriterijum izbora rešenja može da varira od
slučaja do slučaja, ali obično se usvaja najbliža vrednost vrednosti iz prethodnog koraka.
Broj rešenja zavisi od broja zglobova, odnosno od broja stepeni slobode robota i od
parametara ��, ����, ��, odnosno geometrije segmenata. Takođe, opseg rotacije u zglobu utiče na
broj rešenja. Na primer, robot PUMA 560 može da dostigne neku tačku u radnom prostoru sa
osam različitih rešenja. Sl.3.3.1. pokazuje četiri rešenja za isti položaj i orijentaciju. Za svako
prikazano rešenje postoji još po jedno rešenje u kojem se zadnja tri zgloba kreću saglasno
obrascima:
��� = �� + �
��� = −�� 3.3.1
��� = �� + �
Znači, ukupno osam rešenja za jednu tačku. Usled ograničenja rotacije zglobova neka od
ovih rešenja mogu da budu nedostižna. Generalno, što je više parametara segmenata različito od
nule biće više rešenja. Ovo pokazuje koliko je važno da segmenti robota imaju što jednostavniju
geometriju.
20
Sl.3.3.1. - Četiri rešenja za robot PUMA 560
Na primer, robot na Sl.3.3.2. ima ravan simetrije u kojoj su smešteni centri zglobova
“nadlaktice“ i “podlaktice“, pa kod njega ne postoji levo i desno, već postoje samo dva rešenja,
kad je ruka robota gore i dole.
Sl.3.3.2. - Robot sa dva rešenja
21
1 1 11 12 13
1 1 21 22 23
16
31 32 33
0 0
0 0
0 0 1 0
0 0 0 1 0 0 0 1
x
y
z
c s p p p r
s c p p p r
p p p r
A
Tab.3.3.1. pokazuje kako parametri segmenata �� utiču na broj rešenja inverzne
kinematike. Sa većim brojem parametara �� broj rešenja je veći. Za robot sa šest stepeni slobode
kretanja i svim parametrima �� različitim od nule, broj rešenja može da bude do 16.
�� Broj rešenja �� = �� = �� = 0 ≤ 4
�� = �� = 0 ≤ 8 �� = 0 ≤ 16
Svi �� = 0 ≤ 16
Tab.3.3.1. - Broj rešenja zavisno od parametra �� različitih od nule.
3.4 Metod rešavanja
Kod sistema nelinearnih jednačina ne postoji opšti postupak rešavanja. Rešenja mogu da
se podele na dve opšte grupe: rešenja u zatvorenom obliku i numerička rešenja. Numerička
rešenja su iterativna i zahtevaju veoma mnogo računarskog vremena pa nisu pogodna za primenu
kod robota. Ovde će da se posmatra analitički postupak rešavanja koji daje rešenja u zatvorenom
obliku. Postupak rešavanja je prikazan za robot PUMA 560 [6]. Potrebno je da se reši matrična
jednačina:
��� = �
��� ��� ��� ��������
0
������
0
������
0
��
��
1
� = ���(��)���(��)���(��)���(��)���(��)���(��) 3.4.1
Množenjem prethodne jednačine inverznom matricom ����� dobija se:
�������� = ���(��)���(��)���(��)���(��)���(��) 3.4.2
Odavde se dobija:
3.4.3
Ovaj postupak množenja obe strane jednačine sa inverznom matricom razdvaja promenljive.
Izjednačavajući elemente druge vrste i četvrte kolone u obe matrice prethodne jednačine dobija
se:
−���� + ���� = �� 3.4.4
22
Da bi rešili ovu transcendentnu nelinearnu jednačinu, uvode se trigonometrijske smene:
�� = �����
�� = ����� 3.4.5
gde je:
� = ���� + ��
� 3.4.6
� = �������
��
Smenom jednačine (3.4.5) u jednačinu (3.4.4) dobija se:
���� − ���� =��
�
3.4.7
Korišćenjem formule za razliku uglova dobijamo:
sin(� − ��) =��
�
3.4.8
odnosno
cos(� − ��) = ±�1 −��
�
��
3.4.9
pa je
� − �� = �������
±��� − ���
3.4.10
konačno se dobija
�� = �������
��− �����
��
±���� + ��
� − ���
3.4.11
23
1 23 1 23 23 2 3 11 12 13
1 23 1 23 23 2 3 21 22 23
36
1 1 3 31 32 330
0 0 0 1 0 0 0 1
x
y
z
c c s c s a c t t t r
c s s s c a s t t t r
s c d t t t r
A
4 5 6 4 6 4 5 6 4 6 4 5 3
5 6 5 6 5 4
36
4 5 6 4 6 4 5 6 4 6 4 5 0
0 0 0 1
c c c s s c c s s c c s a
s c s s c d
s c c c s s c s c c s s
A
Treba naglasiti da postoje dva rešenja za ugao �� zavisno od znaka u izrazu (3.4.11). Sada, pošto
je �� poznato, poznata je leva strana jednačine (3.4.3). Ako se izjednače elementi prve vrste i četvrte kolone i elementi treće vrste i četvrte kolone na obe strane jednačine jednačine (3.4.3)
dobijaju se sledeće jednačine:
���� + ���� = ���� + ����� − �����
−�� = ���� + ����� + ����� 3.4.12
Ako se kvadriraju i saberu jednačine (3.4.4) i (3.4.12) dobija se:
���� − ���� = � 3.4.13
gde je
� =��
� + ��� + ��
� − ��� − ��
� − ��� − ��
�
2��
3.4.14
Treba primetiti da je eliminisana zavisnost od �� u jednačini (3.4.13). Jednačina (3.4.13) je istog
oblika kao jednačina (3.4.4) i može da se reši korišćenjem slične trigonometrijske smene pa se
dobija rešenje za ��:
�� = �����(��, ��) − ����� �� ± ���� + ��
� − ��� 3.4.15
Zavisno od znaka u prethodnoj jednačini postoje dva rešenja za ugao ��. Sada se može prepisati
jednačina (3.4.1) tako da su na levoj strani sve poznate veličine u funkciji ugla ��:
[���(��)]����� = ���(��)���(��)���(��) 3.4.16
odnosno
3.4.17
gde je matrica ��� data u obliku jednačine:
3.4.18
24
Izjednačavajući elemente prve vrste i četvrte kolone i druge vrste i četvrte kolone sa obe strane
jednačine (3.4.17) dobija se:
������� + ������� − ����� − ���� = ��
−������� − ������� − ����� + ���� = �� 3.4.19
Jednačine (3.4.19) mogu simultano da se reše po ��� i ���:
��� =(−���� − ��)�� + ����� + �����(���� − ��)
��� + (���� + ����)�
3.4.20
��� =(���� − ��)�� − ����� + �����(−���� − ��)
��� + (���� + ����)�
Pošto su imenioci jednaki i pozitivni, rešava se po zbiru uglova �� i ��:
��� = ����� �(−���� − ��)�� − ����� + �����(�� − ����)
(���� − ��)�� + ����� + �����(���� + ��)� 3.4.21
Jednačina (3.4.21) daje četiri rešenja za ��� prema četiri moguće kombinacije za �� i ��. Prema
tome, četiri rešenja za �� računaju se prema izrazu:
�� = ��� − �� 3.4.22
Sada je potpuno poznata leva strana jednačine (3.4.18). Izjednačavajući elemente prve vrste i
treće kolone na obe strane jednačine (3.4.18) dobija se:
������ + �������� − ������ = −����
−����� + ����� = ���� 3.4.23
Dokle god je �� ≠ 0 , prethodnu jednačinu možemo da rešimo po ��:
�� = �����−����� + �����
−�������� − �������� + ������
3.4.24
Kada je �� = 0 robot se nalazi u singularnom položaju u kojem su ose zglobova 4 i 6 kolinearne
i uslovljavaju isto kretanje zadnjeg segmenta. Ova situacija se otkriva kada je ����� u jednačini
(3.4.24) približno jednak nuli. Tada se zadržava prethodna vrednost ugla �� i kada se izračuna ��
onda se određuju ostale vrednosti [6].
Ako se ponovo posmatra jednačina (3.4.1), ona može da se napiše u sledećem obliku:
25
1 23 4 1 4 1 23 4 1 4 23 4 2 3 4 3 4 3 4
1 23 4 1 4 1 23 4 1 4 23 4 2 3 4 3 4 3 4
1 23 1 23 23 2 3 4
0 0 0 1
c c c s s s c c c s s c a c c d s a c
c c s s c c c s c c s s a c s d s a s
c s s s c a s d
5 6 5 6 5
6 6
46
5 6 5 6 5
cos cos sin sin sin 0
sin cos 0 0
sin cos sin sin cos 0
0 0 0 1
A
�������� = ���(��)���(��) 3.4.25
gde je ����� dato izrazom:
3.4.26
a matrica ��� je data u obliku sledeće jednačine:
3.4.27
Izjednačavajući elemente prve vrste i treće kolone i treće vrste i treće kolone na obe strane
jednačine (3.4.25) dobija se:
���(������� + ����) + ���(������� − ����) − �������� = −��
���(−�����) + ���(−�����) + ���(−���) = �� 3.4.28
Znači ugao �� je:
�� = �������
��
3.4.29
gde su �� i �� određeni jednačinama (3.4.28). Primenjujući množenje inverznom matricom s leve
strane jednačine (3.4.1) još jednom, dobija se:
�������� = ��� 3.4.30
Izjednačavajući elemente treće vrste i prve kolone i prve vrste i prve kolone sa obe strane
jednačine (3.4.25) dobija se:
�� = �������
��
3.4.31
gde su:
�� = −���(������� − ����) − ���(������� + ����) + ��������
3.4.32
26
�� = ���[(������� + ����)�� − �������] + ���[(������� − ����)�� − �������] − ���(������� + �����)
Zbog znakova plus ili minus koji se javljaju u jednačinama (3.4.11) i (3.4.15), ove jednačine daju
četiri rešanja. Četiri dodatna rešenja se dobijaju usled rotacije zglobova hvataljke robota kao u
ogledalu. Za svako od prva četiri rešenja dobijamo i “preslikano” rešenje preko izraza:
��� = �� + �
��� = −�� 3.4.33
��� = �� + �
Od dobijenih osam rešenja, neka od njih mogu da se odbace zbog prekoračenja granica
kretanja zglobova. Od preostalih rešenja se bira ono najbliže postojećoj izabranoj konfiguraciji
robota.
Može se primetiti da je u celom postupku rešavanja inverznog kinematičkog problema
izbegavano deljenje sa ���� i ����, a vrednost koordinate u obrtnom zglobu je uvek tražena
preko funkcije �����, zbog toga što je tačnost računanja ����� funkcije u celom području njene
definisanosti jednaka, dok je kod funkcija ������ i ������ tačnost računanja problematična u
okolini tačaka 0 i � 2⁄ . Funkcija ����� daje rešenje u intervalu [− � 2⁄ , � 2⁄ ]. Najbolja funkcija
za rešavanje inverznog kinematičkog problema je ona koja se zove ATAN2. Ova funkcija ima
dva argumenta � i �. Rešenje daje u intervalu �– �, ��, zavisno od položaja argumenata � i �.
Ukratko, problem inverzne kinematike se radi na sledeći način: ako hvataljka robota treba
da se kreće npr. po pravoj liniji (putanji) od tačke �(���, ���, ���, ��, ��, ��) do tačke
�(���, ���, ���, ��, ��, ��), za neko vreme � putanja se podeli npr. na 100 intervala i onda se za
svaku među tačku na osnovu poznatih matrica transformacije ����, ����, ����, … , � �����, ����
nalaze vrednosti uglova u zglobovima ��, ��, … , � �. Što znači da, za robot sa 6 stepeni slobode
kretanja treba naći 606 vrednosti uglova ��, ��, … , � �. Iz ovoga se vidi koliko je problem
inverzne kinematike računski obiman [6].
Primer: Rešavanje inverzne kinematike robota Stanford arm sa četiri stepena slobode
kretanja prikazanog na Sl.3.4.1.
Pošto robot ima samo četiri stepena slobode kretanja, jednačina (3.4.1) reduktuje se na oblik:
��� = ������������ 3.4.34
Množenjem ove jednačine sa ����� dobija se:
�������� = ��� = ��������� 3.4.35
27
1 1
1 1101 14
0 0
0 0
0 0 1 0
0 0 0 1 0 0 0 1
x x x x
y y y y
z z z z
c s n o a r
s c n o a r
n o a r
A A
Sl.3.4.1. - Sferni robot (modifikovana Stenfordska ruka) i postavljanje koordinatnih sistema
Leva strana jednačine (3.4.35) iznosi:
3.4.36
odnosno
�������� = �
���(�) ���(�) ���(�) ���(�)���(�)���(�)
0
���(�)���(�)
0
���(�)���(�)
0
���(�)���(�)
1
� 3.4.37
gde je:
��� = ��� + ���
��� = −��� + ��� 3.4.38
��� = �
ako su �, �, � komponente vektora �, �, � ili � zavisno od toga koja je kolona prethodne matrice
odabrana. Desna strana jednačine (3.4.35) iznosi:
28
��� = �
��� 0 ��� (�� + ��)�� + �����
0−���
0
100
0���
0
��
�� + (�� + ��)�� + �����
1
� 3.4.39
Izjedančavanjem elementa druge vrste i četvrte kolone jednačine (3.4.39) dobija se:
���(�) = �� 3.4.40
odnosno
−���� + ���� = �� 3.4.41
Ovo je algebarska jednačina sa jednom nepoznatom. Rešavanje se obavlja pomoću smene:
�� = � ∙ ����
3.4.42
�� = � ∙ ����
gde je:
� = ���� + ��
� i x yarctg r r 3.4.43
Sada jednačina (3.4.41) može da se napiše u sledećem obliku:
���� ∙ ����� − ���� ∙ ����� =��
�
3.4.44
Primenom trigonometrijskog pravila za sinus razlike argumenata, dobija se:
sin(� − ��) = �� �⁄ 3.4.45
Kosinus ugla � − �� je:
cos(� − ��) = ±�1 − (�� �⁄ )� 3.4.46
onda može da napiše
��(� − ��) =��
±��� − ���
3.4.47
U jednačini (3.4.47) znak − označava položaj robota kada je drugi zglob i gornji deo ruke sa leve
strane stuba 1, a znak + kada je drugi zglob sa desne strane. U posmatranom primeru, izabrana je
desna konfiguracija robota, pa se nadalje koristi znak +. Zbog toga, sledi da je koordinata ��:
29
�� = ����� ���
��� − �����
⎝
⎛��
���� + ��
� − ���
⎠
⎞
3.4.48
Izjednačavanjem elementa prve vrste i prve kolone, a potom elementa prve vrste i treće kolone sa
obe strane jednačine (3.4.35) dobija se:
��� = ���� + ����
3.4.49
��� = −��
odavde sledi
��� = ����� �−��
���� + �����
3.4.50
Izjednačavanjem elementa prve vrste i četvrte kolone, a potom elementa treće vrste i četvrte
kolone jednačine (3.4.35), dobijaju se sledeće jednačine:
���� + ���� = (�� + ��)�� + �����
3.4.51
�� = �� + (�� + ��)�� + �����
Ako se prva jednačina reši po ��, a druga po ��, dobija se:
�� =���� + ���� − �����
�� + ��
3.4.52
�� =�� − �� − �����
�� + ��
pa je
�� = ��������� + ���� − �����
�� − �� − �����
3.4.53
Sada je koordinata ��:
�� = ��� − �� 3.4.54
30
Ako se kvadriraju i saberu obe jednačine (3.4.52) dobija se:
��� + 2���� + ��
� = �� 3.4.55
gde je
�� = (���� + ���� − �����)� + (�� − �� − �����)� 3.4.56
Rešavanjem po koordinati �� dobija se:
�� =−2�� ± �4��
� − 4(��� − ��)
2
3.4.57
odnosno, usvajajući samo znak + u prethodnoj jednačini, dobija se:
�� = √�� − �� 3.4.58
3.5 Generalisane brzine i ubrzanja
Kada govorimo o brzini, razmotrićemo na šta se taj pojam odnosi. Prvo, uvešćemo pojam
generalisanih brzina. Generalisane brzine su izvodi po vremenu generalisanih koordinata, dakle
�̇�, �̇�, … , � ̇�. Drugačiji naziv su unutrašnje brzine, jer predstavljaju izvode pomeranja u
zglobovima mehanizma, odnosno brzine relativnog pomeranja segmenata. Dalje, uvodimo i
pojam generalisanih ili unutrašnjih ubrzanja. To su izvodi generalisanih brzina, odnosno drugi
izvodi generalisanih koordinata. Iz toga sledi da su ubrzanja �̈�, �̈�, … , � ̈�. Uvedimo sada, pored
vektora generalisanih koordinata �, još i n-dimenzione vektore generalisanih brzina (�̇) i
ubrzanja (�̈) [5].
�̇ = ��̇� … � ̇��
�
3.5.1
�̈ = ��̈� … � ̈��
�
Sl.3.5.1. - Brzine i ubrzanja segmenata mehanizma [5]
31
Posmatrajmo jedan segment lanca, na primer j-ti, kao telo u prostoru. Tada je stanje tog
tela u nekom trenutku vremena određeno njegovim položajem i brzinom. O položaju je bilo reči
ranije, a sada ćemo razmotriti brzinu. Potrebno je poznavati brzinu težišta C i ugaonu brzinu
segmenta. Označimo vektor brzine težišta sa �����⃗ , a vektor ugaone brzine sa ������⃗ (Sl.3.5.1.).
Razmotrimo mogućnost izražavanja brzina �����⃗ i ������⃗ , u funkciji generalisanih brzina �̇. Ugaona
brzina ������⃗ je posledica superpozicije svih rotacija u zglobovima lanca, počevši od podloge pa do
posmatranog segmenta j. Posmatrajmo zglob ��. Ako je zglob rotacioni, tada je �� = 0 i vektor
rotacije je �̇�������⃗ . S obzirom na to da zglob ne mora biti rotacioni, tada u opštem slučaju vektor
rotacije pišemo u obliku �̇�(1 − ��)������⃗ . Za translatorni zglob (�� = 1) ovaj izraz je jednak nuli.
Slaganjem rotacija dobijamo:
������⃗ = � �̇�(1 − ��)������⃗
�
���
3.5.2
što se može napisati i u rekurzivnoj formi
������⃗ = ���⃗ ��� + �̇�(1 − ��)����⃗ 3.5.3
Brzinu težišta �����⃗ možemo takođe dobiti preko teorije vezanih vektora, ili pak diferencirajući
vektor položaja težišta ��⃗���. Kako je kod lančanih sistema pogodnije raditi sa rekurzivnim
izrazima, to vektor položaja težišta �� pišemo u obliku (Sl.3.5.1.):
��⃗ ��= ��⃗ ����
− ��⃗ ���,� + ��⃗ �,� + ���������⃗ 3.5.4
Diferenciranjem po vremenu dobijamo
�����⃗ = ���⃗ ��� − ����⃗ ��� × ��⃗ ���,� + ������⃗ × ��⃗ �,� + ���̇������⃗ 3.5.5
pri čemu smo koristili činjenicu da je izvod nekog vektora �⃗� vezanog za segment k jednak:
�̇⃗� = ���⃗ � × �⃗� , gde je �� ugaona brzina segmenta. Na ovaj način dobili smo izraze (3.5.3) i
(3.5.5) koji predstavljaju rekurzivne izraze za brzine segmenata. Diferenciranjem po vremenu
dobija se ugaono ubrzanje segmenta (����⃗ = ������̇⃗ ) i ubrzanje težišta (������⃗ = �����⃗̇ ).
�����⃗ = ��⃗ ��� + ��̈�����⃗ + �̇��������⃗ × ����⃗ ��(1 − ��) 3.5.6
������⃗ = ���⃗ ��� − �⃗��� × �⃗���,� − ���⃗ ��� × ����⃗ ��� × �⃗���,�� + ����⃗ × �⃗�,�� + ������⃗ × �������⃗ × �⃗�,�
� � +
��̈�����⃗ + 2�̇�(������⃗ × ����⃗ )��� 3.5.7
gde je �⃗�,�
� dato u sledećem obliku: �⃗�,�� = �⃗�,� + ��������⃗ 3.5.8
32
Ako je poznato da je za nulti segment lanca tj. podlogu mehanizma �⃗�,� = 0 , ���⃗ � = 0 i �⃗� = 0 ,
tada izrazi (3.5.3) - (3.5.7) omogućavaju da se izračunaju postepeno brzine i ubrzanja svih
segmenata lanca, idući od prvog ka poslednjem.
Svi vektori koji se javljaju u ovim izrazima izraženi su u odnosu na nepokretni
koordinatni sistem. Da bi došlo do primene izraza, potrebno je znati �⃗�, �⃗�,�, �⃗���,� za svaki zglob
lanca. Ako je poznata geometrija lanca, tada su ovi vektori poznati ali izraženi u odnosu na
vezane sisteme segmenta tj. poznato je �̅⃗�, �̅⃗�,�, �̅⃗���,�. Dalje, ako je poznat položaj lanca, tj.
koordinate ��, … , � �, tada se mogu izračunati prelazne matrice ��, … , � �. Sada tražene vektore u
nepokretnom koordinatnom sistemu možemo izračunati kao:
�⃗� = ���̅⃗�, �⃗�,� = ���̅⃗�,�, �⃗���,� = �����̅⃗���,� 3.5.9
Ukupno, za izračunavanje svih brzina segmenata lanca potrebno je znati geometriju i položaj
lanca, kao i generalisane brzine �̇. Za ubrzanje je potrebno znati još i �̈.
Treba spomenuti još jednu mogućnost. Umesto izraza (3.5.3) - (3.5.7) i transformacija
(3.5.9) kojima se vektori "prebacuju" u spoljašnji nepokretni sistem, moguće je zadržati vektore
u vezanim sistemima i modifikovati izraze (3.5.3) - (3.5.7) tako da važe za vezane sisteme.
Polazeći od (3.5.3) i (3.5.5), za brzine važi:
����⃗ � = ��,�������⃗ ��� + �̇�(1 − ��)�̅⃗� 3.5.10
�̅⃗� = ��,�����̅⃗��� − ����⃗ ��� × �̅⃗���,�� + ����⃗ � �̅⃗�,�� + �̇����̅⃗� 3.5.11
a analogno se iz (3.5.6) i (3.5.7) dobijaju izrazi za ubrzanja. Bez obzira na to što korišćenje
izraza u vezanim sistemima omogućava veću brzinu računanja, mi ćemo, radi jasnoće, nadalje
zadržati izražavanje u spoljašnjem nepokretnom sistemu. Brzine ���⃗ � i �⃗� je moguće predstaviti u
obliku linearnih formi po generalisanim brzinama:
���⃗ � = � ���⃗ ��
�
���
�̇�
3.5.12
�⃗� = � ���⃗ ��
�
���
�̇�
3.5.13
(gde j u oznakama �⃗�� i �⃗�
� predstavlja gornji indeks, a ne eksponent). Sada je moguće ubrzanja �⃗�
i ���⃗ � predstaviti u obliku linearnih formi po generisanim ubrzanjima:
33
�⃗� = � ���⃗ ��
�
���
�̈� + � ���⃗ ��
�
���
3.5.14
���⃗ � = � ���⃗ ��
�
���
�̈� + � ���⃗ ��
�
���
3.5.15
Pređimo sada na matričnu formu pisanja. U tom cilju, uvodimo dogovor da za svaki
vektor �⃗ odgovarajuću 3 × 1 matricu označavamo sa �. Tada izraze (3.5.12) - (3.5.15) možemo
pisati u sledećem obliku:
�� = Г��̇ 3.5.16
�� = Ω��̇ 3.5.17
�� = Г��̈ + Φ� 3.5.18
�� = Ω��̈ + Θ� 3.5.19
gde su sledeće matrice, dimenzija 3 × � odnosno 3 × 1, čije su kolone koeficijenti linearne
forme:
10 0 j j j
jГ a a 3.5.20
� = ���� 3.5.21
a matrice Ω� i Θ� su:
10 0 Ω j j j
j 3.5.22
� = ���� 3.5.23
Ove matrice se često pišu bez gornjeg indeksa j (samo Г, Φ, � i �) zato što se pri rekurzivnom
sračunavanju samo vrše njihove izmene i dopunjavanja kako bi odgovarale novom segmentu.
Razmotrimo kako se menjaju matrice Г, Φ, � i � pri povećanju indeksa j. Polazeći od rekurzivnih
izraza (3.5.3) - (3.5.7), može se pokazati da je u j-toj iteraciji (prelaz od � − 1 na �) potrebno
izvršiti sledeće izmene i dopunjavanja matrica:
���⃗ ��
= ���⃗ ����
, � = 1, … , � − 1 3.5.24
34
���⃗ ��
= (1 − ��)�⃗�
���⃗ � = ���⃗ ��� + ��⃗ 3.5.25
��⃗ = �̇����⃗ ��� × �⃗��(1 − ��)
���⃗ ��
= ���⃗ ����
− ���⃗ ����
× ��⃗ ���,� + ���⃗ ��
× ��⃗ �,�, � = 1, … , � − 1 3.5.26
���⃗ ��
= ��⃗ ��� + ���⃗ ��
× ��⃗ �,��
���⃗ � = ���⃗ ��� − ���⃗ ��� × ��⃗ ���,� + ���⃗ � × ��⃗ �,�� + ���⃗ 3.5.27
���⃗ = −���⃗ ��� × ����⃗ ��� × �⃗���,�� + ���⃗ � × ����⃗ � × �⃗�,�� + 2���⃗ ��� × �⃗����̇�
Dakle, formiranje matrica Г, Φ, �, � moguće je izvršiti rekurzivno. U svakoj iteraciji (na
primer j) lancu se dodaje novi segment. Izračunava se relativna prelazna matrica (����,�) i
apsolutna (��), pa se vektori geometrije prebacuju u spoljašnji nepokretni sistem. Sada se
primenom rekurzivnih izraza (3.5.24) - (3.5.27) formiraju matrice Г�, Φ�, ��, �� polazeći
od Г���, Φ���, ����, ����.
3.7 Jakobijan transformacije
Kao što se iz dosadašnjeg izlaganja vidi, rešenje inverznog kinematičkog problema daje
vezu između unutrašnjih (upravljanih) i spoljašnjih koordinata. Međutim, u nizu slučajeva je
potrebno da se poznaje ne samo položaj robota, već i njegove brzine i ubrzanja u prostoru
spoljašnjih koordinata. Brzina robota se najčešće meri indirektno merenjem brzina unutrašnjih
koordinata. Poznavajući geometriju robota, njegov položaj i brzinu svake unutrašnje koordinate
brzina hvataljke može da se izračuna. Za to je potrebno naći jednačine koje povezuju kinematiku
unutrašnjih i spoljašnjih koordinata.
Pošto je vektor spoljašnjih koordinata složena funkcija vektora unutrašnjih koordinata
(čije su jednačine određene rešavanjem direktnog kinematičkog problema) može da se napiše:
� = �(�) 3.7.1
Ako želimo da pokažemo uticaj diferencijalne promene unutrašnjih koordinata ∆� na
diferencijalnu promenu spoljašnjih koordinata ∆�, potrebno je naći totalni diferencijal funkcije
položaja �. Taj totalni diferencijal se označava sa � i naziva se Jakobijan transformacije ili
Jakobijanova matrica.
� =��
��
3.7.2
35
odnosno
∆� = �(�)∆� 3.7.3
Jednačine (3.7.1) i (3.7.3) mogu da se napišu i u razvijenom obliku:
�� = ��(��, ��, ��, ��, ��, ��)
�� = ��(��, ��, ��, ��, ��, ��)
�� = ��(��, ��, ��, ��, ��, ��) 3.7.4
� = ��(��, ��, ��, ��, ��, ��)
� = ��(��, ��, ��, ��, ��, ��)
� = ��(��, ��, ��, ��, ��, ��)
gde su ��, ��, �� projekcije vektora položaja � na ose nepokretnog koordinatnog sistema, a �, �, �
uglovi vektora � u odnosu na ose tog sistema.
��� =���
������ +
���
������ +
���
������ +
���
������ +
���
������ +
���
������
��� =���
������ +
���
������ +
���
������ +
���
������ +
���
������ +
���
������
��� =���
������ +
���
������ +
���
������ +
���
������ +
���
������ +
���
������
3.7.5
�� =���
������ +
���
������ +
���
������ +
���
������ +
���
������ +
���
������
�� =���
������ +
���
������ +
���
������ +
���
������ +
���
������ +
���
������
�� =���
������ +
���
������ +
���
������ +
���
������ +
���
������ +
���
������
Deljenjem jednačine (3.7.5) sa ∆� i traženjem granične vrednosti dobija se:
��
��= �̇ = �(�)�̇
3.7.6
36
Što znači da Jakobijan povezuje brzinu vektora unutrašnjih i spoljašnjih koordinata.
Diferenciranjem po vremenu dobija se:
�̈ =��
���̇ + ��̈
3.7.7
Poslednja jednačina daje vezu između unutrašnjih i spoljašnjih ubrzanja.
Za operatera, zadavanje zadatka robotu u prostoru upravljanih koordinata, tj. zadavanje funkcije
�(�) je nepogodno i primenjuje se samo u najjednostvnijim slučajevima. Funkcija �(�) je
poznata preko rešavanja direktnog kinematičkog problema i preslikava prostor unutrašnjih
koordinata u radni prostor koji je operateru znatno bliži i koga može da vidi. Zbog toga je za
operatera lakše da zadatak robotu zadaje u spoljašnjim koordinatama i tako se i radi u tehničkoj
praksi. Ako je kinematika robota zadata u radnom prostoru kao �(�), tada iz jednačina (3.7.6) i
(3.7.7) može da se odredi kinematika unutrašnjih koordinata:
�̇ = ����̇
3.7.8
�̈ = ��� ��̈ −��
���̇�
Pošto je potrebno da se odredi inverzna matrica ���, Jakobijan transformacije mora da bude
kvadratna nesingularna matrica.
Postupak formiranja Jakobijana je prilično složen problem. Prilikom njegovog rešavanja
ima više pristupa. U daljem delu, izložen je jedan postupak kojim se izračunavaju Jakobijan i
pridružena matrica u funkciji unutrašnjih koordinata tj. �(�) i �(�, �̇). Postupak je izveden za
slučaj � = 5 i � = 6, i polazi od izraza (3.6.16) koji se transformiše uvođenjem vektora �.
Preciznije, �� = [�̈�, �̈�, �̈�] je već deo vektora �̈, a preostaje da se �� izrazi u funkciji �̈, �̈, �̈.
Razmotrimo sada kako se može zadati kretanje robota. Jedna od mogućnosti je da se zada
vremenska promena generalisanih (tj. unutrašnjih) koordinata, dakle �(�). Znači, zadajemo
direktna kretanja u zglobovima mehanizma. Pošto znamo kako se tokom vremena menjaju
unutrašnje koordinate ��(�), … , � �(�), kretanje robota je jednoznačno određeno. Zakonima �(�)
određeni su i izvodi �̇(�) i �̈(�). Sada je moguće izračunati spoljašnje kretanje korišćenjem
izraza (3.7.1), (3.7.6) i (3.7.7). Ovakvo sračunavanje spoljašnjeg kretanja �(�) ako je poznato
unutrašnje kretanje �(�), nazivamo direktnim problemom kinematike robota. U nekim prostijim
manipulacionim zadacima direktno se zadaju pomeranja u zglobovima. Primer za to bi bio robot
na Sl.3.7.1. Radni predmet treba preneti prvo iz tačke �� u tačku ��. Pri ovakvom kretanju
menja se koordinata �� za �/2. Takođe, menja se koordinata �� koja vrši obrtanje radnog
predmeta za �/2. Pri kretanju od tačke �� do �� menja se opet koordinata �� unazad za �/2 i
istovremeno se menja �� spuštajući predmet za 0.5 �. Konačno, pri kretanju od tačke �� do
37
početnog položaja �� opet se menja koordinata ��, ovog puta naviše za 0.5 � i menja se
koordinata �� koja obrće predmet unazad za �/2 vraćajući ga tako u početni položaj. Ovakvo
zadavanje kretanja pogodno je samo za jednostavne manipulacione zadatke. U složenijim
zadacima uvek je neophodno ostvariti određeno kretanje hvataljke koja će pri tome izvršiti
postavljeni zadatak (Sl.3.7.2.). Jasno je da je nemoguće unapred predvideti pomeranja u
zglobovima (tj. �(�)) koja bi ostvarila željeno kretanje hvataljke. Zato takve zadatke određujemo
preko vektora spoljašnjeg položaja hvataljke (vektor �). Zadajemo kretanje vrha tako što
zadajemo zakon vremenske promene koordinata ��(�), ��(�) i ��(�). Zakon promene
orijenatacije zadajemo tako što zadamo vremensku promenu uglova �(�), �(�) i �(�). U slučaju
robota sa pet stepeni slobode, �(�) je zavisno i ne zadaje se [5].
Sl.3.7.1. - Zadavanje kretanja preko unutrašnjih koordinata [5]
Sl.3.7.2. - Dva primera zadavanja orijentacije [5]
Zadali smo manipulacioni zadatak preko vremenske promene vektora �(�). Sada ćemo
pokazati kako se može izračunati unutrašnje kretanje �(�) ako je spoljašnje kretanje �(�) zadato.
Zakon �(�) određuje i �̇(�) i �̈(�). Sada bi na osnovu � trebalo odrediti �, na osnovu �̇
odrediti �̇ i konačno, na osnovu �̈ odrediti �̈. Međutim, ovo nije lako realizovati. Već smo ranije
napomenuli da je računanje � iz poznatog � veoma složen posao, često bez jednoznačnog
38
rešenja. Dalje, računanje �̇ pomoću relacije (3.7.6) zahteva prethodno izračunavanje Jakobijana,
a njega ne možemo izračunati, jer ne znamo �. Isto vazi i za pokušaj računanja �̈ pomoću
relacije (3.7.6).
Ovaj složeni problem izračunavanja unutrašnjeg kretanja �(�) iz poznatog spoljašnjeg
kretanja �(�) naziva se inverzni problem kinematike robota. Objasnićemo zašto je rešavanje
ovog problema važno. Sa jedne strane, zaključili smo da je manipulacioni zadatak pogodno
zadati u obliku kretanja hvataljke po zakonu �(�). Sa druge strane, robot se pokreće pomoću
motora koji deluju u zglobovima i koji izazivaju pomeranja u zglobovima. Prema tome, za
delovanje motora bitna je promena koordinata u zglobovima, tj. ��(�), ��(�), … , � �(�). Da bi se
izvršila sinteza upravljanja kretanjem robota neophodno je izračunati unutrašnje kretanje �(�),
odnosno rešiti inverzni problem kinematike.
Sada ćemo objasniti jedan praktičan postupak za numeričko rešavanje inverznog
problema kinematike. Treba naglasiti da je postupak namenjen za korišćenje na računaru.
Pre nego što pređemo na objašnjenje samog postupka, uvešćemo pojam stanja
mehaničkog sistema. U strogu definiciju pojma stanja u smislu teorije sistema ovde nećemo
ulaziti. Zadovoljićemo se time da kažemo da je stanje mehaničkog sistema u nekom trenutku
vremena određeno poznavanjem položaja � i brzine �̇. Ove dve veličine (u stvari dve matrice �
i �̇) definišu stanje, zato što su položaj i brzina one veličine koje se ne mogu trenutno promeniti,
već se do narednog, beskonačno bliskog trenutka vremena mogu promeniti samo beskonačno
malo. Za razliku od njih, ubrzanje se može skokovito menjati u trenucima početka ili prestanka
dejstva sile. Na ovaj način, položaj i brzine se javljaju kao neophodni početni uslovi za
određivanje daljeg kretanja sistema. Zato ove veličine uzete zajedno nazivamo stanjem sistema.
Pod stanjem podrazumevamo par kolona matrica � i �̇.
Analizirajmo sada početno stanje sistema koji ovde posmatramo, a to je kinematički
lanac robota. Početno stanje određeno je vrednostima � i �̇ u početnom trenutku vremena ��,
dakle �(��) i �̇(��). U slučaju neredundantnih i nesingularnih mehanizama početno stanje se
može definisati i preko položaja hvataljke i njene brzine u početnom trenutku ��, dakle �(��)
i �̇(��). Ranije smo već napomenuli da ćemo zadatak zadati preko kretanja hvataljke �(�). Na taj
način biće određeno i početno stanje �(��), �̇(��). Postupak koji ćemo predložiti za rešavanje
inverznog problema kinematike zahteva poznavanje početnog stanja u obliku �(��), �̇(��). Za taj
početni trenutak morao bi se sprovesti ranije spomenuti složeni proračun unutrašnjih koordinata
na osnovu poznatog položaja hvataljke �. Srećom, videćemo da se takav proračun obavlja samo
jednom i to za početni trenutak vremena. Za kasnije trenutke vremena to neće biti potrebno.
Treba još istaći da je, bez obzira na zadavanje zadataka preko kretanja hvataljke, često poznato
početno stanje u obliku �(��), �̇(��). To je zbog toga što robot obično kreće iz nekog svog
uobičajenog početnog položaja u kome su poznati svi položaji zglobova, tj. sve koordinate �.
Početne brzine su tada obično jednake nuli jer robot kreće iz mirovanja.
39
Sledeće što ćemo analizirati biće način numeričkog zadavanja vremenskih funkcija.
Umesto zadavanja nekim analitičkim izrazom, što u složenijim slučajevima nije ni moguće,
funkciju vremena zadajemo u vidu niza tačaka, odnosno nizom njenih vrednosti koje odgovaraju
nizu trenutaka vremena. Primenimo ovo na slučaj zadavanja vremenske promene položaja
hvataljke �(�). Sada bi za svaku komponentu kolona matrice � zadali niz vrednosti koje bi
definisale promenu te komponente tokom vremena. Problem nastaje onda kada treba odrediti
izvode �̇(�) i �̈(�) i kada bi trebalo izvršiti numeričko diferenciranje. Pošto je numeričko
diferenciranje nepoželjan zadatak u svakom proračunu, sada ćemo izložiti postupak kojim će se
ovaj problem izbeći.
Poći ćemo od činjenice da početno stanje �(��), �̇(��) i vremenska promena
ubrzanja �̈(�) jednoznačno određuju kretanje �(�). Zato ćemo manipulacioni zadatak i zadati
preko određenog početnog stanja i vremenske promene ubrzanja. Pokažimo sada na dva primera
da je zadavanje kretanja preko ubrzanja veoma pogodno. Zamislimo da želimo pravolinijsko
kretanje vrha robota između dve tačke uz trougaoni profil brzine (Sl.3.7.3.al), tj. do polovine
putanje (deo ���′) vrh ubrzava, a od polovine (deo �′��) usporava, tako da se u tački �� ponovo
zaustavi. Profil ubrzanja tada je prikazan na Sl.3.7.3.bl i može se lako zadati:
�̈�(�) = �+��, � < �/2−��, � > �/2
� 3.7.9
Sl.3.7.3. - Trougaoni i trapezni profil brzine [5]
odnosno, do polovine vremena izvršenja zadatka ubrzanje je konstantno i iznosi +�, a od
polovine konstantno i negativno i iznosi −�. U slučaju trapeznog profila brzine (Sl.3.7.3.a2),
razlikujemo period ubrzavanja (deo ���′), period konstantne brzine (deo �′�") i period
usporavanja (deo �"��). Profil ubrzanja prikazan je na Sl.3.7.3.b2 i može se izraziti u obliku:
40
�̈�(�) = �
+��, � < ��
0, � � < � < � − ��
−��, � > � − ��
� 3.7.10
gde je � ukupno vreme, a �� vreme ubrzavanja, odnosno usporavanja. Treba reći da ova dva
profila brzine spadaju u najčešće profile u praktičnim manipulacionim zadacima. To je važno jer
vidimo da se upravo u tim najčešćim slučajevima, ubrzanje zadaje veoma jednostavno (relacije
3.7.9 i 3.7.10). Kako smo već rekli da vreme i funkcije vremena posmatramo diskretno, tj. u nizu
trenutaka, to ćemo vremenski interval � u kome se zadatak izvršava podeliti na � podintervala
��, uvodeći vremenske trenutke ��, ��, … , � � (Sl.3.7.4.). Sada ćemo relacije za ubrzanje (3.7.9) i
(3.7.10) tretirati diskretno, tj. zadati nizove vrednosti koje odgovaraju trenucima ��, ��, … , � � [5].
Sl.3.7.4. - Diskretizacija intervala kretanja
Sl.3.7.5. - Šema postupka za numeričko rešavanje inverznog problema kinematike robota [5]
41
Manipulacioni zadatak smatramo zadatim preko zadatog početnog stanja �(��), �̇(��) i
poznate vremenske promene ubrzanja hvataljke �̈(�). Sada ćemo objasniti postupak za rešavanje
inverznog problema kinematike, tj. za izračunavanje unutrašnjeg kretanja �(�). Postupak počinje
u početnom trenutku ��. Kako su �(��) i �̇(��) poznati, tada za taj trenutak možemo izračunati
Jakobijan �(�) i pridruženu matricu �(�, �̇). S obzirom na to da je �̈(��) poznato, sada iz relacije
(3.7.7) izračunavamo vrednost �̈(��) u posmatranom trenutku ��:
�̈ = ���(�̈ − �) 3.7.11
Posmatrajmo sada subinterval između trenutaka �� i ��. Za kratak subinterval ∆� možemo
smatrati da se � na tom subintervalu neće promeniti, već će zadržati vrednost izračunatu za ��.
Tada možemo tokom subintervala ∆�, promenu � posmatrati kao jednako ubrzano kretanje i
formirati izraze kojima ćemo izračunati stanje �(��), �̇(��) u trenutku ��:
�(��) = �(��) + �̇(��)∆� +�
��̈(��)∆�� 3.7.12
�̇(��) = �̇(��) + �̈(��)∆� 3.7.13
Sada, kada znamo stanje �(��), �̇(��) u trenutku ��, počinjemo novi ciklus ponavljajući za
trenutak �� ceo račun koji smo sprovodili za ��. Tako, rekurzivno izračunavamo stanje �, u
trenucima ��, ��, … , � �.
Kao rezultat opisanog postupka dobijamo nizove vrednosti � i �̇ koji odgovaraju nizu
trenutaka vremena, odnosno dobijamo zakon kretanja u unutrašnjim koordinatama �(�) i �̇(�).
Ovim smo rešili inverzni problem kinematike robota [5].
Ceo opisani postupak može se prikazati šemom koja je prikazana na Sl.3.7.5.
4. Primeri
4.1.1 Uvod
Ovaj zadatak opisuje model industrijskih robota, njihovo projektovanje i analizu
primenom softverskog paketa MATLAB, Simulink i Robotics Toolbox [4]. Simulink je
okruženje u MATLAB-u u kojem se uređivanjem blok dijagrama vrše odgovarajuće simulacije, i
to je standradni deo u MATLAB-u. Robotics Toolbox je alat specijalizovan za analizu i
simulaciju industrijskih robota. Ovaj Toolbox obezbeđuje mnoge funkcije koje su korisne u
robotici, uključujući kinematiku, dinamiku i generisanje putanje. To je korisno pri simulaciji, kao
i za analizu eksperimentalnih rezultata koji su dobijeni na stvarnim robotima. Toolbox se zasniva
na opštoj metodi serijskog povezivanja kinematike i dinamike manipulatora. Toolbox takođe
obezbeđuje funkcije za manipulaciju i konvertovanje između tipova podataka, kao što su vektori,
homogene transformacije i jediničnih vektora koji su neophodni za predstavljanje
trodimenzionalnog položaja i orijentacije. Takođe koristi standardno i modifikovano Denavit-
42
Hartenberg-ovo predstavljanje kinematike manipulatora. Korišćenje Toolbox-a sa Simulink-om
obuhvata biblioteku blokova za izradu kinematičkih i dinamičkih modela robota. U daljem delu
prikazaćemo primenu ovog softvera na primeru industrijskog robota PUMA 560.
4.1.2 Matrice transformacije
U oblasti robotike postoji mnogo načina za predstavljanje položaja i orijentacije robota i
efektora, ali u MATLAB-u se koristi homogena transformacija kao moćan alat za manipulaciju
matricama. Homogene transformacije definišu položaj i orijentaciju između dva Kartezijanova
pravougla koordinatna sistema. Matrica homogenih transformacija je kvadratna matrica reda 4x4
koja predstavlja položaj vektora u homogenim koodrinatama iz jednog koordinatnog sistema u
drugi. Možemo smatrati da se homogena matrica transformacije sastoji iz četiri submatrice [2]:
4.1.2.1
Gornja leva 3 × 3 submatrica predstavlja matricu rotacije i može se definisati kao matrica
transformacije koja operiše sa pozicijom vektora u trodimenzionalnom euklidskom prostoru i
mapira njegove koordinate izražene u rotirajućem koordinatnom sistemu ���� na referentni
koordinatni sistem ����. Gornja desna 3 × 1 submatrica predstavlja vektor pozicije koji se
odnosi na poziciju rotirajućeg koordinatnog sistema u odnosu na referentni sistem. Donja leva
1 × 3 submatricca predstavlja transformaciju perspektive. Četvrti element dijagonale je globalni
faktor skaliranja. Homogena matrica transformacije se može koristiti kako bi se objasnio
geometrijski odnos između tela unutar rotirajućeg koordinatnog sistema ���� i referentnog
koordinatnog sistema ����. U industrijskoj robotici se najčešće primenjuje sledeći oblik
homogene matrice transformacije:
4.1.2.2
Ova matrica opisuje odnos između Kartezijanovog koordinatnog sistema u vidu Kartezijanove
translacije, vektor �, i orijentacije izražene u vidu ortonormalne 3 × 3 matrice rotacije, �. Na
primer, u MATLAB-u u Robotics Toolbox-u, homogena transformacija za predstavljanje
translacije od 0.25 � u pravcu � ose može biti kreirana na sledeći način:
43
Kao rezultat toga, u MATLAB komandnom prozoru prikazaće se 4 × 4 homogena matrica
transformacije, �:
I rotacija za 90 � oko � ose:
Kao rezultat toga, u MATLAB komandnom prozoru prikazaće se 4 × 4 homogena matrica
transformacije, �:
Ove transformacije mogu biti povezane množenjem na sledeći način:
A rezultat je:
Mehanički manipulator može biti modeliran kao zglobni lanac u otvorenoj petlji sa više
krutih tela (link-ovi) koji su serijski povezani pomoću obrtnih ili prizmatičnih zglobova koji se
pogone pomoću aktuatora. Jedan kraj lanca je pričvršćen za nepokretnu osnovu (bazu), dok je
drugi kraj slobodan i njemu je priključen alat za manipulaciju predmetima ili za obavaljanje
poslova montaže. Relativno kretanje u zglobovima rezultira pomeranjem link-ova koji na taj
način pozicioniraju ruku u željenoj orijentaciji. Kinematika ruke robota se bavi analitičkim
istraživanjem geometrije kretanja ruke robota u odnosu na fiksni referentni koordinatni sistem u
funkciji vremena, ne uzimajući u obzir snagu/moment koji dovode do tog kretanja. Dakle, to je
44
analitički opis prostornog kretanja robota u funkciji vremena, i posebno se odnosi na položaj i
orijentaciju vrha slobodnog dela ruke robota u prostoru. Kao što je već rečeno, postoje dva
osnovna pitanja koja su u interesu za teorijski i praktičan pristup kinematike ruke robota:
direktna i inverzna kinematika. Budući da se nezavisne promenljive ruke robota odnose na
promenljive u zglobovima i da se zadatak obično navodi u odnosu na referentni koordinatni
sistem, problem inverzne kinematike se češće koristi. Jednostavan blok dijagram koji ukazuje na
vezu između ova dva problema prikazan je na Sl.4.1.2.1.
Sl.4.1.2.1. – Problem direktne i inverzne kinematike
4.1.3 Instalacija Robotics Toolbox-a
Prvo što treba uraditi je, skinuti sa sajta http://petercorke.com/Robotics_Toolbox.html
MATLAB Robotics Toolbox. Da bismo instalirali Robotics Toolbox, treba raspakovati
prethodno skinut sadržaj (folder rvctools) u MATLAB-ovom toolbox folderu (npr. C:\Program
Files\MATLAB\R2009b\toolbox\rvctools). Zatim restartujemo MATLAB. Nakon toga ukucamo
komandu startup_rvc, a zatim unesemo funkcije doc rpy2tr i doc rpy2r, i na kraju ukucamo
rttrdemo.
4.1.4 Primer inverzne kinematike
Problem inverzne kinematike robota je pronalaženje zajedničkih koordinata, s obzirom na
to da homogena transformacija predstavlja poziciju kraja efektora u odnosu na nepokretni
koordinatni sistem. Kao primer rešavanja inverzne kinematike u MATLAB-ovom Robotics
Toolbox-u, prvo generišemo transformaciju koja odgovara određenoj koordinati zgloba:
45
Sada je potrebno generisati matricu homogene transformacije koja određuje položaj i orijentaciju
kraja efektora u odnosu na nepokretni koordinatni sistem, za iznad definisane generalizovane
koordinate:
Kao rezultat dobijamo:
A zatim nalazimo odgovarajuće uglove zglobova pomoću komande ikine:
Postupak inverzne kinematike za bilo koji određeni robot se može izvesti simbolično [3] i
najčešće se efikasno rešenje može dobiti u zatvorenoj formi. Međutim, Toolbox daje samo opšti
opis manipulatora u smislu kinematičkih parametara, tako da se mora koristiti iterativno brojno
rešenje. Takav postupak može biti spor, a izbor početne vrednosti utiče na vreme pretrage pri
traženju rešenja, jer generalno, manipulator se može dovesti u više pozicija za dostizanje jedne
zadate tačke. Početna tačka za rešenje može biti navedena, u suprotnom se podrazumeva da ima
vrednost nula (što nije naročito dobar izbor u ovom slučaju) i obezbeđuje ograničenu kontrolu
nad određenim rešenjem koje će biti pronađeno. Kao rezultat toga, u komandnom prozoru će se
prikazati:
U poređenju sa originalnom vrednošću �:
Treba napomenuti da rešenje nije moguće ako navedena transformacija opisuje tačku van
domašaja manipulatora – u tom slučaju funkcija će vratiti grešku. Inverzna kinematika se može
46
koristiti i za izračunavanje putanje, tj. trajektorije. Na primer, u Kartezijanovom koordinatnom
sistemu može se računati putanja po pravoj liniji.
- generiše vremenski vektor
- definiše početnu tačku putanje sa odgovarajućom matricom
transformacije:
- definiše krajnju tačku putanje sa odgovarajućom matricom
transformacije:
- računa putanju u Kartezijanovom koordinatnom sistemu.
Rezultati su 36 homogene matrice transformacije (koje se prikazuju u komandnom prozoru) koje
određuju položaj i orijentaciju kraja efektora u odnosu na nepokretni koordinatni sistem za 36
tačaka. Prava linija putanje u Katrezijanovom koordinatnom sistemu je određena vremenskim
vektorom, početnom i krajnjom tačkom. Sada rešavamo inverznu kinematiku. Kod računanja
trajektorije, početna koordinata zgloba za svaku tačku se uzima kao rezultat prethodnog
inverznog rešenja.
U komandnom prozoru će se prikazati matrice transformacije koje su izračunate u procesu
iteracije. Takođe, prikazaće se i generalizovane koordinate zglobova za svih 36 tačaka
trajektorije. Proteklo vreme je 0.984000 [�]. Jasno se vidi da je ovakav pristup spor i nije
pogodan za realnu kontrolu robota gde je za problem inverzne kinematike potrebno dobiti
rešenje za nekoliko milisekundi. Razmotrimo putanju zglobova u prostoru koja rezultuje
- početak brojanja
- kraj brojanja
47
pravolinijsku trajektoriju u Dekartovom koordinatnom sistemu. Generalizovane koordinate za
svaki zglob manipulatora se mogu prikazati u vremenskom dijagramu (Sl.4.1.4.1.):
Sl.4.1.4.1. – Promena uglova zglobova u [���] u toku vremena
Naravno, ovu prostornu putanju zglobova možemo animirati korišćenjem sledeće komande:
Izgled manipulatora koji se animira korišćenjem ove komande prikazan je na Sl.4.1.4.2.
48
Sl.4.1.4.2. – 3D manipulator koji je dostupan u MATLAB-ovom Robotics Toolbox-u
4.1.5 Trajektorije
Kao što je već rečeno, homogene transformacije se najčešće koriste za predstavljanje
položaja i orijentacije manipulatora u Dekartovom koordinatnom sistemu. U robotici je često
potrebno baviti se i trajektorijom, tj. putanjom, koja se može predstaviti kao niz tačaka ili uglova
zglobova. Uzmimo primer putanje po kojoj će se kretati PUMA robot, pri čemu će on biti u
uspravnom položaju kada ugao zgloba ima vrednost od 0 �. Prvo generišemo vremenski vektor,
tako da pratimo kretanje u prve 2 [�] sa vremenom uzorkovanja od 56 [��].
A zatim računamo putanje zglobova u prostoru između dve pozicije korišćenjem komande jtraj:
� je matrica sa jednom vrstom po vremenskom koraku (inače, u ovom slučaju 36 vrsta) i šest
kolona, za svaku od šest generalizovanih koordinata. Konkretno, za ovaj slučaj putanje, većinu
kretanja vrše zglobovi 2 i 3, i to se može grafički prikazati korišćenjem strandardnih komandi u
MATLAB-u:
49
Sl.4.1.5.1. – Promena uglova zglobova u toku vremena
Takođe, možemo dobiti profil brzine i ubrzanja. Možemo da diferenciramo putanju ugla koristeći
komandu diff, ali se precizniji rezultati dobijaju korišćenjem komande jtraj koja vraća ugaonu
brzinu i ugaono ubrzanje na sledeći način:
Naravno, i profil ugaone brzine se može prikazati grafički:
50
Sl.4.1.5.2. – Brzine zglobova u toku vremena
I na kraju, profil ugaonog ubrzanja:
51
Sl.4.1.5.3. – Ubrzanje zglobova u toku vremena
4.2 Primer inverzne kinematike u Simulink-u
Predstavićemo primer inverzne kinematike robota u ravni (2D prostor). Robot se sastoji
iz tri dela (kraka) koji su međusobno vezani zglobovima, tako da kraci mogu međusobno da
rotiraju jedan oko drugog, ali ne mogu da se kreću translatorno. Zglob 1 je vezan za nepokretnu
tačku (koordinatni početak), zglob 2 povezuje krak 1 i krak 2, a zglob 3 povezuje krak 2 i krak 3.
Cilj je da kraj trećeg kraka dođe na zadatu poziciju (koju zadajemo u vidu �, � koordinate).
Robot se kreće u �� ravni, tj. obrće se oko � ose. Dužina svakog kraka je 1, tako da je
maksimalna dostižnost robota u pravcu � i � ose 3. Primer je rađen u progamskom paketu
MATLAB u Simulink okruženju. Blok dijagram inverzne kinematike robota prikazan je na
Sl.4.2.1.
52
Sl.4.2.1. – Blok dijagram inverzne kinematike robota u Simulink okruženju
Na Sl.4.2.2.a prikazan je deo blok dijagrama pomoću kojeg unosimo koordinate tačke koju
želimo da dostignemo pomoću robota. Ako se otvori blok za � koordinate, otvara se prozor
(Sl.4.2.2b) u kojem unosimo želejnu vrednost koordinate za � osu. Vidi se da je granica da
vrednost od -3 do 3, jer je ukupna dužina sva tri kraka 3, tako da su sve tačke iznad te vrednosti
nedostižne. Isti slučaj je i za � osu.
a. b.
Sl.4.2.2. – Blok za unošenje željene koordinate koju želimo da dostignemo
Na Sl.4.2.3. prikazan je prozor koje se otvara kada se otvori blok Jacobian. Prikazana šema služi
za izračunavanje matrica Jakobijana pomoću koje se izračunavaju brzine i ubrzanja svih
segmenata robota, ali naravno, računa i uglove koje kraci robota zaklapaju sa x osom.
Matematički to izgleda ovako:
cos sin 0 0
sin cos 0 0( , )
0 0 1 0
0 0 0 1
J z
4.2.1
53
Sl.4.2.3. – Blok dijagram za izračunavanje matrice Jakobijana
Na Sl.4.2.4. prikazan je prozor koje se otvara kada se otvori blok Forward Kinematic. Prikazana
šema služi kao povratna sprega koja upoređuje koordinatu tačke koja je zadata i trenutnu
koordinatu vrha poslednjeg kraka robota koji i treba da dostigne željenu koordinatu. Kako se vrh
robota približava zadatoj koordinati, tako segmenti robota usporavaju svoje kretanje.
Sl.4.2.4. – Blok dijagram u funkciji povratne sprege
Na Sl.4.2.5. prikazan je prozor koje se otvara kada se otvori blok DLS. Prikazana šema služi za
izračunavanje transponovane i inverzne matrice Jakobijana pomoću kojih se izračunavaju
unutrašnje koordinate. Koeficijent pojačanja je jednak � = 0.1.
54
Sl.4.2.5. – Blok dijagram za određivanje inverzne i transponovane matrice Jakobijana
Na Sl.4.2.6. prikazan je blok dijagram koji predstavlja P kontroler. On ustvari predstavlja
proizvod modifikovane matrica Jakobijana, greške, koja predstavlja razliku između zadate
koordinate koju robot treba da dostigne i trenutne koordinate vrha robota i matrice pojačanja koja
je jednaka:
� = �1 00 1
� 4.2.2
Sl.4.2.6. – Blok dijagram za P kontroler
Na Sl.4.2.7. prikazan je blok dijagram Motor. Na slici je prikazana i šema koja predstavlja
funkciju prenosa motora koji pokreće segmente robota. Na Sl.4.2.8. se vidi da je u bloku Motor
definisana početna pozicija svakog segmenta. Početna pozicija za svaki segment je � 4⁄ .
Sl.4.2.7. – Blok dijagram za motor
55
Sl.4.2.8. – Početna pozicija svakog segmenta robota
Na Sl.4.2.9.a prikazano je kretanje segmenata robota. Plava tačka predstavlja zadatu koordinatu
koja treba da se dostigne. Na Sl.4.2.9.b vidi se da je robot dostigao zadatu koordinatu.
a. b.
Sl.4.2.9. – Segmenti robota pre i posle dostizanja zadate tačke
Na Sl.4.2.10. prikazana je brzina kojom su se zglobovi kretali do dostizanja zadate koordinate.
Zuti signal se odnosi na zglob 2, plavi na zglob 3, a ljubičasti na zglob 4, tj. krajnju tačku trećeg
segmenta (vrh robota). Brzine su u ��� �⁄ .
56
Sl.4.2.10. – Brzine zglobova robota
Na Sl.4.2.11. prikazane su vrednosti uglova koje svaki segment robota zaklapa sa x osom.
Sl.4.2.11. – Uglovi koje zaklapaju segmenti robota
57
Literatura
[1] MILIĆEVIĆ, I., Transformation matrix approach to kinematics and dynamics of robot
manipulators, Master thesis, Technical Faculty, Čačak, 2006.
[2] FU, K. S., GONZALEZ, R. C., LEE, C. S. G., Robotics: Control, Sensing, Vision, and
Intelligence, Mc Graw-Hill Book Company, ISBN 0-07-100421-1, 1987.
[3] Robotic and Automation Handbook, edited by Thomas R. Kurfess, CRC Press, 2005. Journal
articles
[4] CORKE, P. I., A Robotics Toolbox for MATLAB, IEEE Robotics and Automation
Magazine, Vol.3, No.1, pp.24-32, March 1996.
[5] http://automatizacija1.etf.rs/poglavlja/Robotika%202.htm
[6] Nikolić, I., V. Čović, Izabrana poglavlja mehanike robota, Mašinski fakultet Beograd,
Beograd, 1999.