Documentos de Académico
Documentos de Profesional
Documentos de Cultura
Práctica 5
Planificación de Trayectorias
5.1.-Introducción
Una vez obtenidos los modelos cinemáticos y dinámicos del robot se puede
abordar el problema del control de los mismos. Definir el movimiento de un robot
implica controlar dicho robot de manera que siga un camino preplanificado. El objetivo
es por tanto establecer cuales son las trayectorias que debe seguir cada articulación del
robot a lo largo del tiempo para conseguir los objetivos fijados, a la vez que se exige
cumplir una serie de restricciones físicas impuestas por los actuadores y de calidad de la
trayectoria, como son suavidad, precisión, etc.
Para un estudio más estructurado del problema de control del robot, éste suele
dividirse en dos bloques:
Esta práctica aborda el control cinemático del robot, revisando los conceptos de
planificación de trayectorias y las herramientas necesarias para la interpolación
polinomial entre los puntos de inicio y final del camino. En primer lugar se presenta un
esquema general del problema de la planificación, más tarde se distingue entre el
espacio cartesiano y el espacio de las articulaciones del robot para poder abordar en
Práctica 5 .- Pág. 1
Prácticas de Robótica utilizando Matlab ®
Hay que hacer notar que el paso 3 debe tratarse con cuidado, pues hay que tener
en cuenta las posibles soluciones múltiples de la transformación inversa, como se vio en
el ejemplo 2.6 de la práctica 2, así como la posible existencia de configuraciones
singulares que impidan la continuidad de la trayectoria deseada.
Práctica 5 .- Pág. 2
Prácticas de Robótica utilizando Matlab ®
Necesidades de movimiento
(libre de colisiones)
Coordenadas cartesianas
(x,y,z,α,β,γ)
• Restricciones de los
actuadores. GENERADOR DE
TRAYECTORIAS
• Modelo cinemático.
Trayectorias articulares
q i (t ), q i (t ), qi (t )
Práctica 5 .- Pág. 3
Prácticas de Robótica utilizando Matlab ®
5.3.-Tipos de Trayectorias.
La mejora tecnológica ha permitido que los robots puedan realizar trayectorias
cada vez más complejas, al poder ser éstas calculadas previamente. A continuación se
cita brevemente una clasificación de tipos de trayectorias de robots comerciales
clásicos.
Práctica 5 .- Pág. 4
Prácticas de Robótica utilizando Matlab ®
• Trayectorias continuas.
En este tipo de trayectorias se pretende que el camino seguido por el extremo del
robot sea conocido. Para ello las trayectorias articulares deben acomodarse
conjuntamente. Cada articulación por separado parece tener un movimiento
desordenado, sin embargo el resultado es que el extremo se mueve siguiendo el
camino previsto.
En segundo lugar, debe hallarse un polinomio interpolador entre los dos puntos
nudos que cumpla con las especificaciones de los actuadores y que proporcione un
movimiento suave al robot. Como se ha visto anteriormente, este punto puede ser
abordado bajo varios enfoques y diferentes tipos de trayectorias. Cada enfoque produce
un resultado distinto pero igualmente válido dependiendo de los requerimientos del
usuario. Se puede pretender que el robot siga una determinada trayectoria en línea recta
o mover el manipulador a lo largo de una trayectoria polinomial uniforme que satisface
las ligaduras de posición y orientación en ambos puntos extremos.
Práctica 5 .- Pág. 5
Prácticas de Robótica utilizando Matlab ®
Con el objetivo de que el movimiento del robot sea suave y esté de acuerdo con
las restricciones de los actuadores se divide cada trayectoria articular en tres segmentos,
correspondientes a la aceleración del motor, el periodo de velocidad máxima y la
deceleración o frenado del motor. La gráfica siguiente muestra esta división en la que se
puede observar como los periodos de aceleración y frenado ofrecen un movimiento
suave del robot.
Posición Articulación i
θ(t3)
θ(t2)
Aceleración
Frenado
Vel. Máx.
θ(t1)
θ(t0)
t0 t1 t2 t3
Tiempo
Las restricciones en los puntos de control que se utilizan para resolver el cálculo de los
polinomios interpoladores son las siguientes:
Práctica 5 .- Pág. 6
Prácticas de Robótica utilizando Matlab ®
h2 (t ) = a 23 t 3 + a 22 t 2 + a 21t + a 20 (5.3)
h3 (t ) = a 34 t 4 + a 33 t 3 + a 32 t 2 + a 31t + a 30 (5.4)
En primer lugar hay que considerar que al estar hallando las trayectorias
articulares para las N articulaciones del manipulador, es conveniente introducir una
variable de tiempo normalizada t∈[0,1], que permita simplificar los cálculos al evitar
tener que contabilizar los desfases temporales, de tal manera que t=0 corresponda al
inicio de cualquiera de los tres segmentos en que se ha dividido el camino y t=1
corresponda al final del segmento de trayectoria.
τ − τ i −1
t= (5.5)
τ − τ i −1
i
Donde:
1
vi = hi (t ) (5.6)
ti
1
ai = 2 hi (t ) (5.7)
ti
Práctica 5 .- Pág. 7
Prácticas de Robótica utilizando Matlab ®
Aplicando las condiciones de contorno para t=0 y t=1 para los puntos intermedios se
obtienen las siguientes relaciones:
Práctica 5 .- Pág. 8
Prácticas de Robótica utilizando Matlab ®
Para aplicar más cómodamente las condiciones de contorno conviene realizar un cambio
de variable, sustituyendo t = t − 1 por t en la ecuación (5.4), de tal manera que t = 0
corresponde al instante final del segmento y t = −1 al instante inicial. La ecuación que
gobierna este segmento pasa a formularse como:
= (5.28)
t3 t3
h3 (−1) 12 a34 − 6a33 + a f t3
2
= (5.29)
t32 t32
y aplicando continuidad:
Práctica 5 .- Pág. 9
Prácticas de Robótica utilizando Matlab ®
Otras relaciones
a0t12
δ 1 = θ1 − θ 0 = h1 (1) − h1 (0) = a14 + a13 + + v0t1 (5.33)
2
δ 2 = θ 2 − θ1 = h2 (1) − h2 (0) = a23 + a22 + a21 (5.34)
a f t32
δ 3 = θ 3 − θ 2 = h3 (0) − h3 (−1) = −a34 + a33 − + v f t3 (5.35)
2
Rescribiendo en notación matricial las ecuaciones (5.22), (5.23), (5.31), (5.32), (5.33),
(5.34), (5.35) se obtiene:
y = Cx (5.36)
donde:
T
⎛ a0 t1
2
a f t32 ⎞
y = ⎜ δ 1 − − v0t1 ,−a0t1 − v0 ,−a0 , δ 2 ,−a f t3 + v f , a f , δ 3 − − v f t3 ⎟
⎜ 2 2 ⎟
⎝ ⎠
T
x = (a13 , a14 , a21 , a22 , a23 , a33 , a34 )
⎡ 1 1 0 0 0 0 0 ⎤
⎢ 3 t 0 ⎥⎥
⎢ 1 4 t1 − 1 t 2 0 0 0
⎢6 t12 12 t12 0 − 2 t 22 0 0 0 ⎥
⎢ ⎥
C = ⎢ 0 0 1 1 1 0 0 ⎥
⎢ 0 0 1 t2 2 t2 3 t 2 − 3 t3 4 t3 ⎥
⎢ ⎥
⎢ 0 0 0 2 t 22 6 t 22 6 t32 − 12 t32 ⎥
⎢ 0 0 0 0 0 1 − 1 ⎥⎦
⎣
Por defecto se ha considerado que la articulación con una solicitud temporal más
exigente frenará al resto de actuadores, trazándose por tanto la trayectoria más corta
posible entre los puntos inicial y final. Para ello se ha escrito la función
SINCRONIZADOR que calcula la nueva velocidad máxima de cada motor necesaria
para sincronizar las trayectorias articulares. Conocidas las posiciones iniciales y finales
de las articulaciones y la velocidad máxima de los motores que las accionan, se
Práctica 5 .- Pág. 10
Prácticas de Robótica utilizando Matlab ®
function [velo2,tmaximo]=sincronizador(q0,qf,velo)
return
%---------------------------------------------------------------------
%Tiempos de respuesta del motor (aceleración y frenado)
%---------------------------------------------------------------------
ti = tmotor(elem,1);
tf = tmotor(elem,2);
%---------------------------------------------------------------------
%Cálculo de los coeficientes.
%---------------------------------------------------------------------
Práctica 5 .- Pág. 11
Prácticas de Robótica utilizando Matlab ®
if vel(elem) ~= 0
%-Determinacion del caso
desp = (qf(elem,1) - q0(elem,1));
ttot = abs(desp/vel(elem));
if ttot > (ti + tf)
caso = 1;
else
caso = 2;
end;
t = (ti + tf)/2;
tt = [ t t ];
A = zeros(2,5);
Práctica 5 .- Pág. 12
Prácticas de Robótica utilizando Matlab ®
A(2,2) = qf(elem,2)*tt(2);
A(2,3) = qf(elem,3)*tt(2)^2/2;
x = inv(B)*b;
A(2,4) = x(3);
A(2,5) = x(4);
end;
return
⇒ El lector debe notar la redefinición del segundo elemento del vector TT que
define los intervalos de tiempo de aceleración, velocidad máxima y frenado en el caso 1,
ajustando la aproximación que se había realizado con la variable TTOT.
Práctica 5 .- Pág. 13
Prácticas de Robótica utilizando Matlab ®
%---------------------------------------------------------------------
% Especificaciones de los tiempos de arranque y frenado de cada motor.
%---------------------------------------------------------------------
tmotor = 0.1*ones(6,2);
%---------------------------------------------------------------------
%Velocidades Maxima de cada motor.
%---------------------------------------------------------------------
velmax = [1.0472;1.0472;1.0472;1.0472;1.0472;1.0472];
%**********************planificador coordinado***********************
%---------------------------------------------------------------------
% Inicialización de los vectores posicion - Velocidad - aceleracion.
%---------------------------------------------------------------------
q = zeros(6,1);
q0 = [q1 q q];
qf = [q2 q q];
%---------------------------------------------------------------------
% Sincronización de los motores para que realizen un movimiento
coordinado
%---------------------------------------------------------------------
[velo2,tmaximo]=sincronizador(q0,qf,velmax);
%---------------------------------------------------------------------
%Inicialización de la escala de tiempo y las matrices.
%---------------------------------------------------------------------
t = 0:0.01:(tmaximo+0.15);
ini=zeros(length(t),1);
pos_plan(:,1)=ini;
vel_plan(:,1)=ini;
ace_plan(:,1)=ini;
%---------------------------------------------------------------------
Práctica 5 .- Pág. 14
Prácticas de Robótica utilizando Matlab ®
for i = 1:6
[caso,A,tt] = calculocoef(i,velo2,q0,qf,tmotor);
posi=evalpos(t,tt,caso,A);
pos_plan(:,i)=posi';
ve=evalvel(t,tt,caso,A);
vel_plan(:,i)=ve';
ace=evalacel(t,tt,caso,A);
ace_plan(:,i)=ace';
end;
return
for i = 1:length(t)
if caso == 1
if (t(i) <= 0)
p = A(1,1);
elseif (t(i)>0)&(t(i)<=tt(1))
ti = t(i)/tt(1);
p = A(1,1)+ A(1,2)*ti+ A(1,3)*ti^2+A(1,4)*ti^3+A(1,5)*ti^4;
elseif (t(i)>tt(1))&(t(i)<=tt(2)+tt(1))
ti = (t(i)-tt(1))/tt(2);
p = A(2,1)+A(2,2)*ti;
elseif (t(i)>tt(2)+tt(1))&(t(i)<=tt(3)+tt(2)+tt(1))
ti = (t(i)-tt(2)-tt(1))/tt(3);
p = A(3,1)+A(3,2)*(ti-1)+A(3,3)*(ti-1).^2+A(3,4)*(ti-
1).^3+A(3,5)*(ti-1).^4;
elseif (t(i)>tt(3)+tt(2)+tt(1))
p = A(3,1);
end;
elseif caso==2
if (t(i) <= 0)
p = A(1,1);
Práctica 5 .- Pág. 15
Prácticas de Robótica utilizando Matlab ®
elseif (t(i)>0)&(t(i)<=tt(1))
ti = t(i)/tt(1);
p = A(1,1)+ A(1,2)*ti+ A(1,3)*ti^2+A(1,4)*ti^3+A(1,5)*ti^4;
elseif (t(i)>tt(1))&(t(i)<=tt(2)+tt(1))
ti = (t(i)-tt(1))/tt(2);
p = A(2,1)+A(2,2)*(ti-1)+A(2,3)*(ti-1).^2+A(2,4)*(ti-
1).^3+A(2,5)*(ti-1).^4;
elseif (t(i)>tt(2)+tt(1))
p = A(2,1);
end;
end;
pos(i)=p;
end;
return
Ejemplo 5.1
%----------------------------------------------------------
%----programa de prueba del software de planificación -----
%---- interpolación 4-3-4 -----
%----------------------------------------------------------
clear
%---------------------------------------------------------------------
% q1 y q2 son las coordenadas articulares inicial y final
%---------------------------------------------------------------------
disp([' ']);
disp([' Vectores q1 y q2 de las coordenadas articulares ']);
disp([' inicial y final. ']);
q1 = rand(6,1)
q2 = rand(6,1)
%---------------------------------------------------------------------
% Llamada a la función PLANIFICADOR
%---------------------------------------------------------------------
Práctica 5 .- Pág. 16
Prácticas de Robótica utilizando Matlab ®
%---------------------------------------------------------------------
% gráficas de los resultados
%---------------------------------------------------------------------
figure
plot(t,pos)
grid
title('Perfil de posición dada por el planificador')
xlabel('Tiempo (seg)'), ylabel('Posición (rad)')
figure
plot(t,vel)
grid
title('Perfil de Velocidad dada por el planificador')
xlabel('Tiempo (seg)'), ylabel('Velocidad (rad/seg)')
figure
plot(t,ace)
grid
title('Perfil de Aceleracion Dada por el planificador')
xlabel('Tiempo (seg)'), ylabel('Aceleracion (rad/seg2)')
Práctica 5 .- Pág. 17
Prácticas de Robótica utilizando Matlab ®
Práctica 5 .- Pág. 18
Prácticas de Robótica utilizando Matlab ®
Práctica 5 .- Pág. 19
s
Prácticas de Robótica utilizando Matlab ®
a
n
Ejemplo 5.2
P1=(x1,y1,z1,α1,β1,γ1)
P2=(x2,y2,z2,α2,β2,γ2)
Práctica 5 .- Pág. 20
»
» [t,pos,
T1=[0 1 vel, ace] 0= 0planificador(q1,q2);
0 -0.3;1 -0.2;0 0 1 0.6; 0 0 0 1]
»
T1 = Prácticas de Robótica utilizando Matlab ®
0 1.0000 0 -0.3000
1.0000 0 0 -0.2000
0 El
0 siguiente
1.0000 ejemplo
0.6000 muestra como se ha utilizado la función
0 0
INVERSEKINEMATIC60 1.0000
para obtener los vectores q1 y q2 a partir de unas matrices
homogéneas
» T2=[0 0 1 -0.3;0 T1 y T20 que
1 0 0.4;1 definen
0 0.5; 0 0en0 coordenadas
1] cartesianas la posición y orientación
de dos puntos en el espacio de trabajo del robot.
T2 =
0 0 1.0000 -0.3000
0 1.0000 0 0.4000
1.0000 0 0 0.5000
0 0 0 1.0000
» q1=inversekinematic6(T1,-1,1)
q1 =
-2.5536
0.7137
-0.6729
0.0000
-0.0408
-0.9828
» q2=inversekinematic6(T2,-1,1)
q2 =
2.3306
0.6579
-0.2558
-2.2892
1.8437
-0.2990
Práctica 5 .- Pág. 21
» plot(t,pos)
» plot(t,vel)
» plot(t,ace)
Prácticas de Robótica utilizando Matlab ®
Práctica 5 .- Pág. 22
» animacion6(pos')
Práctica 5 .- Pág. 23