Michał Grudziński grupa 24A
Marcin Krawiec
Roboty przemysłowe laboratorium 2
Model geometryczny manipulatora robota przemysłowego
Schemat kinematyczny robota MOTOMAN – DA20
Tabela parametrów powyższego robota
Program wyznaczający macierze opisującą położenie i orientację efektora
% 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 d3 d5 d7 a1 a3
syms q1 q2 q3 q4 q5 q6 q7
% wyznaczenie symbolicznej postaci macierzy - funkcja mA
A1=mA(th1,d1,a1,sym(-pi/2))
A2=mA(th2,0,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
T03=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
T03v=subs(T03,{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
T03u=zam(zmie,T03v)
Wyznaczone macierze:
T03 =
[ sin(th7)*(sin(th5)*(cos(th4)*(sin(th1)*sin(th3) - cos(th1)*cos(th2)*cos(th3)) + sin(th4)*(cos(th3)*sin(th1) + cos(th1)*cos(th2)*sin(th3))) + cos(th1)*cos(th5)*sin(th2)) - cos(th7)*(sin(th6)*(cos(th4)*(cos(th3)*sin(th1) + cos(th1)*cos(th2)*sin(th3)) - sin(th4)*(sin(th1)*sin(th3) - cos(th1)*cos(th2)*cos(th3))) + cos(th6)*(cos(th5)*(cos(th4)*(sin(th1)*sin(th3) - cos(th1)*cos(th2)*cos(th3)) + sin(th4)*(cos(th3)*sin(th1) + cos(th1)*cos(th2)*sin(th3))) - cos(th1)*sin(th2)*sin(th5))), sin(th7)*(sin(th6)*(cos(th4)*(cos(th3)*sin(th1) + cos(th1)*cos(th2)*sin(th3)) - sin(th4)*(sin(th1)*sin(th3) - cos(th1)*cos(th2)*cos(th3))) + cos(th6)*(cos(th5)*(cos(th4)*(sin(th1)*sin(th3) - cos(th1)*cos(th2)*cos(th3)) + sin(th4)*(cos(th3)*sin(th1) + cos(th1)*cos(th2)*sin(th3))) - cos(th1)*sin(th2)*sin(th5))) + cos(th7)*(sin(th5)*(cos(th4)*(sin(th1)*sin(th3) - cos(th1)*cos(th2)*cos(th3)) + sin(th4)*(cos(th3)*sin(th1) + cos(th1)*cos(th2)*sin(th3))) + cos(th1)*cos(th5)*sin(th2)), cos(th6)*(cos(th4)*(cos(th3)*sin(th1) + cos(th1)*cos(th2)*sin(th3)) - sin(th4)*(sin(th1)*sin(th3) - cos(th1)*cos(th2)*cos(th3))) - sin(th6)*(cos(th5)*(cos(th4)*(sin(th1)*sin(th3) - cos(th1)*cos(th2)*cos(th3)) + sin(th4)*(cos(th3)*sin(th1) + cos(th1)*cos(th2)*sin(th3))) - cos(th1)*sin(th2)*sin(th5)), d5*(cos(th4)*(cos(th3)*sin(th1) + cos(th1)*cos(th2)*sin(th3)) - sin(th4)*(sin(th1)*sin(th3) - cos(th1)*cos(th2)*cos(th3))) + a1*cos(th1) + d7*(cos(th6)*(cos(th4)*(cos(th3)*sin(th1) + cos(th1)*cos(th2)*sin(th3)) - sin(th4)*(sin(th1)*sin(th3) - cos(th1)*cos(th2)*cos(th3))) - sin(th6)*(cos(th5)*(cos(th4)*(sin(th1)*sin(th3) - cos(th1)*cos(th2)*cos(th3)) + sin(th4)*(cos(th3)*sin(th1) + cos(th1)*cos(th2)*sin(th3))) - cos(th1)*sin(th2)*sin(th5))) + d3*cos(th1)*sin(th2) - a3*sin(th1)*sin(th3) + a3*cos(th1)*cos(th2)*cos(th3)]
[ cos(th7)*(sin(th6)*(cos(th4)*(cos(th1)*cos(th3) - cos(th2)*sin(th1)*sin(th3)) - sin(th4)*(cos(th1)*sin(th3) + cos(th2)*cos(th3)*sin(th1))) + cos(th6)*(cos(th5)*(cos(th4)*(cos(th1)*sin(th3) + cos(th2)*cos(th3)*sin(th1)) + sin(th4)*(cos(th1)*cos(th3) - cos(th2)*sin(th1)*sin(th3))) + sin(th1)*sin(th2)*sin(th5))) - sin(th7)*(sin(th5)*(cos(th4)*(cos(th1)*sin(th3) + cos(th2)*cos(th3)*sin(th1)) + sin(th4)*(cos(th1)*cos(th3) - cos(th2)*sin(th1)*sin(th3))) - cos(th5)*sin(th1)*sin(th2)), - sin(th7)*(sin(th6)*(cos(th4)*(cos(th1)*cos(th3) - cos(th2)*sin(th1)*sin(th3)) - sin(th4)*(cos(th1)*sin(th3) + cos(th2)*cos(th3)*sin(th1))) + cos(th6)*(cos(th5)*(cos(th4)*(cos(th1)*sin(th3) + cos(th2)*cos(th3)*sin(th1)) + sin(th4)*(cos(th1)*cos(th3) - cos(th2)*sin(th1)*sin(th3))) + sin(th1)*sin(th2)*sin(th5))) - cos(th7)*(sin(th5)*(cos(th4)*(cos(th1)*sin(th3) + cos(th2)*cos(th3)*sin(th1)) + sin(th4)*(cos(th1)*cos(th3) - cos(th2)*sin(th1)*sin(th3))) - cos(th5)*sin(th1)*sin(th2)), sin(th6)*(cos(th5)*(cos(th4)*(cos(th1)*sin(th3) + cos(th2)*cos(th3)*sin(th1)) + sin(th4)*(cos(th1)*cos(th3) - cos(th2)*sin(th1)*sin(th3))) + sin(th1)*sin(th2)*sin(th5)) - cos(th6)*(cos(th4)*(cos(th1)*cos(th3) - cos(th2)*sin(th1)*sin(th3)) - sin(th4)*(cos(th1)*sin(th3) + cos(th2)*cos(th3)*sin(th1))), a1*sin(th1) - d5*(cos(th4)*(cos(th1)*cos(th3) - cos(th2)*sin(th1)*sin(th3)) - sin(th4)*(cos(th1)*sin(th3) + cos(th2)*cos(th3)*sin(th1))) - d7*(cos(th6)*(cos(th4)*(cos(th1)*cos(th3) - cos(th2)*sin(th1)*sin(th3)) - sin(th4)*(cos(th1)*sin(th3) + cos(th2)*cos(th3)*sin(th1))) - sin(th6)*(cos(th5)*(cos(th4)*(cos(th1)*sin(th3) + cos(th2)*cos(th3)*sin(th1)) + sin(th4)*(cos(th1)*cos(th3) - cos(th2)*sin(th1)*sin(th3))) + sin(th1)*sin(th2)*sin(th5))) + a3*cos(th1)*sin(th3) + d3*sin(th1)*sin(th2) + a3*cos(th2)*cos(th3)*sin(th1)]
[ sin(th7)*(cos(th2)*cos(th5) - sin(th5)*(sin(th2)*sin(th3)*sin(th4) - cos(th3)*cos(th4)*sin(th2))) + cos(th7)*(cos(th6)*(cos(th2)*sin(th5) + cos(th5)*(sin(th2)*sin(th3)*sin(th4) - cos(th3)*cos(th4)*sin(th2))) + sin(th6)*(cos(th3)*sin(th2)*sin(th4) + cos(th4)*sin(th2)*sin(th3))), cos(th7)*(cos(th2)*cos(th5) - sin(th5)*(sin(th2)*sin(th3)*sin(th4) - cos(th3)*cos(th4)*sin(th2))) - sin(th7)*(cos(th6)*(cos(th2)*sin(th5) + cos(th5)*(sin(th2)*sin(th3)*sin(th4) - cos(th3)*cos(th4)*sin(th2))) + sin(th6)*(cos(th3)*sin(th2)*sin(th4) + cos(th4)*sin(th2)*sin(th3))), sin(th6)*(cos(th2)*sin(th5) + cos(th5)*(sin(th2)*sin(th3)*sin(th4) - cos(th3)*cos(th4)*sin(th2))) - cos(th6)*(cos(th3)*sin(th2)*sin(th4) + cos(th4)*sin(th2)*sin(th3)), d1 - d5*(cos(th3)*sin(th2)*sin(th4) + cos(th4)*sin(th2)*sin(th3)) + d7*(sin(th6)*(cos(th2)*sin(th5) + cos(th5)*(sin(th2)*sin(th3)*sin(th4) - cos(th3)*cos(th4)*sin(th2))) - cos(th6)*(cos(th3)*sin(th2)*sin(th4) + cos(th4)*sin(th2)*sin(th3))) + d3*cos(th2) - a3*cos(th3)*sin(th2)]
[ 0, 0, 0, 1]
T03v =
[ sin(q7)*(sin(q5)*(cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q2)*cos(q3)) + sin(q4)*(cos(q3)*sin(q1) + cos(q1)*cos(q2)*sin(q3))) + cos(q1)*cos(q5)*sin(q2)) - cos(q7)*(cos(q6)*(cos(q5)*(cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q2)*cos(q3)) + sin(q4)*(cos(q3)*sin(q1) + cos(q1)*cos(q2)*sin(q3))) - cos(q1)*sin(q2)*sin(q5)) + sin(q6)*(cos(q4)*(cos(q3)*sin(q1) + cos(q1)*cos(q2)*sin(q3)) - sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q2)*cos(q3)))), cos(q7)*(sin(q5)*(cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q2)*cos(q3)) + sin(q4)*(cos(q3)*sin(q1) + cos(q1)*cos(q2)*sin(q3))) + cos(q1)*cos(q5)*sin(q2)) + sin(q7)*(cos(q6)*(cos(q5)*(cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q2)*cos(q3)) + sin(q4)*(cos(q3)*sin(q1) + cos(q1)*cos(q2)*sin(q3))) - cos(q1)*sin(q2)*sin(q5)) + sin(q6)*(cos(q4)*(cos(q3)*sin(q1) + cos(q1)*cos(q2)*sin(q3)) - sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q2)*cos(q3)))), cos(q6)*(cos(q4)*(cos(q3)*sin(q1) + cos(q1)*cos(q2)*sin(q3)) - sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q2)*cos(q3))) - sin(q6)*(cos(q5)*(cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q2)*cos(q3)) + sin(q4)*(cos(q3)*sin(q1) + cos(q1)*cos(q2)*sin(q3))) - cos(q1)*sin(q2)*sin(q5)), d5*(cos(q4)*(cos(q3)*sin(q1) + cos(q1)*cos(q2)*sin(q3)) - sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q2)*cos(q3))) - d7*(sin(q6)*(cos(q5)*(cos(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q2)*cos(q3)) + sin(q4)*(cos(q3)*sin(q1) + cos(q1)*cos(q2)*sin(q3))) - cos(q1)*sin(q2)*sin(q5)) - cos(q6)*(cos(q4)*(cos(q3)*sin(q1) + cos(q1)*cos(q2)*sin(q3)) - sin(q4)*(sin(q1)*sin(q3) - cos(q1)*cos(q2)*cos(q3)))) + a1*cos(q1) + d3*cos(q1)*sin(q2) - a3*sin(q1)*sin(q3) + a3*cos(q1)*cos(q2)*cos(q3)]
[ cos(q7)*(cos(q6)*(cos(q5)*(cos(q4)*(cos(q1)*sin(q3) + cos(q2)*cos(q3)*sin(q1)) + sin(q4)*(cos(q1)*cos(q3) - cos(q2)*sin(q1)*sin(q3))) + sin(q1)*sin(q2)*sin(q5)) + sin(q6)*(cos(q4)*(cos(q1)*cos(q3) - cos(q2)*sin(q1)*sin(q3)) - sin(q4)*(cos(q1)*sin(q3) + cos(q2)*cos(q3)*sin(q1)))) - sin(q7)*(sin(q5)*(cos(q4)*(cos(q1)*sin(q3) + cos(q2)*cos(q3)*sin(q1)) + sin(q4)*(cos(q1)*cos(q3) - cos(q2)*sin(q1)*sin(q3))) - cos(q5)*sin(q1)*sin(q2)), - sin(q7)*(cos(q6)*(cos(q5)*(cos(q4)*(cos(q1)*sin(q3) + cos(q2)*cos(q3)*sin(q1)) + sin(q4)*(cos(q1)*cos(q3) - cos(q2)*sin(q1)*sin(q3))) + sin(q1)*sin(q2)*sin(q5)) + sin(q6)*(cos(q4)*(cos(q1)*cos(q3) - cos(q2)*sin(q1)*sin(q3)) - sin(q4)*(cos(q1)*sin(q3) + cos(q2)*cos(q3)*sin(q1)))) - cos(q7)*(sin(q5)*(cos(q4)*(cos(q1)*sin(q3) + cos(q2)*cos(q3)*sin(q1)) + sin(q4)*(cos(q1)*cos(q3) - cos(q2)*sin(q1)*sin(q3))) - cos(q5)*sin(q1)*sin(q2)), sin(q6)*(cos(q5)*(cos(q4)*(cos(q1)*sin(q3) + cos(q2)*cos(q3)*sin(q1)) + sin(q4)*(cos(q1)*cos(q3) - cos(q2)*sin(q1)*sin(q3))) + sin(q1)*sin(q2)*sin(q5)) - cos(q6)*(cos(q4)*(cos(q1)*cos(q3) - cos(q2)*sin(q1)*sin(q3)) - sin(q4)*(cos(q1)*sin(q3) + cos(q2)*cos(q3)*sin(q1))), d7*(sin(q6)*(cos(q5)*(cos(q4)*(cos(q1)*sin(q3) + cos(q2)*cos(q3)*sin(q1)) + sin(q4)*(cos(q1)*cos(q3) - cos(q2)*sin(q1)*sin(q3))) + sin(q1)*sin(q2)*sin(q5)) - cos(q6)*(cos(q4)*(cos(q1)*cos(q3) - cos(q2)*sin(q1)*sin(q3)) - sin(q4)*(cos(q1)*sin(q3) + cos(q2)*cos(q3)*sin(q1)))) - d5*(cos(q4)*(cos(q1)*cos(q3) - cos(q2)*sin(q1)*sin(q3)) - sin(q4)*(cos(q1)*sin(q3) + cos(q2)*cos(q3)*sin(q1))) + a1*sin(q1) + a3*cos(q1)*sin(q3) + d3*sin(q1)*sin(q2) + a3*cos(q2)*cos(q3)*sin(q1)]
[ sin(q7)*(cos(q2)*cos(q5) - sin(q5)*(sin(q2)*sin(q3)*sin(q4) - cos(q3)*cos(q4)*sin(q2))) + cos(q7)*(cos(q6)*(cos(q2)*sin(q5) + cos(q5)*(sin(q2)*sin(q3)*sin(q4) - cos(q3)*cos(q4)*sin(q2))) + sin(q6)*(cos(q3)*sin(q2)*sin(q4) + cos(q4)*sin(q2)*sin(q3))), cos(q7)*(cos(q2)*cos(q5) - sin(q5)*(sin(q2)*sin(q3)*sin(q4) - cos(q3)*cos(q4)*sin(q2))) - sin(q7)*(cos(q6)*(cos(q2)*sin(q5) + cos(q5)*(sin(q2)*sin(q3)*sin(q4) - cos(q3)*cos(q4)*sin(q2))) + sin(q6)*(cos(q3)*sin(q2)*sin(q4) + cos(q4)*sin(q2)*sin(q3))), sin(q6)*(cos(q2)*sin(q5) + cos(q5)*(sin(q2)*sin(q3)*sin(q4) - cos(q3)*cos(q4)*sin(q2))) - cos(q6)*(cos(q3)*sin(q2)*sin(q4) + cos(q4)*sin(q2)*sin(q3)), d1 - d5*(cos(q3)*sin(q2)*sin(q4) + cos(q4)*sin(q2)*sin(q3)) + d3*cos(q2) + d7*(sin(q6)*(cos(q2)*sin(q5) + cos(q5)*(sin(q2)*sin(q3)*sin(q4) - cos(q3)*cos(q4)*sin(q2))) - cos(q6)*(cos(q3)*sin(q2)*sin(q4) + cos(q4)*sin(q2)*sin(q3))) - a3*cos(q3)*sin(q2)]
[ 0, 0, 0, 1]
Kod programu dla przyjętych wartości:
% 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(T0e{th1 th2 th3 th4 th5 th6 th7 d1 d3 d5 d7 a1 a3},
{pi/18,0,0,0,0,0,0,0.5,0.13,0.25,0.38,0.25,0.25})
Otrzymana macierz
T0en=
[0.9848 0 -0.1736 0.3830]
[-0.1736 0 -0.9848 -0.7073]
[0 1 0 0.6300]
[0 0 0 1.0000]