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 (q₁, q₂, q_3, q₄, q₅, q₆)

„ 4.- Interpolar entre los puntos articulares obtenidos,

generando una trayectoria en función del tiempo para cada variable articular: q_i(t), que sea realizable por los actuadores.

„ 5.- Si está bien hecho, esta trayectoria se aproximará a la

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

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)

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 * t}i-1 _{+ b = q}i-1

q(ti_{) = a * t}i _{+ b = q}i

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.

Interpolación de trayectorias (V)

„

Condiciones de funcionamiento:

„

1

tramo: q₁(t) = a + b*t q₁(t=0) = q0 _{= a q} 1(t=T1) = a + b*T1 = q1 Ecuación: q₁(t) = q0 _{+ (q}1 _{– q}0_{) / T}1 _{* t} „ 3 tramo: q₃(t) = a₁ + b₁ * (t – T₁) q₃(t=T₁) = q1 _{= a} 1 q3(t=T1+T2) = q2 = a1 + b1*T2 Ecuación: q₃(t) = q₁ + (q2 _{– q}1_{) / T} 2 * (t – T1) „ 2 tramo: q₂(t) = a₂ + b₂ (t- (T₁- τ)) + c₂ (t – T₁ + τ )2

)

(

2

)

(

T

₁

+

τ

=

b

₁

=

b

₂

+

c

₂

T

₁

+

τ

−

T

₁

+

τ

q

&

_τ 2 1 2 1 0 1 1 2 2 4 ) ( ) ( T T T q q T q q c = − − − 2 1 1 0 1 0 1 2 1 1 (T ) a T q q q ) T ( q ) T ( q − τ = − τ = + − − τ =

)

T

T

(

c

b

b

)

T

(

q

&

₁

−

τ

=

=

₂

+

2

_{2 1}

−

τ

−

₁

+

τ

1 0 1 2 ) ( T q q b = −

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 0 1 1 2 2 3 3 4 4 5 5 6 6 7 7 i(t) a t a t a t a t a t a t a t a q = + + + + + + +

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

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 10 11 2 12 3 13 4 14 1 i(t) h (t) a t a t a t a t a q = = + + + + 20 21 2 22 3 23 2 i(t) h (t) a t a t a t a q = = + + + 30 31 2 32 3 33 4 34 3 i(t) h (t) a t a t a t a t a q = = + + + +

Interpolación a tramos (III)

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

t ∈ [0 1] „ Tiempo: „ Velocidad: „ 1 segmento: „ Aceleración: „ 1 segmento: 1 1 − − τ − τ τ − τ = i i i t dt t dh d dt dt t dh d t dh t q i i i i i i ) ( 1 * ) ( ) ( ) ( 1 − − = = = τ τ τ τ & 2 2 2 1 2 2 2 2 ) ( ) ( 1 * ) ( ) ( ) ( dt t h d d dt dt t h d d t h d t q i i i i i i − − = = =

τ

τ

τ

τ

&& 1 11 12 2 13 3 14 3 2 4 ) ( t a t a t a t a t q& = + + + 2 1 12 13 2 14 6 2 12 ) ( t a t a t a t q&& = + +

Interpolación a tramos (III)

„ Condiciones: „ 1 segmento: „ 1.- h₁(t=0) = q0 = a₁₀ „ 2.- v₁(t=0) = v0 = a₁₁ / t₁ => a₁₁ = v0 t₁ „ 3.- a₁(t=0) = a0 = 2*a₂₁ / t₁2 => a₂₁ = a0 * t₁2 /2 „ 4.- h₁(t=1) = q1 = a₁₄ + a₁₃ + a₁₂ + a₁₁ + a₁₀ => a₁₄ + a₁₃ = q1 _{– q}0 _{– v}0 _t 1 – a0 t12 /2 „ 1 y 2 segmento: „ 5.- Continuidad en la velocidad: 2 21 1 11 12 13 14 2 2 1 1 1 0 4 3 2 t a t a a a a t ) ( h t ) ( h = + + + = = & & 0 1 0 21 2 13 1 14 1 1 3 4 v t a a t a t a t + − = − −

Interpolación a tramos (IV)

„ 6.- Continuidad en al aceleración:

„ 2º segmento:

„ 7.- h₂(t=0) = q1 = a₂₀

„ 8.- h₂ (t=1) = q2 = a₂₃+a₂₂+a₂₁+a₂₀ => a₂₃ + a₂₂+a₂₁ = q2 – q1

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

„ 9.- h₃( =0) = q3 = a₃₀ „ 10.- v₃( =0) = v3 = a₃₁ / t₃ => a₃₁ = v3 t₃ „ 11.- a₃ ( =0) = a3 = 2 a₃₂ / t₃2 => a₃₂ = a3 t₃2 / 2 2 2 22 2 1 12 13 14 2 ,i 1 ,i t a 2 t a 2 a 6 a 12 ) 0 ( q ) 1 ( q&& = && → + + = 0 22 2 2 13 2 1 14 2 1 2 6 12 a a t a t a t + − = − ] , [ t ∈ −10 _{t = t −1} t t t

Interpolación a tramos (V)

3 31 32 33 34 2 21 22 23 3 3 2 2 1 1 3 2 4 3 2 t a a a a t a a a t ) ( h t ) ( h − + − + = + + = − = & & „ 2º y 3er segmento: „ 12.- Continuidad en la velocidad: „ 13.- Continuidad en la aceleración „ 14.- h₃(-1) = q2 = a₃₄ - a₃₃ + a₃₂ – a₃₁ + a₃₀ => a₃₃ – a₃₄ = q3 _{– q}2 _{– v}3 _t 3 + a3 t32 / 2 3 3 3 21 2 22 2 23 2 33 3 34 3 1 2 3 3 4 t a v a t a t a t a t a t − + + + = − 2 2 22 23 2 3 32 33 34 3 2 2 6 2 6 12 ) 1 ( ) 1 ( t a a t a a a q q&& = && − → − + = + 3 22 2 2 23 2 2 33 2 3 34 2 3 2 6 6 12 a a t a t a t a t + + + = −

Interpolación a tramos (V)

= ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ ⎞ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝ ⎛ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ ⎞ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝ ⎛ − − − − − 34 33 23 22 21 13 14 2 3 2 3 2 2 2 2 3 3 2 2 2 2 2 2 1 2 1 2 1 1 1 1 0 0 0 0 0 / 12 / 6 / 6 / 2 0 0 0 / 4 / 3 / 3 / 2 / 1 0 0 0 0 1 1 1 0 0 0 0 0 / 2 0 / 6 / 12 0 0 0 0 / 1 / 3 / 4 0 0 0 0 0 1 1 a a a a a a a t t t t t t t t t t t t t t t ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎟ ⎠ ⎞ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎜ ⎝ ⎛ − − − − − − − − − − − 2 2 2 3 3 3 3 2 3 3 3 3 3 1 2 0 0 1 0 2 1 0 1 0 1 t a t v q q a t a v q q a v t a t a t v q q o

Cx

y

=

_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 en la ecuación del tercer segmento:

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

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,