UNIVERSIDAD TECNOLOGICA DEL PERÚ

FACULTAD DE INGENIERÍA SISTEMA Y ELECTRICA

LABORATORIO N° 02

ROBÓTICA I

Informe : Cinemática de robot con 4 grado de libertad

Profesor : Machuca Mines, José

Integrantes:
1) Chipana Quispe, Hugo
2) Guillermo Ricra, Jerson
3) Salvatierra Juro, Elvis

CICLO 2016-III
 COMANDO DE MATLAB

clc
close all
clear all

%Datos cinematicos del robot

d1=0.80;
d2=0.60;
d3=0.70;
d4=0.45;

%Datos
q1i=-45; %limite angular inicial
q1f=135; %limite angular final
dq1i=9;
dq1f=12;

q2i=-90; % limite angular inicial

q2f=90; % limite angular final
dq2i=4;
dq2f=2;

q3i=-135; % limite angular inicial

q3f=45; % limite angular final
dq3i=9;
dq3f=12;

q4i=-180; % limite angular inicial

q4f=0; % limite angular final
dq4i=4;
dq4f=12;

ti=0; %tiempo inicial en segundos

tf=10; %tiempo final en segundos
%MATRICES CORRESPONDIENTES A CADA PAR ARTICULACIÓN-ESLABÓN
% TENIENDO EN CUENTA LA TABLA ANTERIOR
syms L1 L2 L4 T1 T3 q1p q2p q3p q12p q22p q32p
M1=[cos(T1) 0 -sin(T1) 0;
sin(T1) 0 cos(T1) 0;
0 -1 0 L1;
0 0 0 1];

M2=[0 0 1 0;
-1 0 0 0;
0 -1 0 L2;
0 0 0 1];

M3=[cos(T3) 0 sin(T3) 0;
sin(T3) 0 -cos(T3) 0;
0 1 0 0;
0 0 0 1];

M4=[1 0 0 0;
0 1 0 0;
0 0 1 L4;
0 0 0 1];

%MT=MATRICE RESULTANTE

MT=M1*M2*M3*M4

t=ti:0.001:tf;
A=[ti^3 ti^2 ti 1;3*ti^2 2*ti 1 0; tf^3 tf^2 tf 1;3*tf^2 2*tf 1 0]
B=[q1i;dq1i;q1f;dq1f]
X=inv(A)*B
a1=X(1); b1=X(2); c1=X(3); d1=X(4);
q1t=(a1*t.^3+b1*t.^2+c1*t+d1); %posicion
dq1t=(3*a1*t.^2+2*b1*t+c1); %velocidad
ddq1t=(6*a1*t+2*b1); %aceleracion

A=[ti^3 ti^2 ti 1;3*ti^2 2*ti 1 0; tf^3 tf^2 tf 1;3*tf^2 2*tf 1 0]

B=[q2i;dq2i;q2f;dq2f]
X=inv(A)*B
a2=X(1); b2=X(2); c2=X(3); d2=X(4);
q2t=(a2*t.^3+b2*t.^2+c2*t+d2); %posicion
dq2t=(3*a2*t.^2+2*b2*t+c2); %velocidad
ddq2t=(6*a2*t+2*b2); %aceleracion
A=[ti^3 ti^2 ti 1;3*ti^2 2*ti 1 0; tf^3 tf^2 tf 1;3*tf^2 2*tf 1 0]
B=[q3i;dq3i;q3f;dq3f]
X=inv(A)*B
a3=X(1); b3=X(2); c3=X(3); d3=X(4);
q3t=(a3*t.^3+b3*t.^2+c3*t+d3); %posicion
dq3t=(3*a3*t.^2+2*b3*t+c3); %velocidad
ddq3t=(6*a3*t+2*b3); %aceleracion

A=[ti^3 ti^2 ti 1;3*ti^2 2*ti 1 0; tf^3 tf^2 tf 1;3*tf^2 2*tf 1 0]

B=[q4i;dq4i;q4f;dq4f]
X=inv(A)*B
a4=X(1); b4=X(2); c4=X(3); d4=X(4);
q4t=(a4*t.^3+b4*t.^2+c4*t+d4); %posicion
dq4t=(3*a4*t.^2+2*b4*t+c4); %velocidad
ddq4t=(6*a4*t+2*b4); %aceleracion

figure
%Posicion articular
subplot(411)
plot(t,q1t)
subplot(412)
plot(t,q2t)
subplot(413)
plot(t,q3t)
subplot(414)
plot(t,q4t)
figure

%Velocidad articular
subplot(411)
plot(t,dq1t)
subplot(412)
plot(t,dq2t)
subplot(413)
plot(t,dq3t)
subplot(414)
plot(t,dq4t)
figure
%Aceleracion narticular
subplot(411)
plot(t,ddq1t)
subplot(412)
plot(t,ddq2t)
subplot(413)
plot(t,ddq3t)
subplot(414)
plot(t,ddq4t)
%Posicion LIneal Cartesiana
Px=-d4*cosd(q3t).*sind(q1t)-d2*cosd(q1t);
Py=d4*cosd(q1t).*cosd(q3t)+d2*cosd(q1t);
Pz=d4*sind(q3t);
figure
subplot(311)
plot(t,Px)
subplot(312)
plot(t,Py)
subplot(313)
plot(t,Pz)
figure
plot3(Px,Py,Pz)
figure
comet3(Px,Py,Pz)

POSICION ARTICULAR
 VELOCIDAD ARTICULADA

ACELERACION ARTICULADA
POSICION LINEAL CARTESIANA (3D)