Cu so Académico:
TRABAJO FIN DE GRADO EN INGENIERÍA EN TECNOLOGÍAS INDUSTRIALES
DISEÑO DE UN CONTROL DE POSICIÓN X-Y
DE UN QUADROTOR BASADO EN UN
ALGORITMO DE FLUJO ÓPTICO
AUTOR:
TUTOR:
FRANCISCO MIGUEL AROCA JIMÉNEZ
PEDRO GARCÍA GIL
2014-15
Índice de documen os
 Memo ia
 P esupues o
 Anexo de p og amación
Índice de la memo ia
Glosa io de ilus aciones ..................................................................................................................... 1
1. In oducción .................................................................................................................................... 4
1.1. Concep os gene ales ............................................................................................................... 4
1.2. Modelo eó ico ........................................................................................................................ 5
1.3. Mo i ación y jus i icación ........................................................................................................ 7
1.4. An eceden es y soluciones p e ias ......................................................................................... 8
1.5. Obje i os del abajo ............................................................................................................... 8
1.6. No ma i a ................................................................................................................................ 9
2. Desa ollo del abajo ...................................................................................................................... 9
2.1. Plan eamien o gene al ............................................................................................................ 9
2.2. Fundamen os de la isión po compu ado .......................................................................... 10
2.3. Selección del ma e ial (ha dwa e y so wa e) ...................................................................... 14
2.4. Desa ollo del algo i mo de medición y con ol .................................................................... 18
2.4.1. Algo i mos de medición ................................................................................................ 18
Algo i mo gene al ...................................................................................................................... 19
Algo i mo especí ico (cí culos) .................................................................................................. 27
Algo i mo especí ico (colo ) ...................................................................................................... 30
2.4.2. T a amien o de la medida ............................................................................................. 31
2.4.3. Desa ollo del algo i mo de con ol .............................................................................. 36
3. Resul ados y conclusiones ............................................................................................................. 39
Bibliog a ía......................................................................................................................................... 40
Índice del p esupues o
Unidades de ob a .............................................................................................................................. 42
P esupues o inal ............................................................................................................................... 43
Índice del anexo
Código del algo i mo gene al de medición ....................................................................................... 45
Código del algo i mo especí ico de medición (algo i mo de cí culos) .............................................. 48
Código del algo i mo especí ico de medición (algo i mo de colo ) .................................................. 49
Modi icaciones pa a adap a la medida ............................................................................................ 50
Fil ado .............................................................................................................................................. 50
Con ol ............................................................................................................................................... 51
1
Glosa io de ilus aciones
Se ci a aquí el o igen de las imágenes omadas de uen es ex e nas.
Ilus ación 1. D on MQ-9 pa a uso mili a .............................................................................................. 4
h p://upload.wikimedia.o g/wikipedia/commons/ humb/b/b0/MQ-
9_Reape _in_ ligh _%282007%29.jpg/250px-MQ-9_Reape _in_ ligh _%282007%29.jpg
Ilus ación 2. D on de 8 hélices pa a uso ci il ......................................................................................... 4
h p://www.a alaya .com/con en /a gelia- end %C3%A1-su-p ime -d one-supe s%C3%B3nico-en-
2016
Ilus ación 3. D on case o p opulsado con MCIA único unido a un sis ema de co eas......................... 5
h p://diyd ones.com/p o iles/blogs/gas-powe ed-quadcop e -second-pixhawk-based- inalis - o -
he-hack
Ilus ación 4. Quad o o obje o del abajo ............................................................................................ 5
Ilus ación 5. Angulos de o ien ación (φ, 𝜃, ψ) ....................................................................................... 6
Ilus ación 6. Coo denadas globales (x,y,z) ............................................................................................. 6
Ilus ación 7. 1e gi o (yaw)..................................................................................................................... 6
Ilus ación 8. 2ndo gi o ( oll) ................................................................................................................... 6
Ilus ación 9. 3 gi o (pi ch) ..................................................................................................................... 6
Ilus ación 10. Medida de lujo óp ico conside ando un pixel .............................................................. 12
Ilus ación 11. Medida de lujo óp ico conside ando una en ana de 2x2 píxeles ............................... 12
Ilus ación 12. Ejemplo de lujo óp ico denso ....................................................................................... 13
h p://www.hizook.com/ iles/use s/7/Op icalFlow_S ee .jpg
Ilus ación 13. Ejemplo de lujo óp ico dispe so ................................................................................... 13
h p://i.y img.com/ i/OIm6VXa0 Z0/hqde aul .jpg
Ilus ación 14. BeagleBone Black .......................................................................................................... 14
h p://beagleboa d.o g/black
Ilus ación 15. IGEP 2 ............................................................................................................................ 14
h ps://www.isee.biz/p oduc s/igep-p ocesso -boa ds/igep 2-dm3730
Ilus ación 16. Eye Toy Nam ai .............................................................................................................. 16
h p://blog.us.plays a ion.com/2010/11/03/eye oy-inno a ion-and-beyond/
Ilus ación 17. PlayS a ion Eye .............................................................................................................. 16
h p://www.amazon.com/PlayS a ion-Eye-3/dp/B000VTQ3LU
Ilus ación 18. Cáma a PS eye sin ca casa ............................................................................................. 18
Ilus ación 19. De alle del mon aje de la cáma a (líneas ojas = ejes X-Y quad o o ) .......................... 18
Ilus ación 20. Diag ama de lujo del algo i mo gene al inicial ............................................................ 20
Ilus ación 21. De alle del diag ama de lujo (selección de pun os) ..................................................... 21
Ilus ación 22. G á ica de compa ación de algo i mos de selección de pun os ................................... 22
Ilus ación 23. Selección de pun os a in e alos egula es ................................................................... 22
Ilus ación 24. Selección de pun os con GoodFea u es ........................................................................ 22
2
Ilus ación 25. Selección de pun os con FAST ....................................................................................... 22
Ilus ación 26. Selección de pun os con ORB ........................................................................................ 22
Ilus ación 27. De alle del diag ama de lujo (seguimien o de pun os) ................................................ 23
Ilus ación 28. Flujo óp ico con amaño de en ana 10x10 .................................................................. 24
Ilus ación 29. Flujo óp ico con amaño de en ana 3x3 ...................................................................... 24
Ilus ación 30. De alle del diag ama de lujo (cálculo desplazamien o) ............................................... 24
Ilus ación 31. Diag ama de lujo del algo i mo gene al modi icado .................................................... 25
Ilus ación 32. G á ica compa a i a en e algo i mos de medida ........................................................ 26
Ilus ación 33. G á ica de ayec o ia ejemplo ...................................................................................... 26
Ilus ación 34. Diag ama de lujo del algo i mo especí ico ................................................................... 27
Ilus ación 35. P ocedimien o ejecu ado po el algo i mo especí ico .................................................. 28
Ilus ación 36. Resul ado del algo i mo u ilizando 2 cí culos ............................................................... 29
Ilus ación 37. G á ica esul ado expe imen o cí culos ........................................................................ 29
Ilus ación 38. Imagen o iginal .............................................................................................................. 30
Ilus ación 39. Imagen as compa ación y a amien o ....................................................................... 30
Ilus ación 40. Resul ado (pun o ojo = cen o) .................................................................................... 30
Ilus ación 41. Modelo eó ico de la cáma a ........................................................................................ 31
Ilus ación 42. Imagen de ejemplo pa a la calib ación ......................................................................... 31
Ilus ación 43. T iángulo ex aído del modelo ...................................................................................... 32
Ilus ación 44. Validación del modelo con ayec o ia ejemplo ........................................................... 33
Ilus ación 45. Composición de elocidades en X ................................................................................. 33
Ilus ación 46. G á ica de la posición X-Y (sin il a ) ............................................................................ 35
Ilus ación 47. G á ica de elocidad il ada en X (𝛼=0.05) .............................................................. 35
Ilus ación 48. Diag ama del il o de Kalman ....................................................................................... 35
h p://www.scielo.cl/ bpe/img/ingenia e/ 17n3/ ig11-2.GIF
Ilus ación 49. Diag ama de lujo del con ol de o ien ación ............................................................... 36
Ilus ación 50. Diag ama de lujo del con ol de la posición en X ......................................................... 36
Ilus ación 51. Resul ados de la acción p opo cional en uelo ............................................................. 37
Ilus ación 52. Fil ado de la acele ación .............................................................................................. 38
Ilus ación 53. Re a do en la medida de isión ..................................................................................... 38
Ilus ación 54. Medida inal de elocidad ............................................................................................. 39
3
2015
Memo ia

4
1. In oducción
1.1. Concep os gene ales
Los ehículos aé eos no ipulados (VANT o UAV en inglés) ambién llamados d ones, cons i uyen un
g upo muy amplio de ae ona es de ipologías muy dis in as, desde con igu aciones simila es a los
a iones clásicos, has a o as más p óximas al sis ema de uncionamien o de los helicóp e os, con a ias
hélices que p opo cionan la sus en ación necesa ia.
Ilus ación 1. D on MQ-9 pa a uso mili a
Ilus ación 2. D on de 8 hélices pa a uso ci il
Den o de es e úl imo g upo, ambién podemos encon a di e encias p incipalmen e en el núme o de
hélices y en el sis ema de p opulsión. Así, un mayo núme o de hélices p opo ciona un mayo con ol
de la o ien ación a cos a de una ley de con ol más compleja debido al aumen o de los g ados de
libe ad.
Po o a pa e, en cuan o a los sis emas de p opulsión, p incipalmen e se u ilizan 2: mo o es eléc icos
y mo o es de combus ión in e na. El sis ema más popula es el basado en mo o es eléc icos debido a
la acilidad de egulación, poco peso y e sa ilidad aunque iene incon enien es como las limi aciones
de po encia y au onomía debido a la baja po encia especí ica de los mo o es y la baja ene gía especí ica
de las ba e ías que los alimen an. Es os p oblemas no apa ecen con los mo o es de combus ión, su
mayo po encia pe mi e ele a mayo peso y la densidad ene gé ica del combus ible o ece una
au onomía supe io a las ba e ías, aun así es e ipo de mo o es son de di ícil egulación e in oducen
ib aciones indeseables en el sis ema que pueden in e e i en los senso es.
5
Ilus ación 3. D on case o p opulsado con MCIA único unido a un sis ema de co eas
En nues o caso, el quad o o sob e el que se ha abajado se a a de un d on con 4 hélices
con igu adas en o ma de c uz y mo idas po pequeños mo o es eléc icos. Es a es una de las
con igu aciones más popula es po su e sa ilidad y sencillez de con ol.
Ilus ación 4. Quad o o obje o del abajo
1.2. Modelo eó ico
Una ez de inido el ma co de abajo, pasamos a in oduci b e emen e las a iables y g ados de
libe ad del quad o o y el modelo dinámico del mismo.
Pa a plan ea un sis ema de con ol hacen al a a iables que con ola . Las a iables habi uales
u ilizadas en es e ámbi o son los 3 ángulos de o ien ación (pi ch, oll, yaw) ela i os a un sis ema de
e e encia ligado al quad o o y las 3 coo denadas ca esianas globales del quad o o (x, y, z).
6
Ilus ación 5. Angulos de o ien ación (φ, 𝜃, ψ)
Ilus ación 6. Coo denadas globales (x,y,z)
La posición y o ien ación del quad o o se ob ienen asladando el o igen de coo denadas del sis ema
global al cen o del quad o o , pos e io men e e ec uando el gi o co espondien e a la medida yaw
sob e el eje local Z, o ando después en o no al eje X del sis ema de e e encia esul an e el ángulo
oll, y inalmen e, o ando de nue o el ángulo pi ch espec o al eje Y de o ma análoga al gi o an e io .
Ilus ación 7. 1e gi o (yaw)
Ilus ación 8. 2ndo gi o ( oll)
Ilus ación 9. 3 gi o (pi ch)
7
Pasamos aho a a expone el modelo dinámico del quad o o [1], és e de i a de aplica las ecuaciones
de Eule -Lag ange a nues o sis ema incluyendo además la simpli icación de que los ángulos pi ch y
oll son pequeños. El esul ado es el siguien e sis ema de ecuaciones:
Donde 𝑚 es la masa del quad o o , 𝑢 es el empuje o al de los mo o es, los pa áme os 𝑘𝜓, 𝑘𝜃 y 𝑘𝜑
son cons an es que engloban los momen os de ine cia del quad o o espec o a su cen o, la elación
en e ol aje aplicado al mo o y elocidad de gi o del mismo y la ley de las hélices que elaciona su
elocidad de gi o con la sus en ación que p opo cionan. Y las a iables 𝑉𝑥 son los ol ajes aplicados a
cada mo o que cons i ui án la acción de con ol.
1.3. Mo i ación y jus i icación
La azón de se de es e abajo, además de la inalización de los es udios de g ado, es la esolución de
los p oblemas que aca ea no con ola la posición del quad o o que an desde la de i a excesi a y
posible pé dida del apa a o, has a choques con obs áculos o pe sonas que pueden ocasiona g a es
daños an o al quad o o cómo a aquello con lo que choca.
P ocedemos aho a a analiza el modelo ma emá ico pa a comp ende la p oblemá ica del con ol en
el plano X-Y. Tomemos como pun o de pa ida la con igu ación inicial del quad o o en el labo a o io,
és e iene implemen ado ya, g acias a abajos an e io es de o os alumnos y pe sonal in es igado ,
un con ol de las a iables de o ien ación (pi ch, oll y yaw) y de la al u a. Así pues pa a un uelo
“ho e ” (es á ico a una al u a de e minada) no hay más que ajus a las e e encias de pi ch y oll a 0º,
consiguiendo así que el quad o o pe manezca pa alelo al suelo y pone la e e encia de la al u a a un
alo de e minado cons an e 𝑧0. Suponiendo que el con ol unciona co ec amen e y man iene los
ángulos de o ien ación en su e e encia, analicemos que ocu e con las a iables 𝑥 e 𝑦 de nues o
modelo eó ico:
𝑚𝑥󰇘=−𝑢sin𝜃
𝑚𝑦󰇘=𝑢cos𝜃sin𝜑 𝜃=0
𝜑=0
→
𝑥󰇘=0
𝑦󰇘=0
→
𝑥󰇗=𝑣𝑥=𝐶𝑥
𝑦󰇗=𝑣𝑦=𝐶𝑦
Ecuación 2. P oblema de la de i a
Como podemos obse a , de la ecuación 2 se ex ae que el quad o o se mo e á con elocidad
cons an e (no necesa iamen e nula) en el plano X-Y de o ma análoga al mo imien o que end ía un
obje o en una pis a de hielo. Es e mo imien o se comp ueba de o ma expe imen al al pone el
quad o o en uelo, se obse a como pa ece que se desplace en un plano imagina io pa alelo al suelo
y es ablecido a la al u a de la e e encia 𝑧0.
𝑚𝑥󰇘=−𝑢sin𝜃
𝑚𝑦󰇘=𝑢cos𝜃sin𝜑
𝑚𝑧󰇘=𝑢cos𝜃cos𝜑−𝑚𝑔
𝜓󰇘=𝑢𝜓=𝑘𝜓(𝑉𝑓−𝑉𝑙+𝑉𝑏+𝑉𝑟)
𝜃󰇘=𝑢𝜃=𝑘𝜃(𝑉𝑏−𝑉𝑓)
𝜑󰇘 =𝑢𝜑=𝑘𝜑(𝑉𝑙−𝑉𝑟)
Ecuación 1. Modelo ma emá ico simpli icado del quad o o
14
busca una se ie de pun os e in en a as ea los en la imagen siguien e y que se ía u ilizado como
he amien a de “benchma king” pa a ayuda a elegi el ma e ial a u iliza (ha dwa e y so wa e).
2.3. Selección del ma e ial (ha dwa e y so wa e)
En es e pun o se empezó con la selección del so wa e base pa a el algo i mo de lujo óp ico, de en e
las di e sas lib e ías de isión a i icial disponibles se eligió OpenCV po di e sas azones: se a a de
so wa e lib e, mul ipla a o ma con una g an comunidad de usua ios y una amplia documen ación
sob e su uso e ins alación. Además, e siones an e io es de es as lib e ías habían sido u ilizadas ya po
o os alumnos e in es igado es en o os p oyec os simila es con buenos esul ados. Los p og amas
diseñados en el apa ado an e io ue on compilados con es as lib e ías. Más adelan e, en el pun o 2.4
se p o undiza á en el con enido y unciones de es as lib e ías.
Pasemos aho a a la selección de la pla a o ma ha dwa e a u iliza , el conjun o placa-cáma a, hablamos
de es os dos elemen os como conjun o po que, pese a a a se de pa es ísicamen e independien es,
es imp escindible que sean compa ibles pa a el co ec o uncionamien o del sis ema.
Ilus ación 14. BeagleBone Black
Ilus ación 15. IGEP 2
Empezamos es e p oceso de selección compa ando dos placas disponibles en el labo a o io, la IGEP 2
(ac ualmen e en uso en el quad o o ) y la Beaglebone Black (BBB en adelan e). Las especi icaciones
écnicas de ambas placas son simila es al y como exponemos en la siguien e abla:
IGEP 2
BBB
P ocesado
DM3730 ARM Co ex A8 1Ghz
AM335x 1GHz ARM® Co ex-
A8
Memo ia RAM
512 MB
512MB
WiFi
Incluido en la placa
Con adap ado USB WiFi
Conexiones disponibles
1 x USB 2.0
1 x E he ne
3 x UART (2 x RS232 y 1 x
RS485)
3 x SPI
2 x I2C
1 x USB 2.0
1 x E he ne
1 x UART
P ecio
179 €
51.99 €
Tabla 1. Compa ación de las especi icaciones de las placas

15
La IGEP 2, al es a ya ins alada en el quad o o , es aba ya casi p epa ada pa a ejecu a nues o
p og ama de p ueba, a al a de la ins alación de las lib e ías OpenCV, que ue on compiladas en
c uzado desde un o denado de sob emesa pa a la a qui ec u a de la placa (ARM) dejando así la IGEP 2
lis a pa a su uso.
La BBB, po o a pa e, es aba p ác icamen e ecién salida de áb ica, con lo cual ue necesa io un
p oceso de p epa ación del en o no de p og amación ins alando un sis ema ope a i o basado en Linux
(Debian) y aplicando un pa che (Xenomai) al ke nel de Linux que pe mi e la ejecución de p og amas
en “ iempo eal”, las a eas ejecu adas en iempo eal ienen máxima p io idad den o del sis ema, se
les puede asigna una p io idad incluso mayo a la de las p opias a eas del sis ema ope a i o e i ando
así cualquie “cuelgue” del p og ama de uelo que p o oca ía la caída del quad o o .
El sis ema ope a i o seleccionado pa a la BBB ha sido el disponible en la siguien e página web bajo
licencia lib e:
h p://www.machineki .ne /deb/ oo s/wheezy/BBB-eMMC- lashe -debian-7.4-machineki -a mh -
2014-05-19-4gb.img.xz
Se ha elegido es e sis ema conc e o po es a diseñado pa a el manejo de máquinas de ab icación po
con ol numé ico, las necesidades de es e campo son simila es a las nues as y es o se e en el so wa e
incluido en es e sis ema ope a i o dedicado que ya con iene el ke nel con Xenomai y las lib e ías de
OpenCV.
Una ez p epa ados los espec i os en o nos de p og amación, se p ocede a ejecu a el p og ama de
p ueba que habíamos p epa ado en el apa ado an e io . El expe imen o se ejecu a en las siguien es
condiciones:
 Cáma a inmó il y en o no in a ian e.
 El p og ama de la cáma a es el único p og ama en ejecución en la placa.
 Los esul ados son imp esos a iche o pa a no comp ome e la elocidad del p og ama.
En las condiciones expues as los esul ados espe ados son: una medida de desplazamien o nula y un
iempo de ejecución solo dependien e de las ca ac e ís icas de la placa y del sis ema ope a i o.
Los esul ados ob enidos son los siguien es:
IGEP 2
BBB
Desplazamien o máximo (pix)
0.01
0.01
Tiempo de ejecución (mín – med – máx) (ms)
30 – 50 – 120
50 – 80 – 200
Tabla 2. Compa ación del endimien o del p og ama de p ueba en las placas
Pasemos aho a al análisis de los esul ados, a p ime a is a, so p ende que el desplazamien o máximo
de píxeles sea un núme o decimal y no en e o, es e esul ado se explica po la na u aleza del p og ama
u ilizado, que, as ob ene el alo indi idual de lujo óp ico pa a cada píxel, pasa a calcula la media
a i mé ica de odos es os alo es que es lo que conside amos como lujo óp ico o al de la imagen.
16
Dicho es o, cabe ema ca que un “e o ” de 0.01 píxeles es un buen esul ado que podemos asocia
a pequeños cambios de iluminación o incluso al uido p esen e en la imagen. O a conclusión
impo an e que ex aemos de es e esul ado es que la p ecisión de la medida no depende de la placa
u ilizada sino más bien de la cáma a en uso.
Po o a pa e sí que encon amos di e encias en el iempo de ejecución de la a ea, pese a que
es amos abajando en un en o no de iempo eal (Xenomai) el p og ama compilado no hace uso de
es as unciones, lo que hace que pueda se pospues a su ejecución en a o de a eas in e nas del
sis ema, es po eso que no ob enemos un iempo de ejecución ijo. Cabe menciona que as de ec a
es e p oblema se in en ó implemen a el mismo p og ama como a ea de iempo eal pe o sin éxi o,
al pa ece las lib e ías de OpenCV no son compa ibles con las a eas de Xenomai.
El p oblema expues o no comp ome e la alidez del expe imen o ya que se ealizó del mismo modo
en ambas placas, en los esul ados se obse a que la placa BBB iene un iempo de ejecución
lige amen e más al o, c eemos que el o igen de es a di e encia es la in e az de usua io del sis ema
ope a i o de la BBB, en la IGEP 2 no se ejecu a ningún en o no g á ico lo cual e i a ca ga
innecesa iamen e el p ocesado .
Pese a que se pod ía habe desac i ado la in e az g á ica de la BBB pa a mejo a la elocidad de
p ocesamien o, se encon ó o o p oblema que lle ó a oma la IGEP 2 como placa de abajo: los
pue os USB. Pa a el uncionamien o au ónomo del quad o o es necesa ia an o la conexión WiFi pa a
comunica se con el PC en ie a como la cáma a pa a hace el con ol po lujo óp ico. En el caso de la
IGEP 2 es o no es un p oblema po que el WiFi es á in eg ado en la p opia placa y iene un pue o USB
pa a conec a la cáma a, pe o la BBB iene un solo pue o USB al que se debe ían conec a an o la
cáma a como el disposi i o de WiFi, se p obó un ampliado de pue os USB pe o daba p oblemas con
la alimen ación así que se apa ó po el momen o la idea de u iliza la BBB y se omó como placa de
abajo la IGEP 2 que además es aba ya ins alada en el quad o o y enía los pue os con igu ados
pa a las conexiones del es o de senso es.
Pasemos aho a a la selección de la cáma a, las opciones conside adas son las 2 siguien es: Eye Toy
Nam ai y PlayS a ion Eye, ambas son cáma as diseñadas con ines de en e enimien o en juegos
basados en la de ección del mo imien o del jugado , se ajus an pues a nues as necesidades de
de ección de mo imien o.
Ilus ación 16. Eye Toy Nam ai
Ilus ación 17. PlayS a ion Eye
Las especi icaciones écnicas de es as cáma as no se han encon ado y se ha p ocedido a su
de e minación empí ica, en conc e o nos in e esan dos pa áme os: la elocidad de cap u a o FPS, y
17
la esolución, que debe se lo su icien emen e al a como pa a de ec a mo imien o, pe o sin llega a
alo es demasiado al os po que una al a esolución de imagen aumen a exponencialmen e el iempo
de cálculo del lujo óp ico.
Pa a pode u iliza las cáma as es necesa io ins ala p ime o los d i e s co ec os, así pues se buscó el
d i e co espondien e a ambas cáma as y se comp obó si es aba ins alado en la IGEP 2, con esul ado
sa is ac o io. Una ez comp obado es o, se pasó a la ex acción de pa áme os ca ac e ís icos de las
cáma as, con es e in se compiló un pequeño p og ama que oma o os de o ma cíclica a la máxima
elocidad posible y comp ueba el amaño de las imágenes. Los esul ados se exp esan en la abla
siguien e:
Eye Toy Nam ai
PS eye
Velocidad (FPS)
60
180 ó 70
Resolución (pix x pix)
320 x 240 (0.0768 Mpx)
320 x 240 (0.0768 Mpx) ó 640 x 480 (0.3 Mpx)
Tabla 3. Ca ac e ís icas de las cáma as
Como podemos e los FPS son mayo es en el PS eye, sob e odo a baja esolución, es a di e encia se
debe p incipalmen e al d i e ins alado, modi icado po la comunidad de usua ios de Linux pa a
aumen a al máximo posible la elocidad de mues eo de la cáma a.
La esolución de 320x240 píxel hemos comp obado en el labo a o io que es más que su icien e pa a
nues o p opósi o de de ección de mo imien o, además con es a esolución ob enemos un iempo de
cálculo mucho meno (4 eces meno ) que con la esolución de 640x480 píxel, en el caso de que e
u iliza la cáma a pa a o os p opósi os como g abación de ídeo o mapeado median e imagen
u iliza íamos la máxima esolución pe o en es e caso es p io i a ia la elocidad de cálculo.
Con los da os que enemos concluimos que la cáma a PS eye se ajus a mejo a nues os p opósi os y
es po eso que la elegimos como cáma a a u iliza en el abajo.
Con el obje i o de educi peso y mejo a la in eg ación ísica de la cáma a en el conjun o del
quad o o , le qui amos odos los elemen os de p o ección innecesa ios e ins alamos la cáma a en la
pa e in e io , al y como se mues a en las siguien es imágenes:
18
Ilus ación 18. Cáma a PS eye sin ca casa
Ilus ación 19. De alle del mon aje de la cáma a (líneas
ojas = ejes X-Y quad o o )
2.4. Desa ollo del algo i mo de medición y con ol
El desa ollo del algo i mo de con ol se puede di idi en es e apas:
 Medición
 T a amien o de la in o mación ( il ado)
 Con ol
P ocedemos a explica el desa ollo de cada una de ellas.
2.4.1. Algo i mos de medición
An es de explica el desa ollo y uncionamien o de los algo i mos de medición cabe menciona que
se han desa ollado pa alelamen e dos algo i mos: un p ime algo i mo lo más gene al posible que
pudiese unciona en cualquie en o no, y o o más especí ico pa a en o nos isuales semi-
con olados. Es os dos algo i mos se án explicados de o ma independien e pues o que son muy
di e en es an o en con enido como en base eó ica, coincidiendo solo en el esul ado que o ecen,
una medida de posición. Se desa olló ambién una a ian e del algo i mo de cí culos pa a de ec a un
colo de e minado, aunque el uncionamien o es muy simila .
Po o a pa e, debemos no a que, pa a una mejo comp ensión de los p ocesos que se ejecu an en
el algo i mo, en es e documen o hemos incluido di e sas imágenes mos adas po pan alla pe o que
en la ejecución inal del p og ama no se mues an pa a no alen iza la ejecución del p og ama.
Además, no hemos de ol ida la na u aleza indi ec a de es a medida de posición, nues o senso es
una cáma a de la cual ob enemos imágenes y no es has a después de un p ocesamien o in e no de
es as imágenes que se ob iene la medida de posición, es o hace que los pe iodos de mues eo
ob enidos al aplica es os mé odos sean bas an e supe io es a los ob enidos con senso es di ec os
como la IMU.
19
Po úl imo, deci que se ha in en ado en la medida de lo posible que es e apa ado sea lo más cla o
posible educiendo al mínimo necesa io la inclusión de agmen os de código.
Pasemos sin más demo a al desa ollo del p ime o de los algo i mos:
Algo i mo gene al
Es e p og ama pa e de las siguien es suposiciones:
 No enemos ninguna in o mación p e ia del en o no isual en el que se mue e el
quad o o
 La di e encia de posición en e dos ames es pequeña (es o se puede consegui con una
baja elocidad de mo imien o del quad o o o con una al a ecuencia de mues eo de la
cáma a).
 En la imagen hay elemen os “ca ac e ís icos” de ácil seguimien o (un elemen o
ca ac e ís ico es cualquie pa e de la imagen que se di e encie del es o, po ejemplo los
bo des de obje os, más adelan e se apo a án ejemplos isuales de es os elemen os)
El p oceso de medición de la posición se di ide en a ios subp ocesos encadenados que aquí
exponemos en el mismo o den en el que se ejecu an en el p og ama diseñado inicialmen e, incluimos
en es e pun o un diag ama de lujo con la in ención de p opo ciona una isión global del p og ama
como conjun o ( ecupe a emos es a igu a con in de cla i ica los sub-p ocesos y modi icaciones
sucesi as):

20
INICIO
TOMA IMAGEN
BÚSQUEDA DE
PTS1 EN IMG2
IMG1
PTS1
TOMA IMAGEN
SELECCIÓN DE PUNTOS
IMG1
IMG2
PTS1
IMG2
PTS2
DIFERENCIA
DE PUNTOS
PTS1
PTS2
DESP
¿EN
EJECUCIÓN?
ACTUALIZACIÓN
IMG1 = IMG2
IMG2
IMG1
FIN
SÍ
NO
FUNCIÓN
VARIABLE
DECISIÓN
FLUJO DE PROCESOS
FLUJO DE DATOS
LEYENDA
Ilus ación 20. Diag ama de lujo del algo i mo gene al inicial
21
1. Selección de pun os
Conside ando que enemos la cáma a y su d i e co espondien e co ec amen e con igu ados, el
p ime paso de nues o algo i mo es la selección de pun os. A g andes asgos la unción de selección
de pun os debe oma como en ada una imagen y de ol e una se ie de pun os de in e és
seleccionados en esa imagen:
Ilus ación 21. De alle del diag ama de lujo (selección de pun os)
Desca amos di ec amen e la opción de selecciona manualmen e los pun os po la len i ud del
p oceso y po que el obje i o es au oma iza el algo i mo.
Pasamos pues a conside a las di e en es opciones que nos o ece OpenCV pa a la selección de pun os:
En p ime luga se p obó a selecciona pun os a in e alos egula es de píxeles de la imagen, a modo
de cuad ícula, és e mé odo más a de p oba ía se el más ápido (ya que no equie e ope aciones
in e nas de selección) pe o ambién el más imp eciso ya que los pun os seleccionados no enían
ninguna ga an ía de se óp imos pa a el seguimien o.
Pos e io men e se p oba on es unciones in e nas de OpenCV diseñadas especí icamen e pa a la
selección de pun os: GoodFea u esToT ack, FAST y ORB. Los undamen os de es as es unciones son
simila es, a an de busca , median e un p oceso i e a i o, los pun os que más con as an con su
en o no, asignando a cada pun o una pun uación, luego son seleccionados y almacenados los pun os
con mayo pun uación. La di e encia en e ellos es á en los mé odos ma emá icos in e nos que ejecu a
cada unción que hacen a ia los esul ados no ablemen e.
Pa a elegi uno de los cua o algo i mos p opues os, omamos como a iables impo an es las
siguien es: el iempo que consume la unción y los pun os que no se han encon ado (pun os
“pe didos”) en e dos ames. Es a úl ima a iable se puede conside a un indicado de la “calidad” de
los pun os encon ados, si los pun os seleccionados son buenos candida os pa a el seguimien o,
debe ían se ácilmen e encon ados en el ame siguien e, del mismo modo, si los pun os elegidos no
son los adecuados, és os no se encon a án.
El expe imen o de selección se ealizó p ime o con la cáma a en eposo y luego con un lige o
mo imien o hacia un lado desplazando la imagen unos 100 píxel en ho izon al en un pe íodo de
ap oximadamen e 10 segundos, es deci , a una elocidad de más o menos 10 píxel/segundo que no
debe ía se p oblema pa a unas unciones que se ejecu an en el o den de los milisegundos. Como
condición ex a se limi ó la can idad de pun os a encon a a 50 pun os, una can idad su icien e pa a
e ec ua el cálculo de posición sin comp ome e la elocidad del p ocesado .
Los esul ados de la compa ación se mues an en el g á ico siguien e:
PTS1
SELECCIÓN DE PUNTOS
IMG1
22
Ilus ación 22. G á ica de compa ación de algo i mos de selección de pun os
Ilus ación 23. Selección de pun os a in e alos egula es
Ilus ación 24. Selección de pun os con GoodFea u es
Ilus ación 25. Selección de pun os con FAST
Ilus ación 26. Selección de pun os con ORB
IDEAL
In e alos
Regula es
GoodFea u es
ToT ack
FAST
ORB
0
5
10
15
20
25
30
35
0100 200 300 400
Pun os "pe didos"
Tiempo (ms)
23
En la g á ica an e io (Ilus ación 19) podemos e que de los algo i mos expues os, el que más se
ap oxima al algo i mo ideal es el ORB que o ece la mejo elación calidad/ iempo. Es po eso que
incluimos es a unción en nues o p og ama pa a la selección de pun os.
Cabe menciona que cada una de las unciones an e io es admi e una se ie de pa áme os ela i os al
p oceso de selección de pun os que ambién ue on mues eados pa a ob ene la máxima e iciencia y
que aquí sólo se mues an los mejo es esul ados de cada unción.
2. Seguimien o de pun os
Una ez elegidos los pun os a segui , pasamos a busca los en una nue a imagen pa a calcula el
desplazamien o que han su ido. La unción que buscamos debe unciona del modo siguien e:
Ilus ación 27. De alle del diag ama de lujo (seguimien o de pun os)
Tomando como a gumen os la imagen siguien e y los pun os encon ados en la imagen an e io , debe
de e mina la nue a posición de los pun os.
Consul ando la documen ación online de OpenCV la opción suge ida más popula es el algo i mo de
lujo óp ico desa ollado po Lucas-Kanade basado en pi ámides (ap oximaciones sucesi as
educiendo la esolución de la imagen has a llega a la imagen o iginal), implemen ado en la unción
“calcOp icalFlowPy LK”. Analicemos aho a el uncionamien o de es e algo i mo, como a gumen os
admi e a ios pa áme os pe o los más impo an es son:
 Tamaño de la en ana de búsqueda: de e mina el ancho y al o de la en ana al ededo del
pun o o iginal en la cual se a a busca el pun o desplazado, un amaño g ande mejo a la
p ecisión pe o puede comp ome e la elocidad de cálculo. Tomamos como solución de
comp omiso un amaño de 15x15 píxel.
 Núme o de pi ámides: can idad de eces que se educe la esolución de la imagen pa a los
sucesi os cálculos del lujo óp ico. Un alo de 3 mejo a el endimien o del algo i mo espec o
a no aplica educción, alo es supe io es no mejo an la e iciencia e incluso llegan a educi la,
es po es o que asignamos 3 ni eles de pi ámides a nues o algo i mo.
 C i e ios de e minación: de e minan cuando la unción debe pa a de busca el lujo óp ico
pa a un pun o de e minado, en nues o caso hemos dejado los alo es po de ec o po que
uncionan co ec amen e, es os hacen que el algo i mo pa e si el e o de la solución
(di e encia en e una i e ación y la an e io ) es meno a 0.3 píxel o si se han hecho más de 20
i e aciones.
De es os es a gumen os el más no able a ni el de endimien o es el amaño de la en ana,
mos amos aquí dos ejemplos de cálculo del lujo óp ico omando amaños de en ana di e en es, el
lujo óp ico se calcula en e la imagen mos ada en los ejemplos de selección de pun os y o a e sión
de esa misma imagen, desplazada un poco hacia la izquie da y hacia a iba usando un p og ama de
edición de imágenes:
BÚSQUEDA DE
PTS1 EN IMG2
PTS1
IMG2
PTS2
30
Algo i mo especí ico (colo )
Se ha diseñado ambién una e ce a opción de medida simila al algo i mo de los cí culos que elimina
algunos de los p oblemas de és e a cos a de un lige o aumen o del iempo de compu ación po u iliza
la imagen en colo en luga de en escala de g ises.
Es e algo i mo unciona exac amen e igual que el an e io pe o en luga de busca un cí culo, busca
algún elemen o de un colo de e minado, lo aísla y calcula su cen o geomé ico que se oma como
medida de posición. La búsqueda de colo se ealiza haciendo una simple compa ación de los alo es
de los es canales de colo (HSV) asignando un 1 a un píxel si sus colo es se encuen an den o del
ango deseado, y un 0 si no se cumple es a condición.
Después a la imagen bina ia ob enida, se le hace un a amien o pa a elimina el posible uido y se le
calculan los momen os de la imagen con los cuales se ob iene el cen o del obje o de colo .
El p oceso se puede e en las siguien es imágenes (se ha elegido el colo e de pa a la búsqueda):
Ilus ación 38. Imagen o iginal
Ilus ación 39. Imagen as compa ación y a amien o
Ilus ación 40. Resul ado (pun o ojo = cen o)

31
2.4.2. T a amien o de la medida
La medida ob enida con los algo i mos an e io es no se puede u iliza pa a hace el con ol de o ma
di ec a ya que p esen a uido, in e e encias p o enien es de o as a iables, y no es á en las unidades
adecuadas. Es os ac o es jus i ican la necesidad de un a amien o de la medida.
En p ime luga , nues a medida se encuen a en píxel, lo que buscamos es una medida del
desplazamien o en me os (o cen íme os), con es e in es necesa io calib a la cáma a ob eniendo la
co espondencia en e dis ancias en la imagen y dis ancias eales. Desg aciadamen e la calib ación
implemen ada en OpenCV solo pe mi e ob ene es a co espondencia pa a una posición es á ica de la
cáma a, las condiciones de posición y o ien ación de nues a cáma a es án ligadas a las del quad o o
y po lo an o son muy a iables, po lo an o la calib ación de OpenCV esul a in iable así que pasamos
a hace una calib ación sencilla u ilizando un modelo de cáma a básico.
Pa a calib a la cáma a omamos el modelo “pin-hole” que conside a la cáma a como un pun o del
espacio con un ángulo de isión cons an e, con es e ángulo y la al u a de la cáma a como únicos
pa áme os, podemos (aplicando algunas simpli icaciones) calcula el ac o de con e sión de medidas
imagen- ealidad. P ocedemos a expone el expe imen o de calib ación:
Ilus ación 41. Modelo eó ico de la cáma a
Ilus ación 42. Imagen de ejemplo pa a la calib ación
La simpli icación que conside amos pa a es a calib ación es que la cáma a es á si uada de o ma que
el plano de la imagen ( e Ilus ación 41) es pa alelo al suelo, es o hace que las medidas omadas sean
di ec amen e p opo cionales con las medidas eales. La simpli icación es azonable siemp e que los
ángulos de o ación sean pequeños, es a hipó esis se ha conside ado ya en la deducción del modelo
eó ico y se ha comp obado que es co ec a.
La ejecución del expe imen o consis e en oma una imagen de un obje o de longi ud conocida, a una
dis ancia ambién conocida y cumpliendo la simpli icación expues a an e io men e. Conside amos el
iángulo na anja del modelo de la Ilus ación 41, que ampliamos aquí:
32
Ilus ación 43. T iángulo ex aído del modelo
Median e igonome ía simple, es ablecemos una elación en e las 3 a iables impo an es:
an𝐹𝑂𝑉
2=𝑥
2∗𝑧
Ecuación 6. Ecuación modelo 1
Sabiendo además que la medida 𝑥 es á ep esen ada en la imagen po 320 píxel podemos es ablece
la elación me os/píxel median e la siguien e ecuación:
𝑚
𝑝í𝑥=2∗ an𝐹𝑂𝑉
2
320 ∗𝑧
Ecuación 7. Ecuación modelo 2
Queda así es ablecida la elación en e me os eales y píxeles de imagen que solo depende de la al u a
𝑧, po úl imo hemos de de e mina el ángulo de isión (FOV) que es una ca ac e ís ica de la cáma a.
Pa a es o omamos a ias imágenes en las condiciones especi icadas y calculamos el alo :
𝐹𝑂𝑉=2∗a c an( 𝑥
2∗𝑧)≈62.5 º
Ecuación 8. Ecuación modelo 3
Hecho es o enemos un modelo sencillo y con la p ecisión su icien e pa a aplica lo al quad o o .
A modo de ejemplo y alidación mos amos aquí el esul ado de aplica el modelo calculado al ejemplo
de ayec o ia mos ado en la Ilus ación 33, las ca ac e ís icas ísicas del expe imen o ue on
egis adas en el momen o de su ejecución con obje o de aplica más a de el modelo expues o:
𝑭𝑶𝑽
𝟐
𝒛 (𝒂𝒍𝒕𝒖𝒓𝒂)
𝒙
𝟐 (𝒎𝒆𝒅𝒊𝒅𝒂 𝒄𝒐𝒏𝒐𝒄𝒊𝒅𝒂)
33
Ilus ación 44. Validación del modelo con ayec o ia ejemplo
En ojo apa ece la ayec o ia ideal y en azul la ayec o ia medida y escalada, al se el expe imen o
ealizado a una al u a de e minada, el modelo únicamen e ealiza un escalado de los ejes pa a
ajus a los a las dimensiones eales.
Du an e la alidación del modelo p e iamen e expues o, se de ec ó o o p oblema que a ec aba a la
medida: la in luencia de la elocidad angula .
La cáma a se puede conside a como un obse ado ligado al sis ema ísico del quad o o , po lo an o
mide la elocidad ela i a del suelo espec o a sí misma, es a medida es una medida compues a,
incluye an o la elocidad eal en x o y del quad o o , como la elocidad co espondien e al gi o.
Ilus ación 45. Composición de elocidades en X
Ecuación 9. Composición de elocidades en X
Pa a e i a es e e o simplemen e co egimos la medida del lujo óp ico con las medidas de elocidad
angula y al u a p o enien es de o os senso es (IMU y ul asonidos o ba óme o). En un p ime
𝐹𝑂𝑥=−(𝑣𝑥+𝜔𝑦∗𝑧)
34
momen o se in en ó co egi con la medida de elocidad angula que p opo ciona la IMU, el esul ado
no ue bueno y po eso se pasó a u iliza la medida de o ien ación (ángulo) co igiendo sus cambios.
La ex ensión de es a co ección a pun os que no se encuen en inmedia amen e debajo del quad o o
es di ec a ya que, pa a un gi o Wy, odos los pun os que se encuen en en el plano del suelo end án
una componen e de elocidad en X del mismo módulo, y es a componen e es lo que medimos con FO.
Con las co ecciones aplicadas pod íamos, con el algo i mo gene al, ob ene la posición del quad o o
espec o a un o igen a bi a io (pun o de a anque del p og ama) suponiendo un ángulo yaw cons an e
median e la ecuación de la posición expues a an e io men e (Ecuación 4). Es o no iene po qué se
así ya que el ángulo yaw puede a ia , así pues modi icamos la ecuación de la posición pa a adap a la
a un ángulo yaw a iable del siguien e modo:
𝑃𝑂𝑆𝑁=∑𝐹𝑂(𝑐𝑜𝑟𝑟)𝑖
𝑁
𝑖=1 +(𝑥0
𝑦0)𝜓 𝑣𝑎𝑟𝑖𝑎𝑏𝑙𝑒
→
𝑃𝑂𝑆′𝑁=∑( cos𝜓𝑖sin𝜓𝑖
−sin𝜓𝑖cos𝜓𝑖)∗𝐹𝑂(𝑐𝑜𝑟𝑟)𝑖
𝑁
𝑖=1 +(𝑥0
𝑦0)
Ecuación 10. Ecuación modi icada de la posición (yaw a iable)
En es a ecuación simplemen e se ha aplicado una o ación del sis ema de e e encia pa a que el
esul ado sea siemp e la posición en coo denadas globales.
Una ez aplicadas las co ecciones desc i as, podemos a i ma que la medida que ob enemos es una
medida de posición absolu a en x-y del quad o o . Aho a bien, como oda medida, p esen a uido, al
cual además le hemos añadido el uido p esen e en las medidas de al u a y o ien ación al usa sus
alo es pa a las co ecciones an e io es.
Es po es o que la medida equie e un il ado p e io a su uso, con es e in implemen amos un il o
paso bajo pa a in en a echaza el uido median e la siguien e ecuación:
𝑥𝑘=𝑧𝑘∗𝛼+𝑥𝑘−1∗(1−𝛼)
Ecuación 11. Fil o paso bajo
Donde 𝑥𝑘 es la es imación de la posición en el ins an e k, 𝑧𝑘 es la medida de posición del senso y 𝛼 es
el pa áme o del il o, que puede a ia en e 0 y 1, alo es ce canos al 0 implican un il ado muy
ue e, igno ando p ác icamen e al senso , y alo es ce canos al 1 anulan el il ado dejando sin
modi ica la medida p o enien e del senso . Mos amos aquí medidas de posición y elocidad
ob enidas median e es e il ado, las medidas es án en píxeles pa a a a de o ma independien e al
il ado y a las co ecciones an e io es:
35
Ilus ación 46. G á ica de la posición X-Y (sin il a )
Ilus ación 47. G á ica de elocidad il ada en X (𝛼=0.05)
Hemos adap ado en nues o p og ama ambién un il o de Kalman diseñado en abajos an e io es
po los compañe os del labo a o io. El undamen o de es e algo i mo se basa en gene a un modelo
de nues o sis ema que u ilizamos pa a p edeci la siguien e medida y, una ez omada, la medida eal
se u iliza pa a co egi el modelo gene ado.
Ilus ación 48. Diag ama del il o de Kalman
En la imagen apa ece el uncionamien o del il o, los pa áme os que in e esan son las en adas y
salidas. La en ada 𝑧𝑘 es la medida p o enien e del senso , y la salida 𝑥𝑘 es la es imación del es ado
de la a iable in en ando minimiza el e o de medida. Las en adas 𝑄𝑘 y 𝑅𝑘 son las ma ices de
co a ianzas de los uidos p esen es en el p oceso y en el senso espec i amen e, y de e minan el peso
que se le da á a las di e en es a iables y mediciones en el modelo c eado, a mayo co a ianza, meno
peso.
-50
0
50
100
150
200
-300 -200 -100 0
Pos Y (píx)
Pos X (píx)
-100
-80
-60
-40
-20
0
20
40
60
80
-40 60 160 260
Velocidad X (pix/s)
I e aciones

36
2.4.3. Desa ollo del algo i mo de con ol
Una ez omada la medida de posición, se pasa al desa ollo del con ol, pa a pode con ola la
posición, acudimos al modelo eó ico (Ecuación 1) y obse amos que no enemos un con ol di ec o
de la posición (o de la acele ación) sino que hemos de diseña lo pasando po el con ol de o ien ación.
Explica emos el algo i mo de con ol pa a el eje X y el ángulo pi ch pe o la ex ensión al eje Y y el ángulo
oll se ía inmedia a (si conside amos ángulos pequeños). Pa imos del con ol de la o ien ación, que
iene la siguien e o ma:
Ilus ación 49. Diag ama de lujo del con ol de o ien ación
Donde e 𝜃 es la e e encia de pi ch, 𝑢𝜃 es la acción aplicada ( al y como apa ece en el modelo eó ico)
y 𝜃󰇘 es la espues a dinámica del sis ema, a la cual añadimos posibles pe u baciones no p esen es en
el modelo eó ico ( ien o, choques,…) y que luego es cap ada ( as un doble in eg ado ) po el senso
y e oalimen ada al sis ema de con ol.
El con ol que p oponemos pa a la posición X es el siguien e:
Ilus ación 50. Diag ama de lujo del con ol de la posición en X
Como se obse a en el diag ama, se man iene el con ol de o ien ación o iginal al cual se supe pone
un con ol de posición a a és de la a iable
e
𝜃𝑥
que hace la unción de “ac uación” indi ec a sob e
el sis ema quad o o .
En las p uebas ealizadas con el con ol de posición, los esul ados ob enidos no ue on buenos, el
con ol e a ines able, y el quad o o se mo ía de o ma e á ica, eaccionando a las medidas de
posición pe o sin log a con ola es as a iables, se llegó a la conclusión de que un con ol de posición
ac uaba con demasiado e a do.
También se obse ó que el quad o o empezaba a ene sus en ación su icien e pa a ola con los
mo o es a un 75% de su po encia máxima, es o hacía que el con ol no uese iable po que se llegaba
𝑃𝐼𝐷𝜃
𝑠𝑖𝑠𝑡𝑒𝑚𝑎
𝑞𝑢𝑎𝑑𝑟𝑜𝑡𝑜𝑟
𝑠𝑒𝑛𝑠𝑜𝑟 (𝐼𝑀𝑈)
1
𝑠2
e
𝜃
pe
𝜃󰇘
𝜃
𝜃󰇘
𝑢𝜃
𝜃󰇘
−
+
𝑃𝐼𝐷𝜃
𝑠𝑖𝑠𝑡𝑒𝑚𝑎
𝑞𝑢𝑎𝑑𝑟𝑜𝑡𝑜𝑟
𝑠𝑒𝑛𝑠𝑜𝑟 (𝐼𝑀𝑈)
1
𝑠2
𝑠𝑒𝑛𝑠𝑜𝑟 (𝐶𝑎𝑚)
1
𝑠2
𝑃𝐼𝐷𝑥
e
𝜃𝑥
pe
𝜃󰇘
𝜃
𝜃󰇘
𝑢𝜃
𝜃󰇘
−
𝑥󰇘
𝑥
e
𝜃
e
𝑥
−
+
+
+
pe
𝑥󰇘
𝑥󰇘
37
al lími e de po encia al suma la acción de con ol, así que se cambia on los mo o es po o os que
pe mi ían sus en a el quad o o a sólo un 60% de po encia, dando ma gen su icien e pa a la acción
de con ol.
En base a los esul ados del con ol de posición, se decidió pasa a un con ol de elocidad u ilizando
como medida el lujo óp ico di idido en e el pe iodo en e imágenes. Se ha usionado la medida de
elocidad p o enien e de la cáma a con la elocidad in eg ada de la acele ación de la IMU en un il o
de Kalman.
Mos amos aquí una medida omada en uelo con el algo i mo de con ol de la elocidad, en azul se
mues a la elocidad en el eje X, y en ojo, la e e encia de pi ch en iada al con ol de o ien ación:
Ilus ación 51. Resul ados de la acción p opo cional en uelo
En el uelo mos ado en la g á ica sólo se aplicó un con ol p opo cional pa a comp oba el co ec o
compo amien o y espues a a la medida. Los esul ados son sa is ac o ios ya que se consigue
man ene la elocidad en X del quad o o en un ango de ±0.3 m/s.
La acción de i a i a se implemen ó más a de pues la medida de acele ación de la IMU p esen aba
mucho uido, mos amos aquí el esul ado del il o paso bajo aplicado a la acele ación:
38
Ilus ación 52. Fil ado de la acele ación
Aun así, as el il ado, la acción de i a i a deses abilizaba el quad o o y se in en ó implemen a un
con ol PI, con an iwindup, con la inalidad de e i a la de i a a elocidades len as. Que ampoco
uncionó po la misma azón, c eaba ines abilidad en el quad o o .
Se obse ó du an e es as p uebas ambién un g a e e a do ( e g á ica in e io ) en la medida ob enida
de la cáma a a in e alos egula es (pa es planas de la g á ica azul). Es o di icul aba g a emen e el
con ol po deja de ene medida po pe iodos de 2-3 segundos, lo cual dis o sionaba mucho el
modelo elabo ado po el il o de Kalman (g á ica oja). Una ez solucionado es e cuello de bo ella
elacionado con la esc i u a en iche os se p ocedió a ol e a in en a ealiza el con ol.
Ilus ación 53. Re a do en la medida de isión
Los esul ados del con ol aplicado inalmen e se mues an en el apa ado siguien e.
39
3. Resul ados y conclusiones
Los esul ados ob enidos son los siguien es:
Ilus ación 54. Medida inal de elocidad
La g á ica es el esul ado de un con ol de elocidad p opo cional pe o sin e a do en la medida de
isión, las acciones p opo cional y de i a i a no se pudie on p oba po al a de iempo. Como se e,
el sis ema iende a la elocidad 0 aunque con oscilaciones po la simplicidad del con ol p opo cional.
Los expe imen os e i ican que el sis ema unciona acep ablemen e en in e io es y que pod ía se
aplicado como mecanismo de eme gencia (en caso de allo de la señal gps) en ex e io es pa a e i a la
pé dida del quad o o
T as la ealización de es e abajo, concluimos que la isión a i icial es una he amien a po en e y
poli alen e al y como hemos is o pues o que pe mi e calcula posiciones, elocidades e incluso la
o ien ación del quad o o . Pe o es a poli alencia iene un p ecio, que pagamos en las di icul ades de
ob ene una medida p ecisa y aislada, debido a los pa áme os de esolución, elocidad y uido de la
cáma a y a las múl iples in e acciones con o as a iables causadas po la na u aleza del sis ema de
isión empleado.
Mencionamos aquí algunas de las po enciales mejo as que se pod ían aplica a es e sis ema, las
azones de que no se hayan aplicado en la ejecución del abajo son p incipalmen e la limi ación de
ecu sos y iempo:
 Inclusión de una segunda cáma a (es e eo isión) pa a deja de depende de senso es
ex e nos.
 U iliza una cáma a (o más) ex e na al quad o o , eliminando así el ca ác e ela i o de la
medida.
 Pa a el sis ema con la cáma a in eg ada en el quad o o , añadi un sis ema de es abilización
dinámica pa a e i a in e acciones con la elocidad angula .
46
ge imeo day(& e, NULL); //calculamos la di e encia de iempos en e
ime sub(& e, & s, & d); //cap u as, es a pa e se puede ejecu a de
ge imeo day(& s, NULL); // o ma cíclica
pe iodo=(( loa ) d. _sec+1e-6*( loa ) d. _usec);
c Colo (imagen2,imagen2,CV_BGR2GRAY);//pasamos la imagen a escala de g ises
//den o de es e “i ” se seleccionan los pun os de in e és, “ ese eado ” es
//una a iable que se u iliza pa a decidi si se deben busca pun os o no
i ( ese eado ==1){ // ese eado = 1 indica que se deben busca pun os
co ne s1.clea (); //se limpian los pun os an e io es
//se buscan los pun os y se con ie en a un oma o adecuado pa a su
//uso (la unción ORB de uel e más da os de los necesa ios)
O bFea u eDe ec o de (100,1.2 ,8,31,0,2,0,31);
de .de ec (imagen1,keypoin s)
o (i=0;i<in (keypoin s.size());i++){
co ne s1.push_back(keypoin s[i].p );
}
}
i (co ne s1.size()>0){
ese eado = 0; //si se han encon ado pun os (el ec o co ne s1 no
//es á acío), se indica median e ese eado que no
//hace al a busca pun os
//Función de cálculo del lujo óp ico
calcOp icalFlowPy LK(imagen1,imagen2, //imágenes p e ia y ac ual
co ne s1,co ne s2, // ec o es de pun os p e ios y ac uales,
//co ne s2 es la salida de es a unción
s a us,e , //s a us indica los pun os encon ados
//e indica el e o de posición
Size(15,15),3, //Size(x,y) indica el amaño de la en ana
//de búsqueda y el 3 es el núme o de pi ámides
//c i e ios de e minación: 20 i e aciones o e o in e io a 0.3
Te mC i e ia(Te mC i e ia::COUNT+Te mC i e ia::EPS,20,0.3));
o x=0; //inicializamos a iables de desplazamien o a 0

47
o y=0;
nb_depl=0;
//es e bucle calcula la media de los desplazamien os de los píxel
o (i=0;i<in (co ne s1.size());i++)
{
i (s a us[i]!=0)
{
o x = o x + (co ne s2[i].x-co ne s1[i].x);
o y = o y + (co ne s2[i].y-co ne s1[i].y);
nb_depl++;
}
}
//aquí se comp ueba si no se han pe dido odo los pun os
i (nb_depl>0){
o x=o x/nb_depl;
o y=o y/nb_depl;
}
//calculamos la elocidad (de i ada de la posición) il ada pa a
//la acción de i a i a del con ol
elx = (1-0.05)* elx + 0.05*(o x/pe iodo);
ely = (1-0.05)* ely + 0.05*(o y/pe iodo);
//condición de ese , que se pie da más de un 75% de los pun os
i ( loa (nb_depl)/ loa (co ne s1.size())<0.25)
ese eado =1;
//ac ualización de imágenes y ec o es
imagen2.copyTo(imagen1);
s d::swap(co ne s2, co ne s1);
//acumulación del desplazamien o pa a ob ene posición
suma[0] += o x;
suma[1] += o y;
//imp esión a iche o
48
lush( ich lujo);
p in ( ich lujo,"%.6 %+.5 %+.5 n",pe iodo,suma[0],suma[1]);
}
else
imagen2.copyTo(imagen1); //en caso que no se encuen en pun os,
//simplemen e se ac ualiza la imagen
}
Código del algo i mo especí ico de medición (algo i mo de cí culos)
Es e código es á basado en el código de ejemplo p esen e en la página web:
h p://docs.openc .o g/doc/ u o ials/imgp oc/img ans/hough_ci cle/hough_ci cle.h ml
Inicialización:
VideoCap u e cam(0);
cam.se (CV_CAP_PROP_FRAME_HEIGHT,320);
cam.se (CV_CAP_PROP_FRAME_WIDTH,240);
Bucle p incipal:
while(1){
// oma de imagen y a amien o de la misma (escala de g ises y desen oque)
cam >> s c;
c Colo ( s c, s c_g ay, CV_BGR2GRAY );
GaussianBlu ( s c_g ay, s c_g ay, Size(9, 9), 2, 2 );
//aplicación de la ans o mada de Hough pa a encon a cí culos
HoughCi cles( s c_g ay, //imagen
ci cles, // ec o de cen os y adios
CV_HOUGH_GRADIENT, //mé odo u ilizado pa a la de ección
1, //p opo ción in e sa de esolución
s c_g ay. ows/8, //dis ancia mínima en e cen os
200, //ma gen pa a de ección de cí culos
100, //ma gen pa a el il o canny (de ección de bo des)
0,0); // adios minimo y máximo de los cí culos
//adap ación de los da os al o ma o de uso
i (in (ci cles.size())>0){
o (i=0;i<in (ci cles.size());i++){
Poin cen e (c Round(ci cles[i][0]), c Round(ci cles[i][1]));
cen os[i]=cen e ;
}
//si se encuen an 2 cí culos, calcula la inclinación y el cen o
49
i (ci cles.size()==2){
loa yaw;
yaw=a an( loa (cen os[1].y-cen os[0].y)/ loa (cen os[1].x-
cen os[0].x));
lush( ich lujo);
p in ( ich lujo,"% % % n",
(cen os[1].x+cen os[0].x)*0.5-160,
-(cen os[1].y+cen os[0].y)*0.5+120,
yaw*180.0/PI);
}
//si se encuen a solo un cí culo, calcula la posición de es e
i (ci cles.size()==1){
lush( ich lujo);
p in ( ich lujo,"%d %d n",cen os[0].x-160,-cen os[0].y+120);
}
}
Es os algo i mos han sido implemen ados como h eads que se lanzan desde un obje o dedicado pa a
adecua se a la es uc u a del p og ama p incipal del quad o o .
Código del algo i mo especí ico de medición (algo i mo de colo )
Es e código es á basado en el código de ejemplo p esen e en la página web:
h p://openc -s .blogspo .com.es/2010/09/objec -de ec ion-using-colo -sepe a ion.h ml
Po se muy simila al algo i mo de cí culos se explica án solo aquellas unciones que no hayan sido
explicadas ya.
Inicialización:
VideoCap u e cam(0);
cam.se (CV_CAP_PROP_FRAME_HEIGHT,320);
cam.se (CV_CAP_PROP_FRAME_WIDTH,240);
Bucle p incipal:
while(1){
cam >> s c;
ge imeo day(& e, NULL); ime sub(& e, & s, & d);
ge imeo day(& s, NULL);
mediape =(( loa ) d. _sec+1e-6*( loa ) d. _usec);
c Colo (s c, s c, COLOR_BGR2HSV); //pasamos la imagen a HSV
inRange(s c, //imagen o igen
Scala (iLowH, iLowS, iLowV), //lími e in e io del ango
50
Scala (iHighH, iHighS, iHighV), //lími e supe io del ango
s c); //imagen des ino (bina ia)
// a amien o de la imagen (elimina uido)
e ode(s c, s c, ge S uc u ingElemen (MORPH_ELLIPSE, Size(5, 5)) );
dila e( s c, s c, ge S uc u ingElemen (MORPH_ELLIPSE, Size(5, 5)) );
dila e( s c, s c, ge S uc u ingElemen (MORPH_ELLIPSE, Size(5, 5)) );
e ode(s c, s c, ge S uc u ingElemen (MORPH_ELLIPSE, Size(5, 5)) );
//cálculo de momen os
Momen s oMomen s = momen s(s c);
double dM01 = oMomen s.m01;
double dM10 = oMomen s.m10;
double dA ea = oMomen s.m00;
//si se encuen a algún elemen o de á ea su icien e, calcula su cen o
i (dA ea > 100)
{
posX = -(dM10 / dA ea)+160;
posY = (dM01 / dA ea)-120;
}
}
Es os algo i mos han sido implemen ados como h eads que se lanzan desde un obje o dedicado pa a
adecua se a la es uc u a del p og ama p incipal del quad o o .
Modi icaciones pa a adap a la medida
//adap ación de píxel a me os (coe icien = 0.00379259)
Measu eInMe es = Measu eInPixels*coe icien *al u a;
//co ección pa a echaza medidas alsas po gi os
Measu e = Measu e – sin(Angle)*al u a;
Fil ado
//implemen ación del il o paso bajo
Measu eFil e ed = al a*Measu e + (1-al a)*P e iousMeasu e;
No se incluye el código del il o de Kalman po que ha sido desa ollado p incipalmen e po Albe o
Cas illo, compañe o del labo a o io, y no p ocede anexa lo aquí. Solo menciona que es e il o ejecu a
una usión de da os combinando las medidas de la cáma a y la IMU, consiguiendo así una medida más
iable y p ecisa.
51
Con ol
El con ol PID, ya sea de posición o elocidad se ha implemen ado del siguien e modo:
accPx = kPx * ( e x – x); //p opo cional x
accDx = kDx * (-dx); //de i a i a x
accIx = accIx + kIx * ( e x – x); //in eg al x
e Pi chX = accP + accD + accI; //acción o al x
accPy = kPy * ( e y – y); //p opo cional y
accDy = kDy * (-dy); //de i a i a y
accIy = accIy + kIy * ( e y – y); //in eg al y
e RollY = accPy + accDy + accIy; //acción o al y
e Pi chX = sa u acion( e Pi chX,-5,5); //sa u amos las acciones an o en
e RollY = sa u acion( e RollY,-5,5); //x como en y pa a e i a pelig o