Michał Grudziński   

 

 

 

 

 

 

 

 grupa 24A 

Marcin Krawiec 
 
Roboty przemysłowe laboratorium 2a 
Model geometryczny manipulatora robota przemysłoweg 
 

1.  Program użyty do wyznaczenia wartości kątów: 

% skrypt wylicza macierz przekształcenia jednorodnego

 

clear 

all

 

% deklaracja symboli występujących w modelu

 

% zmienne qi tworzone są tylko dla współrzędnych obrotowych

 

syms 

th1

 

th2

 

th3

 

th4

 

th5

 

th6

 

th7

 

d1

 

d2

 

d3

 

d5

 

d7

 

a1

 

a3

 

syms 

q1

 

q2

 

q3

 

q4

 

q5

 

q6

 

q7

 

% wyznaczenie symbolicznej postaci macierzy - funkcja mA

 

A1=mA(th1+sym(pi/18),d1,a1,sym(-pi/2)) 
A2=mA(th2,d2,0,sym(pi/2)) 
A3=mA(th3,d3,a3,0) 
A4=mA(th4,0,0,sym(pi/2)) 
A5=mA(th5,d5,0,sym(-pi/2)) 
A6=mA(th6,0,0,sym(pi/2)) 
A7=mA(th7,d7,0,0) 
  

% mnozenie macierzy

 

T07=A1*A2*A3*A4*A5*A6*A7 

% podstawienie nowych zmiennych za obrotowe zmienne złączowe

 

% do upraszczania zapisu macierzy przekształcenia jednorodnego

 

% zmienne thi lub ali sa zamieniane na qi

 

T07v=subs(T07,{th1 th2 th3 th4 th5 th6 th7},{q1 q2 q3 q4 q5 q6 q7}) 

% wskazanie zmiennych złączowych

 

% zmienne: th1 th2 th3 th4 th5 th6 th7 wskazuj? ‘1’-ki

 

zmie=[[1,0,0,0];[1,0,0,0];[1,0,0,0];[1,0,0,0];[1,0,0,0];[1,0,0,0];[1,0,0,0]] 

% uproszczona postać macierzy przekształcenia jednorodnego

 

% dla celów interpretacji

 

T07u=zam(zmie,T07v) 
  

% podstawienie wartości zmiennych złączowych

 

% i stałych parametrów do macierzy T0e

 

% należy podstawić dane odpowiednio w radianach lub metrach

 

T0en=subs(T07,{th1 th2 th3 th4 th5 th6 th7 d1 d2 d3 d5 d7 a1 a3},{0 0 0 0 0 0 0 0.4 0.25 0.13 0.38 0.32 0.3 
0.25}) 
  
psi=atan2(T0en(3,2),T0en(3,3)) 
fi=atan2(T0en(1,2),T0en(1,1)) 
teta=atan2(-T0en(1,3),sqrt(1-T0en(1,1)^2)) 

 

2.  Uzyskane wyniki: 

psi =    1.5708   =  90º 

fi = 

0  

  =    0º 

teta =   0.7854  =  45º