Aplicación de la inteligencia artificial al control de un robot Lego en configuración de péndulo invertido

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