TEMA 6: GENERACION DE

TRAYECTORIAS PARA UAVs

Antonio González Sorribes (angonsor@upv.es)

Departamento de Ingeniería de Universitat Politècnica

Sistemas y Automática (DISA) de València
Índice

1. Introducción
2. Planificación de Trayectorias
3. Perfil Trapezoidal
4. Planificación de caminos

2
 1) Introducción

• Objetivo: generar las referencias adecuadas de posición o

velocidad que permitan el seguimiento de una trayectoria
preestablecida

• Se asume que el control del guiado de la UAV ya

está implementado.

• Dos casos: (1) trayectorias preestablecidas o (2)

trayectorias dinámicas (p. ej, considerando
evitación de obstáculos)
3
 1) Introducción

Esquema de control completo para el control de trayectorias de

un vehículo del tipo quadrotor

4
 2) Planificación de trayectorias
Trayectoria coordinada desde q 0 = ( x0 , y0 , z0 ) hasta q f = ( x f , y f , z f ) en tiempo T = t f − t0

q f − q0
i i

Considerando movimiento a velocidad constante en T sg : vi =

T
q f − q0
i i

Por tanto para cada articulación : q i (t ) = q0 + (t − t0 ); t0 < t < t f

i

T
3

2 ¿Coordinación?  T común. → ¿valor?

posicion q

1 -Si lo fija el usuario o la tarea, comprobar

0 vel. Máximas/acel. Máximas, y aumentar
-1
0 200 400 600 800 1000 1200 1400
el tiempo T si es necesario.
tiempo

2  q f i − q0i 
T = máx  
velocidad qd

i =1,..., n  vi max 
 
0

-1

-2
0 200 400 600 800 1000 1200 1400

Pb: Velocidad discontinua, ¿aceleración ∞?

tiempo

5
 2) Planificación de trayectorias

Trayectoria coordinada de 3º orden t0 , q 0 , v 0 t f ,q f , v f

Un polinomio temporal de grado 3, tiene 4 coeficientes para satisfacer
las cuatro condiciones de contorno:

q(t ) = a (t − t0 ) 3 + b (t − t0 ) 2 + c (t − t0 ) + d t0 ≤ t ≤ t f , T = t f − t0

Con: v (t ) = 3a (t − t0 ) 2 + 2b (t − t0 ) + c

q(t0 ) = q 0  d = q 0
 c = v
q(t f ) = q f   0
 b = 3(q − q ) / T 2 − (2 v + v ) / T
v (t0 ) = q(t0 ) = v 0 
ɺ  f 0 0 f

v (t f ) = qɺ (t f ) = v f  a = ( v + v ) / T 2 − 2(q − q ) / T 3
 0 f f 0

• Trayectoria coordinada de t0 , q 0 , v 0 , a 0 a t f ,q f , v f ,a f
– Un polinomio temporal de grado 5 tiene 6 coeficientes para
satisfacer las seis condiciones de contorno

6
2) Planificación de trayectorias

Interpolación a tramos

Además de interpolar entre dos puntos manteniendo continuidad, hay que:

Enlazar con otros waypoints, con tiempos, velocidades, aceleraciones o
precisiones de paso que vienen definidas o que se deben planificar
Verificar velocidades y aceleraciones admisibles
Generar consignas en tiempo real, no aumentar la complejidad de cálculo

Los polinomios q(t) de grado elevado:

Elevado coste de cálculo de las configuraciones interpoladas
Dificultad de verificar posiciones, velocidades, aceleraciones admisibles
Tendencia a oscilaciones de la trayectoria
Inflexibilidad ante un posible cambio en el número de waypoints -> replanteo
completo de ecuaciones

7
2) Planificación de trayectorias

Splines cúbicos
Polinomios de grado 3 entre cada dos puntos
Un nuevo waypoint-> un nuevo polinomio
Continuidad en posición y velocidad
Hay que definir tiempos y las velocidades de inicio, fin y de paso en cada
punto

Existen métodos para calcular las velocidad de paso de forma que:

Posición, velocidad y aceleración continuas
Sólo hay que definir tiempos y las velocidades de inicio y fin del
movimiento (nulas)

→ Razonable evaluar restricciones (velocidad parabólica,

aceleración lineal)

8
 2) Planificación de trayectorias

Splines cúbicos
Interpolación con splines
6

4
EJEMPLO: Trayectoria por
2 13 puntos, con instantes de
q

-2
paso fijados y velocidad
0 1 2 3 4 5 6 7 8 9 10

0.6
inicial y final nula
0.4

- Perfil posición
qd

0.2

-0.2
0 1 2 3 4 5 6 7 8 9 10
- Perfil velocidad
0.04 - Perfil Aceleración
0.02
qdd

-0.02

-0.04
0 1 2 3 4 5 6 7 8 9 10
tiempo

9
2) Planificación de trayectorias
Splinestrapezoidal:
Interpolador cúbicosinterp. lineal + transiciones cuadráticas
Interpolación lineal: velocidad constante, aceleración ∞ en transiciones
Idea: velocidades constantes excepto durante las transiciones  con aceleraciones
constantes
Velocidad: forma trapezoidal

q q
Paso por puntos de la trayectoria: Arranque y parada:

q1 qf

q0
q0
q2

t0 t1 t2 t0 tf
t 2θ 2θ t
2θ

10
2) Planificación de trayectorias
Splinestrapezoidal:
Interpolador cúbicosinterp. lineal + transiciones cuadráticas
Interpolación lineal: velocidad constante, aceleración ∞ en transiciones
Idea: velocidades constantes excepto durante las transiciones  con aceleraciones
constantes
Velocidad: forma trapezoidal

q q
Paso por puntos de la trayectoria: Arranque y parada:

q1 qf

q0
q0
q2

t0 t1 t2 t0 tf
t 2θ 2θ t
2θ

11
3) Perfil trapezoidal
Justificación
Se quiere que el UAV vaya de la posición q0 a la posición qf en
un tiempo T. ¿Cómo diseñar la trayectoria adecuada?

Opción 1: Especificar la consigna de posición como una señal

de tipo escalón

- Excesiva sobreoscilación  riesgo de colisión, inestabilidad

- Saturación actuadores
- No cumplimiento de las especificaciones

12
3) Perfil trapezoidal
Justificación
Se quiere que el UAV vaya de la posición q0 a la posición qf en
un tiempo T. ¿Cómo diseñar la trayectoria adecuada?

Opción 2: Especificar la consigna de posición como una señal

de tipo rampa

- Reduce sobreoscilación  trayectorias más suaves

- Perfil de velocidad escalón  aceleración “infinita” en las

transiciones  elevados picos en las acciones de control

13
3) Perfil trapezoidal
Justificación
Se quiere que el UAV vaya de la posición q0 a la posición qf en
un tiempo T. ¿Cómo diseñar la trayectoria adecuada?

Opción 3: Especificar la consigna de velocidad como una

señal de tipo trapezoidal

- Reduce sobreoscilación  trayectorias más suaves y

ajustadas a la consigna
- Menor riesgo de colisiones
- Aceleraciones finitas en las transiciones  reduce picos en
las acciones de control

14
3) Perfil trapezoidal
Tramos de interpolación lineal (velocidad constante) + …
Consideramos sólo una articulación i

Tramo lineal de q0i a q1i , durante t 0 ≤ t ≤ t1 - θ : Tramo lineal de q1i a q2i , durante t1 + θ ≤ t ≤ t 2 :
q1i − q0i q1i − q0i q2i − q1i q2i − q1i
v =
i
= constante v =
i
= constante
t1 − t0 t 2 − t1
0 1
T0 T1
q i (t ) = q0i + v0i (t − t0 ) q i (t ) = q1i + v1i (t − t1 )
q
¿Coordinación?  T0, T1 común. → ¿valor?
-Si lo fija el usuario o la tarea, comprobar
q1
vel. máximas y aumentar T0, T1 si es preciso
-Si el usuario fija la velocidad máxima:
q0

q2  q1i − q0i 
T0 = máx  i 
i =1,..., n  v max 
- Y lo mismo para T usando
 
t0 t1 t2 i .
q2 − q1
1 i
t
2θ

15
3) Perfil trapezoidal

..….… + transiciones cuadráticas (aceleraciones constantes)

Consideramos sólo
una articulación i En el tramo de transición, t1 - θ ≤ t ≤ t1 + θ :
v1i − v0i
a = i
constante
2θ
q1i − q0i q2i − q1i v i (t ) = v0i + a i (t − (t1 − θ )) lineal
v =
i
; v1 =
i

t1 − t0 t 2 − t1
0
ai
q (t ) = q − v θ + v (t − (t1 − θ )) + (t − (t1 − θ )) =
i i i i 2
1 0 0
2
q
v1i − v0i
= q1 + v0 (t − t1 ) +
i i
(t − (t1 − θ ))2
q1 4θ
q0
¿Coordinación?  θ común. → ¿valor?
-Depende de la aceleración (aceleración máxima /
q2 especificada por usuario / por la tarea…)

t0 t1 t2 i =1,..., n
(
2θ = máx v1 − v0 / a i max
i i
)
t
2θ

16
3) Perfil trapezoidal

Ejemplo de movimiento con arranque + punto vía + parada:

Coordinación: todas los grados de libertad (X,Y,Z,yaw)
Se empiezan y termina de acelerar
a la vez en todos los grados de libertad 4
 Mantienen la velocidad constante en

posicion q
2
los mismos instantes de tiempo
Inician y terminan el movimiento 0

a la vez -2
0 500 1000 1500 2000 2500 3000 3500
3.5 tiempo
4
3

velocidad qd
2
2.5
0
2
-2
1.5
-4
posicion

0 500 1000 1500 2000 2500 3000 3500

1
tiempo
10
0.5
aceleración qdd
5
0
0
-0.5
-5
-1
-10
-1.5 0 500 1000 1500 2000 2500 3000 3500
0 500 1000 1500 2000 2500 3000 3500 tiempo
tiempo

17
3) Perfil trapezoidal (duración variable)
Movimiento de arranque y parada
Velocidad de tramo central especificada por usuario
Se añaden tramos parabólicos que alargan la duración total (T+2θ).
El incremento de duración (2θ) depende de la aceleración especificada:
Menor aceleración → acelerar más tiempo → mayor duración total
Caso límite (aceleración mínima): 2θ = T → ¡perfil triangular!
q q’
Fijamos velocidad tramo central y T
qf Menor aceleración para alcanzar v =>
aumentar el tiempo de aceleración 2θ

v
q0

t0 tf t0 tf
2θ 2θ t 2θ 2θ t

18
3) Perfil trapezoidal (duración variable)

Ecuaciones sencillas y fáciles de obtener. Articulación

i-ésima: t ≤ t0 − θ : v0i = 0 constante; t f + θ ≤ t : v if = 0 constante;
q if − q0i q if − q0i
t0 + θ ≤ t ≤ t f − θ : v = i
= constante;
t f − t0 T
En los tramos de transición, t0, f - θ ≤ t ≤ t0, f + θ :
vi
a =±
i

2θ Arranque
a i (t − (t0 − θ ))
v (t ) =  i
i

v + a (t − (t f − θ ))
i
Parada
 i vi
 0q + (t − (t − θ ))2

q (t ) = 
i 4θ 0

q f − v θ + v (t − (t f − θ )) − (t − (t f − θ ))
i
i i i v 2
 4θ

19
3) Perfil trapezoidal (duración variable)

Ecuaciones sencillas y fáciles de obtener. Grado de libertad i-

ésimo:

t ≤ t0 − θ : v0i = 0 constante; t f + θ ≤ t : v if = 0 constante;

q if − q0i q if − q0i
t0 + θ ≤ t ≤ t f − θ : v i = = constante;
t f − t0 T

En el tramo lineal, t 0 + θ ≤ t ≤ t f − θ :
q if − q0i q if − q0i
vi = = constante;
t f − t0 T
q i (t ) = q0i + v i (t − t0 )

20
3) Perfil trapezoidal (duración variable)

Coordinación (caso sencillo): determinar T y 2θ + comprobar

q if − q0i q if − q0i
T = max i
y calcular para cada articulación v =
i
.
i =1,..., n v T
max

Acelerar al máximo para alcanzar v i en tiempo mínimo :

 vi  100
2θ = max  i  y comprobar que : 2θ ≤ T
i =1,..., n  a  A%
 max 
vi
Calcular para cada articulación : a = ± i
y aplicar las fórmulas
2θ
Tiempo total del movimiento = T + 2θ

¿Y si rebaso límites de tiempo de aceleración?

• Idea: aumentar T <-> disminuir v <-> acelerar más tiempo

21
3) Perfil trapezoidal (duración variable)

Coordinación: determinar T y 2θ común

1) Determinar T = tf – t0 (tramo central)

 Cada articulación i:
 q if − q0i 
i i
Tiempo de q a q a velocidad  i
Tvel = i 
0 f
  vmax 
constante (perfil velocidad escalón)   
Tiempo de q0i a q if si siempre acelera (+ a i )   q if − q0i 
 T i
=  
acel
 amax i

y decelera (−a ) (perfil velocidad triangular)
i
 

Si Tvel > Tacel podré alcanzar la velocidad acelerando (perfil trapezoidal).

i i


Si Tvel ≤ Tacel  disminuir la velocidad == aumentar Ti .

i i


 Tomamos T i = max Tvel

i
( i
, Tacel )
y T = max T i
i =1,..., n
( )

22
3) Perfil trapezoidal (duración variable)

Coordinación: determinar T y 2θ común

2) A partir de T = tf – t0 , calcular la velocidad en el tramo central

q if − q0i
para todo i = 1,..., n, calcular : v i = constante.
T
Como T ≥ Tvel
i
→ sabemos que la velocidad v i no supera a la especificada (módulo)

3) Determinar 2θ (tramos de transición) y calcular ai

 v i  100
2θ = max  i  para que la aceleración a i no supere a la especificada (módulo)
i =1,..., n  a  A%
 max

i
v
ai = ± y ya se aplican las ecuaciones para q i (t ), v i (t ) , etc.
2θ
Tiempo total del movimiento = T + 2θ

23
3) Perfil trapezoidal (duración prefijada)
Duración total tf - t0= T + 2θ prefijada (incluido tramo central T + aceleración y
deceleración)
Velocidad tramo central > interp. lineal pura y depende de la aceleración
especificada:
Menor aceleración → mayor velocidad máxima
Caso límite: tramo central de duración nula (valor máximo para v) -> necesito
poder acelerar al menos Fijamos tiempo total T + 2θ
q f − q0 Menor aceleración para alcanzar v =>
amin = 4
q (t − t )2
f 0
q’ aumenta la velocidad máxima necesaria =>
aumenta el tiempo de aceleración
qf v2
v1
v0
q0

t0 tf t0 tf
t t

24
3) Perfil trapezoidal (duración prefijada)

Coordinación (caso sencillo): Se nos da Ttotal=tf - t0= T + 2θ.

Calcular 2θ y comprobar

Sabemos que q if − q0i = v iT = v i (Ttotal − 2θ ) y que a i (2θ ) = v i

(1) Comprobamos que la aceleración máxima es suficiente :
| q if − q0i |
amin = 4 ≤ amax
i

(t f − t0 )
2

(2) Cálculo de 2θ i con aceleración máxima (término dentro raíz positivo) :

| q if − q0i |= amax
i
( 2θ i )(Ttotal − 2θ i ) luego
| q if − q0i |
Ttotal − (Ttotal ) 2 − 4 i
amax
2θ i =
2
(3) Coordinación, cálculo de 2θ → 2θ = max 2θ i
i =1,..., n

25
3) Perfil trapezoidal (duración prefijada)
Coordinación (caso sencillo): Se nos da Ttotal=tf - t0= T + 2θ.
Calcular 2θ y comprobar

(4) Actualización de v i y de a i a partir de 2θ :

q if − q0i vi En el tramo lineal, t 0 + 2θ ≤ t ≤ t f − 2θ :
v =
i
, a =
i

Ttotal − 2θ 2θ q i (t ) = q0i + v i (t − (t0 + θ ) )

(5) Comprobamos la velocidad
| v i |≤ vmax
i
constante
En los tramos de transición, t 0 ≤ t ≤ t 0 + 2θ , t f − 2θ ≤ t ≤ t f :

 a i
(t − t0 ) Arranque
v (t ) =  i
i

v − a (t − (t f − 2θ ))
i

Parada
 i 1 i
q
 0 2 + a (t − t 0 ) 2

q (t ) = 
i

q if − v iθ + v i (t − (t f − 2θ )) − a i (t − (t f − 2θ ))2
1
 2

26
3) Perfil trapezoidal
Perfil trapezoidal I: ejemplo
La distancia a recorrer en cada eje (X, Y, Z) desde la posición inicial hasta
la posición final será: q f − q 0 = (90, −100, 70) m
El tiempo a invertir en cada eje con vmax=2m/s será con un perfil de
velocidad escalón :
q f − q0
Tvel = = ( 45 50 35 ) s
vmax

Si diese tiempo de añadir los tramos de aceleración y deceleración,

finalmente, tendríamos un perfil trapezoidal de velocidad.

¿Y si siempre se acelera y decelera con amax=0.1m/s^2, cuánto tiempo

duraría el movimiento? (perfil triangular de velocidad):

q f − q0
Tacel = = ( 30.0000 31.6228 26.4575 ) s
amax

27
 4) Planificación de caminos

¿Qué camino debe recorrer una aeronave ara

cumplir cierta misión?
Ejemplo: una aeronave que monitoriza el estado de cultivo de un
cierto terreno. ¿Cómo planificar el camino que debe recorrer? ¿altura
de vuelo?

28
 4) Planificación de caminos

Aspectos a tener en cuenta

1) Elección de la altura de vuelo: resolución de la cámara vs
resolución necesaria para identificar objetivos útiles para
monitorización

- A más altura mayor cobertura y menor distancia a recorrer

- A más altura menor capacidad de captación de datos de interés

 SOLUCION DE COMPROMISO

- Altura < altura máxima legal permitida

29
 4) Planificación de caminos

Aspectos a tener en cuenta

2) Ubicación de los waypoints: introducir los puntos de paso por los
que debe pasar la aeronave. La unión de éstos determina el camino

- Teniendo en cuenta el ángulo de visión de la cámara y la altura,

determinar el camino que “minimice” la distancia a recorrer

Se puede establecer un área de solapamiento por “seguridad”

30
 4) Planificación de caminos
Ejemplo
Se quiere monitorizar el estado de maduración de la fruta en un
cultivo cuya foto aérea se adjunta. Para ello, se determina que la
resolución necesaria debe ser de 5 x 5 centímetros y la velocidad
30km/h. La autonomía de la aeronave es de 45 minutos.
El dron está equipado con una cámara de 480x480 pixeles, angular
120ºx120º. Normalmente cámara en posición cenital.

Hmax ?

31
 4) Planificación de caminos
Ejemplo
Foto aérea del cultivo delimitada

355 m

264 m

32
 4) Planificación de caminos

Ejemplo (Paso 1): Cálculo altura máxima

D_horizontal ≤ 480pixeles x 0.05m = 24 m
Resolución requerida
Angulo = 120 º
Altura de vuelo ≤ 12m/ (3)= 6.92 m

Hmax = 6.92 m

D_horizontal = 24m (o inferior)

33
4) Planificación de caminos

Ejemplo (Paso 2): Planificación de caminos

355 m

264 m

34
 4) Planificación de caminos

Ejemplo (Paso 2): Planificación de caminos

- La cobertura visual debe comprender todo el terreno.
- En este caso, trazas horizontales minimizan el número
de maniobras de rotación.
355mm
350
Vertical: 24 x 10 + 2 x 12 = 264m 24m

Horizontal: 331 + 2 x 12 = 355 m

264 m
Margen de distancia en los bordes del área
a monitorizar para no “invadir”
el área exterior a la región de interés y cubrir la
totalidad de dicha región
331m

35
4) Planificación de caminos

Ejemplo (Paso 2): Planificación de caminos

36
4) Planificación de caminos

Ejemplo (Paso 2): Planificación de caminos

Longitud total tramos horizontales: 331 x 11 m

Longitud total tramos verticales: 24 x 10 m

Longitud total a recorrer= 331 x 11+24 x 10 m = 3881 m

Velocidad 30km/h  3.881km / 30km/h = 0.1294 h 

Tiempo de ciclo: 7.762min
Autonomía 45min
Número total (máximo) de ciclos sin recarga = 5
37
4) Planificación de caminos

38