Tecnológico Nacional De México

Instituto Tecnológico De La Laguna

ESPECIALIDAD: Ing. Mecatrónica

CURSO: PLANIFICACION DE MOVIMIENTO DE ROBOTS

TAREA 2: PLANEACION DE MOVIMIENTO DE ROBOTS

ALUMNO: BUGARIN PUENTES JESUS ABRAHAM NC. 18130335

DOCENTE: DR. FIERRO PROA JESUS EDUARDO

GOMEZ PALACIO DGO. 25 DE FEBRERO DEL 2022

Partiendo de la tabla de parámetros de Denavit-Hartenberg Modificados.

j αj dj θj rj

1 0° 0 θ1=0° 0

2 90° d2 θ2=90° 0

3 0° d3 θ3 0

4 90° d4 θ4 r4

5 -90° 0 θ5 0
6 90° 0 θ6 0
 Procedemos a realizar las Matrices de Transformación Homogénea para cada
PDHMod.

𝐶𝑜𝑠(𝜃𝑗) −𝑆𝑒𝑛(𝜃𝑗) 0 𝑑𝑗
𝑗−1 𝐶𝑜𝑠(𝛼𝑗)𝑆𝑒𝑛(𝜃𝑗) 𝐶𝑜𝑠(𝛼𝑗)𝐶𝑜𝑠(𝜃𝑗) −𝑆𝑒𝑛(𝛼𝑗) −𝑟𝑗𝑆𝑒𝑛(𝛼𝑗)
𝑗𝑇 =[ ]
𝑆𝑒𝑛(𝛼𝑗)𝑆𝑒𝑛(𝜃𝑗) 𝑆𝑒𝑛(𝛼𝑗)𝐶𝑜𝑠(𝜃𝑗) 𝐶𝑜𝑠(𝛼𝑗) 𝑟𝑗𝐶𝑜𝑠(𝛼𝑗)
0 0 0 1

𝐶𝑜𝑠(𝜃1) −𝑆𝑒𝑛(𝜃1) 0 0 𝐶𝑜𝑠(𝜃2) −𝑆𝑒𝑛(𝜃2) 0 𝑑2

0 𝑆𝑒𝑛(𝜃1) 𝐶𝑜𝑠(𝜃1) 0 0 1
𝑇=[ 0 0 −1 0
1
𝑇=[ ] 2 ]
0 0 1 0 𝑆𝑒𝑛(𝜃2) 𝐶𝑜𝑠(𝜃2) 0 0
0 0 0 1 0 0 0 1
𝐶𝑜𝑠(𝜃3) −𝑆𝑒𝑛(𝜃3) 0 𝑑3 𝐶𝑜𝑠(𝜃4) −𝑆𝑒𝑛(𝜃4) 0 𝑑4
𝑆𝑒𝑛(𝜃3) 𝐶𝑜𝑠(𝜃3) 0 0 0 0 −1 −𝑟4 ]
𝑇 = [2
3 ] 3
4𝑇 = [
0 0 1 0 𝑆𝑒𝑛(𝜃4) 𝐶𝑜𝑠(𝜃4) 0 0
0 0 0 1 0 0 0 1

𝐶𝑜𝑠(𝜃5) −𝑆𝑒𝑛(𝜃5) 0 0 𝐶𝑜𝑠(𝜃6) −𝑆𝑒𝑛(𝜃6) 0 0

4 0 0 1 0] 5 0 0 −1 0]
5𝑇 = [ 6𝑇 = [
−𝑆𝑒𝑛(𝜃5) −𝐶𝑜𝑠(𝜃5) 0 0 𝑆𝑒𝑛(𝜃6) 𝐶𝑜𝑠(𝜃6) 0 0
0 0 0 1 0 0 0 1

Multiplicamos las Matrices de Transformación Homogénea para obtener la Cinemática Directa de

posición T06= T01*T12*T23*T34*T45*T56
% CINEMATICA DIRECTA DE POSICION T06

syms t1 t2 t3 t4 t5 t6 d2 d3 d4 r4

T01=[cos(t1) -sin(t1) 0 0; sin(t1) cos(t1) 0 0; 0 0 1 0; 0 0 0 1];

T12=[cos(t2) -sin(t2) 0 d2; 0 0 -1 0; sin(t2) cos(t2) 0 0; 0 0 0 1];
T23=[cos(t3) -sin(t3) 0 d3; sin(t3) cos(t3) 0 0; 0 0 1 0; 0 0 0 1];
T34=[cos(t4) -sin(t4) 0 d4; 0 0 -1 -r4; sin(t4) cos(t4) 0 0; 0 0 0 1];
T45=[cos(t5) -sin(t5) 0 0; 0 0 1 0; -sin(t5) -cos(t5) 0 0; 0 0 0 1];
T56=[cos(t6) -sin(t6) 0 0; 0 0 -1 0; sin(t6) cos(t6) 0 0; 0 0 0 1];

%Cinemática Directa de posición T06

T06=simplify(T01*T12*T23*T34*T45*T56);

%MODELADO INVERSO POR PAUL

syms sx sy sz nx ny nz ax ay az px py pz
U0=[sx nx ax px; sy ny ay py; sz nz az pz;0 0 0 1];
eqA=U0(1,1)==T06(1,1);
%Iteración 1
T10=simplify(inv(T01));
U1=T10*U0;
T16=T12*T23*T34*T45*T56;
%Del elemento U1(2,4), T16(2,4)
%U1=T16;
%U1(2,4)=T16(2,4)
%Ecuaciòn= py*cos(t1) - px*sin(t1)=0
% se obtiene t1
a=py;
b=-px;
c=0;
syms eps1;
%eps1=1;
s1=(b*c+eps1*a*sqrt((a^2)+(b^2)-(c^2)))/((a^2)+(b^2));
c1=(a*c-eps1*b*sqrt((a^2)+(b^2)-(c^2)))/((a^2)+(b^2));
tange1=(s1/c1);
%t1a=ATAN2(eps1*py,eps1*px);

T21=inv(T12);
T26=T23*T34*T45*T56;
U2=simplify(T21*U1);
%de (2,4)

%retomando de (2,4); U2(2,4)=T26(2,4)

eq1=(d2 - px*cos(t1) - py*sin(t1))*sin(t2) + pz*cos(t2);%=r4*cos(t3)

%retomando de (1,4); U2(1,4)=T26(1,4)

eq2=-((px*cos(t1) - d2 + py*sin(t1))*cos(t2) + pz*sin(t2)-d3); %=r4*sin(t3)
%elevando al cuadrado ambos terminos y sumandolos
eq3=(eq1^2)+(eq2^2);%=r4^2(cos(t3)^2+sin(t3)^2)
eq3=collect(collect(eq3,sin(t2)),cos(t2));

eq3b=(pz^2 + (px*cos(t1) - d2 + py*sin(t1))^2) + (-2*d3*(px*cos(t1) - d2 +

py*sin(t1)))*cos(t2) + d3^2 - 2*d3*pz*sin(t2);%=r4^2*(1)
eq3b=collect(collect(eq3b,sin(t2)),cos(t2));%=r4^2

a2=(-2*d3*(px*cos(t1) - d2 + py*sin(t1)));
b2=(-2*d3*pz);
ce2=r4^2-( d3^2 + pz^2 + (px*cos(t1) - d2 + py*sin(t1))^2);
syms eps2
s2=simplify((b2*ce2+eps2*a2*sqrt((a2^2)+(b2^2)-(ce2^2)))/((a2^2)+(b2^2)));
c2=simplify((a2*ce2-eps2*b2*sqrt((a2^2)+(b2^2)-(ce2^2)))/((a2^2)+(b2^2)));
tange2=simplify((s2/c2));
%t2a=ATAN2(eps2*s2,eps2*c2);

%De eq1 (U2(2,4)=T26(2,4)) y eq2 (U2(1,4)=T26(1,4))

c3=((d2 - px*cos(t1) - py*sin(t1))*sin(t2) + pz*cos(t2))/r4;
s3=(-((px*cos(t1) - d2 + py*sin(t1))*cos(t2) + pz*sin(t2)-d3))/r4;
syms eps3
tange3=s3/c3;
%t3a=ATAN2(eps3*s3,eps3*c3);
%Siguiendo con las iteraciones
T32=inv(T23); T36=T34*T45*T56;
U3=simplify(T32*U2);

T43=inv(T34);
T46=T45*T56;
U4=simplify(T43*U3);
%usando los elementos (1,4) (U4(1,4)=T46(1,4))
eqn4=collect(collect(U4(1,4),sin(t4)),cos(t4));

a4=(pz*cos(t2)*sin(t3) - d2*cos(t2)*cos(t3) - d3*cos(t3) + pz*cos(t3)*sin(t2)

+ d2*sin(t2)*sin(t3) + px*cos(t1)*cos(t2)*cos(t3) + py*cos(t2)*cos(t3)*sin(t1)
- px*cos(t1)*sin(t2)*sin(t3) - py*sin(t1)*sin(t2)*sin(t3));
b4=(py*cos(t1) - px*sin(t1));
ce4=0;
syms eps4
s4=simplify((b4*ce4+eps4*a4*sqrt((a4^2)+(b4^2)-(ce4^2)))/((a4^2)+(b4^2)));
c4=simplify((a4*ce4-eps4*b4*sqrt((a4^2)+(b4^2)-(ce4^2)))/((a4^2)+(b4^2)));
tange4=simplify((s4/c4));
%t4a=ATAN2(eps4*s4,eps4*c4);

%indefinido, entonces de los elementos (2,3), (U4(2,3)=T46(2,3))

eqn4b=collect(collect(U4(2,3),sin(t4)),cos(t4));
a4=(ay*cos(t1) - ax*sin(t1));
b4=(sin(t3)*(ax*cos(t1)*sin(t2) - az*cos(t2) + ay*sin(t1)*sin(t2)) -
cos(t3)*(az*sin(t2) + ax*cos(t1)*cos(t2) + ay*cos(t2)*sin(t1)));
ce4=0;
s4=simplify((b4*ce4+eps4*a4*sqrt((a4^2)+(b4^2)-(ce4^2)))/((a4^2)+(b4^2)));
c4=simplify((a4*ce4-eps4*b4*sqrt((a4^2)+(b4^2)-(ce4^2)))/((a4^2)+(b4^2)));
tange4=simplify((s4/c4));
syms eps4
%De Iteración 4, utilizamos los elementos (1,3) y (3,3)
%de (1,3) (U4(1,3)=T46(1,3))
s5=cos(t4)*(cos(t3)*(az*sin(t2) + ax*cos(t1)*cos(t2) + ay*cos(t2)*sin(t1)) -
sin(t3)*(ax*cos(t1)*sin(t2) - az*cos(t2) + ay*sin(t1)*sin(t2))) +
sin(t4)*(ay*cos(t1) - ax*sin(t1));
%(3,3) (U4(3,3)=T46(3,3))
c5=-(- sin(t3)*(az*sin(t2) + ax*cos(t1)*cos(t2) + ay*cos(t2)*sin(t1)) -
cos(t3)*(ax*cos(t1)*sin(t2) - az*cos(t2) + ay*sin(t1)*sin(t2)));
tange5=simplify((s5/c5));
%th5a=ATAN2(eps5*s5,eps5*c5);

%De Iteración 4, utilizamos los elementos (2,1) y (2,2)

%de (2,1) (U4(2,1)=T46(2,1))
s6=-(cos(t4)*(sy*cos(t1) - sx*sin(t1)) - sin(t4)*(cos(t3)*(sz*sin(t2) +
sx*cos(t1)*cos(t2) + sy*cos(t2)*sin(t1)) - sin(t3)*(sx*cos(t1)*sin(t2) -
sz*cos(t2) + sy*sin(t1)*sin(t2))));
%de (2,2) (U4(2,2)=T46(2,2))
c6=-(cos(t4)*(ny*cos(t1) - nx*sin(t1)) - sin(t4)*(cos(t3)*(nz*sin(t2) +
nx*cos(t1)*cos(t2) + ny*cos(t2)*sin(t1)) - sin(t3)*(nx*cos(t1)*sin(t2) -
nz*cos(t2) + ny*sin(t1)*sin(t2))));
tange6=simplify((s6/c6));
%Jacobiana directa
%Primera Fila son los elementos de la ecuación de la posición de x
%difereciados parcialmente por cada una de las variables articulares

Posx=simplify(T06(1,4));
J(1,1)=diff(Posx,t1);
J(1,2)=diff(Posx,t2);
J(1,3)=diff(Posx,t3);
J(1,4)=diff(Posx,t4);
J(1,5)=diff(Posx,t5);
J(1,6)=diff(Posx,t6);

%Donde Xp=J(1,:)*(t1p:t6p)

Posy=simplify(T06(2,4));
J(2,1)=diff(Posy,t1);
J(2,2)=diff(Posy,t2);
J(2,3)=diff(Posy,t3);
J(2,4)=diff(Posy,t4);
J(2,5)=diff(Posy,t5);
J(2,6)=diff(Posy,t6);

%Iteracion 5
U5=simplify(T56*T45*T34*T23*T12*T01*U0);