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))

1. Uzyskane wyniki:

psi = 1.5708 = 90º

fi = 0 = 0º

teta = 0.7854 = 45º