LABORATORIO DE ROBOTICA: DISEO DE TRAYECTORIAS DE UN MANIPULADOR

ROBOTICA




INTEGRANTES:
Jorge Armando Pinto Franco
Edwin Hernando Solano Araque










Universidad Autnoma de Bucaramanga
Programa de Ingeniera Mecatrnica
Bucaramanga - Santander



Objetivos


Encontrar el valor de aceleracin que permita a un manipulador industrial trazar una
trayectoria determinada en un tiempo determinado
Obtener los puntos en el espacio de la trayectoria que se desear que siga el
manipulador industrial.
Calcular los valores de los ngulos a lo largo del tiempo que permitan que el
manipulador siga la trayectoria deseada. Utilizando la funcin plot3, simule el
movimiento del brazo.




Marco Terico

El problema bsico a tratar es mover el manipulador desde una posicin inicial hasta
cierta posicin final deseada; esto es, deseamos mover la trama herramienta desde su
valor actual, hasta un valor final deseado. En general, esta movimiento implica tanto un
cambio en la orientacin como en la posicin de la herramienta relativa a la estacin.

Algunas veces es necesario especificar el movimiento con mucho ms detalle que el
necesario para slo declarar la configuracin final deseada. Una manera de incluir ms
detalle en una descripcin de ruta es proporcionar una secuencia de puntos va
deseados (puntos intermedios entre la posiciones inicial y final). Por lo tanto, al completar
el movimiento la trama herramienta debe pasar a travs de un conjunto de posiciones y
orientaciones intermedias, segn lo descrito por los puntos va. Cada uno de estos puntos
va es en realidad una trama que especifica tanto la posicin como la orientacin de la
herramienta en forma relativa a la estacin. El nombre puntos ruta incluye todos los
puntos va, junto con los puntos inicial y final. Recuerde que aunque utilizamos
generalmente el trmino puntos, estamos hablando en realidad de tramas que
proporcionan la posicin y la orientacin. Junto con estas restricciones espaciales del
movimiento, tal vez el usuario tambin desee especificar atributos temporales del
movimiento. Por ejemplo, el tiempo transcurrido entre los puntos va podra especificarse
en la descripcin de la ruta.


Codigo usado para hacer el manipulador en Matlab

%% Diseo de trayectorias de un manipulador
clear
clc

L1=181;
L2=128;
L3=281;
L4=44.78;
L5=184.45;
L6=151;
L7=68;

Largo_12=250;
distancia_x=300;
alura_base=300;
p1 = [distancia_x -Largo_12/2 alura_base];
p2 = [distancia_x Largo_12/2 alura_base];
p3 = [distancia_x Largo_12/2 alura_base+Largo_12];
p4 = [distancia_x 0 alura_base+Largo_12*(1+sind(60))];
p5 = [distancia_x -Largo_12/2 alura_base+Largo_12];

%% Clculo de los Movimientos Rectilneos - Trayectorias Cortas
dt=0.1;
d_t=0.001;

t1_=2;
t2_=6;
t3_=t1_+t2_;

t1=0:d_t:t1_-dt/2;
t12=t1_-dt/2+d_t/2:d_t:t1_+dt/2;
t2=t1_+dt/2:d_t:t2_-dt/2;
t23=t2_-dt/2+d_t/2:d_t:t2_+dt/2;
t3=t2_+dt/2:d_t:t3_;
t=[t1 t12 t2 t23 t3];

l=250;

a=l/(t1_*t2_) % Con aceleracin instantanea
a_=12*l/(12*t1_*t2_-dt^2) % Con aceleracin no instantnea


a_t=a_*[ones(1,length(t1)),zeros(1,length(t12))
,zeros(1,length(t2)),zeros(1,length(t23)),-ones(1,length(t3))];
a_t(length(t1)+1:length(t1)+length(t12))=-
a_/dt*t(length(t1)+1:length(t1)+length(t12)) +
(a_*(2*t1_+dt))/(2*dt)*ones(1,length(t12)); % Ecuaciones de los cambios
de aceleracin
a_t(length(t1)+length(t12)+length(t2)+1:length(t1)+length(t12)+length(t2)
+length(t23))=-
a_/dt*t(length(t1)+length(t12)+length(t2)+1:length(t1)+length(t12)+length
(t2)+length(t23)) + (a_*(2*t2_-dt))/(2*dt)*ones(1,length(t23));
v_t=cumtrapz(a_t)*d_t;
x_t=cumtrapz(v_t)*d_t;

figure(1)
plot(t,a_t)
figure(2)
plot(t,v_t)
figure(3)
plot(t,x_t)

%% Clculo de los Movimientos Rectilneos - Trayectorias Largo
L=sqrt(2*l^2);

a=L/(t1_*t2_) % Con aceleracin instantanea
a_L=12*L/(12*t1_*t2_-dt^2) % Con aceleracin no instantnea


a_t_L=a_L*[ones(1,length(t1)),zeros(1,length(t12))
,zeros(1,length(t2)),zeros(1,length(t23)),-ones(1,length(t3))];
a_t_L(length(t1)+1:length(t1)+length(t12))=-
a_L/dt*t(length(t1)+1:length(t1)+length(t12)) +
(a_L*(2*t1_+dt))/(2*dt)*ones(1,length(t12)); % Ecuaciones de los cambios
de aceleracin
a_t_L(length(t1)+length(t12)+length(t2)+1:length(t1)+length(t12)+length(t
2)+length(t23))=-
a_L/dt*t(length(t1)+length(t12)+length(t2)+1:length(t1)+length(t12)+lengt
h(t2)+length(t23)) + (a_L*(2*t2_-dt))/(2*dt)*ones(1,length(t23));
v_t_L=cumtrapz(a_t_L)*d_t;
x_t_L=cumtrapz(v_t_L)*d_t;

figure(1)
plot(t,a_t_L)
figure(2)
plot(t,v_t_L)
figure(3)
plot(t,x_t_L)

%% Clculo de los puntos de la trayectoria
band=0;
for i=1:dt/d_t:length(x_t)
band=band+1;
x_t_(band)=x_t(i);
end

band=0;
for i=1:dt/d_t:length(x_t_L)
band=band+1;
x_t_L_(band)=x_t_L(i);
end

for i=1:length(x_t_)
T_15(:,i)=p1+[0 0 x_t_(i)];
end

for i=1:length(x_t_)
T_53(:,i)=p5+[0 x_t_(i) 0];
end

for i=1:length(x_t_)
T_32(:,i)=p3+[0 0 -x_t_(i)];
end

for i=1:length(x_t_)
T_21(:,i)=p2+[0 -x_t_(i) 0];
end

for i=1:length(x_t_)
T_13(:,i)=p1+[0 x_t_L_(i)*cosd(45) x_t_L_(i)*sind(45)];
end

for i=1:length(x_t_)
T_34(:,i)=p3+[0 -x_t_(i)*cosd(60) x_t_(i)*sind(60)];
end

for i=1:length(x_t_)
T_45(:,i)=p4+[0 -x_t_(i)*cosd(60) -x_t_(i)*sind(60)];
end

for i=1:length(x_t_)
T_52(:,i)=p5+[0 x_t_L_(i)*cosd(45) -x_t_L_(i)*sind(45)];
end
T_total=[T_15 T_53 T_32 T_21 T_13 T_34 T_45 T_52]

plot3(T_total(1,:),T_total(2,:),T_total(3,:),'r')

%% Trayectoria
XMIN=-300
XMAX=300
YMIN=-300
YMAX=300
ZMIN=0
ZMAX=1000
fval=[0;0;0;0]
Puntos=[];
init=10*(1-0.5*rand(5,1))
for i=1:length(T_total)
Px=T_total(1,i);
Py=T_total(2,i);
Pz=T_total(3,i);

nuevo_punto=[Px;Py;Pz];
Puntos=[Puntos nuevo_punto];

%while sum(fval)<0.001
[x, fval]= fsolve(@(x) fun_robot(x, Px, Py, Pz, L1, L2, L3, L4
,L5 ,L6, L7),init);
%end
init=x;
angulos(:,i)=x;
% Prueba

end
%%
for i=1:length(T_total)
Px=T_total(1,i);
Py=T_total(2,i);
Pz=T_total(3,i);

nuevo_punto=[Px;Py;Pz];
Puntos=[Puntos nuevo_punto];

while sum(fval)<0.001
[x, fval]= fsolve(@(x) fun_robot(x, Px, Py, Pz, L1, L2, L3, L4
,L5 ,L6, L7),init);
end
init=x;
angulos(:,i)=x;
% Prueba
cla
A0=[1 0 0 0;
0 1 0 0;
0 0 1 0;
0 0 0 1];

A1 = MT(L1,'z');
A2 = MR(angulos(1,i),'z');
A3 = MT(L2,'z');
A4 = MR(angulos(2,i),'y');
A5 = MT(L3,'z');
A6 = MR(angulos(3,i),'y');
A7 = MT(L4,'z');
A8 = MT(L5,'x');
A9 = MR(angulos(4,i),'x');
A10 = MT(L6,'x');
A11 = MR(angulos(5,i),'y');
A12 = MT(L7,'x');

PF = A1*A2*A3*A4*A5*A6*A7*A8*A9*A10*A11*A12*A0;

% grafica
frame(A0,'r',1);
hold on;
grid on;
view(3);
Tant=A0;
T=A0*A1;;
frame(T,'r',1);
hold on;
grid on;
view(3);
plot3([Tant(1,4) T(1,4)],[Tant(2,4) T(2,4)],[Tant(3,4)
T(3,4)],'k','Linewidth',40);
axis ([XMIN XMAX YMIN YMAX ZMIN ZMAX]);

T1=T*A2;
frame(T1,'r',1);
hold on;
grid on;
view(3);
plot3([T(1,4) T1(1,4)],[T(2,4) T1(2,4)],[T(3,4)
T1(3,4)],'k','Linewidth',40);


T2=T1*A3;
frame(T2,'r',1);
hold on;
grid on;
view(3);
plot3([T1(1,4) T2(1,4)],[T1(2,4) T2(2,4)],[T1(3,4)
T2(3,4)],'k','Linewidth',22);


T3=T2*A4;
frame(T3,'r',1);
hold on;
grid on;
view(3);
plot3([T2(1,4) T3(1,4)],[T2(2,4) T3(2,4)],[T2(3,4)
T3(3,4)],'k','Linewidth',22);


T4=T3*A5;
frame(T4,'r',1);
hold on;
grid on;
view(3);
plot3([T3(1,4) T4(1,4)],[T3(2,4) T4(2,4)],[T3(3,4)
T4(3,4)],'k','Linewidth',22);


T5=T4*A6;
frame(T5,'r',1);
hold on;
grid on;
view(3);
plot3([T4(1,4) T5(1,4)],[T4(2,4) T5(2,4)],[T4(3,4)
T5(3,4)],'k','Linewidth',22);


T6=T5*A7;
frame(T6,'r',1);
hold on;
grid on;
view(3);
plot3([T5(1,4) T6(1,4)],[T5(2,4) T6(2,4)],[T5(3,4)
T6(3,4)],'k','Linewidth',22);


T7=T6*A8;
frame(T7,'r',1);
hold on;
grid on;
view(3);
plot3([T6(1,4) T7(1,4)],[T6(2,4) T7(2,4)],[T6(3,4)
T7(3,4)],'k','Linewidth',22);


T8=T7*A9;
frame(T8,'r',1);
hold on;
grid on;
view(3);
plot3([T7(1,4) T8(1,4)],[T7(2,4) T8(2,4)],[T7(3,4)
T8(3,4)],'k','Linewidth',22);


T9=T8*A10;
frame(T9,'r',1);
hold on;
grid on;
view(3);
plot3([T8(1,4) T9(1,4)],[T8(2,4) T9(2,4)],[T8(3,4)
T9(3,4)],'k','Linewidth',22);


T10=T9*A11;
frame(T10,'r',1);
hold on;
grid on;
view(3);
plot3([T9(1,4) T10(1,4)],[T9(2,4) T10(2,4)],[T9(3,4)
T10(3,4)],'k','Linewidth',22);


T11=T10*A12;
frame(T11,'r',1);
hold on;
grid on;
view(3);
plot3([T10(1,4) T11(1,4)],[T10(2,4) T11(2,4)],[T10(3,4)
T11(3,4)],'g','Linewidth',22);


hold on;
plot3(Puntos(1,:),Puntos(2,:),Puntos(3,:),'r*')

drawnow;
end

Resultados
En primer lugar, se definen los puntos de las esquinas de la trayectoria que se desea trazar

299
299.5
300
300.5
301
-200
-100
0
100
200
300
400
500
600
700
800
A continuacin, se determina los valores de aceleracin que permitan hacer los tramos largos y los
tramos cortos en el tiempo deseado.

Aceleracin en las trayectorias cortas


Velocidad en las trayectorias cortas


Cambio de posicin en las trayectorias cortas
0 1 2 3 4 5 6 7 8
-25
-20
-15
-10
-5
0
5
10
15
20
25
0 1 2 3 4 5 6 7 8
0
5
10
15
20
25
30
35
40
45
0 1 2 3 4 5 6 7 8
0
50
100
150
200
250
300

Aceleracin en las trayectorias largas


Velocidad en las trayectorias cortas


Cambio de posicin en las trayectorias cortas


0 1 2 3 4 5 6 7 8
-30
-20
-10
0
10
20
30
0 1 2 3 4 5 6 7 8
-10
0
10
20
30
40
50
60
0 1 2 3 4 5 6 7 8
0
50
100
150
200
250
300
350
400
Con los cambios de posicin calculados, determinamos la trayectoria que deseamos que realice la
herramienta del manipulador.


Conociendo la trayectoria que debe seguir la herramienta, calculamos los ngulos que debe tomar
el manipulador para hacer esto.
Valores 1









299
299.5
300
300.5
301
-200
-100
0
100
200
200
300
400
500
600
700
800
0 10 20 30 40 50 60 70
-30
-20
-10
0
10
20
30
Valores 2


Valores 3


Valores 4



0 10 20 30 40 50 60 70
-25
-20
-15
-10
-5
0
5
10
15
20
0 10 20 30 40 50 60 70
-20
-10
0
10
20
30
40
50
60
0 10 20 30 40 50 60 70
-100
-50
0
50
100
150
200
250
300
350
400
Valores 5


Finalmente, podemos simular el movimiento del manipulador mediante la funcin plot3:






0 10 20 30 40 50 60 70
-70
-60
-50
-40
-30
-20
-10



Conclusiones

Para realizar una trayectoria con un manipulador, puede calcularse una serie de
puntos de la herramienta, mediante la definicin de una aceleracin en cada
trayectoria.
El uso de mtodos numricos puede simplificar en forma notoria el clculo de
ngulos que debe tomar un manipulador de ms de tres grados de libertad.
Al trazar la trayectoria de una manipulador, es necesario tener en cuenta la
orientacin que la herramienta debe tomar.