Universidad de Costa Rica

Facultad de Ingeniera
Escuela de Ingeniera Elctrica

IE 0502 Proyecto Elctrico

Diseo e implementacin de un cuadracptero con

sistema de control automtico de estabilidad y
comunicacin inalmbrica de datos utilizando
plataformas de hardware y software libre

Por:
Daniel Barrera Nez

Ciudad Universitaria Rodrigo Facio

Julio del 2012

Diseo e implementacin de un cuadracptero con

sistema de control automtico de estabilidad y
comunicacin inalmbrica de datos utilizando
plataformas de hardware y software libre
Por:
Daniel Barrera Nez

Sometido a la Escuela de Ingeniera Elctrica

de la Facultad de Ingeniera
de la Universidad de Costa Rica
como requisito parcial para optar por el grado de:

BACHILLER EN INGENIERA ELCTRICA

Aprobado por el Tribunal:

_________________________________
Ing. Fabin Abarca Caldern
Profesor Gua

_________________________________
Ing. Fabin Garita Gonzales
Ingeniero lector

_________________________________
Dr. Lucky Lochi Yu Lo
Profesor lector
ii

DEDICATORIA

A mi familia y seres queridos, por el constante apoyo y ayuda brindada durante los aos de
mi carrera universitaria, sin la cual no hubiese podido lograr culminar esta etapa de manera
exitosa.

iii

RECONOCIMIENTOS

A mi profesor gua, el Ing. Fabin Abarca, por su apoyo y consejo a lo largo del
desarrollo del proyecto.

A mis profesores lectores, el Dr Lochi Yu, y el Ing. Fabin Garita.

A mi padre, Edmundo Barrera, por el gran inters y ayuda que fueron

fundamentales en la construccin del primer prototipo del cuadracptero.

iv

NDICE GENERAL
1.

CAPTULO 1: INTRODUCCIN ....................................................... 1

1.1

Objetivos ................................................................................................. 3

1.1.1 Objetivo general .......................................................................................................... 3

1.1.2 Objetivos especficos .................................................................................................. 3

1.2

Metodologa ............................................................................................ 4

CAPTULO II: DESARROLLO TERICO ...................................... 5

2.1

Aerodinmica ......................................................................................... 5

2.2

Vehculos Areos No Tripulados .......................................................... 7

2.2.1 Introduccin ................................................................................................................ 7

2.2.2 El Cuadracptero ........................................................................................................ 8

2.3

Plataforma de Prototipos Electrnicos: Arduino ............................. 16

2.3.1 Breve Introduccin a Arduino .................................................................................. 16

2.3.2 Hardware del Arduino Uno....................................................................................... 17
2.3.3 Programacin ............................................................................................................ 21

2.4

Sensores ................................................................................................. 22

2.4.1 Giroscopio ................................................................................................................. 24

v

2.4.2 Acelermetro ............................................................................................................ 27

2.4.3 Sensor Ultrasnico de Proximidad ........................................................................... 29

2.5

Actuadores ............................................................................................ 32

2.5.1 Motores DC sin escobillas ........................................................................................ 32

2.5.2 Controles electrnicos de Velocidad (ESCs) .......................................................... 35

2.6

Comunicaciones Inalmbricas ............................................................ 37

2.6.1 Comunicacin Serial ................................................................................................. 37

2.6.2 Protocolo Bluetooth .................................................................................................. 38

2.7

Procesamiento de Seales.................................................................... 40

2.7.1 Introduccin .............................................................................................................. 40

2.7.2 Manipulacin de los datos de los sensores (IMU) .................................................... 41
2.7.3 Media Mvil ............................................................................................................. 41

2.8

Control Automtico ............................................................................. 44

2.8.1 Controlador PID ........................................................................................................ 44

2.9

Fuente de Poder ................................................................................... 46

2.9.1 Batera de Polmero de Ion de Litio (LiPo) .............................................................. 46

3.

CAPTULO III: DESARROLLO E IMPLEMENTACIN DEL

UAV 47
3.1

Diagramas de Bloques del Cuadracptero ........................................ 47

vi

3.2

Experimentos ........................................................................................ 52

3.2.1 Validacin del mdulo Bluetooth ............................................................................. 52

3.2.2 Validacin de los controles electrnicos de velocidad (ESC) .................................. 54
3.2.3 Validacin de la Unidad de Medicin Inercial (IMU) ............................................. 55
3.2.4 Validacin del Transductor Ultrasnico de Proximidad........................................... 59
3.2.5 Validacin del sistema de control automtico. ......................................................... 60

3.3

Lista de Componentes Utilizados ....................................................... 64

4.

CAPTULO 4: CONCLUSIONES Y RECOMENDACIONES ...... 67

4.1

Conclusiones ......................................................................................... 67

4.2

Recomendaciones ................................................................................. 70

4.3

Trabajo Futuro .................................................................................... 71

BIBLIOGRAFA ............................................................................................ 72
APNDICES ................................................................................................... 76
Apndice 1. Sketch de validacin del mdulo bluetooth. ........................... 76
Apndice 2. Cdigo completo de programacin del cuadracptero ......... 78

vii

NDICE DE FIGURAS
Figura 1. ngulo de ataque de un ala [30] .............................................................................. 6
Figura 2. Modelo tridimensional de un cuadracptero ........................................................... 8
Figura 3. Diagrama rotacin de cuadracptero alrededor del eje Z [9]. ............................... 12
Figura 4. Diagrama rotacin de cuadracptero alrededor del eje X [9]................................ 13
Figura 5. Diagrama rotacin de cuadracptero alrededor del eje Y [9]................................ 13
Figura 6. Diagrama traslacin de cuadracptero 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 programacin 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 acelermetro ADXL335. Hoja de Especificaciones .... 28
Figura 11. Diagrama de Operacin de Sensor Ultrasnico de Proximidad. Fuente: Google
Images. .................................................................................................................................. 30
Figura 12. Sensor Ultrasnico de Proximidad (HC-SR04). Fuente: Manual de Fabricante. 30
Figura 13. Representacin grfica de funcionamiento de sensor ultrasnico de proximidad.
.............................................................................................................................................. 32
Figura 14. Representacin de Motor DC sin Escobillas ....................................................... 35
Figura 15. Principio de funcionamiento de motores sin escobillas [6] ................................. 37
Figura 16. Ilustracin Media Mvil ...................................................................................... 42
Figura 17. Grfico Ilustrativo de Media Mvil..................................................................... 43
viii

Figura 18. Diagrama de bloques de controlador PID Paralelo ............................................. 46

Figura 19. Diagrama de Bloques de Controlador PID .......................................................... 47
Figura 20. Cuadracptero en configuracin "x" ................................................................... 48
Figura 21. Cuadracptero en configuracin "+". .................................................................. 48
Figura 22. Diagrama de bloques funcionales para cada motor ............................................. 50
Figura 23. Lazo de ejecucin y realimentacin del cuadracptero. ...................................... 50
Figura 24. Diagrama de mtodo de validacin de mdulo bluetooth en conjunto con
Arduino. ................................................................................................................................ 53
Figura 25. Captura de Monitor Serial de Aplicacin de Programacin de Arduino. ........... 53
Figura 26. Comparacin entre seal original y seal filtrada de uno de los acelermetros. 57
Figura 27. Prueba para observar el desfase entre la seal filtrada y la original, con los
motores apagados. ................................................................................................................. 57
Figura 28. Prueba realizada para observar la inmunidad del sistema ante seales no
deseadas. ............................................................................................................................... 58
Figura 29. Experimento para la obtencin de los parmetro de PID de inclinacin Eje X .. 61
Figura 30. Modelo tridimensional del primer prototipo del cuadracptero. ......................... 65
Figura 31. Modelo Real del Primer Prototipo del Cuadracptero. ....................................... 65
Figura 32. Modelo Real del Primer Prototipo del Cuadracptero. ....................................... 66
Figura 33. Parte inferior del primer prototipo del cuadracptero. ........................................ 66

ix

NDICE DE TABLAS

Tabla 1. Cronograma tentativo del desarrollo del dispositivo. ............................................... 4

Tabla 2. Especificaciones Tcnicas del Arduino Uno .......................................................... 18
Tabla 3. Salidas analgicas del IDG-500.............................................................................. 26
Tabla 4. Especificaciones de Acelermetro ADXL335........................................................ 29
Tabla 5. Tabla de asignacin de pines de Arduino ............................................................... 51
Tabla 6. Lista de Componentes y Costos del Cuadracptero ............................................... 64

NOMENCLATURA

ESC: Controlador Electrnico de Velocidad (Del ingls Electronic Speed

Controller)

IEEE: Instituto de Ingenieros Elctricos y Electrnicos.

IMU: Unidad de Medicin Inercial.

MEMS: Sistemas micro-electro-mecnicos (Micro Electro-Mechanical Systems.)

UAV: Vehculo Areo No Tripulado (Del ingls Unmanned Air Vehicle).

PWM: Modulacin por ancho de pulso (del ingls Pulse Width Modulation).

SPI: Interfaz Serial de Perifricos (Del ingls Serial Peripheral Interface).

xi

RESUMEN
El presente trabajo constituye un proyecto de investigacin y desarrollo de un vehculo
areo no tripulado con control de estabilidad autnomo, especficamente un cuadracptero.
La finalidad del proyecto fue tener la oportunidad de poder ahondar ms en determinados
conocimientos obtenidos a los largo de la carrera de ingeniera elctrica, y ponerlos en
prctica mediante el desarrollo de un proyecto multidisciplinario. Para lograr esto, se
realiz una investigacin previa de los componentes requeridos y conceptos necesarios
para el proceso de desarrollo del cuadracptero. Se logr realizar un primer prototipo de un
cuadracptero con control automtico de estabilidad y altitud con comunicacin
inalmbrica va bluetooth con un telfono inteligente que serva de control remoto para el
usuario. Todos los bloques funcionales del cuadracptero fueron validados previamente a
ser integrados al sistema. Este desarrollo se realiz utilizando hardware y software libre, y
componentes de fcil acceso al pblico. Se logr obtener un primer prototipo capaz de
volar pequeas distancias, sin embargo, a la hora de concluir el proyecto se determin que
requera de algoritmos ms 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 vehculo en ambientes con obstculos.

xii

1. CAPTULO 1: Introduccin
La conceptualizacin y creacin de mquinas que tengan la capacidad de operar de manera
autnoma data desde la era clsica. Sin embargo, no fue sino hasta el siglo XIX, junto con
el surgimiento y auge de la revolucin industrial, que se empezaron a vislumbrar las
primeras mquinas relevantes de automatizacin 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 electrnica.

El rea de la aeronutica actualmente constituye una de las ramas con ms presencia dentro
del campo de la robtica, 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 investigacin cientfica. Esta constituye el rea que se pretende
explorar con el avance del proyecto.

Para el desarrollo del presente proyecto, se pretende elaborar un vehculo areo de vuelo
autnomo (UAV), y para ello se busc una plataforma verstil y de construccin sencilla.
Tras realizar una investigacin previa, se eligi tomar como modelo base un quad-rotor, o
tambin conocido como cuadracptero, el cual consiste, como se pude intuir de su nombre,
en una plataforma con cuatro rotores ubicados en los extremos de los ejes de simetra de su
chasis. Esta plataforma presenta varias ventajas en cuanto a su maniobrabilidad y la poca
1

cantidad de componentes mecnicos requeridos para su elaboracin (lo cual muchas veces
representa el verdadero reto de los proyectos de esta ndole).

Para su construccin se pretende hacer uso de hardware open source (Arduino) para el
sistema de control principal y de comunicaciones del dispositivo, y de componentes
electrnicos y mecnicos de fcil acceso al consumidor para el resto de la plataforma.

Dentro de las principales razones por las cuales se toma la decisin de abordar un proyecto
de este ndole, es por la gran cantidad de tpicos que se encuentran involucrados en el
desarrollo de un prototipo de esta categora, y dado que este proyecto se encuentra ligado a
la finalizacin del grado acadmico, permite la oportunidad de aplicar y demostrar una gran
cantidad de conocimientos que se han adquirido a travs de la carrera. Entre las reas de
estudio que se pretenden aplicar se pueden mencionar: microcontroladores, programacin,
sistemas de control automtico, sistemas de comunicacin, anlisis de sistemas, mquinas
elctricas, electrnica analgica, circuitos digitales y hasta un poco de mecnica!

1.1

Objetivos

1.1.1 Objetivo general

Disear e implementar un vehculo areo no tripulado (Cuadracptero) con sistemas de
control automtico de estabilidad y comunicacin inalmbrica mediante el uso de
plataformas de hardware y software libres.

1.1.2 Objetivos especficos

1.1.2.1 Implementar un sistema eficiente de control de potencia para el manejo de motores
DC para el cuadracptero.
1.1.2.2 Disear e implementar un mdulo de sensado de posicin y movimiento, haciendo
uso de sensores de aceleracin y giroscopios.
1.1.2.3 Implementar un sistema de comunicacin inalmbrica mediante el uso de
mdulo Bluetooth.

un

1.1.2.4 Implementar un mdulo de procesamiento y control principal para unificar e

interlazar los mdulos de sensado, comunicacin y potencia utilizando un
microcontrolador ATmega 328 en conjunto con una plataforma Arduino.

1.2

Metodologa

La metodologa que se implement para el desarrollo del proyecto fue una extensa
investigacin de los componentes necesarios (tanto fsicos como tericos) para realizar el
desarrollo del sistema de manera exitosa. Adems, cada uno de los sistemas estudiados,
fueron implementados y validados de manera individual, para luego ser unificados en el
sistema final (cuadracptero). Esto requiri de una extensa investigacin y experimentacin
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 continuacin se muestra el cronograma tentativo para el desarrollo
que se llev a cabo:
Tabla 1. Cronograma tentativo del desarrollo del dispositivo.

CAPTULO II: Desarrollo Terico

En el desarrollo del captulo II, se sintetizarn los temas de mayor importancia para el
desarrollo y entendimiento de los conceptos necesarios para la implementacin del
prototipo del cuadracptero. Estos temas se tratarn de exponer de una manera sencilla para
que sea apto para lectores con o sin mayor conocimiento tcnico del tema.

2.1

Aerodinmica

Con la finalidad de ir desarrollando los temas en un orden estructurado, se iniciar con una
breve seccin de aerodinmica, la cual a grandes rasgos, constituye la base que permite
entender el comportamiento y la forma de operar del cuadracptero. En este apartado se
explicarn brevemente los conceptos aerodinmicos de mayor relevancia para crear la base
terica 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
aerodinmica es el de la fuerza de sustentacin. La fuerza de sustentacin 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 sustentacin es solamente la
componente vertical al vector de direccin del fluido. Dentro de los otros componentes de
esta fuerza creada por el paso del fluido, tambin se encuentra la fuerza de arrastre, la cual
5

tambin juega un papel muy importante en la dinmica de operacin de los vehculos

areos.

En el caso de los vehculos que poseen aspas adheridas a rotores, sta fuerza es la
encargada de crear un par rotacional en el vehculo cuya direccin va en contra del sentido
de giro de las aspas. Por esta razn es que los helicpteros convencionales poseen rotores
de cola que compensan este par rotacional y evita que giren sin control alrededor del eje de
rotacin del rotor principal.

El ngulo de ataque de un ala se encuentra definido como el ngulo que forma la cuerda
geomtrica de un perfil alar con la direccin del aire que incide sobre el ala. Este parmetro
juega un papel muy importante en la generacin de la fuerza de sustentacin y la fuerza de
arrastre de una hlice. Para un mejor entendimiento de este concepto se puede observar la
figura a continuacin.

Figura 1. ngulo de ataque de un ala [30]

La correcta eleccin del ngulo de ataque de las hlices es de suma importancia para
obtener los parmetros de fuerza de sustentacin y de arrastre que son requeridos.
6

2.2

Vehculos Areos No Tripulados

2.2.1 Introduccin
Los vehculos areos no tripulados (popularmente conocidos como UAVs por sus siglas en
ingls), son vehculos areos que estn diseados para poder volar sin necesidad de un
piloto humano a bordo. El vuelo de estas mquinas, 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 tambin, stas mquinas pueden
ser controladas de manera autnoma por un controlador o computador que se encuentre
programado para tal fin.

Dentro de la gama de vehculos areos no tripulados, se puede encontrar una gran escala de
tamaos , configuraciones y formas (vehculos de ala fija, vehculos multirotores, etc), los
cuales inicialmente fueron surgiendo como plataformas controladas remotamente por un
piloto, pero que en los ltimos aos, gracias a los grandes avances de la tecnologa en el
campo de la computacin, la electrnica y comunicaciones, han migrando poco a poco al
sector de vehculos autnomos.

Los cuadracpteros entran dentro de esta categora de vehculos areos no tripulados, y

dependiendo de las caractersticas de cada uno, stos se pueden construir como vehculos
totalmente autnomos, o como vehculos controlados remotamente por un ser humano.
7

2.2.2 El Cuadracptero
El

cuadracptero (tambin conocido como quadrotor o quadcopter)

representa una

plataforma muy verstil y de creciente popularidad que se encuentra dentro de la categora

de vehculos areos no tripulados (UAVs). Este tipo de vehculo areo multirotor se
encuentra formado (como su nombre lo dice) por cuatro rotores, los cuales normalmente se
encuentran dispuestos simtricamente a en los extremos del bastidor del vehculo (el cual es
en forma de X). A diferencia de los helicpteros convencionales, la mayora de los
cuadracpteros poseen hlices con aspas fijas, en contraste con los helicpteros
convencionales, cuyas aspas tienen la capacidad de cambiar su ngulo de ataque. Un
modelo tridimensional general de un cuadracptero se puede observar en la figura a
continuacin:

Figura 2. Modelo tridimensional de un cuadracptero

Dinmica de Movimiento

La dinmica del movimiento de los cuadracpteros se puede resumir en 3 tipos distintos de

movimientos bsicos, los cuales se pueden ejecutar por s solos o en conjunto para lograr el
tipo de movimiento especfico que el controlador del vehculo desee. Estos tipos de
movimientos son los siguientes:

Rotacin alrededor del Eje Z

Para la ilustracin de este movimiento, primero se procede con la definicin del eje z del
vehculo. Este eje imaginario, se encuentra en el centro del bastidor de la mquina, (el
centro de la equis formada por la estructura del vehculo), y es perpendicular al plano
formado por la misma estructura del vehculo. Como se mencion anteriormente, la
rotacin de las aspas crean un torque que tratan de girar el vehculo alrededor del eje de
giro de las aspas. En los helicpteros 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 cuadracptero 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
9

sabe, los cuadracpteros poseen un nmero 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 rotacin de los distintos motores se puede llegar a un punto de
operacin donde el par rotacional neto del vehculo sea igual a cero.

Segn lo expuesto en [5], se puede concluir que tanto la fuerza de sustentacin como la
fuerza de arrastre son funciones que dependen del cuadrado de la velocidad angular. A
continuacin se muestran las ecuaciones de dichas fuerzas:

Ecuacin 2.1
Ecuacin 2.2

Donde

son parmetros propios del fluido (aire) y del tipo de hlices y

corresponde a la velocidad angular de rotacin de los motores. Por lo tanto, si se mantienen

constantes el tipo de fluido donde se est volando, y el tipo de hlices que se estn
utilizando, la nica variable que queda es la velocidad angular. Por esta razn, el par
rotacional creado por la propela de cada motor puede ser conocido y modificado de acuerdo
a las necesidades de vuelo del vehculo.

10

Ahora, como lo que se trata de controlar es la rotacin alrededor del eje z del
cuadracptero, 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 cuadracptero 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 hlices sean simtricas y con las mismas
caractersticas constructivas).

Sean

las velocidades angulares de los motores girando a favor de las manecillas

del reloj 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 vehculo en donde no hay

rotacin alrededor del Eje Z.

Si

, se obtiene un comportamiento del vehculo en donde hay una

rotacin alrededor del Eje Z y en contra de las manecillas del reloj.

Si

, se obtiene un comportamiento del vehculo en donde hay una

rotacin alrededor del Eje Z y a favor de las manecillas del reloj.

A continuacin se encuentra un diagrama de la configuracin de los motores en el
cuadracptero para visualizar de una mejor manera lo explicado anteriormente.
11

Figura 3. Diagrama rotacin de cuadracptero alrededor del eje Z [9].

Rotacin alrededor del Eje X o el Eje Y

La dinmica de ambas rotaciones es la misma. Lo que cambia es alrededor de cual eje se
quiera girar. Para realizar esta rotacin lo que se realiza es disminuir/aumentar la velocidad
de rotacin de uno de los motores y realizar la accin 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
cuadracptero rote inesperadamente alrededor del su eje Z).

A continuacin se puede observar la figura que ilustra el caso anterior para que se visualice
de una manera ms sencilla.
12

Figura 4. Diagrama rotacin de cuadracptero alrededor del eje X [9].

De este modo, si lo que se quiere modificar es el ngulo de rotacin

permanecern constantes, mientras que

, entonces

cambiarn 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 rotacin a favor de las manecillas del reloj constante.

La dinmica resulta la misma para el caso anlogo de la rotacin alrededor del eje Y.

Figura 5. Diagrama rotacin de cuadracptero alrededor del eje Y [9].

13

Una consecuencia de provocar una rotacin alrededor de alguno de los ejes X o Y, es que
se va a obtener un desplazamiento traslacional en caso de que el cuadracptero ya se
encuentre en vuelo, el cual va a ser correspondiente al tipo de inclinacin que tenga el
cuadracptero. Por ejemplo, si el cuadracptero rota alrededor del eje X, se va a obtener
una traslacin a lo largo del eje Y, y si el cuadracptero rota alrededor del eje Y, se va a
obtener una traslacin a lo largo del eje X. Cabe resaltar que tambin se puede obtener una
rotacin simultnea en ambos ejes, lo cual va a provocar una traslacin anloga a esa
rotacin.

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 cuadracptero 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 cuadracptero. 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 anloga para disminuirla.

14

A continuacin se encuentra el esquema que ilustra el proceso descrito:

Figura 6. Diagrama traslacin de cuadracptero a lo largo del eje Z [9].

Resulta fcil llegar a la conclusin de que con una determinada combinacin de cualquiera
de los tres tipos de movimientos descritos anteriormente se puede obtener un control total
sobre la trayectoria y postura del cuadracptero en un determinado momento. El control de
movimiento del cuadracptero se basa en un principio de superposicin, en donde la
velocidad final de cada uno de los motores va a ser la suma dinmica de cada uno de las
velocidades necesarias para lograr cada uno de los tres tipos de movimiento que se estn
ejecutando en determinado momento.

15

2.3

Plataforma de Prototipos Electrnicos: Arduino

Arduino ha surgido en los ltimos aos como una plataforma de creacin de prototipos
electrnicos muy revolucionaria, la cual ha desarrollado de gran manera el campo del
movimiento de Hgalo 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 Introduccin 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 programacin propio de Arduino (basado en el lenguaje de programacin
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 caractersticas propias. Esto resulta muy prctico para
seleccionar una que se adapte lo mejor posible a las caractersticas de cada proyecto.
16

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 caractersticas 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 (seales de modulacin por ancho de pulso). Adems, posee 6
entradas analgicas, un oscilador de 16 MHz, un conector USB para comunicacin con una
PC, un conector de alimentacin y un botn de reset.

Figura 7. Parte superior de Arduino Uno [http://arduino.cc/en/Main/ArduinoBoardUno]

17

A continuacin se presenta una tabla resumen con las principales caractersticas del
Arduino Uno:
Tabla 2. Especificaciones Tcnicas del Arduino Uno

Adicional a la tabla anterior, en la seccin de la bibliografa se puede encontrar un link a la

hoja de especificaciones del microcontrolador ATmega328 en caso de que se quiera
profundizar ms en las especificaciones tcnicas.

Fuente de Poder
El Arduino Uno puede ser alimentado desde su conector USB o mediante una fuente de
poder externa, como por ejemplo una batera. La fuente de poder es seleccionada de manera
18

automtica 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 tensin de alimentacin de la tarjeta puede variar entre 6 y 20 volts, sin embargo cuando
la tensin 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 travs del pin de 5V). Adems, cuando se usa
una tensin de alimentacin superior a 12 volts existe la posibilidad de que el regulador se
sobrecargue y se dae. Por las razones anteriores, la tensin 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 tensin 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 tensin 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 tensin de alimentacin especial. La corriente
mxima que se puede entregar por medio de este canal es de 50 mA.

GND: Es el pin de tierra.

19

Memoria
El ATmega 328 posee 32 kB de memoria, de los cuales 0.5 kB son utilizados por el
bootloader de la tarjeta. Adems posee 2 kB de SRAM y 1 kB de EEPROM, los cuales
pueden ser accesados o modificados mediante una de las libreras 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 mximo de 40 mA. Adems,
ciertos pines tienen funciones especializadas, las cuales se mencionan a continuacin:

Pines Seriales: 0 (RX) y 1 (TX). Son utilizados para recibir y transmitir datos
seriales TTL. Estos pines estn conectados a los pines correspondientes del chip
serial USB-TTL ATmega8U2.

Pines de Interrupcin: 2 y 3. Estos pines se pueden configurar para provocar una

interrupcin en la ejecucin del programa.

Pines de PWM: 3,5,6,9,10 y 11. Proveen una salida PWM de 8 bits de resolucin
mediante la funcin analogWrite().

Pines de comunicacin SPI: 10 (SS), 11 (MOSI), 12 (MISO), 13 (SCK). Estos pines

son utilizados para realizar conexin de dispositivos mediante protocolo SPI
utilizando la librera destinada para tal fin.

20

El Arduino Uno tambin posee 6 entradas analgicas con un convertidor analgico-digital

de 10 bits de resolucin. Por default, esos convertidores miden desde 0 hasta 5 volts, pero
este valor superior se puede modificar mediante el pin AREF y la funcin
analogReference().

2.3.3 Programacin
La programacin del Arduino se realiza mediante el entorno de programacin propio de
Arduino. Los programas que se realizan para Arduino son denominados Sketch. Una vez
que se escribe un programa, el entorno de programacin 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 programacin se puede referir a la pgina
web oficial: http://arduino.cc/en/Reference/HomePage. Adems, Arduino posee una gran
cantidad de libreras realizadas por sus usuarios, las cuales pueden ser descargadas de
manera gratuita para su respectivo uso. A continuacin se muestra una imagen del entorno
de programacin:

21

Figura 8. Entorno de programacin de Arduino (programa de ejemplo).

2.4

Sensores

Las palabras sensor y transductor son dos trminos muy utilizados para referirse a
dispositivos de sensado de alguna variable fsica, sin embargo no significan lo mismo. El
trmino transductor se utiliza cuando se quiere considerar un sistema completo de sensado,
donde se lleva a cabo un cambio entre tipos de energa. Un ejemplo de estos sistemas son
los acelermetros (transductores de aceleracin), donde un cambio de estado de un sistema
mecnico se manifiesta como una salida de tensin elctrica en el dispositivo. Por otro lado
22

el trmino sensor es utilizado principalmente para referirse a dispositivos que responden

ante algn estmulo del ambiente en el que se encuentran cambiando algn parmetro
intrnseco del dispositivo, pero sin realizar ninguna conversin de energa. Un ejemplo de
sensores son los termistores, los cuales son dispositivos que simplemente cambian el valor
de su resistividad en funcin de la temperatura del ambiente donde se encuentren. En el
presente proyecto, se harn uso de transductores para la medicin de los parmetros de
control del quadcopter.

Dos caractersticas de suma importancia para caracterizar los transductores son la

responsividad y la detectabilidad. El primero de los trminos se refiere a la respuesta
del transductor ante un cambio de la variable medida, y el segundo se refiere al valor ms
pequeo de la variable medida que el transductor puede detectar. Estos parmetros resultan
ms prcticos a la hora de caracterizar un sistema que el trmino sensibilidad (el cual
resulta un poco ambiguo).

Dentro de la familia de los transductores se encuentran los sistemas micro

electromecnicos, o MEMS por sus siglas en ingls, los cuales son la base de los
transductores que son utilizados para la adquisicin de la postura del cuadracptero.

23

2.4.1 Giroscopio
2.4.1.1 Descripcin Generalizada
Los giroscopios son dispositivos utilizados para medir o mantener una orientacin
especfica. Antiguamente solo se contaba con los giroscopios mecnicos, que consistan en
discos giratorios suspendidos sobre un eje que poda rotar en varias direcciones. La
dinmica consista en que el disco rotatorio permaneca en la misma posicin
indiferentemente de cmo rotara la plataforma sobre la cual estuviera montado.

Actualmente se cuenta con otro tipo de giroscopios, los cuales son basados en sistemas
micro-electromecnicos, y han aumentado su popularidad en los dispositivos electrnicos
debido a su reducido tamao, bajo precio y buena precisin. Una de las mayores
aplicaciones de los giroscopios es para el desarrollo de unidades de medicin inercial
(IMU) de vehculos o dispositivos que requieran de este tipo de mediciones, como es el
caso de los vehculos areos no tripulados (UAV) para sus sistemas de estabilizacin.

2.4.1.2 Dispositivo Utilizado (IDG-500)

El IDG-500 es un giroscopio de dos ejes que consiste en dos girscopos MEMS vibratorios
independientes. Uno de ellos detecta la velocidad de rotacin alrededor del eje X, y el otro
detecta la velocidad de rotacin alrededor del eje Y. Ambas estructuras se encuentran
24

selladas hermticamente al nivel de la oblea de silicio. Las masas vibratorias de ambos

girscopos, se hacen oscilar electrostticamente a una frecuencia de resonancia mediante un
control automtico muy preciso que controla la oscilacin de las masas. Cuando el sensor
es rotado alrededor del eje X o el eje Y, el efecto Coriolis causa una vibracin que puede
ser detectada por un sensor capacitivo. La seal resultante es amplificada, demodulada, y
filtrada para dar paso a una seal analgica que es proporcional a la tasa de cambio de la
posicin angular en un determinado eje.

A continuacin se puede observar un diagrama de Bode del filtro paso bajo que posee el
giroscopio para acondicionar la seal de salida:

Figura 9. Respuesta de Ganancia y Fase del Filtro del IDG-500

25

El IDG-500 posee dos salidas de velocidad para el eje X, y dos para el eje Y. A
continuacin se muestra una tabla con sus principales parmetros:

Tabla 3. Salidas analgicas del IDG-500

Para ms especificaciones tcnicas del dispositivo, se puede recurrir a la hoja de

especificaciones incluida en la seccin de la bibliografa [36].

26

2.4.2 Acelermetro
2.4.2.1 Dispositivo Utilizado (ADXL335)

El ADXL335 es un sistema completo de medicin de aceleracin de 3 ejes del proveedor

Analog Devices. Este dispositivo tiene un rango de medicin de 3g. Contiene un sensor
micro-electromecnico y un circuito de acondicionamiento de seal para implementar una
arquitectura de medicin de aceleracin de lazo abierto. La salida del transductor, son
tensiones elctricas analgicas directamente proporcionales a la aceleracin en cada eje. El
sistema puede medir aceleracin esttica (como la gravedad), o aceleracin dinmica (como
sistemas en movimiento a velocidades no constantes).

El sensor es un sistema micro-electromcanico 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 aceleracin. La deformacin de
la estructura cuando es sometida a fuerzas de aceleracin 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 medicin de la aceleracin de los
tres ejes ortogonales.

27

A continuacin se puede observar un diagrama de bloques funcionales del sistema de

medicin:

Figura 10. Diagrama de bloques de acelermetro ADXL335. Hoja de Especificaciones

Seguidamente, tambin se muestra una tabla con los parmetros de mayor importancia para
el desarrollo del proyecto, sin embargo tambin se puede recurrir a la hoja de
especificaciones completa en la seccin de bibliografa [37] en caso de que sea necesario.

28

Tabla 4. Especificaciones de Acelermetro ADXL335

2.4.3 Sensor Ultrasnico de Proximidad

2.4.3.1 Descripcin Generalizada

Los sensores ultrasnicos 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 evala la distancia de un objetivo al interpretar los ecos de ondas sonoras o de
radio. Los sensores ultrasnicos generan ondas de alta frecuencia, detectan el eco, y
calculan el intervalo de tiempo entre el momento en que se envi la seal, y el momento en
el que se recibi el eco, para determinar la distancia del objeto que se est avaluando. Esta
tecnologa se encuentra un poco limitada por la forma de las superficies o la consistencia

29

del material del objeto que se est midiendo, dado que determinados materiales pueden dar
lecturas errneas en el transductor.

Figura 11. Diagrama de Operacin de Sensor Ultrasnico de Proximidad. Fuente: Google Images.

2.4.3.2 Dispositivo Utilizado (HC-SR04)

El sensor ultrasnico de proximidad que se eligi para la realizacin del proyecto es el HCSR04, de la compaa ITead Studios. A continuacin se puede apreciar una imagen del
sensor:

Figura 12. Sensor Ultrasnico de Proximidad (HC-SR04). Fuente: Manual de Fabricante.

30

2.4.3.3 Teora de Operacin del Sensor Ultrasnico

La manera en la que este transductor detecta la proximidad de un objeto es la siguiente.

Cuando se introduce un pulso mayor de 10s en la terminal de trigger del dispositivo, el
transductor enva una seal ultrasnica 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 seal
reflejada se le denomina eco. Desde el momento en el que se emite la seal de 40 kHz hasta
que se recibe su eco, el transductor mantendr un 1 lgico en su terminal denominada
ECO. Cuando el transductor recibe de vuelta esta seal la convierte en una seal
elctrica, y el siguiente pulso no podr ser transmitido hasta que el eco de la seal anterior
haya desaparecido. Este tiempo que transcurre entre el momento en que se enva el primer
pulso ultrasnico hasta que se puede enviar el prximo se denomina periodo de ciclo (cycle
period). El periodo de ciclo mnimo recomendado es de 50 ms. La distancia medida por el
transductor es proporcional al tiempo que dure la seal de la terminal de ECO en alto (que
en este caso es una tensin de 5 volts). Si no se detecta ningn obstculo frente al sensor
ultrasnico, este dar una seal de 38 ms en alto en su terminal de ECO.

La expresin que se utiliza para calcular la distancia en centmetros del objeto (o superficie,
una vez dada la duracin del pulso de ECO es la siguiente:

31

Para un mejor entendimiento, a continuacin tambin se brinda una explicacin grfica de

su operacin:

Figura 13. Representacin grfica de funcionamiento de sensor ultrasnico de proximidad.

2.5

Actuadores

2.5.1 Motores DC sin escobillas

Los motores DC sin escobillas (tambin conocidos como motores DC brushless), son
motores sncronos que son alimentados por una fuente elctrica DC. Sin embargo, a
diferencia de los motores DC con escobillas, los motores sin escobillas requieren de un
conmutador electrnico y sensores adicionales para generar una seal de control que
energice cada una de las bobinas del motor en forma secuencial para dar paso a la rotacin
32

del motor. La gran mayora de estos motores se encuentran dentro de la categora de

motores sncronos de imanes permanentes, pero tambin existen en menor proporcin,
motores DC sin escobillas de reluctancia o de induccin.

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. Adems los motores sin escobillas son
ms livianos comparados contra un motor con escobillas de su misma potencia, y tambin
presentan eficiencias y pares superiores por cada watt consumido. Una caracterstica 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 ms robustos, confiables y eficientes.

Una de las desventajas de los motores DC sin escobillas, es que requieren de un sistema
electrnico por aparte para poder operar, normalmente denominado como controlador
electrnico de velocidad (ESC por sus siglas en ingls), y del cual se hablar ms adelante.
El motor DC con escobillas puede operar sin problemas conectado nicamente a una fuente
DC o tambin con un conmutador electrnico o restato para regular su velocidad.

Un parmetro 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
33

alimentacin. Por ejemplo, un motor sin escobillas de 1000 Kv conectado a una fuente de 2
volts, puede girar a 2000 rpm.

Tpicamente los motores DC sin escobillas poseen imanes permanentes adheridos a una
armadura fija, eliminando as los problemas generados para suplir corriente a una armadura
mvil. El controlador electrnico de velocidad es el anlogo los conmutadores mecnicos
en los motores con escobillas, y ste se encarga de cambiar continuamente cada una de las
fases del motor para generar un campo magntico 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 mxima que la configuracin en Delta). [33]

34

Figura 14. Representacin de Motor DC sin Escobillas

2.5.2 Controles electrnicos de Velocidad (ESCs)

Los Controladores Electrnicos de Velocidad (ESC por sus siglas en ingls) son los
dispositivos encargados de coordinar y dar paso a la rotacin y control de los motores sin
escobillas. Estas unidades se encargan de generar la secuencia de pulsos trifsicos que son
requeridos para provocar el movimiento de los motores sin escobillas.

Para controlar la rotacin del motor, el controlador requiere de un medio para determinar la
orientacin del rotor con respecto al estator. Para este fin, algunos diseos utilizan sensores
de efecto Hall (los cuales detectan el campo magntico del rotor) y otros mtodos detectan
la fuerza contra electromotriz inducida en las bobinas no energizadas para inferir la
posicin del rotor. Este mtodo mencionado anteriormente no requiere de sensores
adicionales, y por ende este tipo de sistemas son llamados controladores sin sensores.
35

Los controladores tpicos poseen 3 salidas bidireccionales, las cuales popularmente

consisten en un arreglo de FETs controlados por un circuito lgico. Normalmente este
circuito es un microcontrolador el cual se encarga de administrar la aceleracin, controlar la
velocidad de rotacin y afinar la eficiencia adaptndose al sistema en el cual se encuentra
trabajado el controlador. Para realizar el proceso anterior, normalmente los ESC tienen que
ser configurados con los parmetros del sistema en el cual estn trabajando (como por
ejemplo el tipo de bateras al cual est conectado, las tensiones mnimas de operacin
segura, el tipo de arranque del motor, etc).

El reto al que se enfrentan los controladores que infieren la posicin del rotor con respecto
a la fuerza contra electromotriz de las bobinas que no estn en uso, es a la hora de iniciar el
movimiento, ya que no hay una fuerza contra electromotriz (el rotor se encuentra esttico, y
por ende no hay tensin 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 continuacin se encuentra una figura que ilustra el principio de funcionamiento de los

motores sin escobillas trifsicos. Consiste en un motor de 3 bobinas por fase, cuyo rotor es
su armadura. La secuencia trifsica 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
revolucin completa del motor (para un total de 36 pasos).
36

Figura 15. Principio de funcionamiento de motores sin escobillas [6]

2.6

Comunicaciones Inalmbricas

2.6.1 Comunicacin Serial

Las comunicaciones seriales se refieren al proceso de transmitir datos un bit a la vez, de
una manera secuencial por medio de algn canal de comunicacin. Este tipo de
comunicaciones se realiza cuando el costo de tener varios canales es muy alto, o cuando
realizar un proceso de sincronizacin de varias seales resulta complicado. La desventaja
37

de las comunicaciones seriales, es que poseen tasas de transmisin de datos ms 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
transmisin de datos no es un factor fundamental para la operacin del dispositivo.

Como ejemplos de los buses ms conocidos que implementan comunicacin 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 estndar abierto de comunicaciones inalmbricas, 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 inalmbrica a
los cables RS-232.

Este protocolo est diseado para comunicar dos o ms dispositivos al mismo tiempo, los
cuales normalmente se encuentran gobernados por un dispositivo maestro que se encarga de
realizar la sincronizacin de datos del grupo.

38

El protocolo Bluetooth se encuentra regido por el Bluetooth Special Interest Group o SIG
por sus siglas en ingls, el cual es un grupo formado por ms de diecisis mil compaas de
reas de telecomunicaciones, computacin y dispositivos electrnicos. Este grupo se
encarga de velar por que se cumplan los estndares requeridos para los nuevos dispositivos
que requieran hacer uso de esta tecnologa.

Dada la naturaleza de comunicacin inalmbrica de esta tecnologa, esta presenta ciertos

riesgos de seguridad, debido a la exposicin 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 comunicndose entre s con la seguridad del conocimiento del usuario de
donde se estn transmitiendo los datos. Una vez realizado el emparejamiento, los
dispositivos guardan esta configuracin para futuras transacciones, o bien se puede
restablecer la configuracin y modificar la lista de dispositivos autorizados a compartir
informacin entre s.

Para informacin adicional se puede recurrir al estndar de telecomunicaciones de la IEEE

802.15.1-2002, donde se detalla ms profundamente las caractersticas de este estndar.

39

2.7

Procesamiento de Seales

2.7.1 Introduccin
Debido a la naturaleza analgica de los sensores y transductores que se utilizan para
percibir el entorno y el estado del cuadracptero, stos proporcionan seales de salida que
se van a ver sometidas a ciertos niveles de ruido (causado tanto por vibraciones mecnicas
como por interferencias electromagticas). Por esta razn, resulta de gran importancia para
el adecuado control del vehculo, acondicionar estas seales de manera que se puedan
eliminar los datos provenientes de los sensores que no sean provechosos para el control del
vehculo (o dicho de manera ms sencilla, el ruido).

Para el acondicionamiento de estas seales se encuentran un gran nmero de tcnicas de

procesamiento digital de seales (tanto as que es todo un campo de estudio de la ingeniera
elctrica), sin embargo, debido al alcance de este proyecto y las limitaciones de tiempo y
hardware, solamente se podrn implementar las tcnicas de menor complejidad matemtica,
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 ejecucin de los
algoritmos de control (y por ende el acondicionamiento de las seales), se debe ejecutar en
tiempo real.

40

2.7.2 Manipulacin de los datos de los sensores (IMU)

Como una tcnica para disminuir el nivel de ruido propio de los acelermetros, y disminuir
el efecto del deslizamiento de las medidas de los giroscopios, se puede hacer uso de una
fusin 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 dinmicos que sean relativos a la confiabilidad de las medidas de cada uno de los
sensores, se puede obtener una medida ms robusta del desplazamiento angular real del
vehculo al sumar mediante pesos dinmicos y promediar el resultado final.

2.7.3 Media Mvil

En estadstica, una media mvil, 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
dinmica simplificada de una media mvil se describe a continuacin.

Dada una serie de nmeros y un tamao de subgrupo fijo, el primer elemento de la media
mvil es obtenido sacando el promedio de los valores del primer subgrupo de la serie de
nmeros. Luego, este subgrupo de datos es modificado excluyendo el primer nmero de su
serie, y reemplazndolo por el siguiente nmero de la serie original de datos. Esto genera
41

un nuevo subgrupo de datos, el cual a su vez tiene un valor de promedio numrico nuevo.
Este proceso es repetido hasta abarcar todos los datos de la serie numrica original. A la
grfica que conecta los valores de cada una de las medias obtenidas se le denomina media
mvil.

Las medias mviles tambin pueden ser utilizadas agregando pesos distintos a los valores
que son de mayor inters, para que estos tengan mayor o menor impacto sobre el promedio
de cada subgrupo. Para ilustrar de manera ms sencilla lo explicado anteriormente se puede
observar la siguiente figura:

Figura 16. Ilustracin Media Mvil

42

Dado un grupo de 8 datos, y un tamao 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 grfico lineal donde se pueden observar los datos originales del ejemplo, y los
de la media mvil. Se puede observar como la media mvil 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. Grfico Ilustrativo de Media Mvil

43

Esta es una de las principales utilidades de la media mvil, el atenuar las fluctuaciones de
pequea duracin de los datos originales, y darles ms importancia a las tendencias de largo
plazo. La diferencia entre largo y corto plazo depende de cada aplicacin, por ende, el
tamao 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 mvil tambin 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 ms adecuada y eliminar el ruido del sistema. Cabe resaltar que
esto tambin causa problemas si los parmetros no se encuentran debidamente programados
dado que mientras ms se trate de atenuar el ruido, ms lenta ser la respuesta del sistema.

2.8

Control Automtico

2.8.1 Controlador PID

Los algoritmos de control PID (Proporcional Integro-Diferencial) son los sistemas de
control ms utilizados en la industria de la automatizacin de procesos debido a su
versatilidad y adaptabilidad a diversos procesos controlables.

Los controladores PID son sistemas que trabajan bajo el principio de realimentacin de lazo
cerrado, los cuales se encargan de manipular una o ms variables de un sistema (las cuales
44

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 parmetro
de control de la variable manipulada (entrada del sistema). Estos elementos son: la
componente proporcional, la componente integral y la componente derivativa. Una correcta
sintonizacin de las ganancias de estos parmetros, 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 ms rpida y estable
posible.

La ecuacin generalizada de los controladores PID viene dada por la siguiente expresin:

Donde:

t: corresponde al tiempo transcurrido, o tiempo instantneo.

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.

45

A continuacin se puede observar una representacin grfica del controlador:

Figura 18. Diagrama de bloques de controlador PID Paralelo

2.9

Fuente de Poder

2.9.1 Batera de Polmero de Ion de Litio (LiPo)

Las bateras de Polmero de In de Litio, tambin conocidas como bateras de Polmero de
Litio o LiPo por su abreviatura en ingls, son un tipo de bateras recargables de alto
rendimiento, las cuales normalmente se encuentran compuestas de una o varias celdas que
se disponen tanto en serie (para aumentar la tensin de salida de la batera) como en
paralelo (para aumentar la capacidad y la corriente de descarga de la batera). Se
caracterizan porque permiten niveles altos de descargas de corriente elctrica, motivo por el
cual, estas sern las bateras seleccionadas para el desarrollo del presente cuadracptero.

46

3. CAPTULO III: Desarrollo e Implementacin del UAV

3.1 Diagramas de Bloques del Cuadracptero
A continuacin se presenta el diagrama de bloques a grandes rasgos con el cual se va
proceder con el modelado del cuadracptero por desarrollar. Cada uno de los bloques
representa un subsistema que ser diseado 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 validacin y la puesta en funcionamiento de cada una de ellas resulte ms accesible
al desarrollador.

Mdulo de
Sensado

Mdulo de
Comunicaciones

Mdulo de control
principal

Control de
Potencia

Motores

Fuente de
Alimentacion

Figura 19. Diagrama de Bloques de Controlador PID

47

Cabe aclarar que el cuadracptero que se construir, se disear en configuracin +. A

continuacin se detalla visualmente los dos tipos de configuraciones posibles para el diseo
de cuadracpteros:

Figura 20. Cuadracptero en configuracin "x"

Figura 21. Cuadracptero en configuracin "+".

48

Por sutil que parezca la diferencia, esto influye en la programacin y calibracin que se
realizar en los sistemas de control. Se elige la configuracin + debido a que resulta de
ms sencilla aplicacin.

A continuacin 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 rotacin alrededor del eje X (o Y), y un valor de
velocidad fijo para iniciar con una velocidad conocida cuando se encienda el
cuadracptero. El resultado de dicha suma (la cual se convertir en una seal PWM por el
microcontrolador), ser ingresado en el bloque de control de potencia de los motores
(ESC), donde se convertir la seal PWM resultante, en el conjunto de seales trifsicas
correspondientes para lograr la velocidad angular requerida para cada uno de los motores.

Motor X
Entrada de Seales de
Transductores

PID Altitud

PID Rotacion Eje X

Salida de Velocidad de
Motores
Velocidad Motor X

Valor Fijo

49

Figura 22. Diagrama de bloques funcionales para cada motor

Seguidamente, en el grfico que se muestra a continuacin, se ilustra a grandes rasgos el

lazo de realimentacin y ejecucin. 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 posicin y postura del cuadracptero va a estar determinada por la interaccin del
comportamiento de cada uno de los cuatro motores. Esta postura va a ser traducida en un
conjunto de seales elctricas por los transductores, las cuales van a ser introducidas en los
bloques de control por medio del micro-controlador (Arduino).

Motor 1

Motor 2

Transductores de
Posicin y Postura

Posicin y Postura
Deseada

Motor 3

Motor 4

Figura 23. Lazo de ejecucin y realimentacin del cuadracptero.

50

La tabla de asignacin de entradas y salidas del Arduino ser como se muestra a

continuacin. Cabe resaltar que hay un pin de entrada analgica libre, sin embargo,
prximamente este ser destinado para la salida del eje Z de un nuevo giroscopio. Este
nuevo eje del giroscopio ser requerido para la correccin de la rotacin alrededor del eje Z
del cuadracptero, 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 sealizacin.

Tabla 5. Tabla de asignacin de pines de Arduino

51

3.2 Experimentos
3.2.1

Validacin del mdulo Bluetooth

La validacin de este mdulo se realiz de una manera muy sencilla. Primero se procedi a
la programacin de los parmetros propios del mdulo, como lo es su tasa de transmisin
de datos y su respectivo emparejamiento con la computadora y el telfono 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 apndice 1 de la seccin de anexos. La dinmica
consisti en utilizar 5 leds de distintos colores que se encontraban conectados a las salidas
digitales del microcontrolador, y prenderlos o apagarlos desde el telfono inteligente de
manera remota por medio del adaptador bluetooth utilizado en conjunto con el
microcontrolador Arduino.

A continuacin, se puede observar un diagrama sencillo del proceso que se detall

anteriormente para la validacin del mdulo bluetooth del Arduino.

52

Figura 24. Diagrama de mtodo de validacin de mdulo bluetooth en conjunto con Arduino.

La validacin result exitosa, no hubo ningn tipo de comportamiento anmalo, 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 comunicacin serial entre
el Arduino y el computador por medio del monitor serial de datos que posee la misma
aplicacin con la cual se realiza la programacin del Arduino (se puede observar una
captura del monitor serial en la figura a continuacin).

Figura 25. Captura de Monitor Serial de Aplicacin de Programacin de Arduino.

53

3.2.2

Validacin de los controles electrnicos de velocidad (ESC)

Para la experimentacin con los controladores electrnicos de velocidad se procedi de la

siguiente manera:

Programacin de parmetros de los controladores de velocidad (ESC).

Cada ESC debe ser programado antes de su primera utilizacin. Esto debido a que
requieren de diversos parmetros propios del sistema donde van a ser utilizados para un
funcionamiento ptimo. El proceso a seguir para la programacin de los controladores de
velocidad vara dependiendo de la marca y el modelo, de modo que para la programacin
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

validacin en conjunto con el Arduino Uno. Para esto se realiz un pequeo sketch
exclusivo para el control de velocidad de los motores. A grandes rasgos, el concepto del
experimento de validacin fue utilizar una seal analgica de entrada, generada mediante
un potencimetro y una fuente de tensin no variable, con la cual se lograra obtener un
control lineal proporcional de la velocidad de los motores en funcin de la tensin variable
generada mediante el potencimetro. Los cuatro pines de control de cada uno de los
controladores de velocidad fueron ubicados en las correspondientes salidas PWM del
54

Arduino, y la tensin variable del potencimetro se coloc en una de las entradas

analgicas del microcontrolador. El experimento de validacin result exitoso, y el control
de los motores se pudo llevar a cabo mediante la seal analgica de entrada que fue
generada por el potencimetro.

3.2.3

Validacin de la Unidad de Medicin Inercial (IMU)

Este apartado correspondi a uno de los mayores retos del desarrollo del proyecto. Como se
mencion en el II Captulo, los sensores utilizados para el sistema de medicin inercial
(acelermetros y giroscopios), poseen salidas cuyas seales poseen un determinado nivel de
ruido que va en detrimento de la robustez del control automtico del cuadracptero. Este
ruido es dependiente del tipo de componentes mecnicos que se estn utilizando (como por
ejemplo las caractersticas de las hlices, el material del marco, etc). Por esta razn se tuvo
que implementar una etapa de filtrado digital de las seales para tratar de eliminar la mayor
parte del ruido de cada una de las seales de entrada.

Para lograr esto, se implement la tcnica de la media mvil para realizar un promediado
dinmico de los valores de cada una de estas seales, de modo que se disminuyera el nivel
de ruido, sin desfasar en gran medida la seal filtrada de la seal real. Esto es de suma
importancia debido a que los controladores PID requieren obtener su seal de entrada en
tiempo real para un correcto funcionamiento del sistema.
55

El parmetro requerido para este filtro (cantidad de muestras que se promedian en cada
ciclo de ejecucin), se obtuvo experimentalmente, con un mtodo de prueba y error, hasta
obtener el parmetro que mostrara el mejor resultado posible.

Adems, con la finalidad de disminuir el ruido de las seales, se configur la frecuencia de

muestreo de las seales de los acelermetros y giroscopio a 100 Hz, mientras que la
frecuencia de muestreo del sensor ultrasnico de proximidad se configur a 10 Hz. Esto
debido a que cada muestreo de este sensor puede durar hasta 25 ms (vara dependiendo de
la distancia del objeto o superficie), de modo que si se realizara a la misma frecuencia que
el muestreo de los acelermetros y giroscopios, se alargara mucho el tiempo de ejecucin
de cada ciclo, y por ende se disminuira la frecuencia del lazo principal.

A continuacin se observa un grfico que muestra la seal sin filtrar (en azul) junto con la
seal filtrada (rojo). Esta captura fue tomada con los motores encendidos a un 15% de su
velocidad mxima. Se puede apreciar la gran reduccin del ruido que se logr al realizar la
fusin de las seales de los acelermetros y los giroscopios, junto con el promediado
dinmico expuesto anteriormente. El promediado se realiz con un ndice de 20 muestras
por ciclo.

56

Figura 26. Comparacin entre seal original y seal filtrada de uno de los acelermetros.

Adems se puede observar el desfase que se obtiene entre la seal filtrada (roja) y la seal
sin procesar (azul). Para este experimento se conservaron los mismos parmetros de la
prueba anterior, solamente que se apagaron los motores para observar una seal ms limpia
de los acelermetros. Tambin cabe mencionar que para estas pruebas se utiliz un peso
relativo del 80% para la seal del acelermetro y un peso de 20% para la seal del
giroscopio. Se observ que mientras mayor peso se le asigne a las lecturas del giroscopio,
mayor es el desfase experimentado entre la seal filtrada y la seal original.

Figura 27. Prueba para observar el desfase entre la seal filtrada y la original, con los motores
apagados.

57

Por ltimo, se realiz una prueba para observar la inmunidad del sistema ante la presencia
de seales no deseadas. Para este caso, se aplicaron pequeos golpes al sensor, y se
observaron ambas seales de inters (seal filtrada y seal original). Se puede observar que
el sistema final resulta bastante inmune a este tipo de perturbaciones anmalas 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 seales no deseadas.

Una de las principales razones por la cual se le asigna tanta importancia al

acondicionamiento de las seales, es porque estas sern 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 distribucin gaussiana, no
afectar en gran medida el comportamiento del controlador. Sin embargo, la componente

58

diferencial del controlador, si se ver muy afectada, dado que la pendiente de las seales de
ruido de alta frecuencia es muy pronunciada, motivo por el cual la derivada de estas seales
resulta en una cifra muy alta. Debido a esta razn, 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 ms se disminuye esta ganancia, el controlador no
reacciona de manera adecuada ante cambios bruscos reales en la postura del cuadracptero,
y su debida correccin de postura no se ejecuta de la manera adecuada.

De aqu la importancia de tener una seal bien acondicionada, para poder dimensionar
correctamente la ganancia de la componente derivativa del controlador, y de esta manera
logar que el cuadracptero responda de una mejor manera ante cambios bruscos en su
postura.
3.2.4

Validacin del Transductor Ultrasnico de Proximidad

Para la validacin del transductor ultrasnico se decidi utilizar la librera facilitada por el
fabricante del dispositivo. La dinmica consisti en conectar el dispositivo al Arduino, y
luego observar si las distancias obtenidas mediante el sensor ultrasnico coincidan con las
medidas reales de distancia (las cuales fueron medidas con una cinta mtrica). Los
resultados fueron los esperados, y las lecturas fueron muy precisas. Cabe resaltar que
mediante la experimentacin se pudieron medir distancias que fueron desde los 2 cm hasta
los 500 cm. Fuera de ese rango, las lecturas eran inestables.
59

El Sensor Ultrasnico 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 centmetros (CM) o pulgadas (IN).
delay(500);
}

El resultado de la altura que percibe el mdulo una vez instalado en el cuadracptero es de

6 cm. Las medidas son muy estables.

3.2.5

Validacin del sistema de control automtico.

Para la validacin del sistema de control automtico, 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 matemtico 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 experimentacin hasta llegar a los parmetros que
dieran los mejores resultados.

60

Para realizar este proceso, la dinmica 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 obtencin de los parmetros para el PID de control de rotacin
alrededor del eje X. Para realizar esto, se restringi la rotacin del cuadracptero alrededor
del eje Y al apoyar el eje X en una superficie libre de friccin 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 cuadracptero (mientras tanto, los motores 1 y 3
permanecen apagados).

Figura 29. Experimento para la obtencin de los parmetro de PID de inclinacin 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
61

llegar a un punto donde el cuadracptero se encontrara cerca de empezar a oscilar alrededor

de su punto de referencia (ngulo de inclinacin de cero grados con respecto a la
horizontal). Ya una vez que se obtuvo el valor de este parmetro, 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 parmetros encontrados para el sistema con un tipo de hlice, no son
vlidos si se cambian las hlices, de modo que se tienen que volver a obtener los
parmetros.

Luego de obtener los parmetros del PID del Eje X, se procede a realizar el procedimiento
anlogo pero para el controlador PID del Eje Y. Esto debido a que el cuadracptero no es
completamente simtrico, de modo que los parmetros varan dependiendo de la
distribucin de la masa del vehculo.

El proceso de experimentacin fue bastante largo, sin embargo, se encontr un punto de

operacin donde el cuadracptero se logra estabilizar de manera automtica alrededor del
punto de referencia que se le definiera. Los resultados de esta configuracin se pueden

62

observar ms 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 operacin, 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 parmetros se obtuvieron con hlices de 8 pulgadas de dimetro. Cabe destacar que la
diferencia entre los parmetros del eje X y del Eje Y se debi principalmente a la forma de
la batera (dado que constituye un 30% del peso total) y como se encontraba dispuesta en el
vehculo (dado que no es simtrica).

Este proceso de obtencin de los parmetros se puede facilitar y optimizar al contar con el
equipo necesario para poder modelar el cuadracptero y poder obtener la respuesta al
escaln de cada uno de los ejes. Sin embargo, por motivo de tiempo y recursos, para la
realizacin del presente proyecto no se pudo contar con las herramientas requeridas. No
obstante, fuera del hecho que la obtencin de los parmetros no se dio de la manera
cientfica-matemtica deseada, los resultados que se obtuvieron fueron muy aceptables.

Adems se aclara que para la experimentacin que se detall anteriormente, se incluy una
proteccin 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 mxima de los motores). Esto para
evitar que por algn error de programacin o algo similar, el cuadracptero se salga de
63

control y ocurra algn accidente. La adicin de todo tipo de protecciones es una prctica
recomendada en caso de que se vaya a experimentar o realizar algn cambio del que no se
est completamente seguro de su resultado, de modo que haya seguridad de que el
cuadracptero no se va a salir de control y daarse o lastimar a alguien.

3.3 Lista de Componentes Utilizados

A continuacin se puede observar una tabla resumen con los componentes utilizados en la
construccin 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 Cuadracptero

64

El cdigo de programacin 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 Cuadracptero.

Figura 31. Modelo Real del Primer Prototipo del Cuadracptero.

65

Figura 32. Modelo Real del Primer Prototipo del Cuadracptero.

Figura 33. Parte inferior del primer prototipo del cuadracptero.

66

4. Captulo 4: Conclusiones y Recomendaciones

4.1 Conclusiones

El desarrollo de todos los sistemas separados del cuadracptero y su posterior

integracin result ser un proceso de mayor complejidad que lo proyectado
inicialmente.

La etapa de pruebas y verificacin de cada subsistema del cuadracptero result de

gran importancia y ayuda para entender mejor su funcionamiento y corregir los
errores de diseo 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 validacin y pruebas de los sistemas que requirieron el uso de las

hlices result ser ms peligroso de lo pensado, dado que los motores se encuentran
girando a muy altas velocidades y existe el riesgo de un malfuncionamiento por
algn error de programacin. Se tuvieron que tomar todas las precauciones del caso
para hacer las pruebas de una manera segura.

67

El Filtro Kalman result tener una complejidad ms 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 cuadracptero.

El desarrollo de un proyecto multidisciplinario como el presente, requiere de mucho

tiempo, dedicacin y planeamiento, debido a la gran cantidad de investigacin,
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 apliacin.

Para obtener un prototipo de vuelo autnomo se requiere de mucho procesamiento
matemtico 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 matemtico,
el ATmega328 muestra una disminucin en la frecuencia del lazo de ejecucin del
programa hasta llegar a un nivel donde es muy bajo para la correcta operacin
dinmica de los motores.

La comunicacin de datos va bluetooth se realiz de manera exitosa en ambas vas

(transmisin y recepcin), motivo por el cual se pudo completar exitosamente ese
objetivo especfico. El Arduino se pudo comunicar sin ningn problema tanto con la
68

computadora personal como con el dispositivo Android para enviar los datos de
postura y otras variables de inters para el usuario por medio del shield bluetooth.

Los motores sin escobillas mostraron un desempeo superior al esperado, al trabajar

en conjunto con sus respectivos controladores electrnicos de velocidad. Estos
motores poseen una relacin potencia efectiva/peso mucho ms elevada que su
contraparte con escobillas.

La eleccin y balanceo correcto de las hlices de los motores resulta fundamental

para el buen funcionamiento del sistema, ya que provoca menos vibracin en el
sistema, y un vuelo ms estable.

El uso de un telfono inteligente como mando a distancia del cuadracptero result

ser un muy buen ejercicio acadmico, sin embargo resulta poco prctico tener que
controlar un sistema tan dinmico y de rpida respuesta por medio de una pantalla
tctil.

69

4.2 Recomendaciones

Se recalca el cuidado que hay que tener al hacer las pruebas con las hlices
puestas. Se recomienda utilizar el equipo de proteccin requerido.

Realizar pruebas independientes de cada sistema y entender el impacto del

cambio de cada uno de sus parmetros 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 tensin de la batera utilizada para no descargarla

ms all de los lmites recomendados. Esto puede causar un dao perjudicial en
la batera que va en detrimento de su vida til.

Realizar la sintonizacin de los controladores PID con un mtodo analtico de

sintonizacin 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 verstil.
70

Documentar muy bien todos los experimentos que se realicen, y los cambios que
se le hagan a los sketches (a manera de bitcora).

Hacer buen uso de los profesores tutores para orientar la resolucin de los
problemas de alta complejidad que se presenten durante el desarrollo del
proyecto.

4.3 Trabajo Futuro

Realizar un modelado matemtico del cuadracptero para poder crear un sistema

de control ms 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 ms potente, de modo que se

puedan implementar algoritmos ms complejos, y aadirle ms funcionalidades
al cuadracptero. Adems se requieren ms entradas analgicas para aadir ms
sensores al vehculo (magnetmetros).

71

BIBLIOGRAFA
Artculos 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 QuadRotor 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.

72

7. Alexandros Soumelidis, Peter Gaspar, Gergely Regula, Bela 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 Edicin,
Macmillan, Inglaterra, 1996.
11. Subir Kumar, Saha. Introduccin a la Robtica. I Edicin, Mc Graw Hill, 2010.

Pginas Web:

12. Teora de Diseo Electromagntico de Motores DC y Actuadores

http://www.consult-g2.com/course/chapter9/chapter.html
13. Bateras de Nquel-Cadmio
http://en.wikipedia.org/wiki/Nickel%E2%80%93cadmium_battery
73

14. Motores Sincrnicos 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. Declaracin de Funciones en Arduino
http://arduino.cc/en/Reference/FunctionDeclaration
19. Librera 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. Librera PID para Arduino http://arduino.cc/playground/Code/PIDLibrary.
24. Pgina Oficial Aeroquad http://aeroquad.com/content.php?116
25. Propelas http://en.wikipedia.org/wiki/Propeller_(aircraft)
74

26. Implementacin 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 Acelermetros y Giroscpios para captura de posicin
http://www.starlino.com/quadcopter_acc_gyro.html
29. Gua de Sistema de Medicin 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 Sustentacin http://en.wikipedia.org/wiki/Lift_(force)
32. Perfil Alar http://en.wikipedia.org/wiki/Planform
33. Media Mvil http://en.wikipedia.org/wiki/Moving_average
34. Uso de Media Mvil en Mercados Financieros
http://www.investopedia.com/articles/technical/060401.asp#axzz1xSRDjgzN
35. Hoja de Especificaciones Tcnicas de ATmega328
http://www.atmel.com/Images/8271S.pdf
36. Hoja de Especificaciones Tcnicas de Giroscopio IDG-500.
http://www.sparkfun.com/datasheets/Components/SMD/Datasheet_IDG500.pdf
37. Hoja de Especificaciones Tcnicas de Acelermetro ADXL-335.
http://www.sparkfun.com/datasheets/Components/SMD/adxl335.pdf

75

APNDICES
En este apartado se debe de incluir informacin que por su naturaleza no puede ser
incluida en el desarrollo mismo del proyecto, pero que fue utilizada para su elaboracin.

Apndice 1. Sketch de validacin del mdulo 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
76

{
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();
}
}

77

Apndice 2. Cdigo completo de programacin del cuadracptero

//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. Todava es inestable.
//Se usa una frecuencia de muestreo de 100 Hz
/*
Hardware:
AccX
AccY
AccZ
GyroX
GyroY

<---> ANALOG 0
<---> ANALOG 1
<---> ANALOG 2
<---> ANALOG 3
<---> ANALOG 4

PWM Motor 1
PWM Motor 2
PWM Motor 3
PWM Motor 4

<--->
<--->
<--->
<--->

DIGITAL 3
DIGITAL 9
DIGITAL 10
DIGITAL 11

Ultrasonic Trigger <---> DIGITAL 12

Ultrasonic Echo <---> DIGITAL 13 */
#include "Ultrasonic.h"
#include "PID_v1.h"
#include <EEPROM.h>
#define INPUT_COUNT 5
#define VDD 5000.0f
#define PI 3.14159265358979f

//nmero de entradas analogicas

//voltaje de referencia en milivolts

//Valores e inicializacin de transductor ultrasnico

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];
int zeroLevel[INPUT_COUNT];
int inpSens[INPUT_COUNT];
float wGyro;

// bits 0..5 invert input

// 0..2 accelerometer zero level (mV) @ 0 G
// 3..5 gyro zero level (mV) @ 0 deg/s
// 0..2 acceleromter input sensitivity (mv/g)
// 3..5 gyro input sensitivity (mV/deg/ms)
// gyro weight/smooting factor

78

} 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 combinacin 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;
double adelante_sp=0.00, derecha_sp=0.00, altura_sp=0.00;
double x_angle, y_angle;

// Salidas de los controladores PID

//Setpoints iniciales de los controladores PID
//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 rotacin 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 ubicacin 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 proteccin de velocidad para las pruebas.

79

double sensibilidad_control = 0.4;

double vel_altura_manual = 0;
double vel_altura_manual2 = 0;

//Sensibilidad del joystick. No usar valores superiores a 0.5!

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 parmetros 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);

80

PID_EJE_Y.SetMode(AUTOMATIC);
PID_EJE_Y.SetOutputLimits(-vel_limit_PID, vel_limit_PID);

//Activa control PID de correccin eje Y.

PID_Altura.SetMode(AUTOMATIC);
PID_Altura.SetOutputLimits(0, vel_limit_PID_altura);

//Activa control PID de altitud.

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

81

}
imprimir_valores_debug();
}

void imprimir_valores_debug(){
//Datos para generacin 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));

82

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);

83

//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;
}

84

//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 obtencin de ngulos de inclinacin.

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];
}else{
//evaluate RwGyro vector
if(abs(RwEst[2]) < 0.1){

//initialize with accelerometer readings

85

//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;
}

86