PLANIFICACIÓN EFICIENTE DEL VUELO DE
QUADROTORS PARA RECOGIDA DE DATOS
DE SENSORES INALÁMBRICOS
AUTOR: JAIME MONJE DE LA CALZADA
TUTOR: JOSÉ RAMIRO MARTÍNEZ DE DIOS
PROYECTO FIN DE CARRERA
INGENIERÍA AERONÁUTICA
Dep. de Ingeniería de Sistemas y Automática
Escuela Superior de Ingenieros
Universidad de Sevilla
Agradecimientos
El presente proyecto está dedicado a todas aquellas personas que me animaron a
seguir adelante durante el camino gracias a su ánimo. Entre ellas: Mi familia, mis compañeros
de clase B.G, mis compañeros de residencia de la planta cuarta izquierda, mis amigos del café
Esi, el grupo de Ceuta y mis compañeros de Barcelona. Gracias a todas ellas por todos los
buenos momentos pasados y sus ánimos que me dieron fuerza.
Índice
1 Introducción 1 1.1. Antecedentes históricos
1.1.1. Evolución de configuración del quadrotor 11 1.1.2. Evolución de los UAVs 14
1.2. Contexto del proyecto 15 1.3. Objetivos 17 1.4. Estructura del documento 18
2 Modelado y control del Quadrotor 19 2.1. Introducción 19 2.2. Modelo del vehículo no tripulado 27 2.3. Control del quadrotor 27
2.3.1. Control de las variables acopladas X-θ 28 2.3.2. Control de las variables acopladas Y-ɸ 29 2.3.3. Control de la variable Z 31 2.3.4. Control de la variable ψ 32
2.4. Conclusiones 33
3 Planificación 34 3.1. Introducción 34
3.1.1. Representación de los ejes del quadrotor 34 3.1.2. Representación de los de la antena 35
3.2. Planificación de trayectorias rectilíneas ejes 36 3.2.1. Expresiones de la trayectoria recilínea 36 3.2.1.1 Xi < Xf 38 3.2.1.2 Xi > Xf 39 3.2.1.2 Xi =Xf 40 3.2.1.2 Yi =Yf 42 3.2.2. Algoritmo 43 3.2.3. Selección de dos puntos para cada recta 44
3.3. Planificación de trayectorias circunferenciales unidas mediante trayectorias rectilíneas 47 3.3.1. Obtención de parámetros de la circunferencia 48 3.3.2. Ecuaciones paramétricas de la circunferencia 51 3.3.3. Velocidad angular del quadrotor 51 3.3.4. Elección del radio de curvatura de vuelo 53 3.3.5. Selección de circunferencias 56 3.3.6. Calculo de tiempos de trayectorias 59
3.3.7. Algoritmo 61
3.4. Comparación de ambos métodos 83 3.5. Conclusiones 87
4 Experimentos 88 4.1. Introducción 88 4.2. Descripción del sistema 88 4.3. Experimentos de control 90
4.3.1. Errores 90 4.3.1.1 Error en el control X-θ 90 4.3.1.2 Error en el control Y-ɸ 91 4.3.1.3 Errores del controlador del subsistema Z 93 4.3.1.4 Errores del controlador del subsistema ψ 94 4.3.2. Efecto del ruido 95 4.3.2.1 Efecto del ruido en el subsistema X-θ 95 4.3.2.2 Efecto del ruido en el subsistema Y-ɸ 99 4.3.2.3 Efecto del ruido en el subsistema Z 102 4.3.2.4 Efecto del ruido en el subsistema ψ 105
4.4. Experimentos de planificación 107 4.4.1. Experimentos de planificación para distintas distribuciones de sensores 107 4.4.2. Efecto de la altura 112 4.4.2.1 Z=20 metros 113 4.4.2.2 Z=30 metros 114 4.4.2.3 Z=40 metros 115 4.4.2.4 Conclusión 116 4.4.3. Efectos de la velocidad 117 4.4.4. Efectos del ruido 118
5 Conclusiones 121 5.1. Conclusiones 121 5.2. Ampliaciones del proyecto 123
Referencias 125
Índice de Figuras
Ilustración 1.1. Oehmichen No.2 11
Ilustración 1.2. Bothezat helicopter 12
Ilustración 1.3. Convertawings 12
Ilustración 1.4. Quad Tiltrotor 13
Ilustración 1.5. Firebee 14
Ilustración 1.6. UAV con cámara 15
Ilustración 1.7. Quadrotor 16
Ilustración 2.1. Arquitectura del quadrotor 19
Ilustración 2.2. Sistemas y grados de libertad del vehículo 20
Ilustración 2.3. Subsistemas del quadrotor 23
Ilustración 2.4. Estructura cascada de X y θ 27
Ilustración 2.5. Estructura cascada de ф e Y 27
Ilustración 2.6. Señal deseada y obtenida por control de X 29
Ilustración 2.7. Señal deseada y obtenida por control de Y 30
Ilustración 2.8. Señal deseada y obtenida por control de Z 31
Ilustración 2.9. Señal deseada y obtenida por control de ψ 32
Ilustración 3.1. Ejes del quadrotor 35
Ilustración 3.2. Representación de la antena 35
Ilustración 3.3. Distribución de la velocidad en un tramo rectilíneo 37
Ilustración 3.4. Recta que representa el caso Xi<Xf 38
Ilustración 3.5. Recta que representa el caso Xi>Xf 39
Ilustración 3.6. Recta que representa el caso Xi<Xf 41
Ilustración 3.7. Recta que representa el caso Yi<Yf 42
Ilustración 3.8. Unión de tramos de trayectorias rectilíneas 43
Ilustración 3.9. Algoritmo que ordena los puntos según la cercanía de unos con otros 46
Ilustración 3.10. Algoritmo que define circunferencias 50
Ilustración 3.11. Distribución de velocidades angulares a lo largo de la circunferencia 52
Ilustración 3.12. Circunferencias en el plano Z=0 53
Ilustración 3.13. Relación de d’ y R’ para w= 3 deg/s 54
Ilustración 3.14. Relación de d’ y R’ para w= 5 deg/s 55
Ilustración 3.15. Criterio de distancia elegido 56
Ilustración 3.16. Representación del movimiento en las trayectorias 57
Ilustración 3.17. Criterio en el caso de que queden dos puntos sin asignar circunferencia 58
Ilustración 3.18. Criterio en el caso de que quede un punto sin asignar circunferencia 59
Ilustración 3.19. Serie de trayectorias con tiempos asociados 59
Ilustración 3.20. Serie de trayectorias con recta que une el último punto 60
Ilustración 3.21. Serie de trayectorias con las trayectorias que unen los dos últimos punto 61
Ilustración 3.22. Algoritmo que calcula el primer punto de la circunferencia 65
Ilustración 3.23. Algoritmo que calcula el segundo punto de la circunferencia 67
Ilustración 3.24 Algoritmo que calcula el tercer punto de la circunferencia 69
Ilustración 3.25. Algoritmo que calcula el radio de curvatura de vuelo y la velocidad angular 71
Ilustración 3.26. Algoritmo que calcula el vector Ks que indica los puntos que están dentro de la
circunferencia. 73
Ilustración 3.27. Algoritmo que calcula los nuevos vectores excluyendo puntos que ya han formado
circunferencias. 75
Ilustración 3.28. Algoritmo que calcula los parámetros en el que caso de que queden dos puntos
restantes 78
Ilustración 3.29. Algoritmo que calcula los parámetros de la recta que lleva a la primera
circunferencia 80
Ilustración 3.30. Algoritmo que calcula los parámetros de las diferentes rectas que unen a las
circunferencias 82
Ilustración 3.31. Gráfica de simulación para el primer ejemplo con el primer método 83
Ilustración 3.32. Gráfica de la simulación para el primer ejemplo con el segundo método 84
Ilustración 3.33. Gráfica de la simulación para el segundo ejemplo con el segundo método 85
Ilustración 3.34. Gráfica de la simulación para el segundo ejemplo con el primer método 86
Ilustración 4.1 Esquema del Sistema 88
Ilustración 4.2. Estructura cascada de X y θ 89
Ilustración 4.3. Estructura cascada de ф e Y 89
Ilustración 4.4. Salida de X 90
Ilustración 4.5. Salida de θ 91
Ilustración 4.6. Salida de Y 92
Ilustración 4.7. Salida de Φ 92
Ilustración 4.8. Salida de Z 93
Ilustración 4.9. Salida de ψ 94
Ilustración 4.10. Ruido del sistema X- θ 95
Ilustración 4.11. Salida X sin ruido 96
Ilustración 4.12. Salida θ sin ruido 96
Ilustración 4.13. Salida del lazo interno para una varianza de 97
Ilustración 4.14. Salida X para una varianza de 97
Ilustración 4.15. Salida del lazo interno para una varianza de 98
Ilustración 4.16. Salida para una varianza de 98
Ilustración 4.17. Ruido en el subsistema Y-Φ 99
Ilustración 4.18. Salida Y sin ruido 99
Ilustración 4.19. Salida Φ sin ruido 100
Ilustración 4.20. Salida Φ del subsistema Y e Φ para una varianza de 100
Ilustración 4.21. Salida Y del subsistema Y e Φ para una varianza de 101
Ilustración 4.22. Salida Φ del subsistema Y e Φ para una varianza de 101
Ilustración 4.23. Salida Y del subsistema Y e Φ para una varianza de 102
Ilustración 4.24. Salida Z sin ruido 103
Ilustración 4.25. Salida Z para un ruido gaussiano de 0.01 103
Ilustración 4.26. Salida Z para un ruido gaussiano de 0.1 104
Ilustración 4.27. Salida ψ sin ruido 105
Ilustración 4.28. Salida ψ para ruido gaussiano de 1 106
Ilustración 4.29. Salida ψ para ruido gaussiano de 10 106
Ilustración 4.30. Salida ψ para ruido gaussiano de 20 106
Ilustración 4.31.Posición del quadrotor cuando la señal incide en los sensores 107
Ilustración 4.32. Circunferencias asociadas a cada sensor 108
Ilustración 4.33. Distribución de puntos ejemplo II 109
Ilustración 4.34. Circunferencias asociadas a cada sensor ejemplo II 109
Ilustración 4.35. Distribución de puntos ejemplo III 110
Ilustración 4.36. Circunferencia asociadas a sensores ejempo III 111
Ilustración 4.37. Efecto de la altura a 20 metros 113
Ilustración 4.38. Efecto de la altura a 30 metros 114
Ilustración 4.39. Efecto de la altura a 40 metros 115
Ilustración 4.40. Posición del quadrotor cuando la señal incide en los sensores 117
Ilustración 4.41. Distribución de sensores en los que se analizará el ruido 119
1 INTRODUCCIÓN
1.1 Antecedentes históricos 1.1.1 Evolución de configuración del quadrotor
A principios del siglo XX, el científico francés Charles Richet creó el primer modelo
de quadrotor, aunque su pequeño modelo no consiguió volar. Pero su alumno Breguet fue el
primero en construir un quadrotor pilotado.
Dicho vehículo estaba constituido por cuatro motores con cuatros palas que eran
movidos por un motor Antoinette de 8 cilindros de 40 HP. Un simple sistema de poleas movía
las hélices, sin embargo, en el primer vuelo , no se consiguió mover más de un metro y medio
del suelo.
En 1920, Etienne Oehmichen creó Oehmichen No.2 (Ilustración 1.1) que era un
helicóptero de cuatro rotores y ochos hélices, impulsados por un motor. Este helicóptero
presentaba rotores bípala cuyo ángulo era variable. Para estabilizar lateralmente, cinco de las
palas giraban en el plano horizontal, para la dirección, se usaba la hélice montada en la nariz,
mientras que para la propulsión hacia delante, se usaba el par restante de las hélices. Esta
aeronave presentaba una gran capacidad de control y una buena estabilidad para su época.
Ilustración 1.1 Oehmichen No.2
12 | P á g i n a
Durante 1922, George de Bothezat e Ivan Jerome desarrollaron una aeronave
(Ilustración 1.2) formada por cuatro rotores de seis palas, situados en el extremo de una
estructura en forma de X. Fue el primero en usar hélice de paso colectivo. El control de la
orientación y del ángulo de guiñada se realizaba mediante dos pequeñas hélices de paso
variable. A pesar de su viabilidad, presentaba poca potencia, era mecánicamente complejo y
tenía problemas de fiabilidad.
Ilustración 1.2 Bothezat helicopter
Posteriormente en 1956, la empresa Convertawings diseñó un prototipo (Ilustración 1.3)
de mayor tamaño para uso civil y militar, y que disponía de dos motores que accionaban los
cuatro rotores. En este modelo, no se usaba un rotor de cola, sin embargo, el control se realizaba
mediante la variación de velocidad de cada rotor. Esta forma es la que se establecería en el
diseño actual de los quadrotors.
Ilustración 1.3 Convertawings
Más tarde, Curtis-Wright diseñó un quadrotor para el ejército americano, Curtiss-Wright
VZ-7, en el que el control también era debido a la variación de velocidad de los rotores.
13 | P á g i n a
A finales de los 90, Bell y Boing trabajaron conjuntamente en el quadrotor tripulado, Quad
Tiltrotor (Ilustración 1.4), que tenía dos modalidades de funcionamiento: uno que presentaba el
vuelo vertical con los rotores paralelos al suelo y otro que permitía el vuelo de avance.
Ilustración 1.4 Quad Tiltrotor
14 | P á g i n a
1.1.2 Evolución de UAVs
El origen de estos vehículos se remonta al siglo XIX, cuando se usó un UAV, formado
por un globo que cargaba bombas, durante un ataque de Austria a la ciudad de Viena.
Durante la guerra de secesión americana (1861-1865), ambos bandos también usaron
estos vehículos en forma de globos cargados. Sin embargo, la exactitud y fiabilidad de los
movimientos de estos vehículos eran pequeñas en ese momento.
El desarrollo más profundo de estos vehículos, fue durante la Primera Guerra Mundial y
la Segunda Guerra Mundial en aplicaciones militares. En este tiempo, EEUU creó el prototipo,
Operación Afrodita. Eran B-17 cargados de explosivos y que se estrellaban contra un objetivo
mediante control remoto.
Más tarde, durante la Guerra Fría, EEUU desarrolló vehículos no tripulados para
espionaje y reconocimiento. El primero de ellos fue Firebee (Ilustración 1.5) usado en la
República Comunista de China.
Ilustración 1.5 Firebee
En la Guerra de Vietnam, los vehículos ya contaban con reconocimientos de cámara de
día. E incluso al poco tiempo, dichas aeronaves ya podían disponer de cámaras nocturnas, y de
mejoras de tanto de comunicación como de electrónica.
Actualmente, está en auge el desarrollo y uso de estos vehículos tanto para ámbitos
militares como para ámbitos civiles.
15 | P á g i n a
1.2 Contexto del proyecto
Los UAVs son aeronaves no tripuladas que se usan cada vez más tanto en uso civil como
militar, ya que presentan menor peso, menores dimensiones, e inferiores costes que las
aeronaves tripuladas. Además pueden moverse en entornos complejos sin pérdidas humanas.
Algunas de las aplicaciones más comunes para estos vehículos son las siguientes:
Aplicaciones militares: Se utiliza principalmente para misiones de espionaje, y en
labores militares, ya que evitan las pérdidas humanas.
Supervisión: Especialmente en supervisión de edificios y obras civiles. Para realizar
esta operación, el vehículo dispone de cámara de video (Ilustración 1.6) para
observación y vigilancia.
En ocasiones el tráfico terrestre y aéreo se supervisa mediante UAVs.
Ilustración 1.6 UAV con cámara
Fotografía Aérea: Mediante fotografía aérea y grabación de video, se puede obtener
rápidamente información de un terreno. También, se pueden usar en el ámbito
cinematográfico para grabar diferentes ángulos.
Reconocimiento de accidentes y desastres: Estos vehículos permiten el acceder a zonas
difíciles de llegar, donde han habido accidentes nucleares o desastres naturales.
También, pueden hacer una valoración aérea de un accidente de tráfico.
Agricultura: En algunas ocasiones, los UAVs llevan equipados fertilizantes y pesticidas
que son pulverizados durante el vuelo.
Se pueden distinguir dos tipos de UAVs:
Despegue Vertical: Ala Rotativa y auto-sustentados.
Despegue no vertical: Ala flexible y ala fija.
16 | P á g i n a
Los Helicópteros y los Quadrotors forman parte del grupo de los UAVs de despegue
vertical y ala rotativa.
Los quadrotors presentan más sencillez mecánica que los helicópteros, ya que el control
de ellos se realiza a través de la variación de la velocidad de sus rotores, mientras que en los
helicópteros, el control se realiza variando el ángulo de paso de la pala, manteniendo la
velocidad de giro constante. Sin embargo, el control es más complejo en los quadrotors,
debido al efecto de acoplamiento de la dinámica de los rotores.
Un quadrotor (Ilustración 1.7) es una aeronave que se mueve y se eleva gracias a los
cuatros rotores que están instalados en una estructura con forma de cruz.
Ilustración 1.7 Quadrotor
Dos de sus cuatros rotores giran en sentido antihorario, y los otros dos en el sentido
horario. Esta configuración permitirá que el quadrotor pueda realizar cualquier maniobra.
Otra ventaja del quadrotor, es el uso de motores eléctricos en lugar de motores de
combustión, ya que contaminan menos el aire en lugares tales como interiores de edificios.
Sin embargo, este tipo de motores limitan el tiempo de funcionamiento, siendo su
autonomía de 15 a 20 minutos.
17 | P á g i n a
1.3 Objetivos
El objetivo de este proyecto es planificar eficientemente el vuelo de un quadrotor para
que la señal de su antena llegue a los distintos sensores inalámbricos de un terreno.
Para ello, se parte de la hipótesis de que la antena del quadrotor se encuentra situada en
eje –Z, y que para cumplir el objetivo el vuelo se realizará a altura constante.
Dada una serie de coordenadas en donde se encuentran los diferentes sensores
inalámbricos, se deberá planificar diferentes trayectorias para que la señal de la antena llegue a
todos los sensores inalámbricos. También, se tendrá que estudiar el orden en que la señal de la
antena pasará por cada punto donde se encuentra un sensor, ya que esto permitirá reducir el
tiempo que se tarda en cumplir la misión,si se eligen adecuadamente.
Para cumplir el objetivo, en primer lugar, se desarrollará un modelo cinemático y
dinámico del quadrotor, que posteriormente, se linealizará.
Una vez linealizado este modelo, se buscará un controlador para cada una de las
variables controlables.
Las dos planificaciones que se desarrollarán serán las siguientes:
Una planificación que consistirá en buscar trayectorias rectilíneas en las que el
quadorotor pasará por encima de todos los puntos donde se encuentran los sensores.
Para minimizar el tiempo empleado en cumplir el objetivo, el vehículo empezará su
recorrido por el punto del sensor más cercano al origen, y de ahí en adelante, el
quadrotor irá hacia el punto del sensor más cercano al último punto del sensor por el
que pasó.
Una planificación que empleará tanto trayectorias circunferenciales como rectilíneas.
Por cada tres puntos donde están situados los sensores, se buscará una trayectoria
circunferencial del quadrotor, que permitirá que la señal de la antena pase por esos
puntos. Los tres puntos que se elegirán para buscar el vuelo circunferencial del
quadrotor serán aquellos que presenten menor distancia entre ellos.
Una vez desarrolladas las planificaciones, se realizarán diferentes experimentos para
comprobar la robustez, así como la influencia que tendrían la altura o la velocidad en las
planificaciones anteriores.
Las herramientas que se usarán para realizar lo expuesto anteriormente, serán MATLAB
y SIMULINK. Con dichas herramientas, se podrán conocer la posición del quadrotor a lo
largo del tiempo de forma gráfica, representar las trayectorias circunferenciales, conocer la
posición que tendrá el quadrotor en el espacio cuando la antena cumpla el objetivo, además
de los errores que presentarían.
18 | P á g i n a
1.4 Estructura del documento
En primer lugar, en el Capítulo 2, se desarrollará un modelo cinemático y dinámico del
quadrotor. Más tarde, este modelo se linealizará, y posteriormente, se buscará un control para
cada una de las variables de control.
A continuación, en el Capítulo 3, se detallarán las diferentes planificaciones que
permitirán cumplir el objetivo. Para cada una de las planificaciones que se estudiarán, se
analizarán todos los casos posibles de trayectorias que se puedan optar dentro de ellas. Además,
se representarán los diagramas de flujo de los algoritmos más importantes que se han usado en
el desarrollo de estas planificaciones y se mostrarán varios ejemplos con el fin de comparar
dichas simplificaciones.
Posteriormente, en el Capítulo 4, se realizarán distintos tipos de experimentos mediante
simulación para determinar si el sistema es robusto, y conocer cómo afectaría el cambio de
altura y el cambio de velocidad del vehículo.
Para finalizar, en el Capítulo 5, se expondrán las diferentes conclusiones extraídas del
proyecto así como las posibles líneas de mejora del mismo.
19 | P á g i n a
2 MODELADO Y CONTROL DEL
QUADROTOR
2.1. Introducción
En el siguiente capítulo se desarrollará un modelo para el movimiento del quadrotor,
respecto de un sistema de referencia cuyo origen estaría en el suelo. Posteriormente dicho modelo se
linealizará y se simplificará para disminuir el grado de dificultad, así como para reducir el máximo
acoplamiento posible de las variables que intervienen en el sistema.
Una vez obtenido el modelo simplificado se procederá a obtener los parámetros de control
que mejor se adapten en el sistema simplificado. Se ha optado por un control discreto en lugar de un
control analógico debido a que este es más rápido y es el más utilizado en la actualidad.
2.2. Modelo del vehículo no tripulado
Un quadrotor es un vehículo no tripulado cuya arquitectura se basa en dos brazos
inextensibles y perpendiculares en cuyos extremos se puede encontrar un motor eléctrico (dos por
cada brazo). Dicha estructura se puede observar en la Ilustración 2.1.
Ilustración 2.1 Arquitectura del quadrotor
El movimiento de este vehículo es debido a la diferencia de la velocidad que generan los
cuatros rotores, cuyos componentes son motores eléctricos y hélices.
El mecanismo del quadrotor consiste en:
: un par para generar movimiento de balance debido al desequilibrio de las
fuerzas F2 y F4.
: un par para generar movimiento de cabeceo debido al desequilibrio de las fuerzas
20 | P á g i n a
F1 y F3.
: un par para generar movimiento de guiñada debido al desequilibrio del conjunto
de fuerzas (F1 y F3) y ( F2 y F4) ya que giran en sentido contrario los rotores 1 y 3 de
2 y 4
U: un empuje total que produce el desplazamiento perpendicular al plano del rotor y
es debido a la suma de las cuatro fuerzas que generan los rotores.
En la Ilustración 2.2 se puede observar las diferentes fuerzas y pares que generan
cada rotor, así como los grados libertad que intervienen en el modelo
Se pueden distinguir dos sistemas de referencia:
Un sistema de referencia ligado a los ejes cuerpo del vehículo, 𝐵= {𝑂B XB YB ZB}, en
el cuál los ejes XB y YB coinciden con la dirección de los dos brazos, mientras que el eje ZB
está situado en un eje perpendicular al plano del rotor. El origen de dicho sistema OB coincide
con el centro de masas del vehículo no tripulado.
Un sistema de referencia inercial, W= {𝑂W XW YW ZW}, fijo con respecto a tierra.
Ilustración 2.2 Sistemas y grados de libertad del vehículo
A continuación se procederá a obtener el modelo matemático del sistema. Para ello
se utilizarán las siguientes hipótesis:
El vehículo no tripulado se considera un sólido rígido.
El quadrotor presenta geometría simétrica por lo que su tensor de inercia será
diagonal.
21 | P á g i n a
Se han despreciado efectos de dinámicas traslacionales, el efecto suelo, las fuerzas y
momentos aerodinámicos y los efectos giroscópicos.
Para el modelo matemático se usará el enfoque de Lagrange-Euler, el cual puede
verse detallado en [1].
La formulación de Lagrange-Euler tiene la siguiente expresión:
(
)
donde se puede distinguir:
, la coordenada generalizada
, la energía cinética total
, la energía potencial total
, las fuerzas o pares generalizados producidos por pares o fuerzas no conservativas
Las coordenadas generalizadas son las siguientes:
[ ]
siendo:
[ ] es la posición del centro de masas con respecto al sistema de referencia
inercial.
[ ] son los ángulos asociados a los ejes del vehículo YB XB y ZB
respectivamente.
Aplicando Lagrange-Euler como aparece detallado en [1], se obtienen las siguientes
expresiones:
Para el movimiento traslacional:
siendo el vector unitario del eje ZB.
Para el movimiento rotacional:
22 | P á g i n a
A continuación se muestran las vectores y matrices que intervienen en las
expresiones anteriores :
[
( )
( )
]
[
]
( )( ) ( )( )
( )
( )( ) ( )( )
( )
( )
( )( ) ( )( )
23 | P á g i n a
( )
Observando las expresiones anteriores se llega a las siguientes
conclusiones:
Las ecuaciones del subsistema traslacional dependen de los ángulos de cabeceo,
balanceo y guiñada.
Las ecuaciones del subsistema de rotación no están acopladas con los grados de
libertad de la posición, pero existen términos no lineales como cosenos, senos o
derivadas de los mismos.
Se obtiene por tanto dos subsistemas como se puede observar en la Ilustración 2.3:
Un subsistema de rotación cuyas entradas son los tres pares de control y cuyas
salidas son los ángulos de cabeceo, balanceo y guiñada.
Un subsistema de traslación cuyas entradas son los tres ángulos anteriores y el
empuje total y cuyas salidas son las tres posiciones del centro de gravedad del
vehículo en el espacio.
Ilustración 2.3 Subsistemas del quadrotor
24 | P á g i n a
Para simplificar las expresiones anteriores se procederá a linealizar en torno a
y También se usará la aproximación de ángulos pequeños.
Aplicando lo anterior, se obtienen las siguientes expresiones simplificadas y
linealizadas:
[
]
[
]
( )
( )
( )
25 | P á g i n a
( )
( )
( )
Sustituyendo en las expresiones anteriores se obtiene:
[
( ) ] [
( ) ]
[
( ) ] [
( ) ]
[
( ) ] [
( ) ]
Aplicando transformada de Laplace en las ecuaciones anteriores:
-( - - )
( )
( )
26 | P á g i n a
Utilizando la hipótesis de pequeños ángulos se puede despreciar los productos de
ángulos al cuadrado, dando lugar a:
En las expresiones simplificadas anteriores se puede observar que cada ángulo
está desacoplado del resto.
Si se aplica la transformada de Laplace y la hipótesis de pequeños ángulos para
el sistema de traslación se obtiene:
Dadas las ecuaciones anteriores se pueden tomar las siguientes conclusiones:
El subsistema de orientación está desacoplado del subsistema de traslación.
La posiciones X e Y están acopladas con los ángulos y respectivamente.
La posición Z no está acoplada con ninguna otra variable.
De los seis grados de libertad sólo se pueden controlar cuatro de ellos.
27 | P á g i n a
2.3. Control del quadrotor
A continuación se procederá a obtener los parámetros de control del modelo
obtenido.
El sistema del quadrotor presenta cuatro variables acopladas:
La posición X con el ángulo .
La posición Y con el ángulo .
Para las variables anteriores se utilizará la estructura cascada como se puede
observar en la Ilustración 2.4 y la Ilustración 2.5.
Ilustración 2.4 Estructura cascada de X y
Ilustración 2.5 Estructura cascada de
28 | P á g i n a
El control se ha realizado de forma discreta con un tiempo de muestreo ya que es típico de un quadrotor.
Para discretizar el integrador se ha optado por el método de Euler hacia atrás:
La ley de control de cada sistema viene dada por las siguientes expresiones:
(
)
(
)
(
)
siendo:
: la variable de entrada discretizada.
: el error discretizado.
: el tiempo de integración.
: el tiempo de derivación.
K: es un subíndice que indica el retraso de la variable.
2.3.1. Control de las variables acopladas X y
Para la variable X se ha obtenido que el controlador más óptimo es un PD con los siguientes parámetros:
29 | P á g i n a
Para la orientación se ha obtenido un PID con los siguientes parámetros:
A continuación se muestra en la Ilustración 2.6 con una referencia de una
circunferencia de radio 100, la superposición de la señal deseada y la señal obtenida
con control de la variable X.
Ilustración 2.6 Señal deseada y obtenida por control de X
2.3.2 Control de las variables acopladas Y e
En este caso la posición Y tendrá el siguiente controlador PD:
30 | P á g i n a
Para la orientación se ha obtenido un PID con los siguientes parámetros:
En la Ilustración 2.7 se representa para la referencia de una parábola, la
superposición de la señal deseada y la señal obtenida con control de la variable Y.
Ilustración 2.7 Señal deseada y obtenida por control de Y
31 | P á g i n a
2.3.3. Control de la variable Z
La variable Z presenta un controlador PID
En la Ilustración 2.8 se puede observar la superposición de la señal deseada y
obtenida para una referencia lineal.
Ilustración 2.8 Señal deseada y obtenida por control de Z
32 | P á g i n a
2.3.4. Control de la variable
Para finalizar la variable presenta un controlador PID
A continuación en la Ilustración 2.9 se muestra la superposición de la señal
deseada y obtenida para una referencia lineal.
Ilustración 2.9 Señal deseada y obtenida por control de
33 | P á g i n a
2.4. Conclusiones
A lo largo de este capítulo se han obtenido las siguientes conclusiones:
El modelo matemático del vehículo no tripulado es complejo debido a que presenta
términos nos lineales y hay un gran acoplamiento entre sus variables.
Se han obtenido dos subsistemas diferenciados: una para la orientación que depende
de los pares, y cuyas salidas son los ángulos de cabeceo, balanceo y guiñada, y otro
para la posición cuyas salidas son las posiciones X,Y y Z que depende del empuje
total final y de los ángulos anteriores.
Se ha linealizado y simplificado el modelo matemático para reducir el grado de
dificultad de este.
Tras linealizar y simplificar se han obtenido dos pares de variables acopladas (X, ) y
(Y, ).
Sólo podrán controlarse cuatro variables simultáneamente en el sistema del
quadrotor una vez linealizado y simplificado.
Se ha optado por controlar de forma discreta ya que es más rápido que de forma
analógica y es la que más de utiliza.
Se ha seleccionado un tiempo de muestreo de 0.05 segundos ya que es el típico de un
quadrotor.
Para las posiciones X e Y se ha seleccionado un controlador PD, mientras que para
el resto de variables se ha optado por controladores PID.
34 | P á g i n a
3 PLANIFICACIÓN
3.1. Introducción
En el siguiente capítulo se procederá al estudio de planificación de la trayectoria para
cumplir el objetivo de que la señal de la antena cubra los distintos sensores del suelo.
En el capítulo anterior, se afirmó mediante las ecuaciones del vehículo no tripulado, que no
es posible controlar la posición X y el ángulo θ, o el ángulo ф y la posición Y simultáneamente
debido al acoplamiento que existe en las ecuaciones del vehículo no tripulado.
Para cumplir el objetivo deseado, se partirá de las siguientes simplificaciones e hipótesis:
Las variables que se controlarán serán las posiciones: X, Y, Z y el ángulo de guiñada.
El vehículo no tripulado se moverá en todo momento a altura constante.
La antena del quadrotor estará situada en un eje perpendicular a la planta del vehículo y que
pasa por su centro de gravedad.
La antena se considerará rígida en el quadrotor, por lo que se moverá según lo haga el
vehículo que la contiene.
Por todo lo anterior, se realizarán los siguientes tipos de planificaciones:
Planificación de trayectorias rectilíneas.
Planificación de trayectoria circunferenciales unidas mediantes trayectorias rectilíneas.
A lo largo de este capítulo se desarrollarán las planificaciones anteriores, y se compararán
para conocer cuál es la más rápida y/o precisa.
3.1.1 Representación de los ejes del quadrotor
35 | P á g i n a
Para conocer el movimiento que tendrá el vehículo a lo largo de su trayectoria, se
representará en cada punto de la trayectoria: la posición y la orientación de los ejes en MATLAB.
Para obtener la orientación de los ejes en cada punto de la trayectoria se usará la matriz de
Euler:
(
)
En la Ilustración 3.1 aparecen los ejes representados en un punto:
Ilustración 3.1. Ejes del quadrotor
3.1.2 Representación de los ejes de la antena
La antena del vehículo tripulado, situada en el eje –Z, será representada de la misma
forma en que ha sido representados los ejes como se observa en la ilustración 3.2.
Ilustración 3.2. Representación de la antena
36 | P á g i n a
3.2. Planificación de trayectorias rectilíneas
Una primera opción para cumplir el objetivo de que la señal de la antena pase todos los
sensores será mediante la planificación de trayectorias rectilíneas.
Cuando el quadrotor se mueve en línea recta a velocidad constante, se puede observar
que la antena estará prácticamente en el eje –Z.
Debido a lo anterior, se buscarán trayectorias rectas para que el quadrotor vuele por los
puntos a los que se quiere orientar la antena, ya que las proyecciones del centro de gravedad del
quadrotor coincidirán con los puntos que la señal de la antena incide en el suelo.
La planificación de este tipo de trayectoria se realizará calculando los distintos tiempos
que tarda el quadrotor en recorrer cada recta para ir de un punto a otro.
3.2.1 Expresiones de la trayectoria rectilínea
A continuación, se indicarán las ecuaciones que se usarán en el algoritmo para
planificar estas trayectorias y calcular los distintos tiempos.
La ecuación de la trayectoria de la posición x vendrá dada por la siguiente expresión:
siendo:
v: la velocidad con la que se mueve el vehículo en el eje x.
:la posición inicial de x.
La coordenada x estará relacionada con la coordenada y mediante la siguiente ecuación:
37 | P á g i n a
Estas expresiones dependerán de si son mayores, menores o iguales las coordenadas del
punto del sensor donde se quiere ir con respecto a las coordenadas del sensor por donde acaba
de pasar el quadrotor.
Para que el cambio de cada trayectoria que va de un punto hacia otro punto , sea suave, el quadrotor empezará con una velocidad v=0.1 m/s e irá aumentando su
velocidad en los primeros tramos hasta alcanzar la velocidad deseada que se mantendrá
constante hasta llegar a los últimos tramos donde el vehículo frenará su velocidad a v= 0.1 m/s.
Esto puede verse reflejado en la Ilustración 3.3.
Ilustración 3.3. Distribución de la velocidad en un tramo rectilíneo.
En la figura, se puede observar tres tramos:
Primer tramo( : El quadrotor va primero a una velocidad de 0.25 m/s en los
primeros 0.25 metros(de ), en los siguientes 0.25 metros aumentará la velocidad
a 0.5 m/s de( ) , A continuación en los posteriores 0.5 metros la velocidad será de
1 m/s( ) .A partir del instante , la velocidad aumentará cada vez que
transcurra un metro hasta que alcance la velocidad, , lo que dará comienzo al tramo
intermedio .
Tramo intermedio ( - - : Este tramo dura un tiempo , e irá a la
velocidad a la que se dese que vuele, .
Tramo final ( - - : En este tramo la velocidad del quadrotor disminuirá
realizando un proceso inverso al que se hizo en el primer tramo, hasta llegar a los
últimos 0.25 metros donde su velocidad será de 0.1 m/s.
Al final de cada tramo ( ) la velocidad será de 0.1 m/s, lo que hará posible que el
vehículo tripulado pueda cambiar a otra trayectoria rectilínea sin presentar un aumento
brusco en los ángulos de cabeceo y balanceo.
38 | P á g i n a
A continuación, se presentará diferentes ejemplos de lo anterior distinguiendo
diferentes situaciones según si las coordenadas iniciales son menores, mayores o iguales
a las coordenadas finales
3.2.1.1 Xi<Xf
El caso en que Xi<Xf viene representado en ilustración 3.4.
Ilustración 3.4. Recta que representa el caso Xi<Xf
39 | P á g i n a
A continuación, se muestran algunos ejemplos:
Si :
Si - - :
Si - - :
3.2.1.2 Xi>Xf
Si la coordenada X del punto siguiente es inferior a la del punto anterior, como se
puede observar en la ilustración 3.5, se usarán las siguientes expresiones en el algoritmo:
Ilustración 3.5. Recta que representa el caso Xi>Xf
40 | P á g i n a
. Si :
. Si - - :
3.2.1.3 Xi=Xf
En el caso de que las coordenadas X de dos puntos seguidos coincidan, como aparece
en la Ilustración 3.6, las ecuaciones a implementar serán:
41 | P á g i n a
Ilustración 3.6. Recta que representa el caso Xi=Xf
Si :
Si :
42 | P á g i n a
3.2.1.4 Yi=Yf
El último caso a implementar en el algoritmo es en el cual los dos puntos donde se
desea que pase el vehículo no tripulado tengan la misma Y, como aparece en la Ilustración 3.7
Ilustración 3.7. Recta que representa el caso yi<yf
Si :
Si :
43 | P á g i n a
3.2.2 Algoritmo
El algoritmo para calcular la trayectoria consta de los siguientes pasos:
Elegir los dos puntos por los que pasa cada una de las rectas.
Seleccionar de que tipo es cada recta (creciente, decreciente, paralela al eje Y o paralela
al eje X).
En cada tramo recto calcular los tiempos en los que se produce los cambios de
velocidad, así como el tiempo en el que se llega al punto final del tramo.
Los tiempos obtenidos serán guardados en un vector.
Una vez iniciada la simulación, el código seleccionará el cambio de recta, de velocidad
y de parámetros según entre que tiempos de los almacenados está el tiempo de la
simulación.
A continuación, se hará un ejemplo práctico de este algoritmo. Para ello, se usará la
siguiente ilustración 3.8 que representa los diferentes tramos rectos por los que vuela el
vehículo, y las coordenadas por las que se quiere que pase el quadrotor (puntos rojos).
Ilustración 3.8. Unión de tramos de trayectorias rectilíneas
44 | P á g i n a
Inicialmente, se calculan los parámetros de cada recta y se almacenan en un vector:
[ ]
[ ]
Se puede observar que en el algoritmo los parámetros tomarán el valor cero en los
tramos donde las variables X o Y sean constantes.
Después se calcularán los tiempos de llegada al final de cada recta y se almacenarán en
un vector:
[ ]
Posteriormente, se elegirá el tipo de recta y sus ecuaciones según entre que intervalos
del vector tiempos está el tiempo de simulación.
Ejemplos:
Si : La recta elegida será la primera con los parámetros y .
Si : La recta elegida será la tercera.
Si : La recta elegida será la sexta con los parámetros y .
Durante cada tramo recto el ángulo de guiñada se mantendrá constante y será el que
proporcione que la trayectoria forme 45 ˚con los ejes del vehículo.
3.2.3 Selección de dos puntos para cada recta
Para minimizar el tiempo en el que vehículo cumple el objetivo, cada tramo recto estará formado por
los dos puntos que presenten menor distancia entre ellos.
Este proceso se realiza mediante el siguiente algoritmo en el que x e y son los vectores de
puntos por donde debe pasar la señal antena. Este vector incluye las coordenadas del origen de
referencia.
Este proceso se puede observar en el algoritmo de la ilustración 3.9
45 | P á g i n a
No
Sí
No
Sí
Inicio
Leer x,y,n
𝑣𝑚𝑖𝑛 ← 𝑑 ; 𝑑𝑠 ← ∞
𝑦𝑠𝑎𝑣𝑒 ← 𝑦𝑠 ; 𝑥𝑠𝑎𝑣𝑒 ← 𝑥𝑠
𝑦𝑠 ← 𝑦𝑣𝑚𝑖𝑛 ; 𝑥𝑠 ← 𝑥𝑣𝑚𝑖𝑛
𝑦𝑣𝑚𝑖𝑛 ← 𝑦𝑠𝑎𝑣𝑒 ; 𝑥𝑣𝑚𝑖𝑛
← 𝑥𝑠𝑎𝑣𝑒
𝑠 ← ; 𝑑 ← ∞
;;
r b í l ó
¿ 𝑠 𝑛?
𝑑𝑘 ← 𝑠𝑞𝑟𝑡 𝑥𝑘 − 𝑥𝑠− ^ 𝑦𝑘 − 𝑦𝑠− ^
¿ 𝑘 𝑛?
𝑘 ← 𝑘
𝑠 ← 𝑠
fin
𝑘 ← 𝑠
46 | P á g i n a
x Vector que contiene la coordenada X. Dato
y Vector que contiene la coordenada de Y .Dato
k Contador. Variable auxiliar
s Contador. Variable auxiliar
n Dimensión del vector x e y. Dato
d Resultado. Vector de distancia.
min(d) Función que calcula un vector cuya primera componente es el valor más pequeño del
vector y la segunda componente es el índice del vector. Su argumento es el vector de
distancia.
dk Componente k-ésima del vector d.
ysave Variable auxiliar para almacenar la componente s del vector y.
xsave Variable auxiliar para almacenar la componente s del vector x.
Ilustración 3.9. Algoritmo que ordena los puntos según la cercanía de unos con otros.
47 | P á g i n a
3.3 Planificación de trayectorias circunferenciales unidas mediantes trayectorias rectilíneas.
Durante este apartado se va analizar la posibilidad de cumplir el objetivo mediante
trayectorias circunferenciales unidas con tramos rectilíneos.
Para este tipo de planificación, se usará la propiedad de que una circunferencia está
constituida por tres puntos.
A continuación, se explicarán diferentes cálculos a tener en cuenta en el algoritmo y
finalmente se desarrollará dicho código.
48 | P á g i n a
3.3.1 Obtención de parámetros de la circunferencia
En esta planificación, será necesario obtener la ecuación de la circunferencia dado tres
puntos.
Para ello, se utilizarán las siguientes ecuaciones:
√
En el siguiente diagrama de flujo (Ilustración 3.10) ,se calculan los parámetros de una
circunferencia dados tres puntos:
49 | P á g i n a
No
Sí
Inicio del cálculo de R,xc,yc,x0,y0
como generadorcircu(x,y)
𝑘 ←
¿𝑘 ?
𝑘 ← 𝑘
𝑐𝑘 ← − 𝑥𝑘 ∙ 𝑥𝑘 𝑦𝑘 ∙ 𝑦𝑘
𝑎𝑘 ← 𝑥𝑘
𝑎𝑘 ← 𝑦𝑘
𝑎𝑘 ←
𝑥𝑐 ← −𝑏
𝑦𝑐 ← −𝑏
𝑅 ← 𝑠𝑞𝑟𝑡 −𝑏 𝑥𝑐𝑥𝑐 𝑦𝑐𝑦𝑐
𝑥 𝑥𝑐 − 𝑅
𝑦 𝑦𝑐
Cálculo de la matriz B
A*B=C
fin
50 | P á g i n a
x Argumento.Vector de posición x de puntos que forman la circunferencia.
y Argumento.Vector de posición y de puntos que forman la circunferencia.
Primer vector columna de la matriz A que contiene el vector x.
Primer vector columna de la matriz A que contiene el vector y.
Primer vector columna de la matriz A que contiene un vector de unos.
c Resultado. Vector C.
B Resultado. Vector B.
xc Resultado de la función generadorcircu(x,y). Centro x de la circunferencia
yc Resultado de la función generadorcircu(x,y).Centro y de la circunferencia
R Resultado de la función generadorcircu(x,y).Radio de la circunferencia.
x0 Resultado de la función generadorcircu(x,y).
x0 Resultado de la función generadorcircu(x,y).
Ilustración 3.10. Algoritmo que define circunferencias
51 | P á g i n a
3.3.2 Ecuaciones paramétricas de la circunferencia
Las circunferencias que recorrerá el vehículo no tripulado serán implementadas en el código
mediante sus expresiones paramétricas:
( l ⁄ )
( l ⁄ )
La circunferencia será recorrida en sentido horario y a partir del punto que tenga menor
valor de X e Y, por lo que se usarán las siguientes ecuaciones:
l l
( l
⁄ )
( l
⁄ )
siendo:
: velocidad angular que lleva el vehículo.
l : ángulo que indica la posición anterior por la que paso el vehículo.
l : parámetro que aparece en las ecuaciones de las circunferencias y que indica la
posición de la circunferencia en que se encuentra el vehículo.
3.3.3 Velocidad angular del quadrotor
Si el quadrotor realiza una trayectoria circunferencial, su velocidad angular inicial será de
0.01 deg/s e irá aumentando hasta llegar a la velocidad angular deseada . A partir de este instante,
el quadrotor mantendrá dicha velocidad angular durante 360 deg para que la señal de la antena
llegue a todos los puntos deseados. Al final del trayecto, cuando el vehículo haya terminado de dar
una vuelta con la velocidad angular deseada ( - , el quadrotor disminuirá su velocidad angular
hasta que sea w= 0.01 deg/s.
En la Ilustración 3.11, se representa el perímetro de la circunferencia:
52 | P á g i n a
Ilustración 3.11. Distribución de velocidades angulares a lo largo de la circunferencia
Según entre que tiempos esté el tiempo de simulación, se tendrán los siguientes parámetros:
Si : La velocidad angular será de 0.01 deg/s.
Si : La velocidad angular será de 0.02 deg/s.
Si : La velocidad angular será de 0.03 deg/s.
Si - : La velocidad angular será de - deg/s.
Si : el quadrotor habrá recorrido n grados de la circunferencia y la velocidad
angular será la máxima indicada por el algoritmo, deg/s.
Si : el vehículo habrá dado una vuelta completa a la circunferencia volviendo al
punto inicial a la velocidad máxima indicada en el algoritmo deg/s
Si - : En este caso el dron ha dado una vuelta completa y ha girado n grados más,
a partir de este tiempo la velocidad angular pasa de deg/s a - deg/s
Si - - : La velocidad angular será de 0.02 deg/s.
Si - : La velocidad angular será de 0.01 deg/s.
Se puede deducir de lo anterior:
De a : la velocidad angular aumentará hasta la que se dese que vuele, .
De a - : El quadrotor dará una vuelta completa a la velocidad angular deseada, ,
para que la señal de la antena pasa por donde los sensores que están asociados a esa
circunferencia.
De - a : El vehículo no tripulado disminuirá su velocidad angular hasta 0.01 deg/s ,
para que no sea brusco el cambio que experimente sus ángulos cuando empiece a
realizar una trayectoria rectilínea.
La velocidad angular a la que se quiere que vuele el quadrotor vendrá dada por las
siguientes expresiones:
r
r
53 | P á g i n a
siendo:
: la velocidad a la que vuela el quadrotor.
la velocidad angular del vehículo no tripulado.
: El radio de vuelo.
3.3.4 Elección del radio de curvatura de vuelo
Si el quadrotor realiza una trayectoria circunferencial de radio R, el lugar geométrico de
todos los puntos donde incide la antena será otra circunferencia del mismo centro, y cuyo radio
dependerá de la velocidad del vehículo, la altura y el ángulo de guiñada.
Durante la trayectoria circunferencial, el ángulo de guiñada elegido será el que haga que la
distancia que hay entre el centro de la circunferencia en el plano Z=0, y el punto donde
incide la antena en el suelo sea lo mayor posible.
Para cumplir el objetivo de que la señal de la antena llegue a los sensores inalámbricos, se
buscará el radio de vuelo necesario que haga que el radio de la circunferencia formada por los
puntos que la señal de antena incide en el suelo , sea el de la circunferencia que pase por las
coordenadas donde se encuentran los sensores inalámbricos.
Para visualizar lo anterior, se usará la Ilustración3.12, donde la línea de color azul representa
la circunferencia formada por la proyección de los puntos de centro de gravedad del vehículo (R') y
la de color rojo el lugar geométrico de los puntos que la señal de la antena incide en el suelo (Rp').
Ilustración 3.12.Circunferencias en el plano Z=0
54 | P á g i n a
En el gráfico anterior se puede observar que es la distancia que hay entre la
proyección del centro de gravedad de un punto, y el punto donde la señal de la antena incide en
ese momento para un ángulo de guiñada que haga que la distancia que hay entre el centro de la
circunferencia y el punto donde incide la señal de la antena sea lo mayor posible.
La distancia d’ dependerá de la velocidad angular de la circunferencia y de la altura.
En la ilustración 3.13 y en la ilustración 3.14 , se puede observar como evolucionaría la
distancia d’ en función del radio de vuelo del quadrotor para una altura y una velocidad
angular determinada.
Ilustración 3.13. Relación de d’ y R’ para w= 3 deg/s
55 | P á g i n a
Ilustración 3.14. Relación de d’ y R’ para w= 5 deg/s
Observando los gráficos anteriores, se puede deducir que la relación
es prácticamente
constante para una altura y una velocidad angular determinada. Por esta razón, se supondrá está
relación conocida para varias velocidades angulares y alturas en el algoritmo de planificación.
A partir de las siguientes ecuaciones, se obtiene el radio de vuelo necesario :
(
)
56 | P á g i n a
3.3.5 Selección de circunferencias
Para elegir las circunferencias por las que va a volar el quadrotor, se pueden usar los
siguientes métodos:
Buscar las circunferencias que pasen por el mayor número de puntos.
Seleccionar las circunferencias constituidas por los tres puntos más cercanos.
El primer método es más largo y tedioso que la segunda alternativa, por lo que ha optado
por desarrollar el segundo método que será desarrollado a lo largo de este capítulo.
En este método se cumplirá el criterio que se puede observar siguiente ilustración 3.15.
Ilustración 3.15.Criterio de distancia elegido
57 | P á g i n a
El primer punto que formará la circunferencia será el que esté a menos distancia
( ) de la anterior circunferencia , o del eje de referencia si es el de la primera
circunferencia formada.
El segundo punto que formará la circunferencia será que el que tenga menos
distancia respecto del primer punto .
El tercer punto que forma la circunferencia será el que tenga menor la media
(
) entre la distancia al primer punto y al segundo
punto .
Se deberá comprobar que el tercer punto no esté alineado con los dos anteriores. En el caso
de que esté alineado se cogerá el siguiente punto que tenga menor la media entre la distancia
al primer y al segundo punto.
Una vez formada la circunferencia con los tres puntos anteriores, se comprobará si los otros
puntos restantes pasan por esta circunferencia para que no participe en el proceso de
obtención del resto de circunferencias.
Todas las circunferencias formadas estarán unidas mediantes trayectorias rectas como queda
reflejado en la Ilustración.3.16. El quadrotor pasará primero por la circunferencia formada por los
puntos { } y luego por la formada por los puntos { }
Ilustración 3.16.Representación del movimiento en las trayectorias
58 | P á g i n a
Se formará una circunferencia cuyo centro estará en la mediatriz de la distancia que hay
entre esos dos puntos .(Ilustración 3.17)
Ilustración 3.17. Criterio en el caso de que queden dos puntos sin asignar circunferencia
Si se han formado todas las circunferencias y queda un punto restante:
Se unirá mediante una trayectoria recta: ese punto y la última circunferencia por la que
vuela el quadrotor (Ilustración 3.18)
59 | P á g i n a
Ilustración 3.18.Criterio en el caso de que quede un punto sin asignar circunferencia
3.3.6 Cálculo de tiempos de trayectorias.
Una vez obtenidas las circunferencias y las rectas que cumplen el objetivo, se procede a
calcular los diferentes tiempos que se tarda en llegar a cada tipo de trayectoria. Según entre que
valores de estos tiempos esté el tiempo de simulación , se podrá seleccionar la trayectoria adecuada a
ese momento.
En la Ilustración 3.19 aparece una serie de trayectorias con su tiempo asignado:
Ilustración 3.19. Serie de trayectorias con tiempos asociados
60 | P á g i n a
Se elegirá el tipo trayectoria según los tiempos entre los que esté el tiempo de
simulación:
Ejemplos:
Si : La ecuaciones elegidas serán las asociadas a la primera recta.
Si : La ecuaciones elegidas serán las asociadas a la primera
circunferencia.
Si : La ecuaciones elegidas serán las asociadas la segunda recta.
Si : La ecuaciones elegidas serán las asociadas a la última
circunferencia.
El tiempo a calcular será el de llegar a una circunferencia o a una recta según sea par o
no el índice k de su componente del tiempo:
Si es par: Es el de llegada de la siguiente recta.
Si es impar: Es el de llegada de la siguiente circunferencia.
Las expresiones para calcular el tiempo que se tarda en cada trayectoria son las siguientes:
Tiempo en llegar a la nueva circunferencia :
Tiempo en llegar a la nueva recta:
←
En el caso de que falte un punto al que no le ha sido asignado una circunferencia: Este
punto será unido a la última circunferencia mediante una trayectoria recta, y el parámetro
auxiliar activadorleft1 pasará a tomar el valor 1, como se indica en la Ilustración 3.20. Esto dará
lugar a un aumento del vector de tiempos, que ahora contendrá el tiempo que se tarda en llega a
este último punto.
Ilustración 3.20.Serie de trayectorias con recta que une el último punto
61 | P á g i n a
En el caso de que falten dos puntos por asignar una circunferencia: Estos puntos
formarán una circunferencia y se unirán a la anterior circunferencia mediante una recta como se
indica en la siguiente ilustración 3.21.
Si esto ocurriera el valor de activadorleft2 pasaría de ser cero a uno, lo que indicaría que
el vector de tiempos aumentaría para tener en cuenta está ultima circunferencia.
Ilustración 3.21.Serie de trayectorias con las trayectorias que unen los dos últimos punto
3.3.7 Algoritmo A continuación, se desarrollará el algoritmo que implementará lo expuesto en los
apartados anteriores. Dicho algoritmo consta de los siguientes pasos:
Calcular inicialmente el número de circunferencias (grupo de tres puntos) que existen.
Calcular el punto más cercano al eje de referencia P1.( Ilustración 3.22)
Calcular el punto más cercano al punto anterior P2.( Ilustración 3.23)
Calcular el punto que tenga la menor media en la distancia a los puntos anteriores a P3.
(Ilustración 3.24)
Comprobar que el tercer punto no está alineado con los dos anteriores.
En caso de estar alineado, se cogerá como P3 el siguiente punto que tenga menor media
en la distancia a los puntos anteriores.
Una vez seleccionado los tres puntos, se obtendrán los parámetros de la circunferencia
que pasa por esos puntos: , . , .
A continuación, se calcula la velocidad angular de la circunferencia, así como el radio
de vuelo . (Ilustración 3.25)
Se comprueba si algún punto pertenece a la circunferencia calculada. (Ilustración 3.26)
Se crea un nuevo vector de puntos en el que se excluye los puntos que pertenecen a la
circunferencia calculada anteriormente. (Ilustración 3.27)
62 | P á g i n a
Se calcula de nuevo el número de circunferencias que pueden formar los puntos del
nuevo vector, y se repiten los mismos pasos hasta que el vector de puntos sea menor
que tres.
Si el vector de puntos es de dimensión dos, se calculará la distancia entre eso dos puntos
y el radio de la nueva circunferencia que será la mitad de esa distancia. A continuación
se calculará el radio de vuelo asociado a la nueva circunferencia . El centro de la
circunferencia se calculará con las siguientes expresiones: (Ilustración 3.28)
( )
( )
63 | P á g i n a
Si el vector de punto es de dimensión uno, se guardarán las coordenadas de ese
punto, el cual más tarde se unirá mediante una trayectoria recta a la última circunferencia.
Cuando se haya llegado a la última circunferencia posible, se habrá finalizado este código.
Las salidas de este código serían un vector , , y los siguientes valores si los
hubiera : , , .
Se calculan los puntos , , que son los puntos en los que se cambia de trayectoria
circunferencial a trayectoria recta.
Una vez calculadas todas las circunferencias, se obtendrá la trayectoria de la recta que lleva
hacia esas circunferencias. Para ello se calculan los parámetros m y n de cada recta.
(Ilustración 3.29).
Los primeros parámetros m1 y n1 serán los de la recta que une el origen de coordenadas con
la primera circunferencia y se tendrá en cuenta si la recta es paralela a algunos de los ejes.
El resto de rectas que se calcularán serán entre los puntos de la circunferencia que presentan
menor valor de x: . Esto se puede observar en la (Ilustración 3.30)
A continuación. se calculan los diferentes tiempos en que se recorre cada tramo y se
almacenan en un vector.
Para finalizar, se seleccionará el tipo de trayectoria según entre que valores del vector de
tiempo esté el tiempo de simulación.
64 | P á g i n a
No
Sí
Sí No
Inicio del cálculo del primer
punto que forme parte de la
circunferencia
𝑘 ←
¿𝑘 𝑛?
𝑘 ← 𝑘
𝑣𝑚𝑖𝑛 ← 𝑑
𝑘 ← 𝑣𝑚𝑖𝑛
Leer x,y,n,x0,y0,m
𝑑𝑘 ← 𝑠𝑞𝑟𝑡 𝑥𝑘 − 𝑥 𝑚− ^ 𝑦𝑘 − 𝑦 𝑚− ^
¿𝑚 ?
𝑑𝑘 ← 𝑠𝑞𝑟𝑡 𝑥𝑘 ^ 𝑦𝑘 ^
fin
65 | P á g i n a
x Vector que contiene la coordenada X. Dato.
y Vector que contiene la coordenada de Y .Dato.
k Contador. Variable auxiliar.
n Dimensión del vector x e y. Dato.
m Índice de circunferencia formada. Dato.
d Resultado. Vector distancia respecto a anterior circunferencia o eje de referencia.
min(d) Función que calcula un vector cuya primera componente es el valor más pequeño del
vector d y la segunda componente es el índice del vector. Su argumento es el vector de
distancia.
k1 Índice de la posición del primer punto de la circunferencia en el vector x o y.
x0 Vector que contiene las coordenadas X de menor valor de cada circunferencia.
y0 Vector que contiene las coordenadas Y del menor valor X de cada circunferencia.
Ilustración 3.22 .Algoritmo que calcula el primer punto de la circunferencia
66 | P á g i n a
No
Sí
Inicio del cálculo del segundo
punto que forme parte de la
circunferencia
𝑘 ←
¿𝑘 𝑛?
𝑘 ← 𝑘
𝑑𝑘 ← ∞
𝑣𝑚𝑖𝑛 ← 𝑑
𝑘 ← 𝑣𝑚𝑖𝑛
𝑘 ← 𝑣𝑚𝑖𝑛
Leer x,y,n,k1
𝑑𝑘 ← 𝑠𝑞𝑟𝑡 𝑥𝑘 − 𝑥𝑘 ^ 𝑦𝑘 − 𝑦𝑘 ^
fin
67 | P á g i n a
x Vector que contiene la coordenada X. Dato.
y Vector que contiene la coordenada de Y. Dato.
k Contador. Variable auxiliar.
n Dimensión del vector x e y. Dato.
k1 Índice de la posición del primer punto de la circunferencia en el vector x o y.
d Resultado. Vector distancia de puntos respecto al primero.
min(d) Función que calcula un vector cuya primera componente es el valor más pequeño del
vector d y la segunda componente es el índice del vector. Su argumento es el vector
de distancia.
k2 Índice de la posición del segundo punto de la circunferencia en el vector x o y.
Ilustración 3.23.Algoritmo que calcula el segundo punto de la circunferencia
68 | P á g i n a
No
Sí
Inicio del cálculo del tercer
punto que forme parte de la
circunferencia
𝑘 ←
¿𝑘 𝑛?
𝑘 ← 𝑘
𝑑𝑘 ← ∞
𝑑𝑘 ← ∞
𝑣𝑚𝑖𝑛 ← 𝑑
𝑘 ← 𝑣𝑚𝑖𝑛
𝑘 ← 𝑣𝑚𝑖𝑛
Leer x,y,n,k1,k2
𝑑 𝑘 ← 𝑠𝑞𝑟𝑡 𝑥𝑘 − 𝑥𝑘 ^ 𝑦𝑘 − 𝑦𝑘 ^
𝑑 𝑘 ← 𝑠𝑞𝑟𝑡 𝑥𝑘 − 𝑥𝑘 ^ 𝑦𝑘 − 𝑦𝑘 ^
𝑑 𝑘 ← 𝑑 𝑘 𝑑 𝑘
⁄
Comprobar si el punto está en línea recta con los dos
anteriores y si el caso es afirmativo pasará su distancia
𝑑 𝑘 a ser infinita
fin
69 | P á g i n a
Ilustración 3.24.Algoritmo que calcula el tercer punto de la circunferencia
x Vector que contiene la coordenada X. Dato.
y Vector que contiene la coordenada Y .Dato.
k Contador. Variable auxiliar.
n Dimensión del vector x e y. Dato.
k1 Índice de la posición del primer punto de la circunferencia en el vector x o y.
d13 Resultado. Vector distancia respecto del primer punto que forma la circunferencia
d23 Resultado. Vector distancia respecto del segundo punto que forma la circunferencia
d3 Resultado. Vector que calcula la media entre las distancias al primer y segundo punto.
min(d) Función que calcula un vector cuya primera componente es el valor más pequeño del
vector d y la segunda componente es el índice del vector. Su argumento es el vector de
distancia.
k2 Índice de la posición del segundo punto de la circunferencia en el vector x o y. Dato.
k3 Índice de la posición del tercer punto de la circunferencia en el vector x o y.
70 | P á g i n a
No
Sí
No Sí
Inicio de obtención de velocidad
angular y radio de vuelo para una
circunferencia del quadrotor
𝑠 ←
𝑘 ←
¿𝑘 𝑛?
Leer R,n,w,v,relacionrd
𝑅𝑛 𝑅𝑝/
𝑟𝑒𝑙𝑎𝑐𝑖𝑜𝑛𝑟𝑑
¿𝑤𝑛 ≥ 𝑤𝑠?
𝑘 ← 𝑘
𝑠 ← 𝑠
𝑅 ← 𝑅𝑛
𝑤 ← 𝑤𝑠
𝑤𝑛 ← 𝑣
𝜋
𝑤𝑛 ← 𝑓𝑙𝑜𝑜𝑟 𝑤𝑛
fin
71 | P á g i n a
Ilustración 3.25 Algoritmo que calcula el radio de curvatura de vuelo y la velocidad
angular
Rp Radio que forman los puntos que inciden la antena en el suelo. Dato.
R Radio de vuelo del quadrotor.
w Vector de velocidades angulares de mayor a menor y cuyos valores son
enteros.
relacionrd Vector decreciente que contiene la relación R/d con respecto a su
velocida angular.
v Velocidad de vuelo del quadrotor
k Contador.
s Contador.
floor(x) Función que cálculo la parte entera de un valor numérico y cuyo
argumento es x.
n Número de componentes del vector w y relacionrd.
Rn Variable auxiliar para almacenar datos.
wn Variable auxiliar para almacenar datos.
72 | P á g i n a
No
Sí
No
Sí s
Inicio del cálculo del vector Ks que
contiene los puntos que están dentro
de la circunferencia
𝑘 ←
¿𝑘 𝑛?
𝑘 ← 𝑘
Leer x,y,n,m,xc,yc,R
𝑐𝑜𝑚𝑝𝑟 ← 𝑥𝑘 − 𝑥𝑐𝑚 𝑦𝑘 − 𝑦𝑐𝑚 − 𝑅𝑚
𝑘𝑠𝑘 ←
¿ 𝑐𝑜𝑚𝑝𝑟 𝑎𝑏𝑠 − ?
𝑖𝑛𝑠𝑖𝑑𝑒 ← 𝑖𝑛𝑠𝑖𝑑𝑒
𝑘𝑠𝑘 ← 𝑘
fin
73 | P á g i n a
Ilustración 3.26 .Algoritmo que calcula el vector Ks que indica los puntos que están dentro
de la circunferencia.
x Vector que contiene la coordenada X. Dato.
y Vector que contiene la coordenada Y .Dato.
n Dimensión del vector x e y. Dato.
xc Posición x del centro de la circunferencia .
yc Posición y del centro de la circunferencia .
R Radio de la circunferencia. Dato.
compr Comprobador de si un punto está dentro de la circunferencia. Variable entera.
inside Número de puntos que están dentro de la circunferencia,
Ks Resultado. Vector nulo, en el que el valor de la componente coincidirá con su
índice, y con el índice del vector x o y , del punto que pertenece a la
circunferencia.
m Índice del número de la circunferencia. Dato.
k Contador. Variable auxiliar.
74 | P á g i n a
No
Sí
Sí No
Inicio de la obtención del nuevo
vector xp yp excluyendo los puntos
de las circunferencias ya formadas
𝑠 ←
𝑘 ←
¿𝑘 𝑛?
Leer x,y,ks,n
𝑙 ← 𝑘 − 𝑠
¿𝑘 𝑘𝑠?
𝑘 ← 𝑘
𝑠 ← 𝑠 𝑥𝑝𝑙 ← 𝑥𝑘
𝑦𝑝𝑙 ← 𝑦𝑘
fin
75 | P á g i n a
Ilustración 3.27.Algoritmo que calcula los nuevos vectores excluyendo puntos que ya han
formado circunferencias.
x Vector que contiene la coordenada X. Dato.
y Vector que contiene la coordenada Y .Dato.
n Dimensión del vector x e y. Dato.
k Contador. Variable auxiliar.
s Contador. Variable auxiliar.
l Contador. Variable auxiliar.
Ks Resultado. Vector nulo, en el que el valor de la componente coincidirá con su
índice, y con el índice del vector x o y del punto que pertenece a la circunferencia.
Dato.
xp Resultado. Nuevo vector x en el que se excluyen los puntos que ya han formado
circunferencias.
yp Resultado. Nuevo vector x en el que se excluyen los puntos que ya han formado
circunferencias.
76 | P á g i n a
Inicio del cálculo de la
circunferencia en el caso de que
queden dos puntos
Leer x,y,R_d
𝑑 ← 𝑠𝑞𝑟𝑡 𝑥 − 𝑥 ^ 𝑦 − 𝑦 ^
𝑅𝑙 ← 𝑑 ⁄
𝑅 ← 𝑅𝑙 ∙ ( 𝑅_𝑑⁄ )
−
𝑣𝑚𝑎𝑥 ← 𝑥
𝑣𝑚𝑖𝑛 ← 𝑥
𝑥𝑐𝑙 ←(𝑥 𝑣𝑚𝑎𝑥 − 𝑥 𝑣𝑚𝑖𝑛 )
𝑥 𝑣𝑚𝑖𝑛
𝑦𝑐𝑙 ←(𝑦 𝑣𝑚𝑎𝑥 − 𝑦 𝑣𝑚𝑖𝑛 )
𝑦 𝑣𝑚𝑖𝑛
𝑎𝑐𝑡𝑖𝑣𝑎𝑑𝑜𝑟_𝑙𝑒𝑓𝑡 ←
𝑅𝑚 𝑎𝑐𝑡𝑖𝑣𝑎𝑑𝑜𝑟_𝑙𝑒𝑓𝑡 ← 𝑅
𝑥𝑐𝑚 𝑎𝑐𝑡𝑖𝑣𝑎𝑑𝑜𝑟_𝑙𝑒𝑓𝑡 ← 𝑥𝑐𝑙
𝑦𝑐𝑚 𝑎𝑐𝑡𝑖𝑣𝑎𝑑𝑜𝑟_𝑙𝑒𝑓𝑡 ← 𝑦𝑐𝑙
fin
77 | P á g i n a
x Vector que contiene la coordenada X. Dato.
y Vector que contiene la coordenada Y .Dato.
R_d Relación
. Dato.
d Resultado. Distancia que hay entre dos puntos.
Rl Índice de la posición del segundo punto de la circunferencia en el vector x
o y. Dato.
R2 Resultado. Radio de vuelo.
xcl Resultado. Posición x del centro de la circunferencia.
ycl Resultado. Posición y del centro de la circunferencia.
min(d) Función que busca componente mínima de un vector.
max(d) Función que busca la componente máxima de un vector.
vmin Vector, cuya primera componente es el valor más pequeño x o y, y la
segunda la componente
vmax Vector, cuya primera componente es el valor máximo del vector x o y, y la
segunda la componente
Activador_left2 Variable auxiliar indica que quedan solo dos puntos.
78 | P á g i n a
Ilustración 3.28.Algoritmo que calcula los parámetros en el que caso de que queden dos
puntos restantes
79 | P á g i n a
Sí No
No Sí
Leer x0,y0
Inicio del cálculo de los parámetros
de la recta que llevan a la primera
circunferencia
¿ 𝑥 ?
𝑚 ←
𝑛 ← 𝑚 ←
𝑦
𝑥
𝑛 ← −𝑚 ∙ 𝑥 𝑦
¿𝑦 ?
𝑚 ←
𝑛 ←
fin
80 | P á g i n a
Ilustración 3.29.Algoritmo que calcula los parámetros de la recta que lleva a la primera
circunferencia
x0 Vector que contiene las coordenadas X de menor valor de cada circunferencia. Dato.
y0 Vector que contiene las coordenadas Y del menor valor X de cada circunferencia. Dato.
m1 Resultado. Pendiente de la primera recta.
n1 Resultado. Parámetro n de la primera recta.
81 | P á g i n a
No
Sí
Sí No
No Sí
Leer x0,y0,xf,yfnc,m1,n1
Inicio del cálculo de parámetros de
las rectas que conectan con el resto
de circunferencias
𝑘 ←
¿𝑘 𝑛𝑐?
¿ 𝑥 𝑘 𝑥𝑓𝑘− ?
𝑚𝑘 ←
𝑛𝑘 ← 𝑚𝑘 ←
𝑦 𝑘−𝑦𝑓𝑘−
𝑥 𝑘−𝑥𝑓𝑘−
𝑛𝑘 ← −𝑚𝑘 ∙ 𝑥 𝑘 𝑦 𝑘
¿ 𝑦 𝑘 𝑦𝑓𝑘− ?
𝑚𝑘 ←
𝑛𝑘 ←
𝑘 ← 𝑘
fin
82 | P á g i n a
Ilustración 3.30 .Algoritmo que calcula los parámetros de las diferentes rectas que unen a
las circunferencias
x0 Vector que contiene las coordenadas X de menor valor de cada circunferencia. Dato.
y0 Vector que contiene las coordenadas Y del menor valor X de cada circunferencia. Dato.
m1 Pendiente de la primera recta. Dato.
n1 Resultado. Parámetro n de la primera recta. Dato.
m Resultado. Vector de pendientes de las diferentes rectas.
n Resultado. Vector de parámetros n de las diferentes rectas.
k Contador. Variable auxiliar.
nc Número de circunferencias. Dato.
xf Vector que contiene las coordenadas x del punto de la circunferencia en el que se cambia a
tramo recto.
yf Vector que contiene las coordenadas y del punto de la circunferencia en el que se cambia a
tramo recto.
83 | P á g i n a
3.4 Comparación de ambos métodos
A continuación, se procederá a hacer dos ejemplos mediante los dos métodos expuestos
anteriormente presentando su tiempo y los errores numéricos que existen:
El primer ejemplo será para una velocidad de 20 m/s, una altura de 30 metros y con las
siguientes coordenadas para los sensores:
Usando en el primer método se tarda en cumplir el objetivo: un tiempo de 90 segundos.
En la ilustración 3.31 se puede observar como los ejes del quadrotor están encima de los
puntos por donde tiene que pasar el vehículo. En el gráfico, la línea verde será la ruta por donde
pasa el vehículo, los puntos negros del plano Z=0 serán los puntos donde se encuentran los sensores
y los asteriscos azules del plano Z=0 representan los puntos de incidencia de la señal de la antena
más cercanos a ellos.
Ilustración 3.31.Gráfica de simulación para el primer ejemplo con el primer método
84 | P á g i n a
Los errores vendrán dados por la distancia que hay entre los asteriscos azules y los círculos
negros:
rr r
Mediante el segundo método, se tardar un tiempo de 75 segundos en cumplir el objetivo, y
se puede observar en la Ilustración 3.32.
Ilustración 3.32. Gráfica de la simulación para el primer ejemplo con el segundo método
Los errores mediante este método son los siguientes:
rr r
85 | P á g i n a
El siguiente ejemplo será con los siguientes vectores de coordenadas para los sensores
a una velocidad de 20 m/s y a una altura de 30 metros:
Usando el segundo método el tiempo que se tarda en cumplir el objetivo es de 240
segundos. (Ilustración 3.33)
Ilustración 3.33.Gráfica de la simulación para el segundo ejemplo con el segundo método
86 | P á g i n a
Los errores que presenta el método serán los siguientes:
rr r
rr r
rr r
Si se usa el primer método se tarda en cumplir el objetivo 143.3684 segundos
aproximadamente. (Ilustración 3.34)
Siendo los errores:
rr r
Ilustración 3.34 .Gráfica de la simulación para el segundo ejemplo con el primer método
87 | P á g i n a
Se puede deducir que en el primer ejemplo, el método rectilíneo es más lento en cumplir el
objetivo que con el método que mezcla tramos circunferenciales y rectos. Sin embargo para el
segundo ejemplo es mucho más rápido el primer método que el segundo.
3.5 Conclusiones
A lo largo de este capítulo, se han desarrollado diferentes tipos de planificaciones para que
se cumpla el objetivo de que la señal de la antena del quadrotor pasé por los puntos donde están
situados los sensores de manera eficiente.
La primera alternativa consiste en usar trayectorias rectilíneas para de ir de un punto a otro.
En este método se ha elegido el orden de los puntos de los sensores de forma que haya la menor
distancia posible entre cada punto.
En la segunda alternativa el vuelo del quadrotor para cumplir el objetivo será mediante
tramos rectilíneos y circunferenciales. El criterio de este método será que las circunferencias
seleccionadas sean las constituidas por los tres puntos, donde están situados los sensores, más
cercanos.
Se ha deducido que dependiendo de la distribución de las coordenadas de los sensores, es
más eficiente un método u otro.
88 | P á g i n a
4 EXPERIMENTOS
4.1 Introducción
A lo largo de este capítulo, se realizará una descripción del sistema, y se desarrollarán una
serie de experimentos tanto de control como de planificación, con los que se analizará la robustez
del sistema, así como la influencia que tiene la altura o la velocidad del vehículo en el sistema.
Los experimentos de planificación se centrarán en la planificación que mezcla trayectorias
rectas y circunferenciales ya que es la más compleja.
4.2 Descripción del sistema
En la Ilustración 4.1 se puede observar el sistema completo:
Ilustración 4.1. Esquema del Sistema
En el esquema anterior se puede distinguir los siguientes bloques:
PLANIFICADOR: En este bloque se calcularán los valores de referencia X, Y
e Z, para que el vehículo cumpla el objetivo.
SUBSISTEMA DE X Y CABECEO: Este subsistema se puede observar en la
Ilustración 4.2.
89 | P á g i n a
Ilustración 4.2. Estructura cascada de X y
SUBSISTEMA DE BALANCEO E Y: En la Ilustración 4.3 se puede ver
reflejado este subsistema.
Ilustración 4.3 Estructura cascada de
SUBSISTEMA DE Z
PLANIFICADOR DE LA GUIÑADA: Este bloque consiste en la obtención
del ángulo de guiñada que cumpla en el objetivo. En el caso de que se realice
una trayectoria circunferencial, el ángulo de guiñada será el que haga que el
punto de incidencia de la señal de la antena esté más alejado del centro de la
circunferencia proyectado en el plano Z=0.
SUBSISTEMA DE GUIÑADA
REPRESENTAR VALORES: En este bloque, se calcularán los puntos de
incidencia de la señal en la antena en el suelo y se representará gráficamente el
movimiento de la antena en ese instante.
90 | P á g i n a
4.3 EXPERIMENTOS DE CONTROL
4.3.1 ERRORES
A lo largo de este apartado, se analizarán los errores de los diferentes controladores.
4.3.1.1 Errores en el control de X y
Dada una referencia de X recta, se obtienen las siguientes salidas del subsistema X- :
(Ilustración 4.4 e Ilustración 4.5).
Ilustración 4.4 Salida de X
91 | P á g i n a
Ilustración 4.5 Salida de θ
Para el control de X se obtiene un error cuadrático medio de:
y para el control de :
4.3.1.2 Errores en el control de Y e Φ
A continuación, se representan las salidas del subsistema de Y e Φ, para una referencia de
Y sinoidal. (Ilustración 4.6 e Ilustración 4.7).
92 | P á g i n a
Ilustración 4.6 Salida de Y
Ilustración 4.7 Salida de Φ
93 | P á g i n a
El error cuadrático medio del control en Y es:
y el error cuadrático medio para Φ es:
4.3.1.3 Errores del controlador del subsistema Z
Para una referencia del subsistema Z de valor 20 constante, se obtiene la siguiente señal
como salida del subsistema: (Ilustración 4.8).
Ilustración 4.8 Salida de Z
El error cuadrático medio del controlador en el subsistema Z es:
94 | P á g i n a
4.3.1.4 Errores del controlador del subsistema Ψ
Si en el subsistema de guiñada , se introduce una referencia de valor 3 constante, se
obtendrá como salida la siguiente gráfica (Ilustración 4.9).
Ilustración 4.9 Salida de Ψ
El error cuadrático medio del controlador del subsistema ψ es:
Se puede afirmar que en todos los subsistemas anteriores el control elegido es el
adecuado ya que se obtienen valores de errores cuadráticos pequeños, y por tanto se conseguirá
la señal deseada.
95 | P á g i n a
4.3.2 EFECTO DEL RUIDO
En esta sección, se estudiará como afectará el ruido a cada uno de los subsistemas.
4.3.2.1 Efecto del ruido en el subsistema X y
En primer lugar, se analizará el efecto de un ruido gaussiano en el lazo interno del
subsistema X y θ (Ilustración 4.10).
Ilustración 4.10 Ruido del sistema X- θ
En las siguientes gráficas, se representan las variables de salida en el caso de que no
haya ruido (Ilustración 4.11 e Ilustración 4.12) y para ruidos gaussianos del lazo interno con
diversas varianzas (De Ilustración 4.13 a Ilustración 4.16).
96 | P á g i n a
Ilustración 4.11 Salida sin ruido
Ilustración 4.12 Salida sin ruido
97 | P á g i n a
Ilustración 4.13 Salida del lazo interno para una varianza de
Ilustración 4.14 Salida para una varianza de
98 | P á g i n a
Ilustración 4.15 Salida del lazo interno para una varianza de
Ilustración 4.16 Salida para una varianza de
99 | P á g i n a
Observando las figuras anteriores, se puede deducir que el sistema X-θ será poco
sensible al ruido gaussiano del lazo interno.
4.3.2.2 Efecto del ruido en el subsistema de Y e Φ
Se va a analizar el efecto de un ruido gaussiano en el lazo interno del subsistema Y-Φ
(Ilustración 4.17).
Ilustración 4.17 Ruido en el subsistema Y-Φ
A continuación, se representan las variables de salida para el caso en que no haya ruido
(Ilustración 4.18 e Ilustración 4.19) y para ruidos gaussianos de diferentes varianzas (De Ilustración 4.20 a Ilustración 4.23)
Ilustración 4.18 Salida sin ruido
100 | P á g i n a
Ilustración 4.19 Salida Φ sin ruido
Ilustración 4.20 Salida Φ del subsistema Y e Φ para una varianza de
101 | P á g i n a
Ilustración 4.21 Salida Y del subsistema Y e Φ para una varianza de
Ilustración 4.22 Salida Φ del subsistema Y e Φ para una varianza de
102 | P á g i n a
Ilustración 4.23 Salida Y del subsistema Y e Φ para una varianza de
Al igual que ocurría en el sistema acoplado anterior, se puede afirmar que el sistema es
poco sensible al ruido del lazo interno.
4.3.2.3 Efecto del ruido en el subsistema Z
A continuación, se representan las salidas del subsistema Z en el caso de que no haya
ruido (Ilustración 4.24), y en el caso de que existan ruidos de diferentes varianzas para una
referencia determinada (De Ilustración 4.25 a Ilustración 4.26).
103 | P á g i n a
Ilustración 4.24 Salida z sin ruido
Ilustración 4.25 Salida Z para un ruido gaussiano de 0.01
104 | P á g i n a
Ilustración 4.26 Salida Z para un ruido gaussiano de 0.1
Observando las gráficas, se puede afirmar que el ruido puede ser sensible en el sistema a
partir de una varianza de 0.1.
105 | P á g i n a
4.3.2.4 Influencia del ruido en el subsistema de guiñada
Se han obtenido las siguientes gráficas de la variable de salida del subsistema de
guiñada para el caso en que no intervenga ruido (Ilustración 4.27) y para el caso en el que haya
ruidos gaussianos de diferentes varianzas (De Ilustración 4.28 a Ilustración 4.30)
Ilustración 4.27 Salida ψ sin ruido
Ilustración 4.28 Salida ψ para ruido gaussiano de 1
106 | P á g i n a
Ilustración 4.29 Salida ψ para ruido gaussiano de 10
Ilustración 4.30 Salida ψ para ruido gaussiano de 20
Se puede deducir que a partir de una varianza de valor 20 es importante el efecto del
ruido en el subsistema.
107 | P á g i n a
4.4. EXPERIMENTOS DE PLANIFICACIÓN
4.4.1 EXPERIMENTOS DE PLANIFICACIÓN PARA DISTINTAS
DISTRIBUCIONES DE SENSORES
Ejemplo I
En el siguiente ejemplo se pueden observar dos concentraciones de puntos donde están
situados los sensores (Ilustración 4.31):
Ilustración 4.31 Posición del quadrotor cuando la señal incide en los sensores
Si se aplica la planificación que mezcla trayectorias rectilíneas y circunferenciales, se
obtiene que el quadrotor tiene que realizar cinco trayectorias circunferenciales para recoger los
datos de todos los sensores.
En la Ilustración 4.32, se puede observar que la circunferencia que tiene el mayor radio
está asociado a dos puntos pertenecientes al grupo concentrado de sensores más cercanos al
origen y a un punto de la concentración de sensores que están más alejada del origen.
108 | P á g i n a
Ilustración 4.32 Circunferencias asociadas a cada sensor
rr r
rr r
rr r
rr r
rr r
109 | P á g i n a
Ejemplo II
En este ejemplo (Ilustración 4.33), los puntos de sensores por los que debe pasar la señal de la
antena son los siguientes:
Ilustración 4.33 Distribución de puntos ejemplo II
Aplicando la planificación desarrollada, se volverán a tener cinco circunferencias como pasó en el
ejemplo I, pero en este caso no hay ninguna circunferencia que comparta puntos de ambas
concentraciones (Ilustración 4.34).
110 | P á g i n a
Ilustración 4.34 Circunferencias asociadas a cada sensor ejemplo II
rr r
rr r
rr r
rr r
rr r
Ejemplo III
En el siguiente ejemplo, se puede observar tres sensores cercanos que están alineados y
otro sensor más alejado (Ilustración 4.35).
Ilustración 4.35 Distribución de puntos ejemplo III
En este caso, se puede observar en la Ilustración 4.36 que aunque los tres puntos más
cercanos son los que están alineados, la circunferencia que deberá realizar el quadrotor será la
asociada a los dos primeros puntos alineados y al punto que no está alineado.
111 | P á g i n a
Al tercer punto alineado irá el quadrotor mediante una trayectoria rectilínea tras haber
realizado la trayectoria circunferencial.
Ilustración 4.36 Circunferencia asociadas a sensores ejempo III
Los errores en este caso serán:
rr r
rr r
112 | P á g i n a
4.4.2 EFECTO DE LA ALTURA
A continuación, se va analizar cómo influye la altura en la planificación que mezcla
trayectorias rectilíneas y curvilíneas. Para ello, se realizarán simulaciones a diferentes alturas
con las siguientes coordenadas:
{ }
En este caso el radio de la circunferencia que pasa por todos estos puntos es de 127.48
metros y deberá coincidir con el radio, , que forman los puntos que incide la señal de la
antena cuando se está realizando una maniobra circunferencial a un determinado radio de vuelo,
R.
En el capítulo 3, se vio que el radio de vuelo, R, que cumpla el objetivo dependerá de la
altura y de la velocidad angular del vehículo, ya que la relación R/d’ permanece prácticamente
constante para una altura y una velocidad angular determinada.
La relación entre R y vendrá dada por la siguiente expresión:
donde K es un valor conocido, que dependerá de la altura y la velocidad angular del
quadrotor.
La velocidad angular y el radio de vuelo mantendrán la siguiente relación:
Para analizar el efecto de la altura, se supondrá que la velocidad del quadrotor es
/ , quedando las siguientes expresiones que dependen de la velocidad angular y el
radio de vuelo, y donde K es conocida para una velocidad angular determinada:
113 | P á g i n a
4.4.2.1 Z= 20 metros
Para esta altura, el radio de vuelo necesario para cubrir todos los puntos sería de
121.445 metros y la velocidad angular es de 9 deg/s como se puede observar en la Ilustración
4.37.
Ilustración 4.37 Efecto de la altura a 20 metros
En este caso se tarda en cumplir el objetivo un tiempo de 70.6 segundos y se obtienen
los siguientes errores para cada uno de los cuatros puntos donde está situados los sensores:
rr r
114 | P á g i n a
4.4.2.2 Z= 30 metros
A una altura de 30 metros, el radio de vuelo que cubriría todos los puntos sería de
118.64 metros y la velocidad angular es de 9 deg/s como queda reflejado en la Ilustración 4.38.
Ilustración 4.38 Efecto de la altura a 30 metros
El quadrotor cumplirá el objetivo en 73.79 segundos en estas condiciones y el método
presentará los siguientes errores:
rr r
115 | P á g i n a
4.4.2.3 Z= 40 metros
Si la altura es de 40 metros, el radio de la circunferencia que cubrirá todos los puntos
será de 113.53 metros y la velocidad angular es de 10 deg/s, como aparece en la Ilustración
4.39.
Ilustración 4.39 Efecto de la altura a 40 metros
En este caso se cumplirá el objetivo en 76.45 segundos, presentando los siguientes
errores:
rr r
116 | P á g i n a
4.4.2.4 Conclusión
En el siguiente cuadro, quedan representando los resultados del ejemplo anterior para
varias alturas:
Altura(m) Tiempo(s) en
cumplir el
objetivo
Radio de
vuelo (m) wt (deg/s) K (wt, h)
20 70.6 121.445 9 0.9527
30 73.79 118.64 9 0.9307
40 76.45 113.53 10 0.8906
En este ejemplo, se puede observar que a medida que aumenta la altura disminuye el
radio de vuelo necesario para realizar el objetivo, pero aumenta el tiempo de cumplir el
objetivo.
En el siguiente cuadro, aparece el valor de K para varias alturas y varias velocidades
angulares:
K(wt=1 deg/s) K(wt=4deg/s) K(wt=8deg/s) K(wt=10deg/s) K(wt=12 deg/s)
Z=20 m 0.9933 0.9905 0.9622 0.9421 0.9185
Z=30m 0.9914 0.9857 0.9444 0.9156 0.8826
Z=40m 0.9868 0.9812 0.9272 0.8906 0.8493
117 | P á g i n a
Se puede deducir del cuadro anterior que a medida que aumenta la velocidad angular,
menor será el valor de K, y por tanto menor será el valor de radio de vuelo necesario para
cumplir el objetivo.
También, se puede afirmar que a medida que aumenta la altura mayor será el valor de
K y menor el radio de vuelo necesario para satisfacer el objetivo, ya que habrá más distancia
entre la proyección del centro de gravedad del vehículo en el plano del suelo, y el punto en el
suelo donde está incidiendo la señal de la antena en ese momento.
4.4.3 EFECTOS DE LA VELOCIDAD
A continuación, se va a analizar cómo influye la velocidad en la planificación. Para ello,
se van a realizar varios experimentos con diferentes velocidades.
En esos experimentos, se supondrá que el quadrotor se encuentra a una altura constante
de 30 metros, y las coordenadas de los puntos donde están situados los sensores inalámbricos
serán los siguientes:
{ }
Las maniobras que deberá hacer el quadrotor para cubrir esos puntos será una
circunferencia que cubra los tres primeros ya que son los más cercanos y más tarde se realizará
una trayectoria rectilínea que pasará por el último punto. Esto se puede visualizar en la
Ilustración 4.40.
Ilustración 4.40 Posición del quadrotor cuando la señal incide en los sensores
118 | P á g i n a
En el siguiente cuadro aparecen diferentes velocidades para está simulación junto con el
tiempo que tarda en cumplir el objetivo:
Se puede afirmar observando el cuadro anterior, que a medida que aumenta la velocidad
del vehículo disminuirá el tiempo en cumplir el objetivo.
4.4.4 EFECTOS DE RUIDO
Para finalizar, se analizará la influencia del ruido en la planificación desarrollada. Para ello,
se partirá de la siguiente distribución de puntos:
{ }
Velocidad
(m/s)
Tiempo(s) en
cumplir el
objetivo
20 99.7051
18 100.5061
16 101.5352
14 102.8940
12 104.7536
10 106.42
119 | P á g i n a
Ilustración 4.41 Distribución de sensores en los que se analizará el ruido
Se han obtenido los siguientes resultados para diferentes ruidos gaussianos y diversas
varianzas:
Sin ruido Ruido en Z
Ruido en Z
Ruido en Z
Ruido en Z
Error punto 1 0.3928 0.3928 0.3930 0.3931 0.3931
Error punto 2 0.4158 0.4161 0.4163 0.4168 0.4170
Error punto 3 0.1296 0.1298 0.1301 0.1302 0.1314
Error punto 4 0.0026 0.0026 0.0026 0.0026 0.0026
120 | P á g i n a
Sin ruido Ruido en ψ
Ruido en ψ
Ruido en ψ
Ruido en ψ
Error punto 1 0.3928 0.3937 0.3948 0.3978 0.3998
Error punto 2 0.4158 0.4168 0.4191 0.4263 0.4510
Error punto 3 0.1296 0.1297 0.1297 0.1299 0.1302
Error punto 4 0.0026 0.0026 0.0025 0.0026 0.0027
Sin ruido Ruido en X-θ
Ruido en X-θ
Ruido en X-θ
Ruido en X-θ
Error punto 1 0.3928 0.3928 0.3928 0.3928 0.3927
Error punto 2 0.4158 0.4158 0.4158 0.4158 0.4157
Error punto 3 0.1296 0.1296 0.1296 0.1296 0.1296
Error punto 4 0.0026 0.0026 0.0026 0.0026 0.0027
Sin ruido Ruido en Y-ф
Ruido en Y-ф
Ruido en Y-ф
Ruido en Y-ф
Error punto 1 0.3928 0.3928 0.3929 0.3929 0.3929
Error punto 2 0.4158 0.4158 0.4158 0.4159 0.4157
Error punto 3 0.1296 0.1296 0.1296 0.1296 0.1297
Error punto 4 0.0026 0.0026 0.0026 0.025 0.027
Se puede deducir de los resultados anteriores, que el quadrotor seguirá cumpliendo el
objetivo para diversos ruidos, ya que los errores seguirán siendo igual de pequeños.
121 | P á g i n a
5 CONCLUSIONES
5.1. Conclusiones
El objetivo de este proyecto ha sido planificar de forma eficiente el vuelo de un quadrotor
para que el vehículo cumpla el objetivo de que la señal de su antena pase por las coordenadas donde
se encuentran los sensores inalámbricos de un terreno.
Para realizar lo anteriormente expuesto, en primer lugar se linealizó el modelo cinemático
y dinámico del quadrotor, donde se pudo observar que existían acoplamientos de las variables (X, )
e (Y, ) en las ecuaciones, mientras que la posición Z y el ángulo de guiñada no presentaban
acoplamiento con ninguna variable en dichas expresiones.
Las variables elegidas para ser controladas en este proyecto han sido la posición X Y e
Z, y el ángulo de guiñada. Se ha optado por controlar de manera discreta ya que es más eficaz
que de forma analógica, con un tiempo de muestreo de 0.05 segundos.
Se han considerado en este proyecto, las hipótesis de que el quadrotor se mueve a altura
constante, y que la antena se encuentra en el eje –Z del vehículo.
La primera planificación elegida ha sido aquella en la que el vuelo del quadrotor
consista en trayectorias rectas por las cuales el vehículo llegará a los puntos donde están
situados los sensores y pasara por encima del ellos, ya que si el vuelo es rectilíneo la antena del
quadrotor apuntará hacia abajo. El criterio elegido para seleccionar el orden de sobre cual punto
debe pasar el quadrotor, ha consistido, en primer lugar, ir hacia el punto más cercano el origen,
y de ahí en adelante el quadrotor irá al punto que presente menos distancia con el último por el
que pasó.
La segunda planificación elegida consiste en un vuelo formado por trayectorias
rectilíneas y circunferenciales. Se ha demostrado que si el quadrotor realiza una trayectoria
circunferencial, los puntos que son incididos por la antena en el suelo, formarán parte de otra
circunferencia cuyo centro es el mismo que el de la proyección en el suelo del centro de la
circunferencia con la que vuela el vehículo.
122 | P á g i n a
Los siguientes pasos han formado parte de esta planificación:
Se supone conocida la relación entre el radio de vuelo, y el radio de la circunferencia
que forma los puntos que son incididos por la señal de la antena en el suelo cuando se
está realizando una maniobra circunferencial a una altura y una velocidad angular
determinada.
Utilizando lo anterior, se buscarán circunferencias por la que pase los puntos donde se
encuentren los sensores. Se agruparán en grupo de tres todos los puntos donde está
situados los sensores, y para cada uno de esos grupos se definirá una circunferencia que
pase por ellos.
El criterio elegido para seleccionar los tres puntos de sensores que van a determinar
cada una de las circunferencias, será elegir los puntos que se encuentren más cercanos.
En la elección de estos tres puntos, se ha tenido en cuenta que estos tres puntos no estén
alineados. Si pasará esto, se elegiría el siguiente punto más cercano para formar el
nuevo grupo de tres.
Si alguno de los puntos está dentro de la circunferencia que forma un grupo de tres
puntos diferente al suyo, este punto dejará de formar parte del grupo de tres puntos en el
que se encuentra, y se volverá a realizar una nueva distribución de grupos de tres para
los puntos que a los que todavía no se le ha sido asignado una circunferencia.
Si queda un punto sin asignar circunferencia, el quadrotor irá en línea recta hacia él y
pasará por encima de él, tras recorrer la última circunferencia. De esta manera la señal
llegará a este punto.
Si se han asignado circunferencias a todos los puntos excepto a dos, a estos dos puntos
se le asignará una última circunferencia, cuyo radio será la mediatriz del segmento que
une estos dos puntos.
Mediante varias simulaciones, se ha demostrado que dependiendo de la distribución de
los puntos donde están los sensores, interesará más usar una planificación u otra. También, se
comprobó que los errores con ambos métodos son aceptables.
Mediante una serie de experimentos, se han obtenido las siguientes conclusiones:
Los errores cuadráticos medios son pequeños en cada subsistema y el planificador
cumple el objetivo para diferentes tipos de ruidos.
Para una velocidad angular determinada, al aumentar la altura de vuelo, menor es el
radio de vuelo necesario para el que la señal pasa por todos los puntos deseados.
123 | P á g i n a
Se ha comprobado que al aumentar la velocidad del vehículo, disminuirá el tiempo
empleado en la simulación.
5.2. Ampliaciones del proyecto
Una primera ampliación del proyecto sería combinar las dos planificaciones desarrolladas
en un solo código, de manera que se usará una u otra según interese dada una serie de coordenadas
de sensores.
Una forma de evitar algún obstáculo durante el vuelo del quadrotor, como puede ser un
pájaro, podría ser mediante un cambio de altura. Para ello, sería necesario incluir en el código el
cambio de trayectoria necesario del plano X Y que deberá realizar el vehículo para cumplir el
objetivo en el caso de que cambie la altura en un instante determinado.
Una de las planificaciones que se ha desarrollado en el proyecto, consistía en que el
quadrotor realizaba trayectorias circunferenciales, para que la señal de la antena pasara por tres
sensores del terreno. De forma similar, sería interesante estudiar la posibilidad de que el quadrotor
hiciera una trayectoria elíptica que permitiera que la señal de la antena llegará a cuatros sensores, ya
que un elipse está definida por cuatro puntos.
Desde el punto de vista de control, una posible ampliación sería usar técnicas de control más
avanzadas para que mejore el comportamiento de control en caso de que haya otras perturbaciones
externas, como ráfagas de viento. En los sistemas acoplados, se tendría que estudiar y corregir el
efecto de un ruido en el lazo externo, porque dicho ruido puede afectar a la variable de salida del
lazo interno.
Sería importante incluir el efecto del viento en las expresiones del sistema y comprobar si el
planificador seguiría cumpliendo el objetivo.
Para facilitar el uso del programa al usuario, se podría crear una interfaz gráfica, GUI, que
permitiría al usuario modificar parámetros y/o indicar las coordenadas de sensores de forma
cómoda, en lugar de tener que abrir ficheros.
124 | P á g i n a
REFERENCIAS
[1] Ponds, Paul & Mahony, Robert & Corke, Peter; Modelling and Control of a Quad-Rotor
Robot, Australian National University, Brisbane, 2006.
[2] Mahony, Robert & Kumar, Vijay & Corke, Peter; Multirotor Aerial Vehicles: Modeling,
Estimation and Control of Quadrotor; Robotics & Automation Magazine, Vol. 19
No.3, 2012.
[3] A. Tayebi and S. McGilvray. Attitude stabilization of a four-rotor aerial robot. 43rd IEEE
Conference on Decision and Control. December 14-17, 2004. Atlantis, Paradise Island,
Bahamas.
[4] S. Bouabdallah and R. Siegwart. Advances in Unmanned Aerial Vehicles, chapter 6 -
Design and Control of a Miniature Quadrotor, pages 171–210. Springer, Netherlands,
2007.
[5] Vianna Raffo, Guilherme; Robust Control Strategies for a QuadRotor Helicopter, Tesis
doctoral, Universidad de Sevilla, 2011.
125 | P á g i n a