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
125
Embed
PROYECTO FIN DE CARRERA INGENIERÍA AERONÁUTICA ...bibing.us.es/proyectos/abreproy/60352/fichero/Proyecto_Fin_de_carr… · PROYECTO FIN DE CARRERA INGENIERÍA AERONÁUTICA Dep.
This document is posted to help you gain knowledge. Please leave a comment to let me know what you think about it! Share it to your friends and learn new things together.
Transcript
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
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