Aplicación de la inteligencia artificial al control de un robot Lego en configuración de péndulo invertido
Abstract
Este estudio presentó el diseño y la implementación de un robot basado en Lego Mindstorm EV3 imitación de un péndulo vertical invertido que es capaz de mantenerse en equilibrio, avanzar y detectar formas y colores. Así mismo, se confeccionó un modelo matemático análogo al prototipo mediante un espacio de estados y se revisó y simuló el sistema con un regulador lineal cuadrático (LQR) y un controlador proporcional, integral y derivativo (PID). Se implementó el código funcional en Java leJOS. Finalmente, el robot fue capaz de mantenerse en equilibrio, evitar obstáculos y reconocer colores a lo largo de su recorrido.
Full text
APLICACIÓN DE LA INTELIGENCIA ARTIFICIAL AL CONTROL DE UN ROBOT LEGO EN CONFIGURACIÓN DE PÉNDULO INVERTIDO Por Eva Gala de Pablo UNIVERSIDAD COMPLUTENSE MADRID Doble Grado en Matemáticas e Ingeniería Informática Facultad de Informática Dirigido por: Matilde Santos Peñas Madrid, 2018-2019
Resumen Este estudio presentó el diseño y la implementación de un robot basado en Lego Mindstorm EV3 imitación de un péndulo vertical invertido que es capaz de mantenerse en equilibrio, avanzar y detectar formas y colores. Así mismo, se confeccionó un modelo matemático análogo al prototipo mediante un espacio de estados y se revisó y simuló el sistema con un regulador lineal cuadrático (LQR) y un controlador proporcional, integral y derivativo (PID). Se implementó el código funcional en Java leJOS. Finalmente el robot fue capaz de mantenerse en equilibrio, evitar obstáculos y reconocer colores a lo largo de su recorrido. Abstract This research presents the design and implementation of a robot based on Lego Mindstorm EV3. The robot is a reproduction of an inverted vertical pendulum that is stabilized, moves forward and notices shapes and colours. To this end, a mathematical model analogous to the prototype was stablished through a state-space representation. Additionally, the system was reviewed and simulated with a linearquadratic regulator and a proportionalintegralderivative controller. The code was implemented in Java leJOS. The resulting controller allowed the EV3 robot to stay steady, avoid obstacles and detect colours along its path. Palabras clave Péndulo invertido, Espacio de estados, robot balancín, Lego Mindstorm EV3, Regulador LQR, Controlador PID. Keywords Inverted pendulum, State-space representation, balancing robot, Lego Mindstorm EV3, LQR, PID Controler. i
Índice general Resumen i 1. Introducción 1 1.1. Propósito ........................................ 1 1.2. Planteamiento...................................... 1 1.3. Descripcióndelproblema ............................... 2 1.4. Antecedentes ...................................... 3 1.4.1. Antecedentes teóricos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3 1.4.2. Antecedentes aplicados . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4 1.5. Objetivos ........................................ 6 1.6. Plandetrabajo..................................... 6 2. Modelo matemático 9 2.1. Análisis de las fuerzas y obtención de ecuaciones . . . . . . . . . . . . . . . . . . 9 2.1.1. Linealización del modelo . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11 2.1.2. Diagramadeestados.............................. 12 2.1.3. Función de Transferencia . . . . . . . . . . . . . . . . . . . . . . . . . . . 12 3. Controladores 15 3.1. Controlador lineal cuadrático . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15 3.2. Controlador Proporcional, Integral y Derivativo . . . . . . . . . . . . . . . . . . . 17 4. Simulación 19 4.1. Sistemadelazoabierto................................. 19 4.1.1. Sistema de lazo cerrado . . . . . . . . . . . . . . . . . . . . . . . . . . . . 20 4.1.2. Sistema con control LQR . . . . . . . . . . . . . . . . . . . . . . . . . . . 21 4.1.3. Simulación con control LQR y PID . . . . . . . . . . . . . . . . . . . . . . 22 5. Lenguajes de programación 25 5.1. EV3-G.......................................... 25 5.2. RobotC ......................................... 26 5.3. EV3dev Python ..................................... 26 5.4. LeJOSJava....................................... 27 6. Codicación 29 6.1. Main........................................... 29 6.2. Robot .......................................... 30 6.2.1. Motores..................................... 30 6.2.2. Giroscopio.................................... 30 6.2.3. Grácos..................................... 31 6.2.4. DetectordeColor................................ 31 6.3. Equilibrio ........................................ 34 6.3.1. Aplicación del modelo matemático . . . . . . . . . . . . . . . . . . . . . . 34 6.3.2. LQR....................................... 34 iii
6.3.3. PID ....................................... 36 6.3.4. Resultados ................................... 37 6.4. Evitador......................................... 38 6.5. Control ......................................... 38 7. Conclusiones y futuros proyectos 39 7.1. Conclusiones ...................................... 39 7.2. Futurosproyectos.................................... 40 A. Código 43 A.1. Código del sistema (MATLAB) . . . . . . . . . . . . . . . . . . . . . . . . . . . . 43 A.2.CódigoEV3devenPython............................... 43
Dedicatoria Mamá, aunque empezaste este proyecto conmigo y viste a Gsus caminar, no pudiste verle correr...
Capítulo 1 Introducción La materia en la que se centra este proyecto es la construcción y el control de un sistema de péndulo invertido. Esta cuestión física es conocida por ser uno de los problemas más representativos y habituales en la teoría de control. El proyecto engloba la construcción, el modelado y la programación de un robot Lego Mindstorms EV3 R (EV3 o robot, de ahora en adelante). 1.1. Propósito La elección de este proyecto como trabajo de n de grado viene motivado por la posibilidad de profundizar en la teoría de control; un campo multidisciplinario y transversal en el que intervienen tanto las ciencias de la computación como las matemáticas. Por ejemplo, en el estudio de sistemas dinámicos se requiere la aplicación de ecuaciones diferenciales y conocimientos de álgebra avanzado. Por otro lado, el desarrollo de este proyecto requiere de una programación sólida y en varios lenguajes; y la propia construcción del robot requiere unos conocimientos básicos en robótica. 1.2. Planteamiento El planteamiento inicial que se hizo del proyecto fue, por parte de la directora como el siguiente: Este trabajo responde a la propuesta del concurso de control inteligente, organizada por el grupo temático de Control Inteligente del Comité Español de Automática. El Control Inteligente nace con la intención de aplicar las técnicas de Inteligencia Articial a los problemas de control. La Inteligencia Articial en sí es un campo amplio que abarca lógica, optimización, probabilidad, percepción, razonamiento, toma de decisiones, aprendizaje, etc. El proyecto consiste en aplicar paradigmas de control inteligente a una planta real. El sistema a controlar es un Lego en conguración de péndulo invertido. El prototipo está disponible en la Facultad de Informática. Una vez realizado el montaje de la planta, se trata de diseñar e implementar en el sistema un controlador basado en alguna de las técnicas de control inteligente (lógica fuzzy, redes neuronales, etc.) o una combinación de varias de ellas. Para validar el control, el sistema debe realizar un recorrido a lo largo de un circuito controlando en todo momento el ángulo del péndulo 1
1. Introducción 8
Capítulo 2 Modelo matemático En esta fase del proyecto se procuró encontrar una expresión matemática que representase el comportamiento aproximado del sistema. Es necesario encontrar un equilibrio entre un modelo able y una complejidad aceptable. Para ello, se comenzó con un estudio simple de las fuerzas existentes en un péndulo invertido estándar. 2.1. Análisis de las fuerzas y obtención de ecuaciones Para poder obtener un buen modelo del sistema, se debe analizar por separado cada uno de los cuerpos del modelo que se han explicado en la introducción de la memoria: péndulo y carro. Así bien, cuando este sistema se lleva a la práctica con el EV3, sucede que estas partes no están separadas físicamente. Símbolo Signicado Unidades m Masa del péndulo kg M Masa del carro kg k Constante de fricción del carro con el suelo m·kg/s c Constante de fricción en la unión del péndulo y carro m·kg/s L Longitud del péndulo al centro de gravedad m I Inercia del péndulo kg ·m2 θ Ángulo que forma el péndulo con la vertical rad x Desplazamiento del carro en la horizontal m F Fuerza aplicada al carro N Cuadro 2.1: Tabla con las variables que se usarán en el modelo El péndulo invertido se puede denir y modelar como un cuerpo rígido presente solo en dos dimensiones. La tabla 2.1 muestra las variables que se utilizaron y lo que representa cada una y la Figura 2.1 representa el diagrama de fuerzas que se ejercen sobre el péndulo invertido. Se obtienen las componentes de las fuerzas en estas dos dimensiones con las ecuaciones (2.1) y (2.2). XFi=mai (2.1) XFj=maj (2.2) Estas ecuaciones representan las fuerzas de un movimiento plano en un cuerpo, de acuerdo con la 9
2. Modelo matemático Figura 2.1: Diagramas de cuerpo del sistema Segunda Ley de Newton. También se ha de considerar la Versión Rotacional de la Segunda Ley de Newton, la ecuación (2.3). Siendo Fi y Fj la suma de fuerzas en dirección vertical y horizontal y τ el torque, o la fuerza ejercida sobre el eje de giro. [4] τ=Iα (2.3) Empezando por el péndulo, se pueden encontrar dos aceleraciones representadas como ac y an en la gura 2.1. La aceleración asociada al movimiento lineal del carro ( ac ) es horizontal y la aceleración asociada al movimiento rotatorio ( an ) sigue la dirección del ángulo de giro θ . Tomando las fuerzas y aceleraciones horizontales en el péndulo y aplicando las ecuaciones (2.1) y (2.2) se obtienen las ecuaciones (2.4) y (2.5), respectivamente. Aquí se considera H y V como la suma de fuerzas horizontales y las verticales que se ejercen sobre el péndulo respectivamente. [4] H=m·d2x dt2+d2L·sin(θ) dt2 (2.4) V−mg =m·d2L·cos(θ) dt2 (2.5) Si se estudian las fuerzas asociadas al movimiento rotatorio en el péndulo, con la ecuación (2.3), se obtiene la ecuación (2.6). En este caso se representa dθ dt y d2θ dt2 como ˙ θ y ¨ θ para simplicar la notación. [4] V L sin(θ)−HL cos(θ)−c˙ θ=I·¨ θ (2.6) Por otro lado, cabe destacar que en la ecuación (2.6), el término c˙ θ es la fuerza asociada a la fricción de la unión del péndulo con el carro. Como en el EV3 no existe tal unión, se considera que no se debe de tener en cuenta y se ja una aproximación c≈0 . Y, por último, se calculan las fuerzas horizontales de (2.1) que se ejercen sobre el carro en la ecuación (2.7). [4] 10
2.1 Análisis de las fuerzas y obtención de ecuaciones F−H=M¨x+k˙x (2.7) El termino k˙x hace referencia a la fricción del carro contra el suelo. Siguiendo la línea de muchas fuentes consultadas, se usa la ecuación de la fricción como si fuera fricción viscosa. Este enfoque se debe a que la fricción de la rueda con el suelo es prácticamente inexistente y la única fricción que se tiene que considerar es la fricción propia del motor que, al ser lubricada, se corresponde con una fricción viscosa. Por ello se establece que la fricción es proporcional a la velocidad del cuerpo [4]. En el artículo [16] se puede ver un estudio de los diferentes tipos de fricción que se pueden considerar y, con ello, los resultados obtenidos. En el caso de sólo considerar una fricción viscosa en la modelización, se deduce que los controladores tienden a oscilar ligeramente. Sin embargo, en las conclusiones del artículo se admite que añadir el rozamiento con el suelo, aunque mejora ligeramente los resultados, complica el modelo y sigue presentando ruido, probablemente debido a los sensores. Ahora, para aclarar el origen de las ecuaciones principales de las que se obtendrá el modelo, se procede a calcular las derivadas apropiadas y simplicar la notación. De las ecuaciones (2.4) y (2.5) se obtienen las ecuaciones (2.8) y (2.9). [5] H=m¨x+mL ¨ θcos(θ)−˙ θ2sin(θ) (2.8) V−mg =−mL ¨ θsin(θ) + ˙ θ2cos(θ) (2.9) La expresión de ¨x se obtiene de la suma de (2.8) + (2.7). Para obtener ¨ θ , (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 Para el diseño de un controlador se va a usar una versión lineal de las ecuaciones que se han obtenido anteriormente. Como el péndulo se encuentra estable solo en la vertical, se puede considerar que, idealmente, siempre estará en un estado cercano. Por ello, se aproxima θ≈0 y con ello se aplican las aproximaciones de ángulos pequeños tales como sin(θ)≈θ , cos(θ)≈1 y ˙ θ2θ≈0 [4]. Se obtienen, así, las dos ecuaciones anteriores ((2.10) y (2.11)) linealizadas: ¨x=1 m+M·hF−k˙x−mL¨ θi (2.12) 11
2. Modelo matemático ¨ θ=1 I+L2m·[mL (gθ −¨x)] (2.13) Es importante tener en cuenta que, al usar estas aproximaciones de ángulos pequeños, se está aproximando las series de Taylor del cos(θ) y sin(θ) al primer término. Pero, para que la expansión no contenga términos en potencia de π/180 , en θ se ha denido los ángulos en radianes. Por ello, se ja las unidades del modelo a radianes.[17] 2.1.2. Diagrama de estados Un diagrama de estados es una representación de un modelo matemático de un sistema físico mediante un conjunto de entradas, salidas y variables de estado. Estas entradas y salidas están relacionadas mediante ecuaciones diferenciales que se combinan en una ecuación diferencial matricial de primer orden. La representación de un modelo de esta forma proporciona un modo compacto y conveniente de modelar un sistema con varias entradas y salidas.[18] En nuestro caso se tienen 4 entradas y 4 salidas, que forman un sistema de 4 dimensiones. Para obtener un espacio de estados lineal válido a través de las ecuaciones obtenidas anteriormente es necesario sustituir ¨ θ en la ecuación (2.12) usando la ecuación (2.13) y, de la misma forma, sustituir ¨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) . Para simplicar la notación, se renombran las matrices de la ecuación (2.14) como A y B , el vector [θ˙ θ x ˙x]T como u(t) y se incluye una ecuación que represente la salida del sistema v(t) : ˙u(t) = Au(t) + BF(t) (2.15) v(t) = Cu(t) + DF(t) (2.16) Donde C∈R4×R4 es la matriz identidad y D∈R4 es un vector de ceros. 2.1.3. Función de Transferencia A partir del sistema de estados de la sección anterior se obtuvo la función de transferencia. Primero, de la ecuación (2.15) se obtiene la transformada de Laplace ( R´ınf 0f(t)e−stdt ) con condiciones iniciales cero. [18] tU(t) = AU(t) + B∗V(t) (2.17) V(t) = CU(t) (2.18) 12
2.1 Análisis de las fuerzas y obtención de ecuaciones Y, de la ecuación (2.17), reordenando y sustituyendo en (2.18) se obtiene U(t) = C(tI−A)−1BV(t) (2.19) Con esto, se logra la función de transferencia. [18] H(t) = U(t) V(t)=C(tI−A)−1B (2.20) El denominador de la función de transferencia se hace cero cuando (tI−A) es cero. Lo que es equivalente a que los polos de la función de transferencia son los autovalores de la matriz A . [19,18,5] Esta armación es especialmente útil a la hora de comprobar la inestabilidad del sistema. 13
2. Modelo matemático 14
Capítulo 3 Controladores Para asegurar un correcto funcionamiento del dispositivo y un nal funcional, se aplicó un desarrollo incremental del controlador. Es decir, primero se intentó obtener un controlador LQR funcional y después mejorar éste con un controlador PID. En este capítulo, se expondrá la base teórica que se requiere para entender los controladores que después se han implementado. Pero, para poder entender esta base teórica se tendrán que denir dos conceptos fundamentales: Sistema de lazo abierto Un sistema en el que la salida del sistema no tiene ningún efecto sobre la acción del sistema. [18] Sistema de lazo cerrado o realimentado Un sistema que mantiene una relación determinada entre la salida y la entrada de referencia, comparándolas y usando la diferencia como medio de control. [18] 3.1. Controlador lineal cuadrático La técnica de control LQR se usó como base para ver la veracidad del modelo matemático y hacer un primer controlador válido. La Figura 3.1 muestra un diagrama del controlador. Figura 3.1: Diagrama del controlador LQR. El objetivo de esta técnica es obtener las constantes Kθ , K˙ θ , Kx y K˙x con los que se calculó una 15
3. Controladores suma ponderada que minimizó el error del sistema. Se ha concluido que nuestro sistema se comporta como se rige en la ecuación (2.15). Teniendo en cuenta esto, se dene la función de coste J(t) . [5] J(t) = uT(t)Su(t) + Zt 0uT(e)Qu(e) + F(e)TRF(e)de (3.1) Aunque este método acarrea una reducción del cálculo del factor humano, se requiere introducir al algoritmo ciertos parámetros que condicionan la prioridad de minimización. Las matrices de pesos S , Q y R son las que parametrizan estos criterios. Las matrices S y Q son simétricas y no denidas negativas, mientras R es simétrica y denida positiva. La matriz Q es la matriz de peso para los estados intermedios, la matriz R es la matriz de peso para la acción de control del sistema y la matriz S representa el peso del estado nal. Para ver un ejemplo más ilustrativo, en el primer caso de la ecuación (3.2) el objetivo es minimizar el error total obtenido en las cuatro variables de salida del sistema en el tiempo t . Esta conguración sería práctica en un sistema que no vaya a ser perturbado y vaya a funcionar durante un determinado periodo de tiempo t . En el segundo caso, se centra más en minimizar el gasto total de energía. Podría ser útil para obtener un proyecto más ecológico que equilibrase al robot usando la mínima cantidad de energía posible. S= 1 Q= 0 R= 0 =⇒J=||u(t1)||2 S= 0 Q= 0 R=I =⇒J=Zt1 0 ||F(t)||2dt (3.2) En el caso del sistema que se diseñó, Q∈ M4×4(R) , R∈R y S= 0 , ya que se necesita una estabilidad hasta t=∞ . Por ello, se considera nuestro problema un sistema con solución en tiempo innito cuyo coste viene denido por: J∞=Z∞ 0uT(t)Qu(t) + F(t)TRF(t)dt (3.3) Y, para solucionar este problema, se estima que la solución de minimización viene dada por F(t) = −Ku(t) con K=R−1BTP . Y esta P se obtiene mediante la ecuación matricial de Riccati asociada [18,20]. PA +A∗P−PBR−1B∗P+Q= 0 (3.4) Para el cálculo de este resultado se puede utilizar el código de MATLAB lqr(A,B,Q,R) 16
3.2 Controlador Proporcional, Integral y Derivativo 3.2. Controlador Proporcional, Integral y Derivativo El algoritmo PID se basa, como su propio nombre indica, en acciones de control proporcional, integral y derivativa. Su ganancia proporcional ( Kp ) se calcula pensando en compensar el error actual del sistema; la integral ( Ki ), el cúmulo de errores pasados; y la derivativa ( Kd ), una predicción de los errores futuros. Así, la suma ponderada de estos errores pretende evitar una sobrecompensación o una acción de corrección basada en un pico de los datos. Históricamente este algoritmo es uno de los más usados en control. Es importante tener en cuenta que su uso no garantiza un control óptimo o, ni siquiera, la estabilidad del mismo. A veces, algunas aplicaciones se basan solo en alguna parte de este sistema, dejando de lado alguno de los parámetros. Por ejemplo, hay controladores proporcionales - integrales, o proporcionales - derivativos. En general, este método se puede combinar en parte con un controlador LQR, como se ve en la gura 3.2. El controlador LQR obtiene una acción de control mediante la suma ponderada Figura 3.2: Diagrama del controlador LQR. de las salidas del sistema. A la salida obtenida se le aplica un controlador PID para regular su compensación adecuadamente. La forma matemática de expresar la salida que se obtiene de un controlador PID esta representada en la ecuación (3.5) siendo e(t) el error obtenido en el tiempo t . [18] F(t) = Kpe(t) + KiZt 0 e(˜ t)d˜ t+Kd de(t) dt (3.5) Este controlador y el determinado en el punto anterior LQR serán los que se simulen en el capítulo ?? y se apliquen a la construcción del robot EV3 en el capítulo ?? . 17
4. Simulación 24
Capítulo 5 Lenguajes de programación Para el comienzo de la realización más práctica del proyecto se tuvo que elegir un lenguaje de programación. Se va a explicar brevemente la experiencia que se tuvo con cada lenguaje y las opciones que se valoraron para el proyecto. 5.1. EV3-G Primero se probó el propio software de codicación diseñado por la empresa creadora del robot, el EV3-G. Para la codicación y el control del EV3, se puede descargar gratuitamente en la Página Ocial de Lego el programa Lego Mindstorm EV3. Para un aprendizaje inicial y valorar esta alternativa, se consultó Guía de uso de Lego Company de 2018 [21]. Este software está pensado para la programación en EV3 por los creadores y, por tanto, tiene bastantes funcionalidades con las que no cuentan otros. Por ejemplo, una amplia base de datos de grácos y sonidos disponibles. También es gratuito y permite conectar fácilmente el robot al ordenador por USB o bluetooth . Todas estas razones permitieron que se valorara como software a usar, pero se desechó la idea ya que está diseñado con una intención educativa y para introducir a los entornos de programación. Es muy intuitivo para un usuario principiante, pero tremendamente poco práctico para un programador avanzado. La programación se lleva a cabo a través de bloques de diferentes tipos (esperar, redondear, bucle, lectura de sensor, etc), estilo lenguaje Scratch. Este paradigma de programación es práctico para las funciones exclusivas del robot (iniciar motores, reproducir sonidos, ect.), pero muy intrincado para funciones más generales (bucles, switchs, condicionales, etc.). Se puede ver una muestra de este entorno de programación en la gura 5.1, con un ejemplo de un programa que escribiría Hello world en la pantalla del robot y reproduciría un sonido en un bucle innito. 25
5. Lenguajes de programación Figura 5.1: Código Hello world en Mindstorm EV3. 5.2. RobotC RobotC es un lenguaje de programación basado en C que está especícamente diseñado para la programación en entornos como Lego Mindstorms. Es un lenguaje que se ejecuta con un rmware muy eciente que permite una rapidez notable. Se puede descargar en la Página Ocial, pero es una versión de prueba de 10 días. Para usarlo durante más tiempo se requiere obtener una licencia de $49 al año. Esta es la razón por la que se desechó este lenguaje para el proyecto. Aunque, para un proyecto con visión comercial y a más largo recorrido, sí sería una opción a considerar. No se llegó a profundizar más en si se requería otro sistema operativo o si se tenía que hacer algún cambio en el software del robot, aunque sería un punto a tener en cuenta. 5.3. EV3dev Python EV3dev es un proyecto de código abierto que permite al usuario usar un sistema operativo basado en Debian inicializado desde una tarjeta SD. El código de este proyecto se puede encontrar en el repositorio GitHub y en la Página Ocial del proyecto se explica su uso y los lenguajes que soporta. Que se instale el SO en una tarjeta SD es especialmente útil, ya que permite que se mantenga el propio robot intacto. En este sistema operativo se pueden utilizar múltiples lenguajes como Python , Java, Go, C++, C, Prolog, JavaScript, Ruby, etc. Se eligió Python al ser un lenguaje muy útil actualmente en la vida laboral, fácil de usar y al tener ciertas nociones básicas. Para la programación se utilizó el editor de código Microsoft Visual Studio Code y la extensión de EV3, que permite conectar y volcar el código en el robot. Para la programación de Python enfocada al uso del robot se consultaron múltiples fuentes 26
5.4 LeJOS Java Figura 5.2: Logo EV3dev obtenido de su página ocial. [22,23]. Se desarrolló un primer intento de código de equilibrado que se puede ver en el anexo A.2. El principal problema que se tuvo para el uso de este lenguaje fue la lentitud. Aunque Python puede ser muy rápido con las librerías adecuadas, el lenguaje en sí no lo es tanto. Por ello, el tiempo de recorrido del bucle que se consiguió con la máxima optimización (sin usar librería externas) fue de 0,04 sec. Un problema, ya que el bucle para un adecuado funcionamiento no puede ser mayor que 0,01 sec. Por esta, y más razones, se considero que, aunque se había comenzado programando en este lenguaje, se tantearían otras opciones antes. 5.4. LeJOS Java LeJOS es un rmware para el EV3 que incluye una máquina virtual de Java y, con ello, la posibilidad de programar en este lenguaje. También incluye un librería que permite comunicarse por bluetooth con el rmware original del EV3. LeJOS ofrece características como: lenguaje orientado a objetos, threads, arrays, recursión, sincronización, excepciones, etc. Además, se cuenta con una documentación bastante extensa con la que comenzar a codicar [24]. Figura 5.3: EV3 iniciándose con leJOS. Para su instalación, se procedió de forma similar a EV3dev, con una SD. Éste fue el lenguaje en el que se realizó el proyecto. Las razones son que: 27
5. Lenguajes de programación El paradigma del lenguaje orientado a objetos sería muy práctico para una codicación más intuitiva y limpia. Es el lenguaje que más se ha utilizado en los estudios, y por tanto con el que se está más familiarizado, acelerando el aprendizaje del uso de EV3 y la programación en sí. Todas las fuentes consultadas tienden a usar este lenguaje (si no EV3-G o RobotC), lo que hace probable que sea el que más documentado esté y el más funcional. 28
Capítulo 6 Codicación Una vez se decidió el algoritmo, el método, el programa y el lenguaje de programación, se procedió a iniciar el proceso de programar. Para simplicar la lectura, se va a separar este capítulo en las diferentes clases implementadas explicando, sin entrar en mucho detalle, las funcionalidades de cada una . El código completo del proyecto se puede encontrar en el repositorio GitHub 1 . También se pueden encontrar en ese mismo repositorio múltiples vídeos que muestran los resultados obtenidos. Figura 6.1: Diagrama UML del las clases del proyecto. En la gura 6.1 se ve representadas las clases y las relaciones entre ellas siguiendo el estándar del Lenguaje Unicado de Modelado. 6.1. Main El código comienza aquí y llamará a cada uno de los hilos o clases a ser ejecutadas. Es una clase bastante simple que no hace más que llamar a otras clases y comenzar otros hilos. Comienza 1 https://github.com/vuvuxka/EV3_EGala 29
6. Codicación por llamar a la clase Robot (sección 6.2) para inicializar al EV3 y guardarla como atributo a proporcionar a la mayoría de las demás clases. Desde casi todas las demás clases se llamará a Robot para lecturas y escrituras. A continuación, crea una instancia de una subclase de Equilibrio (sección 6.3), que se ejecutará en un hilo independiente (hereda de Runnable ) y mantendrá al robot de pie. Por otro lado, también se creará una instancia ejecutable que se dedicará a evitar posibles obstáculos y que se ejecutará en otro hilo distinto. Esta clase se llama Evitador (sección 6.4). Por último, se creará una última clase que se ocupará de proporcionarle al robot instrucciones que hacer. Será una interfaz Control (sección 6.5) que sera implementado por distintas clases. 6.2. Robot La clase Robot principalmente se ocupa de todo lo que relaciona al resto de las clases con el robot en sí. De esta forma, si se requiriese una reestructuración de los métodos asociados al robot (por una nueva versión o una librería mejorada), solo habría que hacer los cambios en esta clase. Para cada uno de los sensores se tiene un atributo que será el encargado de leer ese sensor. Aparte, tenemos los siguientes atributos que caracterizarán el estado del robot: Velocidad. Al cambiar este atributo, el robot incrementará su velocidad. Los valores adecuados son -50-50, con signo negativo para el retroceso. Dirección. Al cambiar este atributo, se cambiará la dirección. Se dene el valor como la intensidad con la que se efectúa el giro. Stop . Indica si el robot debería de pararse. Es un método para sincronizar las paradas de todos los bucles. Evitando. Indica si la clase Evitador está cambiando la dirección para evitar un obstáculo. Es la forma que tiene Control de no interferir. 6.2.1. Motores int encoder(Motor m) devuelve a la llamada los tacho counts ( ≈rad ) del motor m . void stop(Motor m) detiene el motor m . void avance(Motor m, int vel) pone al motor m a la velocidad vel . 6.2.2. Giroscopio double rate(int n) devuelve el valor del giroscopio en modo velocidad angular haciendo la media de n vueltas con 2 milisegundos de separación. double angle() devuelve el valor del giroscopio en modo ángulo. 30
6.2 Robot 6.2.3. Grácos Una de las posibilidades que tenía el EV3-G con las que no contaba leJOS era la biblioteca gráca para reproducir en la pantalla. Con leJOS se pueden poner texto, y grácos creados a partir de comandos muy simples (rectas, formas, etc.). Como se quería explorar la posibilidad de ponerle ojos expresivos al robot, se investigó una manera de paliar esta carencia. Primero se requería imágenes libres de derechos para poder utilizar de forma legal, ya que las imágenes de EV3-G están protegidas por copyright de LEGO. Se encontró una base de datos de imágenes que imitaban a éstas pero creadas por usuarios y libres de copyright 2 . Figura 6.2: Imágenes obtenidas para los grácos del robot El problema es que, para poder trazar una imagen en la pantalla LCD del robot en leJOS, se debe utilizar la clase Imagen , que se construye con tres parámetros de entrada: width , height y data , un array de bytes que representa la gura. Por ello, se investigó y se encontró un script 3 de Python que transforma imágenes a arrays de bytes en Java. Así, se transformaron las imágenes de la Figura 6.2 y se introdujeron en la clase Robot . El robot comienza el programa con la cara neutra, cuando se produce un error de desequilibrio (el robot se cae o se coge) se cambia a error. Si el robot se encuentra con un obstáculo que la clase Evitador tiene que esquivar, durante el proceso se pondrá la cara enfadado. La implementación del interfaz Control también puede llamar a cambiar la cara dependiendo de las instrucciones. 6.2.4. Detector de Color Uno de los sensores con los que cuenta el EV3 es un detector de color que se ha incorporado a la parte inferior del diseño que se ha construido. Este sensor tiene cuatro modos distintos que se especican en la tabla 6.1. Para la detección de color se comenzó con el modo ColorID que devuelve el color en forma de 2 Repositorio GitHub: https://github.com/ev3dev/ev3dev/releases 3 Script (https://www.pobot.org/Outil-de-generation-d-images-pour.html) creado por Eric P. 31
6. Codicación Color ID Measures the color ID of a surface Color ID getColorIDMode() Red Measures the intensity of a reected red light N/A, Normalized to (0-1) getRedMode() RGB Measures the RGB color of a surface N/A, Normalized to (0-1) getRGBMode() Ambient Measures the ambient light level N/A, Normalized to (0-1) getAmbientMode() Cuadro 6.1: Modos del sensor de color [24]. valor de un enumerado con quince colores distintos. Se experimentó que el detector era bastante poco certero y confundía los colores en muchos casos. Consideraba la mayoría de los colores iguales al rojo o al negro, y había colores del enumerado que nunca detectaba. Por ello se planteó la posibilidad de utilizar el modo RGB y crear un método en la clase Robot que descifrara los colores básicos necesarios con el código RGB obtenido por el método. Detector RGB RGB, de las siglas en inglés de rojo, verde y azul, es un modelo de color aditivo basado en los colores primarios de la luz. La forma estándar de representación es como un vector c con tres componentes enteras pertenecientes {0,...,255} . Cada uno de los componentes de ese vector representa el valor de ese color, y juntando los tres se obtiene un color especíco. El 0 representa la ausencia de luz, y el valor máximo, 255 , que el componente de ese color primario es máximo. [25] En el caso del sensor RGB se han normalizado estos valores y se obtiene ˆc∈S∞ . Con S∞ la parte positiva de la esfera de radio unidad en norma k·k∞ . Se representa como un cubo en R3 con centro o= (1 2,1 2,1 2) . Se ha decidido centrarse en clasicar solo los colores representativos de la base ortogonal B= {e1, e2, e3} y las sumas simples de éstos. Es decir, se han clasicado los vectores según cercanía a los vectores del conjunto C={~ 0, e1, e2, e1+e2, e3, e1+e3, e2+e3, e1+e2+e3}:= {ˆe1,...,ˆe8} . También se puede entender como los valores binarios del 0 al 7, pero como se van a tratar como vectores, se preere conservar la notación vectorial. En la tabla 6.2 se pueden ver los vectores representantes y el color asignado, y en la gura 6.3 la representación gráca de C . (0,0,0) (0,0,0) Negro (0,0,255) (0,0,1) Azul (0,255,0) (0,1,0) Verde (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) Amarillo (255,255,255) (1,1,1) Blanco Cuadro 6.2: Separación de colores según C . Para la selección del color asignado a un vector c se utiliza la función representado en la ecuación (6.1). Se toma el representante que minimiza el cuadrado de la norma de la diferencia entre este 32
6.2 Robot Figura 6.3: Representación gráca de S∞ con los colores. y el vector c . Es decir, el algoritmo de k-vecinos con k= 1 . [25] S∞−→ C −→ tabla 6.2 Colores c−→ {ˆei:min{kc−ˆeik2 2: ˆei∈ C}} −→ tabla 6.2 color (6.1) Resultados Para probar la validez del detector implementado se construyó una rejilla con distintos colores y se compró los resultados en dos casos: un entorno totalmente iluminado y un entorno a oscuras. La rejilla se puede ver en la imagen 6.4 Figura 6.4: Prueba de la validez del detector de color en entorno iluminado y sin iluminar. Se constató que los resultados para distinguir los colores negro, rojo, blanco, azul y rosa era bastante prometedores. En el entorno oscuro los resultados fueron los mismos, pudiendo considerar que la valoración no es relativa a la iluminación del entorno. 33
7. Conclusiones y futuros proyectos una fase a incluir en todos los proyectos, junto con una comparativa y unas sólidas razones para la elección. El código resultante del proyecto fue un código bien estructurado y que a la altura de las consideraciones iniciales. Se estima un acierto la construcción por módulos e interfaces, ya que ha facilitado mucho la depuración y la construcción. El proyecto está preparado para un crecimiento futuro e incremental con nuevas funcionalidades. 7.2. Futuros proyectos Estudio avanzado de la aplicación de un modelo físico a un robot EV3. Es un tema que no está presente en la bibliografía consultada y que se considera que tiene gran importancia. Estudio de los tiempos de ejecución de python y de la viabilidad de usar librería externas en la programación en EV3 que aclaren si es viable el proyecto. Ampliación de las funciones del robot para combinar el equilibrio con funciones más avanzadas (como una exploración basada en inteligencia articial más compleja). 40
Bibliografía [1] Comité Español de Automática (CEA), IX edición del Concurso PRODEL de Control Inteligente, 2019. [2] J. K. Roberge, The mechanical seal . Trabajo de n de grado, Massachusetts Institute of Technology, 1960. [3] K. H. Lundberg and T. W. Barton, History of Inverted-Pendulum Systems , vol. 42. IFAC, 2010. [4] M. Bugeja, Non-linear swing-up and stabilizing control of an inverted pendulum system, IEEE Region 8 EUROCON 2003: Computer as a Tool - Proceedings , vol. B, pp. 437441, 2003. [5] H. Kwakernaak and R. Sivan, Linear Optimal Control Systems, IEEE Transactions on Automatic Control , vol. 19, pp. 631632, oct 1974. [6] S. Hassenplug, Steve's LegWay, 2002. [7] Michael McNally (Lego Americas), What's NXT? LEGO Group Unveils LEGO(R) MINDSTORMS(TM) NXT Robotics Toolset at Consumer Electronics Show, 2006. [8] P. P. Hurbain, Get up, NXTway!, 2007. [9] R. Watanabe, Motion Control of NXTway(LEGO Segway) Control Experiments with LEGO Mindstorms NXT . PhD thesis, Waseda University, 2007. [10] Y. Yamamoto, NXTway-GS Model-Based Design, 2008. [11] A. Hernández Largacha, M. Martínez Legaspi, and J. Martín Peláez, Control inteligente de péndulo invertido . PhD thesis, Universidad Complutense de Madrid, 2013. [12] A. U. o. M. Sherrard and A. U. o. M. Rhodes, Comparison of the LEGO Mindstorms NXT and EV3 Robotics Education Platforms, Extension Journal , vol. 52, no. #5TOT9, 2014. [13] L. Valk, Tutorial: Self-Balancing EV3 Robot, 2014. [14] P. A. Lora Thola, Estudio e implementación de un controlador para un robot tipo Segway y de los algoritmos que lo capacitan para el seguimiento de trayectorias desconocidas, Máster Universitario de Ingeniería de Sistemas Automáticos y Electrónica Industrial , 2015. [15] C. Bjørn Klint, GyroBoy a self-balancing robot programmed in JAVA with leJOS EV3. PhD thesis, University of Denmark (DTU), 2017. [16] S. A. Campbell, S. Crawford, and K. Morris, Friction and the Inverted Pendulum Stabilization Problem, Journal of Dynamic Systems, Measurement, and Control , vol. 130, no. 5, p. 054502, 2008. 41
BIBLIOGRAFÍA [17] C. H. Holbrow, J. N. Lloyd, J. C. Amato, E. Galvez, and M. E. Parks, Modern Introductory Physics . New York, NY: Springer New York, 2010. [18] K. Ogata, Ingeniería de control moderna 4ED . Pearson, 2003. [19] Control Tutorials for MATLAB & Simulink, Inverted pendulum: State-space methods for controller design, 2012. [20] F. L. Lewis, D. Vrabie, and V. L. Syrmos, Optimal control . New Jersey (USA): John Wiley & Sons, Inc, 2012. [21] Lego Mindstorm, Guía de uso, tech. rep., 2018. [22] N. Ward, EV3 python, 2018. [23] N. Ward, EV3 Basic. [24] EV3 LeJOS Developers Team, Documentación LeJOS, 2013. [25] Yongmei Cai and LinLin Zhang, Average color vector algorithm in color recognition based on a RGB space, in 2012 IEEE 14th International Conference on Communication Technology , pp. 10431047, IEEE, nov 2012. 42
Apéndice A Código A.1. Código del sistema (MATLAB) %% clc clear close all %% Parametros M = 0.7; % masa del carro m = 0.027; % masa del pendulo g = 9.81; % gravedad L = 0.145; % longitud del pendulo k = 0.1; % friccion del carro I = 0.006; % friccion en la union %% Sistema % todas las unidades de salida estan en radianes 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 = zeros(4,1); x0 = [0.2; 0; 0; 0]; % empieza quieto y girado 11 grados polos = eig(A); A.2. Código EV3dev en Python 43
A. Código def equilibrar(): ini.motorRight.reset() g = ini.Gyro.rate # Velocidad Angular del Giroscopio (grados/s) time.sleep(0.002) g = g + ini.Gyro.rate ini.dtheta = g/2.0 - ini.offset_gyro # Velocidad Angular (grados/s) ini.offset_gyro = ini.offset_gyro*0.999 + (0.001*(ini.dtheta + ini. offset_gyro)) # Actualizamos el offset ini.theta = ini.theta + ini.dtheta*ini.dt # Angulo (grados) ini.theta = ini.theta*0.999 - ini.theta*0.001 ini.n = ini.n + 1 if ini.n == ini.n_max: ini.n = 0 ini.xdes = 0 ini.x = ini.motorLeft.position + ini.motorRight.position # Posicion (deg) ini.n_ant = ini.n + 1 if ini.n_ant == ini.n_max: ini.n_ant = 0 ini.encoder[ini.n] = ini.x # Posicion (rotaciones) average = 0 for i in range(ini.n_max): average = average + ini.encoder[i] average = average / ini.n_max ini.dx = average / ini.dt #Posicion ini.e = (k1*ini.theta + k2*ini.dtheta + k3*ini.x + k4*ini.dx) ini.de_dt = (ini.e - ini.e_prev)/ini.dt ini.iedt = ini.iedt + ini.e*ini.dt ini.e_prev = ini.e ini.rot_prev = ini.rotacion if ini.e > 1050: ini.e = 1050 elif ini.e < -1050: ini.e = -1050 v = int(ini.e/ini.motorLeft.max_speed *100) ini.motorLeft.on(speed=SpeedDPS(ini.e)) ini.motorRight.on(speed=SpeedDPS(ini.e)) 44