CINEMTICA DEL ROBOT PCD CUATERNIOS

I. OBJETIVO
I.1. Resolver el problema cinemtico directo de robots articulares por
medio de cuaternios.
II. MARCO TEORICO
II.1. Modelo Cinemtico
PCD: Mtodos Geomtrico y Por Cambios de Base (Desarrollar un breve
resumen terico del tema. Presentarlo en el Informe Final)
III. MATERIAL Y EQUIPO
III.1.Una Hoja , Una PC con SO Windows XP, MATLAB.
IV. PROCEDIMIENTO
IV.1 La figura siguiente representa la estructura de un robot plano de 4
GDL.
Resolver el problema cinemtico directo de este robot, es decir, obtener
x=f1(q1,q4,l1,l4,d1,d3), y=f2(q1,q4,l1,l4,d1,d3) y =f3(q1,q4,l1,l4,d1,d3),
por el mtodo cuaternios.

S0 S1: T(z,l1) Rot(z,q1)

S1 S2: T(z,d2) Rot(z,90) Rot(x,90)
S2 S3: T(z,d3)
S3 S4: T(z,l4) Rot(z,q4)
Desplazamientos:

p1=(0,0,l1)
p2=(0,0,d2)
p3=(0,0,d3)
p4=(0,0,l4)

Donde:

^i=cos qi
C
2

( )
qi
S^i=sen ( )
2

Giros:

^
^
Q1=( C 1 , 0,0, S 1 )

^ ^
Q2x=( C 2 x , S 2 x , 0,0 )=(0.707 , 0.707, 0 , 0)

^
^
Q2z=( C 2 z , 0,0, S 2 z )=( 0.707 , 0 , 0 , 0.707)

^
^
Q4=( C 4 , 0,0, S 4 )

Aplicando las ecuaciones de uso de cuaternios

(0,a0)=Q1(0,a1)

Q1*+(0,p1)

R0=Q1R1

(0,a1)=Q2x[Q2z (0,a2)

R1=Q2R2
(0,a2)=(0,a3)+(0,p3)
R2=R3

(0,a3)=Q4(0,a4)

Q2z*] Q2x* +(0,p2)

Q4*+(0,p4)

R3=Q4R4
Sustituyendo de manera consecutiva se tiene:
(0,a0)=Q1[Q2x[Q2z[Q4(0,a4)Q4*+(0,p4)+(0,p3)Q2z*]Q2x*+(0,p2)]+(0,p1)
(0,a0)=Q1Q2xQ2zQ4(0,a4)Q4*Q2z*Q2x*Q1*+Q1Q2xQ2z(0,p4+p3)Q2z*Q2x*Q1*+
Q1(0,p2)Q1*+(0,p1)
(0,a0)=Q1

clear all
clc
syms q1;
q2=pi/2;
q3=pi/2;
syms q4
syms ax
syms ay
syms az
syms l1
syms d2
syms d3
syms l4

(0,a4)Q*1

2x 2z 4

2x 2z 4

+Q1 2x 2z(0,p4+p3)Q*1 2x 2z +Q1(0,p2)Q1*+(0,p1)

Q1=[cos(q1/2) 0 0 sin(q1/2)];
Q2x=[cos(q2/2) sin(q2/2) 0 0];
Q2z=[cos(q3/2) 0 0 sin(q3/2)];
Q4=[cos(q4/2) 0 0 sin(q4/2)];
P1=[0 0 0 l1];
P2=[0 0 0 d2];
P3=[0 0 0 d3];
P4=[0 0 0 l4];
Qa=[0,ax ay az];
Qb=P4+P3;
Q12x0=Q1(1,1)*Q2x(1,1)(Q1(1,2)*Q2x(1,2)+Q1(1,3)*Q2x(1,3)+Q1(1,4)*Q2x(1,4));
Q12x1=Q1(1,1)*Q2x(1,2)+(Q1(1,2)*Q2x(1,1)+Q1(1,3)*Q2x(1,4)Q1(1,4)*Q2x(1,3));
Q12x2=Q1(1,1)*Q2x(1,3)+(Q1(1,3)*Q2x(1,1)+Q1(1,4)*Q2x(1,2)Q1(1,2)*Q2x(1,4));
Q12x3=Q1(1,1)*Q2x(1,4)+(Q1(1,4)*Q2x(1,1)+Q1(1,2)*Q2x(1,3)Q1(1,3)*Q2x(1,2));
Q12x=[Q12x0 Q12x1 Q12x2 Q12x3];
Q12x2z0=Q12x(1,1)*Q2z(1,1)(Q12x(1,2)*Q2z(1,2)+Q12x(1,3)*Q2z(1,3)+Q12x(1,4)*Q2z(1,4));
Q12x2z1=Q12x(1,1)*Q2z(1,2)+(Q12x(1,2)*Q2z(1,1)+Q12x(1,3)*Q2z(1,4)Q12x(1,4)*Q2z(1,3));
Q12x2z2=Q12x(1,1)*Q2z(1,3)+(Q12x(1,3)*Q2z(1,1)+Q12x(1,4)*Q2z(1,2)Q12x(1,2)*Q2z(1,4));
Q12x2z3=Q12x(1,1)*Q2z(1,4)+(Q12x(1,4)*Q2z(1,1)+Q12x(1,2)*Q2z(1,3)Q12x(1,3)*Q2z(1,2));
Q12x2z=[Q12x2z0 Q12x2z1 Q12x2z2 Q12x2z3];
Q12x2z40=Q12x2z(1,1)*Q4(1,1)(Q12x2z(1,2)*Q4(1,2)+Q12x2z(1,3)*Q4(1,3)+Q12x2z(1,4)*Q4(1,4));
Q12x2z41=Q12x2z(1,1)*Q4(1,2)+(Q12x2z(1,2)*Q4(1,1)+Q12x2z(1,3)*Q4(1,4)Q12x2z(1,4)*Q4(1,3));
Q12x2z42=Q12x2z(1,1)*Q4(1,3)+(Q12x2z(1,3)*Q4(1,1)+Q12x2z(1,4)*Q4(1,2)Q12x2z(1,2)*Q4(1,4));
Q12x2z43=Q12x2z(1,1)*Q4(1,4)+(Q12x2z(1,4)*Q4(1,1)+Q12x2z(1,2)*Q4(1,3)Q12x2z(1,3)*Q4(1,2));
Q12x2z4=[Q12x2z40 Q12x2z41 Q12x2z42 Q12x2z43];
%%
Q12x2z4a0=Q12x2z4(1,1)*Qa(1,1)(Q12x2z4(1,2)*Qa(1,2)+Q12x2z4(1,3)*Qa(1,3)+Q12x2z4(1,4)*Qa(1,4));
Q12x2z4a1=Q12x2z4(1,1)*Qa(1,2)+
(Q12x2z4(1,2)*Qa(1,1)+Q12x2z4(1,3)*Qa(1,4)-Q12x2z4(1,4)*Qa(1,3));
Q12x2z4a2=Q12x2z4(1,1)*Qa(1,3)+
(Q12x2z4(1,3)*Qa(1,1)+Q12x2z4(1,4)*Qa(1,2)-Q12x2z4(1,2)*Qa(1,4));
Q12x2z4a3=Q12x2z4(1,1)*Qa(1,4)+
(Q12x2z4(1,4)*Qa(1,1)+Q12x2z4(1,2)*Qa(1,3)-Q12x2z4(1,3)*Qa(1,2));
Q12x2z4a=[Q12x2z4a0 Q12x2z4a1 Q12x2z4a2 Q12x2z4a3];
Q12x2z4aQ0=Q12x2z4a(1,1)*Q12x2z4(1,1)-(Q12x2z4a(1,2)*Q12x2z4(1,2)+Q12x2z4a(1,3)*-Q12x2z4(1,3)+Q12x2z4a(1,4)*-Q12x2z4(1,4));
Q12x2z4aQ1=Q12x2z4a(1,1)*-Q12x2z4(1,2)+
(Q12x2z4a(1,2)*Q12x2z4(1,1)+Q12x2z4a(1,3)*-Q12x2z4(1,4)Q12x2z4a(1,4)*-Q12x2z4(1,3));
Q12x2z4aQ2=Q12x2z4a(1,1)*-Q12x2z4(1,3)+
(Q12x2z4a(1,3)*Q12x2z4(1,1)+Q12x2z4a(1,4)*-Q12x2z4(1,2)Q12x2z4a(1,2)*-Q12x2z4(1,4));
Q12x2z4aQ3=Q12x2z4a(1,1)*-Q12x2z4(1,4)+
(Q12x2z4a(1,4)*Q12x2z4(1,1)+Q12x2z4a(1,2)*-Q12x2z4(1,3)Q12x2z4a(1,3)*-Q12x2z4(1,2));
Q1234aQ=[Q12x2z4aQ0 Q12x2z4aQ1 Q12x2z4aQ2 Q12x2z4aQ3]
%%

Q12x2zb0=Q12x2z(1,1)*Qb(1,1)(Q12x2z(1,2)*Qb(1,2)+Q12x2z(1,3)*Qb(1,3)+Q12x2z(1,4)*Qb(1,4));
Q12x2zb1=Q12x2z(1,1)*Qb(1,2)+(Q12x2z(1,2)*Qb(1,1)+Q12x2z(1,3)*Qb(1,4)Q12x2z(1,4)*Qb(1,3));
Q12x2zb2=Q12x2z(1,1)*Qb(1,3)+(Q12x2z(1,3)*Qb(1,1)+Q12x2z(1,4)*Qb(1,2)Q12x2z(1,2)*Qb(1,4));
Q12x2zb3=Q12x2z(1,1)*Qb(1,4)+(Q12x2z(1,4)*Qb(1,1)+Q12x2z(1,2)*Qb(1,3)Q12x2z(1,3)*Qb(1,2));
Q12x2zb=[Q12x2zb0 Q12x2zb1 Q12x2zb2 Q12x2zb3];
Q12x2zbQ0=Q12x2zb(1,1)*Q12x2z(1,1)-(Q12x2zb(1,2)*Q12x2z(1,2)+Q12x2zb(1,3)*-Q12x2z(1,3)+Q12x2zb(1,4)*-Q12x2z(1,4));
Q12x2zbQ1=Q12x2zb(1,1)*-Q12x2z(1,2)+
(Q12x2zb(1,2)*Q12x2z(1,1)+Q12x2zb(1,3)*-Q12x2z(1,4)-Q12x2zb(1,4)*Q12x2z(1,3));
Q12x2zbQ2=Q12x2zb(1,1)*-Q12x2z(1,3)+
(Q12x2zb(1,3)*Q12x2z(1,1)+Q12x2zb(1,4)*-Q12x2z(1,2)-Q12x2zb(1,2)*Q12x2z(1,4));
Q12x2zbQ3=Q12x2zb(1,1)*-Q12x2z(1,4)+
(Q12x2zb(1,4)*Q12x2z(1,1)+Q12x2zb(1,2)*-Q12x2z(1,3)-Q12x2zb(1,3)*Q12x2z(1,2));
Q12x2zbQ=[Q12x2zbQ0 Q12x2zbQ1 Q12x2zbQ2 Q12x2zbQ3]
%%
% ax=0;
% ay=0;
% az=0;