Curs3.

1.5. CALCULUL DE CINEMATICĂ INVERSĂ

La calculul de cinematică inversă se consideră că se cunoaşte unde anume trebuie

adus efectorul final într-o etapă a procesului tehnologic, adică se cunosc valorile
numerice pentru poziţia (px, py, pz) şi orientarea (nx, ny, nz, ox, oy, oz, ax, ay, az)
piesei în poziţia ţintă şi trebuie determinat cu cât trebuie comandat fiecare motor (valorile
di şi θi corespunzătoare fiecărei cuple cinematice conducătoare) pentru ca în final să se
realizeze o mișcare până în situarea ȚINTĂ. Adică trebuie determinată câte o valoare
concretă pentru coordonatele CCC (q1, q2, q3, q4, q5, q6 în cazul unui robot cu 6 grade
de mobilitate).

Fig.2.3. Definirea calculelor de cinematică inversă la robotul TRTRRR

Să considerăm de exemplu că în cazul robotului TRTRRR, (la care q1= d1, q2 = θ2, q3 =
d3, q4 = θ4, q5 = θ5 și q6 = θ6) se cunosc valorile numerice pentru coordonatele
carteziene ale situării țintă, adică conform fig.2.3. avem:

Deci avem:

nx=0.473; ox=0.181; ax=0.862; px=553.620;

ny= -0.826; oy=0.431; ay=0.362; py=478.620;
nz= -0.306; oz= -0.884; az=0.354; pz=803.033;
2 Curs3.
Adică invers: se cunosc valorile numerice pentru elementele matricii de situare T și se
cere determinarea coordonatelor CCC aferente situării țintă (q1, q2, q3, q4, q5 și q6),
adică calculul lui d1, θ2, d3, θ4, θ5 și θ6 în cazul robotului TRTRRR.

Pentru efectuarea acestui calcul, întâi se deduc relațiile de calcul ale coordonatelor CCC.
Relaţiile de calcul ale coordonatelor CCC se determină în două etape:
1. în prima etapă se determină relaţiile de calcul corespunzătoare coordonatelor CCC
ale mecanismului generator de traiectorie (q1, q2, q3);
2. în a doua etapă se determină relaţiile de calcul corespunzătoare coordonatelor
CCC ale mecanismului de orientare (q4, q5, q6), în funcţie de valorile obţinute în
prima etapă

1.5.1. Determinarea relaţiilor de calcul ale coordonatelor q1, q2 şi q3

În cazul mecanismelor de orientare de tip sferic, axele cuplelor cinematice

conducătoare sunt concurente şi se intersectează în punctul carpian W (Wrist) numit
și ”punct al încheieturii”. Întâi se determină coordonatele punctului W în sistemul de
coordonate X0Y0Z0, legat de baza robotului. În acest scop se determină componentele
PWX, PWY și PWZ ale vectorului de poziţie PW al punctului carpian W, fig.3.1.

Fig.3.1.Punctul Wrist (W) se găsește la intersecția axelor de rotație q4, q5 şi q6

din cadrul mecanismului de orientare

PWX, PWY și PWZ sunt de fapt coordonatele punctului Wrist măsurate în sistemul de
coordonate X0Y0Z0. Pentru determinarea lor pornim de la ecuația vectorială:

P = PW + PM
rezultă:
PW = P − PM
Curs3. 3

Fig.3.2.Reprezentarea vectorilor PW , P și PM
Notând, conform figurii 3.2., cu pMX, pMY și pMZ proiecțiile vectorului de poziție PM pe
axele Xw, Yw și Zw ale unui sistem de coordonate cu originea în W (și cu axele paralele
cu X0, Y0 și Z0) rezultă:
 PWx   p x   pMx 
P  =  p  −  p 
 Wy   y   My 
 PWz   p z   pMz 
și
PWX = pX - pMX
PWY = pY - pMY
PWZ = pZ - pMZ

Componentele pMX, pMY şi pMZ se determină cu ajutorul versorului de apropiere a şi a

modulului vectorului PM ( PM = d6 pentru fecare robot studiat).

Fig.3.3.
Notând conform figurii 3.3., cu ϕ unghiul dintre vetorul PM și axa Zw (numit și unghi polar)
și cu φ unghiul dintre proiecția vectorului PM pe planul XwWYw și axa Xw (numit și unghi
azimutal) rezultă:
pMX = PM ⋅sinϕcosφ = d6sinϕcosφ
pMY = PM ⋅sinϕsinφ = d6sinϕsinφ
pMZ = PM ⋅cosϕ = d6cosϕ
4 Curs3.
Deci componentele PWX, PWY și PWZ ale vectorului de poziţie PW se pot determina cu
relațiile generale (valabile pt. fiecare robot studiat la curs, laborator și proiect):

PWX = pX - d6sinϕcosφ
PWY = pY - d6sinϕsinφ (3.1.)
PWZ = pZ - d6cosϕ

Unghiul azimutal φ respectiv unghiul polar ϕ se determină din proiecţiile aX, aY şi aZ

(ale versorului a ) pe axele sistemului de coordonate X0Y0Z0 conform tab3.1.a și b,
considerând un domeniu de variaţie [-π, π) pentru φ şi [0, π] pentru ϕ.

ax ay φ az ϕ
ay ax2 + a y2
>0 [-1,1] arctg >0
ax arctg
az
π π
0 >0 2 0 2
π ax2 + a y2
0 <0 − <0
2 π+arctg
az
ay
<0 >0 π+arctg
ax
<0 0 -π
a. b.
Tab.3.1.
Procedura de calcul:
1. Calculul unghiului azimutal φ, pe baza tabelului 3.1.a.
2. Calculul unghiului polar ϕ, pe baza tabelului 3.1.b.
3. Calculul coordonatelor punctului wrist, conform relației (3.1.):
4. Calculul coordonatelor q1, q2, q3 (de exemplu d1, θ2, d3 pentru robotul TRTRRR)

0
Fig.3.4.Exprimarea vectorului PW cu ajutorul matricii T3 și a vectorului rO3W
Curs3. 5
0
Notând cu rO3W vectorul de poziţie al punctului W (Wrist) în raport cu O3 și cu T3 matricea
transformării omogene a mecanismului generator de traiectorie, conform fig.3.4. rezultă:

PW =0T3 ⋅ rO3W

unde: I rO3W I = d4 = este o distanță constantă, la fiecare robot studiat.

Luând în considerare faptul că vectorul rO3W (în sistemul de coordonate X3Y3Z3) are
componentă numai de-a lungul axei Z3, rezultă:

 PWx  0
 PWy  0
0
 P = T3 ⋅ d  (3.2.)
 Wz   4
 1  1
Considerând deja cunoscute valorile concrete ale componetelor PWX, PWY și PWZ, din
ecuaţia matriceală (3.2.) se obţine un sistem de ecuaţii, din care rezultă expresiile de
calcul ale coordonatelor q1, q2 şi q3 aferente mecanismului generator de traiectorie.

1.5.1.1. Deducerea relațiilor de calcul q1, q2 şi q3 pt. robotul TRTRRR

Considerând cunoscute poziţia punctului caracteristic (px, py, pz), orientarea obiectului
manipulat (nx, ny, nz, ox, oy, oz, ax, ay, az) faţă de sistemul de coordonate fix legat de ba-
za robotului precum şi dimensiunile fizice ale dispozitivului de ghidare (a1, d2, a3, d4 şi
d6) se urmăreşte determinarea relațiilor de calcul pentru: d1, θ2 și d3.
Se parcurg următoarele etape:
1. Calculul unghiului azimutal φ pe baza tabelului 3.1.a.
2. Calculul unghiului polar ϕ pe baza tabelului 3.1.b.
3. Calculul componentelor vectorului de poziţie PW al punctului carpian W (Wrist) pe ba-
za relaţiei (3.1.):
PWX = pX - d6sinϕcosφ
PWY = pY - d6sinϕsinφ (3.6.)
PWZ = pZ - d6cosϕ
4. Calculul coordonatelor d1, θ2, d3 corespunzătoare mecanismului generator de tra-
iectorie.

Se consideră deja cunoscute componentele vectorului de poziţie PW . Înlocuind în

0
relația (3.2) matricea de transformare omogenă T3 pentru robotul TRTRRR rezultă:

 PWx   0  C2 0 − S2 − S 2 (a3 + d 3 )  0  − S 2 (a3 + d 3 + d 4 )

P     C2 (a3 + d 3 )   0   C2 (a3 + d 3 + d 4 ) 
 Wy  = 0T3 ⋅  0  =  S 2 0 C2
⋅ =
 PWz  d 4   0 −1 0 a1 + d 2 + d1  d 4   a1 + d 2 + d1 
         
 1  1 0 0 0 1  1  1 
6 Curs3.
de unde se obține următorul sistem de ecuaţii, având necunoscutele d1, θ2, d3:

 P = − S2 ( a3 + d3 + d4 )
 Wx
 PWy = C2 ( a3 + d3 + d4 )
 PWz = a1 + d2 + d1

4.a. Din a treia ecuaţie rezultă direct d1:

d1 = PWz − ( a1 + d2 ) (3.7.)

4.b. Din primele două ecuaţii rezultă:

S2 − PWx
tgθ 2 = =
C2 PWy

 − PWx 
θ 2 = arctg 
 PWy  (3.8.)

• Soluţia de mai sus va fi extinsă pentru domeniul θ2∈[-π, π) cu ajutorul funcției

arctangenta2(p1,p2), unde p1 este numărătorul iar p2 este numitorul fracției.

4.c. Pentru determinarea lui d3 se elimină din primele două ecuaţii pe θ2:
2
2 2
2
PWx PWy
1 = sin θ 2 + cos θ 2 = +
( a3 + d3 + d4 ) 2
( a3 + d3 + d4 ) 2
( a3 + d3 + d4 ) 2 = PWx2 + PWy2
d3 = −( a3 + d4 ) ± PWx
2 2
+ PWy

Având în vedere faptul că d3 reprezintă o distanţă, ea trebuie să fie pozitivă. În acest

sens, în relaţia de mai sus, se ia în considerare numai soluţia cu semnul +, adică re-
zultă:

d3 = −( a3 + d4 ) + PWx
2 2
+ PWy (3.9.)