PLANIFICACIÓN DE TRAYECTORIAS

Índice

„ ¿Qué es una trayectoria?

„ Tipos de trayectorias
„ Punto a punto

„ Coordinadas

„ Continuas

„ Trayectorias en el espacio articular:

„ Lineal

„ Cúbica

„ Parabólica

„ A tramos

„ 1-2-1
„ 4-3-4
 Planificación de trayectorias

„ Objetivo: dado el
punto inicial del robot,
¿qué camino debe
seguir para llegar a su
posición final?
„ Problema: en todo
momento debe cumplir
unas limitaciones:
„ Cinemáticas: rango

de las articulaciones
„ Dinámicas:

velocidades y
aceleraciones
máximas
 Generación 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 número
finito de puntos en esa trayectoria (x, y, z, α, β, γ).
„ 3.- Utilizando la transformación 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 función 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.
Generación 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
transformación homogénea
inversa
„ Coordenadas generalizadas.
„ Ventaja: las ligaduras
dinámicas se plantean en las
variables generalizadas.
 Tipos de trayectorias (I)

„ Trayectoria punto a punto: cada articulación se mueve sin

considerar el estado o evolución de las demás articulaciones.
„ Movimiento eje a eje
„ Cada vez se mueve un eje
„ El tiempo total es la suma de los tiempos de
cada articulación.
Tipos de trayectorias (II)

„ Movimiento simultáneo de ejes

„ Comienzan simultáneamente todos los ejes

„ El tiempo final será el de aquella articulación que tarde

más tiempo en finalizar su movimiento.

 Tipos de trayectorias (III)

„ Trayectorias coordinadas (isócronas)

„ Se plantea un movimiento simultáneo, ralentizando las

articulaciones más rápidas, para que todas tarden el mismo

tiempo en acaba el movimiento.

„ Trayectorias continuas
„ Se fija explícitamente en coordenadas cartesianas el camino

que tiene que seguir el extremo (infinitos puntos)

„ El movimiento de las articulaciones puede parecer errático.
 Interpolación de trayectorias (I)

„ Muchas veces es necesario generar trayectorias no sólo con un

punto inicial y final, sino que también se impone que pase por
determinados puntos:
„ Camino continuo

„ Evitación de obstáculos

„ Suavizado de la trayectoria, etc.

„ Solución: seleccionar algún tipo de función polinómica (spline)

cuyos coeficientes se ajustan para pasar por los puntos
deseados (con velocidades y aceleraciones aceptables)
 Interpolación de trayectorias (II)

„ Interpolación 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 aceleración
es infinita
 Interpolación de trayectorias (III)

„ Para asegurar la continuidad en la velocidad se aproxima por

una función cúbica.
„ Condiciones (4): posición y velocidad en el punto inicial y final
„ Fórmula de interpolación:
 Interpolación de trayectorias (IV)

„ Interpolación parabólica: 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 aceleración máxima.

„ Fórmula de interpolación:
 Interpolación de trayectorias (V)
„ Condiciones de funcionamiento:
„ 1 tramo: q1(t) = a + b*t

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

Ecuación: q1(t) = q0 + (q1 – q0) / T1 * t
„ 3 tramo: q3(t) = a1 + b1 * (t – T1)
q3(t=T1) = q1 = a 1 q3(t=T1+T2) = q2 = a1 + b1*T2
Ecuación: q3(t) = q1 + (q2 – q1) / T2 * (t – T1)
„ 2 tramo: q2(t) = a2 + b2 (t- (T1- τ)) + c2 (t – T1 + τ )2
( q1 − q0 )
q& (T1 − τ) = b = b 2 + 2c 2 (T1 − τ − T1 + τ) b2 =
T1
( q 2 − q1 )T1 − ( q0 − q1 )T2
q& (T1 + τ ) = b1 = b2 + 2c2 (T1 + τ − T1 + τ ) c2 =
4T1T2τ
0 q1 − q0
q1 (T1 − τ) = q 2 (T1 − τ) = q + (T1 − τ) = a 2
T1
 Interpolación 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 más condiciones:
„ Inicial: posición, velocidad y aceleración

„ Despegue: posición

„ Asentamiento: posición

„ Final: Posición, velocidad

y aceleración
„ 8 condiciones =>

Polinomio de orden 7

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

 Interpolación de trayectorias (VI)

„ Para conseguir transiciones suaves se necesitarían órdenes del

polinomio elevados, pero si el orden es alto se producen
comportamientos erráticos en puntos intermedios:

„ Solución: interpolación a tramos.

„ Interpolación más sencilla (2-1-2):

„ Tramos de despegue y asentamiento a máxima aceleración

„ Tramo intermedio a máxima velocidad

„ Son deseables transiciones suaves por lo que suelen usarse en cada

segmento splines de orden 3, 4 o 5
„ Interpolación más usada: 4-3-4

„ Otras posibilidades: 3-3-3, 5-5-5, 3-5-3, etc.

 Interpolación a tramos (I)

„ Interpolación 2-1-2:
„ Fórmula de interpolación:

donde V es la velocidad, y a la
aceleración máxima permitida.
 Interpolación 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
aceleración.
„ 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

 Interpolación a tramos (III)

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

t ∈ [0 1]
τ − τi −1
„ Tiempo: t =
τi − τi −1
dhi (t ) dhi (t ) dt 1 dhi (t )
„ Velocidad: q&i (t ) = = * =
dτ dt dτ τ i − τ i −1 dt

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

3 2
„
t1

d 2hi (t ) d 2hi (t ) dt 1 d 2hi (t )

„ &&i (t ) =
Aceleración: q = * =
dτ 2
dt 2
dτ (τ i − τ i −1 ) 2 dt 2

1 segmento: 12 a14 t 2 + 6 a13 t + 2 a12

„ q&&( t ) = 2
t1
 Interpolación a tramos (III)

„ Condiciones:
„ 1 segmento:
„ 1.- h1(t=0) = q = a10
0

„ 2.- v1(t=0) = v = a11 / t1 => a11 = v t1

0 0

„ 3.- a1(t=0) = a = 2*a21 / t1

2 => a
21 = a * t1 /2
0 0 2

„ 4.- h1(t=1) = q = a14 + a13 + a12 + a11 + a10 =>

1

a14 + a13 = q1 – q0 – v0 t1 – a0 t12 /2

„ 1 y 2 segmento:
„ 5.- Continuidad en la velocidad:

h& 1 ( 1 ) h& 2 ( 0 ) 4 a 14 + 3 a 13 + 2 a 12 + a 11 a 21
= = =
t1 t2 t1 t2
4 3 1
a 14 + a 13 − a 21 = − a 0 t 1 − v 0
t1 t1 t2
 Interpolación a tramos (IV)

„ 6.- Continuidad en al aceleración:

12a14 + 6a13 + 2a12 2a22
&&i,1(1) = q
q &&i,2 (0) → =
2
t1 t2
2
12 6 2
a 14 + a 13 − a 22 = − a 0

t 12 t 12 t 22

„ 2º segmento:
„ 7.- h2(t=0) = q = a20
1

„ 8.- h2 (t=1) = q = a23+a22+a21+a20 => a23 + a22+a21 = q – q

2 2 1

„ 3er segmento (cambio de variable): t ∈[0,1] => t ∈ [ −1,0] => t = t − 1

„ 9.- h3( t=0) = q
3 = a
30
„ 10.- v3(t =0) = v = a31 / t3 => a31 = v3 t3
3

„ 11.- a3 (t =0) = a = 2 a32 / t3

2 => a
32 = a t3 / 2
3 3 2
 Interpolación 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 aceleración

12a34 − 6a33 + 2a32 6a23 + 2a22
q&&2 (1) = q&&3 ( −1) → =
t32 t22
12 6 6 2
− 2
a 34 + 2 a 33 + 2 a 23 + 2 a 22 = a 3
t3 t3 t2 t2

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

a33 – a34 = q3 – q2 – v3 t3 + a3 t32 / 2
 Interpolación a tramos (V)

⎛ 1 1 0 0 0 0 0 ⎞⎛ a14 ⎞ ⎛ 1 t 2 ⎞
⎜ ⎟⎜ ⎟ ⎜q −q − v t −a
0 o 0 1 ⎟
⎜ 1
2 ⎟
⎜ 4 / t1 3/ t1 −1/ t2 0 0 0 0 ⎟⎜ a13 ⎟ ⎜ 0 0 ⎟
− a t1 − v
⎜12/ t 2 6 / t 2 0 − 2 / t 2
0 0 0 ⎟⎜ a21 ⎟ ⎜ ⎟
⎜ 1 1 2
⎟⎜ ⎟ ⎜ − a0 ⎟
⎜ 0 0 1 1 1 0 0 ⎟⎜ a22 ⎟ = ⎜
q 2
− q1 ⎟
⎜ ⎟
⎜ 0 0 1/ t2 2 / t2 3/ t2 − 3/ t3 4 / t3 ⎟⎜ a23 ⎟ ⎜ 3
v − a t3 3 ⎟
⎜ ⎟⎜ ⎟ ⎜ ⎟
2 / t2 6 / t2 6 / t3 −12/ t3 ⎟⎜ a33 ⎟
3
⎜ 0 0 0 2 2 2 2
⎜ a ⎟
⎜ 0 −1 ⎟⎠⎜⎝ a34 ⎟⎠
⎜ 3 2 3 3 t3 ⎟
2
⎝ 0 0 0 0 1 ⎜ q − q − v t3 − a ⎟
⎝ 2⎠

y = Cx x = C −1 y
 Interpolación 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
ecuación 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
„ ¿Cómo se calculan las trayectorias? => depende de las
condiciones que nos impongan
„ Sólo posición: lineal

„ Posición y velocidad: cúbica

„ Posición, velocidad y aceleración:

„ 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 también continuidad en la aceleración: (4-3-4),
(3-3-3), (5-3-5) etc.
 Bibliografía

„ Barrientos: explica las trayectorias, los tipos de trayectorias y

los polinomios pero sin deducirles.

„ Torres: habla de la interpolación cúbica, y la interpolación

lineal con ajuste parabólico => deducen los valores de cada
parámetro 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