PLANIFICACIÓN DE TRAYECTORIAS

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=T1) = a + b*T1 = q1
q1(t=0) = q0 = a
Ecuación: 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
Ecuación: 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
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 =
„
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 ) =
*
=
=
Aceleración: 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
Interpolación 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
Interpolación a tramos (IV)
„
6.- Continuidad en al aceleración:
&&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
0
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
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
=
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
„
Interpolación 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
⎟
1
⎜
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
⎟
2⎠
⎝
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