PLANIFICACIN DE TRAYECTORIAS

ndice

Qu es una trayectoria?
Tipos de trayectorias
Punto a punto
Coordinadas
Continuas
Trayectorias en el espacio articular:
Lineal
Cbica
Parablica
A tramos

1-2-1
4-3-4

Planificacin de trayectorias

Objetivo: dado el

punto inicial del robot,

qu camino debe
seguir para llegar a su
posicin final?

Problema: en todo

momento debe cumplir

unas limitaciones:
Cinemticas: rango
de las articulaciones
Dinmicas:
velocidades y
aceleraciones
mximas

Generacin de trayectorias (I)

1.- Dar el punto inicial y final de la trayectoria (en coordenadas

cartesianas o generalizadas).
2.- Muestrear la trayectoria cartesiana obteniendo un nmero
finito de puntos en esa trayectoria (x, y, z, , , ).
3.- Utilizando la transformacin inversa, convertir cada punto
en sus correspondientes coordenadas articulares (q1, q2, q3, q4,
q5, q6)
4.- Interpolar entre los puntos articulares obtenidos,
generando una trayectoria en funcin del tiempo para cada
variable articular: qi(t), que sea realizable por los actuadores.
5.- Si est bien hecho, esta trayectoria se aproximar a la
deseada en el plano cartesiano.

Generacin de trayectorias (II)

Dos formas de solucionar el

problema:
Coordenadas cartesianas.
Ventaja: movimiento real en
las tres dimensiones, puede
establecerse ligaduras del
entorno.
Desventaja: necesidad de
resolver repetidamente la
transformacin homognea
inversa
Coordenadas generalizadas.

Ventaja: las ligaduras

dinmicas se plantean en las
variables generalizadas.

Tipos de trayectorias (I)

Trayectoria punto a punto: cada articulacin se mueve sin

considerar el estado o evolucin de las dems articulaciones.

Movimiento eje a eje

Cada vez se mueve un eje

El tiempo total es la suma de los tiempos de
cada articulacin.

Tipos de trayectorias (II)

Movimiento simultneo de ejes

Comienzan simultneamente todos los ejes
El tiempo final ser el de aquella articulacin que tarde
ms tiempo en finalizar su movimiento.

Tipos de trayectorias (III)

Trayectorias coordinadas (iscronas)

Se plantea un movimiento simultneo, ralentizando las
articulaciones ms rpidas, para que todas tarden el mismo
tiempo en acaba el movimiento.

Trayectorias continuas
Se fija explcitamente en coordenadas cartesianas el camino
que tiene que seguir el extremo (infinitos puntos)
El movimiento de las articulaciones puede parecer errtico.

Interpolacin de trayectorias (I)

Muchas veces es necesario generar trayectorias no slo con un

punto inicial y final, sino que tambin se impone que pase por
determinados puntos:
Camino continuo
Evitacin de obstculos
Suavizado de la trayectoria, etc.
Solucin: seleccionar algn tipo de funcin polinmica (spline)
cuyos coeficientes se ajustan para pasar por los puntos
deseados (con velocidades y aceleraciones aceptables)

Interpolacin de trayectorias (II)

Interpolacin lineal:
q(t) = a*t + b

Condiciones:
q(ti-1) = a * ti-1 + b = qi-1
q(ti) = a * ti + b = qi
Se despeja a y b y se obtiene:

Problema: la velocidad cambia

bruscamente y la aceleracin
es infinita

Interpolacin de trayectorias (III)

Para asegurar la continuidad en la velocidad se aproxima por

una funcin cbica.
Condiciones (4): posicin y velocidad en el punto inicial y final
Frmula de interpolacin:

Interpolacin de trayectorias (IV)

Interpolacin parablica: el paso

por los puntos intermedios se
planifica para evitar cambios
bruscos: en vez de pasar por el
punto se pasa tan cerca de l como
lo permita la aceleracin mxima.
Frmula de interpolacin:

Interpolacin de trayectorias (V)

Condiciones de funcionamiento:
1 tramo: q1(t) = a + b*t

q1(t=T1) = a + b*T1 = q1
q1(t=0) = q0 = a
Ecuacin: q1(t) = q0 + (q1 q0) / T1 * t

3 tramo: q3(t) = a1 + b1 * (t T1)

q3(t=T1) = q1 = a
q3(t=T1+T2) = q2 = a1 + b1*T2
1
Ecuacin: q3(t) = q1 + (q2 q1) / T2 * (t T1)
2 tramo: q2(t) = a2 + b2 (t- (T1- )) + c2 (t T1 + )2

q& (T1 ) = b = b 2 + 2c 2 (T1 T1 + )

q& (T1 + ) = b1 = b2 + 2c2 (T1 + T1 + )

( q1 q0 )
b2 =
T1
( q 2 q1 )T1 ( q0 q1 )T2
c2 =
4T1T2

q1 q0
(T1 ) = a 2
q1 (T1 ) = q 2 (T1 ) = q +
T1
0

Interpolacin de trayectorias (VI)

Si se desea asignar posiciones, velocidades y aceleraciones del punto

inicial y del punto final (6 condiciones) => polinomio de orden 5
Si se desea pasar por puntos intermedios, se tienen ms condiciones:
Inicial: posicin, velocidad y aceleracin
Despegue: posicin
Asentamiento: posicin
Final: Posicin, velocidad
y aceleracin
8 condiciones =>
Polinomio de orden 7

qi(t) = a7t7 + a6 t6 + a5t5 + a4 t 4 + a3t3 + a2t2 + a1t1 + a0

Interpolacin de trayectorias (VI)

Para conseguir transiciones suaves se necesitaran rdenes del

polinomio elevados, pero si el orden es alto se producen
comportamientos errticos en puntos intermedios:
Solucin: interpolacin a tramos.
Interpolacin ms sencilla (2-1-2):
Tramos de despegue y asentamiento a mxima aceleracin
Tramo intermedio a mxima velocidad
Son deseables transiciones suaves por lo que suelen usarse en cada
segmento splines de orden 3, 4 o 5
Interpolacin ms usada: 4-3-4
Otras posibilidades: 3-3-3, 5-5-5, 3-5-3, etc.

Interpolacin a tramos (I)

Interpolacin 2-1-2:
Frmula de interpolacin:

donde V es la velocidad, y a la
aceleracin mxima permitida.

Interpolacin a tramos (II)

Trayectoria 4-3-4
Los segmentos se ajustan de forma que en los puntos de cambio
(despegue y asentamiento) no cambie ni la velocidad ni la
aceleracin.
Primer segmento: orden 4
Segundo segmento: 3
Tercer segmento: 4

qi(t) = h1(t) = a14t4 + a13t3 + a12t2 + a11t + a10

qi(t) = h2 (t) = a23t3 + a22t2 + a21t + a20
qi(t) = h3(t) = a34t4 + a33t3 + a32t2 + a31t + a30

Interpolacin a tramos (III)

1.- Se normaliza cada segmento para que corresponda al intervalo

t [0 1]
i 1
Tiempo: t =

Velocidad: q&i (t ) =

i i 1

dhi (t ) dhi (t ) dt
1 dhi (t )
*
=
=
d
dt
d i i 1 dt

3
2
1 segmento: q& ( t ) = 4 a14 t + 3a13 t + 2 a12 t + a11

t1

1
d 2hi (t ) d 2hi (t ) dt
d 2hi (t )
&&i (t ) =
*
=
=
Aceleracin: q
2
2
d
dt
d ( i i 1 ) 2 dt 2

1 segmento:

12 a14 t 2 + 6 a13 t + 2 a12

q&&( t ) =
2
t1

Interpolacin a tramos (III)

Condiciones:
1 segmento:
0
1.- h1(t=0) = q = a10
0
0
2.- v1(t=0) = v = a11 / t1 => a11 = v t1
0
0
2 => a
2
3.- a1(t=0) = a = 2*a21 / t1
21 = a * t1 /2
1
4.- h1(t=1) = q = a14 + a13 + a12 + a11 + a10 =>
a14 + a13 = q1 q0 v0 t1 a0 t12 /2
1 y 2 segmento:
5.- Continuidad en la velocidad:
4 a 14 + 3 a 13 + 2 a 12 + a 11
h& 1 ( 1 )
h& 2 ( 0 )
a 21
=
=
=
t1
t2
t1
t2
4
3
1
a 14 +
a 13
a 21 = a 0 t 1 v 0
t1
t1
t2

Interpolacin a tramos (IV)

6.- Continuidad en al aceleracin:

&&i,1(1) = q
&&i,2 (0)
q

12a14 + 6a13 + 2a12

12
6
2
a
+
a

a
14
13
t 12
t 12
t 22

2
t1
22

= a

2a22
t2
2

2 segmento:
1
7.- h2(t=0) = q = a20
2
2
1
8.- h2 (t=1) = q = a23+a22+a21+a20 => a23 + a22+a21 = q q
3er segmento (cambio de variable): t [0,1] => t [ 1,0] => t = t 1
3 = a
9.- h3( t=0) = q
30
3
10.- v3(t =0) = v = a31 / t3
=> a31 = v3 t3
3
3 2
2 => a
11.- a3 (t =0) = a = 2 a32 / t3
32 = a t3 / 2

Interpolacin a tramos (V)

2 y 3er segmento:
12.- Continuidad en la velocidad:

h& 2 (1) h& 3 ( 1) 3a 23 + 2a 22 + a 21 4a 34 + 3a 33 2a 32 + a 31

=
=
=
t2
t3
t2
t3
4
3
3
2
1
a 34 a 33 + a 23 + a 22 + a 21 = v 3 a 3 t 3
t3
t3
t2
t2
t2

13.- Continuidad en la aceleracin

12a34 6a33 + 2a32 6a23 + 2a22
=
t32
t22
6
6
2
+ 2 a 33 + 2 a 23 + 2 a 22 = a 3
t3
t2
t2

q&&2 (1) = q&&3 ( 1)

12
a 34
2
t3

14.- h3(-1) = q2 = a34 - a33 + a32 a31 + a30 =>

a33 a34 = q3 q2 v3 t3 + a3 t32 / 2

Interpolacin a tramos (V)

1
0
0
0
0
0 a14
1

0
0
0
0 a13
4 / t1 3/ t1 1/ t2
2
12/ t 2 6 / t 2

0
2
/
0
0
0 a21
t
1
1
2

0
1
1
1
0
0 a22 =
0
0
0
1/ t2
2 / t2 3/ t2 3/ t3 4 / t3 a23

2
2
2
2
0
0
2 / t2 6 / t2 6 / t3 12/ t3 a33
0
0
1 a34
0
0
0
0
1

y = Cx

x = C 1 y

2
1
t
o
0
0
1
q q v t a

2
0
0

a t1 v

a0

2
1

q
q

3
3

v a t3

3
a

2
3
2
3
3 t3
q q v t3 a

Interpolacin a tramos (VI)

Una vez resuelto el problema, en el segmento 3 debemos

deshacer el cambio de variable, y sustituir t = t 1
en la
ecuacin del tercer segmento:
qi ( t ) = h3 ( t ) = a 34 t 4 + a 33 t 3 + a 32 t 2 + a 31t + a 30
qi ( t ) = h3 ( t ) = a 34 ( t 1) 4 + a 33 ( t 1)3 + a 32 ( t 1) 2 + a 31( t 1) + a 30

h3 ( t ) = a 34 ( t 4 4t 3 + 6t 2 4t + 1) + a 33 ( t 3 3t 2 + 3t 1) + a 32 ( t 2 2t + 1) + a 31( t 1) + a 30

h3 ( t ) = a 34 t 4 + ( 4a 34 + a 33 )t 3 + (6a 34 3a 33 + a 32 )t 2 + ( 4a 34 + 3a 33 2a 32 + a 31 )t +
(a 34 a 33 + a 32 a 31 + a 30 )

Resumen

Qu es una trayectoria?
Pasos a seguir para generar una trayectoria
Tipos de trayectorias
Cmo se calculan las trayectorias? => depende de las
condiciones que nos impongan
Slo posicin: lineal
Posicin y velocidad: cbica
Posicin, velocidad y aceleracin:

trayectoria de 5 orden
O una trayectoria a tramos fijando dos puntos
intermedios por lo que queremos pasar y asegurando
continuidad en la velocidad (2-1-2), si queremos
asegurar tambin continuidad en la aceleracin: (4-3-4),
(3-3-3), (5-3-5) etc.

Bibliografa

Barrientos: explica las trayectorias, los tipos de trayectorias y

los polinomios pero sin deducirles.
Torres: habla de la interpolacin cbica, y la interpolacin
lineal con ajuste parablico => deducen los valores de cada
parmetro y pone ejemplos
Fu: explica con detalle las ecuaciones de la trayectoria 4-3-4,
la 3-5-3 y la 3-3-3-3-3