Control avanzado de un robot móvil autoequilibrado

Proyecto Fin de Carrera Ingeniería de Telecomunicación Formato de Publicación de la Escuela Técnica Superior de Ingeniería Autor: F. Javier Payán Somet Tutor: Juan José Murillo Fuentes Dep. Teoría de la Señal y Comunicaciones Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2013 Proyecto Fin de Máster Máster en Ingeniería Industrial Control avanzado de un robot móvil autoequilibrado Autor: Cecilia González González Tutor: Ignacio Alvarado Aldea Dep. Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2017/2018 Proyecto Fin de Máster Máster en Ingeniería Industrial Control avanzado de un robot móvil autoequilibrado Autor: Cecilia González González Tutor: Ignacio Alvarado Aldea Ignacio Alvarado Aldea Dep. Ingeniería de Sistemas y Automática Escuela Técnica Superior de Ingeniería Universidad de Sevilla Sevilla, 2017/2018 Proyecto Fin de Máster: Control avanzado de un robot móvil autoequilibrado Autor: Cecilia González González Tutor: Ignacio Alvarado Aldea El tribunal nombrado para juzgar el trabajo arriba indicado, compuesto por los siguientes profesores: Presidente: Vocal/es: Secretario: acuerdan otorgarle la calificación de: El Secretario del Tribunal Fecha: Agradecimientos Dedico este trabajo a mi familia. A mis pacientes padres y a mi hermana que siempre sabe escucharme y ayudarme. También quiero dedicar este proyecto a Pablo Krupa, que ha estado siempre resolviendo mis dudas. A mi tutor Ignacio, le agradezco el reto que suponía este trabajo. A mis amigos, que son los que mejor saben lo difícil que puede ser sacar adelante el trabajo de fin de máster compaginándolo con el trabajo. Tengo la suerte de haber podido contar con todos ellos, que conmigo han conseguido que este proyecto salga. Gracias. Cecilia González González Sevilla, 2018 I Resumen Este proyecto consiste en la descripción del Control Predictivo basado en Modelo y su implementación en un sistema real. En este caso, el sistema será un vehículo autoequilibrado. La implementación del MPC en el sistema radica en el desarrollo de este tipo de control en Arduino. Para lo cual es necesario el desarrollo inicial en Maltab, donde se realizan las simulaciones para comprobar el buen funcionamiento del bucle de control. III XÍndice 4.1.4 Matrices QryRrdel SSTO 30 4.1.5 Matriz Tdel SSTO 30 4.1.6 Límites del SSTO 31 4.2 Gráficas 31 4.2.1 Saturación de la actuación y de la velocidad de la rueda 31 4.2.2 Gráficas con perturbación nula 33 Referencia de velocidad nula 33 Referencia de velocidad positiva 34 Referencia de velocidad negativa 35 Varios cambios de referencia 36 4.2.3 Gráficas con perturbación distinta a cero 37 Referencia de velocidad nula 37 5 Implementación en Arduino 39 5.1 Características de la placa 39 Pines de la placa utilizados 40 5.2 Desarrollo del código 41 5.2.1 Reducción del número de operaciones y datos 41 5.2.2 Generación de matrices para Arduino 44 Matriz W−145 5.2.3 Disminución de la aproximación a cero 46 5.2.4 Eliminar las raíces cuadradas 46 5.2.5 Problema 46 Restricción de tiempo del sistema 47 5.3 Solución posible 47 5.3.1 Comparación Arduino Mega vs Arduino Due 47 6 Líneas futuras y conclusión 49 Apéndice A Códigos de Matlab 51 Apéndice B Código de Arduino 57 Índice de Figuras 63 Índice de Tablas 65 Índice de Códigos 67 Bibliografía 69 Notación RCuerpo de los números reales ZCuerpo de los números complejos kvkNorma del vector v s.aSujeto a sin Función seno cos Función coseno ∂y ∂xDerivada parcial de yrespecto a x ⩽Menor o igual ⩾Mayor o igual ≃Aproximado ≡Equivalente a ∆Incremento ∈Pertenece a ⊂Contenido en ATMatriz traspuesta ˙xPrimera derivada de x ¨xSegunda derivada de x d dt Derivada en el tiempo xeq xen el punto de equilibrio xek xen el instante k ˆx x estimado x∗xóptimo rad Radianes sSegundos kg Kilogramo mMetro arg Argumento m´ ın Minimización ∑N−1 i=0Sumatorio desde i=0hasta N−1 MPC Control Predictivo por Modelo FISTA Fast Iterative Shrinkage Thresholding Algorithm SSTO Steady-State Target Optimization OBS Estimador de estados QP Programación cuadrática XI 1 Introducción Derrotados son los que dejan de luchar José Mujica Muchos problemas de control y automatización de procesos industriales son resueltos con control predictivo. Esta estrategia es usada en sistemas que constan de varias variables, con un comportamiento dinámico complejo o inestable. Este tipo de control pretende predecir el comportamiento futuro de la planta a través del conocimiento del modelo matemático del proceso que se desea controlar. El control predictivo se compone de diversas disciplinas, entre ellas, el control predictivo basado en modelo (MPC). El MPC se caracteriza por buscar una actuación que responda a la optimización de una función de coste. Esta está relacionada con el comportamiento futuro del sistema. Para poder calcular la acción del control hace falta resolver un problema de optimización (cuadrática). Existen muchos algoritmos que resuelven este problema. Sin embargo, en este proyecto el algoritmo que se va a desarrollar es el algoritmo FISTA (Fast Iterative Shrinkage Thresholding Algorithm). Se quiere implementar este algoritmo ya que el sistema que se usa es de bajos recursos. Entre las ventajas del control predictivo, este permite formular en el dominio del tiempo, siendo una técnica más intuitiva. Por otra parte, puede tratar con sistemas multivariables y no lineales. Además permite que la ley de control garantice criterios de optimización. Por último, admite la integración de restricciones. Como inconvenientes cabe destacar que requiere un coste computacional alto, debido a la gran cantidad de datos necesarios para los cálculos. Esto complica la aplicación de este tipo de control en sistemas que requieren una rápida actuación como es este caso. Por otra parte, el cumplimiento de las restricciones hace compleja la búsqueda de la solución. Además estas restricciones aparecen tanto en la acción de control como en las variables de estado. Será de gran importancia tener en cuenta la restricción de tiempo del sistema. Esto es así ya que se está implementando el control en un sistema real que requiere una respuesta en un tiempo determinado que no puede sobrepasarse. 1 2 Descripción del sistema y modelo 2.1 Descripción del sistema El Segway es un sistema a controlar muy estudiado, es el péndulo invertido. Este sistema es interesante por ser lineal, multivariable y contener un cero de fase no mínima. Figura 2.1 Vehículo autoequilibrado. Se sabe que este tipo de sistema contiene dos posiciones de equilibrio, una de ellas estable correspondiente al punto de mínima altura del péndulo y la inestable, que se encuentra en el punto de máxima altura. Ya que se habla de péndulo invertido, la posición que se desea controlar es la de equilibrio inestable. Este equilibrio se conseguirá en momentos puntuales. Porque el sistema tiende a no permanecer en él debido a perturbaciones o ruidos en las señales de entrada. Por otra parte, el sistema contiene perturbaciones, lo que hace más sencillo la pérdida de equilibrio. Particularmente, este Segway tiene una perturbación provocada por la diferencia entre el punto geométrico del mismo y el centro de gravedad. Esto provoca que el sistema tienda a inclinarse hacia el centro de gravedad como se puede observar en la Figura 2.2. Como consecuencia, es indispensable un sistema de control que garantice la permanencia del vehículo autoequilibrado en su punto de equilibrio o en su defecto en la posición más cercana al punto de equilibrio. 3 4Capítulo 2. Descripción del sistema y modelo 2.2 Modelado En este apartado se pretende establecer un modelo físico-matemático del sistema. Por lo tanto se realiza una explicación del vehículo autoequilibrado con el uso de ecuaciones de su comportamiento dinámico. Este modelo será el usado en las simulaciones del sistema. Figura 2.2 Esquema del Segway. 2.2.1 Modelo no lineal El modelo no lineal es el modelo real del vehículo autoequilibrado. Para el desarrollo del mismo se hará uso de los parámetros que aparecen en la tabla 2.1. Para poder observar algunos de estos parámetros, se muestran representados en la Figura 2.2. Tabla 2.1 Parámetros físicos del sistema. Parámetro Descripción Valor mrMasa de una rueda 0.14 kg RRadio de las ruedas 0.05 m MMasa del vehículo sin ruedas 0.82 kg LDistancia del eje de las ruedas al CG 0.098 m gAceleración gravitatoria 9.81 m/s2 También será necesaria la definición de los momentos de inercia del sistema, cuyas fórmulas y definición se muestra a continuación: Jr=1 2mrR2 Jφ=ML2 En el desarrollo del modelo no se usarán estos parámetros sino los definidos en la tabla 2.1. La descripción dinámica del sistema estará basada en las ecuaciones de Lagrange. Se define la Lagrangiana 2.2 Modelado 5 Tabla 2.2 Momentos de inercia. Parámetro Descripción Valor JrMomento de inercia de una rueda 1.75 ·10−4kg ·m2 JφMomento de inercia del vehículo sin las ruedas 7.875 ·10−3kg ·m2 como la energía cinética del sistema menos la energía potencial, siendo la energía cinética: la traslacional más la rotacional. d dt ∂L ∂˙qi −∂L ∂qi =Qi L=Ttras +Trot −V Las coordenadas generalizadas elegidas son: Tabla 2.3 Coordenadas generalizadas. Coordenada Descripción φÁngulo de inclinación del segway θÁngulo que se da con el giro de la rueda con respecto al plano vertical Primero se realizará la definición de cada una de las energías para el cálculo de la Lagrangiana. Se desarrolla inicialmente la fórmula de la energía cinética de traslación. Ttras =2·1 2mr(˙x2 r+˙y2 r)+ 1 2M(˙x2 m+˙y2 m)(2.1) donde las velocidades linales del centro de gravedad y de las ruedas se definen como: xr=Rθ˙xr=R˙ θ(2.2) yr=R˙yr=0(2.3) xm=Rθ+Lsin(φ+φ0)˙xm=R˙ θ+L˙ φcos(φ+φ0)(2.4) ym=R+Lcos(φ+φ0)˙ym=−L˙ φsin(φ+φ0)(2.5) Sustituyendo estas expresiones en (2.1), desarrollando y agrupando términos se obtiene: Ttras = (mr+1 2M)R2˙ θ2+1 2ML2˙ φ2+RLM ˙ θ˙ φcos(φ+φ0)(2.6) Seguidamente se determina la energía cinética rotacional. Trot =2·1 2Jr˙ θ2+1 2Jφ˙ φ2(2.7) Sustituyendo Jr y Jφ , referidos a los momentos de inercia de la rueda y del vehículo respectivamente, por los parámetros de la tabla 2.1, la energía rotacional quedará expresada como: 6Capítulo 2. Descripción del sistema y modelo Trot =1 2mrR2˙ θ2+1 2ML2˙ φ2(2.8) Por último, se precisa la ecuación de la energía potencial. V=MgLcos(φ+φ0)(2.9) Teniendo estas tres energías formuladas, se puede definir la Lagrangiana. L= (mr+1 2M)R2˙ θ2+1 2mrR2˙ θ2+ML2˙ φ2+RLM ˙ θ˙ φcos(φ+φ0)−MgLcos(φ+φ0)(2.10) Se definirá el par motor externo aplicado a partir de las ecuaciones de Lagrange de cada una de las coordenadas lagrangianas, es decir φ y θ . El signo del par motor dependerá de la coordenada lagrangiana. Es decir, el signo depende de la influencia del par respecto a la coordenada lagrangiana. En el caso del ángulo de giro de la rueda, θ , se pondrá como positivo. Esto es así porque θ aumenta en el sentido del movimiento. Sin embargo, el ángulo de inclinación tendrá un par motor externo negativo, ya que se busca que el ángulo de inclinación disminuya. Lo que hace que el Segway busque el punto de mayor altura. −T=d dt ∂L ∂˙ φ−∂L ∂φ (2.11) T=d dt ∂L ∂˙ θ−∂L ∂θ (2.12) Se expresa el resultado de cada uno de los términos de las ecuaciones de Lagrange para la coordenada φ . ∂L ∂˙ φ=2ML2˙ φ+MRL ˙ θcos(φ+φ0) = 2b˙ φ+c˙ θcos(φ+φ0)(2.13) d dt ∂L ∂˙ φ=2ML2¨ φ+MRL ¨ θcos(φ+φ0)−MRL ˙ θ˙ φsin(φ+φ0) = 2b¨ φ+c¨ θcos(φ+φ0)−c˙ θ˙ φsin(φ+φ0) (2.14) ∂L ∂φ =−MRL ˙ θ˙ φsin(φ+φ0)+MgLsin(φ+φ0) = −c˙ θ˙ φ2sin(φ+φ0)+d˙ φsin(φ+φ0)(2.15) Se declaran los términos para la coordenada θde las ecuaciones de Lagrange. ∂L ∂˙ θ=2(mr+1 2M)R2˙ θ+mrR2˙ θ+MRL ˙ φcos(φ+φ0) = 2a˙ θ+c˙ φcos(φ+φ0)(2.16) d dt ∂L ∂˙ θ=2(mr+1 2M)R2¨ θ+mrR2¨ θ+MRL ¨ φcos(φ+φ0)−MRL ˙ φ2sin(φ+φ0) = 2a¨ θ+c¨ φcos(φ+φ0)−c˙ φ2sin(φ+φ0) (2.17) ∂L ∂θ =0(2.18) donde a= (mr+1 2M)R2+1 2mrR2b=ML2(2.19) c=MRL d =−MgL (2.20) 2.2 Modelado 7 Finalmente, se puede definir el par motor aplicado sustituyendo las derivadas parciales por los términos antes calculados. −T=2b¨ φ+ccos(φ+φ0)¨ θ−dsin(φ+φ0)˙ φ(2.21) T=2a¨ θ+ccos(φ+φ0)¨ φ−csin(φ+φ0)˙ φ2(2.22) Por lo tanto, igualando las ecuaciones se obtiene la ecuación que expresa el comportamiento dinámico del vehículo autoequilibrado. (2a+ccos(φ+φ0)) ¨ θ+(2b+ccos(φ+φ0)) ¨ φ−c˙ φ2sin(φ+φ0)−dsin(φ+φ0) = 0(2.23) 2.2.2 Modelo lineal El controlador busca llevar el sistema a la posiciíon de equilibrio. Para el diseño del controlador se necesitará linealizar el modelo, es decir linealizar la ecuación que define el comportamiento dinámico del segway. Para realizar esta linealización, se supone que el ángulo de inclinación del vehículo autoequilibrado es muy pequeño por lo que se puede utilizar la siguiente aproximación: φ+φ0<< 1. Ya que en la ecuación aparecen las funciones de sin y cos , considerando que el ángulo φ+φ0<< 1 , se puede afirmar: sin(φ+φ0)≃φ+φ0cos(φ+φ0)≃1(2.24) Se define la función Fque será linealizada en torno al punto de equilibrio. F≡(2a+c)¨ θ+(2b+c)¨ φ−c˙ φ2(φ+φ0)−d(φ+φ0) = 0(2.25) El punto de equilibrio de este sistema teniendo en cuenta la perturbación provocada por la diferencia entre el punto del centro geométrico y el del centro de masa, junto con sus derivadas, será: φeq =−φ0˙ φeq =0(2.26) ¨ φeq =0¨ θeq =ueq =0(2.27) Para linealiar F se aplicará una serie de Taylor alrededor de un punto de operación, en este caso el punto de equilibrio. Ya que la variación entre las variables y su punto de equilibrio es muy pequeña, se desprecian los términos de orden superior. El resultado final será una ecuación diferencial lineal. Es importante destacar que el rango de validez del modelo linealizado estará limitado por el punto de linealización. ∂F ∂u|eq(u−ueq)+ ∂F ∂¨ φ| eq (¨ φ−¨ φeq)+ ∂F ∂˙ φ| eq (˙ φ−˙ φeq)+ ∂F ∂φ | eq (φ−φeq) = 0(2.28) Se muestra lo obtenido en cada una de las derivadas parciales de la función F. ∂F ∂u|eq =2a+c∂F ∂¨ φ| eq =c+2b(2.29) ∂F ∂˙ φ| eq =0∂F ∂φ | eq =−d(2.30) 14 Capítulo 3. Control Predictivo con Algoritmo FISTA Figura 3.1 Ejemplo 1. Ejemplo 2 En este ejemplo el mínimo óptimo de la función no se encuentra entre los límites que se muestran con las gráficas verde y roja, por lo que el mínimo como solución posible será el mínimo entre los valores límite. En este caso el mínimo se da en el límite superior. Figura 3.2 Ejemplo 2. Ejemplo 3 En este ejemplo la función tiene un máximo óptimo y el mínimo se encontrará en el valor mínimo de uno de los límites. En este caso el mínimo se da en el límite inferior. 3.2 Algoritmo FISTA 15 Figura 3.3 Ejemplo 3. 3.2.2 Problema con restricciones de igualdad Se parte del problema inicial: J∗=argm´ ın Z 1 2ZTHZ +fTZ(3.39) s.a.LB ≤Z≤UB (3.40) EZ =b(3.41) Como se ha demostrado, este problema sin las restricciones de igualdad se resuelve con un cálculo directo, sin embargo al añadir la restricción de igualdad se complica la resolución del mismo. Las restricciones de tipo igualdad no establecen fronteras al conjunto de las soluciones factibles del problema, sin embargo reducen las dimensiones del espacio donde el problema estará definido, dificultando así su cumplimiento. Sería más sencillo poder encontrar la solución si se tuvieran solo desigualdades, ya que habría un mayor rango posible de resultados como se muestra en el ejemplo anterior. Extremos condicionados: multiplicadores de Lagrange Para poder calcular los extremos condicionados de la función objetivo, es decir el máximo o mínimo de la misma, se define la función Lagrangiana con la cual se facilita el cálculo, ya que esta consigue transformar el extremo condicionado en un extremo libre, es decir un problema sin restricciones de igualdad. Por lo que para encontrar el mínimo en lugar de usar la función objetivo se utilizará la Lagrangiana. Se define: J(Z) = 1 2ZTHZ +fTZ(3.42) L(Z,x) = J(Z)−xT(EZ −b)(3.43) Se define J(Z) como la función objetivo que se quiere minimizar y L(Z,x) como la función Lagrangiana la cual contiene los multiplicadores de Lagrange. Este método contiene un procedimiento para encontrar máximos y mínimos de funciones de múltiples variables sujetas a restricciones. Consiste por tanto en pasar de un problema restringido con n variables a uno sin restricciones con n+k variables, donde k es la cantidad de restricciones. Las knuevas variables son llamadas multiplicadores de Lagrange. En consecuencia, se definirán tantos multiplicadores de Lagrange como ecuaciones de ligadura o lo que es 16 Capítulo 3. Control Predictivo con Algoritmo FISTA lo mismo restricciones de igualdad. En este caso al tener una única restricción se definirá solo un multiplicador. Es decir se añade solo una variable más al problema eliminando así la restricción de igualdad. Esta nueva variable es x . El tamaño del multiplicador x , dependerá del tamaño de la variable de la ecuación de ligadura, siendo esa variable el vector Z , cuyo tamaño es N∗(n+m) . Por lo tanto, el multiplicador de Lagrange x será definido con las mismas dimensiones que el vector Z. Se sabe que se pretende calcular el Z para el que la función Lagrangiana es mínima, por esta razón se define Z(x). Z(x) = argm´ ın ZL(Z,x)(3.44) Finalmente usando el teorema de la función implícita 1 , el sistema será resoluble. Este teorema permite que en una o más ecuaciones de varias variables se pueda definir una o varias de ellas como función de las demás. Esto lograría que la Langragiana solo dependiera de la variable xhaciendo que la variable Zdepende de x. f(x) = L(Z(x),x)(3.45) Es conocido que el coste óptimo J∗ se obtiene cuando EZ =b , es decir cuando se cumple la condición de igualdad, quedando L(Z∗,x) = J∗ . En una cota no óptima, L(Z,x) será menor que J(Z) al ser xT(EZ∗−b) mayor a cero y estar restándose en la expresión L(Z,x) . También se nombrará L(Z,x) como f(x) que será menor que J(Z), por lo tanto se cumple que: J∗=L(Z∗,x)≥f(x) = L(Z(x),x)(3.46) Es claro entonces que se pretende encontrar la cota inferior más alta, es decir que para alcanzar el óptimo, J∗, se necesita encontrar el xpara el que f(x)sea máximo, se deberá resolver: x∗=argm´ ax xf(x)(3.47) Como conclusión se quiere calcular el Z para el cual la Lagrangiana, L(Z,x) , es mínima. Si se quiere expresar con la variable x , entonces se desea obtener x para la cual f(x) que solo depende de esta variable es máxima. Finalmente se calculará el resultado final de la función objetivo que se usará en el algoritmo FISTA 1Teorema de la función implícita Sea F:A⊂Rn×R7→ Rdefinida en un entorno del punto (x0,y0)∈A,Aabierto de Rn×R. Supongamos que: 1. F(x,y):=F(x1,x2,...,xn,y)∈C(p)(A),p≥1, 2. F(x0,y0):=F(x01,x02,...,xn0,y0) = 0, 3. F0 y(x0,y0) = ∂F(x01,x02,...,xn0,y0) ∂y6=0. Entonces existe un abierto I=Ix×Iy= (x0−h,x0+h)×(y0−k,y0+k) alrededor del punto (x0,y0),I⊂A , y una función f:Ix⊂ Rn7→ Iy⊂Rtal que: 1. F(x,y) = 0en Isi y solo si f(x) = y, 2. f(x)∈C(p)(Ix), 3. Para todo x∈Ix, las derivadas parciales de f(x)se calculan por la fórmula ∂f(x) ∂xi:=∂f(x1,...,xn) ∂xi=−[F0 y(x,f(x))]−1·[F0 xi(x,f(x))],i=1,2,...,n, donde por F0 xidenotamos la derivada parcial ∂F ∂xi. 3.2 Algoritmo FISTA 17 teniendo en cuenta los multiplicadores de Lagrange y añadiendo por tanto la nueva variable relacionada con la restricción de igualdad. Desarrollando la función Z(x)se obtendrá: Z(x) = argm´ ın Z 1 2ZTHZ +fTZ−xT(EZ −b) = =argm´ ın Z 1 2ZTHZ +fTZ−xTEZ +xTb Ya que se busca el Z para el que la función se minimice, se puede afirmar que el término xTb es una constante. Si se define la misma función pero sin la parte constante quedaría: g(z,x) = argm´ ın Z 1 2ZTHZ + ( fT−xTE)Z Por tanto, la función Z(x)como g(x)alcanzan el mínimo en el mismo valor de Z, se puede afirmar: argm´ ın Z 1 2ZTHZ +fTZ−xTEZ +xTb= =argm´ ın Z 1 2ZTHZ + ( fT−xTE)Z Es decir que para encontrar el Z que haga mínima la función, tanto Z(x) como g(z,x) ofrecen el mismo resultado. Por último usando las propiedades de la traspuesta la función g(z,x)quedará: argm´ ın Z 1 2ZTHZ + ( f−ETx)TZ La función obtenida, g(z,x) , será la que se minimice utilizando el algoritmo FISTA. Se tendrán en cuenta las restricciones del vector Zen este cálculo. Teorema local de Taylor El algoritmo FISTA necesitará hacer una serie de iteraciones antes de encontrar la solución. El algoritmo consiste en encontrar el Z óptimo, para el que se minimiza g(z,x) . Considerando que x será constante. La forma más rápida de encontrar la solución es usando el teorema local de Taylor, que propone derivar una aproximación local. Este teorema proporciona la forma de calcular lo que debe variar el valor del vector Z para conseguir encontrar un extremo, es decir un máximo o un mínimo de la función. Una vez encontrado el Z que minimiza g(x,z) , se comprueba si se cumple que la norma de EZ −b es menor o igual al valor de TOL , siendo TOL un valor que se aproxima a cero. Se sabe que si el valor de la norma de un vector tiende a cero, quiere decir que cada uno de los elementos del vector tenderá a cero. Por lo tanto se estarían cumpliendo las restricciones de igualdad. Y se habría encontrado la solución. Si esto no sucede se volverá a actualizar el valor de x , en el que además se le añaden una serie de operaciones posteriores a este cálculo, que agilizan la búsqueda de la solución. Estos cálculos consiguen aproximar el valor de Z a la solución óptima. Por lo tanto se logrará hallar el Z∗ en menor tiempo, ya que se necesitarán menos iteraciones para resolver el problema. Esto es al añadir el paso III y IV en c) en el algoritmo. A continuación se define el teorema local de Taylor: 18 Capítulo 3. Control Predictivo con Algoritmo FISTA f(x+∆x)−f(x)≥ −∆xT(EZ(x)−b)−1 2∆xTW∆x(3.48) siendo W=EH−1ET. Se busca que esta desigualdad se convierta en una igualdad, por lo tanto se debe maximizar h=−∆xT(EZ(x)− b)−1 2∆xTW∆x, ya que lo que se busca es ∆xpara el cual hes máxima, quedando la siguiente función: argm´ ax ∆x−∆xT(EZ(x)−b)−1 2∆xTW∆x=argm´ ın ∆x ∆xT(EZ(x)−b)+ 1 2∆xTW∆x=(3.49) =arg∆xTm´ ın ∆x((EZ(x)−b)+ 1 2W∆x) = arg∆xT((EZ(x)−b)+m´ ın ∆x 1 2W∆x)(3.50) Una función o la mitad de una función alcanza su valor mínimo en el mismo punto. Por lo tanto se cumple que m´ ın∆x1 2W∆x=m´ ın∆xW∆x. Finalmente queda como: arg∆xT((EZ(x)−b)+m´ ın ∆xW∆x) = arg∆xTm´ ın ∆x((EZ(x)−b)+W∆x)(3.51) En este problema de optimización se alcanzará el óptimo en el punto en el que se anule el gradiente, ya que si el gradiente es nulo la pendiente también lo es y por tanto será un punto con un máximo o un mínimo. f(x+∆x)−f(x) ∆x= (EZ(x)−b)+W∆x=0(3.52) Para terminar se obtiene el valor de ∆x para conseguir que el gradiente sea nulo y por tanto se encuentra así un extremo. ∆x=−W−1(EZ(x)−b)(3.53) Como conclusión, se busca variar la x para encontrar el punto donde la pendiente es nula obteniendo así un extremo. Implementación del algoritmo FISTA La implementación del algoritmo FISTA quedaría: 1. k←1,x0←0,y1←0,t1←1,FIN ←NO,Q←W−1 2. Repetir hasta FIN =SI a) Calcular Z(yk)tal que m´ ın Z 1 2ZTHZ + ( f−ETy)TZ s.a.LB ≤Z≤UB b) Si kEZ(yk)−bk≤TOL, entonces FIN ←SI 3.2 Algoritmo FISTA 19 c) Si no I. Calcular ∆y← −Q(EZ(yk)−b) II. Hacer xk←yk+∆y III. Hacer tk+1←1 2(1+q1+4t2 k) IV. Hacer yk+1←xk+tk−1 tk+1(xk−xk−1) V. Hacer k←k+1 3. Z∗←Zk Habría que tener en cuenta que la y que se describe en el algoritmo es la x que se ha descrito en el teorema de Taylor. En ocasiones puede resultar demasiado complejo encontrar la solución óptima, por esta razón se debe añadir dentro de la programación un número máximo de iteraciones. Si se alcanzara esta cantidad de iteraciones se devolvería como resultado la última Z calculada y finalizaría el programa. Se sabe que este valor no será el óptimo, ya que el cálculo se ha parado antes de encontrarlo, pero posiblemente sí sea una buena aproximación del mismo. A continuación se muestran las definiciones de las matrices: El vector Z que contiene hasta N vectores de estado y Nvectores con la acción de control, por lo que su tamaño será de (N(m+n)): Z=            xn(0) um(0) xn(1) um(1) . . . xn(N−1) um(N−1)            donde el vector xestará definido como x=  x1 x2 . . . xn  y el vector uestará definido como u=  u1 u2 . . . um  . La matriz Htendrá unas dimensiones de (N(m+n)xN(m+n)), se define como: H=            Q0 0 0 ··· 0 0R0 0 ··· 0 0 0 Q0··· 0 0 0 0 R··· 0 . . .. . .. . ..... . . 0 0 0 ··· Q0 0 0 0 ··· 0R            20 Capítulo 3. Control Predictivo con Algoritmo FISTA El vector fserá de dimensión (N(m+n)): f=            −Qxr −Rur −Qxr −Rur . . . −Qxr −Rur            La matriz Etendrá dimensiones de ((N+1)nxN(m+n)) y se define: E=        I0000··· 0 0 A B −I0 0 ··· 0 0 0 0 A B −I··· 0 0 . . .. . ..... . .. . . 00000··· A B        El vector ben el modelo general tendrá una dimensión de ((N+1)n)y será definido como: b=        x 0 0 . . . xr        donde el vector x=   x1(0) x2(0) . . . xN(0)   y el vector xr=  xr,1 xr,2 . . . xr,N  . Sin embargo, ya que el sistema tiene la perturbación, el vector bse definirá como: b=        x −Epd −Epd . . . xr−Epd        Los vectores de los límites inferior y superior tendrán las mismas dimensiones que Z y vienen definidos como: LB =            LBx LBu LBx LBu . . . LBx LBu            UB =            UBx UBu UBx UBu . . . UBx UBu            El código implementado en Matlab que describe el algoritmo FISTA se muestra a continuación: Código 3.1 Cálculo del MPC. 3.3 Bucle del MPC y componentes 21 x=zeros((N+1)*n,1);x_ant=zeros((N+1)*n,1);y=zeros((N+1)*n,1); t=1;FIN=0;cont1=0; Qfista=inv(E/H*E'); %matrices f1 y b f1=zeros(N*(n+m),1); for i=1:n+m:N*(n+m) f1(i:i+n-1,1)=-Q*xr; f1(i+n:i+n,1)=-R*ur; end dd=Ep*d_filtrada b=zeros((N+1)*n,1); b(1:n,1)=x_filtrada; for i=1:3:N*n b(n+i:n+i+2)=-dd; end b((N+1)*n-(n-1):(N+1)*n,1)=xr-dd; while FIN==0 z=zeros(n,1); f=f1-E'*y; for i=1:N*(n+m) z1=-f(i,1)/H(i,i); z(i)=max(Llim(i,1),min(z1,Ulim(i,1))); end if norm(E*z-b)<=1e-6 FIN=1; else incry=-Qfista*(E*z-b); x=y+incry; t_sig=0.5*(1+sqrt(1+4*t^2)); y=x+(((t-1)/t_sig)*(x-x_ant)); cont1=cont1+1; x_ant=x; t=t_sig; end if cont1>2000 FIN=1; disp('Se supera el número de iteraciones') end u=z(4); end end 3.3 Bucle del MPC y componentes En este apartado se explicará el bucle de control completo. Como entrada se encuentra el parámetro r , que representa la referencia. En este caso la referencia será la velocidad de la rueda. A su vez en cada bloque existirán una serie de valores ajustables, calculados previamente.Estos valores aparecen en cada uno de lo bloques del controlador y entre ellos están: N , Q y R , parámetros descritos anteriormente en la explicación del Algoritmo FISTA. Los bloques a programar del control son: Filtro, SSTO, MPC y OBS. En cuanto al sistema, este será el cálculado en el Capítulo 1, en la sección del Modelo no Lineal, comprobando así que el control se realiza correctamente y es aplicado al sistema que se ha descrito. 22 Capítulo 3. Control Predictivo con Algoritmo FISTA Primero se muestra un diagrama de bloques como resumen esquemático del lazo de control en la Figura 3.4. Figura 3.4 Diagrama de bloques. Entre los componentes que aparecen en el esquema se hace un pequeño resumen de cada uno de ellos: 1. SSTO o Steady-State Target Optimization: calcula el punto de equilibrio asociado a la referencia que tiene de entrada, es decir rf . En resumen calcula puntos de equilibrio alcanzables por el sistema, xr y ur . En este caso el bloque SSTO también usará el Algoritmo FISTA, ya que este calculo requiere que se realice una optimización asociada a unas restricciones. 2. OBS o estimador de estados: se utilizará el Filtro de Kalman que proporcionará el estado actual estimado, ˆxy la perturbación estimada ˆ d. Toma como entradas la acción de control uy la salida de la planta y. 3. Filtro de referencia: consigue que los cambios de referencia se realicen de forma exponencial en lugar de usar un escalón. Tiene como entrada ry como salida el suavizado o filtrado de esa referencia rf. 4. MPC o Control Predictivo basado en Modelo: este bloque es el explicado en el apartado del Algoritmo FISTA. Se encarga de resolver la minización de la función objetivo sujeta a una serie de restricciones. Por lo tanto este bloque calcula la actuación del sistema, que será la entrada al bloque de la Planta. Para el cómputo de esto, se tendrán como entradas los puntos de equilibrio calculados por el SSTO, es decir xryur, y el estado actual estimado ˆx. 3.3.1 SSTO Este bloque se usa para la obtención óptima de puntos de equilibrio. Puede existir el caso de referencias no alcanzables. Esto es resuelto ya que el SSTO soluciona el problema encontrando puntos óptimos para esas referencias no alcanzables. Su finalidad es conseguir que la salida del sistema converja hacia la referencia, es decir que la salida esté lo más cerca posible del valor de referencia exigido: l´ ım k→∞kyk−rkk=0 3.3 Bucle del MPC y componentes 23 Se quiere por tanto encontrar el valor de un punto de equilibrio, es decir xr , ur y hr . Siendo hr la holgura, que facilita la búsqueda de la solución. El problema planteado en este componente del bucle se define como: (x∗ r,u∗ r,h∗ r) = arg m´ ın xr,ur,hr kxrk2 Qr+kurk2 Rr+khk2 T(3.54) s.a.xr=Axr+Bur+Epd(3.55) r=Cxr+h(3.56) LBx≤xr≤UBx(3.57) LBu≤ur≤UBu(3.58) Al ser un problema de minimización se puede utilizar el algoritmo FISTA al que se ha dado uso anteriormente para la minimización de la diferencia entre el estado de equilibrio y el del modelo lineal en tiempo discreto. En este caso se busca la minimización de la diferencia entre la referencia de la salida del sistema. Ya que se va a dar uso del algoritmo FISTA, este se reformula como un problema QP. Z∗=argm´ ın Z 1 2ZTHZ +fTZ(3.59) s.a.LB ≤Z≤UB (3.60) EZ =b(3.61) Donde la matriz Htendrá unas dimensiones de (m+n+p)x(m+n+p), se define como: H=  Qr0 0 0Rr0 0 0 T   El vector fserá de dimensión m+n+p: f=     0 0 . . . 0      La matriz Etendrá dimensiones de (n+m+p)x(n+p)) y se define: E=A−I B 0 C0I El vector btendrá una dimensión de n+py será definido como: b=−Epd r El vector Z en este caso solo contiene el estado de equilibrio alcanzable xr , el vector de la acción de control de equilibrio alcanzable y una tolerancia h, por lo que su tamaño será de m+n+p: Z=  xr ur h   30 Capítulo 4. Simulación en Matlab 4.1.2 Matrices QyRdel MPC Entre las variables del sistema que interesa más su control se tiene φ y ˙ θ , es decir el ángulo de inclinación del vehículo autoequilibrado y la velocidad de giro de la rueda. Es por esta razón que se debe dar más peso en la matriz de ponderación Qa estas variables y no tanto a la variable ˙ φ. La relación entre Q y R debe ser tal que la respuesta transitoria sea lo menor posible. Además el esfuerzo de control no puede sobrepasar, ni tener un valor demasiado cercano al límite del mismo. Por lo tanto se busca el equilibrio entre rapidez del sistema en cuanto a alcanzar el régimen permanente y no acercarse demasiado al máximo valor impuesto en la restricción del esfuerzo de control. Finalmente, los valores de las matrices de ponderación QyRserán: Q=  10 0 0 010 0 0 10  R=0.1 4.1.3 Matrices Q0yR0del OBS Se sabe que Q0 es la matriz de covarianza del modelo y que R0 es la matriz de covarianza del ruido en las mediciones, es decir el ruido de los sensores. Teniendo en cuenta la relación R0/Q0 , se puede buscar una mayor influencia de la medida de los sensores o una mayor influencia del modelo. Ya que el modelo es bueno, la influencia de los sensores deberá ser superior, por esta razón la relación R0/Q0deberá ser mayor o al menos igual a la unidad. Siendo los valores tomados: Q0=  100 010 001  R0=10 4.1.4 Matrices QryRrdel SSTO En este caso las matrices Qr y Rr tienen el mismo significado que en el MPC. Esto es así ya que el bloque SSTO utiliza el mismo modelo para los cálculos. Por lo tanto, Qr es la matriz que ponderar el error en el estado y Rrla matriz que pondera el error en la señal de control. La diferencia entre el SSTO y el MPC es que el SSTO busca minimizar la diferencia entre la salida de la planta y la referencia que se desea alcanzar. En cambio el MPC pretende minimizar la diferencia entre el estado de referencia y el estado estimado actual. Los valores que se le han asignado a estas matrices son: Qr=  10 0 0 0 1 0 0 0 1  Rr=0.1 4.1.5 Matriz Tdel SSTO En el bloque SSTO se ha añadido una holgura para facilitar la búsqueda de la solución. Esta holgura se define usando la variable h . Esto provoca que el vector z esté modificado al tener que añadirse dicha variable que aparece en la ecuación de minimización. Por lo tanto esta variable tendrá una matriz por la que será multiplicadda siendo esta T. 4.2 Gráficas 31 Para que no surjan problemas y la holgura pueda ser grande, se define un valor que tienda a infinito en esta matriz. T=1080 0 108 4.1.6 Límites del SSTO Por otra parte, también habría que tener en cuenta que al cambiar la dimensión de z también cambia la dimensión del vector de los límites de las variables. Se añadirá en consecuencia un valor límite a la variable h. Se disminuyen los valores límites tanto del estado como de la actuación. Sin embargo, se usará un valor límite de la holgura que tienda a infinito. LB =       −20 ·π 180 −2 −9 −100 −108       UB =       20 ·π 180 2 9 100 108       4.2 Gráficas En este apartado se pueden ver los resultados en Matlab. Una vez ajustados los parámetros se comprobará inicialmente que se sigue la referencia y que los cambios en la misma se realizan corectamente. En cada simulación o experimento se muestra cada una de las curvas de las variables del vector z representadas en la misma gráfica y un conjunto de gráficas con las variables por separado para que puedan ser observadas ajustando su tamaño al límite entre el cual se encuentra la curva de cada una. 4.2.1 Saturación de la actuación y de la velocidad de la rueda Una primera comprobación de que el algoritmo FISTA se cumple con respecto a las restricciones de la actuación se realiza con este experimento. Para poder verificar el cumplimiento de la actuación se utiliza un horizonte de predicción de 9. De esta forma en las gráficas se puede observar que la actuación satura en ciertos momentos aunque el sistema alcanza el equilibrio. Sin embargo, nunca debería llegar a tener un valor tan alto la acción de control. También se puede observar que se alcanza el límite de la velocidad de la rueda. Se confirma así que las restricciones de velocidad también se cumplen con el algoritmo de control. En este ejemplo se busca una velocidad de la rueda de 2 rad/s. 32 Capítulo 4. Simulación en Matlab Figura 4.1 Saturación de la actuación. (a) Ángulo de inclinación. (b) Velocidad de inclinación. (c) Velocidad de la rueda. (d) Acción de control. Figura 4.2 Saturación de la actuación con referencia de velocidad igual a 2 rad/s. 4.2 Gráficas 33 4.2.2 Gráficas con perturbación nula Inicialmente se realizarán experimentos donde se compruebe el seguimiento de la referencia, es decir de la velocidad de la rueda. Se van a hacer cuatro comprobaciones diferentes: 1. Vehículo autoequilibrado parado manteniendo su posición con velocidad de la rueda nula. 2. Vehículo autoequilibrado con velocidad de la rueda positiva. 3. Vehículo autoequilibrado con velocidad de la rueda negativa. 4. Varios cambios de referencia tanto positiva como negativa en la velocidad de la rueda. Referencia de velocidad nula En este caso se busca que el comportamiento del vehículo autoequilibrado sea permanecer parado, ya que la referencia de velocidad de la rueda es nula. Como resultado se debe obtener este seguimiento de velocidad a cero, además de que se alcance el equilibrio inestable del sistema. Esto quiere decir que el ángulo de inclinación deberá de estabilizarse siendo su valor igual a cero. Figura 4.3 Referencia de velocidad nula y sin perturbación. 34 Capítulo 4. Simulación en Matlab (a) Ángulo de inclinación. (b) Velocidad de inclinación. (c) Velocidad de la rueda. (d) Acción de control. Figura 4.4 Referencia nula y perturbación nula. Referencia de velocidad positiva En esta simulación se busca verificar que el sistema es capaz de mantener cierta velocidad positiva en la rueda en lugar de permanecer quieto. La referencia que se le impone es una velocidad de 2 rad/s. Figura 4.5 Referencia de velocidad igual a dos y sin perturbación. 4.2 Gráficas 35 (a) Ángulo de inclinación. (b) Velocidad de inclinación. (c) Velocidad de la rueda. (d) Acción de control. Figura 4.6 Referencia igual a dos y perturbación nula. Referencia de velocidad negativa Por último, se comprueba que el control también funcione para velocidades negativas. En este caso la simulación se ha realizado para una velocidad de la rueda de -1 rad/s. Figura 4.7 Referencia de velocidad igual a menos uno y sin perturbación. 36 Capítulo 4. Simulación en Matlab (a) Ángulo de inclinación. (b) Velocidad de inclinación. (c) Velocidad de la rueda. (d) Acción de control. Figura 4.8 Referencia igual a menos uno y perturbación nula. Varios cambios de referencia En este apartado se cambiará el valor de referencia dos veces. Primero se exige una referencia positiva de 1 rad/sy a continuación, pasados 8 s se exige una referencia de -1 rad/s. Figura 4.9 Referencia de velocidad de uno a menos uno y sin perturbación. 4.2 Gráficas 37 (a) Ángulo de inclinación. (b) Velocidad de inclinación. (c) Velocidad de la rueda. (d) Acción de control. Figura 4.10 Referencia de uno a menos uno y perturbación nula. 4.2.3 Gráficas con perturbación distinta a cero En este punto se busca que el ángulo de inclinación sea contrario a la perturbación alcanzando así el equilibrio, ya que se ha modificado el punto donde se encuentra el centro de masa. Si no se tratara esta perturbación, el segway no podría seguir la referencia de velocidad ya que este tendría que moverse para no perder el equilibrio. Esto es así porque el sistema buscaría un ángulo de inclinación nulo, tomando esto como el punto de equilibrio. Sin embargo, en estos casos el punto de equilibrio se encuentra en el ángulo de inclinación provocado por la perturbación y con signo contrario a la misma. Referencia de velocidad nula Se sabe que el sistema tiene una perturbación provocada por la diferencia entre el centro de gravedad y el centro geométrico. Se comprueba de forma experimental que el ángulo que provoca esto es de 0.03 rad . Por esta razón para que el vehículo autoequilibrado permanezca en equilibrio deberá estar inclinado -0.03 rad. Por otra parte, se añade que también haga un seguimiento de velocidad nula, es decir que permanezca parado. 38 Capítulo 4. Simulación en Matlab Figura 4.11 Referencia de velocidad nula y con perturbación. Estos valores antes descritos se pueden observar mejor en las gráficas por separado de cada una de las variables. Se comprueba que el ángulo de inclinación permanece en un valor de -0.03 rad y que la velocidad de la rueda alcanza una velocidad nula. (a) Ángulo de inclinación. (b) Velocidad de inclinación. (c) Velocidad de la rueda. (d) Acción de control. Figura 4.12 Referencia nula y perturbación igual a 0.03 rad. 5 Implementación en Arduino El objeto final de este trabajo es la implementación del código generado en Matlab en una placa de Arduino. El vehículo autoequilibrado ya se encuentra montado y contiene una placa Arduino Mega 2560, por lo tanto se volcará el código en este dispositivo. El microcontrolador es el que recibe los datos de los sensores y a partir de eso realiza las operaciones necesarias para el calculo de la señal de entrada a los actuadores. 5.1 Características de la placa Entre los elementos más interesantes de la placa se puede destacar su microcontrolador, el tipo de comunicaciones que se pueden realizar y las características electrónicas y eléctricas del dispositivo. Figura 5.1 Placa Arduino Mega 2560. Esta unidad controladora está basada en el microcontrolador Atmel ATmega2560. Este chip es de 8 bits y es capaz de trabajar con una SRAM, una EEPROM y una memoria flash. Esta placa está basada en arquitectura AVR. A continucación se muestran las características del microcontrolador. En cuanto a las comunicaciones, la placa dispone de un conector USB. El código se cargará usando este tipo de comunicación desde el ordenador a la placa. En cuanto a la alimentación del chip, existen dos maneras. Se puede alimentar a través del conector jack, el cual utiliza el segway de forma normal o por el USB, los casos en los que se carga el código a la placa. 39 46 Capítulo 5. Implementación en Arduino 5.2.3 Disminución de la aproximación a cero En el algortimo FISTA, existe una parte en la que se busca que la restricción de igualdad se cumpla. Es decir que EZ =b+d o lo que es lo mismo que kEZ(yk)−b−dk=0 . Ya que esto resultaría demasiado complejo, se busca que esta expresión se aproxime a cero por lo tanto se usa un valor llamado tolerancia. Esta tolerancia será un valor cercano a cero. En el caso de Matlab, es fácil poder exigir que el valor de la tolerancia sea muy pequeño. Esto es así debido a que Matlab es capaz de trabajar con una cantidad muy grande de decimales, hasta 15 posiciones decimales. Esto hace que Matlab sea capaz de ofrecer un resultado mejor. Por lo tanto la convergencia del algoritmo hacia un valor próximo a cero es más sencilla. Es decir se consigue que se cumpla prácticamente la restricción de igualdad. En Arduino esta convergencia es mucho más compleja. Esto es debido a que la capacidad máxima que tiene Arduino en cuanto al número de decimales exactos es 6. Se puede comprobar que esto es así tanto usando datos de tipo float como usando datos de tipo double. Ambos permiten únicamente 6 decimales exactos. Esto quiere decir que Arduino en sus cálculos usa menos de la mitad de decimales que Matlab. Por otra parte, es cierto que se puede pedir a Arduino que muestre más de 6 decimales, sin embargo se comprobará que no van a coincidir con los decimales que en realidad aparecen en Maltab bien calculados. Por esta razón la tolerancia en Arduino no puede ser un valor tan pequeño como en Matlab. Si se exigiera un valor demasiado próximo a cero, el algoritmo no convergería. Esto provocaría que hiciera el máximo número de iteraciones y diera como resultado un solución subóptima. Sin embargo, la solución que ofrezca Arduino siempre será menos exacta que Maltab. Aunque la diferencia en cuanto a los valores de los dígitos de los decimales se puede encontrar entre el cuarto, esta solución será válida para el control del Segway. No es necesaria tanta precisión en el sistema real. Como conclusión, en Maltab se puede exigir una tolerancia de 1·10−6 . En cambio, en Arduino, solo se exigirá una tolerancia de 1·10−2 . De esta forma se solventaría el problema de la no convergencia en Arduino por tener una restricción demasiado estricta. 5.2.4 Eliminar las raíces cuadradas Dentro del algoritmo existe el uso de raíces cuadradas, algo que necesita más tiempo de cálculo en comparación a otras operaciones. Como se quiere conseguir poder realizar el control en el menor tiempo posible, es bueno considerar eliminar cálculos con mayor tiempo de cómputo. El cálculo de la norma es un cálculo en el que al final se necesita hacer la raiz cuadrada, esto se encuentra en el punto del cálculo de la norma, kEZ(yk)−b−dk≤TOL . Es decir pEZ(yk)−b−d≤TOL . Para que Arduino no tenga que realizar el cálculo de la raiz. Lo que se deberá exigir será EZ(yk)−b−d≤TOL2 . De esta forma se elimina ese cómputo y en el valor de la tolerancia, se pondrá el valor de la tolerancia al cuadrado directamente calculado. Sin que Arduino tenga que hacer ninguna multiplicación. Esto no supondrá un gran cambio en la disminución del tiempo de ejecución del algoritmo, pero si se está cercano a alcanzar el tiempo en el que se debe calcular el control, este cambio puede ser decisivo. 5.2.5 Problema Como ya se ha mencionado en los anteriores apartados, el algoritmo FISTA debe ser operado en el menor tiempo y con los menores recursos posibles. Para ello, se han reducido el número de operaciones y datos, al igual que no se han dado restricciones tan estrictas en la convergencia del algoritmo. También se han eliminado operaciones que podían tardar más tiempo en su cómputo. Sin embargo, a pesar de todas estas implementaciones sigue existiendo el problema del tiempo. 5.3 Solución posible 47 Restricción de tiempo del sistema Se debe considerar que el sistema real debe realizar el control cada 40 ms. Por esta razón es importante tener en cuenta que el bucle de control no puede sobrepasar esta restricción de tiempo. Si no se tuviera en cuenta esto, el vehículo autoequilibrado no podría conseguir el equilibrio en ningún caso ya que la acción de control no sería conocida en el tiempo requerido. Esto quiere decir que se está trabajando en un sistema de tiempo real. La respuesta debe ser garantizada dentro de la restricción de tiempo. Por esta razón se realiza la comprobación de que el algoritmo FISTA es calculado en un tiempo menor o igual a 40 ms. Sin embargo, debido a la gran dimensión de las matrices, Arduino necesita un tiempo bastante superior a 40 ms, siendo el mejor de los casos un cálculo que tarda alrededor del segundo. 5.3 Solución posible Hasta ahora todas las modificaciones para el menor uso de recursos y el cómputo en menor tiempo han sido implementadas en el Software. Sin embargo, se comprueba que todas estas transformaciones no son suficientes para el cumplimiento de la restricción de tiempo. Esta restricción es imprescindible cumplirla en el sistema real. Si no se cumple el control nunca será calculado en el instante que es indispensable. Existe otro tipo de solución, esta sería implementar en el vehículo autoequilibrado otro Hardware más adaptado a las necesidades del nuevo tipo de control. Entre las posibles soluciones en cuanto al cambio de Hardware una de ellas sería utilizar una placa de Arduino más adecuada a las exigencias del algortimo FISTA. Una de las placas que parece ofrecer la solución que se precisa es la Arduino Due. 5.3.1 Comparación Arduino Mega vs Arduino Due El Arduino Due es idóneo para proyectos que necesitan una alta capacidad de procesamiento, ya que tiene 32 bits, en lugar de 8 bits. Se podrán realizar operaciones con datos de 4 bytes en un solo ciclo de reloj, en vez de 1 byte en un solo ciclo. Esto quiere decir que los cálculos se realizarán a una mayor velocidad, que es justo lo que necesita este tipo de sistema. De esta forma se solventaría el hándicab de la restricción de tiempo del segway. Sin embargo, es posible que con esta solución no se consiga que el control se realice cada 40 ms. Por lo tanto, también se debería seguir mejorando el software. 48 Capítulo 5. Implementación en Arduino Tabla 5.4 Comparación. Característica Mega Due Microcontrolador ATmega2560 SAM3X8E ARM Cortex-M3 Bits 8bits 32bits Memoria flash 256KB 512KB SRAM 8KB 96KB EEPROM 4KB - Frecuencia de reloj 16MHz 84MHz Temporizadores 6 Timers 9 Timers UARTs 4 pines 4 pines SPI 5 pines 5 pines TWI (I2C) 2 pines 2 pines Tensión nominal 5V3.3V Tensión de entrada (uso normal) 7−12V5−12V Tensión de entrada (límite) 6−20V6−16V Pines de E/S digital 54 pines (15 para PWM) 54 pines (12 para PWM) Pines de E analógica 16 pines 12 pines Pines de S analógica - 2 pines Corriente continua por pin E/S digital 40mA 800mA Corriente continua por pin 3V3 50mA 800mA 6 Líneas futuras y conclusión Como conclusión de este trabajo, existe una línea de mayor desarrollo del Software. Ya que el gran problema de Arduino es realizar los cálculos en un sistema real con una restricción de tiempo, hay más formas de conseguir decrementar el tiempo de cómputo. Una de ellas es el uso de números enteros en lugar de reales. Se ha explicado en el capítulo anterior que la cantidad de decimales exactos que se usen es importante, por lo tanto no sería conveniente eliminarlos. Sin embargo, se podría normalizar el problema para después escalarlo de forma que se usen al menos unos 6 ó 7 decimales. Es decir, que los números con los que se trabajarán serán enteros donde al ser escalados sus cifras decimales pasan a la parte del entero. Por lo tanto, las matrices del algoritmo FISTA serían transformadas para que los límites de u y x se encuentren en el intervalo [0,107]. Estaría de esta forma el problema normalizado y escalado. Una vez calculada la acción de control normalizada y multiplicada por 107 , se realizaría la transformación contraria. De esta forma se daría al sistema el valor real de la actuación. Además de poder hacer este desarrollo del Software, también sería conveniente utilizar una placa de Arduino más acorde a las exigencias del algoritmo de control que se quiere implementar. Esto requeriría reprogramar todo el código del sistema, ya que cambiará según sus especificaciones físicas. Otro solución posible, sería añadir una placa de Arduino que contenga solo el bucle de control, sin la lectura de sensores y la realización de la actuación. Para esto habría que enviar la actuación calculada por la placa que haga el MPC a la placa que ya está instalada. 49 Apéndice A Códigos de Matlab Código A.1 Inicialización de variables globales. global A B Ep C Cd D G n m p LBx UBx LBu UBu Q R b xk E H lim % Parametros del modelo del segway. N=20; % Condiciones iniciales: phi0=0*pi/180; % angulo inical de inclinación del pendulo dphi0=0; % velodidad inicial de giro del pendulo vr0=0; % velocidad de giro de las ruedas. %declaracion variables np=150; Tm=0.04; A=[1.032072427807300 0.040426724937844 0;1.612138909106717 1.032072427807300 0;0 0 1.000000000000000]; B=[-0.000289588215330; -0.014556317106940;0.040000000000000]; Ep=[0.032072427807300; 1.612138909106717;0.040000000000000]; C=[1 0 0;0 1 0;0 0 1]; G=[0 0 1]; Cd=zeros(n,p); D=[0;0;0]; LBx=-[30*pi/180 ; 5 ; 10]; % limite superior e inferior de X UBx=[30*pi/180 ; 5 ; 10]; LBu=-150; % limite superior e inferior de U UBu=150; [fil,n]=size(A); [filb,m]=size(B); p=n; xf=zeros(n,np); %estado y=zeros(p,np); %salida u=zeros(m,np); %acc. cntrol xf(1,1)=phi0; xf(2,1)=dphi0; xf(3,1)=vr0; Q=[10 0 0;0 1 0;0 0 10]; R=0.1; xk=xf(:,1); lim=zeros((n+m)*N,1); 51 52 Appendix A. Códigos de Matlab for i=1:n+m:(n+m)*N lim(i:i+n-1)=UBx; end for i=n+1:n+m:(n+m)*N lim(i:i+m-1)=UBu; end Código A.2 Definición de las variables del MPC. %Variables para el MPC H=zeros(N*(n+m)); E=zeros((N+1)*n,(n+m)*N); I=eye(n); aux=0; for cont=1:N for i=1:n H(i+n*(cont-1)+aux,i+n*(cont-1)+aux)=Q(i,i); end for i=n+1:m+n H(i+n*(cont-1)+aux,i+n*(cont-1)+aux)=R; aux=aux+1; end end aux=0; for i=1:N E(n+1+n*(i-1):2*n+n*(i-1),i+n*(i-1):n+n*(i-1)+aux)=Ad; E(n+1+n*(i-1):2*n+n*(i-1),i+n*(i-1)+n:m+n+n*(i-1)+aux)=Bd; if i<N E(n+1+n*(i-1):2*n+n*(i-1),i+n*(i-1)+n+m:n+m+n+n*(i-1)+aux)=-I; end if i==1 E(i:n,i:n)=I; end aux=aux+1; end Código A.3 Definición de las variables del SSTO. Qr=[10 0 0; 0 1 0; 0 0 1]; Rr=0.1; T=[1e8 0; 0 1e8]; H1=zeros(n+m+2); E1=zeros(n+2,n+m+2); I=eye(n); I1=eye(2); for i=1:n H1(i,i)=Qr(i,i); end H1(n+1:m+n,n+1:m+n)=Rr; H1(n+m+1:n+m+2,n+m+1:n+m+2)=T; E1(1:n,1:n)=A-I; E1(1:n,n+1:n+m)=B; E1(n+1:n+2,1:n)=[1 0 0;0 0 1]; E1(n+1:n+2,n+m+1:n+m+2)=I1; Ulim1=zeros(n+m+2,1); 53 Llim1=zeros(n+m+2,1); Ulim1(1:n,1)=[20*pi/180 ; 2 ; 10]; Ulim1(n+m,1)=100; Ulim1(n+m+1:2+n+m,1)=10^8; Llim1(1:n,1)=-[20*pi/180 ; 2 ; 10]; Llim1(n+m,1)=-100; Llim1(n+m+1:2+n+m,1)=-10^8; Código A.4 Bucle de Control con Algoritmo FISTA. u=0; b=zeros((N+1)*n,1); dr_gorro=0; dr=0; alpha=0.2; r_rampa=0; np=500; Cm=[1 0 0]; r=0; Pest = zeros(3,3); % matriz de covarianza estimada Ppr = zeros(3,3); % matriz de covarianza predicha Ko = zeros(3,3); % matriz de ganancias de Kalman tsim=14; ruido=[0.0015*rand-0.0015*rand 0.0015*rand-0.0015*rand 0.0015*rand-0.0015*rand ]'; for k=1:np h=0.005; w=dr; [t,y] = Rungekutta(@(t,y) Mdlsegwconpert(y,u,w),[(k-1)*Tm k*Tm],xk,h); xk=y(end,:)'; yk=C*xk+ruido; % OBSERVADOR: Filtro de Kalman Ro=I; Qo=diag(1); xpr=A*x_filtrada+B*u+Ep*d_filtrada; Ppr=Qo+A*Pest*A'; Ko=Ppr*C'/(C*Ppr*C'+ Ro); x_filtrada=xpr+Ko*(yk-C*xpr); Pest=(I-Ko*C)*Ppred; d_filtrada=d_filtrada+[-1 0 0]*(C*xk-C*x_filtrada); r_rampa=(1-alpha)*r_rampa+alpha*r'; [xr,ur]=SSTO(H1,E1,[0;r_rampa],Ep*d_filtrada,Ulim1,Llim1,n,m,2); %FISTA x=zeros((N+1)*n,1);x_ant=zeros((N+1)*n,1);y=zeros((N+1)*n,1); t=1;FIN=0;cont1=0; Qfista=inv(E/H*E'); %matrices f1 y b f1=zeros(N*(n+m),1); for i=1:n+m:N*(n+m) f1(i:i+n-1,1)=-Q*xr; f1(i+n:i+n,1)=-R*ur; end dd=Ep*d_filtrada; b=zeros((N+1)*n,1); b(1:n,1)=x_filtrada; for i=1:3:N*n 54 Appendix A. Códigos de Matlab b(n+i:n+i+2)=-dd; end b((N+1)*n-(n-1):(N+1)*n,1)=xr-dd; while FIN==0 z=zeros(n,1); f=f1-E'*y; for i=1:N*(n+m) z1=-f(i,1)/H(i,i); z(i)=max(Llim(i,1),min(z1,Ulim(i,1))); end if norm(E*z-b)<=1e-6 FIN=1; else incry=-Qfista*(E*z-b); x=y+incry; t_sig=0.5*(1+sqrt(1+4*t^2)); y=x+(((t-1)/t_sig)*(x-x_ant)); cont1=cont1+1; x_ant=x; t=t_sig; end if cont1>2000 FIN=1; disp('Se supera el número de iteraciones') end u=z(4); end end Código A.5 Función del SSTO. function [xr,ur]=SSTO1(H1,E1,yr,dd,Ulim,Llim,n,m,p) %Algoritmo FISTA %inicialización x=zeros(n+p,1);x_ant=zeros(n+p,1);y=zeros(n+p,1); t=1;FIN=0;cont1=0; Qfista=inv(E1/H1*E1'); f1=zeros(n+m+p,1); b=zeros(n+p,1); b(1:n,1)=dd; b(n+1:n+p,1)=yr; while FIN==0 z=zeros(n,1); f=f1-E1'*y; for i=1:n+m+p z1=-f(i,1)/H1(i,i); z(i)=max(Llim(i,1),min(z1,Ulim(i,1))); end %norm(E1*z-b) if norm(E1*z-b)<=1e-6 FIN=1; else incry=-Qfista*(E1*z-b); x=y+incry; t_sig=0.5*(1+sqrt(1+4*t^2)); 55 y=x+(((t-1)/t_sig)*(x-x_ant)); cont1=cont1+1; x_ant=x; t=t_sig; end if cont1>2000 FIN=1; disp('Se supera el número de iteraciones') %norm(E1*z-b) end end xr=[z(1);z(2);z(3)]; ur=z(4); end Código A.6 Modelo no lineal. function [T,Y]=Rungekutta(f,intervalo,y0,h) t0=intervalo(1); tf=intervalo(2); y0=y0(:); T=(t0:h:tf)'; N=length(T)-1; Y=ones(N+1,1)*y0'; for k=1:N k1=f(T(k),y0); k2=f(T(k)+h/2,y0+h*k1/2); k3=f(T(k)+h/2,y0+h*k2/2); k4=f(T(k)+h,y0+h*k3); y1=y0+(h/6)*(k1+2*k2+2*k3+k4); Y(k+1,:)=y1'; y0=y1; end end function [ dx ] = Mdlsegwconpert(x,u,w) g=9.81; mr=0.140; % Masa de cada rueda R=0.05; % Radiod e la rueda M=0.82; % Masa del cuerpo del pendulo L=0.098; % Distancia del centro de gravedad al eje de las ruedas Jr=0.5*mr*R^2; J=M*L^2; a=(mr+M/2)*R^2+Jr; b=M*L^2/2+J/2; c=M*R*L; d=-M*g*L; dx=zeros(3,1); dx(1)=x(2); dx(2)=(((c*x(2)^2*sin(x(1)+w))-d*sin(x(1)+w)-u*(2*a+c*cos(x(1)+w)))/(2*b+c*cos( x(1)+w))); dx(3)=u;; end Índice de Figuras 2.1 Vehículo autoequilibrado 3 2.2 Esquema del Segway 4 3.1 Ejemplo 1 14 3.2 Ejemplo 2 14 3.3 Ejemplo 3 15 3.4 Diagrama de bloques 22 3.5 Gráfica del filtro con distinta α27 4.1 Saturación de la actuación 32 4.2 Saturación de la actuación con referencia de velocidad igual a 2 rad/s32 4.3 Referencia de velocidad nula y sin perturbación 33 4.4 Referencia nula y perturbación nula 34 4.5 Referencia de velocidad igual a dos y sin perturbación 34 4.6 Referencia igual a dos y perturbación nula 35 4.7 Referencia de velocidad igual a menos uno y sin perturbación 35 4.8 Referencia igual a menos uno y perturbación nula 36 4.9 Referencia de velocidad de uno a menos uno y sin perturbación 36 4.10 Referencia de uno a menos uno y perturbación nula 37 4.11 Referencia de velocidad nula y con perturbación 38 4.12 Referencia nula y perturbación igual a 0.03 rad 38 5.1 Placa Arduino Mega 2560 39 63 Índice de Tablas 2.1 Parámetros físicos del sistema 4 2.2 Momentos de inercia 5 2.3 Coordenadas generalizadas 5 5.1 Características del microcontrolador 40 5.2 Características electrónicas 40 5.3 Características electrónicas 40 5.4 Comparación 48 65 Índice de Códigos 2.1 Cálculo del sistema en discreto 9 3.1 Cálculo del MPC 20 3.2 Cálculo del SSTO 23 3.3 Cálculo del OBS 26 3.4 Cálculo de las gráficas del filtro 27 5.1 Cálculo de la matriz EZ(yk)−b−d42 5.2 Cálculo de f43 5.3 Cálculo de la matriz W−1(Qfista) 45 A.1 Inicialización de variables globales 51 A.2 Definición de las variables del MPC 52 A.3 Definición de las variables del SSTO 52 A.4 Bucle de Control con Algoritmo FISTA 53 A.5 Función del SSTO 54 A.6 Modelo no lineal 55 B.1 Definición de variables 57 B.2 Carga de datos en Arduino 57 B.3 Cálculo de la actuación 57 B.4 Algoritmo FISTA 58 67 Bibliografía [1] D. Muñoz La Peña C. González, I. Alvarado, Low cost two-wheels self-balancing robot for control education, The International Federation of Automatic Control, July 2017. [2] Carlos Bordons Alba Daniel Rodríguez Ramírez, Apuntes de ingeniería de control, Departamento de Ingeniería de Sistemas y Automática, Universidad de Sevilla, 2005. [3] Cecilia González González, Mejora del software de un vehículo autoequilibrado de tipo péndulo invertido de bajo coste, Universidad de Sevilla, 2016. [4] Victor Manuel Villalar Lara, Control predictivo para un vehículo tipo segway, Universidad de Sevilla, 2017. [5] Alfonso García Navarro, Implementación de controladores predictivos en arduino, Universidad de Sevilla, 2016. [6] Antonio J. Croche Navarro, Vehículo autoequilibrado de tipo péndulo invertido de bajo coste, Universidad de Sevilla, 2013. [7] Renato Álvarez Nodarse, Apuntes de diferenciación de funciones de varias variables, Departamento de Análisis Matemático Facultad de Matemáticas, Septiembre 2017. 69