PROGRAMAREA

ROBOTILOR INDUSTRIALI
CURS 5
Formularea problemei: Problema de cinematica
inverse (IK) consta in determinarea variabilelor
articulatiilor care corespund unei pozitii si orientari
date ale efectorului terminal:
Se dau: , parametrii articulatiilor si segmentelor
robotului.
Se cer: Unghiurile in articulatii corespunzatoare
, a.i. efectorul terminal sa fie pozitionat
conform compunerii.
5. Cinematica Inversa
5.1. Problematica IK
0
6
T
| |
T
q q q
6 1
, ,K =
Observatie: Pozitia si orientarea efectorului
terminal pot fi exprimate in 2 moduri:
, cand DK este exprimata cu
- implicit (DKI)
sau:
, cand DK este exprimata cu
- explicit (DKE).
( ) q T T
n d
0
=
( ) ( ) ( )
n
n
n n
q A q A q A T
1
2
1
2 1
0
1
0
= L
( ) q k x
d
=
( ) q k x =
Comentarii:
Referitor la DKI, pozitia si orientarea efectorului terminal, exprimate
prin matricile de pozitie (3x1), sunt calculate in mod unic (unic
definite), in timp ce, referitor la DKE, k nu e unic, deoarece
unghiurile Euler sau RPY nu sunt unic definite.
Prolema IK este mai complexa decat DK deoarece:
ecuatiile de rezolvare sunt neliniare nu intotdeauna este posibila
gasirea unei solutii in forma inchisa (CIKS);
pot exista solutii multiple;
pot exista solutii infinite (ex. in cazul robotilor redundanti
cinematic);
pot sa nu existe solutii admisibile, considerand structura
cinematica a robotului.
Existenta solutiilor IK: este garantata daca pozitia si orientarea
dorita (data) a efectorului terminal apartin spatiului de lucru cu
dexteritate al robotului.
Problema solutiilor multiple depinde de:
nr. de grade de mobilitate;
nr. de parametri D-H nenuli; in general, nr. solutiilor multiple este cu atat
mai mare cu cat nr. parametrilor D-H nenuli este mai mare.
Exemplu:
Pentru un manipulator cu 6 g.d.l., fara limitari
mecanice ale articulatiilor, sunt in general pana la 16
solutii admisibile (ex. cazuri cot in jos / in sus);
Existenta limitarilor mecanice limiteaza nr. solutiilor
multiple admisibile.
Metode de calcul pentru IK: Exista 2 aboradari pentru
calculul CIKS:
1. Abordarea algebrica: gasirea acelor ecuatii
semnificative ce contin necunoscutele.
2. Abordarea geometrica: gasirea acelor puncte
semnificative pe structura, in raport cu care este
comod sa fie exprimate pozitii si/sau orientari functie
de un nr. redus de necunoscute.
5.2. Tehnici de transformare inversa
pentru solutia cu unghiuri Euler
Abordarea algebrica pentru sistem I de unghiuri
Euler. Matricea (3x3) de Rotatie poate fi
exprimata in functie de unghiurile Euler/sistem I
( ) , ,
(
(
(

+ +

=
=
(
(
(

(
(
(

(
(
(

= =









c c s c s
s c c c c s s s c c c s
s s c c s s c s c s c c
c s
s c
c s
s c c s
s c
R R R R
z x z
1 0 0
0
0
0
0 0 1
1 0 0
0
0
, ' , ' , , ,
Fiind data
(1)
Se cer valorile corespunzatoare ale .
Solutie generala: Se premultiplica Ec. (1)
succesiv cu transformarile inverse in
necunoscute (ex: , ...), si se determina
unghiul necunoscut egaland combinatii de
elemente din ecuatia matriciala rezultata dupa
premultiplicare.
, ,
R
a s n
a s n
a s n
z z z
y y y
x x x
=
(
(
(

, ,
.
1
,

z
R
Se muta o necunoscuta (prin transformarea
inversa R
-1
), prin premultiplicare din membrul
drept al Ec. (1) in membrul stang cunocut al
Ec (1) si se afla aceasta necunoscuta apoi se
muta urmatoarea necunoscuta in membrul
stang, si se repeta procedeul pana la
rezolvarea tuturor necunoscutelor):
sau
(
(
(

(
(
(

=
(
(
(

(
(
(

1 0 0
0
0
0
0
0 0 1
1 0 0
0
0





c s
s c
c s
s c
a s n
a s n
a s n
c s
s c
z z z
y y y
x x x
Egalam elementele (1,3):
unde
(
(
(

=
=
(
(
(

+ + +
+ + +





c c s s s
s c c s c
s c
a s n
a c a s s c s s n c n s
a s a c s s s c n s n c
z z z
y x y x y x
y x y x y x
0
( )
y x
y
x
y x
a a a
a
a
a s a c
=
(
(

=
= +

, 2 tan tan
0
1

Notatie: pt.
Egaland elementele (1,1) si (1,2):
de unde:
( )

+ < <
<
+
+ +
= =
(

= =

y - si x pentru , 0 90 -
y - si x - pentru , 90 180
y si x - pentru , 180 90
y si x pentru , 90 0
, 2 tan tan
1

x y a
x
y
x
y
tg
y x
y x
s s s c s
n s n c c
=
+ =


( )
y x y x
n s n c s s s c a
c
s
+ =
(

, 2 tan tan
1
Egaland elementele (2,3) si (3,3):
de unde
Observatie:
1) Conceptul transformarii inverse consta in a muta
cate o necunoscuta in membrul stang cunoscut, al
ec. matriciale:
, pe rand, si in a rezolva acea
necunoscuta.
2) Se poate utiliza in acest sens si postmultiplicarea.
z
y x
a c
a c a s s
=
= =

( )
z y x
z
y x
a a c a s a
a
a c a s
, 2 tan tan
1
=
(

( )
Euler d
R a s n R = , ,
5.3. Exemplu de CIKS prin abordare
geometrica (Robotul MINIMOVER)
Solutia IK este dezvoltata in 2 pasi:
pasul 1 se calculeaza variabilele articulatiilor de la
trunchi la incheitura ;
pasul 2 se calculeaza variabilele ultimelor 2
articulatii:
Metoda: Bratul, antebratul si gripperul
MINIMOVER sunt proiectate pe planul lor comun
de miscare planul de rotatie in jurul axei z
0
(Fig. 1):
( )
3 2 1
, ,
( )
5 4
,
Fig. 1. Descrierea planului de miscare pentru bratul MINIMOVER
Acest plan de miscare: (se obtine
proiectand al centrului gripper-ului):
pe planul ( );
pe axa ( ).
Pas 1 : Fie spatiul operational:
,
ce descrie pozitia dorita a varfului uneltei
si orientarea efectorului terminal
0 0 0
0 y x z
HH H O
PS H
0 0
y x
o
y x y x
H H pr
0 0 0
. =
0
z
0 0
.
z z
H H pr =
(
(
(
(
(
(

=
5
4

z
y
x
p
p
p
x
( )
z y x
p p p H , ,
( ). ;
5 4
roll pitch = =
Se cunosc : parametrii D-H ai modelului:
.
Rezulta:
Vectorul de pozitie de la umar la incheietura,
SW, este obtinut proiectand punctul S (originea
SC articulatie umar si W originea SC
articulatie incheietura) pe liniile si .
Proiectiile vectorului SW:
TOOL
l d d a a a si , , , ,
5 1 3 2 1
( )
y x
x
y
p p a
p
p
, 2 tan tan
1
1
=
(

0 0
y x
OH
0
z
OH
( )
( )
4 5 1
4 5 1
2 2
sin
cos

+ =
+ + =
TOOL z
TOOL y x
l d d p PZ
l d a p p PR
unde:
= coordonata operationala ce exprima unghiul
absolut de flexare a mainii (unghiul mainii cu planul
orizontal ).
Utilizand teorema cosinusurilor in SEW:
de unde rezulta:
cu conditia de existenta:
4

0 0
y x
( )
3 3 2
2
3
2
2
2 2
cos 2 + = + a a a a PZ PR
(

+
=

3 2
2 2 2
3
2
2 1
3
2
cos
a a
PZ PR a a

3 2
2 2
3 2
a a PZ PR a a + +
Insumand unghiurile si se obtine
:
Pas 2: Proiectand ultimul segment efectorul
terminal WH pe directiile rezulta:
Unde este unghiul roll absolut dorit, la efector.
0 0
,
y x
o
H O SE


SW SE,
2

( ) ( )
3 3 3
2 2
3 1
2
sin , sin , 2 tan =
(

+
+ =

s s
PZ PR
a
PR PZ a
0 0
y x
OH
5 5
4 3 2 4


=
+ =
5

5.4. CIKS pentru manip. cu incheietura

sferica, bazata pe decuplare cinematica
Majoritatea robotilor existenti sunt alcatuiti din
brat si incheietura sferica.
Definitie: O structura cinematica cu 6 g.d.l.
are solutii CIKS daca:
3 axe consecutive de articulatii de rotatie se
intersecteaza intr-un punct comun, ca in cazul
incheturii sferice
3 axe de articulatii de rotatie consecutive sunt
paralele, ca in cazul bratului articulat.
Ideea de baza: Se gaseste un punct
convenabil pe structura, a carui pozitie poate
fi exprimata:
atat ca o functie de pozitie si orientarea dorita
(data) ale efectorului terminal;
cat si ca functie de un numar redus de variabile in
articulatii.
Echivalent: se articuleaza problema IK in 2
subprobleme: solutia pentru pozitie este
decuplata de aceea pentru orientare.
Pentru un manipulator cu incheietura sferica o
alegere naturala a punctului W este localizarea
acestuia la intersectia celor 3 axe de rotatie
terminale (Fig. 2).
Fig. 2. Manipulator cu incheietura sferica (yaw, pitch, roll)
Odata cunoscute pozitia si orientarea efectorului
(prin p si [n s a ] (3x3)), pozitia incheieturii
devine:
(2)
Pe de alta parte, este functie doar de
variabilele ce determina pozitia bratului (de obicei
).
In cazul unui brat redundant, cu 3 grade de
mobilitate, IK poate fi rezolvata in urmatorii pasi:
Calculeaza pozitia W (a incheieturii)
conform Ec.(2) - cunoscute;
Rezolva IK pentru
Calculeaza
a d p p
w 6
=
w
p
3 2 1
, , q q q
( )
3 2 1
, , q q q p
w
( )
3 2 1
, , q q q
( )
3 2 1
0
3
, , q q q R
Calculeaza
Rezolva IK pentru orientare
Prin aceasta decuplare cinematica, se poate
rezolva IK a bratului separat ca aceea a
incheieturii sferice.
( ) R R R
T
0
3 6 5 4
3
6
, , =
( )
6 5 4
, ,
5.4.1. CIKS pentru bratul sferic
Fie bratul sferic din Fig.
3.
Se cer variabilele
articulatiilor
si corespunzatoare
unei pozitii date
pentru efectorul
terminal.
2 1
,
3
d
w
p
Fig. 3. Brat sferic: geometrie,
sisteme de coordonate
Parametrii D-H ai bratului sferic:
HTM pentru articulatiile individuale sunt:
( ) ( ) ( )
(
(
(
(

=
(
(
(
(

=
(
(
(
(

=
1 0 0 0
1 0 0
0 0 1 0
0 0 0 1
;
1 0 0 0
0 1 0
0 0
0 0
;
1 0 0 0
0 1 0
0 0
0 0
3
3
2
3
2
2 2
2 2
2
1
2
1
1 1
1 1
1
0
1
d
d A
d
c s
s c
A
d
c s
s c
A
( ) ( ) ( ) ( ) ( )
3
2
3 2
1
2 1
0
1
0
3
1
1
0
1
| d A A A q T A =

premultiplicare cu
( ) | |
1
1
0
1

A
unde:
(3)
Egalam primele trei elemente din coloana 4 in matricile din
ec. (3)
Fie , deci
( )
(
(
(
(

1 0 0 0
0 0
1 0 0
0 0
1 1
1
1 1
1
0
1
c s
d
s c
A
(
(
(
(

=
(
(
(
(

=
1 0 0 0
1 0 0 0
6
6
6
0
0
0
3
z z
y y
x x
w
w
w
w
w
a d p
a d p
a d p
R
p
p
p
R
T
z
y
x
si
2
tan
1

= t
2
1
2
2
1
1
2
,
1
1
t
t
s
t
t
c
+
=
+

=
Substituind in linia 3 din (4):
Cu solutia:
Cele doua solutii corespund la 2 posturi diferite
Conditia de existenta: pozitivitatea discriminantului:
Se divide linia 1 cu linia 2, se egaleaza in ec. matr.
2
2
2
2
1
1
1
2
d
t
t
p
t
t
p
wy wx
=
+

+
+

( ) 0 2
2
2
2
= + + +
wy wx wy
p d t p t p d
, sau
wy
wy wx wx
p d
d p p p
t
+
+ +
=
2
2
2
2 2
2
2
2 2
d p p
wy wx
+
2
2
1
1 1
c
s
d p
s p c p
wz
wy wx

=
+
+
, rezulta:
( )
1 1 1 2
, 2 tan d p s p c p a
wz wy wx
+ =
Ridicand la patrat si insumand primele 2 linii in (4), duce la:
unde s-a considerat doar solutia cu .
( ) ( )
2
1
2
1 1 3 wz wy wx
p d s p c p d + + =
0
3
> d
5.4.2. CIKS pentru incheietura sferica
Fie o incheietura sferica. Se doreste gasirea variabilelor
articulatiilor corespunzand unei orientari date a
efectorului .
Aceste unghiuri constituie un set de unghiuri Euler fata
de SC de referinta 3. (w)
se calculeaza astfel:
6 5 4
, ,
3
6
R
3
6
R
Este posibil sa se calculeze solutiile ce corespund unghiurilor
Euler/sistem III: RPY, pentru alocarea SC ale incheieturii,
conectat la brat ca in fig. 4:
( )
6 5 4
3 3 3
3 3 3
3 3 3
3
6
, , f
a s n
a s n
a s n
R
z z z
y y y
x x x
=
(
(
(

=
Fig. 4. Alocarea SC pentru incheietura, si conectarea cu bratul
Se observa ca:
Secventa de rotatii va fi:
roll pitch yaw = = =
6 5 4
, ,
4 3 5 3 5 3
; ; : , 0
5 4 3 z y z x x z
O O O O O O si O O O
(
(
(

+
+
=
6 5 6 5 5
6 4 6 5 4 6 4 6 5 4 5 4
6 4 6 5 4 6 4 6 5 4 5 4
3
6
c c s c s
s c c s s c c s s s c s
s s c s c c s s s c c c
R
Solutiile sunt:
Solutia degenereaza cand
atunci nu se pot calcula decat
( )
( ) ( )
( )
( )
( ) ( )
( )
3 3
6
5
2
3
2
3 3
5
3 3
4
3 3
6
5
2
3
2
3 3
5
3 3
4
, 2 atan
2
3
,
2
pt , , 2 atan
, 2 atan
, 2 atan
2
,
2
pt , , 2 atan
, 2 atan
z z
z z z
x y
z z
z z z
x y
a s
a s n
n n
si
a s
a s n
n n
=
|

\
|

\
|
+ =
=
=
|

\
|

|

\
|
+ =
=

\
|
= =
2
3
sau
2
0
5 5

c
6 4

5.5. CIKS pentru robotul ABB IRB 1400
Calculul pozitiei incheieturii (w):
Egalam coloana 4 in ecuatia:
unde
si si date de matricile D-H
asociate
(
(
(

=
(
(
(

z z
y y
x x
wz
wy
wx
a d p
a d p
a d p
p
p
p
6
6
6
( )
2
3
1
2
0
3
1
0
1
A A T A =

( ) ( )
(
(
(
(

= =

1 0 0 0
0 0
1 0 0
0
si , 1 det
1 1
1
1 1 1
1
0
1
0
1
c s
d
a s c
A A
(
(
(
(

= =
1 0 0 0
0
0 0
3
wz
wy
wx
w
w
p
p
p
R
T T
2
3
1
2
0
1
, , A A A
5.6 Proprietati ale solutiilor IK. Unicitate.
5.6.1. Configuratii ale bratului robot
Sunt definite un numar de configuratii ale bratului,
asemanator geometriei bratului uman, si corespunzator
formalismului D-H de stabilire a SC ale segmentelor (vezi Fig.
5).
Fig. 5. Cele 4 configuratii ale bratului antropomorfic,
compatibile cu o pozitie a incheieturii data
a) Brat dreapta, in sus
(Right and Above arm); (RA)
b) Brat dreapta, in jos
(Right and Below arm); (RB)
c) Brat stanga, in sus
(Left and Above arm); (LA)
d) Brat stanga, in jos
(Left and Below arm); (LB)
Semnificatie:
RIGHT (shoulder) ARM: pozitiv (in jurul )
misca incheietura in directia pozitivam
articulatia 3 nefiind activata.
LEFT (shoulder) ARM: pozitiv (in jurul )
misca incheietura in directia negativa,
articulatia 3 nefiind activata
ABOVE ARM (elbow above wrist): Pozitia cotului
la brat {RIGHT/LEFT} fata de SC umar are
coordonata {negativa/pozitiva} de-a lungul axei .
BELOW ARM (elbow below wrist): Pozitia cotului
la brat {RIGHT/LEFT} fata de SC umar are
coordonata {pozitiva/negativa} de-a lungul axei .
2

1
z
0
z
2

1
z
0
z
2
y
2
y
Observatii:
1) Pentru un brat robot articulat vertical, data fiind
pozitia incheieturii, exista 4 configuratii posibile
ale bratului (16 solutii IK)
2) In programul de calcul on-line al (CIKS
a bratului), se specifica configuratia dorita,
evitandu-se astfel multiplicitatea solutiilor.
3 2 1
, , q q q
5.6.2. Existenta solutiilor IK
Conditia de existenta a solutiilor IK:
a) Daca pozitia dorita a varfului sculei, , se afla in
afara anvelopei spatiului de lucru, nu poate exista
solutie IK.
b) Chiar daca , pot exista anumite
orientari R ale sculei care nu sunt realizabile fara
violarea uneia sau mai multor limite de variabile
articulatii.
c) Daca robotul are mai putin decat 3 g.d.l. pentru
orientarea sculei, atunci seturi intregi de orientari nu
sunt realizabile.
p
( ) lucru Sp.de p
Sa consideram formularea HTM pentru ecuatia bratului
(DKI):
( )
| |
3 , , 1 , ;
1 0 0 0
K =
(
(
(
(

=
= j i
p
p
p
r R
q T
z
y
x
ij
Base
Tool
(f
0
)
(f
0
) )=Ecuatia bratului constituie, aparent, un sistem de
12 ecuatii algebrice neliniare simultane in n necunoscute:
.
( ) q n dim =
Pentru unei solutii generale a problemei IK,
pentru care poate fi gasit un vector care
genereaza o configuratie a sculei arbitrara, numarul
necunoscutelor trebuie sa fie cel putin egal
cu numarul restrictiilor:
(f.1)
Limita inferioara reprezinta o conditie
necesara, dar nu suficienta pentru existenta unei
solutii la problema IK, cand se impune o configuratie
a sculei arbitrara;
Evident, trebuie sa fie satisfacute conditiile a, b, c,
de existenta, ca restrictii aditionale asupra valorilor p
si R;
Chiar atunci cand a, b, c sunt indeplinite nu exista
garantia ca poate fi obtinute o CIKS la problema IK.
( )
q
n
q q , ,
1
K
6 n
6 , = n n
Metoda generala de rezolvare a problemei IK
in cazul robotilor cu incheietura sferica se
simplifica.
Fie un robot cu n g.d.l.:
df
Ultima axa este o axa de roll scula;
Robotul are o incheietura sferica (cele n-3 axe de la
capatul bratului sunt concurente).
Problema IK se descopune in 2 subprobleme,
prin decuplarea problemei IK originale la
incheietura.
6 4 n
5.6.3. Unicitatea solutiilor IK
Cand exista IKSs, tipic ele nu sunt unice
Solutii multiple apar in mai multe situatii:
Pentru robotii cu axe (Roboti cinematic redundanti
au mai multe g.d.l. decat sunt necesare pentru a
stabili configuratii scula arbitrare). Aceste g.d.l.
suplimentare adauga flexibilitate manipulatorului.
Exemplu:
Un RI redundant poate fi utilizat pentru accesul la
obiecte dispuse dupa obstacole si pentru manipularea
lor. Vezi structura SCARA din Fig. 6, vazuta de sus:
Fig. 6. Accesul in jurul unul obstacol cu robot SCARA redundant
Mai multe solutii distincte pot apare atunci cand marimea anvelopei
spatiului articulatiilor este suficient de larga (chiar daca robotul nu este
cinematic redundant) (vezi Fig. 7).
Fig. 7 Solutii multiple cu un
robot neredundant
Obs:
Tipic se prefera solutia Elbow-Up (reduce riscul
de coliziune intre segmentele robotului si
obstacole dispuse pe suprafata de lucru). Se
impune prin z
0
indicatori binari ai bratului.
Limitele de deplasare pentru articulatiile: umarului,
cotului, flexarii mainii, sunt suficient de largi.
5.7. Configuratia uneltei (Tool
Configuration)
Pentru a calcula solutia problemei IK,
configuratia dorita a uneltei trebuie
specificata ca data de intrare.
Pana in prezent, configuratia uneltei a fost
reprezentata prin perechea , unde:
: pozitia uneltei relativ la baza
: orientarea uneltei relativa la baza (2/3 din
informatia din R este redundanta)
{ } R p,
p
R
Vectorul configuratiei uneltei (TCV)
Fie informatia de orientare a uneltei data de vectorul
de apropiere
Vectorul :
specifica atat unghiul yaw/unealta cat si unghiul
pitch/unealta
nu specifica unghiul roll/unealta (unghiul roll reprezinta
rotatia in jurul ).
Ideea: deoarece vectorul specifica orientarea
uneltei mai putin unghiul roll/unealta, se extinde
informatia continuta in vectorul de apropiere cu
informatia referitoare la unghiul roll.
( ) a r R col a =
3
, 3
a
a
a
a