APLICACIÓN DE LA INTELIGENCIA ARTIFICIAL
AL CONTROL DE UN ROBOT LEGO EN
CONFIGURACIÓN DE PÉNDULO INVERTIDO
Po
E a Gala de Pablo
UNIVERSIDAD COMPLUTENSE
MADRID
Doble G ado en Ma emá icas e Ingenie ía In o má ica
Facul ad de In o má ica
Di igido po :
Ma ilde San os Peñas
Mad id, 2018-2019
Resumen
Es e es udio p esen ó el diseño y la implemen ación de un obo basado en Lego Minds o m EV3
imi ación de un péndulo e ical in e ido que es capaz de man ene se en equilib io, a anza y
de ec a o mas y colo es. Así mismo, se con eccionó un modelo ma emá ico análogo al p o o ipo
median e un espacio de es ados y se e isó y simuló el sis ema con un egulado lineal cuad á ico
(LQR) y un con olado p opo cional, in eg al y de i a i o (PID). Se implemen ó el código
uncional en Ja a leJOS. Finalmen e el obo ue capaz de man ene se en equilib io, e i a
obs áculos y econoce colo es a lo la go de su eco ido.
Abs ac
This esea ch p esen s he design and implemen a ion o a obo based on Lego Minds o m EV3.
The obo is a ep oduc ion o an in e ed e ical pendulum ha is s abilized, mo es o wa d
and no ices shapes and colou s. To his end, a ma hema ical model analogous o he p o o ype
was s ablished h ough a s a e-space ep esen a ion. Addi ionally, he sys em was e iewed and
simula ed wi h a linea quad a ic egula o and a p opo ionalin eg alde i a i e con olle .
The code was implemen ed in Ja a leJOS. The esul ing con olle allowed he EV3 obo o
s ay s eady, a oid obs acles and de ec colou s along i s pa h.
Palab as cla e
Péndulo in e ido, Espacio de es ados, obo balancín, Lego Minds o m EV3, Regulado LQR,
Con olado PID.
Keywo ds
In e ed pendulum, S a e-space ep esen a ion, balancing obo , Lego Minds o m EV3, LQR,
PID Con ole .
i
Índice gene al
Resumen
i
1. In oducción 1
1.1. P opósi o ........................................ 1
1.2. Plan eamien o...................................... 1
1.3. Desc ipcióndelp oblema ............................... 2
1.4. An eceden es ...................................... 3
1.4.1. An eceden es eó icos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3
1.4.2. An eceden es aplicados . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
1.5. Obje i os ........................................ 6
1.6. Plande abajo..................................... 6
2. Modelo ma emá ico 9
2.1. Análisis de las ue zas y ob ención de ecuaciones . . . . . . . . . . . . . . . . . . 9
2.1.1. Linealización del modelo . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
2.1.2. Diag amadees ados.............................. 12
2.1.3. Función de T ans e encia . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
3. Con olado es 15
3.1. Con olado lineal cuad á ico . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
3.2. Con olado P opo cional, In eg al y De i a i o . . . . . . . . . . . . . . . . . . . 17
4. Simulación 19
4.1. Sis emadelazoabie o................................. 19
4.1.1. Sis ema de lazo ce ado . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20
4.1.2. Sis ema con con ol LQR . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
4.1.3. Simulación con con ol LQR y PID . . . . . . . . . . . . . . . . . . . . . . 22
5. Lenguajes de p og amación 25
5.1. EV3-G.......................................... 25
5.2. Robo C ......................................... 26
5.3. EV3de
Py hon
..................................... 26
5.4. LeJOSJa a....................................... 27
6. Codicación 29
6.1. Main........................................... 29
6.2. Robo .......................................... 30
6.2.1. Mo o es..................................... 30
6.2.2. Gi oscopio.................................... 30
6.2.3. G ácos..................................... 31
6.2.4. De ec o deColo ................................ 31
6.3. Equilib io ........................................ 34
6.3.1. Aplicación del modelo ma emá ico . . . . . . . . . . . . . . . . . . . . . . 34
6.3.2. LQR....................................... 34
iii

6.3.3. PID ....................................... 36
6.3.4. Resul ados ................................... 37
6.4. E i ado ......................................... 38
6.5. Con ol ......................................... 38
7. Conclusiones y u u os p oyec os 39
7.1. Conclusiones ...................................... 39
7.2. Fu u osp oyec os.................................... 40
A. Código 43
A.1. Código del sis ema (MATLAB) . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43
A.2.CódigoEV3de enPy hon............................... 43
Dedica o ia
Mamá, aunque empezas e es e p oyec o conmigo y is e a Gsus camina , no pudis e e le co-
e ...
Capí ulo 1
In oducción
La ma e ia en la que se cen a es e p oyec o es la cons ucción y el con ol de un sis ema de
péndulo in e ido. Es a cues ión ísica es conocida po se uno de los p oblemas más ep esen a-
i os y habi uales en la eo ía de con ol. El p oyec o engloba la cons ucción, el modelado y la
p og amación de un obo Lego Minds o ms EV3
R

(EV3 o obo , de aho a en adelan e).
1.1. P opósi o
La elección de es e p oyec o como abajo de n de g ado iene mo i ado po la posibilidad de
p o undiza en la eo ía de con ol; un campo mul idisciplina io y ans e sal en el que in e ie-
nen an o las ciencias de la compu ación como las ma emá icas. Po ejemplo, en el es udio de
sis emas dinámicos se equie e la aplicación de ecuaciones di e enciales y conocimien os de álge-
b a a anzado. Po o o lado, el desa ollo de es e p oyec o equie e de una p og amación sólida
y en a ios lenguajes; y la p opia cons ucción del obo equie e unos conocimien os básicos en
obó ica.
1.2. Plan eamien o
El plan eamien o inicial que se hizo del p oyec o ue, po pa e de la di ec o a como el siguien e:
Es e abajo esponde a la p opues a del concu so de con ol in eligen e, o ganizada po el
g upo emá ico de Con ol In eligen e del Comi é Español de Au omá ica.
El Con ol In eligen e nace con la in ención de aplica las écnicas de In eligencia A icial a los
p oblemas de con ol. La In eligencia A icial en sí es un campo amplio que aba ca lógica, op imi-
zación, p obabilidad, pe cepción, azonamien o, oma de decisiones, ap endizaje, e c. El p oyec o
consis e en aplica pa adigmas de con ol in eligen e a una plan a eal. El sis ema a con ola
es un Lego en congu ación de péndulo in e ido. El p o o ipo es á disponible en la Facul ad de
In o má ica. Una ez ealizado el mon aje de la plan a, se a a de diseña e implemen a en el
sis ema un con olado basado en alguna de las écnicas de con ol in eligen e (lógica uzzy, edes
neu onales, e c.) o una combinación de a ias de ellas. Pa a alida el con ol, el sis ema debe
ealiza un eco ido a lo la go de un ci cui o con olando en odo momen o el ángulo del péndulo
1
1. In oducción
8

Capí ulo 2
Modelo ma emá ico
En es a ase del p oyec o se p ocu ó encon a una exp esión ma emá ica que ep esen ase el
compo amien o ap oximado del sis ema. Es necesa io encon a un equilib io en e un modelo
able y una complejidad acep able. Pa a ello, se comenzó con un es udio simple de las ue zas
exis en es en un péndulo in e ido es ánda .
2.1. Análisis de las ue zas y ob ención de ecuaciones
Pa a pode ob ene un buen modelo del sis ema, se debe analiza po sepa ado cada uno de los
cue pos del modelo que se han explicado en la in oducción de la memo ia: péndulo y ca o. Así
bien, cuando es e sis ema se lle a a la p ác ica con el EV3, sucede que es as pa es no es án
sepa adas ísicamen e.
Símbolo Signicado Unidades
m
Masa del péndulo
kg
M
Masa del ca o
kg
k
Cons an e de icción del ca o con el suelo
m·kg/s
c
Cons an e de icción en la unión del péndulo y ca o
m·kg/s
L
Longi ud del péndulo al cen o de g a edad
m
I
Ine cia del péndulo
kg ·m2
θ
Ángulo que o ma el péndulo con la e ical
ad
x
Desplazamien o del ca o en la ho izon al
m
F
Fue za aplicada al ca o
N
Cuad o 2.1: Tabla con las a iables que se usa án en el modelo
El péndulo in e ido se puede deni y modela como un cue po ígido p esen e solo en dos
dimensiones. La abla 2.1 mues a las a iables que se u iliza on y lo que ep esen a cada una
y la Figu a 2.1 ep esen a el diag ama de ue zas que se eje cen sob e el péndulo in e ido. Se
ob ienen las componen es de las ue zas en es as dos dimensiones con las ecuaciones (2.1) y (2.2).
XFi=mai
(2.1)
XFj=maj
(2.2)
Es as ecuaciones ep esen an las ue zas de un mo imien o plano en un cue po, de acue do con la
9
2. Modelo ma emá ico
Figu a 2.1: Diag amas de cue po del sis ema
Segunda Ley de New on. También se ha de conside a la Ve sión Ro acional de la Segunda Ley
de New on, la ecuación (2.3). Siendo
Fi
y
Fj
la suma de ue zas en di ección e ical y ho izon al
y
τ
el o que, o la ue za eje cida sob e el eje de gi o. [4]
τ=Iα
(2.3)
Empezando po el péndulo, se pueden encon a dos acele aciones ep esen adas como
ac
y
an
en la gu a 2.1. La acele ación asociada al mo imien o lineal del ca o (
ac
) es ho izon al y la
acele ación asociada al mo imien o o a o io (
an
) sigue la di ección del ángulo de gi o
θ
.
Tomando las ue zas y acele aciones ho izon ales en el péndulo y aplicando las ecuaciones (2.1)
y (2.2) se ob ienen las ecuaciones (2.4) y (2.5), espec i amen e. Aquí se conside a
H
y
V
como
la suma de ue zas ho izon ales y las e icales que se eje cen sob e el péndulo espec i amen e.
[4]
H=m·d2x
d 2+d2L·sin(θ)
d 2
(2.4)
V−mg =m·d2L·cos(θ)
d 2
(2.5)
Si se es udian las ue zas asociadas al mo imien o o a o io en el péndulo, con la ecuación (2.3),
se ob iene la ecuación (2.6). En es e caso se ep esen a
dθ
d
y
d2θ
d 2
como
˙
θ
y
¨
θ
pa a simplica la
no ación. [4]
V L sin(θ)−HL cos(θ)−c˙
θ=I·¨
θ
(2.6)
Po o o lado, cabe des aca que en la ecuación (2.6), el é mino
c˙
θ
es la ue za asociada a la
icción de la unión del péndulo con el ca o. Como en el EV3 no exis e al unión, se conside a
que no se debe de ene en cuen a y se ja una ap oximación
c≈0
.
Y, po úl imo, se calculan las ue zas ho izon ales de (2.1) que se eje cen sob e el ca o en la
ecuación (2.7). [4]
10
2.1 Análisis de las ue zas y ob ención de ecuaciones
F−H=M¨x+k˙x
(2.7)
El e mino
k˙x
hace e e encia a la icción del ca o con a el suelo. Siguiendo la línea de muchas
uen es consul adas, se usa la ecuación de la icción como si ue a icción iscosa. Es e en oque
se debe a que la icción de la ueda con el suelo es p ác icamen e inexis en e y la única icción
que se iene que conside a es la icción p opia del mo o que, al se lub icada, se co esponde
con una icción iscosa. Po ello se es ablece que la icción es p opo cional a la elocidad del
cue po [4].
En el a ículo [16] se puede e un es udio de los di e en es ipos de icción que se pueden
conside a y, con ello, los esul ados ob enidos. En el caso de sólo conside a una icción iscosa
en la modelización, se deduce que los con olado es ienden a oscila lige amen e. Sin emba go,
en las conclusiones del a ículo se admi e que añadi el ozamien o con el suelo, aunque mejo a
lige amen e los esul ados, complica el modelo y sigue p esen ando uido, p obablemen e debido
a los senso es.
Aho a, pa a acla a el o igen de las ecuaciones p incipales de las que se ob end á el modelo, se
p ocede a calcula las de i adas ap opiadas y simplica la no ación. De las ecuaciones (2.4) y
(2.5) se ob ienen las ecuaciones (2.8) y (2.9). [5]
H=m¨x+mL ¨
θcos(θ)−˙
θ2sin(θ)
(2.8)
V−mg =−mL ¨
θsin(θ) + ˙
θ2cos(θ)
(2.9)
La exp esión de
¨x
se ob iene de la suma de (2.8)
+
(2.7). Pa a ob ene
¨
θ
, (2.6)
−Lcos(θ)
(2.8)
+
Lsin(θ)
(2.9).
¨x=1
m+M·hF−k˙x−mL ¨
θcos(θ) + ˙
θ2sin(θ)i
(2.10)
¨
θ=1
I+L2m·[mL (gsin(θ)−¨xcos(θ))]
(2.11)
2.1.1. Linealización del modelo
Pa a el diseño de un con olado se a a usa una e sión lineal de las ecuaciones que se han
ob enido an e io men e.
Como el péndulo se encuen a es able solo en la e ical, se puede conside a que, idealmen e,
siemp e es a á en un es ado ce cano. Po ello, se ap oxima
θ≈0
y con ello se aplican las
ap oximaciones de ángulos pequeños ales como
sin(θ)≈θ
,
cos(θ)≈1
y
˙
θ2θ≈0
[4]. Se ob ienen,
así, las dos ecuaciones an e io es ((2.10) y (2.11)) linealizadas:
¨x=1
m+M·hF−k˙x−mL¨
θi
(2.12)
11
2. Modelo ma emá ico
¨
θ=1
I+L2m·[mL (gθ −¨x)]
(2.13)
Es impo an e ene en cuen a que, al usa es as ap oximaciones de ángulos pequeños, se es á
ap oximando las se ies de Taylo del
cos(θ)
y
sin(θ)
al p ime é mino. Pe o, pa a que la expansión
no con enga é minos en po encia de
π/180
, en
θ
se ha denido los ángulos en adianes. Po ello,
se ja las unidades del modelo a adianes.[17]
2.1.2. Diag ama de es ados
Un diag ama de es ados es una ep esen ación de un modelo ma emá ico de un sis ema ísico
median e un conjun o de en adas, salidas y a iables de es ado. Es as en adas y salidas es-
án elacionadas median e ecuaciones di e enciales que se combinan en una ecuación di e encial
ma icial de p ime o den. La ep esen ación de un modelo de es a o ma p opo ciona un modo
compac o y con enien e de modela un sis ema con a ias en adas y salidas.[18]
En nues o caso se ienen 4 en adas y 4 salidas, que o man un sis ema de 4 dimensiones.
Pa a ob ene un espacio de es ados lineal álido a a és de las ecuaciones ob enidas an e io men e
es necesa io sus i ui
¨
θ
en la ecuación (2.12) usando la ecuación (2.13) y, de la misma o ma,
sus i ui
¨x
en la ecuación (2.13) usando la ecuación(2.12). [4,18]






˙
θ
¨
θ
˙x
¨x






=






0 1 0 0
Lmgp10 0 Lmkp1
M+m
0 0 0 1
−(Lm)2gp2
I+L2m0 0 −kp2






·






θ
˙
θ
x
˙x






+






0
−Lmp1
M+m
0
p2






·F
(2.14)
Con
p1=M+m
I(M+m)+L2mM
y
p2=I+L2m
I(M+m+L2mM)
.
Pa a simplica la no ación, se enomb an las ma ices de la ecuación (2.14) como
A
y
B
, el
ec o
[θ˙
θ x ˙x]T
como
u( )
y se incluye una ecuación que ep esen e la salida del sis ema
( )
:
˙u( ) = Au( ) + BF( )
(2.15)
( ) = Cu( ) + DF( )
(2.16)
Donde
C∈R4×R4
es la ma iz iden idad y
D∈R4
es un ec o de ce os.
2.1.3. Función de T ans e encia
A pa i del sis ema de es ados de la sección an e io se ob u o la unción de ans e encia.
P ime o, de la ecuación (2.15) se ob iene la ans o mada de Laplace (
R´ın
0 ( )e−s d
) con con-
diciones iniciales ce o. [18]
U( ) = AU( ) + B∗V( )
(2.17)
V( ) = CU( )
(2.18)
12
2.1 Análisis de las ue zas y ob ención de ecuaciones
Y, de la ecuación (2.17), eo denando y sus i uyendo en (2.18) se ob iene
U( ) = C( I−A)−1BV( )
(2.19)
Con es o, se log a la unción de ans e encia. [18]
H( ) = U( )
V( )=C( I−A)−1B
(2.20)
El denominado de la unción de ans e encia se hace ce o cuando
( I−A)
es ce o. Lo que es
equi alen e a que los polos de la unción de ans e encia son los au o alo es de la ma iz
A
.
[19,18,5]
Es a a mación es especialmen e ú il a la ho a de comp oba la ines abilidad del sis ema.
13

2. Modelo ma emá ico
14
Capí ulo 3
Con olado es
Pa a asegu a un co ec o uncionamien o del disposi i o y un nal uncional, se aplicó un desa-
ollo inc emen al del con olado . Es deci , p ime o se in en ó ob ene un con olado LQR
uncional y después mejo a és e con un con olado PID.
En es e capí ulo, se expond á la base eó ica que se equie e pa a en ende los con olado es que
después se han implemen ado. Pe o, pa a pode en ende es a base eó ica se end án que deni
dos concep os undamen ales:
Sis ema de lazo abie o
Un sis ema en el que la salida del sis ema no iene ningún e ec o
sob e la acción del sis ema. [18]
Sis ema de lazo ce ado o ealimen ado
Un sis ema que man iene una elación de e mina-
da en e la salida y la en ada de e e encia, compa ándolas y usando la di e encia como
medio de con ol. [18]
3.1. Con olado lineal cuad á ico
La écnica de con ol LQR se usó como base pa a e la e acidad del modelo ma emá ico y
hace un p ime con olado álido. La Figu a 3.1 mues a un diag ama del con olado .
Figu a 3.1: Diag ama del con olado LQR.
El obje i o de es a écnica es ob ene las cons an es
Kθ
,
K˙
θ
,
Kx
y
K˙x
con los que se calculó una
15
3. Con olado es
suma ponde ada que minimizó el e o del sis ema.
Se ha concluido que nues o sis ema se compo a como se ige en la ecuación (2.15). Teniendo
en cuen a es o, se dene la unción de cos e
J( )
. [5]
J( ) = uT( )Su( ) + Z
0uT(e)Qu(e) + F(e)TRF(e)de
(3.1)
Aunque es e mé odo aca ea una educción del cálculo del ac o humano, se equie e in oduci
al algo i mo cie os pa áme os que condicionan la p io idad de minimización. Las ma ices de
pesos
S
,
Q
y
R
son las que pa ame izan es os c i e ios. Las ma ices
S
y
Q
son simé icas y
no denidas nega i as, mien as
R
es simé ica y denida posi i a. La ma iz
Q
es la ma iz de
peso pa a los es ados in e medios, la ma iz
R
es la ma iz de peso pa a la acción de con ol del
sis ema y la ma iz
S
ep esen a el peso del es ado nal.
Pa a e un ejemplo más ilus a i o, en el p ime caso de la ecuación (3.2) el obje i o es mini-
miza el e o o al ob enido en las cua o a iables de salida del sis ema en el iempo
. Es a
congu ación se ía p ác ica en un sis ema que no aya a se pe u bado y aya a unciona
du an e un de e minado pe iodo de iempo
.
En el segundo caso, se cen a más en minimiza el gas o o al de ene gía. Pod ía se ú il pa a
ob ene un p oyec o más ecológico que equilib ase al obo usando la mínima can idad de ene gía
posible.









S= 1
Q= 0
R= 0
=⇒J=||u( 1)||2









S= 0
Q= 0
R=I
=⇒J=Z 1
0
||F( )||2d
(3.2)
En el caso del sis ema que se diseñó,
Q∈ M4×4(R)
,
R∈R
y
S= 0
, ya que se necesi a una
es abilidad has a
=∞
. Po ello, se conside a nues o p oblema un sis ema con solución en
iempo inni o cuyo cos e iene denido po :
J∞=Z∞
0uT( )Qu( ) + F( )TRF( )d
(3.3)
Y, pa a soluciona es e p oblema, se es ima que la solución de minimización iene dada po
F( ) = −Ku( )
con
K=R−1BTP
. Y es a
P
se ob iene median e la ecuación ma icial de
Ricca i asociada [18,20].
PA +A∗P−PBR−1B∗P+Q= 0
(3.4)
Pa a el cálculo de es e esul ado se puede u iliza el código de MATLAB
lq (A,B,Q,R)
16
3.2 Con olado P opo cional, In eg al y De i a i o
3.2. Con olado P opo cional, In eg al y De i a i o
El algo i mo PID se basa, como su p opio nomb e indica, en acciones de con ol p opo cional,
in eg al y de i a i a. Su ganancia p opo cional (
Kp
) se calcula pensando en compensa el e o
ac ual del sis ema; la in eg al (
Ki
), el cúmulo de e o es pasados; y la de i a i a (
Kd
), una
p edicción de los e o es u u os. Así, la suma ponde ada de es os e o es p e ende e i a una
sob ecompensación o una acción de co ección basada en un pico de los da os.
His ó icamen e es e algo i mo es uno de los más usados en con ol. Es impo an e ene en cuen a
que su uso no ga an iza un con ol óp imo o, ni siquie a, la es abilidad del mismo.
A eces, algunas aplicaciones se basan solo en alguna pa e de es e sis ema, dejando de lado alguno
de los pa áme os. Po ejemplo, hay con olado es p opo cionales - in eg ales, o p opo cionales -
de i a i os.
En gene al, es e mé odo se puede combina en pa e con un con olado LQR, como se e en
la gu a 3.2. El con olado LQR ob iene una acción de con ol median e la suma ponde ada
Figu a 3.2: Diag ama del con olado LQR.
de las salidas del sis ema. A la salida ob enida se le aplica un con olado PID pa a egula su
compensación adecuadamen e.
La o ma ma emá ica de exp esa la salida que se ob iene de un con olado PID es a ep esen ada
en la ecuación (3.5) siendo
e( )
el e o ob enido en el iempo
. [18]
F( ) = Kpe( ) + KiZ
0
e(˜
)d˜
+Kd
de( )
d
(3.5)
Es e con olado y el de e minado en el pun o an e io LQR se án los que se simulen en el
capí ulo
??
y se apliquen a la cons ucción del obo EV3 en el capí ulo
??
.
17
4. Simulación
24

Capí ulo 5
Lenguajes de p og amación
Pa a el comienzo de la ealización más p ác ica del p oyec o se u o que elegi un lenguaje de
p og amación. Se a a explica b e emen e la expe iencia que se u o con cada lenguaje y las
opciones que se alo a on pa a el p oyec o.
5.1. EV3-G
P ime o se p obó el p opio so wa e de codicación diseñado po la emp esa c eado a del obo ,
el EV3-G. Pa a la codicación y el con ol del EV3, se puede desca ga g a ui amen e en la
Página Ocial de Lego el p og ama Lego Minds o m EV3. Pa a un ap endizaje inicial y alo a
es a al e na i a, se consul ó Guía de uso de Lego Company de 2018 [21].
Es e so wa e es á pensado pa a la p og amación en EV3 po los c eado es y, po an o, iene
bas an es uncionalidades con las que no cuen an o os. Po ejemplo, una amplia base de da os
de g ácos y sonidos disponibles. También es g a ui o y pe mi e conec a ácilmen e el obo al
o denado po USB o
blue oo h
.
Todas es as azones pe mi ie on que se alo a a como so wa e a usa , pe o se desechó la idea
ya que es á diseñado con una in ención educa i a y pa a in oduci a los en o nos de p og ama-
ción. Es muy in ui i o pa a un usua io p incipian e, pe o emendamen e poco p ác ico pa a un
p og amado a anzado. La p og amación se lle a a cabo a a és de bloques de di e en es ipos
(espe a , edondea , bucle, lec u a de senso , e c), es ilo lenguaje Sc a ch.
Es e pa adigma de p og amación es p ác ico pa a las unciones exclusi as del obo (inicia
mo o es, ep oduci sonidos, ec .), pe o muy in incado pa a unciones más gene ales (bucles,
swi chs, condicionales, e c.).
Se puede e una mues a de es e en o no de p og amación en la gu a 5.1, con un ejemplo de
un p og ama que esc ibi ía Hello wo ld en la pan alla del obo y ep oduci ía un sonido en un
bucle inni o.
25
5. Lenguajes de p og amación
Figu a 5.1: Código Hello wo ld en Minds o m EV3.
5.2. Robo C
Robo C es un lenguaje de p og amación basado en C que es á especícamen e diseñado pa a la
p og amación en en o nos como Lego Minds o ms. Es un lenguaje que se ejecu a con un
 mwa e
muy ecien e que pe mi e una apidez no able.
Se puede desca ga en la Página Ocial, pe o es una e sión de p ueba de 10 días. Pa a usa lo
du an e más iempo se equie e ob ene una licencia de $49 al año.
Es a es la azón po la que se desechó es e lenguaje pa a el p oyec o. Aunque, pa a un p oyec o
con isión come cial y a más la go eco ido, sí se ía una opción a conside a .
No se llegó a p o undiza más en si se eque ía o o sis ema ope a i o o si se enía que hace
algún cambio en el so wa e del obo , aunque se ía un pun o a ene en cuen a.
5.3. EV3de
Py hon
EV3de es un p oyec o de código abie o que pe mi e al usua io usa un sis ema ope a i o basado
en Debian inicializado desde una a je a SD. El código de es e p oyec o se puede encon a en
el eposi o io Gi Hub y en la Página Ocial del p oyec o se explica su uso y los lenguajes que
sopo a.
Que se ins ale el SO en una a je a SD es especialmen e ú il, ya que pe mi e que se man enga el
p opio obo in ac o.
En es e sis ema ope a i o se pueden u iliza múl iples lenguajes como
Py hon
, Ja a, Go, C++,
C, P olog, Ja aSc ip , Ruby, e c.
Se eligió
Py hon
al se un lenguaje muy ú il ac ualmen e en la ida labo al, ácil de usa y
al ene cie as nociones básicas. Pa a la p og amación se u ilizó el edi o de código Mic oso
Visual S udio Code y la ex ensión de EV3, que pe mi e conec a y olca el código en el obo .
Pa a la p og amación de
Py hon
en ocada al uso del obo se consul a on múl iples uen es
26
5.4 LeJOS Ja a
Figu a 5.2: Logo EV3de ob enido de su página ocial.
[22,23]. Se desa olló un p ime in en o de código de equilib ado que se puede e en el anexo
A.2.
El p incipal p oblema que se u o pa a el uso de es e lenguaje ue la len i ud. Aunque
Py hon
puede se muy ápido con las lib e ías adecuadas, el lenguaje en sí no lo es an o. Po ello,
el iempo de eco ido del bucle que se consiguió con la máxima op imización (sin usa lib e ía
ex e nas) ue de 0,04 sec. Un p oblema, ya que el bucle pa a un adecuado uncionamien o no puede
se mayo que 0,01 sec. Po es a, y más azones, se conside o que, aunque se había comenzado
p og amando en es e lenguaje, se an ea ían o as opciones an es.
5.4. LeJOS Ja a
LeJOS es un
 mwa e
pa a el EV3 que incluye una máquina i ual de Ja a y, con ello, la
posibilidad de p og ama en es e lenguaje. También incluye un lib e ía que pe mi e comunica se
po
blue oo h
con el
 mwa e
o iginal del EV3.
LeJOS o ece ca ac e ís icas como: lenguaje o ien ado a obje os, h eads, a ays, ecu sión, sin-
c onización, excepciones, e c. Además, se cuen a con una documen ación bas an e ex ensa con
la que comenza a codica [24].
Figu a 5.3: EV3 iniciándose con leJOS.
Pa a su ins alación, se p ocedió de o ma simila a EV3de , con una SD.
És e ue el lenguaje en el que se ealizó el p oyec o. Las azones son que:
27
5. Lenguajes de p og amación
El pa adigma del lenguaje o ien ado a obje os se ía muy p ác ico pa a una codicación más
in ui i a y limpia.
Es el lenguaje que más se ha u ilizado en los es udios, y po an o con el que se es á más
amilia izado, acele ando el ap endizaje del uso de EV3 y la p og amación en sí.
Todas las uen es consul adas ienden a usa es e lenguaje (si no EV3-G o Robo C), lo que
hace p obable que sea el que más documen ado es é y el más uncional.
28
Capí ulo 6
Codicación
Una ez se decidió el algo i mo, el mé odo, el p og ama y el lenguaje de p og amación, se p oce-
dió a inicia el p oceso de p og ama . Pa a simplica la lec u a, se a a sepa a es e capí ulo en
las di e en es clases implemen adas explicando, sin en a en mucho de alle, las uncionalidades
de cada una . El código comple o del p oyec o se puede encon a en el eposi o io Gi Hub
1
. Tam-
bién se pueden encon a en ese mismo eposi o io múl iples ídeos que mues an los esul ados
ob enidos.
Figu a 6.1: Diag ama
UML
del las clases del p oyec o.
En la gu a 6.1 se e ep esen adas las clases y las elaciones en e ellas siguiendo el es ánda
del Lenguaje Unicado de Modelado.
6.1. Main
El código comienza aquí y llama á a cada uno de los hilos o clases a se ejecu adas. Es una clase
bas an e simple que no hace más que llama a o as clases y comenza o os hilos. Comienza
1
h ps://gi hub.com/ u uxka/EV3_EGala
29

6. Codicación
po llama a la clase
Robo
(sección 6.2) pa a inicializa al EV3 y gua da la como a ibu o a
p opo ciona a la mayo ía de las demás clases. Desde casi odas las demás clases se llama á a
Robo
pa a lec u as y esc i u as.
A con inuación, c ea una ins ancia de una subclase de
Equilib io
(sección 6.3), que se ejecu a á
en un hilo independien e (he eda de
Runnable
) y man end á al obo de pie.
Po o o lado, ambién se c ea á una ins ancia ejecu able que se dedica á a e i a posibles
obs áculos y que se ejecu a á en o o hilo dis in o. Es a clase se llama
E i ado
(sección 6.4).
Po úl imo, se c ea á una úl ima clase que se ocupa á de p opo ciona le al obo ins ucciones
que hace . Se á una in e az
Con ol
(sección 6.5) que se a implemen ado po dis in as clases.
6.2. Robo
La clase
Robo
p incipalmen e se ocupa de odo lo que elaciona al es o de las clases con el
obo en sí. De es a o ma, si se equi iese una ees uc u ación de los mé odos asociados al obo
(po una nue a e sión o una lib e ía mejo ada), solo hab ía que hace los cambios en es a clase.
Pa a cada uno de los senso es se iene un a ibu o que se á el enca gado de lee ese senso .
Apa e, enemos los siguien es a ibu os que ca ac e iza án el es ado del obo :
Velocidad.
Al cambia es e a ibu o, el obo inc emen a á su elocidad. Los alo es adecuados
son -50-50, con signo nega i o pa a el e oceso.
Di ección.
Al cambia es e a ibu o, se cambia á la di ección. Se dene el alo como la in en-
sidad con la que se e ec úa el gi o.
S op
.
Indica si el obo debe ía de pa a se. Es un mé odo pa a sinc oniza las pa adas de odos
los bucles.
E i ando.
Indica si la clase
E i ado
es á cambiando la di ección pa a e i a un obs áculo. Es
la o ma que iene
Con ol
de no in e e i .
6.2.1. Mo o es
in encode (Mo o m)
de uel e a la llamada los
acho coun s
(
≈ ad
) del mo o
m
.
oid s op(Mo o m)
de iene el mo o
m
.
oid a ance(Mo o m, in el)
pone al mo o
m
a la elocidad
el
.
6.2.2. Gi oscopio
double a e(in n)
de uel e el alo del gi oscopio en modo  elocidad angula  haciendo la
media de
n
uel as con 2 milisegundos de sepa ación.
double angle()
de uel e el alo del gi oscopio en modo ángulo.
30
6.2 Robo
6.2.3. G ácos
Una de las posibilidades que enía el EV3-G con las que no con aba leJOS e a la biblio eca g áca
pa a ep oduci en la pan alla. Con leJOS se pueden pone ex o, y g ácos c eados a pa i de
comandos muy simples ( ec as, o mas, e c.). Como se que ía explo a la posibilidad de pone le
ojos exp esi os al obo , se in es igó una mane a de palia es a ca encia.
P ime o se eque ía imágenes lib es de de echos pa a pode u iliza de o ma legal, ya que las
imágenes de EV3-G es án p o egidas po
copy igh
de LEGO. Se encon ó una base de da os de
imágenes que imi aban a és as pe o c eadas po usua ios y lib es de
copy igh
2
.
Figu a 6.2: Imágenes ob enidas pa a los g ácos del obo
El p oblema es que, pa a pode aza una imagen en la pan alla LCD del obo en leJOS, se
debe u iliza la clase
Imagen
, que se cons uye con es pa áme os de en ada:
wid h
,
heigh
y
da a
, un a ay de by es que ep esen a la gu a. Po ello, se in es igó y se encon ó un
sc ip
3
de
Py hon que ans o ma imágenes a a ays de by es en Ja a. Así, se ans o ma on las imágenes
de la Figu a 6.2 y se in oduje on en la clase
Robo
.
El obo comienza el p og ama con la ca a neu a, cuando se p oduce un e o de desequilib io
(el obo se cae o se coge) se cambia a e o . Si el obo se encuen a con un obs áculo que la clase
E i ado
iene que esqui a , du an e el p oceso se pond á la ca a en adado. La implemen ación
del in e az
Con ol
ambién puede llama a cambia la ca a dependiendo de las ins ucciones.
6.2.4. De ec o de Colo
Uno de los senso es con los que cuen a el EV3 es un de ec o de colo que se ha inco po ado a
la pa e in e io del diseño que se ha cons uido. Es e senso iene cua o modos dis in os que se
especican en la abla 6.1.
Pa a la de ección de colo se comenzó con el modo
Colo ID
que de uel e el colo en o ma de
2
Reposi o io Gi Hub: h ps://gi hub.com/e 3de /e 3de / eleases
3
Sc ip
(h ps://www.pobo .o g/Ou il-de-gene a ion-d-images-pou .h ml) c eado po E ic P.
31
6. Codicación
Colo ID Measu es he colo ID o a su ace Colo ID ge Colo IDMode()
Red Measu es he in ensi y o a eec ed ed ligh N/A, No malized o (0-1) ge RedMode()
RGB Measu es he RGB colo o a su ace N/A, No malized o (0-1) ge RGBMode()
Ambien Measu es he ambien ligh le el N/A, No malized o (0-1) ge Ambien Mode()
Cuad o 6.1: Modos del senso de colo [24].
alo de un enume ado con quince colo es dis in os. Se expe imen ó que el de ec o e a bas an e
poco ce e o y con undía los colo es en muchos casos. Conside aba la mayo ía de los colo es
iguales al ojo o al neg o, y había colo es del enume ado que nunca de ec aba.
Po ello se plan eó la posibilidad de u iliza el modo
RGB
y c ea un mé odo en la clase
Robo
que desci a a los colo es básicos necesa ios con el código RGB ob enido po el mé odo.
De ec o RGB
RGB, de las siglas en inglés de ojo, e de y azul, es un modelo de colo adi i o basado en los
colo es p ima ios de la luz. La o ma es ánda de ep esen ación es como un ec o
c
con es
componen es en e as pe enecien es
{0,...,255}
. Cada uno de los componen es de ese ec o
ep esen a el alo de ese colo , y jun ando los es se ob iene un colo especíco. El 0 ep esen a
la ausencia de luz, y el alo máximo,
255
, que el componen e de ese colo p ima io es máximo.
[25]
En el caso del senso
RGB
se han no malizado es os alo es y se ob iene
ˆc∈S∞
. Con
S∞
la
pa e posi i a de la es e a de adio unidad en no ma
k·k∞
. Se ep esen a como un cubo en
R3
con cen o
o= (1
2,1
2,1
2)
.
Se ha decidido cen a se en clasica solo los colo es ep esen a i os de la base o ogonal
B=
{e1, e2, e3}
y las sumas simples de és os. Es deci , se han clasicado los ec o es según ce canía
a los ec o es del conjun o
C={~
0, e1, e2, e1+e2, e3, e1+e3, e2+e3, e1+e2+e3}:= {ˆe1,...,ˆe8}
.
También se puede en ende como los alo es bina ios del 0 al 7, pe o como se an a a a como
ec o es, se p ee e conse a la no ación ec o ial.
En la abla 6.2 se pueden e los ec o es ep esen an es y el colo asignado, y en la gu a 6.3 la
ep esen ación g áca de
C
.
(0,0,0) (0,0,0) Neg o
(0,0,255) (0,0,1) Azul
(0,255,0) (0,1,0) Ve de
(0,255,255) (0,1,1) Azul
(255,0,0) (1,0,0) Rojo
(255,0,255) (1,0,1) Rosa
(255,255,0) (1,1,0) Ama illo
(255,255,255) (1,1,1) Blanco
Cuad o 6.2: Sepa ación de colo es según
C
.
Pa a la selección del colo asignado a un ec o
c
se u iliza la unción ep esen ado en la ecuación
(6.1). Se oma el ep esen an e que minimiza el cuad ado de la no ma de la di e encia en e es e
32
6.2 Robo
Figu a 6.3: Rep esen ación g áca de
S∞
con los colo es.
y el ec o
c
. Es deci , el algo i mo de k- ecinos con
k= 1
. [25]
S∞−→ C −→
abla 6.2
Colo es
c−→ {ˆei:min{kc−ˆeik2
2: ˆei∈ C}} −→
abla 6.2
colo (6.1)
Resul ados
Pa a p oba la alidez del de ec o implemen ado se cons uyó una ejilla con dis in os colo es y
se comp ó los esul ados en dos casos: un en o no o almen e iluminado y un en o no a oscu as.
La ejilla se puede e en la imagen 6.4
Figu a 6.4: P ueba de la alidez del de ec o de colo en en o no iluminado y sin ilumina .
Se cons a ó que los esul ados pa a dis ingui los colo es neg o, ojo, blanco, azul y osa e a bas-
an e p ome edo es. En el en o no oscu o los esul ados ue on los mismos, pudiendo conside a
que la alo ación no es ela i a a la iluminación del en o no.
33
7. Conclusiones y u u os p oyec os
una ase a inclui en odos los p oyec os, jun o con una compa a i a y unas sólidas azones pa a
la elección.
El código esul an e del p oyec o ue un código bien es uc u ado y que a la al u a de las con-
side aciones iniciales. Se es ima un acie o la cons ucción po módulos e in e aces, ya que ha
acili ado mucho la depu ación y la cons ucción. El p oyec o es á p epa ado pa a un c ecimien o
u u o e inc emen al con nue as uncionalidades.
7.2. Fu u os p oyec os
Es udio a anzado de la aplicación de un modelo ísico a un obo EV3. Es un ema que no
es á p esen e en la bibliog a ía consul ada y que se conside a que iene g an impo ancia.
Es udio de los iempos de ejecución de
py hon
y de la iabilidad de usa lib e ía ex e nas
en la p og amación en EV3 que acla en si es iable el p oyec o.
Ampliación de las unciones del obo pa a combina el equilib io con unciones más a an-
zadas (como una explo ación basada en in eligencia a icial más compleja).
40

Bibliog a ía
[1] Comi é Español de Au omá ica (CEA), IX edición del Concu so PRODEL de Con ol
In eligen e, 2019.
[2] J. K. Robe ge,
The mechanical seal
. T abajo de n de g ado, Massachuse s Ins i u e o
Technology, 1960.
[3] K. H. Lundbe g and T. W. Ba on,
His o y o In e ed-Pendulum Sys ems
, ol. 42. IFAC,
2010.
[4] M. Bugeja, Non-linea swing-up and s abilizing con ol o an in e ed pendulum sys em,
IEEE Region 8 EUROCON 2003: Compu e as a Tool - P oceedings
, ol. B, pp. 437441,
2003.
[5] H. Kwake naak and R. Si an, Linea Op imal Con ol Sys ems,
IEEE T ansac ions on
Au oma ic Con ol
, ol. 19, pp. 631632, oc 1974.
[6] S. Hassenplug, S e e's LegWay, 2002.
[7] Michael McNally (Lego Ame icas), Wha 's NXT? LEGO G oup Un eils LEGO(R) MINDS-
TORMS(TM) NXT Robo ics Toolse a Consume Elec onics Show, 2006.
[8] P. P. Hu bain, Ge up, NXTway!, 2007.
[9] R. Wa anabe,
Mo ion Con ol o NXTway(LEGO Segway) Con ol Expe imen s wi h LEGO
Minds o ms NXT
. PhD hesis, Waseda Uni e si y, 2007.
[10] Y. Yamamo o, NXTway-GS Model-Based Design, 2008.
[11] A. He nández La gacha, M. Ma ínez Legaspi, and J. Ma ín Peláez,
Con ol in eligen e de
péndulo in e ido
. PhD hesis, Uni e sidad Complu ense de Mad id, 2013.
[12] A. U. o. M. She a d and A. U. o. M. Rhodes, Compa ison o he LEGO Minds o ms NXT
and EV3 Robo ics Educa ion Pla o ms,
Ex ension Jou nal
, ol. 52, no. #5TOT9, 2014.
[13] L. Valk, Tu o ial: Sel -Balancing EV3 Robo , 2014.
[14] P. A. Lo a Thola, Es udio e implemen ación de un con olado pa a un obo ipo Segway y
de los algo i mos que lo capaci an pa a el seguimien o de ayec o ias desconocidas,
Más e
Uni e si a io de Ingenie ía de Sis emas Au omá icos y Elec ónica Indus ial
, 2015.
[15] C. Bjø n Klin ,
Gy oBoy  a sel -balancing obo p og ammed in JAVA wi h leJOS EV3.
PhD hesis, Uni e si y o Denma k (DTU), 2017.
[16] S. A. Campbell, S. C aw o d, and K. Mo is, F ic ion and he In e ed Pendulum S abili-
za ion P oblem,
Jou nal o Dynamic Sys ems, Measu emen , and Con ol
, ol. 130, no. 5,
p. 054502, 2008.
41
BIBLIOGRAFÍA
[17] C. H. Holb ow, J. N. Lloyd, J. C. Ama o, E. Gal ez, and M. E. Pa ks,
Mode n In oduc o y
Physics
. New Yo k, NY: Sp inge New Yo k, 2010.
[18] K. Oga a,
Ingenie ía de con ol mode na 4ED
. Pea son, 2003.
[19] Con ol Tu o ials o MATLAB & Simulink, In e ed pendulum: S a e-space me hods o
con olle design, 2012.
[20] F. L. Lewis, D. V abie, and V. L. Sy mos,
Op imal con ol
. New Je sey (USA): John Wiley
& Sons, Inc, 2012.
[21] Lego Minds o m, Guía de uso, ech. ep., 2018.
[22] N. Wa d, EV3 py hon, 2018.
[23] N. Wa d, EV3 Basic.
[24] EV3 LeJOS De elope s Team, Documen ación LeJOS, 2013.
[25] Yongmei Cai and LinLin Zhang, A e age colo ec o algo i hm in colo ecogni ion based
on a RGB space, in
2012 IEEE 14 h In e na ional Con e ence on Communica ion Techno-
logy
, pp. 10431047, IEEE, no 2012.
42
Apéndice A
Código
A.1. Código del sis ema (MATLAB)
%%
clc
clea
close all
%% Pa ame os
M = 0.7;
% masa del ca o
m = 0.027;
% masa del pendulo
g = 9.81;
% g a edad
L = 0.145;
% longi ud del pendulo
k = 0.1;
% iccion del ca o
I = 0.006;
% iccion en la union
%% Sis ema
% odas las unidades de salida es an en adianes
p1 = (M + m)/(I*(M + m) + L^2*m*M);
p2 = (I + L^2*m)/(I*(M + m + L^2*m*M));
A = [ 0, 1, 0, 0;
L*m*g*p1, 0, 0, (L*m*k*p1)/(M + m);
0, 0, 0, 1;
(-(L*m)^2*g*p2)/(I + L^2*m), 0, 0, -k*p2];
B = [0; (-L*m*p1)/(M + m); 0; p2];
C = eye(4);
D = ze os(4,1);
x0 = [0.2; 0; 0; 0];
% empieza quie o y gi ado 11 g ados
polos = eig(A);
A.2. Código EV3de en Py hon
43
A. Código
de equilib a ():
ini.mo o Righ . ese ()
g = ini.Gy o. a e
# Velocidad Angula del Gi oscopio (g ados/s)
ime.sleep(0.002)
g = g + ini.Gy o. a e
ini.d he a = g/2.0 - ini.o se _gy o
# Velocidad Angula (g ados/s)
ini.o se _gy o = ini.o se _gy o*0.999 + (0.001*(ini.d he a + ini.
o se _gy o))
# Ac ualizamos el o se
ini. he a = ini. he a + ini.d he a*ini.d
# Angulo (g ados)
ini. he a = ini. he a*0.999 - ini. he a*0.001
ini.n = ini.n + 1
i ini.n == ini.n_max:
ini.n = 0
ini.xdes = 0
ini.x = ini.mo o Le .posi ion + ini.mo o Righ .posi ion
# Posicion
(deg)
ini.n_an = ini.n + 1
i ini.n_an == ini.n_max:
ini.n_an = 0
ini.encode [ini.n] = ini.x
# Posicion ( o aciones)
a e age = 0
o i in ange(ini.n_max):
a e age = a e age + ini.encode [i]
a e age = a e age / ini.n_max
ini.dx = a e age / ini.d
#Posicion
ini.e = (k1*ini. he a + k2*ini.d he a + k3*ini.x + k4*ini.dx)
ini.de_d = (ini.e - ini.e_p e )/ini.d
ini.ied = ini.ied + ini.e*ini.d
ini.e_p e = ini.e
ini. o _p e = ini. o acion
i ini.e > 1050:
ini.e = 1050
eli ini.e < -1050:
ini.e = -1050
= in (ini.e/ini.mo o Le .max_speed *100)
ini.mo o Le .on(speed=SpeedDPS(ini.e))
ini.mo o Righ .on(speed=SpeedDPS(ini.e))
44