PLANIFICACIÓN DE TRAYECTORIAS

Índice

¿Qué es una trayectoria?Tipos de trayectorias

Punto a puntoCoordinadasContinuas

Trayectorias en el espacio articular:LinealCúbicaParabólicaA tramos

1-2-14-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 articulacionesDiná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 ejeEl tiempo total es la suma de los tiempos de cada articulación.

Tipos de trayectorias (II)

Movimiento simultáneo de ejesComienzan simultáneamente todos los ejesEl 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 continuasSe 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 continuoEvitación de obstáculosSuavizado 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 finalFó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

)(2)( 112211 τττ +−++==+ TTcbbTq& τ21

210

112

2 4)()(

TTTqqTqqc −−−

=

211

010

1211 a)T(T

qqq)T(q)T(q =τ−−

+=τ−=τ−

)TT(cbb)T(q τ+−τ−+==τ− 11221 2&1

01

2)(

Tqqb −

=

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 5Si se desea pasar por puntos intermedios, se tienen más condiciones:

Inicial: posición, velocidad y aceleraciónDespegue: posiciónAsentamiento: posiciónFinal: Posición, velocidady aceleración8 condiciones =>

Polinomio de orden 7

01

12

23

34

45

56

67

7i atatatatatatata)t(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-4Otras 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-4Los 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 4Segundo segmento: 3Tercer segmento: 4

10112

123

134

141i atatatata)t(h)t(q ++++==

20212

223

232i atatata)t(h)t(q +++==

30312

323

334

343i atatatata)t(h)t(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

−

−τ−ττ−τ

=ii

it

dttdh

ddt

dttdh

dtdhtq i

ii

iii

)(1*)()()(1−−

===ττττ

&

2

2

21

2

2

2

2 )()(

1*)()()(dt

thdddt

dtthd

dthdtq i

ii

iii

−−===

ττττ&&

1

11122

133

14 234)(t

atatatatq +++=&

21

12132

14 2612)(t

atatatq ++=&&

Interpolación a tramos (III)

Condiciones:1 segmento:

1.- h1(t=0) = q0 = a10

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

3.- a1(t=0) = a0 = 2*a21 / t12 => a21 = a0 * t12 /24.- h1(t=1) = q1 = a14 + a13 + a12 + a11 + a10 =>

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

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

2

21

1

11121314

2

2

1

1 23401t

at

aaaat

)(ht

)(h=

+++==

&&

01

021

213

114

1

134 vtaat

at

at

−−=−+

Interpolación a tramos (IV)

6.- Continuidad en al aceleración:

2º segmento:7.- h2(t=0) = q1 = a20

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

3er segmento (cambio de variable): t ∈[0,1] => => 9.- h3( =0) = q3 = a30

10.- v3( =0) = v3 = a31 / t3 => a31 = v3 t3

11.- a3 ( =0) = a3 = 2 a32 / t32 => a32 = a3 t3

2 / 2

22

2221

1213142,i1,i

t

a2

t

a2a6a12)0(q)1(q =

++→= &&&&

0222

2132

1142

1

2612 aat

at

at

−=−+

],[t 01−∈ 1−= ttttt

Interpolación a tramos (V)

3

31323334

2

212223

3

3

2

2 2342311t

aaaat

aaat

)(ht

)(h +−+−=

++=

−=&&

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

13.- Continuidad en la aceleración

14.- h3(-1) = q2 = a34 - a33 + a32 – a31 + a30 =>a33 – a34 = q3 – q2 – v3 t3 + a3 t3

2 / 2

333

212

222

232

333

343

12334 tavat

at

at

at

at

−=+++−

22

222323

32333432

262612)1()1(t

aat

aaaqq +=

+−→−= &&&&

3222

2232

2332

3342

3

26612 aat

at

at

at

=+++−

Interpolación a tramos (V)

=

⎟⎟⎟⎟⎟⎟⎟⎟⎟

⎠

⎞

⎜⎜⎜⎜⎜⎜⎜⎜⎜

⎝

⎛

⎟⎟⎟⎟⎟⎟⎟⎟⎟

⎠

⎞

⎜⎜⎜⎜⎜⎜⎜⎜⎜

⎝

⎛

−−

−

−−

34

33

23

22

21

13

14

23

23

22

22

33222

22

21

21

211

1100000/12/6/6/2000

/4/3/3/2/1000011100000/20/6/120000/1/3/40000011

aaaaaaa

ttttttttt

tttttt

⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟⎟

⎠

⎞

⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜⎜

⎝

⎛

−−−

−−

−−−

−−−

2

2

233

3323

33

33

12

0

01

0

210

101

tatvqq

atav

qqa

vta

tatvqq o

Cxy = yCx 1−=

Interpolación a tramos (VI)

Una vez resuelto el problema, en el segmento 3 debemos deshacer el cambio de variable, y sustituir en laecuación del tercer segmento:

1−= tt

30312

323

334

343 atatatata)t(h)t(qi ++++==

30312

3223

33234

343 1121331464 a)t(a)tt(a)ttt(a)tttt(a)t(h +−++−+−+−++−+−=

30312

323

334

343 1111 a)t(a)t(a)t(a)t(a)t(h)t(qi +−+−+−+−==

)aaaaa(t)aaaa(t)aaa(t)aa(ta)t(h

3031323334

313233342

3233343

33344

343 234364+−+−

++−+−++−++−+=

Resumen

¿Qué es una trayectoria?Pasos a seguir para generar una trayectoriaTipos de trayectorias¿Cómo se calculan las trayectorias? => depende de las condiciones que nos impongan

Sólo posición: linealPosición y velocidad: cúbicaPosició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