monocopteto
DESCRIPTION
MonocopteroTRANSCRIPT
Universidad de Costa Rica
Facultad de Ingeniería
Escuela de Ingeniería Eléctrica
IE – 0502 Proyecto Eléctrico
Diseño e implementación de un cuadracóptero con
sistema de control automático de estabilidad y
comunicación inalámbrica de datos utilizando
plataformas de hardware y software libre
Por:
Daniel Barrera Núñez
Ciudad Universitaria Rodrigo Facio
Julio del 2012
ii
Diseño e implementación de un cuadracóptero con
sistema de control automático de estabilidad y
comunicación inalámbrica de datos utilizando
plataformas de hardware y software libre
Por:
Daniel Barrera Núñez
Sometido a la Escuela de Ingeniería Eléctrica
de la Facultad de Ingeniería
de la Universidad de Costa Rica
como requisito parcial para optar por el grado de:
BACHILLER EN INGENIERÍA ELÉCTRICA
Aprobado por el Tribunal:
_________________________________
Ing. Fabián Abarca Calderón
Profesor Guía
_________________________________ _________________________________
Ing. Fabián Garita Gonzales Dr. Lucky Lochi Yu Lo
Ingeniero lector Profesor lector
iii
DEDICATORIA
A mi familia y seres queridos, por el constante apoyo y ayuda brindada durante los años de
mi carrera universitaria, sin la cual no hubiese podido lograr culminar esta etapa de manera
exitosa.
iv
RECONOCIMIENTOS
A mi profesor guía, el Ing. Fabián Abarca, por su apoyo y consejo a lo largo del
desarrollo del proyecto.
A mis profesores lectores, el Dr Lochi Yu, y el Ing. Fabián Garita.
A mi padre, Edmundo Barrera, por el gran interés y ayuda que fueron
fundamentales en la construcción del primer prototipo del cuadracóptero.
v
ÍNDICE GENERAL
1. CAPÍTULO 1: INTRODUCCIÓN ....................................................... 1
1.1 Objetivos ................................................................................................. 3
1.1.1 Objetivo general .......................................................................................................... 3
1.1.2 Objetivos específicos .................................................................................................. 3
1.2 Metodología ............................................................................................ 4
2 CAPÍTULO II: DESARROLLO TEÓRICO ...................................... 5
2.1 Aerodinámica ......................................................................................... 5
2.2 Vehículos Aéreos No Tripulados .......................................................... 7
2.2.1 Introducción ................................................................................................................ 7
2.2.2 El Cuadracóptero ........................................................................................................ 8
2.3 Plataforma de Prototipos Electrónicos: Arduino ............................. 16
2.3.1 Breve Introducción a Arduino .................................................................................. 16
2.3.2 Hardware del Arduino Uno ....................................................................................... 17
2.3.3 Programación ............................................................................................................ 21
2.4 Sensores ................................................................................................. 22
2.4.1 Giroscopio ................................................................................................................. 24
vi
2.4.2 Acelerómetro ............................................................................................................ 27
2.4.3 Sensor Ultrasónico de Proximidad ........................................................................... 29
2.5 Actuadores ............................................................................................ 32
2.5.1 Motores DC sin escobillas ........................................................................................ 32
2.5.2 Controles electrónicos de Velocidad (ESC´s) .......................................................... 35
2.6 Comunicaciones Inalámbricas ............................................................ 37
2.6.1 Comunicación Serial ................................................................................................. 37
2.6.2 Protocolo Bluetooth .................................................................................................. 38
2.7 Procesamiento de Señales .................................................................... 40
2.7.1 Introducción .............................................................................................................. 40
2.7.2 Manipulación de los datos de los sensores (IMU) .................................................... 41
2.7.3 Media Móvil ............................................................................................................. 41
2.8 Control Automático ............................................................................. 44
2.8.1 Controlador PID ........................................................................................................ 44
2.9 Fuente de Poder ................................................................................... 46
2.9.1 Batería de Polímero de Ion de Litio (LiPo) .............................................................. 46
3. CAPÍTULO III: DESARROLLO E IMPLEMENTACIÓN DEL
UAV 47
3.1 Diagramas de Bloques del Cuadracóptero ........................................ 47
vii
3.2 Experimentos ........................................................................................ 52
3.2.1 Validación del módulo Bluetooth ............................................................................. 52
3.2.2 Validación de los controles electrónicos de velocidad (ESC) .................................. 54
3.2.3 Validación de la Unidad de Medición Inercial (IMU) ............................................. 55
3.2.4 Validación del Transductor Ultrasónico de Proximidad ........................................... 59
3.2.5 Validación del sistema de control automático. ......................................................... 60
3.3 Lista de Componentes Utilizados ....................................................... 64
4. CAPÍTULO 4: CONCLUSIONES Y RECOMENDACIONES ...... 67
4.1 Conclusiones ......................................................................................... 67
4.2 Recomendaciones ................................................................................. 70
4.3 Trabajo Futuro .................................................................................... 71
BIBLIOGRAFÍA ............................................................................................ 72
APÉNDICES ................................................................................................... 76
Apéndice 1. Sketch de validación del módulo bluetooth. ........................... 76
Apéndice 2. Código completo de programación del cuadracóptero ......... 78
viii
ÍNDICE DE FIGURAS
Figura 1. Ángulo de ataque de un ala [30] .............................................................................. 6
Figura 2. Modelo tridimensional de un cuadracóptero ........................................................... 8
Figura 3. Diagrama rotación de cuadracóptero alrededor del eje Z [9]. ............................... 12
Figura 4. Diagrama rotación de cuadracóptero alrededor del eje X [9]................................ 13
Figura 5. Diagrama rotación de cuadracóptero alrededor del eje Y [9]................................ 13
Figura 6. Diagrama traslación de cuadracóptero a lo largo del eje Z [9]. ............................ 15
Figura 7. Parte superior de Arduino Uno [http://arduino.cc/en/Main/ArduinoBoardUno] .. 17
Figura 8. Entorno de programación de Arduino (programa de ejemplo). ............................ 22
Figura 9. Respuesta de Ganancia y Fase del Filtro del IDG-500 ......................................... 25
Figura 10. Diagrama de bloques de acelerómetro ADXL335. Hoja de Especificaciones .... 28
Figura 11. Diagrama de Operación de Sensor Ultrasónico de Proximidad. Fuente: Google
Images. .................................................................................................................................. 30
Figura 12. Sensor Ultrasónico de Proximidad (HC-SR04). Fuente: Manual de Fabricante. 30
Figura 13. Representación gráfica de funcionamiento de sensor ultrasónico de proximidad.
.............................................................................................................................................. 32
Figura 14. Representación de Motor DC sin Escobillas ....................................................... 35
Figura 15. Principio de funcionamiento de motores sin escobillas [6] ................................. 37
Figura 16. Ilustración Media Móvil ...................................................................................... 42
Figura 17. Gráfico Ilustrativo de Media Móvil ..................................................................... 43
ix
Figura 18. Diagrama de bloques de controlador PID Paralelo ............................................. 46
Figura 19. Diagrama de Bloques de Controlador PID .......................................................... 47
Figura 20. Cuadracóptero en configuración "x" ................................................................... 48
Figura 21. Cuadracóptero en configuración "+". .................................................................. 48
Figura 22. Diagrama de bloques funcionales para cada motor ............................................. 50
Figura 23. Lazo de ejecución y realimentación del cuadracóptero. ...................................... 50
Figura 24. Diagrama de método de validación de módulo bluetooth en conjunto con
Arduino. ................................................................................................................................ 53
Figura 25. Captura de Monitor Serial de Aplicación de Programación de Arduino. ........... 53
Figura 26. Comparación entre señal original y señal filtrada de uno de los acelerómetros. 57
Figura 27. Prueba para observar el desfase entre la señal filtrada y la original, con los
motores apagados. ................................................................................................................. 57
Figura 28. Prueba realizada para observar la inmunidad del sistema ante señales no
deseadas. ............................................................................................................................... 58
Figura 29. Experimento para la obtención de los parámetro de PID de inclinación Eje X .. 61
Figura 30. Modelo tridimensional del primer prototipo del cuadracóptero. ......................... 65
Figura 31. Modelo Real del Primer Prototipo del Cuadracóptero. ....................................... 65
Figura 32. Modelo Real del Primer Prototipo del Cuadracóptero. ....................................... 66
Figura 33. Parte inferior del primer prototipo del cuadracóptero. ........................................ 66
x
ÍNDICE DE TABLAS
Tabla 1. Cronograma tentativo del desarrollo del dispositivo. ............................................... 4
Tabla 2. Especificaciones Técnicas del Arduino Uno .......................................................... 18
Tabla 3. Salidas analógicas del IDG-500 .............................................................................. 26
Tabla 4. Especificaciones de Acelerómetro ADXL335 ........................................................ 29
Tabla 5. Tabla de asignación de pines de Arduino ............................................................... 51
Tabla 6. Lista de Componentes y Costos del Cuadracóptero ............................................... 64
xi
NOMENCLATURA
ESC: Controlador Electrónico de Velocidad (Del inglés Electronic Speed
Controller)
IEEE: Instituto de Ingenieros Eléctricos y Electrónicos.
IMU: Unidad de Medición Inercial.
MEMS: Sistemas micro-electro-mecánicos (Micro Electro-Mechanical Systems.)
UAV: Vehículo Aéreo No Tripulado (Del inglés Unmanned Air Vehicle).
PWM: Modulación por ancho de pulso (del inglés Pulse Width Modulation).
SPI: Interfaz Serial de Periféricos (Del inglés “Serial Peripheral Interface”).
xii
RESUMEN
El presente trabajo constituye un proyecto de investigación y desarrollo de un vehículo
aéreo no tripulado con control de estabilidad autónomo, específicamente un cuadracóptero.
La finalidad del proyecto fue tener la oportunidad de poder ahondar más en determinados
conocimientos obtenidos a los largo de la carrera de ingeniería eléctrica, y ponerlos en
práctica mediante el desarrollo de un proyecto multidisciplinario. Para lograr esto, se
realizó una investigación previa de los componentes requeridos y conceptos necesarios
para el proceso de desarrollo del cuadracóptero. Se logró realizar un primer prototipo de un
cuadracóptero con control automático de estabilidad y altitud con comunicación
inalámbrica vía bluetooth con un teléfono inteligente que servía de control remoto para el
usuario. Todos los bloques funcionales del cuadracóptero fueron validados previamente a
ser integrados al sistema. Este desarrollo se realizó utilizando hardware y software libre, y
componentes de fácil acceso al público. Se logró obtener un primer prototipo capaz de
volar pequeñas distancias, sin embargo, a la hora de concluir el proyecto se determinó que
requería de algoritmos más robustos y un mejor microcontrolador para poder
implementarlos. Esto con la finalidad de lograr obtener una mayor robustez y seguridad a la
hora de volar el vehículo en ambientes con obstáculos.
1
1. CAPÍTULO 1: Introducción
La conceptualización y creación de máquinas que tengan la capacidad de operar de manera
autónoma data desde la era clásica. Sin embargo, no fue sino hasta el siglo XIX, junto con
el surgimiento y auge de la revolución industrial, que se empezaron a vislumbrar las
primeras máquinas relevantes de automatización de tareas. A partir de ese momento, éste se
convirtió en un campo de estudio en constante desarrollo, el cual, a partir de mediados del
siglo XX, empezó a crecer de manera exponencial, impulsado principalmente por los
grandes avances que se estaban dando en el área de la electrónica.
El área de la aeronáutica actualmente constituye una de las ramas con más presencia dentro
del campo de la robótica, en especial con el surgimiento de UAVs (Unmanned Aerial
Vehicles) desarrollados para una gran diversidad de tareas, que van desde aplicaciones
militares, hasta equipos de investigación científica. Esta constituye el área que se pretende
explorar con el avance del proyecto.
Para el desarrollo del presente proyecto, se pretende elaborar un vehículo aéreo de vuelo
autónomo (UAV), y para ello se buscó una plataforma versátil y de construcción sencilla.
Tras realizar una investigación previa, se eligió tomar como modelo base un quad-rotor, o
también conocido como cuadracóptero, el cual consiste, como se pude intuir de su nombre,
en una plataforma con cuatro rotores ubicados en los extremos de los ejes de simetría de su
chasis. Esta plataforma presenta varias ventajas en cuanto a su maniobrabilidad y la poca
2
cantidad de componentes mecánicos requeridos para su elaboración (lo cual muchas veces
representa el verdadero reto de los proyectos de esta índole).
Para su construcción se pretende hacer uso de hardware “open source” (Arduino) para el
sistema de control principal y de comunicaciones del dispositivo, y de componentes
electrónicos y mecánicos de fácil acceso al consumidor para el resto de la plataforma.
Dentro de las principales razones por las cuales se toma la decisión de abordar un proyecto
de este índole, es por la gran cantidad de tópicos que se encuentran involucrados en el
desarrollo de un prototipo de esta categoría, y dado que este proyecto se encuentra ligado a
la finalización del grado académico, permite la oportunidad de aplicar y demostrar una gran
cantidad de conocimientos que se han adquirido a través de la carrera. Entre las áreas de
estudio que se pretenden aplicar se pueden mencionar: microcontroladores, programación,
sistemas de control automático, sistemas de comunicación, análisis de sistemas, máquinas
eléctricas, electrónica analógica, circuitos digitales… y ¡hasta un poco de mecánica!
3
1.1 Objetivos
1.1.1 Objetivo general
Diseñar e implementar un vehículo aéreo no tripulado (Cuadracóptero) con sistemas de
control automático de estabilidad y comunicación inalámbrica mediante el uso de
plataformas de hardware y software libres.
1.1.2 Objetivos específicos
1.1.2.1 Implementar un sistema eficiente de control de potencia para el manejo de motores
DC para el cuadracóptero.
1.1.2.2 Diseñar e implementar un módulo de sensado de posición y movimiento, haciendo
uso de sensores de aceleración y giroscopios.
1.1.2.3 Implementar un sistema de comunicación inalámbrica mediante el uso de un
módulo Bluetooth.
1.1.2.4 Implementar un módulo de procesamiento y control principal para unificar e
interlazar los módulos de sensado, comunicación y potencia utilizando un
microcontrolador ATmega 328 en conjunto con una plataforma Arduino.
4
1.2 Metodología
La metodología que se implementó para el desarrollo del proyecto fue una extensa
investigación de los componentes necesarios (tanto físicos como teóricos) para realizar el
desarrollo del sistema de manera exitosa. Además, cada uno de los sistemas estudiados,
fueron implementados y validados de manera individual, para luego ser unificados en el
sistema final (cuadracóptero). Esto requirió de una extensa investigación y experimentación
para poder llegar a obtener el comportamiento esperado de cada uno de los subsistemas.
El cronograma original del proyecto a desarrollar fue evolucionando con el transcurso del
tiempo, sin embargo, a continuación se muestra el cronograma tentativo para el desarrollo
que se llevó a cabo:
Tabla 1. Cronograma tentativo del desarrollo del dispositivo.
5
2 CAPÍTULO II: Desarrollo Teórico
En el desarrollo del capítulo II, se sintetizarán los temas de mayor importancia para el
desarrollo y entendimiento de los conceptos necesarios para la implementación del
prototipo del cuadracóptero. Estos temas se tratarán de exponer de una manera sencilla para
que sea apto para lectores con o sin mayor conocimiento técnico del tema.
2.1 Aerodinámica
Con la finalidad de ir desarrollando los temas en un orden estructurado, se iniciará con una
breve sección de aerodinámica, la cual a grandes rasgos, constituye la base que permite
entender el comportamiento y la forma de operar del cuadracóptero. En este apartado se
explicarán brevemente los conceptos aerodinámicos de mayor relevancia para crear la base
teórica que permita un mejor entendimiento y desarrollo del proyecto.
Uno de los conceptos fundamentales que surge cuando se empieza a estudiar el campo de la
aerodinámica es el de la fuerza de sustentación. La fuerza de sustentación se define como la
fuerza perpendicular al flujo de un fluido (en nuestro caso el aire) que se crea cuando el
fluido pasa por la superficie de un objeto. El paso del fluido por la superficie del objeto crea
una fuerza con varios componentes, pero la fuerza de sustentación es solamente la
componente vertical al vector de dirección del fluido. Dentro de los otros componentes de
esta fuerza creada por el paso del fluido, también se encuentra la fuerza de arrastre, la cual
6
también juega un papel muy importante en la dinámica de operación de los vehículos
aéreos.
En el caso de los vehículos que poseen aspas adheridas a rotores, ésta fuerza es la
encargada de crear un par rotacional en el vehículo cuya dirección va en contra del sentido
de giro de las aspas. Por esta razón es que los helicópteros convencionales poseen rotores
de cola que compensan este par rotacional y evita que giren sin control alrededor del eje de
rotación del rotor principal.
El ángulo de ataque de un ala se encuentra definido como el ángulo que forma la cuerda
geométrica de un perfil alar con la dirección del aire que incide sobre el ala. Este parámetro
juega un papel muy importante en la generación de la fuerza de sustentación y la fuerza de
arrastre de una hélice. Para un mejor entendimiento de este concepto se puede observar la
figura a continuación.
Figura 1. Ángulo de ataque de un ala [30]
La correcta elección del ángulo de ataque de las hélices es de suma importancia para
obtener los parámetros de fuerza de sustentación y de arrastre que son requeridos.
7
2.2 Vehículos Aéreos No Tripulados
2.2.1 Introducción
Los vehículos aéreos no tripulados (popularmente conocidos como UAVs por sus siglas en
inglés), son vehículos aéreos que están diseñados para poder volar sin necesidad de un
piloto humano a bordo. El vuelo de estas máquinas, normalmente se encuentra controlado
por un piloto humano que se encuentra en un centro de mando en tierra, desde donde puede
transmitirle las instrucciones de vuelo de manera remota, o también, éstas máquinas pueden
ser controladas de manera autónoma por un controlador o computador que se encuentre
programado para tal fin.
Dentro de la gama de vehículos aéreos no tripulados, se puede encontrar una gran escala de
tamaños , configuraciones y formas (vehículos de ala fija, vehículos multirotores, etc), los
cuales inicialmente fueron surgiendo como plataformas controladas remotamente por un
piloto, pero que en los últimos años, gracias a los grandes avances de la tecnología en el
campo de la computación, la electrónica y comunicaciones, han migrando poco a poco al
sector de vehículos autónomos.
Los cuadracópteros entran dentro de esta categoría de vehículos aéreos no tripulados, y
dependiendo de las características de cada uno, éstos se pueden construir como vehículos
totalmente autónomos, o como vehículos controlados remotamente por un ser humano.
8
2.2.2 El Cuadracóptero
El cuadracóptero (también conocido como quadrotor o quadcopter) representa una
plataforma muy versátil y de creciente popularidad que se encuentra dentro de la categoría
de vehículos aéreos no tripulados (UAVs). Este tipo de vehículo aéreo multirotor se
encuentra formado (como su nombre lo dice) por cuatro rotores, los cuales normalmente se
encuentran dispuestos simétricamente a en los extremos del bastidor del vehículo (el cual es
en forma de X). A diferencia de los helicópteros convencionales, la mayoría de los
cuadracópteros poseen hélices con aspas fijas, en contraste con los helicópteros
convencionales, cuyas aspas tienen la capacidad de cambiar su ángulo de ataque. Un
modelo tridimensional general de un cuadracóptero se puede observar en la figura a
continuación:
Figura 2. Modelo tridimensional de un cuadracóptero
9
Dinámica de Movimiento
La dinámica del movimiento de los cuadracópteros se puede resumir en 3 tipos distintos de
movimientos básicos, los cuales se pueden ejecutar por sí solos o en conjunto para lograr el
tipo de movimiento específico que el controlador del vehículo desee. Estos tipos de
movimientos son los siguientes:
Rotación alrededor del Eje Z
Para la ilustración de este movimiento, primero se procede con la definición del eje “z” del
vehículo. Este eje imaginario, se encuentra en el centro del bastidor de la máquina, (el
centro de la equis formada por la estructura del vehículo), y es perpendicular al plano
formado por la misma estructura del vehículo. Como se mencionó anteriormente, la
rotación de las aspas crean un torque que tratan de girar el vehículo alrededor del eje de
giro de las aspas. En los helicópteros convencionales, este problema se corrige mediante un
rotor en su cola que genere una fuerza tangente al radio de giro del rotor principal, la cual
sea suficiente para crear un par opuesto que contrarreste el efecto del par generado por el
rotor principal.
En un cuadracóptero no se cuenta con rotores de cola (ni colas en sí), de modo que este
problema se soluciona de una manera distinta (la cual resulta muy ingeniosa). Como se
10
sabe, los cuadracópteros poseen un número par de rotores, por lo tanto se puede tener dos
rotores girando en un sentido, y dos rotores girando en otro sentido. De esta manera, al
manipular la velocidad de rotación de los distintos motores se puede llegar a un punto de
operación donde el par rotacional neto del vehículo sea igual a cero.
Según lo expuesto en [5], se puede concluir que tanto la fuerza de sustentación como la
fuerza de arrastre son funciones que dependen del cuadrado de la velocidad angular. A
continuación se muestran las ecuaciones de dichas fuerzas:
Ecuación 2.1
Ecuación 2.2
Donde son parámetros propios del fluido (aire) y del tipo de hélices y
corresponde a la velocidad angular de rotación de los motores. Por lo tanto, si se mantienen
constantes el tipo de fluido donde se está volando, y el tipo de hélices que se están
utilizando, la única variable que queda es la velocidad angular. Por esta razón, el par
rotacional creado por la propela de cada motor puede ser conocido y modificado de acuerdo
a las necesidades de vuelo del vehículo.
11
Ahora, como lo que se trata de controlar es la rotación alrededor del eje z del
cuadracóptero, basta con igualar los pares rotacionales creados por los motores que giran en
contra de las manecillas del reloj, y los pares rotacionales creados por los motores que giran
a favor de las manecillas del reloj (en caso de que se quiera que el cuadracóptero no gire
alrededor del eje z). Esto lo logramos controlando las velocidades netas de cada uno de los
sentidos de giro (partiendo del hecho que las hélices sean simétricas y con las mismas
características constructivas).
Sean y las velocidades angulares de los motores girando a favor de las manecillas
del reloj y y las velocidades angulares de los motores girando en contra de las
manecillas del reloj, se pueden obtener 3 casos:
Si + + , se obtiene un comportamiento del vehículo en donde no hay
rotación alrededor del Eje Z.
Si + + , se obtiene un comportamiento del vehículo en donde hay una
rotación alrededor del Eje Z y en contra de las manecillas del reloj.
Si + + , se obtiene un comportamiento del vehículo en donde hay una
rotación alrededor del Eje Z y a favor de las manecillas del reloj.
A continuación se encuentra un diagrama de la configuración de los motores en el
cuadracóptero para visualizar de una mejor manera lo explicado anteriormente.
12
Figura 3. Diagrama rotación de cuadracóptero alrededor del eje Z [9].
Rotación alrededor del Eje X o el Eje Y
La dinámica de ambas rotaciones es la misma. Lo que cambia es alrededor de cual eje se
quiera girar. Para realizar esta rotación lo que se realiza es disminuir/aumentar la velocidad
de rotación de uno de los motores y realizar la acción inversa en el motor que se encuentre
al lado opuesto del eje. Cabe resaltar que la velocidad neta (ya sea en contra o a favor de
reloj) de los motores debe permanecer constante en caso de que no se quiera que el
cuadracóptero rote inesperadamente alrededor del su eje Z).
A continuación se puede observar la figura que ilustra el caso anterior para que se visualice
de una manera más sencilla.
13
Figura 4. Diagrama rotación de cuadracóptero alrededor del eje X [9].
De este modo, si lo que se quiere modificar es el ángulo de rotación , entonces y
permanecerán constantes, mientras que y cambiarán su valor dependiendo si el
desplazamiento angular es positivo o negativo. Cabe resaltar que si se realiza un cambio de
en , entonces debe realizarse al mismo tiempo un cambio de en para
mantener la velocidad neta de rotación a favor de las manecillas del reloj constante.
La dinámica resulta la misma para el caso análogo de la rotación alrededor del eje Y.
Figura 5. Diagrama rotación de cuadracóptero alrededor del eje Y [9].
14
Una consecuencia de provocar una rotación alrededor de alguno de los ejes X o Y, es que
se va a obtener un desplazamiento traslacional en caso de que el cuadracóptero ya se
encuentre en vuelo, el cual va a ser correspondiente al tipo de inclinación que tenga el
cuadracóptero. Por ejemplo, si el cuadracóptero rota alrededor del eje X, se va a obtener
una traslación a lo largo del eje Y, y si el cuadracóptero rota alrededor del eje Y, se va a
obtener una traslación a lo largo del eje X. Cabe resaltar que también se puede obtener una
rotación simultánea en ambos ejes, lo cual va a provocar una traslación análoga a esa
rotación.
Control de Altitud
El control de altitud resulta de una manera muy intuitiva. Se parte de que la altitud se
asocia con el desplazamiento del cuadracóptero a lo largo su eje Z (el cual no
necesariamente se encuentra perpendicular al suelo).
Para realizar el control de altitud, basta con aumentar o disminuir de manera equivalente las
velocidades de los cuatro motores del cuadracóptero. Si se quiere ganar altitud, se procede a
aumentar la velocidad de todos los motores en un factor equivalente a la rapidez con la que
se quiere lograr este cambio de altitud, y de manera análoga para disminuirla.
15
A continuación se encuentra el esquema que ilustra el proceso descrito:
Figura 6. Diagrama traslación de cuadracóptero a lo largo del eje Z [9].
Resulta fácil llegar a la conclusión de que con una determinada combinación de cualquiera
de los tres tipos de movimientos descritos anteriormente se puede obtener un control total
sobre la trayectoria y postura del cuadracóptero en un determinado momento. El control de
movimiento del cuadracóptero se basa en un principio de superposición, en donde la
velocidad final de cada uno de los motores va a ser la suma dinámica de cada uno de las
velocidades necesarias para lograr cada uno de los tres tipos de movimiento que se estén
ejecutando en determinado momento.
16
2.3 Plataforma de Prototipos Electrónicos: Arduino
Arduino ha surgido en los últimos años como una plataforma de creación de prototipos
electrónicos muy revolucionaria, la cual ha desarrollado de gran manera el campo del
movimiento de “Hágalo usted mismo” (Do It Yourself). Esto se debido a su gran facilidad,
versatilidad y flexibilidad de uso tanto de su hardware como de su software, la cual requiere
poca experiencia y conocimiento del tema para iniciar con el desarrollo de prototipos.
2.3.1 Breve Introducción a Arduino
Arduino nace como una plataforma que puede percibir su entorno mediante una gran gama
de sensores, y puede afectar sus alrededores controlando motores, luces y otra gran
variedad de actuadores. El microcontrolador de sus tarjetas es programado mediante el
lenguaje de programación propio de Arduino (basado en el lenguaje de programación
“Processing”, y muy similar a C), y el entorno de desarrollo de Arduino (basado en el
entorno desarrollado para la plataforma de prototipado “Wiring”). Los proyectos realizados
con Arduino pueden ser utilizados de manera independiente, o pueden comunicarse con
software que se encuentre corriendo en otro computador.
La familia de Arduino cuenta con una gran variedad de integrantes (tipos de tarjetas
distintas), cada una con su forma y características propias. Esto resulta muy práctico para
seleccionar una que se adapte lo mejor posible a las características de cada proyecto.
17
2.3.2 Hardware del Arduino Uno
Dado que para el presente proyecto se decidió utilizar el Arduino Uno, se va a profundizar
un poco en sus características particulares. El Arduino Uno es uno de los integrantes de la
familia de PCBs de Arduino, y es un microcontrolador basado en el ATmega328. Este
microcontrolador posee 14 pines de entrada/salida digitales, de los cuales 6 se pueden
utilizar como salidas PWM (señales de modulación por ancho de pulso). Además, posee 6
entradas analógicas, un oscilador de 16 MHz, un conector USB para comunicación con una
PC, un conector de alimentación y un botón de reset.
Figura 7. Parte superior de Arduino Uno [http://arduino.cc/en/Main/ArduinoBoardUno]
18
A continuación se presenta una tabla resumen con las principales características del
Arduino Uno:
Tabla 2. Especificaciones Técnicas del Arduino Uno
Adicional a la tabla anterior, en la sección de la bibliografía se puede encontrar un link a la
hoja de especificaciones del microcontrolador ATmega328 en caso de que se quiera
profundizar más en las especificaciones técnicas.
Fuente de Poder
El Arduino Uno puede ser alimentado desde su conector USB o mediante una fuente de
poder externa, como por ejemplo una batería. La fuente de poder es seleccionada de manera
19
automática por el dispositivo, y se puede acoplar al Arduino por medio de su conector de
2.1mm o mediante los pines de “Vin” y “GND”.
La tensión de alimentación de la tarjeta puede variar entre 6 y 20 volts, sin embargo cuando
la tensión es inferior a 7 volts, el regulador de 5 volts puede actuar de manera inestable y no
suplir los 5 volts deseados (que se obtienen a través del pin de 5V). Además, cuando se usa
una tensión de alimentación superior a 12 volts existe la posibilidad de que el regulador se
sobrecargue y se dañe. Por las razones anteriores, la tensión de entrada recomendada para el
Arduino es de 7 a 12 volts.
Los pines de potencia de la tarjeta son los siguientes:
Vin: Es el pin de entrada cuando se está utilizando una fuente de poder externa (no
USB). La tensión recomendada en este pin es de 7 a 12 volts. Este pin se encuentra
en paralelo con el conector de entrada de 2.1 mm.
5V: Es la salida del regulador de tensión de 5 volts de la tarjeta.
3.3V: Es la salida del regulador de 3.3 volts de la tarjeta utilizado para sensores y
actuadores que requieran de esta tensión de alimentación especial. La corriente
máxima que se puede entregar por medio de este canal es de 50 mA.
GND: Es el pin de tierra.
20
Memoria
El ATmega 328 posee 32 kB de memoria, de los cuales 0.5 kB son utilizados por el
“bootloader” de la tarjeta. Además posee 2 kB de SRAM y 1 kB de EEPROM, los cuales
pueden ser accesados o modificados mediante una de las librerías destinadas para tal fin.
Entradas y Salidas
El Arduino Uno posee 14 pines digitales, los cuales pueden ser utilizados como entradas o
salidas. Estos pines trabajan a 5 volts, y pueden entregar un máximo de 40 mA. Además,
ciertos pines tienen funciones especializadas, las cuales se mencionan a continuación:
Pines Seriales: 0 (RX) y 1 (TX). Son utilizados para recibir y transmitir datos
seriales TTL. Estos pines están conectados a los pines correspondientes del chip
serial USB-TTL ATmega8U2.
Pines de Interrupción: 2 y 3. Estos pines se pueden configurar para provocar una
interrupción en la ejecución del programa.
Pines de PWM: 3,5,6,9,10 y 11. Proveen una salida PWM de 8 bits de resolución
mediante la función analogWrite().
Pines de comunicación SPI: 10 (SS), 11 (MOSI), 12 (MISO), 13 (SCK). Estos pines
son utilizados para realizar conexión de dispositivos mediante protocolo SPI
utilizando la librería destinada para tal fin.
21
El Arduino Uno también posee 6 entradas analógicas con un convertidor analógico-digital
de 10 bits de resolución. Por default, esos convertidores miden desde 0 hasta 5 volts, pero
este valor superior se puede modificar mediante el pin AREF y la función
analogReference().
2.3.3 Programación
La programación del Arduino se realiza mediante el entorno de programación propio de
Arduino. Los programas que se realizan para Arduino son denominados “Sketch”. Una vez
que se escribe un programa, el entorno de programación se encarga de compilarlo y
cargarlo al Arduino que se tenga conectado al computador. Para un mayor detalle de las
funciones, estructuras y valores del lenguaje de programación se puede referir a la página
web oficial: “http://arduino.cc/en/Reference/HomePage”. Además, Arduino posee una gran
cantidad de librerías realizadas por sus usuarios, las cuales pueden ser descargadas de
manera gratuita para su respectivo uso. A continuación se muestra una imagen del entorno
de programación:
22
Figura 8. Entorno de programación de Arduino (programa de ejemplo).
2.4 Sensores
Las palabras “sensor” y “transductor” son dos términos muy utilizados para referirse a
dispositivos de sensado de alguna variable física, sin embargo no significan lo mismo. El
término transductor se utiliza cuando se quiere considerar un sistema completo de sensado,
donde se lleva a cabo un cambio entre tipos de energía. Un ejemplo de estos sistemas son
los acelerómetros (transductores de aceleración), donde un cambio de estado de un sistema
mecánico se manifiesta como una salida de tensión eléctrica en el dispositivo. Por otro lado
23
el término sensor es utilizado principalmente para referirse a dispositivos que responden
ante algún estímulo del ambiente en el que se encuentran cambiando algún parámetro
intrínseco del dispositivo, pero sin realizar ninguna conversión de energía. Un ejemplo de
sensores son los termistores, los cuales son dispositivos que simplemente cambian el valor
de su resistividad en función de la temperatura del ambiente donde se encuentren. En el
presente proyecto, se harán uso de transductores para la medición de los parámetros de
control del quadcopter.
Dos características de suma importancia para caracterizar los transductores son la
“responsividad” y la “detectabilidad”. El primero de los términos se refiere a la respuesta
del transductor ante un cambio de la variable medida, y el segundo se refiere al valor más
pequeño de la variable medida que el transductor puede detectar. Estos parámetros resultan
más prácticos a la hora de caracterizar un sistema que el término “sensibilidad” (el cual
resulta un poco ambiguo).
Dentro de la familia de los transductores se encuentran los sistemas micro
electromecánicos, o MEMS por sus siglas en inglés, los cuales son la base de los
transductores que son utilizados para la adquisición de la postura del cuadracóptero.
24
2.4.1 Giroscopio
2.4.1.1 Descripción Generalizada
Los giroscopios son dispositivos utilizados para medir o mantener una orientación
específica. Antiguamente solo se contaba con los giroscopios mecánicos, que consistían en
discos giratorios suspendidos sobre un eje que podía rotar en varias direcciones. La
dinámica consistía en que el disco rotatorio permanecía en la misma posición
indiferentemente de cómo rotara la plataforma sobre la cual estuviera montado.
Actualmente se cuenta con otro tipo de giroscopios, los cuales son basados en sistemas
micro-electromecánicos, y han aumentado su popularidad en los dispositivos electrónicos
debido a su reducido tamaño, bajo precio y buena precisión. Una de las mayores
aplicaciones de los giroscopios es para el desarrollo de unidades de medición inercial
(IMU) de vehículos o dispositivos que requieran de este tipo de mediciones, como es el
caso de los vehículos aéreos no tripulados (UAV) para sus sistemas de estabilización.
2.4.1.2 Dispositivo Utilizado (IDG-500)
El IDG-500 es un giroscopio de dos ejes que consiste en dos giróscopos MEMS vibratorios
independientes. Uno de ellos detecta la velocidad de rotación alrededor del eje X, y el otro
detecta la velocidad de rotación alrededor del eje Y. Ambas estructuras se encuentran
25
selladas herméticamente al nivel de la oblea de silicio. Las masas vibratorias de ambos
giróscopos, se hacen oscilar electrostáticamente a una frecuencia de resonancia mediante un
control automático muy preciso que controla la oscilación de las masas. Cuando el sensor
es rotado alrededor del eje X o el eje Y, el efecto Coriolis causa una vibración que puede
ser detectada por un sensor capacitivo. La señal resultante es amplificada, demodulada, y
filtrada para dar paso a una señal analógica que es proporcional a la tasa de cambio de la
posición angular en un determinado eje.
A continuación se puede observar un diagrama de Bode del filtro paso bajo que posee el
giroscopio para acondicionar la señal de salida:
Figura 9. Respuesta de Ganancia y Fase del Filtro del IDG-500
26
El IDG-500 posee dos salidas de velocidad para el eje X, y dos para el eje Y. A
continuación se muestra una tabla con sus principales parámetros:
Tabla 3. Salidas analógicas del IDG-500
Para más especificaciones técnicas del dispositivo, se puede recurrir a la hoja de
especificaciones incluida en la sección de la bibliografía [36].
27
2.4.2 Acelerómetro
2.4.2.1 Dispositivo Utilizado (ADXL335)
El ADXL335 es un sistema completo de medición de aceleración de 3 ejes del proveedor
Analog Devices. Este dispositivo tiene un rango de medición de ±3g. Contiene un sensor
micro-electromecánico y un circuito de acondicionamiento de señal para implementar una
arquitectura de medición de aceleración de lazo abierto. La salida del transductor, son
tensiones eléctricas analógicas directamente proporcionales a la aceleración en cada eje. El
sistema puede medir aceleración estática (como la gravedad), o aceleración dinámica (como
sistemas en movimiento a velocidades no constantes).
El sensor es un sistema micro-electromécanico hecho en silicio sobre la superficie de una
oblea de silicio. Posee resortes de silicio que suspenden una estructura sobre la superficie
de la oblea, y proveen una resistencia contra las fuerzas de aceleración. La deformación de
la estructura cuando es sometida a fuerzas de aceleración es medida mediante un capacitor
diferencial que consiste en una placa fija en la oblea y otra placa adherida a la masa
suspendida. El sensor utiliza una sola estructura para la medición de la aceleración de los
tres ejes ortogonales.
28
A continuación se puede observar un diagrama de bloques funcionales del sistema de
medición:
Figura 10. Diagrama de bloques de acelerómetro ADXL335. Hoja de Especificaciones
Seguidamente, también se muestra una tabla con los parámetros de mayor importancia para
el desarrollo del proyecto, sin embargo también se puede recurrir a la hoja de
especificaciones completa en la sección de bibliografía [37] en caso de que sea necesario.
29
Tabla 4. Especificaciones de Acelerómetro ADXL335
2.4.3 Sensor Ultrasónico de Proximidad
2.4.3.1 Descripción Generalizada
Los sensores ultrasónicos de proximidad (que en realidad son transductores pero son
conocidos popularmente como sensores), trabajan bajo un principio muy similar a un radar,
en el cual se evalúa la distancia de un objetivo al interpretar los ecos de ondas sonoras o de
radio. Los sensores ultrasónicos generan ondas de alta frecuencia, detectan el eco, y
calculan el intervalo de tiempo entre el momento en que se envió la señal, y el momento en
el que se recibió el eco, para determinar la distancia del objeto que se está avaluando. Esta
tecnología se encuentra un poco limitada por la forma de las superficies o la consistencia
30
del material del objeto que se está midiendo, dado que determinados materiales pueden dar
lecturas erróneas en el transductor.
Figura 11. Diagrama de Operación de Sensor Ultrasónico de Proximidad. Fuente: Google Images.
2.4.3.2 Dispositivo Utilizado (HC-SR04)
El sensor ultrasónico de proximidad que se eligió para la realización del proyecto es el HC-
SR04, de la compañía ITead Studios. A continuación se puede apreciar una imagen del
sensor:
Figura 12. Sensor Ultrasónico de Proximidad (HC-SR04). Fuente: Manual de Fabricante.
31
2.4.3.3 Teoría de Operación del Sensor Ultrasónico
La manera en la que este transductor detecta la proximidad de un objeto es la siguiente.
Cuando se introduce un pulso mayor de 10µs en la terminal de “trigger” del dispositivo, el
transductor envía una señal ultrasónica que se encuentra compuesta de 8 pulsos de 40 kHz,
la cual eventualmente será reflejada por un objeto (en caso de que lo hubiese). A esta señal
reflejada se le denomina eco. Desde el momento en el que se emite la señal de 40 kHz hasta
que se recibe su eco, el transductor mantendrá un 1 lógico en su terminal denominada
“ECO”. Cuando el transductor recibe de vuelta esta señal la convierte en una señal
eléctrica, y el siguiente pulso no podrá ser transmitido hasta que el eco de la señal anterior
haya desaparecido. Este tiempo que transcurre entre el momento en que se envía el primer
pulso ultrasónico hasta que se puede enviar el próximo se denomina periodo de ciclo (cycle
period). El periodo de ciclo mínimo recomendado es de 50 ms. La distancia medida por el
transductor es proporcional al tiempo que dure la señal de la terminal de ECO en alto (que
en este caso es una tensión de 5 volts). Si no se detecta ningún obstáculo frente al sensor
ultrasónico, este dará una señal de 38 ms en alto en su terminal de ECO.
La expresión que se utiliza para calcular la distancia en centímetros del objeto (o superficie,
una vez dada la duración del pulso de ECO es la siguiente:
32
Para un mejor entendimiento, a continuación también se brinda una explicación gráfica de
su operación:
Figura 13. Representación gráfica de funcionamiento de sensor ultrasónico de proximidad.
2.5 Actuadores
2.5.1 Motores DC sin escobillas
Los motores DC sin escobillas (también conocidos como motores DC brushless), son
motores síncronos que son alimentados por una fuente eléctrica DC. Sin embargo, a
diferencia de los motores DC con escobillas, los motores sin escobillas requieren de un
conmutador electrónico y sensores adicionales para generar una señal de control que
energice cada una de las bobinas del motor en forma secuencial para dar paso a la rotación
33
del motor. La gran mayoría de estos motores se encuentran dentro de la categoría de
motores síncronos de imanes permanentes, pero también existen en menor proporción,
motores DC sin escobillas de reluctancia o de inducción.
Los motores DC sin escobillas presentan grandes ventajas frente a su contraparte de
escobillas, como por ejemplo, la obvia ausencia de escobillas, que representan una gran
desventaja debido a su desgaste y mantenimiento. Además los motores sin escobillas son
más livianos comparados contra un motor con escobillas de su misma potencia, y también
presentan eficiencias y pares superiores por cada watt consumido. Una característica muy
importante derivada de la ausencia de escobillas, es que los motores DC sin escobillas no
producen chispa alguna cuando se encuentran trabajando. Esto hace a los motores DC sin
escobillas más robustos, confiables y eficientes.
Una de las desventajas de los motores DC sin escobillas, es que requieren de un sistema
electrónico por aparte para poder operar, normalmente denominado como “controlador
electrónico de velocidad” (ESC por sus siglas en inglés), y del cual se hablará más adelante.
El motor DC con escobillas puede operar sin problemas conectado únicamente a una fuente
DC o también con un conmutador electrónico o reóstato para regular su velocidad.
Un parámetro muy importante de los motores DC sin escobillas es la constante “Kv”, la
cual indica la cantidad de revoluciones por minuto a la que gira un motor por cada volt de
34
alimentación. Por ejemplo, un motor sin escobillas de 1000 Kv conectado a una fuente de 2
volts, puede girar a 2000 rpm.
Típicamente los motores DC sin escobillas poseen imanes permanentes adheridos a una
armadura fija, eliminando así los problemas generados para suplir corriente a una armadura
móvil. El controlador electrónico de velocidad es el análogo los conmutadores mecánicos
en los motores con escobillas, y éste se encarga de cambiar continuamente cada una de las
fases del motor para generar un campo magnético rotativo que se encargue de inducir el
movimiento del motor.
Las bobinas de los motores sin escobillas se pueden encontrar tanto en configuraciones
Delta (las cuales entregan menor torque a bajas velocidades, pero generan mayores
velocidades) como configuraciones Estrella (que generan mayor torque a bajas velocidades
pero menor velocidad máxima que la configuración en Delta). [33]
35
Figura 14. Representación de Motor DC sin Escobillas
2.5.2 Controles electrónicos de Velocidad (ESC´s)
Los Controladores Electrónicos de Velocidad (ESC por sus siglas en inglés) son los
dispositivos encargados de coordinar y dar paso a la rotación y control de los motores sin
escobillas. Estas unidades se encargan de generar la secuencia de pulsos trifásicos que son
requeridos para provocar el movimiento de los motores sin escobillas.
Para controlar la rotación del motor, el controlador requiere de un medio para determinar la
orientación del rotor con respecto al estator. Para este fin, algunos diseños utilizan sensores
de efecto Hall (los cuales detectan el campo magnético del rotor) y otros métodos detectan
la fuerza contra electromotriz inducida en las bobinas no energizadas para inferir la
posición del rotor. Este método mencionado anteriormente no requiere de sensores
adicionales, y por ende este tipo de sistemas son llamados controladores sin sensores.
36
Los controladores típicos poseen 3 salidas bidireccionales, las cuales popularmente
consisten en un arreglo de FETs controlados por un circuito lógico. Normalmente este
circuito es un microcontrolador el cual se encarga de administrar la aceleración, controlar la
velocidad de rotación y afinar la eficiencia adaptándose al sistema en el cual se encuentra
trabajado el controlador. Para realizar el proceso anterior, normalmente los ESC tienen que
ser configurados con los parámetros del sistema en el cual están trabajando (como por
ejemplo el tipo de baterías al cual está conectado, las tensiones mínimas de operación
segura, el tipo de arranque del motor, etc).
El reto al que se enfrentan los controladores que infieren la posición del rotor con respecto
a la fuerza contra electromotriz de las bobinas que no están en uso, es a la hora de iniciar el
movimiento, ya que no hay una fuerza contra electromotriz (el rotor se encuentra estático, y
por ende no hay tensión inducida). Usualmente, el procedimiento que se sigue es iniciar el
movimiento desde una fase arbitraria, y luego corregirla en caso de que no sea la correcta.
A continuación se encuentra una figura que ilustra el principio de funcionamiento de los
motores sin escobillas trifásicos. Consiste en un motor de 3 bobinas por fase, cuyo rotor es
su armadura. La secuencia trifásica de pulsos que se encargan de energizar las bobinas de
las fases A, B y C son generados por el ESC. Para este ejemplo en particular, se necesita
repetir 6 veces una secuencia de 6 combinaciones distintas de pulsos para lograr una
revolución completa del motor (para un total de 36 pasos).
37
Figura 15. Principio de funcionamiento de motores sin escobillas [6]
2.6 Comunicaciones Inalámbricas
2.6.1 Comunicación Serial
Las comunicaciones seriales se refieren al proceso de transmitir datos un bit a la vez, de
una manera secuencial por medio de algún canal de comunicación. Este tipo de
comunicaciones se realiza cuando el costo de tener varios canales es muy alto, o cuando
realizar un proceso de sincronización de varias señales resulta complicado. La desventaja
38
de las comunicaciones seriales, es que poseen tasas de transmisión de datos más bajas que
las comunicaciones en paralelo, dado que se pueden transmitir menos datos por unidad de
tiempo, sin embargo, resulta ventajoso para reducir costos cuando la velocidad de
transmisión de datos no es un factor fundamental para la operación del dispositivo.
Como ejemplos de los buses más conocidos que implementan comunicación de tipo serial
se pueden mencionar: SPI, I2C (ambas pueden ser implementadas mediante el Arduino),
USB, PCI, Bluetooth y 1-Wire.
2.6.2 Protocolo Bluetooth
Bluetooth es un estándar abierto de comunicaciones inalámbricas, que permite intercambiar
datos de manera serial a distancias cortas mediante radiofrecuencias que van desde los 2400
MHz hasta los 2480 MHz. Este protocolo fue creado por la empresa de telecomunicaciones
Ericsson en 1994, el cual fue concebido originalmente como una alternativa inalámbrica a
los cables RS-232.
Este protocolo está diseñado para comunicar dos o más dispositivos al mismo tiempo, los
cuales normalmente se encuentran gobernados por un dispositivo maestro que se encarga de
realizar la sincronización de datos del grupo.
39
El protocolo Bluetooth se encuentra regido por el “Bluetooth Special Interest Group” o SIG
por sus siglas en inglés, el cual es un grupo formado por más de dieciséis mil compañías de
áreas de telecomunicaciones, computación y dispositivos electrónicos. Este grupo se
encarga de velar por que se cumplan los estándares requeridos para los nuevos dispositivos
que requieran hacer uso de esta tecnología.
Dada la naturaleza de comunicación inalámbrica de esta tecnología, esta presenta ciertos
riesgos de seguridad, debido a la exposición de los datos que se transmiten. Para evitar
exponer datos privados a receptores no deseados, este protocolo cuenta con un proceso de
emparejamiento, en el cual se crea un primer enlace donde el usuario identifica los
dispositivos que quiere comunicar entre sí, y una vez realizado el proceso, los dispositivos
pueden seguir comunicándose entre sí con la seguridad del conocimiento del usuario de
donde se están transmitiendo los datos. Una vez realizado el emparejamiento, los
dispositivos guardan esta configuración para futuras transacciones, o bien se puede
restablecer la configuración y modificar la lista de dispositivos autorizados a compartir
información entre sí.
Para información adicional se puede recurrir al estándar de telecomunicaciones de la IEEE
802.15.1-2002, donde se detalla más profundamente las características de este estándar.
40
2.7 Procesamiento de Señales
2.7.1 Introducción
Debido a la naturaleza analógica de los sensores y transductores que se utilizan para
percibir el entorno y el estado del cuadracóptero, éstos proporcionan señales de salida que
se van a ver sometidas a ciertos niveles de ruido (causado tanto por vibraciones mecánicas
como por interferencias electromagéticas). Por esta razón, resulta de gran importancia para
el adecuado control del vehículo, acondicionar estas señales de manera que se puedan
eliminar los datos provenientes de los sensores que no sean provechosos para el control del
vehículo (o dicho de manera más sencilla, el ruido).
Para el acondicionamiento de estas señales se encuentran un gran número de técnicas de
procesamiento digital de señales (tanto así que es todo un campo de estudio de la ingeniería
eléctrica), sin embargo, debido al alcance de este proyecto y las limitaciones de tiempo y
hardware, solamente se podrán implementar las técnicas de menor complejidad matemática,
y que no requieran de mucho recurso computacional (recordemos que solo se va a contar
con un microcontrolador de 8 bits y 16 MIPS). Esto debido a que la ejecución de los
algoritmos de control (y por ende el acondicionamiento de las señales), se debe ejecutar en
tiempo real.
41
2.7.2 Manipulación de los datos de los sensores (IMU)
Como una técnica para disminuir el nivel de ruido propio de los acelerómetros, y disminuir
el efecto del deslizamiento de las medidas de los giroscopios, se puede hacer uso de una
fusión de las mediciones tomadas de ambos transductores. Dado que se puede obtener una
medida del desplazamiento angular por medio de ambos sensores, entonces, al asignar
pesos dinámicos que sean relativos a la confiabilidad de las medidas de cada uno de los
sensores, se puede obtener una medida más robusta del desplazamiento angular real del
vehículo al sumar mediante pesos dinámicos y promediar el resultado final.
2.7.3 Media Móvil
En estadística, una media móvil, es un tipo de filtro utilizado para analizar grupos de datos
creando una serie de promedios de diferentes subgrupos del grupo global de datos. La
dinámica simplificada de una media móvil se describe a continuación.
Dada una serie de números y un tamaño de subgrupo fijo, el primer elemento de la media
móvil es obtenido sacando el promedio de los valores del primer subgrupo de la serie de
números. Luego, este subgrupo de datos es modificado excluyendo el primer número de su
serie, y reemplazándolo por el siguiente número de la serie original de datos. Esto genera
42
un nuevo subgrupo de datos, el cual a su vez tiene un valor de promedio numérico nuevo.
Este proceso es repetido hasta abarcar todos los datos de la serie numérica original. A la
gráfica que conecta los valores de cada una de las medias obtenidas se le denomina “media
móvil”.
Las medias móviles también pueden ser utilizadas agregando pesos distintos a los valores
que son de mayor interés, para que estos tengan mayor o menor impacto sobre el promedio
de cada subgrupo. Para ilustrar de manera más sencilla lo explicado anteriormente se puede
observar la siguiente figura:
Figura 16. Ilustración Media Móvil
43
Dado un grupo de 8 datos, y un tamaño de subgrupo de 3 datos, se procede a sacar el
promedio de los tres primeros datos. Luego el subgrupo se desplaza una casilla hacia la
izquierda y se obtiene de nuevo el promedio del subgrupo. Este proceso se repite hasta
llegar al final de los datos, o en el caso de que sea un proceso que genera datos
continuamente, hasta que termine el proceso (como lo será nuestro caso). Seguidamente se
observa un gráfico lineal donde se pueden observar los datos originales del ejemplo, y los
de la media móvil. Se puede observar como la media móvil sigue la misma tendencia de los
datos originales, solamente que se ve menos afectada por los picos que existan en la serie
de datos original.
Figura 17. Gráfico Ilustrativo de Media Móvil
44
Esta es una de las principales utilidades de la media móvil, el atenuar las fluctuaciones de
pequeña duración de los datos originales, y darles más importancia a las tendencias de largo
plazo. La diferencia entre largo y corto plazo depende de cada aplicación, por ende, el
tamaño de los subgrupos, y el peso de los valores (en caso de que se requieran agregar) se
deben programar adecuadamente dependiendo del fin para el cual se utilicen.
La media móvil también se puede ver como un tipo de filtro paso bajo, donde los datos
aleatorios de alta frecuencia pueden ser atenuados por medio del promediado de valores
para obtener una tendencia más adecuada y eliminar el ruido del sistema. Cabe resaltar que
esto también causa problemas si los parámetros no se encuentran debidamente programados
dado que mientras más se trate de atenuar el ruido, más lenta será la respuesta del sistema.
2.8 Control Automático
2.8.1 Controlador PID
Los algoritmos de control PID (Proporcional Integro-Diferencial) son los sistemas de
control más utilizados en la industria de la automatización de procesos debido a su
versatilidad y adaptabilidad a diversos procesos controlables.
Los controladores PID son sistemas que trabajan bajo el principio de realimentación de lazo
cerrado, los cuales se encargan de manipular una o más variables de un sistema (las cuales
45
se trabajan como entradas del sistema) con la finalidad de reducir la diferencia existente
entre la salida real del sistema y la salida deseada del sistema (conocido como el error).
Los algoritmos de control PID poseen 3 componentes cuya suma da resultado al parámetro
de control de la variable manipulada (entrada del sistema). Estos elementos son: la
componente proporcional, la componente integral y la componente derivativa. Una correcta
sintonización de las ganancias de estos parámetros, y un apropiado modelado de todo el
sistema, es lo que da paso a lograr una respuesta de la salida del sistema en la que se
minimice su diferencia con respecto al valor esperado, de la manera más rápida y estable
posible.
La ecuación generalizada de los controladores PID viene dada por la siguiente expresión:
Donde:
t: corresponde al tiempo transcurrido, o tiempo instantáneo.
u(t): corresponde a la salida del controlador.
e(t): corresponde al error (valor deseado – salida real del sistema).
: corresponde a la ganancia proporcional del controlador.
: corresponde a la ganancia integral del controlador.
: corresponde a la ganancia derivativa del controlador.
46
A continuación se puede observar una representación gráfica del controlador:
Figura 18. Diagrama de bloques de controlador PID Paralelo
2.9 Fuente de Poder
2.9.1 Batería de Polímero de Ion de Litio (LiPo)
Las baterías de Polímero de Ión de Litio, también conocidas como baterías de Polímero de
Litio o LiPo por su abreviatura en inglés, son un tipo de baterías recargables de alto
rendimiento, las cuales normalmente se encuentran compuestas de una o varias celdas que
se disponen tanto en serie (para aumentar la tensión de salida de la batería) como en
paralelo (para aumentar la capacidad y la corriente de descarga de la batería). Se
caracterizan porque permiten niveles altos de descargas de corriente eléctrica, motivo por el
cual, estas serán las baterías seleccionadas para el desarrollo del presente cuadracóptero.
47
3. CAPÍTULO III: Desarrollo e Implementación del UAV
3.1 Diagramas de Bloques del Cuadracóptero
A continuación se presenta el diagrama de bloques a grandes rasgos con el cual se va
proceder con el modelado del cuadracóptero por desarrollar. Cada uno de los bloques
representa un subsistema que será diseñado y validado por aparte, para luego formar parte
de todo el sistema. Esto permite segmentar el proyecto en diversas etapas las cuales hacen
que la validación y la puesta en funcionamiento de cada una de ellas resulte más accesible
al desarrollador.
Módulo de control
principal
Control de
PotenciaMotores
Módulo de
Sensado
Módulo de
Comunicaciones
Fuente de
Alimentacion
Figura 19. Diagrama de Bloques de Controlador PID
48
Cabe aclarar que el cuadracóptero que se construirá, se diseñará en configuración “+”. A
continuación se detalla visualmente los dos tipos de configuraciones posibles para el diseño
de cuadracópteros:
Figura 20. Cuadracóptero en configuración "x"
Figura 21. Cuadracóptero en configuración "+".
49
Por sutil que parezca la diferencia, esto influye en la programación y calibración que se
realizará en los sistemas de control. Se elige la configuración “+” debido a que resulta de
más sencilla aplicación.
A continuación se expone el diagrama de bloques funcional para cada uno de los motores.
La velocidad final de cada uno de ellos, estará constituida por la suma de la salida del PID
de control de altitud, el PID de control de rotación alrededor del eje X (o Y), y un valor de
velocidad fijo para iniciar con una velocidad conocida cuando se encienda el
cuadracóptero. El resultado de dicha suma (la cual se convertirá en una señal PWM por el
microcontrolador), será ingresado en el bloque de control de potencia de los motores
(ESC), donde se convertirá la señal PWM resultante, en el conjunto de señales trifásicas
correspondientes para lograr la velocidad angular requerida para cada uno de los motores.
PID Rotacion Eje X
PID Altitud
Velocidad Motor X
Valor Fijo
S
Motor X
Entrada de Señales de Transductores
Salida de Velocidad de Motores
50
Figura 22. Diagrama de bloques funcionales para cada motor
Seguidamente, en el gráfico que se muestra a continuación, se ilustra a grandes rasgos el
lazo de realimentación y ejecución. Cada uno de los bloques de los motores se debe
interpretar como el subsistema que se explicó anteriormente. Como se puede apreciar en la
figura, la posición y postura del cuadracóptero va a estar determinada por la interacción del
comportamiento de cada uno de los cuatro motores. Esta postura va a ser traducida en un
conjunto de señales eléctricas por los transductores, las cuales van a ser introducidas en los
bloques de control por medio del micro-controlador (Arduino).
Motor 1
Motor 2
Motor 3
Motor 4
S Posición y Postura
DeseadaTransductores de Posición y Postura
Figura 23. Lazo de ejecución y realimentación del cuadracóptero.
51
La tabla de asignación de entradas y salidas del Arduino será como se muestra a
continuación. Cabe resaltar que hay un pin de entrada analógica libre, sin embargo,
próximamente este será destinado para la salida del eje “Z” de un nuevo giroscopio. Este
nuevo eje del giroscopio será requerido para la corrección de la rotación alrededor del eje Z
del cuadracóptero, la cual es causada por la diferencia de los pares rotacionales entre los
motores CW y los motores CCW. Esta tabla se propuso de esta manera dado que el
giroscopio con el cual se cuenta actualmente solo posee dos grados de libertad.
Como se observa, se tienen seis entradas/salidas digitales libres, las cuales se pueden
utilizar en un futuro para LEDs de señalización.
Tabla 5. Tabla de asignación de pines de Arduino
52
3.2 Experimentos
3.2.1 Validación del módulo Bluetooth
La validación de este módulo se realizó de una manera muy sencilla. Primero se procedió a
la programación de los parámetros propios del módulo, como lo es su tasa de transmisión
de datos y su respectivo emparejamiento con la computadora y el teléfono inteligente. Este
proceso se realizó siguiendo los pasos detallados en la hoja de especificaciones del
dispositivo.
Luego, se realizó un sketch sencillo para el Arduino, el cual permitiera observar que se
estaban transmitiendo los datos correctamente entre la plataforma Android y el Arduino. El
sketch utilizado se puede observar en el apéndice 1 de la sección de anexos. La dinámica
consistió en utilizar 5 leds de distintos colores que se encontraban conectados a las salidas
digitales del microcontrolador, y prenderlos o apagarlos desde el teléfono inteligente de
manera remota por medio del adaptador bluetooth utilizado en conjunto con el
microcontrolador Arduino.
A continuación, se puede observar un diagrama sencillo del proceso que se detalló
anteriormente para la validación del módulo bluetooth del Arduino.
53
Figura 24. Diagrama de método de validación de módulo bluetooth en conjunto con Arduino.
La validación resultó exitosa, no hubo ningún tipo de comportamiento anómalo, y se logró
transmitir a una distancia de aproximadamente 15 metros con el arduino en un cuarto y el
transmisor en otro cuarto distinto. Sumado a la prueba visual realizada mediante los LEDs,
se comprobó la integridad de los datos transmitidos mediante la comunicación serial entre
el Arduino y el computador por medio del monitor serial de datos que posee la misma
aplicación con la cual se realiza la programación del Arduino (se puede observar una
captura del monitor serial en la figura a continuación).
Figura 25. Captura de Monitor Serial de Aplicación de Programación de Arduino.
54
3.2.2 Validación de los controles electrónicos de velocidad (ESC)
Para la experimentación con los controladores electrónicos de velocidad se procedió de la
siguiente manera:
Programación de parámetros de los controladores de velocidad (ESC).
Cada ESC debe ser programado antes de su primera utilización. Esto debido a que
requieren de diversos parámetros propios del sistema donde van a ser utilizados para un
funcionamiento óptimo. El proceso a seguir para la programación de los controladores de
velocidad varía dependiendo de la marca y el modelo, de modo que para la programación
de los controladores utilizados se siguieron las instrucciones suministradas por el
proveedor.
Luego de que los cuatro controladores fueron programados, se procedió a su respectiva
validación en conjunto con el Arduino Uno. Para esto se realizó un pequeño sketch
exclusivo para el control de velocidad de los motores. A grandes rasgos, el concepto del
experimento de validación fue utilizar una señal analógica de entrada, generada mediante
un potenciómetro y una fuente de tensión no variable, con la cual se lograra obtener un
control lineal proporcional de la velocidad de los motores en función de la tensión variable
generada mediante el potenciómetro. Los cuatro pines de control de cada uno de los
controladores de velocidad fueron ubicados en las correspondientes salidas PWM del
55
Arduino, y la tensión variable del potenciómetro se colocó en una de las entradas
analógicas del microcontrolador. El experimento de validación resultó exitoso, y el control
de los motores se pudo llevar a cabo mediante la señal analógica de entrada que fue
generada por el potenciómetro.
3.2.3 Validación de la Unidad de Medición Inercial (IMU)
Este apartado correspondió a uno de los mayores retos del desarrollo del proyecto. Como se
mencionó en el II Capítulo, los sensores utilizados para el sistema de medición inercial
(acelerómetros y giroscopios), poseen salidas cuyas señales poseen un determinado nivel de
ruido que va en detrimento de la robustez del control automático del cuadracóptero. Este
ruido es dependiente del tipo de componentes mecánicos que se estén utilizando (como por
ejemplo las características de las hélices, el material del marco, etc). Por esta razón se tuvo
que implementar una etapa de filtrado digital de las señales para tratar de eliminar la mayor
parte del ruido de cada una de las señales de entrada.
Para lograr esto, se implementó la técnica de la media móvil para realizar un promediado
dinámico de los valores de cada una de estas señales, de modo que se disminuyera el nivel
de ruido, sin desfasar en gran medida la señal filtrada de la señal real. Esto es de suma
importancia debido a que los controladores PID requieren obtener su señal de entrada en
tiempo real para un correcto funcionamiento del sistema.
56
El parámetro requerido para este filtro (cantidad de muestras que se promedian en cada
ciclo de ejecución), se obtuvo experimentalmente, con un método de prueba y error, hasta
obtener el parámetro que mostrara el mejor resultado posible.
Además, con la finalidad de disminuir el ruido de las señales, se configuró la frecuencia de
muestreo de las señales de los acelerómetros y giroscopio a 100 Hz, mientras que la
frecuencia de muestreo del sensor ultrasónico de proximidad se configuró a 10 Hz. Esto
debido a que cada muestreo de este sensor puede durar hasta 25 ms (varía dependiendo de
la distancia del objeto o superficie), de modo que si se realizara a la misma frecuencia que
el muestreo de los acelerómetros y giroscopios, se alargaría mucho el tiempo de ejecución
de cada ciclo, y por ende se disminuiría la frecuencia del lazo principal.
A continuación se observa un gráfico que muestra la señal sin filtrar (en azul) junto con la
señal filtrada (rojo). Esta captura fue tomada con los motores encendidos a un 15% de su
velocidad máxima. Se puede apreciar la gran reducción del ruido que se logró al realizar la
fusión de las señales de los acelerómetros y los giroscopios, junto con el promediado
dinámico expuesto anteriormente. El promediado se realizó con un índice de 20 muestras
por ciclo.
57
Figura 26. Comparación entre señal original y señal filtrada de uno de los acelerómetros.
Además se puede observar el desfase que se obtiene entre la señal filtrada (roja) y la señal
sin procesar (azul). Para este experimento se conservaron los mismos parámetros de la
prueba anterior, solamente que se apagaron los motores para observar una señal más limpia
de los acelerómetros. También cabe mencionar que para estas pruebas se utilizó un peso
relativo del 80% para la señal del acelerómetro y un peso de 20% para la señal del
giroscopio. Se observó que mientras mayor peso se le asigne a las lecturas del giroscopio,
mayor es el desfase experimentado entre la señal filtrada y la señal original.
Figura 27. Prueba para observar el desfase entre la señal filtrada y la original, con los motores
apagados.
58
Por último, se realizó una prueba para observar la inmunidad del sistema ante la presencia
de señales no deseadas. Para este caso, se aplicaron pequeños golpes al sensor, y se
observaron ambas señales de interés (señal filtrada y señal original). Se puede observar que
el sistema final resulta bastante inmune a este tipo de perturbaciones anómalas en el
sistema. Esto es muy importante para evitar comportamientos no deseados de todo el
sistema ante la presencia de perturbaciones no deseadas en las lecturas de los sensores.
Figura 28. Prueba realizada para observar la inmunidad del sistema ante señales no deseadas.
Una de las principales razones por la cual se le asigna tanta importancia al
acondicionamiento de las señales, es porque estas serán introducidas en el controlador PID,
de modo que dependiendo de las ganancias de cada una de las componentes, así será el
impacto en cada una de ellas. En el caso de la parte integral del controlador, no se tiene
mayor problema con el ruido, dado que si este presenta una distribución gaussiana, no
afectará en gran medida el comportamiento del controlador. Sin embargo, la componente
59
diferencial del controlador, si se verá muy afectada, dado que la pendiente de las señales de
ruido de alta frecuencia es muy pronunciada, motivo por el cual la derivada de estas señales
resulta en una cifra muy alta. Debido a esta razón, la ganancia de la componente derivativa
del controlador se tiene que subdimensionar para lograr que sea inmune a este ruido. El
problema con esto, es que mientras más se disminuye esta ganancia, el controlador no
reacciona de manera adecuada ante cambios bruscos reales en la postura del cuadracóptero,
y su debida corrección de postura no se ejecuta de la manera adecuada.
De aquí la importancia de tener una señal bien acondicionada, para poder dimensionar
correctamente la ganancia de la componente derivativa del controlador, y de esta manera
logar que el cuadracóptero responda de una mejor manera ante cambios bruscos en su
postura.
3.2.4 Validación del Transductor Ultrasónico de Proximidad
Para la validación del transductor ultrasónico se decidió utilizar la librería facilitada por el
fabricante del dispositivo. La dinámica consistió en conectar el dispositivo al Arduino, y
luego observar si las distancias obtenidas mediante el sensor ultrasónico coincidían con las
medidas reales de distancia (las cuales fueron medidas con una cinta métrica). Los
resultados fueron los esperados, y las lecturas fueron muy precisas. Cabe resaltar que
mediante la experimentación se pudieron medir distancias que fueron desde los 2 cm hasta
los 500 cm. Fuera de ese rango, las lecturas eran inestables.
60
El Sensor Ultrasónico de Proximidad se validó con el siguiente sketch:
#include "Ultrasonic.h"
Ultrasonic ultrasonic(12,13); // Trigger en Pin 12 y Echo en Pin 13
void setup() {
Serial.begin(115200);
Serial.print("testing...");
}
void loop()
{
Serial.println
(ultrasonic.Ranging(CM)); //La salida se puede obtener en centímetros (CM) o pulgadas (IN).
delay(500);
}
El resultado de la altura que percibe el módulo una vez instalado en el cuadracóptero es de
6 cm. Las medidas son muy estables.
3.2.5 Validación del sistema de control automático.
Para la validación del sistema de control automático, primero se procedió a obtener las
ganancias de las componentes de cada uno de los controladores PID. Dado que no se cuenta
con un modelo matemático del sistema, y la respuesta del sistema a los escalones es un
proceso complejo de obtener sin el equipo necesario, las ganancias de cada PID se
obtuvieron mediante un proceso de experimentación hasta llegar a los parámetros que
dieran los mejores resultados.
61
Para realizar este proceso, la dinámica que se siguió fue calibrar cada PID por separado
hasta obtener el comportamiento deseado, para luego ponerlos a trabajar todos juntos una
vez que ya estuvieran correctamente calibrados.
Primero se procedió con la obtención de los parámetros para el PID de control de rotación
alrededor del eje X. Para realizar esto, se restringió la rotación del cuadracóptero alrededor
del eje Y al apoyar el eje X en una superficie libre de fricción que lo permitiera rotar. De
esta manera, solo se trabajó con los motores 2 y 4 encendidos, los cuales eran los
encargados de mantener el equilibrio del cuadracóptero (mientras tanto, los motores 1 y 3
permanecen apagados).
Figura 29. Experimento para la obtención de los parámetro de PID de inclinación Eje X
Inicialmente, el proceso que se siguió, fue configurar todas las ganancias de este PID en 0.
Luego, poco a poco se fue aumentando la ganancia proporcional del controlador hasta
62
llegar a un punto donde el cuadracóptero se encontrara cerca de empezar a oscilar alrededor
de su punto de referencia (ángulo de inclinación de cero grados con respecto a la
horizontal). Ya una vez que se obtuvo el valor de este parámetro, se siguió el mismo
proceso pero para la ganancia diferencial, solo que esta vez con la ganancia proporcional en
el valor encontrado anteriormente. Una vez que se encuentra el valor de la ganancia
proporcional de esta manera, se prosigue a buscar el valor de la ganancia de la componente
integral, partiendo del hecho que se conocen las otras dos.
Cabe resaltar, que los parámetros encontrados para el sistema con un tipo de hélice, no son
válidos si se cambian las hélices, de modo que se tienen que volver a obtener los
parámetros.
Luego de obtener los parámetros del PID del Eje X, se procede a realizar el procedimiento
análogo pero para el controlador PID del Eje Y. Esto debido a que el cuadracóptero no es
completamente simétrico, de modo que los parámetros varían dependiendo de la
distribución de la masa del vehículo.
El proceso de experimentación fue bastante largo, sin embargo, se encontró un punto de
operación donde el cuadracóptero se logra estabilizar de manera automática alrededor del
punto de referencia que se le definiera. Los resultados de esta configuración se pueden
63
observar más claramente en el video que se encuentra ingresando en el siguiente link:
http://www.youtube.com/watch?v=-r1t5x4S7XY&feature=plcp.
Para este punto de operación, las ganancias que se obtuvieron fueron las siguientes:
Para el Eje X: Kp=5; Ki=4.5; Kd=3.1
Para el Eje Y: Kp=5.4; Ki=4.5; Kd=3.3
Los parámetros se obtuvieron con hélices de 8 pulgadas de diámetro. Cabe destacar que la
diferencia entre los parámetros del eje X y del Eje Y se debió principalmente a la forma de
la batería (dado que constituye un 30% del peso total) y como se encontraba dispuesta en el
vehículo (dado que no es simétrica).
Este proceso de obtención de los parámetros se puede facilitar y optimizar al contar con el
equipo necesario para poder modelar el cuadracóptero y poder obtener la respuesta al
escalón de cada uno de los ejes. Sin embargo, por motivo de tiempo y recursos, para la
realización del presente proyecto no se pudo contar con las herramientas requeridas. No
obstante, fuera del hecho que la obtención de los parámetros no se dio de la manera
científica-matemática deseada, los resultados que se obtuvieron fueron muy aceptables.
Además se aclara que para la experimentación que se detalló anteriormente, se incluyó una
protección en la salida de los controladores PID, de modo que la salida se sature a cierto
nivel conocido (en nuestro caso un 30% de la velocidad máxima de los motores). Esto para
evitar que por algún error de programación o algo similar, el cuadracóptero se salga de
64
control y ocurra algún accidente. La adición de todo tipo de protecciones es una práctica
recomendada en caso de que se vaya a experimentar o realizar algún cambio del que no se
esté completamente seguro de su resultado, de modo que haya seguridad de que el
cuadracóptero no se va a salir de control y dañarse o lastimar a alguien.
3.3 Lista de Componentes Utilizados
A continuación se puede observar una tabla resumen con los componentes utilizados en la
construcción del prototipo y sus costos correspondientes. Se aclara que el marco se realizó
de manera casera, motivo por el cual no se encuentra dentro de la tabla de costos:
Tabla 6. Lista de Componentes y Costos del Cuadracóptero
65
El código de programación del Arduino que se utilizò para realizar las pruebas se encuentra
en el Anexo2. Una vez que se ensambló por completo el prototipo, el resultado final fue el
siguiente:
Figura 30. Modelo tridimensional del primer prototipo del Cuadracóptero.
Figura 31. Modelo Real del Primer Prototipo del Cuadracóptero.
66
Figura 32. Modelo Real del Primer Prototipo del Cuadracóptero.
Figura 33. Parte inferior del primer prototipo del cuadracóptero.
67
4. Capítulo 4: Conclusiones y Recomendaciones
4.1 Conclusiones
El desarrollo de todos los sistemas separados del cuadracóptero y su posterior
integración resultó ser un proceso de mayor complejidad que lo proyectado
inicialmente.
La etapa de pruebas y verificación de cada subsistema del cuadracóptero resultó de
gran importancia y ayuda para entender mejor su funcionamiento y corregir los
errores de diseño que se realizaron inicialmente. De esta manera, a la hora de ser
integrados se tiene un mayor nivel de confiabilidad de que van a funcionar
correctamente.
La etapa de validación y pruebas de los sistemas que requirieron el uso de las
hélices resultó ser más peligroso de lo pensado, dado que los motores se encuentran
girando a muy altas velocidades y existe el riesgo de un malfuncionamiento por
algún error de programación. Se tuvieron que tomar todas las precauciones del caso
para hacer las pruebas de una manera segura.
68
El Filtro Kalman resultó tener una complejidad más alta de lo pensado inicialmente,
motivo por el cual, dadas las limitaciones del alcance del proyecto, no se pudo
implementar en la entrega final del primer prototipo del cuadracóptero.
El desarrollo de un proyecto multidisciplinario como el presente, requiere de mucho
tiempo, dedicación y planeamiento, debido a la gran cantidad de investigación,
pruebas y experimentos que se tienen que llevar a cabo en un lapso de tiempo muy
limitado. Esto es de mucha importancia para poder llegar a tener un prototipo
estable y funcional.
El microcontrolador del arduino se encuentra subdimensionado para la apliación.
Para obtener un prototipo de vuelo autónomo se requiere de mucho procesamiento
matemático en tiempo real, motivo por el cual el Arduino no es adecuado para un
vuelo que no sea asistido por un piloto. Al tener mucho procesamiento matemático,
el ATmega328 muestra una disminución en la frecuencia del lazo de ejecución del
programa hasta llegar a un nivel donde es muy bajo para la correcta operación
dinámica de los motores.
La comunicación de datos vía bluetooth se realizó de manera exitosa en ambas vías
(transmisión y recepción), motivo por el cual se pudo completar exitosamente ese
objetivo específico. El Arduino se pudo comunicar sin ningún problema tanto con la
69
computadora personal como con el dispositivo Android para enviar los datos de
postura y otras variables de interés para el usuario por medio del shield bluetooth.
Los motores sin escobillas mostraron un desempeño superior al esperado, al trabajar
en conjunto con sus respectivos controladores electrónicos de velocidad. Estos
motores poseen una relación potencia efectiva/peso mucho más elevada que su
contraparte con escobillas.
La elección y balanceo correcto de las hélices de los motores resulta fundamental
para el buen funcionamiento del sistema, ya que provoca menos vibración en el
sistema, y un vuelo más estable.
El uso de un teléfono inteligente como mando a distancia del cuadracóptero resultó
ser un muy buen ejercicio académico, sin embargo resulta poco práctico tener que
controlar un sistema tan dinámico y de rápida respuesta por medio de una pantalla
táctil.
70
4.2 Recomendaciones
Se recalca el cuidado que hay que tener al hacer las pruebas con las hélices
puestas. Se recomienda utilizar el equipo de protección requerido.
Realizar pruebas independientes de cada sistema y entender el impacto del
cambio de cada uno de sus parámetros en su comportamiento final.
Realizar un buen cronograma del proyecto y apegarse. No olvidar dejar un lapso
de tiempo para imprevistos en el desarrollo del proyecto.
Tener cuidado de vigilar la tensión de la batería utilizada para no descargarla
más allá de los límites recomendados. Esto puede causar un daño perjudicial en
la batería que va en detrimento de su vida útil.
Realizar la sintonización de los controladores PID con un método analítico de
sintonización para mejorar la robustez y confiabilidad del sistema.
Tomar en cuenta la necesidad de plataformas de prueba que se van a tener que
utilizar para validaciones. Este es un recurso muy importante dado que permite
realizar los experimentos requeridos de una manera segura y versátil.
71
Documentar muy bien todos los experimentos que se realicen, y los cambios que
se le hagan a los sketches (a manera de bitácora).
Hacer buen uso de los profesores tutores para orientar la resolución de los
problemas de alta complejidad que se presenten durante el desarrollo del
proyecto.
4.3 Trabajo Futuro
Realizar un modelado matemático del cuadracóptero para poder crear un sistema
de control más robusto mediante el uso de este modelo (Como por ejemplo con
Filtros de Kalman).
Realizar un estudio del consumo de potencia de cada uno de los bloques
funcionales del dispositivo para buscar maneras de optimizar su tiempo de
vuelo.
Cambiar el microcontrolador actual por uno más potente, de modo que se
puedan implementar algoritmos más complejos, y añadirle más funcionalidades
al cuadracóptero. Además se requieren más entradas analógicas para añadir más
sensores al vehículo (magnetómetros).
72
BIBLIOGRAFÍA
Artículos y Papers:
1. Martin, Philippe; Salaun, Erwan. “The True Role of Accelerometer Feedback in
Quadrotor Control”. 2010 IEEE International Conference on Robotics and
Automation. Mayo 2010.
2. Norafizah, Abas; Ari, Legowo; Rini, Akmeliawati. “Parameter Identification of an
Autonomous Quadrotor”. 2011 4th International Conference on Mechatronics
(ICOM). Malaysia, Mayo 2011.
3. Atheer L. Salih, M. Moghavvemi, Haider A. F. Mohamed. “Modelling and PID Controller
Design for a Quadrotor Unmanned Air Vehicle”. Centre for Research in Applied
Electronics (CRAE), University of Malaya.
4. Paul Pounds, Robert Mahony, Peter Corke. “Modelling and Control of a Quad-
Rotor Robot”. Australian National University, Canberra, Australia.
5. Jun Li, Yuntang Li. “Dynamic Analysis and PID Control for a Quadrotor”. 2011
IEEE International Conference on Mechatronics and Automation. Agosto
2010.
6. Roman Czyba. “Design of Attitude Control System for an UAV Type-Quadrotor
Based on Dynamic Contraction Method”. 2009 IEEE/ASME International
Conference on Advanced Intelligent Mechatronics. Singapore, 2009.
73
7. Alexandros Soumelidis, P´eter G´asp´ar, Gergely Regula, B´ela Lantos. “Control of
an experimental mini quad-rotor UAV”. 16th Mediterranean Conference on
Control and Automation. France, 2008.
8. Kento Yamasaki, Takashi Takimoto, Keiichi Yoshino. “Attitude Control of
Quadrotor Helicopters with Sensor Offsets”.
9. A. Zul Azfar, D. Hazry. “A Simple Approach on Implementing IMU Sensor Fusion
in PID Controller for Stabilizing Quadrotor Flight Control”. 2011 IEEE 7th
International Colloquium on Signal Processing and its Applications.
Libros:
10. Usher M.J. and Keating D.A. “Sensors and Transducers”, Segunda Edición,
Macmillan, Inglaterra, 1996.
11. Subir Kumar, Saha. “Introducción a la Robótica”. I Edición, Mc Graw Hill, 2010.
Páginas Web:
12. “Teoría de Diseño Electromagnético de Motores DC y Actuadores”
http://www.consult-g2.com/course/chapter9/chapter.html
13. “Baterías de Níquel-Cadmio”
http://en.wikipedia.org/wiki/Nickel%E2%80%93cadmium_battery
74
14. “Motores Sincrónicos” http://en.wikipedia.org/wiki/Synchronous_motor
15. “Controlando Motores DC sin escobillas con Arduino”
http://elabz.com/brushless-dc-motor-with-arduino/
16. Nicolas Oros. “Carro a Control Remoto Android”
http://www.cogsci.uci.edu/~noros/android_car.html
17. “Arduino Bluetooth Shield”
http://iteadstudio.com/store/index.php?main_page=product_info&cPath=18&produ
cts_id=307
18. “Declaración de Funciones en Arduino”
http://arduino.cc/en/Reference/FunctionDeclaration
19. “Librería de Comunicaciones Seriales de Arduino”
http://arduino.cc/en/Reference/SoftwareSerial
20. “Comunicaciones seriales mediante Arduino”
http://www.smacula.co.uk/2011/07/arduino-serial-communication.html
21. “Comunicaciones Seriales de Plataforma Arduino”
http://arduino.cc/en/Reference/Serial
22. “Mejorando el controlador PID para Arduino”
http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/
23. “Librería PID para Arduino” http://arduino.cc/playground/Code/PIDLibrary.
24. “Página Oficial Aeroquad” http://aeroquad.com/content.php?116
25. “ Propelas” http://en.wikipedia.org/wiki/Propeller_(aircraft)
75
26. “Implementación de Filtro Kalman Simplificado mediante Arduino”
http://www.starlino.com/imu_kalman_arduino.html
27. “Filtro Kalman” http://www.cs.unc.edu/~welch/kalman/
28. “Uso de Acelerómetros y Giroscópios para captura de posición”
http://www.starlino.com/quadcopter_acc_gyro.html
29. “Guía de Sistema de Medición Inercial” http://www.starlino.com/imu_guide.html
30. “Ángulo de Ataque de Alas”
http://es.wikipedia.org/wiki/%C3%81ngulo_de_ataque
31. “Fuerza de Sustentación” http://en.wikipedia.org/wiki/Lift_(force)
32. “Perfil Alar” http://en.wikipedia.org/wiki/Planform
33. “Media Móvil” http://en.wikipedia.org/wiki/Moving_average
34. “Uso de Media Móvil en Mercados Financieros”
http://www.investopedia.com/articles/technical/060401.asp#axzz1xSRDjgzN
35. Hoja de Especificaciones Técnicas de ATmega328”
http://www.atmel.com/Images/8271S.pdf
36. “Hoja de Especificaciones Técnicas de Giroscopio IDG-500”.
http://www.sparkfun.com/datasheets/Components/SMD/Datasheet_IDG500.pdf
37. “Hoja de Especificaciones Técnicas de Acelerómetro ADXL-335”.
http://www.sparkfun.com/datasheets/Components/SMD/adxl335.pdf
76
APÉNDICES
En este apartado se debe de incluir información que por su naturaleza no puede ser
incluida en el desarrollo mismo del proyecto, pero que fue utilizada para su elaboración.
Apéndice 1. Sketch de validación del módulo bluetooth.
int incomingByte = 0; // variable para almacenar dato transmitido
int arriba = 12;
int abajo = 11;
int derecha = 10;
int izquierda = 9;
int centro = 8;
void setup() {
Serial.begin(115200); // define el baudrate a 115200 bps
pinMode(arriba, OUTPUT);
pinMode(abajo, OUTPUT);
pinMode(derecha, OUTPUT);
pinMode(izquierda, OUTPUT);
pinMode(centro, OUTPUT);
}
void loop() {
if (Serial.available() > 0) {
// lee el byte transmitido:
incomingByte = Serial.read();
// se imprime lo que se recibió:
Serial.print("Recibí: ");
Serial.println(incomingByte, DEC);
if (incomingByte == 85)//encender el led de arriba
77
{
digitalWrite(arriba, HIGH);
delay(100);
digitalWrite(arriba, LOW);
delay(100);
}
if (incomingByte == 68)//encender el led de abajo
{
digitalWrite(abajo, HIGH);
delay(100);
digitalWrite(abajo, LOW);
delay(100);
}
if (incomingByte == 82)//encender el led de izquierda
{
digitalWrite(derecha, HIGH);
delay(100);
digitalWrite(derecha, LOW);
delay(100);
}
if (incomingByte == 76)//encender el led de izquierda
{
digitalWrite(izquierda, HIGH);
delay(100);
digitalWrite(izquierda, LOW);
delay(100);
}
Serial.flush();
}
}
78
Apéndice 2. Código completo de programación del cuadracóptero
//Sketch para probar controlador PID con el IMU. Se va a utilizar para variar la frecuencia de muestreo del IMU y ver su
impacto en la senal de salida. Se utiliza solamente un PID por eje y las helices pequenas. Da buen resultado con los
parametros actuales. Todavía es inestable.
//Se usa una frecuencia de muestreo de 100 Hz
/*
Hardware:
AccX <---> ANALOG 0
AccY <---> ANALOG 1
AccZ <---> ANALOG 2
GyroX <---> ANALOG 3
GyroY <---> ANALOG 4
PWM Motor 1 <---> DIGITAL 3
PWM Motor 2 <---> DIGITAL 9
PWM Motor 3 <---> DIGITAL 10
PWM Motor 4 <---> DIGITAL 11
Ultrasonic Trigger <---> DIGITAL 12
Ultrasonic Echo <---> DIGITAL 13 */
#include "Ultrasonic.h"
#include "PID_v1.h"
#include <EEPROM.h>
#define INPUT_COUNT 5 //número de entradas analogicas
#define VDD 5000.0f //voltaje de referencia en milivolts
#define PI 3.14159265358979f
//Valores e inicialización de transductor ultrasónico
double altura;
int altura2;
Ultrasonic ultrasonic(12,13); // Trigger en Pin 12 y Echo en Pin 13
double time_1_debug;
double time_now_debug;
//Valores para estructura de IMU
int an[INPUT_COUNT]; //analog inputs
char firstSample; //marks first sample
struct {
char inpInvert[INPUT_COUNT]; // bits 0..5 invert input
int zeroLevel[INPUT_COUNT]; // 0..2 accelerometer zero level (mV) @ 0 G
// 3..5 gyro zero level (mV) @ 0 deg/s
int inpSens[INPUT_COUNT]; // 0..2 acceleromter input sensitivity (mv/g)
// 3..5 gyro input sensitivity (mV/deg/ms)
float wGyro; // gyro weight/smooting factor
79
} config;
// "w" se refiere a cada uno de los ejes, por ejemplo RwAcc[0],RwAcc[1],RwAcc[2] means RxAcc,RyAcc,RzAcc
//Variables tienen que ser globales (se utilizan en getEstimatedInclination)
float RwEst[3]; //Rw estimado de la combinación de RwAcc y RwGyro
unsigned long lastMicros;
//Variables para la obtencion del angulo de inclinacion
unsigned long interval; //intervalo entre muestras
float RwAcc[3]; //proyeccion del vector de gravedad en cada eje, medido por acelerometro
float RwGyro[3]; //Rw obtenido de la ultima lectura del giroscopio
float Awz[2];
//Variables para uso del controlador PID. Todas tienen que tener formato de double por definicion de la funcion.
double vel_adelante, vel_derecha, vel_altura; // Salidas de los controladores PID
double adelante_sp=0.00, derecha_sp=0.00, altura_sp=0.00; //Setpoints iniciales de los controladores PID
double x_angle, y_angle; //Entradas de controladores PID
//Constantes PID
double kp=5, ki=4, kd=3; //Constantes de ganancia de controladores PID inclinacion
double kpa=2, kia=0.0, kda=0.0; //Constantes de ganancia de controladores PID altura
//Controladores PID. Se dedica 1 controlador a cada eje de rotación y otro para la altura. Nomenclatura PID(&Input,
&Output, &Setpoint, Kp, Ki, Kd, Direction)
PID PID_EJE_X(&x_angle, &vel_adelante, &adelante_sp, kp, ki, kd, REVERSE);
PID PID_EJE_Y(&y_angle, &vel_derecha, &derecha_sp,kp,ki,kd, DIRECT);
PID PID_Altura(&altura, &vel_altura, &altura_sp, kpa,kia,kda, DIRECT);
//Valores para control de velocidad de los motores
int vel1, vel2, vel3, vel4; //Velocidades individuales finales de los motores
int motor1=3, motor2=9, motor3=10, motor4=11; //Pines de ubicación de los motores
unsigned long time_now, time_1, time_2;
byte incomingByte1[4]; // Para datos seriales de dispositivo android
int ib;
//Variables de control y valores consigna
int Activado=0; //Para inicializar el dispositivo con los motores apagados. Cero Apagados, uno encendidos
int altitud_maxima= 100; //Altitud maxima deseada en centimetros
int general_vel=155; //Velocidad general de los motores
int vel_limit_PID = 20; //Limite de velocidad para ámbito de salida de controlador PID postura
int vel_limit_PID_altura = 20; //Limite de velocidad para ámbito de salida de controlador PID altitud
double vel_prot = 200; //Limite de protección de velocidad para las pruebas.
80
double sensibilidad_control = 0.4; //Sensibilidad del joystick. No usar valores superiores a 0.5!
double vel_altura_manual = 0;
double vel_altura_manual2 = 0;
void setup() {
static int i;
Serial.begin(115200);
//Se asignan los pines de salida para los PWM de los motores
pinMode(motor1, OUTPUT);
pinMode(motor2, OUTPUT);
pinMode(motor3, OUTPUT);
pinMode(motor4, OUTPUT);
//Se inicializan las salidas de los motores para activar los ESCs
analogWrite(motor1,140);
analogWrite(motor2,140);
analogWrite(motor3,140);
analogWrite(motor4,140);
delay(3000); //Valor de espera para inicializacion de los ESCs
//Ahora se inicializan los parámetros del Acelerometro y giroscopio
for(i=0;i<=2;i++){ // X,Y,Z axis
config.zeroLevel[i] = 1360; // Accelerometer zero level (mV) @ 0 G --> Se obtuvo en experimento
config.inpSens[i] = 300; // Accelerometer Sensisitivity mV/g ---> Se obtuvo del datasheet.
}
for(i=3;i<=4;i++){
config.inpSens[i] = 2000; // Gyro Sensitivity mV/deg/ms
config.zeroLevel[i] = 1350; // Gyro Zero Level (mV) @ 0 deg/s
}
config.inpInvert[0] = 1; //Acc X
config.inpInvert[1] = 1; //Acc Y
config.inpInvert[2] = 1; //Acc Z
//Gyro readings are sometimes inverted according to accelerometer coordonate system
config.inpInvert[3] = -1; //Gyro X
config.inpInvert[4] = -1; //Gyro Y
config.wGyro = 10; //Variable para asignar el peso de las lecturas del giroscopio
firstSample = 1;
//Inicializacion de controlador PID
PID_EJE_X.SetMode(AUTOMATIC); //Activa control PID de correccion eje X.
PID_EJE_X.SetOutputLimits(-vel_limit_PID, vel_limit_PID);
81
PID_EJE_Y.SetMode(AUTOMATIC); //Activa control PID de corrección eje Y.
PID_EJE_Y.SetOutputLimits(-vel_limit_PID, vel_limit_PID);
PID_Altura.SetMode(AUTOMATIC); //Activa control PID de altitud.
PID_Altura.SetOutputLimits(0, vel_limit_PID_altura);
PID_EJE_X.SetSampleTime(20);
PID_EJE_Y.SetSampleTime(20);
PID_Altura.SetSampleTime(100);
}
void loop() {
ComunicacionBluetooth();
time_now=millis();
//Mediciones a 100 Hz
if((time_now-time_1)>10) {
time_1=time_now;
getEstimatedInclination();
if(abs(RwEst[0]-x_angle)<0.10) //Esto es para filtrar picos no deseados en la lectura de la inclinacion
x_angle= RwEst[0];
if(abs(RwEst[1]-y_angle)<0.10) //Esto es para filtrar picos no deseados en la lectura de la inclinacion
y_angle= RwEst[1];
}
//Mediciones a 10 Hz
if((time_now-time_2)>100) {
time_2=time_now;
altura2= altura;
altura= ultrasonic.Ranging(CM);
if(abs(altura-altura2)>3) altura=altura2; //Si la diferencia es mayor a 3 cm se pone de nuevo el valor anterior. Esto para
evitar valores no deseados en forma de picos.
}
PID_EJE_X.Compute();
PID_EJE_Y.Compute();
PID_Altura.Compute();
actualizar_velocidades();
if(vel1>vel_prot||vel2>vel_prot||vel3>vel_prot||vel4>vel_prot){
Activado=0; //apaga motores
82
}
imprimir_valores_debug();
}
void imprimir_valores_debug(){
//Datos para generación de curvas con serial chart software
time_now_debug=millis();
if((time_now_debug-time_1_debug)>50) {
time_1_debug=time_now_debug;
Serial.print(interval); //microseconds since last sample, please note that printing more data will increase interval
Serial.print(",");
Serial.print((vel_adelante*0.01)); //velocidad PID derecha
Serial.print(",");
Serial.print(x_angle); //Inclination Y axis (estimated / filtered)
Serial.println("");
}
/*
//Data para debug de la velocidad de los motores
time_now=millis();
if((time_now-time_1)>200) {
time_1=time_now;
Serial.println("Xaxis ");
Serial.println(x_angle, DEC);
Serial.println("Yaxis ");
Serial.println(y_angle, DEC);
Serial.println(" ");
Serial.println("Vel1");
Serial.println(vel1, DEC);
Serial.println("Vel2");
Serial.println(vel2, DEC);
Serial.println("Vel3");
Serial.println(vel3, DEC);
Serial.println("Vel4");
Serial.println(vel4, DEC);
//Obtencion de datos con el proposito de Debug de Acelerometro
Serial.println("XaxisRaw: ");
Serial.print(analogRead(0));
83
Serial.print(", ");
Serial.print("YaxisRaw: ");
Serial.print(analogRead(1));
Serial.print(", ");
Serial.print("ZaxisRaw: ");
Serial.print(analogRead(2));
Serial.print(", ");
Serial.print("Zero Level: ");
Serial.print(config.zeroLevel[0]);
Serial.println("");
}
*/
}
void actualizar_velocidades(){
vel1= general_vel + vel_adelante + vel_altura;
vel2= general_vel + vel_derecha + vel_altura;
vel3= general_vel - vel_adelante + vel_altura;
vel4= general_vel - vel_derecha + vel_altura;
if(Activado==0){ //Apaga los motores
analogWrite(motor1,140);
analogWrite(motor2,140);
analogWrite(motor3,140);
analogWrite(motor4,140);
}
if(Activado==1){ //Prende los motores
analogWrite(motor1,vel1);
analogWrite(motor2,vel2);
analogWrite(motor3,vel3);
analogWrite(motor4,vel4);
}
}
void ComunicacionBluetooth(){ //Para uso con interfaz Bluebots
static int i;
if (Serial.available() > 0)
{
// Lee los 4 bytes del control remoto:
for(i=0;i<4;i++) {
incomingByte1[i] = Serial.read();
// Serial.print("I received: ");
// Serial.println(incomingByte1[i], DEC);
84
}
//Comando para aumentar el setpoint de la altitud
if (incomingByte1[0]==83) {
altura_sp= (incomingByte1[1]*altitud_maxima)/100;
vel_altura_manual2 =vel_altura_manual;
vel_altura_manual= (incomingByte1[1]*altitud_maxima)/100;
if(vel_altura_manual==255) vel_altura_manual=vel_altura_manual2;
Serial.print("Vel Manual: ");
Serial.println(vel_altura_manual, DEC);
}
//Comando para los setpoints de inclinacion. Recibe los comandos del joystick de la aplicacion.
if (incomingByte1[0]==43 || incomingByte1[0]==45) { //Indica posicion y signo de joystick en eje X
if(incomingByte1[0]==43){
if(incomingByte1[1]<=5) adelante_sp=0;
else adelante_sp= (incomingByte1[1]*sensibilidad_control)/125;
}
if (incomingByte1[0]==45){
if(incomingByte1[1]<=5) adelante_sp=0;
else adelante_sp= (-1*incomingByte1[1]*sensibilidad_control)/125;
}
if (incomingByte1[2]==43){
if(incomingByte1[3]<=5) derecha_sp=0;
else derecha_sp= (incomingByte1[3]*sensibilidad_control)/125;
}
if (incomingByte1[2]==45){
if(incomingByte1[3]<=5) derecha_sp=0;
else derecha_sp= (-1*incomingByte1[3]*sensibilidad_control)/125;
}
85
}
//Comando para parar todos los motores
if (incomingByte1[0]==66) { //66 es el valor del primer byte correspondiente a los botones del 1 al 5
if (incomingByte1[1]==49) Activado=1; //Enciende los motores. Boton 1
if (incomingByte1[1]==50) Activado=0; //Apaga los motores. Boton 2
if (incomingByte1[1]==51) { //Reset de setpoints
adelante_sp=0;
altura_sp=0;
derecha_sp=0;
}
}
}
}
//Tomado de libreria creada Por Starlino para obtención de ángulos de inclinación. void getEstimatedInclination(){
static int i,w;
static float tmpf,tmpf2;
static unsigned long newMicros; //new timestamp
static char signRzGyro;
//get raw adc readings
newMicros = micros(); //save the time when sample is taken
for(i=0;i<INPUT_COUNT;i++) an[i]= analogRead(i);
//compute interval since last sampling time
interval = newMicros - lastMicros; //please note that overflows are ok, since for example 0x0001 - 0x00FE will be
equal to 2
lastMicros = newMicros; //save for next loop, please note interval will be invalid in first sample but we don't use
it
//get accelerometer readings in g, gives us RwAcc vector
for(w=0;w<=2;w++) RwAcc[w] = getInput(w);
//normalize vector (convert to a vector with same direction and with length 1)
normalize3DVector(RwAcc);
if (firstSample){
for(w=0;w<=2;w++) RwEst[w] = RwAcc[w]; //initialize with accelerometer readings
}else{
//evaluate RwGyro vector
if(abs(RwEst[2]) < 0.1){
86
//Rz is too small and because it is used as reference for computing Axz, Ayz it's error fluctuations will amplify leading
to bad results
//in this case skip the gyro data and just use previous estimate
for(w=0;w<=2;w++) RwGyro[w] = RwEst[w];
}else{
//get angles between projection of R on ZX/ZY plane and Z axis, based on last RwEst
for(w=0;w<=1;w++){
tmpf = getInput(3 + w); //get current gyro rate in deg/ms
tmpf *= interval / 1000.0f; //get angle change in deg
Awz[w] = atan2(RwEst[w],RwEst[2]) * 180 / PI; //get angle and convert to degrees
Awz[w] += tmpf; //get updated angle according to gyro movement
}
//estimate sign of RzGyro by looking in what qudrant the angle Axz is,
//RzGyro is pozitive if Axz in range -90 ..90 => cos(Awz) >= 0
signRzGyro = ( cos(Awz[0] * PI / 180) >=0 ) ? 1 : -1;
//reverse calculation of RwGyro from Awz angles
for(w=0;w<=1;w++){
RwGyro[0] = sin(Awz[0] * PI / 180);
RwGyro[0] /= sqrt( 1 + squared(cos(Awz[0] * PI / 180)) * squared(tan(Awz[1] * PI / 180)) );
RwGyro[1] = sin(Awz[1] * PI / 180);
RwGyro[1] /= sqrt( 1 + squared(cos(Awz[1] * PI / 180)) * squared(tan(Awz[0] * PI / 180)) );
}
RwGyro[2] = signRzGyro * sqrt(1 - squared(RwGyro[0]) - squared(RwGyro[1]));
}
//combine Accelerometer and gyro readings
for(w=0;w<=2;w++) RwEst[w] = (RwAcc[w] + config.wGyro* RwGyro[w]) / (1 + config.wGyro);
normalize3DVector(RwEst);
}
firstSample = 0;
}
void normalize3DVector(float* vector){
static float R;
R = sqrt(vector[0]*vector[0] + vector[1]*vector[1] + vector[2]*vector[2]);
vector[0] /= R;
vector[1] /= R;
vector[2] /= R;
}
float squared(float x){
return x*x;
}
//For accelerometer it will return g (acceleration) , applies when xyz = 0..2
//For gyro it will return deg/ms (rate of rotation) , applies when xyz = 3..5
float getInput(char i){
static float tmpf; //temporary variable
tmpf = an[i] * VDD / 1023.0f; //voltage (mV)
tmpf -= config.zeroLevel[i]; //voltage relative to zero level (mV)
tmpf /= config.inpSens[i]; //input sensitivity in mV/G(acc) or mV/deg/ms(gyro)
tmpf *= config.inpInvert[i]; //invert axis value according to configuration
return tmpf;
}