Equa ion Chap e 1 Sec ion 1
P oyec o Fin de G ado
G ado en Ingenie ía Elec ónica, Robó ica y
Meca ónica
Simulación en Gazebo de quad o o pa a anspo e
de ca ga colgan e
Dep. Ingenie ía de Sis emas y Au omá ica
Escuela Técnica Supe io de Ingenie ía
Uni e sidad de Se illa
Au o : Pablo Gómez-Camb one o Ma ín
Tu o : Manuel Gil O ega Lina es
Manuel Va gas Villanue a
Se illa, 2017
iii
P oyec o Fin de G ado
G ado en Ingenie ía Elec ónica, Robó ica y Meca ónica
Simulación en Gazebo de quad o o pa a
anspo e de ca ga colgan e
Au o :
Pablo Gómez-Camb one o Ma ín
Tu o :
Manuel Gil O ega Lina es
P o eso i ula
Manuel Va gas Villanue a
P o eso i ula
Dep. Ingenie ía de Sis emas y Au omá ica
Escuela Técnica Supe io de Ingenie ía
Uni e sidad de Se illa
Se illa, 2017
P oyec o Fin de Ca e a: Simulación en Gazebo de quad o o pa a anspo e de ca ga colgan e
Au o :
Pablo Gómez-Camb one o Ma ín
Tu o :
Manuel Gil O ega Lina es
Manuel Va gas Villanue a
El ibunal nomb ado pa a juzga el P oyec o a iba indicado, compues o po los siguien es miemb os:
P esiden e:
Vocales:
Sec e a io:
Acue dan o o ga le la cali icación de:
Se illa, 2017

El Sec e a io del T ibunal
ii
A mi amilia
A mis maes os
A mis amigos
ix
Ag adecimien os
En p ime luga , me gus a ía ag adece a mis u o es, Manuel Va gas y Manuel Gil, la con ianza deposi ada en
mi, pe mi iendo mi inco po ación a un p oyec o como es e, el cual ha ampliado en g an medida mis
conocimien os. También ag adezco oda la a ención y los ecu sos p es ados du an e el desa ollo del mismo.
G acias a compañe os como Ma io Jimenez y Al edo, po habe se ido de apoyo en de e minados aspec os
del abajo.
Sob e odo, g acias a Jesús Lozano, a quién conside o un magní ico p o esional en la ma e ia, po la g an ayuda
p es ada, po su paciencia y po ab i me los ojos a un mundo an ex enso como es el de los ehículos aé eos no
ipulados.
G acias a mi amilia, quienes desde el comienzo de es os du os cua o años han sabido apoya me, guia me y
ayuda me a oma las decisiones que al inal de es e ayec o conside o han sido las mejo es. G acias po siemp e
es a ahí y po enseña me a nunca da me po encido.
Finalmen e, ag adece ambién a mis amigos, a los de siemp e, po anima me como solo ellos saben hace , y a
los que han ido su giendo en el anscu so de es a e apa uni e si a ia, po se an ieles compañe os de iaje y
hace más lle ade os odos esos momen os complicados.
Pablo Gómez-Camb one o Ma ín
Escuela Técnica Supe io de Ingenie ía
Se illa, 2017
7 In e az de Con ol P opia 47
7.1. Componen es del p og ama 47
7.2. In e az g á ica 49
8 Con ol 53
8.1. Con igu ación del código del p og ama 53
8.2. Modo LOITER 55
8.3. De ección de la ca ga 61
8.4. PID pa a con ol de ca ga 66
9 Conclusión 67
Índice de Tablas 69
Índice de Figu as 71
Índice de Comandos 73
Índice de Códigos 75
Bibliog a ía 77

x ii
Índice
Ag adecimien os ix
Resumen xi
Abs ac xiii
Índice Ab e iado x
Índice x ii
No ación xix
1 In oducción 1
2 So wa e 3
2.1. E le-Cop e 3
2.2. E le-B ain 4
2.3. A dupilo 4
2.4. So wa e In The Loop (SITL) simula o 5
2.5. Robo Ope a ing Sys em (ROS) 5
2.5.1 Esquema de uncionamien o 5
2.6. Gazebo 6
2.7. MAVROS 7
2.7.1 MAVLink 7
3 P epa ación del En o no 9
3.1. P oceso de ins alación 9
3.2. Ca ac e ís icas del en o no mon ado 10
3.3. P ime a p ueba 11
4 Simulación del Op ical Flow 15
4.1. Op ical Flow y LIDAR 15
4.2. P ime mé odo 15
4.2.1 PX4 de Pixhawk 16
4.2.2 XML 16
4.2.3 Fo ma o SDF (Simula ion Desc ip ion Fo ma ) 17
4.2.4 Fo ma o URDF (Uni e sal Robo ic Desc ip ion Fo ma ) 19
4.2.5 Conclusiones del p ime mé odo 21
4.3. Segundo mé odo 22
4.3.1 A chi o lida _senso .u d .xac o 22
4.3.2 A chi o gene ic_came a.u d .xac o 24
4.3.3 A chi o e lecop e .xac o 25
4.3.4 Resul ado y conclusiones del segundo mé odo 26
5 C eación del Mundo de T abajo 29
5.1. A chi os de una simulación 29
5.1.1 A chi os .wo ld 29
5.1.2 A chi os .u d , .xac o y .sd 29
5.1.3 A chi os .launch 29
5.2. Pla a o ma 30
5.2.1 Modi icación de las colisiones del d one 32
5.3. Ca ga 34
5.3.1 P ime bloque: modelado ísico e ine cial de la ca ga 34
5.3.2 Segundo bloque: modelado del compo amien o dinámico de la ca ga 36
5.4. Modi icaciones necesa ias en el es o del código 38
5.4.1 Modi icacion del a chi o emp y.wo ld 38
5.4.2 Modi icacion del a chi o e lecop e .xac o 38
5.4.3 Modi icación del a chi o e lecop e _spawn.launch 39
5.5. Resul ados y conclusiones 39
6 Comunicación 41
6.1. Con igu ación inicial 41
6.2. Comunicación po e minal 42
6.3. Comunicación usando QG oundCon ol 43
7 In e az de Con ol P opia 47
7.1. Componen es del p og ama 47
7.2. In e az g á ica 49
8 Con ol 53
8.1. Con igu ación del código del p og ama 53
8.2. Modo LOITER 55
8.2.1 Es udio sob e el Pi ch y el Roll 56
8.2.2 Conclusiones de los expe imen os 57
8.3. De ección de la ca ga 61
8.4. PID pa a con ol de ca ga 66
9 Conclusión 67
Índice de Tablas 69
Índice de Figu as 71
Índice de Comandos 73
Índice de Códigos 75
Bibliog a ía 77
xix
No ación
Kd
Join
cm
𝜏
C m
𝜌𝑎𝑔𝑢𝑎
Link
E p
XML
FCU
Amo iguamien o
A iculación
Cen íme os
Cons an e de iempo
Cons ain o ce mixing
Densidad del agua
Enlace
E o educ ion pa ame e
Ex ensible Ma kup Language
Fligh Con ol Uni
FDM
K
𝐾𝑑
𝐾𝑖
𝐾𝑃
𝑔, g
GUI
Fligh Dynamics Model
Ganancia del Sis ema
Ganancia de i a i a
Ganancia in eg al
Ganancia p opo cional
G amos
G aphical Use In e ace
GCS
HTML
∆𝑌
∆𝑈
Kg
G ound Con ol S a ion
Hype Tex Ma kup Language
Inc emen o de la salida
Inc emen o de la señal de con ol
Kilog amos
LIDAR
L
𝑚𝑎𝑔𝑢𝑎
m
Ligh Imaging De ec ion and Ranging
Longi ud del hilo
Masa del agua
Me o
MAVLink
𝐼
𝐼𝐶𝑀
𝜋
Mic o Ai Vehicle Communica ion P o ocol
Momen o de ine cia
Momen o de ine cia en el cen o de masas
PI
𝑅𝑒𝑠𝑓
RTL
kp
Radio de la es e a
Re u n To Load
Rigidez de con ac o
ROS
SDF
SLAM
SITL
SGML
𝑡𝑠90%
𝑇𝑑
𝑇𝑖
URDF
Robo Ope a ing Sys em
Simula ion Desc ip ion Fo ma
Simul aneous Loca ion and Mapping
So wa e In The Loop
S anda d Gene alized Ma kup Language
Tiempo de subida (al 90%)
Tiempo de i a i e
TIempo in eg al
Uni e sal Robo ic Desc ip ion Fo ma
UAV
UDP
𝑉𝑒𝑠𝑓
Unmanned Ae ial Vehicle
Use Da ag am P o ocol
Volumen de la es e a
1 INTRODUCCIÓN
l mundo elacionado con los ehículos aé eos no ipulados ha su ido una expansión espec acula en los úl imos
años, aunque sus comienzos se emon an incluso a los años 20. Es e c ecimien o no se ha p oducido únicamen e
en un mismo ámbi o, sino que el abanico de ac i idades en los que se demandan ac ualmen e los UAVs
(Unmanned Ae ial Vehicle) es inmenso. Aunque sus p ime os usos ue on p incipalmen e aplicaciones
mili a es, pos e io men e se comenza on a u iliza en el en o no ci il, come cial y cinema og á ico. Dicho auge
gene ó a su ez la búsqueda de componen es más op imizados, como ba e ías que o eciesen una mayo
au onomía, ma e iales más lige os, a la ez que esis en es y la inclusión de sis emas de senso ización o de
he amien as de manipulación, como b azos obó icos.
Den o de la de inición de UAV se pueden encon a a ias amilias, pe o el p esen e documen o es á en ocado
a los d ones de ipo quad o o , que son uno de los ehículos aé eos no ipulados más no edosos en la ac ualidad.
Cons i uyen una he amien a pe ec a pa a ámbi os de igilancia, de ección e iden i icación de pe sonas, obje os
o incendios, g acias a su acilidad pa a inco po a cáma as y senso es de posicionamien o, además de su
capacidad pa a man ene se es ables en pleno uelo.
O a de sus g andes en ajas es que pueden llega a zonas de di ícil acceso pa a las pe sonas, de modo que no es
necesa io co e iesgos como los que se ienen en o es eléc icas, pa es in e io es de puen es, zonas ex e io es
de edi icios al os o zonas en las que ha ocu ido alguna ca ás o e na u al.
Además de es as aplicaciones, una de las más in e esan es es el anspo e de ca gas y me cancías. Hoy en día
ya exis en emp esas que ealizan en íos de paque es u ilizando es e sis ema, como Amazon, cuyo UAV se
E
Figu a 1.1 D one con cáma a inco po ada
Figu a 1.2 D one des inado a sal amen o

In oducción
2
2
mues a en la Figu a 1.3. Aunque, además de es a aplicación, más adelan e se pod ía u iliza es a habilidad pa a
el esca e de pe sonas del ma o de zonas a las que solo se puede accede po ai e.
F en e a es os obje i os en los que exis e una ca ga suspendida, el p oblema p incipal es el balanceo de la misma.
Pa a su solución es necesa ia la implemen ación de un con ol adecuado median e el cual el d one consiga
man ene es able la misma.
Es a úl ima aplicación es la que ha inspi ado el p esen e abajo, que se incluye den o de un g upo de p oyec os
de in de ca e a y de mas e , odos bajo la supe isión del Depa amen o de Sis emas y Au omá ica de la Escuela
Técnica Supe io de Ingenie ía y lle ados a cabo po di e en es alumnos, que implican un es udio del modelado
de un sis ema compues o po un d one más una ca ga suspendida, la simulación del mismo haciendo uso de la
he amien a de simulación Gazebo y su implemen ación ísica u ilizando pa a ello el d one que ecibe el nomb e
de E le-Cop e , pe enecien e a la emp esa E le Robo ics, y el cual ha sido adqui ido po el depa amen o an es
mencionado.
Conc e amen e, es e p oyec o se cen a en la segunda de las a eas nomb adas. Pa a es o se u iliza á el modelo
del E le-Cop e en Gazebo p opo cionado po E le Robo ics y se adap a á el mismo de mane a que incluya la
ca ga suspendida, así como odos los senso es de los que dispone el d one ísico. Dicha simulación se
implemen a á en un o denado apo ado po el depa amen o, y se a a á al mismo de la misma mane a a como
se a a ía el sis ema ísico, po lo que se es ablece á la conexión en e dicho o denado y un po á il, desde el
cuál se pod á con ola el UAV.
Una ez mon ado odo el en o no de simulación, se p oba á un modo de uelo del d one y se es udia á su
compo amien o con el in de implemen a un con olado pa a la ca ga, u ilizando pa a ello un p og ama en C
que pe mi e el manejo del ehículo de mane a emo a y la implemen ación de los con olado es pe inen es con
los que man ene es able la ca ga suspendida.
Se consegui á así un sis ema de simulación y p uebas cen ado en las ac i idades que se han mencionado
p e iamen e que pe mi i á u u os es udios en di e en es p oyec os de in es igación, así como la con inuación
de es e abajo como pa e de abajos de in de g ado o de in de más e ealizados po nue os alumnos, quienes
además pod án u iliza el p esen e documen o como guía y pun o de in o mación de allada en lo e e en e al
p oyec o a ado.
Figu a 1.3 D one de la emp esa Amazon pa a epa o de paque es
3
2 SOFTWARE
an sido nume osos los componen es so wa e necesa ios pa a la ealización de es e p oyec o, es po es o po lo
que se des ina á el p esen e capí ulo a ealiza una b e e p esen ación de los mismos, de allando sus
ca ac e ís icas más impo an es, así como el papel que han jugado en el abajo.
Es necesa io esal a que oda la in o mación incluida en los siguien es apa ados se encuen a ecogida en las
páginas web o iciales an o de E le Robo ics como en las de cada una de las he amien as en especi ico.
2.1. E le-Cop e
Es e es el nomb e que ecibe el quad o o u ilizado y cons i uye, po ello, el pila undamen al de es e p oyec o.
Aunque el ha dwa e del mismo no es a á p esen e en la elabo ación del abajo, es impo an e p esen a lo ya que
en la simulación lle ada a cabo el d one que apa ece es una éplica iel de dicho UAV.
Se a a del p ime d one in eligen e basado en el Sis ema Ope a i o Linux, lo que supone una g an no edad
den o de su campo. U iliza ma cos obó icos como ROS (Robo Ope a ing Sys em) y es á di igido po el ce eb o
a i ical E le-B ain 2, que se á desc i o pos e io men e.
Se incluyen en la siguien e abla algunas de sus ca ac e ís icas p incipales, ya que es as se e án e lejadas en el
modelo i ual del disposi i o.
Tabla 2.1 Ca ac e ís icas come ciales del E le-Cop e
Ca ac e ís ica
Desc ipción
Dimensiones
370 x 370 x 95 mm
Peso
878 g
Ca ga ú il
1 kg
Hélices
10 x 4.5 o 9.4 x 4.3 (pulgadas X
paso de la hélice)
Colo
Neg o y ama illo
El E le-Cop e se con ie e así en un disposi i o ideal pa a ope aciones en el ex e io , y cuyas uncionalidades
y con olado es pueden se modi icados po el usua io de mane a sencilla. Po ejemplo, des aca la posibilidad
de ins ala aplicaciones en el d on u ilizando pa a ello simplemen e un na egado . De es a mane a, se puede
H
Figu a 2.1 E le-Cop e
So wa e
4
4
conside a como una he amien a pe ec a con la que lle a a cabo es udios, in es igaciones y expe imen os
di e sos.
2.2. E le-B ain
O o de los disposi i os esenciales es es e ce eb o a i icial implemen ado po la emp esa E le Robo ics, ya que
en el se encuen a odo el so wa e y la senso ización necesa ia pa a con e i el E le-Cop e en un disposi i o
au ónomo.
P opo ciona una Unidad de Con ol de uelo o FCU (Fligh Con ol Uni ), un o denado enca gado de apo a
con oles de uelo básicos, ambién denominado au opilo o, y un o denado complemen a io basado en la
Raspbe y PI 2. Además de es o, incluye nume osos senso es como son una IMU, un ba óme o, GPS, una
b újula digi al, con posibilidad a su ez de añadi le cáma as. Pe mi e conexiones po pue o I2C, UART,
E he he y USB, aunque ambién es compa ible con an enas Wi i. Po úl imo, es muy impo an e comen a su
compa ibilidad con ROS, de mane a que se pueden desa olla odo ipo de aplicaciones obó icas.
Como se ha dicho an e io men e, aunque dicho disposi i o no apa ece á de mane a ísica en es e abajo, se ha
u ilizado ambién un modelado i ual del mismo en la simulación.
2.3. A dupilo
A dupilo es un con olado o au opilo o de código abie o que esul a de g an ayuda en la cons ucción de
ehículos au ónomos de odo ipo. En especial, la e sión u ilizada en es e caso es el A duCop e , ya que es
especí ica pa a el uso en d ones.
Median e es e so wa e se es capaz de ealiza un con ol del quad o o de mane a manual o au omá ica. Incluye,
a su ez, nume osos modos de uelo (Ac o, S abilize, Loi e , Guided, Re u n- o-Launch, Al Hol , e c) con los
que se pueden lle a a cabo di e sas ayec o ias y expe imen os con el d one. Además, es un so wa e
o almen e lexible y pe sonalizable, de mane a que, aunque la g an mayo ía de los pa áme os y con igu aciones
es án ya p ep og amadas, es os se pueden modi ica y amplia si el usua io lo c ee con enien e.
O a de sus en ajas es que es compa ible con la g an mayo ía de p og amas de es ación de ie a, como pueden
se el APM Planne , el Mission Planne o el QG oundCon ol, que consis en en una in e az g á ica de g an
u ilidad pa a la ejecución de misiones au ónomas complejas, a la ez que pe mi en la isualización de los
di e en es alo es de los senso es, pa áme os y con igu aciones del d one.
Figu a 2.2 E le B ain 2
Figu a 2.3 Logo ipo de A dupilo
5
Pa a su simulación, esul a de g an ayuda el uso del simulado SITL, que se expond á a con inuación.
2.4. So wa e In The Loop (SITL) simula o
Es e simulado pe mi e p oba el compo amien o del quad o o sin necesidad de u iliza el sis ema ísico
co espondien e. Se a a de una cons ucción del código del au opilo o en C++ que p opo ciona un ejecu able
con el cual es posible p oba el compo amien o del código sin ha dwa e.
En de ini i a, median e SITL se puede ejecu a A duPilo di ec amen e en un o denado . De es e modo, se saca
p o echo de la g an po abilidad del au opilo o A duPilo , que puede implemen a se en di e en es pla a o mas,
siendo el o denado una de ellas.
Cuando se ejecu a SITL, los da os del senso p o ienen de un modelo de dinámica de uelo en un simulado de
uelo. A duPilo iene una amplia gama de simulado es de ehículos inco po ados, y puede in e ac ua con
a ios simulado es ex e nos. Además, es ácil añadi nue os ipos de senso es y ehículos simulados.
O a de las en ajas de dispone de A duPilo en SITL es que es e le da acceso a una g an a iedad de
he amien as de desa ollo como depu ado es in e ac i os, analizado es es á icos y he amien as de análisis
dinámico, po lo que desa olla y p oba nue as ca ac e ís i as en A duPilo es mucho más sencillo.
2.5. Robo Ope a ing Sys em (ROS)
ROS consis e en un amewo k lexible pa a esc ibi aplicaciones so wa e des inadas a se implemen adas sob e
sis emas obo izados. En conc e o, se a a de una colección de he amien as y lib e ias cuyo p incipal obje i o
es simpli ica la a ea de c ea un compo amien o obó ico complejo y obus o a a és de una amplia a iedad
de pla a o mas obó icas. En de ini i a, p o ee los se icios es ánda de un sis ema ope a i o.
Se c eó como mé odo de desa ollo de so wa e de obó ica colabo a i a, de modo que di e en es g upos de
pe sonas pueden compa i en e ellos las di e en es he amien as que cada uno de ellos desa olle.
Es así po lo que ROS posee dos pa es básicas, una de sis ema ope a i o, ROS, y o a que es un conjun o de
paque es apo ados po la con ibución de usua ios (s acks) que implemen an uncionalidades di e sas como
SLAM (Simul aneous Loca ion and Mapping), plani icación, pe cepción, simulación, e c.
2.5.1 Esquema de uncionamien o
ROS es á basado en una a qui ec u a de g a os donde el p ocesamien o oma luga en los nodos, los cuales
pueden ecibi , manda y mul iplexa mensajes de senso es, con ol, es ados, plani icaciones y ac uado es.
Figu a 2.4 Logo ipo de ROS
P epa ación del En o no
12
12
sou ce ~/simula ion/ os_ca kin_ws/de el/se up.bash
oslaunch a dupilo _si l_gazebo_plugin e lecop e _spawn.launch
Comando 3.5 Comandos pa a lanza la simulación
• T as hace lo an e io , se debe espe a a que el mundo ca gue po comple o, lo cual se sab á po que, as
inicializa se odos los pa áme os y senso es del E le-Cop e , en el e minal de MAVP oxy apa ece á
un mensaje de Fligh ba e y 100 pe cen . T as ecibi es e mensaje, se deben ca ga los pa áme os del
E le-Cop e . Pa a ello, en el p ime e minal se debe ejecu a lo siguien e:
•
pa am load /[ u a_al_di ec o io_pe sonal]/simula ion/a dupilo /Tools/F ame_pa ams/E le-
Cop e .pa am
# Ejemplo: pa am load /home/pablo/simula ion/a dupilo /Tools/F ame_pa ams/E le-Cop e .pa am
Comando 3.6 Comandos pa a ca ga los pa áme os de E le-Cop e
Una ez hecho es o sin que se haya p oducido ningún p oblema signi ica á que ya es á ca gada la simulación
po comple o. En es e momen o se puede p oba a le an a el d one haciendo uso de los di e en es modos que
p o ee el au opilo o. Se mues an a con inuación la o ma de hace lo ola median e los modos GUIDED y
Figu a 3.3 Te minales con los comandos pa a lanza la simulación
Figu a 3.4 Inicio de la simulación

13
LOITER. Pa a ello, en el e minal del MAVP oxy se esc ibe lo que apa ece en la abla de comandos 3.4.
mode GUIDED
a m h o le
akeo 2
Comando 3.7 Comandos pa a a ma y despega el d one en modo GUIDED
O bien, u ilizando el modo LOITER, con el cual se sob eesc iben los dis in os canales de Roll, Pi ch, Yaw o
Th o le.
mode LOITER
a m h o le
c 3 1600
c 3 1500
Comando 3.8 Comandos pa a a ma y despega el d one en modo LOITER
Si se p oduce algún allo al a ma los mo o es, se debe p oba a pone a ce o el pa áme o ARMING_CHECK
desde el e minal del MAVP oxy (el p ime o de los dos), al y como se indica en la abla de comandos 3.9.
pa am se ARMING_CHECK 0
Comando 3.9 Comandos pa a es ablece el pa áme o ARMING_CHECK a ce o
Una ez hecho es o, se debe einicia la simulación pa a que se gua de dicha ac ualización. Una ez einiciada,
p oba de nue o los comandos 3.7 o 3.8.
Pa a a e iza el d one simplemen e hab ía que cambia el modo a LAND.
Además de lo an e io , ambién puede esul a in e esan e el mos a las imágenes de las cáma as que lle a
inco po adas el d one. Es o se consegui ía como se mues a en la abla de comandos 3.10.
Figu a 3.5 E le-Cop e olando
P epa ación del En o no
14
14
os un image_ iew image_ iew image:=/e lecop e / on /image_ on _ aw
os un image_ iew image_ iew image:=/e lecop e /bo om/image_ aw
Comando 3.10 Comandos pa a isualiza las cáma as del E le-Cop e
Con es os úl imos comandos, lo que se consigue en de ini i a es ecibi los da os de las cáma as que se es án
publicando en los opics de nomb es image_ on _ aw y image_ aw.
Figu a 3.6 Simulación de E le-Cop e con cáma as ac i adas
15
4 SIMULACIÓN DEL OPTICAL FLOW
na ez que se dispone de odo el en o no de simulación inicial, lo p ime o que hay que ene en cuen a es que
se necesi a una simulación de un senso de lujo óp ico simila al que se encuen a ins alado en el d one ísico,
ya que con la ayuda de es e se pod án conoce alo es de posicionamien o y elocidad del E le-Cop e , al y
como se explica á pos e io men e.
Es po es o po lo que se dedica es e capí ulo a de alla los dos p ocedimien os con los que se ha abo dado es e
p oblema, siendo el segundo de los expues os el que se ha op ado po u iliza .
4.1. Op ical Flow y LIDAR
Un op ical low o senso de lujo óp ico es un senso que iene la capacidad de medi el mo imien o a a és de
una e e encia isual y emi i una medición a pa i de la misma. Exis en a ias con igu aciones de es e ipo de
senso es, pe o el que se ha u ilizado an o en el E le-Cop e ísico como en el de la simulación se a a de un
senso de lujo óp ico acompañado de un gi óscopo de es ejes y de un LIDAR o sona , con el cual se
complemen an las medidas omadas po el op ical low, disminuyendo el e o de la medida o al, y además se
ob ienen mediciones de al u a.
Su uncionamien o se basa en el uso de la ex u a del suelo y o as ca ac e ís icas isibles pa a de e mina la
elocidad del ehículo, en e o as medidas. Además, dicho senso se á u ilizado en el E le-Cop e pa a
posiciona el d one jun o con las medidas del GPS.
Un LIDAR iene la misma inalidad que un sona . Se a a de un disposi i o que pe mi e de e mina la dis ancia
en e un emiso y una supe icie, u ilizando pa a ello un haz láse pulsado. Es a dis ancia es de e minada
median e la medición del iempo de e aso en e la emisión del pulso y su de ección as se e lejada. Aún así,
es necesa io de e mina la posición ins an ánea de dicho senso , pa a lo cual se u iliza el GPS. Conocidos es os
da os y la dis ancia del senso a la supe icie, se consigue ob ene las coo denadas especí icas del d one.
4.2. P ime mé odo
El senso de lujo óp ico implan ado en el d one ísico se a a del op ical low de la emp esa Pixhawk, o a de
las g andes compañías des inadas a la ab icación de, en su mayo pa e, au opilo os.
Es po es o po lo que el código que apa ece implemen ado en Gazebo de su op ical low equie e de un p oceso
de adap ación pa a pode inclui lo en el modelo de E le-Cop e . De es o se a a el p ime mé odo p esen ado.
En el, se in en a á desacopla el op ical low de PX4 implemen ado en el modelo del d one que ecibe el nomb e
de I is, pa a pos e io men e inclui el mismo en el modelo de E le-Cop e .
U
Figu a 4.1 Op ical Flow de Pixhawk
Simulación del Op ical Flow
16
16
4.2.1 PX4 de Pixhawk
El PX4 de Pixhawk se a a de o o de los g andes au opilo os que exis en en la ac ualidad. Funciona
p ac icamen e de la misma mane a que el A duPilo y posee ca ac e ís icas simila es como el se un so wa e de
código abie o, el o ece dis in os modos de uelo pa a las ae ona es o su compa ibilidad con di e sas GCS.
Además, ambién es compa ible con mul i ud de pla a o mas como ROS, Gazebo o con el p o ocolo MAVLink,
al igual que el A duPilo .
En el ámbi o de la simulación se compo a de la misma mane a que el p ime au opilo o p esen ado, ya que
ambién se puede u iliza SITL con el in de p oba su uncionamien o sin eque imien o de ha dwa e ninguno.
En especial, el modelo simulado del op ical low de es a misma ma ca es el que in e esa pa a el p oyec o. Como
se ha comen ado an e io men e, es necesa io el desacople del mismo del modelo del d one que o ece PX4,
llamado I is y, una ez sepa ado de es e, se necesi a inclui dicho modelo del senso al que ya se iene mon ado
del E le-Cop e .
F en e a es e obje i o, lo p incipal es consegui ans o ma el modelo del senso del o ma o SDF (Simula ion
Desc ip ion Fo ma ) al o ma o URDF (Uni e sal Robo ic Desc ip ion Fo ma ), los cuales se expond án a
con inuación.
4.2.2 XML
Los dos o ma os u ilizados pa a de ini los modelos de odos los obo s que se deseen simula , que son SDF y
URDF, se basan en el o ma o XML. Es e es un me a-lenguaje que pe mi e de ini lenguajes de ma cado (pe o
no es uno de ellos) adecuados a usos de e minados, de mane a que se pueden almacena da os de o ma legible.
Figu a 4.2 Au opilo o PX4 de Pixhawk
Figu a 4.3 D one I is
17
Se a a de un subconjun o de SGML (S anda d Gene alized Ma kup Language), simpli icado y adapa ado a
In e ne . Es impo an e no con undi lo con HTML (Hype Tex Ma kup Language), el cual es un lenguaje
de inido po SGML, po lo an o, una aplicación de dicho lenguaje.
Con su uso se consigue es uc u a documen os de modo que la in o mación que con ienen pueda se
almacenada, ansmi ida o p ocesada po nume osas aplicaciones y disposi i os.
Posee nume osas en ajas, en e ellas su ex ensibilidad median e la adición de nue as e ique as, su
compa ibilidad en e aplicaciones de dis in as pla a o mas y su ácil comp ensión debido a su o ganización y a
la capacidad de ans o ma da os en in o mación.
4.2.3 Fo ma o SDF (Simula ion Desc ip ion Fo ma )
SDF es, como se ha comen ado, un o ma o de XML con el cual se desc iben obje os y en o nos pa a
simulaciones de obo s, así como su isualización y con ol. Todos los a chi os manejados po Gazebo ienen
es e o ma o. Haciendo uso del mismo se consigue desc ibi odos los aspec os de un sis ema obo izado, desde
obo s simples basados en un chasis con uedas a obo s humanoides.
Los documen os esc i os en es e o ma o se es uc u an de la siguien e mane a:
• Un a chi o SDF puede inclui un elemen o mundo (<wo ld>) en el que se de inen odos los elemen os
que apa ece án en el, un modelo (<model>) con el que se de ine un obo comple o o cualquie o o
obje o ísico, o un elemen o luz (<ligh >) con el que se desc ibe la uen e de luz de una simulación. Es
necesa io indica una e sión de xml y una e sión de sd .
<?xml e sión=’1.0’?>
<sd e sión=’1.6’>
<wo ld name=’po _de ec o’>
. . .
</wo ld>
</sd >
Código 4.1 Ejemplo de código de mundo en SDF
<?xml e sión=’1.0’?>
<sd e sión=’1.6’>
<model name=’mi_modelo’>
. . .
</model>
</sd >
Código 4.2 Ejemplo de código de modelo en SDF
<?xml e sión=’1.0’?>
<sd e sión=’1.6’>
<ligh name=’mi_luz’>
. . .
</ligh >

Simulación del Op ical Flow
18
18
</sd >
Código 4.3 Ejemplo de modelo de luz en SDF
• Den o de la de inición del mundo pueden apa ece o os muchos a ibu os como ien o (<wind>),
inclusión de o os a chi os desde una di ección (<include>), g a edad (<g a i y>), a mós e a
(<a mosphe e>), modelos de obo s u obje os ísicos (<model>) y plugins (<plugin>) median e los
cuales se le pueden añadi códigos que apo an dinámica yc on ol, en e o os.
<?xml e sión=’1.0’?>
<sd e sión=’1.6’>
<wo ld name=’po _de ec o’>
<include>
. . .
</include>
<model name=’mi_modelo_1’>
. . .
</model>
<model name=’mi_modelo_2’>
. . .
</model>
. . .
</wo ld>
</sd >
Código 4.4 Ejemplo de a ibu os den o de un modelo de mundo en SDF
• En la desc ipción de un modelo, las pa es más impo an es, aunque exis en o os muchos a ibu os, son
la inclusión de a chi os (<include>) , la posición en x, y y z, y la o ien ación en oll, pi ch, yaw, del
obje o espec o a un cue po de e e encia (<pose>), que se puede indica median e su co espondien e
a ibu o (< ame>), y los a ibu os más u ilizados, ya que de inen el obo po comple o, que son los
enlaces (<link>) y las a iculaciones (<join >).
<?xml e sión=’1.0’?>
<sd e sión=’1.6’>
<model name=’mi_modelo’>
<pose>0 0 0.5 0 0 0</pose>
<link name=’mi_enlace_1’>
. . .
</link>
<join ype=” e olu e” name=’mi_a iculacion_1’>
. . .
19
</join >
. . .
</model>
</sd >
Código 4.5 Ejemplo de a ibu os den o de un modelo en SDF
• En los links y join s se incluyen o os muchos a ibu os pa a cada uno de ellos, algunos de los cuales se
e án los apa ados 5.2 y 5.3. En lo e e en e al elemen o link, se le deben indica p opiedades como su
posición, su ine cia, su masa, las colisiones, su isualización o si posee algún ipo de senso , en e o as.
Respec o al join , se incluyen en su in e io elemen os como quienes son los links pad e e hijo de dicha
a iculación, a ibu os imp escindibles en la co ec a de inición de un join , la posición, los ejes de
o ación pa a a iculaciones de o ación o el eje de aslación pa a las a iculaciones p ismá icas, la
exis encia de senso es e cé e a.
Se han explicado los componen es de es e ipo de o ma o que más se han u ilizado en el p oyec o, aunque, como
se ha comen ado, exis en muchos más además de es os, pe o la explicación de enida de ellos no es obje o de es e
documen o. De odos modos, si se desea conoce más sob e es e ipo de lenguaje, se ecomienda segui la página
o icial del o ma o SDF adjun ada en la bibliog a ía.
4.2.4 Fo ma o URDF (Uni e sal Robo ic Desc ip ion Fo ma )
URDF es, al igual que SDF, un a chi o con o ma o XML u ilizado pa a desc ibi obo s. Es e ipo de o ma o
ca ece de muchas ca ac e ís icas que, po el con a io, posee SDF. Po ejemplo, URDF sólo puede especi ica
las p opiedades cinemá icas y dinámicas de un solo obo de o ma aislada, pe o no se puede p ede ini una
posición del obo den o de un mundo, ampoco se pueden especi ica join loops (a iculaciones pa alelas) y
ca ece de algunas p opiedades como la icción. Tampoco pe mi en desc ibi elemen os que no sean obo s,
como luces, mapas con elie e, e c. Se a a de un o ma o menos lexible que SDF, ya que di ie e un poco del
o ma o co ien e de XML y no iene un mecanismo de compa ibilidad con e siones an e io es.
Sin emba go, es e es el lenguaje p incipal que u iliza ROS pa a la de inición de obo s, aunque si se desea u iliza
Gazebo es necesa io ene en cuen a cie os aspec os especiales pa a que uncionen los p og amas co ec amen e,
ya que Gazebo ealiza una con e sión de o ma os de URDF a SDF de mane a au omá ica, y algunos de los
a ibu os de un ipo de o ma o son incompa ibles en el o o.
Debido a lo comen ado p e iamen e, en URDF exis e un elemen o llamado <gazebo> a a és del cual se
consiguen de ini p opiedades necesa ias pa a la simulación en Gazebo. De es e modo se pueden de ini cie os
a ibu os que apa ecen en el o ma o SDF, pe o no en el o ma o URDF. Si no se incluye dicho elemen o no
hab á ningún p oblema, ya que se asigna án los alo es po de ec o. El elemen o <gazebo> se puede aplica a
dis in as e ique as, que son < obo >, <link> y <join >. Pa a cada uno de ellos se inclui án dis in os a ibu os,
los cuales se encuen an ecogidos en las ablas 4.1, 4.2 y 4.3.
Tabla 4.1 Elemen o <gazebo> pa a la e ique a < obo >
Nomb e
Tipo
Desc ipción
s a ic
bool
Si es ue, el modelo pe manece inmó il. En cualquie o o caso el modelo es simulado
jun o con su dinámica.
Los elemen os de enlace y a iculación (<link> y <join >) uncionan de igual mane a que pa a el o ma o SDF.
De es a o ma, en los enlaces se deben de ini las colisiones, las isualizaciones de los mismos, un elemen o
<gazebo> y además un elemen o <ine ial> en el que se desc ibe la masa y las ine cias de dicho link.
Simulación del Op ical Flow
20
20
Tabla 4.2 Elemen o <gazebo> pa a la e ique a <link>
Nomb e
Tipo
Desc ipción
ma e ial
alue
Ma e ial de un elemen o isual, incluyendo ex u a y colo
g a i y
bool
Usa g a edad
dampingFac o
double
Fac o de amo iguamien o
maxVel
double
Valo máximo de uncación en la co ección de la elocidad de con ac o
minDep h
double
Minima p o undidad pe mi ida an es de aplica el impulso de co ección de
con ac o
mu1
double
Coe icien es de ición
mu2
di 1
s ing
Té mino que indica la di ección de mu1 en un ma co de e e encia local de
colisiones
kp
double
Rigidez de con ac o Kp y el amo iguamien o Kd
kd
sel Collide
bool
Si es ue, el link puede choca con o os links del modelo
maxCon ac s
in
Núme o máximo de con ac os pe mi idos en e dos en idades
lase Re o
double
Valo de in ensidad de uel o po el senso láse
Pa a el join , se deben especi ica los enlaces pad e e hijo, el o igen de la a iculación espec o al pad e, los ejes,
si p oceden, un é mino <gazebo> de mane a opcional y é minos de dinámica.
Tabla 4.3 Elemen o <gazebo> pa a la e ique a <join >
Nomb e
Tipo
Desc ipción
s opC m
double
Fue za de es icción de pa ada de la a iculación (c m) y pa áme o de
educción de e o (e p)
s opE p
p o ideFeedback
bool
Pe mi e que las a iculaciones publiquen sus da os de ue za-pa a a és
de un plugin de Gazebo
implici Sp ingDampe
bool
Si es án a ue se u iliza á el e p y el c m pa a simula la amo iguación.
c mDamping
udgeFac o
double
Té mino de escalado pa a es ablece los lími es de un mo o a icula .
21
Debe es a en e uno y ce o.
Con el in de abaja co ec amen e con a chi os URDF en Gazebo es con enien e sabe lo siguien e:
• Es imp escindible que odos los elemen os <link> posean un elemen o <ine ial> con alo es
di e en es de ce o pa a la masa y la ine cia, ya que Gazebo igno a á dichos enlaces en caso con a io al
hace la con e sión.
• Cada elemen o <link> debe posee opcionalmen e un único elemen o <gazebo> median e el cual se
pueden con e i colo es y ex u as al o ma o de Gazebo o añadi plugins de senso es.
• Igualmen e, po cada elemen o <join > se debe añadi de mane a opcional un elemen o <gazebo> pa a
es ablece la dinámica de amo iguación de mane a adecuada o pa a ag ega plugins de con ol al
ac uado .
• También se debe u iliza un elemen o <gazebo> po cada elemen o < obo >.
• Si se desea que el obo se encuen e unido al mundo se debe añadi un enlace <link name=”wo ld”/>.
4.2.5 Conclusiones del p ime mé odo
Una ez comp endidos odos los ac o es que in luyen en el modelado de un elemen o en Gazebo ya se puede
p ocede al obje i o que pe sigue el p esen e apa ado, la implemen ación del op ical low de PX4 sob e el d one
del E le-Cop e .
En pocas palab as, en la página web de PX4 que apa ece en la bibliog a ía se puede encon a una guía de PX4,
ambién incluida en las e e encias de es e documen o. En es a guía se explica odo el p ocedimien o necesa io
pa a consegui implemen a una simulación en Gazebo con sopo e de SITL y ROS en la cual apa ece el d one
I is de PX4. Es in e esan e, si se desea u iliza es e mé odo de implemen ación, segui las dis in as ins ucciones
que se mues an en es a guía.
Una ez ob enido el modelo del I is se puede ealiza un análisis de los a chi os que de los que se compone
dicha simulación. En e odos los modelos que se adquie en apa ece án es median e los cuales se implemen a
el senso deseado. Es os es a chi os, de o ma os SDF, se encuen an ubicados bajo los nomb es de
i is_op _ low.sd , low_cam/model.sd y LIDAR/model.sd .
El p ime o de ellos se enca ga de uni los o os dos modelos al ame del i is median e join s de ipo e olu e,
asignándoles además una posición de e e encia a cada uno de ellos.
Po o o lado, en el modelo del op ical low ( low_cam/model.sd ) se desc iben, como es de espe a , odos los
pa áme os que de inen la simulación de dicho senso . Es a de inición se lle a a cabo con dos e ique as, una de
ipo <link> y o a de ipo <senso >. Tal y como se desc ibió en el subapa ado 4.2.3, en el <link> se de inen la
posición inicial, la masa y la ine cia en el bloque <ine ial> y las p opiedades isuales del senso en el bloque
< isual>, en el cual se indica que se a a de un cubo. Respec o a la e ique a de ipo <senso >, en ella se
desc iben el ipo y las p opiedades del senso , indicando que se á de ipo came a. Den o de es e bloque se
indican especi icaciones ales como si se man end á siemp e el senso ac ualizado (Always_on), la asa de
ac ualización de los da os (Upda e_ a e), si se isualiza á el senso en el GUI (Visualize), el opic de ROS en el
que se publica án los da os ecogidos po el mismo (Topic), así como los dis in os pa áme os de la cáma a como
campo de is a ho izon al, el ancho y la al u a de la imagen, las dis ancias mínimas y máximas que se án
aducidas y el uido gaussiano in oducido. Po úl imo, se incluye un plugin que le inclui á la dinámica al senso
bajo el nomb e de libgazebo_op icalFlow_plugin.so.
Respec o al modelo del LIDAR (LIDAR/model.sd ) se desc iben del mismo modo el bloque <link> donde
igualmen e en <ine ial> se de inen su posición, masa e ine cias y a con inuación se encuen a su desc ipción
isual como un cilind o de colo neg o. A con inuación, apa ece de nue o la desc ipción del ipo de senso del
que se a a, que en es e caso es un láse , y en el cual se especi ican a ibu os como el núme o de ayos simulados
po cada ciclo del láse (samples), la esolución ( esolu ion), los ángulos máximo y mínimo (min_angle y
max_angle), el ango de los ayos ( ange) y el uido gaussiano incluido en el senso . Finalmen e apa ecen de
nue o los plugins incluidos y se indica que se man end á siemp e ac ualizado, la asa de ac ualización de los
da os y se ac i a la isualización.
Simulación del Op ical Flow
28
28

29
5 CREACIÓN DEL MUNDO DE TRABAJO
as ealiza odos los pasos de los capí ulos an e io es ya se iene la simulación p incipal del p oyec o, la que se
compone del E le-Cop e , con odos sus senso es, den o de un mundo en el que ealiza expe imen os.
Aún así, aún al an algunos e oques po hace sob e dicho en o no, con los cuales inaliza ían las a eas de
modelado. Es os son la adición de la ca ga unida al d one y, a causa de dicha unión, se debe modi ica la posición
inicial del d one pa a que se encuen e sob e un sopo e de un me o de al u a, con el in de que la pelo a pa a
de una posición de suspensión.
En es e quin o capí ulo se desc ibi án a modo in oducción los dis in os a chi os que componen una simulación
de Gazebo pa a, a con inuación, p esen a los códigos c eados pa a el mencionado sopo e y pa a la ca ga.
Finalmen e, se expond án los esul ados de es as ac ualizaciones as se p obadas.
5.1. A chi os de una simulación
Muchos son los a chi os que in luyen en la simulación de un en o no en Gazebo y, ya que a con inuación se
p e ende desc ibi el p ocedimien o que se ha seguido a la ho a de c ea o modi ica algunos de ellos, se
p esen a á en es e apa ado una b e e in oducción de los mismos.
5.1.1 A chi os .wo ld
Es os se a an de a chi os en o ma o SDF que, como su p opio nomb e indica, incluyen, al y como se explicó
en el subapa ado 4.2.3, la desc ipción de un mundo. Es o implica que en su in e io se de inen odos los
componen es que apa ece án en un de e minado mundo y las p opiedades y plugins del mismo. Comen a que
es e ipo de a chi os se encuen an ubicados den o del en o no ins alado en el apa ado 3.1, en el di ec o io
~/simula ion/ os_ca kin_ws/s c/a dupilo _si l_gazebo_plugin/a dupilo _si l_gazebo_plugin/wo lds.
Den o de es as p opiedades apa ecen especi icaciones como el cielo del mundo, la g a edad, el ien o,
especi icaciones ísicas y de iempo de la simualción o la luz que incidi á sob e la misma.
Pa a indica los elemen os que se desea que apa ezcan en el en o no que se c ee, se deben inclui los mismos
median e e ique as <include>, o c eando den o de es os a chi os bloques ipo <model>.
5.1.2 A chi os .u d , .xac o y .sd
Como se p esen ó en el apa ado 4.2, los dis in os modelos que desc iben cualquie sis ema ísico en Gazebo
pueden es a en o ma o SDF o en URDF, pudiéndose a su ez u iliza Xac o en es e segundo caso.
Es os modelos se encuen an ubicados, den o del en o no ins alado en el apa ado 3.1, an o en la di ección
~/simula ion/ os_ca kin_ws/s c/a dupilo _si l_gazebo_plugin/a dupilo _si l_gazebo_plugin/u d como en
~/.gazebo/models. Es a úl ima se a a de una ca pe a ocul a den o del di ec o io pe sonal.
5.1.3 A chi os .launch
Es e ipo de a chi os ambién es án desc i os en o ma o XML y son aquellos que pueden se ejecu ados
median e el uso del paque e de ROS oslaunch.
Den o de dichos documen os se encuen an, si se e ie en a una simulación en Gazebo, los di e en es a chi os
que se desean ejecu a al lanza una simulación, además de odas las condiciones iniciales de dicha simulación
y a gumen os especí icos que deban adqui i de e minados pa áme os de la misma. En ealidad, al ejecu a el
comando 3.5, lo que se es a haciendo es lanza a ios a chi os de es e ipo, los cuales, a su ez, indican qué o os
a chi os se deben ejecu a , den o de los cuales hay incluidos más documen os e e en es a modelos, y así
sucesi amen e, de mane a que se a c eando un á bol de a chi os que an ejecu ándose y e e enciándose en e
ellos, gene ando inalmen e la simulación deseada.
T
C eación del Mundo de T abajo
30
30
5.2. Pla a o ma
F en e a la necesidad de modela una ca ga suspendida del d one a 60 cm, lo ideal se ía dispone de una
pla a o ma de ap oximadamen e un me o de al u a, con el in de que, al inicia la simulación, el d one es u iese
sob e ella, y la ca ga colgada, sin con ac o ninguno con el suelo.
Dicho modelo se c ea á en SDF, debido a las acilidades que es e o ma o o ece en un p oceso de modelado, y
ya que no es necesa io adjun a el mismo al de E le-Cop e , sino que simplemen e debe inclui se en el mismo
mundo.
Es e sopo e se á lo más básico posible, a ándose simplemen e de cua o pa as cilínd icas de un me o de al u a
sob e las cuales se apoya á el d one. El código 5.1 se co esponde con el modelado de dicha pla a o ma.
<?xml e sion='1.0'?>
<sd e sion='1.4'>
<model name="sopo e_d one">
<s a ic> alse</s a ic>
<link name=' i s _leg'>
<pose>0.141 -0.141 0.11 0 0 0</pose>
<collision name=' i s _leg_collision'>
<geome y>
<cylinde >
< adius>.7</ adius>
<leng h>1</leng h>
</cylinde >
</geome y>
</collision>
< isual name=' i s _leg_ isual'>
<geome y>
<cylinde >
< adius>.7</ adius>
<leng h>1</leng h>
</cylinde >
</geome y>
<ma e ial>
<sc ip >
<u i> ile:Gazebo/media/ma e ials/sc ip s/gazebo.ma e ial</u i>
<name>Gazebo/Black</name>
</sc ip >
</ma e ial>
</ isual>
</link>
<link name='second_leg'>
<pose>-0.141 0.141 0.11 0 0 0</pose>
<collision name='second_leg_collision'>
<geome y>
<cylinde >
< adius>.7</ adius>
<leng h>1</leng h>
</cylinde >
</geome y>
</collision>
< isual name='second_leg_ isual'>
<geome y>
<cylinde >
< adius>.7</ adius>
<leng h>1</leng h>
</cylinde >
</geome y>
<ma e ial>
31
<sc ip >
<u i> ile:Gazebo/media/ma e ials/sc ip s/gazebo.ma e ial</u i>
<name>Gazebo/Black</name>
</sc ip >
</ma e ial>
</ isual>
</link>
<join name=" i s -second_leg_join " ype=" ixed">
<pa en > i s _leg</pa en >
<child>second_leg</child>
</join >
. . .
Código 5.1 F agmen o del código del sopo e
Pa a que no ocupe demasiado, solo se incluye el agmen o del código pa a dos de las pa as del sopo e, ya que
se puede ex apola dicho o ma o al es o de pa as.
Como se puede ap ecia en el código, se de ine un link pa a cada pa a de la pla a o ma, llamados i s _leg,
second_leg, hi d_leg y ou h_leg. La posición de cada una de ellas se de ine de mane a que en e las cua o
o men un cuad ado con cen o en el o igen de coo denadas del mundo de Gazebo.
Po o o lado, an o la pa e de colisión como la pa e isual ienen los mismos componen es pa a odas las pa as,
de mane a que es as sean iguales en e ellas. Dichos bloques desc iben cilind os de un adio de 7 cm y de una
al u a de un me o. Además, en la pa e isual se especi ica el colo que se quie e que adquie an las pa as. La
ca pe a indicada donde se encuen a la de inición del colo debe es a ubicada en la misma di ección que el
modelo del sopo e pa a pode u iliza lo.
Finalmen e, se puede obse a como es án desc i as las a iculaciones. Es as son de ipo ijo y an uniendo la
p ime a pa a con la segunda, la segunda con la e ce a, la e ce a con la cua a y es a de nue o con la p ime a.
Así se consigue que odas ellas se compo en como un conjun o, p opo cionando una mayo es abilidad al
sopo e.
Figu a 5.1 Posiciones de las pa as de la pla a o ma
C eación del Mundo de T abajo
32
32
5.2.1 Modi icación de las colisiones del d one
T as consegui una simulación álida del sopo e, es necesa io si ua el d one encima del mismo. Lo e e en e a
el como se ha modi icado su posición inicial se comen a á en el apa ado 5.4, pe o hay que ene o o aspec o en
cuen a, las colisiones.
Tal y como es án de inidas en el modelo de E le-Cop e , el mismo que se ha ins alado y mon ado, las colisiones
que posee el d one son una pa a cada hélice de o ma cilínd ica y un cubo pa a el ame, cuya al u a a desde el
lími e in e io de las pa as, sin inclui las mismas, has a la pa e supe io del d one. Se incluye una imagen a
con inuación pa a acla a dicho concep o.
De es e modo el d one no se sos end á sob e el sopo e, que es lo que se p e ende, ya que no posee colisiones en
la base de las pa as. Es po es o po lo que se ha de ealiza un a chi o que de ina dichas colisiones, y es el que
ecibe el nomb e de legs_bases.u d .xac o. En es e, al y como se mues a en el código 5.2, se incluyen al inal
de cada pa a unos links compues os únicamen e po colisiones de geome ía cúbica y muy pequeñas pe o
su icien es pa a hace que el E le-Cop e se man enga sob e el sopo e. Es os cua o links, uno po cada pa a, se
unen al ame del d one median e a iculaciones ijas.
Figu a 5.2 Modelo de la pla a o ma
Figu a 5.3 Colisiones iniciales del E le-Cop e
33
<?xml e sion="1.0" ?>
< obo name="legs_bases">
<link name= "leg1_link">
<collision name="leg1_base_collision">
<o igin xyz="-0.134 0.134 -0.0495" py="0 0 0" />
<geome y>
<box size="0.01 0.01 0.0005"/>
</geome y>
</collision>
</link>
<join name="leg1_join " ype=" ixed">
<o igin xyz="0 0 0" py="0.0 0.0 0.0" />
<pa en link="base_link" />
<child link="leg1_link"/>
</join >
<link name= "leg2_link">
<collision name="leg2_base_collision">
<o igin xyz="0.134 0.134 -0.0495" py="0 0 0" />
<geome y>
<box size="0.01 0.01 0.0005"/>
</geome y>
</collision>
</link>
<join name="leg2_join " ype=" ixed">
<o igin xyz="0 0 0" py="0.0 0.0 0.0" />
<pa en link="base_link" />
<child link="leg2_link"/>
</join >
...
Código 5.2 F agmen o del código legs_bases.u d .xac o
Solo se incluyen las colisiones de dos de las pa as ya que el es o se ealiza de igual mane a, e i ando así una
ex ensión innecesa ia de dicho código en el p esen e documen o.
Una ez ealizado es o, e incluyendo es e modelo en el gene al, se ob iene el esul ado mos ado en la Figu a
5.4.
Figu a 5.4 E le-Cop e sob e pla a o ma

C eación del Mundo de T abajo
34
34
5.3. Ca ga
El modelado de la ca ga es o o de los pun os más impo an es de es e p oyec o. En de ini i a, se ha de consegui
simula una pelo a na anja colgada a 60 cm del d one y que posea una dinámica lo más pa ecida posible a la que
end ía una ca ga suspendida de un hilo.
El modelo de la ca ga se encuen a de inido en o ma o URDF en un a chi o independien e llamado
cha ge.u d .xac o, el cual se incluye en e lecop e .xac o al y como se indica á en el apa ado 5.4.
Se puede di idi su código en dos bloques di e en es: uno en el que se de ine el modelo ísico de la ca ga y o o
median e el cual se consigue simula la dinámica de la misma como si se encon ase colgada de un hilo.
5.3.1 P ime bloque: modelado ísico e ine cial de la ca ga
Tal y como se puede ap ecia e el código 5.3, la ca ga es a compues a simplemen e po una es e a con unos
de e minados alo es de masa y de ine cia. El adio de la misma es de dos cm, el cual se especi ica an o pa a la
isualización como pa a la colisión de la misma. Es e alo se ha sacado omando como e e encia el adio de
una pelo a de ping-pong o icial, que es la que en de ini i a se u iliza á. Además, o a de las especi icaciones es
que dicha pelo a es a á en un p incipio llena de agua. Con es os da os, y conociendo la ecuación del olumen de
una es e a, así como la densidad del agua, se puede calcula la masa o al de agua y sumá sela a la de la pelo a.
De es e modo: Equa ion Chap e 5 Sec ion 1
𝑉𝑒𝑠𝑓 =4
3𝜋𝑅𝑒𝑠𝑓
3,𝑅𝑒𝑠𝑓 =2 𝑐𝑚
(5.1)
𝜌𝑎𝑔𝑢𝑎 =𝑚𝑎𝑔𝑢𝑎
𝑉𝑒𝑠𝑓 ,𝜌𝑎𝑔𝑢𝑎 =1 𝑔
𝑐𝑚3
(5.2)
𝜌𝑎𝑔𝑢𝑎=𝑚𝑎𝑔𝑢𝑎
4
3𝜋𝑅𝑒𝑠𝑓
3
(5.3)
𝑚𝑎𝑔𝑢𝑎=4
3𝜌𝑎𝑔𝑢𝑎𝜋𝑅𝑒𝑠𝑓
3=8.377 𝑔
(5.4)
Sabiendo que la masa de una pelo a de ping-pong es ap oximadamen e 2.7 g, la masa o al de la ca ga se ía:
𝑚𝑡𝑜𝑡𝑎𝑙 =𝑚𝑝𝑒𝑙𝑜𝑡𝑎+𝑚𝑎𝑔𝑢𝑎 =2.7+8.377=11 𝑔
(5.5)
Una ez se iene es o, pa a el cálculo de las ine cias se iene en cuen a el Teo ema de S eine , el cual a i ma que
si se conoce el momen o de ine cia con espec o a un eje que pase po el cen o de masas de un obje o, en onces
se puede conoce el momen o de ine cia con espec o a cualquie o o eje pa alelo a es e p ime o que se encuen e
a una dis ancia L de la siguien e mane a:
𝐼=𝐼𝐶𝑀+𝑚𝐿2
(5.6)
Donde 𝐼𝐶𝑀 ep esen a el momen o de ine cia en el cen o de masas del obje o y 𝑚 se co esponde con la masa
o al del mismo.
En es e caso, enemos que la dis ancia del hilo es de 60 cm, mien as que los alo es de adio y masa son los
p esen ados an e io men e. Además, al se la ca ga una es e a, el momen o de ine cia de su cen o de masas
se ía:
35
𝐼𝐶𝑀 =2
5𝑚𝑅2
(5.7)
Sabido es o, se puede calcula la ma iz de ine cias en los ejes de la ca ga, sabiendo que:
• Los é minos c uzados de la ma iz son nulos.
• El e ec o del cable solo se iene pa a los ejes x e y, siendo es e nulo en z.
Siguiendo esas dos pau as, y aplicando el eo ema de S eine , se concluye que la ma iz de ine cias se á la
siguien e:
𝐼=
[
(2
5𝑚𝑡𝑜𝑡𝑎𝑙𝑅𝑒𝑠𝑓
2)+𝑚𝑡𝑜𝑡𝑎𝑙𝐿20 0
0 (2
5𝑚𝑡𝑜𝑡𝑎𝑙𝑅𝑒𝑠𝑓
2)+𝑚𝑡𝑜𝑡𝑎𝑙𝐿20
0 0 2
5𝑚𝑡𝑜𝑡𝑎𝑙𝑅𝑒𝑠𝑓
2
]
(5.8)
Sus i uyendo en la an e io ma iz los dis in os da os que se ienen, sabiendo que la masa ha de es a el kg y la
dis ancia en me os:
𝐼=[3.962×10−3 0 0
0 3.962×10−3 0
0 0 1.76×10−6] 𝐾𝑔𝑚2
(5.9)
Es os son los co espondien es alo es de ine cia que se añadi án a la ca ga, al y como se mues a en el código
5.3.
Se puede ap ecia que an o el o igen del bloque <ine ial> como en los de <collision> y < isual> poseen un
alo de -0.6. Es o se debe a que dicho o igen es a e e enciado al link pad e, que en es e caso se á el ame del
d one, llamado base_link en el código.
También se indica al inal del código el colo que se desea que posea la ca ga, en es e caso na anja.
<?xml e sion="1.0" ?>
< obo name="suspended_cha ge" xmlns:xac o="h p:// os.o g/wiki/xac o">
<link name="suspended_cha ge_link">
<ine ial>
<mass alue="0.011"/>
<o igin py="0 0 0" xyz="0 0 -0.6"/>
<ine ia ixx="3.962e-3" ixy="0" ixz="0" iyy="3.962e-3" iyz="0"
izz="1.76e-6"/> <!-- [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] [kg.m^2] -
->
</ine ial>
<collision>
<o igin py="0 0 0" xyz="0 0 -0.6"/>
<geome y>
<sphe e adius="0.02"/>
</geome y>
</collision>
< isual>
<o igin py="0 0 0" xyz="0 0 -0.6"/>
<geome y>
<sphe e adius="0.02"/>
</geome y>
</ isual>
</link>
C eación del Mundo de T abajo
36
36
...
<gazebo e e ence="suspended_cha ge_link">
<ma e ial>Gazebo/O ange</ma e ial>
</gazebo>
</ obo >
Código 5.3 F agme o del código cha ge.u d .xac o des inado al modelado ísico de la ca ga
5.3.2 Segundo bloque: modelado del compo amien o dinámico de la ca ga
La segunda pa e en la que se ha di idido la explicación del código consis e en como se ha simulado el
mo imien o de balanceo de la ca ga.
Más allá de la adición de ine cias a la misma, se debía encon a alguna o ma de simula el mo imien o que
posee ía una ca ga suspendida de un hilo, es deci , un mo imien o de balanceo.
En o ma o SDF se ía simple la simulación de dicho mo imien o, ya que solo con inclui una a iculación de
ipo ball a la dis ancia deseada en e la ca ga y el d one, y aplicándole a la misma las ine cias adecuadas, se
ob end ía un esul ado bas an e ealis a. El p oblema es á en que en es e caso se es a usando URDF con el in de
pode inclui dicho modelo de mane a di ec a al modelo e lecop e .xac o, y en dicho o ma o no exis e la
a iculación ipo ball.
La solución a es e p oblema eside en la encadenación de es a iculaciones o a i as, una po cada eje. Se
mues a en el código 5.4 la o ma de implemen a dicha solución.
...
<join name="Cha gePsi" ype="con inuous">
<pa en link="base_link"/>
<child link="Cha gePsi_link"/>
<axis xyz="1 0 0"/>
</join >
<link name="Cha gePsi_link">
<ine ial>
<mass alue="5e-6"/>
Figu a 5.5 D one con ca ga suspendida omado de [20]
37
<ine ia ixx="5.8083e-9" ixy="0" ixz="0" iyy="5.8083e-9" iyz="0"
izz="5.8083e-9"/>
</ine ial>
</link>
<join name="Cha geThe a" ype="con inuous">
<pa en link="Cha gePsi_link"/>
<child link="Cha geThe a_link"/>
<axis xyz="0 1 0"/>
</join >
<link name="Cha geThe a_link">
<ine ial>
<mass alue="5e-6"/>
<ine ia ixx="5.8083e-9" ixy="0" ixz="0" iyy="5.8083e-9" iyz="0"
izz="5.8083e-9"/>
</ine ial>
</link>
<join name="Cha gePhi" ype="con inuous">
<pa en link="Cha geThe a_link"/>
<child link="suspended_cha ge_link"/>
<axis xyz="0 0 1"/>
</join >
...
Código 5.4 F agme o del código cha ge.u d .xac o des inado al compo amien o dinámico de la ca ga
Se puede obse e que a los enlaces Cha gePsi_link, Cha geThe a_link y Cha gePhi_link se les asigna masas e
ine cias muy pequeñas con el in de que, en e a las masas e ine cias del es o del modelo, sean desp eciadas.
El lec o puede p egun a se el po que es necesa io en onces añadi dichos alo es, pudiendo es ablece los a
ce o. La espues a a es a cues ión eside en un a gumen o ealizado en el subapa ado 4.2.4, el cual explica que,
en el caso en el que un link posea alo es de masa o de ine cias nulos, Gazebo lo igno a á al ealiza la con e sión
de o ma o URDF a SDF.
Con es e segundo bloque se comple a el código de la ca ga suspendida, ob eniéndose el esul ado mos ado en
la igu a 5.6.
Figu a 5.6 E le-Cop e con sopo e y ca ga suspendida
Comunicación
44
44
No bas a solo con es o pa a que la simulación del E le-Cop e eciba odas aquellas o denes que se le den. Es
eque ida la c eación de un e minal MAVP oxy en el PCDRONE que u ilice ambién el pue o que se le ha
asignado a UDP en el QG oundCon ol. Pa a consegui es o, se u iliza el comando 6.3, en cual se deben indica
la IP del PCUSER y el mismo pue o que el u ilizado en la con igu ación del QG oundCon ol. Un ejemplo de
es e comando es el que apa ece en la igu a 6.4, que se co esponde ía con la con igu ación apo ada en la igu a
6.3.
ma p oxy.py --mas e =127.0.0.1:14551 --ou <IP PCUSER>:<PUERTO UTILIZADO>
Comando 6.3 Comando pa a la c eación de un e minal MAVP oxy
Figu a 6.3 Con igu ación de comunicaciones de
QG oundCon ol
Figu a 6.4 C eación de un e minal MAVP oxy

45
El p ocedimien o a segui pa a p oba es o comenza ía con el lanzamien o de la simulación del E le-Cop e jun o
con el MAVP oxy en el PCDRONE y, una ez ca gado odo co ec amen e, se debe conec a el
QG oundCon ol. T as es o la conexión se hab á es ablecido y se pod á c ea una misión de uelo, de iniendo
las di e en es o denes que debe cumpli el d one y que es e las ejecu e una as o a, o bien indicándole al mismo
paso a paso lo que ealiza , lo cual se co esponde ía con el en ío de comandos po e minal.
En la igu a 6.5 se mues a a la izquie da la sucesión de posiciones a segui que se le ha en iado desde
QG oundCon ol, y a la de echa apa ece el d one en la úl ima de dichas posiciones. An es de ealiza odo es o,
como es de espe a , es necesa io a ma y despega el d one, u ilizando pa a ello los pulsado es que apa ecen en
la pa e in e io de la pan alla pa a cada uno de los casos.
Figu a 6.5 Sucesión de pun os en QG oundCon ol (a iba) y simulación (abajo)
Comunicación
46
46
47
7 INTERFAZ DE CONTROL PROPIA
as habe es ablecido la conexión y habe p obado la misma según los dos mé odos que se han p esen ado, es
impo an e dispone de una pla a o ma basada en dicha comunicación que pe mi a un con ol o al de la
simulación, en la cual el usua io enga la posibilidad de implemen a di e sos bucles de con ol y aplicá selos al
d one, median e la que se pueda, ambién, lle a a cabo el con ol de la ca ga con el in de que es a no se balancee
cons an emen e, o simplemen e, que pe mi a auna odos los concep os is os has a aho a sob e los di e en es
comandos aplicables en la simulación.
Es a in e az ha sido desa ollada po Jesús Lozano Rod iguez, pe sona que ha pe mi ido an o su uso en es e
p oyec o como la modi icación de la misma pa a consegui los obje i os que se pe siguen.
7.1. Componen es del p og ama
La in e az u ilizada se a a de un p og ama en C++ compues o po di e en es unciones e hilos ( h eads), donde
cada uno de ellos lle a a cabo una unción de e minada. A con inuación, se p esen a una isión gene al de es os
componen es, pe o sin ex ende se demasiado, ya que no es el obje i o de es e documen o lle a a cabo la
desc ipción uno a uno de cada de los dis in os elemen os de es e código y, además, po que sólo algunos de ellos
han sido obje o de modi icación du an e el desa ollo del p oyec o.
Los componen es del p og ama son los siguien es:
• Ca pe a CONTROL: aquí se encuen an, como es de espe a , odos los a chi os que in luyen de
alguna mane a en los p ocedimien os de con ol del d one. Den o de la misma se incluye el hilo de
con ol, median e el cual, como se expond á en el apa ado 8.1, apa ecen las sucesiones de comandos
que se le en ían al d one con el in de que lle e a cabo di e en es u inas de uelo. Además, es en dicho
di ec o io donde se deben inclui odos aquellos con olado es que se u ilicen en el código.
• Ca pe a CVISION: en es a ca pe a es án los códigos enca gados de de e minados p ocesos de
isualización en la in e az, los hilos p incipales se enca gan de ep esen a las imágenes cap adas po
las cáma as del E le-Cop e y, sob e ellas dibuja po ejemplo cie as líneas de e e encia pa a la ca ga,
o, en el caso de que se o dene el de ec a es a, dibuja una señal sob e la pelo a. Es a de ección es a
implemen ada con un il o de Kalman median e el cual se consigue segui la posición de la ca ga en
cada momen o, a a és de la es imación de la misma pa a cada ins an e, calculando el e o en e a la
posición eal y ac ualizando la posición del obje i o si uado sob e la ca ga en unción de es e. Hay dos
hilos de p ocesamien o, uno po cada una de las cáma as.
Además de dichos hilos, ambién hay a chi os con los que se consigue c ea en anas median e las
cuales ajus a los alo es de de ección de la ca ga o de la ayec o ia, y a chi os pa a mos a los alo es
de la cáma a o a isos de a mado del d one.
• Ca pe a GUI: incluye el hilo pa a inicia y ac ualiza la in e az g á ica o GUI (G aphical Use
In e ace).
• Ca pe a PLOTS: con iene las unciones enca gadas de g a ica los alo es de oll, pi ch y yaw an o
de la ca ga como del d one, además de los alo es que adquie e cada uno de los canales del E le-Cop e .
Cabe menciona que, aunque dichas g á icas es án disponibles, a la ho a de g a ica di e en es alo es
en es e p oyec o se ha p e e ido u iliza un mé odo di e en e que se explica á en el apa ado 8.2.
• Ca pe a ROS: aquí se encuen an odas las unciones que lle an a cabo labo es elacionadas con ROS,
es deci , ealizan au omá icamen e muchas de las a eas que se hacían en apa ados an e io es de o ma
manual, como el uso de los comandos necesa ios las ob enciones de las imágenes o las conexiones ía
MAVROS. Además, hay un hilo en el que se indican odas las susc ipciones y publicaciones que se
deben ealiza sob e de e minados opics de ROS, con el in de sob eesc ibi dichos alo es de la
simualción o adqui i la in o mación que se publica en ellos, como, po ejemplo, alo es de senso es
del d one.
T
In e az de Con ol P opia
48
48
• A chi o main.cpp: es e p og ama es el enca gado de i ejecu ando uno a uno los di e en es hilos del
p og ama.
• A chi o mainwindow.cpp: p og ama enca gado de incializa y con igu ación de odos los subsis emas
y de sus a iables, como pueden se los alo es booleanos que eciben las señales de ac i ación de los
pulsado es que posee la in e az, el colo que se de ec a á a a és de la cáma a pa a la ca ga y pa a la
ayec o ia.
Asi mismo, se es ablecen los pa áme os del d one as lanza la simulación, al y como se hacía en con
los comandos 3.6 y 4.1, además de o os muchos pa áme os que se desea que engan un alo especí ico
du an e la simulación.
A con inuación, se incluyen las ins ucciones necesa ias pa a a ende a los cambios del GUI con el in
de ac ualiza las señales que se le en íen, como el pulsado de los bo ones o la ac ualización de las
imágenes, de los senso es como la IMU, de los da os de la ca ga y del d one, o de los da os de
MAVLink.
Finalmen e, se es ablecen las elaciones necesa ias pa a ealiza las a eas co ec as an e la señalización
de que se ejecu en las mismas a a és de la in e az g á ica.
• A chi o sha ed_memo y.cpp: es e úl imo a chi o se a a de un código enca gado de o ganiza la
memo ia compa ida de odo el p og ama median e la ape u a y el cie e de di e en es mu ex.
Cada uno de es os a chi os se encuen a acompañado de o o p og ama con el mismo nomb e y ex ensión .h en
el cual se de inen a iables y cons an es que se án u ilizados en los a chi os de ex ensión .cpp co espondien es.
Se incluye a con inuación, a modo de esumen, una abla con la o ganización de dichos p og amas.
Tabla 7.1 P og amas de la in e az p opia
…/s c
CONTROL
pid.cpp
h ead_p ocessing_con ol.cpp
CVISION
Dialog_a isoARMED.cpp
Dialog_De ec a Obje o.cpp
Dialog_De ec a T ayec o ia.cpp
Dialog_ alo aspicam.cpp
kalman.cpp
h ead_p ocessing_c ision1.cpp
h ead_p ocessing_c ision2.cpp
GUI
h ead_p ocessing_gui.cpp
PLOTS
Dialog_Resul ados_Angulos_Ca ga.cpp
Dialog_Resul ados_Angulos_D one.cpp
Dialog_Resul ados_Canales_RC.cpp
qcus omplo .cpp
49
ROS
os_image.cpp
os_ma os_imu.cpp
os_ma os_se .cpp
os_ aspicam_se .cpp
subsc ibe_ma os_s a e.cpp
h ead_p ocessing_ os.cpp
main.cpp
mainwindow.cpp
sha ed_memo y.cpp
7.2. In e az g á ica
Como se ha explicado an e io men e, al ejecu a el código an e io , lo que se gene a es una in e az g á ica que
ac úa como cen o de mando o es ación de con ol de ie a y, desde la cual, se puede maneja an o un d one
eal como, en es e caso, la simulación. Su apa iencia es la de la igu a 7.1.
En la imagen se puede e como, una ez lanzada la simuación, se pueden isualiza las imágenes de las cáma as
en la pa e supe io .
Una ez que se ha lanzado una simulación, se debe pulsa el bo ón de Ac i a Sis ema, has a que comienza a
pa padea el indicado neg o in e io , mos ando que el au opilo o se encuen a conec ado desde ese momen o.
T as es o se pueden ac i a ambas cáma as en Ac i a Cáma a 1 y Ac i a Cáma a 2, donde la cáma a uno es
Figu a 7.1 Apa iencia de la in e az g á ica p opia

In e az de Con ol P opia
50
50
la in e io y la dos es la on al. Los dis in os pa áme os u ilizados pa a la cáma a in e io se pueden isualiza
con el bo ón Valo es de la Raspicam (Cama a 1).
Al pulsa De ec a Obje o (Ca ga) comenza á dicho p oceso y apa ece á una señal e de sob e la ca ga
cons an emen e, y se á cuando, una ez se haya implemen ado el con olado , se pod á pulsa Con ola Obje o
(Ca ga) pa a comenza dicha a ea. Sabiendo que la de ección de la ca ga se lle a a cabo en unción del colo
de la misma, con el bo ón Ajus e Obje o (Ca ga) a Con ola se pod á modi ica el mismo en el caso de que se
desee u iliza una onalidad di e en e. En es e caso, se hab á de con igu a dicha onalidad median e su código
de colo es, con ayuda de la en ana eme gen e.
Figu a 7.2 Pa áme os aspicam
Figu a 7.3 Ajus e Obje o (Ca ga) a Con ola
51
Además de es o, en el siguien e bloque de la in e az se pueden ap ecia dos pes añas di e en es. La p ime a de
ellas si e pa a indica un modo de uelo (GUIDED, LOITER, LAND, ALT HOLD, e c), el cual se es ablece
pulsando el bo ón Se Mode, siemp e y cuando la simulación se es é ejecu ando. La segunda pes aña iene un
uncionamien o simila , pe o en su caso se es ablecen misiones de uelo, es deci , dis in os modos de uelo
encadenados en e los cuales, po ejemplo, se puede ealiza un con ol de la ca ga, o cualquie cosa que el
usua io desee. También se dispone de un bo ón pa a a ma el d one (ARM).
Lo siguien e que se puede e es un g an núme o de indicado es, en e los que se incluyen, en e o os, el es ado
de la conexión, el modo de uelo ac ual, los alo es de los dis in os canales, la posición de la cáma a en
milíme os y píxeles o las elocidades, acele aciones y ángulos del d one.
Finalmen e, en la esquina in e io de echa hay es bo ones median e los que se pueden g a ica los canales, los
ángulos del d one y los ángulos de la ca ga.
Figu a 7.4 G á ica de la ca ga ob enida po la in e az
In e az de Con ol P opia
52
52
53
8 CONTROL
na ez p esen ada la in e az que se a a u iliza pa a los pos e io es expe imen os, se llega a es e úl imo capí ulo.
En el se explica án las modi icaciones del código del capí ulo an e io que han sido necesa ias pa a consegui
los obje i os que se plan ean a con inuación.
En es e momen o, se ha á una simulación en la que se le aplica án es elocidades de e e encia di e en es al
E le-Cop e an o en el eje x como en el eje y, y se analiza án los esul ados de la misma. Además, se a a explica
el p oceso median e el cual se consigue conoce la posición de la ca ga espec o a de e minados pun os de
e e encia especi icados po el usua io con el in de, a pa i de ahí, ob ene el e o en posición de la misma y
ac ua al espec o u ilizando pa a ello un con olado con el que in en a es abiliza la pelo a.
8.1. Con igu ación del código del p og ama
Tal y como se in odujo en el capí ulo 7, sob e el código adqui ido se han ealizado cie as modi icaciones con
el in de u iliza el mismo pa a ines especí icos, la mayo ía de los cuales se han basado en el código
h ead_p ocessing_con ol.cpp.
Es e a chi o di e encia en e dis in as u inas o misiones de uelo en unción de aquella que se le indica desde
el GUI c eado. Es a dis inción se implemen a median e bucles i den o de los cuales se le adjudica un caso a
cada una de las u inas. De es e modo, median e un bucle swi ch se ejecu a án las acciones p ede inidas
dependiendo del caso que se haya indicado. Po ejemplo, hay un caso pa a despega , o o pa a a e iza , un
e ce o en el que se despega se espe a un de e minado iempo y se a e iza, e c.
En es e p oyec o, se ha c eado un caso en especi ico en el cual, con la ayuda del uso de un con ado , se ealizan
las siguien es a eas:
• Al inicia se se es ablece el modo ALT HOLD y se ajus an los alo es de cada uno de los canales a 1500,
que es el alo en el que odos ellos se encuen an en eposo.
• A con inuación, se cambia el modo a LOITER, se a ma el E le-Cope y se sob esc ibe el canal del
Th o le, in oduciendo el alo 1600. Debido a es o, el d one comenza á a ealiza un mo imien o en
el eje z posi i o a una elocidad cons an e y no pa a á has a el momen o en que dicho alo uel a a
es ablece se en 1500. Como el lec o pod á pe cibi , es e p ocedimien o es en ealidad lo que se ejecu a
median e los comandos 3.8.
• Lo siguien e que se hace es sob eesc ibi el canal del Pi ch siguiendo la se ie 1500-1550-1600 en
in e alos de 500 i e aciones. Es as i e aciones son las que se miden g acias al con ado . Una ez que
se ealiza es e p oceso, se uel e a es ablece el alo del Pi ch en 1500.
• Después de es o se ealiza lo mismo, pe o en es e caso se sob esc ibe el canal co espondien e al Roll,
siguiendo el mismo pa ón de alo es en los mismos in e alos.
• Finalmen e se a e iza el d one.
Se incluye a con inuación el agmen o con el que se implemen a dicho p oceso.
U
Con ol
60
60
Los esul ados ob enidos pa a el mo imien o en el eje y son muy pa ecidos a los del caso an e io . Comen a
que, an e un inc emen o de la e e encia en elocidad po el canal del Roll, el d one se desplaza en el sen ido
nega i o del eje y, po lo que la posición e e en e al o igen del sis ema de e e encia omado dec ece, omando
alo es nega i os. Debido a es o, el alo de la elocidad ambién apa ece á como nega i o, lo que se aduce
en una elocidad posi i a en el sen ido nega i o del eje en cues ión. Con es o se consigue deduci que an e
inc emen os en la e e encia se p oduce un gi o en Roll posi i o, como se ap ecia en la g á ica e ce a, y
ice e sa.
Se obse a como an o en posición como en elocidad y ángulo se sigue de nue o co ec amen e a la e e encia.
Además, se uel en a p oduci las oscilaciones comen adas an e io men e en es e caso pa a el ángulo de Roll,
adqui iendo de nue o unos alo es máximos de ap oximadamen e 5 g ados.
Po úl imo, especi ica que, en é minos de elocidad, los iempos de subida pa a cada escalón son de 8.7
segundos, 8.8 segundos y 7.5 segundos, alcanzando as las mismas elocidades en o no a los 0.45 m/s, 1.73
m/s y 2.9 m/s.
Figu a 8.4 Es udio del mo imien o en el eje y

61
8.3. De ección de la ca ga
Con el obje i o de lle a a cabo un con ol de la ca ga, y ya que se dispone del mé odo de de ección de la ca ga,
al y como se puede e en la igu a 8.4, es necesa io conoce la localización de la misma espec o a su posición
de es abilidad, que se co esponde con el cen o de la imagen que se p esen a en la in e az g á ica.
Den o del caso 21 del a chi o h ead_p ocessing_con ol.cpp se encuen a de inido odo el p ocedimien o pa a
lle a a cabo dicha de ección además del con ol de la pelo a. Se p esen a en el código 8.2 un agmen o de dicho
caso.
Figu a 8.5 In e az g á ica con ca ga de ec ada
Con ol
62
62
case 21: //TakeOFF1m_CONTROL_CARGA_LAND
i (sha e_memo y->ge Con ola Obje o())
{
...
// Ge he Image cen e
ImageX = InImage.cols / 2;
ImageY = InImage. ows / 2;
// De ec Objec
Objec oX = sha e_memo y->ge Pun oXImg();
Objec oY = sha e_memo y->ge Pun oYImg();
// Mo e he o igin o he cen e o he image
X al = Objec oX - ImageX;
Y al = -(Objec oY - ImageY);
// Change om ca esian coo dina es o pola coo dina es
Radius = sq ((X al)^2+(Y al)^2); // Dis ance om he objec o he o igin
The a = a an2(Y al,X al); // Angle o he objec ela i e o he o igin
i (The a > 0 && The a < M_PI_2) // Cuad an e supe io de echa
{
i (Radius <= 10) // Ci culo pequeño
{
// Quie o
max_ oll = BASERC;
// Quie o
max_pi ch = BASERC;
} else i ( Radius > 10 && Radius <= 100) // Ci culo g ande
{
// Hacia de echa
max_ oll = 1525;
// Hacia adelan e
max_pi ch = 1475;
} else i (Radius > 100) // Fue a del Ci culo g ande
{
max_ oll = 1550;
// Hacia adelan e
max_pi ch = 1450;
}
} else i (The a > M_PI_2 && The a < M_PI) // Cuad an e supe io izquie da
{
i (Radius <= 10) // Ci culo pequeño
{
// Quie o
max_ oll = BASERC;
// Quie o
max_pi ch = BASERC;
} else i ( Radius > 10 && Radius <= 100) // Ci culo g ande
{
// Hacia izquie da
max_ oll = 1475;
// Hacia adelan e
max_pi ch = 1475;
} else i (Radius > 100) // Fue a del Ci culo g ande
{
// Hacia izquie da
max_ oll = 1450;
// Hacia adelan e
max_pi ch = 1450;
}
}
...
<CUADRANTES INFERIORES>
...
// Calcula e Roll and Pi ch depending on he mode
i (mode_con ol == "LOITER")
{
au o_Roll = max_ oll;
au o_Pi ch = max_pi ch;
63
} else {
au o_Roll = BASERC;
au o_Pi ch = BASERC;
}
// Limi he Roll
i (au o_Roll > MAXRC)
{
au o_Roll = MAXRC;
} else i (au o_Roll < MINRC)
{
au o_Roll = MINRC;
}
// Limi he Pi ch
i (au o_Pi ch > MAXRC)
{
au o_Pi ch = MAXRC;
} else i (au o_Pi ch < MINRC)
{
au o_Pi ch = MINRC;
}
...
sha e_memo y->se Pi ch(au o_Roll);
sha e_memo y->se Roll(au o_Pi ch);
sha e_memo y->se Th o le(BASERC);
}
b eak;
}
Código 8.2 F agmen o del caso 21 del código h ead_p ocessing_con ol.cpp
Lo p ime o que se ealiza es la oma de la imagen y la de inición de su cen o. Se ha p ede inido que dicha
imagen se á de 640x360, po lo que el cen o se localiza en la posición (320,180), pa iendo del pun o (0,0) que
se encuen a en la esquina supe io izquie da. A con inuación, se ob iene la posición de la ca ga, y se calcula el
e o de dicha posición espec o al cen o de la imagen.
La imagen, al y como se mues a en la igu a 7.2, se di ide en di e en es egiones con ayuda de unas ma cas
ealizadas sob e la misma. Conc e amen e, se dis inguen los cua o cuad an es y dos ci cun e encias que di iden
cada cuad an e en es egiones di e en es. Den o de cada cuad an e, po an o, se dis ingui án es posibles
es ados de la ca ga, que son, cen ada, en cuyo caso se encon a ía den o del cí culo de meno adio, a media
dis ancia, si es á posicionada en e el cí culo pequeño y el g ande, o lejos, si se encuen a ue a de ambos cí culos.
Dependiendo de es a dis ancia y del cuad an e en el que es e si uada, el d one debe á lle a a cabo una al e ación
del alo el canal de Pi ch y Roll de mane a co ec a con el obje i o de posiciona la ca ga en el cen o.
Aho a bien, la de ección de la posición de la ca ga se puede implemen a de dos mane as di e en es. Una de
ellas se ía comp obando si los e o es en x e y adquie en alo es nega i os o posi i os, de modo que asi se
iden i ica ía el cuad an e, y a con inuación, comp oba el alo absolu o de dichos e o es pa a calcula la
dis ancia que sepa a a la ca ga del cen o, deducindo así en que egión se encuen a. La o a posibilidad es
median e el uso de coo denadas pola es, que es lo implemen ado en el código 8.2. Lo p ime o que hab ía que
hace es aslada el o igen de la esquina supe io izquie da al cen o de la imagen y, a con inuación, ealiza la
con e sión de coo denadas ca esianas a pola es. T as es o, median e el ángulo he a se iden i ica ía el cuad an e
mien as que con la ayuda del adio se calcula ía la egión donde es a si uada la ca ga.
Con ol
64
64
En onces, como se e en el código 8.2, median e bucles se consigue ealiza es a dis inción de egiones, y den o
de cada caso especí ico se sob esc iben los canales de Pi ch y de Roll con alo es de e minados. El mo imien o
del d one se ige de la siguien e mane a:
• Canal Pi ch > 1500: desplazamien o hacia a ás.
• Canal Pi ch < 1500: desplazamien o hacia adelan e.
• Canal Roll > 1500: desplazamien o hacia la de echa.
• Canal Roll < 1500: desplazamien o hacia la izquie da.
En onces dependiendo de la posición de la ca ga, se aplica án mo imien os de la siguien e mane a con el in de
cen a la ca ga:
• Ca ga en cuad an e supe io izquie do: mo imien o hacia adelan e y hacia la izquie da.
• Ca ga en cuad an e supe io de echo: mo imien o hacia adelan e y hacia la de echa.
• Ca ga en cuad an e in e io izquie do: mo imien o hacia a ás y hacia la izquie da.
• Ca ga en cuad an e in e io de echo: mo imien o hacia a ás y hacia la de echa.
Los alo es aplicados en los co espondien es canales depende án de la dis ancia a la que es é la pelo a del
cen o, de es e modo:
• Ca ga en cí culo pequeño: 1500.
• Ca ga en e el cí culo pequeño y el g ande: escalón de 25 hacia a iba o hacia abajo pa iendo de 1500.
• Ca ga ue a de ambos cí culos: escalón de 75 hacia a iba o hacia abajo pa iendo de 1500.
Es e mé odo se a a ía de un con ol odo o nada, que es bas an e imp eciso. Una o ma de mejo a lo se ía la
implemen ación de un con ol p opo cional, con el cual, dependiendo del e o pa a cada uno de los ejes, se le
aplica ía una e e encia de elocidad especí ica al canal del pi ch o al del oll, en unción ambién de la ganancia
p opo cional que se le aplique al con olado . El código 8.3 es el implemen ado pa a ello.
En el se puede ap ecia como pa a el caso del e o en el eje x se lle a a cabo un con ol p opo cional pa a el
oll, limi ando la ac uación a un mínimo de 1525, y de igual mane a se hace pa a el caso del e o en el eje y.
Una ez es ablecidos los lími es en e los que abaja ía la ganancia p opo cional median e la ecuación 8.1, se
ajus ó la misma has a ob ene un compo amien o ela i amen e co ec o.Equa ion Chap e 8 Sec ion 1
𝐾𝑃=∆𝑈
𝑒
(8.1)
Aún así, el esul ado ob enido as la ejecución de dicho p og ama sob e la simulación sigue sin se el mejo de
Figu a 8.6 Posición de la ca ga en ca esianas (izquie da) y en pola es (de echa) omado de [19]
65
odos. Es o se debe a que un con ol p opo cional no consigue anula los e o es en égimen pe manen e, po lo
que la ca ga pe manece á oscilando, eso si, de mane a menos ag esi a, as el con ol de la misma.
case 21: //TakeOFF1m_CONTROL_CARGA_LAND
i (sha e_memo y->ge Con ola Obje o())
{
...
// Ge he Image cen e
ImageX = InImage.cols / 2;
ImageY = InImage. ows / 2;
// De ec Objec
Objec oX = sha e_memo y->ge Pun oXImg();
Objec oY = sha e_memo y->ge Pun oYImg();
// Calcula e he e o be ween Image cen e and Objec o cen e
E X = Objec oX - ImageX;
E Y = Objec oY - ImageY;
Kp = 0.45;
i (abs(E X) <= 10)
Roll = BASERC;
else
{
PROPX = Kp*E X;
i (abs(PROPX) < 25)
PROPX = 25;
Roll = PROPX + 1500;
}
i (abs(E Y) <= 10)
Pi ch = BASERC;
else
{
PROPY = Kp*E Y;
i (abs(PROPY) < 25)
PROPY = 25;
Pi ch = PROPY + 1500;
}
// Calcula e Roll and Pi ch depending on he mode
i (mode_con ol == "LOITER")
{
au o_Roll = Roll;
au o_Pi ch = Pi ch;
} else {
au o_Roll = BASERC;
au o_Pi ch = BASERC;
}
// Limi he Roll
i (au o_Roll > MAXRC)
{
au o_Roll = MAXRC;
} else i (au o_Roll < MINRC)
{
au o_Roll = MINRC;
}
// Limi he Pi ch
i (au o_Pi ch > MAXRC)
{
au o_Pi ch = MAXRC;
} else i (au o_Pi ch < MINRC)
{
au o_Pi ch = MINRC;
}
...
sha e_memo y->se Pi ch(au o_Roll);
sha e_memo y->se Roll(au o_Pi ch);
sha e_memo y->se Th o le(BASERC);
}
b eak;
}
Código 8.3 Con ol p opo cional de la ca ga

Con ol
66
66
8.4. PID pa a con ol de ca ga
T as el es udio an e io , se pod ía implemen a un con olado PID pa a es abiliza la ca ga, pa a lo cual se
end ía que ealiza algún expe imen o.
En es e caso, se end ía una caja neg a basada en el sis ema del d one más la ca ga suspendida, cuya en ada
se ía la e e encia de elocidad po el canal del oll o po el canal del pi ch, y la salida se ía la posición en x o en
y, espec i amen e, de la ca ga espec o al cen o de la imagen.
Debido a es o, el expe imen o pod ía consis i en la aplicación de un escalón en la e e encia de elocidad del
canal del pi ch, espe ando a con inuación a que la ca ga se es abilice en un pun o de la imagen. El
compo amien o de la ca ga debe ía se simila al de un sis ema de segundo o den subamo iguado, po lo que
g a icando los alo es omados po la misma en el eje y (pa a el caso del pi ch) y es udiando dicha g á ica, se
pod ía ob ene la unción de ans e encia del compo amien o de la ca ga y, a pa i de es a, ob ene los
pa áme os de un con olado PID adecuado pa a el con ol de la misma.
A con inuación, se ealiza ía el mismo expe imen o pa a el caso del oll.
Ya que el compo amien o de la ca ga no es ideal, es p obable que una en ada en escalón no se aduzca en un
desplazamien o de la ca ga únicamen e en el eje co espondien e, po lo que, si el esul ado del expe imen o
an e io no es del odo ace ado, se pod ía in en a ealiza el mismo, pe o an e una en ada impulsional.
Una ez ob enidos los pa áme os que de inen un con olado PID, es deci , las ganancias p opo cional, in eg al
y de i a i a (Kp, Ki y Kd), se p ocede ía a apo a sus alo es al código PID implemen ado.
Los es udios de es os compo amien os y sus esul ados se án obje o de in es igaciones pos e io es a las que en
es e p oyec o se abo dan.
67
9 CONCLUSIÓN
inalmen e se ha conseguido dispone de un mundo i ual en el que apa ece el E le-Cop e jun o con una ca ga
suspendida de el a 60 cm de dis ancia, y con un compo amien o dinámico muy simila al que adquie e el sis ema
eal. Además, se ha conseguido con e i el o denado en el que dicho en o no se encuen a mon ado en una
máquina independiende de aquel desde el cual el usua io con ola la simulación. De es e modo, se u iliza dicha
simulación como si se del d one eal se a ase, con la en aja de pode p oba di e en es eje cicios sin co e el
iesgo de que se es elle y pueda daña se algún componen e.
Po úl imo, se ha in oducido el p oyec o en el ámbi o del con ol de la ca ga, ealizando es udios del
compo amien o de los con olado es que u iliza el modo LOITER o ecido po el au opilo o que se encuen a
ins alado, es ableciendo un p ocedimien o median e el cual se lle a a cabo un econocimien o de la ca ga y una
deducción de su posición espec o a su posición de es abilidad e indagando en la o ma median e la que se
pod ían es ablece los pa áme os de en ada de un supues o con olado PID que se implemen ase con el
obje i o de es abiliza la pelo a.
En esumen, se piensa habe alcanzado sa is ac o iamen e los obje i os p incipales del p oyec o, los cuales se
a aban del modelado del en o no i ual, y habe ampliado dichos obje i os con la comunicación, los es udios
de compo amien o y las bases pa a las ac i idades de con ol a ealiza . Pa a odos es os obje i os se ha
necesi ado pone en p ác ica muchos concep os ap endidos du an e es os cua o años, pe o, además, se conside a
habe ampliado conside ablemen e el conocimien o en el ámbi o del mundo de los UAVs.
Las u u as ampliaciones y líneas de in es igación haciendo uso del sis ema apo ado pod ían basa se en mejo as
en el modelado de la simulación, haciendo la misma más p ecisa y ealis a, e oques sob e los con olado es del
E le-Cop e con el in, po ejemplo, de elimina esas oscilaciones que se p oducían en los ángulos de pi ch y
oll, la implemen ación de con olado es pa a la ca ga, an o pa a man ene es able la misma como pa a
consegui que se man enga en un pun o de e minado mien as se desc ibe una de e minada ayec o ia o u iliza
el mismo sis ema de econocimien o de la ca ga pa a calcula el pun o donde se debe deposi a la ca ga, siendo
dicho pun o obje i o una pla a o ma ija o mó il, como po ejemplo, un ehículo e es e.
F
Conclusión
68
68
69
ÍNDICE DE TABLAS
Tabla 2.1 Ca ac e ís icas come ciales del E le-Cop e 3
Tabla 4.1 Elemen o <gazebo> pa a la e ique a < obo > 19
Tabla 4.2 Elemen o <gazebo> pa a la e ique a <link> 20
Tabla 4.3 Elemen o <gazebo> pa a la e ique a <join > 20
Tabla 7.1 P og amas de la in e az p opia 48
Índice de Códigos
76
76

77
BIBLIOGRAFÍA
[1] Docs de E le Robo ics. Disponible en h p://docs.e le obo ics.com/
[2] Fo o de E le Robo ics. Disponible en h p:// o um.e le obo ics.com/
[3] Ins alación del en o no del E le-Cop e .
Disponible en h p://docs.e le obo ics.com/simula ion/con igu ing_you _en i onmen
[4] He amien as de MAVROS. Disponible en h ps://e le obo ics.gi books.io/e le- obo ics-ma - ools-
ee/con en /en/
[5] Modos de uelo E le-Cop e . Disponible en h ps://e le obo ics.gi books.io/e le- obo ics-e le-
cop e /con en /es/ ligh _modes/index.h ml
[6] Página o icial A dupilo . Disponible en h p://a dupilo .o g/a dupilo /
[7] Pa áme os de A dupilo . Disponible en h p://a dupilo .o g/cop e /docs/pa ame e s.h ml
[8] Guía PX4. Disponible en h ps://www.gi book.com/book/px4/ i mwa e-de guide/de ails
[9] Wiki de ROS. Disponible en h p://wiki. os.o g/
[10] Fo o de ROS. Disponible en h ps://discou se. os.o g/
[11] Solución al p oblema de ins alación. Disponible en h p://answe s. os.o g/ques ion/141151/building-
os_con ol-on-hyd o/
[12] Fo ma o URDF. Disponible en h p://wiki. os.o g/u d
[13] Fo ma o SDF. Disponible en h p://sd o ma .o g/spec
[14] Tu o iales de Gazebo. Disponible en h p://gazebosim.o g/ u o ials
[15] Fo o de Gazebo. Disponible en h p://answe s.gazebosim.o g/ques ions/
[16] Guía MAVROS. Disponible en h p://wiki. os.o g/ma os
[17] Guía MAVLink. Disponible en h p://www.qg oundcon ol.o g/ma link/s a
[18] Guía del usua io QG oundCon ol. Disponible en h ps://donlake lye .gi books.io/qg oundcon ol-
use -guide/con en /en/
Bibliog a ía
78
78
[19] Localización de la ca ga. Disponible en h p://www.ludep.com/d one-new-pid-wi h-pola -coo dina es-
and-how o-imp o e- eac i i y-and-accu acy/
[20] I ana Palunko, Ra ael Fie o, and Pa icio C uz, T ajec o y gene a ion o swing- ee maneu e s o a
quad o o wi h suspended payload: A dynamic p og amming app oach, Robo ics and Au oma ion
(ICRA), 2012 IEEE In e na ional Con e ence on.
[21] F anz Bahne , Modeling, Simula ion and con ol o a quadcop e ca ying a slung load
[22] Oc a io Al edo Ga cía Campos, Modelado, simulación y con ol de un quad o o pa a anspo e de
ca ga colgan e. Ex ensión al caso idimensional, P oyec o de Fin de Ca e a, Dep. Ingenie ía de
Sis emas y Au omá ica. Escuela Técnica Supe io de Ingenie ía. Uni e sidad de Se illa, 2017.