ROBOTI INDUSTRIALI

TEMA

Pentru robotul PUMA 600, ale carui caracteristici inertiale si parametrii Hartemberg - Denavit se cunosc, se vor determina urmatoarele:

Coordonatele articulare pentru doua puncte apartinand spatiului de lucru al robotului definite in coordonatele lor externe(operationale).

Legile de miscare pentru primele trei grade de libertate intre cele doua puncte definite in "1" in coordonate articulare (vitezele initiala si finala sunt nule), interpolate cu functii de gradul intai.

Vitezele si acceleratiile (unghiulare si ale centrelor de masa) gradelor de libertate "1"si "3" pentru deplasarea pe traiectoria definita la "1", conform legilor de miscare determinate in "4".

Planificarea traiectoriei intre cele doua puncte definite prin "1" (vitezele initiala si finala sunt nule) in sistemul 4-3-4, pentru gradul de libertate "3".

1. Coordonatele articulare pentru doua puncte

apartinand spatiului de lucru al robotului definite in coordonatele lor externe (operationale)

Problema cinematica inversa permite calculul coordonatelor articulare care aduc endefectorul in pozitia si orientarea dorita date fiind coordonatelor operationale.

Coordonatele externe (operationale) pentru doua puncte din spatiul de lucru al robotului sunt :

Punct initial: x = 0,27 m; y = 0,32 m; z = 0,60 m.

Punct final: x = 0,59 m; y = 0,47 m; z = 0,22 m.

Rezolvand problema cinematica inversa cu ajutorul urmatorului program in care introducem coordonatele externe pentru punctul initial si pentru punctul final , vom obtine urmatoarele valori ale coordonatelor interne pentru puctul initial, respectiv pentru punctul final.

a=;

b=;

al=;

p=

f1[t3]=a[[4]] Cos[t3]+b[[4]] Sin[t3] Sin[al[[4]]]+a[[3]];

f2[t3]=Cos[al[[3]]] (Sin[t3] a[[4]]-

Cos[t3] Sin[al[[4]]] b[[4]])-Sin[al[[3]]] (Cos[al[[4]]] b[[4]]+b[[3]]);

f3[t3]=Sin[al[[3]]] (Sin[t3] a[[4]]-Cos[t3] Sin[al[[4]]] b[[4]])+

Cos[al[[3]]] (Cos[al[[4]]] b[[4]]+b[[3]]);

F1[t2_,t3_]=Cos[t2] f1[t3]-Sin[t2] f2[t3];

F2[t2_,t3_]=Sin[t2] f1[t3]+Cos[t2] f2[t3];

F3[t2_,t3_]=f3[t3]+b[[2]];

g1[t2_,t3_]=F1[t2,t3]+a[[2]];

g2[t2_,t3_]=Cos[al[[2]]] F2[t2,t3]-Sin[al[[2]]] F3[t2,t3];

g3[t2_,t3_]=Sin[al[[2]]] F2[t2,t3]+Cos[al[[2]]] F3[t2,t3];

P1[t1_,t2_,t3_]=Cos[t1] g1[t2,t3]-Sin[t1] g2[t2,t3];

P2[t1_,t2_,t3_]=Sin[t1] g1[t2,t3]+Cos[t1] g2[t2,t3];

P3[t1_,t2_,t3_]=g3[t2,t3]+b[[1]];

x=FindRoot[,,,]

Print[x];

Valorile coordonatelor interne vor fi:

Punct initial: q₁¹ = 0,52 m; q₂¹ = -1,34 m; q₃¹ = - 0,61 m.

Punct final: q₁² = 0,58 m; q₂² = - 1,41 m; q₃² = - 0,16 m.

Legile de miscare pentru primele trei grade de libertate

intre cele doua puncte definite in "1" in coordonate articulare

(vitezele initiala si finala sunt nule), interpolate cu functii de gradul intai

Vectorii pozitiei initiale (in coordonate articulare) sunt:

q₁¹ = 0,52 m; q₂¹ = -1,34 m; q₃¹ = - 0,61 m.

Vectorii pozitiei initiale (in coordonate articulare) sunt:

q₁² = 0,58 m; q₂² = - 1,41 m; q₃² = - 0,16 m.

Vom obtine legea de miscare in coordonate articulare prin interpolarea unei functii de gradul III.

Cerinte:

q₁(t) = a₁₀t³ + a₁₁t²+ a₁₂t+ a₁₃

q₂(t) = a₂₀t³ + a₂₁t²+ a₂₂t+ a₂₃

q₃(t) = a₃₀t³ + a₃₁t²+ a₃₂t+ a₃₃

Pentru determinarea acestor legi de miscare trebuie sa aflam pentru fiecare functie cei patru coeficienti. Pentru aceasta avem nevoie de un sistem de patru ecuatii pe care il obtinem din urmatoarele conditii:

Valoarea functiilor in punctul initial t = 0 trebuie sa coincida cu coordonatele articulare ale punctului initial (determinate la punctul 1).

Valoarea functiilor in punctul final t = 1 trebuie sa coincida cu coordonatele articulare ale punctului final (determinate la punctul 1).

Viteza initiala sa fie zero.

Viteza finala sa fie zero.

Deci:.

v    pentru prima lege de miscare avem:

q₁(t) = a₁₀t³ + a₁₁t²+ a₁₂t+ a₁₃

Conditie: q₁(0) = q₁¹ a₁₃ = 0,52

q₁(1) = q₁² a₁₀ + a₁₁+ a₁₂+ a₁₃ = 0,58

q₁(0) = 0 a₁₂ = 0

q₁(1) = 0 3a₁₀ +2a₁₁ + a₁₂ = 0

Pentru rezlvarea sistemului vom utiliza programul "Mathematica":

Solve[,]

Vom obtine urmatori coeficienti: a₁₀ = -0,12; a₁₁= 0,18; a₁₂= 0; a₁₃ = 0,52

Rezulta ca:

q₁(t) = - 0,12t³ + 0,18t²+ 0,52

v    pentru a doua lege de miscare avem:

q₂(t) = a₂₀t³ + a₂₁t²+ a₂₂t+ a₂₃

Conditie: q₂(0) = q₂¹ a₂₃ = -1,34

q₂(1) = q₂² a₂₀ + a₂₁+ a₂₂+ a₂₃ = -1,41

q₂(0) = 0 a₂₂ = 0

q₂(1) = 0 3a₂₀ +2a₂₁ + a₂₂ = 0

Pentru rezlvarea sistemului vom utiliza programul "Mathematica":

Solve[,]

Vom obtine urmatori coeficienti: a₂₀ = 0,14; a₂₁= -0,21; a₂₂= 0; a₂₃ = -1,34

Rezulta ca:

q₂(t) = 0,14t³ -0,21t²-1,34

v    pentru a treia lege de miscare avem:

q₃(t) = a₃₀t³ + a₃₁t²+ a₃₂t+ a₃₃

Conditie: q₃(0) = q₃¹ a₃₃ = -0,61

q₃(1) = q₃² a₃₀ + a₃₁+ a₃₂+ a₃₃ = -0,16

q₃(0) = 0 a₃₂ = 0

q₃(1) = 0 3a₃₀ +2a₃₁ + a₃₂ = 0

Pentru rezlvarea sistemului vom utiliza programul "Mathematica":

Solve[,]

Vom obtine urmatori coeficienti: a₃₀ = - 0,9; a₃₁= 1,35; a₃₂= 0; a₃₃ = -0,61

Rezulta ca:

q₃(t) = - 0,9t³ +1,35t²- 0,61

Vitezele si acceleratiile (unghiulare si ale centrelor de masa)

gradelor de libertate "1"si "3" pentru deplasarea pe traiectoria definita la "1", conform legilor de miscare determinate in "4".

Legile de miscare pentru primele trei grade de libertate sunt:

q₁(t) = - 0,12t³ + 0,18t²+ 0,52

q₂(t) = 0,14t³ -0,21t²-1,34

q₃(t) = - 0,9t³ +1,35t²- 0,61

Se calculeaza viteza unghiulara pentru elementul "3" : w[3], acceleratia unghiulara pentru elementul "3": dw[3] si acceleratia centrului de masa pentru elementul "3" : ca[3].

Viteza unghiulara si acceleratia elementului "i" se calculeaza cu relatiile:

			WI-1 + I x eI , daca cupla cinematica "i" este de rotatie

WI-1 , daca cupla cinematica "i" este de translatie

w_i =

wI-1 , daca cupla cinematica "i" este de translatie

w_I =

pentru I=1,., n unde w₀ si w₀ sunt vitezele unghiulare si acceleratiile bazei.

Pentru calculul vitezelor si acceleratiilor centrelor de greutate, fie c_I vectorul de pozitie al centrului de greutate, C_I al elementului i , q_I fiind vectorul directionat de la O_i la C_i . Vectorul de pozitie a doua centre de greutate succesive este dat de relatia:

c_i = c_i-1 - q_i-1 + a_i + q_i sau

Dupa diferentierea ecuatiei de mai sus fata de timp obtinem formulele pentru calculul vitezelor si acceleratiilor centrelor de greutate atat pentru cupla cinematica de rotatie cat si pentru cupla cinematica de translatie.

alf=

tet=

bb=

aa=

rt=

f1[x_]=-0.12 x ^ 3+0.18x ^ 2 + 0.52;

f2[x_]=- 0.14 x ^ 3-0.21 x ^ 2 -1.34;

f3[x_]=-0.9 x ^ 3+1.35 x ^ 2 -0.61;

f4[x_]=x; f5[x_]=x; f6[x_]=x;

f=

e=,,}

cr=,,}

m=

ro1=,,}

ro2=,,}

ro3=,,}

ro4=,,}

ro5=,,}

ro6=,,}

r=

i1=,,}

i2=,,}

i3=,,}

i4=,,}

i5=,,}

i6=,,}

am=

For[i=1,i<7,i++,ar=rt[[i]];g[x]=f[[i]];aj=aa[[i]];bj=bb[[i]];

te=tet[[i]];al=alf[[i]];

If[ar==1,

q[x_]=,

,

};

Q[i_]=Transpose[q[x]];

a[i_]=,,},

q[x_]=,

,

};

Q[i_]=Transpose[q[x]];

a[i_]=,,}];

If[ar==1,

dw[0]=cr;w[0]=cr;ca[0]=cr;r[[0]]=cr;w[i_]=Q[i] . w[i-1]+D[g[x],x]e;

wi=w[i];aw=w[i-1];alt=D[g[x],x]e;

d1=wi[[2]] alt[[3]]-wi[[3]] alt[[2]];

d2=wi[[1]] alt[[3]]-wi[[3]] alt[[1]];

d3=wi[[1]] alt[[2]]-wi[[2]] alt[[1]];

expr1[i_]=;

dw[i_]=Q[i] . dw[i-1]+D[g[x],]e+expr1[i];adw=dw[i-1];dwa=dw[i];

di=a[i]-r[[i-1]];

exp1=adw[[2]] di[[3]]-adw[[3]] di[[2]];

exp2=adw[[1]] di[[3]]-adw[[3]] di[[1]];

exp3=adw[[1]] di[[2]]-adw[[2]] di[[1]];

expr2[i_]=;

expa1=aw[[2]] di[[3]]-aw[[3]] di[[2]];

expa2=aw[[1]] di[[3]]-aw[[3]] di[[1]];

expa3=aw[[1]] di[[2]]-aw[[2]] di[[1]];

expr3[i_]=;

aex=expr3[i];

expb1=aw[[2]] aex[[3]]-aw[[3]] aex[[2]];

expb2=aw[[1]] aex[[3]]-aw[[3]] aex[[1]];

expb3=aw[[1]] aex[[2]]-aw[[2]] aex[[1]];

expr4[i_]=;

roi=r[[i]];

expc1=dwa[[2]] roi[[3]]-dwa[[3]] roi[[2]];

expc2=dwa[[1]] roi[[3]]-dwa[[3]] roi[[1]];

expc3=dwa[[1]] roi[[2]]-dwa[[2]] roi[[1]];

expr5[i_]=;

expd1=wi[[2]] roi[[3]]-wi[[3]] roi[[2]];

expd2=wi[[1]] roi[[3]]-wi[[3]] roi[[1]];

expd3=wi[[1]] roi[[2]]-wi[[2]] roi[[1]];

expr6[i_]=;

bex=expr6[i];

expe1=wi[[2]] bex[[3]]-wi[[3]] bex[[2]];

expe2=wi[[1]] bex[[3]]-wi[[3]] bex[[1]];

expe3=wi[[1]] bex[[2]]-wi[[2]] bex[[1]];

expr7[i_]=;

ca[i]=Q[i] . (ca[i-1]+expr2[i]+expr4[i])+expr5[i]+expr7[i];

dw[0]=cr;w[0]=cr;ca[0]=cr;r[[0]]=cr;

w[i_]=Q[i] . w[i-1];wi=w[i];aw=w[i-1];alt=D[g[x],x]e;

dw[i_]=Q[i] . dw[i-1];adw=dw[i-1];dwa=dw[i];

d1=wi[[2]] alt[[3]]-wi[[3]] alt[[2]];

d2=wi[[1]] alt[[3]]-wi[[3]] alt[[1]];

d3=wi[[1]] alt[[2]]-wi[[2]] alt[[1]];

expr1[i_]=;

di=a[i]-r[[i-1]];

exp1=adw[[2]] di[[3]]-adw[[3]] di[[2]];

exp2=adw[[1]] di[[3]]-adw[[3]] di[[1]];

exp3=adw[[1]] di[[2]]-adw[[2]] di[[1]];

expr2[i_]=;

expa1=aw[[2]] di[[3]]-aw[[3]] di[[2]];

expa2=aw[[1]] di[[3]]-aw[[3]] di[[1]];

expa3=aw[[1]] di[[2]]-aw[[2]] di[[1]];

expr3[i_]=;

aex=expr3[i];

expb1=aw[[2]] aex[[3]]-aw[[3]] aex[[2]];

expb2=aw[[1]] aex[[3]]-aw[[3]] aex[[1]];

expb3=aw[[1]] aex[[2]]-aw[[2]] aex[[1]];

expr4[i_]=;

roi=r[[i]];

expc1=dwa[[2]] roi[[3]]-dwa[[3]] roi[[2]];

expc2=dwa[[1]] roi[[3]]-dwa[[3]] roi[[1]];

expc3=dwa[[1]] roi[[2]]-dwa[[2]] roi[[1]];

expr5[i_]=;

expd1=wi[[2]] roi[[3]]-wi[[3]] roi[[2]];

expd2=wi[[1]] roi[[3]]-wi[[3]] roi[[1]];

expd3=wi[[1]] roi[[2]]-wi[[2]] roi[[1]];

expr6[i_]=;

bex=expr6[i];

expe1=wi[[2]] bex[[3]]-wi[[3]] bex[[2]];

expe2=wi[[1]] bex[[3]]-wi[[3]] bex[[1]];

expe3=wi[[1]] bex[[2]]-wi[[2]] bex[[1]];

expr7[i_]=;

ca[i]=Q[i] . (ca[i-1]+expr2[i]+expr4[i])+expr5[i]+expr7[i]-D[g[x],]e-2.expr1[i]]];

-Pi -Pi Pi -Pi

2 2 2 2

Pi

2

Print [w[3]]

Print[dw[3]]

Print[ca[3]]

4. Planificarea traiectoriei intre cele doua puncte definite prin '1'

(vitezele initiala si finala sunt nule) in sistemul 4-3-4, pentru gardul de libertate '3'

Planificarea traiectoriei in coordonate articulare se realizeaza atunci cand ne intereseaza pozitia initiala si finala. Planificarea traiectoriei in coordonate operationale ne intereseaza atunci cand deplasarea endefectorului se face de-a lungul unei traiectorii.

Pentru deplasarea endefectorului intre punctul initial si final, definite in coordonate articulare, se determina legile de variatie ale coordonatelor articulare ale gradului de libertate "3", utilizand doua puncte nodale intermediare (unul de plecare si celelalt de apropiere). Coordonatele articulare pentru punctul initial sunt:

q₁¹ = 0,52 m;

q₂¹ = -1,34 m;

q₃¹ = - 0,61 m.

Coordonatele articulare pentru punctul final :

q₁² = 0.58 m;

q₂² = - 1.41 m;

q₃² = - 0.16 m.

Pentru gradul de libertate "3" coordonata articulara a punctului initial este P_I(-0,61), iar coordonata articulara a a punctului final este P_f(-0.16). Vitezele si acceleratiile initiale si finale sunt:v_i = 0, v_f = 0, a_i = 0, a_f = 0,

Pentru a asigura o buna precizie de pozitionare trebuie sa fragmentam traiectoria in trei segmente. Astfel, traiectoria initiala se imparte in trei traiectorii:

o portiune de pornire PiA, de-a lungul careia se realizeaza acceleratia si se ating viteza si acceleratia maxima;

o portiune AB, de-a lungul careia viteza si acceleratia raman constante, ea reprezinta cea mai mare parte din traiectoria initiala PiPf;

o portiune finala BPf, de pozitionare, care are ca scop asigurarea unei bune precizii de pozitionare prin micsorarea vitezei si acceleratiei.

Prin punctele Pi,A,B,Pf traiectoria se imparte in trei zone, astfel h(t) este legea care descrie traiectoria in coordonate articulare si va fi o functie in trei trepte. Pentru prima portiune avem h₁ (t) o functie de gradul 4, pentru a doua avem h₂ (t) o functie de gradul 3 si pentru a treia h₃ (t) o functie de gradul 4. Functiile alese permit sa se efectueze interpolarea traiectoriei pentru punctele nodale date care asigura realizarea conditiei continuitatii pozitiei, a vitezei si a acceleratiei pe tot intervalul de timp [t₀, t_f].

Tinand cont de cele mentionate mai sus vom avea 14 conditii din care vom afla cele 14 necunoscute din functiile h₁ (t), h₂ (t), h₃ (t).

Pentru punctul initial Pi:

h₁ = a₁₄t⁴ + a₁₃t³+ a₁₂t²+ a₁₁t+ a₁₀ , t [0,1]

Conditia pentru pozitie: h₁(0) = - 0.61 (ecuatia 1)

h₁(0) = a₁₀ = - 0,61

Conditia pentru viteza: (ecuatia 2)

v₁(t) = 4 a₁₄t³ + 3a₁₃t²+ 2a₁₂t+ a₁₁

v₁(0) = a₁₁=0

Conditia pentru acceleratie:  (ecuatia 3)

a₁(t) = 12a₁₄t² + 6a₁₃t+ 2a₁₂

a₁(0) = 2a₁₂=0

Pentru punctul de plecare A:

h₂ = a₂₃t³ + a₂₂t²+ a₂₁t+ a₂₀ , t [0,1]

Conditia de continuitate a pozitiei: h₁(1) = h₂(0) (ecuatia 4)

h₂(0) = a₂₀ = - 0,51

a₁₄ + a₁₃+ a₁₀=a₂₀

Conditia de continuitate a vitezei: (ecuatia 5)

v₂(t) = 3a₂₃t² + 2a₂₂t+ a₂₁

v₂(0) = a₂₁

4a₁₄ + 3a₁₃ = a₂₁

Conditia de continuitate a acceleratiei:

(ecuatia 6)
a₂(t) = 6a₂₃t + 2a₂₂

a₂(0) = 2a₂₂

12a₁₄t + 6a₁₃=2a₂₂

Pentru punctul de apropiere B:

h₃(t)= a₃₄t⁴ + a₃₃t³+ a₃₂t²+ a₃₁t +a₃₀ , t [0,1]

Pentru t₁=t-1, t ₁ [-1,0]

h₃(t₁)= a'₃₄t₁⁴ + a'₃₃ t₁³+ a'₃₂ t₁²+ a'₃₁ t₁ +a'₃₀

h₃(t₁)= a₃₄t₁⁴ + (4a₃₄+ a₃₃)t₁³+ (6a₃₄ +3a₃₃ + a₃₂) t₁² + (4a₃₄ +3a₃₃ + 2a₃₂+a₃₁) t₁+

(a₃₄ +a₃₃ + a₃₂+a₃₁+a₃₀)

a'₃₄= a₃₄

a'₃₃= 4a₃₄+a₃₃

a'₃₂=6a₃₄+3a₃₃+a₃₂

a'₃₁=4a₃₄+3a₃₃+2a₃₂+a₃₁

a'₃₀=a₃₄+a₃₃+a₃₂+a₃₁+a₃₀

Conditia de continuitate a pozitiei: h₂(1) = h₃(-1)

a₂₃+a₂₂+a₂₁+a₂₀= a'₃₄-a'₃₃+a'₃₂-a'₃₁+a'₃₀  (ecuatia 7)

Conditia de continuitate a vitezei:

3a₂₃+2a₂₂+a₂₁= -4a'₃₄+3a'₃₃-2a'₃₂₊a'₃₁  (ecuatia 8)

Conditia de continuitate a acceleratiei:

6a₂₃+2a₂₂= 12a'₃₄-6a'₃₃+2a'₃₂ (ecuatia 9)

Pentru punctul final P_f:

Conditia pentru pozitie: h₃(0)= -0.16

h₃(0)= a'₃₀= -0.16  (ecuatia 10)

Conditia pentru viteza:

a'₃₁=0 (ecuatia 11)

Conditia pentru acceleratie:

a'₃₂=0 (ecuatia 12)

Conditiile de definire ale punctelor nodale intermediare sunt:

- pentru punctul A: h₂(0) = q₃¹ +Δ = -0,61+ 0,1 (ecuatia 13)

- pentru punctul B: h₂(1) = q₃² -Δ = -0.16 - 0,1 (ecuatia 14)

Astfel, cele 14 ecuatii cu 14 necunoscute sunt prezentate mai jos:

Acest sistem va fi rezolvat cu ajutorul unui program in MATH2.2:

Solve[,

Pentru t [0,1] se calculeaza a₃₄, a₃₃, a₃₂, a₃₁,a₃₀, rezolvandu-se sistemul de ecuatii urmator

a₃₄ = a

4a₃₄ +a₃₃ = a'₃₃

6a₃₄ + 3a₃₃ +a₃₂ = a

4a₃₄ +3a₃₃ +2a₃₂ +a₃₁ = a'₃₁

a₃₄ +a₃₃ +a₃₂ +a₃₁ + a₃₀ = a'₃₀

se ajunge la urmatoarele valori:

a₃₀ = - 0,8075

a₃₁ = 1,7675

a₃₂ = -1,4175

a₃₃ = 0,1225

a₃₄ = 0,175

Polinoamele h₁(t) h₂(t) h₃(t):

h₁(t)= -0,075t⁴ + 0,175t³ -0,61 , t [0,1]

h₂(t)= -0,05t³ +0,075t² +0,225t -0,51 , t [0,1]

h₃(t)= 0,175t⁴ +0,1225t³ -1,4175t² +1,7675t -0,8075 , t [0,1]

ANEXA

Caracteristicile robotului PUMA 600

Binecunoscutul robot industrial Puma 600 prezinta urmatoarele caracteristici inertiale:

Unitatile de masura pentru, , sunt respectiv :

, , .

Parametrii H - D ai acestui robot industrial sunt redati mai jos:

articultia initial

1 0 0 0 0

2 -90 0 -1,149 0

3 0 0,432 0 0

4 90 0,02 -0,430 0

5 -90 0 0 0

6 90 0 -0,056

Traiecoriile celor 6 articulatii sunt descrise de urmatoarele curbe :

pentru i=1,2,...6

In urma rularii programului de simulare al comportamentului dinamic al robotilor industriali se obtin urmatoarele grafice de variatie ale momentelor motrice in raport cu timpul :

BIBLIOGRAFIE

Note de curs

Chircor M - Roboti industriali, Ovidius University Press