Untitled

Nasze zadanie polegało na przygotowaniu procedury obliczającej współrzędne złączowe danej pozycji efektora. Wejściem naszej procedury jest macierz przekształcenia jednorodnego T0E a wynikiem działania są wartości zmiennych złączowych q₁, q₃, q₄ i d₂.

W celu wyliczenia wartości zmiennych złączowych skorzystano z zależności wyznaczonych w sprawozdaniu nr 1.

Wszystkie wartości zmiennych zostały dobrane tak jak w sprawozdaniu nr 1 (tak aby były w zakresie)

Poniżej zawartość mPlików:

syms q1 d2 a2 q3 a3 q4 d5

A1=mA(q1,0,0,0);

A2=mA(0,-d2,a2,0);

A3=mA(q3,0,a3,sym(-pi/2));

A4=mA(q4,0,0,sym(-pi/2));

A5=mA(sym(pi/2),d5,0,0);

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

T0e=A1*A2*A3*A4*A5;

zam(zmie,T03);

% przyjecie parametrow

% wyznaczanie przykladowych wartosci i macierzy transformacji

T0E=subs(T0e,{q1,q3,q4,a2,a3,d2,d5},{q1r,q3r,q4r,a2z,a3z,d2z,d5z});

if abs(T0E(i,j))<0.0000001

% wyznaczenie wektora pa

pax=T0E(1,4)-T0E(1,3)*d5z;

pay=T0E(2,4)-T0E(2,3)*d5z;

paz=T0E(3,4)-T0E(3,3)*d5z;

%obliczenie kata Theta3

K=((pax^2+pay^2-a2z^2-a3z^2)/(2*a3z*a2z));

q31r=atan2(sqrt(1-K^2),K);

q31s=q31r*r2s;

q32r=atan2(-sqrt(1-K^2),K);

q32s=q32r*r2s;

%obliczenie kata Theta1

K1=pay*(a3z*cos(q31r)+a2z)-pax*(a3z*sin(q31r));

K2=pax*(a3z*cos(q31r)+a2z)+pay*(a3z*sin(q31r));

q11r=atan2(K1,K2);

q11s=q11r*r2s;

K12=pay*(a3z*cos(q32r)+a2z)-pax*(a3z*sin(q32r));

K22=pax*(a3z*cos(q32r)+a2z)+pay*(a3z*sin(q32r));

q12r=atan2(K12,K22);

q12s=q12r*r2s;

%obliczenie kąta Theta4

q41r=atan2(T0E(3,2),-T0E(1,2)*cos(q11r+q31r)-T0E(2,2)*sin(q11r+q31r));

q41s=q41r*r2s;

q42r=atan2(T0E(3,2),-T0E(1,2)*cos(q12r+q32r)-T0E(2,2)*sin(q12r+q32r));

q42s=q42r*r2s;

% zakresy ruchu

zrq1=[-120*s2r, 120*s2r];

if ((q11r>zrq1(1))&&(q11r<zrq1(2)))

t1=(sprintf('Pierwszy zestaw rozwiązań:\nKąt Theta1.1 w zakresie ruchu i wynosi: %3.0f st',q11s));

t1=(sprintf('Kąt Theta1.1 poza zakresem ruchu i wynosi: %3.0f st',q11s));

if ((q12r>zrq1(1))&&(q12r<zrq1(2)))

t12=(sprintf('Drugi zestaw rozwiązań:\nKąt Theta1.2 w zakresie ruchu i wynosi: %3.0f st',q12s));

t12=(sprintf('Kąt Theta1.2 poza zakresem ruchu i wynosi: %3.0f st',q12s));

zrq3=[-90*s2r, 90*s2r];

if ((q31r>zrq3(1))&&(q31r<zrq3(2)))

t2=(sprintf('Kąt Theta3.1 w zakresie ruchu i wynosi: %3.0f st',q31s));

t2=(sprintf('Kąt Theta3.1 poza zakresem ruchu i wynosi: %3.0f st',q31s));

if ((q32r>zrq3(1))&&(q32r<zrq3(2)))

t22=(sprintf('Kąt Theta3.2 w zakresie ruchu i wynosi: %3.0f st',q32s));

t22=(sprintf('Kąt Theta3.2 poza zakresem ruchu i wynosi: %3.0f st',q32s));

zrq4=[-90*s2r, 0];

if ((q41r>zrq4(1))&&(q41r<zrq4(2)))

t3=(sprintf('Kąt Theta4.1 w zakresie ruchu i wynosi: %3.0f st',q41s));

t3=(sprintf('Kąt Theta4.1 poza zakresem ruchu i wynosi: %3.0f st',q41s));

if ((q42r>zrq4(1))&&(q42r<zrq4(2)))

t32=(sprintf('Kąt Theta4.2 w zakresie ruchu i wynosi: %3.0f st',q42s));

t32=(sprintf('Kąt Theta4.2 poza zakresem ruchu i wynosi: %3.0f st',q42s));

zrd2=[0.3, 0.8];

if ((d21>zrd2(1))&&(d21<zrd2(2)))

t4=(sprintf('Przesów D2 w zakresie ruchu i wynosi: %1.1f m\n',d21));

t4=(sprintf('Przesów D2 poza zakresem ruchu i wynosi: %1.1f m\n',d21));

% wyznaczenie pa

pas=subs(T0e,{q1,q3,q4,a2,a3,d2,d5},{q11r,q31r,q4r,a2z,a3z,d21,d5z});

if abs(pas(i,j))<0.0000001

psx=pas(1,4)-pas(1,3)*d5z;

psy=pas(2,4)-pas(2,3)*d5z;

psz=pas(3,4)-pas(3,3)*d5z;

% dpax dpay dpaz powinny byc rowne 0

% wyswietlenie wartości zadanych i wartości obliczonych

qz=[ d2z q1z q3z q4z];

t5=(sprintf('Wartości zadane:\nd2: %1.1f m\tq1: %1.0f st\tq3: %1.0f st\tq4: %1.0f st\t',d2z,q1z,q3z,q4z));

qo=[ d21 q11s q31s q41s];

t6=(sprintf('\nWartości wyliczone :\nzestaw 1 -> d2: %1.1f m\tq1: %1.0f st\tq3: %1.0f st\tq4: %1.0f st\t\nzestaw 2 -> d2: %1.1f m\tq1: %1.0f st\tq3: %1.0f st\tq4: %1.0f st\t',d21,q11s,q31s,q41s,d21,q12s,q32s,q42s));

%macierz rotacji

R03=T03(1:3,1:3);

W wyniku działania powyższych mPlików otrzymano:

Wyznaczenie 3-ch pierwszych współrzędnych złączowych poprzez numeryczne rozwiązanie układu równań:

syms th1 d2 a2 th3 a3 th4 d4

A1=mA(th1,0,0,0);

A2=mA(0,d2,a2,0);

A3=mA(th3,0,a3,sym(-pi/2));

T03p=subs(T03,{a2,a3},{0.12,0.2});

%transformacja kąt-radian radian-kąt

%współrzędne punktu końcowego

paz=0.35;%0.6;

%układ równań

y1=pax-T03p(1,4);

y2=pay-T03p(2,4);

y3=paz-T03p(3,4);

%wyliczenie zmiennych

rozw=solve(y1,y2,y3,'th1,th3,d2');

d21=double(A(1,1));

q11r=double(B(1,1));

q11s=q11r*r2s;

q31r=double(C(1,1));

q31s=q31r*r2s;

%przyjęcie dopuszczalnych zakresów ruchu

zrq1=[-120*s2r, 120*s2r];

zrq3=[-90*s2r, 90*s2r];

zrd2=[0.3, 0.8];

t1=(sprintf('Współrzedne położenia Pax= %1.2f m\tPay= %1.2f m\tPaz= %1.2f m\t',pax,pay,paz));

%sprawdzenie, czy otrzymane wartości mieszczą się w zakresie ruchu

if ((q11r>zrq1(1))&&(q11r<zrq1(2)))

t2=(sprintf('Kąt Theta1.1 w zakresie ruchu i wynosi: %3.0f st',q11s));

t2=(sprintf('Kąt Theta1.1 poza zakresem ruchu i wynosi: %3.0f st',q11s));

if ((q31r>zrq3(1))&&(q31r<zrq3(2)))

t3=(sprintf('Kąt Theta3.1 w zakresie ruchu i wynosi: %3.0f st',q31s));

t3=(sprintf('Kąt Theta3.1 poza zakresem ruchu i wynosi: %3.0f st',q31s));

if ((d21>zrd2(1))&&(d21<zrd2(2)))

t4=(sprintf('Przesów D2 w zakresie ruchu i wynosi: %1.1f m\n',d21));

t4=(sprintf('Przesów D2 poza zakresem ruchu i wynosi: %1.1f m\n',d21));

Następnym etapem jest przetestowanie działania obliczeń analitycznych i numerycznych dla 6 punktów przestrzeni roboczej dla 3-ch pierwszych zmiennych złączowych:

Jak widać wyniki uzyskane za pomocą obu metod są takie same. Metoda numeryczna jest o tyle łatwiejsza że nie wymaga od użytkownika umiejętności przekształceń macierzy i wyznaczania kątów.


Modelowanie Manipulatorów
Wydział: IMIR	Kierunek: Automatyka i Robotyka	Rok: I mgr
Temat: Kinematyka - lab2		Grupa: 2
Prowadzący: mgr inż. Dariusz Baran
Imię i nazwisko: Paweł Piec i Marcin Baszak		Data: 20.03.2011r

Współrzędne punktów			Metoda analityczna			Metoda numeryczna
x	y	z	q₁[^o]	q₃[^o]	d₂[m]	q₁[^o]	q₃[^o]	d₂[m]
0.05	-0.28	-0.35	-53	-39	0.35	-53	-39	0.35
0.15	-0.23	-0.40	-23	-51	0.40	-23	-51	0.40
0.30	0.00	-0,45	0	0	0.45	0	0	0.45
0.28	0.05	-0.50	-16	39	0.50	-16	39	0.50
0.26	0.10	-0.55	-10	46	0.55	-10	46	0.55
0.24	0.15	-0.60	4	41	0.60	4	41	0.60