Curs4.

1.5.2. Determinarea relaţiilor de calcul ale coordonatelor q4, q5 şi q6

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 baza robotului,
• dimensiunile fizice ale dispozitivului de ghidare (a1, d2, a3, d4 şi d6);
• valorile calculate pentru q1, q2 şi q3 (cu ajutorul relațiilor de calcul deduse în
paragraful 1.5.1 )
se urmăreşte determinarea relațiilor de calcul pentru coordonatele: q4, q5 şi q6 (care
sunt notate de obicei cu θ4, θ5 şi θ6).

Se porneşte de la matricea de situare T care indică atât poziţia cât şi orientarea piesei în
spaţiu, exprimată faţă de sistemul de coordonate fix legat de baza robotului. Dacă se
consideră din cadrul acestei matrici doar primele 3 linii şi primele 3 coloane, se obţine sub-
matricea R care exprimă doar orientarea piesei în spaţiu, în raport cu sistemul de
coordonate fix:
n x o x a x p x 
  n x o x a x 
n o a p  ⇒ R = n y o y a y

T =
y y y y
n z o z a z p z   
   n z o z a z 
0 0 0 1 
Se poate scrie:
R = 0R3 ⋅3 R6 (3.3.)
0
unde primul termen este submatricea de orientare a matricii T3 iar al doilea termen este
3 3
submatricea de orientare a matricii T6 . Submatricea de orientare R6 arată orientarea pie-
sei (având punctul caracteristic M) faţă de originea mecanismului de orientare, adică faţă
de O3.
n Mx o Mx a Mx 
3
R6 = n My o My a My 
 
 n Mz o Mz a Mz 
Elementele acestei matrici sunt proiecţiile versorilor n , o şi a pe axele de coordonate
O3X3Y3Z3 şi se pot determina din relaţia:
3
( )
R6 = 0 R3
−1
⋅R
care s-a obţinut prin înmulţirea la stânga a ecuaţiei (3.3.) cu inversa submatricii de orienta-
re a mecanismului generator de traiectorie.
Astfel rezultă următoarea relaţie:

n Mx o Mx a Mx   nx ox ax 
R6 = n My o My a My  = ( 0 R3 ) ⋅ ny oy a y 
3 −1
(3.4.)
   
 n Mz o Mz a Mz   nz oz az 
2 Curs4.
Efectuînd înmulţirea şi egalând cele două matrici termen cu termen rezultă relaţiile de
calcul pentru nMx, nMy, nMz, oMx etc.
3
Pe de altă parte relaţiile de calcul pentru elementele matricii R6 rezultă şi din relația:
3
R6 =3R4 ⋅4R6
unde:
3
R4 este submatricea de orientare a matricii de transformare omogenă 3 A4
4
R6 este submatricea de orientare a matricii de transformare omogenă 4T6 obţinut prin
4 5
înmulţirea matricilor A5 şi A6 .

Din relaţia de mai sus rezultă:

( 3R4 ) −1⋅3R6 =4R6

adică:
n Mx o Mx a Mx 
( 3R4 ) −1   4
⋅ n My o My a My = R6 (3.5.)
 
 n Mz o Mz a Mz 
Considerând cunoscute elementele nMx, nMy, nMz, oMx, oMy, oMz, aMx, aMy, aMz din
ecuaţia matricială de mai sus rezultă un sistem cu 9 ecuaţii şi 3 necunoscute. Necunoscu-
tele sunt chiar coordonatele θ4, θ5 şi θ6 sub formă de sinus şi cosinus.

1.5.2.1. Deducerea relațiilor de calcul q4, q5 şi q6 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) și valorile calculate pentru pentru: d1, θ2 și d3, se urmăreşte determinarea relațiilor
de calcul: θ4, θ5 şi θ6.

Se parcurg următoarele etape; (continuarea etapelor din paragraful 1.5.2.1):

3
5. Calculul elementelor matricii de orientare R6 pe baza relaţiei (3.4.):

Întâi se determină submatricea de orientare 0 R3 a matricii 0T3 luând numai primele 3

linii și primele 3 coloane, apoi se calculează matricea inversă (R)
0
3
−1
. Rezultă:

C2 0 − S2 − S 2 (a3 + d 3 )
S C2 0 − S2   C2 S2 0 
0 C2 C2 (a3 + d 3 )  0
0
T3 =  2
0 −1 0 a1 + d 2 + d1 
⇒ R3 =  S 2 0 ( )
C2  ⇒ 0 R3
−1
=  0 0 − 1
   0 − 1 0  − S 2 C 2 0 
0 0 0 1 
Curs4. 3
Înlocuind în relația (3.4.):

nMx oMx aMx   C 2 S 2 0   nx ox ax 

3
R6 = nMy oMy aMy  =  0 0 − 1 ⋅ n y oy a y 
 nMz oMz aMz  − S 2 C 2 0   nz oz az 
rezultă:
nMx = nxC2+nyS2; oMx = oxC2+oyS2; aMx = axC2+ayS2;
nMy = -nz; oMy = -oz; aMy = -az; (3.10)
nMz = -nxS2+nyC2; oMz = -oxS2+oyC2; aMz = -axS2+ayC2;
6. Calculul coordonatelor θ4, θ5 şi θ6 corespunzătoare mecanismului de orientare.

Se consideră deja cunoscute coordonatele: d1, θ2, d3 respectiv elementele matricii

3
de orientare R6 (adică nMx, nMy, nMz, oMx, oMy, oMz, aMx, aMy, aMz). Întâi se
determină submatricea de orientare 3 R4 a matricii 3 A4 luând numai primele 3 linii și
primele 3 coloane, apoi se calculează matricea inversă (R)
3
4
−1
. Rezultă:

 S4 0 C4 0
− C  S4 0 C4   S4 − C4 0
0 S4 0  3
3
A4 =  4
 0 −1 0 d4 
⇒ R4 = − C4 ( )
0 S 4  ⇒ 3 R4
−1
=  0 0 − 1
   0 − 1 0  C4 S4 0 
 0 0 0 1

4
Apoi se determină submatricea de orientare R6 a matricii de transformare omogenă
4
T6 prin luând numai primele 3 linii și primele 3 coloane. Se calculează:

C5 0 S5 0 C6 − S6 0 0  C5C6 − C5 S6 S5 S5 d 6 

S 0 − C5 0  S6 C6 0 0   S5C6 − S5 S 6 − C5 − C5 d 6 
4
T6 = 4A5 ⋅5 A6 =  5 ⋅ =
0 1 0 0  0 0 1 d 6   S6 C6 0 0 
     
0 0 0 1  0 0 0 1  0 0 0 1 
și se obține:
C5C6 − C5 S 6 S5 
4
R6 =  S 5C6 − S5 S6 − C5 
 S 6 C6 0 

Aplicând apoi relaţia (3.5.):

 S4 − C4 0  nMx oMx a Mx  C5C6 − C5 S 6 S5 
0 0 − 1 ⋅ nMy oMy a My  =  S 5C6 − S5 S6 − C5 

C 4 S4 0   nMz oMz a Mz   S 6 C6 0 
4 Curs4.
se obţine, din egalarea termen cu termen a celor două matrici rezultate, un sistem de
9 ecuaţii, având necunoscutele θ4, θ5 şi θ6. Din cele 9 ecuaţii vom alege pentru de-
terminarea parametrilor cinematici θ4, θ5 şi θ6 următoarele ecuaţii:
C4a Mx + S4a My = 0
S4a Mx − C4a My = S5
−a Mz = −C5
C4n Mx + S4n My = S6
C4o Mx + S4o My = C6
6.a. Din prima ecuaţie rezultă:
S4 −a Mx
tgθ 4 = =
C4 a My
 −a 
θ 4 = arctg Mx  (3.11.)
 a My 
• Soluţia de mai sus va fi extinsă pentru domeniul θ4∈[-π/2, +π/2] cu ajutorul funcției
arctangenta1(p1,p2), unde p1 este numărătorul iar p2 este numitorul fracției.
6.b. Din ecuaţia a doua şi a treia rezultă:
S5 S4a Mx − C4a My
tgθ 5 = =
C5 a Mz
 S4a Mx − C4a My 
θ5 = arctg  (3.12.)
 a Mz 
• Soluţia de mai sus va fi extinsă pentru domeniul θ5∈[-π/2, +π/2] cu ajutorul funcției
arctangenta1(p1,p2), unde p1 este numărătorul iar p2 este numitorul fracției.
6.c. Din ultimele două ecuaţii rezultă:
S6 C4n Mx + S4n My
tgθ 6 = =
C6 C4o Mx + S4o My
 C4n Mx + S4n My 
θ 6 = arctg  (3.13.)
 C4o Mx + S4o My 
• Soluţia de mai sus va fi extinsă pentru domeniul θ6∈[-π, +π) cu ajutorul funcției
arctangenta2(p1,p2), unde p1 este numărătorul iar p2 este numitorul fracției.

1.5.3. Exemplu de calcul de cinematică inversă în cazul robotului TRTRRR

Să considerăm că în cazul robotului TRTRRR, se cunosc valorile numerice pentru

coordonatele carteziene ale situării țintă, de exemplu:
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;
Curs4. 5
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.
Considerând pentru dimensiunile fizice constante ale elementelor dispozitivului de ghidare
următoarele valori:
a1=450, d2= 100, a3= 230, d4=70 și d6=150.
în MATLAB se realizează următoarele calcule:
1. Calculul unghiului azimutal φ pe baza tabelului 3.1.a.
if (ax>0) azimutal=atan(ay/ax);end
if (ax<0) & (ay<0) azimutal=-pi+atan(ay/ax);end
if (ax<0) & (ay==0) azimutal=-pi;end
if (ax<0) & (ay>0) azimutal=pi+atan(ay/ax);end
if (ax==0) & (ay>0) azimutal=pi/2;end
if (ax==0) & (ay<0) azimutal=-pi/2;end
2. Calculul unghiului polar ϕ pe baza tabelului 3.1.b.
if az>0 polar=atan(sqrt(ax*ax+ay*ay)/az);end
if az==0 polar=pi/2;end
if az<0 polar=pi+atan(sqrt(ax*ax+ay*ay)/az);end
3. Calculul coordonatelor punctului wrist, pe baza relațieilor (3.1.) și (3.6.)
Pwx=px-d6*sin(polar)*cos(azimutal);
Pwy=py-d6*sin(polar)*sin(azimutal);
Pwz=pz-d6*cos(polar);
4. Calculul coordonatelor q1, q2, q3 (adică d1, θ2, d3 pentru robotul TRTRRR), pe
baza relațiilor (3.7.), (3.8.) și (3.9.).
d1=Pwz-(a1+d2);
teta2=arctangenta2(-Pwx,Pwy);
d3=sqrt((-Pwx)^2+(Pwy)^2)-(a3+d4);
3
5. Calculul elementelor matricii de orientare R6 pe baza relației (3.10.)
nmx=nx*cos(teta2)+ny*sin(teta2);
nmy=-nz;
nmz=-nx*sin(teta2)+ny*cos(teta2);
omx=ox*cos(teta2)+oy*sin(teta2);
omy=-oz;
omz=-ox*sin(teta2)+oy*cos(teta2);
amx=ax*cos(teta2)+ay*sin(teta2);
amy=-az;
amz=-ax*sin(teta2)+ay*cos(teta2);
6. Calculul coordonatelor q4, q5 şi q6 (adică θ4, θ5 şi θ6 pentru robotul TRTRRR), pe
baza relațiilor (3.11.), (3.12.) și (3.13.).

teta4=arctangenta1(-amx,amy);
teta5=arctangenta1(sin(teta4)*amx-cos(teta4)*amy,amz);
teta6=arctangenta2(nmx*cos(teta4)+nmy*sin(teta4),omx*
cos(teta4)+omy*sin(teta4));
6 Curs4.
7. Transformarea mărimilor unghiulare obținute, din radiani în grade

teta2=teta2*180/pi;
teta4=teta4*180/pi;
teta5=teta5*180/pi;
teta6=teta6*180/pi;

Efectuând calculele conform punctelor 1-7 prezentate mai sus, pentru coordonatele CCC
aferente situării, adică pentru d1, θ2, d3, θ4, θ5 și θ6 de mai sus se obțin următoarele
valori: d1=200, teta2=-45, d3=300, teta4=45, teta5=30 și teta6=60.

Observații:
1. În paragraful 1.4.2. s-a efectuat un exemplu de calcul de cinematică directă în cazul
robotului TRTRRR (având dimensiunile fizice constante ale elementelor dispozitivului
de ghidare următoarele valori:a1=450, d2= 100, a3= 230, d4=70 și d6=150.).S-a
plecat de la un set de coordonate CCC (d1, θ2, d3, θ4, θ5 și θ6), stabilit arbitrar:

d1=200, teta2=-45, d3=300, teta4=45, teta5=30 și teta6=60

și s-au obținut coordonatele carteziene ale situării aferente, adică:

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. În paragraful 1.5.3. s-a efectuat un exemplu de calcul de cinematică inversă în cazul

robotului TRTRRR (având de asemenea dimensiunile fizice constante ale elementelor
dispozitivului de ghidare următoarele valori: a1=450, d2= 100, a3= 230, d4=70 și
d6=150.).S-a plecat de la coordonatele carteziene ale situării obținute la punctul 1:

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;

și s-au reobținut coordonatele CCC inițiale:

d1=200, teta2=-45, d3=300, teta4=45, teta5=30 și teta6=60.

3. Pentru implementarea în MATLAB a funcțiilor arctangenta1 respectiv arctangenta2

se recomandă următoarele secvențe de program, salvate în fișiere separate.

function arctg=arctangenta1(p1,p2)
if p2~=0 arctg=atan(p1/p2);
else
if abs(p1)<0.01 arctg=0;
else
if p1>0 arctg=pi/2;end
if p1<0 arctg=-pi/2;end
end
end
Curs4. 7

function arctg=arctangenta2(p1,p2)
if p2~=0
if p2>0 arctg=atan(p1/p2);end
if (p2<0)&(p1==0) arctg=-pi;end
if (p2<0)&(p1>0) arctg=pi+atan(p1/p2);end
if (p2<0)&(p1<0) arctg=-pi+atan(p1/p2); end
else
if abs(p1)<0.01 arctg=0;
else
if p1>0 arctg=pi/2;end
if p1<0 arctg=-pi/2;end
end
end
end